一种融合多传感器的移动机器人室内地图构建方法
阅读说明：本技术 一种融合多传感器的移动机器人室内地图构建方法 (Mobile robot indoor map construction method fusing multiple sensors ) 是由 刘冉 秦正泓 张华� 何永平 肖宇峰 付文鹏 张静 刘满禄 于 2019-09-26 设计创作，主要内容包括：本发明公开了一种融合多传感器的移动机器人室内地图构建方法,包括以下步骤：通过UWB、里程计和激光雷达分别采集移动机器人与锚点的距离信息、移动机器人的位姿信息和环境信息；根据距离信息、位姿信息和环境信息,构建顶点-约束图；通过图优化算法对顶点-约束图进行优化,获取优化后的移动机器人轨迹数据；通过优化后的移动机器人轨迹数据和环境信息构建栅格地图。本发明的里程计提供了机器人短时间内精确的位姿变化,融合UWB定位信息能够提供长时间精确的位姿变化,结合激光雷达的数据进行地图构建,解决了激光雷达在构建室内复杂环境地图时精度不高的问题。(The invention discloses a construction method of an indoor map of a mobile robot fusing multiple sensors, which comprises the following steps: respectively acquiring distance information between the mobile robot and an anchor point, pose information of the mobile robot and environment information through a UWB (ultra wide band), a speedometer and a laser radar; constructing a vertex-constraint graph according to the distance information, the pose information and the environment information; optimizing the vertex-constraint graph through a graph optimization algorithm to obtain optimized track data of the mobile robot; and constructing a grid map through the optimized mobile robot track data and the environment information. The odometer provided by the invention provides accurate pose change of the robot in a short time, integrates UWB positioning information to provide accurate pose change for a long time, and performs map construction by combining data of the laser radar, thereby solving the problem of low precision of the laser radar in constructing an indoor complex environment map.)
技术领域
本发明属于移动机器人室内建图技术领域，具体涉及一种融合多传感器的移动机器人室内地图构建方法。
背景技术
近年来，移动机器人技术在工业领域、医疗领域和服务领域都发挥了重要作用，且在国防和空间探测领域等有害与危险场合也得到了很好的应用。在移动机器人的研究领域中，SLAM一直是一个热门的研究话题，它为机器人提供导航地图和实时位置，而这些都是机器人执行路径规划、路径跟踪的前提，因此它在移动机器人导航中占有十分重要的位置。
由于激光雷达具有精度高、范围广和传输速度快的优点，激光雷达在移动机器人导航应用中日益广泛，基于激光扫描系统的室内环境地图的构建技术被广泛地运用到机器人的导航中，常用于机器人定位，环境地图的构建以及路径规划。一般的激光扫描仪价格十分昂贵，虽然目前市面上有很多便宜的激光出现，但是往往测量范围有限，而且分辨率较低。机器人的里程计往往可以通过光电编码器获取，其所带来的误差，会随着时间的增长越来越大，从而造成机器人位姿估计出现严重偏差。所以二维激光雷达在面对复杂的室内环境时建图精度会严重降低。
发明内容
针对现有技术中的上述不足，本发明提供的一种融合多传感器的移动机器人室内地图构建方法解决了现有技术室内建图精度低的问题。
为了达到上述发明目的，本发明采用的技术方案为：一种融合多传感器的移动机器人室内地图构建方法，包括以下步骤：
S1、通过UWB、里程计和激光雷达分别采集移动机器人与锚点的距离信息、移动机器人的位姿信息和环境信息；
S2、根据距离信息、位姿信息和环境信息，构建顶点-约束图；
S3、通过图优化算法对顶点-约束图进行优化，获取优化后的移动机器人轨迹数据；
S4、通过优化后的移动机器人轨迹数据和环境信息构建栅格地图。
进一步地，所述步骤S1包括以下步骤：
S1.1、在移动机器人上搭载UWB标签和激光雷达，所述机器人上设置有里程计和编码器；
S1.2、通过UWB标签采集移动机器人与锚点的距离信息；
S1.3、通过里程计获得机器人的位姿数据，所述里程计通过编码器获取数据；
S1.4、通过激光雷达扫描得到环境信息。
进一步地，所述步骤S2包括以下步骤：
S2.1、根据移动机器人的位姿信息构成顶点，构建基于里程计的边，得到第一初始图；
S2.2、在第一初始图上添加UWB约束，构建基于UWB的边，得到第二初始图；
S2.3、通过环境数据进行闭环检测，在第二初始图上添加基于激光的边，构建激光-闭环边，得到顶点-约束图。
进一步地，所述步骤S2.3中激光-闭环边的构建步骤如下：
A1、构建源点云集合Q＝{q1,q2,…,qN}和目标点云集合P＝{p1,p2,…,pN}；
A2、构建目标点云集合P的旋转矩阵R和平移矩阵T，通过旋转矩阵R和平移矩阵T构建目标函数E(R,T)；
A3、设定一个阈值，并判断E(R,T)是否小于阈值，若是则判断为闭环，并结束本次激光-闭环边的构建，若否则进入步骤A4；
A4、将旋转矩阵R和平移矩阵T代入源点云集合Q中，得到点集M；
A5、将点集M代入目标点云集合P，得到新的旋转矩阵R'和新的平移矩阵T'；
A6令R＝R'，T＝T'，将更新过后的R和T代入目标函数E(R,T)中，并返回步骤A3。
进一步地，所述目标函数E(R,T)为：
其中，N表示目标点云集合中点的总数，i＝1,2,...,N，qi表示源点云集合中的第i个点，pi表示目标点云集合中的第i个点。
进一步地，所述步骤S3中通过图优化算法对顶点-约束图进行优化的具体方法为：调整顶点-约束图中的位姿顶点，使位姿信息的误差函数F(x)最小，得到最大化满足约束的位姿顶点。
进一步地，所述误差函数F(x)为：
其中，xi表示位姿顶点i，xj表示位姿顶点j，C表示图的顶点间存在约束关系的集合，Ωij表示xi和xj之间观察值的信息矩阵，eij(xi,xj,zij)表示xi和xj满足约束zij的程度，zij表示位姿顶点i和位姿顶点j之间通过传感器获取的实际观察值，所述实际观察值zij包括相邻位姿顶点i与位姿顶点j之间的位姿变换、位姿顶点i与锚点j之间的距离和非相邻位姿顶点i与位姿顶点j之间的位姿变换。
进一步地，所述步骤S4包括以下分步骤：
S4.1、根据环境信息，将环境分割为若干个栅格单元；
S4.2、计算每个栅格单元被占用的概率lt,ij，被占用的概率lt,ij大于或等于0.8的栅格单元表示为障碍物，将有障碍物的栅格用灰色表示，将无障碍物的栅格用白色表示，得到栅格地图。
进一步地，所述每个栅格单元被占用的概率lt,ij：
其中，lt-1,ij表示前一刻时间栅格被占用的概率，p(mij|zt,xt)表示栅格mij被占用的后验概率，mij表示横坐标为i纵坐标为j的栅格单元，zt为t时刻的观测值，xt为t时刻移动机器人的位姿。
本发明的有益效果为：
(1)本发明通过UWB、里程计和激光雷达分别采集移动机器人与锚点的距离信息、移动机器人的位姿信息和环境信息进行地图构建，多种传感器采集数据，使构建的地图更为准确。
(2)本发明通过构建顶点-约束图，并用图优化算法对顶点-约束图进行优化，使移动机器人的位姿信息更加准确，为构建精准的室内地图打好了基础。
(3)本发明使用里程计获得了机器人短时间内精确的位姿变化，融合UWB的定位信息能够获得机器人长时间的精确位姿变化，同时结合激光雷达的测量数据进行精准的地图构建，解决了激光雷达在构建室内复杂环境地图时精度不高的问题。
附图说明
图1为本发明提出的一种融合多传感器的移动机器人室内地图构建方法的流程图。
图2为本发明的实验结果对比图。
具体实施方式
下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。
下面结合附图详细说明本发明的实施例。
如图1所示，一种融合多传感器的移动机器人室内地图构建方法，包括以下步骤：
S1、通过UWB、里程计和激光雷达分别采集移动机器人与锚点的距离信息、移动机器人的位姿信息和环境信息；
S2、根据距离信息、位姿信息和环境信息，构建顶点-约束图；
S3、通过图优化算法对顶点-约束图进行优化，获取优化后的移动机器人轨迹数据；
S4、通过优化后的移动机器人轨迹数据和环境信息构建栅格地图。
步骤S1包括以下步骤：
S1.1、在移动机器人上搭载UWB标签和激光雷达，所述机器人上设置有里程计和编码器；
S1.2、通过UWB标签采集移动机器人与锚点的距离信息；
S1.3、通过里程计获得机器人的位姿数据，所述里程计通过编码器获取数据；
S1.4、通过激光雷达扫描得到环境信息。
步骤S2包括以下步骤：
S2.1、根据移动机器人的位姿信息构成顶点，构建基于里程计的边，得到第一初始图；
S2.2、在第一初始图上添加UWB约束，构建基于UWB的边，得到第二初始图；
S2.3、通过环境数据进行闭环检测，在第二初始图上添加基于激光的边，构建激光-闭环边，得到顶点-约束图。
步骤S2.3中激光-闭环边的计算步骤如下：
A1、构建源点云集合Q＝{q1,q2,…,qN}和目标点云集合P＝{p1,p2,…,pN}；
A2、构建目标点云集合P的旋转矩阵R和平移矩阵T，通过旋转矩阵R和平移矩阵T构建目标函数E(R,T)；
A3、设定一个阈值，并判断E(R,T)是否小于阈值，若是则判断为闭环，并结束本次激光-闭环边的构建，若否则进入步骤A4；
A4、将旋转矩阵R和平移矩阵T代入源点云集合Q中，得到点集M；
A5、将点集M代入目标点云集合P，得到新的旋转矩阵R'和新的平移矩阵T'；
A6令R＝R'，T＝T'，将更新过后的R和T代入目标函数E(R,T)中，并返回步骤A3。
目标函数E(R,T)为：
其中，N表示目标点云集合中点的总数，i＝1,2,...,N，qi表示源点云集合中的第i个点，pi表示目标点云集合中的第i个点。
步骤S3中通过图优化算法对顶点-约束图进行优化的具体方法为：调整顶点-约束图中的位姿顶点，使位姿信息的误差函数F(x)最小，得到最大化满足约束的位姿顶点。
误差函数F(x)为：
其中，xi表示位姿顶点i，xj表示位姿顶点j，C表示图的顶点间存在约束关系的集合，Ωij表示xi和xj之间观察值的信息矩阵，eij(xi,xj,zij)表示xi和xj满足约束zij的程度，zij表示位姿顶点i和位姿顶点j之间通过传感器获取的实际观察值，所述实际观察值zij包括相邻位姿顶点i与位姿顶点j之间的位姿变换、位姿顶点i与锚点j之间的距离和非相邻位姿顶点i与位姿顶点j之间的位姿变换。
步骤S4包括以下分步骤：
S4.1、根据环境信息，将环境分割为若干个栅格单元；
S4.2、计算每个栅格单元被占用的概率lt,ij，被占用的概率lt,ij大于或等于0.8的栅格单元表示为障碍物，将有障碍物的栅格用灰色表示，将无障碍物的栅格用白色表示，得到栅格地图。
所述每个栅格单元被占用的概率lt,ij：
其中，lt-1,ij表示前一刻时间栅格被占用的概率，p(mij|zt,xt)表示栅格mij被占用的后验概率，mij表示横坐标为i纵坐标为j的栅格单元，zt为t时刻的观测值，所述观测值表示激光雷达测量得到的移动机器人与障碍物之间的距离和角度，xt为t时刻移动机器人的位姿。
在本实施例中，实验场景选择为两对边为墙体的走廊。
如图2所示，a图为原始里程计所构建的地图，b图为里程计和UWB共同构建的地图，c图为本发明通过UWB、里程计和激光雷达构建的地图；通过实验结果对比分析，可以发现本发明所构建的地图对墙体的构建更为精准，且有效的识别了障碍物。
本发明通过UWB、里程计和激光雷达分别采集移动机器人与锚点的距离信息、移动机器人的位姿信息和环境信息进行地图构建，多种传感器采集数据，使构建的地图更为准确。
本发明通过构建顶点-约束图，并用图优化算法对顶点-约束图进行优化，使移动机器人的位姿信息更加准确，为构建精准的室内地图打好了基础。
本发明使用里程计获得了机器人短时间内精确的位姿变化，融合UWB的定位信息能够获得机器人长时间的精确位姿变化，同时结合激光雷达的测量数据进行精准的地图构建，解决了激光雷达在构建室内复杂环境地图时精度不高的问题。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：一种单目视觉定位的IMU辅助跟踪方法及装置