一种基于动态障碍物运动信息的室内移动机器人局部路径规划方法

本发明属于移动机器人局部路径规划领域，具体涉及一种基于动态障碍物运动信息的移动机器人局部路径规划方法。

背景技术：

随着移动机器人产业的飞速发展，人们对其智能化水平要求越来越高，而路径规划技术是移动机器人的关键技术，特别是对于局部路径规划，它决定着机器人是否能够避开已知环境中的未知障碍物。

目前主流的局部路径规划算法有teb算法、人工势场法、dwa算法，它们被广泛用于各类移动机器人上，且能很好的避开环境中新出现的静态障碍，但这些算法在躲避动态障碍物时，都是将其当做瞬时的静态障碍处理，缺乏主动性与安全性。

对于室内移动机器人无法通过gps进行精确定位，只能通过机器人所带的传感器进行环境感知从而估计自身位姿，这使机器人判断动态物体十分困难，所以如何引导移动机器人在导航过程中安全避开动态障碍物成为一个热点、难点问题。

技术实现要素：

针对现有技术存在的问题，本发明为解决现有技术中存在的问题采用的技术方案如下：

一种基于动态障碍物运动信息的室内移动机器人局部路径规划方法，其特征在于：在结合amcl(adaptivemontecarlolocalization)算法基础上通过单线激光雷达获取所有障碍物点云的位置信息，再对所有激光点云进行分割与直线拟合并去除地图中已知的障碍，并用合适大小的圆圈标记新出现的可能的动态障碍，再通过圆圈圆心坐标变化进行动态障碍物检测，不断获取动态障碍的圆心坐标并利用最小二乘求解其运动方程，最后扩大动态障碍物速度方向代价地图，通过结合dwa算法(dynamicwindowapproach,动态窗口法)实现安全避开动态障碍。

所述一种基于动态障碍物运动信息的室内移动机器人局部路径规划方法，具体包括如下步骤：

步骤1、使用amcl算法实现机器人定位，得到机器人的初始位姿(xrobot,yrobot,σrobot)；

步骤2、设打在障碍物上激光点云数据为(ρi，θi)(i＝1,2,...,n)，其中ρi代表第i条激光点的距离，θi代表第i条激光的角度，在得到机器人自身位姿估计后，根据激光雷达返回的信息，计算障碍物的绝对位置信息(xobs,yobs)，计算公式如式(1)，其中xrobot为机器人在全局地图中的横坐标值，yrobot为机器人纵坐标值，σrobot为机器人的方位角，

步骤3、由于激光点距离越大，激光越稀疏，可保留所有阈值范围内的激光点形成激光点云其中m为符合要求激光点的总个数，遍历所有激光数据，求相邻激光坐标距离的平方其中表示第k与第k-1个激光点之间距离的平方；

步骤4、设置距离阈值为其中r为阈值常量，p为比例常量，ρk为第k个激光数据(ρk，θk)的距离极值，若：则初次判断第k个激光数据为断点，遍历所有数据，将所有激光点根据断点进行分开，设置每组数据点个数最少为pmin，去除激光点数小于pmin的激光组；

步骤5、设第k个激光数据为第一组激光坐标的断点，则第一组数据为利用最小二乘实现直线拟合，设拟合的直线为：y＝a*x+b，参数a，b按式(2)求解：

去掉所拟合直线上首尾两端点，计算该组数据中间k-3个激光点到直线的距离，设第n个点距直线的距离为设置距离阈值其中β为第二次分割的阈值常量，p为比例常量(同上)，ρn为第n个激光数据(ρn，θn)的距离极值，若则判断第n个激光数据为断点；

步骤6、循环步骤3-步骤5至所有激光数据被分组，利用最小二乘求解每组数据的直线方程；

步骤7、设lm与lm+1为所求两条相邻直线，直线lm方程为：y＝am*x+bm，直线上的最后一点为直线lm+1方程为：y＝am+1*x+bm+1，直线上第一个点为

步骤8、比较直线lm与lm+1，若|am-am+1|<amax且|bm-bm+1|<bmax且则判断lm、lm+1为同一条直线并进行合并，反之，不进行处理，其中amax为斜率阈值，bmax为截距阈值，为距离阈值，r为阈值常量，p为比例常量，为直线lm+1中第一个激光数据的距离极值；

步骤9、将拟合得到的直线与二维栅格地图进行对比，若直线在栅格地图上，则不再对这些直线进行处理。假设障碍物为圆形，设每条直线为圆的内接等边三角形的一边，若有直线lj：y＝aj*x+bj，其两个端点分别为根据式(3)可求圆的半径r1：

为了保证障碍物被完全包含，设置附加边界br，则最终圆的半径r根据式(4)求得，并用圆圈进行标记：

滤除环境中一些过大的非动态障碍，设置圆的最大容许半径为rmax，若r＜rmax，根据(5)式求该圆的圆心坐标：

步骤10、不断获取圆心坐标与对应时间，设可能的动态障碍物在t1时刻的圆心坐标为t2时刻的圆心坐标为根据式(6)计算其瞬时速度vi：

设置速度阈值为vmax，若vi＜vmax，判断其为静态障碍物，不做处理；若vi＞vmax，判断其为动态障碍物；

步骤11、不断更新动态障碍物前g(g＞0)个坐标点，利用最小二乘实现直线拟合ycenter＝ac*xcenter+b，用此方程作为动态障碍的运动状态方程；

步骤12、利用dwa局部路径规划算法实现避障，根据机器人速度限制、加速度限制、离障碍物距离限制对速度(vrobot，ωrobot)进行采样，设计评价函数对每组采样数据进行评分，最后选择评分最高的速度组合；

步骤13、机器人在导航过程中，会对环境地图以及所有出现的障碍物进行一个安全距离的膨胀处理，称为代价地图，为了让dwa局部路径规划器更好的实现动态避障，在得到障碍物的运动状态后，扩大动态障碍物速度方向的膨胀区域，膨胀规则(相当于给障碍物增大一个安全距离)是用一个更大的等腰三角形重叠原始膨胀区域，求得障碍物的运动直线方程后，以障碍物运动方向为等腰三角形区域的上顶点，三角形的大小可以根据障碍物的移动速度进行调整，计算出三角形三边的直线方程，再取三角形三边上及内部点若干作为膨胀位置，用来模拟人工膨胀区域；

步骤14、通过扩大动态障碍物速度方向的代价地图后，增大了dwa算法所规划的路径与动态障碍物之间的距离，能够使机器人更安全的避开动态障碍物，为室内移动机器人适应高动态环境提供了理论基础。

针对目前的局部路径规划算法在躲避动障碍物时都是将其当成瞬时的静态障碍物进行避障，缺乏主动性与安全性，而本发明方法能够更安全的避障，具体优点如下：

(1)利用单个二维激光雷达就识别、判断了动态障碍物，并且计算出了动态障碍物的运动方程；(2)在判断计算出动态障碍物的运动方程之后，创新性的扩大了动态障碍速度方向的代价地图，再结合dwa算法用于局部动态避障，这样的避障效果更好。

附图说明

图1为障碍物点云位置计算原理图，机器人初始位姿(xrobot,yrobot,σrobot)由amcl算法估计得出，利用坐标变换可求得障碍物点云坐标(xobs,yobs)；

图2为第二次寻找断点的示意图，将原本的折线在拐点处分开；

图3为激光点云拟合成直线、标记新出现障碍物的过程；

图4为动态障碍物代价地图扩大规则，其中左边为原始代价地图，右边为扩大之后的代价地图；

图5为本发明的总体流程图。

具体实施方式

下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明，机器人在拥有先验地图的前提下，通过amcl定位算法获取到机器人的初始位姿(xrobot,yrobot,σrobot)。机器人上的激光雷达获取到激光点云数据为(ρi，θi)(i＝1,2,...,n)，其中ρi代表第i条激光点的距离，θi代表第i条激光的角度。根据式(1)容易计算得到每个激光点的绝对位置，也就是障碍物绝对位置信息(xobs,yobs)，如图1所示为障碍物点云位置计算原理图。

根据不同精度的激光雷达可设置不同的距离阈值，保留阈值范围内的激光点形成点云m为符合要求激光点的总个数，历所有激光数据，求相邻激光坐标距离的平方

比较与阈值的大小，若：则第k个激光数据为断点。遍历所有数据，将激光点根据断点进行分开。设每组数据点个数最少为pmin，去除激光点数小于pmin的激光组。

若第k个激光数据为第一组激光坐标的断点，则第一组数据为利用最小二乘实现直线拟合，得到直线：y＝a*x+b，参数a，b按式(2)求解。

计算这组数据中间k-3个激光点到直线的距离，求得第n个点距直线的距离若则判断第n个激光数据为断点，如图2所示。

循环以上三个步骤直至所有激光数据均被处理并求解每组数据的直线方程。如果有相邻直线lm、lm+1，其中直线lm的方程为：y＝am*x+bm，直线上最后一点为直线lm+1方程为：y＝am+1*x+bm+1，直线上第一个点为

若|am-am+1|<amax且|bm-bm+1|<bmax且合并直线lm、lm+1，反之，不进行处理。

若有直线lj：y＝aj*x+bj不在栅格地图上，且两个端点分别为根据式(3)可求得外接圆的半径r1。

添加附加边界br，最终圆的半径r根据式(4)求得，滤除环境中一些过大的非动态障碍，若r＜rmax，根据(5)式求该圆的圆心坐标并进行标记，如图3所示。

不断获取圆心坐标与对应时间，设可能的动态障碍物在t1时刻的圆心坐标为t2时刻的圆心坐标为根据式(6)计算其瞬时速度vi，若vi＜vmax，判断其为静态障碍物，不做处理；若vi＞vmax，判断其为动态障碍物。

不断更新动态障碍物前g(g＞0)个坐标点，利用最小二乘求解得到直线ycenter＝ac*xcenter+bc，用此方程作为动态障碍的运动状态方程。

扩大动态障碍物速度方向的膨胀区域，用一个更大的等腰三角形重叠原始膨胀区域。如图4所示，求得障碍物的运动直线方程后，以障碍物运动方向为等腰三角形区域的上顶点，三角形的大小可以根据障碍物的移动速度进行调整，计算出三角形三边的直线方程，再取三角形三边上及内部点若干作为膨胀位置，用来模拟人工膨胀区域。

通过扩大动态障碍物速度方向的代价地图后，结合dwa算法实现动态避障，增大了dwa算法所规划的路径与动态障碍物之间的距离，能够使机器人更安全的避开动态障碍物，本发明方法不断获取障碍物上的点云位置信息，再根据点云特征对其进行分割分组，利用最小二乘对每组激光数据进行直线拟合，最后与环境地图进行比对，去除了已知障碍，同时用圆圈标记新加入障碍物，通过其圆心的实时变化判断了动态障碍物，试验结果表明了该方法的可行性。

本发明的保护范围并不限于上述的实施例，显然，本领域的技术人员可以对本发明进行各种改动和变形而不脱离本发明的范围和精神。倘若这些改动和变形属于本发明权利要求及其等同技术的范围内，则本发明的意图也包含这些改动和变形在内。