一种利用北斗测定惯性导航系统与测速仪安装夹角的方法与流程

技术特征：

1.一种利用北斗测定惯性导航系统与测速仪安装夹角的方法，其特征在于，将惯性导航系统安装在载车车厢内，北斗接收机安装在车顶，测速仪通过软轴与驱动轮轴承连接，惯性导航系统完成初始对准后开始运动，其横坐标为纬度，纵坐标为经度；惯性导航系统包含的3个陀螺仪的随机漂移均为0.01°/h，常值漂移均为0.02°/h；惯性导航系统包含的3个加速度计随机漂移均为50μg，常值漂移均为100μg，测速仪初始刻度因子为0.01米/脉冲，包括步骤：

步骤一、建立惯性导航系统动态误差模型，状态变量包括位置误差、速度误差、失准角和惯性器件漂移误差：

在东北天坐标系下，惯性导航系统动态误差模型为：

式中，t为时间值，是正实数；x_INS(t)表示惯性导航系统动态误差模型的状态向量，由位置误差δP、速度误差δVⁿ、失准角陀螺仪零偏误差δε_g和加速度计零偏误差组成；w_INS(t)表示惯性导航系统动态误差模型的系统噪声；F_INS(t)为惯性导航系统动态误差转移矩阵；表示惯性导航系统动态误差模型状态向量的导数；

对惯性导航系统动态误差模型进行离散化处理，得到：

x_INS(k)＝F_INS(k，k-1)x_INS(k-1)+w_INS(k)

式中，k表示时刻，为正整数，k-1时刻至k时刻的时间间隔记为ΔT_Dis，表示误差模型离散化周期；F_INS(k，k-1)表示k-1时刻至k时刻的惯性导航系统动态误差转移矩阵；x_INS(k)为离散化后的k时刻惯性导航系统的状态向量；w_INS(k)表示k时刻的惯性导航系统动态误差模型的系统噪声，是均值为零，方差为Q_INS的白噪声序列，Q_INS为非负矩阵；

步骤二、建立测速仪动态误差模型，该模型包含的状态变量为安装夹角误差和和测速仪刻度因子误差，其中安装夹角误差包括航向安装夹角误差与俯仰安装夹角误差，

测速仪动态误差模型为：${\stackrel{\·}{x}}_{\mathrm{VMS}}\left(t\right)=F_{\mathrm{VMS}}\left(t\right)x_{\mathrm{VMS}}\left(t\right)+w_{\mathrm{VMS}}\left(t\right)$

式中，x_VMS(t)＝[δα δβ δτ]^T，x_VMS(t)表示测速仪动态误差模型的状态向量，由航向安装夹角误差δα、俯仰安装夹角误差δβ和测速仪刻度因子误差δτ组成；w_VMS(t)表示测速仪动态误差模型的系统噪声；F_VMS(t)为测速仪动态误差转移矩阵；表示测速仪动态误差模型状态向量的导数；

对测速仪动态误差模型进行离散化处理，得到：

x_VMS(k)＝F_VMS(k，k-1)x_VMS(k-1)+w_VMS(k)

式中，F_VMS(k，k-1)表示k-1时刻至k时刻的测速仪动态误差转移矩阵；x_VMS(k)为离散化后的k时刻测速仪动态误差模型的状态向量；w_VMS(k)表示k时刻的测速仪动态误差模型的系统噪声，是均值为零，方差为Q_VMS的白噪声序列，Q_VMS为非负矩阵；

步骤三、综合步骤一建立的惯性导航系统动态误差模型，以及步骤二建立的测速仪动态误差模型，构建组合导航系统动态误差模型：

式中，$x\left(t\right)=\left[\begin{array}{c}{x}_{\mathrm{INS}}\left(t\right)\\ x_{\mathrm{VMS}}\left(t\right)\end{array}\right]$表示组合导航系统动态误差模型的状态向量，由惯性导航系统动态误差模型的状态向量x_INS(t)和测速仪动态误差模型的状态向量x_VMS(t)组成；$w\left(t\right)=\left[\begin{array}{c}{w}_{\mathrm{INS}}\left(t\right)\\ w_{\mathrm{VMS}}\left(t\right)\end{array}\right]$表示组合导航系统动态误差模型的系统噪声，由惯性导航系统动态误差模型的系统噪声w_INS(t)和测速仪动态误差模型的系统噪声w_VMS(t)组成；$F\left(t\right)=\left[\begin{array}{cc}{F}_{\mathrm{INS}}\left(t\right)& 0_{15\×3}\\ 0_{3\×15}& F_{\mathrm{VMS}}\left(t\right)\end{array}\right]$为组合导航系统动态误差转移矩阵，0_15×3表示15行3列的零矩阵；0_3×15表示3行15列的零矩阵；表示组合导航系统动态误差模型状态向量的导数；

对组合导航系统动态误差模型进行离散化处理，得到：

x(k)＝F(k，k-1)x(k-1)+w(k)

式中，F(k，k-1)表示k-1时刻至k时刻的组合导航系统动态误差转移矩阵，x(k)为离散化后的k时刻组合导航系统动态误差模型的状态向量；w(k)表示k时刻的组合导航系统动态误差模型的系统噪声，是均值为零，方差为Q的白噪声序列，Q值根据Q_INS和Q_VMS确定；

步骤四、计算惯性导航系统的位置增量输出、北斗接收机的位置增量输出和测速仪的位置增量输出，具体过程为：

(I)计算惯性导航系统位置增量输出

根据惯性导航系统给定的速度，计算j-1时刻至j时刻的位置增量计算公式为：

${\mathrm{\ΔR}}_j^{\mathrm{INS}}={\mathrm{\Σ}}_{r=(j-1)\×N_1+1}^{j\×N_1}{V}_{\mathrm{\τ}}^n\×{\mathrm{\ΔT}}_{\mathrm{INS}}$

其中，j表示时刻，为正整数，j-1时刻至j时刻的时间间隔记为ΔT_Kal，表示卡尔曼滤波周期，根据载体行驶环境和组合导航系统精度要求确定；ΔT_INS表示惯性导航系统速度输出周期；r表示惯性导航系统在j-1时刻至j时刻输出结果的序列值；表示惯性导航系统输出的序列值为r的速度；N₁＝ΔT_kal/ΔT_INS；

(II)计算北斗接收机位置增量输出

根据北斗接收机给定的速度，同时考虑北斗接收机与惯性导航系统之间的杆臂效应，计算j-1时刻至j时刻的位置增量计算公式为：

${\mathrm{\ΔR}}_j^{\mathrm{BD}}={\mathrm{\Σ}}_{q=(j-1)\×N_2+1}^{j\×N_2}{V}_q^{\mathrm{BD}}\×{\mathrm{\ΔT}}_{\mathrm{BD}}+C_j({\mathrm{\ω}}_j\×L^{\mathrm{BD}})\×{\mathrm{\ΔT}}_{\mathrm{Kal}}$

其中，ΔT_BD表示北斗接收机速度输出周期；q表示北斗接收机在j-1时刻至j时刻输出结果的序列值；表示北斗接收机输出的序列值为q的速度；N₂＝ΔT_Kal/ΔT_BD；C_j表示惯性导航系统在j时刻输出的姿态矩阵，ω_j表示惯性导航系统在j时刻输出的角速度，L^BD表示由北斗接收机天线至惯性导航系统壳体中心的杆臂向量；

(III)计算测速仪位置增量输出

①利用步骤七估计的j-1时刻的测速仪刻度因子误差δτ_j-1对测速仪在j-1时刻至j时刻的路程增量进行修正，即δτ_j-1初始值设置为零，s_j为修正后的路程增量，为修正前采集的路程增量，

②根据步骤八更新的j-1时刻的航向安装夹角α_j-1和俯仰安装夹角β_j-1，构建方向余弦向量M_j-1，M_j-1是将测速仪测得的路程增量s_j投影到惯性导航系统体坐标系中，α_j-1和β_j-1的初始值设置为零，

$M_{j-1}=\left[\begin{array}{c}-\sin \left({\mathrm{\α}}_{j-1}\right)\cos \left({\mathrm{\β}}_{j-1}\right)\\ \cos \left({\mathrm{\α}}_{j-1}\right)\cos \left({\mathrm{\β}}_{j-1}\right)\\ \sin \left({\mathrm{\β}}_{j-1}\right)\end{array}\right]$

③将修正后的测速仪路程增量s_j投影到东北天坐标系中，并考虑测速仪与惯性导航系统之间的杆臂效应，计算测速仪在j-1时刻至j时刻的位置增量计算公式为：

${\mathrm{\ΔR}}_j^{\mathrm{VMS}}=C_j{M}_{j-1}{s}_j+C_j({\mathrm{\ω}}_j\×L^{\mathrm{VMS}})\×{\mathrm{\ΔT}}_{\mathrm{Kal}}$

其中，L^VMS表示由测速仪至惯性导航系统壳体中心的杆臂向量，可预先测量获得；

步骤五、以步骤四计算的惯性导航系统位置增量、北斗接收机位置增量和测速仪位置增量为基础，构建动态误差模型观测值Z_j，计算公式为：

$Z_j=\left[\begin{array}{c}{\mathrm{\ΔR}}_j^{\mathrm{INS}}-{\mathrm{\ΔR}}_j^{\mathrm{VMS}}\\ {\mathrm{\ΔR}}_j^{\mathrm{INS}}-{\mathrm{\ΔR}}_j^{\mathrm{BD}}\end{array}\right]$

步骤六、根据步骤五中的公式Z_j，建立步骤三所述的组合导航系统动态误差模型的观测方程：

Y_j＝H_jx_j+ρ_j

其中，Y_j表示j时刻的观测序列；ρ_j表示j时刻的组合导航系统动态误差模型的观测方程噪声，是均值为零，方差为R的白噪声序列，R值根据实际应用环境人为设定，R为非负矩阵；H_j为j时刻的观测矩阵，公式如下：

$H_j=\left[\begin{array}{c}\begin{array}{cc}{0}_{3\×3}& I_{3\×3}-\left(C_j{M}_{j-1}{s}_j\right)\×0_{3\×6}-C_j{U}_{j-1}{s}_j-C_j{M}_{j-1}{s}_j\end{array}\\ \begin{array}{ccc}{0}_{3\×3}& {\mathrm{\ΔT}}_{\mathrm{Kal}}\×I_{3\×3}& 0_{3\×12}\end{array}\end{array}\right]$

其中，0_3×3表示3阶零矩阵；0_3×6表示3行6列零矩阵；0_3×12表示3行12列零矩阵；I_3×3表示3阶单位阵；(C_jM_j-1s_j)×表示由C_jM_j-1s_j构成的斜负对称矩阵；U_j-1由步骤八更新的j-1时刻的航向安装夹角α_j-1和俯仰安装夹角β_j-1所形成，α_j-1和β_j-1的初始值设置为零：

$U_{j-1}=\left[\begin{array}{cc}-\cos \left({\mathrm{\α}}_{j-1}\right)\cos \left({\mathrm{\β}}_{j-1}\right)& \sin \left({\mathrm{\α}}_{j-1}\right)\sin \left({\mathrm{\β}}_{j-1}\right)\\ -\sin \left({\mathrm{\α}}_{j-1}\right)\cos \left({\mathrm{\β}}_{j-1}\right)& -\cos \left({\mathrm{\α}}_{j-1}\right)\sin \left({\mathrm{\β}}_{j-1}\right)\\ 0& \cos \left({\mathrm{\β}}_{j-1}\right)\end{array}\right]$

其中，j时刻和k时刻有如下关系：

N×(t_k-t_k-1)＝t_j-t_j-1

其中，N为正整数；t_k-t_k-1表示k-1时刻至k时刻的时间间隔；t_j-t_j-1表示j-1时刻至j时刻的时间间隔；

步骤七、根据步骤三建立的组合导航系统动态误差模型及步骤六中的观测方程，结合步骤五给出的观测值，采用卡尔曼滤波器对组合导航系统动态误差模型的状态向量x(k)进行估计：

卡尔曼滤波器为经典卡尔曼滤波器，计算过程如下：

$\hat{x}(k,k-1)=F(k,k-1)\hat{x}(k-1)$

P(k，k-1)＝F(k，k-1)P(k-1)[F(k，k-1)]^T+Q

$K\left(k\right)=P(k,k-1)H_k^T{[H_{k}P(k,k-1)H_k^T+R]}^{-1}$

$\hat{x}\left(k\right)=\hat{x}(k,k-1)+K\left(k\right)[Z_k-H_k\hat{x}(k,k-1)]$

P(k)＝P(k，k-1)-K(k)H_kP(k，k-1)

其中，表示组合导航系统动态误差模型的状态向量x(k)的一步预测；表示组合导航系统动态误差模型的状态向量x(k)的估计值；P(k，k-1)表示一步预测方差；P(k)表示估计方差；K(k)表示滤波增益；

若k≠j，则仅进行状态递推，即令滤波增益K(k)为同等维数的零矩阵；若k＝j，则进 行上述完整计算；

经过上述步骤即可得到k时刻组合导航系统动态误差模型状态向量x(k)的估计值包含位置误差(δP)_k、速度误差(δVⁿ)_k、失准角陀螺仪零偏误差(δε_g)_k、加速度计零偏误差航向安装夹角误差(δα)_k、俯仰安装夹角误差(δβ)_k和测速仪刻度因子误差(δτ）_k；

步骤八、根据步骤七的估计结果对组合导航系统进行误差修正，具体是指利用步骤七获取的误差估计结果对k时刻惯性导航系统的位置输出速度输出和姿态矩阵输出C_k进行校正，同时对陀螺仪零偏ε_g、加速度计零偏航向安装夹角α、俯仰安装夹角β和测速仪刻度因子τ进行更新，计算公式为：

$P_k^{\mathrm{Cor}}=P_k^{\mathrm{INS}}-{\left(\mathrm{\δP}\right)}_k$

$V_k^{\mathrm{Cor}}=V_k^n-{\left({\mathrm{\δV}}^n\right)}_k$

(ε_g)_k＝(ε_g)_k-1-(δε_g)_k

${\left({\▿}_{\mathrm{\α}}\right)}_k={\left({\▿}_{\mathrm{\α}}\right)}_{k-1}-{\left({\mathrm{\δ}\▿}_{\mathrm{\α}}\right)}_k$

α_k＝α_k-1-(δα)_k

β_k＝β_k-1-(δβ)_k

τ_k＝[1-(δτ)_k]×τ_k-1

其中，和分别表示校正后的位置、速度和姿态矩阵；×表示由构成的斜负对称矩阵；(ε_g)_k和分别表示k时刻的陀螺仪零偏和加速度计零偏，初始值均为三维零向量；α_k、β_k和τ_k分别表示k时刻的航向安装夹角、俯仰安装夹角和测速仪刻度因子，初始值均设置为零；

当惯性导航系统进行下一时刻导航解算时，利用(ε_g)_k和对所采集的角速度和比力进行补偿，并利用α_k、β_k和τ_k对步骤四及步骤六中相应的变量进行更新；

步骤九、对步骤一、二和三中的组合导航系统动态误差模型一步转移矩阵F(k，k-1)进行更新，将k+1的值赋给k，设置系统状态为18维零向量，然后返回到步骤四。