姿态解算原理(一)——旋转矩阵

像我们常见的MPU6050、MPU9250等等都是一种捷联式的惯性元件,还是一种低成本的,还有一种是平台式的惯性导航,不过我们能够用得起的就是低成本的MEMS惯性元件,本文的内容是姿态解算的原理
进入正题,姿态解算原理很简单,我们需要一种传感器感知刚体的旋转运动,而陀螺仪就是一种这样的元件,它用来感知惯性空间的角速度,有了角速度自然就可以积分得到角度,一维的就是这样,我们直接积分就好了,而二维的、三维的我们同样可以积分,不过这就涉及到矩阵运算了。
下面展示的是旋转矩阵进行坐标转换,假设我们在三维坐标系下旋转了绕Z轴旋转了一个角度

原来是X1-Y1-Z1坐标系,绕Z轴旋转α角度之后就是X2-Y2-Z2坐标系了,假设在原来的坐标系上有一个向量OF,我们怎么来描述OF经过旋转之后的变化呢?我们就可以通过旋转矩阵来实现,其实就是对向量进行了线性变换,得到了一个新的向量,模不变,但是基底变了
姿态解算原理(一)——旋转矩阵_第1张图片
如上图,通过计算,高中的数学基础就可以得到,我们可以得到F在两个坐标系下之间的关系,写成矩阵形式:
姿态解算原理(一)——旋转矩阵_第2张图片
这就是绕z轴旋转α的旋转矩阵,可以看出是正交矩阵,因此它的逆就是它的转置,如果我们再分别得到绕X、Y轴的姿态矩阵,就可以得到绕三轴任意旋转的姿态矩阵了,其中绕X轴的为俯仰角pitch,写为θ,绕Y轴的为滚转角roll,写作γ,绕Z轴的为航向角yaw,写作φ

姿态解算原理(一)——旋转矩阵_第3张图片
写出来就是这样,之后我们再将三个矩阵相乘,得到姿态矩阵姿态解算原理(一)——旋转矩阵_第4张图片

有了姿态矩阵,我们可以看出,姿态矩阵是用三个姿态角表示的,通过姿态矩阵,我们可以反求出姿态角

你可能感兴趣的