一种基于贪心算法的无人机RRT路径优化方法与流程

本发明属于无人机应用技术领域，尤其涉及一种基于贪心算法的无人机rrt路径优化方法。

背景技术：

目前，业内常用的现有技术是这样的：

rrt算法叫做快速拓展随机树方法，是一种基于抽样的方法，通过在可行空间生成随机点进行路径规划。快速扩展随机树方法首先空间内随机采样，然后以起始点作为拓展树的根节点，向周围节点进行搜索。通过碰撞检测选取可行节点作为路径点，并加入根节点列表。通过不断将搜索树导向空白区域，构造出存在通路的随机树路线图，直到拓展树到达终点范围之内结束扩展树，最后从终点开始反向搜索路径点上一级的根节点直到起始点，由此形成了一条可行通路作为飞行路线。rrt优点：无需对环境进行精确建模，减少了运算量和程序复杂程度；具有概率完备性，只要有通路就一定可以规划出一条可行路径；方便添加各种动力学和运动学约束；无局部最小值问题，很少出现死循环和碰撞的情况；表现出在局部区域快速规划的性质，并且通过随机采样的方法可以快速避障。rrt缺点：由于从节点向外搜索具有一定的盲目性和多样性，每一路生成的路线都不一样；同时规划的路线非常曲折，不具有最优性，大部分都不是最短路径，随机探索也造成规划时间变长；规划时需要事先得知全局地图，实际上属于离线规划，必须对全局地图探索后才能进行规划。

综上所述，现有技术存在的问题是：

(1)现有技术中，rrt算法生成路径多曲折，生成路径点具有随机性，路径不具有最优性，造成规划时间长，浪费时间，降低工作效率。

(2)现有技术中无人机在路径上不能以一定速度平滑转弯、连续飞行，使得工作不具有连续性，降低使用效果。

解决上述技术问题的难度和意义：

一架特定的无人机，必然有一定的航时。在复杂空间执行任务时，如果按照rrt方法规划出的路径飞行，则会出现走走停停的情况，无法连续飞行。这使得动力消耗的过快，降低了执行任务的有效时间和速度。

rrt算法规划出的路径比较曲折，在一定的通路中，必然存在某些航点与跨过多个航点之后的航点之间同样不存在障碍物。如果通过某些方法，将这些跨过的航点剔除，则会有效降低路径的距离和曲折性。

技术实现要素：

针对现有技术存在的问题，本发明提供了一种基于贪心算法的无人机rrt路径优化方法。rrt算法生成路径多曲折，生成路径点具有随机性，路径不具有最优性，为了使rrt生成的路径得到优化，本发明使得路径变短、平滑的平滑算法，为了使无人机在路径上以一定速度平滑转弯、连续飞行，加入了基于插值方法的曲线拟合方法，使轨迹得到优化。

本发明是这样实现的，在进行贪心算法之前，先基于rrt算法得到一条连接起点qinit和目标点qgoal的路径，原始路径上的点qi∈q,i＝1、2...m但是q中的路径过于曲折，因此需要通过贪心算法进行平滑处理。所谓平滑，指的是将不必路过的点剔除。

在点集q中，基于贪心算法路径平滑的过程为：

步骤一：以qinit作为当前点qi，初始化n＝0；

步骤二：从qi开始对q集中点qm-n进行碰撞检测，n∈[0，1，2...(m-i-1)]，即在qi与qm-n的连线上以一定距离均匀取点；

步骤三：判断步骤二中生成的均匀点是否处于障碍物上或地图之外。由于rrt算法是全局规划算法，事先已经知道可通过空间cfree和障碍物空间cobs。只需要计算qi与qm-n的连线上的点，距离cobs中的点的距离，即可判断是否存在处于障碍物上的点或者地图之外的点。若存在转向步骤四，若不存在转向步骤五；均匀取点和距离计算可按照下式进行计算，其中d是距离，x，y分别为平面内的横坐标和纵坐标，r是均匀取点的步长；

步骤四：存在障碍物，即连线经过不可飞区，n＝n+1，判断qi是否为qm-n-1，如果是则i＝i+1，转至6；否则，返回步骤二；

步骤五：不存在障碍物，连接qi和qm-n，使qi＝qm-n转至步骤六；

步骤六：判断i是否等于m；如果是则结束程序，否则转至步骤二。

本发明提供的另一目的在于针对上述过程，提供的贪心算法伪代码，具体为：

(1)初始化i＝1，n＝0，m为路径点数量

(2)fori≠m

(3)对qi和qm-n进行碰撞检测；

(4)ifcollision(qi，qm-n)=0

(5)连接qi和qm-n，将qm-n加入新的路径列表；

(6)i＝m-n，n＝0，continue；

(7)elsen＝n+1；

(8)ifi≠m-n-1

(9)continue；

(10)elsei++；

(11)end

(12)end

(13)end。

综上所述，本发明的优点及积极效果为：如附图2-图6，其中短折线是rrt算法得到的路径，显然是非常曲折的。长折线是经过贪心算法剔除航点的结果。显然长折线经过的点要少很多，这使的通过该区域的路径更加平滑。如表1所示，经过平滑后的路径比之前减少了一部分长度，是的路径距离缩短，有助于节省通过时间和运动能量。

本发明针对无人机rrt算法路径规划得到的路径过于曲折的情况，设计了利用贪心算法剔除不必要航点的方法。通过剔除冗余航点，优化折线曲率，从而达到减短路径的目的，使规划结果达到当下最优，路径点减少，路径得到了大幅的优化，在无人机或其他机器人路径规划的应用上具有一定工程价值。

附图说明

图1是本发明实施例提供的基于贪心算法的无人机rrt路径优化方法过程流程图。

图2是本发明实施例提供的贪心算法路径平滑原理图。

图3是本发明实施例提供的障碍物地图路径平滑示意图。

图4是本发明实施例提供的迷宫地图路径平滑示意图。

图5是本发明实施例提供的对平滑后路径拟合示意图。

图6是本发明实施例提供的对原始路径拟合示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

为了使生成路径更短且更少的转折，应该加入平滑算法，这里采用的平滑算法是贪心算法。贪心算法是指在求解问题时，总是做出当前情况最好的选择，不在全局最优加以考虑，而是在某种意义上达到局部最优解。在路径平滑中，贪心算法的策略是在已得路径上，尽可能得到一条路程最优的可行路径。

本发明实施例提供的贪心算法路径平滑过程：首先通过rrt算法得到一条连接起点qinit和目标点qgoal的路径，原始路径上的点qi∈q，i＝1、2...m，在点集q中：

如图1所示，本发明实施例提供的基于贪心算法的无人机rrt路径优化方法包括以下步骤：

s101：以qinit作为当前点qi，初始化n＝0；

s102：从qi开始对q集中点qm-n进行碰撞检测，n∈[0，1，2...(m-i-1)]，即在qi与qm-n的连线上以一定距离均匀取点；

s103：判断是否存在点处于障碍物上或地图之外，若存在转向s104，若不存在转向s105；

s104：存在障碍物，即连线经过不可飞区，n＝n+1，判断qi是否为qm-n-1，如果是则i＝i+1，转至6；否则，返回s102；

s105：不存在障碍物，连接qi和qm-n，使qi＝qm-n转至s106；

s106：判断i是否等于m；如果是则结束程序，否则转至s102。

本发明实施例提供的贪心算法伪代码，具体为：

(1)初始化i＝1，n＝0，m为路径点数量

(2)fori≠m

(3)对qi和qm-n进行碰撞检测；

(4)ifcollision(qi，qm-n)=0

(5)连接qi和qm-n，将qm-n加入新的路径列表；

(6)i＝m-n，n＝0，continue；

(7)elsen＝n+1；

(8)ifi≠m-n-1

(9)continue；

(10)elsei++；

(11)end

(12)end

(13)end。

下面结合具体的仿真试验对本发明的应用原理作进一步详细说明；

由本发明得到改进后的rrt算法，包括标准rrt算法、平滑算法、曲线拟合算法结合的改进rrt算法。

如图3-图4所示，仿真验证试验环境为圆形障碍物地图和迷宫型地图。参数设置：k＝10000，p＝0.3，步长q＝30。

工况一：圆形障碍物地图，起点[50，50]终点[350，425]

工况二：迷宫型地图，起点[60，60]终点[400，400]

如表1所示，对改进前后算法性能对比及分析；

表1：改进前后算法性能对比

结果：经过平滑算法进行路径优化后，生成路径长度明显变短，平滑后的路径点明显减少；在圆形障碍物地图中，平滑算法减少路程的比例大致为19.08％，在迷宫型地图中减少的路程比例大致为16.70％；在本次研究的地图中路径点基本减少到个位数。证明了平滑算法减少路程长度和曲折的有效作用。

如图5-图6所示，复杂环境迷宫地图中，对平滑后的路线(黑色线段)进行b样条拟合后的路径(绿色曲线)容易发生与障碍物的碰撞；如果对原来的路线进行拟合，则很少发生碰撞，且路线也得到了平滑。

所以在简单环境的避障中，可以进行平滑处理后，对平滑处理后的路径进行曲线拟合，所得路线趋近于最优。

在复杂环境，可有两种策略，一是只进行平滑处理，得到优化后的直线线段，无人机控制策略为走到一个路径点停止后，变向继续前往下一个路径点，路径较短趋近于最优；第二种策略是只进行曲线拟合，无人机可以以均速平滑通过路径，路线略曲折，但保证了无人机飞行安全性。

结果：本发明对于无人机rrt算法路径规划，具有减短其路径、利用贪心思想使结果达到当下最优，路径点减少，路径得到了大幅的优化，在无人机或其他机器人路径规划的应用上具有一定工程价值。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。