1.几个基本概念。
地理坐标系:北东地。北为X轴、东为Y轴、地为Z轴。
机体坐标系:见下图。
姿态角:我的理解是,roll角是Zb在ZOY平面与Z轴的夹角。pitch角是Zb在ZOX平面与Z轴的夹角。yaw角是Xb在XOY平面与X轴的夹角。
旋转矩阵:地理坐标系到机体坐标系的转换矩阵。旋转矩阵为正交阵,逆即转置。
传感器:都是基于机体坐标系的。加速度计测三轴的加速度,磁力计测三轴的磁感应强度,陀螺仪测三轴的角速度。注意角速度的定义,方向是沿着三轴的。得到姿态角主要靠陀螺仪测得的角速度积分,但存在漂移误差,因此要用加速度计和磁力计随时校正。
很重要的:在地理坐标系下,重力加速度坐标始终为(0,0,1g),磁场坐标为(M,0,D)。在地球表面,磁场始终是沿着磁感应线指北的,某点的磁场强度为磁感应线在该点的切线,它在北方有个分量,在垂直方向有个分量,在Y轴是没有分量的,即(M,0,D)。这里还有一点,地理的北极和磁场的北极是不重合的,有一定的夹角,即磁偏角。根据经纬度是可以查到这个夹角的大小的。我们定义的地理坐标系的北当然是地理的北。但用磁力计求得实际是Xb与地磁的北的夹角,我们最后会加上磁偏角,这样就是Xb与地理北的夹角了,就是真正的航向角啦。