地形跟随中的航迹规划与跟踪

王 彪， 董 超， 黄 毅， 唐超颖

(南京航空航天大学自动化学院，南京 211106)

0 引言

实现地形跟随的方案之一是基于地形数据，采用航迹规划算法确定期望航迹曲线，再通过航迹跟踪控制完成。高精度的地形数据为该方案的实现增加了可行性。

航迹规划是综合考虑多种因素所得最优或可行的飞行航迹[1]。通常的航迹规划分为初始航迹生成和航迹光顺两步。国内外一般采用Voronoi图法[2]、概略图法[3]等结合Dijkstra,A*[4]等最短路径搜索算法生成初始折线航迹，再通过合适的曲线光顺算法获得满足飞行性能约束的光滑航迹。利用B样条[5-6]、Dubins曲线[7]和Reeds-Shepp曲线[8]光顺所得航迹不利于后续跟踪。本文综合考虑现有规划算法的研究弊端、地形跟随问题中航迹规划的需求，采用文献[9]最优航迹方法，通过最小化一个最优性能函数，借鉴文献[10]设计满足飞行器机动性能约束的三次样条函数，再施以高阶连续性约束，将航迹生成和航迹光顺一步完成。

经典的Pure Pursuit，LOS及其各变种的跟踪方法[11-12]对飞行器的机动能力有较高要求，针对该问题提出了许多非线性制导律。其中:NELSON等[13]提出的矢量场(Vector Field,VF)制导律逐渐成为无人机制导领域的著名工具，它们分别构造了直线与圆弧矢量场生成航向角指令，在有风条件下仿真有效;朱欣华等[14]针对两轮驱动机器人的路径跟踪问题，通过适量场法精确跟踪较为复杂的正弦路径;XU等[15]在自动水面舰艇控制系统中仿真验证矢量场法比LOS算法有更好的跟踪性能;文献[16]研究证明矢量场路径跟踪依赖更少的控制量。尽管如此，目前在地形跟随中应用矢量场路径跟踪技术的研究仍然较少。本文基于矢量场方法研究面向地形跟随的航迹跟踪策略，针对参考路径构造矢量场，根据最大航迹角变化率给出矢量场全局可行的条件，用于指导调整矢量场参数，再考虑无人机的输入约束，设计以过载为控制量的跟踪控制器，有效完成地形跟随任务。

1 地形跟随最优航迹规划

地形跟随中的最优航迹规划是对于给定区域的三维地形剖面，一条从初始位置到终点的满足预定安全高度和飞行器机动极限的飞行轨迹。

1.1 递推三次样条曲线设计

在地形跟随航迹规划问题中，航迹曲线至少满足二阶可导才能满足飞行器自身航迹角和过载等机动性能约束。本节以如下三次样条函数[10]描述区间(Li-1,Li)(i=1,2,…,n)上期望地形跟随航迹，即

(1)

式中：hi-1为区间(Li-1,Li)左节点高程信息;si-1为该点一阶导数，即斜率；ki-1为二阶导数，即等效曲率；pi=(ki-ki-1)/Δ为区间内三阶导数的差分近似，Δ=Li-Li-1为地形区间步长。对式(1)逐次进行空间求导可得

(2)

k(L)=ki-1+pi(L-Li-1)

(3)

可见，对于二阶导数k区间内为L的线性函数，只要确定各节点参数hi,si,ki即可确定三次样条曲线形状。

特别地，若取固定步长，则节点Li处曲线参数hi,si,ki之间的递推关系及与初值h0,s0,k0的关系为

hi=h0+Δ(s0+s1+…+si-2+si-1)+Δ2(2k0+3k1+…+3ki-1+ki)/6

(4)

si=s0+Δ(k0+2k1+…+2ki-1+ki)/2

(5)

式(4)中的s1+…+si-2+si-1根据式(5)又可改写为

s1+…+si-2+si-1=(i-1)s0+Δ[(i-1)k0+

(2i-3)k1+(2i-5)k2+…+3ki-2+ki-1]/2

(6)

从而

(7)

可见全程曲线各节点参数si及hi，取决于各节点的二阶导数值ki(i=1,2,…,n)和初值h0,s0,k0。

于是，在预知初值h0,s0,k0的前提下，确定各节点上的ki值可推全程航迹，由h0,s0,k0递推而来的节点参数保证了全程曲线的光滑性。

1.2 最优航迹建模

在执行地形跟随飞行任务中，通常希望尽量贴近地形飞行，以充分利用地形掩蔽。因此，最优航迹就是航路点与地形点之间的高度差尽可能小，同时满足飞行安全和飞行器机动约束条件。定义性能指标函数J度量航迹与地形轮廓的贴合程度，即

(8)

式中：qi≥0,li≥0,qi+li>0；ei表示航路点与理想高度的误差ei=hi-Ti-H0，hi为航迹高程，Ti为地形高程，H0是设定的安全高程。

地形跟随的航迹不仅要满足式(9)高程偏差要求，还要满足飞行器航迹倾斜角θ与法向加速度aN的限制，即

(9)

航迹倾斜角的正切即航迹曲线的斜率，因此航迹倾斜角限制可以转化为航迹曲线的斜率约束，即

smin≤s≤smax

(10)

式中：smin=tanθmin;smax=tanθmax。

在给定飞行速度V的条件下，法向加速度与航迹曲率K满足关系K=aN/V2，同时，曲率K与其二阶导数k满足K=k|cos3θ|，在航迹倾斜角较小时曲率值略等于二阶导数为

kmin≤k≤kmax

(11)

式中：kmin=aNmin/V2；kmax=aNmax/V2。

于是，在地形区间(L0,Ln)上地形跟随最优航迹模型可以完整描述为：在式(9)～式(11)约束下，使式(8)性能指标函数最小的航迹。

根据上节推导，式(9)约束可以写成

(12)

式(10)约束可以写成

(13)

定义中间变量xi=Δ2(ki-kmin)，表示各节点的二阶导数值与极值关系，并整理成向量形式，即

x=Δ2(k′-kminη)

(14)

(15)

(16)

式(8)度量函数写成矩阵向量形式为

(17)

式中，

(18)

根据推导，误差向量e=Dx-d，式(17)等效为

(19)

略去常数项，地形跟随最优航迹规划问题转化为求解标准二次型的数学规划问题，即

(20)

s.t.Ax≥bx≥0

式中:R=DTQD；gT=lTD-dTQD;A=(DF-F-I)T；b=(dfNfpkδη)T。

2 矢量场法航迹跟踪

矢量场航迹跟踪分为建立矢量场模型和计算控制量使飞行器沿矢量场回到期望路径两步。

2.1 问题描述

对于地形跟随任务来说，只考虑飞行器的纵垂质心运动，保持飞行器速度恒定。以法向过载为输入的固定翼飞行器纵垂质心运动模型为

(21)

式中：p=(x′h)T是飞行器位置;γ为航迹倾斜角；n为铅垂面法向过载。

2.2 矢量场制导算法

航迹规划算法所得最优航迹为期望位置的航路点，各点之间路径为直线段，如图1所示，黑色实线WiWi+1为期望路段，路段方向角γL，构造如图矢量场，γd为矢量场中点与航路段的夹角，γd表示期望的飞行方向。

图1 直线矢量场

对于飞行平面一点p，ep表示p到期望路段的垂直距离，矢量场的构造满足以下3个条件：1) 当ep→∞时，矢量以角度γ∞指向路径；2) 当ep=0时，矢量与路径方向相同；3) 当ep∈(0,∞)时，矢量方向连续变化。

根据上述构造条件，构造如下矢量场[13]

(22)

式中：γ∞与kL>0为矢量场参数；γ∞决定指向路径的角度；kL影响矢量场从γ∞到0的变化率。

矢量场所给的航迹角指令不仅与矢量场有关，还取决于航路段的方向角γL，因此制导指令为

(23)

2.3 可行矢量场

根据上述定义推导满足矢量场可行性条件。由式(22)矢量场可得γd的时间导数为

(24)

当飞行器沿矢量场飞行时有γ-γL=γd，即

(25)

(26)

当γ∞在(0,π/2]时，0≤(2γ∞/π)arctan(kLep)<π/2，式(26)可写为

(2γ∞/π)arctankLep=arctan (γ∞/kLπep)。

(27)

矢量场的可行性保证了无论初始距离ep有多大，只要飞行器的航迹角已与矢量场方向对齐，飞行器就可沿着式(22)矢量场确定的方向飞行。根据定义，若矢量场可行，则必须满足

(28)

综上，在飞行器速度恒定的情况下，可以得出调整矢量场参数以满足可行性条件的方法。

1) 确定γ∞。由于γ∞∈(0,π/2]，为计算方便，γ∞可取为π的倍数，如π/3。

2) 将γ∞代入式(27)，求出ep 0(kL)。

3) 将ep 0(kL)代入式(25)求出|f(ep 0)|，根据式(28)可行性条件算出kL。

图2 矢量场角度变化率

3 仿真验证

3.1 最优航迹规划

为验证最优航迹规划算法的有效性，本文取目标区域地形二维剖面离散数据，全程50 km，步长50 m，结果如图3所示。图4为航路点与地形相对高度，最小值为43 m，无撞地风险，均值约为68.8 m。

图3 地形跟随最优航迹

图4 最优航迹相对高度

图5进一步验证规划航迹的机动能力的限制。航迹曲线正向坡度最大值为13.38°，负向坡度最小值为-9.754°，均在预设的飞行航迹角极限值[-10°,15°]范围内，过载最大为3.997g，最小0.5g，也在预设过载限制[0.2g,4.8g]内。

图5 坡度与过载

3.2 跟踪结果

f(ep)max=f(ep 0)≈0.385VkL。

(29)

跟踪结果如图6所示，飞行器持续紧密地跟踪参考航迹。图7为跟踪误差ep，最大约10 m。航迹角控制器的控制量如图8所示，图8中蓝色实线和虚线分别为过载上、下限，飞行器全程过载均未超限。

图6 航迹跟踪结果

图7 跟踪误差

图8 过载与航迹倾斜角速率

4 结束语

为实现飞行器地形跟随任务，本文设计了一种固定翼飞行器地形跟随实现方案。首先采用一种基于三次样条函数的最优规划算法，直接生成光顺可飞的参考航迹，然后采用矢量场法推导制导律，给出矢量场全局可行条件和参数设计规则，使得所构造的矢量场符合飞行器机动能力约束。对真实地形数据进行仿真飞行，所得结果满足预定要求，飞行器完成了地形跟随任务，表明了本文所研究方案及算法的有效性。