基于梯度下降的自适应姿态融合算法*

陈 卓， 任久春， 朱 谦

(复旦大学 信息科学与工程学院，上海 200433)

0 引 言

惯性导航系统(inertial navigation system， INS)基于惯性传感器与姿态解算方法测量物体的姿态信息，以进行相应控制。在国内外学者的研究推动下，姿态融合算法体系已较为成熟。其中广泛采用的扩展卡尔曼滤波(extended Kalman filtering，EKF)[1]，无迹卡尔曼滤波(unscented Kalman filtering，UKF)[2]以及一些自适应卡尔曼滤波(adaptive Kalman filtering，AKF)[3,4]算法都基于传统卡尔曼滤波器进行设计，具有较高的预测精度，但其涉及协方差矩阵的计算，复杂度高，在预测过程中难以建立可靠稳定的状态方程。对于加速度的高频干扰，文献[4]设计的第一代帆船运动姿态测量系统虽然基于卡尔曼滤波算法设置了自适应因子，但其作用范围有限，鲁棒性较差，物体动态测量性能受到限制。互补滤波融合算法[5,6]利用陀螺仪的高频特性与加速度计的低频特性优势互补，设计通带滤波，简单高效，但难以确定滤波器的截止频率，易受噪声与运动加速度的干扰。文献[7]利用加速度计、磁力计测量得到的场量变化，基于梯度下降算法(gradient descent algorithm，GDA)，修正角速度测量姿态。该方法通用性强，测量精度优于卡尔曼滤波算法，计算复杂度较低，适用于小型嵌入式系统。文献[8]在此基础上设计了一种动态步长梯度下降+运动加速度抑制处理的(dynamic gradient descent algorithm+motion acceleration processing，DGDA+MAP)姿态算法，通过求解物体运动加速度模长，为梯度下降设计动态变化步长，从而提升了四旋翼飞行器在强机动、高速运动状态下的姿态解算性能。

本文针对帆船运动训练中稳定性和实时性的实际需求，基于梯度下降姿态融合算法设计了两种改进的自适应方法，分别根据前一时间段中的平均运动加速度模长和加速度增量的变化得到当前的自适应因子控制优化步长，从而提高了系统在高动状态中的稳定性，实验结果表明其测量精确度优于文献[4，8]，并已成功应用于第二代帆船运动姿态测量系统中。

1 惯导系统中的运动表示

惯性导航系统依靠惯性测量单元(inertial measurement unit，IMU)得到物体的姿态信息进行导航控制。IMU包含三轴加速度计、三轴陀螺仪等惯性测量元件[9]。在惯性导航系统中，物体的运动可以抽象为载体坐标系S相对于参考坐标系E的运动，两者之间的相互关系可用方向余弦矩阵[10]来表示。图1的欧拉角[11]、横滚角φ、俯仰角θ与航向角ψ在惯性导航系统中可以用于确定物体的任意姿态，又称为姿态角。

图1 三维坐标系下的欧拉角示意

在实际工程应用中，由于四元数法[12]求解微分方程式计算量较小，多用其求解运动姿态。

2 姿态融合算法

2.1 基于梯度下降的姿态融合算法

2.2 改进的自适应算法

考虑到在高速运动状态下加速度计易受高频干扰，因此本文在文献[7]算法基础上设计了两种加速度控制的自适应算法，加速度控制的自适应梯度下降算法(acceleration-controlled adaptive gradient descent algorithm，ACAGDA)和增量加速度控制的自适应梯度下降算法(delta-acceleration-controlled adaptive gradient descent algorithm，ΔACAGDA)。通过考察当前时刻之前N个采样点(包含当前时刻)的运动加速度及加速度的变化量从而得到稳定的自适应梯度下降步长，减少了瞬时噪声的影响，提高算法可信度。

2.2.1 ACAGDA

(2)

2.2.2 ΔACAGDA

物体在实际运动状态中的复杂多变导致了加速度的不断变化，即使处于静止状态下，由于噪声干扰，传感器所测量得到的加速度也不能维持在某一常数，因此，本文根据实际测量经验设计了基于ΔACAGDA算法。设t时刻之前N个加速度采样值矢量变化量之和为δN，根据δN求取三个轴向的均方根变化量RMSδ，RMSδ的大小表示物体在某一轴向的运动是否超出静态范围，因此，需要设置阈值η。同样引入常数控制因子γδ与加速度自适应因子λδ用于调节β

(4)

当RMSδ未超过阈值时，表示这一时间段物体处于静止或者拟静止状态，此时加速度变化量很小，近似为重力加速度，自适应因子不对步长进行限制；当RMSδ超过阈值时，加速度发生大幅变化，表示物体处于高速运动状态，λδ与RMSδ呈负相关，通过RMSδ的大小对β进行反向调节，本文采用平方项加快衰减。

3 实验结果与性能分析

3.1 姿态测量系统

本文基于第二代帆船运动姿态倾角测量仪(以下简称倾角仪)进行算法验证测试，倾角仪以三轴陀螺仪L3G4200D与三轴加速度计LSM303DLH作为MEMS惯性测量单元，在第一代测量系统设计结构上进行改进。在实验过程中，根据文献[4]中设计的重力摆验证测试方法将倾角仪固定在实验装置上，控制仪器处于不同的运动状态，通过蓝牙导出姿态角数据，同时用示波器记录重力摆装置上角度传感器的输出电压值，即可得到真实的角度参照数据进行对比。

3.2 实验结果分析

设计传感器采样率为100 Hz，为了测试物体在静态和动态两种状态中的姿态解算性能，实验模拟帆船运动设计了多种姿态角度变化场景。图2为典型的绕轴运动，即倾角快速变化时四种姿态解算方法的测量结果。实验经过多次快速绕轴运动与静止状态交替测试，可以看出，本文提出的2种方法解算得到的曲线跟踪度较好，尤其在运动状态的过度阶段曲线更加平滑，横滚角测量方法与其类似。图3为图2相对应的测量残差，从比较结果中可以看出通过引入负相关平方项以及对N点运动加速度平均模长的考察，其动态自适应性能相对单点运动加速度抑制有了一定程度的改善，加大了对高频干扰的抑制。而通过加速度增量来进行梯度下降步长的自适应调制得到的残差曲线幅度最小，性能达到最优。

图2 4种姿态解算下的俯仰角

图3 4种姿态解算下的俯仰角残差

4种算法在静态和动态两种状态下其测量得到的俯仰角(θ,φ)和横滚角相对于真实角度的均方根误差(root mean square error，RMSE)，在静止状态中，4种算法性能差距不大，其中AKF(0.078°,0.086°)与 ACAGDA(0.081°,0.092°)2种方法的误差均达到了0.1°以内。而在运动状态中，ACAGDA(1.859°,1.873°)的测量误差相对于AKF(4.741°,4.563°)、DGDA+MAP(2.301°,2.197°)两种方法分别减少了59.87 %,16.98 %， ACAGDA(1.145°,1.166°)则相对于AKF、DGDA+MAP、ACAGDA三种方法分别减少了75.15 %,48.59 %与38.08 %，本文提出的2种自适应方法具有更好的稳定性与精确度，能够更加适用于海面帆船运动船体姿态测量的实际需求。

4 结 论

本文为了提高帆船运动中船体姿态测量实际应用中的稳定性，减少物体在高速运动状态下附加加速度的干扰，基于梯度下降算法，设计了两种加速度控制步长的自适应姿态解算方法，其性能相对于文献[4,8]所提出的方法有较为明显的改善，其中基于N点增量加速度的控制方法具有更高的稳定性与有效性，已经实现于第二代帆船运动姿态测量系统中。