上一节我们使I2C总线获取了MPU6050的三轴加速度、三轴角速度,并且介绍了一种简单的初始状态校准方法;这一节我们继续,在已有的底层驱动基础上,移植MPU6050芯片官方的DMP库,来获取角度信息。
1)姿态角计算的一些基础知识
在上一节中,我们初始化时约定了Z轴向上,也即知道了初始的角度;再加上后续获取的实时的角速度,利用角速度进行积分,就可以获取姿态角信息。
在实际编程实现时,积分就是累加和,随着时间的增加,积分(累加)的数据越来越多,误差也会越来越大。因此,单独使用陀螺仪的角速度去计算得到姿态角是不行的,误差会越来越大,需要使用加速度计的值来修正。
为什么加速度计的值可以来修正角度呢?我们知道,加速度计能测到重力加速度的方向,只要飞行器的运动不算太剧烈,就能近似地认为在一段时间里,加速度计的测量值只有重力,而且姿态角的变化只转过了很小的一个角。
我们仍以下图为例,假定初始时Z轴向上,绕X轴旋转的角度为滚转角;绕Y轴旋转的角度为俯仰角,绕Z轴旋转的角度为偏航角。
那么,滚转角(绕X轴转)和俯仰角(绕Y轴转)发生变化时,MPU6050测出来的重力加速度g的方向也会发送变化。而加速度计测出来的重力加速度g的方向是实时的,不会受长时间计算累加的影响。所以,我们可以用加速度计测到的g的方向,来修正陀螺仪积分得到的滚转角和俯仰角。
而偏航角(绕Z轴转)变化时,MPU6050测到的重力加速度g方向是不会变化的,所以g的方向不能用来修正偏航角。要想修正偏航角,需要测量地磁偏角的传感器信息。