一种陆用双轴旋转惯导的零速修正方法

1.本发明涉及陆用双轴旋转惯导导航误差抑制领域，特别涉及一种陆用双轴旋转惯导的零速修正方法。


背景技术：

2.陆用双轴旋转惯导由惯性测量单元(initial measurement unit，简称为imu)和转位机构组成，工作原理为：将imu安装在转位机构上，使imu在导航过程中可以相对固定的坐标系轴旋转，由于imu通过特定的旋转，将惯性器件的常值误差调制成为均值为零的周期变化量，可以在现有的惯性器件精度水平上，大大的提高陆用双轴旋转惯导长航时的导航精度。因此，陆用双轴旋转惯导被广泛地应用在需要高精度惯性导航系统的陆用运载体中，例如：自行榴弹炮、炮兵观察车、测地车、侦察车、机动导弹发射架等陆用运载体中。
3.零速修正是指利用陆用车辆停车时载体的实际速度为零作为观测信息，对惯导的导航误差进行修正。零速修正是一种简便而有效的实时误差抑制手段，在同等硬件条件下，定位精度高于纯惯导的导航精度。目前，现有的陆用惯导零速修正方法集中于捷联惯导，例如：杨鹏翔等设计的激光陀螺捷联惯导系统零速修正方案将捷联惯导的定位精度提高到44.2m(cep)，冯庆奇等提出一种基于二次多项式拟合和卡尔曼滤波的零速修正方法应用于捷联惯导中，使其定位误差小于30m。然而，陆用双轴旋转惯导在机械结构上相比捷联惯导多了转位机构，合理的设计零速修正时转位机构的旋转方案能较好的提高零速修正精度；但是，目前还未有成熟的针对陆用双轴旋转惯导的零速修正方法。


技术实现要素：

4.本发明的目的是提供一种使用简便、高精度的陆用双轴旋转惯导的零速修正方法。
5.为此，本发明技术方案如下：
6.一种陆用双轴旋转惯导的零速修正方法，步骤如下：
7.s1、设计卡尔曼滤波器；
8.s101、定义卡尔曼滤波器的状态量为一个十五维的向量，即x
15
，相应表达式为：
[0009][0010]
式中，x
15
为所构建的十五维卡尔曼滤波器状态量；为陆用双轴旋转惯导的姿态误差，误差，和分别为陆用双轴旋转惯导的东向误差角、北向误差角和天向误差角；δv为陆用双轴旋转惯导的速度误差，δv＝[δv
e δv
n δvu]
t
，δve、δvn和δvu分别为陆用双轴旋转惯导的东向速度误差、北向速度误差和天向速度误差；δp为陆用双轴旋转惯导的位置误差，δp＝[δl δλ δh]
t
，δl、δλ和δh分别为陆用双轴旋转惯导的纬度误差、经度误差和高度误差；ε为陆用双轴旋转惯导imu中的陀螺零偏误差，ε＝[ε
x ε
y εz]
t
，ε
x
、εy和εz分别为陆用双轴旋转惯导imu中的x向陀螺零偏误差、y向陀螺零偏误差和z向陀螺零偏误
差；为陆用双轴旋转惯导imu中的加速度计零偏误差，为陆用双轴旋转惯导imu中的加速度计零偏误差，和分别为陆用双轴旋转惯导imu中的x向加速度计零偏误差、y向加速度计零偏误差和z向加速度计零偏误差；
[0011]
s102、建立卡尔曼滤波器的状态方程：
[0012][0013]
式中，在f
15
中，f
11
、f
12
、f
13
、f
21
、f
22
、f
13
、f
32
、f
33
为非零矩阵元素，其具体表达式如下：
[0014][0015][0016][0017]
[0018][0019][0020]
其中，l为当地地理纬度，λ为当地地理经度，h为当地地理高度；ve、vn和vu分别为陆用双轴旋转惯导的东向速度、北向速度和天向速度；rm和rn分别为当地地球子午圈和卯酉圈半径；ω
ie
为地球自转角速率；和分别为惯性测量单元测量的东向比力、北向比力和天向比力；为惯性测量单元坐标系到导航坐标系的转换矩阵；
[0021]g15
为测量噪声输入矩阵，其表达式为：
[0022]
u为测量噪声，其表达式为：其中，ug为陀螺的测量噪声，ug＝[u
gx u
gy u
gz
]
t
，u
gx
为x向陀螺的测量噪声，u
gy
为y向陀螺的测量噪声，u
gz
为z向陀螺的测量噪声；ua为加速度计的测量噪声，ua＝[u
ax u
ay u
az
]
t
，u
ax
为x向加速度计的测量噪声、u
ay
为y向加速度计的测量噪声，u
az
为z向加速度计的测量噪声；
[0023]
s103、建立卡尔曼滤波器的观测方程：
[0024]z15
＝h
15
x
15
+v，
[0025]
式中，z
15
＝[01×
3vt 01×9]
t
为正向导航零速修正的观测向量，v＝[v
e v
n vu]
t
为正向导航零速修正的速度向量；v为观测噪声；h
15
为观测矩阵，其表达式为：h
15
＝[03×
3 i
3 03×9]，其中，i3为三行三列的单位矩阵，即03×3为三行三列的零矩阵，03×9为三行九列的零矩阵；
[0026]
s2、设计对准阶段的转位方案，包括：1)转位机构基于天向轴朝天，某一水平轴朝车头方向的初始位置进行粗对准，和2)转位机构基于初始位置进行精对准，且精对准阶段中途转位机构的天向轴以10
°
/s的转动速度正向旋转180
°
；
[0027]
s3、设计导航阶段的转位方案：陆用双轴旋转惯导的转位机构保持天向轴朝天，某一水平轴始终指向车头的方式，采用180
°
转停多次的方式进行转位；
[0028]
s4、设计零速修正方案：
[0029]
s401、利用惯性测量单元的陀螺仪和加速度计分别实时获得角速度信息和加速度信息，并基于惯性测量单元上导航计算机板上内嵌的惯性导航解算方程，计算出惯性导航结果；
[0030]
s402、利用卡尔曼滤波器对陆用双轴旋转惯导的导航误差和惯性器件误差中的陀螺零偏误差和加速度计零偏误差进行实时预测，包括：
[0031]
状态1：在陆用运载体开车前的静止过程中，开启陆用双轴旋转惯导并实施步骤s2的对准阶段的转位方案，同时利用步骤s1设计的卡尔曼滤波器实时解算得到陆用双轴旋转惯导对准阶段下的卡尔曼滤波器的状态量；
[0032]
状态2：在陆用运载体行驶的过程中，陆用双轴旋转惯导实施步骤s3的导航阶段的转位方案，同时利用步骤s1设计的卡尔曼滤波器实时解算得到陆用运载体行驶中陆用双轴旋转惯导导航阶段下的卡尔曼滤波器的状态量；
[0033]
状态3：在陆用运载体开车后的暂停行驶的过程中，陆用双轴旋转惯导实施步骤s3的导航阶段的转位方案，同时利用步骤s1设计的卡尔曼滤波器实时解算得到陆用运载体开车后暂停行驶中陆用双轴旋转惯导导航阶段下的卡尔曼滤波器的状态量；
[0034]
s403、将不同阶段下获得的卡尔曼滤波器的状态量用于导航误差的补偿；
[0035]
补偿1：将经过步骤s402的状态1得到的卡尔曼滤波器的状态量中的导航误差补偿至步骤s401实时输出的惯性导航结果中，其惯性器件误差中的陀螺零偏误差和加速度计零偏误差则以写入惯性测量单元上导航计算机板上内嵌的惯性导航解算方程中的方式进行补偿；
[0036]
补偿2：将经过步骤s402的状态3得到的卡尔曼滤波器的状态量中的导航误差补偿至步骤s401实时输出的惯性导航结果中，其惯性器件误差中的陀螺零偏误差和加速度计零偏误差则以写入惯性测量单元上导航计算机板上内嵌的惯性导航解算方程中的方式进行补偿。
[0037]
进一步地，在步骤s3中，设计导航阶段的转位方案为：
[0038]
设定转位机构的天向轴朝天，某一水平轴朝车头方向时为初始位置a，并记转位机构的天向轴为z
t
；基于此，一个转位周期的转位方案为：
[0039]
1)第一次转位：转位机构在初始位置a停止30s，然后以10
°
/s的均速转速绕z
t
正向旋转180
°
到达b位置，整个转动时间为18s；
[0040]
2)第二次转位：转位机构在b位置停止12s，然后以10
°
/s的均速转速绕z
t
正向旋转180
°
回到a位置，整个转动时间为18s；
[0041]
3)第三次转位：转位机构在初始位置a停止12s，然后以10
°
/s的均速转速绕z
t
反向旋转180
°
到达b位置，整个转动时间为18s；
[0042]
4)第四次转位：转位机构在b位置停止12s，然后以10
°
/s的均速转速绕z
t
反向旋转180
°
回到a位置，整个转动时间为18s。
[0043]
进一步地，在步骤s2中，对准阶段的转位方案为：
[0044]
s201、粗对准阶段：陆用双轴旋转惯导开机，陆用双轴旋转惯导的转位机构保持在初始位置进行至少3分钟的粗对准；其中，初始位置为：转位机构的天向轴朝天，某一水平轴朝车头方向；
[0045]
s202、精对准阶段：粗对准阶段完成后，陆用双轴旋转惯导先进行精对准至少10分钟，而后陆用双轴旋转惯导的转位机构的天向轴以10
°
/s的转动速度正向旋转180
°
，陆用双轴旋转惯导再进行至少10分钟的精对准。
[0046]
与现有技术相比，该陆用双轴旋转惯导的零速修正方法发挥陆用双轴旋转惯导中转位机构的优势，转位机构的旋转方案设计合理，零速修正方法简便、精度高，具有很好的实用价值；经试验验证，采用本技术的零速修正后，陆用双轴旋转惯导输出的速度精度提高55％，位置精度提高81％；证明了本技术提供的陆用双轴旋转惯导零速修正方法的正确性和准确性，能很好地抑制陆用双轴旋转惯导的速度和位置误差，有很好的实用性。
附图说明
[0047]
图1为本发明的陆用双轴旋转惯导的零速修正方法的流程图；
[0048]
图2为本发明的步骤s3中设计的导航阶段转位方案中一个转位周期的转位示意图；
[0049]
图3为本发明的步骤s4中设计的零速修正方案的原理示意图；
[0050]
图4(a)为本发明的实施例中陆用双轴旋转惯导在进行零速修正前的速度误差示意图；
[0051]
图4(b)为本发明的实施例中陆用双轴旋转惯导在进行零速修正后的速度误差示意图；
[0052]
图5(a)为本发明的实施例中陆用双轴旋转惯导在进行零速修正前的位置误差示意图；
[0053]
图5(b)为本发明的实施例中陆用双轴旋转惯导在进行零速修正后的位置误差示意图。
具体实施方式
[0054]
下面结合附图及具体实施例对本发明做进一步的说明，但下述实施例绝非对本发明有任何限制。
[0055]
如图1所示，该陆用双轴旋转惯导的零速修正方法的具体实施步骤如下：
[0056]
s1、设计卡尔曼滤波器；
[0057]
具体地，该步骤s1的实施步骤为：
[0058]
s101、定义卡尔曼滤波器的状态量：
[0059]
基于陆用双轴旋转惯导的导航误差包括位置误差、速度误差和姿态误差，惯性器件误差中的陀螺零偏误差和加速度计零偏误差是影响陆用双轴旋转惯导导航精度最重要的误差，因此，构建以导航误差和惯性器件误差中的陀螺零偏误差和加速度计零偏误差为集合的向量作为卡尔曼滤波器的状态量；采用上述方式构建的卡尔曼滤波器的状态量包括了本身的测量误差(即陆用双轴旋转惯导的导航误差)以及造成测量误差的误差来源(即惯性器件误差)，因此，基于该卡尔曼滤波器的状态量才能实现同时获得导航误差(即输出误差)和惯性器件误差(即造成输出误差的误差源)；
[0060]
定义卡尔曼滤波器的状态量为一个十五维的向量，即x
15
，相应表达式为：
[0061][0062]
式中，x
15
为所构建的十五维卡尔曼滤波器状态量；为陆用双轴旋转惯导的姿态误差，误差，和分别为陆用双轴旋转惯导的东向误差角、北向误差角和天向误差角；δv为陆用双轴旋转惯导的速度误差，δv＝[δveδvnδvu]
t
，δve、δvn和δvu分别为陆用双轴旋转惯导的东向速度误差、北向速度误差和天向速度误差；δp为陆用双轴旋转惯导的位置误差，δp＝[δlδλδh]
t
，δl、δλ和δh分别为陆用双轴旋转惯导的纬度误差、经度误差和高度误差；ε为陆用双轴旋转惯导imu中的陀螺零偏误差，ε＝[ε
x
εyεz]
t
，ε
x
、εy和εz分别为陆用双轴旋转惯导imu中的x向陀螺零偏误差、y向陀螺零偏误差和z向陀螺零偏误差；为陆用双轴旋转惯导imu中的加速度计零偏误差，为陆用双轴旋转惯导imu中的加速度计零偏误差，和分别为陆用双轴旋转惯导imu中的x向加速度计零偏误差、y向加速度计零偏误差和z向加速度计零偏误差；
[0063]
s102、建立卡尔曼滤波器的状态方程：
[0064]
根据步骤s101构建的十五维卡尔曼滤波器状态量、以及已知的惯导误差方程，建立卡尔曼滤波器的状态方程为：
[0065][0066]
式中，在f
15
中，f
11
、f
12
、f
13
、f
21
、f
22
、f
13
、f
32
、f
33
为非零矩阵元素，其具体表达式如下：
[0067][0068]
[0069][0070][0071][0072][0073]
其中，l为当地地理纬度，λ为当地地理经度，h为当地地理高度；ve、vn和vu分别为陆用双轴旋转惯导的东向速度、北向速度和天向速度；rm和rn分别为当地地球子午圈和卯酉圈半径；ω
ie
为地球自转角速率；和分别为惯性测量单元测量的东向比力、北向比力和天向比力；为惯性测量单元坐标系到导航坐标系的转换矩阵；
[0074]g15
为测量噪声输入矩阵，其表达式为：
[0075]
u为测量噪声，其表达式为：其中，ug为陀螺的测量噪声，ug＝[u
gx u
gy u
gz
]
t
，u
gx
为x向陀螺的测量噪声，u
gy
为y向陀螺的测量噪声，u
gz
为z向陀螺的测量噪声；ua为加速度计的测量噪声，ua＝[u
ax u
ay u
az
]
t
，u
ax
为x向加速度计的测量噪声、u
ay
为y向加速度计的测量噪声，u
az
为z向加速度计的测量噪声；
[0076]
s103、建立卡尔曼滤波器的观测方程：
[0077]
滤波器观测方程为：
[0078]z15
＝h
15
x
15
+v，
[0079]
式中，z
15
＝[01×
3vt 01×9]
t
为正向导航零速修正的观测向量，v＝[v
e v
n vu]
t
为正向导航零速修正的速度向量；v为观测噪声；h
15
为观测矩阵，由于选用陆用双轴旋转惯导的速度解算结果作为观测量，因此，h
15
的表达式为：
[0080]h15
＝[03×3i
303
×9]，
[0081]
式中，i3为三行三列的单位矩阵，即03×3为三行三列的零矩阵，03×9为三行九列的零矩阵。
[0082]
s2、设计对准阶段的转位方案；
[0083]
将陆用双轴旋转惯导安装于陆用运载体上，陆用运载体在开车前的静止状态下，首先将陆用双轴旋转惯导开机，并在开机后采用两位置对准的转位方案进行陆用双轴旋转惯导的对准，使陆用双轴旋转惯导在陆用运载体静止的状态下经历对准阶段，包括粗对准阶段和精对准阶段；具体地，在该阶段过程中，陆用双轴旋转惯导至少需要经历至少3分钟的粗对准阶段和至少20分钟的精对准阶段，且在精对准至少10分钟后，陆用双轴旋转惯导的转位机构的天向轴以10
°
/s的转动速度正向旋转180
°
，再进行至少10分钟的精对准；整个对准阶段时长为至少23分钟；
[0084]
在本实施例中，对准阶段的转位方案为：
[0085]
1)粗对准阶段：陆用双轴旋转惯导开机，陆用双轴旋转惯导的转位机构保持初始位置(天向轴朝天，某一水平轴朝车头方向)，在初始位置，陆用双轴旋转惯导进行3分钟的初对准；
[0086]
2)精对准阶段：完成粗对准后，陆用双轴旋转惯导进行10分钟的精对准，然后转位机构的天向轴以10
°
/s的转动速度正向旋转180
°
，而后陆用双轴旋转惯导继续进行10分钟的精对准，精对准完成；
[0087]
在粗对准和精对准完成后，陆用双轴旋转惯导即可进入导航阶段；
[0088]
s3、设计导航阶段的转位方案；
[0089]
陆用运载体在运行过程中，会出现短暂停驶的情况(如车辆加油、人员原地休息等车辆中途停车的情况)，而该情况下由于陆用运载体的实际速度为零，因此为适于零速修正的时机；然而，由于每次零速修正间隔通常很短，通常在10分钟左右，因此每组转位方案的时间必须也要在较短的时间内完成；另外，在导航阶段，陆用运载体机动的特点是仅在陆用运载体的前进方向有加速、减速运动；因此，导航阶段的转位方案采用180
°
转停的方式，即保持某个水平轴始终指向车头，以尽可能的调制此轴加速度计标度因数误差的影响且不激励另一个水平轴加速度计标度因数误差，同时，保证在一个转位周期内正、反两个方向的旋转角度相同；
[0090]
如图2所示，在本实施例中，设计导航阶段的转位方案如下：
[0091]
设定转位机构的某个水平轴指向车头时为初始位置a(天向轴朝天，某一水平轴朝车头方向)，并记转位机构的天向轴为z
t
；基于此，一个转位周期的转位方案：
[0092]
1)第一次转位：转位机构在初始位置a停止30s，然后以10
°
/s的均速转速绕z
t
正向旋转180
°
到达b位置，整个转动时间为18s；
[0093]
2)第二次转位：转位机构在b位置停止12s，然后以10
°
/s的均速转速绕z
t
正向旋转
180
°
回到a位置，整个转动时间为18s；
[0094]
3)第三次转位：转位机构在初始位置a停止12s，然后以10
°
/s的均速转速绕z
t
反向旋转180
°
到达b位置，整个转动时间为18s；
[0095]
4)第四次转位：转位机构在b位置停止12s，然后以10
°
/s的均速转速绕z
t
反向旋转180
°
回到a位置，整个转动时间为18s；
[0096]
如上所述，一次完整转位调制共包括四次转位动作，每次完整一次转位的转位周期为120s，则在10分钟零速修正停车间隔内共进行5个转位周期的转位；
[0097]
基于上述技术方案，还可以通过适当增加上述各次转位的停止时间或转动时间，以对该转位方案进行调整；
[0098]
s4、设计零速修正方案；
[0099]
如图3所示，由于陆用运载体机动导致惯导的导航误差传播规律复杂，导致传统曲线拟合法零速修正难以实现较高精度；因此，本技术的零速修正方案基于卡尔曼滤波的原理设计而成；具体步骤为：
[0100]
s401、利用惯性测量单元的陀螺仪和加速度计分别实时获得角速度信息和加速度信息，并基于惯性测量单元上导航计算机板上内嵌的惯性导航解算方程，计算出惯性导航结果；
[0101]
s402、利用卡尔曼滤波器对陆用双轴旋转惯导的导航误差和惯性器件误差中的陀螺零偏误差和加速度计零偏误差进行实时预测，包括：
[0102]
状态1：在陆用运载体开车前的静止过程中，开启陆用双轴旋转惯导并实施步骤s2的对准阶段的转位方案，同时利用步骤s1设计的卡尔曼滤波器实时解算得到陆用双轴旋转惯导对准阶段下的卡尔曼滤波器的状态量；
[0103]
状态2：在陆用运载体行驶的过程中，陆用双轴旋转惯导实施步骤s3的导航阶段的转位方案，同时利用步骤s1设计的卡尔曼滤波器实时解算得到陆用运载体行驶中陆用双轴旋转惯导导航阶段下的卡尔曼滤波器的状态量；
[0104]
状态3：在陆用运载体开车后的暂停行驶的过程中，陆用双轴旋转惯导实施步骤s3的导航阶段的转位方案，同时利用步骤s1设计的卡尔曼滤波器实时解算得到陆用运载体开车后暂停行驶中陆用双轴旋转惯导导航阶段下的卡尔曼滤波器的状态量；
[0105]
s403、将不同阶段下获得的卡尔曼滤波器的状态量用于导航误差的补偿；
[0106]
补偿1：将经过步骤s402的状态1得到的卡尔曼滤波器的状态量中的导航误差补偿至步骤s401实时输出的惯性导航结果中，其惯性器件误差中的陀螺零偏误差和加速度计零偏误差则以写入惯性测量单元上导航计算机板上内嵌的惯性导航解算方程中的方式进行补偿；
[0107]
补偿2：将经过步骤s402的状态3得到的卡尔曼滤波器的状态量中的导航误差补偿至步骤s401实时输出的惯性导航结果中，其惯性器件误差中的陀螺零偏误差和加速度计零偏误差则以写入惯性测量单元上导航计算机板上内嵌的惯性导航解算方程中的方式进行补偿；
[0108]
其中，卡尔曼滤波器状态量的导航误差直接补导航误差是直接补到陆用双轴旋转惯导的导航输出结果，又称为即时补偿；而卡尔曼滤波器状态量的惯性器件误差中的陀螺零偏误差和加速度计零偏误差则是写入惯性测量单元上导航计算机板上内嵌的惯性导航
解算方程，在陆用运载体重新回到行驶状态下，陆用双轴旋转惯导的导航结果由于是采用补偿了惯性器件误差中的陀螺零偏误差和加速度计零偏误差后的惯性导航解算方程计算的，因此也得到了补偿，该种补偿方式又称为长久补偿；而经过步骤s402的状态2得到的卡尔曼滤波器的状态量中的导航误差和惯性器件误差中的陀螺零偏误差和加速度计零偏误差不进行补偿使用。
[0109]
为验证本发明提出的陆用双轴旋转惯导零速修正方法的正确性和准确性，选用一套陆用双轴旋转惯导进行了车载试验；其中，选用的陆用双轴旋转惯导内的惯性测量单元由三个零偏稳定性为0.01
°
/h的激光陀螺仪和三个零偏稳定性为10μg的加速度计组成；选用的陆用双轴旋转惯导的转位机构的姿态控制精度为5
″
(1σ)；在试验车上安装选用的陆用双轴旋转惯导以及一套gps。
[0110]
车载试验的具体实施方案设计为：首先，对准阶段停车23分钟，陆用双轴旋转惯导开机并按本发明提出的方法控制转位；然后，对准阶段停车23分钟后，开车让试验车随意跑动1小时，在随意跑动过程中随机停车10分钟进行导航阶段的零速修正；采集试验过程的陆用双轴旋转惯导的陀螺和加速度计输出，分别按未零速修正和本发明提出的零速修正方法计算得到速度和位置解算结果，并以安装于试验车上的gps提供的速度和位置作为参照得到按未零速修正和本发明提出的零速修正方法计算得到速度和位置解算结果的误差。
[0111]
如图4(a)所示为该车载试验中陆用双轴旋转惯导在进行零速修正前的速度误差示意图；如图4(b)所示为该车载试验中陆用双轴旋转惯导在进行零速修正后的速度误差示意图；对比附图4(a)和附图4(b)，经过零速修正后最大速度误差从0.09m/s减小到0.04m/s，速度精度提高55％；如图5(a)所示为该车载试验中陆用双轴旋转惯导在进行零速修正前的位置误差示意图；如图5(b)所示为该车载试验中陆用双轴旋转惯导在进行零速修正后的位置误差示意图；对比附图5(a)和附图5(b)，经过零速修正后最大位置误差从70m减小到13m，位置精度提高81％；综上，上述车载试验证明了本发明提供的陆用双轴旋转惯导零速修正方法的正确性和准确性，能很好地抑制陆用双轴旋转惯导的速度和位置误差，有很好的实用性。
[0112]
本发明未详细公开的部分属于本领域的公知技术。尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化时显而易见的，一切利用本发明构思的发明创造均为保护之列。