基于反向姿态跟踪的自主式惯性导航行进间初始对准方法技术

一种基于反向姿态跟踪的自主式惯性导航行进间初始对准方法，它包括以下步骤：一、建立过渡参考坐标系；二、在载体运动过程中，根据传感器子系统采样数据计算粗略姿态矩阵；三、跟踪微分器处理里程计速度微分；四、利用存储的传感器子系统采样数据，反向姿态跟踪计算初始对准开始时刻姿态矩阵；五、利用存储的传感器子系统采样数据进行惯性导航解算，建立Kalman滤波器进行精对准，获取精确的姿态矩阵和载体位置信息；本发明专利技术所述方法，充分利用了传感器采样数据，在仅知道载体初始位置的条件下，即可实现行进间的精确对准姿态阵获取和位置导航，大大提高了载体机动能力，是行之有效的行进间初始对准方法。

【技术实现步骤摘要】
一、
本专利技术提出基于反向姿态跟踪的自主式惯性导航行进间初始对准方法，它涉及一种惯性导航行进间初始对准方法，属于惯性导航初始对准
二、
技术介绍
惯性导航行进间初始对准是指载体在运动过程中完成惯性导航系统初始对准的技术，因此，它是动基座初始对准技术的一种。惯性导航行进间初始对准技术对于增强载体的机动能力和快速反应能力具有不可估量的意义和作用。因此，如何在载体运动过程中实现初始对准是一个值得研究的课题。与传统的静基座初始对准环境不同，在载体运动状态下，载体的位置、速度、加速度以及角速度都在不断地发生变化，其对初始对准的影响主要具体表现为：一方面，线运动会使惯性导航基本方程中的对地加速度、哥式加速度等参量时刻变化，因此在运动状态下无法利用加速度计输出数据测得重力加速度的精确信息；另一方面，运动条件下载体振动使得干扰角速度具有很宽的频带，陀螺仪输出信号信噪比较低，无法从陀螺仪输出数据中将地球自转角速度这一对准的有用信息提取出来。可见，在载体运动条件下，就不能单纯依靠陀螺仪和加速度计的直接测量信息进行初始对准，而需要引入测距或测速信息，以补偿运动过程中有害加速度对初始对准精度的影响。目前，惯性导航行进间对准方法目前主要有捷联罗经法、惯性系对准方法以及最优估计对准方法等。捷联罗经法应用了成熟的经典控制理论方法实现行进间初始对准，原理简单但对准时间较长且对陀螺的低频干扰较为敏感，需要根据运动环境选取合适的控制参数。惯性系对准方法以惯性空间为中间过渡坐标系，隔离载体角运动对初始对准的干扰，但该方法仅对测量误差做简单处理，因而对准精度不高且不能获取载体的位置信息。最优估计对准方法建立起惯性导航误差方程，利用里程计等测速传感器的测速信息作为量测信息进行卡尔曼(Kalman)滤波，估计出平台失准角等关键误差从而实现行进间初始对准。该方法多是基于惯性导航线性化误差模型的，需要先在静止条件下获取粗略的初始姿态矩阵才可进行，因此，在一定程度上削弱了载体的机动性优势。由此可见，各方法各有其特点和适用性。为了既能在运动过程中获取粗略姿态矩阵，又能实现高精度姿态对准和位置导航，本专利技术单纯以里程计速度采样数据为对准辅助信息提出一种基于反向姿态跟踪的自主式惯性导航行进间对准方法。该方法在仅知道初始位置的条件下，即可实现高精度的惯性导航行进间初始对准。三、
技术实现思路
针对现有技术中存在的问题，本专利技术提出基于反向姿态跟踪的自主式惯性导航行进间初始对准方法。首先在载体运动过程中，利用里程计的测速信息为辅助进行粗对准获取粗略姿态矩阵，同时保存惯性测量元件(包括陀螺仪和加速度计)和里程计的采样数据。然后进行反向姿态跟踪求得初始对准开始时刻的姿态矩阵。最后在此基础上，利用保存的惯性测量元件采样数据和里程计速度采样数据进行Kalman滤波精对准。最终实现高精度的姿态对准和位置导航。本专利技术所提出的基于反向姿态跟踪的自主式惯性导航行进间初始对准方法，用于车载惯性导航系统，该系统包括传感器子系统、数据存储模块、粗对准计算模块、反向姿态跟踪计算模块和精对准计算模块。它们之间的关系是：传感器子系统的采样数据分别传递给数据存储模块和粗对准计算模块；粗对准计算模块计算出粗略初始姿态矩阵并将其传递给反向姿态跟踪计算模块；反向姿态跟踪计算模块利用数据存储模块的传感器数据进行反向姿态跟踪，并将其计算结果传递给精对准计算模块。精对准计算模块再利用数据存储模块的数据进行精对准计算获取精确对准姿态矩阵和位置导航结果。本专利技术提出基于反向姿态跟踪的自主式惯性导航行进间初始对准方法，具体包括以下步骤：步骤一：建立过渡参考坐标系；整个对准算法建立的四个重要的过渡参考坐标系为初始时刻惯性坐标系(i0系)、初始时刻地球坐标系(e0系)、初始时刻导航坐标系(n0系)和初始时刻载体坐标系(ib0系)，其定义如下：(1)初始时刻惯性坐标系(i0系)初始对准开始时，OXi0在当地子午面内，与赤道平面平行；OZi0指向地球自转轴方向；OYi0与OXi0和OZi0组成右手螺旋坐标系；初始对准开始后，i0系三轴相对于惯性空间不变；(2)初始时刻地球坐标系(e0系)初始时刻地球坐标系以地球中心为坐标系原点，OXe0在赤道平面内指向当地子午线方向；OZi0指向地球自转轴方向；OYi0与OXi0和OZi0组成右手螺旋坐标系；该坐标系相对于地球表面静止不动；(3)初始时刻导航坐标系(n0系)将初始对准开始时刻的东-北-天导航坐标系作为初始时刻导航坐标系；初始对准开始后，该坐标系相对于地球表面不动；(4)初始时刻载体坐标系(ib0系)将初始对准开始时刻的右-前-上载体坐标系作为初始时刻载体坐标系；初始对准开始后，该坐标系相对于惯性空间不动；步骤二：在载体运动过程中，根据传感器子系统采样数据计算粗略姿态矩阵；以i0系为过渡参考坐标系，导航坐标系(n系)相对于载体坐标系(b系)姿态矩阵的计算分为两个部分，其计算过程如式(1)所示Cbn=Ci0n·Cbi0---(1)]]>其中，为n系相对于i0系的转换矩阵；为i0系相对于b系的转换矩阵；a计算矩阵通过过渡参考坐标系n0系、e0系，该矩阵的计算过程又可分为四个部分，即：Ci0n=Cen·Cn0e·Ce0n0·Ci0e0---(2)]]>式中，为n系相对于地理坐标系(e系)的转换矩阵；为e系相对于n0系的转换矩阵；为n0系相对于e0系的转换矩阵；为e0系相对于i0系的转换矩阵；由于在粗对准模块中无法精确获取载体位置，因此，式(2)近似为：Ci0n≈Ce0n0·Ci0e0---(3)]]>其中，Ce0n0=010-sinL00cosL0cosL00sinL0---(4)]]>Ci0e0=cos(ωie·Δt)sin(ωie·Δt)0-sin(ωie·Δt)cos(ωie·Δt)0001---(5)]]>式中，L0为初始对准开始时的地理纬度；ωie为地球自转角速度；Δt为当前时刻t与初始对准开始时刻tstart的时间差，即Δt＝t-tstart；将式(4-5)代入式(3)中后，可得：Ci0n=-sin(ωie·Δt)cos(ωie&Cente本文档来自技高网...

【技术保护点】
一种基于反向姿态跟踪的自主式惯性导航行进间初始对准方法，其特征在于：它包括以下步骤：步骤一：建立过渡参考坐标系；整个对准算法建立的四个重要的过渡参考坐标系为初始时刻惯性坐标系即i0系、初始时刻地球坐标系即e0系、初始时刻导航坐标系即n0系和初始时刻载体坐标系即ib0系，其定义如下：(1)初始时刻惯性坐标系即i0系初始对准开始时，OXi0在当地子午面内，与赤道平面平行；OZi0指向地球自转轴方向；OYi0与OXi0和OZi0组成右手螺旋坐标系；初始对准开始后，i0系三轴相对于惯性空间不变；(2)初始时刻地球坐标系即e0系初始时刻地球坐标系以地球中心为坐标系原点，OXe0在赤道平面内指向当地子午线方向；OZi0指向地球自转轴方向；OYi0与OXi0和OZi0组成右手螺旋坐标系；该坐标系相对于地球表面静止不动；(3)初始时刻导航坐标系即n0系将初始对准开始时刻的东‑北‑天导航坐标系作为初始时刻导航坐标系；初始对准开始后，该坐标系相对于地球表面不动；(4)初始时刻载体坐标系即ib0系将初始对准开始时刻的右‑前‑上载体坐标系作为初始时刻载体坐标系；初始对准开始后，该坐标系相对于惯性空间不动；步骤二：在载体运动过程中，根据传感器子系统采样数据计算粗略姿态矩阵；以i0系为过渡参考坐标系，导航坐标系即n系相对于载体坐标系即b系姿态矩阵的计算分为两个部分，其计算过程如式(1)所示Cbn=Ci0n·Cbi0---(1)]]>其中，为n系相对于i0系的转换矩阵；为i0系相对于b系的转换矩阵；a计算矩阵通过过渡参考坐标系n0系、e0系，该矩阵的计算过程又可分为四个部分，即：Ci0n=Cen·Cn0e·Ce0n0·Ci0e0---(2)]]>式中，为n系相对于地理坐标系即e系的转换矩阵；为e系相对于n0系的转换矩阵；为n0系相对于e0系的转换矩阵；为e0系相对于i0系的转换矩阵；由于在粗对准模块中无法精确获取载体位置，因此，式(2)近似为：Ci0n≈Ce0n0·Ci0e0---(3)]]>其中，Ce0n0=010-sin L00cos L0cos L00sin L0---(4)]]>Ci0e0=cos(ωie·Δt)sin(ωie·Δt)0-sin(ωie·Δt)cos(ωie·Δt)0001---(5)]]>式中，L0为初始对准开始时的地理纬度；ωie为地球自转角速度；Δt为当前时刻t与初始对准开始时刻tstart的时间差，即Δt＝t‑tstart；将式(4‑5)代入式(3)中后，得：Ci0n=-sin(ωie·Δt)cos(ωie·Δt)0-sin L0·cos(ωie·Δt)-sin L0·sin(ωie·Δt)cos L0cos L0·cos(ωie·Δt)cos L0·sin(ωie·Δt)sin L0---(6)]]>b计算矩阵以ib0系为过渡参考坐标系，i0系相对于b系的转换矩阵的计算分为两个部分，即：Cbi0=Cib0i0·Cbib0---(7)]]>其中，为i0系相对于ib0系的转换矩阵；为ib0系相对于b系的转换矩阵；利用陀螺仪的测量角速度通过姿态矩阵微分方程(8)直接得到矩阵式中，由于ib0系与i0系均相对于惯性坐标系即i系不动，因此用惯性测量元件的角速度采样数据直接代替另外，在初始对准开始时刻，ib0系与b系重合，所以矩阵的积分计算初值为单位阵I3×3；矩阵的计算需要用到地球重力加速度在惯性系中慢漂的性质；其计算方法如下：ib0系和i0系均相对惯性空间不动，因此易知，矩阵为以常值矩阵；对载体速度两边求导得：v·n=C·bn·vb+Cbn·v·b---(9)]]>其中，为载体加速度在n系中的分量；为载体加速度在b系中的分量；将其代入惯性导航基本方程中，则惯性导航基本方程可改写如式(10)所示的形式；C·bn·vb+Cbn·v&CenterD...

【技术特征摘要】
1.一种基于反向姿态跟踪的自主式惯性导航行进间初始对准方法，其特征在于：它包
括以下步骤：
步骤一：建立过渡参考坐标系；
整个对准算法建立的四个重要的过渡参考坐标系为初始时刻惯性坐标系即i0系、
初始时刻地球坐标系即e0系、初始时刻导航坐标系即n0系和初始时刻载体坐标系即
ib0系，其定义如下：
(1)初始时刻惯性坐标系即i0系
初始对准开始时，OXi0在当地子午面内，与赤道平面平行；OZi0指向地球自转
轴方向；OYi0与OXi0和OZi0组成右手螺旋坐标系；初始对准开始后，i0系三轴相对
于惯性空间不变；
(2)初始时刻地球坐标系即e0系
初始时刻地球坐标系以地球中心为坐标系原点，OXe0在赤道平面内指向当地子
午线方向；OZi0指向地球自转轴方向；OYi0与OXi0和OZi0组成右手螺旋坐标系；该
坐标系相对于地球表面静止不动；
(3)初始时刻导航坐标系即n0系
将初始对准开始时刻的东-北-天导航坐标系作为初始时刻导航坐标系；初始对准
开始后，该坐标系相对于地球表面不动；
(4)初始时刻载体坐标系即ib0系
将初始对准开始时刻的右-前-上载体坐标系作为初始时刻载体坐标系；初始对准
开始后，该坐标系相对于惯性空间不动；
步骤二：在载体运动过程中，根据传感器子系统采样数据计算粗略姿态矩阵；
以i0系为过渡参考坐标系，导航坐标系即n系相对于载体坐标系即b系姿态矩阵
的计算分为两个部分，其计算过程如式(1)所示
Cbn=Ci0n·Cbi0---(1)]]>其中，为n系相对于i0系的转换矩阵；为i0系相对于b系的转换矩阵；
a计算矩阵通过过渡参考坐标系n0系、e0系，该矩阵的计算过程又可分为四个部分，即：
Ci0n=Cen·Cn0e·Ce0n0·Ci0e0---(2)]]>式中，为n系相对于地理坐标系即e系的转换矩阵；为e系相对于n0系的转换
矩阵；为n0系相对于e0系的转换矩阵；为e0系相对于i0系的转换矩阵；
由于在粗对准模块中无法精确获取载体位置，因此，式(2)近似为：
Ci0n≈Ce0n0·Ci0e0---(3)]]>其中，
Ce0n0=010-sinL00cosL0cosL00sinL0---(4)]]>Ci0e0=cos(ωie·Δt)sin(ωie·Δt)0-sin(ωie·Δt)cos(ωie·Δt)0001---(5)]]>式中，L0为初始对准开始时的地理纬度；ωie为地球自转角速度；Δt为当前时刻t与
初始对准开始时刻tstart的时间差，即Δt＝t-tstart；将式(4-5)代入式(3)中后，得：
Ci0n=-sin(ωie·Δt)cos(ωie·Δt)0-sinL0·cos(ωie·Δt)-sinL0·sin(ωie·Δt)cosL0cosL0·cos(ωie·Δt)cosL0·sin(ωie·Δt)sinL0---(6)]]>b计算矩阵以ib0系为过渡参考坐标系，i0系相对于b系的转换矩阵的计算分为两个部分，
即：
Cbi0=Cib0i0·Cbib0---(7)]]>其中，为i0系相对于ib0系的转换矩阵；为ib0系相对于b系的转换矩阵；
利用陀螺仪的测量角速度通过姿态矩阵微分方程(8)直接得到矩阵式中，由于ib0系与i0系均相对于惯性坐标系即i系不动，因此用惯性测量元件的角速
度采样数据直接代替另外，在初始对准开始时刻，ib0系与b系重合，所以
矩阵的积分计算初值为单位阵I3×3；
矩阵的计算需要用到地球重力加速度在惯性系中慢漂的性质；其计算方法如
下：
ib0系和i0系均相对惯性空间不动，因此易知，矩阵为以常值矩阵；对载体速
度两边求导得：
v·n=C·bn·vb+Cbn·v·b---(9)]]>其中，为载体加速度在n系中的分量；为载体加速度在b系中的分量；
将其代入惯性导航基本方程中，则惯性导航基本方程可改写如式(10)所示的形
式；
C·bn·vb+Cbn·v·b+(ωien+ωinn)×vn-fn=gn---(10)]]>其中，gn为地球重力加速度在n系中的分量；为地球自转角速度在n系中的分量；
为n系相对于i系的旋转角速度；fn为惯性测量元件的比力采样数据在n系中的分
量；
又已知姿态矩阵微分方程其中为b系相对于n系的旋转角速
度，将其代入(10)中，得：
Cbn·ωnbb×vb+Cbn·v·b+(ωien+ωinn)×vn-fn=gn---(11)]]>式中，忽略不计，上式进一步改写为式(12)；
v·b+ωibb×vb-fb=gb---(12)]]>其中，gb为地球重力加速度在b系中的分量；fb为惯性测量元件的比力采样数据；vb用里程计速度采样数据构成的速度测量矢量代替；而用里程计速度
测量矢量微分代替；需要说明的是，由于里程计速度采样数据中含有噪
声，直接微分会放大噪声而导致对准精度下降，因此需要使用跟踪微分器对进行
微分处理得到噪声干扰较小的为了获取矩阵需要通过gb与gn间的重要关系式(13)；
Cbib0·gb=Ci0ib0·Cni0·gn---(13)]]>式中，地球重力加速度在b系下的分量gn＝[00-g]T为已知量；矩阵均
可由前面的计算得到，因此矩阵的求解只需根据式(13)构造三个不在同一平面
的向量即可；
为了构造向量，同时削弱噪声，将(13)式两边进行积分处理，得到：
ztib0=Ci0ib0·rti0---(14)]]>式中，得到转换矩阵需要通过gb与gn间的重要关系式
Cbib0·gb=Ci0ib0·Cni0·gn---(15)]]>其中，均可由前面的计算得到；将上式两边积分得
ztib0=Ci0ib0·rti0---(16)]]>取两个时刻t1和t2的积分值和则由式(17)计算得到；
Ci0ib0=(zt1ib0)T(zt1ib0×zt2ib0)T(zt1ib0×zt2ib0×zt1ib0)T·(rt1i0)T(rt1i0×rt2i0)T(rt1i0×rt2i0×rt1i0)T-1---(17)]]>至此，在载体运动过程中，利用式(6)、(8)和(17)计算得到初始对准结束时
刻tend的粗略姿态矩阵但该姿态矩阵精度不高，且此时不能获取载体位置，因
此需要将传感器采样数据保存以做进一步处理；
步骤三：跟踪微分器处理里程计速度微分；
令则有下列非线性跟踪微分器
x·1=x2x·2=-r·sign(x1-v(t)+x2|x2|2r)---(18)]]>式中，r为跟踪微分器参数，v(t)为含有噪声的待跟踪信号，在此为里程计速度采样
数据；假设η为采样周期，则离散化计算方法如下所示；
x1(k+1)=x1(k)+η·x2(k)x2(k+1)=x2(k)-η·r·sign(x1(k)-v(k+1)+x2...

【专利技术属性】
技术研发人员：王新龙，明轩，
申请(专利权)人：北京航空航天大学，
类型：发明
国别省市：北京;11