详解互补滤波四元数中向量叉积与陀螺仪角速度补偿问题（Mahony算法）
内容导读
互联网集市收集整理的这篇技术教程文章主要介绍了详解互补滤波四元数中向量叉积与陀螺仪角速度补偿问题（Mahony算法），小编现在分享给大家，供广大互联网技能从业者学习和参考。文章包含8185字，纯文字阅读大概需要12分钟。
内容图文
作者：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>;
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
这段代码中[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="{"spm":"1001.2101.3001.4259"}"></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控制陀螺仪偏差的收敛速率
有人会问，为什么不能直接将预测的重力向量和测量得到的重力向量直接相减呢?上面已经解释了用叉积运算时有相应的物理意义的，二直接相减得到的向量值与角度无关，是不能用来补偿角速度的。
啰啰嗦嗦这么多，到这里就算写完了，若有不当之处，恳请指正！
内容总结
以上是互联网集市为您收集整理的详解互补滤波四元数中向量叉积与陀螺仪角速度补偿问题（Mahony算法）全部内容，希望文章能够帮你解决详解互补滤波四元数中向量叉积与陀螺仪角速度补偿问题（Mahony算法）所遇到的程序开发问题。 如果觉得互联网集市技术教程内容还不错，欢迎将互联网集市网站推荐给程序员好友。
内容备注
版权声明：本文内容由互联网用户自发贡献，该文观点与技术仅代表作者本人。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容， 请发送邮件至 gblab@vip.qq.com 举报，一经查实，本站将立刻删除。
内容手机端
扫描二维码推送至手机访问。
来源：【匿名】