专利名称：：一种实现陆用惯性导航系统运动对准的方法
技术领域：
：本发明涉及一种实现陆用惯性导航系统运动对准的方法，适用于各类惯性导航系统的初始对准，属于惯性导航系统初始对准
技术领域：
。
背景技术：
：惯性导航系统是复杂的高精度机电综合系统，由于具有完全自主性的优点而广泛应用于陆海空天领域。惯性导航系统在转入导航状态前需要进行初始对准以提供必要的初始值，依据载体的运动情况划分，对准分为静基座对准和运动对准。同静基座对准相比，运动对准在缩短对准时间、提高对准精度和机动性等方面具有明显的优势。为实现惯性导航系统在运动过程中的对准，必须引入外部观测信息(包括速度、位置等)。对于陆用车辆而言，通常采用全球定位系统(GPS)作为辅助导航设备来实现惯性导航系统运动对准。目前已有的GPS辅助惯性导航系统实现运动对准的方法，即传统卡尔曼滤波方法，其原理是根据GPS输出的位置或速度与惯性导航系统的位置或速度之差作为观测量，在惯性导航系统动态误差模型的基础上，采用传统卡尔曼滤波方法对惯性导航系统的误差状态进行估计，对姿态矩阵进行开环或闭环方式反馈，实现运动对准。这种方法的缺点是在实际应用中，由于车辆的随机振动和行驶环境(如山区、隧道等)的影响，GPS的速度输出带有各种干扰噪声，且干扰信号统计特性不完全已知。对这些受干扰的观测量如果不采取有效的处理方法而直接进行卡尔曼滤波，则不仅会降低当前的估计精度，对于后续的估计效果也会产生影响，严重时会引起滤波发散，导致对准失败。
发明内容本发明的目的是为了克服已有技术存在的不足，提出一种实现陆用惯性导航系统运动对准的方法。本发明的基本原理是在惯性导航系统动态误差模型和观测方程的基础上，以GPS输出速度作为观测量，采用卡尔曼滤波对载车的加加速度进行估计，并判别GPS速度输出是否为野值。若GPS速度输出为野值，采用卡尔曼滤波平滑方法对当前状态进行递推；否则，采用主、从卡尔曼滤波器来实现惯性导航系统状态和噪声统计特性的同步估计，主滤波器对惯性导航系统的状态进行估计，其新息和方差作为从滤波器的观测量；从滤波器对主滤波器观测噪声的均值和方差进行估计，估计结果为下一次主滤波器的滤波提供噪声特性输入；从而实现陆用惯性导航系统的运动对准。本发明的目的是通过下述技术方案实现的。一种实现陆用惯性导航系统运动对准的方法，包括如下步骤步骤一、建立包括位置误差、速度误差、失准角和惯性器件误差漂移的惯性导航系统动态误差模型；动态误差模型为Φ角误差方程或Ψ角误差方程；在东北天坐标系下，动态误差模型表示如公式1所示。i:(0=F(0-v:(0+H;(0(1)式中，t为时间值，是正实数；χ(t)表示惯性导航系统动态误差模型的状态向量，由位置误差SP、速度误差δVn、失准角陀螺仪零偏￡g和加速度计零偏▽组成；w(t)表示惯性导航系统动态误差模型的系统噪声；F(t)为转移矩阵；表示惯性导航系统动态误差模型的状态向量的变化量。对该动态误差模型进行离散化处理，可得Xk=FHXkJwk(2)式中，k表示时间值，为正整数Jlri表示离散化的惯性导航系统的一步转移矩阵，Xk为离散化后的k时刻惯性导航系统的状态向量，Wk表示k时刻的惯性导航系统动态误差模型的系统噪声，是均值为零，方差为Q的白噪声序列，Q值根据实际应用环境人为设定，Q为正实数。步骤二、建立步骤一所述的惯性导航系统动态误差模型的观测方程，如公式3所示。式中，Yk表示k时刻的观测量；&_表示惯性导航系统的速度输出；厂/#表示经杆臂补偿后GPS的速度输出；Hk为k时刻的观测矩阵，Hk=；O3x3表示3阶时刻的惯性导航系统v『，计算水平合速度噪声序列，2值根据实际应用环境人为设定，β为正实数。步骤三、对GPS输出进行实时野值检测。通过GPS测量得到的当前时刻载车东向速度和北向速度Zk，并以此作为观测量，建立加加速度跟踪模型，如公式4所示。时刻水平加速度和水平加加速ζk表示离散后加加速度跟踪模式中，t为时间值，为正实数；v(t)表示水平合速度，a(t)和々(0分别表示水平加速度和水平加加速度；ζ(t)表示加加速度跟踪模型的系统噪声，是均值为零，方差为。的白对公式4进行离散化，离散化后的系统方程表示为式中，Vk表示k时刻水平合速度，ak和分别表示度，Gk^1表示离散化的加加速度跟踪模型的一步转移矩阵，型的系统噪声。加加速度跟踪模型的观测方程表示为0矩阵；I3x3表示3阶单位阵，O3x9表示3行9列的0矩阵；nk表示k动态误差模型的观测方程噪声。式中，Zk表示k时刻水平合速度的观测量，其计算公式为:Zk=V(vD2+(ν,Γ)2，Ik表示加加速度跟踪模型的观测方程噪声，是均值为零，方差为及的白噪声序列，及值根据实际应用环境人为设定，及为正实数。根据公式5和公式6，采用卡尔曼滤波器即可估计出k时刻水平合速度vk、相应的加速度ak和加加速度和。根据载车的类型和对准动态情况，预先设定阈值Jmax，如果μμ成立，则判断当前的观测量为有效值，执行步骤四；否则，判断其为野值，在该时刻做卡尔曼平滑，同时将k+Ι的值赋给k，然后重复步骤三。步骤四、对惯性导航系统动态误差模型的观测方程噪声ηk的均值Mf和方差Cf进行估计；根据步骤二中的惯性导航系统动态误差模型的观测方程得到的k时刻的观测量Yk以及观测矩阵Hk，采用主、从两个卡尔曼滤波器分别对k时刻惯性导航系统的状态xk、k时刻的观测噪声nk的均值Mf和方差Cf进行估计；具体为Mf和Cf的状态方程为表示主对角线元素；该状态方程的系统噪声h\r，是均值为零，方差为(^的白噪声序列，Qk值根据实际应用环境人为设定，Qk为正实数;Mi'%Hi1行1列的向量；I1为HI1行HI1列的单位阵；diag(Cf)为m2行1列的向量；I2为m2行m2列的单位阵。公式7对应的观测方程为式中，量测噪声是均值为零，方差为Rk的白噪声序列，Rk值根据实际应用环境人为设定，Rk为正实数。根据公式7和公式9，米用从卡尔曼滤波器即可估计出k时刻的观测噪声Ilk的均值Mf和方差Cf；从卡尔曼滤波器为经典卡尔曼滤波器。步骤五、对惯性导航系统的状态Xk进行估计；根据步骤一建立的惯性导航系统动态误差模型及步骤二中的观测方程，结合步骤四给出的观测噪声ηk的均值Mf和方差Cf的估计值，采用主卡尔曼滤波器对惯性导航系统动态误差模型的状态向量Xk进行估计。具体为主卡尔曼滤波器为经典卡尔曼滤波器的改进，计算过程如公式1014所示Pk＝Pk|k-1－KkHkPk|k-1(14)其中，^1W表示惯性导航系统动态误差模型的状态向量Xk的一步预测；Pkllri表示一步预测方差；么表示惯性导航系统动态误差模型的状态向量Xk的估计值；Pk表示估计方差；Kk表示滤波增益；if(l)表示步骤四中M^的估计值；对(2)表示Cf的估计值，其主对角线元素等于步骤四中diag(Cf)的估计值，非主对角线的元素为0。经过上述步骤即可得到k时刻惯性导航系统动态误差模型状态向量Xk的估计值L·，包含位置误差(δP)k，速度误差(δVn)k和失准角辨。利用这些误差估计结果对惯性导航系统的位置输出/Tw、速度输出Waw和姿态矩阵输出进行校正，通过公式1517即可得到修正后的载车的位置A、速度A、姿态矩阵(々>。其中，/Tv、和O分别表示k时刻惯性导航系统输出的位置、速度和姿态矩阵，是已知量；I3x3表示3阶单位阵；辨χ表示由辦构成的斜负对称阵。步骤六、对步骤一中的惯性导航系统动态误差模型一步转移矩阵Flri进行更新，同时将k+Ι的值赋给k，然后返同到步骤二。有益效果与已有的GPS辅助惯性导航系统实现运动对准的方法比较，本发明方法放宽了传统卡尔曼滤波方法对噪声特性的苛刻要求，解决了车辆运动过程中由于GPS包含各类未知噪声而造成对准精度不高及对准时间延长的缺陷；野值检测方法充分利用了运动对准过程中车辆的行驶特点，采用卡尔曼滤波方法实现了实时检测；噪声特性同样采用卡尔曼滤波实现实时估计，与现有技术相比，不需要对当前时刻之前的若干个时刻残差进行存储，使其更加适用于惯性导航系统在环境恶劣下的初始对准。图1为本发明的具体实施例中的载车运行轨迹示意图；图2为本发明的具体实施例中的俯仰角误差比较示意图；图3为本发明的具体实施例中的横滚角误差比较示意图；图4为本发明的具体实施例中的航向角误差比较示意图。具体实施例方式下面结合附图和具体实施例对本发明进行详细说明。本实施例中，将惯性导航系统固联在载车上，将GPS接收器安装在车顶。载车静止30秒进行粗对准后开始运动，运动轨迹如图1所示，其横坐标为纬度，纵坐标为经度。3个陀螺仪的随机漂移均为0.01/h,常值漂移均为0.02/h；3个加速度计随机漂移均为50μg，常值漂移均为100μg；初始纬度为39.800343；初始经度为116.166874；初始高程为40.87m；野值检测阈值.Jfflax=0.5；惯性导航系统动态误差模型的状态向量知为零向量；惯性导航系统动态误差模型的观测方程噪声nk的均值Mf和方差Cf的状态方程中的初始值i。R为零向量。其过程如下步骤一、建立包括位置误差、速度误差、失准角和惯性器件误差漂移的惯性导航系统动态误差模型；动态误差模型为Φ角误差方程或Ψ角误差方程；在东北天坐标系下，动态误差模型表示如公式1所示。动态误差模型为Φ角误差方程，F(t)表示为各个矩阵块的形式表示为式中，[PEPnPu]表示地球坐标系相对地理坐标系的转动角速率；L和h分别表示载体所处纬度和高程；Rm和Rt分别表示卯酉圈和子午圈的半径；R_表示卯酉圈的半径关于纬度求导，可表示如下Rmm=SRm/dL=6Roesin(Z)cos(Z)(20)其中，Rtl为地球半径，e为椭圆率。式中，[ωΕωηω]表示地球自转角速率在东北天方向的分量；[νΕvNVu]表示载体在东北天方向的运动速度；Rtt表示子午圈的半径关于纬度求导，可表示如下Ru=dR1/dL=2Roesin(I)cos(I)式中，[fEfNfU]表示加速度计测量的东向、北向和天向的比力,F25=Cnh式中，Cnb表示姿态矩阵.对该动态误差模型进行离散化处理，可得公式2⑵步骤二、建立步骤一所述的惯性导航系统动态误差模型的观测方程，如公式3所不。式中，Yk表示k时刻的观测量；FTs'表示惯性导航系统的速度输出；表示经杆臂补偿后GPS的速度输出；Hk为k时刻的观测矩阵，Hk=；O3x3表示3阶ΠΦΕ眸.T丰宗q阶你眸_η丰宗qτQ万Il的ΠΦΕ眸.η丰宗时刻的惯性导航系统。步骤三、对GPS输出进行实时野值检测。通过GPS测量得到的当前时刻载车东向速度和北向速度νZk，并以此作为观测量，建立加加速度跟踪模型，如公式4所示。式中，t为时间值，为正实数；v(t)表示水平合速度，a(t)和表示主对角线元素；该状态方程的系统噪声rRH^r，是均值为零，方差为(^的白噪声序列，Qk值根据实际应用环境人为设定，Qk为正实数;Mf为Hi1行1列的向量；I1为HI1行HI1列的单位阵；diag(Cf)为m2行1列的向量；I2为m2行m2列的单位阵。公式7对应的观测方程为式中，Zk表示k时刻水平合速度的观测量，其计算公式为-M=V(vD2+(νΓ')2，Ik表示加加速度跟踪模型的观测方程噪声，是均值为零，方差为及的白噪声序列，及值根据实际应用环境人为设定，及为正实数。根据公式5和公式6，采用卡尔曼滤波器即可估计出k时刻水平合速度vk、相应的加速度ak和加加速度和&。根据载车的类型和对准动态情况，预先设定阈值Jmax，如果|么|^/_成立，则判断当前的观测量为有效值，执行步骤四；否则，判断其为野值，在该时刻做卡尔曼平滑，同时将k+l的值赋给k，然后重复步骤三。步骤四、对惯性导航系统动态误差模型的观测方程噪声ηk的均值Mf和方差C^进行估计；根据步骤二中的惯性导航系统动态误差模型的观测方程得到的k时刻的观测量Yk以及观测矩阵Hk，采用主、从两个卡尔曼滤波器分别对k时刻惯性导航系统的状态xk、k时刻的观测噪声nk的均值Mf和方差Cf进行估计；具体为M〖和Cf的状态方程为对公式4进行离散化，离散化后的系统方程表示为式中，Vk表示k时刻水平合速度，ak和&分别表示k时刻水平加速度和水平加加速度，Gk—表示离散化的加加速度跟踪模型的一步转移矩阵，ζk表示离散后加加速度跟踪模型的系统噪声。加加速度跟踪模型的观测方程表示为式中，Ck表示残差序列的方差，满足Gif]TJdPPkllri的初始值分别为知和Po，均为人为设定值；此后，h和PklH由主卡尔曼滤波器提供；公式8可进一步整理为式中，量测噪声是均值为零，方差为Rk的白噪声序列，Rk值根据实际应用环境人为设定，Rk为正实数。根据公式7和公式9，采用从卡尔曼滤波器即可估计出k时刻的观测噪声Ilk的均值Mf和方差C^；从卡尔曼滤波器为经典卡尔曼滤波器。步骤五、对惯性导航系统的状态Xk进行估计；根据步骤一建立的惯性导航系统动态误差模型及步骤二中的观测方程，结合步骤四给出的观测噪声ηk的均值Mf和方差Cf的估计值，采用主卡尔曼滤波器对惯性导航系统动态误差模型的状态向量Xk进行估计。具体为主卡尔曼滤波器为经典卡尔曼滤波器的改进，计算过程如公式1014所示(12)(13)Pk=Pk丨H-KkHkPk丨Η(14)其中，JblM表示惯性导航系统动态误差模型的状态向量Xk的一步预测；Pkllri表示一步预测方差；;b表示惯性导航系统动态误差模型的状态向量Xk的估计值；Pk表示估计方差；Kk表示滤波增益；对(1)表示步骤四中Mf的估计值；对(2)表示C的估计值，其主对角线元素等于步骤四中diag(Cf)的估计值，非主对角线的元素为0。经过上述步骤即可得到k时刻惯性导航系统动态误差模型状态向量Xk的估计值L·，包含位置误差(δP)k，速度误差(δVn)k和失准角辦。利用这些误差估计结果对惯性导航系统的位置输出Zfw、速度输出’和姿态矩阵输出进行校正，通过公式1517即可得到修正后的载车的位置A、速度A、姿态矩阵(々>。(15)(16)其中，Cw、PTs'和(0分别表示k时刻惯性导航系统输出的位置、速度和姿态矩阵，是已知量；I3x3表示3阶单位阵；辦χ表示由辦构成的斜负对称阵。步骤六、对步骤一中的惯性导航系统动态误差模型一步转移矩阵Flri进行更新，同时将k+Ι的值赋给k，然后返回到步骤二。为了说明本发明的效果，采用传统卡尔曼滤波方法在相同的实验设置下进行试验，得到俯仰角误差比较图如图2所示，其横坐标为时间，纵坐标为俯仰角误差值；横滚角误差比较图如图3所示，其横坐标为时间，纵坐标为横滚角误差值；航向角误差比较图如图4所示，其横坐标为时间，纵坐标为航向角误差值。从这三幅图中可以看出，采用本发明后对准结果受环境扰动的影响较小，自收敛后(100秒-600秒)的均方差结果如表1所示。表1两种方法估计结果的均方差比较表中均方差的计算结果表明本发明在抑制野值及噪声特性的估计中能够起到有效的作用。以上所述仅是本发明的优选实施方式，应当指出，对于本
技术领域：
的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进，或者对其中部分技术特征进行等同替换，这些改进和替换也应视为本发明的保护范围。权利要求一种实现陆用惯性导航系统运动对准的方法，其特征在于包括如下步骤步骤一、建立包括位置误差、速度误差、失准角和惯性器件误差漂移的惯性导航系统动态误差模型；在东北天坐标系下，动态误差模型表示如公式1所示；<mrow><mover><mi>x</mi><mo>·</mo></mover><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mi>F</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mi>x</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>+</mo><mi>w</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></mrow>式中，t为时间值，是正实数；x(t)表示惯性导航系统动态误差模型的状态向量，由位置误差δP、速度误差δVn、失准角陀螺仪零偏δg和加速度计零偏组成；w(t)表示惯性导航系统动态误差模型的系统噪声；F(t)为转移矩阵；表示惯性导航系统动态误差模型的状态向量的变化量；对该动态误差模型进行离散化处理，可得xk＝Fk1xk1+wk(2)式中，k表示时间值，为正整数；Fk1表示离散化的惯性导航系统的一步转移矩阵，xk为离散化后的k时刻惯性导航系统的状态向量，wk表示k时刻的惯性导航系统动态误差模型的系统噪声，是均值为零，方差为Q的白噪声序列，Q值根据实际应用环境人为设定，Q为正实数；步骤二、建立步骤一所述的惯性导航系统动态误差模型的观测方程，如公式3所示；<mrow><msub><mi>Y</mi><mi>k</mi></msub><mo>=</mo><msubsup><mi>V</mi><mi>k</mi><mi>INS</mi></msubsup><mo>-</mo><msubsup><mi>V</mi><mi>k</mi><mi>GPS</mi></msubsup><mo>=</mo><msub><mi>H</mi><mi>k</mi></msub><msub><mi>x</mi><mi>k</mi></msub><mo>+</mo><msub><mi>η</mi><mi>k</mi></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>3</mn><mo>)</mo></mrow></mrow>式中，Yk表示k时刻的观测量；表示惯性导航系统的速度输出；表示经杆臂补偿后GPS的速度输出；Hk为k时刻的观测矩阵，Hk＝[O3×3I3×3O3×9]；O3×3表示3阶O矩阵；I3×3表示3阶单位阵，O3×9表示3行9列的O矩阵；ηk表示k时刻的惯性导航系统动态误差模型的观测方程噪声；步骤三、对GPS输出进行实时野值检测；通过GPS测量得到的当前时刻载车东向速度和北向速度计算水平合速度Zk，并以此作为观测量，建立加加速度跟踪模型，如公式4所示；<mrow><mfrac><mi>d</mi><mi>dt</mi></mfrac><mfencedopen='['close=']'><mtable><mtr><mtd><mi>v</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr><mtr><mtd><mi>a</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr><mtr><mtd><mover><mi>a</mi><mo>·</mo></mover><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr></mtable></mfenced><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><mn>0</mn></mtd><mtd><mn>1</mn></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>1</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd></mtr></mtable></mfenced><mfencedopen='['close=']'><mtable><mtr><mtd><mi>v</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr><mtr><mtd><mi>a</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr><mtr><mtd><mover><mi>a</mi><mo>·</mo></mover><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr></mtable></mfenced><mo>+</mo><mi>ζ</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>4</mn><mo>)</mo></mrow></mrow>式中，t为时间值，为正实数；v(t)表示水平合速度，a(t)和分别表示水平加速度和水平加加速度；ζ(t)表示加加速度跟踪模型的系统噪声，是均值为零，方差为的白噪声序列，值根据实际应用环境人为设定，为正实数；对公式4进行离散化，离散化后的系统方程表示为<mrow><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>v</mi><mi>k</mi></msub></mtd></mtr><mtr><mtd><msub><mi>a</mi><mi>k</mi></msub></mtd></mtr><mtr><mtd><msub><mover><mi>a</mi><mo>·</mo></mover><mi>k</mi></msub></mtd></mtr></mtable></mfenced><mo>=</mo><msub><mi>G</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>v</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub></mtd></mtr><mtr><mtd><msub><mi>a</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub></mtd></mtr><mtr><mtd><msub><mover><mi>a</mi><mo>·</mo></mover><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub></mtd></mtr></mtable></mfenced><mo>+</mo><msub><mi>ζ</mi><mi>k</mi></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>5</mn><mo>)</mo></mrow></mrow>式中，vk表示k时刻水平合速度，ak和分别表示k时刻水平加速度和水平加加速度，Gk1表示离散化的加加速度跟踪模型的一步转移矩阵，ζk表示离散后加加速度跟踪模型的系统噪声；加加速度跟踪模型的观测方程表示为<mrow><msub><mi>Z</mi><mi>k</mi></msub><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><mn>1</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd></mtr></mtable></mfenced><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>v</mi><mi>k</mi></msub></mtd></mtr><mtr><mtd><msub><mi>a</mi><mi>k</mi></msub></mtd></mtr><mtr><mtd><msub><mover><mi>a</mi><mo>·</mo></mover><mi>k</mi></msub></mtd></mtr></mtable></mfenced><mo>+</mo><msub><mi>ξ</mi><mi>k</mi></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>6</mn><mo>)</mo></mrow></mrow>式中，Zk表示k时刻水平合速度的观测量，其计算公式为ξk表示加加速度跟踪模型的观测方程噪声，是均值为零，方差为的白噪声序列，值根据实际应用环境人为设定，为正实数；根据公式5和公式6，采用卡尔曼滤波器即可估计出k时刻水平合速度vk、相应的加速度ak和加加速度和根据载车的类型和对准动态情况，预先设定阈值Jmax，如果成立，则判断当前的观测量为有效值，执行步骤四；否则，判断其为野值，在该时刻做卡尔曼平滑，同时将k+1的值赋给k，然后重复步骤三；步骤四、对惯性导航系统动态误差模型的观测方程噪声ηk的均值和方差进行估计；根据步骤二中的惯性导航系统动态误差模型的观测方程得到的k时刻的观测量Yk以及观测矩阵Hk，采用主、从两个卡尔曼滤波器分别对k时刻惯性导航系统的状态xk、k时刻的观测噪声ηk的均值和方差进行估计；具体为和的状态方程为<mrow><msubsup><mi>x</mi><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow><mi>R</mi></msubsup><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>I</mi><mn>1</mn></msub></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><msub><mi>I</mi><mn>2</mn></msub></mtd></mtr></mtable></mfenced><msubsup><mi>x</mi><mi>k</mi><mi>R</mi></msubsup><mo>+</mo><msubsup><mi>w</mi><mi>k</mi><mi>R</mi></msubsup><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>7</mn><mo>)</mo></mrow></mrow>式中，表示主对角线元素；该状态方程的系统噪声是均值为零，方差为QR的白噪声序列，QR值根据实际应用环境人为设定，QR为正实数；为m1行1列的向量；I1为m1行m1列的单位阵；为m2行1列的向量；I2为m2行m2列的单位阵；公式7对应的观测方程为<mrow><msubsup><mi>z</mi><mi>k</mi><mi>R</mi></msubsup><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>Y</mi><mi>k</mi></msub><mo>-</mo><msub><mi>H</mi><mi>k</mi></msub><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub></mtd></mtr><mtr><mtd><mi>diag</mi><mrow><mo>(</mo><msub><mi>C</mi><mi>k</mi></msub><mo>-</mo><msub><mi>H</mi><mi>k</mi></msub><msub><mi>P</mi><mrow><mi>k</mi><mo>|</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><msubsup><mi>H</mi><mi>k</mi><mi>T</mi></msubsup><mo>)</mo></mrow></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>8</mn><mo>)</mo></mrow></mrow>式中，Ck表示残差序列的方差，满足和Pk|k1的初始值分别为和P0，均为人为设定值；此后，和Pk|k1由主卡尔曼滤波器提供；公式8可进一步整理为<mrow><msubsup><mi>z</mi><mi>k</mi><mi>R</mi></msubsup><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><mi>I</mi></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mi>I</mi></mtd></mtr></mtable></mfenced><msubsup><mi>x</mi><mi>k</mi><mi>R</mi></msubsup><mo>+</mo><msubsup><mi>η</mi><mi>k</mi><mi>R</mi></msubsup><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>9</mn><mo>)</mo></mrow></mrow>式中，量测噪声是均值为零，方差为RR的白噪声序列，RR值根据实际应用环境人为设定，RR为正实数；根据公式7和公式9，采用从卡尔曼滤波器即可估计出k时刻的观测噪声ηk的均值和方差从卡尔曼滤波器为经典卡尔曼滤波器；步骤五、对惯性导航系统的状态xk进行估计；根据步骤一建立的惯性导航系统动态误差模型及步骤二中的观测方程，结合步骤四给出的观测噪声ηk的均值和方差的估计值，采用主卡尔曼滤波器对惯性导航系统动态误差模型的状态向量xk进行估计；具体为主卡尔曼滤波器为经典卡尔曼滤波器的改进，计算过程如公式10～14所示<mrow><msub><mover><mi>x</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>|</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>=</mo><msub><mi>F</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><msub><mover><mi>x</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>10</mn><mo>)</mo></mrow></mrow><mrow><msub><mi>P</mi><mrow><mi>k</mi><mo>|</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>=</mo><msub><mi>F</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><msub><mi>P</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><msubsup><mi>F</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow><mi>T</mi></msubsup><mo>+</mo><msub><mi>Q</mi><mi>k</mi></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>11</mn><mo>)</mo></mrow></mrow><mrow><msub><mi>K</mi><mi>k</mi></msub><mo>=</mo><msub><mi>P</mi><mrow><mi>k</mi><mo>|</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><msubsup><mi>H</mi><mi>k</mi><mi>T</mi></msubsup><msup><mrow><mo>[</mo><msub><mi>H</mi><mi>k</mi></msub><msub><mi>P</mi><mrow><mi>k</mi><mo>|</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><msubsup><mi>H</mi><mi>k</mi><mi>T</mi></msubsup><mo>+</mo><msubsup><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi><mi>R</mi></msubsup><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow><mo>]</mo></mrow><mrow><mo>-</mo><mn>1</mn></mrow></msup><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>12</mn><mo>)</mo></mrow></mrow><mrow><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub><mo>=</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>|</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>+</mo><msub><mi>K</mi><mi>k</mi></msub><mo>[</mo><msub><mi>Y</mi><mi>k</mi></msub><mo>-</mo><msub><mi>F</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><msub><mover><mi>x</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>|</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>-</mo><msubsup><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi><mi>R</mi></msubsup><mrow><mo>(</mo><mn>2</mn><mo>)</mo></mrow><mo>]</mo><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>13</mn><mo>)</mo></mrow></mrow>Pk＝Pk|k1KkHkPk|k1(14)其中，表示惯性导航系统动态误差模型的状态向量xk的一步预测；Pk|k1表示一步预测方差；表示惯性导航系统动态误差模型的状态向量xk的估计值；Pk表示估计方差；Kk表示滤波增益；表示步骤四中的估计值；表示的估计值，其主对角线元素等于步骤四中的估计值，非主对角线的元素为0；经过上述步骤即可得到k时刻惯性导航系统动态误差模型状态向量xk的估计值包含位置误差(δP)k，速度误差(δVn)k和失准角利用这些误差估计结果对惯性导航系统的位置输出速度输出和姿态矩阵输出进行校正，通过公式15～17即可得到修正后的载车的位置速度姿态矩阵<mrow><msub><mover><mi>P</mi><mo>~</mo></mover><mi>k</mi></msub><mo>=</mo><msubsup><mi>P</mi><mi>k</mi><mi>INS</mi></msubsup><mo>-</mo><msub><mrow><mo>(</mo><mi>δP</mi><mo>)</mo></mrow><mi>k</mi></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>15</mn><mo>)</mo></mrow></mrow><mrow><msub><mover><mi>V</mi><mo>~</mo></mover><mi>k</mi></msub><mo>=</mo><msubsup><mi>V</mi><mi>k</mi><mi>INS</mi></msubsup><mo>-</mo><msub><mrow><mo>(</mo><msup><mi>δV</mi><mi>n</mi></msup><mo>)</mo></mrow><mi>k</mi></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>16</mn><mo>)</mo></mrow></mrow>其中，和分别表示k时刻惯性导航系统输出的位置、速度和姿态矩阵，是已知量；I3×3表示3阶单位阵；表示由构成的斜负对称阵；步骤六、对步骤一中的惯性导航系统动态误差模型一步转移矩阵Fk1进行更新，同时将k+1的值赋给k，然后返回到步骤二。FSA00000190917000012.tif,FSA00000190917000013.tif,FSA00000190917000014.tif,FSA00000190917000015.tif,FSA00000190917000017.tif,FSA00000190917000018.tif,FSA00000190917000021.tif,FSA00000190917000022.tif,FSA00000190917000024.tif,FSA00000190917000025.tif,FSA00000190917000026.tif,FSA00000190917000027.tif,FSA00000190917000029.tif,FSA000001909170000211.tif,FSA000001909170000212.tif,FSA000001909170000213.tif,FSA000001909170000214.tif,FSA000001909170000215.tif,FSA00000190917000031.tif,FSA00000190917000032.tif,FSA00000190917000033.tif,FSA00000190917000034.tif,FSA00000190917000035.tif,FSA00000190917000036.tif,FSA00000190917000037.tif,FSA00000190917000039.tif,FSA000001909170000310.tif,FSA000001909170000311.tif,FSA000001909170000312.tif,FSA000001909170000313.tif,FSA000001909170000314.tif,FSA000001909170000316.tif,FSA000001909170000317.tif,FSA000001909170000318.tif,FSA000001909170000319.tif,FSA00000190917000041.tif,FSA00000190917000042.tif,FSA00000190917000043.tif,FSA00000190917000044.tif,FSA00000190917000045.tif,FSA000001909170000410.tif,FSA000001909170000411.tif,FSA000001909170000412.tif,FSA000001909170000413.tif,FSA000001909170000414.tif,FSA000001909170000415.tif,FSA000001909170000416.tif,FSA000001909170000417.tif,FSA000001909170000418.tif,FSA000001909170000419.tif,FSA000001909170000420.tif,FSA000001909170000421.tif,FSA000001909170000422.tif,FSA000001909170000423.tif,FSA000001909170000424.tif,FSA00000190917000053.tif,FSA00000190917000054.tif,FSA00000190917000055.tif,FSA00000190917000056.tif,FSA00000190917000057.tif1.一种实现陆用惯性导航系统运动对准的方法，其特征在于包括如下步骤步骤一、建立包括位置误差、速度误差、失准角和惯性器件误差漂移的惯性导航系统动态误差模型；在东北天坐标系下，动态误差模型表示如公式1所示；x(t)=F(t)x(t)+w(t)(1)式中，t为时间值，是正实数；功)=[评TSV"T(pJe/V/yqa)表示惯性导航系统动态误差模型的状态向量，由位置误差SP、速度误差、失准角9、陀螺仪零偏5g和加速度计零偏▽组成；w(t)表示惯性导航系统动态误差模型的系统噪声；F(t)为转移矩阵表示惯性导航系统动态误差模型的状态向量的变化量；对该动态误差模型进行离散化处理，可得xk=Fk-iXk-i+wk(2)式中，k表示时间值，为正整数；Fh表示离散化的惯性导航系统的一步转移矩阵，xk为离散化后的k时刻惯性导航系统的状态向量，wk表示k时刻的惯性导航系统动态误差模型的系统噪声，是均值为零，方差为Q的白噪声序列，Q值根据实际应用环境人为设定，Q为正实数；步骤二、建立步骤一所述的惯性导航系统动态误差模型的观测方程，如公式3所示；Yk=VlNS一VkGPS=HkXk+rjk(3)式中，Yk表示k时刻的观测量；K/w表示惯性导航系统的速度输出；表示经杆臂补偿后GPS的速度输出；Hk为k时刻的观测矩阵，Hk=;03X3表示3阶0矩眸.t丰宗qRfr^/ffrE^n丰宗■^千Qm的n妬眸.n丰宗t时刻的惯性导航系统云力态序列，0值根据实际应用环境人为设定，2为正实数；对公式4进行离散化，离散化后的系统方程表示为式中，vk表示k时刻水平合速度，ak和&分别表示Gh表示离散化的加加速度跟踪模型的一步转移矩阵，系统噪声；误差模型的观测方程噪声；步骤三、对GPS输出进行实时野值检测；通过GPS测量得到的当前时刻载车东向速度和北向速度力并以此作为观测量，建立加加速度跟踪模型，如公式4所示；式中，t为时间值，为正实数；v(t)表示水平合速度，a(t)和WO加加速度跟踪模型的观测方程表示为式中，zk表示k时刻水平合速度的观测量，其计算公式为:，ξk表示加加速度跟踪模型的观测方程噪声，是均值为零，方差为及的白噪声序列，及值根据实际应用环境人为设定，及为正实数；根据公式5和公式6，采用卡尔曼滤波器即可估计出k时刻水平合速度vk、相应的加速度ak和加加速度和&；根据载车的类型和对准动态情况，预先设定阈值J_，如果<成立，则判断当前的观测量为有效值，执行步骤四；否则，判断其为野值，在该时刻做卡尔曼平滑，同时将k+1的值赋给k，然后重复步骤三；步骤四、对惯性导航系统动态误差模型的观测方程噪声nk的均值Mf和方差Cf进行估根据步骤二中的惯性导航系统动态误差模型的观测方程得到的k时刻的观测量Yk以及观测矩阵Hk，采用主、从两个卡尔曼滤波器分别对k时刻惯性导航系统的状态xk、k时刻的观测噪声nk的均值Mf和方差Cf进行估计；具体为Mf和Cf的状态方程为式中，表示主对角线元素；该状态方程的系统噪声是均值为零，方差为Qk的白噪声序列，Qe值根据实际应用环境人为设定，Qk为正实数-M为mi行1列的向量山为mi行mi列的单位阵；diag(CAR)为m2行1列的向量；12为m2行m2列的单位阵；公式7对应的观测方程为式中，Ck表示残差序列的方差，满足GiJPPk^的初始值分别为h和P。，均为人为设定值；此后，h和Pkh由主卡尔曼滤波器提供；公式8可进一步整理为式中，量测噪声是均值为零，方差为RK的白噪声序列，RK值根据实际应用环境人为设定，RK为正实数；根据公式7和公式9，采用从卡尔曼滤波器即可估计出k时刻的观测噪声nk的均值Mf和方差C；从卡尔曼滤波器为经典卡尔曼滤波器；步骤五、对惯性导航系统的状态xk进行估计；根据步骤一建立的惯性导航系统动态误差模型及步骤二中的观测方程，结合步骤四给出的观测噪声nk的均值Mf和方差cf的估计值，采用主卡尔曼滤波器对惯性导航系统动态误差模型的状态向量Xk进行估计；具体为主卡尔曼滤波器为经典卡尔曼滤波器的改进，计算过程如公式1014所示其中，如w表示惯性导航系统动态误差模型的状态向量xk的一步预测；Pk^表示一步预测方差；么表示惯性导航系统动态误差模型的状态向量xk的估计值；Pk表示估计方差；Kk表示滤波增益；if⑴表示步骤四中M，的估计值；#(2)表示Cf的估计值，其主对角线元素等于步骤四中diag(Cf)的估计值，非主对角线的元素为0；经过上述步骤即可得到k时刻惯性导航系统动态误差模型状态向量xk的估计值jb，包含位置误差(SP)k，速度误差(SVn)k和失准角辦；利用这些误差估计结果对惯性导航系统的位置输出、速度输出F/MS'和姿态矩阵(CO输出进行校正，通过公式1517即可得到修正后的载车的位置月、速度巧、姿态矩阵(Q>；其中，Pk'NS、「广和(C》分别表示k时刻惯性导航系统输出的位置、速度和姿态矩阵，是已知量；I3X3表示3阶单位阵；辦x表示由辦构成的斜负对称阵；步骤六、对步骤一中的惯性导航系统动态误差模型一步转移矩阵Fh进行更新，同时将k+1的值赋给k，然后返回到步骤二。2.如权利要求1所述的一种实现陆用惯性导航系统运动对准的方法，其特征在于步骤一中所述的动态误差模型为角误差方程或V角误差方程。全文摘要本发明涉及一种实现陆用惯性导航系统运动对准的方法，属于惯性导航系统初始对准
技术领域：
。本发明在惯性导航系统动态误差模型和观测方程的基础上，以GPS输出速度作为观测量，采用卡尔曼滤波对载车的加加速度进行估计，并判别GPS速度输出是否为野值。若GPS速度输出为野值，采用卡尔曼滤波平滑方法对当前状态进行递推；否则，采用主、从卡尔曼滤波器来实现惯性导航系统状态和噪声统计特性的同步估计，主滤波器对惯性导航系统的状态进行估计，其新息和方差作为从滤波器的观测量；从滤波器对主滤波器观测噪声的均值和方差进行估计，估计结果为下一次主滤波器的滤波提供噪声特性输入；从而实现陆用惯性导航系统的运动对准。文档编号G01C21/16GK101900573SQ201010227110公开日2010年12月1日申请日期2010年7月15日优先权日2010年7月15日发明者付梦印,林杰,王清哲,肖烜,邓志红申请人:北京理工大学