CN104236551B - 一种蛇形机器人基于激光测距仪的地图创建方法 - Google Patents

Abstract

本发明公开了一种蛇形机器人基于激光测距仪的地图创建方法，包括：启动蛇形机器人和激光测距仪，随着蛇形机器人的运动，激光测距仪实现对环境信息的采集，并对采集的激光数据进行预处理；进行地图初始化，将当前的激光的测量值与当前的地图进行匹配；用Monte‑Carlo算法对机器人粒子集的粒子位姿及权值进行更新，计算机器人粒子集概率中心位姿；用改进的Bresenham算法进行地图的更新。本发明具有算法简单易懂、计算简单准确度高，适用于快速移动的机器人的优点。

Description

一种蛇形机器人基于激光测距仪的地图创建方法

技术领域

本发明属于移动机器人地图创建技术领域，特别涉及一种基于激光测距仪在未知环境中的地图创建方法。

背景技术

移动机器人通常是基于环境地图才能够实现定位和导航，所以地图创建是研究移动机器人的一个基本问题。SLAM问题在智能移动机器人领域的重要性受到了广泛的关注，出现了许多较为成熟的理论和实现方法。目前有很多针对SLAM问题的研究，SLAM常见的算法有EKF-SLAM算法、FastSLAM算法、DP-SLAM算法等。

基于EKF-SLAM的算法在不确定信息的表达上非常简洁和高效，它采用一个多维的高斯模型N(μ_t,∑_t)来描述机器人位姿和地图的联合后验分布，其维度为(2N+3)，N为环境特征的数目，但EKF-SLAM算法难以解决高度非线性和数据关联问题。

FastSLAM算法运用RBPF思想对后验概率进行因式分解，将SLAM问题分解成机器人路径估计和基于路径估计的地图创建两个子问题，其中路径的估计采用粒子滤波器，环境特征的估计采用EKF。其相比于传统的EKF-SLAM算法，降低了算法的复杂度，效率更高，但计算量仍然比较大，程序实现复杂。

基于粒子滤波的DP-SLAM算法是通过粒子滤波保持机器人的位姿和地图的联合概率分布，采用的是子地图系统，它为每个粒子创建一个子地图，然后再进行子地图关联来获得一幅完整的环境地图。这个算法在计算和环境地图创建过程中会消耗大量的内存。此外，该算法需要一个功能强大且价格昂贵的激光传感器(SICK)。

大多数SLAM算法都是针对缓慢移动的机器人进行测试，速度一般不超过1米/秒。到目前为止几乎所有SLAM算法的计算和实现都需要大量代码，要理解并测试他们的算法需要花费很大精力。

发明内容

本发明所要解决的技术问题在于克服现有技术的不足，提供一种MiniSLAM算法，具有算法简单易懂、计算量少、适用于快速移动的蛇形机器人等优点。

本发明采用以下技术方案：

一种蛇形机器人基于激光测距仪的地图创建方法，包括如下步骤：

步骤(1)：通过激光测距仪进行环境感知，获取环境信息；

步骤(2)：将当前激光测距仪的测量值与当前地图进行匹配；

步骤(3)：对蛇形机器人粒子集的粒子位姿及权值进行更新；

步骤(4)：进行地图更新。

步骤(1)中，所述激光测距仪为测距范围20mm到4000mm的URG04-LX激光测距仪，该激光测距仪的最大测量距离小于4m；采用阈值滤波方法对扫描的激光数据进行预处理滤除噪声点；并采用一个恒定的纵向和转速值对每次激光扫描值进行纠正。所述采用恒定的纵向和转速值对激光数据纠正并非本领域技术人员的常规技术实践，本申请是在蛇形机器人建图的过程中对蛇形机器人的运动步态进行建模，分析运动步态对激光数据及建图的影响，创造性的采用恒定的纵向和转速值对激光数据进行纠正。

进一步的，步骤(2)中，所述激光测距仪每一次扫描完后，会将观测值与当前地图中的特征点进行匹配；当与环境中的特征点较远时，依据最近邻数据关联准则，选取距离观测值最近的环境路标作为其匹配点；环境特征点间的距离相距越远，误匹配率越低，当误匹配率小于最大匹配误差阈值时，认为匹配成功。

进一步的，步骤(3)所述的对蛇形机器人粒子集的粒子位姿及权值进行更新，采用的是Monte-Carlo算法；每次蛇形机器人感知完环境后这些粒子会重新采样；算法结束时，粒子会收敛于蛇形机器人的实际位置；在每个时刻，由初始状态和到当前时刻所有的测量值Z^k＝{z_i,i＝1...k}估计当前蛇形机器人的状态。

进一步的，所述Monte-Carlo算法包含3个阶段：

预测阶段：预测阶段用运动模型以概率密度函数的形式来预测当前蛇形机器人的位姿；假设当前的状态x_k仅依赖于之前的状态x_k-1和已知的控制输入u_k-1，该运动模型被认定为条件密度p(x_k|x_k-1,u_k-1)，对于一阶马尔可夫过程，先验概率密度就可以通过积分得到

p(x_k|Z^k-1)＝∫p(x_k|x_k-1,u_k-1)p(x_k-1|Z^k-1)dx_k-1

更新阶段：预测阶段用测量模型整合传感器的信息来获得后验概率密度p(x_k|Z^k)；假定对于x_k测量值z_k与之前的测量值Z^k-1是条件独立的；测量模型以似然函数p(z_k|x_k)的形式给出，这种形式表示在观测值z_k的情况下，蛇形机器人在x_k的可能性；通过贝叶斯公式更新先验值，得到后验概率密度

重采样阶段：重采样的目的是剔除权值较小的粒子，复制权值较大的粒子；通过对后验概率密度再采样N_s次，得到新的粒子。

进一步的，步骤(4)所述的地图更新方法采用的是改进的Bresenham算法，只采用整数运算，避免了除法运算和浮点数运算，降低了算法复杂度，提高了计算速度。

与现有技术相比，本发明的有益效果是：

(1)本发明提供的基于激光测距仪的MiniSLAM算法，以蛇形机器人为平台，实现了SLAM算法在蛇形机器人上的应用。正所周知，现如今的地图创建方法采用的移动机器人多是移动小车或者四旋翼飞机，其运动平稳，易于控制。蛇形机器人不是普通的移动机器人，其具有复杂的运动步态，本申请是针对蛇形机器人的特殊运动步态进行的地图创建方法，目前为止没有针对蛇形机器人进行地图构建的论文或者专利发表；

(2)本发明提供的基于激光测距仪的MiniSLAM算法，采用蒙特卡洛算法将激光束与当前地图进行匹配，并进行蛇形机器人的状态更新，算法简单；

(3)本发明提供的基于激光测距仪的MiniSLAM算法，采用改进的Bresenham算法，只采用整数运算，避免了除法运算和浮点数运算，降低了算法复杂度，提高了计算速度。此外，常规的Bresenham算法是用来画直线、画圆的，本申请则创造性的对其进行改造来创建环境地图，属于首创。

附图说明

图1为实验所用的装有URG04-LX激光测距仪的蛇形机器人；

图2为实验环境的简易平面示意图；

图3为蛇形机器人通过激光测距仪创建地图的原理框图。

具体实施方式

本发明是关于一种蛇形机器人基于激光测距仪的地图创建方法，具体包括如下步骤：

(1)环境感知。通过激光测距仪进行环境感知，获取环境信息。

打开激光测距仪，通过控制蛇形机器人以直行的运动步态按顺时针的方向绕图2中的虚线轨迹运动一圈，进行环境感知。其中，图2中2-1为实验桌，2-2为地面上的箱子，2-3为凳子。

URG04-LX激光测距仪测距范围20mm到4000mm，1米内精度5mm，1米外精度为测量距离的1％，最大扫描角度240度，角分辨率0.36度，扫描周期100ms，RS232或者USB通讯。①它最大测量距离限制在4m，在杂乱的环境中，传感器的测量值经常性的返回0或者模糊性的测量值，在此采用阈值滤波对激光的原始测量值进行滤波；②测量频率10Hz同样受到限制，假设蛇形机器人以2m/s的速度靠近墙，墙看起来会出现倾斜现象，这是因为最后一个测量点相比第一个测量点在240°的时间间隔内，移动了240/360*2/10＝13.33cm。考虑到这一点，用一个恒定的纵向和转速值对每一次激光扫描值进行纠正。

(2)特征匹配。将当前激光的测量值与当前地图进行匹配。

激光测距仪每一次扫描完后，会将观测值与当前地图中的特征点进行匹配，在与环境中的特征点较远时，可依据最近邻数据关联准则，选取距离观测值最近的环境路标作为其匹配点。环境特征点间的距离相距越远，误匹配率越低，当误匹配率小于最大匹配误差阈值时，认为匹配成功。

(3)状态更新。用Monte-Carlo算法对蛇形机器人粒子集的粒子位姿及权值进行更新，计算蛇形机器人粒子集概率中心位姿。

初始假定粒子可能位于环境中的任何位置，随着蛇形机器人移动，粒子也做相应的移动。每次蛇形机器人感知完环境后这些粒子会重新采样。算法结束时，粒子会收敛于蛇形机器人的实际位置。在每个时刻，由初始状态和到当前时刻所有的测量值Z^k＝{z_i,i＝1...k}估计当前机蛇形器人的状态。蒙特卡洛算法包含3个阶段：

预测阶段：预测阶段用运动模型以概率密度函数的形式来预测当前蛇形机器人的位姿。假设当前的状态x_k仅依赖于之前的状态x_k-1和已知的控制输入u_k-1，该运动模型被认定为条件密度p(x_k|x_k-1,u_k-1)，对于一阶马尔可夫过程，先验概率密度就可以通过积分得到

p(x_k|Z^k-1)＝∫p(x_k|x_k-1,u_k-1)p(x_k-1|Z^k-1)dx_k-1

更新阶段：预测阶段用测量模型整合传感器的信息来获得后验概率密度p(x_k|Z^k)。假定对于x_k测量值z_k与之前的测量值Z^k-1是条件独立的。测量模型以似然函数p(z_k|x_k)的形式给出，这种形式表示在观测值z_k的情况下，蛇形机器人在x_k的可能性。通过贝叶斯公式更新先验值，得到后验概率密度

重采样阶段：重采样的目的是剔除权值较小的粒子，复制权值较大的粒子，通过对后验概率密度再采样N_s次，得到新的粒子。

(4)地图更新。用改进的Bresenham算法进行地图更新。

假设每次要绘制的两个端点是P₁(x1,y1)和P₂(x2,y2)，需要确定介于它们之间的像素序列。线段宽是Δx＝x2-x1，高为Δy＝y2-y1，所有穿过两个端点的直线都满足等式-Δx(y-y1)+Δy(x-x1)＝0。

定义一个函数F(x,y)＝-2Δx(y-y1)+2Δy(x-x1)，如果这里没有乘以2的话，决策参数就会有浮点数存在。

对于直线方程y＝kx+b,改进的布雷森汉姆算法流程为：

A|Δx|＞＝|Δy|的情况，即-1＜k＜1时

step1输入线段的两个端点P₁(x1,y1)和P₂(x2,y2)，并且存储到P₁(x1,y1)和P₂(x2,y2)中；

step2将P₁载入桢缓存，绘画第一个起始点；

step3获得一个决策参数的第一个值：P＝2Δy-Δx

step4从n＝0开始，在沿线经过每个xn处，进行下面的监测:

if Pn＜0,下一个点绘制的是(xn+1,yn)，并且Pn+1＝Pn+2|Δy|

if Pn＞＝0,下一个点绘制的是(xn+1,yn+1)，并且

step5重复执行|Δx|-1次step4；

B.|Δx|＜|Δy|的情况，即k＜-1或者k＞1时

将x坐标轴与y坐标轴颠倒，可以得出与上面相同的结论，只是原先是x，现在换成了y。地图更新结束后得到最终的环境地图。

本发明提供的一种蛇形机器人基于激光测距仪的MiniSLAM算法，以安装有URG04-LX激光测距仪的蛇形机器人为实验平台，具有算法简单易懂、计算简单准确度高，适用于快速移动的蛇形机器人的优点。

最后应说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (3)

1.一种蛇形机器人基于激光测距仪的地图创建方法，其特征在于，包括以下步骤：

步骤(1)：通过激光测距仪进行环境感知，获取环境信息；

步骤(2)：将当前激光测距仪的测量值与当前地图进行匹配；

步骤(3)：对蛇形机器人粒子集的粒子位姿及权值进行更新；所述的对蛇形机器人粒子集的粒子位姿及权值进行更新，采用的是Monte-Carlo算法；每次蛇形机器人感知完环境后这些粒子会重新采样；算法结束时，粒子会收敛于蛇形机器人的实际位置；在每个时刻，由初始状态和到当前时刻所有的测量值Z^k＝{z_i,i＝1...k}估计当前蛇形机器人的状态；所述Monte-Carlo算法包含3个阶段：

预测阶段：预测阶段用运动模型以概率密度函数的形式来预测当前蛇形机器人的位姿；假设当前的状态x_k仅依赖于之前的状态x_k-1和已知的控制输入u_k-1，该运动模型被认定为条件密度p(x_k|x_k-1,u_k-1)，对于一阶马尔可夫过程，先验概率密度就可以通过积分得到

p(x_k|Z^k-1)＝∫p(x_k|x_k-1,u_k-1)p(x_k-1|Z^k-1)dx_k-1

更新阶段：预测阶段用测量模型整合传感器的信息来获得后验概率密度p(x_k|Z^k)；假定对于x_k测量值z_k与之前的测量值Z^k-1是条件独立的；测量模型以似然函数p(z_k|x_k)的形式给出，这种形式表示在观测值z_k的情况下，蛇形机器人在x_k的可能性；通过贝叶斯公式更新先验值，得到后验概率密度

$p\left(x_k\right|Z^k)=\frac{p\left(z_k\right|x_k)p\left(x_k\right|Z^{k-1})}{p\left(z_k\right|Z^{k-1})}$

重采样阶段：重采样的目的是剔除权值较小的粒子，复制权值较大的粒子,通过对后验概率密度再采样N_s次，得到新的粒子；步骤(4)：进行地图更新；所述的地图更新方法采用的是改进的Bresenham算法，只采用整数运算。

2.根据权利要求1所述的一种蛇形机器人基于激光测距仪的地图创建方法，其特征在于：所述激光测距仪为测距范围20mm到4000mm的URG04-LX激光测距仪，该激光测距仪的最大测量距离小于4m；采用阈值滤波方法对扫描的激光数据进行预处理滤除噪声点；并采用一个恒定的纵向和转速值对每次激光扫描值进行纠正。

3.根据权利要求1所述的一种蛇形机器人基于激光测距仪的地图创建方法，其特征在于：所述激光测距仪每一次扫描完后，会将观测值与当前地图中的特征点进行匹配；当与环境中的特征点较远时，采用最近邻数据关联准则，选取距离观测值最近的环境路标作为其匹配点；环境特征点间的距离相距越远，误匹配率越低，当误匹配率小于最大匹配误差阈值时，认为匹配成功。