姿态解算
)
方向余弦矩阵
基本概念
已知欧拉角求方向余弦
ψ θ γ \psi \theta \gamma ψθγ
[ c o s γ c o s ψ + s i n γ s i n ψ s i n θ − c o s γ s i n ψ s i n θ − s i n γ c o s θ s i n ψ c o s θ c o s ψ c o s θ s i n θ s i n γ c o s ψ − c o s γ s i n ψ s i n θ − s i n γ s i n ψ − c o s γ c o s ψ s i n θ c o s γ c o s θ ] \begin{bmatrix} cos \gamma cos\psi +sin\gamma sin\psi sin \theta& -cos\gamma sin\psi sin\theta & -sin\gamma cos\theta \\ sin\psi cos\theta&cos\psi cos\theta &sin\theta \\sin\gamma cos\psi -cos\gamma sin\psi sin\theta &-sin\gamma sin\psi -cos\gamma cos\psi sin\theta&cos\gamma cos\theta \end{bmatrix} ⎣⎡cosγcosψ+sinγsinψsinθsinψcosθsinγcosψ−cosγsinψsinθ−cosγsinψsinθcosψcosθ−sinγsinψ−cosγcosψsinθ−sinγcosθsinθcosγcosθ⎦⎤
已知方向余弦求欧拉角
方向余弦矩阵： [ T 11 T 21 T 31 T 12 T 22 T 32 T 13 T 23 T 33 ] \begin{bmatrix} T_{11}&T_{21}&T_{31}\\T_{12}&T_{22}&T_{32} \\T_{13}&T_{23}&T_{33}\end{bmatrix} ⎣⎡T11T12T13T21T22T23T31T32T33⎦⎤
推出：
横滚角： θ = a r c s i n T 32 \theta=arcsinT_{32} θ=arcsinT32
俯仰角：
γ = a r c t a n ( − T 32 T 33 ) \gamma=arctan(-\frac{T_{32}}{T_{33}}) γ=arctan(−T33T32)
偏航角：
ψ = a r c t a n ( T 12 T 22 ) \psi =arctan(\frac {T_{12}}{T_{22}}) ψ=arctan(T22T12)
四元数
基本概念
四元数是由实数和虚数组成的一组超复数，假设Q是一个四元数。
Q = w + xi + yj + zj 其中w表示四元数的实数大小，x,y,z表示虚数大小,实部单位为1，虚部单为为i,j,k。
四元数表征姿态
其中w为实数部分， v ⃗ \vec{v} v为矢量部分，首先讨论如何把一个空间向量转换到四元数。
假设在三维空间中有一个点（x,y,z),拓展到四元数空间有
p ⃗ = [ 0 ( x y z ) ] \vec{p}=[0(x y z)] p=[0(xyz)]
p ⃗ \vec{p} p就是三维空间中一点四元数的表示，而旋转是通过旋转四元数来实现的。现在假设空间中有一个向量 n ⃗ \vec{n} n, ( n x , n y , n z ) (n_x,n_y,n_z) (nx,ny,nz),若用四元数来表示点p(x,y,z)绕向量 n ⃗ \vec{n} n旋转 θ \theta θ角度后 p , p^, p,
定义旋转四元数为 p ⃗ \vec{p} p则对于本次旋转有：
p ⃗ = [ c o s \vec{p}=[cos p=[cos ( θ 2 ) (\frac{\theta}{2}) (2θ) sin ( θ 2 ) (\frac{\theta}{2}) (2θ) n ⃗ \vec{n} n]=[cos ( θ 2 ) (\frac{\theta}{2}) (2θ) . . . sin ( θ 2 ) (\frac{\theta}{2}) (2θ) n x ⃗ \vec{n_x} nx . . . sin ( θ 2 ) (\frac{\theta}{2}) (2θ) n y ⃗ \vec{n_y} ny . . . sin ( θ 2 ) (\frac{\theta}{2}) (2θ) n z ⃗ \vec{n_z} nz ]
(其中.为分隔用)
其中
| n x ⃗ \vec{n_x} nx | n y ⃗ \vec{n_y} ny | n z ⃗ \vec{n_z} nz |
|---|---|---|
| n x ⃗ \vec{n_x} nx= n x n_x nx*i | n y ⃗ \vec{n_y} ny= n y n_y ny*j | n z ⃗ \vec{n_z} nz= n z n_z nz*k |
若想得到旋转后的矩阵只需要执行以下式子
p , ⃗ = q ⃗ p ⃗ q ⃗ − 1 \vec{p^,}=\vec{q}\vec{p} \vec{q}^-1 p,=qpq−1
( q ⃗ − 1 为 q ⃗ 的 逆 矩 阵 ) (\vec{q}^-1为\vec{q}的逆矩阵) (q−1为q的逆矩阵)
想 要 推 导 下 面 的 公 式 需 要 了 解 两 个 概 念 想要推导下面的公式需要了解两个概念 想要推导下面的公式需要了解两个概念
<1四元数的共轭
<2四元数的逆
假设空间中有一个点p(0,1,0)饶z轴逆时针旋转90度，求旋转后的 p , p^, p,点。理论上 p , p^, p,点为（-1,0,0）。
首先把p(0,1,0)拓展成一个四元数 p ⃗ \vec{p} p=(0,0,j,0),然后定义旋转四元数，旋转轴为z轴化为单位向量（0,0,1）,因为旋转角度为90度所以——
q ⃗ = ( c o s 4 5 。 , 0 , 0 , 1 ∗ s i n 4 5 。 k ) = ( 2 2 , 0 , 0 , 2 2 k ) \vec{q}=(cos45^。,0,0,1*sin45^。k)=(\frac{\sqrt 2}{2},0,0,\frac{\sqrt 2}{2}k) q=(cos45。,0,0,1∗sin45。k)=(22,0,0,22k)
单位四元数的逆的共轭是相等的
q ⃗ − 1 = ( 2 2 , 0 , 0 , − 2 2 k ) \vec{q}^-1=(\frac{\sqrt 2}{2},0,0,-\frac{\sqrt 2}{2}k) q−1=(22,0,0,−22k)
由四元数叉乘计算公式 p , ⃗ = q ⃗ p ⃗ q ⃗ − 1 \vec{p^,}=\vec{q}\vec{p} \vec{q}^-1 p,=qpq−1 用上述乘法公式计算
q ⃗ p ⃗ = ( 0 , 2 2 k j , 2 2 j , 0 ) \vec{q}\vec{p}=(0,\frac{\sqrt 2}{2}kj,\frac{\sqrt 2}{2}j,0) qp=(0,22kj,22j,0)
k*j=-i再乘以 q ⃗ − 1 得 到 p ⃗ , = [ 0 , − i , 0 , 0 ] \vec{q}^{-1}得到\vec{p}^{,}=[0,-i,0,0] q−1得到p,=[0,−i,0,0],带回去得到点[-1,0,0]即为旋转之后的坐标。
四元数与方向余弦
由前面的介绍知道了旋转四元数 q = c o s θ 2 + x ∗ s i n θ 2 ∗ i + y ∗ s i n θ 2 ∗ j + z ∗ s i n θ 2 ∗ k q=cos\frac {\theta}{2}+x*sin\frac{\theta}{2}*i+y*sin\frac {\theta}{2}*j+z*sin\frac{\theta}{2}*k q=cos2θ+x∗sin2θ∗i+y∗sin2θ∗j+z∗sin2θ∗k
这里令 q 0 = c o s θ 2 , q 1 = s i n θ 2 , q 2 = s i n θ 2 , q 3 = s i n θ 2 , q_0=cos\frac{\theta}{2},q_1=sin\frac{\theta} {2},q_2=sin\frac{\theta}{2},q_3=sin\frac{\theta}{2}, q0=cos2θ,q1=sin2θ,q2=sin2θ,q3=sin2θ,
q= q 0 + q 1 i + q 2 j + q 3 k q_0+q_1i+q_2j+q_3k q0+q1i+q2j+q3k四元数旋转的方向余弦矩阵公式如下：
[ x b y b z b ] = [ q 0 2 + q 1 2 − q 2 2 − q 3 2 2 ( q 1 q 2 + q 0 q 3 ) 2 ( q 1 q 3 − q 0 q 2 ) 2 （ q 1 q 2 − q 0 q 3 ） q 0 2 − q 1 2 + q 2 2 − q 3 2 2 ( q 0 q 1 + q 2 q 3 ) 2 ( q 1 q 2 − q 0 q 3 ) 2 ( q 2 q 3 − q 0 q 1 ) q 0 2 − q 1 2 − q 2 2 + q 3 2 ] ⋅ [ x n y n z n ] \begin{bmatrix} x_b\\y_b\\z_b\end{bmatrix}=\begin{bmatrix}q_0^2+q_1^2-q_2^2-q_3^2& 2(q_1q_2+q_0q_3)&2(q_1q_3-q_0q_2) \\2（q_1q_2-q_0q_3）&q_0^2-q_1^2+q_2^2-q_3^2&2(q_0q_1+q_2q_3) \\2(q_1q_2-q_0q_3)&2(q_2q_3-q_0q_1)&q_0^2-q_1^2-q_2^2+q_3^2 \\ \end{bmatrix}\cdot\begin{bmatrix}x_n \\y_n \\z_n\end{bmatrix} ⎣⎡xbybzb⎦⎤=⎣⎡q02+q12−q22−q322（q1q2−q0q3）2(q1q2−q0q3)2(q1q2+q0q3)q02−q12+q22−q322(q2q3−q0q1)2(q1q3−q0q2)2(q0q1+q2q3)q02−q12−q22+q32⎦⎤⋅⎣⎡xnynzn⎦⎤
四 元 数 表 示 的 旋 转 矩 阵 公 式 四元数表示的旋转矩阵公式 四元数表示的旋转矩阵公式
姿态表示的三种方法
欧拉角
直观的欧拉角 横滚角roll 俯仰角pitch 偏航角 yaw
方向余弦矩阵
[ c o s γ c o s ψ + s i n γ s i n ψ s i n θ − c o s γ s i n ψ s i n θ − s i n γ c o s θ s i n ψ c o s θ c o s ψ c o s θ s i n θ s i n γ c o s ψ − c o s γ s i n ψ s i n θ − s i n γ s i n ψ − c o s γ c o s ψ s i n θ c o s γ c o s θ ] \begin{bmatrix} cos \gamma cos\psi +sin\gamma sin\psi sin \theta& -cos\gamma sin\psi sin\theta & -sin\gamma cos\theta \\ sin\psi cos\theta&cos\psi cos\theta &sin\theta \\sin\gamma cos\psi -cos\gamma sin\psi sin\theta &-sin\gamma sin\psi -cos\gamma cos\psi sin\theta&cos\gamma cos\theta \end{bmatrix} ⎣⎡cosγcosψ+sinγsinψsinθsinψcosθsinγcosψ−cosγsinψsinθ−cosγsinψsinθcosψcosθ−sinγsinψ−cosγcosψsinθ−sinγcosθsinθcosγcosθ⎦⎤
四元数
[ x b y b z b ] = [ q 0 2 + q 1 2 − q 2 2 − q 3 2 2 ( q 1 q 2 + q 0 q 3 ) 2 ( q 1 q 3 − q 0 q 2 ) 2 （ q 1 q 2 − q 0 q 3 ） q 0 2 − q 1 2 + q 2 2 − q 3 2 2 ( q 0 q 1 + q 2 q 3 ) 2 ( q 1 q 2 − q 0 q 3 ) 2 ( q 2 q 3 − q 0 q 1 ) q 0 2 − q 1 2 − q 2 2 + q 3 2 ] ⋅ [ x n y n z n ] \begin{bmatrix} x_b\\y_b\\z_b\end{bmatrix}=\begin{bmatrix}q_0^2+q_1^2-q_2^2-q_3^2& 2(q_1q_2+q_0q_3)&2(q_1q_3-q_0q_2) \\2（q_1q_2-q_0q_3）&q_0^2-q_1^2+q_2^2-q_3^2&2(q_0q_1+q_2q_3) \\2(q_1q_2-q_0q_3)&2(q_2q_3-q_0q_1)&q_0^2-q_1^2-q_2^2+q_3^2 \\ \end{bmatrix}\cdot\begin{bmatrix}x_n \\y_n \\z_n\end{bmatrix} ⎣⎡xbybzb⎦⎤=⎣⎡q02+q12−q22−q322（q1q2−q0q3）2(q1q2−q0q3)2(q1q2+q0q3)q02−q12+q22−q322(q2q3−q0q1)2(q1q3−q0q2)2(q0q1+q2q3)q02−q12−q22+q32⎦⎤⋅⎣⎡xnynzn⎦⎤
s p a r k − 1 spark-1 spark−1
发布者：全栈程序员-站长，转载请注明出处：https://javaforall.net/205541.html原文链接：https://javaforall.net
