[发明专利]一种改进RRT算法重连策略的路径规划方法

权利要求书

1.一种改进RRT算法重连策略的路径规划方法，其特征在于，所述方法包括：

通过ROS系统下SLAM建图算法等方式获取实验场景地图，并确定在所述的实验场景地图中的起始点和目标点；

将起始点作为树扩展的根节点，在全局范围内进行随机采样，同时为了保证整体往目标点前进，采用目标导向采样策略，给予一定的概率将目标点作为采样点；

根据随机采样点，利用目标偏置及障碍物偏置策略进行树的扩展结点的选取；

根据扩展结点，寻找可达的最远父结点；

针对所选可达结点，在可达结点与可达结点的父结点之间利用二分法创建距离障碍物较近的结点；

根据所述的距离障碍物较近的结点，与扩展结点利用二分法进一步分割，同时确保距离障碍物较近的结点与父结点、扩展结点的连线为安全可通行路径，最终针对扩展结点创建新的父结点；

在新的父结点附近创建一定半径范围的超体球，利用三角不等式规则判断超体球内其他结点与所述新的父结点进行连接的路径，是否为更优路径；若是，则进行连接更新路径表及结点列表；若否，跳过此结点；

重复所述过程，直至达到最大迭代或扩展至目标点结束。

2.根据权利要求1所述的改进RRT算法重连策略的路径规划方法，其特征在于，地图应用场景主要为二维环境，其中，

工作空间为二维空间，且工作空间下仅有一个移动机器人、一个起始位置SP与目标位置GP；路径规划问题的目标是快地找到一条与障碍物无碰撞的最优或接近最优的路径，若无可行路径返回失败；为此，提出以下假设：

假设1：工作空间中存在多个规则或不规则静态障碍物；

假设2：移动机器人为一个圆形实体；因此为考虑移动机器人实际尺寸，障碍物根据移动机器人的半径进行扩展，因此移动机器人可以认为是一个点；

假设3：不考虑移动机器人的运动学约束；

假设4：移动机器人的运动是全方位的，可以向任何方向移动；

令Φ为定义的状态空间，Φ_obs为障碍空间，Φ_expand为扩展空间，Φ_free＝Φ-Φ_obs-Φ_expand为自由空间；目标区域被定义为半径为r的圆，可表述为：

r：Φ_goal＝{s||s-s_goal||≤r}

本文研究的目标即为计算一条可行路径，可表述为：

σ：[0，T]→Φ_free

其中σ(0)＝s_init为初始状态，σ(T)∈Φ_goal为目标状态。

3.根据权利要求1所述的改进RRT算法重连策略的路径规划方法，其特征在于，目标导向采样策略的函数公式为：

其中，x_rand是随机采样点，x_goal是目标点，p是预设概率值，p(x_rand)是随机生成概率值。

4.根据权利要求1所述的改进RRT算法重连策略的路径规划方法，其特征在于，目标偏置策略与障碍物偏置策略内容如下：

基于人工势场法思想与BG-RRT，引入目标偏置因子使得新生成结点具备一定的概率往目标点偏置；同时在一定半径范围内利用障碍物斥力偏置因子η，使得新生成的结点往障碍物的切向量方向偏置，那么，新生成结点q_new可重新表述为：

其中，T(·)表示求其垂直向量，dist(·)表示计算最近的距离；目标偏置因子与斥力偏置因子η可通过下式给出：

其中函数sigmoid(·)可将返回值约束与-1与1之间，sigmoid(·)函数表达式如下：