Ze známých GPS souřadnic nosiče a bodu určíme nejprve vzdálenost (po povrchu Země) $l$ a úhel $\alpha$ mezi nimi. Zeměpisnou šířku nosiče budu značit jako $\varphi_1$ a zeměpisnou délku nosiče jako $\lambda_1$. Obdobně pak pro souřadnice bodu na mapě pouze s tím rozdílem, že dolní index bude $2$.
Vzdálenost určíme pomocí tzv. haversine vzorce, který lze nalézt například zde [1]
\begin{align} a &= \sin^2\left( \left(\varphi_2 - \varphi_1\right)/2\right) + \cos\left( \varphi_1\right) \cos\left( \varphi_2\right) \sin^2\left( \left(\lambda_2 - \lambda_1\right)/2\right), \\
l &= 2\mathrm{R}\,\text{atan2}\left( \sqrt{a}, \sqrt{1-a} \right), \end{align} kde $\mathrm{R}$ je poloměr Země v kilometrech.
Úhel určíme podle vztahu (viz [1])
\begin{equation} \alpha = \text{atan2}\left( \sin\left(\Delta\lambda\right) \cos\left(\varphi_2\right), \cos\left(\varphi_1\right)\sin\left(\varphi_2\right) - \sin\left(\varphi_1\right)\cos\left(\varphi_2\right)\cos\left(\Delta\lambda\right)\right), \end{equation} kde $\Delta\lambda = \left(\lambda_2 - \lambda_1\right)$.
Konečně polohový vektor bodu na mapě vůči nosiči v jeho vnější souřadnicové soustavě je \begin{equation} \mathbf{X} = \begin{bmatrix}
l\cos\left(\alpha\right)\\
l\sin\left(\alpha\right)\\
h_1 - h_2
\end{bmatrix}, \end{equation} kde $h_1$ je nadmořská výška nosiče a $h_2$ je nadmořská výška bodu na mapě. Tento vektor transformujeme do vnitřní souřadnicové soustavy nosiče pomocí rotační matice $\mathbf{R}(\mathbf{q})$ z předchozího příspěvku. Tato matice však představuje transformaci z vnitřní souřadnicové soustavy do vnější. Inverzní transformaci získáme její transpozicí. Polohový vektor $\mathbf{X}$ ve vnitřní souřadnicové soustavě tedy určíme následovně \begin{equation} \mathbf{X}' = \mathbf{R}^T\mathbf{X}. \end{equation} Poslední úpravou polohové vektoru je jeho transformace do souřadnicové soustavy samotné kamerové platformy, tedy vlastně pouze posunutí počátku vnitřní souřadnicové soustavy nosiče do bodu, kde mají průsečík obě osy otáčení kamerové platformy. To lze udělat například pomocí transformační matice $\mathbf{T}_2^0(\mathbf{q}_1, \mathbf{q}_2)$ z předchozího příspěvku s nulovými kloubovými souřadnicemi. \begin{equation} \mathbf{X}'' = \left(\mathbf{T}_2^0(\mathbf{q}_1, \mathbf{q}_2)\right)^T\mathbf{X}' \end{equation} Kýžené kloubové souřadnice pak již určíme snadno pomocí následujících vztahů \begin{align} q_1 &= \pi - \text{atan2}(X''_x, X''_y),\\
q_2 &= \pi/2 + \text{atan2}(X''_x, X''_z), \end{align} přičemž ony posuny o zlomky $\pi$ jsou dány orientací inkrementálních enkodérů (viz příspěvek Individuální projekt (2)).
Žádné komentáře:
Okomentovat