一种双轴旋转惯导动态误差抑制方法

1.本发明涉及双轴旋转惯导误差标定与抑制技术领域，特别涉及一种双轴旋转惯导动态误差抑制方法。


背景技术：

2.双轴旋转惯导由惯性测量单元(initial measurement unit，简称为imu)和转位机构组成，工作原理为：将imu安装在转位机构上，使imu在导航过程中可以相对固定的坐标系轴旋转，由于imu通过特定的旋转，将惯性器件的常值误差调制成为均值为零的周期变化量，可以在现有的惯性器件精度水平上，大大的提高双轴旋转惯导长航时的导航精度。因此，双轴旋转惯导被广泛地应用在需要高精度惯性导航系统的应用场合中，例如：舰艇、战车、飞机等运载体中。
3.在双轴旋转惯导的实际使用中，合理的双轴转位方案虽然可以完全调制imu中惯性器件的常值零偏，但是无法调制由运载体运动激励惯性器件标度因数误差、安装误差、加速度计二阶非线性系数和加速度计内杆臂引起的动态误差。因此，为提高双轴旋转惯导在动态环境下的精度，需要对双轴旋转惯导的惯性器件标度因数误差、安装误差、加速度计二阶非线性系数和加速度计内杆臂进行精确标定，并在标定后进行补偿以抑制双轴旋转惯导的动态误差。
4.为了达到这个目的，已公开发明专利cn103575296b提供了一种双轴旋转惯导系统自标定方法，该方法利用遗传算法优化标定试验路径并利用最小二乘法处理标定数据标定出了双轴旋转惯导imu中惯性器件的零偏、标度因素和安装误差；已公开发明专利cn104165638b提出了一种双轴旋转惯导系统多位置自主标定方法，该方法利用卡尔曼滤波和最小二乘法标定出了陀螺和加速度计的零偏、标度因素和安装误差；已公开发明专利cn108981751a公开了一种双轴旋转惯导系统的八位置在线标定方法，该方法标定出了imu的零偏、标度因素和安装误差。上述现有方法能较好地标定出imu的零偏、标度因素和安装误差；然而，上述方案均未考虑加速度计二阶非线性系数和加速度计内杆臂引起的双轴旋转惯导动态误差，导致了双轴旋转惯导依然存在动态环境下出现较大误差的问题。
5.因此，为了克服上述现有方法中存在着的未考虑加速度计二阶非线性系数和加速度计内杆臂引起的双轴旋转惯导动态误差导致双轴旋转惯导在动态环境下误差较大的问题，需要提供一种简便、高精度的，全面考虑imu的零偏、标度因素、安装误差、加速度计二阶非线性系数和加速度计内杆臂的双轴旋转惯导动态误差抑制方法。


技术实现要素：

6.本发明的目的是提供一种解决目前现有的双轴旋转惯导误差标定与抑制方法中存在着的未考虑加速度计二阶非线性系数和加速度计内杆臂引起的双轴旋转惯导动态误差导致双轴旋转惯导在动态环境下误差较大的问题的双轴旋转惯导动态误差抑制方法。
7.为此，本发明技术方案如下：
8.一种双轴旋转惯导动态误差抑制方法，步骤如下：
9.s1、构建陀螺敏感轴系、加速度计敏感轴系、imu坐标系、地心惯性坐标系、地球坐标系和导航坐标系，并确定陀螺的安装误差矩阵和加速度计的安装误差矩阵；
10.s2、基于imu的零偏、标度因素、安装误差、加速度计二阶非线性系数和加速度计内杆臂引发的误差，确定双轴旋转惯导在导航坐标系下的误差方程；
11.s3、基于双轴旋转惯导的误差方程，构建三十六维卡尔曼滤波器，包括三十六维卡尔曼滤波器的状态量、三十六维卡尔曼滤波器滤波器的状态方程、三十六维卡尔曼滤波器滤波器的观测方程和卡尔曼滤波估计结果的反馈补偿形式；其中，三十六维卡尔曼滤波器的状态量包含双轴旋转惯导的姿态误差角速度误差δv，位置误差δp，陀螺的标定参数误差xg，加速度计的标定参数误差xa，加速度计二阶非线性系数标定误差x
ka2
和内杆臂误差xr；
12.s4、标定试验：开机预热完成后，双轴转位机构根据设计的转位路径进行转动，保存该过程中由双轴旋转惯导的陀螺输出的角速率数据和加速度计输出的比力数据，作为标定数据；其中，转位路径应设计应满足：双轴旋转惯导的惯性测量单元(imu)的每个轴均经过单方向的两次180
°
旋转，以及imu的每个轴均历经指天、指地两个位置；
13.s5、利用卡尔曼滤波算法和步骤s3构建的卡尔曼滤波器对步骤s4的标定数据进行处理，使卡尔曼滤波器中的状态量从初值开始随步骤s4的多次转位而得到不断迭代和反馈补偿，最终获得卡尔曼滤波器中的状态量估计值；其中，三十六维状态量的初值基于双轴旋转的初对准结果和仪器参数进行赋值；在最优的卡尔曼滤波器中的状态量估计值中，关于惯性器件误差的状态量分量为标定参数；
14.s6、将标定参数代入陀螺的标定模型和加速度计的标定模型中，得到标定后输出的角速率和比力，并基于标定后的角速率和比力得到经过加速度计内杆臂补偿的加速度，进而解算得到双轴旋转惯导经动态误差实时补偿后的导航结果。
15.进一步地，步骤s1的具体实施步骤为：
16.s101、构建陀螺敏感轴系，即g系，该坐标系以三个陀螺的测量中心点为坐标原点，xg轴与x向陀螺的敏感轴方向一致，yg轴与y向陀螺的敏感轴方向一致，zg轴与z向陀螺的敏感轴方向一致；
17.s102、构建加速度计敏感轴系，即a系，该坐标系以g系的坐标原点为坐标原点，xa轴与x向加速度计的敏感轴方向一致，ya轴与y向加速度计的敏感轴方向一致，za轴与z向加速度计的敏感轴方向一致；
18.s103、构建imu坐标系，即m系，该坐标系以g系的坐标原点为坐标原点，xm轴与陀螺敏感轴xg轴一致，ym轴在陀螺敏感轴xg轴与yg轴构成的平面中，且与yg轴相差小角度γ
yz
，zm轴为陀螺敏感轴zg轴绕xm轴旋转小角度γ
zx
，再绕ym轴旋转小角度γ
zy
；
19.s104、构建地心惯性坐标系，即i系，该坐标系以地球中心为坐标原点，xi和yi轴在地球赤道平面内，且xi轴指向春分点，zi轴为地球自转轴，并指向北极；
20.s105、构建地球坐标系，即e系，该坐标系以地球中心为坐标原点，xe和ye轴在地球赤道平面内，且xe指向本初子午线，ze轴为地球自转轴，并指向北极；
21.s106、构建导航坐标系，即n系，该坐标系为东北天地理坐标系，其坐标原点为imu坐标系的坐标原点，xn轴指向东，yn轴指向北，zn轴指向天；
22.s107、基于陀螺敏感轴系和imu坐标系，陀螺的安装误差矩阵的表达式为：
[0023][0024]
式中，为陀螺的安装误差矩阵，γ
yz
、γ
zy
和γ
zx
为陀螺敏感轴系相对于imu坐标系的三个安装误差角，具体地，γ
yz
表示y向陀螺敏感轴yg轴绕m系的zm轴的偏转角度，γ
zy
表示z向陀螺敏感轴zg轴绕m系的ym轴的偏转角度，γ
zx
表示z向陀螺敏感轴zg轴绕m系的xm轴的偏转角度；
[0025]
s108、加速度计的安装误差矩阵由六个加速度计安装误差角构成，其表达式为：
[0026][0027]
式中，为加速度计的安装误差矩阵，α
xz
、α
xy
、α
yz
、α
yx
、α
zy
和α
zx
为加速度计敏感轴系相对于imu坐标系的六个安装误差角，具体地，α
xz
表示x向加速度计敏感轴xa轴绕m系的zm轴的偏转角度，α
xy
表示x向加速度计敏感轴xa轴绕m系的ym轴的偏转角度，α
yz
表示y向加速度计敏感轴ya轴绕m系的zm轴的偏转角度，α
yx
表示y向加速度计敏感轴ya轴绕m系的xm轴的偏转角度，α
zy
表示z向加速度计敏感轴za轴绕m系的ym轴的偏转角度，α
zx
表示z向加速度计敏感轴za轴绕m系的xm轴的偏转角度。
[0028]
进一步地，步骤s2的具体实施步骤为：
[0029]
双轴旋转惯导在导航坐标系(n系)下的误差方程，其表达式为：
[0030][0031][0032][0033][0034][0035]
式中，为相对于时间的导数，为双轴旋转惯导的姿态误差角，和分别为双轴旋转惯导的东向误差角、北向误差角和天向误差角；为n系相对于i系的转动角速率，其由地球自转与载体运动产生；为导航解算中的估计误差；为m系到n系的坐标转换矩阵；为陀螺的测量误差；为δvn相对于时间的导数，δvn为双轴旋转惯导的速度误差，δvn＝[δv
e δv
n δvu]
t
，δve、δvn和δvu分别为东向速度误差、北向速度误差和天向速度误差；fn为n系下的比力；为地球自转角速率；为载体绕地球运动产生的角速率；；vn为双轴旋转惯导的速度，vn＝[v
e v
n vu]
t
，ve、vn和vu分别为双轴旋转惯导的东向速度、北向速度和天向速度；为的误差；为的
误差；δfm为加速度计的测量误差；为加速度计内杆臂误差；δg为重力矢量误差；为δl相对于时间的导数，δl为纬度误差；为δλ相对于时间的导数，δλ为经度误差；为δh相对于时间的导数，δh为高度误差；l、λ和h分别为当地地理纬度、经度和高度；rm和rn分别为当地地球子午圈和卯酉圈半径；
[0036]
其中，陀螺的测量误差的表达式为：式中，为陀螺在m系下的测量值；δkg为陀螺的标度因数和安装误差的误差矩阵，和分别为x向陀螺标度因数y向陀螺标度因数和z向陀螺标度因数的误差；δγ
yz
、δγ
zy
和δγ
zx
分别为陀螺安装误差角γ
yz
、γ
zy
和γ
zx
的误差；εm为陀螺的零偏误差，其表达式为：εm＝[ε
x ε
y εz]
t
，式中，ε
x
、εy和εz分别为x向陀螺、y向陀螺和z向陀螺的零偏误差；
[0037]
加速度计的测量误差δfm的表达式为：式中，为加速度计在m系下的测量值；δka为加速度计的标度因数和安装误差的误差矩阵，和分别为x向加速度计标度因数y向加速度计标度因数和z向加速度计标度因数的误差；δα
xz
、δα
xy
、δα
yz
、δα
yx
、δα
zy
和δα
zx
分别为加速度计安装误差角α
xz
、α
xy
、α
yz
、α
yx
、α
zy
和α
zx
的误差；δk
a2
为加速度计的二阶非线性系数的标定误差矩阵，δk
ax2
、δk
ay2
和δk
az2
分别为x向加速度计的二阶非线性系数k
ax2
的标定误差、y向加速度计的二阶非线性系数k
ay2
的标定误差和z向加速度计的二阶非线性系数k
az2
的标定误差；为加速度计的零偏误差，和分别为x向加速度计、y向加速度计和z向加速度计的零偏误差；
[0038]
加速度计内杆臂误差的表达式为：式中，式中，和分别为内杆臂误差在m系中xm轴、ym轴和zm轴上的分量，其中，和分别为陀螺在m系下的测量值在xm轴、ym轴和zm轴上的分量；δr
x
、δry和δrz分别为x向加速度计内杆臂r
x
、y向加速度计内杆臂ry和z向加速度计内杆臂rz的误差。
[0039]
进一步地，步骤s3的具体实施步骤如下：
[0040]
s301、构建三十六维卡尔曼滤波器的状态量x
36
：
[0041][0042]
式中，式中，为双轴旋转惯导的姿态误差角，δv为速度误差，δv＝[δv
e δv
n δvu]
t
；δp为位置误差，δp＝[δl δλ δh]
t
；xg为陀螺的标定参数误差，xa为加速度计的标定参数误差，x
ka2
为加速度计二阶非线性系数标定误差，x
ka2
＝[δk
ax2 δk
ay2 δk
az2
]
t
；xr为内杆臂误差，xr＝[δr
x δr
y δrz]
t
；
[0043]
s302、构建三十六维卡尔曼滤波器滤波器的状态方程：
[0044]
式中，在f
33
中，
[0045][0046][0047]
[0048][0049][0050][0051][0052]f36
中，
[0053]g36
为测量噪声输入矩阵为，u为测量噪声，其中，ug为陀螺的测量噪声，ua为加速度计的测量噪声；
[0054]
s303、构建三十六维卡尔曼滤波器滤波器的观测方程：z＝h
36
x
36
+v，
[0055]
式中，z为观测量，以速度为观测量且在标定时双轴旋转惯导保持静止状态，则z＝[v
e v
n vu]
t
；h
36
为观测矩阵，h
36
＝[03×
3 i
3 03×
30
]，i3为三行三列的单位矩阵，即
03×
30
为三行三十列的零矩阵，03×3为三行三列的零矩阵；v是观测噪声；
[0056]
s304、确定卡尔曼滤波估计结果的反馈补偿形式：
[0057][0058][0059][0060][0061][0062][0063][0064]
式中，为的反对称矩阵，为双轴旋转惯导的的解算结果；为双轴旋转惯导的vn的解算结果；为双轴旋转惯导的l的解算结果；为双轴旋转惯导的λ的解算结果；为双轴旋转惯导的h的解算结果；，为卡尔曼滤波估计结果反馈补偿前的kg；为卡尔曼滤波估计结果反馈补偿前的εm；，为卡尔曼滤波估计结果反馈补偿前的ka；为卡尔曼滤波估计结果反馈补偿前的为卡尔曼滤波估计结果反馈补偿前的k
a2
；为卡尔曼滤波估计结果反馈补偿前的r
x
；为卡尔曼滤波估计结果反馈补偿前的ry；为卡尔曼滤波估计结果反馈补偿前的rz。
[0065]
进一步地，在步骤s4中，双轴转位机构的转位路径具体设计为：
[0066]
基于次序0对应初始位置：双轴旋转惯导以其xm轴朝东向，ym轴朝北向，zm轴朝天向的姿态，静止10分钟；
[0067]
次序1：以中框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝东向，ym轴朝天向，zm轴朝南向；转位完成后，静止180s；
[0068]
次序2：以中框轴为旋转轴，顺时针旋转180
°
；该状态下，双轴旋转惯导的xm轴朝东向，ym轴朝地向，zm轴朝北向；转位完成后，静止180s；
[0069]
次序3：以中框轴为旋转轴，顺时针旋转180
°
；该状态下，双轴旋转惯导的xm轴朝东向，ym轴朝天向，zm轴朝南向；转位完成后，静止180s；
[0070]
次序4：以内框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝天向，ym轴朝西向，zm轴朝南向；转位完成后，静止180s；
[0071]
次序5：以内框轴为旋转轴，顺时针旋转180
°
；该状态下，双轴旋转惯导的xm轴朝地向，ym轴朝东向，zm轴朝南向；转位完成后，静止180s；
[0072]
次序6：以内框轴为旋转轴，顺时针旋转180
°
；该状态下，双轴旋转惯导的xm轴朝天向，ym轴朝西向，zm轴朝南向；转位完成后，静止180s；
[0073]
次序7：以中框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝南向，ym轴朝西向，zm轴朝地向；转位完成后，静止180s；
[0074]
次序8：以中框轴为旋转轴，顺时针旋转180
°
；该状态下，双轴旋转惯导的xm轴朝北
向，ym轴朝西向，zm轴朝天向；转位完成后，静止180s；
[0075]
次序9：以中框轴为旋转轴，顺时针旋转180
°
；该状态下，双轴旋转惯导的xm轴朝南向，ym轴朝西向，zm轴朝地向；转位完成后，静止180s；
[0076]
次序10：以中框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝地向，ym轴朝西向，zm轴朝北向；转位完成后，静止180s；
[0077]
次序11：以中框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝北向，ym轴朝西向，zm轴朝天向；转位完成后，静止180s；
[0078]
次序12：以中框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝天向，ym轴朝西向，zm轴朝南向；转位完成后，静止180s；
[0079]
次序13：以内框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝西向，ym轴朝地向，zm轴朝南向；转位完成后，静止180s；
[0080]
次序14：以内框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝地向，ym轴朝东向，zm轴朝南向；转位完成后，静止180s；
[0081]
次序15：以内框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝东向，ym轴朝天向，zm轴朝南向；转位完成后，静止180s；
[0082]
次序16：以中框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝东向，ym轴朝南向，zm轴朝地向；转位完成后，静止180s；
[0083]
次序17：以中框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝东向，ym轴朝地向，zm轴朝北向；转位完成后，静止180s；
[0084]
次序18：以中框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝东向，ym轴朝北向，zm轴朝天向；转位完成后，静止180s。
[0085]
进一步地，在步骤s4中，一次标定实验中按照设计的转位路径进行1～3次双轴转位机构的转位。
[0086]
进一步地，步骤s5的具体实施步骤为：
[0087]
s501、对三十六维状态量进行初值赋值：和δv的初值分别由步骤s4中双轴旋转的初对准结果中的初始姿态角和初始速度进行赋值；δp的初值由步骤s4中双轴旋转惯导完成初对准后输出的初始位置减去实际实验位置得到的差值进行赋值；xg、xa、x
ka2
和xr的初值根据双轴旋转惯导中陀螺和加速度计的仪器实际情况进行赋值；
[0088]
s502、基于步骤s501确定的三十六维状态量初值作为卡尔曼滤波的状态量的初值，将步骤s301构建的三十六维状态量分别带入步骤s302构建的三十六维卡尔曼滤波器状态方程和步骤s303构建的三十六维卡尔曼滤波器观测方程中，采用卡尔曼滤波算法和步骤s3构建的卡尔曼滤波器处理标定数据，通过对卡尔曼滤波器中的状态量从初值开始不断迭代和反馈补偿，获得卡尔曼滤波器中的状态量估计值；其中，状态量估计值中关于惯性器件误差的状态量分量即准确的标定参数，包括：x向陀螺标度因数y向陀螺标度因数和z向陀螺标度因数陀螺安装误差角γ
yz
、γ
zy
和γ
zx
；x向陀螺零偏y向陀螺零偏和z向陀螺零偏x向加速度计标度因数y向加速度计标度因数和z向加速度计标度因数的误差；加速度计安装误差角α
xz
、α
xy
、α
yz
、α
yx
、α
zy
和α
zx
；x向加速度计的二阶非线性系数k
ax2
、y向加速度计的二阶非线性系数k
ay2
和z向加速度计的二阶非线性系数k
az2
；
x向加速度计零偏y向加速度计零偏和z向加速度计零偏x向加速度计内杆臂r
x
、y向加度计内杆臂ry和z向加速度计内杆臂rz；
[0089]
进一步地，步骤s6的具体实施步骤为：
[0090]
s601、采用现有的六十四次序旋转调制方案作为双轴旋转惯导导航过程中的转位机构旋转方案，并在每转动至一个位置后停止10s时间，获得旋转调制后的惯性器件输出结果，包括陀螺输出的角速率和加速度计输出的比力
[0091]
s602、将由步骤s5获得的标定参数分别代入至陀螺标定模型和加速度计标定模型中，以获得陀螺标定后的输出和加速度计标定后的输出；其中，
[0092]
(1)陀螺的标定模型为：
[0093]
式中，
[0094]
(2)加速度计的标定模型为：
[0095]
式中，
[0096]
s603、利用经过步骤s602得到的陀螺标定后的输出和加速度计标定后的输出，利用x向加速度计内杆臂r
x
、y向加速度计内杆臂ry和z向加速度计内杆臂rz对加速度数据进行实时补偿，其具体公式为：
[0097][0098][0099][0100]
式中，a
xb
、a
yb
和a
zb
为加速度计内杆臂补偿后的加速度实时结果在m系中xm轴、ym轴和zm轴的分量；a
xc
、a
yc
和a
zc
为加速度计内杆臂补偿前的加速度实时结果在m系中xm轴、ym轴和zm轴的分量；ω
xc
、ω
yc
和ω
zc
为角速率在m系中xm轴、ym轴和zm轴的分量；
[0101]
s604、将步骤s603中的a
xb
、a
yb
、a
zb
和ω
xc
、ω
yc
、ω
zc
带入惯导解算方程，获得双轴旋转惯导的导航结果，即采用本技术实现的对双轴旋转惯导动态误差进行实时补偿后的导航结果。
[0102]
与现有技术相比，该双轴旋转惯导动态误差抑制方法的有益效果在于：
[0103]
(1)本技术的双轴旋转惯导动态误差抑制方法简便且精度高，全面考虑imu的零偏、标度因素、安装误差、加速度计二阶非线性系数和加速度计内杆臂导致的双轴旋转惯导动态误差；
[0104]
(2)采用本技术的方法进行标定补偿后，由动态引起的速度误差明显减少，其效果可达90％，且能提高动态环境下双轴旋转惯导的定位精度约8米，证明本技术方法的正确性和准确性，即能很好抑制双轴旋转惯导动态误差，实用性佳。
附图说明
[0105]
图1为本发明的双轴旋转惯导动态误差抑制方法的流程示意图；
[0106]
图2为本发明在步骤s1中构建的陀螺敏感轴系和imu坐标系间坐标系转换的示意图；
[0107]
图3(a)为本发明实施例中双轴旋转惯导在进行动态误差补偿前后的东向速度误差对比的示意图；
[0108]
图3(b)为本发明实施例中双轴旋转惯导在进行动态误差补偿前后的北向速度误差对比的示意图；
[0109]
图4(a)为本发明实施例中双轴旋转惯导在进行动态误差补偿前后的东向位置误差对比的示意图；
[0110]
图4(b)为本发明实施例中双轴旋转惯导在进行动态误差补偿前后的北向位置误差对比的示意图。
具体实施方式
[0111]
下面结合附图及具体实施例对本发明做进一步的说明，但下述实施例绝非对本发明有任何限制。
[0112]
如图1所示，该双轴旋转惯导动态误差抑制方法的具体实施步骤如下：
[0113]
s1、构建陀螺敏感轴系(g系)、加速度计敏感轴系(a系)、imu坐标系(m系)、地心惯性坐标系(i系)、地球坐标系(e系)和导航坐标系(n系)，并确定陀螺的安装误差矩阵和加速度计的安装误差矩阵；
[0114]
具体地，在该步骤s1中，坐标系构建如下：
[0115]
(1)陀螺敏感轴系，又简称为g系，其表示为：o-xgygzg；该坐标系以三个陀螺的测量中心点为坐标原点(o点)，其xg轴与x向陀螺的敏感轴方向一致，其yg轴与y向陀螺的敏感轴方向一致，其zg轴与z向陀螺的敏感轴方向一致；由于双轴旋转惯导的imu中，三个陀螺无法保证严格正交安装，因此，g系不是正交坐标系；
[0116]
(2)加速度计敏感轴系，又简称为a系，其表示为：o-xayaza；该坐标系的坐标原点与g系的坐标原点重合，即以g系的坐标原点(o点)为坐标原点，xa轴与x向加速度计的敏感轴方向一致，ya轴与y向加速度计的敏感轴方向一致，za轴与z向加速度计的敏感轴方向一致；同样地，由于双轴旋转惯导的imu中，三个加速度计无法保证严格正交安装，因此，a系也不是正交坐标系；
[0117]
(3)imu坐标系，又简称为m系，其表示为：o-xmymzm；由于惯性导航解算需要将各惯性器件的输出投影到同一个正交坐标系中，因此，该坐标系为用于将陀螺和加速度计的输出投影为正交向量的正交坐标系；如图2所示，该坐标系的坐标原点与g系的坐标原点重合，即以g系的坐标原点(o点)为坐标原点，xm轴与陀螺敏感轴xg轴一致，ym轴在陀螺敏感轴xg轴与yg轴构成的平面中，且与yg轴相差小角度γ
yz
，zm轴为陀螺敏感轴zg轴绕xm轴旋转小角度γ
zx
，再绕ym轴旋转小角度γ
zy
；
[0118]
(4)地心惯性坐标系，又简称为i系，其表示为o
e-xiyizi，该坐标系作为惯性器件的输出的参考基准，其坐标原点oe为地球中心，xi和yi轴在地球赤道平面内，且xi轴指向春分点(即赤道面与黄道面的交线再与天球相交的交点之一)，zi轴为地球自转轴，并指向北
极；
[0119]
(5)地球坐标系，又简称为e系，其表示为o
e-xeyeze；该坐标系的坐标原点oe为地球中心，xe和ye轴在地球赤道平面内，且xe指向本初子午线，ze轴为地球自转轴，并指向北极；该地球坐标系与地球固连，e系相对于i系的角速率为地球自转角速率；
[0120]
(6)导航坐标系，又简称为n系，其表示为o-xnynzn；该坐标系作为惯导在求解导航参数时采用的坐标系，具体为东北天地理坐标系，其坐标原点o为imu坐标系的坐标原点，xn轴指向东，yn轴指向北，zn轴指向天，n系相对于e系的方位关系就是载体的地理位置；
[0121]
基于上述构建的陀螺敏感轴系(g系)和imu坐标系(m系)，可以确定陀螺的安装误差矩阵的表达式为：
[0122][0123]
式中，为陀螺的安装误差矩阵，γ
yz
、γ
zy
和γ
zx
为陀螺敏感轴系相对于imu坐标系的三个安装误差角，具体地，γ
yz
表示y向陀螺敏感轴yg轴绕m系的zm轴的偏转角度，γ
zy
表示z向陀螺敏感轴zg轴绕m系的ym轴的偏转角度，γ
zx
表示z向陀螺敏感轴zg轴绕m系的xm轴的偏转角度；
[0124]
另外，由于陀螺组件与加速度计组件之间存在小角度安装误差，因此，为了将加速度计的测量信息转换到imu坐标系下，加速度计的安装误差矩阵由六个加速度计安装误差角构成，其表达式为：
[0125][0126]
式中，为加速度计的安装误差矩阵，α
xz
、α
xy
、α
yz
、α
yx
、α
zy
和α
zx
为加速度计敏感轴系相对于imu坐标系的六个安装误差角，具体地，α
xz
表示x向加速度计敏感轴xa轴绕m系的zm轴的偏转角度，α
xy
表示x向加速度计敏感轴xa轴绕m系的ym轴的偏转角度，α
yz
表示y向加速度计敏感轴ya轴绕m系的zm轴的偏转角度，α
yx
表示y向加速度计敏感轴ya轴绕m系的xm轴的偏转角度，α
zy
表示z向加速度计敏感轴za轴绕m系的ym轴的偏转角度，α
zx
表示z向加速度计敏感轴za轴绕m系的xm轴的偏转角度；
[0127]
s2、基于imu的零偏、标度因素、安装误差、加速度计二阶非线性系数和加速度计内杆臂引发的误差，确定双轴旋转惯导在导航坐标系(n系)下的误差方程，其表达式为：
[0128][0129][0130][0131]
[0132][0133]
式中，为相对于时间的导数，为双轴旋转惯导的姿态误差角，和分别为双轴旋转惯导的东向误差角、北向误差角和天向误差角；为n系相对于i系的转动角速率，其由地球自转与载体运动产生；为导航解算中的估计误差；为m系到n系的坐标转换矩阵；为陀螺的测量误差；为δvn相对于时间的导数，δvn为双轴旋转惯导的速度误差，δvn＝[δv
e δv
n δvu]
t
，δve、δvn和δvu分别为东向速度误差、北向速度误差和天向速度误差；fn为n系下的比力；为地球自转角速率；为载体绕地球运动产生的角速率；；vn为双轴旋转惯导的速度，vn＝[v
e v
n vu]
t
，ve、vn和vu分别为双轴旋转惯导的东向速度、北向速度和天向速度；为的误差；为的误差；δfm为加速度计的测量误差；为加速度计内杆臂误差；δg为重力矢量误差；为δl相对于时间的导数，δl为纬度误差；为δλ相对于时间的导数，δλ为经度误差；为δh相对于时间的导数，δh为高度误差；l、λ和h分别为当地地理纬度、经度和高度；rm和rn分别为当地地球子午圈和卯酉圈半径；
[0134]
其中，陀螺的测量误差的表达式为：
[0135]
式中，为陀螺在m系下的测量值；δkg为陀螺的标度因数和安装误差的误差矩阵，和分别为x向陀螺标度因数y向陀螺标度因数和z向陀螺标度因数的误差；δγ
yz
、δγ
zy
和δγ
zx
分别为陀螺安装误差角γ
yz
、γ
zy
和γ
zx
的误差；
[0136]
εm为陀螺的零偏误差，其表达式为：εm＝[ε
x ε
y εz]
t
，
[0137]
式中，ε
x
、εy和εz分别为x向陀螺、y向陀螺和z向陀螺的零偏误差；
[0138]
加速度计的测量误差δfm的表达式为：
[0139]
式中，为加速度计在m系下的测量值；δka为加速度计的标度因数和安装误差的误差矩阵，和分别为x向加速度计标度因数y向加速度计标度因数和z向加速度计标度因数的误差；δα
xz
、δα
xy
、δα
yz
、δα
yx
、δα
zy
和δα
zx
分别为加速度计安装误差角α
xz
、α
xy
、α
yz
、α
yx
、α
zy
和α
zx
的误差；δk
a2
为加速度计的二阶非线性系数的标定误差矩阵，δk
ax2
、δk
ay2
和
δk
az2
分别为x向加速度计的二阶非线性系数k
ax2
的标定误差、y向加速度计的二阶非线性系数k
ay2
的标定误差和z向加速度计的二阶非线性系数k
az2
的标定误差；为加速度计的零偏误差，和分别为x向加速度计、y向加速度计和z向加速度计的零偏误差；
[0140]
加速度计内杆臂误差的表达式为：
[0141]
式中，和分别为内杆臂误差在m系中xm轴、ym轴和zm轴上的分量，分量，其中，和分别为陀螺在m系下的测量值在xm轴、ym轴和zm轴上的分量；δr
x
、δry和δrz分别为x向加速度计内杆臂r
x
、y向加速度计内杆臂ry和z向加速度计内杆臂rz的误差；
[0142]
s3、基于步骤s2确定的双轴旋转惯导的误差方程，构建三十六维卡尔曼滤波器，其具体实施步骤如下：
[0143]
s301、构建三十六维卡尔曼滤波器的状态量x
36
为：
[0144][0145]
式中，为双轴旋转惯导的姿态误差角，δv为速度误差，δv＝[δv
e δv
n δvu]
t
；δp为位置误差，δp＝[δl δλ δh]
t
；xg为陀螺的标定参数误差，xa为加速度计的标定参数误差，x
ka2
为加速度计二阶非线性系数标定误差，x
ka2
＝[δk
ax2 δk
ay2 δk
az2
]
t
；xr为内杆臂误差，xr＝[δr
x δr
y δrz]
t
；
[0146]
s302、构建三十六维卡尔曼滤波器滤波器的状态方程：
[0147][0148]
式中，其中，在f
33
中，每个非零矩阵元素为：
[0149][0150][0151][0152][0153][0154]
[0155][0156]f36
中，
[0157]g36
为测量噪声输入矩阵为，u为测量噪声，其中，ug为陀螺的测量噪声，ua为加速度计的测量噪声；
[0158]
s303、构建三十六维卡尔曼滤波器滤波器的观测方程为：
[0159]
z＝h
36
x
36
+v，
[0160]
式中，z为观测量，以速度为观测量且在标定时双轴旋转惯导保持静止状态，则z＝[v
e v
n vu]
t
；h
36
为观测矩阵，h
36
＝[03×
3 i
3 03×
30
]，i3为三行三列的单位矩阵，即03×
30
为三行三十列的零矩阵，03×3为三行三列的零矩阵；v是观测噪声；
[0161]
s304、确定卡尔曼滤波估计结果的反馈补偿形式：
[0162][0163][0164][0165][0166][0167][0168][0169]
式中，为的反对称矩阵，即为双轴旋转惯导的的解算结果；为双轴旋转惯导的vn的解算结果；为双轴旋转惯导的l的解算结果；为双轴旋转惯导的λ的解算结果；为双轴旋转惯导的h的解算结果；，为卡尔曼滤波估计结
果反馈补偿前的kg；为卡尔曼滤波估计结果反馈补偿前的εm；，为卡尔曼滤波估计结果反馈补偿前的ka；为卡尔曼滤波估计结果反馈补偿前的为卡尔曼滤波估计结果反馈补偿前的k
a2
；为卡尔曼滤波估计结果反馈补偿前的r
x
；为卡尔曼滤波估计结果反馈补偿前的ry；为卡尔曼滤波估计结果反馈补偿前的rz；
[0170]
s4、设计标定试验中双轴转位机构的转位路径并进行标定试验；
[0171]
该步骤s4的具体步骤为：
[0172]
s401、设计标定试验中双轴转位机构的转位路径；
[0173]
双轴旋转惯导系统的双轴转位机构包括有中框轴与内框轴，二者相互垂直并在同一平面内；通过控制中框轴和内框轴的旋转来控制双轴转位机构的转位路径，使得imu坐标系不断变换姿态；设定双轴转位机构的转速为5
°
/s；转位路径如下表1所示，具体为：
[0174]
次序0：双轴转位机构在初始位置上，此时，其中框轴朝东向，内框轴朝天向，对应地，双轴旋转惯导的xm轴朝东向，ym轴朝北向，zm轴朝天向；双轴转位机构初始静止10分钟，使双轴旋转惯导在静止过程中进行初对准，并获得初始的包括姿态角和速度在内的导航解算结果，之后即进入导航解算状态；
[0175]
次序1：以中框轴为旋转轴，顺时针旋转90
°
，旋转后的转位机构姿态记为次序1，对应双轴旋转惯导的具体姿态为：xm轴朝东向，ym轴朝天向，zm轴朝南向；并在旋转后的次序1的位置静止180s；
[0176]
次序2：以中框轴为旋转轴，顺时针旋转180
°
，旋转后的转位机构姿态记为次序2，对应双轴旋转惯导的具体姿态为：xm轴朝东向，ym轴朝地向，zm轴朝北向；并在旋转后的次序2的位置静止180s；
[0177]
次序3：以中框轴为旋转轴，顺时针旋转180
°
，旋转后的转位机构姿态记为次序3，对应双轴旋转惯导的具体姿态为：xm轴朝东向，ym轴朝天向，zm轴朝南向；并在旋转后的次序3的位置静止180s；
[0178]
次序4：以内框轴为旋转轴，顺时针旋转90
°
，旋转后的转位机构姿态记为次序4，对应双轴旋转惯导的具体姿态为：xm轴朝天向，ym轴朝西向，zm轴朝南向；并在旋转后的次序4的位置静止180s；
[0179]
次序5：以内框轴为旋转轴，顺时针旋转180
°
，旋转后的转位机构姿态记为次序5，对应双轴旋转惯导的具体姿态为：xm轴朝地向，ym轴朝东向，zm轴朝南向；并在旋转后的次序5的位置静止180s；
[0180]
次序6：以内框轴为旋转轴，顺时针旋转180
°
，旋转后的转位机构姿态记为次序6，对应双轴旋转惯导的具体姿态为：xm轴朝天向，ym轴朝西向，zm轴朝南向；并在旋转后的次序6的位置静止180s；
[0181]
次序7：以中框轴为旋转轴，顺时针旋转90
°
，旋转后的转位机构姿态记为次序7，对应双轴旋转惯导的具体姿态为：xm轴朝南向，ym轴朝西向，zm轴朝地向；并在旋转后的次序7的位置静止180s；
[0182]
次序8：以中框轴为旋转轴，顺时针旋转180
°
，旋转后的转位机构姿态记为次序8，对应双轴旋转惯导的具体姿态为：xm轴朝北向，ym轴朝西向，zm轴朝天向；并在旋转后的次序8的位置静止180s；
[0183]
次序9：以中框轴为旋转轴，顺时针旋转180
°
，旋转后的转位机构姿态记为次序9，对应双轴旋转惯导的具体姿态为：xm轴朝南向，ym轴朝西向，zm轴朝地向；并在旋转后的次序9的位置静止180s；
[0184]
次序10：以中框轴为旋转轴，顺时针旋转90
°
，旋转后的转位机构姿态记为次序10，对应双轴旋转惯导的具体姿态为：xm轴朝地向，ym轴朝西向，zm轴朝北向；并在旋转后的次序10的位置静止180s；
[0185]
次序11：以中框轴为旋转轴，顺时针旋转90
°
，旋转后的转位机构姿态记为次序11，对应双轴旋转惯导的具体姿态为：xm轴朝北向，ym轴朝西向，zm轴朝天向；并在旋转后的次序11的位置静止180s；
[0186]
次序12：以中框轴为旋转轴，顺时针旋转90
°
，旋转后的转位机构姿态记为次序12，对应双轴旋转惯导的具体姿态为：xm轴朝天向，ym轴朝西向，zm轴朝南向；并在旋转后的次序12的位置静止180s；
[0187]
次序13：以内框轴为旋转轴，顺时针旋转90
°
，旋转后的转位机构姿态记为次序13，对应双轴旋转惯导的具体姿态为：xm轴朝西向，ym轴朝地向，zm轴朝南向；并在旋转后的次序13的位置静止180s；
[0188]
次序14：以内框轴为旋转轴，顺时针旋转90
°
，旋转后的转位机构姿态记为次序14，对应双轴旋转惯导的具体姿态为：xm轴朝地向，ym轴朝东向，zm轴朝南向；并在旋转后的次序14的位置静止180s；
[0189]
次序15：以内框轴为旋转轴，顺时针旋转90
°
，旋转后的转位机构姿态记为次序15，对应双轴旋转惯导的具体姿态为：xm轴朝东向，ym轴朝天向，zm轴朝南向；并在旋转后的次序15的位置静止180s；
[0190]
次序16：以中框轴为旋转轴，顺时针旋转90
°
，旋转后的转位机构姿态记为次序16，对应双轴旋转惯导的具体姿态为：xm轴朝东向，ym轴朝南向，zm轴朝地向；并在旋转后的次序16的位置静止180s；
[0191]
次序17：以中框轴为旋转轴，顺时针旋转90
°
，旋转后的转位机构姿态记为次序17，对应双轴旋转惯导的具体姿态为：xm轴朝东向，ym轴朝地向，zm轴朝北向；并在旋转后的次序17的位置静止180s；
[0192]
次序18：以中框轴为旋转轴，顺时针旋转90
°
，旋转后的转位机构姿态记为次序18，对应双轴旋转惯导的具体姿态为：xm轴朝东向，ym轴朝北向，zm轴朝天向；并在旋转后的次序18的位置静止180s；
[0193]
表1：
[0194][0195]
整个转位过程中共涉及18次转序，且每次转动至指定位置后停止180s，即整个转位路径可以在1小时内完成；
[0196]
在上述转位路径中，前九次转序(次序1～次序9)的目的在于激励陀螺标度因数误差和安装误差，因此共包含了imu的每个轴单方向的两次180
°
旋转；后九次转序(次序10～次序18)的目的在于激励加速度计标度因数和安装误差，因此共包含了imu的每个轴的指天、指地两个位置；与此同时，上述标定路径还可以满足加速度计二阶非线性系数的激励和解耦；由于实际标定中，陀螺的随机噪声比较大，卡尔曼滤波器中陀螺零偏误差的估计通常需要较长时间，因此在一次标定中进行两次的标定路径转位，既可以保证各标定参数误差的估计曲线完全收敛，最终获得卡尔曼滤波器中的状态量估计值；；
[0197]
s402、开机预热至少四个小时，以减小温度变化对标定的影响；
[0198]
s403、完成标定实验，即按照步骤s401设计的标定转位路径控制双轴转位机构转动，并保存由陀螺输出的角速率数据和加速度计输出的比力数据，作为标定数据；
[0199]
s5、利用步骤s3构建的卡尔曼滤波器处理由步骤s4得到标定数据，以获得标定参数，其具体实施过程如下：
[0200]
s501、给定三十六维卡尔曼滤波器中三十六维状态量的初值，即给定的初值；其中，
[0201]
和δv＝[δv
e δv
n δvu]
t
的初值由双轴旋转的初对准结果中的初始姿态角和初始速度直接赋值，具体为：将通过步骤s4在初始状态下转位机构静止10分钟
进行双轴旋转惯导初对准后所得到姿态的初值：和速度的初值：v0＝[v
e0 v
n0 v
u0
]
t
作为和δv的初值，即初始状态下，δv＝v0；
[0202]
δp的初值由步骤s4的双轴旋转惯导完成初对准后输出的初始位置减去实际实验室位置的差值进行赋值，具体为：1)利用gps获得实验室位置为p
t
＝[l
t λ
t h
t
]
t
，其中，l
t
、λ
t
和h
t
分别为实验室的纬度、经度和高度；2)步骤s4的双轴旋转惯导完成初对准后输出的初始位置为p0＝[l
0 λ
0 h0]
t
；3)δp的初值设置为：[l
0-l
t λ
0-λ
t h
0-h
t
]
t
；
[0203]
xg、xa、x
ka2
和xr的初值根据具体双轴旋转惯导所用的陀螺和加速度计的仪器实际情况后进行赋值(即通过仪器类型、精度水平以及机械结构设计特点进行人为考量后确定初值)；
[0204]
s502、基于步骤s501确定的三十六维状态量初值作为卡尔曼滤波的状态量的初值，将步骤s301构建的三十六维状态量带入步骤s302构建的三十六维卡尔曼滤波器状态方程和步骤s303构建的三十六维卡尔曼滤波器观测方程，利用卡尔曼滤波算法和步骤s3构建的卡尔曼滤波器，处理标定数据，使卡尔曼滤波器中的状态量从初值开始随步骤s4的多次转位而得到不断迭代和反馈补偿，反馈补偿采用s304确定的估计结果的反馈补偿补偿方式，最终获得卡尔曼滤波器中的状态量估计值；
[0205]
其中，在状态量估计值中，关于惯性器件误差的状态量分量即为经过步骤s5获得准确的标定参数，包括：x向陀螺标度因数y向陀螺标度因数和z向陀螺标度因数陀螺安装误差角γ
yz
、γ
zy
和γ
zx
；x向陀螺零偏y向陀螺零偏和z向陀螺零偏x向加速度计标度因数y向加速度计标度因数和z向加速度计标度因数的误差；加速度计安装误差角α
xz
、α
xy
、α
yz
、α
yx
、α
zy
和α
zx
；x向加速度计的二阶非线性系数k
ax2
、y向加速度计的二阶非线性系数k
ay2
和z向加速度计的二阶非线性系数k
az2
；x向加速度计零偏y向加速度计零偏和z向加速度计零偏x向加速度计内杆臂r
x
、y向加速度计内杆臂ry和z向加速度计内杆臂rz；
[0206]
s6、将标定参数代入标定模型中，以补偿双轴旋转惯导的输出；其具体实施步骤为：
[0207]
s601、采用常用的转位机构旋转方案(例如：六十四次序旋转调制方案)作为双轴旋转惯导导航过程中的转位机构旋转方案，并在每转动至一个位置后停止10s时间，获得旋转调制后的惯性器件输出结果，包括陀螺输出的角速率和加速度计输出的比力
[0208]
其中，在该步骤s603中，六十四次序旋转调制方案是一种针对双轴旋转惯导使用过程中的公知的转位方案，即安装在运载体上进行导航时候的转位方案，因此在本实施例中不作详细说明；
[0209]
s602、将由步骤s5获得的标定参数分别代入至陀螺标定模型和加速度计标定模型中，以获得陀螺标定后的输出和加速度计标定后的输出；其中，
[0210]
(1)陀螺的标定模型为：
[0211]
式中，
[0212]
(2)加速度计的标定模型为：
[0213]
式中，
[0214]
s603、利用经过步骤s602得到的陀螺标定后的输出和加速度计标定后的输出，利用x向加速度计内杆臂r
x
、y向加速度计内杆臂ry和z向加速度计内杆臂rz对加速度数据进行实时补偿，其具体公式为：
[0215][0216][0217][0218]
式中，a
xb
、a
yb
和a
zb
为加速度计内杆臂补偿后的加速度实时结果在m系中xm轴、ym轴和zm轴的分量；a
xc
、a
yc
和a
zc
为加速度计内杆臂补偿前的加速度实时结果fm在m系中xm轴、ym轴和zm轴的分量；ω
xc
、ω
yc
和ω
zc
为角速率在m系中xm轴、ym轴和zm轴的分量；
[0219]
s604、将步骤s603中的a
xb
、a
yb
、a
zb
和ω
xc
、ω
yc
和ω
zc
带入惯导解算方程，获得双轴旋转惯导的导航结果；
[0220]
经过步骤s604得到的导航结果，即为采用本技术实现的对双轴旋转惯导动态误差进行实时补偿后的导航结果。
[0221]
为验证本发明提出的双轴旋转惯导动态误差抑制方法的正确性和准确性，选用一套双轴旋转惯导进行了标定实验，选用的双轴旋转惯导内的imu由三个零偏稳定性为0.01
°
/h的激光陀螺仪和三个零偏稳定性为10μg的加速度计组成；选用的双轴旋转惯导的转位机构的姿态控制精度为5
″
(1σ)。
[0222]
首先，采用本发明提供的方法进行误差参数的标定，x
36
中xg、xa、x
ka2
和xr的初值根据所选用的双轴旋转惯导所使用的陀螺、加速度计类型和精度水平以及机械结构设计确定为：x向陀螺标度因数y向陀螺标度因数和z向陀螺标度因数的初值均为-0.84
″
/pulse；陀螺安装误差角γ
yz
、γ
zy
和γ
zx
的初值均为10
′
；x向陀螺零偏y向陀螺零偏和z向陀螺零偏的初值均为0.01
°
/h；x向加速度计标度因数y向加速度计标度因数和z向加速度计标度因数的初值均为450μm/s/pulse；加速度计安装误差角α
xz
、α
xy
、α
yz
、α
yx
、α
zy
和α
zx
的初值均为10
′
；x向加速度计的二阶非线性系数k
ax2
、y向加速度计的二阶非线性系数k
ay2
和z向加速度计的二阶非线性系数k
az2
的初值均为20μg/g2；x向加速度计零偏y向加速度计零偏和z向加速度计零偏的初值均为1000μg；x向加速度计内杆
臂r
x
、y向加速度计内杆臂ry和z向加速度计内杆臂rz的初值均为3cm；
[0223]
根据设置的初值和本发明提供的方法获得的标定参数如下：
[0224]
表2：
[0225][0226]
在获得标定参数后，将选用的双轴惯导安装在试验车上进行车载动态实验，实验包括10分钟静态初始对准和10分钟导航解算，首先在试验车进行情况下进行10分钟静态初始对准，在对准完成后进入导航状态，试验车开始以15m/s的速度直线开动模拟动态环境，并在双轴旋转惯导进入导航状态约4分钟后，试验车180
°
转弯，在转弯后继续以15m/s的速度直线开动；在导航状态约8分钟后，试验车再次180
°
转弯，两次旋转方向相同。
[0227]
分别采用未考虑加速度计二次非线性参数、加速度计内杆臂误差的标定方法和本发明提出的方法的两组参数进行对同一组实验数据进行导航解算，将导航解算结果与安装于试验车上的gps输出的速度和位置进行对比。
[0228]
如图3(a)所示为双轴旋转惯导在进行动态误差补偿前、后的东向速度误差对比的示意图；如图3(b)所示为双轴旋转惯导在进行动态误差补偿前、后的北向速度误差对比的示意图。从上述两幅图中对比可以看出，在未补偿加速度计二次非线性参数、加速度计内杆臂误差的情况下，双轴旋转惯导180
°
转向将导致在东向和北向产生约0.025m/s的速度误差，而采用本发明提出的方法进行标定补偿后，由动态引起的速度误差明显减少，效果可达90％；
[0229]
如图4(a)所示为双轴旋转惯导在进行动态误差补偿前、后的东向位置误差对比的示意图；如图4(b)所示为双轴旋转惯导在进行动态误差补偿前、后的北向位置误差对比的示意图。从上述两幅图中对比可以看出，对比未补偿加速度计二次非线性参数、加速度计内杆臂误差和本发明得到的导航结果的位置误差可见，在短航时的导航环境中，通过本发明提供的方法可以提高动态环境下双轴旋转惯导的定位精度约8米。
[0230]
综上所述，上述实验证明了本发明提供的双轴旋转惯导动态误差抑制方法的正确性和准确性，具有能够能很好地抑制双轴旋转惯导动态误差的效果，有很好的实用性。
[0231]
本发明未详细公开的部分属于本领域的公知技术。尽管上面对本发明说明性的具
体实施方式进行了描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化时显而易见的，一切利用本发明构思的发明创造均为保护之列。

技术特征：
1.一种双轴旋转惯导动态误差抑制方法，其特征在于，步骤如下：s1、构建陀螺敏感轴系、加速度计敏感轴系、imu坐标系、地心惯性坐标系、地球坐标系和导航坐标系，并确定陀螺的安装误差矩阵和加速度计的安装误差矩阵；s2、基于imu的零偏、标度因素、安装误差、加速度计二阶非线性系数和加速度计内杆臂引发的误差，确定双轴旋转惯导在导航坐标系下的误差方程；s3、基于双轴旋转惯导的误差方程，构建三十六维卡尔曼滤波器，包括三十六维卡尔曼滤波器的状态量、三十六维卡尔曼滤波器滤波器的状态方程、三十六维卡尔曼滤波器滤波器的观测方程和卡尔曼滤波估计结果的反馈补偿形式；其中，三十六维卡尔曼滤波器的状态量包含双轴旋转惯导的姿态误差角速度误差δv，位置误差δp，陀螺的标定参数误差x
g
，加速度计的标定参数误差x
a
，加速度计二阶非线性系数标定误差x
ka2
和内杆臂误差x
r
；s4、标定试验：开机预热完成后，双轴转位机构根据设计的转位路径进行转动，保存该过程中由双轴旋转惯导的陀螺输出的角速率数据和加速度计输出的比力数据，作为标定数据；其中，转位路径应设计应满足：双轴旋转惯导惯性测量单元的每个轴均经过单方向的两次180
°
旋转，以及惯性测量单元的每个轴均历经指天、指地两个位置；s5、利用卡尔曼滤波算法和步骤s3构建的卡尔曼滤波器对步骤s4的标定数据进行处理，使卡尔曼滤波器中的状态量从初值开始随步骤s4的多次转位而得到不断迭代和反馈补偿，最终获得卡尔曼滤波器中的状态量估计值；其中，三十六维状态量的初值基于双轴旋转的初对准结果和仪器参数进行赋值；在最优的卡尔曼滤波器中的状态量估计值中，关于惯性器件误差的状态量分量为标定参数；s6、将标定参数代入陀螺的标定模型和加速度计的标定模型中，得到标定后输出的角速率和比力，并基于标定后的角速率和比力得到经过加速度计内杆臂补偿的加速度，进而解算得到双轴旋转惯导经动态误差实时补偿后的导航结果。2.根据权利要求1所述的双轴旋转惯导动态误差抑制方法，其特征在于，步骤s1的具体实施步骤为：s101、构建陀螺敏感轴系，即g系，该坐标系以三个陀螺的测量中心点为坐标原点，xg轴与x向陀螺的敏感轴方向一致，yg轴与y向陀螺的敏感轴方向一致，zg轴与z向陀螺的敏感轴方向一致；s102、构建加速度计敏感轴系，即a系，该坐标系以g系的坐标原点为坐标原点，xa轴与x向加速度计的敏感轴方向一致，ya轴与y向加速度计的敏感轴方向一致，za轴与z向加速度计的敏感轴方向一致；s103、构建imu坐标系，即m系，该坐标系以g系的坐标原点为坐标原点，xm轴与陀螺敏感轴xg轴一致，ym轴在陀螺敏感轴xg轴与yg轴构成的平面中，且与yg轴相差小角度γ
yz
，zm轴为陀螺敏感轴zg轴绕xm轴旋转小角度γ
zx
，再绕ym轴旋转小角度γ
zy
；s104、构建地心惯性坐标系，即i系，该坐标系以地球中心为坐标原点，xi和yi轴在地球赤道平面内，且xi轴指向春分点，zi轴为地球自转轴，并指向北极；s105、构建地球坐标系，即e系，该坐标系以地球中心为坐标原点，xe和ye轴在地球赤道平面内，且xe指向本初子午线，ze轴为地球自转轴，并指向北极；s106、构建导航坐标系，即n系，该坐标系为东北天地理坐标系，其坐标原点为imu坐标系的坐标原点，xn轴指向东，yn轴指向北，zn轴指向天；
s107、基于陀螺敏感轴系和imu坐标系，陀螺的安装误差矩阵的表达式为：式中，为陀螺的安装误差矩阵，γ
yz
、γ
zy
和γ
zx
为陀螺敏感轴系相对于imu坐标系的三个安装误差角，具体地，γ
yz
表示y向陀螺敏感轴yg轴绕m系的zm轴的偏转角度，γ
zy
表示z向陀螺敏感轴zg轴绕m系的ym轴的偏转角度，γ
zx
表示z向陀螺敏感轴zg轴绕m系的xm轴的偏转角度；s108、加速度计的安装误差矩阵由六个加速度计安装误差角构成，其表达式为：式中，为加速度计的安装误差矩阵，α
xz
、α
xy
、α
yz
、α
yx
、α
zy
和α
zx
为加速度计敏感轴系相对于imu坐标系的六个安装误差角，具体地，α
xz
表示x向加速度计敏感轴xa轴绕m系的zm轴的偏转角度，α
xy
表示x向加速度计敏感轴xa轴绕m系的ym轴的偏转角度，α
yz
表示y向加速度计敏感轴ya轴绕m系的zm轴的偏转角度，α
yx
表示y向加速度计敏感轴ya轴绕m系的xm轴的偏转角度，α
zy
表示z向加速度计敏感轴za轴绕m系的ym轴的偏转角度，α
zx
表示z向加速度计敏感轴za轴绕m系的xm轴的偏转角度。3.根据权利要求2所述的双轴旋转惯导动态误差抑制方法，其特征在于，步骤s2的具体实施步骤为：双轴旋转惯导在导航坐标系下的误差方程，其表达式为：式中，为相对于时间的导数，为双轴旋转惯导的姿态误差角，为双轴旋转惯导的姿态误差角，和分别为双轴旋转惯导的东向误差角、北向误差角和天向误差角；为n系相对于i系的转动角速率，其由地球自转与载体运动产生；为导航解算中的估计误差；为m系到n系的坐标转换矩阵；为陀螺的测量误差；为δv
n
相对于时间的导数，δv
n
为双轴旋转惯导的速度误差，δv
n
＝[δv
e δv
n δv
u
]
t
，δv
e
、δv
n
和δv
u
分别为东向速度误差、北向速度误差和天向速度误差；f
n
为n系下的比力；为地球自转角速率；为载体绕地球运动产生的角速率；；v
n
为双轴旋转惯导的速度，v
n
＝[v
e v
n v
u
]
t
，v
e
、v
n
和v
u
分别为双轴旋转惯导
的东向速度、北向速度和天向速度；为的误差；为的误差；δf
m
为加速度计的测量误差；为加速度计内杆臂误差；δg为重力矢量误差；为δl相对于时间的导数，δl为纬度误差；为δλ相对于时间的导数，δλ为经度误差；为δh相对于时间的导数，δh为高度误差；l、λ和h分别为当地地理纬度、经度和高度；r
m
和r
n
分别为当地地球子午圈和卯酉圈半径；其中，陀螺的测量误差的表达式为：式中，为陀螺在m系下的测量值；δk
g
为陀螺的标度因数和安装误差的误差矩阵，为陀螺的标度因数和安装误差的误差矩阵，和分别为x向陀螺标度因数y向陀螺标度因数和z向陀螺标度因数的误差；δγ
yz
、δγ
zy
和δγ
zx
分别为陀螺安装误差角γ
yz
、γ
zy
和γ
zx
的误差；ε
m
为陀螺的零偏误差，其表达式为：ε
m
＝[ε
x ε
y ε
z
]
t
，式中，ε
x
、ε
y
和ε
z
分别为x向陀螺、y向陀螺和z向陀螺的零偏误差；加速度计的测量误差δf
m
的表达式为：式中，为加速度计在m系下的测量值；δk
a
为加速度计的标度因数和安装误差的误差矩阵，矩阵，和分别为x向加速度计标度因数y向加速度计标度因数和z向加速度计标度因数的误差；δα
xz
、δα
xy
、δα
yz
、δα
yx
、δα
zy
和δα
zx
分别为加速度计安装误差角α
xz
、α
xy
、α
yz
、α
yx
、α
zy
和α
zx
的误差；δk
a2
为加速度计的二阶非线性系数的标定误差矩阵，δk
ax2
、δk
ay2
和δk
az2
分别为x向加速度计的二阶非线性系数k
ax2
的标定误差、y向加速度计的二阶非线性系数k
ay2
的标定误差和z向加速度计的二阶非线性系数k
az2
的标定误差；为加速度计的零偏误差，为加速度计的零偏误差，和分别为x向加速度计、y向加速度计和z向加速度计的零偏误差；加速度计内杆臂误差的表达式为：
式中，和分别为内杆臂误差在m系中x
m
轴、y
m
轴和z
m
轴上的分量，轴上的分量，其中，和分别为陀螺在m系下的测量值在x
m
轴、y
m
轴和z
m
轴上的分量；δr
x
、δr
y
和δr
z
分别为x向加速度计内杆臂r
x
、y向加速度计内杆臂r
y
和z向加速度计内杆臂r
z
的误差。4.根据权利要求3所述的双轴旋转惯导动态误差抑制方法，其特征在于，步骤s3的具体实施步骤如下：s301、构建三十六维卡尔曼滤波器的状态量x
36
：式中，式中，为双轴旋转惯导的姿态误差角，δv为速度误差，δv＝[δv
e δv
n δv
u
]
t
；δp为位置误差，δp＝[δl δλ δh]
t
；x
g
为陀螺的标定参数误差，x
a
为加速度计的标定参数误差，x
ka2
为加速度计二阶非线性系数标定误差，x
ka2
＝[δk
ax2 δk
ay2 δk
az2
]
t
；x
r
为内杆臂误差，x
r
＝[δr
x δr
y δr
z
]
t
；s302、构建三十六维卡尔曼滤波器滤波器的状态方程：式中，在f
33
中，每个非零矩阵元素为：
f
36
中，g
36
为测量噪声输入矩阵为，u为测量噪声，其中，u
g
为陀螺的测量噪声，u
a
为加速度计的测量噪声；s303、构建三十六维卡尔曼滤波器滤波器的观测方程：z＝h
36
x
36
+v，式中，z为观测量，以速度为观测量且在标定时双轴旋转惯导保持静止状态，则z＝[v
e v
n v
u
]
t
；h
36
为观测矩阵，h
36
＝[03×
3 i
3 03×
30
]，i3为三行三列的单位矩阵，即03×
30
为三行三十列的零矩阵，03×3为三行三列的零矩阵；v是观测噪声；s304、确定卡尔曼滤波估计结果的反馈补偿形式：式中，为的反对称矩阵，为双轴旋转惯导的的解算结果；为双轴旋转惯导的v
n
的解算结果；为双轴旋转惯导的l的解算结果；为双轴旋转惯导的λ的解算结果；为双轴旋转惯导的h的解算结果；，为卡尔曼滤波估计结果反馈补偿前的k
g
；为卡尔曼滤波估计结果反馈补偿前的ε
m
；，为卡尔曼滤波估计结果反馈补偿前的k
a
；为卡尔曼滤波估计结果反馈补偿前的波估计结果反馈补偿前的为卡尔曼滤波估计结果反馈补偿前的k
a2
；为卡尔曼滤波估计结果反馈补偿前的r
x
；为卡尔曼滤波估计结果反馈补偿前的r
y
；为卡尔曼滤波估计结果反馈补偿前的r
z
。5.根据权利要求1所述的双轴旋转惯导动态误差抑制方法，其特征在于，在步骤s4中，双轴转位机构的转位路径具体设计为：基于次序0对应初始位置：双轴旋转惯导以其xm轴朝东向，ym轴朝北向，zm轴朝天向的姿态，静止10分钟；次序1：以中框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝东向，ym
轴朝天向，zm轴朝南向；转位完成后，静止180s；次序2：以中框轴为旋转轴，顺时针旋转180
°
；该状态下，双轴旋转惯导的xm轴朝东向，ym轴朝地向，zm轴朝北向；转位完成后，静止180s；次序3：以中框轴为旋转轴，顺时针旋转180
°
；该状态下，双轴旋转惯导的xm轴朝东向，ym轴朝天向，zm轴朝南向；转位完成后，静止180s；次序4：以内框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝天向，ym轴朝西向，zm轴朝南向；转位完成后，静止180s；次序5：以内框轴为旋转轴，顺时针旋转180
°
；该状态下，双轴旋转惯导的xm轴朝地向，ym轴朝东向，zm轴朝南向；转位完成后，静止180s；次序6：以内框轴为旋转轴，顺时针旋转180
°
；该状态下，双轴旋转惯导的xm轴朝天向，ym轴朝西向，zm轴朝南向；转位完成后，静止180s；次序7：以中框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝南向，ym轴朝西向，zm轴朝地向；转位完成后，静止180s；次序8：以中框轴为旋转轴，顺时针旋转180
°
；该状态下，双轴旋转惯导的xm轴朝北向，ym轴朝西向，zm轴朝天向；转位完成后，静止180s；次序9：以中框轴为旋转轴，顺时针旋转180
°
；该状态下，双轴旋转惯导的xm轴朝南向，ym轴朝西向，zm轴朝地向；转位完成后，静止180s；次序10：以中框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝地向，ym轴朝西向，zm轴朝北向；转位完成后，静止180s；次序11：以中框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝北向，ym轴朝西向，zm轴朝天向；转位完成后，静止180s；次序12：以中框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝天向，ym轴朝西向，zm轴朝南向；转位完成后，静止180s；次序13：以内框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝西向，ym轴朝地向，zm轴朝南向；转位完成后，静止180s；次序14：以内框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝地向，ym轴朝东向，zm轴朝南向；转位完成后，静止180s；次序15：以内框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝东向，ym轴朝天向，zm轴朝南向；转位完成后，静止180s；次序16：以中框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝东向，ym轴朝南向，zm轴朝地向；转位完成后，静止180s；次序17：以中框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝东向，ym轴朝地向，zm轴朝北向；转位完成后，静止180s；次序18：以中框轴为旋转轴，顺时针旋转90
°
；该状态下，双轴旋转惯导的xm轴朝东向，ym轴朝北向，zm轴朝天向；转位完成后，静止180s。6.根据权利要求5所述的双轴旋转惯导动态误差抑制方法，其特征在于，在步骤s4中，一次标定实验中按照设计的转位路径进行两次双轴转位机构的转位。7.根据权利要求3所述的双轴旋转惯导动态误差抑制方法，其特征在于，步骤s5的具体实施步骤为：
s501、对三十六维状态量进行初值赋值：和δv的初值分别由步骤s4中双轴旋转的初对准结果中的初始姿态角和初始速度进行赋值；δp的初值由步骤s4中双轴旋转惯导完成初对准后输出的初始位置减去实际实验位置得到的差值进行赋值；x
g
、x
a
、x
ka2
和x
r
的初值根据双轴旋转惯导中陀螺和加速度计的仪器实际情况进行赋值；s502、基于步骤s501确定的三十六维状态量初值作为卡尔曼滤波的状态量的初值，将步骤s301构建的三十六维状态量分别带入步骤s302构建的三十六维卡尔曼滤波器状态方程和步骤s303构建的三十六维卡尔曼滤波器观测方程中，采用卡尔曼滤波算法和步骤s3构建的卡尔曼滤波器处理标定数据，通过对卡尔曼滤波器中的状态量从初值开始不断迭代和反馈补偿，获得卡尔曼滤波器中的状态量估计值；其中，状态量估计值中关于惯性器件误差的状态量分量即准确的标定参数，包括：x向陀螺标度因数y向陀螺标度因数和z向陀螺标度因数陀螺安装误差角γ
yz
、γ
zy
和γ
zx
；x向陀螺零偏y向陀螺零偏和z向陀螺零偏x向加速度计标度因数y向加速度计标度因数和z向加速度计标度因数的误差；加速度计安装误差角α
xz
、α
xy
、α
yz
、α
yx
、α
zy
和α
zx
；x向加速度计的二阶非线性系数k
ax2
、y向加速度计的二阶非线性系数k
ay2
和z向加速度计的二阶非线性系数k
az2
；x向加速度计零偏y向加速度计零偏和z向加速度计零偏x向加速度计内杆臂r
x
、y向加度计内杆臂r
y
和z向加速度计内杆臂r
z
。8.根据权利要求7所述的双轴旋转惯导动态误差抑制方法，其特征在于，步骤s6的具体实施步骤为：s601、采用现有的六十四次序旋转调制方案作为双轴旋转惯导导航过程中的转位机构旋转方案，并在每转动至一个位置后停止10s时间，获得旋转调制后的惯性器件输出结果，包括陀螺输出的角速率和加速度计输出的比力s602、将由步骤s5获得的标定参数分别代入至陀螺标定模型和加速度计标定模型中，以获得陀螺标定后的输出和加速度计标定后的输出；其中，(1)陀螺的标定模型为：式中，(2)加速度计的标定模型为：式中，s603、利用经过步骤s602得到的陀螺标定后的输出和加速度计标定后的输出，利用x向加速度计内杆臂r
x
、y向加速度计内杆臂r
y
和z向加速度计内杆臂r
z
对加速度数据进行实时补偿，其具体公式为：
式中，a
xb
、a
yb
和a
zb
为加速度计内杆臂补偿后的加速度实时结果在m系中xm轴、ym轴和zm轴的分量；a
xc
、a
yc
和a
zc
为加速度计内杆臂补偿前的加速度实时结果在m系中xm轴、ym轴和zm轴的分量；ω
xc
、ω
yc
和ω
zc
为角速率在m系中xm轴、ym轴和zm轴的分量；s604、将步骤s603中的a
xb
、a
yb
、a
zb
和ω
xc
、ω
yc
、ω
zc
带入惯导解算方程，经解算得到的双轴旋转惯导的导航结果，即为对双轴旋转惯导动态误差进行实时补偿后的导航结果。

技术总结
本发明公开了一种双轴旋转惯导动态误差抑制方法，步骤为：S1、构建坐标系，确定陀螺和加速度计的安装误差矩阵；S2、确定双轴旋转惯导在导航坐标系下的误差方程；S3、构建三十六维卡尔曼滤波器；S4、双轴转位机构根据设计的转位路径进行转动，将该过程中的角速率数据和比力数据作为标定数据；S5、利用卡尔曼滤波算法和卡尔曼滤波器处理标定数据，以得到卡尔曼滤波器中的状态量估计值；S6、利用标定参数得到标定后输出的角速率和比力、以及经过加速度计内杆臂补偿的加速度，进而解算得到导航结果；该方法全面考虑零偏、标度因素、安装误差、加速度计二阶非线性系数和加速度计内杆臂导致的双轴旋转惯导动态误差，应用简便、精度高，实用性佳。实用性佳。实用性佳。


技术研发人员：涂勇强 蔡庆中 杨功流 李晶 尹洪亮
受保护的技术使用者：北京航空航天大学
技术研发日：2022.07.15
技术公布日：2022/11/1