一种基于动态步长的目标引力双向RRT路径规划方法技术

本发明专利技术涉及一种基于动态步长的目标引力双向RRT路径规划方法，属于智能机器人路径规划领域。本发明专利技术基于目标引力思想的双向RRT算法的基础上引入动态步长概念，利用步长的动态变化来扩展RRT随机树的新节点Xnew，不仅可以利用双向RRT算法的随机性，还可以利用目标引力思想使新节点Xnew朝着目标点Xgoal方向扩展的特性，同时解决了在多障碍物环境下目标不可达问题，能有效地提高路径搜索成功率，加强避障能力。

【技术实现步骤摘要】
一种基于动态步长的目标引力双向RRT路径规划方法
本专利技术涉及一种基于动态步长的目标引力双向RRT路径规划方法，属于智能机器人路径规划领域。
技术介绍
智能机器人在移动过程中不可避免会遇到各式各样的障碍物，灵活、实时的躲开这些障碍物是衡量其性能的关键指标。具有避障功能的智能机器人拥有相当高的社会价值，被大量应用于航天、军事、制造业、医疗等领域。路径规划技术是智能机器人领域中的核心问题之一，近年来受到外界广泛关注。智能机器人的工作环境越来越复杂，基本上存在大量无规则障碍物且障碍物分布不均匀的复杂环境，包括固定障碍物和移动障碍物，以及一些突发情况产生的障碍物。路径规划范围又分为全局规划与局部规划，全局规划在遇突发情况时不能有效地生成一条完整的路径；局部规划在遇见突发情况时可以采取措施重新规划新的生长路径。双向RRT算法虽然可以解决环境约束、时间约束、机器人本身的动力学约束以及高维空间路径规划等问题，但由于该方法需要对全局空间进行随机采样，导致实时性差、效率低；基于目标引力思想的双向RRT算法可以降低路径搜索的随机性，但在多障碍物的环境下却存在目标不可达问题，效率较低，且未能使规划的路径达到最优或次优解。
技术实现思路
本专利技术提供了一种基于动态步长的目标引力双向RRT路径规划方法，以用于通过动态调整步长的方式实现路径的规划。本专利技术的技术方案是：一种基于动态步长的目标引力双向RRT路径规划方法，所述方法步骤如下：Step1、设置智能机器人所处的状态空间C，静态障碍物随机分布，大小不一；Step2、确定智能机器人在状态空间C中的初始点Xinit和目标点Xgoal位置；其中，状态空间C指代工作环境，状态空间C由自由空间Xfree和障碍空间Xobs构成，障碍空间Xobs由静态障碍物构成，自由空间Xfree表示除障碍空间Xobs之外的空间；初始点Xinit和目标点Xgoal位于自由空间Xfree；Step3、同时以初始点Xinit和目标点Xgoal为起点分别扩展RRT随机树Ti和Tj，搜索过程中，根据基于动态步长的目标引力双向RRT算法新节点Xnew的位置确定公式来搜索新节点Xnew，每搜索新节点Xnew一次，将其加入到对应的随机树中，直到两颗随机树新节点Xnew在状态空间C中相遇，选出两颗随机树新节点Xnew连通后生成的最优路径并保存。所述根据基于动态步长的目标引力双向RRT算法新节点Xnew的位置确定公式来搜索新节点Xnew，具体为：其中，ρ为朝着随机点Xrand方向生长的步长，k为引力场系数，kρ为朝着目标点Xgoal方向生长的步长，||Xrand－Xnear||代表欧拉范数定义的Xrand与Xnear之间的距离，||Xgoal－Xnear||代表欧拉范数定义的Xgoal与Xnear之间的距离，Xrand为每次扩展RRT随机树时在自由区域Xfree中随机选取的点，Xnear为每次扩展RRT随机树时在当前RRT随机树中与Xrand距离最近的点，Xnew每次新增加的扩展点。当RRT随机树在运动范围内遇到障碍物时取ρ大于等于kρ，根据扩展新节点Xnew的公式在ρ大于等于kρ时生成新节点Xnew的方法，RRT随机树将具有双向RRT算法的随机性，偏向随机点Xrand方向生长，不会碰到障碍物；当RRT随机树运动范围内没有遇到障碍物时取ρ小于kρ，根据扩展新节点Xnew的公式在ρ小于kρ时生成新节点Xnew的方法，RRT随机树偏向终点方向生长。取值步长ρ固定不变，通过调节引力场系数k来决定新节点Xnew的扩展方向；或者引力场系数k固定不变，通过调节步长ρ来决定新节点Xnew的扩展方向。本专利技术的有益效果是：本专利技术基于目标引力思想的双向RRT算法的基础上引入动态步长概念，利用步长的动态变化来扩展RRT随机树的新节点Xnew，不仅可以利用双向RRT算法的随机性，还可以利用目标引力思想使新节点Xnew朝着目标点Xgoal方向扩展的特性，同时解决了在多障碍物环境下目标不可达问题，能有效地提高路径搜索成功率，加强避障能力。附图说明图1为根据本专利技术提出的基于动态步长的目标引力双向RRT路径规划算法的主流程图；图2为实验环境图；图3为基于目标引力的双向RRT算法新节点前方无障碍物扩展示意图；图4为基于目标引力的双向RRT算法路径规划过程图；图5为基于目标引力的双向RRT算法路径规划结果图；图6为基于动态步长的目标引力双向RRT算法新节点前方有障碍物扩展示意图；图7为基于动态步长的目标引力双向RRT算法路径规划过程图；图8为基于动态步长的目标引力双向RRT算法路径规划结果图。具体实施方式下面结合附图和实施例，对本专利技术作进一步说明，但本专利技术的内容并不限于所述范围。实施例1：如图1-8所示，一种基于动态步长的目标引力双向RRT路径规划方法，其主流程图如图1所示，所述方法的具体步骤如下：步骤一、搭建智能机器人的工作环境，将其设置在1000×1000的环境中，如图2所示，静态障碍物用黑色不规则形状随机分布，大小不一；步骤二、确定智能机器人在状态空间C中的初始点Xinit位置和目标点Xgoal位置。图2左下角[0,0]为智能机器人的初始点Xinit,右上角[1000，1000]为智能机器人的目标点Xgoal。其中，状态空间C指代工作环境；状态空间C由自由空间Xfree和障碍空间Xobs构成，障碍空间Xobs由静态障碍物构成，自由空间Xfree表示除障碍空间Xobs之外的空间；初始点Xinit和目标点Xgoal位于自由空间Xfree。步骤三、同时以初始点Xinit和目标点Xgoal为起点分别扩展RRT随机树Ti和Tj；步骤四、以随机树Ti为例，每一个新节点Xnew处都引入一个目标引力思想，不仅有原来随机方向的随机点Xrand，还增加了目标方向的目标点Xgoal。根据基于动态步长的目标引力双向RRT算法新节点Xnew的位置确定公式来搜索新节点。式中：ρ(ρ>0)：表示朝着随机点Xrand方向生长的步长；k(k>0)：表示引力场系数，k与ρ之积kρ为朝着目标点Xgoal方向生长的步长；||Xrand－Xnear||：表示欧拉范数定义的Xrand与Xnear之间的距离；||Xgoal－Xnear||：代表欧拉范数定义的Xgoal与Xnear之间的距离；实际步长为|(k+1)ρ|，公式表示将(Xrand－Xnear)向量单位化后乘以步长ρ，加上(Xgoal－Xnear)向量单位化后乘以步长kρ，再加上Xnear，最后得出新节点Xnew。初始化时随机树Ti只包含一个节点：初始点Xinit。首先从状态空间中随机选择一个采样点Xrand；然后从随机树中选择一个距离Xrand最近的节点Xnear，此时Xnear为Xinit。实验环境为windows2007，Intel(R)Core(TM)2、3GHZ、2G内存，编译工具为MATLAB2012b。状态空间C为1000×1000，X轴与Y轴坐标范围均为0～1000，原点为[0,0]，两点间的直线距离约为1414m。引入目标引力思想后在随机点Xrand与目标点Xgoal的共同作用下，新节点Xnew处在以Xnear、随机点Xrand与目标点Xgoal组成的三角区域中，如图3所示，在前方无障碍本文档来自技高网...

【技术保护点】
1.一种基于动态步长的目标引力双向RRT路径规划方法，其特征在于：所述方法步骤如下：Step1、设置智能机器人所处的状态空间C，静态障碍物随机分布，大小不一；Step2、确定智能机器人在状态空间C中的初始点Xinit和目标点Xgoal位置；其中，状态空间C指代工作环境，状态空间C由自由空间Xfree和障碍空间Xobs构成，障碍空间Xobs由静态障碍物构成，自由空间Xfree表示除障碍空间Xobs之外的空间；初始点Xinit和目标点Xgoal位于自由空间Xfree；Step3、同时以初始点Xinit和目标点Xgoal为起点分别扩展RRT随机树Ti和Tj，搜索过程中，根据基于动态步长的目标引力双向RRT算法新节点Xnew的位置确定公式来搜索新节点Xnew，每搜索新节点Xnew一次，将其加入到对应的随机树中，直到两颗随机树新节点Xnew在状态空间C中相遇，选出两颗随机树新节点Xnew连通后生成的最优路径并保存。

【技术特征摘要】
1.一种基于动态步长的目标引力双向RRT路径规划方法，其特征在于：所述方法步骤如下：Step1、设置智能机器人所处的状态空间C，静态障碍物随机分布，大小不一；Step2、确定智能机器人在状态空间C中的初始点Xinit和目标点Xgoal位置；其中，状态空间C指代工作环境，状态空间C由自由空间Xfree和障碍空间Xobs构成，障碍空间Xobs由静态障碍物构成，自由空间Xfree表示除障碍空间Xobs之外的空间；初始点Xinit和目标点Xgoal位于自由空间Xfree；Step3、同时以初始点Xinit和目标点Xgoal为起点分别扩展RRT随机树Ti和Tj，搜索过程中，根据基于动态步长的目标引力双向RRT算法新节点Xnew的位置确定公式来搜索新节点Xnew，每搜索新节点Xnew一次，将其加入到对应的随机树中，直到两颗随机树新节点Xnew在状态空间C中相遇，选出两颗随机树新节点Xnew连通后生成的最优路径并保存。2.根据权利要求1所述的基于动态步长的目标引力双向RRT路径规划方法，其特征在于：所述根据基于动态步长的目标引力双向RRT算法新节点...

【专利技术属性】
技术研发人员：胡兵，向凤红，毛剑琳，郭宁，
申请(专利权)人：昆明理工大学，
类型：发明
国别省市：云南,53