最开始接触姿态结算的时候,四元数、欧拉角和方向余弦矩阵的相互转化便是在进阶路上的一条小老虎,看的多了,终于也算熟悉了。
引用别人的“凡是把一本书读100遍的人,没有一个人不成大器”这句话,希望对大家有所勉励。
基础理论
对于在三维空间里的一个参考系,任何坐标系的取向,都可以用三个欧拉角
来表现,一个与参考系重合的坐标系,通过绕 Z 轴的偏航角(Yaw),绕 Y 轴的
俯仰角(Pitch)和绕 X 轴的横滚角(Roll)旋转到当前姿态,常用坐标系如下:
![5154aa2a98b337ab173cf915792730aa.png](https://i-blog.csdnimg.cn/blog_migrate/5d889091f909e0fbec4f63c0c90579d1.jpeg)
一、方向余弦矩阵 (参考《捷联惯性导航技术》张天光等)
假定初始时刻载体坐标系和世界坐标系重合, 那么任一向量在载体坐标系和世界坐标系中的表达式相同, 即
推导如图:
![b17a795d8ae999a9bf82728906628a00.png](https://i-blog.csdnimg.cn/blog_migrate/5358742a6d7e133d0612a72e05598202.jpeg)
再绕 Y 轴旋转角度
再绕 X 轴旋转角度
此时
因此从世界坐标系到载体坐标系的变换矩阵可由下式给出
那么,从载体坐标系到世界坐标系的转变矩阵可以表示如下:
此外,注意到当姿态角很小的时候,
二、欧拉角与四元数
按Z-Y-X的旋转变换顺序有:
四元数与旋转矩阵(常用来做坐标变换)
2.1 b系转到R系的坐标变换矩阵
2.2 R系转到b系的坐标变换矩阵
对比R和
三、四元数的更新
Q对时间求导数:
因为
所以
所以
代码实现:
void