CN111474932B - 一种集成情景经验的移动机器人建图与导航方法 - Google Patents

Abstract

一种集成情景经验的移动机器人建图与导航方法，属于移动机器人导航领域。该方法基于系统框架实现，系统框架包含三个模块：点云地图、情景经验地图与路径规划移动机器人导航过程中，点云地图、情景经验地图与路径规划三个模块基于机器人操作系统ROS通信，其中，点云地图为情景经验地图实时提供移动机器人的位姿；路径规划模块根据点云地图获取移动机器人位姿，根据情景经验地图获取事件序列，从而定位移动机器人当前所在事件。本发明对情景经验进行建模，结合局部环境信息构建情景经验地图，完成移动机器人对环境在拓扑关系与几何关系上理解，该系统能够较好的完成导航任务，规划出一条从定位点到达目标点的最优路径，并且具有较高的导航精度。

Description

一种集成情景经验的移动机器人建图与导航方法

技术领域

本发明属于移动机器人导航领域，涉及一种基于情景经验的移动机器人建图与导航方法。

背景技术

自主导航技术作为移动机器人系统的核心能力，智能化水平要求越来越高。其研究内容包括建图、定位、路径规划与控制。目前，多数研究工作作为独立的模块展开，不能够简单的将各个模块的研究成果直接组合成为一个导航系统，例如，视觉SLAM算法生成的地图与路径规划的地图存在明显差异。

为实现在环境中自主导航，移动机器人应当被赋予理解世界等能力，其在环境中时刻掌握自己在世界坐标系下的位姿，并且能够根据特定的目标规划适当路径。可以通过向移动机器人提供一个环境详细描述的高精度地图来实现移动机器人导航，然而在实际条件中，高精度地图的构建、优化、存储与更新具有较高的计算成本，且很多信息对于导航任务冗余。根据神经学家观察，人类不需要回忆所有的环境细节就能够轻松完成导航任务，往往依靠自己以前的认知经验进行路径规划与导航，这又从逻辑上质疑了移动机器人完成自主导航任务依赖于高精度环境地图。

发明内容

针对上述问题，本发明提出一种集成情景经验的移动机器人建图与导航方法。

本发明采用的技术方案为：

一种集成情景经验的移动机器人建图与导航方法，该方法基于系统框架实现，系统框架包含三个模块：点云地图、情景经验地图与路径规划。简化人类情景记忆形成过程，构建封装场景感知、位姿信息和事件转移集的情景经验地图，从几何与拓扑关系上理解环境。移动机器人根据点云地图与情景经验地图实现路径规划，基于机器人操作系统(RobotOperatingSystem，ROS)通信各个模块从而完成导航任务。具体步骤如下：

第一步，构建点云地图。

点云地图的构建基于特征点法的前端和基于优化的后端，用于定位移动机器人在环境中的位置并且辅助构建情景经验地图。

第二步，构建情景经验地图。

建模由λ个子事件构成的事件空间E，E＝{e₁,e₂,…,e_λ}。每个子事件包含场景感知、位姿和事件转移集，建模为三元组(o,P,C)：其中，o为场景感知，由相机采集场景图像并提取环境显著路标；P为位姿信息(x,y,yaw)，表达事件节点在世界坐标下的位置和朝向信息；C为事件转移集，描述从当前事件可直接转移到达事件集合，由激光测量数据计算得到。三元组

即任意情景经验对应唯一事件。事件转移权值矩阵W为：

由此，情景经验地图M可表示为离散有限的事件空间E和事件转移权值矩阵W组成的网络：

M＝{E,W}， (2)

基于移动机器人场景感知传感器和点云地图定位系统，获取以事件空间E为顶点、事件转移权值为边的增量式情景经验，构建情景经验地图，具体过程如下：

(1)整合位姿

通过图像提取环境路标点与地图点匹配，定位当前相机位姿^wO_c。将相机位姿转换到移动机器人位姿^wO_r，根据手眼标定获取相机坐标系到移动机器人坐标系的变换矩阵T_RC：

其中，R为旋转矩阵，t是平移向量。

计算得到移动机器人的定位：^wO_r＝T_RC·^wO_c； (4)

(2)确定事件转移集

使用激光传感器判断事件之间是否可达，确定当前事件的转移集。激光传感器对于单点的测量数据为：点到激光传感器距离

激光束与激光传感器横坐标夹角

k表示一周扫描中激光束的编号。激光传感器中心在移动机器人坐标系的坐标为

移动机器人坐标系在世界坐标系下的位姿为

根据“激光-移动机器人”坐标关系可将激光对单点的测量数据

转换为世界坐标下的坐标

当前状态神经元e_c为圆心，在半径D范围内获取事件转移集，即记录

的激光传感器扫描数据，根据公式(5)计算扫描点在世界坐标下的坐标，由此，得到该区域内障碍物边缘的采样点集：

为简化计算，设移动机器人为一个点，将事件间转移需要的空间按移动机器人半径r进行膨胀，即将移动机器人所占空间表示为事件转移需要的空间，进一步简化，将空间扩展为矩形。根据公式(6)、(7)可计算出矩形的四个顶点坐标：

其中，e_c＝[x_c,y_c]为当前事件位置，e_i＝[x_i,y_i]为转移范围内的某个事件位置。利用向量外积性质判断激光扫描点p是否在矩形内部。对任意矩形，如果集合S中存在点p满足向量关系：

则点p(即障碍物点)在事件转移需要的空间范围内，即事件间不能直接转移。

(3)计算事件转移权值

事件转移权值代表事件间转移代价，由两部分决定：一方面为事件间的欧式距离；另一方面为场景感知的匹配度。根据公式(9)、(10)计算得到：

ε_ct＝1-Match(o_c,o_t)， (9)

其中，o_c为当前事件观测得到的场景感知，o_t为其状态转移集中事件存储的环境场景感知，Match函数计算场景感知匹配度。Eucdis函数计算欧式距离，η为权值系数。

第三步，设计基于情景经验地图的路径规划算法。

基于情景经验地图设计移动机器人全局事件序列规划算法与局部行为控制算法。定位过程中，点云地图系统仅完成跟踪相机任务，地图信息不更新。当新的场景图像输入系统，定位相机位姿，计算移动机器人在世界坐标系下位姿(x,y,yaw)。给定导航任务，结合情景经验地图应用Dijkstra算法计算一条到达目标点的所有事件转移代价和最小的路径，即全局最优路径。导航过程中移动机器人在每一个事件节点利用激光测量信息更新事件转移集，一旦事件转移集被更新，则重新进行全局事件序列规划。局部行为控制根据移动机器人当前位姿与局部目标事件的位姿计算得到。

移动机器人导航过程中，点云地图、情景经验地图与路径规划三个模块基于机器人操作系统ROS通信，其中，点云地图为情景经验地图实时提供移动机器人的位姿；路径规划模块根据点云地图获取移动机器人位姿，根据情景经验地图获取事件序列，从而定位移动机器人当前所在事件。

本发明的有益效果为：本发明对情景经验进行建模，结合局部环境信息构建情景经验地图，完成移动机器人对环境在拓扑关系与几何关系上理解，该系统能够较好的完成导航任务，规划出一条从定位点到达目标点的最优路径，并且具有较高的导航精度。

附图说明

图1为本发明实例中导航系统框图；

图2为本发明实例中点云地图构建流程；

图3为本发明实例中情景经验地图构建流程；

图4为本发明实例中“相机-移动机器人”坐标关系；

图5为本发明实例中“激光-移动机器人-世界”坐标关系；

图6为本发明实例中事件转移集的计算方式；

图7为本发明实例中判断点在矩形内部的方式；

图8为本发明实例中当前位姿与局部目标事件位姿关系。

图9为本发明实例中构建的情景经验地图。

图10为本发明实例中根据情景经验地图规划路径结果。

图11为本发明实例中导航路线与规划路线对比结果。

具体实施方式

以下结合附图和技术方案，进一步说明本发明的具体实施方式。

图1为本发明实例中导航系统框图，如图1所示，点云地图被用于定位移动机器人并且辅助情景经验地图的构建。情景经验地图记录移动机器人的经验信息，形成对环境在几何关系与拓扑关系的综合表述。移动机器人基于情景经验地图进行路径规划，计算导航任务的事件序列。导航系统基于机器人操作系统ROS实现各个模块通信。

图2为本发明实例中点云地图构建流程，如图2所示，点云地图在移动机器人探索环境过程中增量式构建与优化，系统输入视频流，输出实时相机位姿跟踪与三维点云，系统包含前端跟踪与后端优化两部分。前端跟踪根据帧间像素关系估计相对位姿，本发明采用基于特征点法的视觉前端对每一帧图像提取特征点，然后根据特征匹配关系求解当前帧位姿的初始估计并对初始位姿估计进行调优。后端优化首先加入新的关键帧，优化更新局部地图，并对每一个关键帧做闭环检测处理，一旦检测到闭环，则使用全局位姿图优化纠正前端视觉里程计的累积误差。基于优化的后端算法将m帧初始位姿估计与n个路标点作为待优化状态量构建非线性最小二乘：

ξ_i表示第i帧相机位姿估计，p_j为第j个路标点，z_ij表示在相机位姿ξ_i处观测到路标点p_j像素坐标，即测量值.根据观测方程计算路标点p_j在位姿ξ_i下的重投影坐标h(ξ_i,p_j)，即重投影值。重投影误差

作为残差。该非线性最小二乘方程使用LM(Levenberg-Marquardt)算法求解，迭代过程中同时优化调整相机位姿与空间路标点位置。

图3为本发明实例中情景经验地图构建流程，如图3所示，情景经验地图构建系统输入点云地图和场景感知(包括视觉图像与激光测量)，输出为情景经验地图。建图过程中，根据点云地图进行定位，判断当前事件是否为新事件：如果是新事件，则记录事件，提取当前场景感知，整合位姿，根据激光测量确定事件转移集，结合当前场景与转移集中场景感知的匹配度计算事件转移权值矩阵；如果是已有事件，则直接更新当前事件与其对应的连接权值。

记录事件的具体过程骤如下：

(1)整合位姿

相机与移动机器人位姿关系如图4所示，通过图像提取环境的路标点与地图点匹配，定位当前相机位姿^wO_c。将相机位姿转换到移动机器人位姿^wO_r，坐标关系如图4所示。根据手眼标定获取相机坐标系到移动机器人坐标系的变换矩阵T_RC：

其中，R为旋转矩阵，t是平移向量。计算得到移动机器人的定位：

^wO_r＝T_RC·^wO_c， (3)

(2)确定事件转移集

利用激光传感器判断事件之间是否可达，确定当前事件的转移集。激光传感器对于单点的测量数据为：点到激光传感器距离

激光束与激光传感器横坐标夹角

k表示一周扫描中激光束的编号。激光传感器中心在移动机器人坐标系的坐标为

移动机器人坐标系在世界坐标系下的位姿为

坐标关系如图5所示。根据坐标关系可将激光对单点的测量数据

转换为世界坐标下的坐标

当前状态神经元e_c为圆心，在半径D范围内获取事件转移集，如图6所示，即记录

的激光传感器扫描数据，根据公式(4)计算扫描点在世界坐标下的坐标，由此，得到该区域内障碍物边缘的采样点集：

为简化计算，设移动机器人为一个点，将事件间转移需要的空间按移动机器人半径r进行膨胀，即将移动机器人所占空间表示为事件转移需要的空间，进一步简化，将空间扩展为矩形，如图6中的虚线矩形。根据公式(5)、(6)可计算出矩形的四个顶点坐标：

其中，e_c＝[x_c,y_c]为当前事件位置，e_i＝[x_i,y_i]为转移范围内的某个事件位置。利用向量外积性质判断激光扫描点p是否在矩形内部。对任意矩形，如图7所示，如果集合S中存在点p满足向量关系：

则点p(即障碍物点)在事件转移需要的空间范围内，即事件间不能直接转移。例如在图6中，事件e_j在事件e_c的转移集中，e_i则不在其转移集中。

(3)计算事件转移权值

事件转移权值代表事件间转移代价，由两部分决定：一方面为事件间的欧式距离；另一方面为场景感知的匹配度。根据公式(8)、(9)计算得到：

ε_ct＝1-Match(o_c,o_t)， (8)

其中，o_c为当前事件观测得到的场景感知，o_t为其状态转移集中事件存储的环境场景感知，Match函数计算场景感知匹配度。Eucdis函数计算欧式距离，η为权值系数。

路径规划过程中，移动机器人采集场景感知，根据点云地图与情景经验地图定位当前事件。定位过程中，点云地图系统仅完成跟踪相机任务，地图信息不更新。当新的场景图像输入系统，定位相机位姿，计算移动机器人在世界坐标系下位姿(x,y,yaw)。给定导航任务，结合情景经验地图应用Dijkstra算法计算一条到达目标点的所有事件转移代价和最小的路径，即全局最优路径。导航过程中移动机器人在每一个事件节点利用激光测量信息更新事件转移集，一旦事件转移集被更新，则重新进行全局路径规划。局部行为控制根据移动机器人当前定位信息与局部目标事件的位姿关系计算得到，如图8所示，计算公式如下：

dist＝Eucdis(p_c,p_t)， (12)

其中，p_c为移动机器人当前位置，yaw_c为当前位置偏航角，p_t为下一目标点位置，angleWrap函数将各象限计算的角度映射到正确值，Eucdis函数计算两点之间的欧式距离。

图9为本发明实例中构建的情景经验地图，如图9所示，图中每个事件表示为一个圆圈，封装了场景感知、位姿信息和事件转移集。每一个事件与其转移集中的任意一个事件有一个边连接，其转移代价存储在事件转移权值矩阵中。

图10为本发明实例中基于构建的情景经验地图的路径规划结果，如图10所示，移动机器人分别执行两个导航任务，在定位点接收到任务信息，执行全局事件序列规划，任务1与任务2规划事件序列分别包含13个事件。

图11为本发明实例中移动机器人的导航路线与规划路线对比结果，如图11所示，移动机器人基于路径规划结果进行导航实验，实验结果表明：基于本发明设计的建图与导航方法，移动机器人具有较高的导航精度。

最后应说明的是：以上实例仅用以说明本发明的技术方案，而非对其进行限制；尽管参照本实施实例对本发明进行了详细的说明，本领域的普通人员应当理解；其依然可以对前述各实例所记载的技术方案进行修改，或者对其中部分技术特征进行同等替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实例技术方案的精神和范围。

Claims (1)

1.一种集成情景经验的移动机器人建图与导航方法，其特征在于，该方法基于系统框架实现，系统框架包含三个模块：点云地图、情景经验地图与路径规划，移动机器人导航过程中，点云地图、情景经验地图与路径规划三个模块基于机器人操作系统ROS通信，其中，点云地图为情景经验地图实时提供移动机器人的位姿，路径规划模块根据点云地图获取移动机器人位姿，根据情景经验地图获取事件序列，定位移动机器人当前所在事件；具体步骤如下：

第一步，构建点云地图；

点云地图的构建基于特征点法的前端和基于优化的后端，用于定位移动机器人在环境中的位置并且辅助构建情景经验地图；

第二步，构建情景经验地图；

建模由λ个子事件构成的事件空间E，E＝{e₁,e₂,…,e_λ}；每个子事件包含场景感知、位姿和事件转移集，建模为三元组(o,P,C)：其中，o为场景感知，由相机采集场景图像并提取环境显著路标；P为位姿信息(x,y,yaw)，表达事件节点在世界坐标下的位置和朝向信息；C为事件转移集，描述从当前事件可直接转移到达事件集合，由激光测量数据计算得到；三元组

即任意情景经验对应唯一事件；事件转移权值矩阵W为：

由此，情景经验地图M可表示为离散有限的事件空间E和事件转移权值矩阵W组成的网络：

M＝{E,W}， (2)

基于移动机器人场景感知传感器和点云地图定位系统，获取以事件空间E为顶点、事件转移权值为边的增量式情景经验，构建情景经验地图，具体过程如下：

(1)整合位姿

通过图像提取环境路标点与地图点匹配，定位当前相机位姿^wO_c；将相机位姿转换到移动机器人位姿^wO_r，根据手眼标定获取相机坐标系到移动机器人坐标系的变换矩阵T_RC：

其中，R为旋转矩阵，t是平移向量；

计算得到移动机器人的定位：^wO_r＝T_RC·^wO_c； (4)

(2)确定事件转移集

使用激光传感器判断事件之间是否可达，确定当前事件的转移集；激光传感器对于单点的测量数据为：点到激光传感器距离

激光束与激光传感器横坐标夹角

其中k表示一周扫描中激光束的编号；激光传感器中心在移动机器人坐标系的坐标为

移动机器人坐标系在世界坐标系下的位姿为

根据“激光-移动机器人”坐标关系可将激光对单点的测量数据

转换为世界坐标下的坐标

当前状态神经元e_c为圆心，在半径D范围内获取事件转移集，即记录

的激光传感器扫描数据，根据公式(5)计算扫描点在世界坐标下的坐标，由此，得到该区域内障碍物边缘的采样点集：

为简化计算，设移动机器人为一个点，将事件间转移需要的空间按移动机器人半径r进行膨胀，即将移动机器人所占空间表示为事件转移需要的空间，进一步简化，将空间扩展为矩形；根据公式(6)、(7)可计算出矩形的四个顶点坐标：

其中，e_c＝[x_c,y_c]为当前事件位置，e_i＝[x_i,y_i]为转移范围内的某个事件位置；利用向量外积性质判断激光扫描点p是否在矩形内部；对任意矩形，如果集合S中存在点p满足向量关系：

则障碍物点p在事件转移需要的空间范围内，即事件间不能直接转移；

(3)计算事件转移权值

事件转移权值代表事件间转移代价，由两部分决定：一方面为事件间的欧式距离；另一方面为场景感知的匹配度；根据公式(9)、(10)计算得到：

ε_ct＝1-Match(o_c,o_t)， (9)

其中，o_c为当前事件观测得到的场景感知，o_t为其状态转移集中事件存储的环境场景感知，Match函数计算场景感知匹配度；Eucdis函数计算欧式距离，η为权值系数；

第三步，设计基于情景经验地图的路径规划算法；

基于情景经验地图设计移动机器人全局事件序列规划算法与局部行为控制算法；定位过程中，点云地图系统仅完成跟踪相机任务，地图信息不更新；当新的场景图像输入系统，定位相机位姿，计算移动机器人在世界坐标系下位姿(x,y,yaw)；给定导航任务，结合情景经验地图计算一条到达目标点的所有事件转移代价和最小的路径，即全局最优路径；导航过程中移动机器人在每一个事件节点利用激光测量信息更新事件转移集，一旦事件转移集被更新，则重新进行全局事件序列规划；局部行为控制根据移动机器人当前位姿与局部目标事件的位姿计算得到。

Images