[发明专利]一种基于动态障碍物的室内移动机器人局部路径规划方法

权利要求书

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

步骤1、使用AMCL算法实现机器人定位，得到机器人的初始位姿(X_robot,Y_robot,σ_robot)；

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

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

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

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

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

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

步骤7、设L_m与L_m+1为所求两条相邻直线，直线L_m方程为：Y＝a_m*X+b_m，直线上的最后一点为直线L_m+1方程为：Y＝a_m+1*X+b_m+1，直线上第一个点为

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

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

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

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

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

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

步骤11、不断更新动态障碍物前G个坐标点，G＞0，利用最小二乘实现直线拟合Y_center＝a_c*X_center+b_c，用此方程作为动态障碍的运动状态方程；

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

步骤13、机器人在导航过程中，会对环境地图以及所有出现的障碍物进行一个安全距离的膨胀处理，称为代价地图，为了让DWA局部路径规划器更好的实现动态避障，在得到障碍物的运动状态后，扩大动态障碍物速度方向的膨胀区域；

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

2.如权利要求1所述的一种基于动态障碍物运动信息的室内移动机器人局部路径规划方法，其特征在于：所述步骤13膨胀区域的膨胀规则是用一个更大的等腰三角形重叠原始膨胀区域，求得障碍物的运动直线方程后，以障碍物运动方向为等腰三角形区域的上顶点，三角形的大小可以根据障碍物的移动速度进行调整，计算出三角形三边的直线方程，再取三角形三边上及内部点若干作为膨胀位置，用来模拟人工膨胀区域。