CN109857130A - 一种基于误差四元数的导弹双回路姿态控制方法 - Google Patents

Abstract

本发明公开了一种基于误差四元数的导弹双回路姿态控制方法，通过期望欧拉姿态角求取期望姿态四元数，并通过导航系统给出导弹当前姿态四元数，将两者结合得到误差四元数，然后通过姿态四元数跟踪回路即外回路和弹体姿态角速度稳定回路即内回路，通过内回路和外回路结合，共同控制导弹姿态。本发明采用误差四元数进行导弹姿态控制器设计，避免了采用欧拉角设计引起的奇异问题和误差姿态角解耦问题，有利于导弹大机动全方位飞行。

Description

一种基于误差四元数的导弹双回路姿态控制方法

技术领域

本发明涉及一种导弹姿态控制方法，具体涉及一种基于误差四元数的导弹双回路姿态控制方法，属于导弹控制领域。

背景技术

传统导弹姿态控制方法大都采用欧拉角姿态信息，由导航系统输出导弹相对于导航坐标系的当前欧拉姿态角，由制导系统形成期望欧拉姿态角，姿控系统将期望姿态角与当前姿态角相减形成误差姿态角指令，再经过解耦后作为姿控外回路的控制输入。

传统运用误差姿态角进行姿态控制的方法有两个缺陷：

第一，运用欧拉角转动定义弹体坐标系在导航坐标系中的姿态在某些状态下会出现奇异值。若导航坐标系到弹体坐标系采用2-3-1(偏航、俯仰、滚动)的转动次序，俯仰角不能为90°，否则偏航角和滚动角无明确物理意义；若导航坐标系到弹体坐标系采用3-2-1(俯仰、偏航、滚动)的转动次序，偏航角不能为90°，否则俯仰角和滚动角无明确物理意义。对于大机动导弹来说，无论采用哪种转动次序描述导航坐标系到弹体坐标系的转换矩阵，都可能存在角度奇异问题。

第二，不能直接用误差姿态角作为姿控外回路的控制输入对导弹进行控制。导弹的执行机构安装在弹体上，执行机构动作后产生的控制力和力矩直接作用在弹体上，而误差姿态角由两个欧拉角相减得到，不能直接用于姿控回路的输入，否则将产生三通道控制耦合问题。解决该问题的方法通常是对误差姿态角进行解耦处理。

针对传统欧拉姿态角控制出现的问题，青岛海山海洋装备有限公司201610108631.4号专利公开了一种基于误差四元数反馈的ROV姿态控制方法，该方法针对无人遥控潜水器(Remote Operated Vehicle)三自由度旋转姿态进行建模，姿态传感器对ROV进行检测输出姿态四元数，与目标四元数对比求出误差四元数，输入PID控制器计算得出ROV坐标系上三个轴所需力矩，控制ROV姿态运动。该方法解决ROV的全角度控制问题，但没有给出控制量与误差四元数定量的对应关系，且简单的PID控制器不适用于导弹姿态控制。

“基于误差四元数的捷联惯导全姿态导航与控制”(中国惯性技术学报第15卷第4期2007年8月)一文针对飞行器控制提出由误差四元数直接控制舵偏输出(推力矢量输出)的方法，同样没有给出控制量与误差四元数定量的对应关系，且采用简单的PID控制器。

“一种基于误差四元数的战术导弹垂直发射姿态调转控制器”(宇航学报第25卷第1期2004年1月)一文针对导弹垂直发射姿态控制采用李亚普诺夫第二方法进行控制系统设计，得出了基于误差四元数反馈控制器，该方法的缺陷是需要知道被控对象的精确模型，且控制律形式复杂，不便于工程应用。

“基于四元数的垂直发射拦截导弹姿态自抗扰控制”(电光与控制第21卷第5期2014年5月)一文针对导弹垂直发射姿态控制设计了一种基于四元数的自抗扰姿态控制器，该方法的缺陷是需要设计的控制参数太多，且没有给出参数选取方法，不便于工程应用。

发明内容

本发明的目的在于克服现有技术的不足，提出一种基于误差四元数的导弹双回路姿态控制方法。

本发明具体是这样实现的：

一种基于误差四元数的导弹双回路姿态控制方法，是通过期望欧拉姿态角求取期望姿态四元数，并通过导航系统给出导弹当前姿态四元数，将两者结合得到误差四元数，然后通过姿态四元数跟踪回路即外回路，和弹体姿态角速度稳定回路即内回路，通过内回路和外回路结合，共同控制导弹姿态。

更进一步的方案是：

要预先定义导航坐标系oxyz和弹体坐标系o₁x₁y₁z₁

导航坐标系原点取发射瞬间弹体质心在地面的投影点；ox轴在发射点水平面内，指向发射瞄准方向；oy轴沿发射点的铅垂线，指向上；oz轴垂直于oxy平面，构成右手坐标系，该坐标系为导航参考系，与地球固连，随地球转动；弹体坐标系原点取火箭弹质心；ox₁轴沿火箭弹纵轴指向头部；oy₁轴在弹体纵向对称面内，垂直ox₁轴，指向上；oz₁轴垂直于ox₁y₁平面，构成右手坐标系。

更进一步的方案是：

通过期望欧拉姿态角求取期望姿态四元数的具体步骤如下：

定义导弹在导航坐标系中的三个姿态角分别为：俯仰角偏航角ψ，滚动角γ；将导弹运动需要到达的期望欧拉姿态角位置转换表示为四元数q_cx

q_cx＝[q_cx0 q_cx1 q_cx2 q_cx3] (1)

其中q_cx0为实部，q_cx1，q_cx2，q_cx3为三个虚部；

当采用3-2-1转动次序(导航坐标系到弹体坐标系的转动次序为俯仰、偏航、滚动)定义欧拉姿态角时，转换公式如下：

当采用2-3-1转动次序(导航坐标系到弹体坐标系的转动次序为偏航、俯仰、滚动)定义欧拉姿态角时，转换公式如下：

更进一步的方案是：

误差四元数的求解过程如下：

首先，获取导弹当前姿态四元数q₀

q₀＝[q₀₀ q₀₁ q₀₂ q₀₃]^T (4)

根据得到的导弹的当前姿态四元数q₀，导弹的期望姿态四元数q_cx，设所需转动四元数为q_e，则四元数运算法则可得

符号表示四元数乘法运算，将式(5)进行变换可得

展开可得

其中，导弹转动角度与误差四元数的对应关系如下：

欧拉定理表明，从一个坐标系转到另一个坐标系，可以通过绕空间某一瞬时轴转动一个角度来实现；假设从与q₀关联的坐标系绕3-2-1的顺序分别旋转Δψ_e，Δγ_e(其对应的四元数为q_e)到与q_cx关联的坐标系，得到

由于导弹每一周期都在控制，因此Δψ_e，Δγ_e为小量，由式(8)可推出：

若与q₀关联的坐标系绕2-3-1的顺序分别旋转Δψ_e，Δγ_e(对应的四元数为q_e不变)到与q_cx关联的坐标系，容易得到

通过(10)式同样可推出(9)式，因此不管绕哪一种顺序旋转，均可得到导弹转动角度与误差四元数的关系如下

可选择2·q_e1，2·q_e2，2·q_e3分别作为导弹绕弹体系ox₁，oy₁，oz₁轴的转动角度控制量。

更进一步的方案是：

通过内回路和外回路结合，共同控制导弹姿态，具体如下：

根据得到的导弹转动角度控制量2·q_e1，2·q_e2，2·q_e3，设计双回路姿态控制器，该控制器的内回路为弹体姿态角速度稳定回路，外回路为姿态四元数跟踪回路；控制器的目的是使误差四元数q_e1，q_e2，q_e3趋近于0，导弹当前姿态四元数跟踪期望姿态四元数；俯仰、偏航、滚动三通道控制方程如下：

其中，δ_γ、δ_ψ、为控制导弹姿态运动的滚动、偏航、俯仰通道舵偏；a^γ、a^ψ、分别为三通道外回路控制增益；W^γ(s)、W^ψ(s)、分别为三通道外回路矫正网络；分别为三通道内回路控制增益；ω_x、ω_y、ω_z分别为弹体滚动、偏航、俯仰角速度，由导航系统给出； 为陷波滤波网络，用于滤除弹体一阶弹性频率；RTD为弧度转度系数，RTD≈57.2957795。

本发明对比现有技术的优点在于：

(1)采用误差四元数进行导弹姿态控制器设计，避免了采用欧拉角设计引起的奇异问题和误差姿态角解耦问题，有利于导弹大机动全方位飞行；

(2)给出了导弹转动角度与误差四元数定量对应关系，便于对控制系统进行准确建模，在专利“一种基于误差四元数反馈的ROV姿态控制方法”和论文“基于误差四元数的捷联惯导全姿态导航与控制”中并未给出这种定量对应关系；

(2)比采用基于误差四元数的单回路PID控制器具有更好的性能，单回路PID控制器采用微分环节增强控制器的稳定性，在实际工程中，微分环节易引入高频测量噪声，影响控制器品质；

(3)比采用李亚普诺夫第二方法和自抗扰控制方法设计的控制器易于工程应用，采用李亚普诺夫第二方法需要精确知道被控对象的数学模型，否则不能保证设计出的控制器是否满足稳定性要求，自抗扰控制方法需要设计的参数过多，“基于四元数的垂直发射拦截导弹姿态自抗扰控制”一文中需要设计15个控制参数，且无现成的参数选取方法，大大增加了控制器的设计难度。

附图说明

图1是基于误差四元数的导弹双回路姿态控制流程图。

图2是“×”型尾翼气动布局导弹后视图。

图3是初始滚动角0°时导弹垂直发射坐标关系图。

图4是初始滚动角0°时导弹垂直发射舵机偏转示意图。

图5是初始滚动角90°时导弹垂直发射坐标关系图。

图6是初始滚动角90°时导弹垂直发射舵机偏转示意图。

具体实施方式

下面结合附图对本发明做进一步说明。

如附图1所示，本发明提供的基于误差四元数的导弹双回路姿态控制方法，包括：

(1)定义导航坐标系oxyz和弹体坐标系o₁x₁y₁z₁

导航坐标系原点取发射瞬间弹体质心在地面的投影点；ox轴在发射点水平面内，指向发射瞄准方向；oy轴沿发射点的铅垂线，指向上；oz轴垂直于oxy平面，构成右手坐标系，该坐标系为导航参考系，与地球固连，随地球转动；弹体坐标系原点取火箭弹质心；ox₁轴沿火箭弹纵轴指向头部；oy₁轴在弹体纵向对称面内，垂直ox₁轴，指向上；oz₁轴垂直于ox₁y₁平面，构成右手坐标系；

(2)求取导弹期望姿态四元数

定义导弹在导航坐标系中的三个姿态角分别为：俯仰角偏航角ψ，滚动角γ。将导弹运动需要到达的期望欧拉姿态角位置转换表示为四元数q_cx

q_cx＝[q_cx0 q_cx1 q_cx2 q_cx3] (1)

其中q_cx0为实部，q_cx1，q_cx2，q_cx3为三个虚部。

当采用3-2-1转动次序(导航坐标系到弹体坐标系的转动次序为俯仰、偏航、滚动)定义欧拉姿态角时，转换公式如下：

当采用2-3-1转动次序(导航坐标系到弹体坐标系的转动次序为偏航、俯仰、滚动)定义欧拉姿态角时，转换公式如下：

(3)获取导弹当前姿态四元数q₀

q₀＝[q₀₀ q₀₁ q₀₂ q₀₃]^T (4)

导弹当前姿态四元数q₀可由导航系统直接输出。

(4)求出误差四元数

通过前面两步，得到了导弹的当前姿态四元数q₀，导弹的期望姿态四元数q_cx，设所需转动四元数为q_e，则四元数运算法则可得

符号表示四元数乘法运算，将式(5)进行变换可得

展开可得

(5)导弹转动角度与误差四元数的对应关系

欧拉定理表明，从一个坐标系转到另一个坐标系，可以通过绕空间某一瞬时轴转动一个角度来实现。假设从与q₀关联的坐标系绕3-2-1的顺序分别旋转Δψ_e，Δγ_e(其对应的四元数为q_e)到与q_cx关联的坐标系，容易得到

由于导弹每一周期都在控制，因此Δψ_e，Δγ_e为小量，由式(8)可推出：

若与q₀关联的坐标系绕2-3-1的顺序分别旋转Δψ_e，Δγ_e(对应的四元数为q_e不变)到与q_cx关联的坐标系，容易得到

通过(10)式同样可推出(9)式，因此不管绕哪一种顺序旋转，均可得到导弹转动角度与误差四元数的关系如下

可选择2·q_e1，2·q_e2，2·q_e3分别作为导弹绕弹体系ox₁，oy₁，oz₁轴的转动角度控制量。

(6)设计导弹双回路姿态控制器

根据前面得到的导弹转动角度控制量2·q_e1，2·q_e2，2·q_e3，可以设计双回路姿态控制器，该控制器的内回路为弹体姿态角速度稳定回路，外回路为姿态四元数跟踪回路。控制器的目的是使误差四元数q_e1，q_e2，q_e3趋近于0，导弹当前姿态四元数跟踪期望姿态四元数。俯仰、偏航、滚动三通道控制方程如下：

其中，δ_γ、δ_ψ、为控制导弹姿态运动的滚动、偏航、俯仰通道舵偏；a^γ、a^ψ、分别为三通道外回路控制增益；W^γ(s)、W^ψ(s)、分别为三通道外回路矫正网络；分别为三通道内回路控制增益；ω_x、ω_y、ω_z分别为弹体滚动、偏航、俯仰角速度，由导航系统给出； 为陷波滤波网络，用于滤除弹体一阶弹性频率；RTD为弧度转度系数，RTD≈57.2957795。

本发明实施例提出的基于误差四元数的导弹双回路姿态控制方法，主要包括：期望欧拉姿态角到期望姿态四元数转换方法，误差四元数求取方法，导弹转动角度与误差四元数的对应关系求取方法和基于误差四元数的双回路姿态控制方程。

具体应用例1

假设某导弹采用“×”型尾翼气动布局，如附图2所示。导弹为垂直发射状态，如图3所示，当前俯仰姿态角为90°，滚动、偏航姿态角为0°。现需要对导弹在射面内进行转弯控制，朝ox方向飞行。下一时刻期望欧拉姿态角为ψ_cx＝0°，γ_cx＝0°(基于3-2-1转动次序定义)。

第一步，由公式(2)可得期望姿态四元数

q_cx＝[q_cx0 q_cx1 q_cx2 q_cx3]^T＝[0.7193398 0.0 0.0 0.6946584]^T

第二步，由导航系统可得导弹的当前姿态四元数

q₀＝[q₀₀ q₀₁ q₀₂ q₀₃]^T＝[0.7071068 0.0 0.0 0.7071068]^T

第三步，由公式(7)可得误差四元数

q_e＝[q_e0 q_e1 q_e2 q_e3]^T＝[0.9998477 0.0 0.0 0.0174524]^T

第四步，由公式(11)确定导弹姿态转动角度控制量

第五步，由公式(12)～(14)求出导弹控制舵偏

假设，a^γ、a^ψ、三通道外回路控制增益为1；W^γ(s)、W^ψ(s)、三通道外回路矫正网络为单位传函；三通道内回路控制增益为1；ω_x、ω_y、ω_z三通道弹体初始角速度为0； 陷波滤波网络为单位传函；由公式(12)～(14)计算可得

式(15)为计算出的控制舵偏，将其按照舵偏分配公式分配后传输给导弹的四个舵机，如图4所示，即可控制导弹姿态运动。从式(15)中可以看出，当滚动、偏航初始姿态角为0°时，控制导弹在射面(oxy平面)内转弯飞行，需要控制弹体俯仰舵偏。

具体应用例2

假设某导弹采用“×”型尾翼气动布局，为垂直发射状态，如图5所示，当前俯仰姿态角为90°，滚动姿态角为90°，偏航姿态角为0°。现需要对导弹在射面内进行转弯控制，朝ox方向飞行。下一时刻期望欧拉姿态角为ψ_cx＝0°，γ_cx＝90°(基于3-2-1转动次序定义)。

第一步，由公式(2)可得期望姿态四元数

q_cx＝[q_cx0 q_cx1 q_cx2 q_cx3]^T＝[0.50865 0.50865 0.4911976 0.4911976]^T

第二步，由导航系统可得导弹的当前姿态四元数

q₀＝[q₀₀ q₀₁ q₀₂ q₀₃]^T＝[0.5 0.5 0.5 0.5]^T

第三步，由公式(7)可得误差四元数

q_e＝[q_e0 q_e1 q_e2 q_e3]^T＝[0.9998477 0.0 0.0174524 0]^T

第四步，由公式(11)确定导弹姿态转动角度控制量

第五步，由公式(12)～(14)求出导弹控制舵偏

假设，a^γ、a^ψ、三通道外回路控制增益为1；W^γ(s)、W^ψ(s)、三通道外回路矫正网络为单位传函；三通道内回路控制增益为1；ω_x、ω_y、ω_z三通道弹体初始角速度为0； 陷波滤波网络为单位传函；由公式(12)～(14)计算可得

式(16)为计算出的控制舵偏，将其按照舵偏分配公式分配后传输给导弹舵机，如图6所示，即可控制导弹姿态运动。从式(16)中可以看出，与实施用例1不同，当滚动初始姿态角为90°，偏航姿态角为0°时，控制导弹在射面内转弯飞行，需要控制弹体偏航舵偏，基于误差四元数的姿态控制方法，在计算过程中自动完成了控制量解耦。

其他情形下的控制舵偏计算方法与以上两个实例相似，只需按照本发明给出的设计步骤计算即可。通过以上两个实例进一步说明了本发明的优点。

尽管这里参照本发明的解释性实施例对本发明进行了描述，上述实施例仅为本发明较佳的实施方式，本发明的实施方式并不受上述实施例的限制，应该理解，本领域技术人员可以设计出很多其他的修改和实施方式，这些修改和实施方式将落在本申请公开的原则范围和精神之内。

Claims (5)

1.一种基于误差四元数的导弹双回路姿态控制方法，其特征在于：

通过期望欧拉姿态角求取期望姿态四元数，并通过导航系统给出导弹当前姿态四元数，将两者结合得到误差四元数，然后通过姿态四元数跟踪回路即外回路，和弹体姿态角速度稳定回路即内回路，通过内回路和外回路结合，共同控制导弹姿态。

2.根据权利要求1所述基于误差四元数的导弹双回路姿态控制方法，其特征在于：

要预先定义导航坐标系oxyz和弹体坐标系o₁x₁y₁z₁

导航坐标系原点取发射瞬间弹体质心在地面的投影点；ox轴在发射点水平面内，指向发射瞄准方向；oy轴沿发射点的铅垂线，指向上；oz轴垂直于oxy平面，构成右手坐标系，该坐标系为导航参考系，与地球固连，随地球转动；弹体坐标系原点取火箭弹质心；ox₁轴沿火箭弹纵轴指向头部；oy₁轴在弹体纵向对称面内，垂直ox₁轴，指向上；oz₁轴垂直于ox₁y₁平面，构成右手坐标系。

3.根据权利要求2所述基于误差四元数的导弹双回路姿态控制方法，其特征在于：

通过期望欧拉姿态角求取期望姿态四元数的具体步骤如下：

定义导弹在导航坐标系中的三个姿态角分别为：俯仰角偏航角ψ，滚动角γ；将导弹运动需要到达的期望欧拉姿态角位置转换表示为四元数q_cx

q_cx＝[q_cx0 q_cx1 q_cx2 q_cx3] (1)

其中q_cx0为实部，q_cx1，q_cx2，q_cx3为三个虚部；

当采用3-2-1转动次序(导航坐标系到弹体坐标系的转动次序为俯仰、偏航、滚动)定义欧拉姿态角时，转换公式如下：

当采用2-3-1转动次序(导航坐标系到弹体坐标系的转动次序为偏航、俯仰、滚动)定义欧拉姿态角时，转换公式如下：

4.根据权利要求3所述基于误差四元数的导弹双回路姿态控制方法，其特征在于：

误差四元数的求解过程如下：

首先，获取导弹当前姿态四元数q₀

q₀＝[q₀₀ q₀₁ q₀₂ q₀₃]^T (4)

根据得到的导弹的当前姿态四元数q₀，导弹的期望姿态四元数q_cx，设所需转动四元数为q_e，则四元数运算法则可得

符号表示四元数乘法运算，将式(5)进行变换可得

展开可得

其中，导弹转动角度与误差四元数的对应关系如下：

欧拉定理表明，从一个坐标系转到另一个坐标系，可以通过绕空间某一瞬时轴转动一个角度来实现；假设从与q₀关联的坐标系绕3-2-1的顺序分别旋转Δψ_e，Δγ_e(其对应的四元数为q_e)到与q_cx关联的坐标系，得到

由于导弹每一周期都在控制，因此Δψ_e，Δγ_e为小量，由式(8)可推出：

若与q₀关联的坐标系绕2-3-1的顺序分别旋转Δψ_e，Δγ_e(对应的四元数为q_e不变)到与q_cx关联的坐标系，容易得到

通过(10)式同样可推出(9)式，因此不管绕哪一种顺序旋转，均可得到导弹转动角度与误差四元数的关系如下

选择2·q_e1，2·q_e2，2·q_e3分别作为导弹绕弹体系ox₁，oy₁，oz₁轴的转动角度控制量。

5.根据权利要求4所述基于误差四元数的导弹双回路姿态控制方法，其特征在于：

通过内回路和外回路结合，共同控制导弹姿态，具体如下：

根据得到的导弹转动角度控制量2·q_e1，2·q_e2，2·q_e3，设计双回路姿态控制器，该控制器的内回路为弹体姿态角速度稳定回路，外回路为姿态四元数跟踪回路；控制器的目的是使误差四元数q_e1，q_e2，q_e3趋近于0，导弹当前姿态四元数跟踪期望姿态四元数；俯仰、偏航、滚动三通道控制方程如下：

其中，δ_γ、δ_ψ、为控制导弹姿态运动的滚动、偏航、俯仰通道舵偏；a^γ、a^ψ、分别为三通道外回路控制增益；W^γ(s)、W^ψ(s)、分别为三通道外回路矫正网络；分别为三通道内回路控制增益；ω_x、ω_y、ω_z分别为弹体滚动、偏航、俯仰角速度，由导航系统给出；W_l ^γ(s)、W_l ^ψ(s)、为陷波滤波网络，用于滤除弹体一阶弹性频率；RTD为弧度转度系数，RTD≈57.2957795。