[发明专利]一种双轴稳定平台的动态姿态测量方法

权利要求书

1.一种双轴稳定平台的动态姿态测量方法，其特征在于，采用卡尔曼Kalman滤波技术进行捷联惯导系统的初始对准、组合导航和组合校准，将失准角、速度误差、位置误差估计出来，用它对捷联惯导系统的四元数、解算速度、解算位置进行修正，使计算的导航坐标系与真实的导航坐标系尽可能的重合；

姿态解算算法模块工作流程,包括：

1、粗对准；2、姿态四元数更新；3、姿态角计算；4、姿态角速率计算；5、比力坐标变换；6、速度更新；7、位置更新；8、Kalman滤波计算；9、旋转控制指令计算；

步骤一：粗对准

粗对准即寻找大略的方位余弦阵是进行卡尔曼Kalman精对准的基础，方案的基本思路以惯性坐标系作为参考基准，初始固定两个惯性坐标系，之后更新载体相对惯性坐标系的角运动四元数，并利用重力和载体线运动在两个惯性坐标系间的变换关系求取两个凝固惯性坐标系的变换阵，由此反推

先将方位余弦阵分解为

问题转为分别求取和步骤如下：

求

将写成

其中

λ₀和L₀为对准开始时刻经度和纬度，λ_t和L_t为对准t时刻的经度和纬度，(4.1.3)、(4.1.4)、(4.1.5)和(4.1.6)代入(4.1.2)得

求

与姿态更新算法相同，利用姿态四元数更新求取微分方程

其中为陀螺敏感的载体角速度，对准开始时刻ib0系与b系重合，

求

在惯性系下

动基座下加速度计输出

g^b为重力在b系上投影，为载体线运动加速度

定义有

在载体做匀速运动或等幅摇摆运动条件下，近似的有

其中，由步骤1)求出，gⁿ＝[0 0 g]^T

另外由速度基本方程

写成

则

其中，v^b为外参考对地速度，定义整理上式有

其中，

任意取对准过程中的两个时刻点求得

将以上和代入式(4.1)中即可求得方位余弦阵

由以上推导可以看出，对准的最大误差源为对准期间载体运动的加速度而对于双轴稳定平台而言相对于g^b为小量，即使在行进中也只要求短时间内载体的机动不大即可完成较高精度的对准，动态适用性较强；

步骤二：姿态四元数更新

n系到p系的变换关系可用转动四元数Q来表示，四元数是由一个实数单位和3个虚数单位组成的包含4个实元的超复数，形式如下：

其中q0、q1、q2、q3为四个实数，为虚数单位的正交基；

如果空间中的矢量固定不动，若动坐标系相对参考系转动了一个角度，在这里动坐标系为p系，参考坐标系为n系，用四元数描述的矢量在两个坐标系上的变换关系为

式中，

将(4.2.3)代入(4.2.2)，按四元数计算得

又由

即可求得变换阵

以上就是利用四元数求得的变换阵，若知道n系到p系的姿态四元数Q便可求得对应的变换阵；

设tk时刻的旋转坐标系为p(k)，导航坐标系为n(k)，tk+1时刻的载体坐标系为p(k+1)，导航坐标系为n(k+1)，记p(k)至p(k+1)的转动四元数为q(h)，n(k)至b(k)的转动四元数为Q(tk)，n(k+1)至b(k+1)的转动四元数为Q(tk+1)，n(k)至n(k+1)的转动四元数为p(h)，其中h＝tk+1-tk为更新周期，则

根据式(4.2.2)又有

利用(4.2.2)与(4.2.5)的等价关系，式(4.2.7)等价为

比较(4.2.8)和(4.2.9)，得

其中

Φ为b(k)至b(k+1)的等效旋转矢量，由以下等效旋转矢量微分方程求其数值解

其中，Φ＝Φ(h)为一个姿态更新周期(tk,tk+1]内b(k)至b(k+1)的等效旋转矢量，ω为载体角速率矢量，后两项反映了转动不可交换误差补偿量；

方程(4.2.12)可用泰勒级数展开法进行求解，在一个姿态更新周期(tk,tk+1]内对载体角速度进行多项式拟合，用比更新周期短的采样周期对陀螺信号进行多子样采样，利用每个采样点的角增量信息求解拟合多项式的系数，拟合后的姿态角速度代回Φ(h)的泰勒级数展开式中即可得出旋转矢量的数值解；

若载体做同频不同相的横摇与纵摇振动时，会在方位轴上产生圆锥误差，当振动满足一定的相位和频率关系时，任意两轴的振动将在第三个轴向上引起漂移，圆锥误差采用旋转矢量四子样优化算法给予补偿：

其中，Δθ₁、Δθ₂、Δθ₃、Δθ₄分别为(tk,tk+h/4]、(tk+h/4,tk+h/2]、(tk+h/2,tk+3h/4]、(tk+3h/4,tk+1]采样周期内的陀螺输出角增量；

另外，n系相对于i系在更新周期间转过的角度为

为e系相对于i系的自转角速率在n系的投影，为n系相对于e系的角速率在n系上的投影，由以下两式计算：

式中的速度、纬度和曲率半径均是上一导航解算更新周期的结果，有

在一个姿态更新周期内，导航坐标系的旋转变化极其缓慢，为极小值，在计算机中不宜做被除数，对式(4.2.17)进行泰勒展开，得四阶近似：

将(4.2.11)和(4.2.18)代入(4.2.10)后进行归一化处理

Q(t_k+1)＝Q(t_k+1)/||Q(t_k+1)|| (4.2.19)

步骤三：姿态角计算

上式结果代入式(4.2.6)，即可计算出惯性测量组件的姿态变换阵单轴旋转激光惯导的惯性测量组件绕旋转轴Oz_p1旋转，假设旋转角位置为γ，p1系到b0系的姿态变换阵为

n系到p1系的姿态变换阵为

经过旋转角变换后，n系到b0系的姿态变换阵为

惯导系统的姿态变换阵一般表达式为

可计算舰船载体的姿态变换阵

其中，为设备在舰上经过与真水平和真航向标校后得到的姿态零位阵；

最后由式(3.2.4)和式(4.3.5)比较实时提取载体的水平姿态角和航向角

四元数初值由初始姿态角进行计算

其中，姿态角的初始值H0、P0、R0由初始对准确定；步骤四：姿态角速率计算

按采样周期h(h＝tk-tk-1)取被平滑量的N个采样，令

再令

A＝(B^TB)^-1B^T (4.4.2)

定义

A(2,:)＝[A(2,1) A(2,2) ... A(2,N)] (4.4.3)

A(2,:)表示矩阵A的第二行元素，元素个数为N个，可以经过预先计算后存储备用；

当前tk时刻的姿态角速率和的计算如下

其中R(t_k+i-N)、P(t_k+i-N)和H(t_k+i-N)为tk+i-N时刻的横摇角、纵摇角和航向角；

步骤五：比力坐标变换

加速度计输出比力f^p经由坐标变换得

加速度计安装在旋转机构的旋转轴上，可忽略内杆臂效应，即

步骤六：速度更新

惯导基本方程

其中，为地理坐标系相对地球坐标系在地理坐标系中的投影，称为地速。将式(4.6.1)写成分量的形式有：

即

其中，记

δA称为有害加速度；

则

在一个速度更新周期(tk,tk+1]内，有

加速度计输出脉冲为速度增量，式(4.43)中的积分项由下式计算

其中，将记为Δv^p为加速度计输出脉冲数经过标度变换后的速度增量，h为更新周期；

式(4.6.7)右边第一项为比力引起的速度补偿量；

式(4.6.8)在载体动态情况下会产生与载体线运动和角运动相关的常值误差，若载体在线运动方向同时作角运动，便会产生旋转效应，若载体沿纵轴作线振动的同时又沿横轴作同频且同相的角振动时，在载体的方位轴方向便会产生直流量，即引入了常值的速度增量误差，上述的运动方式与划艇运动类似：一方面浆绕艇的侧向轴作周期摇动，另一方面艇沿纵轴作间歇性前进，故称为划桨效应，对旋转效应与划桨效应的补偿方法为对Δv^p进行多子样采样，利用样点信息拟合更新周期内的运动轨迹，优化的目标是在划桨运动的极端情况下使算法引起的误差尽可能的小，从而确定拟合多项式的系数；

旋转效应补偿项为一次补偿项，对划浆效应补偿项采用优化四子样算法，计算式如下

Δv^p＝Δv+Δv_rot+Δv_scul (4.6.9)

其中，旋转效应补偿量

优化的四子样划桨效应补偿量

Δv＝Δv₁+Δv₂+Δv₃+Δv₄ (4.6.12)

Δθ＝Δθ₁+Δθ₂+Δθ₃+Δθ₄ (4.6.13)

Δv₁、Δv₂、Δv₃、Δv₄分别为(tk,tk+h/4]、(tk+h/4,tk+h/2]、(tk+h/2,tk+3h/4]、(tk+3h/4,tk+1]采样周期内的加速度计输出的速度增量；

步骤七：位置更新

在纯惯性工作模式下，惯导系统的高度通道是发散的，对于水面应用，采用高通滤波方式求取瞬时垂向速度和瞬时位移；

经纬度的微分方程如下

在一个位置更新周期(tk,tk+1]内，有

步骤八：卡尔曼Kalman滤波计算

算法如下：

状态方程

将陀螺仪漂移和加速度计偏置均可看做随机常数过程，有状态变量选取15维

连续系统的状态方程为：

由惯性系统误差传播方程，其中：

为陀螺漂移和加速度计偏置测量噪声；

量测方程

以位置为观测量的量测方程为

式中，下标s表示捷联惯导系统解算输出，下标r表示参考基准输出，H_v＝[0_3×6 I_3×30_3×6]，V为3维位置观测白噪声；

修正

卡尔曼Kalman滤波对失准角进行估计，并实时修正姿态阵后得到比较精确的姿态阵；

利用卡尔曼Kalman滤波技术估计的失准角φ_E、φ_N、φ_U，修正角即为：

采用四元数法修正为：

其中：

速度位置的修正则采用速度、位置滤波输出值替代即可；

连续系统的离散化

以上建立了连续系统误差模型，在程序中实现需进行离散化，离散化实质上是根据连续系统的系统矩阵计算出离散系统的转移阵，以及根据连续系统的系统过程方差强度阵计算出离散系统的噪声方差阵；

连续系统的状态方程模型如下：

式中，X是系统的n维状态向量，F是n×n维系统矩阵，W是p维系统过程噪声，G是n×p维噪声输入矩阵，Z是系统的m维观测向量，V是m维观测噪声，H是m×n维观测矩阵；

Φ_k,k-1的计算

利用定常系统的计算方法，状态转移阵Φ_k,k-1与系统矩阵F由如下关系：

其中，(tk-1,tk]为预测周期，记h＝tk－tk-1。预测周期h一般较短，对式(4.9.18)进行泰勒展开，得

的计算

连续系统的系统噪声向量W(t)的协方差阵为Q(t)，则输入噪声的方差阵为：

Q_q＝G(t)Q(t)G^T(t) (式-20)

Kalman滤波的输入噪声方差与连续系统的输入噪声方差Q_q有如下关系：

步骤九：旋转控制目标角位置计算

稳定平台的目标为隔离载体水平角运动，对于控制而言，约束条件即为使∫(R+α)dt＝0，∫(P+β)dt＝0，其中R为横摇角、P为纵摇角、α为横摇角指令、β为纵摇角指令，即旋转必须具有跟踪水平姿态角变化的功能；

输出的指令角转送给控制算法计算，转换为控制量或控制信号。