基于动量梯度下降法的水下机器人姿态采集方法

摘要

本发明公开了一种基于动量梯度下降法的水下机器人姿态采集方法，具体步骤如下：对系统采集的加速度计和磁力计数据采用中位值平均滤波算法进行滤波；对参考坐标系下的重力加速度值与上一时刻最优姿态四元数做叉乘处理后与加速度计的实际测量值做误差处理得到误差向量，磁力数据做相似处理，得到另一个误差向量；对误差向量构造目标函数，利用梯度下降法求解目标函数的梯度，并进行归一化处理；将归一化后的梯度值与收敛步长β相结合，并将此结果来修正由陀螺仪数据求解姿态四元数过程产生的累积误差；通过一阶龙格库塔法更新出当前时刻的最优姿态值。本发明所得数据精度高，实时性好且过程计算量小，在水下机器人姿态控制中具有很好的应用前景。

说明书

技术领域

本发明涉及一种水下机器人姿态采集方法，尤其涉及一种基于动量梯度下降法的水下机器人姿态采集方法。

背景技术

随着世界人口飞增、土地资源紧缺、生态环境恶化，践行海洋战略是一条急需而有效的道路。在对海洋情况进行探查的过程中，水下机器人(RemotelyOperatedVehicle—ROV)正在成为目前各国进行海洋资源探索与开发的有效工具。由于水下工作条件恶劣、周围环境复杂多变，水下机器人由于受到机械手以及水流等干扰因素的影响，且机械手的姿态和持重、载荷等情况、水流情况很难定量得出，所以姿态解算的准确性和快速性直接决定着水下机器人执行任务过程中的稳定性和可靠性。

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

发明内容

发明目的：本发明的目的旨在提供一种对水下机器人姿态数据实时、准确采集且计算量小的采集方法。

技术方案：本发明所述一种基于动量梯度下降法的水下机器人姿态采集方法，包括以下步骤：

(1)利用九轴传感器采集姿态原始数据并输出到单片机；

(2)采用中位值平均滤波法对三轴加速度原始数据和三轴磁力计原始数据进行滤波处理，再将陀螺仪数据、加速度数据和磁力数据分别用四元数法表示；

(3)对参考坐标系下的重力加速度值和地磁值分别与上一时刻最优姿态四元数做叉乘处理，再将得到的载体坐标系下的重力加速度估计值与加速度计的实际测量值再做误差处理得到重力加速度误差向量，将得到的载体坐标系下的地磁估计值与磁力计的实际测量值再做误差处理得到地磁误差向量；

(4)对重力加速度和地磁的误差向量构造目标函数，利用梯度下降法求解目标函数的梯度并进行归一化处理；

(5)将归一化后的梯度值与收敛步长β相结合，并将此结果用修正由陀螺仪数据求解姿态四元数过程产生的累积误差；

(6)通过一阶龙格库塔法更新出当前时刻的最优姿态值。

所述步骤2中，四元数法表示的数据分别为：

其中ω为陀螺仪数据，g

所述步骤3中，重力加速度和地磁的误差向量为分别为：

进一步的，所述步骤4中，所述误差向量的目标函数为f(Q

所述步骤5中，修正后的四元数更新方程为：

进一步的，所述四元数更新方程引入的收敛步长β和动量ν

β＝αTω+||f||

其中：

所述步骤6中，通过一阶龙格库塔法更新出当前时刻的最优姿态值为：

有益效果：与现有技术相比，本发明具有以下显著优点：采用中位值平均滤波算法对加速度计和磁力计数据进行滤波去噪，数据更加平滑准确；在保证了解算出来的姿态数据精度高的同时，计算量小且实时性好；动量梯度下降算法避免了磁力计补偿需要知道当地磁场角度的局限。

附图说明

图1为本发明所述基于动量梯度下降法的水下机器人姿态采集方法流程图；

图2为本发明实验过程中采集到的姿态数据图表；

图3为实验过程中采集到的翻滚角数据图表；

图4为实验过程中采集到的俯仰角数据图表；

图5为实验过程中采集到的偏航角数据图表。

具体实施方式

下面结合附图和实施例对本发明的技术方案作进一步的说明。

发明人在对水下机器人姿态采集的研究中发现：通过多传感器数据融合后解算的信息，能够有效利用不同MEMS器件之间的互补特性，稳定地、精确地反应参考坐标系下的ROV姿态角，其结果胜于单个MEMS器件的处理效果。由此发明人进行了进一步的基于动量梯度下降法的水下机器人姿态采集方法研究。

如图1所示，本发明所述的基于动量梯度下降法的水下机器人姿态采集方法，流程如下：

(1)在本实施例中，单片机通过IIC通信获取MPU9250输出的九轴传感器姿态原始数据，采用中位值平均滤波法对三轴加速度原始数据和三轴磁力计原始数据进行滤波处理，并用四元数法表示得：

对参考坐标系下的重力加速度值和地磁值分别与上一时刻最优姿态四元数做叉乘处理，再将得到的载体坐标系下的重力加速度估计值与加速度计的实际测量值再做误差处理得到重力加速度误差向量，将得到的载体坐标系下的地磁估计值与磁力计的实际测量值再做误差处理得到地磁误差向量。

在参考坐标系下加速度计输出值用四元数表示恒为g

(2)对误差向量构造目标函数，利用梯度下降法求解目标函数的梯度，并进行归一化处理；梯度下降法的核心是构建目标函数，本文的姿态四元数最优估计方法通过对加速度计和磁力计的误差矢量

同理，地磁误差向量为：

(3)将归一化后的梯度值与收敛步长β相结合，并将此结果来修正由陀螺仪数据求解姿态四元数过程产生的累积误差：

目标函数的雅可比矩阵J

当目标函数f(Q

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

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

其中：

(4)通过一阶龙格库塔法更新出当前时刻的最优姿态值。

在实验验证过程中中，采集得到姿态数据如图2所示，将翻滚角、俯仰角和偏航角的数据图表分别列出，如图3-5所示。

根据图2较大范围内数据图表可知，本发明所述水下机器人姿态采集方法获取到的姿态数据，大范围内数据受到噪声干扰造成的误差更少，更加平滑。

根据图3-5，得到翻滚角、俯仰角和偏航角的数据情况，明显可知，虽偶有偏差，但本发明所述姿态采集方法所采集到的姿态数据整体的误差小，准确度高且实时性好。