风场环境下的巡飞弹航迹跟踪运动补偿算法

李增彦， 李小民， 刘秋生

(1.军械工程学院 无人机工程系， 河北 石家庄 050003；2.军械工程学院 弹药工程系， 河北 石家庄 050003)

风场环境下的巡飞弹航迹跟踪运动补偿算法

李增彦1， 李小民1， 刘秋生2

(1.军械工程学院 无人机工程系， 河北 石家庄 050003；2.军械工程学院 弹药工程系， 河北 石家庄 050003)

针对巡飞弹航迹跟踪过程受风速、风向影响的问题，在风场环境下，通过建立运动模型，研究并提出了基于变增益虚拟参考点和系统外回路动态补偿的非线性制导律算法。根据制导律参数中固定距离限制的问题，设计了变增益虚拟参考点并分析了航迹跟踪的稳定性条件，针对系统外回路滞后特性设计了动态反馈补偿因子，同时给出了参数计算方法及运动约束条件。通过数学仿真方法分别对无风及有风条件下的制导律进行验证，考虑到实际飞行试验困难的问题，基于vc++搭建了半实物实时仿真平台，通过硬件在回路仿真，验证了航迹跟踪性能。结果表明：改进制导律可有效克服风干扰、具有较高的跟踪精度且易于工程实现。

兵器科学与技术； 巡飞弹； 航迹跟踪； 非线性制导律； 运动补偿； 半实物仿真

0 引言

巡飞弹作为新型智能化弹药，集无人机技术与弹药技术于一体[1]，无论是侦察还是攻击任务，精确的航迹跟踪是完成飞行任务的必备条件及前提。同时，模拟验证机载控制器及航迹跟踪算法的性能是研制过程中性能考核等急需手段。

巡飞弹沿设定的航点自主跟踪飞行，可节省能源消耗、遂行作战任务，飞行环境中不可避免地存在风干扰[2-3]，其中侧风及顺风干扰对航迹跟踪的影响较为明显。目前已有大量文献对飞行器的航迹跟踪算法进行了相关研究。Sujit等[4]对比分析了实现较易、鲁棒性强的常用航迹跟踪算法，可将其思想总结为5类：虚拟运动目标法[5-6]、视线导引法[7]、线性二次型法[8]、向量场法[9]、虚拟目标距离法[10]。前3类方法的鲁棒性受风扰动影响且存在较大的横向偏差；向量场法横向误差最小，但需调整的参数较多，并存在抖震现象。虚拟目标距离法在算法实现、参数调整及横向偏差方面具有较大优势。Sanghyuk 等利用虚拟目标距离法的思想研究了非线性制导算法并将该算法推广到三维航迹跟踪中[11]，在直线、曲线、圆、跟踪运动目标等任意类型的航迹跟踪中均具有较好的实验结果，但该方法仍存在一定误差，固定距离L的限制导致不同风向条件下鲁棒性较差，从而影响航迹跟踪的稳定性，同时未考虑系统外回路的反馈补偿。

在实验验证环节，与常用的现场调试方法相比，大多数算法对飞行器的数学模型进行了简化，采用边界条件仿真测试进行分析对比。而在飞行器控制及导航算法研究中，半实物仿真方法具有仿真模型精度高、能够快速对控制算法及参数进行更改等优点[12]，尤其针对远程飞行器可以及时地判断设计的可行性、提高开发效率、降低研制周期和研究成本。

本文首先根据巡飞弹受风场影响的问题建立运动模型，针对非线性制导律中固定距离限制及系统外回路滞后特性的问题进行研究及改进，并从算法实现及工程应用角度给出了参数计算方法及运动约束条件；然后通过数学仿真方法对制导律进行验证并结合半实物仿真思想，基于vc++搭建实时仿真平台；最后通过硬件在回路仿真，验证航迹跟踪性能。

1 风场条件下的巡飞弹制导律

1.1 运动模型

风扰动条件下，采用北东地大地坐标系，巡飞弹在空间中的质心运动学方程[2]可描述为

(1)

式中：x、y、z表示大地坐标系下3个坐标分量；va、γ、χ分别为空速、航迹倾角和航迹偏角；vw为飞行环境中风速，并在x、y、z方向进行分解。针对二维水平面的航迹跟随问题，控制巡飞弹航迹倾角γ=0°，则飞行器的高度z恒定，(1)式可简化为

(2)

式中：vg为巡飞弹的对地速度。

1.2 制导律

巡飞弹航迹跟踪控制过程如图1所示。

图1 非线性制导律几何示意图Fig.1 Geometric sketch of nonlinear guidance law

图1中，期望航迹为A至B，当AB为直线时，巡飞弹至AB间距离为d，参考点P至AB距离为dr.L为巡飞弹至参考点的固定距离，其与vg间夹角为η，当飞行过程中滚转角为φ时，总升力为FL，O、R、as分别为向心运动的圆心、半径和加速度。

由图1中几何关系及牛顿第二定律可知：

(3)

式中：m为巡飞弹质量

传统方法在航迹跟踪控制中，通常采用空速va作为速度控制回路的参考量，一般为恒空速控制。当存在风干扰时，飞行器的空速受风速影响，导致滚转角控制量对风速较为敏感，因此采用飞行器对地速度vg作为制导律中速度参考，从而得出如下制导律[10-11]：

(4)

为了避免飞行器在风场环境中失速，采用恒空速控制方法。由(2)式及(4)式分析可知，制导律中滚转角指令同时受vg及参考距离L影响。当L为常量时，飞行过程中风速、风向改变后，vg随之改变，使得制导律中控制量受风场影响，并且制导律中未考虑受控对象的运动特性。

2 改进制导律

为了改进制导律受风干扰及受控对象影响的特性，首先分析变增益参考距离方法及其稳定性，利用运动补偿方法改善控制器滞后问题，并根据制导律特点给出参数计算方法及飞行器运动约束条件。

2.1 变增益法及其稳定性分析

由(4)式中的飞行器制导律可知，当对地速度vg与航线夹角η趋于0°时，滚转角控制量为0°，飞行器能够保持稳定水平飞行状态，即

(5)

(6)

(7)

令C为常量，使得变增益参考距离l∝vg.

为了确定C的取值范围及对控制系统稳定性的影响，将飞行器滚转响应特性看成1阶惯性环节时，制导律控制回路可描述为图2中的结构。

图2 制导律控制回路Fig.2 Guidance law control loop

其特征方程可整理得

(8)

式中：Tr为惯性系统的时间常数。当Tr=0时，认为飞行器滚转响应不存在惯性特性；当Tr>0时，反应了飞行器的滚转惯性，且惯性越大，时间常数越大。

通过特征方程利用根轨迹法对系统稳定性进行分析，选取不同时间常数Tr并令C取值为等差数列{0.5,1,…,10}得到图3特征根趋势。由图3可知：当飞行器滚转过程不存在惯性环节时，全部特征根位于左半平面，系统为稳定状态且不受C的影响；当飞行器受滚转惯性及飞行控制系统作用时，随着C的增大且当C>Tr时系统由不稳定状态变为稳定状态，因此必须保证控制参数中C始终大于Tr使系统稳定。

图3 特征方程根轨迹Fig.3 Root locus of characteristic equation

2.2 制导律反馈补偿

图4 加速度动态反馈补偿Fig.4 Dynamic feedback compensation of acceleration

由于侧风会对飞行器产生横侧向加速度，利用对地速度vg及空速va估计风速角∠ga，当侧风影响较大时，以比例系数K作为反馈增益，δ为分割区间角度，考虑如下区间函数：

(9)

则经过动态反馈补偿的改进制导律为

(10)

2.3 制导律参数计算及运动约束

由图1可知，η为向量vg与向量l之间的夹角，直接计算虚拟目标点坐标较难，采用向量形式对夹角η进行计算。设地速vg及航线AB间夹角η1为

(11)

飞行器到航线AB的垂直距离d为

(12)

则由(12)式可得虚拟距离l与航线AB间夹角η2为

(13)

从而制导律中夹角η可由(11)式～(13)式得出：

(14)

由于固定距离L的限制导致航点切换策略受飞行速度影响，制导律中过大的加速度使滚转角设定值φc超出飞行器姿态回路动态范围。因此采用虚拟距离l=Cvg作为航点切换条件，在转弯半径中加入接近距离延迟量vgΔt并作如下转弯半径及滚转角约束：

(15)

式中：Rmin为飞行器最小转弯半径。

3 数学仿真及结果分析

1)在无风条件下跟踪四边形直线航迹如图5所示。

图5 无风条件下跟踪四边形直线航迹Fig.5 Quadrilateral flight path without wind

由图5中仿真结果可知：如图5(a)所示，在无风条件下，由于参数C为定值且vg不受风的影响，非线性制导律及改进后的制导律航迹基本一致；而图5(b)中横向偏差d表明，加入动态反馈补偿后，控制律可以有效地改善航迹跟踪能力，提升跟踪速度；非线性制导律中虚拟参考距离(同时为航点切换条件)为固定值L，从图5(c)可以看出改进后虚拟距离l成为动态变量，使航点切换时机得到控制及改善；由图5(d)可知，对加速度进行动态反馈补偿后，提高了滚转角指令值的动态范围，航点切换时可达最大角度约束，该约束角度可根据飞行控制系统性能进行设置，而在飞行器接近水平飞行状态时，滚转角指令更加平滑且无严重超调量。

2)通常的风场条件只考虑水平或垂直方向的风向影响，且风速远小于飞行器速度。为验证制导律的抗风性能，设飞行环境中存在常值风vw,x为8 m/s，vw,y为6 m/s，约为0.5vg，在该条件下跟踪四边形直线航迹，如图6所示。

图6 风场条件下跟踪四边形直线航迹Fig.6 Quadrilateral straight flight path with wind

由图6中仿真结果可知：如图6(a)所示，在非线性制导律的作用下当存在常值风的影响时，飞行器严重偏离航线，跟踪精度、速度受风向、风速的影响较大，而飞行航向的改变使得风场对飞行器的影响也在不断变化；从图6(b)中可以看出，改进后制导律的跟踪速度明显优于非线性制导律，加入动态反馈补偿后，在跟踪时的快速性得到小幅度提升且超调量变小；图6(c)则表明，受风向、风速的影响，改进后的制导律能够自适应调整虚拟参考距离及航点切换条件，使得航点跟踪时的抗风能力得到加强，而非线性制导律无法改变风场环境对于飞行器速度的影响；在风干扰环境下，通过加速度动态反馈补偿，从图6(d)中可以看出，进入直线航线过程中飞行器滚转角指令仍然较为平滑且无严重超调量，有利于飞行控制系统对于飞行器的控制。

4 半实物仿真实验验证

4.1 整体框架及实现方式

巡飞弹作为远程飞行器，控制及导航算法研究实验无法直接进行验证。为了实现其控制及导航等研究需求，必须搭建考虑飞行器气动参数摄动，传感器模型等的飞行仿真框架[13-14]，利用可靠的手段完成实验，在验证环节，目前常用方法为基于Matlab/Simulink的纯软件数学仿真和硬件在回路仿真，已有的仿真模式暴露出一些不足，如数字仿真全为模拟数据不具备真实性，仿真模型执行效率低且控制周期不等长，控制算法未在飞控系统中运行等。

为了保证仿真模型运行的实时性，基于VS2010采用多线程方式进行编程，搭建了巡飞弹数学模型，代码执行周期设置为5 ms，并利用Windows RTX硬实时操作系统保证了模型解算的实时性，考虑到飞行器姿态及航迹控制中的调试需求，利用OpenGL实现飞行器状态可视化；为验证控制算法硬件可实现性及改进制导律的实用性，将制导律写入基于STM32F405的飞行控制系统中。

半实物仿真系统框架如图7所示：硬件在回路仿真时，先将飞行控制系统固联于三轴转台内框中并进行航点装订；仿真计算机中运行六自由度仿真模型，仿真数据(姿态角及角速度)通过VMIC反射内存卡发送至转台控制计算机[13](通信协议基于UDP实现)，该数据可直接驱动跟踪精度为角秒级的三轴转台进行姿态跟踪；同时仿真模型中模拟传感器数据(包括全球定位系统(GPS)、空速、加速度、磁强、气压等，在理想飞行器仿真模型的基础上加入传感器误差模型噪声[13-14])通过仿真计算机发送至飞行控制系统，而转台旋转角速率信息可利用飞行控制系统中陀螺仪直接测量，飞行控制系统接收传感器数据并通过制导律中控制参数解算输出飞行器控制舵量，该控制舵量直接发送至仿真计算机，仿真模型接收到控制信号后进行模型的姿态控制，形成闭环回路。

图7 半实物仿真系统框架Fig.7 Framework of HIL system

4.2 含GPS信息参数计算

由于第3节中位置坐标位于平面坐标系，而GPS采集的位置信息属于经纬高(LLA)坐标系，为了计算两点间距离及航向需转换到地心地固(ECEF)坐标系。假设飞行控制系统中两点为A=(λA,φA)，B=(λB,φB)，λ和φ分别代表纬度和经度，利用球面半正矢公式可知AB间存在如下关系：

(16)

式中：Re为地球半径；Δφ、Δλ分别为A、B两点之间的经度、纬度差。又因为Haversine公式hav(·)为

(17)

(18)

利用等距方位投影公式可知向量AB可分解为

(19)

式中：ABe表示AB的水平分量；ABn表示AB的垂直分量。从而得出AB的航向角ψAB为

(20)

最终飞行控制系统制导律中含GPS信息的夹角η可由(18)式和(20)式求得，而加速度反馈中航向角ψ的导数为

(21)

式中：r、q分别为飞行器机体坐标系下运动的偏航角速度和滚转角速度；θ为大地坐标系下运动的俯仰角。从而根据4.2节公式，适用于工程实现的完整制导律公式中滚转角期望值可由GPS信息及航点位置进行计算。

4.3 实验条件及结果

针对某小型巡飞弹进行自动航迹跟随实验，六自由度仿真模型中存在一定范围气动参数摄动值，传感器模型中加入随机白噪声，初始高度500 m，设定巡飞速度20 m/s，风速vw，x、vw，y为-7 m/s，约为0.5va；在半实物仿真实验验证环节，设制导律中参数C=4.33，k=-0.7，|φc|≤50°，任务航点仍采用四航点导航方式，位置见表1，表中h为飞行高度。

表1 航点位置Tab.1 Waypoint position

巡飞弹经过约20 s空中搭载，通过空投方式进入自动任务阶段。分别对改进制导律、L非线性制导律在有风及无风条件下进行仿真实验，参数调整过程中，为了保证与数值仿真条件相对应，设置恒空速控制方式使其速度稳定到20 m/s，设置高度控制器使其高度稳定到200 m. 经过控制器参数调整，仿真结果如图8所示，其中图8(a)中图例适用于图8(b)和图8(c)。

图8 巡飞弹半实物仿真结果Fig.8 HIL simulation result of loitering munition

通过飞行控制系统数据记录仪中数据对半实物仿真实验结果进行对比分析，如图8所示：巡飞弹自动任务模式下通过了4个航点，从图8(a)中可以看出，巡飞弹均能按照指定的路径进行航迹跟随，无风条件下，L制导律与改进制导律具有相同的参考距离，因此其飞行轨迹基本一致，当存在一定角度约为0.5va的风场影响时，采用L制导律的飞行器随着风向变化，航迹跟随过程产生严重的偏差，而采用改进制导律的飞行器能够根据风向自适应调整航点切换时机，航线跟随误差也显著减小，巡飞弹能够快速进入指定的航线；图8(b)中绘制了航迹跟踪过程中飞行器高度变化情况，由于高度控制器的影响，巡飞弹实际飞行高度与第1节中理想条件会存在一定偏差，存在“掉高”现象，但高度误差始终保持在10 m以内，可知存在高度误差影响时，改进制导律仍然适用；图8(c)为巡飞弹飞行过程中的空速变化，采用速度控制回路，巡飞弹能够保证在(20±5) m/s的速度下执行飞行任务，制导律对空速要求较低，受空速变化影响小。

5 结论

风场环境下的巡飞弹执行自动航线任务时，传统制导律方法受风速、风向影响较大，实现过程复杂，本文在建立风场条件下巡飞弹运动模型的基础上，采用了非线性制导律；针对非线性制导律中固定距离限制的问题，设计了变增益虚拟参考点，分析并给出了航迹跟踪的稳定性条件；针对系统外回路滞后特性进行了加速度反馈补偿设计，同时给出了制导律中参数计算方法及运动约束条件；针对实际飞行验证困难的问题，结合半实物仿真思想，基于vc++搭建了实时仿真平台，采用硬件在回路仿真闭环试验方法，验证了航迹跟踪精度。数学及半实物仿真实验结果表明，所设计的控制器具有如下特点：

1)在非线性制导律中设计变增益虚拟目标距离l使得飞行器能够克服约飞行速度50%左右任意方向风的影响，可以自适应调整航点切换距离。

2)引入加速度动态反馈补偿后，改进制导律提高了滚转角指令值的动态范围，跟踪精度及快速性得到提升。

3)当飞行器在航点切换或者高度变化情况下，改进制导律仍具有较好的航迹跟踪能力，且受飞行过程中空速变化带来的影响较小。

4)半实物仿真实验及制导律的硬件实现表明，所设计的控制器易于工程实现，参数计算方法为研发人员提供了借鉴。

References)

[1] 庞艳珂，韩磊，张民权，等. 攻击型巡飞弹技术现状及发展趋势[J]. 兵工学报, 2010, 31(增刊2): 149-152. PANG Yan-ke, HAN Lei, ZHANG Min-quan, et al. Status and development trends of loitering attack missiles [J]. Acta Armamentarii, 2010, 31(S2): 149-152. (in Chinese)

[2] 张坤，高晓光. 未知风场扰动下无人机三维航迹跟踪鲁棒最优控制[J]. 电子与信息学报, 2015, 37(12): 3009-3015. ZHANG Kun, GAO Xiao-guang. Robust optimal control for unmanned aerial vehicles’ three-dimensional trajectory tracking in wind disturbance [J]. Journal of Electronics & Information Technology, 2015, 37(12): 3009-3015. (in Chinese)

[3] 管军，易文俊，常思江，等. 某型无人机三维空间航迹跟踪控制方法研究[J]. 兵工学报, 2016, 37(1): 64-70. GUAN Jun, YI Wen-jun, CHANG Si-jiang, et al. Study of flight path tracking and control of an UAV in 3D space [J]. Acta Armamentarii, 2016, 37(1): 64-70. (in Chinese)

[4] Sujit P B, Srikanth S, João B S. Unmanned aerial vehicle path following: a survey and analysis of algorithms for fixed-wing unmanned aerial vehicles [J]. IEEE Control System Magazine, 2014, 34(1): 42-59.

[5] Zhang J, Li Q, Cheng N, et al. Nonlinear path-following method for fixed-wing unmanned aerial vehicles [J]. Journal of Zhejiang University-Science C(Computers & Electronics), 2013, 14(2): 125-132.

[6] Eran D B M, Peter W G. Synthetic-waypoint guidance algorithm for following a desired flight trajectory [J]. Journal of Guidance Control and Dynamics, 2010, 33(2): 601-606.

[7] Venkatraman K, Mani V, Kothari M, et al. A suboptimal path planning algorithm using rapidly-exploring random trees [J]. International Journal of Aerospace Innovations, 2010, 2(1): 93-104.

[8] Ratnoo A. Adaptive optimal path following for high wind flights [C]∥Proceedings of the 18th IFAC World Congress. Milano, Italy: IFAC, 2011: 12985-12990.

[9] Nelson D R, Barber D B, Mclain T W, et al. Vector field path following for miniature air vehicles [J]. IEEE Transactions on Robotics, 2007, 23(3): 519-529.

[10] Park S, Deyst J, How J P. A new nonlinear guidance logic for trajectory tracking [C]∥AIAA Guidance, Navigation, and Control Conference and Exhibit. Rhode Island, US: AIAA, 2004:16-19.

[11] Park S. Autonomous aerobatic on commanded path [J]. Aerospace Science and Technology, 2012, 22(1): 64-74.

[12] 余波，朱纪洪，范勇，等. 飞行控制系统大闭环半物理仿真研究[J]. 航天控制, 2010, 28(2): 51-55. YU Bo, ZHU Ji-hong, FAN Yong, et al. Research on large-scale hardware-in-closed-loop simulation of flight control system [J]. Aerospace Control, 2010, 28(2): 51-55. (in Chinese)

[13] 李军伟，程咏梅，陈克喆，等. 基于飞行仿真的捷联惯导算法测试平台[J]. 中国惯性技术学报, 2012, 20(5): 530-535. LI Jun-wei, CHENG Yong-mei, CHEN Ke-zhe, et al. Test platform for SINS algorithm based on flight simulation [J]. Journal of Chinese Inertial Technology, 2012, 20(5): 530-535. (in Chinese)

[14] 刘莉，王岩松，周思达，等. 考虑弹体弹性的导弹半物理仿真方法与影响分析[J]. 北京航空航天大学学报, 2016, 42(4): 639-645. LIU Li, WANG Yan-song, ZHOU Si-da, et al. Hardware-in-the-loop simulation method and influence analysis of missiles considering the elasticity of the body [J]. Journal of Beijing University of Aeronautics and Astronautics, 2016, 42(4): 639-645. (in Chinese)

Trajectory Tracking Algorithm for Motion Compensation of Loitering Munition under Wind Environment

LI Zeng-yan1， LI Xiao-min1， LIU Qiu-sheng2

(1.Department of Unmanned Aerial Vehicle Engineering, Ordnance Engineering College, Shijiazhuang 050003, Hebei, China;2.Department of Ammunition Engineering, Ordnance Engineering College, Shijiazhuang 050003, Hebei, China)

The trajectory tracking process of loitering munition is affected by wind speed and wind direction. A motion model with wind is established, and a nonlinear guidance law based on the variable gain virtual reference point and outer loop motion compensation is proposed. According to the fixed distance limit of the guidance law parameters, the variable gain virtual reference point is designed, and the stability condition of trajectory tracking is analyzed. Then the dynamic feedback compensation factor is designed for the hysteresis characteristic of the system’s outer loop, and the calculation method of the parameters and the kinematic constraints are also given. The guidance law is verified with wind and without wind through mathematical simulation. Considering the difficulty of actual flight, the tracking performance is tested on the semi physical real-time simulation platform which is built based on vc++. The results show that the improved guidance law can effectively overcome the wind disturbance, and it has high tracking accuracy but is easy to be realized.

ordnance science and technology; loitering munition; trajectory tracking; nonlinear guidance law; motion compensation; semi physical simulation

2016-06-08

武器装备“十二五”预先研究项目(51325050101)

李增彦(1987—), 男, 博士研究生。E-mail: lizengyan2012@163.com; 李小民(1968—), 男, 教授, 博士生导师。E-mail: lxmfy2000@263.net

V249.1

A

1000-1093(2016)12-2377-08

10.3969/j.issn.1000-1093.2016.12.025