一种基于改进RRT的无人机实时路径规划方法与流程

本发明属于无人机操作技术领域，尤其涉及一种基于改进rrt的无人机实时路径规划方法。

背景技术：

目前，业内常用的现有技术是这样的：快速扩展随机树(rrt)路径规划方法具有适应性强、生成速度快、概率完备性等诸多优点，但必须依靠于已知的全局地图，才能进行规划。所以，一般来说rrt是一种全局规划方法，无法实现无人机的实时路线生成。但是随着无人机应用的广泛化，在室内任务环境中只考虑全局静态的障碍物是不够的，室内环境中存在不确定性和动态性的障碍物，也应该研究相应的避障策略。局部规划是对局部信息采集并在一定区域内规划出可行路径，当环境是动态不确定的，局部规划可以实时跟新扫描数据，从而重新规划路线以对环境变化做出反应。这样的在线规划系统很好的满足了规划实时性，目的是真正实现无人机的自主导航和飞行。常见的局部规划方法有滚动窗口规划、动态规划、人工势场规划方法等。

滚动窗口规划又称作动态窗口法(dynamicwindowapproach，dwa)，它的基本原理是根据局部环境信息制造规划窗口，对行动进行预测分析，反馈调节行动规划。滚动窗口法对于在线路径生成是一种可行的方法，每一次窗口规划只生成一个局部环境中的路径，到达下一航迹点，再根据探测的环境信息进行下一次规划，不需要一次性完成对于全局的环境探测。

人工势场法是一种基于类比物理中势能的思想，将路径规划问题类比为在空间内寻找势能最优解的问题。它将目标点视作空间内的势能最小点(可近似认为为零势能点)，空间内障碍物的势能为无穷大，模拟无人机受到目标点的吸引力和障碍物的排斥力，从而在空间内上得到一个人工定义的势能场，无人机会趋向于向低势能方向上移动，形成一条势能逐渐降低的轨迹，如果势能场在空间内是连续且只具有一个极小值时，则可以到达势能最低点，即目标点。但当空间的势能场有多个极小值，则非目标点的局部最小值点会导致无人机到达局部最小后无法寻找势能更低点，从而永远无法到达目标点，导致路径规划失败。

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

(1)现有技术的算法只适用于全局规划，造成受限制，操作不便，降低使用效率。

(2)现有技术中需要对整个空间进行随机探索，随机探索的范围大，计算量增多，无法有效实现在线规划。

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

现有的成熟技术都是在全局地图已知的情况下进行的路径规划。实际上在很多情况下，无人机是在未知环境执行任务。因此也就无法预先获得全局的地图，进而无法进行全局的路径规划。这就使得全局规划方法在应用上有局限性。

由此提出了基于滚动窗口的改进rrt算法。由于只规划无人机附近的路径，因而全局地图不必已知。这使得无人机在未知环境的生存能力大大提升。

技术实现要素：

针对现有技术存在的问题，本发明提供了一种基于改进rrt的无人机实时路径规划方法。

本发明是这样实现的，一种基于改进rrt的无人机实时路径规划方法，包括以下步骤：

步骤一：构建局部滚动窗口，通过无人机搭载的传感器获得局部环境信息，构建局部滚动窗口；

步骤二：局部子目标点的设置，子目标点位于滚动窗口的边界上，即在边界上选择可行并符合某种全局映射条件的点，作为局部子目标点；

步骤三：局部rrt树算法随机采样规划，在窗口内的规划不已到达子目标为结束，以可行路径的规划是否到达窗口边界为结束条件，当rrt延伸到窗口边界，则停止向子目标点规划，以新节点作为中心建立新的滚动规划；

步骤四：算法终止原则。

进一步，步骤一中，构建局部滚动窗口，具体为：

通过无人机搭载的传感器获得局部环境信息，规划的窗口以周期的形式驱动，每一次规划的窗口区域为ein(qr(tt)＝{p|p∈c，d(q，qr(t))≤r}，qr(t)表示滚动窗口的中心，r是滚动窗口的半径，一般为传感器的探测半径。

进一步，步骤二中，局部子目标点的设置，具体包括：

将位于滚动窗口边界上且位于当前点与目标点qgoal连线上的点作为子目标点qtempgoal；

将(xc，yc)作为当前滚动窗口的中心，半径为r的虚线圆即是滚动窗口范围，(xg，yg)为全局目标点，(xt0，yto)是窗口中心与目标点连线和虚线圆的交点，定义为局部子目标点qtempgoal。

因为rrt树枝拓展中，树枝的生长方向取决于qrand的随机选择，在未到达目标点处时，只要生成的qrand在可行区域内且靠近子目标点qtempgoal，树枝就会向可行且偏向目标点方向生长。如果子目标点在障碍物上，标准的rrt规划则无法到达，此时则在圆上随机生成可行的子目标点，目的是在区域内快速生成一条达到窗口边界的可行路径。

theta＝rand()·2π(2)

此时，在窗口内rrt的随机节点选择应该考虑与子目标点的关系和如何停止搜索。

进一步，步骤三中，局部rrt树算法随机采样规划，具体包括：

定义圆上随机一点与水平线所成角度为θ＝β+α，β是qrand偏离子目标点的角度，将θ的选择构造成一个关于α对称分布的正态函数；即

β＝randn·π

因此构造出在(-180°，180°)内随机分布的角度β，以角度β偏离子目标点选择qrand；

在窗口内的规划不已到达子目标为结束，因为子目标点有可能处于威胁区，而是以可行路径的规划是否到达窗口边界为结束条件，当rrt延伸到窗口边界，则停止向子目标点规划，以新节点作为中心建立新的滚动规划。

进一步，步骤四中，算法终止原则，具体为

在滚动窗口一次次向全局目标点进行滚动时，当全局目标点qgoal为于了滚动窗口的范围内，即d(qgoal，qr(t))≤r，则直接以qgoal作为目标点进行规划，到达qgoal，算法结束。

本发明的另一目的在于提供的滚动rrt算法的大致流程如下：

(1)初始化滚动窗口内rrt树的变量；

(2)判断qgoal是否位于滚动窗口内，如果是则转至步骤3，不是转至步骤4；

(3)将qgoal作为目标点，在边界上随机生成节点qrand；

(4)在滚动窗口边界上得到局部子目标点qtempgoal，在边界上生成随机节点qrand；

(5)在当前树种选择最近树节点qnear，并按照一定的步长生成节点qnew；

(6)在qnear、qnew连线上进行碰撞检测；

(7)判断qnew是否处于可飞区且qnear、qnew连线上无障碍物分布，如不满足则返回步骤2，如满足则转至步骤6；

(8)将qnew加入树列表，判断是到达qgoal，如果不是则转至步骤9；是则结束算法；

(9)判断是否到达滚动窗口的边界，如果不是则返回步骤2，如果是则转至步骤10；

(10)从qnew反向搜索得到局部路径，并以qnew为中心建立新的滚动窗口，返回步骤1；

综上所述，本发明的优点及积极效果为：

本发明可以对原本只适用于全局规划的算法进行改进和融合，利用根据局部已知的环境信息，构建窗口，通过某种方法确定子目标点，然后在此环境中利用全局规划算法进行探索。在窗口滚动前进的过程中，窗口内的环境信息不断更新，实现规划建图与反馈，最终到达目标点。对于rrt算法而言，相比全局规划，这种基于滚动窗口的改进方法，不必对整个空间进行随机探索，而是将规划限制在无数个持续更新的窗口中，随机探索的范围减小，计算量减小，并且可以实现在线规划。

附图说明

图1是本发明实施例提供的基于改进rrt的无人机实时路径规划方法流程图。

图2是本发明实施例提供的滚动rrt子目标点选择图。

图3是本发明实施例提供的滚动rrt在圆形障碍物中的仿真结果；

图中：a为q＝50；b为q＝30；c为q＝20；d为q＝10。

具体实施方式

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

下面结合附图对本发明的应用原理做详细描述。

如图1所示，本发明实施例提供的基于改进rrt的无人机实时路径规划方法，包括以下步骤：

s101：构建局部滚动窗口，通过无人机搭载的传感器获得局部环境信息，构建局部滚动窗口；

s102：局部子目标点的设置，子目标点位于滚动窗口的边界上，即在边界上选择可行并符合某种全局映射条件的点，作为局部子目标点；

s103：局部rrt树算法随机采样规划，在窗口内的规划不已到达子目标为结束，以可行路径的规划是否到达窗口边界为结束条件，当rrt延伸到窗口边界，则停止向子目标点规划，以新节点作为中心建立新的滚动规划；

s104：算法终止原则。

步骤s101中，本发明实施例提供的构建局部滚动窗口，具体为：

通过无人机搭载的传感器获得局部环境信息，规划的窗口以周期的形式驱动，每一次规划的窗口区域为win(qr(t))＝{p|p∈c，d(q，qr(t))≤r}，qr(t)表示滚动窗口的中心，r是滚动窗口的半径，一般为传感器的探测半径。

步骤s102中，本发明实施例提供的局部子目标点的设置，具体包括：

将位于滚动窗口边界上且位于当前点与目标点qgoal连线上的点作为子目标点qtempgoal；

将(xc，yc)作为当前滚动窗口的中心，半径为r的虚线圆即是滚动窗口范围，(xg，yg)为全局目标点，(xt0，yto)是窗口中心与目标点连线和虚线圆的交点，定义为局部子目标点qtrmpgoal。

因为rrt树枝拓展中，树枝的生长方向取决于qrand的随机选择，在未到达目标点处时，只要生成的qrand在可行区域内且靠近子目标点qtempgoal，树枝就会向可行且偏向目标点方向生长。如果子目标点在障碍物上，标准的rrt规划则无法到达，此时则在圆上随机生成可行的子目标点，目的是在区域内快速生成一条达到窗口边界的可行路径。

theta＝rand()·2π(2)

此时，在窗口内rrt的随机节点选择应该考虑与子目标点的关系和如何停止搜索。

步骤s103中，本发明实施例提供的局部rrt树算法随机采样规划，具体包括：

定义圆上随机一点与水平线所成角度为θ＝β+α，β是qrand偏离子目标点的角度，将θ的选择构造成一个关于α对称分布的正态函数；即

β＝randn·π

因此构造出在(-180°，180°)内随机分布的角度β，以角度β偏离子目标点选择qrand；

在窗口内的规划不已到达子目标为结束，因为子目标点有可能处于威胁区，而是以可行路径的规划是否到达窗口边界为结束条件，当rrt延伸到窗口边界，则停止向子目标点规划，以新节点作为中心建立新的滚动规划。

步骤s105中，本发明实施例提供的算法终止原则，具体为

在滚动窗口一次次向全局目标点进行滚动时，当全局目标点qgoal为于了滚动窗口的范围内，即d(qgoal，qr(t))≤r，则直接以qgoal作为目标点进行规划，到达qgoal，算法结束。

本发明实施例提供的滚动rrt算法的大致流程如下：

(1)初始化滚动窗口内rrt树的变量；

(2)判断qgoal是否位于滚动窗口内，如果是则转至步骤3，不是转至步骤4；

(3)将qgoal作为目标点，在边界上随机生成节点qrand；

(4)在滚动窗口边界上得到局部子目标点qtempgoal，在边界上生成随机节点qrand；

(5)在当前树种选择最近树节点qnear，并按照一定的步长生成节点qnew；

(6)在qnear、qnew连线上进行碰撞检测；

(7)判断qnew是否处于可飞区且qnear、qnew连线上无障碍物分布，如不满足则返回步骤2，如满足则转至步骤6；

(8)将qnew加入树列表，判断是到达qgoal，如果不是则转至步骤9；是则结束算法；

(9)判断是否到达滚动窗口的边界，如果不是则返回步骤2，如果是则转至步骤10；

(10)从qnew反向搜索得到局部路径，并以qnew为中心建立新的滚动窗口，返回步骤1；

本发明实施例提供的工作原理部分：

以滚动窗口规划算法为基本框架，将rrt算法作为每次窗口内路径规划策略，并通过路径的修剪和平滑，形成局部区域的rrt扩展树，连接当前点和局部子目标点，实现运动控制和反馈调节，到达新的路径点后更新窗口信息，再次用rrt方法进行路径规划。

下面结合具体实施例对本发明的应用原理作进一步详细说明；

实施例1；

1、构建局部滚动窗口

局部环境信息是通过无人机搭载的传感器获得的，规划的窗口以周期的形式驱动，定义每一次规划的窗口区域为win(q_r(t))＝{p|p∈c，d(q，q_r(t))≤r}，q_r(t)表示滚动窗口的中心，r是滚动窗口的半径，一般为传感器的探测半径。上述窗口也称为无人机的视野域。

由于无人机在实时路线规划和自主导航的过程中，最好能够保持一定的速度连续飞行，而不需要停止等待下一次窗口规划产生了路径后再行进。所以为了连续飞行，必须在未到达局部子目标点之前，就提前进行下一窗口规划，把当前规划的目标点当做下一次规划窗口的中心。即得到下一次滚动规划路线后，无人机则完成这次规划剩余点和下一条路径的步进和路径平滑处理。这里需要提前获知下一窗口的部分环境信息，所以定义r略小于传感器的探测半径，是为了在探测器获得的局部环境下可以满足构建大于一个的滚动窗口。

2、局部子目标点的选择

在滚动rrt中局部子目标点的选择，应该符合其是全局目标点的一个映射，并且可以避开障碍物区域，是一个可行的路径点。一般来说，为了可以充分地利用滚动窗口内的局部环境信息，定义子目标点位于滚动窗口的边界上，即在边界上选择可行并符合某种全局映射条件的点，作为局部子目标点。这里

将位于滚动窗口边界上且位于当前点与目标点qgoal连线上的点作为子目标点qtempgoal。

如图2所示，将(xc，yc)作为当前滚动窗口的中心，半径为r的虚线圆即是滚动窗口范围，(xg，yg)为全局目标点，(xt0，yto)是窗口中心与目标点连线和虚线圆的交点，定义为局部子目标点qtempgoal。

因为rrt树枝拓展中，树枝的生长方向取决于qrand的随机选择，在未到达目标点处时，只要生成的qrand在可行区域内且靠近子目标点qtempgoal，树枝就会向可行且偏向目标点方向生长。如果子目标点在障碍物上，标准的rrt规划则无法到达，此时则在圆上随机生成可行的子目标点，目的是在区域内快速生成一条达到窗口边界的可行路径。

theta＝rand()·2π(2)

此时，在窗口内rrt的随机节点选择应该考虑与子目标点的关系和如何停止搜索。

3、局部rrt算法随机采样策略

从rrt每一次树的拓展过程来看，随机节点qrand的选择是在整个空间内具有随机性，这里qrand的选择限制在滚动窗口的边界上，进行发展，qrand将以概率p选择为图2中圆周上的子目标点(xt0，yto)，(1-p)的概率选择为圆周上的任一点，如图中的随机点(xt，yt)。所以qrand会趋向于圆周上距离子目标点近的点选取，且越靠近子目标点概率越大，如果qrand在障碍物上则重新选取。

如图2所示，定义圆上随机一点与水平线所成角度为θ＝β+α，β是qrand偏离子目标点的角度，将θ的选择构造成一个关于α对称分布的正态函数。即

β＝randn·π

因此构造出在(-180°，180°)内随机分布的角度β，以角度β偏离子目标点选择qrand。

在窗口内的规划不已到达子目标为结束，因为子目标点有可能处于威胁区，而是以可行路径的规划是否到达窗口边界为结束条件，当rrt延伸到窗口边界，则停止向子目标点规划，以新节点作为中心建立新的滚动规划。

4、算法终止原则

在滚动窗口一次次向全局目标点进行滚动时，当全局目标点qgoal为于了滚动窗口的范围内，即d(qgoal，qr(t))≤r，则直接以qgoal作为目标点进行规划，到达qgoal，算法结束。

实施例2；

本发明实施例提供的滚动rrt算法的大致流程如下：

(1)初始化滚动窗口内rrt树的变量；

(2)判断是否位于滚动窗口内，如果是则转至步骤3，不是转至步骤4；

(3)将作为目标点，在边界上随机生成节点

(4)在滚动窗口边界上得到局部子目标点，在边界上生成随机节点；

(5)在当前树种选择最近树节点，并按照一定的步长生成节点；

(6)在、连线上进行碰撞检测；

(7)判断是否处于可飞区且、连线上无障碍物分布，如不满足则返回步骤2，如满足则转至步骤6；

(8)将加入树列表，判断是到达，如果不是则转至步骤9；是则结束算法；

(9)判断是否到达滚动窗口的边界，如果不是则返回步骤2，如果是则转至步骤10；

(10)从反向搜索得到局部路径，并以为中心建立新的滚动窗口，返回步骤1；

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

仿真试验1；

设置500x500的仿真环境，有4个分布的圆形障碍物，滚动窗口半径为100，探索步长为50，起点坐标为(50，50)，终点坐标为(400，400)，分别对偏小的步长q＝50，30，20，10的情况进行仿真，其他参数k＝10000，p＝0.3，滚动窗口半径r＝100，主要考察对于障碍物避障情况。并比较其与标准rrt在相同步长和概率的情况下的执行情况。计算机硬件设备为intel(r)单核处理器2.1ghz，内存为4gb。在matlab环境下编制仿真程序。

如图3所示，本发明提供的滚动rrt在圆形障碍物中的仿真结果；

其中，图3-a中q＝50；图3-b中q＝30；图3-c中q＝20；图3-d中q＝10；

如图3所示，仿真验证中表明此算法在实时性上平均花费0.02s就可完成一次窗口规划化，第一次窗口规划完成时间在0.01-0.08s，在不考虑无人机移动花费时间的情况下，规划时间短满足无人机实时在线路径规划的需求。

随机节点的选择策略为趋向于子目标点生长，子目标点是全局目标的映射，因此局部树的生成在某种程度上是趋向于目标生长的。当子目标点一直为全局目标点的直接映射时，规划路线可以非常平直，规划效率快小步长在局部规划中虽然节点数多，但表现出了用于全局规划时不同的特点。小步长生成路线短于大步长，生成窗口数量少，路径平滑，且更有利于实现无人机的运动学约束，小步长在局部规划中充分发挥了优势

这种算法对于将rrt算法应用到无人机在线实时规划中具有一定的工程应用价值。

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