[发明专利]一种融合可视图法和稳定稀疏随机快速树机器人规划算法

说明书

本发明涉及一种融合可视图法和稳定稀疏随机快速树机器人规划算法。包括以下步骤：S1.基于可视图法构建拓扑图对环境进行建模；S2.利用dijkstra算法得到最短路径，并作为参考路径；S3.对参考路径进行分割，结合SST算法利用平均采样策略在参考路径一定范围内随机采样；S4.利用Bias‑goal提高算法效率；S5.在当前拓展范围内，根据Dubins距离选择离当前采样点区域范围内的最近树节点；S6.采用横向控制策略选取控制量对系统模型进行积分，耗散最优的节点优先拓展；S7.若拓展过程无碰撞，考察生成的新节点是否在局部邻域内是最优节点；若最优则加入树结构，并修剪当前区域的主导节点。本发明利用稳定稀疏随机快速树算法对可视图法生成的路径进行优化，以得到符合非完整性约束机器人约束的最优路径。

技术领域

本发明属于人工智能自动控制领域，更具体地，涉及一种融合可视图法和稳定稀疏随机快速树机器人规划算法。

背景技术

可视图法(Visibility Graph)是由Lozano和Wesley提出的。可视图法将所有实际的障碍物等效成投影在平面内的多边形集合。并将起始点和目标点在空间中对应的点扩充到多边形集合中，然后将所有障碍物的顶点(设V0是所有障碍物的顶点构成的集合)、起始点s和目标点g用直线组合相连，同时要求3者之间的连线不能穿越障碍物，即直线是“可视的“，给图中的边赋权值，构造图G(V,E)，节点集V＝V0∪(s,g)，E为所有弧段(Pi，Pj)即边的集合，其中连接G中第i和第j个结点的线段与任何障碍物均不相交。因为图中相邻的顶点能相互看到，所以G(V,E)称为可视图。然后采用某种算法(如Dijkstra算法)搜索从起始点s到目标点g的最优路径。规划最优路径的问题就转化为从起始点到目标点经过这些可视直线的最短距离问题。

可视图法的优点是概念直观，简单，可得到起始点到目标点的最近路径；缺点是灵活性不好，目标点或障碍物或起始点发生改变的时候，重新构造可视图较麻烦，但是在任何时候都可以获得最优路径。

随机快速树算法(Rapidly-exploring Random Tree,RRT)是一种通过随机构建空间填充树来有效搜索非凸，高维空间的算法。树是从搜索空间中随机抽取的样本逐步构建的，并且本质上倾向于朝向大部分未探测区域生长。该算法的优势主要体现在模型兼容性好、不受网格分辨率影响和兼容动态环境等方面，然而，RRT算法的应用受限于路径质量问题，无法随着采样数量的增加而优化。

稳定稀疏随机快速树算法(Stable Sparse RRT，SST)是一种渐优的随机采样算法，是对RRT算法的选择母节点、拓展过程进行改进和提高，并增加了树修剪过程得到的。SST选择母节点过程是一个优化选择过程，其遵循优化优先的策略。在当前拓展范围内，在随机点δ_BN的区域范围内，耗散最优的节点优先拓展。在拓展环节，SST基于蒙特卡洛方法随机在控制域内选择控制量，然后对系统模型进行积分。如果拓展过程无碰撞，考察x_new是否在局部邻域δ_s内是最优节点。如果最优，那么将其加入树结构，并修剪在δ_s域内占据主导地位的已存在节点。修剪是SST算法优化的核心过程，其利用s域将C空间划分为有限个小空间，并在每一次迭代中检查s域内的节点是否是局部优化的。通过这一过程，SST维持在现有迭代规模下，s域内节点的优化属性。这种优化可否决次优节点的加入，同时鼓励优化节点进行更多拓展从而达到全局优化的目的。

对于非结构化道路环境下非完整约束的机器人的规划问题，目前没有完整成熟的技术，现有的技术存在以下几点问题：1.无法确保生成的路径尽可能最短；2.生成的路径不满足机器人的非完整性约束；3.算法收敛速度慢。