ICM2060姿态解算过程与互补滤波算法提高测量精度原理(一)

华龙宇

已于 2024-09-11 23:14:31 修改

阅读量989

点赞数 18

文章标签：算法单片机 stm32 智能硬件

于 2024-09-11 00:50:35 首次发布

本文链接：https://blog.csdn.net/nanmenyiguo/article/details/142070966

版权

1、坐标系(描述物体各各点的位姿关系的重要参照):

2、欧拉角(描述机体绕坐标系三个坐标轴(x,y,z轴）的旋转角度)

        本篇是作者在使用ICM2060获得精确的Roll(翻滚角)，Pitch(俯仰角)。由于ICM2060是集成了3轴加速度计与3轴陀螺仪的模块，而加速度计没办法精确感知Yaw(偏航角)的变化，所以仅靠ICM2060模块只能获得精准的Roll，Pitch的数据。如果想要获取精准的Yaw角，则还需要依靠其他模块来辅助计算，如磁力计。
        如果读者看不懂这里的介绍也没关系，接下来我会按照（我的习惯）以问题引导的方式来逐一带大家来了解使用ICM2060进行姿态解算的过程的。
        姿态解算实际上是分两个部分，姿态与解算。“姿态”是指物体当前姿势与原始姿势的相对状态；“解算”则是指根据机体上采集的原始数据计算出当前的姿态。接下来让我们循序渐进的来了解这个过程：

1、坐标系(描述物体各各点的位姿关系的重要参照):

一般我们描述一个物体的当前位置肯定都是基于起点的初始坐标系来描述的，而在本文姿态解算中都默认以地理方位中的北东地坐标系为起点坐标系。

北东地坐标系定义为：X轴：指向正北；Y轴：指向东向；Z轴：与XY轴形成右手坐标系，指向地向。

有了描述机体位置的坐标系后，我们还需要一个描述机体上各各物体在机体什么位置的坐标系，我们称其为机体(载)坐标系，各轴定义如下：

2、欧拉角(描述机体绕坐标系三个坐标轴(x,y,z轴）的旋转角度)

欧拉角（Euler Angles）是用来描述刚体在三维空间中的姿态的三组角度。通常，欧拉角采用三个旋转角度来表示一个刚体的姿态，这些角度分别是绕特定轴的旋转，常见的定义有绕 Z 轴、Y 轴和 X 轴的旋转，称为 航向角（Yaw）、俯仰角（Pitch） 和 滚转角（Roll）。具体定义如下：

滚转角（Roll，绕 X 轴旋转）： 滚转角描述物体围绕自身前进方向的旋转。例如，飞机在飞行中左右倾斜的动作就是由滚转角决定的。
俯仰角（Pitch，绕 Y 轴旋转）： 俯仰角描述物体围绕侧向轴（左右方向）的旋转，类似于飞机抬头或低头时的运动。
航向角（Yaw，绕 Z 轴旋转）： 航向角描述物体绕垂直于地面的轴旋转，类似于飞机改变飞行方向时的旋转。

3、欧拉角的表示旋转的方法：

欧拉角的表示方式有不同的旋转顺序，最常用的一种是 Z-Y-X 顺序，即：航向角（Yaw）： 绕 Z 轴旋转，俯仰角（Pitch）： 在旋转后的参考系中绕 Y 轴旋转，滚转角（Roll）： 在进一步旋转后的参考系中绕 X 轴旋转，注意不同顺序的欧拉角定义会导致不同的姿态描述，示例：

所以使用欧拉角时一定要确定一个唯一的旋转次序，本文则默认采用X—Y—Z的旋转次序，之后便不再叙述了。

4、方向旋转矩阵以及方向余弦的推导：

为了便于计算向量从起始坐标系到最终位置的旋转关系，我们可以引入线性代数中的矩阵概念。简单介绍可以理解为，想要找到一个万能的函数F(x)，使得我们能在已知任意一个点的初始坐标后，通过F(x)函数运算，准确找到旋转后的位置。那么我们如何找到这个万能函数呢？

组合旋转：为了表示物体在三维空间中的整体旋转，可以将这些矩阵按照一定顺序相乘。常见的组合方式是 X-Y-Z 旋转顺序，即依次绕 Z 轴、Y 轴、X 轴旋转对应的角度。组合旋转矩阵(矩阵乘法是从右往左的)为：

根据旋转矩阵，我们很容易就知道了如何通过旋转矩阵计算欧拉角，以偏航角计算为例：（为例描述方便，简化了矩阵，使用符号取代每一位）：

细心的你在看到上面的公式，肯定会想到以下问题：

1.奇异性问题(万向节死锁)：

2.大量的三角运算以及自变量与代求量相同：大量的三角函数计算会使计算难度陡然上升，计算时间变长不利于实时获取数据，同时由于我们本身就是为了求解这三个角度，但这三个角度却在上面的公式中成为自变量，这明显不符合逻辑。为此我们需要一种新的计算方式大名鼎鼎的四元数来解决这些问题。

5、四元数的引入：

由于篇幅问题，我们直接就是介绍一下四元数的相关结论，就不去探究它的原理了，这方面的知识网上也是比较好找的。
四元数（Quaternion）是一种扩展的复数，用来表示三维空间中的旋转。它由威廉·哈密顿在1843年引入，并广泛应用于计算机图形学、机器人学和航空航天等领域，因其能有效避免万向节死锁问题而受到青睐。

四元数的优点：

1. 避免万向节死锁：欧拉角在某些旋转角度组合时会发生万向节死锁，导致丧失一个旋转自由度，而四元数不会发生这种问题。它通过四个参数来描述旋转，可以连续、无奇异性地表示任意三维旋转。
2. 插值平滑性：四元数在进行旋转插值时，比欧拉角或旋转矩阵更加平滑。常用的球形线性插值（SLERP）可以在不同旋转状态间产生平滑过渡，特别适合动画和航天器姿态控制等场景。
3. 计算效率：四元数的乘法可以用来组合旋转，计算效率较高，尤其在处理连续旋转和复杂旋转时，比旋转矩阵更简洁。

6、四元数表示的方向余旋矩阵：

想要使用四元数来表达物体旋转的关系（也就是机体坐标系与初始坐标系之间的变换关系），我们就需要找到四元数与坐标变换的关系。好在由前人的探索为我们发现这座桥梁，即罗德里格旋转公式：，其中 I 是 3x3 单位矩阵，K 是由旋转轴定义的反对称矩阵：

我们将四元数代入，通过一些数学计算，就可以得到本章节最为重要的一个知识点了，即四元数版的方向余弦旋转矩阵，以及前人计算得出的通过四元数旋转矩阵转换为欧拉角的公式

对于这个四元数转换为欧拉角的公式本人认为为了便于理解可以这样解释：我们在回想起之前1.4小节，提到的欧拉角版本的方向余弦旋转矩阵，我们从公式的角度来讲，由于这两个矩阵在规定都表示从机体到地理坐标系的旋转关系时，其每一个位的元素相互对应，所以欧拉角可以利用反三角函数来计算出角度大小，同样也对四元数版本的旋转矩阵来说有用（可以理解为，每一个位置的元素体现形式不一样了，但元素与元素之间的运算关系任然存在）。

自此我们已经解决了如何使用四元数来规避使用欧拉角计算带来问题的同时，又能利用四元数与欧拉角的转换关系进行随时转换。因为欧拉角描述姿态更加直观，四元数进行数学计算更加高效。

7、四元数的求解与更新：

        根据前面的介绍，我们已经知道了如何利用四元数来计算欧拉角的大小，那么这样一来我们就需要解决最后一个问题，即如何算出四元数以及如何更新使其能实时表达角度。
        我们使用的 ICM2060 是一个六轴运动跟踪装置，结合了三轴陀螺仪和三轴加速度计。陀螺仪提供的是各轴的角速度数据。根据角速度与角度之间的关系，角速度的积分等于角度。因此，为了使用四元数来计算角度，我们需要将四元数与角速度结合起来。
我们已知旋转四元数的表达公式为（由来可以不必深究，当成定理即可）：
        可以看到它有一个角度变量，如果我们让这个式子对时间 t 求微分，经过一些列复杂的数学运算后得到：

这时我已经建立起了四元数与角速度之间的关系了。不过此时这个式子代表的是，旋转四元数的导数表示了四元数随时间的变化率，即使我们有四元数的导数公式，也必须通过数值积分方法来计算其在不同时刻的值。为此龙格库塔法等数值解法提供了这种积分的手段。

将两个式子融合可以得到：( Δt 表示四元数更新周期)

8、问题启发(互补滤波算法的引入):

        自此通过本篇文章的介绍，我们已经基本了解了，我们是如何使用ICM2060采集到的角速度数据来计算欧拉角，描述机体姿态。通过结尾的公式，让我们知道了只要有角速度大小即可完成后续的所有操作，但是如果你的角速度数据不总是那么的精准的话，就会导致你之后的所有计算结果出现偏移，导致功亏一篑。
        可能在这里你会想我直接用一个非常精准的测量模块不就行了吗。可惜在这里小编就得让各位走进现实了，在现实生产中我们是不可能做到0误差的模块的，虽然能做到极小，但是问题在于市面上计算位姿用的主要模块是靠陀螺仪，而他反馈的是角速度信息，而角速度转化为角度需要对时间进行积分，这就导致即便是再小的误差，在时间的慢慢积累下也终将铸成大错。
        出于篇幅考虑，我打算将这部分问题放到下一篇文章进行分析，带大家了解互补滤波算法使用以及代码实现。并且我也会尽快更新的，如果以上有不足之处可以私信也可以发在评论区进行讨论，我会第一时间进行修改的。
ICM2060姿态解算过程与互补滤波算法提高测量精度原理(二)