姿态解算

2017-08-06 本文已影响0人 Zerxoi

介绍

实际上就是获取物体绕3个轴旋转的角度（欧拉角),Roll(x),Pitch(y),Yaw(z)。

Flight_dynamics

IMU(Inertial measurement unit)

陀螺仪

陀螺仪可以感知物体在旋转时绕各轴转的角速度，精度虽然可靠，积分存在漂移现象。

加速度计

加速度计感知物体所受的加速度在各轴上的分量，物体静止时则只感知竖直向下的重力加速度，可以根据重力加速度在各轴分量求解欧拉角，但如果存在外力干扰则无法求解。

磁力计

磁力计与加速度计相似，但是感知的不是加速度，而是地磁场方向在各轴上的分量。由于不同地区的地磁场方向不同，参照向量并不像加速度计一样方便选取，因此不能直接用于求解姿态角，同时也使得校准很麻烦。又因为地磁场很小，因此地磁计易受外界磁场干扰。

数学工具

数学不好，不会走远，我对此深信不疑。

坐标系

地理坐标系
地球坐标系又叫做地理坐标系，是固定不变的。正北，正东，正向上构成了这个坐标系的X，Y，Z轴，我们用坐标系N表示。

物体坐标系
待测物体上固定着一个坐标系，我们一般称之为物体坐标系。

四元数

表示3维向量在空间中的旋转
链接：
[1].Understanding Quaternions
[2].Quaternion
[3].Transformations using Quaternions

旋转矩阵

表示任意维度向量的线性变换
链接：
[1].线性代数的本质

一阶龙格－库塔法

链接：
[1].Runge–Kutta methods

分析

概括

在理想情况下，完全可以使用陀螺仪获得3个欧拉角，但是因为漂移现象的存在，这并不现实；因此需要加速度计修正陀螺仪数据，从而消除漂移，但是加速度计无法感知Z轴的旋转；因此需要地磁计来获取绕Z轴的旋转角度。

四元数和旋转矩阵都能表示对3维空间向量的旋转。对于同一个变换，若将二者用同一形式表示（无论是四元数转换成旋转矩阵，还是旋转矩阵变成四元数），二者相同。
向量

$\vec{v}=(x,y,z)$
四元数
$\ q=(q_1,q_2,q_3,q_4)=(\cos{\frac{\theta}{2}},\sin{\frac{\theta}{2}}*vx,\sin{\frac{\theta}{2}}*vy,\sin{\frac{\theta}{2}}*vz)$
$\ q^{-1}=(\cos{\frac{\theta}{2}},-\sin{\frac{\theta}{2}}*vx,-\sin{\frac{\theta}{2}}*vy,-\sin{\frac{\theta}{2}}*vz)$
向量纯四元数形式
$\ p=(0,x,y,z)$
运算
$\ p_{out}=q*p*q^{-1}$

Step 1

静止时（无外力干扰），根据我提坐标系上的重力加速度分量与世界坐标系的关系获取初始欧拉角，再将欧拉角转换成四元数。
参考资料：
Conversion Euler to Quaternion
代码：

Roll=atan2(Ay,-Az);
Pitch=atan2(-1*Ax,-1*Az);
Yaw=0;

Q0=cos(0.5 * Roll) * cos(0.5 * Pitch) * cos(0.5 * Yaw) + sin(0.5 * Roll) * sin(0.5 * Pitch) * sin(0.5 * Yaw);
Q1=sin(0.5 * Roll) * cos(0.5 * Pitch) * cos(0.5 * Yaw) - cos(0.5 * Roll) * sin(0.5 * Pitch) * sin(0.5 * Yaw);
Q2=cos(0.5 * Roll) * sin(0.5 * Pitch) * cos(0.5 * Yaw) + sin(0.5 * Roll) * cos(0.5 * Pitch) * sin(0.5 * Yaw);
Q3=cos(0.5 * Roll) * cos(0.5 * Pitch) * sin(0.5 * Yaw) -sin(0.5 * Roll) * sin(0.5 * Pitch) * cos(0.5 * Yaw);

Step 2

注释
Q:四元数
eg.[q_w,q_x,q_y,q_z]^T
V:向量
eg1. [x,y,z]^T
eg2.G [0,0,1]^T
C:旋转矩阵
Q_v: 向量V的四元数形式
三维向量[x,y,z]^T的纯四元数形式为[0,x,y,z]^T
C_q:四元数的旋转矩阵形式
V_b:物体坐标系下的向量V
V_n:世界坐标系下的向量V

$C_n^b$
:从世界坐标系转换到物体坐标系的旋转矩阵
$C_b^n$ :从物体坐标系转换到世界坐标系的旋转矩阵

将下式转化成矩阵形式
Q * Q_v * Q^-1
C_q * V_b = Cⁿ * V_b = G

[1].Transformations using Quaternions
[2].Conversion Quaternion to Matrix

01: $sh(x)=\frac{e^x+e^{-x}}{2}}$ 02: $C_n^k$ 03: $\begin{align}\sqrt{37}&=\sqrt{\frac{73^2-1}{12^2}}\&=\sqrt{\frac{73^2}{12^2}\cdot\frac{73^2-1}{73^2}}\&=\sqrt{\frac{73^2}{12^2}}\sqrt{\frac{73^2-1}{73^2}}\&=\frac{73}{12}\sqrt{1-\frac{1}{73^2}}\&\approx\frac{73}{12}\left(1-\frac{1}{2\cdot73^2}\right)\end{align}$ 04: $\begin{array}{c|lcr}n&\text{Left}&\text{Center}&\text{Right}\\hline1&0.24&1&125\2&-1&189&-8\3&-20&2000&1+10i\\end{array}$ 05: $\mathbb{N,Z,Q,R,C}$ 06: $\left{\begin{array}{ll}a_1x+b_1y+c_1z&=d_1+e_1\a_2x+b_2y&=d_2\a_3x+b_3y+c_3z&=d_3\end{array}\right.$

07: ![][07]

[07]: http://latex.codecogs.com/svg.latex?f\left(\left[\frac{1+\left{x,y\right}}{\left(\frac{x}{y}+\frac{y}{x}\right)\left(u+1\right)}+a\right]^{3/2}\right)