一种基于贪心算法的无人机RRT路径优化方法技术

本发明专利技术属于无人机应用技术领域，公开了一种基于贪心算法的无人机RRT路径优化方法，以qinit为当前点qi，初始化n＝0；从qi开始对Q集中点qm‑n进行碰撞检测，n∈[0，1，2...(m‑i‑1)]，即在qi与qm‑n的连线上以一定距离均匀取点；判断是否存在点处于障碍物上或地图之外，若存在转向步骤四，否则转向步骤五；存在障碍物，即连线经过不可飞区，n＝n+1，判断qi是否为qm‑n‑1，如果是则i＝i+1；否则返回；不存在障碍物，连接qi和qm‑n，使qi＝qm‑n转至步骤六；判断i是否等于m；是则结束程序。本发明专利技术对于无人机RRT算法路径规划，具有减短其路径、利用贪心思想使结果达到当下最优，路径点减少，路径得到大幅的优化，在无人机或其他机器人路径规划的应用上具有一定工程价值。

RRT Path Optimization Method for UAV Based on Greedy Algorithms

The invention belongs to the field of UAV application technology, and discloses a greedy algorithm-based RRT path optimization method for UAV, which takes qinit as the current point Qi and initializes n=0; starts from qi, collision detection of Q concentration point qm_n, n <[0,1,2], i.e., taking a point evenly at a certain distance along the line between Qi and qm_n; to determine whether there are points on obstacles or on maps; In addition, if there is a steering step 4, or a steering step 5; if there are obstacles, i.e., if the line passes through the non-flying zone, n = n + 1, judge whether Qi is QM n 1, if I = I + 1; otherwise return; if there are no obstacles, connect Qi and QM n, so that Qi = QM n is transferred to step 6; judge whether I is equal to m; and end the procedure. The invention has the advantages of shortening the RRT path of UAV, making use of greedy idea to achieve the current optimal result, reducing the path points, and greatly optimizing the path. It has certain engineering value in the application of UAV or other robot path planning.

【技术实现步骤摘要】
一种基于贪心算法的无人机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(本文档来自技高网...

【技术保护点】
1.一种基于贪心算法的无人机RRT路径优化方法，其特征在于，所述的基于贪心算法的无人机RRT路径优化方法的贪心算法路径平滑过程：通过RRT算法得到一条连接起点qinit和目标点qgoal的路径，原始路径上的点qi∈Q，i＝1、2...m，在点集Q中。

【技术特征摘要】
1.一种基于贪心算法的无人机RRT路径优化方法，其特征在于，所述的基于贪心算法的无人机RRT路径优化方法的贪心算法路径平滑过程：通过RRT算法得到一条连接起点qinit和目标点qgoal的路径，原始路径上的点qi∈Q，i＝1、2...m，在点集Q中。2.如权利要求1所述的基于贪心算法的无人机RRT路径优化方法，其特征在于，所述基于贪心算法的无人机RRT路径优化方法具体包括：步骤一：以qinit作为当前点qi，初始化n＝0；步骤二：从qi开始对Q集中点qm-n进行碰撞检测，n∈[0，1，2...(m-i-1)]，即在qi与qm-n的连线上以一定距离均匀取点；步骤三：判断是否存在点处于障碍物上或地图之外，若存在转向步骤四，若不存在转向步骤五；步骤四：存在障碍物，即连线经过不可飞区，n＝n+1，判断qi是否为qm-n-1，如果是则i＝...

【专利技术属性】
技术研发人员：李璟璐，丁久辉，
申请(专利权)人：智灵飞北京科技有限公司，
类型：发明
国别省市：北京,11