CN113391633A - 一种面向城市环境的移动机器人融合路径规划方法 - Google Patents

Abstract

本发明公开了一种面向城市环境的移动机器人融合路径规划方法，属于移动机器人运动控制技术领域。该方法包括如下步骤：第一步：移动机器人根据激光雷达探测到的障碍物信息，判断采用全局规划还是局部路径规划；第二步：当机器人与障碍物之间的距离大于设定阈值时，采用人工势场法进行全局路径规划方式；当机器人与障碍物之间的距离小于设定的阀值时，采用DDPG算法进行局部避障；第三步：判断移动机器人与目标点的距离，若小于设定阀值，则完成任务；若大于设定阀值，重复第一步和第二步，直至到达目标点。本发明实现移动机器人从起点到终点找到一条无碰撞的最优路径。

Description

一种面向城市环境的移动机器人融合路径规划方法

技术领域

本发明涉及一种面向城市环境的移动机器人融合路径规划方法，属于移动机器人运动控制技术领域。

背景技术

移动机器人完成其它任务的首要前提就是要移动到指定目标位置，因此路径规划技术是移动机器人技术中关键的技术之一。移动机器人在城市环境中要完成路径规划，不仅要考虑到静态的障碍物，同时也要考虑到动态的障碍物；由于城市环境十分复杂多变，要建立完整的地图信息十分困难，因此未知环境下的移动机器人路径规划十分重要，同时也给移动机器人进行路径规划增加了很大的难度。

目前路径规划算法可以分为全局路径规划和局部路径规划两大类。全局算法包括A*算法，Dijkstra算法，可视图算法，自由空间法等；全局路径规划算法通常需要提前获取全局地图信息，但是移动机器人工作的环境通常是动态的，并且难以获取准确的环境信息，因此只能将全局路径规划，划分为若干个子规划过程，即局部路径规划。局部路径规划大致可以分为4类：随机采样规划方法、人工势场法、图搜索法和群智能优化法。快速扩展随机树算法(Rapidly-exploring Random Trees,RRT)是经典的基于随机采样的算法之一，该算法能快速实现路径规划而得到广泛的应用，但是其无法保证路径最优。人工势场法虽然能应用于未知环境中，但是其容易陷入局部最优。基于群智能算法的代表算法包含遗传算法、粒子算法、蚁群算法及一些融合算法等，群智能算法因其计算复杂度高，不适用于未知动态环境中机器人路径规划。

近年来随着强化学习和神经网络的发展，深度强化学习算法被认为是解决未知动态环境最具有潜力的方法，其中就有DQN(Deep Q-learning Network深度Q网络)算法、DDPG(Deep Deterministic Policy Gradient深度确定性策略梯度)算法、A3C(AsynchronousAdvantage Actor-Critic异步优势动作评价算法)等，深度强化学习算法可以实现端到端的移动机器人路径规划，不需要对环境进行建模；但是深度强化学习算法存在着收敛速度慢，不稳定等问题急需解决。

因此，能否很好的实现移动机器人在未知动态环境中的路径规划。决定着移动机器人的发展和应用，所以移动机器人路径规划技术是十分重要的。

发明内容

针对上述DDPG算法，本发明提出了一种面向城市环境的移动机器人融合路径规划方法，实现移动机器人从起点到终点找到一条无碰撞的最优路径。

本发明为解决其技术问题采用如下技术方案：

一种面向城市环境的移动机器人融合路径规划方法，包括如下步骤：

第一步：移动机器人根据激光雷达探测到的障碍物信息，判断采用全局规划还是局部路径规划；

第二步：当机器人与障碍物之间的距离大于设定阈值时，采用人工势场法进行全局路径规划方式；当机器人与障碍物之间的距离小于设定的阀值时，采用DDPG算法进行局部避障；

第三步：判断移动机器人与目标点的距离，若小于设定阀值，则完成任务；若大于设定阀值，重复第一步和第二步，直至到达目标点。

第二步中所述DDPG算法中的奖励函数是根据移动机器人是否到达目标点、与目标点的距离以及是否与障碍物发生碰撞设计的。

当移动机器人到达目标点时的奖励函数如下：

式中，R_{目标点奖励}为机器人是否到达目标点时的奖励，r_{到达目标点}为机器人到达目标点时的奖励，d₁为设定的机器人距离目标点的阀值，χ_t为t时刻移动机器人与目标点的距离；

移动机器人与目标点之间的距离变化，相关的奖励函数如下：

其中：R_距离奖励为机器人与目标点的距离发生变化时的奖励，r_{机器人与目标点的距离}为机器人与目标点距离的奖励值，χ_t为t时刻移动机器人与目标点的距离，χ_t-1为t-1时刻移动机器人与机器人之间的距离；

当移动机器人与障碍物发生碰撞时的奖励函数如下：

式中，R_{障碍物奖励}为机器人是否与障碍物发生碰撞时的奖励，r_碰撞为机器人与障碍物碰撞时的奖励，d₂为设定机器人距离障碍物的最小距离，d_i为激光雷达检测到最近的障碍物的距离。

第三步中所述移动机器人与目标点之间的距离：

其中：p_x和p_y分别表示移动机器人当前时刻的x轴坐标，y轴坐标，g_x和g_y分别表示目标点的x轴坐标和y轴坐标。

本发明的有益效果如下：

本发明方法，提出了使用基于人工势场法的动作选择策略进行修正训练完的DDPG算法策略网络给出的结果，保证了移动机器人能够找到一条无碰撞、相对更短、更加平滑的路径。在没有对移动机器人进行运动学建模，直接通过改进的DDPG算法实现了端到端的导航控制。在四种仿真实验环境中进行了验证，仿真实验表明，对于移动机器人，本发明能够有效实现未知动态环境下的路径规划，并提供一条相对平滑、距离更短的路径。

附图说明

图1为DDPG算法中的神经网络结构图。

图2(a)是环境一示意图；图2(b)是环境二示意图；图2(c)是环境三示意图；图2(d)是环境四示意图。

图3(a)DDPG算法在环境一中的奖励值坐标图；图3(b)DDPG算法在环境二中的奖励值坐标图；图3(c)DDPG算法在环境三中的奖励值坐标图；图3(d)DDPG算法在环境四中的奖励值坐标图。

图4为融合规划方法流程图。

图5(a)是融合算法与DDPG算法在环境一中的路径图；图5(b)是融合算法与DDPG算法在环境二中的路径图；图5(c)是融合算法与DDPG算法在环境三中的路径图；图5(d)是融合算法与DDPG算法在环境四中的路径图。

图6为融合规划方法与DDPG算法到达目标点成功率对比图。

图7(a)是移动机器人使用DDPG算法和融合算法在环境一中行驶过程的角速度图；图7(b)是移动机器人使用DDPG算法和融合算法在环境二中行驶过程的角速度图；图7(c)是移动机器人使用DDPG算法和融合算法在环境三中行驶过程的角速度图；图7(d)是移动机器人使用DDPG算法和融合算法在环境四中行驶过程的角速度图。

具体实施方式

下面结合附图对本发明做更进一步的解释。

一种面向城市环境下移动机器人融合路径规划方法，包括局部路径规划和全局路径规划。局部路径规划中，将机器人与障碍物的距离及方位作为关键因素，使用DDPG算法进行避障，并朝着目标点进行移动。全局路径规划中，采用机器人与目标点的距离方位以及与障碍物的距离方位等信息，通过人工势场法的计算方式得到移动机器人下一步的动作，实现移动机器人的路径规划。

本发明策略网络由四层神经网络构成，其网络结构如图1所示，输入层为移动机器人的当前状态，输出层为移动机器人的线速度和角速度，中间含2层隐含层。其中，输入层包含12个神经元，隐含层1包含250个神经元，第一层与隐含层1的连接方式为全连接，采用ReLU非线性激活函数；隐含层2的输入包含250个神经元，隐含层1与隐含层2的连接方式为全连接，使用ReLU激活函数；输出层包含2个神经元，隐含层2与输出层的连接方式同样为全连接，使用Sigmoid激活函数。价值网络采用类似的网络结构，与策略网络的主要区别在于输入层包含两部分，一部分为机器人当前状态，另外一部分为机器人当前动作值；输出层含一个神经元，为相应的Q值。神经网络的输入不包含图像信息，因此本发明采用全连接神经网络；同时为了与经典的DDPG算法对比，本发明采用四层神经网络。

当移动机器人周围存在障碍物时，其输出动作主要是由训练完的DDPG算法计算而来：

a_t＝π(s_t)

式中，a_t为移动机器人的动作，为二维数据，其中包含移动机器人的线速度和角速度。π(s_t)表示DDPG算法中的动作选择策略。

本发明仿真环境设计是基于ROS操作系统和Gazebo仿真软件。考虑到训练环境障碍物信息尽量多样性，本发明使用不同形状的障碍物模拟环境，环境中包含圆柱体障碍物、正方体障碍物、墙壁等，模拟环境的俯视图如图2所示。为了使移动机器人获得更好的避障能力，在环境三和四都设有三个动态障碍物，分别沿X轴方向运动、Y轴方向运动，和XY平面内运动；此外，环境四中还设置了含直角的墙壁障碍。

具体的，路径规划阶段包括如下具体步骤：

1、DDPG算法训练阶段

我们一共搭建了四种不同复杂程度的仿真环境，环境一为定目标点无障碍物，环境二为定目标点和静态障碍物，环境三为定目标点和动态障碍物，环境四为动目标点和动障碍物。环境三、四中的动障碍物一个是沿x轴方向做变速运动，另一个是沿y轴方向上做变速运动，最后一个是在x、y轴方向上做变速运动，其都有一定的范围界限。

我们在每个环境中训练一千次，在环境一和环境二中，训练模型基本达到较好的收敛效果，但是在环境三和环境四中，存在着一定程度的震荡。其训练奖励值结果如图3所示，图中epsiode_reward表示每个回合总的奖励值，epsiode表示回合数。

进一步的，所述DDPG算法中的奖励函数根据是否到达目标点、与目标点的距离以及是否与障碍物发生碰撞设计如下：

当移动机器人到达目标点时的奖励：

式中，R_{目标点奖励}为机器人是否到达目标点时的奖励，r_{到达目标点}为机器人到达目标点时的奖励，d₁为设定的机器人距离目标点的阀值，χ_t为t时刻移动机器人与目标点的距离。

当移动机器人与障碍物发生碰撞时的奖励：

式中，R_{障碍物奖励}为机器人是否与障碍物发生碰撞时的奖励，r_碰撞为机器人与障碍物碰撞时的奖励，d₂为设定机器人距离障碍物的最小距离，d_i为激光雷达检测到最近的障碍物的距离。

为引导移动机器人向着目标点前进，我们根据移动机器人与目标点之间的距离变化，设计了相关的奖励函数：

其中：R_距离奖励为机器人与目标点的距离发生变化时的奖励，r_{机器人与目标点的距离}为机器人与目标点距离的奖励值，χ_t为t时刻移动机器人与目标点的距离，χ_t-1为t-1时刻移动机器人与机器人之间的距离，

最后总的奖励函数为：

R＝R_{目标点奖励}+R_{障碍物奖励}+R_距离奖励

其中：R为机器人获得总的奖励值；

2、融合规划方法阶段：

我们首先加载训练完的DDPG算法的模型，然后机器人判断当前所处的状态，根据当前所处的状态选择进入全局路径规划模式还是局部路径规划模式，其具体流程图如图4所示：

所述的移动机器人全局路径规划的方法，其特征在于，所述全局路径的选择上根据移动机器人与目标点和周围障碍物的距离以及方位信息所共同确定的，a＝[a_v,a_ω]其中a_v＝0.25为移动机器人的线速度，a_ω＝α(bχ+n)+0.6τd_β(cβ²+tβ)为移动机器人的角速度，ρ为移动机器人与目标点的方位角ρ∈[-π，π]，，当ρ≥0,α＝1,ρ＜0,α＝-1，b为常数，n为常数，c为常数，β为最短激光束所在的序号，当(5-β)≥0,τ＝1，当(5-β)＜0,τ＝-1，d_β机器人与障碍物之间的距离，t为常数，χ为移动机器人当前位置与目标点的距离，β表示与移动机器人最近的障碍物所在激光雷达束的编号。

图5从路径规划的平滑程度及路径长度的角度给出移动机器人在四种环境中的路径规划效果。图例中ddpg表示深度确定性策略梯度算法，improve_ddpg表示改进后的融合算法。从图中可以看出在环境一二中，路径的平滑程度明显变好，路径长度也有了一定程度的减小，环境三四中路径的长度明显减小了很多。

表1路径规划长度实验结果对比

表1出DDPG算法和融合规划算法的路径长度对比数据，可以看出融合规划算法可使移动机器人行驶路径长度减少3％-10％。

图6从移动机器人路径规划稳定性的角度给出了移动机器人到达目标点的称功率指标。图中success rate表示到达目标点的成功率，environment表示仿真环境，DDPG表示深度确定性策略梯度算法,improve_DDPG表示融合算法。可以看出融合算法可使移动机器人在未知动态环境下的路径规划成功率提高10％。图7给出了移动机器人在行驶过程中角速度变化情况；图中angular velocity表示移动机器人的角速度，step表示步数，ddpg表示深度确定性策略梯度算法,fusion algorithm表示融合算法。

表2给出了机器人行驶过程中角速度超过1.0rad/s次数和角速度绝对值统计情况，移动机器人在行驶过程中角速度超过1.0rad/s的次数可减少5％至12.5％，可以看出融合算法明显提高行驶路径的平滑度。

表2角速度大于1.0rad/s的次数统计

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

Claims (4)

1.一种面向城市环境的移动机器人融合路径规划方法，其特征在于，包括如下步骤：

第一步：移动机器人根据激光雷达探测到的障碍物信息，判断采用全局规划还是局部路径规划；

第二步：当机器人与障碍物之间的距离大于设定阈值时，采用人工势场法进行全局路径规划方式；当机器人与障碍物之间的距离小于设定的阀值时，采用DDPG算法进行局部避障；

第三步：判断移动机器人与目标点的距离，若小于设定阀值，则完成任务；若大于设定阀值，重复第一步和第二步，直至到达目标点。

2.根据权利要求1所述的一种面向城市环境的移动机器人融合路径规划方法，其特征在于，第二步中所述DDPG算法中的奖励函数是根据移动机器人是否到达目标点、与目标点的距离以及是否与障碍物发生碰撞设计的。

3.根据权利要求2所述的一种面向城市环境的移动机器人融合路径规划方法，其特征在于，当移动机器人到达目标点时的奖励函数如下：

式中，R_{目标点奖励}为机器人是否到达目标点时的奖励，r_{到达目标点}为机器人到达目标点时的奖励，d₁为设定的机器人距离目标点的阀值，χ_t为t时刻移动机器人与目标点的距离；

移动机器人与目标点之间的距离变化，相关的奖励函数如下：

其中：R_距离奖励为机器人与目标点的距离发生变化时的奖励，r_{机器人与目标点的距离}为机器人与目标点距离的奖励值，χ_t为t时刻移动机器人与目标点的距离，χ_t-1为t-1时刻移动机器人与机器人之间的距离；

当移动机器人与障碍物发生碰撞时的奖励函数如下：

式中，R_{障碍物奖励}为机器人是否与障碍物发生碰撞时的奖励，r_碰撞为机器人与障碍物碰撞时的奖励，d₂为设定机器人距离障碍物的最小距离，d_i为激光雷达检测到最近的障碍物的距离。

4.根据权利要求1所述的一种面向城市环境的移动机器人融合路径规划方法，其特征在于，第三步中所述移动机器人与目标点之间的距离：

其中：p_x和p_y分别表示移动机器人当前时刻的x轴坐标，y轴坐标，g_x和g_y分别表示目标点的x轴坐标和y轴坐标。

Images