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

权利要求书

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

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

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

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

其中，在融合定位过程中，当视觉定位结果或激光定位结果输入到卡尔曼滤波器时，首先由激光定位结果对滤波器进行初始化，其次依据构建的运动模型，以x方向为例：X_new＝X_old+V_x*t+0.5*a*t²,V_{x_new}＝V_{x_old}+a*t,对移动机器人当前位姿及其对应的协方差矩阵进行预估。

2.根据权利要求1所述的系统，其特征在于，以里程计的预测值作为约束，利用预测方程和测量方程，以重投影误差最小为优化目标，在流行上进行联合优化，其中联合优化公式如下所示：

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

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

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

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

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

其中，在融合定位过程中，当视觉定位结果或激光定位结果输入到卡尔曼滤波器时，首先由激光定位结果对滤波器进行初始化，其次依据构建的运动模型，以x方向为例：X_new＝X_old+V_x*t+0.5*a*t²,V_{x_new}＝V_{x_old}+a*t,对移动机器人当前位姿及其对应的协方差矩阵进行预估。

4.根据权利要求3所述的设备，其特征在于，以里程计的预测值作为约束，利用预测方程和测量方程，以重投影误差最小为优化目标，在流行上进行联合优化，其中联合优化公式如下所示：

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