CN111897362B - 一种复杂环境下的翼伞组合式航迹规划方法 - Google Patents

Abstract

本发明公开了一种复杂环境下的翼伞组合式航迹规划方法，将翼伞空投区域分为有障碍物存在的障碍区和以目标点为圆心且空旷平坦的圆形着陆区；在障碍区内采用快速搜索随机树路径规划算法快速搜索可行轨迹，搜索的终止条件是路径延伸到的位置与目标着陆点之间的水平距离小于或等于圆形着陆区域的半径，在障碍区内仅考虑避障任务；将障碍区内的航迹搜索的终止状态作为圆形着陆区内航迹规划的初始状态，在圆形着陆区内采用分段归航的规划设计完成翼伞高度控制和逆风对准任务。本发明将翼伞在复杂环境下航迹规划需要满足的避障和其他要求分为两个阶段处理，同时又能满足两个航迹规划阶段的顺利衔接，实现了复杂环境下的翼伞航迹规划。

Description

一种复杂环境下的翼伞组合式航迹规划方法

技术领域

本发明属于路径规划领域，特别涉及了一种翼伞航迹规划方法。

背景技术

翼伞定点空投是一种应用愈加广泛的物资投送方式，航迹规划是实现翼伞定点空投的前提。传统的翼伞航迹规划方法主要有分段航迹规划和最优控制航迹规划方式。然而这两种航迹规划方式均是在空投区域足够空旷平坦的条件下进行的，若空投环境较为复杂，例如存在高山或者高大建筑物阻挡，则传统分段航迹规划和最优控制航迹规划方法均不再适用。

发明内容

为了解决上述背景技术提到的技术问题，本发明提出了一种复杂环境下的翼伞组合式航迹规划方法，解决空投区域内有障碍物，传统翼伞航迹规划方法不再适用的问题。

为了实现上述技术目的，本发明的技术方案为：

一种复杂环境下的翼伞组合式航迹规划方法，将翼伞空投区域分为有障碍物存在的障碍区和以目标着陆点为圆心且空旷平坦的圆形着陆区；在障碍区内采用快速搜索随机树路径规划算法快速搜索可行轨迹，搜索的终止条件是路径延伸到的位置与目标着陆点之间的水平距离小于或等于圆形着陆区域的半径，在障碍区内仅考虑避障任务；将障碍区内的航迹搜索的终止状态作为圆形着陆区内航迹规划的初始状态，在圆形着陆区内采用分段归航的方式设计航迹，在圆形着陆区内完成翼伞高度控制和逆风对准任务。

进一步地，在障碍区内采用快速搜索随机树路径规划算法快速搜索可行轨迹的方法如下：

记样点P_i的坐标为(x_i,y_i)，邻节点P_j的坐标为(x_j,y_j,z_j)，则新节点P_j+1的位置(x_j+1,y_j+1,z_j+1)按下式进行更新：

上式中，Step为搜索步长，k为翼伞滑翔比，θ_j+1为新节点的搜索方向，其计算式如下：

新节点是否可行的判断依据如下：

f(x,y)-z＜0,(x,y,z)∈L

上式中，f(x,y)为空投区域地形的函数表达式，(x,y,z)为翼伞在大地坐标系下的位置，L为线段P_jP_j+1上所有点构成的集合。

进一步地，增设安全距离d，将新节点是否可行的判断依据修改为：

f(x,y)-z＜d,(x,y,z)∈L。

进一步地，对于所述样点的产生，1/2的概率来自随机产生，其余1/2的概率直接将目标着陆点作为样点。

进一步地，设相邻两个节点的搜索方向之差为β，其与搜索步长Step和翼伞转弯半径R之间满足下式：

则：

上式中，R_min为最小翼伞转弯半径。

进一步地，令Step＝R_min，则：

进一步地，圆形着陆区中的轨迹分为5段，依次为向心归航段、过渡段1、高度控制段、过渡段2以及逆风着陆段，其中向心归航段和逆风着陆段为直线段，过渡段1、高度控制段和过渡段2为弧线段；

其中：

向心归航段的长度如下：

上式中，AB为向心归航段矢量，A为向心归航段的起点，B为向心归航段的终点，x_a为A点在大地坐标系X轴上的坐标，

为过渡段1的圆心C₁在大地坐标系X轴上的坐标，R_min为最小翼伞转弯半径，令过渡段的半径为R_min；

过渡段1的圆心角如下：

上式中，α₁为过渡段1的圆心角，θ_ep为高度控制段的角度，

为矢量

与大地坐标系X轴的正向夹角，s为符号变量，s＝1表示航迹逆时针旋转，s＝-1表示航迹顺时针旋转；

高度控制段的圆心角如下：

α₂＝2π-∠C₂OE-s·θ_ep

上式中，α₂为高度控制段的圆心角，C₂为过渡段2的圆心坐标，E为逆风着陆段的起点，O为目标着陆点；

过渡段2的圆心角如下：

上式中，α₃为过渡段2的圆心角；

逆风着陆段的长度如下：

上式中，R_ep为高度控制段的转弯半径。

进一步地，分段归航的目标函数如下：

上式中，J为目标函数，k为翼伞滑翔比，z_a为A点翼伞飞行高度，z_f为目标着陆点的海拔高度。

进一步地，令R_ep≤R_th，则分段归航的目标函数如下：

上式中，R_th为圆形着陆区半径，R_th不小于最小翼伞转弯半径，τ表示高度控制段完整盘旋的圈数。

进一步地，z_a＞k·R_th。

采用上述技术方案带来的有益效果：

本发明将RRT路径规划和分段航迹规划方式相结合，利用RRT随机搜索可行路径实现翼伞避障。同时又针对RRT搜索方向随机及实现避障的同时难以实现翼伞高度控制的问题，通过分段航迹规划的方式解决了这些问题。两种方法互相配合，将避障问题和翼伞高度控制、逆风对准着陆控制问题分为两步解决，同时还满足了落点精度要求，实现了复杂环境下的翼伞航迹规划问题。

附图说明

图1是本发明中翼伞组合式避障航迹规划方法原理示意图；

图2是本发明中随机搜索方向与搜索步长及翼伞转弯半径的几何关系示意图；

图3是本发明中着陆区内分段航迹规划原理示意图。

具体实施方式

以下将结合附图，对本发明的技术方案进行详细说明。

本发明设计了一种复杂环境下的翼伞组合式航迹规划方法。根据实际空投环境的特点，即归航过程中翼伞会受地形阻碍，而着陆点一般会选在地形较为空旷平坦的区域。该情况下，翼伞航迹规划需要实现避障、高度控制及逆风对准着陆同时还要满足一定的落点精度。为了实现上述目的，将翼伞空投区域分为有障碍物存在的障碍区及以目标着陆点为圆心且空旷平坦的圆形着陆区。如图1所示，在障碍区内采用快速搜索随机树(RRT)路径规划算法快速搜索可行轨迹，搜索的终止条件是路径延伸到的位置与目标着陆点之间的水平距离小于或等于圆形着陆区域半径，在障碍区内仅考虑避障任务。接着，将障碍区内的航迹搜索的终止状态作为着陆区内航迹规划的初始状态，在着陆区内采用分段归航的方式设计轨迹，将着陆区内的航迹分为直线向心归航段、过渡段1、高度控制段、过渡段2以及逆风着陆段总共5段轨迹。在着陆区内完成翼伞高度控制及逆风对准任务。

下面说明该翼伞组合式避障航迹规划方法的具体实现。

在障碍区路径规划借助于RRT路径规划算法。RRT算法是一种基于采样的路径规划算法，常用于移动机器人路径规划，其基本思想是以产生随机点的方式通过一个步长向目标着陆点搜索前进，有效躲避障碍物，避免陷入局部最优，收敛速度快。具体方法如下：当RRT算法在状态空间内随机采样到一个节点时，首先在当前树中寻找与该采样点距离最近的邻节点，然后连接采样节点和邻节点，生成新的节点。记随机采样点为P_rand，邻节点为P_nearest，新节点为P_new，搜索步长为Step。则新节点的表达式为：

上式的含义是当采样节点与邻节点距离大于或者等于搜索步长Step时，在采样节点与邻节点的连线上，距离邻节点一个搜索步长处生成新节点。而当采样节点与邻节点距离小于搜索步长时，直接将临界点作为新节点。其中，sign(·)为符号函数，其表达式为：

如果邻节点与新节点之间的路径与障碍物发生碰撞，则删除新节点重新采样，反之将新节点***树中，即完成树的一次生长过程。一般情况下RRT算法采样点生成遵照以下规则：

其中λ∈[0,1]，即有λ的概率，随机产生采样点，其余1-λ的概率直接将目标着陆点作为采样点。一般取λ＝0.5。

基于上述RRT算法的基本思路，并结合翼伞运动特性将其扩展到三维空间，来实现翼伞航迹规划。翼伞质点模型为：

其中，(x,y,z)为翼伞在大地坐标系下的位置，V为翼伞总的速度，

为翼伞航向，γ为翼伞下滑角。可以通过襟翼来控制翼伞航向及下滑角。稳定状态下翼伞运动可以分解为水平方向的匀速运动及竖直方向的匀速运动，翼伞滑翔比k定义为：

则下滑角γ可以表示为：

因此，稳态下翼伞的运动轨迹是一条具有恒定下滑角的曲线。

空投区域地形函数表达式为：

z＝f(x,y)∈[z_min,z_max] (7)

其中，x∈[x_min,x_max]及y∈[y_min,y_max]为空投区域的边界。翼伞初始投放点作为树的根节点，在每次迭代过程，记随机样点P_i(x_i,y_i)，邻节点记为P_j(x_j,y_j,z_j)。则新节点位置依照下列公式进行更新：

其中，Step为搜索步长，k为翼伞滑翔比，θ_j+1为树枝延伸方向，也可称为新节点的搜索方向，其计算公式为：

新节点是否可行的判断依据为：

f(x,y)-z＜0(x,y,z)∈L (10)

f(x,y)为地形的函数表达式，L表示线段P_jP_j+1上所有的点构成的集合。式(10)的实质是判断经过点P_j和点P_j+1的路径会不会与障碍物发生碰撞，如果不会发生碰撞，则证明新节点可行，随机树可向该处延伸。否则表明此节点不可行，不能向该处更新节点。

基于上述方案的优选方案，可以增设安全距离d。将式(10)改写为：

f(x,y)-z＜d(x,y,z)∈L (11)

以确保搜索到的轨迹远离障碍物，保证翼伞的安全。

基于上述方案的优选方案，样点的产生，1/2概率来自随机产生，其余1/2的概率直接将目标着陆点作为样点。这样做的好处是在满足避障的同时加快搜索速度。

由于襟翼控制能力有限，且当非对称襟翼控制量较大时，翼伞会失速。因此翼伞转向速度具有一定的上限，对应翼伞转弯半径的下限R_min。因此随机搜索过程的搜索方向必须要受到约束。基于上述方案的优选方案，根据几何关系，如图2所示，相邻两个节点的搜索方向之差记为β，其与搜索步长Step及翼伞转弯半径R之间满足关系式：

则：

基于上述方案的优选方案，搜索步长Step的取值不能过大，否则会影响路径的平滑程度，不利于翼伞***跟踪控制。同时，Step的取值也不宜过小，否则会降低搜索速度。取：

Step＝R_min (14)

则此时连续两步搜索方向的角度差要满足：

以便寻找到较为平滑的轨迹，满足实际空投需要。

重复上述操作，直到有节点更新到着陆区内，此时随机搜索过程结束。判断是否进入着陆区的依据为：

||P_jO||≤R_th (16)

式中，O为目标着陆点，P_jO为节点与目标着陆点构成的矢量在水平面的投影矢量，R_th为着陆区域的半径，即当有节点满足与目标着陆点水平距离小于或等于着陆区半径时随机搜索过程随之结束。该节点为随机搜索的最终节点，最终节点、根节点及连接两者之间的所有节点即为要找寻的障碍区内的路径节点，对其进行插值及平滑处理，即可得到障碍区内的航迹。此时，即完成了障碍区内的避障航迹规划任务。

由于利用RRT算法进行随机搜索，翼伞在障碍区内难以实现在避障的同时进行高度控制及逆风对准操作，因此避障段终点状态和目标着陆点最终状态还存在差距。此时翼伞还存在高度势能未得到释放，且此时翼伞方向随机，不能满足逆风对准着陆要求。因此还需要在着陆区内设计翼伞的航迹，以满足高度控制及逆风对准要求，提高翼伞落点精度。着陆区内航迹设计要满足航迹不能超出着陆区半径，否则可能会碰到着陆区之外的障碍物。因此最优控制归航方式由于无法将航迹限制在一个确定范围内，显然不适合用来设计着陆区内的翼伞航迹。基于上述方案的优选方案，分段航迹规划方式是一个较为理想的选择。

着陆区内的航迹设计如图3所示，A点是随机搜索终止的节点，同时也是着陆区航迹的初始位置。类似于传统分段航迹规划，省略开始的转弯过度段。航迹由AB向心归航段，

过渡段1，

高度控制段，

过渡段2及EO逆风着陆段组成。高度控制段转弯半径记为R_ep，对应圆心角α₂，过渡段半径为翼伞最小转弯半径R_min。将轨迹投影在XOY平面，A点坐标为随机搜索段终止节点的坐标，记为(x_a,y_a,z_a)，方向为随机搜索终止节点A的搜索方向。根据几何关系，过渡段1的圆心C₁点坐标可以表示为：

AB段长度为：

定义一个符号变量s，设定顺时针转弯时s＝-1，逆时针转弯时s＝1。基于上述方案的优选方案，若目标着陆点位于最终节点(进入点)及其搜索方向约束构成的矢量左侧，取s＝1，航迹采取逆时针旋转，反之则取s＝-1，分段航迹采取顺时针旋转。矢量C₁B与X轴正向夹角：

则过渡段1的圆心角α₁：

∠C₂OE的计算公式为：

高度控制段转弯圆弧对应圆心角α₂为：

过渡段2的圆心角α₃：

EO段距离为：

建立目标函数：

其中，k为稳定状态下的翼伞滑翔比，z_a为A点翼伞飞行高度，z_f为目标着陆点海拔高度。基于上述方案的优选方案，高度控制段转弯半径R_ep不能大于着陆区半径R_th，即：

R_ep≤R_th (26)

因此目标函数进一步可以进一步建立为：

参数τ的大小表示高度控制段完整盘旋的圈数，目的是通过合理选择盘旋圈数，既能保证翼伞高度势能得到释放，同时确保高度控制段转弯半径满足式(26)。着陆区内的航迹规划目标是求取高度控制段转弯半径R_ep及进入高度控制段的角度θ_ep，可以借用优化算法进行求解。

基于上述方案的优选方案，着陆区半径R_th不能小于翼伞最小转弯半径R_min，否则难以实现翼伞高度控制。R_th在条件允许的情况下越大越好。

基于上述方案的优选方案，进入点的高度z_a要满足：

z_a＞k·R_th (28)

以保证翼伞有足够的高度可以到达目标着陆点。

至此，如图1所示，通过对障碍区及着陆区内的航迹分别进行设计，并组合起来，使得航迹同时具备避障、高度控制、逆风对准及落点精度要求。

可以将上述内容归纳为如下步骤：

(1)初始化操作，给定翼伞初始位置信息及目标着陆点位置及风向信息；

(2)判断翼伞初始位置(根节点)是否位于空投区域内，若不在则无法继续进行航迹规划，若在空投区域内，则进行下一步；

(3)根据翼伞和目标着陆点水平距离是否小于着陆区半径判断翼伞位置是否已经进入圆形着陆区域内，若已经进入着陆区，则无需进行避障航迹规划，直接采用分段的方式设计着陆区内的翼伞航迹。若当前位置未进入着陆区内(位于障碍区内)，则进行下一步；

(4)采用RRT算法快速搜索可行路径如图1所示，根据图2的几何关系，判断每次搜索的搜索方向是否满足式(15)。若不满足，则重新生成样点，并再次进行判断。若满足约束，则进入下一步；

(5)判断新样点是否在空投区域内，若不在，则返回步骤(4)。若在空投区域内，则进入下一步；

(6)进行路径检查，根据式(10)检查新节点与上一节点构成的路径是否与障碍物发生碰撞，若发生碰撞，则返回步骤(4)。否则表明新节点可行，进入下一步；

(7)根据式(8)更新路径，将新节点添加进路径。然后返回步骤(3)，若翼伞未进入着陆区，则重复步骤(4)、(5)、(6)。若已经进入，则求解分段航迹规划目标函数J，求解轨迹参数，进行着陆区内分段航迹规划；

(8)图1中进入点与根节点及之间连接的所有节点所构成的路径即为搜索到的可行路径，对其做平滑处理，使轨迹更为平滑。RRT搜索得到的路径和分段归航路径即为完整的路径。

为了清晰表述本发明一种翼伞组合式避障航迹规划方法的原理，以普通RRT路径搜索方法为例，进行了介绍。但不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，采用其他类似的可行路径搜索方法，在技术方案基础上所做的任何改动，均归入本发明保护范围之内。

Claims (9)

1.一种复杂环境下的翼伞组合式航迹规划方法，其特征在于：将翼伞空投区域分为有障碍物存在的障碍区和以目标着陆点为圆心且空旷平坦的圆形着陆区；在障碍区内采用快速搜索随机树路径规划算法快速搜索可行轨迹，搜索的终止条件是路径延伸到的位置与目标着陆点之间的水平距离小于或等于圆形着陆区域的半径，在障碍区内仅考虑避障任务；将障碍区内的航迹搜索的终止状态作为圆形着陆区内航迹规划的初始状态，在圆形着陆区内采用分段归航的方式设计航迹，在圆形着陆区内完成翼伞高度控制和逆风对准任务；

在障碍区内采用快速搜索随机树路径规划算法快速搜索可行轨迹的方法如下：

记样点P_i的坐标为(x_i,y_i)，邻节点P_j的坐标为(x_j,y_j,z_j)，则新节点P_j+1的位置(x_j+1,y_j+1,z_j+1)按下式进行更新：

上式中，Step为搜索步长，k为翼伞滑翔比，θ_j+1为新节点的搜索方向，其计算式如下：

新节点是否可行的判断依据如下：

f(x,y)-z＜0,(x,y,z)∈L

上式中，f(x,y)为空投区域地形的函数表达式，(x,y,z)为翼伞在大地坐标系下的位置，L为线段P_jP_j+1上所有点构成的集合。

2.根据权利要求1所述复杂环境下的翼伞组合式航迹规划方法，其特征在于：增设安全距离d，将新节点是否可行的判断依据修改为：

f(x,y)-z＜d,(x,y,z)∈L。

3.根据权利要求1所述复杂环境下的翼伞组合式航迹规划方法，其特征在于：对于所述样点的产生，1/2的概率来自随机产生，其余1/2的概率直接将目标着陆点作为样点。

4.根据权利要求1所述复杂环境下的翼伞组合式航迹规划方法，其特征在于：设相邻两个节点的搜索方向之差为β，其与搜索步长Step和翼伞转弯半径R之间满足下式：

则：

上式中，R_min为最小翼伞转弯半径。

5.根据权利要求4所述复杂环境下的翼伞组合式航迹规划方法，其特征在于：令Step＝R_min，则：

6.根据权利要求1所述复杂环境下的翼伞组合式航迹规划方法，其特征在于：圆形着陆区中的轨迹分为5段，依次为向心归航段、过渡段1、高度控制段、过渡段2以及逆风着陆段，其中向心归航段和逆风着陆段为直线段，过渡段1、高度控制段和过渡段2为弧线段；

其中：

向心归航段的长度如下：

上式中，AB为向心归航段矢量，A为向心归航段的起点，B为向心归航段的终点，x_a为A点在大地坐标系X轴上的坐标，

为过渡段1的圆心C₁在大地坐标系X轴上的坐标，R_min为最小翼伞转弯半径，令过渡段的半径为R_min；

过渡段1的圆心角如下：

上式中，α₁为过渡段1的圆心角，θ_ep为高度控制段的角度，∠C₁B为矢量C₁B与大地坐标系X轴的正向夹角，s为符号变量，s＝1表示航迹逆时针旋转，s＝-1表示航迹顺时针旋转；

高度控制段的圆心角如下：

α₂＝2π-∠C₂OE-s·θ_ep

上式中，α₂为高度控制段的圆心角，C₂为过渡段2的圆心坐标，E为逆风着陆段的起点，O为目标着陆点；

过渡段2的圆心角如下：

上式中，α₃为过渡段2的圆心角；

逆风着陆段的长度如下：

上式中，R_ep为高度控制段的转弯半径。

7.根据权利要求6所述复杂环境下的翼伞组合式航迹规划方法，其特征在于：分段归航的目标函数如下：

上式中，J为目标函数，k为翼伞滑翔比，z_a为A点翼伞飞行高度，z_f为目标着陆点的海拔高度。

8.根据权利要求7所述复杂环境下的翼伞组合式航迹规划方法，其特征在于：令R_ep≤R_th，则分段归航的目标函数如下：

上式中，R_th为圆形着陆区半径，R_th不小于最小翼伞转弯半径，τ表示高度控制段完整盘旋的圈数。

9.根据权利要求8所述复杂环境下的翼伞组合式航迹规划方法，其特征在于：z_a＞k·R_th。

Images