Řízení pohybu

Regulátor sledování dráhy

Jak se z posloupnosti bodů a aktuální polohy stanou dvě čísla pro motory.

Plánovač vydá dráhu: posloupnost bodů, z nichž každý nese toleranci $\varepsilon$, v rámci které má robot projet. Regulátor z ní má desetkrát za sekundu vyrobit dvě čísla — dopřednou rychlost $v$ a rychlost otáčení $\omega$ — tak, aby robot každý uzel projel v toleranci a přitom co nejrychleji. Uzly se neprojíždějí zastavením.

Úloha se dělí na dvě části, které mají velmi různou frekvenci. Plánování proběhne jednou pro celou dráhu a předpočítá všechno, co plyne z geometrie. Exekuce běží každý takt a vychází z aktuální pózy robotu. Není to úspora výpočtu, je to dělba odpovědnosti: tvar dráhy se mezi takty nemění, poloha robotu ano.

Proč ne jeden proporcionální člen

Nabízí se počítat $\omega$ úměrně příčné odchylce od trasy. Takový zásah ale neví nic o tom, že $\omega$ je shora omezené, že se příkaz mění jen jednou za 100 ms a že póza ze senzorové fúze je o kus zpožděná — a v téhle soustavě z toho vzniká kmitání. Regulátor proto každý takt raději přeplánuje manévr z reálné pózy a zásah vydá přes kinematický profil (rampa omezená zrychlením). Zpětná vazba tu nevede přes zesílení, ale přes plán.

maximální rychlost	$v_{max} = 1{,}2$ m/s (provozní profil jezdí na 1 m/s)
maximální otáčení	$\omega_{max} = \pi/6$ rad/s = 30 °/s
zrychlení	$a = 0{,}5$ m/s² (stejné pro brzdění)
perioda regulátoru	$T_s = 100$ ms
rozchod kol	0,41 m
tolerance uzlu	$\varepsilon = 0{,}1$ m, z toho 1 cm rezerva

Roh je oblouk

Dráha je lomená čára a lomený bod se projet nedá — v něm by se muselo zastavit a otočit. Tolerance $\varepsilon$ ale říká, jak daleko od uzlu smí robot projet, a to je přesně místo pro oblouk vepsaný do rohu.

Roh se prokládá kružnicí, která se dotýká obou úseků. Její střed leží na ose úhlu; nejbližší bod oblouku je od uzlu vzdálený právě tolik, kolik dovoluje tolerance $\varepsilon$. Tím je poloměr $R$ jednoznačně určený úhlem zatáčky $\theta$.

Střed vepsané kružnice leží na ose úhlu ve vzdálenosti $R/\cos(\theta/2)$ od uzlu, takže nejbližší bod oblouku je od uzlu vzdálený o rozdíl (1). Položíme-li ho rovný toleranci, vyjde poloměr (2); tečná délka rohu je (3).

$$\begin{align} e &= \frac{R}{\cos(\theta/2)} - R = R\,\frac{1-\cos(\theta/2)}{\cos(\theta/2)} \tag{1}\\ R &= \varepsilon\,\frac{\cos(\theta/2)}{1-\cos(\theta/2)} \tag{2}\\ T &= R\,\tan(\theta/2) \tag{3} \end{align}$$

$\theta$úhel zatáčky — změna směru mezi vstupním a výstupním úsekem

$R$poloměr oblouku vepsaného do rohu

$\varepsilon$tolerance průjezdu uzlem

$T$tečná délka — jak daleko před uzlem oblouk začíná

Oba krajní případy vycházejí samy: pro $\theta \to 0$ (rovný průjezd) roste $R$ nade všechny meze a roh neomezuje nic, pro $\theta \to \pi$ (otočka) jde $R$ k nule, tedy k zastavení. Zbývá jedno hlídání: tečná délka $T$ nesmí přesáhnout polovinu kratšího sousedního úseku, jinak by se dva sousední oblouky překryly. Když k tomu dojde, poloměr se zmenší — a robot v tom rohu pojede pomaleji, než by geometrie sama žádala.

Kolik se v rohu smí jet

Po oblouku o poloměru $R$ jede robot s úhlovou rychlostí $v/R$, a ta je shora omezená. Odtud strop rychlosti v uzlu (4).

$$\begin{align} v_{roh} &= \omega_{max}\,R \tag{4} \end{align}$$

$\theta$	$R$ [m]	$\omega_{max}R$ [m/s]	strop v uzlu [m/s]	co ho určuje
90°	0,217	0,11	0,11	otáčení
60°	0,582	0,30	0,30	otáčení
45°	1,092	0,57	0,57	otáčení
40°	1,402	0,73	0,73	otáčení
≈ 32°	2,292	1,20	1,20	zlom
30°	2,551	1,34	1,20	$v_{max}$
15°	10,43	5,46	1,20	$v_{max}$

Tabulka je počítaná pro $\varepsilon = 0{,}09$ m (tedy 10 cm bez rezervy, viz níž) a hodnoty robotu z boxu nahoře. Zajímavý je zlom u $\theta \approx 32^\circ$: ostřejší rohy sráží rychlost limit otáčení, mírnější už nesráží nic a jede se naplno. Pravoúhlá zatáčka znamená 0,11 m/s — jedenáctinu plné rychlosti.

Oblouk, ne klotoida

Robot ve skutečnosti oblouk nejede. Rotační rychlost se nemůže změnit skokem, takže křivost náběhá a skutečná křivka je klotoida. Rozhoduje, kolik stupňů robot projede, než se otáčení rozběhne na $\omega_{max}$ — vztah (5), kde $\alpha$ je úhlové zrychlení plynoucí z rozchodu kol.

$$\begin{align} \varphi &= \frac{\omega_{max}^{2}}{2\alpha},\qquad \alpha = \frac{a}{\text{rozchod}/2} = \frac{0{,}5}{0{,}205} \approx 2{,}44\ \text{rad/s}^2 \ \Rightarrow\ \varphi \approx 3{,}2^\circ \tag{5} \end{align}$$

Proti běžné zatáčce 30–90° je náběh 3,2° malý, takže oblouk je dobrá aproximace. Příčný posun klotoidy proti oblouku $\Delta \approx L_c^2/(24R)$ vychází v celém rozsahu pod 1,2 mm — nejhůř právě u toho zlomu, kde se potkávají oba limity. Neřeší se to přesnější křivkou, ale tak, že se z tolerance odečte 1 cm rezervy a rohy se plánují na zbytek. Rezerva zároveň kryje seříznutí zatáčky při vlastním řízení.

Brzdná obálka

Strop v uzlu sám o sobě nestačí — do pomalého uzlu se musí stihnout dobrzdit, a to začíná dávno před ním. Přes celou dráhu se proto od konce projde zpětným průchodem (6): rychlost na vstupu úseku nesmí přesáhnout ani vlastní strop uzlu, ani rychlost, ze které se ještě dá ubrzdit na strop uzlu následujícího.

$$\begin{align} v_{vstup} &= \min\Bigl(v_{uzel},\ \sqrt{v_{v\acute{y}stup}^{2} + 2aL}\Bigr) \tag{6} \end{align}$$

$L$délka úseku

$a$zrychlení (a brzdění)

$v_{uzel}$vlastní strop uzlu — z rohu podle (4), z $v_{max}$, nebo zadaný plánovačem

Výsledkem je brzdná obálka: nejvyšší rychlost v místě $s$, ze které ještě stihnu splnit všechno, co je přede mnou. Není to průběh rychlosti, je to strop.

Brzdná obálka nad dráhou s pravoúhlým rohem ve dvou metrech a zastavením v 5,5 m. Uzel v jednom metru má vlastní strop 1,20 m/s (prázdné kolečko), zpětný průchod ho ale srazí na 1,01 m/s — jinak by se roh nedal ubrzdit. Za rohem obálka skokem vyskočí zpět: je to strop, ne průběh rychlosti, a zrychlení už řeší regulátor živě.

Z 1,2 m/s se při $a = 0{,}5$ m/s² brzdí 1,44 m. Pravoúhlý roh, ve kterém se smí jen 0,11 m/s, si tedy vyžádá začátek brzdění 1,43 m před sebou — a když je předchozí úsek dlouhý jen metr, nesmí se do něj vletět rychleji než $\sqrt{0{,}11^2 + 2\cdot 0{,}5\cdot 1} = 1{,}01$ m/s. Odtud se omezení šíří dál dozadu, dokud ho nepřebije $v_{max}$.

Zpětný průchod ano, dopředný ne. Akcelerační průchod by musel předpokládat, jakou rychlostí robot do dráhy vjede — a to plán neví. Zrychlení proto počítá regulátor živě ze skutečné rychlosti, kterou zná až v okamžiku jízdy. Plán nese jen to, co plyne z budoucnosti, ne z minulosti.

Exekuce: dvě čísla každých sto milisekund

Každý takt se udělá totéž:

Lokalizace. Kolmý průmět pózy na úseky dráhy dá ujetou délku a index úseku. Hledá se jen lokálně kolem posledního známého úseku — robot se za 100 ms daleko nedostane.
Dopředná rychlost. Jediný dotaz na kinematický profil: jak rychle mám jet, abych ve vzdálenosti $d$ byl na stropu $v_k$. Stačí nejbližší uzel před robotem, protože jeho strop už přes zpětný průchod obsahuje celou budoucnost — z něj se dá ubrzdit na další, z toho na další, a tak dál. Indukcí je to bezpečné.
Otáčení. Úhlová odchylka $\beta$ na cílový uzel, z ní zásah $\omega$ přes týž profil.
Vazba obojího. Dopředná rychlost se shora omezí tak, aby se robot stihnul natočit — vztah (7). Je-li $|\beta| > \pi/2$, je dopředná rychlost nula a robot se otočí na místě.

$$\begin{align} v &\le \frac{d}{k\,T_{rot}} \tag{7} \end{align}$$

$d$vzdálenost k cílovému bodu řízení

$T_{rot}$doba, za kterou se robot na cílový bod natočí

$k$koeficient stability, $k = 4$

$\beta$úhlová odchylka na cílový bod

Poslední bod vypadá nevinně a je to nejzajímavější místo celého regulátoru.

Omezovač, který se díval na vlastní výstup

Vztah (7) říká rozumnou věc: nejeď dál, než kam se stihneš natočit. Všechno ale závisí na tom, odkud se vezme $d$.

Původně se robot natáčel na virtuální bod na ideální trase, umístěný tím dál, čím rychleji jede — vztah (8). To samo o sobě dává smysl: při vyšší rychlosti se robot dívá dál dopředu. Dosazením (8) do (7) ale vznikne nerovnost, ve které je $v$ na obou stranách.

$$\begin{align} d &= \max(d_{min},\ \tau v),\qquad d_{min} = 0{,}15\ \text{m},\ \tau = 0{,}3\ \text{s} \tag{8}\\ v &\le \frac{\max(d_{min},\ \tau v)}{k\,T_{rot}} \tag{9} \end{align}$$

Ve větvi, kde $\tau v > d_{min}$, se z (9) stane $v \le v \cdot \tau/(k\,T_{rot})$. Ta je pro $\tau/(k\,T_{rot}) < 1$ splnitelná jedině pro $v \le 0$ — soustava se tedy vždycky sesune do druhé větve a ustálí se na podlaze (10).

$$\begin{align} v &\le \frac{d_{min}}{k\,T_{rot}} \tag{10} \end{align}$$

Tentýž omezovač dvakrát. Vlevo dostává na vstup vzdálenost odvozenou z rychlosti, kterou sám právě omezil — a soustava se sesune na podlahu. Vpravo přichází vzdálenost z geometrie dráhy, tedy zvenčí; nerovnost zůstane nerovností a přestane být rovnicí o sobě samé.

Na robotu to vyšlo takhle. Dorovnání odchylky 11,9° trvá při $\omega_{max} = 30$ °/s celých $T_{rot} = 0{,}786$ s, takže (10) dává $0{,}15/(4\cdot 0{,}786) = \mathbf{0{,}048}$ m/s. Robot přesně tolik jel — ačkoli plánovač povoloval 1,2 m/s a brzdná obálka 0,95 m/s. Iterace vztahu (9) je rychlá: 1,20 → 0,11 → 0,048 m/s. Aby ze vztahu (7) vyšlo zpátky 1,2 m/s, musel by být cílový bod 3,8 m daleko.

Smyčka by se otevřela jedině při $\tau \ge k\,T_{rot}$, tedy $T_{rot} \le 0{,}075$ s. Při 30 °/s je to odchylka pod 2,3° — prakticky nikdy. Nebyla to tedy mez, do které robot občas narazí; byl to stav, ze kterého se soustava sama nedostane.

Léčba: cílem je uzel dráhy

Robot se natáčí na skutečný uzel dráhy před sebou a do téže vzdálenosti se váže dopředná rychlost. Směr i vzdálenost tak pocházejí z jednoho bodu — jsou to dvě strany téhož: mířím tam a tam se musím stihnout natočit. $d$ je vzdálenost daná geometrií dráhy, ne vlastním výstupem.

Uzel blíž než $d_{min}$ se přeskakuje. Bez toho by to nešlo ze dvou důvodů: azimut k bodu, na kterém robot prakticky stojí, poskakuje o desítky stupňů, a vzdálenost k němu jde k nule — takže by (7) robota zastavil na každém uzlu. $d_{min}$ tedy zůstalo, ale už neurčuje cíl, jen práh „tenhle uzel už mám za sebou“.

Přesnost průjezdu se tím přesunula tam, kam patří: řídí ji hustota uzlů a jejich tolerance. Kde je potřeba projet přesně, nasází plánovač uzly blízko sebe — a protože se dráha přeplánovává z aktuální pózy každý snímek kamery, příčná odchylka se průběžně vynuluje sama.

Nebyl to špatný vzorec. Vztahy (7) i (8) jsou každý zvlášť správné a obhajitelné, a oba by prošly kontrolou jednotek i testem sebe sama. Vada vznikla teprve jejich složením — omezovač dostal na vstup veličinu odvozenou z vlastního výstupu. Takovou věc neodhalí kontrola jedné funkce; vidí ji až otázka, odkud se to číslo bere.

A nakonec dvě čísla pro motory

Výstup regulátoru se převede na příkaz pro motorovou jednotku podle (11): dopředná rychlost zůstane, rotace se přepočte na offset na kolo.

$$\begin{align} \mathit{dif} &= \omega\,\frac{\text{rozchod}}{2} \tag{11} \end{align}$$

Ta polovina tam patří právě proto, že $\mathit{dif}$ je offset na jedno kolo, ne rozdíl rychlostí kol: driver ho k jednomu kolu přičte a od druhého odečte, takže $v_P - v_L = \omega \cdot \text{rozchod} = 2\,\mathit{dif}$. Bez ní robot zatáčel dvakrát rychleji, než regulátor chtěl.

Když plán přestane chodit. Řídicí smyčka běží nezávisle na tom, jestli vyšší vrstvy stíhají. Když déle než půl sekundy nepřijde nová dráha, robot nedostane volant z ruky — dobrzdí po poslední známé trase. V zatáčce je řízené dobrzdění po posledním plánu pořád lepší než pustit řízení.

maximální rychlost	\(v_{max} = 1{,}2\) m/s (provozní profil jezdí na 1 m/s)
maximální otáčení	\(\omega_{max} = \pi/6\) rad/s = 30 °/s
zrychlení	\(a = 0{,}5\) m/s² (stejné pro brzdění)
perioda regulátoru	\(T_s = 100\) ms
rozchod kol	0,41 m
tolerance uzlu	\(\varepsilon = 0{,}1\) m, z toho 1 cm rezerva