CN110083165A - 一种机器人在复杂狭窄环境下路径规划方法 - Google Patents

Abstract

本发明公开了一种机器人在复杂狭窄环境下路径规划方法，将RRT‑Connect算法和强化学习算法相结合，保留了随机树的一定随机性，并根据障碍物密度程度选择合适的步长，既可以实现机器人在障碍物密集环境快速随机探索，在障碍物稀疏环境快速通过，并增加了随机树分布的导向性，提高收敛速度，又具能够在与环境的交互过程中提升规划性能，使规划出的路径接近最优路径，避免陷入局部最小值；本发明改进算法与原始标准算法相比,规划出来的路径更优,耗费时间更少。

Description

一种机器人在复杂狭窄环境下路径规划方法

技术领域

本发明涉及一种路径规划方法，具体说是一种机器人在复杂狭窄环境下路径规划方法。

背景技术

路径规划是智能机器人研究领域的一个重要方向，机器人的路径规划分为全局路径规划和局部路径规划。目前常用的局部路径规划方法主要基于人工势场法，栅格法等，并且结合遗传算法、模糊逻辑算法、神经网络等智能搜索算法，以提高搜索效率、系统鲁棒性以及适应能力。

模糊方法是在线规划中通常采用的一种规划方法，包括建模和局部规划。但模糊逻辑方法模糊逻辑规则需要依赖人为经验，制约了该算法在机器人路径规划领域的广泛应用。

遗传算法是模拟自然界遗传机制和生物进化论而形成的一种过程搜索最优解的算法，在机器人路径规划研究中应用较多，但遗传算法适应广泛环境的能力弱，且模型进化速度很难掌控，因此算法具有一定的局限性，另外，遗传算法容易陷入局部最优解，导致算法出现早熟现象。

神经网络方法利用类似于大脑神经网络突触连接的结构来对信息进行表达和处理，但神经网络在学习过程中需要对大量样本进行训练，需要人为设定较多的经验参数，因此应用在机器人路径规划领域时存在一定的局限性。

发明内容

针对现有技术存在上述缺陷，本申请提供了一种机器人在复杂狭窄环境下路径规划方法，利用结合强化学习的RRT-Connect(RL-RRT-C)算法解决路径规划问题，不仅能够与环境交互，提高移动机器人在未知环境中的自适应性和自学习能力，还能广泛应用于各种复杂障碍物以及狭窄的环境。

为实现上述目的，本申请的技术方案为：一种机器人在复杂狭窄环境下路径规划方法，具体包括如下步骤：

步骤一：初始化地图信息；

步骤二：创建双树；

步骤三：基于SARSA算法随机采样节点x_rand；

步骤四：搜索最近邻节点x_near；

步骤五：搜索扩展新节点x_new；

步骤六，扩展双树；

步骤七，反向搜索规划最终路径。

进一步的，在步骤一中，初始化地图信息，导入环境地图，包括环境边界和大小不一的障碍物信息，定义二维平面上X∈R²为配置节点空间，X的元素称为配置节点；同时二维空间中分布着有限静态障碍物区域集合则无障碍物区域为X_free:＝X-X_obs，R²为二维空间。

进一步的，在步骤二中，初始化双树T₁和T₂，根据机器人的相对起始点，配置节点x_init(x_s,y_s)及偏角θ_s，即起始点的位姿信息X_init(x_s,y_s,θ_s)，再配置相对目标节点x_goal(x_g,y_g)及偏角θ_g，即目标点的位姿信息X_goal(x_g,y_g,θ_g)，以及根据障碍物密集情况的不同选择不同的步长ρ₁、ρ₂。

进一步的，在步骤三中，同时以初始点X_init(x_s,y_s,θ_s)和目标点X_goal(x_g,y_g,θ_g)为起点分别扩展随机树T₁和T₂,采用强化学习SARSA算法学习避障策略在无障碍区域内生成随机采样点x_rand,，设R:∑_free→R≥0为回报函数，将每个无碰撞轨迹映射到非负回报，降低搜索的随机性,提高搜索效率；

随机节点x_rand是根据扩展函数选择随机树的新节点，节点回报函数R(x)通过计算每个节点到目标和障碍物的回报值来影响随机节点的选取，根据障碍物的出现与否设置两种不同的权值，当出现障碍物则躲避障碍物动作函数优先级最高，先保证安全，其次是目标动作函数，再考虑导向目标，选择的原则是使受到正的奖惩的概率增大；这样设计既可绕过障碍物，又可朝着目标点方向生长。R(x)公式表示为：

其中，为避障动作回报值，目标动作回报值，k是动作回报函数的权值，取值范围0到1，k越接近1则表示越重视躲避障碍物情况，k越接近0则表示越重视目标回报。

x_t表示节点当前位置，如图3所示，当x_t＜d₁时，表示会发生碰撞，会获得R＝-2k-(1-k)＝-k-1的回报值；当d₁＜x_t＜d₂时，表示需要先躲避障碍物，执行避障动作，如果离目标越来越近即会获得R＝2k+(1-k)＝k+1的回报值；当d₂＜x_t时，表示节点周围没有障碍物，执行导向目标动作，如果到达目标点时，会获得最大回报值，其他情况会获得的回报值；即

随机树节点的扩展受到随机采样点的影响，通过以上方式的处理，随机采样点不再是在可行域内随机分布，而是具有一定的趋向性，从而路径规划的随机性受到一定的限制，使得规划性能和规划质量得到提高。

进一步的，在步骤五中，在选择新的点x_new时，首先根据x_near找到随机树上Q值介于Q^*(s,a)-ΔQ到Q^*(s,a)的节点，然后通过计算从节点中选择符合机器人角度约束条件|θ(x)|≤θ_max以及满足避障条件的节点，最后根据障碍物密集情况的可变步长ρ₁、ρ₂，选择最近邻节点x_near的点即x_new，其中Q^*(s,a)为最佳Q值，ΔQ为Q值变化范围；

扩展新的点x_new是根据E(x)扩展函数,E(x)包含R(x)节点回报函数，L(x)目标距离函数以及角度约束条件θ，表示为如下：

E(x)＝R(x)+L(x)+θ (3)

L(x)目标距离函数通过计算随机扩展点与当前目标节点的距离来影响新节点的选取，L(x)公式表示为：

其中，ρ为随机树生长的可变步长，x_rand和x_near分别表示随机扩展点和最近临节点。

当环境中的障碍物密集时，ρ为ρ₁；当环境中的障碍物稀疏时，ρ为ρ₂。

将式(2)、(4)代入式(3)中得到：

根据式(4)得到引入E(x)扩展函数后新节点x_new的生成公式：

E(x)扩展函数对于扩展树的生长起着关键的影响。一方面，当节点处于无障碍物的环境时，目标动作回报函数比重会增大，这样节点移动在很大程度上被L(x)目标距离函数所影响，每走一步都是高效的。另一方面，当节点与障碍物距离较近时，减小目标动作回报函数影响比重，增大避障动作回报函数影响比重，使扩展树迅速绕开障碍物，这样节点在很大程度上被R(x)节点回报函数所影响，并且旋转角度符合约束，节点得以平稳向无障碍区域移动。扩展函数为机器人的路径重规划提供了高效、准确的导向信息。

进一步的，在步骤七中，反向搜索规划最终路径，直到两颗随机树新节点在状态区域范围内相遇，路径规划完成，即在σ:[0,1]→X_free找到从初始配置σ(0)＝x_init开始并到达目标σ(1)＝x_goal配置的无碰撞路径；用公式表示为最优运动规划问题是找到解决运动规划问题的无碰撞路径σ^*:[0,1]→X_free，并最小化回报函数R(·)，即，R(σ^*)为最优无碰撞路径，R(σ′)为无碰撞路径。

本发明由于采用以上技术方案，能够取得如下的技术效果：本发明在路径规划算法方面，解决了复杂障碍物狭窄环境下的机器人局部路径动态重规划问题，采用强化学习驱动的规划思想，学习避障策略，将预测控制与反馈机制有效结合起来，其在复杂障碍物狭窄环境下路径规划应用中具有高效性、有效性和适应性。

附图说明

图1为本方法流程图；

图2为RL-RRT-C算法具体流程图；

图3为距离位置示意图；

图4为转向角约束图；

图5为两种算法在障碍物稀疏狭窄通道性能比较图；

图6为两种算法在障碍物密集狭窄通道性能比较图；

图7为不同环境下RRT-Connect算法和RL-RRT-C算法算法有效节点比较图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步详细的描述：以此为例对本申请做进一步的描述说明。

本申请采用RRT-RL-C算法利用SARSA(λ)方法，在节点扩展时选择高回报的节点，新节点总是处于靠近目标点且性能评价好的位置，使得随机搜索树在每一次循环中作用发挥到最大，并且扩展树分支叶子节点数少、迭代次数少、规划路径短，在复杂的配置空间中更快寻找最优或近似最优解。RRT-Connect树在空间中是随机扩展的，因而存在父节点与子节点相对位置的随机性，然而考虑到机器人前进过程中的稳定性，RL-RRT-C算法引入路径转角的阈值，它是由三个相邻节点的位置关系加以约束。在RRT中取三个相邻的节点x_new，x_near，以及x_near的父节点x_init，则三个节点可构成两个向量，分别为向量向量取向量构成的夹角为θ，如图4转向角约束所示。在不同的环境地图中，障碍物分布也不同，当环境地图中的障碍物分布密集时，即ρ₁＝15；当环境地图中的障碍物分布稀疏时，随机树的扩展应当高效，步长的选取应该较长的，即ρ₂＝30。

如图5-6所示，对密集程度不同的障碍物环境、狭窄通道时本方法的优势；

表1 RRT-Connect和RL-RRT-C算法不同环境节点数比较

表2 RRT-Connect和RL-RRT-C算法不同环境性能比较

本发明在路径规划算法方面，应用RRT-RL-C算法解决复杂障碍物狭窄环境下的机器人局部路径动态重规划问题，采用强化学习驱动的规划思想，学习避障策略，将预测控制与反馈机制有效结合起来，并将其和其他路径规划算法做仿真对比，如表1、2和图7所示，验证了其在起复杂障碍物狭窄环境下路径规划应用中的高效性、有效性和适应性。

以上所述，仅为本发明创造较佳的具体实施方式，但本发明创造的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明创造披露的技术范围内，根据本发明创造的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明创造的保护范围之内。

Claims (6)

1.一种机器人在复杂狭窄环境下路径规划方法，其特征在于，具体包括如下步骤：

步骤一：初始化地图信息；

步骤二：创建双树；

步骤三：基于SARSA算法随机采样节点x_rand；

步骤四：搜索最近邻节点x_near；

步骤五：搜索扩展新节点x_new；

步骤六，扩展双树；

步骤七，反向搜索规划最终路径。

2.根据权利要求1所述一种机器人在复杂狭窄环境下路径规划方法，其特征在于，在步骤一中，初始化地图信息，导入环境地图，包括环境边界和大小不一的障碍物信息，定义二维平面上X∈R²为配置节点空间，X的元素称为配置节点；同时二维空间中分布着有限静态障碍物区域集合则无障碍物区域为X_free:＝X-X_obs，R²为二维空间。

3.根据权利要求1所述一种机器人在复杂狭窄环境下路径规划方法，其特征在于，在步骤二中，初始化双树T₁和T₂，根据机器人的相对起始点，配置节点x_init(x_s,y_s)及偏角θ_s，即起始点的位姿信息X_init(x_s,y_s,θ_s)，再配置相对目标节点x_goal(x_g,y_g)及偏角θ_g，即目标点的位姿信息X_goal(x_g,y_g,θ_g)，以及根据障碍物密集情况的不同选择不同的步长ρ₁、ρ₂。

4.根据权利要求3所述一种机器人在复杂狭窄环境下路径规划方法，其特征在于，在步骤三中，同时以初始点X_init(x_s,y_s,θ_s)和目标点X_goal(x_g,y_g,θ_g)为起点分别扩展随机树T₁和T₂,采用强化学习SARSA算法学习避障策略在无障碍区域内生成随机采样点x_rand,，设R:∑_free→R≥0为回报函数，将每个无碰撞轨迹映射到非负回报；

随机节点x_rand是根据扩展函数选择随机树的新节点，节点回报函数R(x)通过计算每个节点到目标和障碍物的回报值来影响随机节点的选取，根据障碍物的出现与否设置两种不同的权值，当出现障碍物则躲避障碍物动作函数优先级最高，其次是目标动作函数，再考虑导向目标，选择的原则是使受到正的奖惩的概率增大；R(x)公式表示为：

其中，为避障动作回报值，目标动作回报值，k是动作回报函数的权值，取值范围0到1，k越接近1则表示越重视躲避障碍物情况，k越接近0则表示越重视目标回报；

设x_t表示节点当前位置，当x_t＜d₁时，表示会发生碰撞，会获得r_t ^e＝-1，R＝-2k-(1-k)＝-k-1的回报值；当d₁＜x_t＜d₂时，表示需要先躲避障碍物，执行避障动作，如果离目标越来越近即r_t ^e＝1，会获得R＝2k+(1-k)＝k+1的回报值；当d₂＜x_t时，表示节点周围没有障碍物，执行导向目标动作，如果到达目标点时，会获得最大回报值，其他情况会获得的回报值；即

5.根据权利要求1所述一种机器人在复杂狭窄环境下路径规划方法，其特征在于，在步骤五中，在选择新的点x_new时，首先根据x_near找到随机树上Q值介于Q^*(s,a)-ΔQ到Q^*(s,a)的节点，然后通过计算从节点中选择符合机器人角度约束条件|θ(x)|≤θ_max以及满足避障条件的节点，最后根据障碍物密集情况的可变步长ρ₁、ρ₂，选择最近邻节点x_near的点即x_new，其中Q^*(s,a)为最佳Q值，ΔQ为Q值变化范围；

扩展新的点x_new是根据E(x)扩展函数,E(x)包含R(x)节点回报函数，L(x)目标距离函数以及角度约束条件θ，表示为如下：

E(x)＝R(x)+L(x)+θ (3)

L(x)目标距离函数通过计算随机扩展点与当前目标节点的距离来影响新节点的选取，L(x)公式表示为：

其中，ρ为随机树生长的可变步长，x_rand和x_near分别表示随机扩展点和最近临节点。

当环境中的障碍物密集时，ρ为ρ₁；当环境中的障碍物稀疏时，ρ为ρ₂；

将式(2)、(4)代入式(3)中得到：

根据式(4)得到引入E(x)扩展函数后新节点x_new的生成公式：

6.根据权利要求1所述一种机器人在复杂狭窄环境下路径规划方法，其特征在于，在步骤七中，反向搜索规划最终路径，直到两颗随机树新节点在状态区域范围内相遇，路径规划完成，即在σ:[0,1]→X_free找到从初始配置σ(0)＝x_init开始并到达目标σ(1)＝x_goal配置的无碰撞路径；用公式表示为最优运动规划问题是找到解决运动规划问题的无碰撞路径σ^*:[0,1]→X_free，并最小化回报函数，即，R(σ^*)为最优无碰撞路径，R(σ′)为无碰撞路径。