CN112254729A - 一种基于多传感器融合的移动机器人定位方法 - Google Patents

Abstract

本发明公开了一种基于多传感器融合的移动机器人定位方法，通过融合多种传感器定位的优势，解决了单纯视觉惯性定位在弱纹理环境下定位精度较差和激光里程计在动态或结构相似环境下定位鲁棒性弱等问题，具有较高的可靠性和定位精度，此外，本发明所提供的融合定位方法整体上硬件造价较低，且对运算平台的算力要求较低。

Description

一种基于多传感器融合的移动机器人定位方法

技术领域

本发明属于移动机器人导航定位技术领域，具体涉及一种基于多传感器融合的移动机器人定位方法。

背景技术

定位导航技术是机器人实现自主移动的关键技术之一，主要用于为机器人实现规避障碍物、移动至目标点、自主移动作业等功能提供指引与保障。

由于空间环境GPS有源定位信号的缺失以及精度低等问题，当前机器人在定位中常用的传感器主要包括激光雷达、IMU、视觉传感器等。然而由于机器人的应用场景复杂，单一传感器的使用往往难以实现定位方法上的通用性，如2D激光雷达在动态或几何结构相似环境中表现较差、视觉定位方法在弱纹理场景下定位精度较低等。

发明内容

有鉴于此，本发明提供了一种基于多传感器融合的移动机器人定位方法，能够融合全局定位信息实现移动机器人在实现高精度、强可靠性的主动定位的同时实现全局定位。

本发明提供的一种基于多传感器融合的移动机器人定位方法，包括以下步骤：

步骤1、将多个传感器安装至待定位机器人机体上；根据所述传感器获取的数据，采用视觉惯性联合位姿估计得到所述待定位机器人的初步空间位姿，采用激光里程计得到待定位机器人的初步定位数据；

步骤2、采用扩展卡尔曼滤波将所述初步空间位姿及初步定位数据融合为所述待定位机器人的空间位姿；若当前计算为首次计算或到达更新周期时，执行步骤3；否则，执行步骤4；

步骤3、根据全局空间位姿坐标系计算出所述待定位机器人的全局测量位姿，求解相同时刻的所述空间位姿与全局测量位姿之间的变换矩阵；

步骤4、采用所述变换矩阵将所述空间位姿转换为所述待定位机器人移动过程中的实时全局位姿。

进一步地，所述步骤3中的所述全局空间位姿坐标系为采用二维码建立的全局空间位姿坐标系。

进一步地，所述步骤3中求解相同时刻的所述空间位姿与全局测量位姿之间的变换矩阵，包括以下步骤：

采用最小二乘法求解实现所述空间位姿与全局测量位姿之间的变换矩阵，设置如下残差函数：

其中，P_i为所述待定位机器人的i时刻的所述空间位姿，P_i-1为所述待定位机器人的i-1时刻的所述空间位姿，

为所述待定位机器人的相邻两时刻空间位姿增量，P_m为m时刻所述空间位姿通过所述变换矩阵计算得到的实时全局位姿，G_m为m时刻所述待定位机器人的全局测量位姿，∑_i为i时刻所述空间位姿对应的协方差，∑_g为所述全局测量位姿对应的协方差，

为m时刻与n时刻所述空间位姿与通过所述变换矩阵计算得到的实时全局位姿之间的残差。

有益效果：

1、本发明通过融合多种传感器定位的优势，解决了单纯视觉惯性定位在弱纹理环境下定位精度较差和激光里程计在动态或结构相似环境下定位鲁棒性弱等问题，具有较高的可靠性和定位精度，此外，本发明所提供的融合定位方法整体上硬件造价较低，且对运算平台的算力要求较低。

2、本发明提供了简单有效的全局位姿估计融入方式，通过扩充融入其他有源定位信息，可有效减小里程计中的累计误差，实现全局定位，提升方法的通用性。

附图说明

图1为本发明提供的一种基于多传感器融合的移动机器人定位方法的流程图。

图2为本发明提供的一种基于多传感器融合的移动机器人定位方法的利用扩展卡尔曼滤波融合多源位姿信息的流程图。

图3为本发明提供的一种基于多传感器融合的移动机器人定位方法的加入全局定位信息融合估计的流程图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

本发明提供的一种基于多传感器融合的移动机器人定位方法，如图1所示，具体包括以下步骤：

步骤1、将多个传感器安装至待定位机器人机体上；根据传感器获取的数据，采用视觉惯性联合位姿估计得到待定位机器人的初步空间位姿，采用激光里程计得到待定位机器人的初步定位数据。

具体来说，首先将多个传感器安装至待定位机器人机体上的过程，包括：

步骤1.1、传感器的安装与标定。

采用2D激光雷达、IMU以及单目摄像头作为定位传感器，也可融入轮速计等其他传感器，在安装IMU以及单目摄像头时，尽量使其位于机器人机体中心位置。使用标定工具完成对于IMU随机误差和数据噪声的标定，同时还需完成IMU与单目传感器、IMU与激光雷达之间外部参数的标定。

步骤1.2、多传感器信息的采集、读取与同步。

工控机通过USB接口实时采集多个传感器的数据，并对采集的数据标记时间戳。进行时间戳对齐的操作，以确保相邻两帧图像数据和激光点云数据之间有相匹配的IMU数据。

然后，基于传感器获取的信息，采用视觉惯性联合位姿估计得到待定位机器人的初步空间位姿，采用激光里程计得到待定位机器人的初步定位数据，具体过程如下：

步骤1.3、将时间对齐的IMU数据与图像数据传入视觉惯性位姿估计模块，建立运动模型。提取相机图像中的FAST角点，并利用光流法来追踪这些角点在接下来相邻图像中的位置。对IMU数据进行预积分，求取其在相邻两帧图像时间间隔内的运动估计。

在该模块初始化阶段，利用五点法计算相邻图像帧间的运动估计。将视觉运动估计与IMU预积分量对齐，建立构建残差函数，利用非线性优化的方式求得相机与IMU之间的旋转外参、IMU中陀螺仪的随机误差估计、相机中的重力方向、机器人运动初速度以及***模块的尺度等信息，实现相机坐标系与世界坐标系的对齐，初始化完成。

在之后的运动估计中，通过IMU预积分实现对相机位姿的预测，通过相邻图像帧间的位移三角化特征点，利用滑动窗口算法，对固定数量的位姿估计变量可以构建下式所示的残差函数：

上式中的残差项从左至右依次对应先验残差项、空间点重投影误差、IMU残差，通过图优化的方式可以实现对机器人位姿更精准的估计。

步骤1.4、视觉惯性位姿估计模块初始化完成之后，等待获取到的下一帧激光点云数据，根据点云数据的时间戳，利用步骤三中估计的IMU随机游走误差参数实现较为精确的IMU预积分，通过从预积分位姿中通过线性插值的方式获得对应的激光里程计初始位姿，完成初始化。之后通过对IMU预积分位姿进行线性插值的方式获取一帧点云在不同时刻相对于初始采集时刻位姿的位移变化量，再利用球面线性插值的方法实现对于每个激光点信息中运动畸变的去除。完成点云畸变校准后，采用IMLS-ICP的方法对相邻帧间点云进行运动信息估计，实现位姿坐标x、y以及偏航角的更新。

步骤2、采用扩展卡尔曼滤波将初步空间位姿及初步定位数据融合为待定位机器人的空间位姿，如图2所示。

需要建立***的预测模型与观测模型，***状态变量为6维，分别对应x、y、z、roll、pitch、yaw，状态预测函数设置如下：

其中，vel[0]与vel[1]为***输入的控制量，vel[0]为t-1时刻至t时刻之间机器人位姿中yaw角的变化量，vel[1]为t-1至t时刻间位移的大小。该部分数据可以取自IMU预积分数据，也可以通过扩充轮速计测量测到。

对于观测模型而言，测量数据的更新均为线性，其观测变换矩阵H为6×6的单位阵。激光里程计部分带来x、y、yaw状态量的观测更新，视觉惯性联合位姿估计模块带来x、y、z、roll、pitch、yaw状态量的观测更新。同时为了避免不同位姿估计模块之间坐标系的转换，非线性卡尔曼滤波融合的为不同模块位姿估计在相同时间区间内的位姿增量。

步骤3、根据全局空间位姿坐标系计算出待定位机器人的全局测量位姿，求解相同时刻的空间位姿与全局测量位姿之间的变换矩阵，如图3所示。

制作有限数量的Tags二维码，之后需要测量制作的Tags的尺寸并分配给每个二维码唯一的ID，之后还需标定相机的内参，得到相机的焦距信息。建立室内定位空间的全局坐标系，将有限数量的二维码分散地部署在全局空间内，并测量每个二维码的三维空间坐标。机器人在工作过程中借助AprilTag库提取二维码中的信息，计算二维码与机器人之间的位姿关系，分时断续实现机体全局坐标的计算。机器人对于全局测量位姿坐标的获取不限于本实例说明的方式，也可使用UWB等有源定位***提供。

由于仅采用二维码计算待定位机器人的全局测量位姿，不仅会存在计算误差，而且还会增大计算量和计算时间，因此，本发明采用求解空间位姿与全局测量位姿之间的变换矩阵的方式，将步骤3中获取的主动定位的空间位姿与全局测量位姿进行对齐，再采用变换矩阵将步骤3中得到的空间位姿转换为实时全局位姿，将有效地提高机器人全局位姿获取的实时性及准确性。

具体来说，本发明采用最小二乘法求解空间位姿与全局测量位姿之间的变换矩阵，构建如下残差函数：

其中，P_i为待定位机器人的i时刻的空间位姿，P_i-1为待定位机器人的i-1时刻的空间位姿，

为待定位机器人的相邻两时刻空间位姿增量，P_m为m时刻空间位姿通过变换矩阵计算得到的实时全局位姿，G_m为m时刻待定位机器人的全局测量位姿，∑_i为i时刻空间位姿对应的协方差，∑_g为全局测量位姿对应的协方差，

为m时刻与n时刻空间位姿与通过变换矩阵计算得到的实时全局位姿之间的残差。

在首次计算待定位机器人的实时全局位姿时，需要计算变换矩阵，此外，随着测量更新，当计算误差达到设定阈值或计算周期达到设定时限时，需要重新计算变换矩阵，以便不断迭代更新变换矩阵，使全局位姿的计算精度满足要求。

步骤4、采用变换矩阵将空间位姿转换为待定位机器人移动过程中的实时全局位姿。

采用步骤3计算得到的变换矩阵，将步骤2中得到空间位姿转换为实时全局位姿。

综上，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种基于多传感器融合的移动机器人定位方法，其特征在于，包括以下步骤：

步骤1、将多个传感器安装至待定位机器人机体上；根据所述传感器获取的数据，采用视觉惯性联合位姿估计得到所述待定位机器人的初步空间位姿，采用激光里程计得到待定位机器人的初步定位数据；

步骤2、采用扩展卡尔曼滤波将所述初步空间位姿及初步定位数据融合为所述待定位机器人的空间位姿；若当前计算为首次计算或到达更新周期时，执行步骤3；否则，执行步骤4；

步骤3、根据全局空间位姿坐标系计算出所述待定位机器人的全局测量位姿，求解相同时刻的所述空间位姿与全局测量位姿之间的变换矩阵；

步骤4、采用所述变换矩阵将所述空间位姿转换为所述待定位机器人移动过程中的实时全局位姿。

2.根据权利要求1所述的方法，其特征在于，所述步骤3中的所述全局空间位姿坐标系为采用二维码建立的全局空间位姿坐标系。

3.根据权利要求2所述的方法，其特征在于，所述步骤3中求解相同时刻的所述空间位姿与全局测量位姿之间的变换矩阵，包括以下步骤：

采用最小二乘法求解实现所述空间位姿与全局测量位姿之间的变换矩阵，设置如下残差函数：

其中，P_i为所述待定位机器人的i时刻的所述空间位姿，P_i-1为所述待定位机器人的i-1时刻的所述空间位姿，

为所述待定位机器人的相邻两时刻空间位姿增量，P_m为m时刻所述空间位姿通过所述变换矩阵计算得到的实时全局位姿，G_m为m时刻所述待定位机器人的全局测量位姿，∑_i为i时刻所述空间位姿对应的协方差，∑_g为所述全局测量位姿对应的协方差，

为m时刻与n时刻所述空间位姿与通过所述变换矩阵计算得到的实时全局位姿之间的残差。

Images