Hlavice
V příspěvku Individuální projekt (2) jsem uvažoval inerciálně stabilizovanou platformu jako otevřený kinematický řetězec a ten popsal Denavit-Hartenbergovou notací. Pro tuto notaci je definována transformační matice ze souřadnicové soustavy kloubu $n$ do $n-1$ následovně (viz [1])$$ \mathbf{T}_{n}^{n-1} (\theta_n, d_n, a_n, \alpha_n)=
\left[
\begin{array}{cccc}
\cos\theta_n & -\sin\theta_n \cos\alpha_n & \sin\theta_n \sin\alpha_n & a_n \cos\theta_n \\
\sin\theta_n & \cos\theta_n \cos\alpha_n & -\cos\theta_n \sin\alpha_n & a_n \sin\theta_n \\
0 & \sin\alpha_n & \cos\alpha_n & d_n \\
0 & 0 & 0 & 1
\end{array}
\right].
$$ Transformační matice pro celý kinematický řetězec je pouze součinem transformačních matic jednotlivých kloubů.
$$ \mathbf{T}_{2}^{0} (q_1, q_2) = \mathbf{T}_{1}^{0}(q_1) \mathbf{T}_{2}^{1}(q_2)
$$
Nosič
Transformační matice pro přechod z vnitřní souřadnicové soustavy nosiče do vnější souřadnicové soustavy je dána rotační maticí, kterou získáme převodem z quaternionu $\mathbf{q}=(q_w, q_x, q_y, q_z)$ pomocí následujícího vztahu
$$
\mathbf{R}(\mathbf{q}) = \begin{bmatrix}
1 - 2 q_y^2 - 2 q_z^2 & 2 q_x q_y - 2 q_z q_w & 2 q_x q_z + 2 q_y q_w \\
2 q_x q_y + 2 q_z q_w & 1 - 2 q_x^2 - 2 q_z^2 & 2 q_y q_z - 2 q_x q_w \\
2 q_x q_z - 2 q_y q_w & 2 q_y q_z + 2 q_x q_w & 1 - 2 q_x^2 - 2 q_y^2
\end{bmatrix}.
$$
Samotný výpočet
Jelikož je osa kamery a dálkoměru totožná s osou $x$ posledního rotačního kloubu, vypadá vektor směřující na místo na Zemi, které je ve středu obrazu kamery, v souřadnicové soustavě tohoto kloubu následovně$$ \mathbf{X}(d)=
\left[
\begin{array}{c}
X_x\\
X_y\\
X_z\\
1
\end{array}
\right]=
\left[
\begin{array}{c}
d\\
0\\
0\\
1
\end{array}
\right],
$$ kde $d$ je změřená vzdálenost dálkoměrem v kilometrech. Tento vektor transformujeme pomocí výše uvedených matic do vnější souřadnicové soustavy nosiče.
$$ \mathbf{X}' (\mathbf{q}, q_1, q_2, d)= \mathbf{R}(\mathbf{q})\mathbf{T}_2^0(q_1, q_2) \mathbf{X}(d)
$$ Bod na Zemi na nějž ukazuje vektor $\mathbf{X}'$ však chceme vyjádřit v GPS souřadnicích. To provedeme tak, že z vektoru $\mathbf{X}'$ určíme vzdálenost nosiče na Zemi $l$ a úhel $\alpha$ mezi nosičem a bodem ve středu kamery.
\begin{align*}
l &= \sqrt{\mathbf{X}'^2_x + \mathbf{X}'^2_y}\\
\alpha &= \text{atan2}\left(\mathbf{X}'_y, \mathbf{X}'_x\right)
\end{align*}
Konečně, GPS souřadnice bodu ve středu obrazu z kamery spočteme pomocí vztahů, které jsou uvedeny například zde [2].
\begin{align*}
\phi_2 &= \mathrm{asin}( \sin(\phi_1)\cos(\mathrm{l/R}) + \cos(\phi_1)\sin(\mathrm{l/R})\cos(\alpha) ),\\
\lambda_2 &= \lambda_1 + \mathrm{atan2}(\sin(\theta)\sin(l/\mathrm{R})\cos(\phi_1), \cos(l/\mathrm{R})-\sin(\phi_1)\sin(\phi_2) ),
\end{align*}
kde $\phi_1$, $\lambda_1$ jsou zeměpisná šířka, resp délka nosiče, $R$ je poloměr Země v kilometrech a $\phi_2$, $\lambda_2$ jsou zeměpisná šířka, resp délka kýženého bodu na Zemi.
Zdroje
[1] P. Corke. Robotics, Vision and Control: Fundamental Algorithms in MATLAB. Springer, 1st edition, Nov. 2011.[2] http://www.movable-type.co.uk/scripts/latlong.html
Žádné komentáře:
Okomentovat