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

本发明涉及一种利用北斗测定惯性导航系统与测速仪安装夹角的方法，适用于由北斗接收机、惯性导航系统和测速仪构成的组合导航系统，属于组合导航
技术领域：
。
背景技术：
：惯性导航系统利用牛顿第二定律，在不依赖外部光线、电磁波、声波和磁场等信息的情况下，通过对重力场和惯性力的测量来感知物体的角运动和线运动，实现对运载体的导航。其优点包括输出信息全面、工作自主和抗外界干扰能力强等，在陆、海、空、天领域中获得了广泛应用。不足之处在于惯性导航系统本质上是一种航位推算系统，导航误差随时间不断积累。为改善惯性导航系统长时间工作的导航精度，一般采用组合导航的方式实现惯性导航系统的误差校正。在陆用导航领域，随着北斗导航系统的日臻完善，由北斗接收机和惯性导航系统构成的车载组合导航系统的应用越来越广泛。北斗/惯性组合导航系统在北斗卫星信号接收良好的条件下，能够实现高精度导航，但是当车辆行驶在山区、隧道、城市峡谷等环境时，由于北斗信号受到遮挡，组合导航系统的精度将不可避免地受到影响。因此，有必要在北斗/惯性组合导航系统的基础上，将测速仪纳入到组合导航系统中，构成北斗/惯性/测速仪组合导航系统。在北斗接收机正常工作的情况下，使用北斗/惯性/测速仪组合导航模式，在北斗卫星信号受遮挡的情况下，切换到惯性/测速仪组合导航模式。由于惯性导航系统和测速仪分别安装在车体的不同部位，导致惯性导航系统的体坐标系和测速仪的体坐标系存在一定夹角。为保证惯性导航系统测量的载体速度和测速仪测量的载体速度具有共同基准，需要对安装夹角进行估计。目前计算惯性导航系统和测速仪安装夹角的主要方法包括：(1)利用两个精确的已知路标点，通过跑车实验计算安装夹角；(2)利用GPS与测速仪组合实现安装夹角估计；(3)利用惯性导航系统与测速仪组合实现安装夹角估计。第一种方法的不足在于需要路标点辅助，限制了组合导航系统的应用范围，且所计算的安装夹角是固定值，当夹角发生改变时需要重新测定；第二种方法的不足在于进行安装夹角估计时，GPS信号不能受遮挡，对车辆行驶环境提出较高的要求，且估计时间较长；第三种方法要求惯性导航系统具有很高的精度，难以用于中低精度惯性导航系统中。技术实现要素：本发明的目的是克服已有技术存在的不足，提供一种利用北斗测定惯性导航系统与测速仪安装夹角的方法。该方法通过融合北斗接收机和惯性导航系统的信息，得到比各单独系统精度更高的输出信息，进而通过建模实现惯性导航系统与测速仪安装夹角测定，降低测定过程中对车辆行驶环境的要求，缩短测定时间，且适用于各类精度的惯性导航系统。一种利用北斗测定惯性导航系统与测速仪安装夹角的方法，将惯性导航系统安装在载车车厢内，北斗接收机安装在车顶，测速仪通过软轴与驱动轮轴承连接，惯性导航系统完成初始对准后开始运动，其横坐标为纬度，纵坐标为经度；惯性导航系统包含的3个陀螺仪的随机漂移均为0.01°/h，常值漂移均为0.02°/h；惯性导航系统包含的3个加速度计随机漂移均为50μg，常值漂移均为100μg，测速仪初始刻度因子为0.01米/脉冲，包括步骤：步骤一、建立惯性导航系统动态误差模型，状态变量包括位置误差、速度误差、失准角和惯性器件漂移误差：在东北天坐标系下，惯性导航系统动态误差模型为：式中，t为时间值，是正实数；xINS(t)表示惯性导航系统动态误差模型的状态向量，由位置误差δP、速度误差δVn、失准角陀螺仪零偏误差δεg和加速度计零偏误差δVa组成；wINS(t)表示惯性导航系统动态误差模型的系统噪声；FINS(t)为惯性导航系统动态误差转移矩阵；表示惯性导航系统动态误差模型状态向量的导数；对惯性导航系统动态误差模型进行离散化处理，得到：xINS(k)＝FINS(k，k-1)xINS(k-1)+wINS(k)式中，k表示时刻，为正整数，k-1时刻至k时刻的时间间隔记为ΔTDis，表示误差模型离散化周期；FINS(k，k-1)表示k-1时刻至k时刻的惯性导航系统动态误差转移矩阵；xINS(k)为离散化后的k时刻惯性导航系统的状态向量；wINS(k)表示k时刻的惯性导航系统动态误差模型的系统噪声，是均值为零，方差为QINS的白噪声序列，QINS为非负矩阵；步骤二、建立测速仪动态误差模型，该模型包含的状态变量为安装夹角误差和和测速仪刻度因子误差，其中安装夹角误差包括航向安装夹角误差与俯仰安装夹角误差，测速仪动态误差模型为：x.VMS(t)=FVMS(t)xVMS(t)+wVMS(t)]]>式中，xVMS(t)＝[δαδβδτ]T，xVMS(t)表示测速仪动态误差模型的状态向量，由航向安装夹角误差δα、俯仰安装夹角误差δβ和测速仪刻度因子误差δτ组成；wVMS(t)表示测速仪 动态误差模型的系统噪声；FVMS(t)为测速仪动态误差转移矩阵；表示测速仪动态误差模型状态向量的导数；对测速仪动态误差模型进行离散化处理，得到：xVMS(k)＝FVMS(k，k-1)xVMS(k-1)+wVMS(k)式中，FVMS(k，k-1)表示k-1时刻至k时刻的测速仪动态误差转移矩阵；xVMS(k)为离散化后的k时刻测速仪动态误差模型的状态向量；wVMS(k)表示k时刻的测速仪动态误差模型的系统噪声，是均值为零，方差为QVMS的白噪声序列，QVMS为非负矩阵；步骤三、综合步骤一建立的惯性导航系统动态误差模型，以及步骤二建立的测速仪动态误差模型，构建组合导航系统动态误差模型：式中，x(t)=xINS(t)xVMS(t)]]>表示组合导航系统动态误差模型的状态向量，由惯性导航系统动态误差模型的状态向量xINS(t)和测速仪动态误差模型的状态向量xVMS(t)组成；w(t)=wINS(t)wVMS(t)]]>表示组合导航系统动态误差模型的系统噪声，由惯性导航系统动态误差模型的系统噪声wINS(t)和测速仪动态误差模型的系统噪声wVMS(t)组成；F(t)=FINS(t)O15×3O3×15FVMS(t)]]>为组合导航系统动态误差转移矩阵，O15×3表示15行3列的零矩阵；O3×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值根据QINS和QVMS确定；步骤四、计算惯性导航系统的位置增量输出、北斗接收机的位置增量输出和测速仪的位置增量输出，具体过程为：(I)计算惯性导航系统位置增量输出根据惯性导航系统给定的速度，计算j-1时刻至j时刻的位置增量计算公式为：ΔRjINS=Σr=(j-1)×N1+1j×N1Vrn×ΔTINS]]>其中，j表示时刻，为正整数，j-1时刻至j时刻的时间间隔记为ΔTKal，表示卡尔曼滤波周期，根据载体行驶环境和组合导航系统精度要求确定；ΔTINS表示惯性导航系统速度输出周 期；r表示惯性导航系统在j-1时刻至j时刻输出结果的序列值；表示惯性导航系统输出的序列值为r的速度；N1＝ΔTKal/ΔTINS；(II)计算北斗接收机位置增量输出根据北斗接收机给定的速度，同时考虑北斗接收机与惯性导航系统之间的杆臂效应，计算j-1时刻至j时刻的位置增量计算公式为：ΔRjBD=Σq=(j-1)×N2+1j×NzVqBD×ΔTBD+Cj(ωj×LBD)×ΔTKal]]>其中，ΔTBD表示北斗接收机速度输出周期；q表示北斗接收机在j-1时刻至j时刻输出结果的序列值；表示北斗接收机输出的序列值为q的速度；N2＝ΔTKal/ΔTBD；Cj表示惯性导航系统在j时刻输出的姿态矩阵，ωj表示惯性导航系统在j时刻输出的角速度，LBD表示由北斗接收机天线至惯性导航系统壳体中心的杆臂向量；(III)计算测速仪位置增量输出①利用步骤七估计的j-1时刻的测速仪刻度因子误差δτj-1对测速仪在j-1时刻至j时刻的路程增量进行修正，即δτj-1初始值设置为零，sj为修正后的路程增量，为修正前采集的路程增量，②根据步骤八更新的j-1时刻的航向安装夹角αj-1和俯仰安装夹角βj-1，构建方向余弦向量Mj-1，Mj-1是将测速仪测得的路程增量sj投影到惯性导航系统体坐标系中，αj-1和βj-1的初始值设置为零，Mj-1=-sin(αj-1)cos(βj-1)cos(αj-1)cos(βj-1)sin(βj-1)]]>③将修正后的测速仪路程增量sj投影到东北天坐标系中，并考虑测速仪与惯性导航系统之间的杆臂效应，计算测速仪在j-1时刻至j时刻的位置增量计算公式为：ΔRjVMS=CjMj-1sj+Cj(ωj×LVMS)×ΔTKal]]>其中，LVMS表示由测速仪至惯性导航系统壳体中心的杆臂向量，可预先测量获得；步骤五、以步骤四计算的惯性导航系统位置增量、北斗接收机位置增量和测速仪位置增量为基础，构建动态误差模型观测值Zj，计算公式为：Zj=ΔRjINS-ΔRjVMSΔRjINS-ΔRjBD]]>步骤六、根据步骤五中的公式Zj，建立步骤三所述的组合导航系统动态误差模型的观测方程：Yj＝Hjxj+ρj其中，Yj表示j时刻的观测序列；ρj表示j时刻的组合导航系统动态误差模型的观测方程噪声，是均值为零，方差为R的白噪声序列，R值根据实际应用环境人为设定，R为非负矩阵；Hj为j时刻的观测矩阵，公式如下：Hj=O3×3I3×3-(CjMj-1sj)×O3×6-CjUj-1sj-CjMj-1sjO3×3ΔTKal×I3×3O3×12]]>其中，O3×3表示3阶零矩阵；O3×6表示3行6列零矩阵；O3×12表示3行12列零矩阵；I3×3表示3阶单位阵；(CjMj-1sj)×表示由CjMj-1sj构成的斜负对称矩阵；Uj-1由步骤八更新的j-1时刻的航向安装夹角αj-1和俯仰安装夹角βj-1所形成，αj-1和βj-1的初始值设置为零：Uj-1=-cos(αj-1)cos(βj-1)sin(αj-1)sin(βj-1)-sin(αj-1)cos(βj-1)-cos(αj-1)sin(βj-1)0cos(βj-1)]]>其中，j时刻和k时刻有如下关系：N×(tk-tk-1)＝tj-tj-1其中，N为正整数；tk-tk-1表示k-1时刻至k时刻的时间间隔；tj-tj-1表示j-1时刻至j时刻的时间间隔；步骤七、根据步骤三建立的组合导航系统动态误差模型及步骤六中的观测方程，结合步骤五给出的观测值，采用卡尔曼滤波器对组合导航系统动态误差模型的状态向量x(k)进行估计：卡尔曼滤波器为经典卡尔曼滤波器，计算过程如下：x^(k,k-1)=F(k,k-1)x^(k-1)]]>P(k，k-1)＝F(k，k-1)P(k-1)[F(k，k-1)]T+QK(k)=P(k,k-1)HkT[HkP(k,k-1)HkT+R]-1]]>x^(k)=x^(k,k-1)+K(k)[Zk-Hkx^(k,k-1)]]]>P(k)＝P(k，k-1)-K(k)HkP(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、速度误差(δVn)k、失准角陀螺仪零偏误差(δεg)k、加速度计零偏误差航向安装夹角误差(δα)k、俯仰安装夹角误差(δβ)k和测速仪刻度因子误差(δτ)k；步骤八、根据步骤七的估计结果对组合导航系统进行误差修正，具体是指利用步骤七获取的误差估计结果对k时刻惯性导航系统的位置输出速度输出和姿态矩阵输出Ck进行校正，同时对陀螺仪零偏εg、加速度计零偏航向安装夹角α、俯仰安装夹角β和测速仪刻度因子τ进行更新，计算公式为：PkCor=PkINS-(δP)k]]>VkCor=Vkn-(δVn)k]]>(εg)k＝(εg)k-1-(δεg)k(▿a)k=(▿a)k-1-(δ▿a)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维零向量，然后返回到步骤四。与现有技术相比较，本发明的优点是：(1)通过融合北斗接收机和惯性导航系统的信息， 能够得到比单独使用北斗接收机或惯性导航系统精度更高的输出信息，从而更加快速和准确地实现安装夹角的测定；(2)惯性导航系统具有低通滤波的效果，与北斗接收机联合使用，能够克服北斗接收机输出存在野值时对安装夹角测定精度的影响，放宽了安装夹角测定过程中对运载体行驶环境的要求；(3)北斗接收机具有较高精度的速度输出，即便与较低精度的惯性导航系统组合使用，也可实现高精度导航信息输出，进而实现惯性导航系统与测速仪安装夹角的测定，放宽了惯性导航系统的精度要求。附图说明图1为本发明的具体实施例中的载车运行轨迹示意图。图2为本发明的具体实施例中的航向安装夹角估计示意图。图3为本发明的具体实施例中的俯仰安装夹角估计示意图。具体实施方式本发明的技术关键点在于：①建立安装夹角误差与测速仪路程增量误差的关系模型，对观测矩阵进行精确建模；②根据北斗接收机的速度输出进行运载体位置增量计算，用于构建组合导航系统的观测值；③采用闭环反馈更新的方式，实现惯性导航系统与测速仪安装夹角的快速准确估计。一种利用北斗测定惯性导航系统与测速仪安装夹角的方法，将惯性导航系统安装在载车车厢内，北斗接收机安装在车顶，测速仪通过软轴与驱动轮轴承连接，惯性导航系统完成初始对准后开始运动，其横坐标为纬度，纵坐标为经度，惯性导航系统包含的3个陀螺仪的随机漂移均为0.01°/h，常值漂移均为0.02°/h；惯性导航系统包含的3个加速度计随机漂移均为50μg，常值漂移均为100μg，测速仪初始刻度因子为0.01米/脉冲，包括下列步骤：步骤一、建立惯性导航系统动态误差模型，该模型为Φ角误差方程或Ψ角误差方程，状态包括位置误差、速度误差、失准角和惯性器件漂移误差。在东北天坐标系下，惯性导航系统动态误差模型表示如公式(1)所示：x·INS(t)=FINS(t)xINS(t)+wINS(t)---(1)]]>式中，t为时间值，是正实数；表示惯性导航系统动态误差模型的状态向量，由位置误差δP、速度误差δVn、失准角陀螺仪零偏误差δεg和加速度计零偏误差组成；wINS(t)表示惯性导航系统动态误差模型的系统噪声；FINS(t)为惯性导航系统动态误差转移矩阵；表示惯性导航系统动态误差模型 状态向量的导数；对惯性导航系统动态误差模型进行离散化处理，可得：xINS(k)＝FINS(k，k-1)xINS(k-1)+wINS(k)(2)式中，k表示时刻，为正整数，k-1时刻至k时刻的时间间隔记为ΔTDis，表示误差模型离散化周期，属于人为设定，一般根据载体行驶环境和组合导航系统精度要求进行确定；FINS(k，k-1)表示k-1时刻至k时刻的惯性导航系统动态误差转移矩阵；xINS(k)为离散化后的k时刻惯性导航系统的状态向量；wINS(k)表示k时刻的惯性导航系统动态误差模型的系统噪声，是均值为零，方差为QINS的白噪声序列，QINS值根据实际应用环境人为设定，QINS为非负矩阵。步骤二、建立测速仪动态误差模型，该模型包含的状态为安装夹角误差(包括航向安装夹角误差与俯仰安装夹角误差)和测速仪刻度因子误差。测速仪动态误差模型表示如公式(3)所示：x·VMS(t)=FVMS(t)xVMS(t)+wVMS(t)---(3)]]>式中，xVMS(t)＝[δαδβδτ]T，xVMS(t)表示测速仪动态误差模型的状态向量，由航向安装夹角误差δα、俯仰安装夹角误差δβ和测速仪刻度因子误差δτ组成；wVMS(t)表示测速仪动态误差模型的系统噪声；FVMS(t)为测速仪动态误差转移矩阵；表示测速仪动态误差模型状态向量的导数。对测速仪动态误差模型进行离散化处理，可得：xVMS(k)＝FVMS(k，k-1)xVMS(k-1)+wVMS(k)(4)式中，FVMS(k，k-1)表示k-1时刻至k时刻的测速仪动态误差转移矩阵；xVMS(k)为离散化后的k时刻测速仪动态误差模型的状态向量；wVMS(k)表示k时刻的测速仪动态误差模型的系统噪声，是均值为零，方差为QVMS的白噪声序列，QVMS值根据实际应用环境人为设定，QVMS为非负矩阵。步骤三、综合步骤一建立的惯性导航系统动态误差模型，以及步骤二建立的测速仪动态误差模型，构成如下组合导航系统动态误差模型：x·(t)=F(t)x(t)+w(t)---(5)]]>式中，x(t)=xINS(t)xVMS(t)]]>表示组合导航系统动态误差模型的状态向量，由惯性导航系统动态误差模型的状态向量xINS(t)和测速仪动态误差模型的状态向量xVMS(t)组成；w(t)=wINS(t)wVMS(t)]]>表示组合导航系统动态误差模型的系统噪声，由惯性导航系统动态误差模 型的系统噪声wINS(t)和测速仪动态误差模型的系统噪声wVMS(t)组成；F(t)=FINS(t)015×3o3×15FVMS(t)]]>为组合导航系统动态误差转移矩阵，O15×3表示15行3列的零矩阵；O3×15表示3行15列的零矩阵；表示组合导航系统动态误差模型状态向量的导数。对组合导航系统动态误差模型进行离散化处理，可得：x(k)＝F(k，k-1)x(k-1)+w(k)(6)式中，F(k，k-1)表示k-1时刻至k时刻的组合导航系统动态误差转移矩阵，x(k)为离散化后的k时刻组合导航系统动态误差模型的状态向量；w(k)表示k时刻的组合导航系统动态误差模型的系统噪声，是均值为零，方差为Q的白噪声序列，Q值根据QINS和QVMS确定。步骤四、计算惯性导航系统的位置增量输出、北斗接收机的位置增量输出和测速仪的位置增量输出。具体过程为：(I)惯性导航系统位置增量输出计算根据惯性导航系统给定的速度，计算j-1时刻至j时刻的位置增量计算公式为：ΔRjINS=Σr=(j-1)×N1+1j×N1VTn×ΔTINS---(7)]]>其中，j表示时刻，为正整数，j-1时刻至j时刻的时间间隔记为ΔTKal，表示卡尔曼滤波周期，属于人为设定，一般根据载体行驶环境和组合导航系统精度要求进行确定；ΔTINS表示惯性导航系统速度输出周期；r表示惯性导航系统在j-1时刻至j时刻输出结果的序列值；表示惯性导航系统输出的序列值为r的速度；N1＝ΔTKal/ΔTINS。(II)北斗接收机位置增量输出计算根据北斗接收机给定的速度，同时考虑北斗接收机与惯性导航系统之间的杆臂效应，计算j-1时刻至j时刻的位置增量计算公式为：ΔRjBD=Σq=(j-1)×N2+1j×NzVqBD×ΔTBD+Cj(ωj×LBD)×ΔTKal---(8)]]>其中，ΔTBD表示北斗接收机速度输出周期；q表示北斗接收机在j-1时刻至j时刻输出结果的序列值；表示北斗接收机输出的序列值为q的速度；N2＝ΔTkal/ΔTBD；Cj表示惯性导航系统在j时刻输出的姿态矩阵，ωj表示惯性导航系统在j时刻输出的角速度，LBD表示由北斗接收机天线至惯性导航系统壳体中心的杆臂向量，可预先测量获得。(III)测速仪位置增量输出计算①利用步骤七估计的j-1时刻的测速仪刻度因子误差δτj-1(初始值设置为零)对测速仪 在j-1时刻至j时刻的路程增量进行修正，即sj为修正后的路程增量，为修正前采集的路程增量；②根据步骤八更新的j-1时刻的航向安装夹角αj-1(初始值设置为零)和俯仰安装夹角βj-1(初始值设置为零)构建方向余弦向量Mj-1：Mj-1=-sin(αj-1)cos(βj-1)cos(αj-1)cos(βj-1)sin(βj-1)---(9)]]>Mj-1的作用是将测速仪测得的路程增量sj投影到惯性导航系统体坐标系中。③将修正后的测速仪路程增量sj投影到东北天坐标系中，并考虑测速仪与惯性导航系统之间的杆臂效应，计算测速仪在j-1时刻至j时刻的位置增量计算公式为：ΔRjVMS=CjMj-1sj+Cj(ωj×LVMS)×ΔTKal---(10)]]>其中，LVMS表示由测速仪至惯性导航系统壳体中心的杆臂向量，可预先测量获得。步骤五、以步骤四计算的惯性导航系统位置增量、北斗接收机位置增量和测速仪位置增量为基础，构建动态误差模型观测值Zj，计算公式为：Zj=ΔRjINS-ΔRjVMSΔRjINS-ΔRjRD---(11)]]>步骤六、根据步骤五中的公式(11)，建立步骤三所述的组合导航系统动态误差模型的观测方程，如公式(12)所示：Yj＝Hjxj+ρj(12)其中，Yj表示j时刻的观测序列；ρj表示j时刻的组合导航系统动态误差模型的观测方程噪声，是均值为零，方差为R的白噪声序列，R值根据实际应用环境人为设定，R为非负矩阵；Hj为j时刻的观测矩阵，其具体形式如公式(13)所示：Hj=o3×3I3×3-(CjMj-1sj)×o3×6-CjUj-1sj-CjUj-1sj-CjMj-1sjo3×3ΔTKal×I3×3o3×12---(13)]]>其中，O3×3表示3阶零矩阵；O3×6表示3行6列零矩阵；O3×12表示3行12列零矩阵；I3×3表示3阶单位阵；(CjMj-1sj)×表示由CjMj-1sj构成的斜负对称矩阵；Uj-1由步骤八更新的j-1时刻的航向安装夹角αj-1(初始值设置为零)和俯仰安装夹角βj-1(初始值设置为零) 所形成：Uj-1=-cos(αj-1)cos(βj-1)sin(αj-1)sin(βj-1)-sin(αj-1)cos(βj-1)-cos(αj-1)sin(βj-1)0cos(βj-1)---(14)]]>需要注意的是，本发明中j时刻和k时刻一般有如下关系：N×(tk-tk-1)＝tj-tj-1(15)其中，N为正整数；tk-tk-1表示k-1时刻至k时刻的时间间隔；tj-tj-1表示j-1时刻至j时刻的时间间隔。公式(15)表明对组合导航系统进行离散化的频率比卡尔曼滤波的频率快。步骤七、对公式(6)所示组合导航系统的误差状态x(k)进行估计。根据步骤三建立的组合导航系统动态误差模型及步骤六中的观测方程，结合步骤五给出的观测值，采用卡尔曼滤波器对组合导航系统动态误差模型的状态向量x(k)进行估计。具体为：卡尔曼滤波器为经典卡尔曼滤波器，计算过程如公式(16)～(20)所示：x^(k,k-1)=F(k,k-1)x^(k-1)---(16)]]>P(k，k-1)＝F(k，k-1)P(k-1)[F(k，k-1)]T+Q(17)K(k)=P(k,k-1)HkT[HkP(k,k-1)HkT+R]-1---(18)]]>x^(k)=x^(k,k-1)+K(k)[Zk-Hkx^(k,k-1)]---(19)]]>P(k)＝P(k，k-1)-K(k)HkP(k，k-1)(20)其中，表示组合导航系统动态误差模型的状态向量x(k)的一步预测；表示组合导航系统动态误差模型的状态向量x(k)的估计值；P(k，k-1)表示一步预测方差；P(k)表示估计方差；K(k)表示滤波增益。从公式(15)可知，组合导航系统动态误差模型的离散化周期和卡尔曼滤波周期往往不一致，在进行公式(16)～(20)的计算时，若k≠j，则仅进行状态递推，即令滤波增益K(k)为同等维数的零矩阵；若k＝j，则进行上述完整计算。经过上述步骤即可得到k时刻组合导航系统动态误差模型状态向量x(k)的估计值包含位置误差(δP)k、速度误差(δVn)k、失准角陀螺仪零偏误差(δεg)k、加速度计零偏误差航向安装夹角误差(δα)k、俯仰安装夹角误差(δβ)k和测速仪刻度因子误差(δτ)k。步骤八、根据步骤七的估计结果对组合导航系统进行误差修正。利用步骤七获取的误差估计结果对k时刻惯性导航系统的位置输出速度输出和姿 态矩阵输出Ck进行校正，同时对陀螺仪零偏εg、加速度计零偏航向安装夹角α、俯仰安装夹角β和测速仪刻度因子τ进行更新，计算公式为：PkCor=PkINS-(δP)k---(21)]]>VkCor=Vkn-(δVn)k---(22)]]>(εg)k＝(εg)k-1-(δεg)k(24)(▿a)k=(▿a)k-1-(δ▿a)k---(25)]]>αk＝αk-1-(δα)k(26)βk＝βk-1-(δβ)k(27)τk＝[1-(δτ)k]×τk-1(28)其中，和分别表示校正后的位置、速度和姿态矩阵；×表示由构成的斜负对称矩阵；(εg)k和分别表示k时刻的陀螺仪零偏和加速度计零偏，初始值均为三维零向量；αk、βk和τk分别表示k时刻的航向安装夹角、俯仰安装夹角和测速仪刻度因子，初始值均设置为零。当惯性导航系统进行下一时刻导航解算时，利用(εg)k和对所采集的角速度和比力进行补偿，并利用αk、βk和τk对步骤四及步骤六中相应的变量进行更新。步骤九、对步骤一、二和三中的组合导航系统动态误差模型一步转移矩阵F(k，k-1)进行更新，将k+1的值赋给k，设置系统状态为18维零向量，然后返回到步骤四。在本发明的具体实施例中，将惯性导航系统安装在载车车厢内，北斗接收机安装在车顶，测速仪通过软轴与驱动轮轴承连接。惯性导航系统完成初始对准后开始运动，运动轨迹如图1所示，其横坐标为纬度(单位：度)，纵坐标为经度(单位：度)，跑车时间2400秒。惯性导航系统包含的3个陀螺仪的随机漂移均为0.01°/h，常值漂移均为0.02°/h；惯性导航系统包含的3个加速度计随机漂移均为50μg，常值漂移均为100μg。北斗接收机的水平定位精度为3米(1σ)。测速仪初始刻度因子为0.01米/脉冲。本发明的实施过程如下：步骤一、建立包括位置误差、速度误差、失准角和惯性器件漂移误差在内的惯性导航系统动态误差模型；动态误差模型为Φ角误差方程或Ψ角误差方程。本例中动态误差模型为Φ角误差方程，其形式如公式(1)所述，其中，FINS(t)表示为：FINS(t)=F11F12O3×3O3×3O3×3F21F22F23O3×3F25F31F32F33F34O3×3O6×3O6×3O6×3O6×3O6×3---(29)]]>各个矩阵块的形式表示为：F11=ρERmmRm+h0ρERm+hρNsec(L)(tan(L)-RttRt+h)0-ρNsec(L)Rt+h000---(30)]]>式中，[ρEρNρU]表示地球坐标系相对地理坐标系的转动角速率；L和h分别表示载体所处纬度和高程；Rm和Rt分别表示卯酉圈和子午圈的半径；Rmm表示卯酉圈的半径关于纬度求导，Rtt表示子午圈的半径关于纬度求导，可分别表示如下：Rmm=∂Rm/∂L=6×R0×e×sin(L)×cos(L)---(31)]]>Rtt=∂Rt/∂L=2×R0×e×sin(L)×cos(L)---(32)]]>其中，R0为地球半径，e为椭圆率。F12=01Rm+h0sec(L)Rt+h00001---(33)]]>F21=(2ωN+ρNsec2(L)-ρURttRt+h)vN+(2ωU+ρNRttRt+h)vU0vUρNRt+h-ρNtan(L)Rt+hvU(-2ωN-ρNsec2(L)+ρURttRt+h)vE-ρERmmRm+hvU0ρNtan(L)Rt+hvE-vUρERm+hρERmmRm+hvN-(2ωU+ρNRttRt+h)vE0vNρERm+h-vEρNRt+h---(34)]]>式中，[ωEωNωU]T表示地球自转角速率在东北天方向的分量；[vEvNvU]T表示载体在东北天方向的运动速度。F22=tan(L)vN-vURt+h2ωU+ρU-2ωN-ρN-2ωU-2ρU-vURm+hρE2ωN+2ρN-2ρE0---(35)]]>F23=0-fUfNfU0-fE-fNfE0---(36)]]>式中，[fEfNfU]T表示加速度计测量的东向、北向和天向的比力。F25=Cbn---(37)]]>式中，表示姿态矩阵。F31=-ρERmmRm+h0-ρERm+h-ωU-ρNRttRt+h0-ρNRt+hωN+ρNsec2(L)-ρNtan(L)Rt+h0-ρNtan(L)Rt+h---(38)]]>F32=0-1Rm+h01Rt+h00tan(L)Rt+h00---(39)]]>F33=0ωU+ρU-ωN-ρN-ωU-ρU0ρEωN+ρN-ρE0---(40)]]>F34=-Cbn---(41)]]>对上述具体的惯性导航系统动态误差模型进行离散化处理，得到和公式(2)一致的离散化形式：xINS(k)＝FINS(k，k-1)xINS(k-1)+wINS(k)(42)本例中，离散化周期ΔTDis＝0.1。步骤二、建立包括安装夹角(包括航向安装夹角与俯仰安装夹角)和测速仪刻度因子误差在内的测速仪动态误差模型。测速仪动态误差模型表示如公式(3)，其中，FVMS(t)＝O3×3(43)对上述具体的测速仪动态误差模型进行离散化处理，得到和公式(4)一致的离散化形式：xVMS(k)＝FVMS(k，k-1)xVMS(k-1)+wVMS(k)(44)其中，根据离散化公式可知，FVMS(k，k-1)＝I3×3。步骤三、将测速仪动态误差模型的状态向量xVMS(t)扩展到惯性导航系统动态误差模型中，构成如公式(5)所示的组合导航系统动态误差模型。根据公式(29)和公式(43)可知，公式(5)中的F(t)形式为：F(t)=F11F12O3×3O3×3O3×3O3×3F21F22F23O3×3F25O3×3F31F32F33F34O3×3O3×3O9×3O9×3O9×3O9×3O9×3O9×3---(45)]]>对上述具体的组合导航系统动态误差模型进行离散化处理，得到和公式(6)一致的离散化形式：x(k)＝F(k，k-1)x(k-1)+w(k)(46)本例中，w(k)的方差阵Q＝diag{(0.1)2，(0.1)2，(0.2)2，(0.001)2，(0.001)2，(0.001)2，(0.01)2，(0.01)2，(0.01)2，0，0，0，0，0，0，0，0，0}。步骤四、计算惯性导航系统的位置增量输出、北斗接收机的位置增量输出和测速仪的位置增量输出，具体过程为：(I)惯性导航系统位置增量输出计算根据惯性导航系统给定的速度，计算j-1时刻至j时刻的位置增量计算公式为：ΔRjINS=Σr=(j-1)×N1+1j×N1Vrn×ΔTINS---(47)]]>其中，j表示时刻，为正整数，j-1时刻至j时刻的时间间隔记为ΔTKal，表示卡尔曼滤波周期，属于人为设定，一般根据载体行驶环境和组合导航系统精度要求进行确定；ΔTINS表示惯性导航系统速度输出周期；r表示惯性导航系统在j-1时刻至j时刻输出结果的序列值；表示惯性导航系统输出的序列值为r的速度；N1＝ΔTKal/ΔTINS。本例中，ΔTKal＝1，ΔTINS＝0.01，N1＝100。(II)北斗接收机位置增量输出计算根据北斗接收机给定的速度，同时考虑北斗接收机与惯性导航系统之间的杆臂效应，计算j-1时刻至j时刻的位置增量计算公式为：ΔRjBD=Σq=(j-1)×N2+1j×N2VqBD×ΔTBD+Cj(ωj×LBD)×ΔTKal---(48)]]>其中，ΔTBD表示北斗接收机速度输出周期；q表示北斗接收机在j-1时刻至j时刻输出结果的序列值；表示北斗接收机输出的序列值为q的速度；N2＝ΔTKal/ΔTBD；Cj表示惯性导航系统在j时刻输出的姿态矩阵，ωj表示惯性导航系统在j时刻输出的角速度，LBD表示由北斗接收机天线至惯性导航系统壳体中心的杆臂向量，可预先测量获得。本例中，ΔTBD＝0.2，N2＝5，LBD＝[0；-0.1；-1]。(III)测速仪位置增量输出计算①利用步骤七估计的j-1时刻的测速仪刻度因子误差δτj-1(初始值设置为零)对测速仪在j-1时刻至j时刻的路程增量进行修正，即sj为修正后的路程增量，为修正前采集的路程增量；②根据步骤八更新的j-1时刻的航向安装夹角αj-1(初始值设置为零)和俯仰安装夹角 βj-1(初始值设置为零)构建方向余弦向量Mj-1：Mj-1=-sin(αj-1)cos(βj-1)cos(αj-1)cos(βj-1)sin(βj-1)---(49)]]>Mj-1的作用是将测速仪测得的路程增量sj投影到惯性导航系统体坐标系中。③将修正后的测速仪路程增量sj投影到东北天坐标系中，并考虑测速仪与惯性导航系统之间的杆臂效应，计算测速仪在j-1时刻至j时刻的位置增量计算公式为：ΔRjVMS=CjMj-1sjCj(ωj×LVMS)×ΔTKal---(50)]]>其中，LVMS表示由测速仪至惯性导航系统壳体中心的杆臂向量，可预先测量获得。本例中，LVMS＝[0.1；0；0.3]。步骤五、以步骤四计算的惯性导航系统位置增量、北斗接收机位置增量和测速仪位置增量为基础，构建动态误差模型观测值Zj：Zj=ΔRjINS-ΔRjVMSΔRjINS-ΔRjBD---(51)]]>步骤六、根据步骤五中的公式(11)，建立步骤三所述的组合导航系统动态误差模型的观测方程：Yj＝Hjxj+ρj(52)其中，Yj表示j时刻的观测序列；ρj表示j时刻的组合导航系统动态误差模型的观测方程噪声，是均值为零，方差为R的白噪声序列，R值根据实际应用环境人为设定，R为非负矩阵，本例中，R＝diag{(0.2)2，(0.2)2，(0.2)2，(0.05)2，(0.05)2，(0.05)2}；Hj为j时刻的观测矩阵，其具体形式为：Hj=O3×3I3×3-(CjMj-1sj)×O3×6-CjUj-1sj-CjMj-1sjO3×3ΔTKal×l3×3O3×12---(53)]]>其中，O3×3表示3阶零矩阵；O3×6表示3行6列零矩阵；O3×12表示3行12列零矩阵；I3×3表示3阶单位阵；(CjMj-1sj)×表示由CjMj-1sj构成的斜负对称矩阵；Uj-1由步骤八更新的j-1时刻的航向安装夹角αj-1(初始值设置为零)和俯仰安装夹角βj-1(初始值设置为零)所形成：Uj-1=-cos(αj-1)cos(βj-1)sin(αj-1)sin(βj-1)-sin(αj-1)cos(βj-1)-cos(αj-1)sin(βj-1)0cos(βj-1)---(54)]]>需要注意的是，本发明中j时刻和k时刻-般有如下关系：N×(tk-tk-1)＝tj-tj-1(55)其中，N为正整数，本例取N＝10。步骤七、对组合导航系统的误差状态x(k)进行估计。根据步骤三建立的组合导航系统动态误差模型及步骤六中的观测方程，结合步骤五给出的观测值，采用卡尔曼滤波器对组合导航系统动态误差模型的状态向量x(k)进行估计。具体为：卡尔曼滤波器为经典卡尔曼滤波器，计算过程如公式(56)～(60)所示：x^(k,k-1)=F(k,k-1)x^(k-1)---(56)]]>P(k，k-1)＝F(k，k-1)P(k-1)[F(k，k-1)]T+Q(57)K(k)=P(k,k-1)HkT[HkP(k,k-1)HkT+R]-1---(58)]]>x^(k)=x^(k,k-1)+K(k)[Zk-Hkx^(k,k-1)]---(59)]]>P(k)＝P(k，k-1)-K(k)HkP(k，k-1)(60)其中，表示组合导航系统动态误差模型的状态向量x(k)的一步预测；表示组合导航系统动态误差模型的状态向量x(k)的估计值；P(k，k-1)表示一步预测方差；P(k)表示估计方差；K(k)表示滤波增益。本例中，取值为18维零向量，P(0)＝diag{(1)2，(1)2，(2)2，(0.01)2，(0.01)2，(0.01)2，(0.00052)2，(0.00052)2，(0.00087)2，(0.001)2，(0.001)2，(0.001)2，(0.01)2，(0.01)2，(0.01)2，(0.0017)2，(0.0017)2，(0.001)2}。在进行公式(56)～(60)的计算时，若k≠j，则仅进行状态递推，即令滤波增益K(k)为18×18的零矩阵；若k＝j，则进行上述完整计算。经过上述步骤即可得到k时刻组合导航系统动态误差模型状态向量x(k)的估计值包含位置误差(δP)k、速度误差(δVn)k、失准角陀螺仪零偏误差(δεg)k、加速度计零偏误差航向安装夹角误差(δα)k、俯仰安装夹角误差(δβ)k和测速仪刻度因子误差(δτ)k。步骤八、根据步骤七的估计结果对组合导航系统进行误差修正。利用步骤七获取的误差估计结果对k时刻惯性导航系统的位置输出速度输出和姿态矩阵输出Ck进行校正，同时对陀螺仪零偏εg、加速度计零偏航向安装夹角α、俯仰安 装夹角β和测速仪刻度因子τ进行更新，计算公式为：PkCor=PkINS-(δP)k---(61)]]>VkCor=Vkn-(δVn)k---(62)]]>(εg)k＝(εg)k-1-(δεg)k(64)(▿a)k=(▿a)k-1-(δ▿a)k---(65)]]>αk＝αk-1-(δα)k(66)βk＝βk-1-(δβ)k(67)τk＝[1-(δτ)k]×τk-1(68)其中，和分别表示校正后的位置、速度和姿态矩阵；×表示由构成的斜负对称矩阵；(εg)k和分别表示k时刻的陀螺仪零偏和加速度计零偏，初始值均为三维零向量；αk、βk和τk分别表示k时刻的航向安装夹角、俯仰安装夹角和测速仪刻度因子，初始值均设置为零。当惯性导航系统进行下一时刻导航解算时，利用(εg)k和对所采集的角速度和比力进行补偿，并利用αk、βk和τk对步骤四及步骤六中相应的变量进行更新。步骤九、对步骤一、二和三中的组合导航系统动态误差模型一步转移矩阵F(k，k-1)进行更新，将k+1的值赋给k，设置系统状态为18维零向量，然后返回到步骤四。采用本发明对惯性导航系统和测速仪的安装夹角进行估计，所得的航向安装夹角和俯仰安装夹角分别如图2和图3所示，其横坐标为时间(单位：秒)，纵坐标为安装夹角(单位：度)。从这两幅图可看出，航向安装夹角和俯仰安装夹角在估计开始后快速收敛，并分别于170秒和90秒后趋于常值，说明本发明提出的方法能够快速准确地对安装夹角进行测定。以上所述仅是本发明的优选实施方式，应当指出，对于本
技术领域：
的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进，或者对其中部分技术特征进行等同替换，这些改进和替换也应视为本发明的保护范围。当前第1页1 2 3