CN109828588A - 一种基于多传感器融合的机器人室内路径规划方法 - Google Patents

Abstract

一种基于多传感器融合的机器人室内路径规划方法，机器系统在使用视觉定位与建图(SLAM)技术重建环境地图后，使用多传感器协调工作方式，使机器人朝在地图中确定的目标地点进行路径规划，并在运行途中能够根据系统搭载的传感器获取的环境数据实现避障与行驶中的位姿校正以及运行方向的修正。为了能够有效验证方法的有效性，测试所使用机器人移动平台搭载了深度相机、超声波传感器、以及惯性传感器等设备。通过在室内环境进行多次测试证明多传感器协同工作在机器人基于室内工作环境进行移动路径规划中能够提高建图精度、并有效实现机器的自主路径规划。

Description

一种基于多传感器融合的机器人室内路径规划方法

技术领域

本发明涉及到机械控制、图像处理等领域，更确切地说是基于多传感器协作的自主路径规划方法,对于在室内环境中机器人进行路径规划时存在的位姿误差与路径偏移等问题，本发明提出基于多传感器协同工作的方法来尝试提高机器人在室内环境中进行路径规划的鲁棒性与精确度。

背景技术

移动机器人作为一个集成环境感知、路径规划、动态决策与工作指令执行等多重功能的综合系统，可以协助完成复杂冗长的工作，提高工作的效率与安全性。

目前基于视觉传感器实现机器人的图像处理与路径规划是移动机器人常用的一种处理环境信息进行路径规划的方法。但只依赖单传感器运行的情况下，系统容易受到传感器性能与环境因素的限制，仅就视觉传感器而言其运行时的丢帧与图像模糊会对系统处理的效果产生影响，且在低复杂度的环境中运行会导致获取图像的纹理相对稀疏，导致图像特征难以提取而丢失跟踪。为此需要通过多传感器协同的方法获取环境数据以便精准地对环境信息进行处理与反馈，增强系统的检测与处理能力，提高系统运行的信任度。

为了应对移动机器人动态变化的位姿估计需求，需要系统在全面、快速地获取环境信息的同时能对位姿数据进行精确计算，目前常用的方法包括了以视觉 SLAM为主并依靠激光雷达协助检测环境信息或是使用GPS获取地图数据协助系统进行路径规划的方法。而在室内环境中，为了保证机器人系统工作的可信度，需要利用多种传感器测量冗余并互补传感器信息。

发明内容

为了确保机器可以通过多传感器协同的方式提高运行时的位姿精度与路径规划的效率，保证机器人系统在室内环境中工作的可信度，能针对移动机器人的运行需求、使多种传感器能够在同一系统内相互弥补、共同工作，本发明提供一种基于多传感器融合的机器人室内路径规划方法，使各传感器能够产生扬长避短的效果，机器系统的路径规划功能能够满足鲁棒性与实时性的需求。

为解决技术问题，本发明所采用的技术方案为：

一种基于多传感器融合的机器人室内路径规划方法，所述方法包括以下步骤：

1)将视觉传感器用于建图工作，将视觉SLAM作为室内建图应用基础，采用ORBSLAM作为建图的基础方式，ORBSLAM提取特征点是基于FAST特征，并使用 BRIEF描述子描述特征方向性，因此在建图时能够快速提取相机图像特征并基于特征进行多项操作，确保建图的高效性；

2)将深度相机获取的点云数据作为在环境中获取可行驶区域的可靠依据，点云对应于环境中具体物件的三维模型，能还原空间中物体与场景的各种细节，使系统能够对环境做出精准的反馈，在机器进行路径规划时可以根据环境内的点云数据判断可能构成阻碍的障碍物并做出避让，以免因为意外碰撞而产生路径偏移；

3)根据环境信息与在地图内设定的目标点位置规划行进路线，将快速随机生成树法作为系统规划运动路径的方法，该方法的目的是从机器人所处区域的初始位姿q_init到目标点位姿q_goal之间找到一条能够顺利运行的路径，系统将机器人的初始位姿作为该方法中的根节点，逐渐在空间内增加可移动路径的叶节点，通过连接路径节点来获取地图中的可行驶路径；

4)在机器人系统运行期间将惯导传感器获取的位移信息与视觉SLAM获取的位姿数据结合，为了应对机器在工作时环境随着机器移动而动态变化的工作特性采用将高频率输出数据的惯导传感器与视觉传感器结合的方法，惯导可以在机器人移动时精准获取机器人的六自由度数据，将其与视觉SLAM获取的位姿数据结合，使用卡尔曼滤波器的离散控制系统针对系统当前的位姿数据与测量获取的运行速率预测下一时刻机器的新位姿与系统的状态协方差矩阵，并更新校正位姿数据，减小在路径规划期间系统产生的位姿偏差或是由传感器不断运行所导致的累积运动误差产生的影响，为了防止惯导设备高频输出导致产生累积误差的问题，卡尔曼滤波使用输出频率相对较低的视觉SLAM的位姿数据作为观测值，结合惯导获取的位移数据可以在优化位姿的同时避免运行中累积误差的产生，同时也防止卡尔曼滤波处理惯导高频输出数据而产生处理负荷；

5)在机器进行路径规划的途中如果出现需要进行避障的情况，则在完成避障之后需要系统校正由避障导致的运动方向偏移，因此需要机器搭载的超声波传感器获取避障之后的环境物体距离信息，根据环境内障碍物的距离信息以及当前机器与地图内目标点之间的方向与距离信息校正机器的运行路线，而在机器正常行驶期间超声波传感器停止采集环境数据，减少系统处理的数据内容，避免为运行增加不必要的负荷；

6)重复1)～5)直至机器抵达地图中的指定目标点。

进一步，在所述步骤2)中，在机器人系统运行期间将惯导传感器IMU获取的位移信息与视觉SLAM以松耦合的方式结合，采用松耦合的系统内各项传感器依然可以作为独立的模块运行；使用卡尔曼滤波器的离散控制系统针对系统当前的位姿数据与测量获取的运行速率预测下一时刻机器的新位姿与系统的状态协方差矩阵，IMU使用的高精度加速度计与陀螺仪可以在机器运动时精准获取物体在空间的加速度数据与其横滚、俯仰、偏航的角度信息，根据IMU获取的机器当前时刻的六自由度数据可以计算系统的误差状态量与系统协方差矩阵的最优估计值，根据系统的卡尔曼增益值融合IMU获取的测量数据与滤波器估算的结果计算当前时刻的状态估计值，以此获取当前系统状态协方差矩阵的更新值；

而由于IMU设备高频率输出运动信息的特性，在长期运行的路径规划过程中容易产生累积误差，因此将视觉SLAM以较低频率输出的位姿数据作为卡尔曼滤波计算的观测值输入；之后可根据系统计算的当前时刻的观测矩阵与观测噪声方差矩阵可以不断更新系统内卡尔曼滤波输出的位姿数据，以这一方式不断迭代获取的数据可用于在机器移动运行过程中对系统内的位姿信息进行校正。

再进一步，在所述步骤4)中，快速识别可行驶区域是机器人实现自主路径规划的前提，在规划机器人行进路线时基于深度相机获取的点云数据，可以对环境内的点云进行平面分割来获取可行驶区域的位置,即对点云数据做降采样之后通过计算点云平面的平面法线，根据法线空间分割的结果，对空间内的点云进行聚类来，并根据空间内点云平面与相机坐标系原点之间的距离划分不同的点云平面,由于可行驶区域平面在可识别的平面内高度最低且与相机坐标系水平面平行，在系统运行可以将符合上述条件的点云平面作为可行驶区域，并可将该平面作为参考平面，判断超出可行驶区域平面一定高度的物体点云为行进路线上的障碍物。

在所述步骤5)中，为了应对系统在运行时由于避障所产生的路径偏差，需要系统根据环境信息与目标位置重新规划行进方向，为此在机器平台上搭载了超声波传感器用于获取环境障碍物的信息以使用人工势场法校正机器运行的方向，将机器放置于虚构的力场内，设目标点对机器产生始终指向目标点的引力,表示为：

其中ξ代表尺度因子，而ρ(q，q_goal)为当前机器与目标间的距离，即引力会随着机器人与目标点的距离增加而单调递增；

同时设环境内的障碍物对机器产生虚拟的斥力以避免发生碰撞，表示为：

η为斥力的尺度因子，ρ(q，q_obs)则表示物体与障碍物之间的距离，而ρ₀则代表环境中障碍物的影响半径，斥力随物体与障碍物之间的距离增大而逐渐减小，一旦物体与障碍物的距离超过障碍物的影响半径，则障碍物对物体不会继续产生斥力影响，在确定机器完成避障之后结合系统在当前位置所受的斥力与引力，可以重新确认机器人的移动方向与行进路线，同时由于超声波传感器采集周围环境信息并将数据交于系统处理仅限于确认机器完成避障操作的执行之后，在无需避障时超声波传感器不会进行工作，因此不会大幅度增加系统运行时的负荷。

本发明的有益效果主要表现在:多传感器的协同工作能够使各传感器弥补相互之间运行的缺陷，在确保系统工作的计算消耗不会过度提高的情况下，使机器在综合系统的作用下进行路径规划时能有可观的位姿精度与运行效果。

附图说明

图1是视觉惯导姿态的真值测量图。

图2是系统基于相机、基于惯导传感器以及多传感器融合之后运行时获取的位姿数据的对比图。

具体实施方式

下面结合附图对本发明做进一步说明。

一种基于多传感器融合的机器人室内路径规划方法，所述方法包括以下步骤：

1)将视觉传感器用于建图工作，将视觉SLAM作为室内建图应用基础，采用ORBSLAM作为建图的基础方式，ORBSLAM提取特征点是基于FAST特征，并使用 BRIEF描述子描述特征方向性，因此在建图时能够快速提取相机图像特征并基于特征进行多项操作，确保建图的高效性；

2)将深度相机获取的点云数据作为在环境中获取可行驶区域的可靠依据，点云对应于环境中具体物件的三维模型，能还原空间中物体与场景的各种细节，使系统能够对环境做出精准的反馈，在机器进行路径规划时可以根据环境内的点云数据判断可能构成阻碍的障碍物并做出避让，以免因为意外碰撞而产生路径偏移；

3)根据环境信息与在地图内设定的目标点位置规划行进路线，将快速随机生成树法作为系统规划运动路径的方法，该方法的目的是从机器人所处区域的初始位姿q_init到目标点位姿q_goal之间找到一条能够顺利运行的路径，系统将机器人的初始位姿作为该方法中的根节点，逐渐在空间内增加可移动路径的叶节点，通过连接路径节点来获取地图中的可行驶路径；

4)在机器人系统运行期间将惯导传感器获取的位移信息与视觉SLAM获取的位姿数据结合，为了应对机器在工作时环境随着机器移动而动态变化的工作特性采用将高频率输出数据的惯导传感器与视觉传感器结合的方法，惯导可以在机器人移动时精准获取机器人的六自由度数据，将其与视觉SLAM获取的位姿数据结合，使用卡尔曼滤波器的离散控制系统针对系统当前的位姿数据与测量获取的运行速率预测下一时刻机器的新位姿与系统的状态协方差矩阵，并更新校正位姿数据，减小在路径规划期间系统产生的位姿偏差或是由传感器不断运行所导致的累积运动误差产生的影响，为了防止惯导设备高频输出导致产生累积误差的问题，卡尔曼滤波使用输出频率相对较低的视觉SLAM的位姿数据作为观测值，结合惯导获取的位移数据可以在优化位姿的同时避免运行中累积误差的产生，同时也防止卡尔曼滤波处理惯导高频输出数据而产生处理负荷；

5)在机器进行路径规划的途中如果出现需要进行避障的情况，则在完成避障之后需要系统校正由避障导致的运动方向偏移，因此需要机器搭载的超声波传感器获取避障之后的环境物体距离信息，根据环境内障碍物的距离信息以及当前机器与地图内目标点之间的方向与距离信息校正机器的运行路线，而在机器正常行驶期间超声波传感器停止采集环境数据，减少系统处理的数据内容，避免为运行增加不必要的负荷；

6)重复1)～5)直至机器抵达地图中的指定目标点。

所述步骤2)中，在机器人系统运行期间将惯导传感器(Inertial measurementunit,以下略写IMU)获取的位移信息与视觉SLAM以松耦合的方式结合，采用松耦合的系统内各项传感器依然可以作为独立的模块运行。使用卡尔曼滤波器的离散控制系统针对系统当前的位姿数据与测量获取的运行速率预测下一时刻机器的新位姿与系统的状态协方差矩阵，IMU使用的高精度加速度计与陀螺仪可以在机器运动时精准获取物体在空间的加速度数据与其横滚、俯仰、偏航的角度信息，根据IMU获取的机器当前时刻的六自由度数据可以计算系统的误差状态量与系统协方差矩阵的最优估计值，根据系统的卡尔曼增益值融合IMU获取的测量数据与滤波器估算的结果计算当前时刻的状态估计值，以此获取当前系统状态协方差矩阵的更新值。

而由于IMU设备高频率输出运动信息的特性，在长期运行的路径规划过程中容易产生累积误差，因此将视觉SLAM以较低频率输出的位姿数据作为卡尔曼滤波计算的观测值输入。之后可根据系统计算的当前时刻的观测矩阵与观测噪声方差矩阵可以不断更新系统内卡尔曼滤波输出的位姿数据，以这一方式不断迭代获取的数据可用于在机器移动运行过程中对系统内的位姿信息进行校正。图1为运行时测量的机器人姿态真值数据的记录。图2为系统基于相机、基于惯导传感器以及多传感器融合之后运行时获取的位姿数据的对比。

所述步骤4)中，快速识别可行驶区域是机器人实现自主路径规划的前提，在规划机器人行进路线时基于深度相机获取的点云数据，可以对环境内的点云进行平面分割来获取相机坐标系内可行驶区域的位置,即对点云数据做降采样之后通过计算点云平面的平面法线，根据法线空间分割的结果，对空间内的点云进行聚类来，并根据空间内点云平面与相机坐标系原点之间的距离划分不同的点云平面,由于可行驶区域平面一般在可识别的平面内高度最低且与相机坐标系水平面平行，在系统运行可以将符合上述条件的点云平面作为可行驶区域，并可将该平面作为参考平面，判断超出可行驶区域平面一定高度的物体点云为行进路线上的障碍物。

所述步骤5)中，为了应对系统在运行时由于避障所产生的路径偏差，需要系统根据环境信息与目标位置重新规划行进方向，为此在机器平台上搭载了超声波传感器用于获取环境障碍物的信息以使用人工势场法校正机器运行的方向：将机器放置于虚构的力场内，设目标点对机器产生始终指向目标点的引力,表示为:

其中ξ代表尺度因子，而ρ(q,q_goal)为当前机器与目标间的距离，即引力会随着机器人与目标点的距离增加而单调递增。

同时设环境内的障碍物对机器产生虚拟的斥力以避免发生碰撞，表示为:

η为斥力的尺度因子，ρ(q,q_obs)则表示物体与障碍物之间的距离，而ρ₀则代表环境中障碍物的影响半径，斥力随物体与障碍物之间的距离增大而逐渐减小，一旦物体与障碍物的距离超过障碍物的影响半径，则障碍物对物体不会继续产生斥力影响，在确定机器完成避障之后结合系统在当前位置所受的斥力与引力，可以重新确认机器人的移动方向与行进路线。同时由于超声波传感器采集周围环境信息并将数据交于系统处理仅限于确认机器完成避障操作的执行之后，在无需避障时超声波传感器不会进行工作，因此不会大幅度增加系统运行时的负荷。

Claims (4)

1.一种基于多传感器融合的机器人室内路径规划方法，其特征在于：所述内容包括以下步骤：

1)将视觉传感器用于建图工作，将视觉SLAM作为室内建图应用基础，采用ORBSLAM作为建图的基础，ORBSLAM提取特征点是基于FAST特征，并使用BRIEF描述子描述特征方向性，因此在建图时能够快速提取相机图像特征并基于特征进行多项操作，确保建图的高效性；

2)将深度相机获取的点云数据作为在环境中获取可行驶区域的可靠依据，点云对应于环境中具体物件的三维模型，能还原空间中物体与场景的各种细节，使系统能够对环境做出精准的反馈；在机器进行路径规划时可以根据环境内的点云数据判断可能构成阻碍的障碍物并做出避让，以免因为意外碰撞而产生路径偏移；

3)根据环境信息与在地图内设定的目标点位置规划行进路线，将快速随机生成树法作为系统规划运动路径的方法，该方法的目的是从机器人所处区域的初始位姿q_init到目标点q_goal位姿之间找到一条能够顺利运行的路径；将机器人的初始位姿作为该方法中的根节点，逐渐在空间内增加可移动路径的叶节点，通过连接路径节点来获取地图中的可行驶路径；

4)在机器人系统运行期间将惯导传感器获取的位移信息与视觉SLAM获取的位姿数据结合，为了应对机器在工作时环境随着机器移动而动态变化的工作特性采用将高频率输出数据的惯导传感器与视觉传感器结合的方法，惯导可以在机器人移动时精准获取机器人的六自由度数据，将其与视觉SLAM获取的位姿数据结合，使用卡尔曼滤波器的离散控制系统针对系统当前的位姿数据与测量获取的运行速率预测下一时刻机器的新位姿与系统的状态协方差矩阵，并更新校正位姿数据，减小在路径规划期间系统产生的位姿偏差或是由传感器不断运行所导致的累积运动误差产生的影响；卡尔曼滤波使用输出频率相对较低的视觉SLAM的位姿数据作为观测值，结合惯导获取的位移数据可以在优化位姿的同时避免运行中累积误差的产生，同时也防止卡尔曼滤波处理惯导高频输出数据而产生处理负荷；

5)在机器进行路径规划的途中如果出现需要进行避障的情况，则在完成避障之后需要系统校正由避障导致的运动方向偏移，因此需要机器搭载的超声波传感器获取避障之后的环境物体距离信息，根据环境内障碍物的距离信息以及当前机器与地图内目标点之间的方向与距离信息校正机器的运行路线，而在机器正常行驶期间超声波传感器停止采集环境数据，减少系统处理的数据内容，避免为运行增加不必要的负荷；

6)重复1)～5)直至机器抵达地图中的指定目标点。

2.如权利要求1所述的基于多传感器融合的机器人室内路径规划方法，其特征在于：在所述步骤2)中，在机器人系统运行期间将惯导传感器IMU获取的位移信息与视觉SLAM以松耦合的方式结合，采用松耦合的系统内各项传感器依然可以作为独立的模块运行；使用卡尔曼滤波器的离散控制系统针对系统当前的位姿数据与测量获取的运行速率预测下一时刻机器的新位姿与系统的状态协方差矩阵，IMU使用的高精度加速度计与陀螺仪可以在机器运动时精准获取物体在空间的加速度数据与其横滚、俯仰、偏航的角度信息，根据IMU获取的机器当前时刻的六自由度数据可以计算系统的误差状态量与系统协方差矩阵的最优估计值，根据系统的卡尔曼增益值融合IMU获取的测量数据与滤波器估算的结果计算当前时刻的状态估计值，以此获取当前系统状态协方差矩阵的更新值；

而由于IMU设备高频率输出运动信息的特性，在长期运行的路径规划过程中容易产生累积误差，因此将视觉SLAM以较低频率输出的位姿数据作为卡尔曼滤波计算的观测值输入；之后可根据系统计算的当前时刻的观测矩阵与观测噪声方差矩阵可以不断更新系统内卡尔曼滤波输出的位姿数据，以这一方式不断迭代获取的数据可用于在机器移动运行过程中对系统内的位姿信息进行校正。

3.如权利要求1或2所述的基于多传感器融合的机器人室内路径规划方法，其特征在于：在所述步骤4)中，快速识别可行驶区域是机器人实现自主路径规划的前提，在规划机器人行进路线时基于深度相机获取的点云数据，可以对环境内的点云进行平面分割来获取可行驶区域的位置,即对点云数据做降采样之后通过计算点云平面的平面法线，根据法线空间分割的结果，对空间内的点云进行聚类来，并根据空间内点云平面与相机坐标系原点之间的距离划分不同的点云平面,由于可行驶区域平面在可识别的平面内高度最低且与相机坐标系水平面平行，在系统运行可以将符合上述条件的点云平面作为可行驶区域，并可将该平面作为参考平面，判断超出可行驶区域平面一定高度的物体点云为行进路线上的障碍物。

4.如权利要求1或2所述的基于多传感器融合的机器人室内路径规划方法，其特征在于：在所述步骤5)中，为了应对系统在运行时由于避障所产生的路径偏差，需要系统根据环境信息与目标位置重新规划行进方向，为此在机器平台上搭载了超声波传感器用于获取环境障碍物的信息以使用人工势场法校正机器运行的方向，将机器放置于虚构的力场内，设目标点对机器产生始终指向目标点的引力,表示为：

其中ξ代表尺度因子，而ρ(q,q_goal)为当前机器与目标间的距离，即引力会随着机器人与目标点的距离增加而单调递增；

同时设环境内的障碍物对机器产生虚拟的斥力以避免发生碰撞，表示为：

η为斥力的尺度因子，ρ(q,q_obs)则表示物体与障碍物之间的距离，而ρ₀则代表环境中障碍物的影响半径，斥力随物体与障碍物之间的距离增大而逐渐减小，一旦物体与障碍物的距离超过障碍物的影响半径，则障碍物对物体不会继续产生斥力影响，在确定机器完成避障之后结合系统在当前位置所受的斥力与引力，可以重新确认机器人的移动方向与行进路线，同时由于超声波传感器采集周围环境信息并将数据交于系统处理仅限于确认机器完成避障操作的执行之后，在无需避障时超声波传感器不会进行工作，因此不会大幅度增加系统运行时的负荷。