基于动量梯度下降法的姿态数据采集系统

朱延栓，戴晓强，兰 武，吴 伟，杨淦华

（江苏科技大学 电子信息学院，江苏 镇江 212003）

0 引言

面对世界人口飞增、土地资源紧缺和生态环境恶化，人类将目光逐渐转向了海洋。由于海洋中复杂的水下情况，水下机器人（Remotely Operated Vehicle，ROV）成为目前各国进行海洋资源探索与开发的有效工具。由于工作条件恶劣、周围环境复杂多变，水下机器人受到机械手以及水流等干扰因素的影响，且机械手的姿态、持重、载荷、水流等情况很难定量得出，故姿态解算的准确性和快速性直接决定着水下机器人执行任务过程中的稳定性和可靠性［1］。

由于机械陀螺仪及光纤陀螺仪等高精度惯性导航传感器价格昂贵、体积较大，因此在应用中大多利用微机电系统（MEMS）技术制造的加速度计、陀螺仪和磁力计等传感器进行姿态检测。九轴原始数据由三轴陀螺仪、三轴加速度计和三轴磁力计提供，陀螺仪用来检测ROV 运动时的角速度，对其进行积分处理再经过坐标转换后可得到参考坐标系下的姿态角度，短时间内数据精确度高，但随着积分的存在，误差也随时间的增加而增大；加速度计检测ROV 运动的加速度信息，其静态性能良好，长时间工作不存在漂移现象，但是由于电机运动产生的噪声会使数据出现异常；磁力计检测ROV 周围的磁场信息，精确度高，但对周围磁场强度敏感，易受干扰［2-4］。通过多传感器数据融合后的解算信息，能够有效利用不同MEMS 器件之间的互补特性，稳定地、精确地反应参考坐标系下的ROV 姿态角，其结果胜于单个MEMS 器件的处理效果［5-9］。

文献［10］在互补滤波器中引入PI 控制思想，通过自适应调整截止频率实现了陀螺仪、加速度计和磁力计数据的有效融合，但是姿态收敛速度较慢；文献［11］研究了互补滤波的阶数对惯性传感器数据融合的影响，实验结果证明融合后的数据精度随互补滤波器阶数的增加而提高，但是高阶滤波器计算量太大；文献［12］采用卡尔曼滤波器，通过加速度计和磁力计对陀螺仪偏差进行矫正，实验结果证明了在磁场异常环境下稳定效果好且精度较高，但是由于卡尔曼滤波计算量大，对于频率低的单片机而言数据实时性不如互补滤波和梯度下降法；文献［13］在对惯性传感器姿态解算过程中，通过引入梯度下降法对四元数进行寻优估计，实验结果表明该过程收敛速度块、测得的俯仰角误差较小，但是该算法使用定步长解算姿态角，解算精度不高、收敛速度较慢。

本文基于梯度下降法设计了以STM32F429 系列单片机为主控制器的姿态检测系统，MPU9250 九轴传感器为姿态检测模块的姿态采集系统，由陀螺仪测量物体运动的角速度，进而解算成角度。利用加速度计和磁力计分别测量加速度和地磁场，作为机器人的观测矢量校准陀螺仪的测量值。在数据融合方面，改进的梯度下降法巧妙地避免了磁力计补偿需要知道当地磁场角度的问题，当补偿较小时3 个姿态静态值非常稳定，慢速运动下可以达到较高精度的要求；步长较大时，可以快速跟进高速运动下的姿态变化。

1 水下机器人组成及姿态采集系统设计

1.1 水下机器人组成

本文所述水下机器人是江苏科技大学自主设计并研发的一款应用于船体检测及作业的开架式ROV，本体如图1 所示。机器人本体携带摄像头及水下灯等多种设备完成水下作业任务，水下机器人通过脐带缆与水面控制台相连。脐带缆内置四根网线和两根电源线，实现对水下系统的供电以及水陆信息交互。水面控制台负责显示各设备工况信息并实现控制指令下达。

1.2 姿态采集系统设计

设计的姿态采集系统以STM32F429 单片机为核心，通过MPU9250 九轴传感器对运动姿态进行检测，对获取的九轴传感器信息进行滤波去噪，用梯度下降算法对水下机器人的姿态信息进行解算，融合磁力计信息修正航向角。处理后的姿态角度信息通过串口发送到上位机，并在上位机软件上完成数据的曲线绘制。系统设计框图如图2 所示。

Fig.1 Underwater robot body图1 水下机器人本体

Fig.2 Overall design of the system hardware图2 系统硬件整体设计

MPU9250 内部集成了3 轴加速度计、3 轴陀螺仪以及3轴磁力计，具有3 个16 位加速度AD 输出，3 个16 位陀螺仪AD 输出，3 个16 位磁力计AD 输出，采用IIC 通讯方式，可以直接输出9 轴的全部数据。MPU9250 九轴传感器参数如表1 所示。

Table 1 MPU9250 parameters table表1 MPU9250 参数表

2 姿态解算算法

水下机器人的位姿信息需要通过地理坐标系n和载体坐标系b的坐标转换求得，地理坐标系采用东北天作为固定参考坐标系，选择水下机器人的质心作为载体坐标系原点。水下机器人在执行任务时，随着载体坐标系的不停变化，用其相对于地理坐标系的变化角描述水下机器人位姿变化，水下机器人载体坐标系绕地理坐标系X 轴旋转称为横滚角γ、绕Y 轴旋转称为俯仰角θ、绕Z 轴旋转称为偏航角ψ［14-15］。转换公式为：

其中，xb、yb、zb为水下机器人机体坐标轴，xn、yn、zn为地理坐标轴，为地理坐标系与机体坐标系的转换矩阵，为：

2.1 四元数法原理

四元数q 的微分方程˙=Ωq，写成矩阵形式为：

式中的ωx、ωy、ωz是在陀螺仪中采集的三轴角速度。使用一阶龙格库塔法更新四元数，结合上述微分方程进而可得到当前时刻的水下机器人姿态信息：

四元数转换为欧拉角的表达式如下：

2.2 梯度下降算法

梯度下降法是一种通过沿着与目标函数梯度相反的方向逐次迭代，使目标函数不断逼近局部最优解的数值方法［16-17］。在姿态数据融合系统中，通过不同传感器解算的姿态数据确定目标函数，并通过不断迭代的过程实现对最优姿态数值的逼近。本文基于动量梯度下降法估计姿态四元数的流程如图3 所示。

Fig.3 Flow chart of momentum gradient descent method图3 动量梯度下降法处理流程

本文通过梯度下降法估计最优姿态四元数，将姿态四元数的估计问题转换为一个最优值求取的过程。首先对参考坐标系下的重力加速度值与上一时刻最优姿态四元数做叉乘处理，将得到的载体坐标系下的重力加速度估计值与加速度计的实际测量值再做误差处理得到误差向量，磁力计处理过程相似，得到另一个误差向量；其次对误差向量构造目标函数，利用梯度下降法求解目标函数的梯度，并进行归一化处理［18］；然后将归一化后的梯度值与收敛步长β 相结合，并利用此结果修正由陀螺仪数据求解姿态四元数过程产生的累积误差；最后通过一阶龙格库塔法更新出当前时刻的最优姿态值［8-9］。梯度下降法的核心是构建目标函数，姿态四元数最优估计方法通过对加速度计和磁力计的误差矢量构造误差目标函数，重力加速度误差函数和地磁误差函数为：

在误差目标函数下降最快的方向，计算其梯度值并作归一化处理，并利用此梯度修正经陀螺仪解算的单位姿态四元数向量。采用梯度下降法，沿着负梯度方向不断迭代修正四元数向量，修正后的四元数更新方程为：

其中，β为收敛步长，β值的设定需依据水下机器人角速度ω和系统采样的实际周期T。β过小会导致四元数更新过程收敛速度慢，而β过大会引起系统震荡，静态效果差［19-20］。传统的梯度下降法采用固定步长解算姿态角，当水下机器人运动速度变化较快时，容易导致解算的姿态角收敛缓慢、精度低，甚至发生错误。因此，本文对传统梯度下降法进行改进，引入动态步长和动量：

3 实验结果与分析

3.1 实验平台

本文实验参考的实际姿态采用某公司生产的AH100B惯性导航模块，该模块具有全姿态右移的稳定性和实时性，广泛应用于各种运动姿态监测领域。

将互补滤波算法（CF）和动量梯度下降算法（SGDM）解算ROV 姿态的过程用C 语言表示成代码形式，并下载到惯性数据处理板中进行实验。通过姿态数据处理板将解算的姿态数据和AH100B 参考姿态数据通过串口发送到上位机，并将该数据保存至txt 文件中，并通过MATLAB 绘制曲线，便于对姿态角度变化进行分析。水下机器人姿态数据采集板实物如图4 所示（彩图扫OSID 码可见，下同）。

Fig.4 ROV attitude data acquisition board图4 水下机器人姿态数据采集板

3.2 静态实验与分析

将姿态数据处理板固定在静止的水平平面上，采用本文提出的互补滤波算法和动量梯度下降算法相比较，输出的三轴姿态信息如图5 所示。

Fig.5 Static attitude solution data图5 静态时姿态解算数据

姿态数据采集静态实验中，翻滚角、俯仰角、偏航角数据的理想值为0°。通过对比表1 中的三轴姿态数据误差，动量梯度下降算法解算的横滚角、俯仰角、偏航角的标准差分别为0.144 8、0.124 6、0.151 1，相比互补滤波算法解算的横滚角、俯仰角、偏航角的标准差分别降低1.22%、47.47%、70.31%。结果表明，静态时动量梯度下降算法解算的姿态角精度更高，效果明显优于互补滤波算法。

Table 2 Data comparison of the two algorithms in static state表2 静态时两种算法数据比较

3.3 动态实验与分析

将姿态数据处理板和AH100B 一起固定在转轴上，绕着三轴分别旋转，观察姿态数据处理的动态性能。从图6和表3 可以得出，在动态环境下互补滤波算法解算的姿态角误差大且震荡严重；而本文提出的动量梯度下降算法解算的姿态角标准差比互补滤波算法解算的姿态角分别降低31.51%、20.62%、37.01%，与参考姿态角吻合度高、误差小、收敛速度快。综上所述，本文所提出的动量梯度下降算法明显优于互补滤波算法。

Fig.6 Dynamic attitude solution data图6 动态时姿态解算数据

Table 3 Data comparison of the two algorithms in dynamic state表3 动态时两种算法数据比较

4 结语

本文设计了基于动量梯度下降算法的姿态数据采集系统，实验结果表明互补滤波法融合精度低且收敛速度较慢。动态梯度下降法得到的姿态信息更为精准、波动范围小、数据保持性更好、实时性高，且巧妙避免磁力计补偿需要知道当地磁场角度的问题，在水下机器人姿态控制中具有很好的应用前景。但由于硬件系统没有采取屏蔽措施，当附近存在磁铁或大电流时，姿态数据会产生较大误差。下一步将在设计中对整个系统增加屏蔽罩，提高系统抗干扰性能。