复杂环境下翼伞系统的组合式航迹规划

李宇辉，赵敏,3,*，陈奇，姚敏,3，何紫阳

1. 南京航空航天大学 自动化学院，南京 210016

2. 淮阴工学院 电子信息工程学院，淮安 223003

3. 高速交通设施无损检测与监控技术-工业和信息化部重点实验室，南京 210016

翼伞系统是一种具有高负载及良好控制性能的柔性飞行器。相比于圆形降落伞系统只能随风而动，无法对其施以有效控制，翼伞系统在继承了圆形降落伞负载能力强的优点的同时，还可以通过控制左右后缘襟翼的偏转量来控制翼伞系统的飞行方向及下滑角，实现精确空投。利用翼伞空投进行物资投送及航天器回收是一个非常不错的选择。国内外众多学者都进行了翼伞系统的相关研究[1]。实现翼伞精确空投的基础是为翼伞系统规划合理的航迹，常见的翼伞系统航迹规划方法有分段航迹规划及最优控制航迹规划，这两种方法的前提是空间内不存在障碍物的影响。而实际翼伞系统归航途中可能会遇到高山或者高大建筑物的阻碍，此时就要考虑如何合理地规划航迹，以使翼伞系统可以绕开这些障碍物，从而顺利实现归航。

在无障碍环境下，翼伞系统航迹规划的方法主要包括分段航迹规划及最优控制航迹规划。分段航迹规划是一种较为经典的方式，其基本思想是将翼伞系统航迹设计为螺旋曲线和直线的组合，要满足由初始状态和目标状态构成的几何约束，追求落点精度及逆风着陆。熊菁等[2]研究了分段归航方式下的翼伞系统航迹设计问题，并采用遗传算法(GA)对分段归航的目标函数进行求解。陶金等[3-4]分别采用粒子群算法(PSO)及量子遗传算法进行了翼伞系统分段归航航迹规划目标函数的求解，均取得了不错的效果。赵志豪等[5]对常规的分段归航方式进行了改进，舍弃了翼伞系统由向心归航段到能量控制段之间的过渡阶段，改为由切点切向进入能量控制段，并采用改进型鱼群算法对目标函数进行求解。高峰等[6]建立了翼伞系统高精度9自由度动力学模型，通过数值分析得到稳态滑翔比及转弯角速率限值。以耗能最小为目标函数，耗时和转弯半径为输入变量，建立了5段归航轨迹优化模型。将5段归航轨迹作为初始条件，利用伪谱法进行轨迹优化，给出归航最优参考路径，并对比分析了5段归航与直接归航过程中的能量损耗。除了分段归航方式之外，最优控制归航也是一种常见的航迹规划方式。相比于分段归航方式，最优控制归航方式还额外追求控制能耗最小。熊菁[7]做了开创性的研究，采用间接法分析和研究了翼伞系统最优控制航迹规划问题。高海涛等[8-9]分别采用伪谱法进行目标函数的求解，其基本思想是将航迹规划问题转化为非线性规划问题。此外，梁海燕等[10]将控制量参数化，将最优控制问题简化为参数优化问题，降低了目标函数的求解难度。

在复杂环境下的翼伞系统航迹规划领域，本文作者[11]提出了一种基于梯度下降法的翼伞系统最优分段航迹规划方法，该方法将控制变量参数化，将逆风精确着陆、控制能耗小、能实现避障等多目标优化问题转化为加权单目标优化问题，并通过梯度下降法求解得到分段常值最优归航航迹。孙昊等[12]针对传统质点模型所规划的目标轨迹难以满足复杂环境下的系统动力学约束问题，将翼伞系统六自由度动力学模型引入翼伞系统归航的航迹规划中，并通过改进高斯伪谱法，设计了一种基于分段点规划、离散点初次规划、离散点自重构的三阶轨迹优化策略。

实际翼伞空投环境一般较为复杂，但往往具有这样的特点，即翼伞系统归航过程中会遇到高山或者高大建筑物阻碍，只有目标点周围的小范围内没有障碍物。此时，难以单独采用分段归航或者最优控制归航方式来设计翼伞系统的航迹。必须采取相应方法，规划一条能够绕开障碍物并最终到达目标点，满足能量控制及逆风着陆的要求，同时也要满足一定的落点精度要求的翼伞系统航迹。

为了解决上述复杂环境下的翼伞系统航迹规划问题，本文考虑实际空投任务中的环境特点，将空间划分为障碍区和着陆区，在障碍区域内采用经过适用性改进的快速搜索随机树(RRT)算法搜索可行路径。并且采用相应的处理，以生成平滑的参考轨迹。考虑到RRT算法方向随机，难以直接实现逆风对准着陆且在避障过程中不易进行能量控制的问题，在翼伞系统进入着陆区内，采用分段归航的方式设计航迹，实现翼伞系统能量控制及逆风着陆。同时考虑了两个区域航迹的衔接问题。

1 翼伞系统质点模型

常见的翼伞系统模型有质点模型、被视作刚体的六自由度模型、考虑伞体与载荷之间相对运动的九自由度或八自由度模型等。对于翼伞系统航迹规划问题，主要是研究翼伞系统整体的运动，翼伞系统的姿态变化及系统各结构之间的相对运动不是航迹规划所要考虑的主要因素，因此，参考翼伞系统质点模型即可满足要求。

考虑稳态情况下，翼伞系统滑翔比和速度保持不变，翼伞系统的质点模型可以表示为

(1)

式中：(x,y,z)为翼伞系统在大地坐标系下的位置；V为翼伞系统和速度；φ为翼伞系统航向角；u为控制量，与非对称襟翼偏转量存在对应关系；γ为翼伞系统下滑角。

稳定状态下，翼伞系统运动可以分解为水平方向的匀速运动及竖直方向的匀速运动，翼伞系统滑翔比k定义为

(2)

则下滑角γ可以表示为

(3)

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

2 复杂环境下翼伞系统航迹规划问题描述

在实际翼伞空投任务中，翼伞系统在归航途中会遇到高山或者高大建筑物阻碍，需要绕开这些障碍物，仅在着陆点附近存在范围有限的空旷区域，即为目标着陆区域。图1为复杂环境下的翼伞空投示意图。图1中：φ0为初始航向角。大地坐标系原点为空投的目标着陆点，以目标点为中心，周围半径为Rth的圆形区域为没有障碍物的着陆区。而在空投的初始点与着陆区之间则是存在高山或者高大建筑物的障碍区，要实现定点空投，必须合理地规划航迹，以使翼伞系统绕过障碍物，并最终到达目标点。

图1 复杂环境下空投示意图

根据上述有障碍时翼伞空投的特点，将复杂环境下翼伞系统的航迹规划要求总结为以下几点：

1) 系统能够躲避障碍物，不发生碰撞。

2) 航迹应平滑，可被跟踪。

3) 满足逆风着陆。

4) 落点精度尽可能高。

采用数学的方法来描述复杂环境翼伞系统航迹规划问题，假设翼伞系统初始状态为

(4)

式中：(x0,y0,z0)为空投初始时刻t0时翼伞系统的位置。翼伞系统在穿出障碍区，亦即进入着陆区瞬间的状态为

(5)

式中：ts为穿越障碍区的最终时刻，也是进入着陆区域的初始时刻。翼伞系统到达目标点时的状态为

(6)

式中：tf为着陆时刻；φf为着陆时翼伞系统的航向角，此时φf应该为着陆点风向的反方向。

在障碍区内，翼伞系统轨迹表示为

(7)

令Xobs表示障碍物占据的空间区域，因此，复杂环境下翼伞系统航迹规划实现避障及航迹应平滑的要求可表示为

(8)

式中：σ为轨迹变化率的最大值，其值越小表示轨迹越光滑。

为了方便分析计算，假定目标点为大地坐标系原点，风向沿着X轴正向。此时，对逆风着陆及落点精度要求,可以通过式(9)目标函数表示：

(9)

对于述(9)目标函数可以通过加权的方式，将多目标函数求解问题转化为单目标函数求解问题，即

J=k1J1+k2J2

(10)

式中：k1、k2为权重，可以根据实际空投任务的侧重点选择，再借助GA或PSO等优化算法进行求解。

3 翼伞系统组合式航迹规划策略

根据有障碍物存在时的翼伞空投任务要求，特提出复杂环境下翼伞系统的组合式航迹规划策略，如图2所示。

图2 复杂环境翼伞系统组合式航迹规划原理示意图

其核心思想是将空投区域分为有障碍物的障碍区和无障碍物的着陆区。在障碍区内采用进行了适用性改进的RRT算法搜索可行路径，完成避障任务。而当翼伞系统进入着陆区，采用分段的方式设计航迹，完成能量控制及逆风着陆任务。同时要考虑两段航迹之间的顺利衔接。

3.1 基本RRT算法

当前用于复杂环境路径规划的常见算法主要有禁忌算法、GA、人工势场法、模糊逻辑算法、RRT算法及贪心算法等。其中RRT算法是一种基于采样的路径规划算法，相比于其他路径规划算法具有求解速度快的优势，常用于移动机器人路径规划，适合解决高维空间和复杂约束下的路径规划问题[13-15]。其基本思想是以产生随机点的方式通过一个步长向目标点搜索前进。能够有效躲避障碍物，避免陷入局部最优，且收敛速度快。

RRT算法在状态空间内随机采样到一个节点时，首先在当前树中寻找与该采样点距离最近的邻节点，然后连接采样节点和邻节点，生成新的节点。记随机采样点为Prand,邻节点为Pnearest,新节点为Pnew,搜索步长为Step。则新节点的表达式为

(11)

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

(12)

如果相邻节点与新节点之间的路径与障碍物发生碰撞，则删除该新节点并重新进行采样，反之将新节点插入树中，即完成随机搜索树的一次生长过程。通常情况下RRT算法采样点的生成遵照以下规则：

(13)

式中：λ∈[0,1],即有λ的概率，随机产生采样点，其余1-λ的概率直接将目标点作为采样点。通常取λ=0.5。

RRT算法随机搜索过程相邻两步之间的几何关系如图3所示，记矢量Pn-1Pn的方向为节点Pn的搜索方向，其与X轴夹角为φn，相邻两节点的搜索方向角度差为β，则存在以下几何关系：

图3 随机搜索几何关系示意图

(14)

式中：R为与连续两条树枝相切的圆的半径。

3.2 基于RRT算法翼伞系统航迹规划

翼伞系统在障碍区内的航迹规划借鉴于平面机器人路径规划，并根据翼伞系统运动特性，将其扩展到三维空间。翼伞系统由于自身的特性，其运动轨迹在平面的投影为直线或者圆弧，RRT算法产生的路径为树状，为折线路径，显然与翼伞系统运动轨迹存在一定的差距。因此采用RRT算法进行翼伞系统航迹规划的同时也必须考虑路径平滑处理问题。本文基于RRT的翼伞系统航迹规划策略如图4所示。

图4 基于RRT算法翼伞系统航迹规划示意图

将RRT算法的相邻两个搜索节点的中点作为轨迹节点。相邻的轨迹节点之间以相切于相邻两条树枝的圆弧作为过渡。通过这样的处理方式，解决了RRT算法产生的轨迹存在棱角的问题，使得产生的轨迹为光滑曲线。随机搜索树的根节点与第一个轨迹节点之间的线段及最后一个轨迹节点与随机搜索终止节点之间的线段与各轨迹节点之间的若干圆弧过渡段共同组成了障碍区内的翼伞系统的轨迹。

(15)

式中:Step为搜索步长；θ为树枝延伸方向，其计算公式为

(16)

由于受到翼伞系统的运动特性的限制，轨迹要光滑，且要满足最小转弯半径限制。基于此，令连续两个相邻搜索节点的中点作为翼伞系统轨迹节点。则第n深度的翼伞系统轨迹节点水平投影坐标为

(17)

式(15)和式(16)描述了路径节点在水平面投影的更新规律，翼伞系统的航迹为三维轨迹，需要将二维平面RRT算法推广到三维空间。根据图4 的几何关系，同时，还需要满足翼伞系统质点模型的约束，确保生成的航迹具有恒定的下滑角。结合式(14)，则轨迹节点Xn-1与Xn之间的轨迹可以通过式(18)表示:

ϖ∈[0,βn)

(18)

式中：ϖ表示角度自变量；sign(·)为取符号函数，符号为正表示逆时针旋转，反之则为顺时针旋转。

上述RRT算法的适用性改进是在水平方向根据平面RRT算法来生成轨迹节点，之后在水平坐标及翼伞系统的滑翔比的约束下生成翼伞系统的三维轨迹，以此来确保翼伞系统的航迹具有稳定的下滑角。本研究中改进型RRT算法的实质是水平方向随机，而垂直方向并非随机。

对于复杂环境下翼伞空投任务，本文建立的空间地形模型的表达式为

Xobs=f(x,y)=

(19)

式中：x∈[xmin,xmax]、y∈[ymin,ymax]表示空投区域的范围。路径可行性的判断依据为

(20)

式中：d为安全距离，可根据实际需要进行设置。

翼伞系统由于控制能力有限，翼伞系统转向运动的角速率具有一定的上限，对应翼伞系统转弯半径R的下限Rmin。为了适应翼伞系统实际运动特性，需要对随机搜索树的搜索方向施加以相应的约束，根据式(14)，可以得出：

(21)

搜索步长的值不宜设置过大，否则不利于搜索到可行路径。同时Step的取值也不宜过小，否则会降低算法求解速度。取

Step=Rmin

(22)

则式(21)可进一步简化为

(23)

只有当βn满足约束条件进行搜索时，才可以得到能够满足翼伞系统归航需要的轨迹。否则可能导致轨迹曲率过大，无法被翼伞系统跟踪。

随机搜索的初始状态为式(4)，终止状态为式(5)，随机搜索终止的判断判断依据为

(24)

式中：O为目标点；PnO|xy为搜索节点与目标点构成的矢量在水平面的投影矢量；Rth为着陆区域的半径。在障碍区内翼伞系统航迹的水平距离为

(25)

翼伞系统下降的高度为

(26)

当满足随机搜索终止条件时，表明翼伞进入着陆区域内，但此时翼伞系统还未到达目标点。

3.3 着陆区内航迹规划

在完成障碍区航迹规划任务后，翼伞系统进入着陆区，此时的初始状态即为随机搜索的终止状态。着陆区内航迹规划旨在实现翼伞系统能量控制及逆风着陆。在无障碍的环境中可以借助分段航迹规划或者最优控制航迹规划的方式设计航迹，以实现能量控制和逆风着陆。在采用最优控制归航方式时，翼伞系统会先向远离目标点方向飞行以便消耗自身高度势能，当能量消耗到合适的程度后掉转方向，向着目标点靠近，并进行逆风对准着陆，因此需要较大的空间。而分段归航的方式则可以人为地将翼伞系统的航迹限制在一个有限的范围内。因此，对于范围有限的着陆区，采用分段航迹规划的方式设计航迹更为合适[16-18]。着陆区内分段航迹规划原理如图5所示。

图5 着陆区航迹规划示意图

(27)

C1点坐标为

(28)

AB段长度为

(29)

定义一个符号变量s，设定顺时针转弯时s=-1，逆时针转弯时s=1。则C1B与X轴正向夹角为

(30)

则第1个过渡圆弧的圆心角α1为

α1=2π+s(θep-∠C1BX)∈[0,2π]

(31)

∠C2OE的计算公式为

(32)

能量控制段转弯圆弧对应圆心角α2为

α2=2π+s(-s·∠C2OE-θep)=

2π-∠C2OE-sθep∈[0,2π]

(33)

第2个过渡圆弧的圆心角α3为

(34)

EO段距离为

(35)

建立目标函数：

(36)

式中：k为稳定状态下的翼伞系统滑翔比；zA为点A高度；zf为目标着陆点的海拔高度。

从本研究所针对的翼伞空投环境的特点来看，能量控制段转弯半径Rep不能大于着陆区半径Rth，即

Rep≤Rth

(37)

否则可能撞到障碍物。因此目标函数可进一步建立为

τ=0,1,2，…

(38)

参数τ的值表示翼伞系统盘旋的圈数，目的是通过合理选择盘旋圈数，在确保翼伞系统高度势能得到释放的同时，也能确保将翼伞的运动范围限制在着陆区内，避免与障碍物发生碰撞。

进入点A的高度与初始高度及在障碍区内的路径长度有关，满足：

(39)

在进入着陆区后，翼伞系统下降的总高度为

H2=zA-zf

(40)

若要满足翼伞系统能够最终到达目标点，则需要满足：

(41)

由于翼伞系统最小转弯半径的限制，着陆区半径必须要满足一定的条件。根据图5所示的几何关系，可以得出着陆区半径Rth满足：

Rth≥2Rmin

(42)

着陆区内的航迹规划目标是求取参数Rep及进入能量控制段的角度θep，可以借用GA或者PSO等优化算法进行求解。

3.4 算法流程

本文复杂环境下翼伞系统组合式航迹规划算法流程如图6所示，主要分为如下几个步骤：

图6 复杂环境下翼伞系统组合式航迹规划算法流程图

步骤1初始化，给定翼伞系统初始点坐标(x(t0),y(t0),z(t0))、航向角φ(t0)、目标点位置(x(tf),y(tf),z(tf))、目标点航向角φ(tf)、搜索步长Step等信息。

步骤2判断翼伞系统位置是否在空投区域内。若不在，则结束。反之，则进行步骤3。

步骤3判断翼伞系统位置是否已经进入着陆区域内，即是否满足式(24)。若翼伞系统在障碍区内，如果是第一次循环则执行步骤4。否则执行步骤5，采用RRT算法进行翼伞系统可行路径搜索。若已进入着陆区，则跳转到步骤9。

步骤4先从初始投放点向初始航向的方向延伸一个搜索步长生成一个节点(默认一个步长的圆形范围内无障碍物)，该步骤的目的在于确保规划的航迹的初始方向与所给定的初始航向的方向相同。之后将该节点作为RRT算法的根节点。

步骤5随机生成样点，检查搜索方向是否满足式(23)的约束。若不满足约束，则返回并重新随机生成样点。若满足搜索方向的约束，则进入步骤6。

步骤6检查生成的采样节点是否在空投区域内，若不在空投区域内则返回步骤5。若在空投区域内，则进行步骤7。

步骤7进行路径检查，根据式(20)检查新生成路径是否与障碍物发生碰撞。若发生碰撞则删除样点，并返回步骤5。否则表明路径可行，进入步骤8。

步骤8根据式(18)更新路径，将新的节点插入进路径。之后返回到步骤3，如果没有进入着陆区，则重复步骤5～步骤7。若进入着陆区内，则执行步骤9。

步骤9进行着陆区内的航迹规划。将进入着陆区时的样点信息作为着陆区内分段航迹规划的起点，调用优化算法求解着陆区内航迹参数。至此，复杂环境下翼伞系统航迹规划完成。

4 仿真及分析

设置翼伞系统初始投放点的坐标为(1 800，1 800， 1 300) m，为了方便显示完整轨迹，目标点坐标设为(300,400,0) m。设置搜索步长Step=Rmin=50 m，滑翔比k=3，空投区域为2 000 m×2 000 m×2 000 m的立方体空间，着陆区半径Rth=300 m,目标点附近风向与X轴平行，指向X轴正方向。值得注意的是，本文山峰模型的设置更具有一般性，即山峰模型的水平截面不必为标准圆，且山峰之间可以存在重叠。本文采用翼伞海拔高度与当前水平坐标下的地表海拔高度的相对大小作为判断是否发生碰撞的依据。因此如果翼伞系统当前的高度低于山峰之间的间隙的高度则很可能导致无法搜索到可行路径。山峰之间的间隙的宽度及翼伞投放高度等因素都会影响航迹规划的速度。山峰模型的参数可以根据实际情况进行调整。本文以表1所示的山峰模型参数为例，来验证提出的航迹规划算法的可行性。在MATLAB上运行算法，航迹规划结果如图7所示。

表1 山峰模型参数

图7(a)和图7(b)分别为RRT随机搜索过程的3D及2D视角图，可以看出，在进行了多次随机搜索后，当有节点进入着陆区内，RRT随机搜索过程结束，切换为分段航迹规划方式。图7(c)和图7(d)分别为最终得到的航迹的3D及2D视角。由于本文对RRT算法进行的改进，使得生成的航迹较为平滑，且最终落点精度较高，并实现了逆风着陆。图7(e)为利用GA求解分段航迹规划目标函数的适应度变化曲线，可以看出在迭代了20步左右的时候，粒子适应度最优值已经达到0.000 342 21， 所有个体的平均适应度达到0.008 195 2，这表明航迹误差已基本收敛为0。

图7 复杂环境下翼伞系统组合式航迹规划

将投放点海拔高度增大，进行高海拔空投翼伞系统的航迹规划。设定翼伞系统初始点坐标为(1 800，1 800，1 600) m，其余各条件与前一组仿真实验相同。航迹规划结果如图8所示。可以看出当海拔高度增大后，通过采用盘旋的方式，实现了翼伞系统能量控制，同时将分段归航段的轨迹限制在无障碍物的着陆区内，从而避免和障碍物发生碰撞。

图8 高海拔复杂环境下翼伞系统组合式航迹规划

本文的算法在CPU为i7-9750H、内存为16 G 的笔记本电脑上运行，两组仿真实验的算法运行时间分别为16.67 s和18.33 s，求解速度较快，体现了RRT算法在路径规划方面求解速度的优势。从航迹规划的效果来看，本文提出的翼伞系统组合式航迹规划策略，能够解决复杂环境下的翼伞系统的航迹规划问题，规划得到的航迹能够满足翼伞空投需要。

5 结 论

本文提出了一种复杂环境下的翼伞系统组合式航迹规划策略，根据实际空投的环境特点，将翼伞空投区域分为障碍区和着陆区。在障碍区内采用RRT算法快速搜索可行路径，根据翼伞系统运动特性，对RRT算法进行了适用性改进，使其满足于翼伞系统航迹规划需要，在障碍区内完成翼伞系统避障任务。又针对RRT算法搜索方向随机，难以实现翼伞系统逆风着陆及翼伞系统能量控制的问题，采用分段规划的方式，借助GA对目标函数进行优化求解，实现了着陆区内的航迹规划。在研究中考虑了两段航迹的衔接问题，以实现航迹的顺利过渡。得到的航迹能够同时满足避障、能量控制及逆风着陆的要求。航迹较为平滑，能够被跟踪，且算法具有较快的求解速度。

本文的研究也存在一些局限性。首先，未考虑平均风及由高大障碍物所引起的风切变对翼伞航迹规划的影响，这可能会给翼伞系统的轨迹跟踪控制造成一定的困难。未来可以将平均风及风切变对于翼伞系统航迹规划的影响作为一个重要的研究方向。其次，本文算法适用于着陆点周围有较大的空旷区域的环境，当空投环境更为复杂，即着陆点附近存在障碍物时，提出的策略则难以适用。改进型的双向RRT算法或许可以成为解决该问题的一个可行方法，但仍需进行进一步的研究。