详解互补滤波四元数中向量叉积与陀螺仪角速度补偿问题（Mahony算法）

2023-08-11 13:49:52

作者：Leyvi
时间：2017.1.10

一、归一化与坐标转换

很多做四轴的网友对互补滤波四元数姿态解算代码中的向量叉积和陀螺仪积分补偿问题有疑问，我也查了很多资料，写下这篇博文与大家共同学习。
先放一段互补滤波和四元数姿态解算的代码：

/**
 * 6DOF 互补滤波姿态估计(via Mahony)
 * @param[in] halfT:状态估计周期的一半
 */
const float Kp = 3.5, Ki = 0.05;
float exInt, eyInt, ezInt;
float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // roll,pitch,yaw 都为 0 时对应的四元数值。
void IMUupdate(float gx, float gy, float gz, float ax,float ay, float az)
{
    float norm;
    float vx, vy, vz;
    float ex, ey, ez;
float q0q<span class="hljs-number">0</span> = <span class="hljs-keyword">q</span><span class="hljs-number">0</span><span class="hljs-variable">*q0</span>;
float q0q1 = <span class="hljs-keyword">q</span><span class="hljs-number">0</span><span class="hljs-variable">*q1</span>;
float q0q2 = <span class="hljs-keyword">q</span><span class="hljs-number">0</span><span class="hljs-variable">*q2</span>;
float q1q1 = q1<span class="hljs-variable">*q1</span>;
float q1q3 = q1<span class="hljs-variable">*q3</span>;
float q2q2 = q2<span class="hljs-variable">*q2</span>;
float q2q3 = q2<span class="hljs-variable">*q3</span>;
float q3q3 = q3<span class="hljs-variable">*q3</span>;

<span class="hljs-keyword">if</span>(ax<span class="hljs-variable">*ay</span><span class="hljs-variable">*az</span>==<span class="hljs-number">0</span>)
    <span class="hljs-keyword">return</span>;

<span class="hljs-regexp">//</span> 第一步：对加速度数据进行归一化
norm = <span class="hljs-keyword">sqrt</span>(ax<span class="hljs-variable">*ax</span> + ay<span class="hljs-variable">*ay</span> + az<span class="hljs-variable">*az</span>); 
ax = ax / norm; 
ay = ay / norm; 
az = az / norm; 

<span class="hljs-regexp">//</span> 第二步：DCM矩阵旋转
vx = <span class="hljs-number">2</span><span class="hljs-variable">*(</span>q1q3 - q0q2); 
vy = <span class="hljs-number">2</span><span class="hljs-variable">*(</span>q0q1 + q2q3); 
vz = q0q<span class="hljs-number">0</span> - q1q1 - q2q2 + q3q3 ;

<span class="hljs-regexp">//</span> 第三步：在机体坐标系下做向量叉积得到补偿数据
ex = ay<span class="hljs-variable">*vz</span> - az<span class="hljs-variable">*vy</span> ;
ey = az<span class="hljs-variable">*vx</span> - ax<span class="hljs-variable">*vz</span> ;
ez = ax<span class="hljs-variable">*vy</span> - ay<span class="hljs-variable">*vx</span> ;

<span class="hljs-regexp">//</span> 第四步：对误差进行PI计算，补偿角速度
exInt = exInt + ex * Ki;
eyInt = eyInt + ey * Ki;
ezInt = ezInt + ez * Ki;

gx = gx + Kp<span class="hljs-variable">*ex</span> + exInt;
gy = gy + Kp<span class="hljs-variable">*ey</span> + eyInt;
gz = gz + Kp<span class="hljs-variable">*ez</span> + ezInt;

<span class="hljs-regexp">//</span> 第五步：按照四元数微分公式进行四元数更新
<span class="hljs-keyword">q</span><span class="hljs-number">0</span> = <span class="hljs-keyword">q</span><span class="hljs-number">0</span> + (-q1<span class="hljs-variable">*gx</span> - q2<span class="hljs-variable">*gy</span> - q3<span class="hljs-variable">*gz</span>)<span class="hljs-variable">*halfT</span>;
q1 = q1 + (<span class="hljs-keyword">q</span><span class="hljs-number">0</span><span class="hljs-variable">*gx</span> + q2<span class="hljs-variable">*gz</span> - q3<span class="hljs-variable">*gy</span>)<span class="hljs-variable">*halfT</span>;
q2 = q2 + (<span class="hljs-keyword">q</span><span class="hljs-number">0</span><span class="hljs-variable">*gy</span> - q1<span class="hljs-variable">*gz</span> + q3<span class="hljs-variable">*gx</span>)<span class="hljs-variable">*halfT</span>;
q3 = q3 + (<span class="hljs-keyword">q</span><span class="hljs-number">0</span><span class="hljs-variable">*gz</span> + q1<span class="hljs-variable">*gy</span> - q2<span class="hljs-variable">*gx</span>)<span class="hljs-variable">*halfT</span>;

norm = <span class="hljs-keyword">sqrt</span>(<span class="hljs-keyword">q</span><span class="hljs-number">0</span><span class="hljs-variable">*q0</span> + q1<span class="hljs-variable">*q1</span> + q2<span class="hljs-variable">*q2</span> + q3<span class="hljs-variable">*q3</span>);
<span class="hljs-keyword">q</span><span class="hljs-number">0</span> = <span class="hljs-keyword">q</span><span class="hljs-number">0</span>/norm;
q1 = q1/norm;
q2 = q2/norm;
q3 = q3/norm;

roll =  atan2f(<span class="hljs-number">2</span><span class="hljs-variable">*q2</span><span class="hljs-variable">*q3</span> + <span class="hljs-number">2</span><span class="hljs-variable">*q0</span><span class="hljs-variable">*q1</span>, -<span class="hljs-number">2</span><span class="hljs-variable">*q1</span><span class="hljs-variable">*q1</span> - <span class="hljs-number">2</span><span class="hljs-variable">*q2</span><span class="hljs-variable">*q2</span> + <span class="hljs-number">1</span>)<span class="hljs-variable">*57</span>.<span class="hljs-number">3</span>;     
pitch =  asinf(<span class="hljs-number">2</span><span class="hljs-variable">*q1</span><span class="hljs-variable">*q3</span> - <span class="hljs-number">2</span><span class="hljs-variable">*q0</span><span class="hljs-variable">*q2</span>)<span class="hljs-variable">*57</span>.<span class="hljs-number">3</span>;                                                          
yaw  =  -atan2f(<span class="hljs-number">2</span><span class="hljs-variable">*q1</span><span class="hljs-variable">*q2</span> + <span class="hljs-number">2</span><span class="hljs-variable">*q0</span><span class="hljs-variable">*q3</span>, -<span class="hljs-number">2</span><span class="hljs-variable">*q2</span><span class="hljs-variable">*q2</span> -<span class="hljs-number">2</span><span class="hljs-variable">*q3</span><span class="hljs-variable">*q3</span> + <span class="hljs-number">1</span>)<span class="hljs-variable">*57</span>.<span class="hljs-number">3</span>;

}

这段代码中[ax,ay,az]是加速度计测量得到的重力向量，然后对其归一化，但是为什么要对它归一化处理呢？不是画蛇添足，而是后面大有用途！
[vx,vy,vz]是世界坐标系重力分向量[0,0,1]经过DCM旋转矩阵计算得到的机体坐标系中的重力向量,即

图1中盒子模型中的小球处于失重状态，坐标系与世界坐标系重合。
图2中盒子坐标系仍与世界坐标系重合，描述为[x,y,z],不同的是小球收到重力作用，盒子模型坐标系中重力向量为[0,0,1],世界坐标系中也是[0,0,1].
图3中盒子在世界坐标系中与地面夹角为45度，但是盒子坐标系仍是[x,y,z]，重力向量变为了[-0.71,0,-0.71]，注意这个向量是盒子模型坐标系的向量值，转换到世界坐标系仍是[0,0,1].
所以无论盒子怎么旋转重力向量在世界坐标系中都是[0,0,1],但是我们有加速度计读到的值是基于盒子模型坐标系的，也就是我们所说的机体坐标系。现在就明白了，刚才为什么要用四元数将[0，0，1]乘一个旋转矩阵，我们之后关于加速度计和陀螺仪的计算都是基于机体坐标系的。

二、叉乘运算

重点来了，我们得到了加速度计测得的[ax,ay,az]，还有机体坐标系标准的重力向量，下面我们就要对它们做叉积运算：
首先我们先来一点向量叉积的预备知识

由上面知识我们得到了下面的计算矩阵：
⎡⎣⎢exeyez⎤⎦⎥=⎡⎣⎢0az−ay−az0axay−ax0⎤⎦⎥⎡⎣⎢vxvyvz⎤⎦⎥[exeyez]=[0−azayaz0−ax−ayax0][vxvyvz]
最后得到PI滤波计算公式

    exInt = exInt + ex * Ki;
    eyInt = eyInt + ey * Ki;
    ezInt = ezInt + ez * Ki;
gx = gx + Kp<span class="hljs-variable">*ex</span> + exInt;
gy = gy + Kp<span class="hljs-variable">*ey</span> + eyInt;
gz = gz + Kp<span class="hljs-variable">*ez</span> + ezInt;<div class="hljs-button {2}" data-title="复制" data-report-click="{&quot;spm&quot;:&quot;1001.2101.3001.4259&quot;}"></div></code><ul class="pre-numbering" style=""><li style="color: rgb(153, 153, 153);">1</li><li style="color: rgb(153, 153, 153);">2</li><li style="color: rgb(153, 153, 153);">3</li><li style="color: rgb(153, 153, 153);">4</li><li style="color: rgb(153, 153, 153);">5</li><li style="color: rgb(153, 153, 153);">6</li><li style="color: rgb(153, 153, 153);">7</li></ul></pre>

其中：
1、比例增益kp控制收敛到加速度计速率
2、积分增益ki控制陀螺仪偏差的收敛速率
有人会问，为什么不能直接将预测的重力向量和测量得到的重力向量直接相减呢?上面已经解释了用叉积运算时有相应的物理意义的，二直接相减得到的向量值与角度无关，是不能用来补偿角速度的。
啰啰嗦嗦这么多，到这里就算写完了，若有不当之处，恳请指正！

码农公寓

一、归一化与坐标转换

二、叉乘运算

相关文章