在给出位姿解算的方法之前,我们先来看看四元数所表达的意义,和正交矩阵的关系。
我们设有:
单位四元数:$q=cos\frac{\theta}{2}+(ai+bj+ck)sin\frac{\theta}{2}=q_0+q_1i+q_2j+q_3k$ 其中,$a^2+b^2+c^2=1$
纯四元数:$p=xi+yj+zk$
则,经过旋转变化可以得到一个新的纯四元数。这里我们需要知道,四元数的数乘运算表达旋转的过程,具体可以参照四元数的数学理论。
那么,有:$p_2=qp_1q^{-1}$ 这里,$q^{-1}$是四元数q的逆,就是$q^{-1}=q_0-q_1i-q_2j-q_3k$,当$|q|=1$时,也有$q^{-1}=q^*$也就是共轭。
于是,有:$$\left\lgroup \matrix{x_2 \cr y_2 \cr z_2} \right\rgroup = M(q) \left\lgroup \matrix{x_1 \cr y_1 \cr z_1} \right\rgroup=\left\lgroup \matrix{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) \cr 2(q_1q_2+q_0q_3)&q_0^2-q_1^2+q_2^2-q_3^2&2(q_2q_3-q_0q_1) \cr 2(q_1q_3-q_0q_2)&2(q_2q_3+q_0q_1)&q_0^2-q_1^2-q_2^2+q_3^2} \right\rgroup \left\lgroup \matrix{x_1 \cr y_1 \cr z_1} \right\rgroup$$
同样,也可以有:$p_0=q^{-1}p_1q$表示为:
$$\left\lgroup \matrix{x_0 \cr y_0 \cr z_0} \right\rgroup = M(q)^T \left\lgroup \matrix{x_1 \cr y_1 \cr z_1} \right\rgroup=\left\lgroup \matrix{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) \cr 2(q_1q_2-q_0q_3)&q_0^2-q_1^2+q_2^2-q_3^2&2(q_2q_3+q_0q_1) \cr 2(q_1q_3+q_0q_2)&2(q_2q_3-q_0q_1)&q_0^2-q_1^2-q_2^2+q_3^2} \right\rgroup \left\lgroup \matrix{x_1 \cr y_1 \cr z_1} \right\rgroup$$这里的$p_0、p_1、p_2$表示$p_0、p_1$分别进行q变换得到$p_1、p_2$
与变换矩阵的关系
我们将单位四元数的表达式带入$M(q)$中,可以得到:$$M(q)=\left\lgroup \matrix{cos\theta+a^2(1-cos\theta) & ab(1-cos\theta)-csin\theta & bsin\theta + ac(1-cos\theta) \cr ab(1-cos\theta)+csin\theta & cos\theta+b^2(1-cos\theta) & -asin\theta+bc(1-cos\theta) \cr -bsin\theta+ac(1-cos\theta) & asin\theta+bc(1-cos\theta) & cos\theta+c^2(1-cos\theta)} \right\rgroup$$
单位四元数的变换等效于按顺序绕x、y、z轴转动$X、Y、Z$角度所产生的变换矩阵
也就是说:
因此,才有了反求出的欧拉角:
$$Roll=X=atan\frac{m_{21}}{m_{22}}$$
$$Pitch=Y=-asin(m_{20})$$
$$Yaw=Z=atan\frac{m_{10}}{m_{00}}$$
$m_{ij}$表示矩阵当中第i行第j列的元素,,当然这里的RollPitchYaw是对于特定坐标系的,详细查看这里
接下来我们给出四元数法的基本算法:
初始化:
$K_p = 100;$#PI控制的P调节
$K_i = 0.02;$#PI控制的I调节
$T = 0.001;$#采样周期
$q_0=1;q_1=q_2=q_3=0;$#初始四元数假设
循环主体:
①传感器测量值,在机体坐标系下,测量到世界坐标系下绝对矢量——重力加速度g的姿态方向矢量。
$norm=sqrt(a_x^2+a_y^2+a_z^2);$#计算模
$a_x /= norm$
$a_y /= norm$
$a_z /= norm$#归一化,计算出当前机体坐标系下重力的方向
②状态估计值,在机体坐标系下,通过$Q\rightarrow M$关系结算的世界坐标系下绝对矢量——重力加速度g的姿态方向矢量。
$V_x=2(q_1q_3-q_0q_2)$
$V_y=2(q_0q_1+q_2q_3)$
$V_z=q_0^2-q_1^2-q_2^2+q_3^2$ \计算重力在当前假设坐标系下的方向,其实就是$M(q)$矩阵当中的最后一列,也就是它与$\left\lgroup \matrix{0 \cr 0 \cr 1} \right\rgroup$相乘
③获取同一坐标系下的测量值与估计值误差。
$error = a \times V$ \获得两坐标系对待同一重力的方向描述的误差
④通过PI互补滤波器,将姿态误差补偿到角速度上,修正角速度积分漂移。
$exInt = error_x·K_i$ \ 积分误差反馈
$eyInt = error_y·K_i$
$ezInt = error_z·K_i$
$g_x += K_p·error_x + exInt$
$g_y += K_p·error_y + eyInt$
$g_z += K_p·error_z + ezInt$#为保持假设坐标系能够随机体转动而转动,同时接受误差的作用,从而获得当前假设坐标系应有的转动变换
⑥通过一届龙格库塔(欧拉求积)求解四元数微分方程,即四元数方向空间经过修正角速度作用发生旋转过程的积分,直接数乘以g相当于对t求过导数了,具体的看这里,这个博客写的挺细的了。
$q_0 += \frac{T}{2}(-q_1g_x-q_2g_y-q_3g_z)$
$q_1 += \frac{T}{2}(q_0g_x+q_2g_z-q_3g_y)$
$q_2 += \frac{T}{2}(q_0g_y-q_1g_z+q_3g_x)$
$q_3 += \frac{T}{2}(q_0g_z+q_1g_y-q_2g_x)$#四元数更新,也是更新当前假设坐标系
⑦
归一化四元数
⑧位姿解算,获得机体坐标系顺次绕轴x、y、z角度Roll、Pitch、Yaw后得到世界坐标系过程的欧拉角: $57.3=\frac{180}{\pi}$
$Yaw=atan\frac{2q_2q_2-2q_0q_3}{2q_0^2+2q_1^2-1}\times57.3$
$Roll=atan\frac{2q_1q_0+2q_2q_3}{2q_0^2+2q_3^2-1}\times57.3$
$Roll=-asin(2q_1q_3-2q_0q_2)\times57.3$