[发明专利]基于动量梯度下降法的水下机器人姿态采集方法

说明书

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

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

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

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

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

其中ω为陀螺仪数据，g_b为加速度数据，m_b为磁力数据。

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