基于速度误差修正模型的极地双惯导协同标定方法

1.本发明属于导航技术领域，涉及惯导系统的外场标定方法，特别涉及基于速度误差修正模型的极地双惯导协同标定方法，适用于两套及以上带有双轴或三轴转位机构的惯导系统间在极地的协同标定。


背景技术：

2.极地具有重大的战略和经济价值，北极航道是连接东亚、欧洲和北美洲东岸的最短航道，蕴藏着石油、天然气、煤炭等各类资源。各类舰船的极地航行离不开高精度、高可靠性导航设备提供导航定位信息。由于经线收敛于地理极点、地磁线收敛于极点附近，传统磁罗盘定向的方式不适用于极区导航，此外，高纬度地区卫星高度角降低，卫星导航存在盲区，极光和磁暴的干扰会导致无线电导航失效等，低纬度地区常用的导航手段并不适用于极地范围内的导航定位。惯性导航是一种完全自主、抗干扰强的导航方式，是极地环境中最主要导航方式。然而惯性导航在极区也面临着计算溢出导致误差增大、缺少航向参考等问题，需要在进入极区后切换导航坐标系。
3.具备极地航行能力的舰船航行时长通常较长，对惯导系统的定位精度要求高。标定技术能够对惯导系统的误差项进行补偿，是提高惯导系统导航精度的关键技术。惯导系统在出厂前会在高精度转台上进行精确标定，但是由于环境等诸多因素的影响，一定时间后惯导系统的误差参数会发生变化，引起导航精度下降。随着旋转调制惯导系统、“三自”惯组技术的成熟，外场标定技术由于其免拆卸、成本低等优点，越来越成为标定技术研究的重点。在中低纬度下，传统的外场标定技术以外界准确参考信息作为观测，利用卡尔曼滤波实现系统级标定。然而在极区特殊的环境下，能够接收到的外界参考信息极其有限，外场标定技术的使用会受到限制。这对于需要定期标定或者故障后修复的惯导系统而言其导航精度会受到严重影响。具备极地航行能力的舰船出于可靠性的需要通常会搭载多套带有转位机构的惯导系统，利用两套惯导系统的冗余信息，以两套惯导系统间的相对速度、位置作为约束观测，构建联合状态卡尔曼滤波器能够使惯导系统的系统性误差得到标定估计。若在舰船运动过程中进行标定，传统的速度误差方程中的比力项需要微分得到，动态环境下误差较大，会影响标定的精度。
4.本发明针对目前存在的问题，提出基于速度误差修正模型的极地双惯导协同标定方法，适用于装备多套带有转位机构惯导系统的载体在极地环境下的标定。本发明以地球椭球模型下的横地理坐标系为导航坐标系，将两套惯导系统在横地理坐标系下的相对速度、相对位置作为约束观测，建立了地球椭球模型横地理坐标系下基于速度误差修正模型的双惯导系统联合状态卡尔曼滤波器。该方法不受载体运动状态的影响，在静基座、动基座下均能对待标定惯导系统的全误差参数进行在线标定，解决了极地无外界基准信息情况下惯导系统的外场标定难题；以两套惯导系统间的相对误差为观测量，标定精度不受惯导系统绝对误差的影响；通过速度误差修正模型消除了模型中的比力项，提高了在运动状态下的标定精度。


技术实现要素：

5.本发明提出基于速度误差修正模型的极地双惯导协同标定方法，以地球椭球模型下的横地理坐标系为导航坐标系，实现了在极地环境无外界基准信息时，对具备自标定能力的惯导系统的陀螺标度因数、加速度计标度因数、安装误差角的标定。本标定方案不受载体运动状态的影响，在静基座、动基座条件下均能完成标定；通过速度误差修正模型提高了动基座下的标定精度；不受参考惯导系统绝对误差的影响。本发明标定精度能够满足导航级惯导系统的需求，具有重要工程实用价值。
6.为解决上述技术问题，本发明提出的解决方案为：
7.基于速度误差修正模型的极地双惯导协同标定方法，所述方法包括以下步骤：
8.(1)构建基于地球椭球模型下的横地球坐标系和横地理坐标系；
9.以北纬0
°
东经90
°
点为横地球坐标系下的北极点，定义为横北极点，北纬0
°
西经90
°
点为横地球坐标系下的南极点，定义为横南极点，0
°
经线和180
°
经线包围的椭圆面为横赤道面，取横北极、横南极和北极点组成的半个大椭圆为0
°
横经线，所在平面为横本初子午线，将地球坐标系与新定义的横地球坐标系之间的转换关系表示为：
[0010][0011]
式中，e系表示地球坐标系，e
′
系表示横地球坐标系，表示地球坐标系与横地球坐标系转换的方向余弦矩阵；
[0012]
基于横经纬网定义横地理坐标系，横北向指向横北极，所在位置的法线向上为天向，按照右手坐标系定义横东向，将横地理坐标系与地理坐标系之间的转换关系表示为：
[0013][0014]
式中，n系表示地理坐标系，t系表示横地理坐标系，β表示地理坐标系与横地理坐标系之间的旋转角度；
[0015]
确定β与经纬度、横经纬度之间的转换关系：
[0016][0017][0018]
确定方向余弦矩阵和速度矢量的转换关系：
[0019][0020][0021]
式中，表示载体坐标系与横地理坐标系之间转换的方向余弦矩阵，表示载体坐标系与地理坐标系之间转换的方向余弦矩阵，v
t
表示横地理坐标系下的载体速度矢量，vn表示地理坐标系下的载体速度矢量；
[0022]
将载体所在位置的法线与横赤道面的夹角定义为横纬度，与横本初子午面的夹角定义为横经度，将地球坐标系下定义的经纬度与横经纬度之间的转换关系表示为：
[0023][0024][0025][0026][0027]
式中，l
t
表示横纬度，λ
t
表示横经度，l表示纬度，λ表示经度；
[0028]
(2)构建两套惯导系统的误差模型；
[0029]
定义正常工作的双轴旋转调制惯导系统为惯导1，其体坐标系b1定义为“右-前-上”，待标定的惯导系统为惯导2，其体坐标系b2定义为“右-前-上”；
[0030]
惯导1的标度因数误差及安装误差很小进而忽略，将惯导1的误差模型定义为：
[0031][0032][0033]
其中，
[0034][0035][0036]
式中，表示惯导1的陀螺组件误差，表示惯导1的加速度计组件误差，表示惯导1的x轴陀螺漂移，表示惯导1的y轴陀螺漂移，表示惯导1的z轴陀螺漂移，表示惯导1的x轴加速度计零偏，表示惯导1的y轴加速度计零偏，表示惯导1的z轴加速度计零偏，表示惯导1的陀螺漂移，表示惯导1的加速度计零偏，为惯导1的陀螺噪声，为惯导1加速度计噪声；
[0037]
考虑标度因数误差、安装角误差及零偏误差，将惯导2的误差模型定义为：
[0038][0039][0040]
其中，
[0041][0042][0043]
式中，表示惯导2的陀螺组件误差，表示惯导2的加速度计组件误差，表示惯导2陀螺组件输出的理论角速度矢量，表示惯导2加速度计组件测得的理论比力
矢量，表示惯导2的x轴陀螺漂移，表示惯导2的y轴陀螺漂移，表示惯导2的z轴陀螺漂移，表示惯导2的x轴加速度计零偏，表示惯导2的y轴加速度计零偏，表示惯导2的z轴加速度计零偏，表示惯导2的陀螺漂移，表示惯导2的加速度计零偏，为惯导2的陀螺噪声，为惯导2加速度计噪声；δκg和δμg表示陀螺的标度因数误差矩阵和安装误差矩阵，δκa和δμa表示加速度计的标度因数误差矩阵和安装误差矩阵；
[0044]
确定δκg和δκa：
[0045][0046][0047]
式中，δκ
gx
、δκ
gy
和δκ
gz
分别表示x轴陀螺、y轴陀螺和z轴陀螺的标度因数误差，δκ
ax
、δκ
ay
和δκ
az
分别表示x轴加速度计、y轴加速度计和z轴加速度计的标度因数误差；
[0048]
确定δμg和δμa：
[0049][0050][0051]
式中，δμ
gyx
、δμ
gzx
和δμ
gzy
表示陀螺组件的三个安装误差角，δμ
ayx
、δμ
azx
、δμ
azy
、δμ
axy
、δμ
axz
和δμ
ayz
表示加速度计组件的六个安装误差角；
[0052]
(3)利用两套惯导系统输出的姿态、速度、位置相关信息，建立在横地理坐标系下的联合状态卡尔曼滤波器，具体步骤为：
[0053]
(3.1)确定横地理坐标系下基于速度误差修正模型的系统联合误差方程：
[0054][0055][0056][0057][0058][0059][0060]
其中，
[0061][0062][0063][0064][0065]
式中，φ
1t
表示惯导1在横地理坐标系下的姿态误差角，表示横地理坐标系下的惯导1的速度误差矢量，δr
1t
表示惯导1在横地理坐标系下的位置误差，表示与惯导1横纬度误差、横地理坐标系下速度误差相关的横地理坐标系相对惯性坐标系的角速度误差，表示与惯导1横纬度误差相关的地球自转角速度误差，表示惯导1体坐标系至横地理坐标系的方向余弦矩阵，表示惯导2在横地理坐标系下的姿态误差角，表示横地理坐标系下的惯导2的速度误差矢量，表示惯导2在横地理坐标系下的位置误差，表示与惯导2横纬度误差、横地理坐标系下速度误差相关的横地理坐标系相对惯性坐标系的角速度误差，表示与惯导2横纬度误差相关的地球自转角速度误差，表示惯导2体坐标系至横地理坐标系的方向余弦矩阵，为横地理坐标系相对于惯性坐标系的旋转角速度，为地球自转角速度向量，为横地理坐标系相对于地球坐标系的旋转角速度，g
t
表示载体所在位置的重力矢量，v
t
表示载体在横地理坐标系下的速度，分别表示载体在横东向、横北向、天向的速度，h为载体所在位置的高度，r
x
为载体所在位置的横东向的曲率半径，ry为载体所在位置的横北向的曲率半径，为载体所在位置的扭曲率，re和rn分别为载体所在位置的卯酉圈半径和子午圈半径；
[0066]
(3.2)确定联合状态方程：
[0067][0068]
其中，
[0069][0070][0071][0072][0073][0074][0075]
[0076][0077][0078][0079][0080][0081]f87
＝f
rv f
88
＝f
rr
[0082][0083]
式中，0i×j表示i行j列的零矩阵，ω
ie
表示地球自转角速度大小，g
t
分别载体位置处的重力加速度的值，c
23
表示矩阵的第二、第三列，c3表示矩阵的第三列，表示惯导2的x轴陀螺输出值，表示惯导2的y轴陀螺输出值，表示惯导2的z轴陀螺输出值，表示惯导2的x轴加速度计输出值，表示惯导2的y轴加速度计输出值，表示惯导2的z轴加速度计输出值；
[0084]
将状态向量x(t)表示为：
[0085][0086]
式中，表示惯导1在横东向、横北向、天向的姿态误差，分别表示惯导1在横东向、横北向、天向的速度误差，表示惯导1的横纬度误差，表示惯导1的横经度误差，δh1表示惯导1的高度误差，表示惯导2在横东向、横北向、天向的姿态误差，分别表示惯导2在横东向、横北向、天向的速度误差，表示惯导2的横纬度误差，表示惯导2的横经度误差，δh2表示惯导2的高度误差；
[0087]
将噪声分布矩阵及噪声矩阵表示为：
[0088][0089][0090]
(3.3)确定状态约束观测方程：
[0091]
将惯导1、惯导2系统输出的速度、位置分别表示为：
[0092][0093][0094]
式中，和分别表示惯导1和惯导2输出的横地理坐标系下的速度矢量，v
t
表示公共点的速度真值；惯导1与惯导2之间的外杆臂参数在系统安装完毕后通过标定补偿，表示惯导1输出的位置信息，表示惯导2输出的位置信息，表示b2坐标系相对于横地理坐标系的旋转角速度，表示两套惯导间的外杆臂，表示两套惯导间的外杆臂在惯导2体坐标系下的投影，r
t
表示公共点的位置在横地理坐标系下的真值；
[0095]
由于两套系统反映的是同一载体的速度信息、位置信息，观测量实质上构成了惯导1、惯导2各自速度误差、位置误差的约束，将观测量表示为：
[0096][0097][0098]
式中，υv、υr为相应的速度观测噪声、位置观测噪声；
[0099]
增加外界高度信息的观测
[0100][0101]
式中，为惯导1输出的高度值，υh为高度观测的噪声；
[0102]
将观测方程表示为：
[0103]
z(t)＝h(t)x(t)+υ(t)
[0104]
其中，
[0105][0106][0107]
h1＝[0 0 1]
[0108]
υ(t)＝[(υv)
t (υr)
t υh]
t
[0109]
式中，i3×3表示3行3列的单位矩阵；
[0110]
(4)确定两套惯导系统的转位次序：
[0111]
惯导1的转位次序为双轴16次序，具体转位流程如下：
[0112]
次序1：z轴以9
°
/s正向旋转180
°
，转停100s；
[0113]
次序2：y轴以9
°
/s反向旋转180
°
，转停100s；
[0114]
次序3：z轴以9
°
/s正向旋转180
°
，转停100s；
[0115]
次序4：y轴以9
°
/s反向旋转180
°
，转停100s；
[0116]
次序5：y轴以9
°
/s反向旋转180
°
，转停100s；
[0117]
次序6：z轴以9
°
/s正向旋转180
°
，转停100s；
[0118]
次序7：y轴以9
°
/s反向旋转180
°
，转停100s；
[0119]
次序8：z轴以9
°
/s正向旋转180
°
，转停100s；
[0120]
次序9：z轴以9
°
/s反向旋转180
°
，转停100s；
[0121]
次序10：y轴以9
°
/s正向旋转180
°
，转停100s；
[0122]
次序11：z轴以9
°
/s反向旋转180
°
，转停100s；
[0123]
次序12：y轴以9
°
/s正向旋转180
°
，转停100s；
[0124]
次序13：y轴以9
°
/s正向旋转180
°
，转停100s；
[0125]
次序14：z轴以9
°
/s反向旋转180
°
，转停100s；
[0126]
次序15：y轴以9
°
/s正向旋转180
°
，转停100s；
[0127]
次序16：z轴以9
°
/s反向旋转180
°
，转停100s；
[0128]
惯导2的转位次序为18次序，具体转位流程如下：
[0129]
次序1：y轴以9
°
/s正向旋转90
°
，转停180s；
[0130]
次序2：y轴以9
°
/s正向旋转180
°
，转停180s；
[0131]
次序3：y轴以9
°
/s正向旋转180
°
，转停180s；
[0132]
次序4：z轴以9
°
/s正向旋转90
°
，转停180s；
[0133]
次序5：z轴以9
°
/s正向旋转180
°
，转停180s；
[0134]
次序6：z轴以9
°
/s正向旋转180
°
，转停180s；
[0135]
次序7：x轴以9
°
/s正向旋转90
°
，转停180s；
[0136]
次序8：x轴以9
°
/s正向旋转180
°
，转停180s；
[0137]
次序9：x轴以9
°
/s正向旋转180
°
，转停180s；
[0138]
次序10：x轴以9
°
/s正向旋转90
°
，转停180s；
[0139]
次序11：x轴以9
°
/s正向旋转90
°
，转停180s；
[0140]
次序12：x轴以9
°
/s正向旋转90
°
，转停180s；
[0141]
次序13：z轴以9
°
/s正向旋转90
°
，转停180s；
[0142]
次序14：z轴以9
°
/s正向旋转90
°
，转停180s；
[0143]
次序15：z轴以9
°
/s正向旋转90
°
，转停180s；
[0144]
次序16：y轴以9
°
/s正向旋转90
°
，转停180s；
[0145]
次序17：y轴以9
°
/s正向旋转90
°
，转停180s；
[0146]
次序18：y轴以9
°
/s正向旋转90
°
，转停180s；
[0147]
基于联合转位方式，惯导1处于双轴旋转调制导航状态，惯导2处于标定状态，其陀螺标度因数误差、加速度计标度因数误差、安装误差均得到激励，根据步骤(3)所述方案，建立联合状态卡尔曼滤波器即实现惯导2的极区外场标定。
[0148]
进一步的，本发明方法对载体的运动状态无要求，载体处于系泊状态或运动状态均能实现在线标定。
[0149]
进一步的，本发明方法不仅仅适用于高纬度地区，在非横北极、横南极的中纬度地区同样适用。
[0150]
进一步的，惯导1与惯导2处于零位时的相对姿态在安装完毕后标定得到，惯导2在标定初始时刻的姿态基于两套惯导的相对姿态通过与惯导1传递对准获得。
[0151]
进一步的，步骤(4)所述的联合转位次序适用于两套及以上具有双轴转位机构的惯导系统间的在线标定，对于双轴和三轴惯导系统之间、多套三轴惯导系统间的在线标定也适用。
[0152]
进一步的，步骤(4)所述的联合转位次序仅为基于两套具有双轴转位机构的惯导系统的优选方案，对于其他旋转调制次序与标定次序之间的联合转位方案，也属于本发明的范畴。
[0153]
综上所述，本发明的优点及积极效果为：本发明建立了地球椭球模型横地理坐标系下基于速度误差修正模型的联合状态卡尔曼滤波器，解决了传统的以当地水平坐标系为导航坐标系的惯性导航系统在极区失效的问题，通过两套惯导系统协同转位，利用两套惯导系统的冗余信息实现了在极地环境下的标定，通过速度修正模型消除比力项，提高了动基座下的标定精度，该标定方法不受极地特殊地理位置和恶劣自然环境的影响，具有重要工程意义。
附图说明
[0154]
图1是本发明实施例提供的流程图。
具体实施方式
[0155]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
[0156]
由于经线在极地快速收敛，以当地水平地理坐标系为导航坐标系的惯导系统会存在很大误差。由于磁暴、电离层的干扰等，在极区缺少可靠的外界参考信息，在极区的导航
定位主要依赖于惯性导航系统。然而，当惯导系统在极地环境下需要定期标定或者故障后更换了部分组件需要重新标定，传统的外场标定技术不能满足这一需求，这会影响惯导系统的精度。此外，传统速度误差方程中的比力项无法直接测得，在动基座下比力计算不准会影响标定的精度。针对这些问题，本发明提出基于速度误差修正模型的极地双惯导协同标定方法，所述的标定方法如图1所示。具体实施方式如下：
[0157]
(1)构建基于地球椭球模型下的横地球坐标系和横地理坐标系；
[0158]
以北纬0
°
东经90
°
点为横地球坐标系下的北极点，定义为横北极点，北纬0
°
西经90
°
点为横地球坐标系下的南极点，定义为横南极点，0
°
经线和180
°
经线包围的椭圆面为横赤道面，取横北极、横南极和北极点组成的半个大椭圆为0
°
横经线，所在平面为横本初子午线，将地球坐标系与新定义的横地球坐标系之间的转换关系表示为：
[0159][0160]
式中，e系表示地球坐标系，e
′
系表示横地球坐标系，表示地球坐标系与横地球坐标系转换的方向余弦矩阵；
[0161]
基于横经纬网定义横地理坐标系，横北向指向横北极，所在位置的法线向上为天向，按照右手坐标系定义横东向，将横地理坐标系与地理坐标系之间的转换关系表示为：
[0162][0163]
式中，n系表示地理坐标系，t系表示横地理坐标系，β表示地理坐标系与横地理坐标系之间的旋转角度；
[0164]
确定β与经纬度、横经纬度之间的转换关系：
[0165][0166][0167]
确定方向余弦矩阵和速度矢量的转换关系：
[0168][0169][0170]
式中，表示载体坐标系与横地理坐标系之间转换的方向余弦矩阵，表示载体坐标系与地理坐标系之间转换的方向余弦矩阵，v
t
表示横地理坐标系下的载体速度矢量，vn表示地理坐标系下的载体速度矢量；
[0171]
将载体所在位置的法线与横赤道面的夹角定义为横纬度，与横本初子午面的夹角定义为横经度，将地球坐标系下定义的经纬度与横经纬度之间的转换关系表示为：
[0172][0173][0174][0175][0176]
式中，l
t
表示横纬度，λ
t
表示横经度，l表示纬度，λ表示经度；
[0177]
(2)构建两套惯导系统的误差模型；
[0178]
定义正常工作的双轴旋转调制惯导系统为惯导1，其体坐标系b1定义为“右-前-上”，待标定的惯导系统为惯导2，其体坐标系b2定义为“右-前-上”；
[0179]
惯导1的标度因数误差及安装误差很小进而忽略，将惯导1的误差模型定义为：
[0180][0181][0182]
其中，
[0183][0184][0185]
式中，表示惯导1的陀螺组件误差，表示惯导1的加速度计组件误差，表示惯导1的x轴陀螺漂移，表示惯导1的y轴陀螺漂移，表示惯导1的z轴陀螺漂移，表示惯导1的x轴加速度计零偏，表示惯导1的y轴加速度计零偏，表示惯导1的z轴加速度计零偏，表示惯导1的陀螺漂移，表示惯导1的加速度计零偏，为惯导1的陀螺噪声，为惯导1加速度计噪声；
[0186]
考虑标度因数误差、安装角误差及零偏误差，将惯导2的误差模型定义为：
[0187][0188][0189]
其中，
[0190][0191][0192]
式中，表示惯导2的陀螺组件误差，表示惯导2的加速度计组件误差，表示惯导2陀螺组件输出的理论角速度矢量，表示惯导2加速度计组件测得的理论比力矢量，表示惯导2的x轴陀螺漂移，表示惯导2的y轴陀螺漂移，表示惯导2的z轴陀螺
漂移，表示惯导2的x轴加速度计零偏，表示惯导2的y轴加速度计零偏，表示惯导2的z轴加速度计零偏，表示惯导2的陀螺漂移，表示惯导2的加速度计零偏，为惯导2的陀螺噪声，为惯导2加速度计噪声；δκg和δμg表示陀螺的标度因数误差矩阵和安装误差矩阵，δκa和δμa表示加速度计的标度因数误差矩阵和安装误差矩阵；
[0193]
确定δκg和δκa：
[0194][0195][0196]
式中，δκ
gx
、δκ
gy
和δκ
gz
分别表示x轴陀螺、y轴陀螺和z轴陀螺的标度因数误差，δκ
ax
、δκ
ay
和δκ
az
分别表示x轴加速度计、y轴加速度计和z轴加速度计的标度因数误差；
[0197]
确定δμg和δμa：
[0198][0199][0200]
式中，δμ
gyx
、δμ
gzx
和δμ
gzy
表示陀螺组件的三个安装误差角，δμ
ayx
、δμ
azx
、δμ
azy
、δμ
axy
、δμ
axz
和δμ
ayz
表示加速度计组件的六个安装误差角；
[0201]
(3)利用两套惯导系统输出的姿态、速度、位置相关信息，建立在横地理坐标系下的联合状态卡尔曼滤波器，具体步骤为：
[0202]
(3.1)确定横地理坐标系下基于速度误差修正模型的系统联合误差方程：
[0203][0204][0205][0206][0207][0208][0209]
其中，
[0210]
[0211][0212][0213][0214]
式中，φ
1t
表示惯导1在横地理坐标系下的姿态误差角，表示横地理坐标系下的惯导1的速度误差矢量，φr
1t
表示惯导1在横地理坐标系下的位置误差，表示与惯导1横纬度误差、横地理坐标系下速度误差相关的横地理坐标系相对惯性坐标系的角速度误差，表示与惯导1横纬度误差相关的地球自转角速度误差，表示惯导1体坐标系至横地理坐标系的方向余弦矩阵，表示惯导2在横地理坐标系下的姿态误差角，表示横地理坐标系下的惯导2的速度误差矢量，表示惯导2在横地理坐标系下的位置误差，表示与惯导2横纬度误差、横地理坐标系下速度误差相关的横地理坐标系相对惯性坐标系的角速度误差，表示与惯导2横纬度误差相关的地球自转角速度误差，表示惯导2体坐标系至横地理坐标系的方向余弦矩阵，为横地理坐标系相对于惯性坐标系的旋转角速度，为地球自转角速度向量，为横地理坐标系相对于地球坐标系的旋转角速度，g
t
表示载体所在位置的重力矢量，v
t
表示载体在横地理坐标系下的速度，分别表示载体在横东向、横北向、天向的速度，h为载体所在位置的高度，r
x
为载体所在位置的横东向的曲率半径，ry为载体所在位置的横北向的曲率半径，为载体所在位置的扭曲率，re和rn分别为载体所在位置的卯酉圈半径和子午圈半径；
[0215]
(3.2)确定联合状态方程：
[0216][0217]
其中，
[0218]
[0219][0220][0221][0222][0223][0224][0225]
[0226][0227][0228][0229][0230]f87
＝f
rv f
88
＝f
rr
[0231][0232]
式中，0i×j表示i行j列的零矩阵，ω
ie
表示地球自转角速度大小，g
t
分别载体位置处的重力加速度的值，c
23
表示矩阵的第二、第三列，c3表示矩阵的第三列，表示惯导2的x轴陀螺输出值，表示惯导2的y轴陀螺输出值，表示惯导2的z轴陀螺输出值，表示惯导2的x轴加速度计输出值，表示惯导2的y轴加速度计输出值，表示惯导2的z轴加速度计输出值；
[0233]
将状态向量x(t)表示为：
[0234][0235]
式中，表示惯导1在横东向、横北向、天向的姿态误差，分别表示惯导1在横东向、横北向、天向的速度误差，表示惯导1的横纬度误差，表示惯导1的横经度误差，δh1表示惯导1的高度误差，表示惯导2在横东向、横北
向、天向的姿态误差，分别表示惯导2在横东向、横北向、天向的速度误差，表示惯导2的横纬度误差，表示惯导2的横经度误差，δh2表示惯导2的高度误差；
[0236]
将噪声分布矩阵及噪声矩阵表示为：
[0237][0238][0239]
(3.3)确定状态约束观测方程：
[0240]
将惯导1、惯导2系统输出的速度、位置分别表示为：
[0241][0242][0243]
式中，和分别表示惯导1和惯导2输出的横地理坐标系下的速度矢量，v
t
表示公共点的速度真值；惯导1与惯导2之间的外杆臂参数在系统安装完毕后通过标定补偿，表示惯导1输出的位置信息，表示惯导2输出的位置信息，表示b2坐标系相对于横地理坐标系的旋转角速度，表示两套惯导间的外杆臂，表示两套惯导间的外杆臂在惯导2体坐标系下的投影，r
t
表示公共点的位置在横地理坐标系下的真值；
[0244]
由于两套系统反映的是同一载体的速度信息、位置信息，观测量实质上构成了惯导1、惯导2各自速度误差、位置误差的约束，将观测量表示为：
[0245][0246][0247]
式中，υv、υr为相应的速度观测噪声、位置观测噪声；
[0248]
增加外界高度信息的观测
[0249][0250]
式中，为惯导1输出的高度值，υh为高度观测的噪声；
[0251]
将观测方程表示为：
[0252]
z(t)＝h(t)x(t)+υ(t)
[0253]
其中，
[0254]
[0255][0256]
h1＝[0 0 1]
[0257]
υ(t)＝[(υv)
t (υr)
t υh]
t
[0258]
式中，i3×3表示3行3列的单位矩阵；
[0259]
(4)确定两套惯导系统的转位次序：
[0260]
惯导1的转位次序为双轴16次序，具体转位流程如下：
[0261]
次序1：z轴以9
°
/s正向旋转180
°
，转停100s；
[0262]
次序2：y轴以9
°
/s反向旋转180
°
，转停100s；
[0263]
次序3：z轴以9
°
/s正向旋转180
°
，转停100s；
[0264]
次序4：y轴以9
°
/s反向旋转180
°
，转停100s；
[0265]
次序5：y轴以9
°
/s反向旋转180
°
，转停100s；
[0266]
次序6：z轴以9
°
/s正向旋转180
°
，转停100s；
[0267]
次序7：y轴以9
°
/s反向旋转180
°
，转停100s；
[0268]
次序8：z轴以9
°
/s正向旋转180
°
，转停100s；
[0269]
次序9：z轴以9
°
/s反向旋转180
°
，转停100s；
[0270]
次序10：y轴以9
°
/s正向旋转180
°
，转停100s；
[0271]
次序11：z轴以9
°
/s反向旋转180
°
，转停100s；
[0272]
次序12：y轴以9
°
/s正向旋转180
°
，转停100s；
[0273]
次序13：y轴以9
°
/s正向旋转180
°
，转停100s；
[0274]
次序14：z轴以9
°
/s反向旋转180
°
，转停100s；
[0275]
次序15：y轴以9
°
/s正向旋转180
°
，转停100s；
[0276]
次序16：z轴以9
°
/s反向旋转180
°
，转停100s；
[0277]
惯导2的转位次序为18次序，具体转位流程如下：
[0278]
次序1：y轴以9
°
/s正向旋转90
°
，转停180s；
[0279]
次序2：y轴以9
°
/s正向旋转180
°
，转停180s；
[0280]
次序3：y轴以9
°
/s正向旋转180
°
，转停180s；
[0281]
次序4：z轴以9
°
/s正向旋转90
°
，转停180s；
[0282]
次序5：z轴以9
°
/s正向旋转180
°
，转停180s；
[0283]
次序6：z轴以9
°
/s正向旋转180
°
，转停180s；
[0284]
次序7：x轴以9
°
/s正向旋转90
°
，转停180s；
[0285]
次序8：x轴以9
°
/s正向旋转180
°
，转停180s；
[0286]
次序9：x轴以9
°
/s正向旋转180
°
，转停180s；
[0287]
次序10：x轴以9
°
/s正向旋转90
°
，转停180s；
[0288]
次序11：x轴以9
°
/s正向旋转90
°
，转停180s；
[0289]
次序12：x轴以9
°
/s正向旋转90
°
，转停180s；
[0290]
次序13：z轴以9
°
/s正向旋转90
°
，转停180s；
[0291]
次序14：z轴以9
°
/s正向旋转90
°
，转停180s；
[0292]
次序15：z轴以9
°
/s正向旋转90
°
，转停180s；
[0293]
次序16：y轴以9
°
/s正向旋转90
°
，转停180s；
[0294]
次序17：y轴以9
°
/s正向旋转90
°
，转停180s；
[0295]
次序18：y轴以9
°
/s正向旋转90
°
，转停180s；
[0296]
基于联合转位方式，惯导1处于双轴旋转调制导航状态，惯导2处于标定状态，其陀螺标度因数误差、加速度计标度因数误差、安装误差均得到激励，根据步骤(3)所述方案，建立联合状态卡尔曼滤波器即实现惯导2的极区外场标定。
[0297]
本发明方法对载体的运动状态无要求，载体处于系泊状态或运动状态均能实现在线标定。
[0298]
本发明方法不仅仅适用于高纬度地区，在非横北极、横南极的中纬度地区同样适用。
[0299]
惯导1与惯导2处于零位时的相对姿态在安装完毕后标定得到，惯导2在标定初始时刻的姿态基于两套惯导的相对姿态通过与惯导1传递对准获得。
[0300]
所述步骤(4)中的联合转位次序适用于两套及以上具有双轴转位机构的惯导系统间的在线标定，对于双轴和三轴惯导系统之间、多套三轴惯导系统间的在线标定也适用。
[0301]
所述步骤(4)中的联合转位次序仅为基于两套具有双轴转位机构的惯导系统的优选方案，对于其他旋转调制次序与标定次序之间的联合转位方案，也属于本发明的范畴。
[0302]
以上所述仅是本发明的优选实施方式，并不用以限制本发明，凡属于本发明思路下的技术方案均属于本发明的保护范围。在不脱离本发明原理前提下的若干改进和润饰等，这些改进和润饰也应视为本发明的保护范围。

技术特征：
1.基于速度误差修正模型的极地双惯导协同标定方法，其特征在于，所述方法包括以下步骤：(1)构建基于地球椭球模型下的横地球坐标系和横地理坐标系；以北纬0
°
东经90
°
点为横地球坐标系下的北极点，定义为横北极点，北纬0
°
西经90
°
点为横地球坐标系下的南极点，定义为横南极点，0
°
经线和180
°
经线包围的椭圆面为横赤道面，取横北极、横南极和北极点组成的半个大椭圆为0
°
横经线，所在平面为横本初子午线，将地球坐标系与新定义的横地球坐标系之间的转换关系表示为：式中，e系表示地球坐标系，e
′
系表示横地球坐标系，表示地球坐标系与横地球坐标系转换的方向余弦矩阵；基于横经纬网定义横地理坐标系，横北向指向横北极，所在位置的法线向上为天向，按照右手坐标系定义横东向，将横地理坐标系与地理坐标系之间的转换关系表示为：式中，n系表示地理坐标系，t系表示横地理坐标系，β表示地理坐标系与横地理坐标系之间的旋转角度；确定β与经纬度、横经纬度之间的转换关系：确定β与经纬度、横经纬度之间的转换关系：确定方向余弦矩阵和速度矢量的转换关系：确定方向余弦矩阵和速度矢量的转换关系：式中，表示载体坐标系与横地理坐标系之间转换的方向余弦矩阵，表示载体坐标系与地理坐标系之间转换的方向余弦矩阵，v
t
表示横地理坐标系下的载体速度矢量，v
n
表示地理坐标系下的载体速度矢量；将载体所在位置的法线与横赤道面的夹角定义为横纬度，与横本初子午面的夹角定义为横经度，将地球坐标系下定义的经纬度与横经纬度之间的转换关系表示为：为横经度，将地球坐标系下定义的经纬度与横经纬度之间的转换关系表示为：
式中，l
t
表示横纬度，λ
t
表示横经度，l表示纬度，λ表示经度；(2)构建两套惯导系统的误差模型；定义正常工作的双轴旋转调制惯导系统为惯导1，其体坐标系b1定义为“右-前-上”，待标定的惯导系统为惯导2，其体坐标系b2定义为“右-前-上”；惯导1的标度因数误差及安装误差很小进而忽略，将惯导1的误差模型定义为：惯导1的标度因数误差及安装误差很小进而忽略，将惯导1的误差模型定义为：其中，其中，式中，表示惯导1的陀螺组件误差，表示惯导1的加速度计组件误差，表示惯导1的x轴陀螺漂移，表示惯导1的y轴陀螺漂移，表示惯导1的z轴陀螺漂移，表示惯导1的x轴加速度计零偏，表示惯导1的y轴加速度计零偏，表示惯导1的z轴加速度计零偏，表示惯导1的陀螺漂移，表示惯导1的加速度计零偏，为惯导1的陀螺噪声，为惯导1加速度计噪声；考虑标度因数误差、安装角误差及零偏误差，将惯导2的误差模型定义为：考虑标度因数误差、安装角误差及零偏误差，将惯导2的误差模型定义为：其中，其中，式中，表示惯导2的陀螺组件误差，表示惯导2的加速度计组件误差，表示惯导2陀螺组件输出的理论角速度矢量，表示惯导2加速度计组件测得的理论比力矢量，表示惯导2的x轴陀螺漂移，表示惯导2的y轴陀螺漂移，表示惯导2的z轴陀螺漂移，表示惯导2的x轴加速度计零偏，表示惯导2的y轴加速度计零偏，表示惯导2的z轴加速度计零偏，表示惯导2的陀螺漂移，表示惯导2的加速度计零偏，为惯导2的陀螺噪声，为惯导2加速度计噪声；δκ
g
和δμ
g
表示陀螺的标度因数误差矩阵和安装误差矩阵，δκ
a
和δμ
a
表示加速度计的标度因数误差矩阵和安装误差矩阵；确定δκ
g
和δκ
a
：
式中，δκ
gx
、δκ
gy
和δκ
gz
分别表示x轴陀螺、y轴陀螺和z轴陀螺的标度因数误差，δκ
ax
、δκ
ay
和δκ
az
分别表示x轴加速度计、y轴加速度计和z轴加速度计的标度因数误差；确定δμ
g
和δμ
a
：：式中，δμ
gyx
、δμ
gzx
和δμ
gzy
表示陀螺组件的三个安装误差角，δμ
ayx
、δμ
azx
、δμ
azy
、δμ
axy
、δμ
axz
和δμ
ayz
表示加速度计组件的六个安装误差角；(3)利用两套惯导系统输出的姿态、速度、位置相关信息，建立在横地理坐标系下的联合状态卡尔曼滤波器，具体步骤为：(3.1)确定横地理坐标系下基于速度误差修正模型的系统联合误差方程：(3.1)确定横地理坐标系下基于速度误差修正模型的系统联合误差方程：(3.1)确定横地理坐标系下基于速度误差修正模型的系统联合误差方程：(3.1)确定横地理坐标系下基于速度误差修正模型的系统联合误差方程：(3.1)确定横地理坐标系下基于速度误差修正模型的系统联合误差方程：(3.1)确定横地理坐标系下基于速度误差修正模型的系统联合误差方程：其中，其中，其中，
式中，φ
1t
表示惯导1在横地理坐标系下的姿态误差角，表示横地理坐标系下的惯导1的速度误差矢量，δr
1t
表示惯导1在横地理坐标系下的位置误差，表示与惯导1横纬度误差、横地理坐标系下速度误差相关的横地理坐标系相对惯性坐标系的角速度误差，表示与惯导1横纬度误差相关的地球自转角速度误差，表示惯导1体坐标系至横地理坐标系的方向余弦矩阵，φ
2t
表示惯导2在横地理坐标系下的姿态误差角，表示横地理坐标系下的惯导2的速度误差矢量，表示惯导2在横地理坐标系下的位置误差，表示与惯导2横纬度误差、横地理坐标系下速度误差相关的横地理坐标系相对惯性坐标系的角速度误差，表示与惯导2横纬度误差相关的地球自转角速度误差，表示惯导2体坐标系至横地理坐标系的方向余弦矩阵，为横地理坐标系相对于惯性坐标系的旋转角速度，为地球自转角速度向量，为横地理坐标系相对于地球坐标系的旋转角速度，g
t
表示载体所在位置的重力矢量，v
t
表示载体在横地理坐标系下的速度，分别表示载体在横东向、横北向、天向的速度，h为载体所在位置的高度，r
x
为载体所在位置的横东向的曲率半径，r
y
为载体所在位置的横北向的曲率半径，为载体所在位置的扭曲率，r
e
和r
n
分别为载体所在位置的卯酉圈半径和子午圈半径；(3.2)确定联合状态方程：其中，
式中，0
i
×
j
表示i行j列的零矩阵，ω
ie
表示地球自转角速度大小，g
t
分别载体位置处的重力加速度的值，c
23
表示矩阵的第二、第三列，c3表示矩阵的第三列，表示惯导2的x轴陀螺输出值，表示惯导2的y轴陀螺输出值，表示惯导2的z轴陀螺输出值，表示惯导2的x轴加速度计输出值，表示惯导2的y轴加速度计输出值，表示惯导2的z轴加速度计输出值；将状态向量x(t)表示为：式中，表示惯导1在横东向、横北向、天向的姿态误差，分别表示惯导1在横东向、横北向、天向的速度误差，表示惯导1的横纬度误差，表示惯导1的横经度误差，δh1表示惯导1的高度误差，表示惯导2在横东向、横北向、天向
的姿态误差，分别表示惯导2在横东向、横北向、天向的速度误差，表示惯导2的横纬度误差，表示惯导2的横经度误差，δh2表示惯导2的高度误差；将噪声分布矩阵及噪声矩阵表示为：将噪声分布矩阵及噪声矩阵表示为：(3.3)确定状态约束观测方程：将惯导1、惯导2系统输出的速度、位置分别表示为：将惯导1、惯导2系统输出的速度、位置分别表示为：式中，和分别表示惯导1和惯导2输出的横地理坐标系下的速度矢量，v
t
表示公共点的速度真值；惯导1与惯导2之间的外杆臂参数在系统安装完毕后通过标定补偿，表示惯导1输出的位置信息，表示惯导2输出的位置信息，表示b2坐标系相对于横地理坐标系的旋转角速度，表示两套惯导间的外杆臂，表示两套惯导间的外杆臂在惯导2体坐标系下的投影，r
t
表示公共点的位置在横地理坐标系下的真值；由于两套系统反映的是同一载体的速度信息、位置信息，观测量实质上构成了惯导1、惯导2各自速度误差、位置误差的约束，将观测量表示为：惯导2各自速度误差、位置误差的约束，将观测量表示为：式中，υ
v
、υ
r
为相应的速度观测噪声、位置观测噪声；增加外界高度信息的观测增加外界高度信息的观测式中，为惯导1输出的高度值，υ
h
为高度观测的噪声；将观测方程表示为：z(t)＝h(t)x(t)+υ(t)其中，
h1＝[0 0 1]υ(t)＝[(υ
v
)
t (υ
r
)
t υ
h
]
t
式中，i3×3表示3行3列的单位矩阵；(4)确定两套惯导系统的转位次序：惯导1的转位次序为双轴16次序，具体转位流程如下：次序1：z轴以9
°
/s正向旋转180
°
，转停100s；次序2：y轴以9
°
/s反向旋转180
°
，转停100s；次序3：z轴以9
°
/s正向旋转180
°
，转停100s；次序4：y轴以9
°
/s反向旋转180
°
，转停100s；次序5：y轴以9
°
/s反向旋转180
°
，转停100s；次序6：z轴以9
°
/s正向旋转180
°
，转停100s；次序7：y轴以9
°
/s反向旋转180
°
，转停100s；次序8：z轴以9
°
/s正向旋转180
°
，转停100s；次序9：z轴以9
°
/s反向旋转180
°
，转停100s；次序10：y轴以9
°
/s正向旋转180
°
，转停100s；次序11：z轴以9
°
/s反向旋转180
°
，转停100s；次序12：y轴以9
°
/s正向旋转180
°
，转停100s；次序13：y轴以9
°
/s正向旋转180
°
，转停100s；次序14：z轴以9
°
/s反向旋转180
°
，转停100s；次序15：y轴以9
°
/s正向旋转180
°
，转停100s；次序16：z轴以9
°
/s反向旋转180
°
，转停100s；惯导2的转位次序为18次序，具体转位流程如下：次序1：y轴以9
°
/s正向旋转90
°
，转停180s；次序2：y轴以9
°
/s正向旋转180
°
，转停180s；次序3：y轴以9
°
/s正向旋转180
°
，转停180s；次序4：z轴以9
°
/s正向旋转90
°
，转停180s；次序5：z轴以9
°
/s正向旋转180
°
，转停180s；次序6：z轴以9
°
/s正向旋转180
°
，转停180s；次序7：x轴以9
°
/s正向旋转90
°
，转停180s；次序8：x轴以9
°
/s正向旋转180
°
，转停180s；次序9：x轴以9
°
/s正向旋转180
°
，转停180s；次序10：x轴以9
°
/s正向旋转90
°
，转停180s；次序11：x轴以9
°
/s正向旋转90
°
，转停180s；次序12：x轴以9
°
/s正向旋转90
°
，转停180s；次序13：z轴以9
°
/s正向旋转90
°
，转停180s；次序14：z轴以9
°
/s正向旋转90
°
，转停180s；
次序15：z轴以9
°
/s正向旋转90
°
，转停180s；次序16：y轴以9
°
/s正向旋转90
°
，转停180s；次序17：y轴以9
°
/s正向旋转90
°
，转停180s；次序18：y轴以9
°
/s正向旋转90
°
，转停180s；基于联合转位方式，惯导1处于双轴旋转调制导航状态，惯导2处于标定状态，其陀螺标度因数误差、加速度计标度因数误差、安装误差均得到激励，根据步骤(3)所述方案，建立联合状态卡尔曼滤波器即实现惯导2的极区外场标定。2.如权利要求1所述的基于速度误差修正模型的极地双惯导协同标定方法，其特征在于，所述方法对载体的运动状态无要求，载体处于系泊状态或运动状态均能实现在线标定。3.如权利要求1所述的基于速度误差修正模型的极地双惯导协同标定方法，其特征在于，所述方法不仅仅适用于高纬度地区，在非横北极、横南极的中纬度地区同样适用。4.如权利要求1所述的基于速度误差修正模型的极地双惯导协同标定方法，其特征在于，惯导1与惯导2处于零位时的相对姿态在安装完毕后标定得到，惯导2在标定初始时刻的姿态基于两套惯导的相对姿态通过与惯导1传递对准获得。5.如权利要求1所述的基于速度误差修正模型的极地双惯导协同标定方法，其特征在于，步骤(4)所述的联合转位次序适用于两套及以上具有双轴转位机构的惯导系统间的在线标定，对于双轴和三轴惯导系统之间、多套三轴惯导系统间的在线标定也适用。

技术总结
本发明属于导航领域，公开了基于速度误差修正模型的极地双惯导协同标定方法，适用于装备多套带有转位机构惯导系统的载体在极地的自主标定。本发明针对极地航行的舰船的惯导系统无外界参考信息时自标定能力受限的问题，以地球椭球模型下的横地理坐标系为导航坐标系，将两套惯导系统在横地理坐标系下的相对速度、相对位置为约束观测，建立了双惯导系统联合误差状态卡尔曼滤波器，在极地无外界基准信息的情况下，对待标定惯导系统的误差参数进行标定估计。本发明方法完全自主，以两套惯导系统的相对误差为约束观测，标定精度不受正常工作惯导系统绝对误差的影响；通过修正速度误差模型能提高动基座下的标定精度，具有重要工程意义。义。义。


技术研发人员：王林 梁钟泓 廖志坤 郭鸿刚 王元涵 郝艳 陈宏翔 宋东阳
受保护的技术使用者：中国人民解放军国防科技大学
技术研发日：2023.03.11
技术公布日：2023/9/14