基于四元数的四旋翼无人飞行器轨迹跟踪控制

基于深度强化学习的四旋翼航迹跟踪控制方法在科技的海洋中，四旋翼无人机犹如一艘精巧的帆船，而深度强化学习技术则是引领它穿越波涛的罗盘。

本文将探讨这一技术的奥秘，以及它如何革新航迹跟踪控制领域。

首先，我们必须认识到，四旋翼无人机的航迹跟踪控制是一项极具挑战性的任务。

它要求无人机在复杂的环境中精确地遵循预定路径，这就像要求一位舞者在狂风暴雨中完成一套完美的舞蹈动作。

传统的控制方法往往难以应对这种高度动态和不确定性的环境，而深度强化学习提供了一种全新的解决思路。

深度强化学习，这一机器学习的分支，通过让机器自我学习最优策略，来实现对复杂系统的控制。

在这个过程中，算法不断与环境互动，通过试错来优化其行为。

这就像是给无人机装上了一双会思考的眼睛，让它能够在飞行中自我调整，适应各种未知的挑战。

那么，深度强化学习是如何在四旋翼无人机的航迹跟踪控制中发挥作用的呢？首先，我们需要构建一个准确的模型来描述无人机的动态特性和环境因素。

这个模型就像是一张精细的地图，为无人机的飞行提供指导。

然后，我们设计一个奖励函数，用来评价无人机的飞行表现。

这个奖励函数就像是一面镜子，反映出无人机是否偏离了预定的航迹。

最后，我们利用深度神经网络来学习最优的控制策略。

这个网络就像是无人机的大脑，能够处理复杂的信息并做出决策。

在实际应用中，这种方法展现出了惊人的效果。

无人机能够在风速变化、障碍物突然出现等极端情况下，依然紧密地跟随预定航迹。

这就像是在狂风巨浪中依然能够保持航线的船只，展现了深度强化学习的强大能力。

然而，我们也必须看到这项技术面临的挑战。

深度强化学习需要大量的数据和计算资源，这对于实际部署来说是一个不小的障碍。

此外，如何确保学习过程的稳定性和安全性，也是一个亟待解决的问题。

总的来说，基于深度强化学习的四旋翼航迹跟踪控制方法为我们打开了一扇通往未来的大门。

它不仅提高了无人机的性能，也为我们提供了一个理解复杂系统的新视角。

尽管这条路上充满了挑战，但正如航海家面对茫茫大海时的勇气一样，我们也有理由相信，这项技术将带领我们驶向一个更加智能和自主的未来。

基于MPC和ESO-DO的四旋翼轨迹跟踪控制
孙嫚憶;毕文豪;张安;刁玉豪
【期刊名称】《指挥控制与仿真》
【年(卷),期】2024(46)2
【摘要】针对扰动作用和模型不确定性下四旋翼无人机精确轨迹跟踪控制问题,提出了一种主动干扰抑制和模型预测控制(Model Predictive Control, MPC)策略。

模型预测控制器通过扩张状态观测器(Extended State Observer, ESO)和扰动观测器(Disturbance Observer, DO)来估计和补偿干扰,从而实现位置环精确控制。

在存在外部干扰和参数不确定性的情况下,通过仿真实验,证明了所提出的方法提高了对建模误差和干扰的鲁棒性,同时实现了对参考轨迹的平滑跟踪。

【总页数】11页(P104-114)
【作者】孙嫚憶;毕文豪;张安;刁玉豪
【作者单位】西北工业大学
【正文语种】中文
【中图分类】V279
【相关文献】
1.基于四元数的四旋翼无人飞行器轨迹跟踪控制
2.基于IBC-PID控制的四旋翼轨迹跟踪控制
3.基于最优迭代学习控制的四旋翼轨迹跟踪控制
4.基于MPC的四旋翼无人机航迹跟踪控制系统
5.基于改进MPC算法的四旋翼无人机轨迹跟踪控制
因版权原因，仅展示原文概要，查看原文内容请购买。

X型四旋翼无人机建模及四元数控制
丁少宾;肖长诗;刘金根;文元桥
【期刊名称】《系统仿真学报》
【年(卷),期】2015(0)12
【摘要】对X型四旋翼无人机做了合理假设以及受力分析,推导并建立了X型四旋翼无人机全面的动力学数学模型,考虑了无人机平动、转动空气阻力,并且将转子、螺旋桨和机体看成多刚体系统。

在Solidworks软件中,建立了无人机实物模型以获得无人机惯性参数。

直接以四元数作为反馈控制量,设计出多通道双回路矢量PD 控制系统。

以Matlab/Simulink为平台,对四元数反馈控系统和欧拉角反馈控制系统进行对比控制仿真。

从仿真结果来看,这2种反馈模式都能对无人机模型进行位置、姿态跟踪等控制,但是四元数反馈控制系统具有过渡时间短、计算量少以及无奇点产生的优点。

【总页数】6页(P3057-3062)
【作者】丁少宾;肖长诗;刘金根;文元桥
【作者单位】湖北省内河航运技术重点实验室;武汉理工大学信息工程学院光纤传感技术与信息处理教育部重点实验室
【正文语种】中文
【中图分类】V249.1
【相关文献】
1.四旋翼无人机建模与PID控制器设计
2.超/特高压输电线路巡检四旋翼无人机的建模与回馈递推控制
3.自主巡线四旋翼无人机建模及姿态控制器设计
4.四旋翼无人机的控制原理及建模
5.带单关节机械臂四旋翼无人机的建模与控制
因版权原因，仅展示原文概要，查看原文内容请购买。

四旋翼无人机轨迹跟踪控制研究秦澍祺;王国胜;梁冰【摘要】本文通过使用黎卡提( Riccati ) 矩阵方程来求解微型四旋翼无人机的线性二次型跟踪( Linear Quadratic Tracking LQT )控制器.首先,根据四旋翼的悬停条件,线性化四旋翼的非线性模型,用以解决最优控制问题.然后,通过定义成本函数来更好的权衡跟踪性能和能量消耗.最后,通过使用黎卡提方程来求解时变的最优控制增益.通过仿真表明,与传统的 PID 或者是固定增益的 LQR 控制相比, LQT 控制器具有良好的跟踪控制性能.【期刊名称】《科技视界》【年(卷),期】2018(000)029【总页数】2页(P101-102)【关键词】四旋翼无人机;最优控制;线性二次型跟踪控制器【作者】秦澍祺;王国胜;梁冰【作者单位】陆军装甲兵学院兵器与控制系,中国北京 100072 ;陆军装甲兵学院兵器与控制系,中国北京 100072 ;江西理工大学信息工程学院,江西赣州 341000【正文语种】中文【中图分类】V2490 引言四旋翼是一个具有高机动的、非线性的、耦合的和欠驱动的系统。

所以许多研究人员设计了各种的线性、非线性或者混合控制技术来控制四旋翼飞行器。

比如传统的PID 控制器[1]、反步法控制器[2]、滑膜控制器[3]、模型预测控制器[4]和的线性二次型[5]控制器等。

这些控制器一般使用恒定的控制增益作为状态反馈控制，专注于对四旋翼无人机的稳态控制而不是轨迹或者目标跟踪的精度。

本文则提出使用线性二次型跟踪器（LQT），通过轨迹来调节控制增益从而更好追踪期望轨迹。

同时与线性PID 和LQR 控制器作为对比，三者都在状态估计上加入了相同的白噪声干扰模拟实际环境。

1 模型建立与线性化四旋翼飞行器模型考虑为线性定常系统，状态矩阵A、B、C 和D 都为静态的不随时间改变。

定义如下：x 为状态向量，y 为输出向量，u 为输入向量，根据文献[6]建立的四旋翼模型，线性化后状态空间矩阵A、B 为：其中，12 维状态量[x y z u v w φ θ ψ p q r]T 包括位置、速度、角度、角速度。

基于四元数的四轴无人机姿态的估计和控制作者：冀亮钱正洪白茹来源：《现代电子技术》2015年第11期摘要：介绍一种只需少量计算就可实现四轴无人机姿态的估计和控制的新颖方法。

整个算法采用四元数表示，四元数运算基于惯性测量单元（IMU）的加速度和陀螺仪传感器。

姿态估计使用四元数代替三角函数，允许用加速度数据分析推导和优化梯度下降算法来减少由陀螺仪传感器产生的误差。

姿态控制将当前姿态与目标姿态的姿态误差反馈到PD控制器来实现想要的目标姿态。

姿态估计中用到的梯度下降算法通过四元数对应的欧拉角的均方误差验证，静态均方误差小于0.945°。

实际的飞行测试可以实现一个稳定的飞行，同时验证了姿态控制算法。

关键词：四轴无人机；四元数；姿态估计；姿态控制中图分类号： TN967⁃34 文献标识码： A 文章编号： 1004⁃373X（2015）11⁃0112⁃05 Attitude estimation and control of quaternion based quad⁃axis UAVJI Liang， QIAN Zheng⁃hong， BAI Ru（Center for Integrated Spintronic Devices， Hangzhou Dianzi University， Hangzhou 310018， China）Abstract： A novel method with few calculations is presented to realize estimation and control of quad⁃axis UAV. The whole algorithm is expressed with quaternion， and the quaternion calculation is based on the acceleration of inertial measurement units （IMU） and gyroscope sensor. The quaternion is applied in attitude estimation instead of trigonometric function. Gradient descent algorithm is analyzed， deduced and optimized with acceleration data to reduce the error generated by gyroscope sensor. The error of current attitude and target attitude is fed back to PD controller with attitude control， to realize the expectant target attitude. The gradient descent algorithm applied in attitude estimation is verified with RMS error which is obtained from Euler angle corresponded with quaternion. The static RMS error is less than 0.945°. The stable flying is realized by actual flight test， meanwhile the attitude control algorithm is verified.Keywords： quad⁃axis UAV； quaternion； attitude estimation； attitude control0 引言四轴飞行器是众多无人机中的典型代表，它通过四个电机作为引擎来实现空中飞行。

2020年第48卷第9期D驱动控制rive and control 罗鼎馨等 四旋翼载重无人机双闭环轨迹跟踪控制39　收稿日期:2020-04-23基金项目:湖南省自然科学基金资助项目(2015JJ3126)四旋翼载重无人机双闭环轨迹跟踪控制罗鼎馨1,2,蒋　近1,2,许迪文1(1.湘潭大学信息工程学院,湘潭411105;2.湘潭大学智能计算与信息处理教育部重点实验室,湘潭411105)摘　要:针对在常规滑模和PID 控制下的四旋翼载重无人机在无人机飞行运动及负载变化时难以实现精准轨迹跟踪的问题,提出一种基于自适应非奇异终端滑模与改进的自抗扰控制相结合的双闭环轨迹跟踪控制方法㊂外环的位置控制系统采用自适应非奇异终端滑模控制算法,外环系统状态能在有限时间内迅速达到收敛平衡状态,增强了系统的鲁棒性;内环的姿态控制系统采用改进的自抗扰控制算法,设计一个新的非线性状态误差反馈函数,相比原来的状态误差反馈函数更平滑,较好地减小了误差反馈控制的抖振影响,闭环系统更加稳定㊂仿真结果证明,所设计的控制系统能精准快捷地实现了四旋翼载重无人机的轨迹跟踪控制㊂关键词:四旋翼载重无人机;自适应非奇异终端滑模;自抗扰控制;轨迹跟踪中图分类号:TM464 文献标志码:A 文章编号:1004-7018(2020)09-0039-06Dual Closed-Loop Trajectory Tracking Control of Quadrotor Load UAV LUO Ding -xin 1,2,JIANG Jin 1,2,XU Di -wen 1(1.College of Information Engineering,Xiangtan University,Xiangtan 411105,China;2.Key Laboratory of Intelligent Computing and Information Processing,Ministry of Education,Xiangtan University,Xiangtan 411105,China)Abstract :It is difficult to achieve accurate trajectory tracking of quadrotor load UAV under control of conventionalsliding mode and PID during flight movement and load changes.A method of dual closed-loop trajectory tracking control of the quadrotor load UAV by combining adaptive nonsingular terminal sliding mode with improved active disturbance rejection control was proposed.By adopting the algorithm of adaptive non-singular terminal sliding mode control for the position con⁃trol system of the outer loop,the state of the outer loop system can quickly converge to the equilibrium state in a finite time,and the robustness of the system could be better strengthen.A new state function of error feedback designed by adopting the algorithm the improved active-disturbance rejection control in the attitude control system of the inner loop,which is smoot⁃her than the original function,thus reducing the buffeting effect of the error feedback control and making the closed-loopsystem more stable.The simulation results show that the designed control system can realize trajectory tracking control of the quadrotor load UAV faster and better.Key words :quadrotor load UAV,nonsingular terminal sliding mode,active disturbance rejection control (ADRC),trajectory tracking0　引　言四旋翼无人机作为一种可以垂直起降㊁空中悬停的小型飞行器,在一些民用领域,农业领域以及工业领域,都有它的用武之地㊂例如在国内物流行业急剧增长之际,运输需求量的骤增及消费服务要求的提升,给业界带来了巨大压力,不管是快递行业㊁邮政行业还是外卖行业,都面临着人力成本与配送效率上的巨大矛盾㊂在这样的背景下,载物运输无人机也应运而生㊂载物无人机相比其他传统运输方式,有其独特优势,比如能较好的适用于各种复杂的环境㊁飞行运送速度快㊁规模化运输成本低㊂但在运输过程中,载重无人机还需要提高适应环境的能力,在外界干扰下保证稳定飞行;同时载重无人机在执行多处任务投放时,到达投放地点上空后,投下载物并能保证飞行轨迹不变㊂目前,常用于四旋翼无人机轨迹跟踪控制的控制算法有PID 算法㊁反步法㊁滑模算法㊁自抗扰算法等㊂文献[1]采用模糊PID 算法设计的轨迹跟踪控制器,在实现对目标轨迹的跟踪时,算法简单有效,但是抗干扰能力较差㊂文献[2]采用一种积分饱和反步控制,可以较好地抵抗无人机轨迹跟踪过程中所受到的常值干扰和变值干扰,但是该算法需要比较精确的建模,而无人机在实际工作过程中,模型和外界干扰难以精确建模㊂文献[3]引入了一种滑模线性自抗扰控制器,可以对未知扰动进行较好地估 D驱动控制rive and control 2020年第48卷第9期 罗鼎馨等 四旋翼载重无人机双闭环轨迹跟踪控制 40　计和补偿,但是从抗干扰能力来看,在自抗扰控制中使用非线性的扩张观测器会具有更强的扰动抑制能力㊂文献[4]采用了一种模糊自适应滑模的控制策略,通过对位置和姿态滑模控制器中的趋近律切换项进行模糊化,来降低抖振影响㊂然而自适应滑模算法收敛速度较慢,趋近平衡点时间比较长,并且模糊规则的制定也需要较多的经验㊂文献[5]采用一种模糊自抗扰算法,对自抗扰控制的非线性状态误差反馈环节进行了模糊处理,提高了自抗扰控制器的自适应性,但缺点是参数过多,会使算法变得繁琐㊂综合考虑现有研究成果,本文采用结合自适应非奇异终端滑模算法和改进的自抗扰算法,设计四旋翼载重无人机双闭环控制策略㊂本文的主要工作包括:1)将载重无人机的控制系统分为外环位置子系统和内环姿态子系统,从而降低系统控制难度㊂2)采用非奇异终端滑模算法来实现位置子系统的跟踪控制,并对载重无人机的负载变化进行了自适应估计㊂3)采用改进的自抗扰控制策略对姿态子系统进行控制,该控制策略有效地克服了传统自抗扰控制中误差反馈控制律的非线性函数的不平滑特性,能较好地应对外界干扰和负载变化,改善了控制品质㊂1　四旋翼载重无人机动力学建模首先,建立四旋翼载重无人机的坐标系如图1所示,直角坐标系E(O,X1,Y1,Z1)是对应于大地的惯性坐标系,直角坐标系B(O2,X2,Y2,Z2)是机体坐标系,在坐标系B中,O2是无人机的质心点㊂图1　惯性坐标系和机体坐标系示意图从机体坐标系到惯性坐标系的转换矩阵如下:R=cosθcosψ-cosφsinψ+sinφsinθcosψsinφsinψ+cosφsinθcosψcosθsinψcosφcosψ+sinφsinθsinψ-sinφcosψ+cosφsinθsinψ-sinθsinφcosθcosφcoséëêêêùûúúúθ(1) 根据欧拉-拉格朗日建模原理,可得四旋翼载重无人机平移运动和姿态运动的动力学方程组[2]:p㊃=v(2)mv㊃=T-G+Δ(3)λ㊃=W(λ)㊃ω(4) Jω㊃=-ω×Jω+F b-F c+τ(5) 式(2)中,向量p=[x,y,z]T表示无人机质心O2在惯性坐标系E中的位置;向量v=[v x,v y,v z]T 表示无人机在惯性坐标系E中的速度㊂式(3)中,m为无人机的总质量,四个旋翼向上的牵引力T=F a Re3,其中F a为无人机的总升力,R 为坐标转换矩阵,且有F a=b(Ω21+Ω22+Ω23+Ω24),b为升力系数,Ω为电机转速;G=mg e3表示无人机所受的重力,其中e3=[0,0,1]T为竖直方向单位向量;Δ表示无人机在飞行中所受到的外界干扰力㊂式(4)中,λ=(θ,φ,ψ)表示旋转姿态角,ω= (ωx,ωy,ωz)表示无人机的机体角速率,W(λ)为旋转姿态角与机体角速率之间的转换矩阵,其表达式如下:W(λ)=1sinφtanθcosφtanθ0cosφ-sinφ0sinφ/cosθcosφ/coséëêêêùûúúúθ(6) 式(5)中,J为在惯性坐标系下总的转动惯量矩阵,其表达式如下:J=I x0-I x sinθ0I y cos2φ+I z sin2φ(I y-I z)sinφcosφcosθ-I x sinθ(I y-I z)sinφcosφcosθI x sin2θ+I y sin2φcos2θ+I z cos2φcos2éëêêêùûúúúθ(7)式中:I x,I y,I z分别为电机在X,Y,Z轴产生的转动惯量㊂式(5)中,F b代表姿态控制输入力矩,其表达式:F b=F bϕF bθF béëêêêùûúúúψ=bl(Ω24-Ω22)bl(Ω23-Ω21)k(Ω22+Ω24-Ω21-Ω23éëêêêùûúúú)(8)式中:b,k均为空气动力学中的参数;l表示无人机质心与电机之间的距离㊂式(5)中,F c为陀螺力矩,F c=J㊃-12∂∂λ(λ㊃T J);τ为无人机所受干扰力矩㊂2　四旋翼载重无人机控制器设计2.1　控制系统结构四旋翼载重无人机在投下所载物后,重心和质 2020年第48卷第9期 D驱动控制rive and control 罗鼎馨等 四旋翼载重无人机双闭环轨迹跟踪控制41　量都发生了改变,导致无人机的运动轨迹和姿态都较大地偏离期望值㊂因此,本文设计了相应的控制器来解决载重无人机飞行轨迹的跟踪问题,如图2所示㊂该闭环系统采用了内外环结构,外环为位置子系统,设计了一种自适应非奇异终端滑模控制器来实现平移运动,同时设计了对应的自适应律来控制质量突变㊂内环为姿态子系统,通过改进的自抗扰控制器,实现姿态稳定和跟踪㊂图2　四旋翼载重无人机控制系统结构图2.2　自适应非奇异终端滑模控制器设计假设期望跟踪的参考位置为p d ,则将式(2)代入式(3)中可得四旋翼载重无人机平移子系统的系统状态方程:p ¨=(T -G +Δ)/m}y =p(9) 为保证系统状态能快速收敛至平衡点,本文采用了非奇异终端滑模控制,其函数定义:s =e +1βe ㊃p 1/q 1(10)式中:e =p -p d 为跟踪误差;β>0为常数;p 1和q 1为正奇数,且1<p 1/q 1<2㊂由非奇异终端滑模控制函数式(10)和等速趋近律s ㊃=-k sgn(s ),其中k >0,并结合式(9),可推出平移子系统控制律:T =m (-β㊃q 1p 1㊃e ㊃2-p 1/q 1-ζsgn(s )+p ¨d )+G -Δ(11)式中:ζ=k ㊃β㊃q 1p 1㊃e ㊃1-p 1/q 1,可知ζ>0,则系统跟踪误差可在有限时间内收敛至0,又因为0<2-p 1/q 1<1,则当e ㊃趋于0时,e ㊃2-p 1/q 1不会趋于无穷大,因此不会存在传统终端滑模面的奇异性问题㊂将G =mg e 3代入式(11),经简化后可得:T =mT --Δ(12)式中:T -=-β㊃q 1p 1㊃e ㊃2-p 1/q 1-ζsgn(s )+p ¨d +ge 3(13) 为了减少滑模控制所固有的抖振现象,这里将式(13)中的符号函数sgn(s )用饱和函数sat(s )替代,饱和函数sat(s ):sat(s )=sgn(s ) s >εsε s ≤{ε(14)式中:ε为边界层厚度㊂由式(9)可知,无人机的平移子系统是一个二阶系统,升力T 与外界干扰Δ并非线性关系,从而无法通过单一的滑模控制来保证系统抗干扰稳定性,所以需要设计一个自适应律来估计外界干扰㊂为保证对期望的参考位置信号的精确跟踪,应将m和Δ取自适应值,这里用m ^和Δ^分别代表质量和外界干扰力的估计值㊂根据梯度下降法,m ^和Δ^自适应律设计如下:Δ^㊃=γ1sm ^㊃=-γ2s T T}-(15) 在得到了虚拟控制量T 后,可由T =F a Re 3得出总升力F a 和姿态子系统中的横滚角φ和俯仰θ的期望求解量㊂令T =T x ,T y ,T []z T ,代入T =F a Re 3,R 为坐标转换矩阵,e 3=[0,0,1]T ,最终可得:F a =T zcos φd cos θdθd =arctan(T x cos ψd -T y sin ψdT z )φd =arctan(cos θd T x sin ψd -T y cos ψd T züþýïïïïïïïï)(16)2.3　改进的自抗扰控制器设计2.3.1　经典自抗扰控制器经典自抗扰控制器是由韩京清研究员所设计,它汲取了经典PID 控制器中 基于误差反馈来消除误差”的精髓,并且能使未精确建模的被控对象在控制品质和控制精度上有根本的提高㊂自抗扰控制器最突出的特征就是把作用于被控对象的所有不确定因素作用都归结为 未知扰动”,而用对象的输入输出数据对它进行估计并给予补偿㊂因此在应对质量变化和风力干扰等诸多不确定因素时,采用自抗扰控制器对无人机的三个飞行姿态角进行控制,能有更好的适应性和鲁棒性,保证飞行姿态的精确跟踪㊂经典自抗扰控制器由跟踪微分器(以下简称TD)㊁扩张状态观测器(以下简称ESO)和非线性误差反馈控制律(以下简称NLSEF)三个部分组成㊂(1)TDTD 可以跟踪参考输入信号,并获取其微分信号,然后再安排预期的过渡过程来减少系统信号的D驱动控制rive and control 2020年第48卷第9期罗鼎馨等 四旋翼载重无人机双闭环轨迹跟踪控制42　超调,增强系统的鲁棒性㊂TD 形式如下:x 1(k +1)=x 1(k )+hx 2(k )x 2(k +1)=x 2(k )+hfhan [x 1(k )-v (k ),x 2(k ),r ,h }](17)式中:r 为跟踪快慢控制参数;h 为积分步长㊂fhan =-r sgn(a ) a >δ-r aδa ≤{δa =x 2(k )+a 0-δ2sgn[y (k )] y (k )>δ0x 2(k )+hy (k )　y (k )≤δ{a 0=δ2+8r y (k )δ=rh ,δ0=δhy (k )=x 1(k )-v (k )+hx 2(k ) (2)ESO当把系统动态中异于系统状态标准型的部分视为扰动总和时,ESO 可以实现实时估计和消除扰动总和的功能,从而把存在扰动和不确定性的非线性被控系统还原为标准的积分串联型系统㊂e (k )=z 1(k )-y (k )z 1(k +1)=z 1(k )+h [z 2(k )-β01e (k )]z 2(k +1)=z 2(k )+h {z 3(k )-β02fal [e (k ),1/2,δ]+ f 0[z 1(k ),z 2(k )]+b 0u (k )}z 3(k +1)=z 3(k )-hβ03fal [e (k ),1/4,δüþýïïïïïï](18)式中:β01,β02,β03分别为输出误差的校正参数;z 1,z 2是被观测对象的状态量,z 3为扰动及未建模动态的估计值㊂(3)NLSEFNLSEF 是一种非线性函数的组合形式,它可以针对系统的扰动总和提供动态补偿,与传统的PID 组合形式相比提高了动态性能,而且不会显著增加计算复杂度㊂二阶NLSEF 的离散形式如下:e 1(k +1)=x 1(k +1)-z 1(k +1)e 2(k +1)=x 2(k +1)-z 2(k +1)U (k +1)=U 0(k +1)- z 3(k +1)+f 0[z 1(k +1),z 2(k +1)]b 0U 0(k +1)=ρ1fal [e 1(k +1),α1,δ1]+ ρ2fal [e 2(k +1),α2,δ1üþýïïïïïïïïïï](19)式中:e 1,e 2为期望的过渡过程与系统输出的估计值之间的偏差及其微分;α1,α2,δ1为非线性函数fal (㊃)相关参数;U 0为非线性误差反馈控制输出;U 为控制量;b 0为补偿系数㊂2.3.2　改进的自抗扰控制器在经典自抗扰控制器中,ESO 和NLSEF 中都存在一个非线性函数fal (㊃):fal (e ,α,δ)=eδα-1 e ≤δe αsgn(e ) e >{δ(20)式中:0<α<1,δ是非线性函数的区间长度㊂该函数具有 小误差,大增益”, 大误差,小增益”的特性,但是函数曲线在原点附近是线性段的连续幂次函数,其不光滑特性,容易使误差反馈控制力出现比较强烈的抖振现象㊂本文提出一个新的函数Lfal (㊃)㊂它既可以实现fal (㊃)该有的功能作用,而且相比之下新函数更加平滑,从而减小了抖振影响㊂Lfal (㊃)函数如下:Lfal (x ,α)=x α㊃th(x 2)=x α㊃1-e -x 1+e -x(21)式中:0<α<1,当α=0.5时,函数曲线如图3所示㊂对比图3中曲线,Lfal (㊃)更加平滑,且当控制系数增加时,函数将具有较小的增益,可以有效地降低控制输入饱和的发生概率㊂将Lfal (㊃)替换经典自抗扰控制器中NLSEF 的fal (㊃)后,u 0的表达式改写:u 0=ρ1㊃Lfal (e 1,α1)+ρ2㊃Lfal (e 2,α2)(22) 这一改进可明显改善非线性误差反馈控制的光滑效应,从而提高最终的控制品质㊂图3　两种非线性误差反馈函数对比图2.3.3　基于改进自抗扰的载重无人机姿态控制器设计根据四旋翼载重无人机姿态运动的动力学方程,将式(4)求导,再代入式(5)中,即可得姿态子系统的状态方程:λ¨=WJ -1(-ω×J ω)+WJ -1F b +WJ -1(-F c +τ)+W ㊃ω　(23) 四旋翼载重无人机姿态子系统控制框图如图4所示㊂根据自抗扰控制思想,式(23)中前两项分别为系统已建模动态和控制输入项,而后两项则为系统未建模动态及外界干扰项,视为扰动总和㊂四旋翼载重无人机姿态子系统可以分为三个独立的通 2020年第48卷第9期 D驱动控制rive and control 罗鼎馨等 四旋翼载重无人机双闭环轨迹跟踪控制43　道:滚转通道㊁俯仰通道和偏航通道,每个通道都可以用相同的方式来设计一个自抗扰控制器,当对某一个通道进行控制时,可以将其他通道的耦合量和扰动均当成扰动总和,由状态观测器ESO 观测出,并给予补偿,从而可实现系统各通道的解耦控制㊂图4　四旋翼载重无人机姿态子系统控制框图由于三个姿态控制通道所用的改进自抗扰控制器结构一致,因此这里仅以滚转角θ为例,对姿态角的控制流程进行介绍㊂首先由外环控制器得到一个期望滚转角θd ,经过TD 可得一个原信号的跟踪信号v 11及其微分信号v 12,再分别与ESO 实时观测到的θ1和θ2相减,获得误差信号e 11和e 12㊂将这两个误差信号经过NLSEF 式(22)的计算后即可得到误差反馈控制量,最后对误差反馈控制量进行一次扰动总和估计值θ3的补偿,即可得滚转角的最终控制量u 1=(u 01-θ3)/b 0,其中参数b 0是决定补偿强弱的补偿因子㊂3　仿真结果本文针对四旋翼载重无人机的内外环控制均做了改进和仿真实验对比㊂一是针对外环的位置子系统,采用了自适应非奇异快速终端滑模控制算法控制,与传统的自适应滑模控制算法进行仿真实验对比㊂二是对于内环的姿态子系统,设计了一种改进的自抗扰控制器,与经典自抗扰控制器控制下的仿真实验效果做了对比㊂空载时的四旋翼无人机模型参数如表1所示㊂在仿真实验中,假定四旋翼载重无人机载有三个质量相同的物体,其质量均为1.5kg ,然后分别在特定的三个位置定点投下载物㊂其表1　四旋翼载重无人机的模型参数参量数值参量数值无人机质量m /kg5机臂长度l /m 0.3转动惯量I y /(kg㊃m 2)5.153×10-3转动惯量I z /(kg㊃m 2) 3.835×10-2升力因子b /(N㊃s 2㊃rad -2)3.125×10-5转动惯量I x /(kg㊃m 2)5.153×10-3阻力因子k /(N㊃s 2㊃m㊃rad -2)7.5×10-7中外环控制器参数:β=diag (1.1,1.1,2),p 1=diag (15,15,13),q 1=diag (13,13,11),ζ=diag (0.01,0.01,0.01),ε=diag (0.02,0.02,0.02)㊂内环中改进自抗扰控制器参数如下:h =0.01s,r =100,β01=100,β02=80,β03=100,α=0.45,ρ1=10和ρ2=100㊂ 同时在仿真过程中,对四旋翼载重无人机设置了外界模拟干扰信号,其中外界气动干扰力:Δ=[0.1sin(0.1πt ),0.1cos(0.1πt ),0.1cos(0.1πt )],单位为N;干扰力矩:τ=[0.3sin (0.1πt )+0.1,0.4cos(0.1πt )+0.1,0.5sin(0.1πt )+0.2],单位为N㊃m㊂设四旋翼载重无人机初始状态均为0,即初始位置为惯性坐标系的原点,初始姿态角均为0㊂期望跟踪的参考轨迹设为p d =2sin t æèçöø÷2,2cos t æèçöø÷2,2éëêêùûúút ,仿真实验结果如图5~图11所示㊂图5中,四旋翼载重无人机先从初始位置起飞,再按期望轨迹做螺旋上升运动之后,载重无人机分别在图中坐标为(0,-2,12.5),(0.9,-1.8,26.9)和(-1,-1.7,42)处各投下一个载物,此时,载重无人机这三个时刻的飞行轨迹会瞬间突变,偏离期望图5　轨迹跟踪三维效果图6　两种算法控制下x 轴跟踪误差对比图图7　两种算法控制下y 轴跟踪误差对比图图8　两种算法控制下z 轴跟踪误差对比图图9　质量自适应估计 D驱动控制rive and control 2020年第48卷第9期 罗鼎馨等 四旋翼载重无人机双闭环轨迹跟踪控制 44　图10　在两种控制下总升力对比图图11　在两种控制下控制力矩对比图路径,但之后又快速回到期望轨迹中,且偏离误差较小㊂图6~图8为载重无人机在x,y,z轴方向上位置跟踪误差的变化曲线㊂仿真实验对非奇异终端滑模算法与传统滑模算法的控制效果做了对比㊂由图6~图8可见,在非奇异终端滑模方法下,载重无人机在4s内可由初始位置飞行进入到预定轨迹,与传统滑模方法相比,时间缩短了1s㊂且三次卸载对无人机的x轴和y轴位置上的误差影响不大,但会对垂直方向的z轴坐标位置造成较大的偏差,相比传统滑模,非奇异终端滑模算法使偏差值能更快地收敛到平衡位置附近㊂由此验证了在无人机飞行运动和质量突变时,本文设计的外环控制器较传统滑模控制具有更好的动态性能㊂图9为四旋翼载重无人机的质量变化图㊂由图9可见,三次卸载所产生的质量突变㊂初始载重无人机总质量为9.5kg,而估计值m^为8kg,在自适应律作用下,两次卸载之后,无人机总质量估计偏差减小很多㊂最终完成三次卸载后的无人机总质量为5 kg,且估计偏差非常小㊂图10为总升力对比图,图11为控制输入的力矩变化对比图㊂从图10㊁图11的仿真结果研究可知,无人机进行了三次卸载,这三个时刻的总升力和控制力矩都发生较为明显的抖振,而在改进非线性函数Lfal(㊃)作用下,改进型自抗扰控制器的控制抖振效果明显减小,更有利于无人机的飞行姿态稳定㊂由仿真实验最终结果可知,本文所设计的算法,在四旋翼载重无人机飞行轨迹跟踪控制中,抗扰动性较好,系统响应平稳快速㊂4　结　语本文以四旋翼载重无人机为研究对象,针对其在飞行运动和卸载过程中出现的外界干扰和质量变化,设计了一种内外环的控制策略㊂其外环位置子系统采用自适应非奇异终端滑模算法,无人机在受到外界干扰和质量突变时,系统位置误差能够快速收敛,达到平衡;其内环姿态子系统中设计了一种改进型自抗扰控制器,可有效削弱系统抖振,更好地增强了系统的鲁棒性㊂最后通过与自适应滑模和经典自抗扰控制算法进行MATLAB/Simulink仿真对比,验证了本文所提控制策略的优越性㊂参考文献[1]　SI W,SHE H,WANG Z.Fuzzy PID controller for UAV trackingmoving target[C]//201729th Chinese Control and Decision Con⁃ference(CCDC).IEEE,Chongqi,China,2017:1260-1264. [2]　周来宏,窦景欣,张居乾,等.基于改进反步法的四旋翼无人机轨迹跟踪控制[J].东北大学学报(自然科学版),2018,39(1):66-70.[3]　唐堂,罗晓曙,吕万德,等.四旋翼无人机滑模自抗扰控制[J].广西师范大学学报(自然科学版),2018,36(2):56-62. [4]　冯恺鹏,贾云飞,柴金宝,等.四旋翼无人机模糊自适应滑模控制[J].飞行力学,2018,36(6):49-53.[5]　窦景欣,孔祥希,闻邦椿.四旋翼无人机模糊自抗扰姿态控制及稳定性分析[J].中国惯性技术学报,2015,23(6):824-830.[6]　CAI Z H,LOU J,ZHAO J,et al.Quadrotor trajectory tracking a-nd obstacle avoidance by chaotic greywolf optimiza-tion-based ac⁃tive disturbance rejection control[J].Mechanical Systems and Sig⁃nal Processing,2019,128:636-654.[7]　CHEN G,JIN B,CHEN Y.Accurate and robust body positiontrajectory tracking of six-legged walking robots with nonsingular terminal sliding mode control method[J].Applied Mathematical Modelling,2020,77:1348-1372.[8]　许璟,蔡晨晓,李勇奇,等.小型四旋翼无人机双闭环轨迹跟踪与控制(英文)[J].控制理论与应用,2015,32(10):1335-1342.[9]　ZHAO L,DAI L W,XIA Y Q,et al.Att-itude control for quadro⁃tors subjected to wind disturbances via active disturbance rejection control and integral sliding mode control[J].Mechanical Systems and Signal Processing,2019,129:531-545.[10]　MOUSSA L,MOHAMED C.Robust adaptive nonsingular fast ter⁃minal sliding mode tracking control for an uncertain quadrotorUAV subjected to disturbances[J].ISA Transactions,2020(99):290-304.作者简介:罗鼎馨(1989 ),男,硕士研究生,主要研究方向为先进控制理论与应用,无人机轨迹跟踪㊂。

２０２０年８月第３８卷第４期西北工业大学学报ＪｏｕｒｎａｌｏｆＮｏｒｔｈｗｅｓｔｅｒｎＰｏｌｙｔｅｃｈｎｉｃａｌＵｎｉｖｅｒｓｉｔｙＡｕｇ．Ｖｏｌ．３８２０２０Ｎｏ．４ｈｔｔｐｓ：／／ｄｏｉ．ｏｒｇ／１０．１０５１／ｊｎｗｐｕ／２０２０３８４０７０５收稿日期：２０１９⁃１０⁃０８基金项目：国家自然科学基金（１１６７２２３６）资助作者简介：宋宏川（１９８９ ），西北工业大学博士研究生，主要从事飞行器设计研究㊂基于四元数和动态逆的无人机精确航迹控制宋宏川１，詹浩１，夏露１，张楠２（１．西北工业大学航空学院，陕西西安㊀７１００７２；２．西安地平线电子科技有限公司，陕西西安㊀７１００７２）摘㊀要：精确跟踪飞机飞行性能边界内剧烈变化的三维航迹是无人机应用于空战的关键问题，这要求飞机的飞行控制律设计不仅要适应动力学的非线性特性还要解决飞行过程中的奇异性问题㊂在非线性动态逆（ＮＤＩ）理论的基础上探讨了连续航迹的控制问题，聚焦于飞机飞行速度和航迹角描述航迹产生的奇异性（也称万向节死锁），针对当飞机在沿引发奇异性的连续航迹飞行时，航迹坐标轴系会出现突变的现象，提出了一种修正航迹轴系的方法来解决航迹坐标轴系经过奇异点突变的问题㊂最终，设计了基于四元数的机动产生器，结合提出的修正航迹轴系方法，实现了不会产生奇异性问题的精确航迹控制，并且取得了良好的跟踪效果㊂关㊀键㊀词：非线性动态逆；机动产生器；航迹跟踪；奇异性；四元数中图分类号：Ｖ２１１．３㊀㊀㊀文献标志码：Ａ㊀㊀㊀文章编号：１０００⁃２７５８（２０２０）０４⁃０７０５⁃１０㊀㊀控制飞机在飞行性能边界范围内精确追踪三维空间中剧烈变化的航迹是无人机自主空中格斗的一个关键问题㊂在追踪三维航迹的过程中，飞机可能会遇到动力学强烈的非线性高耦合特性㊁奇异性（万向节死锁）㊁持续高过载㊁短时间内剧烈的速度高度以及过载变化等问题㊂飞机在大迎角（偏离线性化的平衡点）飞行时，基于传统增益调参方法（ｇａｉｎ⁃ｓｃｈｅｄｕｌｅｄ）的飞行控制律会产生非线性和高耦合的问题，所以需要新的控制方法设计飞行控制律㊂非线性动态逆方法［１⁃３］是一种状态反馈线性化方法㊂通过构造系统状态的非线性反馈对消被控对象的非线性，从而构成全局线性化；然后，在伪线性系统的基础上，可以使用线性化系统的设计方法，实现所需的系统响应㊂因而非线性动态逆方法是一种一般化的通用控制策略和方法［４］，已经被广泛运用在飞行控制律设计上，尤其是在军用飞机的控制律设计上［５］㊂例如美军目前最先进的Ｆ⁃３５战斗机平台采用的就是非线性动态逆飞行控制律设计方法［６］㊂从２０世纪８０年代末以来，国内外研究者应用非线性动态逆方法在飞行控制律设计领域做了大量的研究工作㊂Ｓｎｅｌｌ等［３］结合非线性动态逆与奇异摄动理论，把飞机的飞行参数分为快变量（［ｐ，ｑ，ｒ］Ｔ）㊁较慢变量（［α，β，μ］Ｔ）和慢变量（［Ｖ，γ，χ］Ｔ），分别对应上述变量设计快㊁较慢回路（也称内㊁外回路）和机动产生器，详细讨论了快回路和较慢回路的设计㊂参考文献［７⁃８］都对机动产生器做了详细的介绍，但文献［７］主要研究的是不会产生奇异性的着陆控制系统，因此没有涉及到机动产生器的奇异性问题㊂文献［８］提及使用［Ｖ，γ，χ］Ｔ表示航迹会在γ＝ʃ９０ʎ时引发奇异性问题，但因为研究的机动并不会经过γ＝ʃ９０ʎ，所以也没有对奇异性问题进行深入的讨论㊂很多国内外研究者采用四元数代替欧拉角表示飞机的姿态，来解决飞机在飞行过程中出现的奇异性问题㊂Ｒｏｂｉｎｓｏｎ在文献［９］中介绍了如何使用四元数（ｑｕａｔｅｒｎｉｏｎ）表示刚体的姿态㊁四元数和欧拉角的关系以及四元数的运算法则㊂文献［１０］详细讨论了当使用欧拉角表示刚体姿态时产生的奇异性（ｓｉｎｇｕｌａｒｉｔｉｅｓ）也称万向节死锁（ｇｉｍｂａｌｌｏｃｋ）问题㊂参考文献［１１］引入四元数来解决飞行过程中出现的姿态回路（［α，β，μ］Ｔ）奇异性问题，在此基础上设计了基于四元数的控制律架构，但文中并未讨论基于四元数的航迹回路（［Ｖ，γ，χ］Ｔ）设计㊂同时，文西㊀北㊀工㊀业㊀大㊀学㊀学㊀报第３８卷献［１１］对航迹倾斜角μ的多值问题也没有进行讨论㊂本文简要介绍了机动产生器的设计，深入探讨了航迹倾斜角μ的求解，并对航迹坐标轴系（本文简称为航迹轴系，用ｎ表示）在飞机沿着会引发奇异性航迹飞行时，产生突变的现象进行了研究㊂为了解决航迹轴系的突变问题，对现有的航迹轴系进行修正，提出了修正航迹坐标轴系（本文简称为修正航迹轴系，用ｍ表示）㊂最后结合四元数和非线性动态逆理论的思想对机动产生器进行设计，实现了基于四元数的机动产生器，解决了连续航迹控制的奇异性问题㊂１　动态逆控制器设计一般非线性系统（被控对象）都可表达为状态量㊁控制量和输出量的形式̇ｘ＝Ｆ（ｘ，ｕ）ｙ＝ｈ（ｘ）（１）式中：Ｆ（ｘ，ｕ）为状态量ｘ和控制量ｕ的非线性函数；ｈ（ｘ）是关于状态量ｘ的非线性函数㊂如果系统可以被描述成微分几何理论中的仿射非线性形式̇ｘ＝ｆ（ｘ）＋ｇ（ｘ）ｕｙ＝ｈ（ｘ）（２）与一般非线性系统相比，仿射非线性系统中状态的导数关于控制输入ｕ是线性的（（２）式）㊂若ｇ（ｘ）可逆，令ｕ＝ｇ－１（ｘ）［－ｆ（ｘ）＋ｖ］，则̇ｘ＝ｖ㊂假设ｖ＝̇ｘｄ＝ω（ｘｃ－ｘ）（３）式中：ω为系统的频带或增益；̇ｘｄ表示期望状态变化率；ｘｃ表示指令状态量，系统成为一阶多变量线性解耦结构㊂期望闭环动力学系统为一阶环节的非线性动态逆控制器的控制框图如图１所示［４］㊂图１㊀ＮＤＩ控制器结构利用非线性动态逆设计飞行控制律，主要有２种综合方法，本文采用的是状态反馈型方法［４］㊂直接应用状态反馈型方法，必须满足状态量的维数和控制量维数相同，常规布局的固定翼飞机一般有１２个状态量（见（４）式）以及４个控制量（见（５）式）㊂ｘ＝［ｐ，ｑ，ｒ，α，β，μ，Ｖ，γ，χ，ｘ，ｙ，ｚ］Ｔ（４）ｕ＝［δＴ，δａ，δｅ，δｒ］Ｔ（５）结合奇异摄动理论，即状态变量的时标分离方法，飞机状态量（见（４）式）可以分解成快慢不同的４组：［ｐ，ｑ，ｒ］Ｔ为飞机转动角速度矢量，ｐ，ｑ，ｒ分别表示飞机角速度在机体坐标轴系纵㊁横和竖轴的分量，该组变量变化快，称为快变量㊂［α，β，μ］Ｔ为相对于风轴的姿态矢量，α，β，μ分别表示飞机迎角㊁侧滑角和航迹倾斜角，该组变量比角速度矢量变化慢，称为较慢变量㊂［Ｖ，γ，χ］Ｔ为相对于地轴的速度矢量，Ｖ，γ，χ分别表示飞机飞行速度㊁爬升角和航迹方位角，变化慢，称慢变量㊂［ｘ，ｙ，ｚ］Ｔ为飞机对地位置矢量㊂［δＴ，δａ，δｅ，δｒ］Ｔ为飞机的控制量，δＴ是飞机油门控制量，δａ，δｅ，δｒ分别是飞机副翼㊁升降舵和方向舵偏角㊂将３组不同快慢的变量，用状态反馈的综合控制方法进行设计，可形成３组不同频带的子系统，根据奇异摄动理论，当频带相差３ ５倍时，在工程上回路之间的影响忽略不计，可以各自独立设计回路㊂快回路的仿射非线性形式̇ｘｆ＝ｆｆ（ｘｆ）＋ｇｆ（ｘｆ）ｕｆ（６）式中：下标ｆ代表快回路；ｘｆ表示快回路的状态变量（ｘｆ＝［ｐ，ｑ，ｒ］Ｔ）；ｕｆ表示快回路的控制变量（ｕｆ＝［δａ，δｅ，δｒ］Ｔ）㊂假设快回路期望的闭环动力学系统为一阶环节ｖｆ＝̇ｐｄ̇ｑｄ̇ｒｄéëêêêêùûúúúú＝ωｐ０００ωｑ０００ωｒéëêêêêùûúúúúｐｃ－ｐｑｃ－ｑｒｃ－ｒéëêêêêùûúúúú（７）ｖｆ表示期望快变量变化率；̇ｐｄ，̇ｑｄ，̇ｒｄ表示期望角速度变化率；ωｐ，ωｑ，ωｒ分别表示滚转㊁俯仰和偏航角速度通道的带宽；ｐｃ，ｑｃ，ｒｃ表示指令角速度㊂根据动态逆原理，为了获得̇ｐｄ，̇ｑｄ，̇ｒｄ，控制输入ｕｆ应为ｕｆ＝ｇ－１ｆ（ｘｆ）［－ｆｆ（ｘｆ）＋ｖｆ］（８）同理较慢回路的仿射非线性形式㊃６０７㊃第４期宋宏川，等：基于四元数和动态逆的无人机精确航迹控制̇ｘｍ＝ｆｍ（ｘｍ）＋ｇｍ（ｘｍ）ｕｍ（９）式中：下标ｍ代表较慢回路；ｘｍ表示较慢回路的状态变量（ｘｍ＝［α，β，μ］Ｔ）；ｕｍ表示较慢回路的控制变量（ｕｍ＝［ｐ，ｑ，ｒ］Ｔ）㊂假设较慢回路期望的闭环动力学系统为一阶环节ｖｍ＝̇μｄ̇αｄ̇βｄéëêêêêùûúúúú＝ωμ０００ωα０００ωβéëêêêêùûúúúúμｃ－μαｃ－αβｃ－βéëêêêêùûúúúú（１０）ｖｍ表示期望较慢变量变化率；̇αｄ，̇βｄ，̇μｄ分别表示期望迎角变化率㊁期望侧滑角变化率和期望航迹倾斜角变化率；ωα，ωβ，ωμ分别表示迎角㊁侧滑角和航迹倾斜角通道的带宽；αｃ，βｃ，μｃ分别表示指令迎角㊁指令侧滑角和指令航迹倾斜角㊂根据动态逆原理，为了获得̇αｄ，̇βｄ，̇μｄ，控制输入ｕｍ应为ｕｍ＝ｇ－１ｍ（ｘｍ）［－ｆｍ（ｘｍ）＋ｖｍ］（１１）２㊀机动产生器设计慢变量［Ｖ，γ，χ］Ｔ属于轨迹控制，对应状态反馈动态逆方法的慢回路设计，参考文献［１２］使用慢变量来描述飞机的轨迹指令，也称对应的控制器为机动产生器㊂后来关于慢回路的论文基本沿用了这个名称㊂机动产生器的设计思想与快㊁较慢回路相似，但是机动产生器的控制输入［α，β，μ］Ｔ在（１２）式中以三角函数的形式存在，而且气动力（升力Ｌ，阻力Ｄ和侧力Ｙ）是状态控制输入［α，β，μ］Ｔ的非线性函数，因此航迹轴系下力方程（１２）式无法写为类似（８）式和（１１）式的仿射非线性形式［１３］，不能直接运用动态逆理论进行求解，只能通过简化航迹轴系下力方程进行求解［８］㊂ｍ̇Ｖ＝Ｔｃｏｓβｃｏｓα－Ｄ－ｍｇｓｉｎγｍＶ̇χｃｏｓγ＝Ｔ（ｓｉｎαｓｉｎμ－㊀ｓｉｎβｃｏｓαｃｏｓμ）＋Ｙｃｏｓμ＋ＬｓｉｎμｍＶ̇γ＝Ｔ（ｓｉｎαｃｏｓμ＋ｓｉｎβｃｏｓαｓｉｎμ）－㊀Ｙｓｉｎμ＋Ｌｃｏｓμ－ｍｇｃｏｓγ（１２）㊀㊀本文不考虑风的影响，航迹迎角αｋ和迎角α重合，航迹侧滑角βｋ和侧滑角β重合，航迹速度（地速）Ｖｋ和飞行速度Ｖ大小相等，不加以区分㊂（１２）式中ｍ表示飞机质量，ｇ表示重力加速度，̇Ｖ表示飞行速度变化率，̇χ表示航迹方位角变化率，̇γ表示爬升角变化率，Ｔ表示推力大小，沿机体坐标轴系ｘ轴正方向，Ｄ表示阻力大小，沿气流坐标轴系ｘ轴负方向，Ｌ表示升力大小，沿气流坐标轴系ｚ轴负方向，Ｙ表示侧力大小，沿气流坐标轴系ｙ轴正方向㊂假设飞行过程中侧力Ｙ和侧滑角β都为０，航迹轴系下力方程可以简化为以下形式ｍ̇Ｖ＝Ｔｃｏｓα－Ｄ－ｍｇｓｉｎγｍＶ̇χｃｏｓγ＝（Ｔｓｉｎα＋Ｌ）ｓｉｎμｍＶ̇γ＋ｍｇｃｏｓγ＝（Ｔｓｉｎα＋Ｌ）ｃｏｓμ（１３）把（１３）式中的̇Ｖ，̇χ，̇γ用其期望值̇Ｖｄ，̇χｄ，̇γｄ代替，假设机动产生器的闭环系统为一阶环节̇Ｖｄ̇γｄ̇χｄéëêêêêùûúúúú＝ωＶ０００ωγ０００ωχéëêêêêùûúúúúＶｃ－Ｖγｃ－γχｃ－χéëêêêêùûúúúú（１４）ωＶ，ωγ，ωχ分别对应回路的带宽；Ｖｃ，γｃ，χｃ分别表示指令飞行速度，指令爬升角和指令航迹方位角㊂根据简化方程（１３）式和（１４）式，解得指令航迹倾斜角μｃμｃ＝ｔａｎ－１Ｖ̇χｄｃｏｓγＶ̇γｄ＋ｇｃｏｓγ（１５）再根据（１３）式和（１４）式构建只有一个未知数指令迎角αｃ的方程ｔａｎαｃｃｏｓμｃ（ｍ̇Ｖｄ＋Ｄ（Ｖ，αｃ）＋ｍｇｓｉｎγ）＋㊀Ｌ（Ｖ，αｃ）ｃｏｓμｃ－ｍＶ̇γｄ－ｍｇｃｏｓγ＝０（１６）式中：Ｄ（Ｖ，αｃ）表示阻力为飞行速度Ｖ和指令迎角αｃ的函数；Ｌ（Ｖ，αｃ）表示升力为飞行速度Ｖ和指令迎角αｃ的函数㊂在侧力和侧滑角都为０的假设下，根据（１５）式和（１６）式，已知航迹指令［Ｖｃ，γｃ，χｃ］Ｔ，求解得到慢回路的指令值［αｃ，βｃ，μｃ］Ｔ（βｃ＝０）㊂机动产生器㊁较慢回路以及快回路［３⁃４］构成了无人机航迹追踪的控制回路㊂３㊀航迹倾斜角的多解问题当利用反正切函数求解指令航迹倾斜角μｃ（见（１５）式），反正切函数的值域为（－９０ʎ，９０ʎ），小于实际飞行中μ的变化范围㊂如果直接用反正切函数求解，μ在绝对值接近９０ʎ时产生指令跳变，会导致仿真过程中飞行参数的发散［８］㊂图２ａ）表示飞机被控制向右上方运动（̇χｄ和̇γｄ㊃７０７㊃西㊀北㊀工㊀业㊀大㊀学㊀学㊀报第３８卷都大于０），图２ｂ）表示飞机被控制向左下方运动（̇χｄ和̇γｄ都小于０）），假设２个图中的飞行状态经过（１５）式中的公式计算ｔａｎμ的值都等于３㊂如果直接使用（１５）式的反正切函数求解μ，只能求解出图２ａ）中６０ʎ的航迹倾斜角，与实际飞行相比会漏掉图２ｂ）中倾斜角为－１２０ʎ的飞行状态㊂图２㊀航迹倾角的多解示意图为了解决（１５）式计算的航迹倾斜角值域与实际飞行航迹倾斜角值域不一致的问题，本文将从飞机受力与运动的角度讨论如何让μ的计算值与实际值为一一映射㊂在侧滑角为０的假设下，飞机只受到推力Ｔ㊁阻力Ｄ㊁升力Ｌ和重力Ｇ的作用㊂飞机所受重力之外的合力沿速度方向和垂直于速度方向分解，分别为（Ｔｃｏｓα－Ｄ）和（Ｔｓｉｎα＋Ｌ）㊂因为人的生理限制，要避免承受负过载㊂有人驾驶飞机做机动时，一般都是采用拉杆的方式，所以认为（Ｔｓｉｎα＋Ｌ）大于０㊂根据（１３）式，ｍＶ̇χｄｃｏｓγ表示（Ｔｓｉｎα＋Ｌ）在航迹轴系ｙｎ轴的分量，－（ｍＶ̇γｄ＋ｍｇｃｏｓγ）表示（Ｔｓｉｎα＋Ｌ）在航迹轴系ｚｎ轴的分量㊂通过（１５）式的值㊁Ｖ̇χｄｃｏｓγ的方向和Ｖ̇γｄ＋ｇｃｏｓγ的方向就能唯一确定μｃ的值㊂因此，μｃ的值不仅由（１５）式决定，还与Ｖ̇χｄｃｏｓγ和Ｖ̇γｄ＋ｇｃｏｓγ的方向有关㊂μｃ的计算方法与计算机编程语言中的ａｔａｎ２函数的计算方法一致，还与ａｔａｎ２函数的值域也一致㊂本文把（１５）式中的ｔａｎ－１替换为ａｔａｎ２函数求解，如（１７）式所示μｃ＝ａｔａｎ２（Ｖ̇χｄｃｏｓγ，Ｖ̇γｄ＋ｇｃｏｓγ）（１７）式中ａｔａｎ２（ｙ，ｘ）＝ｔａｎ－１ｙｘæèçöø÷㊀㊀㊀ｘ＞０ｔａｎ－１ｙｘæèçöø÷＋π㊀ｙȡ０，ｘ＜０ｔａｎ－１ｙｘæèçöø÷－π㊀ｙ＜０，ｘ＜０π２㊀㊀㊀㊀㊀㊀ｙ＞０，ｘ＝０－π２㊀㊀㊀㊀㊀ｙ＜０，ｘ＝０ｕｎｄｅｆｉｎｅｄ㊀㊀㊀ｙ＝０，ｘ＝０ìîíïïïïïïïïïïïïïï㊀㊀相比于有人驾驶飞机，无人机可以不用考虑人承受过载的限制，在机动过程中对于（Ｔｓｉｎα＋Ｌ）的正负没有限制㊂无人机μｃ计算以及机动方式都与有人驾驶飞机有所不同㊂假设飞机先向着̇χｄ和̇γｄ都大于０的方向飞行，如图２ａ）所示，再在某一时刻令飞机向̇χｄ和̇γｄ都小于０的方向飞行，如图２ｂ）所示㊂如果是有人驾驶飞机完成上述过程，为保证过载为正，当飞机从向右上飞行改变为向左下飞行时，飞机应该先从图２ａ）中的姿态滚转到图２ｂ）的姿态，再以正过载向̇χｄ和̇γｄ都小于０的方向飞行，这与使用ａｔａｎ２函数计算的结果一致㊂如果无人机允许使用负过载的方式完成机动动作，当此无人机同样从̇χｄ和̇γｄ都大于０的飞行方向改为̇χｄ和̇γｄ都小于０的飞行方向时，它有２种机动选择，一种与有人驾驶飞机的机动过程相同，先滚转，再以正过载飞行㊂另一种是飞机不改变航迹倾斜角，直接以图２ａ）的航迹倾斜角以负过载运动㊂第一种飞行方式在改变飞行状态时，为了精确控制航迹，需要飞机先把过载从正过载卸载至０，再滚转到图２ｂ）的姿态，再把过载从０加载至需用过载㊂而第二种飞行方式，飞机不用滚转，直接以图２ａ）的航迹倾斜角，从正过载变为负的需用过载㊂显然相比于第一种飞行方式，第二种飞行方式滚转的角度更小，过载指令的变化更快，达到指令航迹的时间更短㊂综上，无人机以负过载飞行时在机动性上更有优势㊂所以在实际应用过程中，为了能够更快机动，取以上２个值中与当前μ相近的那个值，如（１８）式所示㊂㊃８０７㊃第４期宋宏川，等：基于四元数和动态逆的无人机精确航迹控制μ１＝ａｔａｎ２（Ｖ̇χｄｃｏｓγ，Ｖ̇γｄ＋ｇｃｏｓγ）μ２＝μ１＋πμｃ＝μ１，｜μ１－μ｜ɤ｜μ２－μ｜μ２，｜μ１－μ｜＞｜μ２－μ｜{（１８）式中：μ１表示保持正过载机动方式对应的指令航迹倾斜角；μ２表示负过载机动方式对应航迹倾斜角㊂４　奇异性问题使用［Ｖ，γ，χ］Ｔ表示连续航迹，在爬升角γ为正负９０ʎ时，地速矢量处于铅垂线上，在水平面的投影退化成一个点，航迹方位角χ没有定义，此即奇异性问题㊂本文把爬升角为正负９０ʎ的点称为奇异点㊂使用四元数代替欧拉角表示刚体的姿态［１０⁃１１］是解决奇异性问题最常用的方法㊂为了能够避免出现奇异性问题，使用四元数ｑｅ２ｎ替代［０，γ，χ］Ｔ表示航迹轴系相对飞机牵连铅垂地面坐标轴系（本文简称为飞机牵连地轴系，用ｅ表示）的姿态，即用［Ｖ，ｑｅ２ｎ］Ｔ来表示三维空间的航迹㊂关于四元数的定义和运算规则可以参考文献［１４］㊂本文中的四元数定义为ｑ＝ｑ０ｉ＋ｑ１ｊ＋ｑ２ｋ＋ｑ３（１９）式中：ｑ０，ｑ１，ｑ２和ｑ３是实数；ｉ，ｊ和ｋ既是互相正交的单位向量，又是虚单位－１㊂（１９）式可以表示为矩阵形式［ｑ０，ｑ１，ｑ２，ｑ３］Ｔ㊂通常使用四元数代替欧拉角表示刚体姿态可以解决奇异性问题，但是利用四元数ｑｅ２ｎ并未能消除航迹轴系相对于飞机牵连地轴系姿态的奇异性问题（见图３）㊂图３㊀飞机经过奇异点航迹轴系突变示意图航迹轴系在国标ＧＢ／Ｔ１４４１０．１⁃２００８中的定义：原点通常固定于飞机的重心，其ｘｎ轴沿航迹速度（也称地速）方向，ｚｎ轴在包含ｘｎ轴的铅垂平面内，垂直于ｘｎ轴，指向下方，ｙｎ轴垂直于ｚｎｘｎ平面指向右方［１５］㊂按照上述航迹轴系的定义，当航迹轴系的ｘｎ轴垂直于水平面时（地速方向为铅垂方向），无法找到满足上述定义的ｚｎ轴㊂所以航迹轴系在地速为铅垂方向时，没有定义㊂相比于一般固联于飞机的坐标轴系，例如机体坐标轴系㊁气流坐标轴系等，他们与飞机牵连地轴系的相对姿态都是由３个姿态角（滚转㊁俯仰和偏航方向的姿态角）描述的㊂一般固联于飞机的坐标轴系相对于飞机牵连地轴系有３个旋转自由度，而航迹轴系相对于飞机牵连地轴系的姿态只有航迹方位角χ和爬升角γ２个姿态角来描述，只有２个旋转自由度，缺少了一个滚转方向的旋转自由度［１６］㊂当飞机经过奇异点时，航迹轴系在空间内产生突变，导致航迹轴系相对于飞机牵连地轴系的姿态产生突变，因此用来描述航迹轴系相对于飞机牵连轴系姿态的四元数ｑｅ２ｎ无可避免地产生突变㊂例如飞机只做单纯的俯仰运动时，在经过爬升角９０ʎ（奇异点之一）时，会引发航迹轴系绕ｘｎ旋转，并导致其突变㊂如图３所示，飞机从右到左以筋斗机动经过爬升角９０ʎ时，根据航迹轴系的定义，其ｙｎ轴和ｚｎ轴绕ｘｎ轴瞬间旋转了１８０ʎ㊂为了解决航迹轴系旋转自由度缺失引起的突变问题，在飞机经过奇异点时，补全航迹轴系的旋转自由度㊂即把图３中左边的航迹轴系在爬升角为９０ʎ时绕ｘｎ轴转动角度－１８０ʎ，这样旋转后的轴系和未经旋转的轴系，在整个机动过程中保证了姿态连续的变化，这个轴系称为修正航迹轴系㊂修正航迹轴系是在当前航迹轴系的基础上绕ｘｎ轴旋转－ｉ㊃１８０ʎ得到的㊂ｑｅ２ｍ＝ｑｅ２ｎˑｑｉ－π，ｉ＝０，１，２ （２０）式中：ｑｅ２ｍ表示修正航迹轴系相对飞机牵连地轴系姿态四元数；ｑｉ－π表示绕航迹轴系ｘｎ轴旋转－１８０ʎ四元数的ｉ次方，ｉ表示飞机在飞行过程中经过的奇异点次数，ˑ表示四元数乘法㊂㊃９０７㊃西㊀北㊀工㊀业㊀大㊀学㊀学㊀报第３８卷５　基于四元数的机动产生器设计假设ｑｃｏｍ表示期望姿态的指令四元数，ｑａｃｔ表示当前姿态的实际四元数，ｑａ２ｂ是表示坐标轴系Ｂ相对坐标轴系Ａ的姿态四元数，ｑａ２ｃ是表示坐标轴系Ｃ相对于坐标轴系Ａ的姿态四元数㊂假设坐标轴系Ｂ固联于飞机，坐标轴系Ａ是惯性轴系，所以坐标轴系Ｂ相对坐标轴系Ａ的姿态表示飞机的实际姿态，假设坐标轴系Ｃ相对于Ａ的姿态表示期望姿态，所以四元数ｑｃｏｍ，ｑａｃｔ，ｑａ２ｂ和ｑａ２ｃ有如下关系ｑａ２ｂ＝ｑａｃｔｑａ２ｃ＝ｑｃｏｍ（２１）㊀㊀要使飞机的实际姿态达到期望姿态，就是让与飞机固联的坐标轴系Ｂ旋转到坐标轴系Ｃ㊂根据四元数的计算法则以及性质，实际姿态四元数和期望姿态四元数关系如（２２）式［１１］所示ｑａｃｔˑｑｅｒｒ＝ｑｃｏｍｑｅｒｒ＝ｑ－１ａｃｔˑｑｃｏｍ（２２）式中，ｑｅｒｒ表示实际姿态与期望姿态的误差四元数㊂根据四元数的求导法则，（２２）式求导得到̇ｑａｃｔˑｑｅｒｒ＋ｑａｃｔˑ̇ｑｅｒｒ＝̇ｑｃｏｍ（２３）㊀㊀再根据四元数和其对应的角速度的关系̇ｑａｃｔ＝̇ｑａ２ｂ＝１２ｑａ２ｂˑωａｂｂ̇ｑｃｏｍ＝̇ｑａ２ｃ＝１２ｑａ２ｃˑωａｃｃ（２４）㊀㊀ωａｂｂ表示坐标轴系Ｂ相对于坐标轴Ａ的旋转角速率在坐标轴系Ｂ中的表示；ωａｃｃ表示坐标轴系Ｃ相对于坐标轴系Ａ的旋转角速率在坐标轴系Ｃ中的表示㊂由（２１）至（２４）式，可以得到ωａｂｂ＝－（２̇ｑｅｒｒ－ｑｅｒｒˑωａｃｃ）ˑｑ－１ｅｒｒ（２５）㊀㊀当ｑｅｒｒ－ｃｍｄ＝［０，０，０，１］Ｔ时，ｑａｃｔ＝ｑｃｏｍ，飞机达到期望的姿态㊂要使飞机的实际姿态到达期望的姿态，需要把ｑｅｒｒ控制到ｑｅｒｒ－ｃｍｄ㊂现在定义̇ｑｅｒｒ－ｄｅｓ为期望误差四元数的变化率，它与实际误差四元数ｑｅｒｒ和最终控制目标的误差四元数ｑｅｒｒ－ｃｍｄ的关系如下̇ｑｅｒｒ－ｄｅｓ＝ωｑΔｑｅｒｒ＝ωｑ（ｑｅｒｒ－ｃｍｄ－Ａｑｅｒｒ）（２６）式中：ωｑ是四元数回路的带宽㊂因为ｑｅｒｒ和－ｑｅｒｒ都是表示从实际坐标轴系旋转到期望坐标轴系的姿态四元数，ｑｅｒｒ表示绕固定轴旋转θ，－ｑｅｒｒ表示绕同一个轴反向旋转２π－θ㊂Ａｑｅｒｒ表示从实际姿态到期望姿态是经过θ和２π－θ中较小角度旋转的四元数，即ｑｅｒｒ和－ｑｅｒｒ中实部ｑ３ȡ０的四元数［１４］㊂如果飞机能够按照期望误差四元数变化率̇ｑｅｒｒ－ｄｅｓ运动，最终能够达到期望的姿态（见（２６）式）㊂为了达到̇ｑｅｒｒ－ｄｅｓ，飞机运动所需角速度为ωａｂｂ－ｃｍｄ［１１］ωａｂｂ－ｃｍｄ＝－（２̇ｑｅｒｒ－ｄｅｓ－ｑｅｒｒˑωａｃｃ）ˑｑ－１ｅｒｒ（２７）㊀㊀现在用ｑｅ２ｃ表示期望修正航迹轴系（本文用ｃ表示期望修正航迹轴系）相对于飞机牵连地轴系的期望姿态四元数，对应（２２）式中的ｑｃｏｍ㊂ｑｅ２ｍ表示实际修正航迹轴系相对于飞机牵连地轴系的实际姿态四元数，对应（２２）式中的ｑａｃｔ㊂现在把四元数ｑｅ２ｃ和ｑｅ２ｍ代入（２２）式得到ｑｅ２ｍ和ｑｅ２ｃ的误差四元数ｑｅｒｒ㊂ｑｅｒｒ＝ｑ－１ｅ２ｍˑｑｅ２ｃ（２８）㊀㊀把四元数ｑｅ２ｃ和ｑｅ２ｍ代入（２７）式，得到修正航迹轴系相对飞机牵连地轴系的指令角速度在修正航迹轴系的投影ωｅｍｍ－ｃｍｄωｅｍｍ－ｃｍｄ＝－（２̇ｑｅｒｒ－ｄｅｓ－ｑｅｒｒˑωｅｃｃ）ˑｑ－１ｅｒｒ（２９）式中，ωｅｃｃ表示期望修正航迹轴系相对于飞机牵连地轴系的角速度在期望修正航迹轴系中的分量㊂ωｅｃｃ无法直接得到，可以通过第ｋ步和第ｋ－１步的四元数ｑｅ２ｃ和（２４）式数值求解，如（３０）式所示［１１］ωｅｃｃ＝２ｑ－１ｅ２ｃｑｅ２ｃ（ｋ）－ｑｅ２ｃ（ｋ－１）Δｔ（３０）㊀㊀根据期望爬升角的变化率̇γｄ㊁期望航迹方位角的变化率̇χｄ和修正航迹轴系相对牵连地轴系的指令角速度ωｅｍｍ－ｃｍｄ＝［ｐｍ－ｃｍｄ，ｑｍ－ｃｍｄ，ｒｍ－ｃｍｄ］Ｔ的运动学关系得到̇γｄ＝ｑｍ－ｃｍｄｃｏｓ（－ｉπ）－ｒｍ－ｃｍｄｓｉｎ（－ｉπ）̇χｄｃｏｓγ＝ｑｍ－ｃｍｄｓｉｎ（－ｉπ）＋ｒｍ－ｃｍｄｃｏｓ（－ｉπ）（３１）式中：ｉ和（２０）式中的ｉ一样，表示经过奇异点的次数；［ｐｍ－ｃｍｄ，ｑｍ－ｃｍｄ，ｒｍ－ｃｍｄ］Ｔ分别表示修正航迹轴系相对牵连地轴系的角速度在修正航迹轴系ｘ，ｙ，ｚ轴的分量㊂基于修正航迹轴系相对飞机牵连地轴系的四元数控制架构如图４所示㊂图４中期望飞行速度变化率̇Ｖｄ和上面的四元数的推导是独立的，本文是通过简单的油门杆和速度㊃０１７㊃第４期宋宏川，等：基于四元数和动态逆的无人机精确航迹控制图４㊀修正航迹轴系相对地轴系的四元数控制架构误差的ＰＩＤ控制器进行控制的，也可以根据（１３）式中的方程求解推力指令Ｔｃ［７］，再根据发动机模型设计飞机的推力控制，详见参考文献［１７］㊂图４说明了基于修正航迹轴系相对于飞机牵连轴系的指令姿态四元数ｑｅ２ｃ和实际姿态四元数ｑｅ２ｍ，求解期望航迹角变化率̇γｄ和̇χｄ的过程㊂基于四元数机动产生器的设计结合提出的修正航迹轴系，保证了飞机在整个航迹追踪过程中不会出现奇异性，以实现对经过奇异点连续航迹的追踪㊂６㊀仿真试验筋斗机动在整个机动过程中会分别经过爬升角正负９０ʎ，因此选择控制飞机追踪筋斗机动的航迹来验证修正航迹轴系和四元数控制架构是否解决了连续航迹追踪的奇异性问题㊂６．１㊀验证航迹轴系突变图５给出了飞机执行筋斗机动过程中，爬升角γ和航迹方位角χ随时间变化的曲线㊂图６给出了四元数ｑｅ２ｎ和ｑｅ２ｍ随时间变化的曲线㊂其中ｑｉ，ｉ＝０，１，２，３ 表示四元数的各个分量，实线表示对应四元数ｑｅ２ｎ的分量，虚线表示对应四元数ｑｅ２ｍ的分量，竖直的虚线与图５中的竖直虚线对应，都表示飞机经过奇异点的时刻㊂图５㊀筋斗机动过程中爬升角和航迹方位角随时间变化图６㊀筋斗机动过程中四元数随时间变化㊃１１７㊃西㊀北㊀工㊀业㊀大㊀学㊀学㊀报第３８卷㊀㊀根据图５和图６可以看出四元数ｑｅ２ｎ会在爬升角为正负９０ʎ时产生突变，而四元数ｑｅ２ｍ则是连续变化的，而且在突变之前ｑｅ２ｎ＝ｑｅ２ｍ，与修正航迹轴系的定义相一致㊂图５和图６的仿真结果证明了单纯的使用四元数ｑｅ２ｎ代替欧拉角来表示连续航迹，还是无法避免出现奇异性问题㊂针对航迹轴系突变造成的奇异性问题，本文在航迹轴系基础上的提出修正航迹轴系，他相对于飞机牵连地轴系的姿态四元数ｑｅ２ｍ是连续变化的，避免了奇异性问题的发生，可以作为连续航迹控制问题的控制变量㊂６．２㊀验证基于四元数和修正航迹轴系的航迹追踪以图６中整个筋斗机动过程中的四元数ｑｅ２ｍ作为指令四元数ｑｃｏｍ，验证基于四元数的航迹追踪框架对于筋斗机动航迹的追踪效果㊂飞机在跟随指令四元数ｑｃｏｍ过程中，修正航迹轴系相对飞机牵连地轴系的姿态四元数ｑｅ２ｍ为飞机的实际四元数ｑａｃｔ㊂由于四元数的物理含义不直观所以绘制与四元数ｑａｃｔ和ｑｃｏｍ分别对应的实际航迹角㊁指令航迹角以及所对应的角度偏差来说明筋斗机动的跟踪效果，如图７所示㊂图７㊀指令航迹角与实际航迹角以及其对应的误差㊀㊀图７ａ）至７ｂ）说明在筋斗过程中，除了爬升角正负９０ʎ时，航迹方位角的误差值一直为０，这是因为在筋斗过程中，在经过奇异点之前，指令航迹方位角没有变化而且一开始实际航迹方位角与指令航迹方位角具有同样的初始值㊂爬升角经过正负９０ʎ时，航迹方位角产生了１８０ʎ的误差，这是因为实际爬升角滞后于指令爬升角产生的㊂图７ｃ）至７ｄ）显示实际爬升角相比于指令爬升角有最大达到近２ʎ的滞后㊂基于图６仿真结果的对比与分析，可以发现即使采用四元数ｑｅ２ｎ代替欧拉角表示航迹轴系相对飞机牵连地轴系的姿态，ｑｅ２ｎ在爬升角正负９０ʎ时还是会发生突变的现象，这正是上文讨论的航迹轴系的突变引起的，所以ｑｅ２ｎ是不能用来作为连续航迹的控制变量指令㊂而当采用本文提出的修正航迹轴系时，修正航迹轴系相对飞机牵连地轴系的姿态四元数ｑｅ２ｍ在整个机动过程中连续变化，可以用来作为连续航迹的控制变量指令㊂图７的仿真结果表示当使用［Ｖ，ｑｅ２ｍ］Ｔ作为航迹控制变量指令时，可以实现三维空间内经过奇异点的连续航迹控制，而且航迹跟踪的滞后量与滞后时间都很小，取得了良好的跟踪效果㊂７㊀结㊀论本文简要介绍了非线性动态逆理论与机动产生㊃２１７㊃。