CN114111771A - 一种双轴稳定平台的动态姿态测量方法 - Google Patents

一种双轴稳定平台的动态姿态测量方法 Download PDF

Info

Publication number
CN114111771A
CN114111771A CN202111416781.9A CN202111416781A CN114111771A CN 114111771 A CN114111771 A CN 114111771A CN 202111416781 A CN202111416781 A CN 202111416781A CN 114111771 A CN114111771 A CN 114111771A
Authority
CN
China
Prior art keywords
rotation
attitude
quaternion
carrier
angle
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202111416781.9A
Other languages
English (en)
Inventor
陆全聪
李阳
杨伟
张德阳
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jiujiang Zhongchuan Instrument Co ltd 1441 Factory
Original Assignee
Jiujiang Zhongchuan Instrument Co ltd 1441 Factory
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Jiujiang Zhongchuan Instrument Co ltd 1441 Factory filed Critical Jiujiang Zhongchuan Instrument Co ltd 1441 Factory
Priority to CN202111416781.9A priority Critical patent/CN114111771A/zh
Publication of CN114111771A publication Critical patent/CN114111771A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/53—Determining attitude

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)

Abstract

一种双轴稳定平台的动态姿态测量方法，采用卡尔曼Kalman滤波技术进行捷联惯导***的初始对准、组合导航和组合校准，将失准角、速度误差、位置误差估计出来，用它对捷联惯导***的四元数、解算速度、解算位置进行修正，使计算的导航坐标系与真实的导航坐标系尽可能的重合，同时，时间允许的情况下也希望将惯性元件的误差（陀螺仪漂移和加速度偏置）估计出来。本方法可以使双轴稳定平台在装载80kg重力仪的大载荷条件和四级海况的大动态条件下仍能保持纵摇角和横摇角≤10"(RMS)的稳定精度。

Description

一种双轴稳定平台的动态姿态测量方法
技术领域
本发明涉及重力仪测量领域，具体而言是一种重力仪双轴稳定平台的动态姿态测量方法。
背景技术
双轴稳定平台保持框架平台稳定在当地地理水平姿态，以隔离水平姿态扰动对重力仪的影响，，以往的双轴稳定平台姿态角测量通过三支加速度计和两轴水平陀螺仪数据的互补滤波融合得到。横摇和纵摇两个水平轴电机通过陀螺仪角速度和加速度计的倾角进行地理水平稳定。
在参考文献《海洋重力仪稳定平台控制***》(中国科学院测量与地球物理研究所，吴鹏飞，汪龙，邹舟，王勇，海洋重力仪稳定平台控制***，申请(专利)号：201620193744.4，2016.03.14)中就是采用直接通过加速度计和陀螺仪数据融合进行控制的方法，除此之外还需多加了角度传感器，使得结构加工精度要求高、难度大，测量传感器噪声大，难以达到高精度姿态测量的需求。
发明内容
本发明其目的就在于提供一种双轴稳定平台的动态姿态测量方法，由于以往的双轴稳定平台姿态角测量通过加速度计和陀螺仪数据的互补滤波融合得到，在大动态时不仅有滤波延时，也容易受外部线加速度干扰。此类稳定平台只能达到角分级精度，不适用于高精度场合。本发明提出一种基于惯性/卫星导航(INS/GPS)组合的姿态测量方法，通过算法设计避免了滤波延时和加速度计干扰导致的测量误差，在80kg大载荷的和四级海况的大动态条件下仍能保持重力仪稳定平台具有10"的稳定精度。
为实现上述目的而采取的技术方案是，一种双轴稳定平台的动态姿态测量方法，采用卡尔曼Kalman滤波技术进行捷联惯导***的初始对准、组合导航和组合校准，将失准角、速度误差、位置误差估计出来，用它对捷联惯导***的四元数、解算速度、解算位置进行修正，使计算的导航坐标系与真实的导航坐标系尽可能的重合；
姿态解算算法模块工作流程包括如下：
1、粗对准；2、姿态四元数更新；3、姿态角计算；4、姿态角速率计算；5、比力坐标变换；6、速度更新；7、位置更新；8、Kalman滤波计算；9、旋转控制指令计算。
步骤一：粗对准
粗对准即寻找大略的方位余弦阵
Figure BDA0003374648990000011
是进行卡尔曼Kalman精对准的基础，方案的基本思路以惯性坐标系作为参考基准，初始固定两个惯性坐标系，之后更新载体相对惯性坐标系的角运动四元数，并利用重力和载体线运动在两个惯性坐标系间的变换关系求取两个凝固惯性坐标系的变换阵，由此反推
Figure BDA0003374648990000021
先将方位余弦阵分解为
Figure BDA0003374648990000022
问题转为分别求取
Figure BDA0003374648990000023
和
Figure BDA0003374648990000024
步骤如下：
1)求
Figure BDA0003374648990000025
将
Figure BDA0003374648990000026
写成
Figure BDA0003374648990000027
其中
Figure BDA0003374648990000028
Figure BDA0003374648990000029
Figure BDA00033746489900000210
Figure BDA00033746489900000211
λ0和L0为对准开始时刻经度和纬度，λt和Lt为对准t时刻的经度和纬度，(4.1.3)、(4.1.4)、(4.1.5)和(4.1.6)代入(4.1.2)得
Figure BDA00033746489900000212
2)求
Figure BDA00033746489900000213
与姿态更新算法相同，利用姿态四元数更新求取微分方程
Figure BDA00033746489900000214
其中
Figure BDA00033746489900000215
为陀螺敏感的载体角速度，对准开始时刻ib0系与b系重合，
Figure BDA0003374648990000031
3)求
Figure BDA0003374648990000032
在惯性系下
Figure BDA0003374648990000033
动基座下加速度计输出
Figure BDA0003374648990000034
gb为重力在b系上投影，
Figure BDA0003374648990000035
为载体线运动加速度
定义
Figure BDA0003374648990000036
有
Figure BDA0003374648990000037
在载体做匀速运动或等幅摇摆运动条件下，近似的
Figure BDA0003374648990000038
有
Figure BDA0003374648990000039
其中，
Figure BDA00033746489900000310
由步骤1)求出，gn＝[0 0 g]T
另外由速度基本方程
Figure BDA00033746489900000311
写成
Figure BDA00033746489900000312
则
Figure BDA00033746489900000313
其中，vb为外参考对地速度，定义
Figure BDA00033746489900000314
整理上式有
Figure BDA00033746489900000315
其中，
Figure BDA00033746489900000316
任意取对准过程中的两个时刻点
Figure BDA00033746489900000317
求得
Figure BDA0003374648990000041
将以上
Figure BDA0003374648990000042
和
Figure BDA0003374648990000043
代入式(4.1)中即可求得方位余弦阵
Figure BDA0003374648990000044
由以上推导可以看出，对准的最大误差源为对准期间载体运动的加速度
Figure BDA0003374648990000045
而对于双轴稳定平台而言
Figure BDA0003374648990000046
相对于gb为小量，即使在行进中也只要求短时间内载体的机动不大即可完成较高精度的对准，动态适用性较强。
步骤二：姿态四元数更新
n系到p系的变换关系可用转动四元数Q来表示，四元数是由一个实数单位和3个虚数单位
Figure BDA0003374648990000047
组成的包含4个实元的超复数，形式如下：
Figure BDA0003374648990000048
其中q0、q1、q2、q3为四个实数，
Figure BDA0003374648990000049
为虚数单位的正交基；
如果空间中的矢量固定不动，若动坐标系相对参考系转动了一个角度，在这里动坐标系为p系，参考坐标系为n系，用四元数描述的矢量在两个坐标系上的变换关系为
Figure BDA00033746489900000410
式中，
Figure BDA00033746489900000411
Figure BDA00033746489900000412
Figure BDA00033746489900000413
Figure BDA00033746489900000414
将(4.2.3)代入(4.2.2)，按四元数计算得
Figure BDA00033746489900000415
又由
Figure BDA00033746489900000416
即可求得变换阵
Figure BDA0003374648990000051
以上就是利用四元数求得的
Figure BDA0003374648990000052
变换阵，若知道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为更新周期，则
Figure BDA0003374648990000053
根据式(4.2.2)又有
Figure BDA0003374648990000054
利用(4.2.2)与(4.2.5)的等价关系，式(4.2.7)等价为
Figure BDA0003374648990000055
比较(4.2.8)和(4.2.9)，得
Figure BDA0003374648990000056
其中
Figure BDA0003374648990000057
Φ为b(k)至b(k+1)的等效旋转矢量，由以下等效旋转矢量微分方程求其数值解
Figure BDA0003374648990000058
其中，Φ＝Φ(h)为一个姿态更新周期(tk,tk+1]内b(k)至b(k+1)的等效旋转矢量，ω为载体角速率矢量，后两项反映了转动不可交换误差补偿量；
方程(4.2.12)可用泰勒级数展开法进行求解，在一个姿态更新周期(tk,tk+1]内对载体角速度进行多项式拟合，用比更新周期短的采样周期对陀螺信号进行多子样采样，利用每个采样点的角增量信息求解拟合多项式的系数，拟合后的姿态角速度代回Φ(h)的泰勒级数展开式中即可得出旋转矢量的数值解；
若载体做同频不同相的横摇与纵摇振动时，会在方位轴上产生圆锥误差，当振动满足一定的相位和频率关系时，任意两轴的振动将在第三个轴向上引起漂移，圆锥误差采用旋转矢量四子样优化算法给予补偿：
Figure BDA0003374648990000061
其中，Δθ1、Δθ2、Δθ3、Δθ4分别为(tk,tk+h/4]、(tk+h/4,tk+h/2]、(tk+h/2,tk+3h/4]、(tk+3h/4,tk+1]采样周期内的陀螺输出角增量；
另外，n系相对于i系在更新周期间转过的角度为
Figure BDA0003374648990000062
Figure BDA0003374648990000063
为e系相对于i系的自转角速率在n系的投影，
Figure BDA0003374648990000064
为n系相对于e系的角速率在n系上的投影，由以下两式计算：
Figure BDA0003374648990000065
Figure BDA0003374648990000066
式中的速度、纬度和曲率半径均是上一导航解算更新周期的结果，有
Figure BDA0003374648990000067
在一个姿态更新周期内，导航坐标系的旋转变化极其缓慢，
Figure BDA0003374648990000068
为极小值，在计算机中不宜做被除数，对式(4.2.17)进行泰勒展开，得四阶近似：
Figure BDA0003374648990000069
将(4.2.11)和(4.2.18)代入(4.2.10)后进行归一化处理
Q(tk+1)＝Q(tk+1)/||Q(tk+1)|| (4.2.19)
步骤三：姿态角计算
上式结果代入式(4.2.6)，即可计算出惯性测量组件的姿态变换阵
Figure BDA00033746489900000610
单轴旋转激光惯导的惯性测量组件绕旋转轴Ozp1旋转，假设旋转角位置为γ，p1系到b0系的姿态变换阵为
Figure BDA00033746489900000611
n系到p1系的姿态变换阵
Figure BDA0003374648990000071
为
Figure BDA0003374648990000072
经过旋转角变换后，n系到b0系的姿态变换阵
Figure BDA0003374648990000073
为
Figure BDA0003374648990000074
惯导***的姿态变换阵一般表达式为
Figure BDA0003374648990000075
可计算舰船载体的姿态变换阵
Figure BDA0003374648990000076
其中，
Figure BDA0003374648990000077
为设备在舰上经过与真水平和真航向标校后得到的姿态零位阵；
最后由式(3.2.4)和式(4.3.5)比较实时提取载体的水平姿态角和航向角
Figure BDA0003374648990000078
Figure BDA0003374648990000079
Figure BDA00033746489900000710
四元数初值由初始姿态角进行计算
Figure BDA0003374648990000081
其中，姿态角的初始值H0、P0、R0由初始对准确定；
步骤四：姿态角速率计算
按采样周期h(h＝tk-tk-1)取被平滑量的N个采样，令
Figure BDA0003374648990000082
再令
A＝(BTB)-1BT (4.4.2)
定义
A(2,:)＝[A(2,1) A(2,2) ... A(2,N)] (4.4.3)
A(2,:)表示矩阵A的第二行元素，元素个数为N个，可以经过预先计算后存储备用；
当前tk时刻的姿态角速率
Figure BDA0003374648990000083
和
Figure BDA0003374648990000084
的计算如下
Figure BDA0003374648990000085
其中R(tk+i-N)、P(tk+i-N)和H(tk+i-N)为tk+i-N时刻的横摇角、纵摇角和航向角；
步骤五：比力坐标变换
加速度计输出比力fp经由坐标变换得
Figure BDA0003374648990000086
加速度计安装在旋转机构的旋转轴上，可忽略内杆臂效应，即
Figure BDA0003374648990000091
步骤六：速度更新
惯导基本方程
Figure BDA0003374648990000092
其中，
Figure BDA0003374648990000093
为地理坐标系相对地球坐标系在地理坐标系中的投影，称为地速。将式(4.6.1)写成分量的形式有：
Figure BDA0003374648990000094
即
Figure BDA0003374648990000095
其中，记
Figure BDA0003374648990000096
δA称为有害加速度；
则
Figure BDA0003374648990000097
在一个速度更新周期(tk,tk+1]内，有
Figure BDA0003374648990000098
加速度计输出脉冲为速度增量，式(4.43)中的积分项由下式计算
Figure BDA0003374648990000101
其中，将
Figure BDA0003374648990000102
记为
Figure BDA0003374648990000103
Δvp为加速度计输出脉冲数经过标度变换后的速度增量，h为更新周期；
式(4.6.7)右边第一项
Figure BDA0003374648990000104
为比力引起的速度补偿量；
Figure BDA0003374648990000105
式(4.6.8)在载体动态情况下会产生与载体线运动和角运动相关的常值误差，若载体在线运动方向同时作角运动，便会产生旋转效应，若载体沿纵轴作线振动的同时又沿横轴作同频且同相的角振动时，在载体的方位轴方向便会产生直流量，即引入了常值的速度增量误差，上述的运动方式与划艇运动类似：一方面浆绕艇的侧向轴作周期摇动，另一方面艇沿纵轴作间歇性前进，故称为划桨效应，对旋转效应与划桨效应的补偿方法为对Δvp进行多子样采样，利用样点信息拟合更新周期内的运动轨迹，优化的目标是在划桨运动的极端情况下使算法引起的误差尽可能的小，从而确定拟合多项式的系数；
旋转效应补偿项为一次补偿项，对划浆效应补偿项采用优化四子样算法，计算式如下
Δvp＝Δv+Δvrot+Δvscul (4.6.9)
其中，旋转效应补偿量
Figure BDA0003374648990000106
优化的四子样划桨效应补偿量
Figure BDA0003374648990000107
Δv＝Δv1+Δv2+Δv3+Δv4 (4.6.12)
Δθ＝Δθ1+Δθ2+Δθ3+Δθ4 (4.6.13)
Δv1、Δv2、Δv3、Δv4分别为(tk,tk+h/4]、(tk+h/4,tk+h/2]、(tk+h/2,tk+3h/4]、(tk+3h/4,tk+1]采样周期内的加速度计输出的速度增量；
步骤七：位置更新
在纯惯性工作模式下，惯导***的高度通道是发散的，对于水面应用，采用高通滤波方式求取瞬时垂向速度和瞬时位移；
经纬度的微分方程如下
Figure BDA0003374648990000111
在一个位置更新周期(tk,tk+1]内，有
Figure BDA0003374648990000112
步骤八：卡尔曼Kalman滤波计算
算法如下：
a)状态方程
将陀螺仪漂移和加速度计偏置均可看做随机常数过程，有
Figure BDA0003374648990000113
状态变量选取15维
Figure BDA0003374648990000114
连续***的状态方程为：
Figure BDA0003374648990000115
由惯性***误差传播方程，其中：
Figure BDA0003374648990000116
Figure BDA0003374648990000117
Figure BDA0003374648990000121
Figure BDA0003374648990000122
Figure BDA0003374648990000123
Figure BDA0003374648990000124
Figure BDA0003374648990000125
Figure BDA0003374648990000126
Figure BDA0003374648990000127
Figure BDA0003374648990000128
Figure BDA0003374648990000129
Figure BDA0003374648990000131
为陀螺漂移和加速度计偏置测量噪声；
b)量测方程
以位置为观测量的量测方程为
Figure BDA0003374648990000132
式中，下标s表示捷联惯导***解算输出，下标r表示参考基准输出，Hv＝[03×6 I3×303×6]，V为3维位置观测白噪声；
c)修正
卡尔曼Kalman滤波对失准角进行估计，并实时修正姿态阵
Figure BDA0003374648990000133
后得到比较精确的姿态阵；
利用卡尔曼Kalman滤波技术估计的失准角φE、φN、φU，修正角即为：
Figure BDA0003374648990000134
采用四元数法修正为：
Figure BDA0003374648990000135
其中：
Figure BDA0003374648990000136
速度位置的修正则采用速度、位置滤波输出值替代即可；
d)连续***的离散化
以上建立了连续***误差模型，在程序中实现需进行离散化，离散化实质上是根据连续***的***矩阵计算出离散***的转移阵，以及根据连续***的***过程方差强度阵计算出离散***的噪声方差阵；
连续***的状态方程模型如下：
Figure BDA0003374648990000137
式中，X是***的n维状态向量，F是n×n维***矩阵，W是p维***过程噪声，G是n×p维噪声输入矩阵，Z是***的m维观测向量，V是m维观测噪声，H是m×n维观测矩阵；
1)Φk,k-1的计算
利用定常***的计算方法，状态转移阵Φk,k-1与***矩阵F由如下关系：
Figure BDA0003374648990000141
其中，(tk-1,tk]为预测周期，记h＝tk－tk-1。预测周期h一般较短，对式(4.9.18)进行泰勒展开，得
Figure BDA0003374648990000142
2)
Figure BDA0003374648990000143
的计算
连续***的***噪声向量W(t)的协方差阵为Q(t)，则输入噪声的方差阵为：
Qq＝G(t)Q(t)GT(t) (式-20)
Kalman滤波的输入噪声方差
Figure BDA0003374648990000144
与连续***的输入噪声方差Qq有如下关系：
Figure BDA0003374648990000145
步骤九：旋转控制目标角位置计算
稳定平台的目标为隔离载体水平角运动，对于控制而言，约束条件即为使∫(R+α)dt＝0，∫(P+β)dt＝0，其中R为横摇角、P为纵摇角、α为横摇角指令、β为纵摇角指令，即旋转必须具有跟踪水平姿态角变化的功能；
输出的指令角转送给控制算法计算，转换为控制量(或控制信号)。
有益效果
与现有技术相比本发明具有以下优点。
本发明提出一种基于惯性/卫星导航(INS/GPS)组合的姿态测量方法，通过算法设计避免了滤波延时和加速度计干扰导致的测量误差，本方法可以使双轴稳定平台在装载80kg重力仪的载荷条件下和四级海况的大动态条件下仍能保持纵摇角和横摇角≤10"(RMS)的稳定精度。
附图说明
以下结合附图和实施例对本发明作进一步说明。
图1为双轴稳定平台的姿态解算原理框图；
图2为基于惯性凝固坐标系的粗对准流程；
图3为姿态解算Kalman滤波原理框图；
图4为车载纵摇(Pitch)和横摇(Roll)数据；
图5为车载航向(Heading)数据；
图6为车载机动条件下的纵摇(Pitch)和横摇(Roll)数据；
图7为车载机动条件下的航向(Heading)数据；
图8为海上试验俯仰角和横摇角数据
图9为双轴稳定平台外形图；
图10为惯性组件硬件关系图。
具体实施方式
一种双轴稳定平台的动态姿态测量方法，采用卡尔曼Kalman滤波技术进行捷联惯导***的初始对准、组合导航和组合校准，将失准角、速度误差、位置误差估计出来，用它对捷联惯导***的四元数、解算速度、解算位置进行修正，使计算的导航坐标系与真实的导航坐标系尽可能的重合。
姿态解算算法模块工作流程包括如下：
1、粗对准；2、姿态四元数更新；3、姿态角计算；4、姿态角速率计算；5、比力坐标变换；6、速度更新；7、位置更新；8、Kalman滤波计算；9、旋转控制指令计算；
步骤一：粗对准
粗对准即寻找大略的方位余弦阵
Figure BDA0003374648990000151
是进行卡尔曼Kalman精对准的基础，方案的基本思路以惯性坐标系作为参考基准，初始固定两个惯性坐标系，之后更新载体相对惯性坐标系的角运动四元数，并利用重力和载体线运动在两个惯性坐标系间的变换关系求取两个凝固惯性坐标系的变换阵，由此反推
Figure BDA0003374648990000161
先将方位余弦阵分解为
Figure BDA0003374648990000162
问题转为分别求取
Figure BDA0003374648990000163
和
Figure BDA0003374648990000164
步骤如下：
4)求
Figure BDA0003374648990000165
将
Figure BDA0003374648990000166
写成
Figure BDA0003374648990000167
其中
Figure BDA0003374648990000168
Figure BDA0003374648990000169
Figure BDA00033746489900001610
Figure BDA00033746489900001611
λ0和L0为对准开始时刻经度和纬度，λt和Lt为对准t时刻的经度和纬度，(4.1.3)、(4.1.4)、(4.1.5)和(4.1.6)代入(4.1.2)得
Figure BDA00033746489900001612
5)求
Figure BDA00033746489900001613
与姿态更新算法相同，利用姿态四元数更新求取微分方程
Figure BDA00033746489900001614
其中
Figure BDA0003374648990000171
为陀螺敏感的载体角速度，对准开始时刻ib0系与b系重合，
Figure BDA0003374648990000172
6)求
Figure BDA0003374648990000173
在惯性系下
Figure BDA0003374648990000174
动基座下加速度计输出
Figure BDA0003374648990000175
gb为重力在b系上投影，
Figure BDA0003374648990000176
为载体线运动加速度
定义
Figure BDA0003374648990000177
有
Figure BDA0003374648990000178
在载体做匀速运动或等幅摇摆运动条件下，近似的
Figure BDA0003374648990000179
有
Figure BDA00033746489900001710
其中，
Figure BDA00033746489900001711
由步骤1)求出，gn＝[0 0 g]T
另外由速度基本方程
Figure BDA00033746489900001712
写成
Figure BDA00033746489900001713
则
Figure BDA00033746489900001714
其中，vb为外参考对地速度，定义
Figure BDA00033746489900001715
整理上式有
Figure BDA00033746489900001716
其中，
Figure BDA00033746489900001717
任意取对准过程中的两个时刻点
Figure BDA0003374648990000181
求得
Figure BDA0003374648990000182
将以上
Figure BDA0003374648990000183
和
Figure BDA0003374648990000184
代入式(4.1)中即可求得方位余弦阵
Figure BDA0003374648990000185
由以上推导可以看出，对准的最大误差源为对准期间载体运动的加速度
Figure BDA0003374648990000186
而对于双轴稳定平台而言
Figure BDA0003374648990000187
相对于gb为小量，即使在行进中也只要求短时间内载体的机动不大即可完成较高精度的对准，动态适用性较强；
步骤二：姿态四元数更新
n系到p系的变换关系可用转动四元数Q来表示，四元数是由一个实数单位和3个虚数单位
Figure BDA0003374648990000188
组成的包含4个实元的超复数，形式如下：
Figure BDA0003374648990000189
其中q0、q1、q2、q3为四个实数，
Figure BDA00033746489900001810
为虚数单位的正交基；
如果空间中的矢量固定不动，若动坐标系相对参考系转动了一个角度，在这里动坐标系为p系，参考坐标系为n系，用四元数描述的矢量在两个坐标系上的变换关系为
Figure BDA00033746489900001811
式中，
Figure BDA00033746489900001812
Figure BDA00033746489900001813
Figure BDA00033746489900001814
Figure BDA00033746489900001815
将(4.2.3)代入(4.2.2)，按四元数计算得
Figure BDA00033746489900001816
又由
Figure BDA00033746489900001817
即可求得变换阵
Figure BDA0003374648990000191
以上就是利用四元数求得的
Figure BDA0003374648990000192
变换阵，若知道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为更新周期，则
Figure BDA0003374648990000193
根据式(4.2.2)又有
Figure BDA0003374648990000194
利用(4.2.2)与(4.2.5)的等价关系，式(4.2.7)等价为
Figure BDA0003374648990000195
比较(4.2.8)和(4.2.9)，得
Figure BDA0003374648990000196
其中
Figure BDA0003374648990000197
Φ为b(k)至b(k+1)的等效旋转矢量，由以下等效旋转矢量微分方程求其数值解
Figure BDA0003374648990000198
其中，Φ＝Φ(h)为一个姿态更新周期(tk,tk+1]内b(k)至b(k+1)的等效旋转矢量，ω为载体角速率矢量，后两项反映了转动不可交换误差补偿量；
方程(4.2.12)可用泰勒级数展开法进行求解，在一个姿态更新周期(tk,tk+1]内对载体角速度进行多项式拟合，用比更新周期短的采样周期对陀螺信号进行多子样采样，利用每个采样点的角增量信息求解拟合多项式的系数，拟合后的姿态角速度代回Φ(h)的泰勒级数展开式中即可得出旋转矢量的数值解；
若载体做同频不同相的横摇与纵摇振动时，会在方位轴上产生圆锥误差，当振动满足一定的相位和频率关系时，任意两轴的振动将在第三个轴向上引起漂移，圆锥误差采用旋转矢量四子样优化算法给予补偿：
Figure BDA0003374648990000201
其中，Δθ1、Δθ2、Δθ3、Δθ4分别为(tk,tk+h/4]、(tk+h/4,tk+h/2]、(tk+h/2,tk+3h/4]、(tk+3h/4,tk+1]采样周期内的陀螺输出角增量；
另外，n系相对于i系在更新周期间转过的角度为
Figure BDA0003374648990000202
Figure BDA0003374648990000203
为e系相对于i系的自转角速率在n系的投影，
Figure BDA0003374648990000204
为n系相对于e系的角速率在n系上的投影，由以下两式计算：
Figure BDA0003374648990000205
Figure BDA0003374648990000206
式中的速度、纬度和曲率半径均是上一导航解算更新周期的结果，有
Figure BDA0003374648990000207
在一个姿态更新周期内，导航坐标系的旋转变化极其缓慢，
Figure BDA0003374648990000208
为极小值，在计算机中不宜做被除数，对式(4.2.17)进行泰勒展开，得四阶近似：
Figure BDA0003374648990000209
将(4.2.11)和(4.2.18)代入(4.2.10)后进行归一化处理
Q(tk+1)＝Q(tk+1)/||Q(tk+1)|| (4.2.19)
步骤三：姿态角计算
上式结果代入式(4.2.6)，即可计算出惯性测量组件的姿态变换阵
Figure BDA00033746489900002010
单轴旋转激光惯导的惯性测量组件绕旋转轴Ozp1旋转，假设旋转角位置为γ，p1系到b0系的姿态变换阵为
Figure BDA0003374648990000211
n系到p1系的姿态变换阵
Figure BDA0003374648990000212
为
Figure BDA0003374648990000213
经过旋转角变换后，n系到b0系的姿态变换阵
Figure BDA0003374648990000214
为
Figure BDA0003374648990000215
惯导***的姿态变换阵一般表达式为
Figure BDA0003374648990000216
可计算舰船载体的姿态变换阵
Figure BDA0003374648990000217
其中，
Figure BDA0003374648990000218
为设备在舰上经过与真水平和真航向标校后得到的姿态零位阵；
最后由式(3.2.4)和式(4.3.5)比较实时提取载体的水平姿态角和航向角
Figure BDA0003374648990000219
Figure BDA00033746489900002110
Figure BDA00033746489900002111
四元数初值由初始姿态角进行计算
Figure BDA0003374648990000221
其中，姿态角的初始值H0、P0、R0由初始对准确定；
步骤四：姿态角速率计算
按采样周期h(h＝tk-tk-1)取被平滑量的N个采样，令
Figure BDA0003374648990000222
再令
A＝(BTB)-1BT (4.4.2)
定义
A(2,:)＝[A(2,1) A(2,2) ... A(2,N)] (4.4.3)
A(2,:)表示矩阵A的第二行元素，元素个数为N个，可以经过预先计算后存储备用；
当前tk时刻的姿态角速率
Figure BDA0003374648990000223
和
Figure BDA0003374648990000224
的计算如下
Figure BDA0003374648990000225
其中R(tk+i-N)、P(tk+i-N)和H(tk+i-N)为tk+i-N时刻的横摇角、纵摇角和航向角；
步骤五：比力坐标变换
加速度计输出比力fp经由坐标变换得
Figure BDA0003374648990000226
加速度计安装在旋转机构的旋转轴上，可忽略内杆臂效应，即
Figure BDA0003374648990000231
步骤六：速度更新
惯导基本方程
Figure BDA0003374648990000232
其中，
Figure BDA0003374648990000233
为地理坐标系相对地球坐标系在地理坐标系中的投影，称为地速，将式(4.6.1)写成分量的形式有：
Figure BDA0003374648990000234
即
Figure BDA0003374648990000235
其中，记
Figure BDA0003374648990000236
δA称为有害加速度；
则
Figure BDA0003374648990000237
在一个速度更新周期(tk,tk+1]内，有
Figure BDA0003374648990000238
加速度计输出脉冲为速度增量，式(4.43)中的积分项由下式计算
Figure BDA0003374648990000241
其中，将
Figure BDA0003374648990000242
记为
Figure BDA0003374648990000243
Δvp为加速度计输出脉冲数经过标度变换后的速度增量，h为更新周期；
式(4.6.7)右边第一项
Figure BDA0003374648990000244
为比力引起的速度补偿量；
Figure BDA0003374648990000245
式(4.6.8)在载体动态情况下会产生与载体线运动和角运动相关的常值误差，若载体在线运动方向同时作角运动，便会产生旋转效应，若载体沿纵轴作线振动的同时又沿横轴作同频且同相的角振动时，在载体的方位轴方向便会产生直流量，即引入了常值的速度增量误差，上述的运动方式与划艇运动类似：一方面浆绕艇的侧向轴作周期摇动，另一方面艇沿纵轴作间歇性前进，故称为划桨效应，对旋转效应与划桨效应的补偿方法为对Δvp进行多子样采样，利用样点信息拟合更新周期内的运动轨迹，优化的目标是在划桨运动的极端情况下使算法引起的误差尽可能的小，从而确定拟合多项式的系数；
旋转效应补偿项为一次补偿项，对划浆效应补偿项采用优化四子样算法，计算式如下
Δvp＝Δv+Δvrot+Δvscul (4.6.9)
其中，旋转效应补偿量
Figure BDA0003374648990000246
优化的四子样划桨效应补偿量
Figure BDA0003374648990000247
Δv＝Δv1+Δv2+Δv3+Δv4 (4.6.12)
Δθ＝Δθ1+Δθ2+Δθ3+Δθ4 (4.6.13)
Δv1、Δv2、Δv3、Δv4分别为(tk,tk+h/4]、(tk+h/4,tk+h/2]、(tk+h/2,tk+3h/4]、(tk+3h/4,tk+1]采样周期内的加速度计输出的速度增量；
步骤七：位置更新
在纯惯性工作模式下，惯导***的高度通道是发散的，对于水面应用，采用高通滤波方式求取瞬时垂向速度和瞬时位移；
经纬度的微分方程如下
Figure BDA0003374648990000251
在一个位置更新周期(tk,tk+1]内，有
Figure BDA0003374648990000252
步骤八：卡尔曼Kalman滤波计算
算法如下：
e)状态方程
将陀螺仪漂移和加速度计偏置均可看做随机常数过程，有
Figure BDA0003374648990000253
状态变量选取15维
Figure BDA0003374648990000254
连续***的状态方程为：
Figure BDA0003374648990000255
由惯性***误差传播方程，其中：
Figure BDA0003374648990000256
Figure BDA0003374648990000257
Figure BDA0003374648990000261
Figure BDA0003374648990000262
Figure BDA0003374648990000263
Figure BDA0003374648990000264
Figure BDA0003374648990000265
Figure BDA0003374648990000266
Figure BDA0003374648990000267
Figure BDA0003374648990000268
Figure BDA0003374648990000269
Figure BDA0003374648990000271
为陀螺漂移和加速度计偏置测量噪声；
f)量测方程
以位置为观测量的量测方程为
Figure BDA0003374648990000272
式中，下标s表示捷联惯导***解算输出，下标r表示参考基准输出，Hv＝[03×6 I3×303×6]，V为3维位置观测白噪声；
g)修正
卡尔曼Kalman滤波对失准角进行估计，并实时修正姿态阵
Figure BDA0003374648990000273
后得到比较精确的姿态阵；
利用卡尔曼Kalman滤波技术估计的失准角φE、φN、φU，修正角即为：
Figure BDA0003374648990000274
采用四元数法修正为：
Figure BDA0003374648990000275
其中：
Figure BDA0003374648990000276
速度位置的修正则采用速度、位置滤波输出值替代即可；
h)连续***的离散化
以上建立了连续***误差模型，在程序中实现需进行离散化，离散化实质上是根据连续***的***矩阵计算出离散***的转移阵，以及根据连续***的***过程方差强度阵计算出离散***的噪声方差阵；
连续***的状态方程模型如下：
Figure BDA0003374648990000277
式中，X是***的n维状态向量，F是n×n维***矩阵，W是p维***过程噪声，G是n×p维噪声输入矩阵，Z是***的m维观测向量，V是m维观测噪声，H是m×n维观测矩阵；
3)Φk,k-1的计算
利用定常***的计算方法，状态转移阵Φk,k-1与***矩阵F由如下关系：
Figure BDA0003374648990000281
其中，(tk-1,tk]为预测周期，记h＝tk－tk-1。预测周期h一般较短，对式(4.9.18)进行泰勒展开，得
Figure BDA0003374648990000282
4)
Figure BDA0003374648990000283
的计算
连续***的***噪声向量W(t)的协方差阵为Q(t)，则输入噪声的方差阵为：
Qq＝G(t)Q(t)GT(t) (式-20)
Kalman滤波的输入噪声方差
Figure BDA0003374648990000284
与连续***的输入噪声方差Qq有如下关系：
Figure BDA0003374648990000285
步骤九：旋转控制目标角位置计算
稳定平台的目标为隔离载体水平角运动，对于控制而言，约束条件即为使∫(R+α)dt＝0，∫(P+β)dt＝0，其中R为横摇角、P为纵摇角、α为横摇角指令、β为纵摇角指令，即旋转必须具有跟踪水平姿态角变化的功能；
输出的指令角转送给控制算法计算，转换为控制量(或控制信号)。
图1是双轴稳定平台的姿态解算原理框图，涵盖了步骤1～9的全部流程，图中绘制了主要数据流程及各阶段所做的数据处理，此原理框图与说明中的算法说明结合具有实现本方法的可能；
图2是基于惯性凝固坐标系的粗对准流程，此过程于捷连惯性导航的专业技术人员来说属于众所周知的技术；
图3是姿态解算Kalman滤波原理框图，此过程使用的卡尔曼滤波器算法是专业技术人员所周知的，但其配置参数是本发明所特有的；
图4-8是陆地车载试验数据，从图4数据我们可以看到在静态条件下的纵摇角误差RMS值为0.0002°(0.72″)，横摇误差RMS值为0.002°(7.2″)，从图6数据我们可以看到在动态条件下纵摇角误差RMS值为0.0026°(9.36″)，横摇误差RMS值为0.0025°(9″)。从图8和图10的航向变化状态能看到静态和动态有明显的差异；
图8数据曲线看到在海上将近5天的连续工作时间内，数据的最大值都没有超过0.01°(3.6″)。图中曲线平直的部分是船只锚泊的时段，曲线上下波动是是船只机动航行的时段。由此可见使用此种控制方法的双轴稳定平台极大提升了在大动态环境下的稳定性，有力保障了重力仪在各类运动环境中的稳定工作；
图9是双轴稳定平台的外形结构图，本技术方案所提出的算法在其中的控制箱中实现。图中代号所示组成部分为：1.冷原子重力仪；2.基座；3.外框；4.惯性组件；5.控制箱；6.显示器；7.锁紧器。中框因视图遮挡未在图中标识；
本技术方案所使用的算法运行于惯性组件中的采集板，惯性组件的硬件构成如图10所示。
惯性测量组件主要包括惯性惯性敏感器、电子线路单元(EU)、台体组件及机箱三部分；
惯性敏感器件包括三个光纤陀螺仪、三个石英加速度计；
电子线路单元包括加速度计信号转换板、采集解算板、直流电源模块。
加速度计信号转换板将三个加速度计的电流信号转换成数字脉冲信号，同时测量加速度计测温器件输出的测温信号，并将这些信号输出到采集解算板上；
采集解算板产生1ms外部触发信号给光纤陀螺和加速度计线路板，通过422串口采集陀螺脉冲信号、温度信号、加速度计脉冲信号、温度信号，进行温度补偿、标度变换得到载体角速度和比力，进行Kalman滤波计算和捷联算法导航解算；采集解算板同时接收外部卫导参考信息，通过以太网/CAN总线/RS-422异步串口/光纤接口将导航解算结果输出给用户，通过显控装置与用户进行交互；
DCDC直流电源模块将电源装置转换的+24V直流电源变换为陀螺、加速度计、加速度计回路板、采集解算板、接口存储板工作所需的各种直流电源。
惯性组件安装于冷原子重力仪下部，和冷原子重力仪一起安装于双轴稳定平台中间的稳定框架上，和冷原子重力仪同步运动。整套设备装备在车辆船舶等运动载体上运动时，需要保证冷原子重力仪保持在水平状态，即平台工作平面与水平面的俯仰角和横滚角满足≤10″的要求。惯性组件是测量这两个关键参数的重要部件，本技术方案所提出的算法则是计算出这两个参数的重要方法。

Claims (1)

1.一种双轴稳定平台的动态姿态测量方法，其特征在于，采用卡尔曼Kalman滤波技术进行捷联惯导***的初始对准、组合导航和组合校准，将失准角、速度误差、位置误差估计出来，用它对捷联惯导***的四元数、解算速度、解算位置进行修正，使计算的导航坐标系与真实的导航坐标系尽可能的重合；
姿态解算算法模块工作流程,包括：
1、粗对准；2、姿态四元数更新；3、姿态角计算；4、姿态角速率计算；5、比力坐标变换；6、速度更新；7、位置更新；8、Kalman滤波计算；9、旋转控制指令计算；
步骤一：粗对准
粗对准即寻找大略的方位余弦阵
Figure FDA0003374648980000011
是进行卡尔曼Kalman精对准的基础，方案的基本思路以惯性坐标系作为参考基准，初始固定两个惯性坐标系，之后更新载体相对惯性坐标系的角运动四元数，并利用重力和载体线运动在两个惯性坐标系间的变换关系求取两个凝固惯性坐标系的变换阵，由此反推
Figure FDA0003374648980000012
先将方位余弦阵分解为
Figure FDA0003374648980000013
问题转为分别求取
Figure FDA0003374648980000014
和
Figure FDA0003374648980000015
步骤如下：
求
Figure FDA0003374648980000016
将
Figure FDA0003374648980000017
写成
Figure FDA0003374648980000018
其中
Figure FDA0003374648980000019
Figure FDA00033746489800000110
Figure FDA00033746489800000111
Figure FDA0003374648980000021
λ0和L0为对准开始时刻经度和纬度，λt和Lt为对准t时刻的经度和纬度，(4.1.3)、(4.1.4)、(4.1.5)和(4.1.6)代入(4.1.2)得
Figure FDA0003374648980000022
求
Figure FDA0003374648980000023
与姿态更新算法相同，利用姿态四元数更新求取微分方程
Figure FDA0003374648980000024
其中
Figure FDA0003374648980000025
为陀螺敏感的载体角速度，对准开始时刻ib0系与b系重合，
Figure FDA0003374648980000026
求
Figure FDA0003374648980000027
在惯性系下
Figure FDA0003374648980000028
动基座下加速度计输出
Figure FDA0003374648980000029
gb为重力在b系上投影，
Figure FDA00033746489800000210
为载体线运动加速度
定义
Figure FDA00033746489800000211
有
Figure FDA00033746489800000212
在载体做匀速运动或等幅摇摆运动条件下，近似的
Figure FDA00033746489800000213
有
Figure FDA00033746489800000214
其中，
Figure FDA00033746489800000215
Figure FDA00033746489800000216
由步骤1)求出，gn＝[0 0 g]T
另外由速度基本方程
Figure FDA00033746489800000217
写成
Figure FDA0003374648980000031
则
Figure FDA0003374648980000032
其中，vb为外参考对地速度，定义
Figure FDA0003374648980000033
整理上式有
Figure FDA0003374648980000034
其中，
Figure FDA0003374648980000035
任意取对准过程中的两个时刻点
Figure FDA0003374648980000036
Figure FDA0003374648980000037
求得
Figure FDA0003374648980000038
将以上
Figure FDA0003374648980000039
和
Figure FDA00033746489800000310
代入式(4.1)中即可求得方位余弦阵
Figure FDA00033746489800000311
由以上推导可以看出，对准的最大误差源为对准期间载体运动的加速度
Figure FDA00033746489800000312
而对于双轴稳定平台而言
Figure FDA00033746489800000313
相对于gb为小量，即使在行进中也只要求短时间内载体的机动不大即可完成较高精度的对准，动态适用性较强；
步骤二：姿态四元数更新
n系到p系的变换关系可用转动四元数Q来表示，四元数是由一个实数单位和3个虚数单位
Figure FDA00033746489800000314
组成的包含4个实元的超复数，形式如下：
Figure FDA00033746489800000315
其中q0、q1、q2、q3为四个实数，
Figure FDA00033746489800000316
为虚数单位的正交基；
如果空间中的矢量固定不动，若动坐标系相对参考系转动了一个角度，在这里动坐标系为p系，参考坐标系为n系，用四元数描述的矢量在两个坐标系上的变换关系为
Figure FDA00033746489800000317
式中，
Figure FDA0003374648980000041
Figure FDA0003374648980000042
Figure FDA0003374648980000043
Figure FDA0003374648980000044
将(4.2.3)代入(4.2.2)，按四元数计算得
Figure FDA0003374648980000045
又由
Figure FDA0003374648980000046
即可求得变换阵
Figure FDA0003374648980000047
以上就是利用四元数求得的
Figure FDA0003374648980000048
变换阵，若知道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为更新周期，则
Figure FDA0003374648980000049
根据式(4.2.2)又有
Figure FDA0003374648980000051
利用(4.2.2)与(4.2.5)的等价关系，式(4.2.7)等价为
Figure FDA0003374648980000052
比较(4.2.8)和(4.2.9)，得
Figure FDA0003374648980000053
其中
Figure FDA0003374648980000054
Φ为b(k)至b(k+1)的等效旋转矢量，由以下等效旋转矢量微分方程求其数值解
Figure FDA0003374648980000055
其中，Φ＝Φ(h)为一个姿态更新周期(tk,tk+1]内b(k)至b(k+1)的等效旋转矢量，ω为载体角速率矢量，后两项反映了转动不可交换误差补偿量；
方程(4.2.12)可用泰勒级数展开法进行求解，在一个姿态更新周期(tk,tk+1]内对载体角速度进行多项式拟合，用比更新周期短的采样周期对陀螺信号进行多子样采样，利用每个采样点的角增量信息求解拟合多项式的系数，拟合后的姿态角速度代回Φ(h)的泰勒级数展开式中即可得出旋转矢量的数值解；
若载体做同频不同相的横摇与纵摇振动时，会在方位轴上产生圆锥误差，当振动满足一定的相位和频率关系时，任意两轴的振动将在第三个轴向上引起漂移，圆锥误差采用旋转矢量四子样优化算法给予补偿：
Figure FDA0003374648980000061
其中，Δθ1、Δθ2、Δθ3、Δθ4分别为(tk,tk+h/4]、(tk+h/4,tk+h/2]、(tk+h/2,tk+3h/4]、(tk+3h/4,tk+1]采样周期内的陀螺输出角增量；
另外，n系相对于i系在更新周期间转过的角度为
Figure FDA0003374648980000062
Figure FDA0003374648980000063
为e系相对于i系的自转角速率在n系的投影，
Figure FDA0003374648980000064
为n系相对于e系的角速率在n系上的投影，由以下两式计算：
Figure FDA0003374648980000065
Figure FDA0003374648980000066
式中的速度、纬度和曲率半径均是上一导航解算更新周期的结果，有
Figure FDA0003374648980000067
在一个姿态更新周期内，导航坐标系的旋转变化极其缓慢，
Figure FDA0003374648980000068
为极小值，在计算机中不宜做被除数，对式(4.2.17)进行泰勒展开，得四阶近似：
Figure FDA0003374648980000069
将(4.2.11)和(4.2.18)代入(4.2.10)后进行归一化处理
Q(tk+1)＝Q(tk+1)/||Q(tk+1)|| (4.2.19)
步骤三：姿态角计算
上式结果代入式(4.2.6)，即可计算出惯性测量组件的姿态变换阵
Figure FDA0003374648980000071
单轴旋转激光惯导的惯性测量组件绕旋转轴Ozp1旋转，假设旋转角位置为γ，p1系到b0系的姿态变换阵为
Figure FDA0003374648980000072
n系到p1系的姿态变换阵
Figure FDA0003374648980000073
为
Figure FDA0003374648980000074
经过旋转角变换后，n系到b0系的姿态变换阵
Figure FDA0003374648980000075
为
Figure FDA0003374648980000076
惯导***的姿态变换阵一般表达式为
Figure FDA0003374648980000077
可计算舰船载体的姿态变换阵
Figure FDA0003374648980000078
其中，
Figure FDA0003374648980000079
为设备在舰上经过与真水平和真航向标校后得到的姿态零位阵；
最后由式(3.2.4)和式(4.3.5)比较实时提取载体的水平姿态角和航向角
Figure FDA00033746489800000710
Figure FDA00033746489800000711
Figure FDA0003374648980000081
四元数初值由初始姿态角进行计算
Figure FDA0003374648980000082
其中，姿态角的初始值H0、P0、R0由初始对准确定；步骤四：姿态角速率计算
按采样周期h(h＝tk-tk-1)取被平滑量的N个采样，令
Figure FDA0003374648980000083
再令
A＝(BTB)-1BT (4.4.2)
定义
A(2,:)＝[A(2,1) A(2,2) ... A(2,N)] (4.4.3)
A(2,:)表示矩阵A的第二行元素，元素个数为N个，可以经过预先计算后存储备用；
当前tk时刻的姿态角速率
Figure FDA0003374648980000097
和
Figure FDA0003374648980000098
的计算如下
Figure FDA0003374648980000091
其中R(tk+i-N)、P(tk+i-N)和H(tk+i-N)为tk+i-N时刻的横摇角、纵摇角和航向角；
步骤五：比力坐标变换
加速度计输出比力fp经由坐标变换得
Figure FDA0003374648980000092
加速度计安装在旋转机构的旋转轴上，可忽略内杆臂效应，即
Figure FDA0003374648980000093
步骤六：速度更新
惯导基本方程
Figure FDA0003374648980000094
其中，
Figure FDA0003374648980000095
为地理坐标系相对地球坐标系在地理坐标系中的投影，称为地速。将式(4.6.1)写成分量的形式有：
Figure FDA0003374648980000096
即
Figure FDA0003374648980000101
其中，记
Figure FDA0003374648980000102
δA称为有害加速度；
则
Figure FDA0003374648980000103
在一个速度更新周期(tk,tk+1]内，有
Figure FDA0003374648980000104
加速度计输出脉冲为速度增量，式(4.43)中的积分项由下式计算
Figure FDA0003374648980000105
其中，将
Figure FDA0003374648980000106
记为
Figure FDA0003374648980000107
Δvp为加速度计输出脉冲数经过标度变换后的速度增量，h为更新周期；
式(4.6.7)右边第一项
Figure FDA0003374648980000108
为比力引起的速度补偿量；
Figure FDA0003374648980000111
式(4.6.8)在载体动态情况下会产生与载体线运动和角运动相关的常值误差，若载体在线运动方向同时作角运动，便会产生旋转效应，若载体沿纵轴作线振动的同时又沿横轴作同频且同相的角振动时，在载体的方位轴方向便会产生直流量，即引入了常值的速度增量误差，上述的运动方式与划艇运动类似：一方面浆绕艇的侧向轴作周期摇动，另一方面艇沿纵轴作间歇性前进，故称为划桨效应，对旋转效应与划桨效应的补偿方法为对Δvp进行多子样采样，利用样点信息拟合更新周期内的运动轨迹，优化的目标是在划桨运动的极端情况下使算法引起的误差尽可能的小，从而确定拟合多项式的系数；
旋转效应补偿项为一次补偿项，对划浆效应补偿项采用优化四子样算法，计算式如下
Δvp＝Δv+Δvrot+Δvscul (4.6.9)
其中，旋转效应补偿量
Figure FDA0003374648980000112
优化的四子样划桨效应补偿量
Figure FDA0003374648980000113
Δv＝Δv1+Δv2+Δv3+Δv4 (4.6.12)
Δθ＝Δθ1+Δθ2+Δθ3+Δθ4 (4.6.13)
Δv1、Δv2、Δv3、Δv4分别为(tk,tk+h/4]、(tk+h/4,tk+h/2]、(tk+h/2,tk+3h/4]、(tk+3h/4,tk+1]采样周期内的加速度计输出的速度增量；
步骤七：位置更新
在纯惯性工作模式下，惯导***的高度通道是发散的，对于水面应用，采用高通滤波方式求取瞬时垂向速度和瞬时位移；
经纬度的微分方程如下
Figure FDA0003374648980000121
在一个位置更新周期(tk,tk+1]内，有
Figure FDA0003374648980000122
步骤八：卡尔曼Kalman滤波计算
算法如下：
状态方程
将陀螺仪漂移和加速度计偏置均可看做随机常数过程，有
Figure FDA0003374648980000123
状态变量选取15维
Figure FDA0003374648980000124
连续***的状态方程为：
Figure FDA0003374648980000125
由惯性***误差传播方程，其中：
Figure FDA0003374648980000127
Figure FDA0003374648980000126
Figure FDA0003374648980000131
Figure FDA0003374648980000132
Figure FDA0003374648980000133
Figure FDA0003374648980000134
Figure FDA0003374648980000135
Figure FDA0003374648980000141
Figure FDA0003374648980000142
Figure FDA0003374648980000143
Figure FDA0003374648980000144
Figure FDA0003374648980000145
为陀螺漂移和加速度计偏置测量噪声；
量测方程
以位置为观测量的量测方程为
Figure FDA0003374648980000146
式中，下标s表示捷联惯导***解算输出，下标r表示参考基准输出，Hv＝[03×6 I3×303×6]，V为3维位置观测白噪声；
修正
卡尔曼Kalman滤波对失准角进行估计，并实时修正姿态阵
Figure FDA0003374648980000147
后得到比较精确的姿态阵；
利用卡尔曼Kalman滤波技术估计的失准角φE、φN、φU，修正角即为：
Figure FDA0003374648980000151
采用四元数法修正为：
Figure FDA0003374648980000152
其中：
Figure FDA0003374648980000153
Figure FDA0003374648980000154
速度位置的修正则采用速度、位置滤波输出值替代即可；
连续***的离散化
以上建立了连续***误差模型，在程序中实现需进行离散化，离散化实质上是根据连续***的***矩阵计算出离散***的转移阵，以及根据连续***的***过程方差强度阵计算出离散***的噪声方差阵；
连续***的状态方程模型如下：
Figure FDA0003374648980000155
式中，X是***的n维状态向量，F是n×n维***矩阵，W是p维***过程噪声，G是n×p维噪声输入矩阵，Z是***的m维观测向量，V是m维观测噪声，H是m×n维观测矩阵；
Φk,k-1的计算
利用定常***的计算方法，状态转移阵Φk,k-1与***矩阵F由如下关系：
Figure FDA0003374648980000161
其中，(tk-1,tk]为预测周期，记h＝tk－tk-1。预测周期h一般较短，对式(4.9.18)进行泰勒展开，得
Figure FDA0003374648980000162
Figure FDA0003374648980000163
的计算
连续***的***噪声向量W(t)的协方差阵为Q(t)，则输入噪声的方差阵为：
Qq＝G(t)Q(t)GT(t) (式-20)
Kalman滤波的输入噪声方差
Figure FDA0003374648980000164
与连续***的输入噪声方差Qq有如下关系：
Figure FDA0003374648980000165
步骤九：旋转控制目标角位置计算
稳定平台的目标为隔离载体水平角运动，对于控制而言，约束条件即为使∫(R+α)dt＝0，∫(P+β)dt＝0，其中R为横摇角、P为纵摇角、α为横摇角指令、β为纵摇角指令，即旋转必须具有跟踪水平姿态角变化的功能；
输出的指令角转送给控制算法计算，转换为控制量或控制信号。
CN202111416781.9A 2021-11-25 2021-11-25 一种双轴稳定平台的动态姿态测量方法 Pending CN114111771A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111416781.9A CN114111771A (zh) 2021-11-25 2021-11-25 一种双轴稳定平台的动态姿态测量方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111416781.9A CN114111771A (zh) 2021-11-25 2021-11-25 一种双轴稳定平台的动态姿态测量方法

Publications (1)

Publication Number Publication Date
CN114111771A true CN114111771A (zh) 2022-03-01

Family

ID=80373343

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111416781.9A Pending CN114111771A (zh) 2021-11-25 2021-11-25 一种双轴稳定平台的动态姿态测量方法

Country Status (1)

Country Link
CN (1) CN114111771A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114719812A (zh) * 2022-03-10 2022-07-08 同济大学 一种用于主动径向控制的线路曲率实时检测***及其方法
CN115371650A (zh) * 2022-08-23 2022-11-22 天津大学 一种六自由度激光标靶测量***及其动态性能提升方法

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CA2511000A1 (en) * 2003-01-21 2004-07-21 Novatel Inc. Inertial gps navigation system with modified kalman filter
DE102017102269A1 (de) * 2016-02-12 2017-08-17 Gm Global Technology Operations, Llc Neigungs- und fehlausrichtungsausgleich für 6-dof-imu unter verwendung von gnss-/ins-daten
CN107525503A (zh) * 2017-08-23 2017-12-29 王伟 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法
CN108180925A (zh) * 2017-12-15 2018-06-19 中国船舶重工集团公司第七0七研究所 一种里程计辅助车载动态对准方法
CN108680186A (zh) * 2018-05-17 2018-10-19 中国人民解放军海军工程大学 基于重力仪平台的捷联式惯导***非线性初始对准方法
CN110319833A (zh) * 2019-07-09 2019-10-11 哈尔滨工程大学 一种无误差的光纤陀螺捷联惯导***速度更新方法
CN110398257A (zh) * 2019-07-17 2019-11-01 哈尔滨工程大学 Gps辅助的sins***快速动基座初始对准方法
AU2020101268A4 (en) * 2020-07-06 2020-08-13 Harbin Engineering University The initial alignment method for sway base
CN112504275A (zh) * 2020-11-16 2021-03-16 哈尔滨工程大学 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法
CN112629538A (zh) * 2020-12-11 2021-04-09 哈尔滨工程大学 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
CN112697141A (zh) * 2020-12-16 2021-04-23 北京航空航天大学 基于逆向导航的惯导/里程计动基座姿态与位置对准方法
CN113281797A (zh) * 2021-05-11 2021-08-20 南京国睿防务***有限公司 一种基于惯性导航的机动侦校雷达设计
EP3896393A1 (en) * 2020-04-17 2021-10-20 Orolia USA Inc. Methods for initializing a navigation system without heading information and devices thereof

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CA2511000A1 (en) * 2003-01-21 2004-07-21 Novatel Inc. Inertial gps navigation system with modified kalman filter
DE102017102269A1 (de) * 2016-02-12 2017-08-17 Gm Global Technology Operations, Llc Neigungs- und fehlausrichtungsausgleich für 6-dof-imu unter verwendung von gnss-/ins-daten
CN107525503A (zh) * 2017-08-23 2017-12-29 王伟 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法
CN108180925A (zh) * 2017-12-15 2018-06-19 中国船舶重工集团公司第七0七研究所 一种里程计辅助车载动态对准方法
CN108680186A (zh) * 2018-05-17 2018-10-19 中国人民解放军海军工程大学 基于重力仪平台的捷联式惯导***非线性初始对准方法
CN110319833A (zh) * 2019-07-09 2019-10-11 哈尔滨工程大学 一种无误差的光纤陀螺捷联惯导***速度更新方法
CN110398257A (zh) * 2019-07-17 2019-11-01 哈尔滨工程大学 Gps辅助的sins***快速动基座初始对准方法
EP3896393A1 (en) * 2020-04-17 2021-10-20 Orolia USA Inc. Methods for initializing a navigation system without heading information and devices thereof
AU2020101268A4 (en) * 2020-07-06 2020-08-13 Harbin Engineering University The initial alignment method for sway base
CN112504275A (zh) * 2020-11-16 2021-03-16 哈尔滨工程大学 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法
CN112629538A (zh) * 2020-12-11 2021-04-09 哈尔滨工程大学 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
CN112697141A (zh) * 2020-12-16 2021-04-23 北京航空航天大学 基于逆向导航的惯导/里程计动基座姿态与位置对准方法
CN113281797A (zh) * 2021-05-11 2021-08-20 南京国睿防务***有限公司 一种基于惯性导航的机动侦校雷达设计

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
HIDE, C (HIDE, C) ; MOORE, T (MOORE, T) ; SMITH, M (SMITH, M): "Adaptive Kalman filtering for low-cost INS/GPS", JOURNAL OF NAVIGATION, vol. 56, no. 1, 1 January 2003 (2003-01-01), pages 143 - 152 *
冯爱国;徐晓苏;: "GPS辅助姿态计算的捷联陀螺罗经实现", 上海海事大学学报, vol. 31, no. 04, 15 December 2010 (2010-12-15), pages 17 - 22 *
徐景硕;罗恬颖;马士国;: "动基座条件下舰载机快速传递对准方法研究", 宇航计测技术, vol. 34, no. 06, 15 December 2014 (2014-12-15), pages 13 - 18 *
薛亮;姜澄宇;常洪龙;袁广民;苑伟政;: "基于状态约束的MIMU/磁强计组合姿态估计滤波算法", 中国惯性技术学报, vol. 17, no. 03, 15 June 2009 (2009-06-15), pages 338 - 343 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114719812A (zh) * 2022-03-10 2022-07-08 同济大学 一种用于主动径向控制的线路曲率实时检测***及其方法
CN114719812B (zh) * 2022-03-10 2023-08-29 同济大学 一种用于主动径向控制的线路曲率实时检测***及其方法
CN115371650A (zh) * 2022-08-23 2022-11-22 天津大学 一种六自由度激光标靶测量***及其动态性能提升方法

Similar Documents

Publication Publication Date Title
CN109813311B (zh) 一种无人机编队协同导航方法
CN101413800B (zh) 导航/稳瞄一体化***的导航、稳瞄方法
EP1582840B1 (en) Inertial navigation system error correction
CN108827310B (zh) 一种船用星敏感器辅助陀螺仪在线标定方法
CN108051866B (zh) 基于捷联惯性/gps组合辅助水平角运动隔离的重力测量方法
US3509765A (en) Inertial navigation system
CN111102993A (zh) 一种旋转调制型捷联惯导***晃动基座初始对准方法
CN114111771A (zh) 一种双轴稳定平台的动态姿态测量方法
Fu et al. Information-reusing alignment technology for rotating inertial navigation system
Fu et al. Autonomous in-motion alignment for land vehicle strapdown inertial navigation system without the aid of external sensors
CN110285815A (zh) 一种可在轨全程应用的微纳卫星多源信息姿态确定方法
CN111189442A (zh) 基于cepf的无人机多源导航信息状态预测方法
CN108627152A (zh) 一种微型无人机基于多传感器数据融合的导航方法
CN112325886A (zh) 一种基于重力梯度仪和陀螺仪组合的航天器自主定姿***
Li et al. Integrated calibration method for dithered RLG POS using a hybrid analytic/Kalman filter approach
CN112902956A (zh) 一种手持式gnss/mems-ins接收机航向初值获取方法、电子设备、存储介质
WO2022006921A1 (zh) 一种水下捷联式重力测量数据处理方法
CN116105730A (zh) 基于合作目标卫星甚短弧观测的仅测角光学组合导航方法
CN105300407B (zh) 一种用于单轴调制激光陀螺惯导***的海上动态启动方法
CN111141285B (zh) 一种航空重力测量装置
RU2208559C1 (ru) Способ определения инерционных характеристик космического аппарата в процессе управления с помощью силовых гироскопов и реактивных двигателей
Xue et al. MEMS-based multi-sensor integrated attitude estimation technology for MAV applications
CN116499492A (zh) 一种基于dvl辅助的匀速直航下捷联罗经粗对准方法
CN111964671B (zh) 一种基于双轴旋转调制的惯性天文组合导航***及方法
Hayal Static calibration of the tactical grade inertial measurement units

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination