基于MARG的两种实时姿态测量算法的分析与比较

　　0　引　　言

　　陀螺是最常用的姿态传感器,但是高性能陀螺的体积大,价格高,这限制了陀螺在某些场合的应用。近年来,在汽车工业的需求推动下,微机械陀螺获得了很大的发展。微机械陀螺与其他陀螺相比,具有体积小、成本低、易于批量生产等优点,从而扩展了陀螺的应用;但是它的精度较低,并且陀螺漂移会在短时间内引起较大的姿态误差。因此必须与其他姿态传感器(例如GPS、倾斜计、磁强计等)组合使用才能给出长期的稳定可靠的姿态信息。常用的解决上述问题的方法是采用卡尔曼滤波器。卡尔曼滤波器利用来自GPS、磁强计、加速度计和倾斜计等传感器的数据来补偿陀螺偏差引起的姿态误差。H.J.Luinge[1]等人采用倾斜计的数据进行误差补偿。Bechmann等人[2]采用四元数互补滤波器将MARG传感器应用于姿态测量。傅建国等人在基于地球重力场和磁场矢量观测进行姿态测量的算法存在的不足,提出了一种新的基于四元数的递推优化估计算法[3-4],

　　本文的主要工作就基于Bechmann等人的研究[5-6],在选定滤波器为EKF的基础上,对QUEST方法和Gauss-Newton方法分别进行算法的研究及仿真,通过不同方法计算得到的四元数与原始采样数据的比对,进一步计算得到的姿态的比对,选出最优的计算四元数的方法,作为卡尔曼滤波器的测量值。

　　1　算法原理

　　1.1　陀螺、加速度计和磁强计姿态确定原理

　　姿态就是联系动坐标系和参考坐标系角位置的参数。欧拉角和四元数是常用的表示转动的两种方式。四元数因为可以避免欧拉角的奇异问题而应用的更加广泛。四元数姿态表达式是描述姿态的最小非奇异参数集,是一种四参数的表达式,表示从一个坐标系向另一个坐标系变换,可以用绕某一单位矢量的单次旋转实现。

　　选择参考坐标系为NED坐标系,定义坐标系如下。

　　1.2　QUEST算法原理

　　QUEST(四元数估计算法)是获得单次观测四元数的一种方法。最初是用来解决经典的Wahba问题[7]———用封闭方法解决基于一组测量值或者观测值,参考坐标系相对于固定坐标系的刚体的姿态。

　　选择参考坐标系为NED坐标系,并忽略磁北极和真北之间的差别。当载体在参考坐标系中静止,并且体坐标系和参考坐标系重合时,加速度计和磁强计的测量输出为:gr=[0　　0　　g],br=[bn　　0　　bd] (6)

　　式中:g是重力加速度,bx是地磁场在NED坐标系中的北向分量,bz是地磁场在NED坐标系中的垂直分量。当载体处于任意姿态时,假定加速度计和磁强计的输出是:gb=[gx　　gy　　gz],bb=[bx　　by　　bz] (7)参考坐标系和体坐标系的关系可按方程(8)表示: