CN106444769B - 一种室内移动机器人增量式环境信息采样的最优路径规划方法 - Google Patents

Abstract

本发明公开了一种室内移动机器人增量式环境信息采样的最优路径规划方法，其步骤为：(1)获取周围环境信息，建立基于障碍物碰撞风险的评估概率；(2)利用增量式环境信息采样的最优路径规划算法进行路径规划；(3)室内移动机器人进行路径选择并进入新的路径规划流程。采用的增量式环境信息采样的最优路径规划算法能够根据室内移动机器人的当前状况和机器人固有的非完整约束，实时规划当前最佳路径，同时，搜索树扩展过程中的碰撞检测环境得到优化，提高了规划效率，使室内移动机器人能够快速安全有效的到达指定位置。

Description

一种室内移动机器人增量式环境信息采样的最优路径规划 方法

技术领域

本发明属于地面轮式机器人的自主导航领域，特别涉及一种室内移动机器人增量式环境信息采样的最优路径规划方法。

背景技术

随着人工智能技术的迅速发展和人们对高质量生活的需求，机器人技术如今越来越受到商业界和企业界的重视，逐渐从工业应用走到人民大众的生活中来。移动服务机器人为其中一种最常见的生活服务机器人，它能够完成一些有益于人类健康的服务工作，如家务劳动、照料病人等。在服务工作过程中，移动服务机器人通常无法预先获取所有环境信息，并可能处于不可预测或动态变化的环境中。考虑到装备的环境感知设备有限和环境感知过程中的噪声干扰,移动服务机器人的安全自主工作面临着极大的挑战。在移动服务机器人系统中，自主导航及路径规划是其能够自主完成任务的核心和大脑。

通常来说，路径规划是在给定的环境中找出从给定起始状态到给定目标状态的无碰撞轨迹或控制器信号的过程。若路径规划算法能够至少找出一条路径或者返回无法找到路径的提示，则说明该算法是完整的。近些年来,路径规划技术发展迅速，涌现出了许多规划算法。常见的路径规划算法如人工势场法,蚁群算法，遗传算法等，在解决一些规划问题时有其优越性。但由于它们计算复杂度高，不适用于多障碍物的复杂动态环境下的机器人规划问题的求解。另外，基于前向图搜索的路径规划算法，如A*、D*和神经动力学等，在规划时虽然能满足最优性和实时性的要求，但是其处理高维问题的计算瓶颈问题，使得其应用具有一定的限制。因此，研究具有学习能力的路径规划算法成为现阶段实时动态路径规划研究的一个主流趋势。而设计一种简单可靠、实时性好、便于实现、能够应付多类动态未知环境的室内移动机器人规划方法是保证服务工作正常有效进行的关键技术和现实难题。

发明内容

本发明针对上述现有技术中现在路径规划方法中难以同时满足动态未知环境规划和实时规划的要求，提出了一种室内移动机器人增量式环境信息采样的最优路径规划方法，采用增量式环境信息采样的最优路径规划方法，随着室内移动机器人的移动，不断加深理解新获得的周围环境信息，迭代扩展搜索树，选取代价函数最小的路径为当前时刻最优的路径。

一种室内移动机器人增量式环境信息采样的最优路径规划方法，包括以下几个步骤：

步骤1：建立已探测区域障碍物碰撞风险的评估概率模型；

步骤2：进行基于增量式环境信息采样的最优路径规划；

在地图范围内以起点为搜索树第一个点，以步骤1获得的评估概率模型生成的随机采样状态点进行增量式迭代扩展，产生新的树节点，并计算新的树节点的代价函数，以树节点的最小代价函数修改树结构，直到生成的路径数达到设定路径数Path_n，以所有生成路径中路径代价函数最小的路径作为潜在最优路径；

步骤3：依据步骤2得到的潜在最优路径和室内移动机器人的当前位置和速度矢量，判断室内移动机器人的偏转角φ_t是否满足轮式机器人不能侧向滑动的非完整约束，从而确定室内移动机器人是否按照最优路径方向移动；

若室内移动机器人的偏转角φ_t不满足轮式机器人不能侧向滑动的非完整约束，则返回步骤2；

若满足，则沿当前时刻规划出的潜在最优路径方向移动，进入下一时刻的路径规划，t＝t+1，返回步骤1，直到室内移动机器人移动到目标位置，完成路径规划；

所述室内移动机器人的偏转角φ_t满足轮式机器人不能侧向滑动的非完整约束是指：φ_t∈[0,60°]∪[120°,180°]；

其中，p_t和p_t-1分别为室内移动机器人当前位置和上一时刻所在位置，为在t时刻，即室内移动机器人在t+1时刻的预抵达位置；

p_t-1由p_t-1＝p_t-vt得到，v为设定的室内移动机器人移动速度。

所述步骤2中最优路径的规划过程如下：

步骤2.1：令k＝1，k表示迭代次数；

步骤2.2：依据步骤1建立的障碍物碰撞风险的评估概率模型指导地图中随机采样的过程，获得随机采样状态点p_rand(k)；

依据步骤1建立的障碍物碰撞风险的评估概率模型指导地图中随机采样能够以一定概率避免直接采到障碍物区域的状态点；

步骤2.3：遍历已生成树{G＝(p,e)}找出距离随机采样状态点p_rand(k)最近的树上节点p_nearest(k)，进入步骤2.4；

步骤2.4：从树上距离最近的节点p_nearest(k)向随机采样状态节点p_rand(k)方向，以步长为S_e生长出新的节点p_new(k)，将节点p_new(k)添加至已生成树中{G＝(p,e)}，计算节点p_new(k)的代价函数，并记录相应节点之间的父子关系，father(p_new(k))＝p_nearest(t)；

其中，树上任意节点的代价函数按以下公式计算：

其中，k_a为障碍物影响的放大系数，取值范围为[50,1000]；C为由节点p_G到其父节点father(p_G)的路径点集合；father(p_G)为p_G的父节点；F(X,Y)表示已探测区域障碍物碰撞风险的评估概率；

树节点的代价函数采用由节点之间的欧式距离和连接路径上的累计危险度概率值组成；

步骤2.5：在已生成树{G＝(p,e)}中找出所有处于以节点p_new(k)为圆心，r_n为半径内的节点集合{p_near}，分别测试p_new(k)若以集合{p_near}中的节点为父节点是否会得到更小的代价函数，若更小，则以最小代价函数对应的节点作为p_new(k)的父节点，并更新节点p_new(k)的代价函数，进入步骤2.6；否则，直接进入步骤2.6；

步骤2.6：判断新增节点p_new(k)与目标位置的距离是否小于树的生长步长S_e，若小于该步长且两点之间的线段无碰撞障碍，则直接将目标位置相连，形成新的路径，且以当前新增节点的代价函数作为最新路径的代价函数，路径计数器Path+1，进入步骤2.7；否则，直接进入步骤2.7；

步骤2.7：测试集合{p_near}中节点若以p_new(k)为父节点是否会得到更小的代价函数，若更小，则更改该节点的父节点为p_new(k)并更新相应节点的代价函数。

步骤2.8：判断路径计数器Path是否达到设定的产生路径数Path_n，若达到，则退出迭代过程，进入步骤2.9，若没达到，则k＝k+1，返回步骤2.2；

步骤2.9：在已生成路径中，选择路径代价函数最小的路径作为当前时刻t的潜在最优规划路径。

所述步骤1的具体过程如下：

首先，通过室内移动机器人自身携带的声呐传感器，获得室内移动机器人的周围环境信息；其次，将室内移动机器人所经过的区域作为已探测区域，依据所述的周围环境信息建立已探测区域障碍物碰撞风险的评估概率模型，并依据声呐传感器实时采集的周围环境信息实时更新已探测区域障碍物碰撞风险的评估概率模型；

所述已探测区域障碍物碰撞风险的评估概率如下：

其中，室内移动机器人工作空间信息集合为所述空间信息包括所有目标位置及所有障碍物位置；{(X,Y)}为已探测区域，在的地图上探测到有M个障碍物，f_i(X,Y)为第i个障碍物的对室内移动机器人路径选择的影响函数，采用正态分布表示如下：

其中，σ_i为第i个障碍物的影响范围系数，取值范围为[0,1]；D_i为第i个障碍物到已探测区域内所有位置的距离矩阵，矩阵大小与地图大小一致为N×N。

所述步骤2中基于障碍物碰撞风险的评估概率模型指导地图中随机采样，其采样的过程如下：

其中，N(·)为归一化处理，F(X,Y)为障碍物碰撞风险的评估概率。

所述步骤1中室内移动机器人自身所携带声呐传感器是指所采用底盘Pioneer-2DX自身携带的声呐探测雷达。

有益效果

与现有技术相比，本发明的优点在于：

1、本发明建立基于障碍物碰撞风险的评估概率模型，将室内移动机器人与探测到障碍物直接的碰撞风险通过概率的方式构建影响模型，使障碍物对周围环境构成连续的影响，并且这种影响只针对室内移动机器人已探测过的区域；

2、本发明与以往传统的移动机器人路径规划相比，通过设计带有障碍碰撞风险的代价函数，缩减了在搜索树扩展过程中，碰撞检测的次数，大幅度缩减了计算时间，提高了算法效率；，保证了室内移动机器人的静态障碍下的良好避障性能，而且满足了动态障碍出现时的实时规划功能，达到具有较高智能化的室内移动机器人的自主导航和路径规划方法。

3、本发明设计了搜索树扩展过程中的两次修正过程，保证了最优路径的获得。

附图说明

图1是本发明所述方法的流程图；

图2是室内移动机器人硬件系统结构示意图；

图3是室内移动机器人非完整约束示意图；

图4是采用本发明所述方法在动态和静态环境下的路径规划效果图，其中，图(a)增量式环境信息采样的最优路径规划方法躲避动态障碍时的效果图；图(b)增量式环境信息采样的最优路径规划方法躲避静态障碍时的搜索树效果图；图(c)增量式环境信息采样的最优路径规划方法最终完成时的效果图。

具体实施方式

以下将结合附图和具体实施案例对本发明做进一步说明。

如图1所示，本发明为一种室内移动机器人增量式环境信息采样的最优路径规划方法，包括以下几个步骤：

步骤1：建立已探测区域障碍物碰撞风险的评估概率模型；

首先，通过室内移动机器人自身携带的声呐传感器，获得室内移动机器人的周围环境信息；其次，将室内移动机器人所经过的区域作为已探测区域，依据所述的周围环境信息建立已探测区域障碍物碰撞风险的评估概率模型，并依据声呐传感器实时采集的周围环境信息实时更新已探测区域障碍物碰撞风险的评估概率模型；

所述步骤1中室内移动机器人自身所携带声呐传感器是指所采用底盘Pioneer-2DX自身携带的声呐探测雷达，如图2所示，。所述已探测区域障碍物碰撞风险的评估概率如下：

其中，室内移动机器人工作空间信息集合为所述空间信息包括所有目标位置及所有障碍物位置；{(X,Y)}为已探测区域，在的地图上探测到有M个障碍物，f_i(X,Y)为第i个障碍物的对室内移动机器人路径选择的影响函数，采用正态分布表示如下：

其中，σ_i为第i个障碍物的影响范围系数，取值范围为[0,1]；D_i为第i个障碍物到已探测区域内所有位置的距离矩阵，矩阵大小与地图大小一致为N*N；

步骤2：进行基于增量式环境信息采样的最优路径规划；

在地图范围内以起点为搜索树第一个点，以步骤1获得的评估概率模型生成的随机采样状态点进行增量式迭代扩展，产生新的树节点，并计算新的树节点的代价函数，以树节点的最小代价函数修改树结构，直到生成的路径数达到设定路径数Path_n，以所有生成路径中路径代价函数最小的路径作为潜在最优路径；

具体过程如下：

步骤2.1：令k＝1，k表示迭代次数；

步骤2.2：依据步骤1建立的障碍物碰撞风险的评估概率模型指导地图中随机采样的过程，获得随机采样状态点p_rand(k)；

依据步骤1建立的障碍物碰撞风险的评估概率模型指导地图中随机采样能够以一定概率避免直接采到障碍物区域的状态点；

步骤2.3：遍历已生成树{G＝(p,e)}找出距离随机采样状态点p_rand(k)最近的树上节点p_nearest(k)，进入步骤2.4；

步骤2.4：从树上距离最近的节点p_nearest(k)向随机采样状态节点p_rand(k)方向，以步长为S_e生长出新的节点p_new(k)，将节点p_new(k)添加至已生成树中{G＝(p,e)}，计算节点p_new(k)的代价函数，并记录相应节点之间的父子关系，father(p_new(k))＝p_nearest(t)；

其中，树上任意节点的代价函数按以下公式计算：

其中，k_a为障碍物影响的放大系数，取值范围为[50,1000]；C为由节点p_G到其父节点father(p_G)的路径点集合；father(p_G)为p_G的父节点；F(X,Y)表示已探测区域障碍物碰撞风险的评估概率；

树节点的代价函数采用由节点之间的欧式距离和连接路径上的累计危险度概率值组成；

步骤2.5：在已生成树{G＝(p,e)}中找出所有处于以节点p_new(k)为圆心，r_n为半径内的节点集合{p_near}，分别测试p_new(k)若以集合{p_near}中的节点为父节点是否会得到更小的代价函数，若更小，则以最小代价函数对应的节点作为p_new(k)的父节点，并更新节点p_new(k)的代价函数，进入步骤2.6；否则，直接进入步骤2.6；

步骤2.6：判断新增节点p_new(k)与目标位置的距离是否小于树的生长步长S_e，若小于该步长且两点之间的线段无碰撞障碍，则直接将目标位置相连，形成新的路径，且以当前新增节点的代价函数作为最新路径的代价函数，路径计数器Path+1，进入步骤2.7；否则，直接进入步骤2.7；

步骤2.7：测试集合{p_near}中节点若以p_new(k)为父节点是否会得到更小的代价函数，若更小，则更改该节点的父节点为p_new(k)并更新相应节点的代价函数。

步骤2.8：判断路径计数器Path是否达到设定的产生路径数Path_n，若达到，则退出迭代过程，进入步骤2.9，若没达到，则k＝k+1，返回步骤2.2；

步骤2.9：在已生成路径中，选择路径代价函数最小的路径作为当前时刻t的潜在最优规划路径。

步骤3：依据步骤2得到的潜在最优路径和室内移动机器人的位置和速度矢量判断室内移动机器人的偏转角φ_t是否满足轮式机器人不能侧向滑动的非完整约束，确定室内移动机器人是否按照潜在最优路径方向移动。若不满足，则返回步骤2。若满足，则沿当前时刻规划出的最佳路径方向移动，进入下一时刻的路径规划，t＝t+1，返回步骤1，直到室内移动机器人移动到目标位置，完成路径规划；

所述室内移动机器人的偏转角φ_t满足轮式机器人不能侧向滑动的非完整约束是指：φ_t∈[0,60°]∪[120°,180°]，如图3所示；

其中，p_t和p_t-1分别为室内移动机器人当前位置和上一时刻所在位置，为在t时刻，即室内移动机器人在t+1时刻的预抵达位置；

p_t-1由p_t-1＝p_t-vt得到，v为设定的室内移动机器人移动速度。

算法性能分析

1、收敛性分析

与快速扩展随机树(RRT)相同，增量式环境信息采样规划算法，其概率采样的状态空间是有限的。但不同的是，增量式环境信息采样规划算法的采样有一定的概率不采有碰撞危险的机器人状态节点，而在树的扩展过程中没有碰撞检测的筛选，并不像RRT那样将有碰撞危险的节点剔除在树外。因此，可以得出。

引理1在t时刻，对于本文构建的搜索树{G＝(p,e)}，树上所有节点

引理1是应用在增量式环境信息采样规划算法和快速扩展随机树同样迭代次数下并且都找到路径完成搜索的情况。增量式环境信息采样规划算法的完整性与RRT类似，为概率完整性。RRT算法的概率完整性有以下的特性：

定理1如果有限大小的地图中确实存在一个可行的路径到目标位置，则当迭代次数k趋于无穷时，目标位置必然被采样采到，即其中p_goal为目标位置。

定理2如果存在一个长度为l的集序列{A}＝({A₀},{A₁},...,{A_l})，其中{A₀}为初始位置，令为在信息空间内取到集{A_l}的概率，则i次迭代后，RRT算法无法找到路径的概率低于e^-0.5(ipl-2l)。

以下用一个具体的应用实例对本发明的操作进行详细描述，本发明的增量式环境信息采样的最优路径规划方法主要应用于室内移动机器人的自主导航和避障规划中，这里主要针对静态和动态两类障碍物环境中的目标点抵达来体现其性能。具体设置如下：

实验环境设置为栅格地图，地图大小N＝50m，地图中兼具动态和静态两类障碍，动态障碍初始位于(5,10)，以速度1m/s在(5,10)与(15,10)之间往返运动，静态障碍物为L形障碍，其中静态障碍物1的三个顶点分别为(18,20)，(25,20)，(25,10)，静态障碍物2的三个顶点分别为(30,40)，(30,35)，(38,35)。室内移动机器人只能通过其底盘携带的八个声呐传感器获得移动前方八个方向的地图障碍物信息。障碍物影响放大系数k_m＝200，障碍物影响范围σ_i＝1，i＝1,2,..,M，室内移动机器人的探测范围R_detect＝10m，基础移动速度v₀＝2m/s，室内移动机器人起始点为(0,0)，目标点为(48,48)。

如图4所示，给出了室内移动机器人在兼具动态和静态环境中的实时动态路径规划轨迹。其中，图(a)增量式环境信息采样的最优路径规划方法躲避动态障碍时的效果图；图(b)增量式环境信息采样的最优路径规划方法躲避静态障碍时的搜索树效果图；图(c)增量式环境信息采样的最优路径规划方法最终完成时的效果图；“○”代表机器人移动路径，“□”代表障碍物，“*”代表目标位置。从图中可以看出，在给出目标位置后，尽管室内移动机器人对环境中的障碍物信息获取不全，且存在动态障碍，其依靠增量式环境信息采样的最优路径规划方法能够实现良好的目标抵达和避障功能，具有较好的规划效率和避障功能。

以上所述仅是本发明技术的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和替换，这些改进和替换也应视为本发明的保护范围。

Claims (4)

1.一种室内移动机器人增量式环境信息采样的最优路径规划方法，其特征在于，包括以下几个步骤：

步骤1：建立已探测区域障碍物碰撞风险的评估概率模型；

步骤2：进行基于增量式环境信息采样的最优路径规划；

在地图范围内以起点为搜索树第一个点，以步骤1获得的评估概率模型生成的随机采样状态点进行增量式迭代扩展，产生新的树节点，并计算新的树节点的代价函数，以树节点的最小代价函数修改树结构，直到生成的路径数达到设定路径数Path_n，以所有生成路径中路径代价函数最小的路径作为潜在最优路径；

步骤3：依据步骤2得到的潜在最优路径和室内移动机器人的当前位置和速度矢量，判断室内移动机器人的偏转角φ_t是否满足轮式机器人不能侧向滑动的非完整约束，从而确定室内移动机器人是否按照最优路径方向移动；

若室内移动机器人的偏转角φ_t不满足轮式机器人不能侧向滑动的非完整约束，则返回步骤2；

若满足，则沿当前时刻规划出的潜在最优路径方向移动，进入下一时刻的路径规划，t＝t+1，返回步骤1，直到室内移动机器人移动到目标位置，完成路径规划；

所述室内移动机器人的偏转角φ_t满足轮式机器人不能侧向滑动的非完整约束是指：φ_t∈[0,60°]∪[120°,180°]；

其中，p_t和p_t-1分别为室内移动机器人当前位置和上一时刻所在位置，为在t时刻，即室内移动机器人在t+1时刻的预抵达位置；

p_t-1由p_t-1＝p_t-vt得到，v为设定的室内移动机器人移动速度；

所述步骤2中最优路径的规划过程如下：

步骤2.1：令k＝1，k表示迭代次数；

步骤2.2：依据步骤1建立的障碍物碰撞风险的评估概率模型指导地图中随机采样的过程，获得随机采样状态点p_rand(k)；

步骤2.3：遍历已生成树{G＝(p,e)}找出距离随机采样状态点p_rand(k)最近的树上节点p_nearest(k，₎进入步骤2.4；

步骤2.4：从树上距离最近的节点p_nearest(k)向随机采样状态节点p_rand(k)方向，以步长为S_e生长出新的节点p_new(k)，将节点p_new(k)添加至已生成树中{G＝(p,e)}，计算节点p_new(k)的代价函数，并记录相应节点之间的父子关系，father(p_new(k))＝p_nearest(t)；

其中，树上任意节点的代价函数按以下公式计算：

其中，k_a为障碍物影响的放大系数，取值范围为[50,1000]；C为由节点p_G到其父节点father(p_G)的路径点集合；father(p_G)为p_G的父节点；F(X,Y)表示已探测区域障碍物碰撞风险的评估概率；

步骤2.5：在已生成树{G＝(p,e)}中找出所有处于以节点p_new(k)为圆心，r_n为半径内的节点集合{p_near}，分别测试p_new(k)若以集合{p_near}中的节点为父节点是否会得到更小的代价函数，若更小，则以最小代价函数对应的节点作为p_new(k)的父节点，并更新节点p_new(k)的代价函数，进入步骤2.6；否则，直接进入步骤2.6；

步骤2.6：判断新增节点p_new(k)与目标位置的距离是否小于树的生长步长S_e，若小于该步长且两点之间的线段无碰撞障碍，则直接将目标位置相连，形成新的路径，且以当前新增节点的代价函数作为最新路径的代价函数，路径计数器Path+1，进入步骤2.7；否则，直接进入步骤2.7；

步骤2.7：测试集合{p_near}中节点若以p_new(k)为父节点是否会得到更小的代价函数，若更小，则更改该节点的父节点为p_new(k)并更新相应节点的代价函数；

步骤2.8：判断路径计数器Path是否达到设定的产生路径数Path_n，若达到，则退出迭代过程，进入步骤2.9，若没达到，则k＝k+1，返回步骤2.2；

步骤2.9：在已生成路径中，选择路径代价函数最小的路径作为当前时刻t的潜在最优规划路径。

2.根据权利要求1所述的方法，其特征在于，所述步骤1的具体过程如下：

首先，通过室内移动机器人自身携带的声呐传感器，获得室内移动机器人的周围环境信息；其次，将室内移动机器人所经过的区域作为已探测区域，依据所述的周围环境信息建立已探测区域障碍物碰撞风险的评估概率模型，并依据声呐传感器实时采集的周围环境信息实时更新已探测区域障碍物碰撞风险的评估概率模型；

所述已探测区域障碍物碰撞风险的评估概率如下：

其中，室内移动机器人工作空间信息集合为所述空间信息包括所有目标位置及所有障碍物位置；{(X,Y)}为已探测区域，在的地图上探测到有M个障碍物，f_i(X,Y)为第i个障碍物的对室内移动机器人路径选择的影响函数，采用正态分布表示如下：

其中，σ_i为第i个障碍物的影响范围系数，取值范围为[0,1]；D_i为第i个障碍物到已探测区域内所有位置的距离矩阵，矩阵大小与地图大小一致为N×N。

3.根据权利要求2所述的方法，其特征在于，所述步骤2中基于障碍物碰撞风险的评估概率模型指导地图中随机采样，其采样的过程如下：

其中，N(·)为归一化处理，F(X,Y)为障碍物碰撞风险的评估概率。

4.根据权利要求3所述的方法，其特征在于，所述步骤1中室内移动机器人自身所携带声呐传感器是指所采用底盘Pioneer-2DX自身携带的声呐探测雷达。