基于路径自主规划的无人机四维战术轨迹跟踪

陆冠华，夏 洁,2，周 锐,2

（1. 北京航空航天大学自动化科学与电气工程学院，北京，100191；2. 北京航空航天大学飞行器控制一体化技术重点实验室，北京，100191）

0 引 言

无人作战飞机相较于有人飞机，具有成本低、战场生存能力强、大过载可用以及损失无人员伤亡的优点[1]。多无人机协同执行打击可以突破单机在感知与执行能力方面的限制，能够更好、更快完成计划打击任务[2]。随着通信技术的发展，“网络中心战”模式将地理上分散的作战单元实现信息共享，进行一体化指挥并进行协同作战，更有效地对敌方实施突然、精确和同时打击[3,4]。类比于飞航导弹协同作战，无人机协同作战的种类也可分为空间协同、时间协同、功能协同和平台协同[5]。在实际作战当中同时使用多种协同方式，以更好实现战术目的。无人机空间协同指多架无人机在空间位置上相互配合对战术目标进行多角度攻击；时间协同是指多架无人机从不同基地或待命空域出发，同时按给定战术轨迹到达攻击准备位置。

四维战术轨迹跟踪是对结合空间协同与时间协同的规划轨迹进行跟踪的技术，同时完成对三维轨迹的精确跟踪和到达时间的约束，最终使多架无人机能够精确执行同时到达战位、编队形成、自主集结等任务，并可使机群在不同方位的各个高度对目标同时进行突防和超过其同时防御能力的立体攻击，以提高打击的致命性，对战术成功实施具有重要意义[6]，为“网络中心战”提供有效实现手段。

在无人机执行四维轨迹规划时，会不可避免的遇到轨迹规划或时间约束超出无人机自身飞行能力的情况，因此同时需要无人机对飞行轨迹进行自主调节与重规划，以满足整体战术要求。

1 三维轨迹跟踪控制器

四维轨迹跟踪问题的研究，需要首先实现较为精确的三维轨迹跟踪作为基础，之后对无人机跟踪航路点的到达时间进行约束，通过加入带有时间反馈的速度控制器和轨迹重规划，使无人机能准确地在规定时刻到达规定地点，实现四维轨迹跟踪。三维轨迹跟踪控制器的设计目的是：使飞机产生必须的加速度来使得飞机的位置、速度等矢量跟踪预先给定的期望轨迹曲线。鉴于目前尚无可以投入实际应用且被证明更有效的制导控制方法，本文使用比例和比例积分控制作为基本的三维轨迹跟踪方法，结合已公开且已被研究用于无人空战飞机计划[7]的F-16战机模型对四维轨迹跟踪研究和试验环境进行搭建。

由于无需考虑驾驶员承受能力，无人作战飞机相较有人驾驶战机可以实现更长时间、更大过载的运动，机动过载则可以提高到20 g，且不受限制的改变飞行姿态。因此本文采用更加激进的纵向和横航向稳定回路的设计，以侧重增加机动性。

试验飞机基本控制器包括高度保持器、速度保持器和横航向航迹跟踪控制器3部分。

1.1 高度保持器

飞机飞行过程中，需要高度保持器跟踪飞机飞行高度指令。采用比例控制，以高度差作为输入生成法向过载指令。经过不断试验调整控制参数使得响应满足快速性与稳定性要求。高度控制器结构如图1所示。

图1 高度控制器结构Fig.1 Height Controller

1.2 速度控制器

速度控制器输入为期望速度，输出油门控制指令。速度控制器采用比例积分控制。飞机高度改变时速度会同时发生变化，但就本文适用的无人机战术轨迹跟踪问题，由于无人机或飞机与导弹不同，巡航速度不高，加入纵向速度反馈作用更显得不明显。采取传统的速度控制即可满足需求。图2为速度控制器结构。

图2 速度控制器结构示意Fig.2 Speed Controler

1.3 横航向轨迹跟踪控制器

横向轨迹跟踪控制器控制无人机贴合航线运动，本文采用带侧偏距与航向角偏差反馈的横航向轨迹跟踪方法，保证了二维水平面内轨迹跟踪精度，同时减小震荡。

输入无人机的航路指令通常以航路点的形式传入无人机机载计算机中，是一系列离散点。按照传统的侧偏距控制原理，通过实时计算无人机自身位置与规划点连线所形成的航迹之间的距离得到无人机当前时刻的侧偏距离，并以此作为控制量引导无人机逐渐向航迹线靠近，最终抵达下一航路点。设前一航路点，后一航路点，当前点坐标。侧偏距计算方法为：设直线方程则在地理坐标系有：

根据点到直线距离，以右偏为正的有向侧偏距为

航向角偏差是指速度方向与当前轨迹线前方向夹角，设定速度右偏时方向为负，即轨迹正方向向量需顺时针旋转才能与空速方向重合时为负。如图3所示，当飞机位置与轨迹线存在较大侧偏距时，飞机朝轨迹线方向改变航向，航向角偏差增大，通过反馈抑制飞机航向朝垂直于轨迹线方向变化的趋势。与单纯的侧偏距控制相比，有效减少飞机围绕轨迹线的反复震荡。其对比如图4所示。

图3 带侧偏距与航向角偏差反馈的横航向轨迹跟踪Fig.3 Lateral Trajectory Tracking with Lateral Offset and Heading Deviation Feedback

图4 跟踪效果对比Fig.4 Comparison of Tracking Effect

续图4

1.4 无人机三维航路点轨迹自主跟踪改进方案

当战术轨迹规划的目的是战术动作引导而非必须完全经过导航点的侦察类任务时，可以对无人机飞行进行优化，使其转弯更平顺，减少如图4所示跟踪情况中经过大转角时与规划线偏离过大的问题。在相邻航迹线夹角为2δ 的情况下，其当前状态（按路线 1行进）和目标状态（按路线 2行进）如图5所示。

图5 轨迹平滑处理Fig.5 Trajectory Smoothing

从图 5中可以看出，如果能提前将跟踪航路切换到下两个航路点连线所形成的跟踪轨迹线，就可以有效控制无人机的飞行位置范围，并且以主动制造小偏离量的方法避免转弯时造成的大偏离量。相比于使用重新规划一系列航路点或继续在之后进行多项式插值法的圆滑处理[8]，该方法对无人机机载计算机造成的计算量负担小得多。只需确定一个提前量插值标点库或提前量函数，就可以完全实现圆滑转弯。

可知此时式中转弯半径r与速度V成正比。据图6所示几何关系推断，在其他条件均不变的简单物理场景中，转弯提前量可由几何计算和三角函数变换表示：

式中 λ为转弯角度。

图6 理想情况下转弯半径与转弯角的几何关系Fig.6 The Geometric Relationship between Turning Radius and Turning Angle under Ideal Condition

但由于飞机运动和所受的气动力，在不同速度的飞行过程中是非线性的，而且虽然在性质和趋势上的相近，但由于气动力非线性因素和多输入控制，简单的几何关系已经不能精确表现所需转弯提前量与飞行速度、转弯角度之间的关系。如图6所示，如果出现转角规划过小时，会出现转弯过早的问题。以此为启发，构造一个关于速度V与转弯角度λ的含参数的方程，在一定量仿真数据支撑下，运用多元线性回归算法拟合参数。

首先根据式（4）求得函数对象的基本形态，利用泰勒展开，考虑到所需大概精度和计算量，仅展开到三次幂：

故可以构造对象函数：

式中 ,,,mnpq分别为多项式系数。

将等式右边展开成多项式：

b）能到达最接近 P2的位置，即

初步拟合结果如图7所示。

由于本文所设计模型不具有超声速能力和设计，因此对于300 m/s的速度而言是一种留有余度的设计，且为了不过多影响中速特性，未过多取点。另外当规划转角过大时，应当舍弃对外方向偏离量的限制，以平衡对“尽量接近目标点”的要求。对于小角度转弯，只需较少提前量即可达到要求，且造成的侧向偏离一般较小，相对精度较低。经过大量试验和评估，拟合所使用的数据不考虑转角大于 135°和小于 45°的情况，且实验证明在任何角度下该方法均能平衡“接近目标点”和“减小偏离量”两个相互冲突的需求。

图7 初步拟合结果Fig.7 Preliminary Fitting Results

由于转弯提前时，侧偏距反馈控制对输出的作用与实际期望转弯方向相反，即当飞机将要右转弯时，实际上飞机处于下一引导线的右侧，而侧偏距反馈将使飞机左转。而随着航向角的偏差越来越小，需要侧偏距反馈使飞机向引导线靠拢。故应使侧偏距的反馈系数随航向角偏差的大小动态调整。当航向偏差角较大时，适当以比例减小侧偏距反馈的比例系数，充分利用飞机转弯性能，兼顾快速转向与精确贴合能力。改进后的侧向跟踪回路如图8所示。

图8 改进侧向跟踪回路Fig.8 Improved Lateral Tracking Loop

在实际飞行当中，飞机会受到气流等各种影响。相比于计算转弯半径之后控制飞机操纵面达到圆轨道转弯的方法，该方法利用飞机自身的制导能力进行近似圆弧转弯，切换引导航迹次数少，避免了因外部干扰等引起切换判断失效的可能性，从而使系统鲁棒性更强。且对转角过大的情况，飞机亦不会出现圆弧转弯法中为了满足“不越过轨迹线”需求而过早转弯的情况。执行自主策略后的跟踪效果与对比如图9所示，跟踪侧偏距变化如图10所示。

从多次设置不同状态下无人机自主执行规划轨迹的结果可以看出，使用多元线性回归拟合出的转弯提前量函数，可以在各种转弯角度、速度下有效改善无人机三维轨迹跟踪中的侧向跟踪效果，并大幅降低在转弯过程中相对规划航线的侧向偏离量。使得整个飞行轨迹总体上契合输入的规划轨迹，明显缩短远距离偏离航线的时间和其间经过的距离。为四维轨迹的精确跟踪创造前提条件。

图9 效果对比Fig.9 Effect Comparison

图10 跟踪侧偏距Fig.10 Tracking Side Offset

2 无人机四维战术轨迹自主跟踪方案

本文所设计的四维战术轨迹自主跟踪方案，首先采取速度控制，使得无人机可以在一定程度内控制到达航路点的时间。但由于飞机速度控制能力有限，故同时设计了无人机自主轨迹部分重规划策略，以将时间控制在飞机速度控制可以实现的范围内。二者协同完成四维轨迹跟踪需求。

2.1 速度控制回路

在实现1.2节所述输入为期望速度，输出油门控制指令的速度控制器后，用于四维轨迹跟踪的速度控制器引入当前航路段剩余时间leftt与当前航路段剩余距离反馈进行实时速度控制：

式中cV为控制速度输出；ct为当前时刻；prevt 为到达上一航路点时刻。由于在自主跟踪三维轨迹过程中更换航路点时刻提前，导致更换航路点的时刻要提前于最接近被更换航路点的时刻。所以选取无人机到达转弯角分线的时刻为近似到达时刻，考虑到系统采样周期，角分线计算以到达距前后两条轨迹线的正方向有向线段的同向（左侧或右侧）距离之差在50 m以内为标准。角分线法更换航路点原理如图11所示。

图11 切换航路点示意Fig.11 Switch Route Point

但图 11表明在转弯角度较大或速度较大的情况下，飞机实际飞过的距离将比预计规划轨迹的距离要短得多。使得飞抵导航点的时间控制偏差也较大。需要对每段四维航迹的速度控制设计改进控制器，增加转弯角度、当前飞行速度输入，综合计算控制速度输出，达到抵消转弯角度和飞行速度对时间维的控制效果影响的目标。

根据几何计算可以大致明确并获得构造距离补偿函数基本方向。构造一个关于速度V与转弯角度λ的含参数的方程。若无人机近似呈半径为r的圆弧运动，并恰好与两条轨迹线相切，则有如图12的几何关系。

图12 距离补偿函数构造基本原理Fig.12 Basic Principle of Constructing Distance Compensation Function

由此关系可以得到过点前规划距离与实际路径距离的差值函数为

式中

以此作为补偿后，时间维控制误差与不使用距离补偿函数对计算时间所用距离进行补偿对比，情况具有明显改善，且时间误差小于1 s。飞机转弯的实际路径由于气流等影响，有一定不确定性，故不应再使用过度拟合的算法追求理想状态下的仿真结果更加精确。使用图 13a的轨迹规划作为试验轨迹，得到使用与不使用距离补偿的控制偏差如图13b所示。

图13 使用距离补偿函数后的效果对比Fig.13 Comparison of Effect after Using Distance Compensation Function

图14 速度控制改进回路Fig.14 Speed Control Improved Circuit

2.2 路径自主重规划方案

采取速度控制，可以使无人机通过改变自身飞行速度，在一定范围内将到达每段航路点的时间控制在十分接近规划时间的范围内。由于飞机的最高速度有限，本文不研究规划时间过短的情况。而另一方面，飞机有其最低维持平飞的飞行速度。当无人机任务规划时临时根据协同作战的战术需要，要求其在某点附近进行等待，或者需要其与其他作战单元配合，经过较长时间到达下一航迹点时，即使使用其最低维持平飞的飞行速度也无法完成。故本文设计了无人机自主轨迹部分重规划策略，通过规划离散路径点，配合上文所述的改进跟踪控制器，实现连续的盘旋与S形摆动路径规划相组合的方式，在轨迹线附近进行绕飞，消耗多余的等待时间，并将剩余时间控制在飞机利用其自身速度控制可以实现的范围内。二者协同完成四维轨迹跟踪需求，且导引路径切换简便，简化了从原路径到重规划路径再到原路径的切换过程。

2.2.1 自主盘旋

飞机在水平面内作等速圆周飞行，称为盘旋。飞机盘旋最常在民航当中见到，客机为等待进近时机或等待恶劣天气状况、航空管制放行等情况时，会在机场附近指定位置进行盘旋等待。在作战当中，攻击机也会盘旋等待其他攻击机到达指定区域进行编队或者等待其他作战单位进行站位，从而达到同时发起攻击的目的。根据战术需要，无人机可能会被指令在一个目标点附近进行盘旋，等待与其他作战单位同时进入攻击位置。此时需要无人机自主进行盘旋。无人机四维轨迹跟踪技术设计自主盘旋需要达成以下4个目标：

a）飞机能在轨迹附近做位置、速度可控，稳定的连续盘旋；

b）盘旋过程中不偏离盘旋位置；

c）能从跟踪当前轨迹平顺进入盘旋状态、平顺改出盘旋状态继续跟踪轨迹规划线；

d）盘旋时间可控，为四维轨迹跟踪提供手段。在前文设计的无人机三维轨迹跟踪改进系统的控制下，无人机自动将所有在地理坐标系上的轨迹中所有折线，全部执行为近似内切弧线轨迹。在这个基础上，如果需要无人机在某处盘旋，则只需知道无人机在当前速度下的最小的转弯半径minR ，根据几何关系，在地面坐标系确定出一个与当前航路点轨迹规划线相切的圆作为盘旋轨迹。之后确定该圆轨迹的外切正四边形，该四边形的一边与当前航路点轨迹规划线相重合，边长为

而此正四边形的4个顶点，即为无人机接收到执行盘旋等待指令时需要规划的航路点如图 15所示。

图15 基于航路点规划的无人机自主盘旋原理Fig.15 Principle of UAV Routing Based on Route Point Planning

在实际跟踪任务当中，由于使用盘旋等待的情况下，飞机不需要进行较高速的飞行，因此将飞机的盘旋等待速度设为 200 m/s，在此条件下经多次仿真实验，选定飞机盘旋半径约3600 m。重复循环跟踪这4个规划点，无人机将沿近似圆轨迹做盘旋运动。试验取圈数为6，实现效果如图16所示。

与传统控制飞机协调转弯的盘旋方法相比，利用轨迹规划实现的无人机盘旋的优势在于：

a）规划层与跟踪控制层分离，盘旋航路点规划完毕后，无需继续计算，无人机轨迹自主跟踪控制系统自动按原有跟踪方法进行轨迹跟踪；

b）同时避免了模态切换带来的一系列突变；

c）受到外部干扰有少许偏离时，无人机原有的制导控制律，能将飞机拉回正常轨迹；

d）每完成一周盘旋的时间稳定；

e）进入和改出盘旋简单，无人机的速度方向与原规划轨迹线方向重合，且无侧偏距。

图16 盘旋试验结果Fig.16 Results of Circling Test

综上，基于轨迹规划的自主盘旋方法，实现了可控稳定盘旋。增强了轨迹跟踪系统的鲁棒性和可靠性。

2.2.2 S形机动

若设飞机盘旋一周需要Loopt ，飞机通过推理调节方法在一段有限长度中的时间调节能力为Thrustt ，则当需调整时间时，需要一种可调轨迹消耗二者调节范围之间的时间。因此设计一种基于轨迹规划的S形机动策略。设计要求有：

a）能从前轨迹平顺进入机动状态、平顺改出机动状态继续跟踪轨迹规划线；

b）因机动产生的轨迹偏离量不超过盘旋机动；c）消耗时间可严格控制。

利用已有的改进的三维轨迹跟踪系统，并进行多种方案设计与大量试验，根据以上3项目标进行对比，最终选定设计方案，航迹点规划方法如图17所示。

图17 S形机动规划方法示意Fig.17 S Shape Maneuver Planning Method

当无人机航向与侧偏距满足要求准备进入S机动时，规划导引点满足：

飞机推力控制有一定时间控制能力，加上气流扰动影响，因此对机动消耗时间的精度要求实际上并不甚高。因此采用多元线性拟合方法确定规划距离参数与消耗时间 ts的关系。经过试验构造拟合对象函数：

式中 a，b，c，d为待拟合参数。拟合结果如图18所示。

图18 拟合结果Fig.18 Fitting Result

这种S形机动的方法，特性代价比高，在规划轨迹方向最短距离完成消耗多余时间的目标，尽可能降低对路径规划长度的要求。且规划计算简单，时间差通过复杂函数反映到单一变量，易于控制。与盘旋策略相同，其对外部干扰抵抗能力强。

2.2.3 方案执行流程

综合以上方法的路径自主重规划方案执行流程如图19所示。

图19 四维轨迹自主跟踪系统运行主要流程Fig.19 Process of 4D Trajectory Autonomous Tracking System

3 仿真验证与分析

为了充分检验系统的可用性，选取一条较为复杂且轨迹条件苛刻、转弯大、航路点控制时间随机的四维战术轨迹进行试验，检验其在各种条件下的实现效果。给定四维规划列于表1。

表1 试验用规划数据Tab.1 Planning Data for Test

选取仿真初始条件：无人机初始速度0V=240 m/s，高度2500 m。按上述战术轨迹飞行，跟踪效果二维图如图20所示。

图20 跟踪效果二维图Fig.20 2D Chart of Tracking Effect

三维轨迹如图21所示。

图21 跟踪效果三维图Fig.21 3D Chart of Tracking Effect

高度、速度响应曲线如图22所示，各航路点时间误差如图23所示。

图22 纵向高度、速度响应曲线Fig.22 Longitudinal Height and Velocity Response Curve

图23 时间维控制误差Fig.23 Time Control Error

仿真结果表明，无人机可以满足三维轨迹精确跟踪要求，在各条件下适应性良好。在规定到达时间超出飞机速度控制范围时，通过自行规划航迹满足在预定时间到达四维航路点，时间误差可以控制在 1 s以内。控制方法和轨迹跟踪方案可以满足无人机编队集结和全方位协同攻击需求。

4 结 论

本文针对四维战术轨迹跟踪需求，设计实现了横纵向三维轨迹跟踪控制器，并对其进行有针对的改进。使得整个飞行轨迹总体上契合输入的规划轨迹，明显缩短远距离偏离航线的时间和其间经过的距离。为四维轨迹的精确跟踪创造前提条件。基于此提出无人机四维战术轨迹自主跟踪方案，设计了无人机自主轨迹部分重规划策略，通过规划离散路径点，实现连续的盘旋与S形摆动路径规划相组合的方式，将剩余时间控制在在飞机利用其自身速度控制可以实现的范围内，完成四维轨迹跟踪需求。并进行了仿真验证。设计中考虑到了外界干扰等因素，保证系统具有较强鲁棒性。为多机协同多方位立体打击的战术提供有效手段。对飞航导弹协同攻击有一定参考价值。