CN112652001B - 基于扩展卡尔曼滤波的水下机器人多传感器融合定位系统 - Google Patents

Abstract

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

Description

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

技术领域

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

背景技术

目前的水下机器人定位主要通过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数据进行融合定位输出的视觉定位结果；

在融合定位过程中，先将里程计结果与视觉定位结果融合，以里程计为坐标系时，视觉定位结果的转换关系可以表述为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数据融合单元、外参标定单元、激光定位单元、视觉定位单元、里程计、路径规划单元、以及定位信息反馈单元；在融合定位过程中，先将里程计结果与视觉定位结果融合得到转化后的视觉定位结果，将转化后的视觉定位结果和激光定位结果输入到EKF数据融合单元，将融合结果输入至路径规划单元和定位信息反馈单元。本发明结合激光建图与视觉建图，可以反应和利用更丰富的环境信息，当某种传感器定位失败时，可避免原地重定位过程，保证移动机器人工作鲁棒性，当某种传感器定位结果发生跳变时，可使用其他传感器依据其置信度对跳变进行修正。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明一个实施例中基于扩展卡尔曼滤波的水下机器人多传感器融合定位系统的结构示意图。

图2是本发明一个实施例中基于扩展卡尔曼滤波的水下机器人多传感器融合定位系统的流程示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明中的水下机器人，指的是可以在水底地面建立三维地图的水下爬行机器人，不限于水下爬行作业机器人、水下爆破机器人、水下挖沙机器人、水下特种作业机器人等。

水下机器人设备具有水下行进装置，可以在当前所处环境内自主移动；另外，水下机器人设备还具有视觉传感器和里程计传感器，可分别在移动过程中采集周围环境图像和反应水下机器人设备运动状态的里程计数据。其中，视觉传感器可以是单目摄像头、双目摄像头或者深度摄像头(Red Green Blue-Depth map，RGB-D)等；里程计传感器包括但不限于：惯性测量单元(Inertial measurement unit，IMU)或者里程计(Odometry)。其中，六轴IMU包括三轴陀螺仪和三轴加速度计，九轴IMU包括三轴磁力计、三轴陀螺仪和三轴加速度计。IMU采集的里程计数据包括自主移动设备在三维空间中移动时的角速度和加速度，除此之外，还可以测量重力加速度，但不限于此。里程计采集的里程计数据一方面是水下机器人设备的位姿(位置和转角)，另一方面是水下机器人设备的速度(前进速度和转向速度)。在此基础上，将视觉、里程计和语义分割技术相结合，应用到水下机器人设备的SLAM系统中，实现对水下机器人设备的联合定位、导航与避障，有效提高SLAM系统的鲁棒性。

参见图1，本发明一个实施例提出了一种基于扩展卡尔曼滤波的水下机器人多传感器融合定位系统，多传感器信息融合定位是基于扩展卡尔曼滤波，主要原理是在里程计坐标系下将视觉定位结果与里程计进行融合，从而提高定位结果的稳定性，再将融合结果进行转化后与地图坐标系下的激光定位结果进行融合，最终通过融合后的定位信息实时矫正机器人的位姿，从而实现更精准、更鲁棒的定位。

所述系统包括：EKF数据融合单元、外参标定单元、激光定位单元、视觉定位单元、里程计、路径规划单元、以及定位信息反馈单元。激光定位单元由激光传感器输入激光传感信号输出激光定位结果；视觉定位单元根据相机和IMU数据进行融合定位输出的视觉定位结果。

依据工作内容可以将该系统划分为三个阶段：

在系统运行的初始阶段，需将此时传感器得到不同坐标系下的水下机器人定位结果进行关联与统一，以便进行之后的融合工作。以里程计为坐标系时，视觉定位结果的转换关系可以表述为T_L＝T_C·T_CL·T_EX，其中T_L为转化后的视觉定位结果，T_C为相机坐标系下的定位结果，T_CL为相机与机器人主体之间的外参，T_EX为初始阶段时不同传感器坐标系下定位结果未清零的初始值。而以地图为坐标系时，需加入里程计与地图之间的外参T_M。在融合定位过程中，当视觉定位结果或激光定位结果输入到卡尔曼滤波器时，首先由激光定位结果对滤波器进行初始化，其次依据构建的运动模型，以x方向为例：X_new＝X_old+V_x*t+0.5*a*t²,V_{x_new}＝V_{x_old}+a*t,对移动机器人当前位姿及其对应的协方差矩阵进行预估，在预估过程中加入了过程噪声，可以通过调整过程噪声来优化运动模型，使之更符合实际。之后基于均方误差最小的准则，将视觉定位结果和激光定位结果作为观测值，对上一步中的预估值进行修正，最终得到融合定位结果。此外，算法考虑到当出现某传感器的定位结果与其他传感器存在较大的差异，或短时间内该传感器输出位置出现较大的跳变时，可依据融合结果对该传感器给出的定位结果进行修正，从而提高长期融合的鲁棒性和有效性。

水下机器人搭载视觉传感器，包括但不限于单目传感器、双目传感器、鱼眼相机、全景相机。同时搭载里程计模块，包括但不限IMU、ODOM等。在水下机器人系统中，通过硬件时钟方法对多个传感器进行同步触发，保证系统的实时性。视觉传感器与里程计模块刚性连接，通过位姿标定的方法计算得到两者之间坐标系的转换cTi。

通过里程计模块预积分计算的方法可以预测出视觉传感器的上一帧到当前帧的相对位置，结合视觉跟踪过程如光流跟踪、特征匹配跟踪等，可以提高视觉跟踪的匹配速度，尤其是在旋转状态下，提高了系统的鲁棒性。通过IMU系统的重力加速度方向计算，可以将视觉地图与实际物理地图的坐标进行统一，同时可以计算视觉地图的尺度信息。

水下机器人的运行系统在k时刻的过程可以描述为x_k＝{R_k，p_k，u_k，b_gk，b_ak}，依次代表k时刻下的旋转、位移、里程计测量量、陀螺仪偏置、加速度计偏置。水下机器人的运动状态可以通过运动方程从k-1时刻和k时刻的输入从而预测当前的水下机器人位姿。以里程计的预测值作为约束，利用预测方程和测量方程，以重投影误差最小为优化目标，在流行上进行联合优化，其中联合优化公式如下所示：

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

视觉系统通过slideWindow的形式对输入的普通视帧做跟踪、三角化、边缘化。对进入到slideWindow中的关键帧，通过分割的方法对逐个像素进行预测，在室内机器人的运行过程中，动态环境以人为主。语义分割将输入的图像的每个像素标签化，对于动态物体，在SLAM系统中对这些区域不进行跟踪，可以大幅度降低动态环境的干扰。对于固定物体，使用其作为定位约束，加入在地图描述中，给出避障信息及可行驶区域信息，可以提高定位导航的鲁棒性。

在室内环境下，相比于传统视觉SLAM，多传感器融合及语义分割的SLAM系统表现出了更高的鲁棒性。地图尺度信息明确，地图坐标与物理坐标对齐。可以应对机器人打滑、视觉匹配错误等问题。

在动态场景下，里程计系统对视觉SLAM做出了先验约束，语义分割则对视场中的动态物体及背景进行分割，使得视觉系统只对背景区域进行匹配跟踪。在导航状态下，通过语义分割对视场中的动态障碍物进行预测分割，可以作为一种先验信息使得导航提前判断。

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

需要说明的是，在本文中，诸如第一和第二之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同因素。

本领域普通技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储在计算机可读取的存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：ROM、RAM、磁碟或者光盘等各种可以存储程序代码的介质中。

最后需要说明的是：以上所述仅为本发明的较佳实施例，仅用于说明本发明的技术方案，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所做的任何修改、等同替换、改进等，均包含在本发明的保护范围内。

Claims (4)

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是协方差矩阵；等号后第一项是里程计数据的误差项，第二项是环境图像上特征点重投影的误差项。

Images