一种大方位失准角下多普勒雷达辅助捷联惯导运动基座粗对准方法

1.本发明属于导航技术领域，具体地涉及一种大方位失准角下多普勒雷达辅助捷联惯导运动基座粗对准方法。


背景技术：

2.初始对准是捷联惯导系统在进行导航解算之前首先需要完成的工作。对准精度和对准时间是捷联惯导系统进行初始对准的两项重要指标，其中对准精度直接影响捷联惯导系统的导航精度，对准时间则决定了捷联惯导系统的应急机动能力。特别是对于现代战争中战场形势瞬息万变，倘若能够实现武器系统中的捷联惯导系统在车载运动基座下即可快速完成较高精度的自主粗对准，那么对于提高武器系统的快速反应与应急机动能力、战场生存能力将具有十分重要的意义。
3.捷联惯导系统在车载运动基座下的自主粗对准问题是惯性导航技术领域近些年来亟待解决的难点问题，其难点主要体现在运动基座下捷联惯导系统自主粗对准时面临着大方位失准角问题，而传统的小失准角模型中建立的线性化假设已失效。
4.但是，现有的大方位失准角下捷联惯导运动基座粗对准技术主要有两种：一种是构建非线性误差方程采用非线性滤波的形式，采用的非线性滤波主要有扩展卡尔曼滤波(ekf)、无迹卡尔曼滤波(ukf)及粒子滤波(pf)等，另一种是基于优化的对准(oba)方法。其中ekf需要计算jacobian矩阵，对于高维复杂系统，存在算法复杂，计算量大的缺点；ukf的性能取决于对噪声的描述，实际应用过程中，准确的噪声分布难以描述，将可能得到性能下降甚至滤波发散的结果；对于高维系统，pf的计算量巨大，目前还难以在嵌入式导航计算机中应用。而传统oba方法没有考虑imu的偏差，这些偏差对矢量观测的精度有重大影响，并不适用于低成本捷联惯导系统。
5.为此，本发明创新性地提出利用大方位失准角的正余弦三角函数代替大方位失准角本身，建立了一种大方位失准角的线性化模型，并利用自主性强、抗干扰性好的车载多普勒测速雷达辅助捷联惯导系统实现大方位失准角下运动基座粗对准。该方法机动性和自主性非常强，抗干扰性和隐蔽性很好，成本较低、工程易于实现，极大地提高了战场环境下武器系统的快速反应能力、应急机动能力以及战场生存能力。


技术实现要素：

6.针对上述存在的在问题，本发明利用大方位失准角的正余弦三角函数代替大方位失准角本身，建立了一种大方位失准角的线性化模型，并将大方位失准角的正余弦三角函数列入粗对准滤波的系统状态，利用多普勒测速雷达与捷联惯导系统的输出构造量测，采用卡尔曼滤波进行粗对准滤波计算，总体计算量小，算法稳定性高，滤波收敛速度快，而且无需外部特殊机动辅助。
7.为了实现上述目的，本发明所采用的技术方案如下：
8.一种大方位失准角下多普勒雷达辅助捷联惯导运动基座粗对准方法，包括：
9.步骤1：定义陀螺常值漂移和加速度计常值误差，建立理想导航坐标系n到实际导航坐标系n
′
的坐标变换矩阵，在此基础上建立大方位失准角条件下捷联惯导的数学平台姿态误差方程和速度误差方程；
10.步骤2：基于步骤1所建立的捷联惯导数学平台姿态误差方程和速度误差方程，选取捷联惯导运动基座粗对准的系统状态，建立大方位失准角下捷联惯导运动基座粗对准的状态方程，并获取状态矩阵；
11.步骤3：构造多普勒测速雷达辅助捷联惯导粗对准的量测，并将该量测和步骤1的坐标变换矩阵结合，得到大方位失准角下捷联惯导运动基座粗对准的量测方程和量测矩阵；
12.步骤4：基于步骤2和3中所建立的捷联惯导运动基座粗对准的状态方程与量测方程，利用tk时刻的量测zk通过卡尔曼滤波就可以实现大方位失准角下捷联惯导系统运动基座粗对准滤波解算，进而获得捷联惯导失准角等状态的最优估计结果此时根据该估计结果来校正捷联惯导的数学平台姿态误差，实现捷联惯导系统的粗对准。
13.优选的，所述步骤1包括：
14.步骤1.1：定义陀螺随机常值漂移和加速度计随机常值误差，建立理想导航坐标系n到实际导航坐标系n
′
的坐标变换矩阵；
15.步骤1.2：结合步骤1.1的坐标变换矩阵，建立大方位失准角条件下捷联惯导的数学平台姿态误差方程；
16.步骤1.3：结合步骤1.1的坐标变换矩阵，建立大方位失准角条件下捷联惯导的速度误差方程。
17.优选的，所述步骤1.1包括：
18.步骤1.1.1：捷联惯导的核心部件是陀螺仪和加速度计，首先研究陀螺仪和加速度计的漂移误差，定义陀螺随机常值漂移和加速度计随机常值误差，记载体坐标系下陀螺随机常值漂移矢量为εb，其三个分量分别为ε
bx
、ε
by
和ε
bz
，加速度计随机常值误差矢量为其三个分量分别为三个分量分别为
19.步骤1.1.2：用三次转动表示理想导航坐标系n与实际导航坐标系n
′
之间的误差，三次转动姿态误差角大小分别为φe、φn和φu，其中φe和φn为小角度，φu为大角度，建立三次转动姿态误差角的坐标变换矩阵；
20.步骤1.1.3：建立理想导航坐标系n到实际导航坐标系n
′
的坐标变换矩阵。
21.优选的，所述步骤1.2包括：
22.步骤1.2.1：实际导航坐标系n
′
相对于惯性坐标系i的旋转角速率为等效为理想导航坐标系n相对于惯性坐标系i的旋转角速率减去导航坐标系下陀螺随机常值漂移矢量εn，得到公式；
[0023][0024]
其中，εn＝[ε
e ε
n εu]
t
，其中，εe,εn,εu分别表示导航坐标系下陀螺随机常值漂移的东向、北向、天向分量；
[0025]
步骤1.2.2：将失准角的变化率表示为实际导航坐标系n
′
相对于理想导航坐标系n的旋转角速率
[0026][0027]
步骤1.2.3：将步骤1.2.1的分解：
[0028][0029]
步骤1.2.4：将步骤1.2.1的公式带入步骤1.2.3的分解公式中，表示为：
[0030][0031]
其中，i表示一个3
×
3的单位矩阵；
[0032]
步骤1.2.5：将步骤1.1.3的坐标变换矩阵代入步骤1.2.4的公式中，展开和整理得到大方位失准角条件下捷联惯导的实际数学平台姿态误差方程：
[0033][0034][0035]
其中，分别为旋转角速率的东向、北向、天向三个分量，为φe的导数，为φn的导数，分别为旋转角速率的东向、北向分量，在小角度条件下
[0036]
优选的，所述步骤1.3包括：
[0037]
步骤1.3.1：根据比力方程，理想速度值方程公式为：
[0038][0039]
其中，n系中速度矢量的导数，b系到n系坐标变换阵，fb：加速度计输出，e系相对于i系旋转角速度在n系下投影，n系相对于e系旋转角速度在n系下投影，vn：n系中速度矢量，gn：重力加速度在n系中投影，各个参数均为理想值；
[0040]
实际速度值计算公式为：
[0041][0042]
其中，该公式中的参数与理想速度值方程公式中的参数定义相同，该公式中参数为实际值；
[0043]
其中，和之间的误差为和fb之间的误差为和vn之间的误差为δvn；
[0044]
步骤1.3.2：将步骤1.3.1的代入到步骤1.3.1的实际速度值计算公式后再减去理想速度值，得到：
[0045]
[0046]
其中，为理想导航系到实际导航系的坐标变换阵，fn为n系下的比力输出；
[0047]
步骤1.3.3：将步骤1.1.3的坐标变换矩阵代入步骤1.3.2的公式中，整理后得到大方位失准角条件下捷联惯导的速度误差方程：
[0048][0049][0050][0051]
其中，公式中参数带下标e，n，u为其在东向、北向、天向三个方向上的投影。
[0052]
优选的，所述步骤2具体为：
[0053]
步骤2.1：基于步骤1所建立的捷联惯导数学平台姿态误差方程和速度误差方程，选取捷联惯导运动基座粗对准的系统状态向量x为：
[0054][0055]
ε
bx
,ε
by
,ε
bz
表示εb在x，y，z轴上的投影，表示在x，y，z轴上的投影；
[0056]
步骤2.2：在步骤2.1的基础上，建立大方位失准角下捷联惯导运动基座粗对准的状态方程：
[0057][0058]
其中，f为状态矩阵；g为噪声驱动矩阵；w为系统噪声。
[0059]
优选的，所述步骤3具体为：
[0060]
步骤3.1：设多普勒测速雷达输出的载体运动速度在载体坐标系下投影形式为vb；捷联惯导系统输出的载体运动速度在实际导航坐标系下投影形式为载体姿态矩阵为则构造出多普勒测速雷达辅助捷联惯导对准的量测z为：
[0061][0062]
步骤3.2：将步骤1.3.1的代入步骤3.1的z公式中，得到z变形公式：
[0063][0064]
步骤3.3：将步骤1.1.3的坐标变换矩阵代入步骤3.2的z变形公式中，整理得到粗对准量测方程：
[0065]
z＝hx+v
[0066]
其中，量测矩阵h＝[h
1 h
2 o3×6]，
[0067]
[0068]
其中，h1和h2为简化表示需要，无特殊含义，h1中参数ve，vn，vu为vn在e、n、u三个方向上的投影。
[0069]
与现有技术相比，本发明的有益效果是：
[0070]
1.本发明采用多普勒测速雷达辅助捷联惯导进行大方位失准角下运动基座粗对准，相较于常用的gps/北斗辅助对准，本发明无需接收外界信号，自主性和机动性非常强、抗干扰性和隐蔽性很好，成本较低、工程易于实现，可以适用于现代战场复杂恶劣环境，能够极大地提高战场环境下武器系统的快速反应能力、应急机动能力以及战场生存能力；
[0071]
2.本发明创新性地提出利用大方位失准角的正余弦三角函数代替大方位失准角本身，建立了一种大方位失准角的线性化模型，采用线性的卡尔曼滤波即可实现捷联惯导系统在大方位失准角下运动基座粗对准，有效地避免了非线性滤波所带来的算法复杂、计算量大等问题。
附图说明
[0072]
附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。
[0073]
在附图中：
[0074]
图1为本发明的方法流程图；
[0075]
图2为本发明大方位失准角下捷联惯导系统粗对准滤波算法原理图；
[0076]
图3为本发明实施例仿真验证实验的载体(车辆)运动轨迹图；
[0077]
图4为本发明实施例仿真验证实验的捷联惯导系统在大方位失准角下进行粗对准400s后的失准角图。
具体实施方式
[0078]
以下结合附图对本发明的优选实施例进行说明，应当理解，此处所描述的优选实施例仅用于说明和解释本发明，并不用于限定本发明。
[0079]
实施例：
[0080]
参照附图1-4所示，一种大方位失准角下多普勒雷达辅助捷联惯导运动基座粗对准方法，包括：
[0081]
步骤1：捷联惯导的核心部件是陀螺仪和加速度计，通常将它们统称为惯性器件，惯性器件误差是捷联惯导最基本的误差，其对捷联惯导导航解算误差产生直接影响，是捷联惯导一切误差的来源。惯性器件误差具体包括安装误差、刻度系数误差和漂移误差等。其中，安装误差、刻度系数误差以及漂移误差中的确定性部分经过实验标定可以得到有效补偿。本发明只研究陀螺和加速度计的随机误差，定义陀螺随机常值漂移和加速度计随机常值误差，建立理想导航坐标系n到实际导航坐标系n
′
的坐标变换矩阵，在此基础上建立大方位失准角条件下捷联惯导的数学平台姿态误差方程和速度误差方程。包括：
[0082]
步骤1.1：定义陀螺随机常值漂移和加速度计随机常值误差，建立理想导航坐标系n到实际导航坐标系n
′
的坐标变换矩阵。具体包括：
[0083]
步骤1.1.1：捷联惯导的核心部件是陀螺仪和加速度计，首先研究陀螺仪和加速度计的漂移误差，定义陀螺随机常值漂移和加速度计随机常值误差，记载体坐标系下陀螺随
机常值漂移矢量为εb，其三个分量分别为ε
bx
、ε
by
和ε
bz
，加速度计随机常值误差矢量为其三个分量分别为三个分量分别为
[0084]
步骤1.1.2：设载体坐标系为右前上坐标系，即坐标系的三个轴分别沿着载体的横轴向右、纵轴向前以及航向轴向上；导航坐标系为东北天地理坐标系，即坐标系的三个轴分别指向地理东向、地理北向以及沿着垂线指向天向。由于各种误差源的影响，用三次转动姿态误差表示理想导航坐标系n与实际导航坐标系n
′
之间的误差，三次转动姿态误差角大小分别为φe、φn和φu，其中φe和φn为小角度，φu为大角度，记φ＝[φ
e φ
n φu]
t
，建立三次转动姿态误差角的坐标变换矩阵：
[0085][0086][0087][0088]
分别是三次转动所对应的坐标变换矩阵；为理想导航坐标系n相对于实际导航坐标系n
′
的坐标变换矩阵。
[0089]
步骤1.1.3：在步骤1.1.2的基础上，建立理想导航坐标系n到实际导航坐标系n
′
的坐标变换矩阵：
[0090][0091]
步骤1.2：结合步骤1.1的坐标变换矩阵，建立大方位失准角条件下捷联惯导的数学平台姿态误差方程；
[0092]
步骤1.2.1：实际导航坐标系n
′
相对于惯性坐标系i的旋转角速率为等效为理想导航坐标系n相对于惯性坐标系i的旋转角速率减去导航坐标系下陀螺随机常值漂移矢量εn，得到公式；
[0093][0094]
其中，εn＝[ε
e ε
n εu]
t
，其中，εe,εn,εu分别表示陀螺漂移的东向、北向、天向分量；
[0095]
步骤1.2.2：将失准角的变化率表示为实际导航坐标系n
′
相对于理想导航坐标系n得旋转角速率
[0096]
[0097]
步骤1.2.3：将步骤1.2.1的分解：
[0098][0099]
步骤1.2.4：将步骤1.2.1式(5)的公式带入步骤1.2.3式(7)的分解公式中，表示为：
[0100][0101]
其中，i表示一个3
×
3的单位矩阵；
[0102]
步骤1.2.5：将步骤1.1.3式(4)的坐标变换矩阵代入步骤1.2.4式(8)的公式中，得到：
[0103][0104]
展开和整理得到大方位失准角条件下捷联惯导的实际数学平台姿态误差方程：
[0105][0106][0107]
其中，分别为旋转角速率的东向、北向、天向三个分量，为φe的导数，为φn的导数，分别为旋转角速率的东向、北向分量，在小角度条件下
[0108]
由于卡尔曼滤波周期短，载体在短时间内姿态角变化量很小，故本发明假设每一个滤波周期中φu保持不变。基于上述假设，sinφu和cosφu在各个滤波周期内将为定值，得到：
[0109][0110]
步骤1.3：结合步骤1.1的坐标变换矩阵，建立大方位失准角条件下捷联惯导的速度误差方程。
[0111]
步骤1.3.1：根据比力方程，理想速度值方程公式为：
[0112][0113]
其中，n系中速度矢量的导数，b系到n系坐标变换阵，fb：加速度计输出，e系相对于i系旋转角速度在n系下投影，n系相对于e系旋转角速度在n系下投影，vn：n系中速度矢量，gn：重力加速度在n系中投影，各个参数均为理想值；
[0114]
实际速度值计算公式为：
[0115][0116]
其中，该公式中的参数与理想速度值方程公式中的参数定义相同，该公式中参数
为实际值；其中，
[0117][0118][0119][0120]
和之间的误差为和fb之间的误差为和vn之间的误差为δvn，剩余的和gn的误差量很小，作为小量忽略。
[0121]
步骤1.3.2：将步骤1.3.1式(15)、式(16)和式(17)的步骤1.3.2：将步骤1.3.1式(15)、式(16)和式(17)的代入到步骤1.3.1式(13)的实际速度值计算公式后再减去理想速度值，得到：
[0122][0123]
其中，为理想导航系到实际导航系的坐标变换阵，fn为n系下的比力输出；
[0124]
步骤1.3.3：将步骤1.1.3式(4)的坐标变换矩阵代入步骤1.3.2式(18)的公式中，展开后得到：
[0125][0126]
将式(19)整理后得到大方位失准角条件下捷联惯导的速度误差方程：
[0127][0128][0129][0130]
其中，公式中参数带下标e，n，u为其在东向、北向、天向三个方向上的投影。
[0131]
的微分方程中存在非线性项，做以下假设：水平失准角与方位失准角正弦三角函数的乘积为小量，与方位失准角余弦三角函数乘积为其本身。因此δvu的微分方程可近似为：
[0132][0133]
步骤2：基于步骤1所建立的捷联惯导数学平台姿态误差方程和速度误差方程，选取捷联惯导运动基座粗对准的系统状态，建立大方位失准角下捷联惯导运动基座粗对准的状态方程，并获取状态矩阵；具体为：
[0134]
步骤2.1：步骤1已经对大方位失准角下捷联惯导的数学平台姿态误差方程和速度误差方程进行了推导，并以此作为获取运动基座粗对准的状态矩阵的依据。
[0135]
基于步骤1所建立的捷联惯导数学平台姿态误差方程和速度误差方程，选取捷联惯导运动基座粗对准的系统状态向量x为：
[0136][0137]
ε
bx
,ε
by
,ε
bz
表示εb在x，y，z轴上的投影，表示在x，y，z轴上的投影；
[0138]
步骤2.2：在步骤2.1的基础上，建立大方位失准角下捷联惯导运动基座粗对准的状态方程：
[0139][0140]
其中，f为状态矩阵；g为噪声驱动矩阵；w为系统噪声，w＝[w
gx
,w
gy
,0,0,w
ax
,w
ay
,w
az
]
t
，这里w
gx
,w
gy
分别为沿载体x,y轴上陀螺仪的白噪声，w
ax
,w
ay
,w
az
分别为沿载体x,y,z轴上加速度计的白噪声，即e[w(t)]＝0且e[w(t)w
t
(τ)]＝qδ(t-τ)，q为w的方差强度阵。状态矩阵f表示为：
[0141][0142]
其中，记：
[0143]
[0144][0145]
其中，ω
ie
为地球自转角速度，l为纬度，h为海拔高度。
[0146]
步骤3：多普勒测速雷达通常沿着载体的轴向进行安装。由于多普勒测速雷达输出的速度是载体运动速度在载体坐标系下的投影形式，而捷联惯导系统输出的速度则是载体运动速度在导航坐标系下的投影形式，为了利用上述两个系统输出的速度构造辅助对准的量测，需要利用捷联惯导系统输出的载体姿态矩阵，将多普勒测速雷达的速度输出由载体坐标系转换到导航坐标系下。
[0147]
构造多普勒测速雷达辅助捷联惯导粗对准的量测，并将该量测和步骤1的坐标变换矩阵结合，得到大方位失准角下捷联惯导运动基座粗对准的量测方程和量测矩阵；具体为：
[0148]
步骤3.1：设多普勒测速雷达输出的载体运动速度在载体坐标系下投影形式为vb；捷联惯导系统输出的载体运动速度在实际导航坐标系下投影形式为载体姿态矩阵为则构造出多普勒测速雷达辅助捷联惯导对准的量测z为：
[0149][0150]
步骤3.2：将步骤1.3.1式(17)的代入步骤3.1式(28)的z公式中，得到式(28)的z变形公式：
[0151][0152]
步骤3.3：将步骤1.1.3式(4)的坐标变换矩阵代入步骤3.2式(29)的z变形公式中，做同(22)的简化得到：
[0153][0154]
进一步的整理得到粗对准量测方程：
[0155]
z＝hx+v
ꢀꢀꢀꢀꢀꢀꢀꢀ
(31)
[0156]
量测矩阵h＝[h
1 h
2 o3×6]，其中，
[0157][0158]
其中，h1和h2为简化表示需要，无特殊含义，h1中参数ve，vn，vu为vn在e、n、u三个方向上的投影。
[0159]
步骤4：基于步骤2和3中所建立的捷联惯导运动基座粗对准的状态方程与量测方程，利用tk时刻的量测zk通过卡尔曼滤波就可以实现大方位失准角下捷联惯导系统运动基座粗对准滤波解算，进而获得惯导失准角等状态的最优估计结果此时根据该估计结果来校正捷联惯导的数学平台姿态误差，实现捷联惯导系统的粗对准。
[0160]
仿真结果与分析
[0161]
(1)假设捷联惯导系统中陀螺仪的随机常值漂移、加速度计的随机常值误差分别为0.02
°
/h，10-4
g，其白噪声随机游走分别为g，其白噪声随机游走分别为捷联惯导水平姿态角误差为2
°
，方位角误差为60
°
；辅助粗对准仿真时间为400s，载体(车辆)始终处于运动状态，运动轨迹如图3所示。
[0162]
(2)捷联惯导系统在大方位失准角下进行粗对准400s后的失准角如图4所示，不难发现，通过进行粗对准使捷联惯导系统数学平台的三个姿态误差角取得了有效的收敛，经过粗对准后其估计误差分别控制在40
′
，5
′
，5
′
之内。因此，仿真结果表明多普勒测速雷达测得的载体速度信息为捷联惯导运动基座粗对准发挥了关键作用，而且效果显著。
[0163]
以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。