[发明专利]基于扩展卡尔曼滤波的水下机器人多传感器融合定位系统

说明书

本发明公开了基于扩展卡尔曼滤波的水下机器人多传感器融合定位系统，包括：EKF数据融合单元、外参标定单元、激光定位单元、视觉定位单元、里程计、路径规划单元、以及定位信息反馈单元；在融合定位过程中，先将里程计结果与视觉定位结果融合得到转化后的视觉定位结果，将转化后的视觉定位结果和激光定位结果输入到EKF数据融合单元，将融合结果输入至路径规划单元和定位信息反馈单元。本发明结合激光建图与视觉建图，可以反应和利用更丰富的环境信息，当某种传感器定位失败时，可避免原地重定位过程，保证移动机器人工作鲁棒性，当某种传感器定位结果发生跳变时，可使用其他传感器依据其置信度对跳变进行修正。

技术领域

本发明涉及水下机器人技术领域，特别涉及基于扩展卡尔曼滤波的水下机器人多传感器融合定位系统。

背景技术

目前的水下机器人定位主要通过2D激光雷达构建的栅格地图进行定位，但是2D栅格地图只能反映固定高度的障碍物信息，面对结构单一或动态的工作环境存在定位失败的风险。

在定位过程中如果出现观测高度的障碍物信息发生改变，水下机器人无法在环境变化的情况下进行有效的定位工作，无法实时修改更新已有2D激光雷达构建的栅格地图，无法在激光定位失败的情况下继续运行。

发明内容

本发明实施例提供了一种基于扩展卡尔曼滤波的水下机器人多传感器融合定位系统，结合激光建图与视觉建图，可以反应和利用更丰富的环境信息，当某种传感器定位失败时，可避免原地重定位过程，保证移动机器人工作鲁棒性，当某种传感器定位结果发生跳变时，可使用其他传感器依据其置信度对跳变进行修正。

为了解决上述问题，本发明公开了如下技术方案：

第一方面，提供一种基于扩展卡尔曼滤波的水下机器人多传感器融合定位系统，其特征在于，所述系统包括：EKF数据融合单元、外参标定单元、激光定位单元、视觉定位单元、里程计、路径规划单元、以及定位信息反馈单元；

其中，激光定位单元由激光传感器输入激光传感信号输出激光定位结果；视觉定位单元根据相机和IMU数据进行融合定位输出的视觉定位结果；

在融合定位过程中，先将里程计结果与视觉定位结果融合，以里程计为坐标系时，视觉定位结果的转换关系可以表述为T_L＝T_C·T_CL·T_EX，其中T_L为转化后的视觉定位结果，T_C为相机坐标系下的定位结果，T_CL为相机与机器人主体之间的外参，T_EX为初始阶段时不同传感器坐标系下定位结果未清零的初始值；

将转化后的视觉定位结果和激光定位结果输入到EKF数据融合单元，将融合结果输入至路径规划单元和定位信息反馈单元。

进一步的，以里程计的预测值作为约束，利用预测方程和测量方程，以重投影误差最小为优化目标，在流行上进行联合优化，其中联合优化公式如下所示：

其中，上述公式中i代表第i个里程计数据，k代表第k个关键帧，j代表里程计数据的积分项，e代表误差项，W是协方差矩阵；等号后第一项是里程计数据的误差项，第二项是环境图像上特征点重投影的误差项。

本发明另一实施例，一种水下机器人设备，其特征在于，包括：设备本体，所述设备本体上设有EKF数据融合单元、外参标定单元、激光定位单元、视觉定位单元、里程计、路径规划单元、以及定位信息反馈单元；

其中，激光定位单元由激光传感器输入激光传感信号输出激光定位结果；视觉定位单元根据相机和IMU数据进行融合定位输出的视觉定位结果；