Odometrie

Model diferenciálního podvozku

Jak z otáček dvou kol spočítat, kam se robot posunul a jak se otočil.

Diferenciální podvozek je popsán vztahy (1) a (2).

Poloha robotu v krocích k a k+1. Z rozdílu ujetých drah obou stran vyjde úhel otočení $\omega$, z jejich průměru ujetá vzdálenost $d$.

$$\begin{align} \omega &= \frac{\Delta L - \Delta R}{b} \tag{1}\\ d &= \frac{\Delta L + \Delta R}{2} \tag{2} \end{align}$$

$\omega$úhel otočení

$d$ujetá vzdálenost

$\Delta L$ujetá vzdálenost levou stranou

$\Delta R$ujetá vzdálenost pravou stranou

$b$rozchod kol

Dále musíme určit polohu a orientaci robotu. Podkladem nám bude ujetá vzdálenost a otočení robotu. Pohyb robotu popisují rovnice (3), (4) a (5).

$$\begin{align} \alpha(t) &= \int \omega(t)\,dt \tag{3}\\ x(t) &= \int v(t)\cos\alpha(t)\,dt \tag{4}\\ y(t) &= \int v(t)\sin\alpha(t)\,dt \tag{5} \end{align}$$

Problém jejich řešení spočívá ve faktu, že neznáme funkce pro úhlovou a dopřednou rychlost. Jediné co známe je změna orientace robotu v okamžicích vzorkování a ujetá vzdálenost mezi těmito okamžiky. Možných variant pohybu robotu je mnoho. Viz následující obrázek.

Mezi dvěma vzorky se robot mohl pohybovat mnoha způsoby a každý končí jinde — přestože ujel tutéž vzdálenost a otočil se o týž úhel (tady pro názornost o 90 °). Zvýrazněná varianta b — čtvrtkruh o konstantní dopředné i úhlové rychlosti — je ta, se kterou se dál počítá.

Otázkou je kterou si vybrat? Jednoduchá varianta rozumně aproximující skutečnost je varianta konstantní dopředné a úhlové rychlosti, kdy se bude robot pohybovat po kruhových obloucích. V našem obrázku je tento případ vyobrazen pod písmenem b a popisují ho vztahy (6) a (7). Dosazením (6) do vztahu (3) pro úhlové natočení získáme časový průběh orientace robotu. Dalším dosazením a integrací rovnice (4) na intervalu $T_s$ postupně získáme (9), který popisuje změnu souřadnice x po jedné vzorkovací periodě. Tento vztah je nepraktický kvůli dělení $\omega$.

$$\begin{align} \omega(t) &= \frac{\omega}{T_{s}} \tag{6}\\ v(t) &= \frac{d}{T_{s}} \tag{7}\\ \alpha(t) &= \frac{\omega}{T_{s}}t + \alpha_{c} \tag{8}\\ \Delta x_{k} &= \int_{0}^{T_{s}}{v(t)\cos \alpha(t)dt} = \int_{0}^{T_{s}}{\frac{d}{T_{s}}\cos\left( \frac{\omega}{T_{s}}t+\alpha_{c}\right)dt} = \frac{d}{T_{s}}\frac{T_{s}}{\omega}\sin\left( \frac{\omega}{T_{s}}T_{s}+\alpha_{c}\right) -\frac{d}{T_{s}}\frac{T_{s}}{\omega}\sin\left( \frac{\omega}{T_{s}}0+\alpha_{c}\right) = \\ &= \frac{d}{\omega}\left(\sin\left(\omega+\alpha_{c}\right)-\sin\left(\alpha_{c}\right)\right) \tag{9} \end{align}$$

$\alpha_{c}$počáteční úhel natočení

$T_{s}$vzorkovací perioda

$\Delta x_{k}$změna polohy v ose x

Protože pootočení je malé lze diferenci nahradit derivací v mezilehlém době, jak je naznačeno na následujícím obrázku. Výsledné vztahy pro polohu robotu v diskrétním čase pak popisují rovnice (10), (11) a (12).

Náhrada diference derivací v mezilehlém bodě. Sečna mezi $\alpha$ a $\alpha+\omega$ je rovnoběžná s tečnou v $\alpha+\omega/2$ — přesně vzato se jejich sklon liší o činitel $\sin(\omega/2)\,/\,(\omega/2)$, pro malá $\omega$ zanedbatelný.

$$\begin{align} \alpha_{k+1} &= \alpha_{k} + T_{s}\omega_{k} \tag{10}\\ x_{k+1} &= x_{k} + \Delta x_{k} = T_{s}v_{k}\cos\left(\frac{T_{s}\omega_{k}}{2}+\alpha_{k}\right) \tag{11}\\ y_{k+1} &= y_{k} + \Delta y_{k} = T_{s}v_{k}\sin\left(\frac{T_{s}\omega_{k}}{2}+\alpha_{k}\right) \tag{12} \end{align}$$

$\omega_{k}$úhlová rychlost robotu v kroku k

$\alpha_{k}$orientace robotu v kroku k

$v_{k}$dopředná rychlost v kroku k

$T_{s}$vzorkovací perioda

Jak lze interpretovat získané řešení? V první řadě je nutné poznamenat, že robot se už nepohybuje po kruhovém oblouku, jak jsme na počátku předpokládali, ale po přímce jejíž délka je shodná s délkou oblouku a orientace přímky půlí $\omega$. Tento výpočet proti původnímu předpokladu negeneruje úhlovou chybu a chyba vzdálenosti pro v praxi maximální $\omega = 8^\circ$ je do 0,1 %.

$$\begin{align} \text{relativní chyba } d &= \frac{\text{délka oblouku} - \text{délka tětivy}}{\text{délka oblouku}} = \frac{d-2\frac{d}{\omega}\sin{\frac{\omega}{2}}}{d} = 1-\frac{2}{\omega}\sin{\frac{\omega}{2}} \tag{13} \end{align}$$

Směr je přesný, chybuje jen délka

Stojí za to říct to natvrdo, protože je to nejsilnější věta celého odvození. Obě věci, které jsme cestou potkali — že sečna je rovnoběžná s tečnou v mezilehlém bodě a že tětiva je kratší než oblouk — jsou jedna a tatáž aproximace. Plynou z jediného integrálu: z průměru jednotkového vektoru směru přes lineárně se otáčející úhel.

$$\begin{align} \frac{1}{\omega}\int_{\alpha}^{\alpha+\omega}\bigl(\cos u,\ \sin u\bigr)\,du = \frac{\sin(\omega/2)}{\omega/2}\, \Bigl(\cos\bigl(\alpha+\tfrac{\omega}{2}\bigr),\ \sin\bigl(\alpha+\tfrac{\omega}{2}\bigr)\Bigr) \tag{14} \end{align}$$

Směr toho vektoru je přesně $\alpha+\omega/2$ — bez jakékoli chyby, ať je $\omega$ jakkoli velké. Odtud plyne, že výpočet negeneruje úhlovou chybu. Velikost je přesně činitel $\sin(\omega/2)/(\omega/2)$, o který je tětiva kratší než oblouk. Ten se ve vztazích (11) a (12) zanedbává — místo délky tětivy se dosadí délka oblouku $T_s v_k$ — a měří ho vztah (13). Tentýž činitel proto vychází i jako poměr sklonu sečny ke sklonu tečny na obrázku výš; je to pokaždé týž vektor, jednou čtený přes velikost a jednou přes svou složku.

Pozor na jednu záměnu. Ten činitel je poměr sklonů, ne úhel mezi přímkami. Poměr sklonů je $\sin(\omega/2)/(\omega/2)$ přesně a nezávisle na $\alpha$; úhel mezi sečnou a tečnou je oproti tomu $\arctan m-\arctan\!\bigl(m\cdot\sin(\omega/2)/(\omega/2)\bigr)$ pro $m=\cos(\alpha+\omega/2)$, tedy závisí i na tom, kde v periodě se nacházíme, a pro malá $\omega$ vychází zhruba $\tfrac{m}{1+m^{2}}\cdot\tfrac{\omega^{2}}{24}$, nejvýš $\omega^{2}/48$. Pro $\omega = 8^\circ$ je to nejvýš 0,023 °.