基于矢量场法的无人机三维路径跟踪控制

颜宗渊，王道波，王博航，周晨昶，姜 燕

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

0 引言

无人机路径跟踪问题可以由3个任务来描述：跟踪移动目标、跟踪航点路径以及在固定航点处运动[1]。路径跟踪最新发展主要可以分为3类：基于动力学和运动学模型的算法[2-3]、基于虚拟目标点的算法[4]和基于矢量场的算法[5]。尽管基于动力学和运动学模型的路径跟踪算法可以保证足够的跟踪性能和稳定性，但是其制导命令依赖于模型并且复杂，从而增加了计算复杂度[6]。虚拟目标点制导算法源自导弹制导系统中使用的视线制导方法，现已发展为非线性制导律[4,7-8]，但是，与其他制导律方法相比，该方法引入了更多的简便性和更少的控制工作，因为它没有考虑到无人机的运动学和动力学约束，这些约束在实际应用中会降低制导律的性能。基于矢量场的路径跟踪算法的基本思想是要构造一个理想的航向场将无人机移动到路径上，因此，制导律的计算取决于路径的类型[9]。直线和圆路径是最常见的路径，直线和圆也可以组合构造更复杂的路径。研究发现基于矢量场的路径跟踪算法具有较大优势，可以不依赖模型，且计算复杂度较小，可以加入无人机的运动学和动力学约束，跟踪性能好[10]。本文将二维矢量场法推广到三维空间，实现固定翼无人机的三维路径跟踪控制。

针对航段切换问题，采用基于圆角几何的切换准则，根据无人机是否通过半平面作为切换策略，保证无人机可以平滑过渡2条相邻航段。仿真结果表明，本文提出的三维路径跟踪算法具有极佳的跟踪性能。

1 无人机运动学模型

本文主要研究外回路产生的制导指令使无人机跟随空间三维路径。建立无人机的运动学模型为

(1)

由于风对无人机影响的效应还未研究清楚，一般将风的影响作为扰动输入，由无人机的飞行控制系统实时处理。因此在进行路径跟踪时，通常不会将风的影响考虑进去，认为UAV的空速等于地速，航向角ψ认为等于航线角(假设侧滑角β等于0)[9]。图1展示了无人机空速V、航向角ψ和航迹角γ。

图1 无人机运动模型图示

2 矢量场法

制导律将采用在文献[11]提出的矢量场法。需要跟踪的在三维空间中的路径是2个二维流形的交线，其中二维流形分别由α1(r)=0和α2(r)=0定义，α1和α2为有界的二阶偏微分，r∈R3。一个基本的假设是交线给出的路径是连通的，并且是一维的。定义函数为

(2)

给出V(r)的一阶导数为

(3)

式(3)存在的一个问题是计算得到的u′或许不会等于无人机的空速V，因此,u′需要标准化为

(4)

当u′被标准化为式(4)的值时，由文献[11]证明得到的稳定性依然是有效的。

将无人机在惯性坐标系的速度分量与式(4)得到的速度u=(u1,u2,u3)T进行等价得到

(5)

对式(5)进行求解可以得到期望的航迹γc和期望的航向角ψd，表达式为

(6)

饱和函数被定义为

(7)

假设通过协调转弯方程对内环横向动力学方程进行了精确建模，则可以从式(8)获得产生理想转弯性能的滚转角命令值，即

(8)

kφ为一个正常数。

2.1 直线路径

直线可以由直线的方向和直线上的一个点描述。让cL=(cn,ce,cd)T表示直线上任意的一个点，直线的方向可以由在东北坐标系下的相对正北方向的航向角ψL和相对水平面的航迹角γL表示。因此，直线的方向可以表示为

(9)

qL为一个指向期望直线的方向单位向量。这条直线可以表示为

(10)

垂直于由qL定义的纵向平面的单位向量定义为

(11)

相似地，垂直于由qL定义的横向平面的单位向量为

(12)

因此，pline由以下定义的平面的交线给定

(13)

(14)

因此，在标准化前，期望的速度矢量可以表示为

(15)

图2表示了qL、cL以及由αlon(r)=0和αlat(r)=0定义的平面。

图2 由2个平面定义的空间直线

2.2 螺旋线路径

时间参数化的螺旋路径为

(16)

(17)

Rh为半径；λ=+1为螺旋线顺时针旋转；λh=-1为螺旋线逆时针旋转；γh为沿着螺旋线的航迹角。

为了找到2个面可以定义螺旋线路径，式(16)中的时间参数需要消除，式(16)可以表示为

(18)

除此之外，将式(16)的东向分量除以北向分量可以得到

(19)

求解t，并将其代入到式(16)中的第3个表达式中得到

(20)

除以Rh将这些等式标准化可以得到

(21)

(22)

用除以Rh去进行归一化可以使得控制策略的增益不会随着轨道的大小变化而变化。αcyl和αpl的梯度可以表示为

(23)

(24)

在标准化前，期望的速度矢量可以表示为

(25)

3 三维路径跟踪算法

3.1 基于圆角几何的切换策略

图3 圆角几何策略

三维空间里2条线段的切点的计算如下所述：

a.将空间中相邻的3个航点Wi-1、Wi和Wi+1投影到二维平面，得到wi-1、wi和wi+1。

b.通过下式计算出二维平面内表示相邻航段方向的向量：

(26)

(27)

c.通过图3右侧的表达式计算得到a、b、c点。

d.a、b、c是A、B、C在二维平面的投影点，且A、B在航段上，由此可以计算得出A、B切点;C是螺旋线的圆心，起始高度为A切点高度，由此得到C的坐标。

3.2 切换策略的算法步骤

在切换策略算法中，pathstate是一个切换标志，如果pathstate=1，则代表无人机将沿着直线路径运动；pathstate=2，则代表无人机将沿着螺旋线运动；ψpath代表直线路径的航向，对于螺旋线来说，代表其起始点的航向；γpath是所走路径相对水平面的夹角；对于直线路径，λpath=0、λpath=1代表螺旋线是顺时针方向，λpath=-1代表螺旋线是逆时针方向；ρ是螺旋线的半径；pathflag是一个路径标志，输出用于下一层路径跟踪算法，pathflag=1代表无人机将跟随直线路径，pathflag=2代表无人机将跟随螺旋线路径；C是螺旋线路径的圆心位置坐标，对于直线，是起始航点的坐标。

输入是航点数据pathwaypoit={w1,w2,…,wn}、无人机的位置p=(pn,pe,pd)和最大航迹角γmax，根据无人机的位置实时更新输出值，算法流程如图4所示。

图4 基于圆角几何的切换策略算法流程

切换策略算法初始化设置pathstate=1，计算出由3个航点构成的2条相邻航段方向，采用螺旋线拟合相邻航段，因此，形成了2条直线路径和1条螺旋线路径。无人机先沿着直线路径运动，计算得到直线路径参数，用于输出给路径跟踪模块，当到达半平面H1，无人机开始处于航段过渡阶段，pathstate=2，将沿着螺旋线路径运动，计算得到螺旋线路径参数。当无人机到达半平面H2，说明航段过渡阶段已经运动完，pathstate=1，无人机将开始沿着直线路径运动，重复以上步骤，计算路径跟随所需参数。

3.3 路径跟踪算法步骤

图5 基于矢量场法的路径跟踪算法流程

路径跟踪算法根据pathflag参数确定将要跟随路径的参数。当pathflag=1时，无人机将要跟随直线路径，采用式(15)计算出跟随直线路径所需的速度矢量u′=(u1,u2,u3)T；当pathflag=2时，无人机将要跟随螺旋线路径，采用式(25)计算出跟随螺旋线路径所需的速度矢量u′=(u1,u2,u3)T。由得到的速度矢量u′，计算得到期望的空速Vc、期望的航迹角γc、期望的航向角ψc和期望的滚转角φc。将这些期望参数输入给无人机控制回路中，从而实现无人机的路径跟踪。

4 仿真实验

对本文提出的制导方案进行仿真验证，以评估算法的性能和鲁棒性，无人机的详细模型详见文献[9]，使用硬件是个人计算机Intel Core i7-8750H CPU @2.20 GHz 16 GB RAM。将无人机的起始点和初始航向分别设置为不同的初始值，对直线、螺旋线路径和由此组合的复杂路径进行跟踪，进行多次仿真实验，验证算法的跟踪性能。

4.1 直线路径

参数kφ会影响无人机在横向平面的收敛性能和跟踪表现，kφ的取值范围需在0.2～0.6之间，如果kφ大于0.7，无人机跟随路径可能会出现振荡现象，如果小于0.2，跟踪效果比较差，本文算法将其取为0.3。矩阵K1=diag([k11k22k33])是一个对角矩阵；k11和k22会影响无人机在横向平面上对路径的收敛性能，k33会对其在纵向平面的收敛性能。k11和k22的取值范围在5～20之间，本文取9；k33的取值范围在5～35之间，过大会使得无人机的俯仰角出现振荡现象，本文取15；K2会影响无人机对路径的跟踪性能，取值范围在140～180之间，本文取150。改变无人机的起始点和初始航向，对2个航点直线轨迹进行跟踪，仿真结果如图6所示，可以看到无人机能够快速跟踪设定的直线，而且跟踪误差很小。

图6 直线路径跟踪结果

4.2 螺旋线路径

kφ、K1、K2这些参数无人机对螺旋线路径跟踪的影响和直线路径类似，将kφ取值为1.5，k11和k22都取值为20，k33取值为25，K2取值为150。改变无人机的起始点和初始航向，对螺旋线路径进行跟踪，仿真结果如图7所示。初始时刻无人机对于螺旋线跟踪误差相对较大，这是因为无人机的初始航向与预设路径的航向偏差较大，且距离相对较远，但无人机能够迅速消除误差，跟踪螺旋线路径，而且之后跟踪误差很小。

图7 螺旋线路径跟踪结果

4.3 组合复杂路径

在本节中，无人机要跟踪根据航点设定的一系列轨迹，在2个航段的交接处，无人机将会对飞行螺旋线进行平滑过渡，仿真结果如图8所示。从图8可以看到，无人机很好地跟踪所有路径，且在切换点处没有比较大的超调，跟踪误差极小。期望俯仰角、滚转角、空速与实际对比的结果和误差值的结果如图9和图10所示。在各过渡位置处，无人机的俯仰角和期望滚转角会出现波动，因为无人机需要调整姿态来跟踪下一段路径；期望值与实际值在过渡位置存在一定偏差，这是因为无人机模型存在动力学和运动学约束，导致在过渡点实际值无法等于期望值，不过误差很快就会趋于0，这也验证了本文提出算法的可行性。

图8 组合路径跟踪三维结果

图9 期望俯仰角、滚转角、空速与实际对比

图10 期望与实际俯仰角、滚转角、空速误差值

5 结束语

本文提出了一种基于矢量场法的三维航点制导算法，只需要任意给定合理的空间航点，算法将依据空间航点顺序将航点连接成各航段，使用直线和螺旋线对航段进行近似拟合得到适合无人机飞行的路径。根据路径类型，通过矢量场法计算得到无人机运动的期望空速、期望航向角和期望航迹角，由期望的航向角和航迹角信息可以得到期望滚转角和俯仰角信息，用于无人机的纵向和横向通道的控制，将无人机引导到预定的轨迹上。此外，采用一种基于圆角几何的切换策略，实现各航段的平滑过渡，使用螺旋线拟合相邻航段，螺旋线仅在切点处与2条线段相交，该切点定义了切换半平面的位置，当无人机通过了切换半平面，无人机会跟踪下一航段，故此方法还可以减少路径长度实现路径优化。采用本文提出的算法进行仿真实验，仿真结果表明无人机可以很好地跟踪直线、螺旋线及组合的复杂路径，验证了算法的有效性。