[发明专利]一种水下机器人多传感器融合数据处理方法

摘要

本发明公开了一种水下机器人多传感器融合数据处理方法，基于多个传感器估计水下机器人的姿态、位置和速度信息；通过融合所有传感器测量数据，能够更好地消除具有明显误差的传感器测量数据，从而使水下机器人不易受到单个传感器故障的影响，更适用于水下机器人这类非线性系统；在构建状态向量时把角度偏差和速度偏差加进了状态向量，考虑了角度偏差和速度偏差对于状态更新的影响，来精确地估计水下机器人的姿态、位置和速度信息。通过假设过程噪声由滤波结果和观测结果得到，再用差分进化算法对所得到的过程噪声方差进行最优化选择来提高滤波精度。本发明适用于水下机器人类非线性系统来精确地估计水下机器人的姿态、位置和速度信息。

主权项

1.一种水下机器人多传感器融合数据处理方法，其特征在于，包括以下步骤：S1、搭建水下机器人姿态、位置和速度估计系统，获取载体坐标系下的传感器数据；S2、建立状态变量与观测变量，使用四元数表征姿态信息，并利用姿态信息将位置与速度解算转换到导航坐标系；S3、综合四元数、速度和位置的预测过程得到状态向量的状态转移矩阵；S4、根据状态转移矩阵和过程噪声求出对状态预测影响的控制雅克比矩阵，初始化协方差和过程噪声方差的初始值后就可以建立扩展卡尔曼滤波的状态预测方程；S5、采用磁力计的数据作为四元数的观测信息，采用GPS的数据作为位置与速度的观测信息，综合磁力计与GPS的观测信息得到观测矩阵；S6、进行观测过程噪声的初始化，建立扩展卡尔曼滤波的状态更新方程；S7、根据更新后得到的四元数q解算水下机器人的偏航角、翻滚角、俯仰角，再结合其他状态变量V_N,V_E,V_D,P_N,P_E,P_D估计出水下机器人的姿态、位置和速度信息。