基于四元数坐标变换法的火炮身管姿态伺服控制研究

韩崇伟，李 伟，陈腾飞，杨 刚，张志鹏

(西北机电工程研究所，陕西 咸阳 712099)



基于四元数坐标变换法的火炮身管姿态伺服控制研究

韩崇伟，李 伟，陈腾飞，杨 刚，张志鹏

(西北机电工程研究所，陕西 咸阳 712099)

某履带式自行火炮随动系统采用捷联惯性导航装置来检测火炮身管姿态，实现射角射向的高精度控制。为解决火炮随动系统高低向伺服和方位向伺服存在的控制耦合问题，采用四元数法，将捷联惯导测量值与射角射向主令诸元从地理坐标系下转换到车体坐标系下，分别解出方位向和高低向调转误差，通过位置控制器和电机驱动系统完成火炮身管姿态闭环控制。在车体初始姿态分别为水平和倾斜6°条件下进行仿真和试验。结果表明：采用四元数坐标变换法的火炮身管姿态伺服控制系统具有实现简单、调炮平稳、瞄准精确和超调小等技术特点，满足系统设计要求。

兵器科学与技术；自行火炮；捷联惯导；四元数；伺服系统

火炮身管姿态伺服控制系统包括方位向伺服和高低向伺服系统，采用捷联惯性导航装置(SINS)来直接检测火炮身管在地理坐标系下的姿态角(包括方位角、俯仰角和横滚角)，分别与SINS输出的方位角和俯仰角构成位置闭环系统。根据射角、射向求解误差，实现方位向伺服和高低向(俯仰向)伺服，可使火炮身管绕炮塔方向轴作回转运动、绕俯仰(耳)轴作俯仰转动。与采用火炮车体坐标系下测角反馈的传统伺服系统相比，实现身管姿态伺服的难点在于：惯导的安装方式造成方位向伺服和高低向伺服的控制不再独立而发生耦合，且都受车体姿态影响；车体为履带式底盘，采用油气悬挂，火炮身管在调转过程中的不平衡力矩易引起车体姿态变化，对姿态伺服系统形成干扰。因此，建立包含车体姿态扰动的身管姿态伺服控制模型十分必要。

文献[1-4]对火炮身管指向控制进行了研究和探讨，但未涉及伺服系统及车体姿态的干扰问题。文献[5]根据全闭环伺服操瞄控制的SINS测量反馈和操瞄主令诸元直接求差，将操瞄控制模型等效变换至平动坐标系后，耦合干扰、未知干扰等可看作是各伺服分系统的总干扰，采用自抗扰控制(ADRC)理论分别设计伺服分系统的控制器，对系统的不确定扰动进行实时估计，并实施补偿，从而解决操瞄系统伺服控制的耦合干扰对瞄准性能的影响问题，取得了良好的效果。但是ADRC的控制参数非常多，设计能够满足使用要求的控制器难度较大。

笔者根据火炮身管姿态伺服控制中方位向回转运动和高低向运动的特点，采用四元数法完成了其从地理坐标系到车体坐标系的转换，解出了火炮身管的方位向调转角和高低向调转角，通过位置控制器和电机驱动系统实现了身管姿态伺服控制。

1 坐标系定义

SINS平行于火炮身管且固定于火炮耳轴上，随火炮身管转动，炮塔可绕方位向回转轴左右转动，火炮身管在炮塔上可绕俯仰轴作上下起落运动，其简化图如图1所示。图中所涉及的坐标系包括地理坐标系、SINS机体坐标系、火炮身管俯仰部分坐标系、炮塔坐标系和车体坐标系。

地理坐标系Onxnynzn即SINS导航坐标系，将原点On定为SINS所处位置点，Onyn轴平行于水平面指向正北，Onxn轴平行于水平面指正东，Onzn轴垂直水平面向上。

SINS机体坐标系Obxbybzb的原点Ob为SINS惯性组件的中心， Obyb为SINS机体的纵轴，Obxb为SINS机体的横轴，Obzb垂直于Obyb， 组成的平面向上。

火炮身管俯仰部分坐标系Opxpypzp的原点Op为火炮俯仰转动(耳)轴与火炮身管俯仰平面的交点，Opxp轴与火炮身管轴线重合指向弹丸射向，Opyp轴与耳轴重合指向炮塔右侧，Opzp轴垂直于Opxp与Opyp构成的平面指向上。理想状态下，SINS机体坐标系Obxbybzb的横轴与耳轴平行,SINS机体坐标系Obxbybzb的纵轴与Opyp平行。火炮身管俯仰部分绕Opxp轴即耳轴旋转为εb角,称为高低角。

炮塔坐标系Ohxhyhzh的原点Oh为炮塔方位回转轴与方位回转平面的交点，Ohyh为火炮身管轴线向炮塔方向回转平面的投影，指向弹丸射向，Ohzh轴沿炮塔方向回转轴指向上，与Obzb平行，Ohxh轴垂直于Ohyh与Ohzh构成的平面向右。炮塔绕Ohyh轴转动的角度为βb角,称为方位角。

(1)

式中，ψ、θ和γ为SINS检测的姿态角,分别为方位角、俯仰角和横滚角。

各坐标系原点不重合，若需要可平移至重合，车体姿态也可以类似确定。

2 身管姿态伺服系统控制模型

在当前地理坐标系下的身管姿态和目标位置已知、车体姿态未知的情况下，利用四元数法进行坐标变换。根据身管指向的主令诸元(包括射角、射向)和当前的SINS输出的方位角和俯仰角，求解车体坐标系下的高低和方向误差量α、β，该过程为逆向计算法。

火炮身管在运动过程中，考虑不平衡力矩影响车体姿态变化，采用Langrange法建立履带式自行火炮车体及其悬挂、火炮俯仰部分等动力学模型，受篇幅限制，直接给出某履带式自行火炮身管姿态伺服系统控制原理，如图2所示。

式中，bh、bp分别为伺服被控对象的方向、高低控制增益。位置控制器均采用PI控制器。

3 四元数坐标变换法的控制误差求取

身管初始姿态时，已知SINS的姿态为ψ0、θ0、γ0，身管指向目标的主令诸元为ψ1、θ1，该过程在地理坐标系下用四元数可以表述为[8]

Q=Qγ1⊗Qψ⊗Qθ⊗Q-γ0

(2)

身管在方向伺服系统和高低伺服系统的带动下在车体坐标下分别调转α、β至目标指向，用四元数描述的最简过程为

Q=Qα⊗Qβ

(3)

根据坐标系的定义，火炮身管指向的调转运动为同一个旋转运动的不同坐标系下的数学描述，故式(2)、(3)旋转四元数等效，从而建立非线性方程组，化简如下：

(4)

跟等效旋转矢量Q的系数有关，限于篇幅，此处略。

由于非线性方程组(4)的个数多于未知数的个数，在此采用最小二乘解的广义逆法求解出PI位置器的控制误差量α、β，整个调炮过程车体皆保持水平情况下，很容易验证β=Δψ,α=Δθ，否则是不相等的。同时发现，采用四元数的坐标变换法算法略微复杂，现有的嵌入式计算机技术容易实现，但没有需要设计的参数，其运算的参数比如高低轴g的参数都根据惯导的参数实时精确确定，而方向轴f是根据不同时刻高低轴g叉乘得到。而对于整个火炮身管全闭环指向的控制的伺服系统PI位置控制器参数可根据常规方法并结合anti-windup控制方法设计。

方向轴(方向回转平面法向)和高低轴(火炮耳轴)在地理坐标系下的矢量分别为f=b1i+b2j+b3k，g=a1i+a2j+a3k。当SINS安装在身管后，其位置与耳轴的相对位置就固定，根据坐标系的定义，此时就可先标定在SINS横轴与耳轴矢量在俯仰部分坐标系下的关系。那么第l步g在火炮俯仰部分坐标系下的实时矢量为gp(l)=a1pi+a2pj+a3pk，其中a1p=a3p=0，a2p=1，那么其中实时高低轴矢量g(l)为

(5)

方向矢量的通用求解是根据高低矢量，即

(6)

式中，L为间隔的计算步距，根据求解的Δα动态确定，以适应车体姿态的缓慢变化，反应车体姿态的f变化也不快。由于SINS的姿态输出值含有较小不确定值，若Δα为小角度时，f的求解将不准确。由于f的变化比较缓慢，利用求解的Δα=|α0-αi|来确定L是完全可行的。α0、α1分别为改变确定f后的第1次求解值和最后1次求解值。

4 仿真与验证

自行火炮驻停时车体方位角为ψb=80°，火炮在车体系下的初始角度分别为：方位角βb=0°，高低角εb=0°，方位主令ψref=2 500mrad，高低主令θref=1 000mrad。高低伺服和方位伺服的驱动传递函数分别为：bh=337，bp=523，ah=25，ap=40。饱和幅值为10V，死区绝对值为0.008V。方向随动P参数为0.5，I参数为0.001；高低随动P参数为0.7，I参数为0.001。采用四元数坐标变换法求取控制误差量后，火炮身管姿态伺服控制分别在车体初始姿态水平(θb=0°，φb=0°)和车体初始姿态倾斜6°(θb=6°，φb=6°)的情况下进行仿真，得到结果如图3～6所示。

图3、4分别为车体初始姿态水平及姿态倾斜6°条件下调炮时车体姿态变化曲线，表明车体姿态随着伺服系统调转控制结束也趋于稳定至定值，伺服系统调转启动、制动加速度对车体姿态而言是较大的扰动。同时，车体姿态变化也是瞄准系统的扰动，由于车体悬挂系统的弹性变形导致车体姿态变化，放大了对操瞄控制系统的扰动。

图5、6分别为火炮身管姿态控制应用四元数坐标变化法后，在车体初始姿态水平及姿态倾斜6°条件下火炮身管在大地坐标下的调炮角度曲线。在车体初始姿态为水平情况下，高低伺服和方向伺服系统到位调整时间较短。而在车体初始倾斜6°条件下，方向伺服调整时间略长，有约10mrad的超调，但满足以0.1mrad误差作为到位评定标准下，其调炮时间满足系统要求，且调炮过程中未引起车体姿态振荡，车体姿态平稳变化趋于稳定。虽然采用四元数的坐标变化法，算法略微复杂，但对控制器的设计要求降低了。

图7、8分别为车体坐标系下的火炮身管指向在车体初始姿态水平及车体初始姿态倾斜6°条件下α、β的调转角度，与图5、6比较可知，β≠Δψ,α≠Δθ，即使在车体姿态初始姿态为水平的情况下，因为火炮身管在调转过程中的其不平衡力矩使车体姿态缓慢歪斜，不再水平，故通常情况下β≠Δψ,α≠Δθ。

5 结论

考虑到火炮身管姿态伺服控制系统中的各伺服分系统车体坐标系下存在速度耦合干扰，笔者采用四元数坐标变换法，将捷联惯导测量值与主令诸元从地理坐标系下转换到车体坐标下，进而求解火炮身管方位向和高低向调转角。这个过程无需其他参数，且对控制器的设计要求不高。仿真研究结果表明：采用四元数坐标变换法的火炮身管伺服系统能在有限时间内实现平稳、精确瞄准，超调小，满足系统设计要求。该模型的建立和控制方法的应用为此类系统的研究提供了另一种途径。

References)

[1]严德斌，胡朝根，陈红军．基于激光陀螺捷联惯导技术的火炮身管指向系统设计[J]．舰船电子工程，2011，31(9)：57-59. YAN Debin，HU Chaogen，CHEN Hongjun．Design of gun turntable system based on ring laser gyro strap-on inertial navigation system[J]．Ship Electronic Engineering， 2011，31(9)：57-59.(in Chinese)

[2]胡田，付庆红，佟俐．刚体姿态控制在火炮自动操瞄中的应用[J]．火力与指挥控制，2009，34(12)：148-150． HU Tian，FU Qinghong，TONG Li．Application of rigid body attitude control in gun’s automatic aiming[J]．Fire Control & Command Control，2009，34(12)：148-150.(in Chinese)

[3]GU Y L，LOH R N K，COLEMAB N,et al．Control of wea-pon pointing systems base on robotic formulation[C]∥ Proceeding of the 1992 American Control Conference. USA：IEEE, 1992:413-419.

[4]田建辉，钱林方，徐亚栋，等．无人炮塔火力线跟踪神经滑模控制[J]．兵工学报，2011，32(6)：641-645.

TIAN Jianhui，QIAN Linfang，XU Yadong，et al．Neural sliding mode control for tracking of axis of firepower of unmanned turret[J]．Acta Armamentarii, 2011，32(6)：641- 645.(in Chinese)

[5]李伟，杨刚，陈腾飞，等．某全闭环操瞄系统的火炮身管指向控制研究[J]．兵工学报，2015，36(9)：1811- 1818． LI Wei，YANG Gang，CHEN Tengfei，et al．Reasearch on gun pointing control of a full closed-loop aimming system[J]．Acta Armamentarii, 2015，36(9)：1811-1818.(in Chinese)

[6]喻凡．车辆动力学及其控制[M]．北京：机械工业出版社, 2013：174-182. YU Fan．Armored vehicle dynamics and control[M]．Beijing：China Machine Press, 2013:174-182.(in Chinese)

[7]IKENAGA S,LEWIS F L,CAMPOS J,et al．Active suspension control of ground vehicle based on a full-vehicle model[C]∥Proceedings of 2000 American Control Confe-rence．USA：IEEE，2000，6: 4019-4024．

[8]汤勇刚，杨建文，李宗良. 基于四元数法的火炮随动系统解耦控制[J].火力与指挥控制学报，2002，27(5) :75- 78． TANG Yonggang，YANG Jianwen，LI Zongliang．Decoup-ling control of fire control servo system based on quaternion method[J]．Fire Control & Command Control，2002，27(5): 75-78．(in Chinese)

Gun Tube Position Servo Control Based on Quaternion Coordinate Transformation Method

HAN Chongwei, LI Wei, CHEN Tengfei, YANG Gang, ZHANG Zhipeng

(Northwest Institute of Mechanical & Electrical Engineering, Xianyang 712099, Shaanxi, China)

Strapdown Inertial Navigation System (SINS) is used to measure the pointing of gun barrel for highly accurate aiming with tracked self-propelled artillery. However, it causes the control coupling of pitching servo and azimuth servo and the aiming property to be easily affected by vehicle posture. According to the full closed-loop aiming system model under vehicle coordinates, the servo control model under quasi geodetic coordinates including the disturbance of mismatched uncertainty is obtained. Active Disturbance Rejection Controllers (ADRC) is designed for each axial servo system, with the coupling and modeling error considered to be external disturbance. ADRC can be used to observe the external disturbance and compensate for the nonlinear state error controller to eliminate the overshoot caused by it. Simulation of servo aiming systems with the initial posture of vehicle being horizontal and inclination degree being 6 degrees is carried out respectively. The results show that compared with the adaptive sliding mode variable structure control, the servo system using ADRC strategy obtains more steady and more accurate targeting results, and gets smoother controller output, but suppresses the unmatched uncertain interface effectively, which satisfies the design requirements of the system.

ordnance science and technology;self-propelled artillery;SINS;quaternion;servo system

10.19323/j.issn.1673-6524.2016.04.003

2015-12-17

韩崇伟(1963—)，男，研究员级高级工程师，主要从事火炮伺服系统技术研究。E-mail：hangreat202@163.com

TJ818,TP273

A

1673-6524(2016)04-0010-05