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

权利要求书

1.一种基于动量梯度下降法的水下机器人姿态采集方法，其特征在于，包括以下步骤：

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

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

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

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

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

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

2.根据权利要求1所述的水下机器人姿态采集方法，其特征在于，所述步骤2中，四元数法表示的数据分别为：

3.根据权利要求1所述的水下机器人姿态采集方法，其特征在于，所述步骤3中，重力加速度和地磁的误差向量为分别为：

4.根据权利要求1所述的水下机器人姿态采集方法，其特征在于，所述步骤4中，所述误差向量的目标函数为f(Q_t-1,g_b,m_b)＝[f_g f_m]^T，J＝[J_g J_m]^T，误差目标函数的梯度为其中：

5.根据权利要求1所述的水下机器人姿态采集方法，其特征在于，所述步骤5中，修正后的四元数更新方程为：

6.根据权利要求5所述的水下机器人姿态采集方法，其特征在于，所述四元数更新方程引入的收敛步长β和动量ν_i分别为：β＝αTω+||f||²，

其中：α为系数；||f||²为目标函数的二范数的平方，κ为衰减系数。

7.根据权利要求1所述的水下机器人姿态采集方法，其特征在于，所述步骤6中，通过一阶龙格库塔法更新出当前时刻的最优姿态值为：