申请/专利权人：北京自动化控制设备研究所

申请日：2016-11-23

公开（公告）日：2021-06-08

公开（公告）号：CN108088443B

主分类号：G01C21/16(20060101)

分类号：G01C21/16(20060101);G01C21/20(20060101)

优先权：

专利状态码：有效-授权

法律状态：2021.06.08#授权;2018.06.22#实质审查的生效;2018.05.29#公开

摘要：本发明属于车辆定位导航技术领域，具体涉及一种定位定向设备速度补偿方法。本发明的速度补偿方法，包括以下步骤：建立卡尔曼滤波模型；系统初始化；天向和水平位移分量计算；东向和北向位移分量计算；等效速度计算，并更新卡尔曼滤波模型信息；对系统进行卡尔曼滤波计算；对惯导姿态阵、速度、位置误差进行修正，对里程计刻度系数和安装误差进行修正。本发明需要解决现有定位定向设备在航向机动过程中会出现定位定向误差的累积，最终影响导航定位的精度的技术问题，在计算速度量测信息时考虑了侧向速度，对其进行了有效的补偿，有利于减小在航向机动过程中出现的定位定向误差累积，提高了定位定向设备性能，保证了导航的精度。

主权项：1.一种定位定向设备速度补偿方法，其特征在于：包括以下步骤：步骤1、建立卡尔曼滤波模型定义坐标系如下：n系：导航坐标系oxnynzn，为东北天地理坐标系，xn轴指向东，yn轴指向北，zn轴指向天；b系：载体坐标系oxbybzb，为右前上右坐标系，xb轴指向载体的右方，yb轴指向载体的前方，zb轴指向载体的上方；取状态向量X为 其中，[δVe，δVn，δVu]为速度误差矢量，δVe为东向速度误差、δVn为北向速度误差、δVu为天向速度误差；[φe，φn，φu]为姿态误差矢量，φe为东向姿态误差角、φn为北向姿态误差角、φu为天向姿态误差角；[δλ，δL，δh]为位置误差矢量，δλ为经度误差、δL为纬度误差、δh为高度误差； 为加速度计零偏矢量，为xb轴加速度计零偏量、为yb轴加速度计零偏量、为zb轴加速度计零偏量；[εx，εy，εz]为陀螺漂移矢量，εx为xb轴陀螺漂移量、εy为yb轴陀螺漂移量、εz为zb轴陀螺漂移量；[φax，δKD，φaz]为里程计误差矢量，φax为xb轴安装误差、δKD为里程计刻度系数误差、φaz为zb轴安装误差；Δt：为惯导、里程计速度间的时间延迟；[Rx，Ry，Rz]为杆臂矢量，Rx为xb轴方向杆臂、Ry为yb轴方向杆臂、Rz为zb轴方向杆臂；状态方程为： 其中，G为系统噪声矩阵，W为系统噪声，A为系统状态矩阵， 为从载体坐标系oxbybzb到导航坐标系oxnynzn的坐标变换矩阵，通过导航解算得到；ωie为地球自转角速率，RM和RN分别为地球子午圈和卯酉圈半径，L为纬度，Vu为天向速度，Vn为北向速度，Ve为东向速度，fe、fn和fu分别为东北天向的等效加速度计测量值；量测方程为：Z＝HX+V其中，Z为量测量，H为量测矩阵，V为量测噪声； 分别为等效速度在东向、北向和天向的分量； 为坐标变换矩阵的第i行第j列元素，i＝1，2，3；j＝1，2，3；VD为里程计输出等效速度； 为陀螺测量值向量； 分别为东北天向速度的一阶导数；步骤2、系统初始化对惯导系统进行对准，并开始惯性导航解算，获得姿态角[θ，γ，ψ]T，速度和[λ，φ，h]T，同时通过里程计获得位移ΔS；ΔS＝KD·Npluse为标量，KD为里程计刻度系数，Npluse为里程计输出脉冲；对准方法采用静基座对准或动基座对准方法；步骤3、天向和水平位移分量计算根据步骤2导航解算得到的坐标变换矩阵计算里程计位移的天向分量和水平分量； 为3×3矩阵，则里程计输出位移的天向分量为： 水平分量为： 其中为的第3行第2列的元素；步骤4、东向和北向位移分量计算根据步骤2导航解算得到的速度计算里程计位移的东向分量和北向分量；具体方法如下： 其中，为程计位移的东向分量，为程计位移的北向分量；步骤5、等效速度计算，并更新卡尔曼滤波模型信息计算滤波周期内的等效速度，具体公式如下： 其中，为等效速度，ΔSnt为ΔSn在时刻t的值，Te为滤波周期，Tn为导航解算周期；更新系统状态矩阵A、量测矩阵H，计算量测量Z；步骤6、对系统进行卡尔曼滤波计算；步骤7、对惯导姿态阵、速度、位置误差进行修正，对里程计刻度系数和安装误差进行修正；其中，姿态误差为φ＝[X3X4X5]T，则惯导姿态阵误差修正公式为I为单位矩阵；惯导速度误差为δV＝[X0X1X2]T，则速度误差修正方法为Vn＝Vn-δV；惯导位置误差修正方法为λ＝λ-X6L＝L-X7h＝h-X8里程计刻度系数误差为δKD＝X15，则里程计刻度系数误差修正方法为KD＝KD×1+δKD；各修正量使用完毕后相应的状态量置为零。

全文数据：_种定位定向设备速度补偿方法技术领域[0001]本发明属于车辆定位导航技术领域，具体涉及一种定位定向设备速度补偿方法。背景技术[0002]在陆用车辆定位导航时，采用惯性导航与里程计组成定位定向设备是一种十分通用的手段，能够获得较为满意的性能。相比于采用卫星导航组成的组合导航系统，具有全自主，全天候，不受外界信息干扰的优点。在广泛应用的同时，对定位定向设备性能要求也随之提升。[0003]现有定位定向方法中，普遍采用里程计速度信息与惯导信息进行滤波计算，兼容零速修正技术，以期获得较高的定位定向精度。但是实际上，在航向机动过程中经常会出现意外的定位定向误差，定位定向误差会逐渐积累，最终影响导航定位的精度。发明内容[0004]本发明需要解决的技术问题为:现有定位定向方法中，定位定向设备在航向机动过程中会出现定位定向误差的累积，最终影响导航定位的精度。[0005]本发明的技术方案如下所述：[0006]—种定位定向设备速度补偿方法，包括以下步骤：[0007]步骤1、建立卡尔曼滤波模型[0008]定义坐标系如下：[0009]η系：导航坐标系oxnynzn，为东北天地理坐标系，xn轴指向东，yn轴指向北，zn轴指向天；[0010]b系：载体坐标系OXbybZb，为右前上右坐标系，xb轴指向载体的右方，yb轴指向载体的前方，Zb轴指向载体的上方；[0011]取状态向量X为[0012][0013]其中，[0014]为速度误差矢量，SVe3为东向速度误差、δνη为北向速度误差、SVu为天向速度误差；[0015]为姿态误差矢量，Φe为东向姿态误差角、Φη为北向姿态误差角、Φu为天向姿态误差角；[0016]_.为位置误差矢量，δλ为经度误差、5L为炜度误差、3h为高度误差；[0017]为加速度计零偏矢量，Vx为Xb轴加速度计零偏量、Vy为yb轴加速度计零偏量、Vz为Zb轴加速度计零偏量；[0018]为陀螺漂移矢量，ExSxb轴陀螺漂移量、εγ为yb轴陀螺漂移量、εζ为Zb轴陀螺漂移量；[0019]为里程计误差矢量，ΦΜ为Xb轴安装误差、为里程计刻度系数误差、Φ3Ζ为Zb轴安装误差；[0020]Δt:为惯导里程计速度间的时间延迟；[0021][Rx，Ry，Rj为杆臂矢量，Rx为Xb轴方向杆臂、Ry为yb轴方向杆臂、Rz为zb轴方向杆臂；[0022]状态方程为：[0023][0024]其中，G为系统噪声矩阵，W为系统噪声，A为系统状态矩阵，[0025][0026][0027][0028][0029][0030][0031][0032][0033][0034]为从载体坐标系oxbybZb到导航坐标系oxnynzn的坐标变换矩阵，通过导航结算得到；[0035]ωie为地球自转角速率，Rm和Rn分别为地球子午圈和卯酉圈半径，L为炜度，Vu为天向速度，Vn为北向速度，Ve3为东向速度，fhfr^Pfu分别为东北天向的等效加速度计测量值[0036]量测方程为：[0037][0038]其中，Z为量测量，H为量测矩阵，V为量测噪声；[0039][0040][0041]分别为等效速度在东向、北向和天向的分量；[0042]为坐标变换矩阵C?的第i行第j列元素[0043]Vd为里程计输出等效速度；[0044]为陀螺测量值向量；[0045]汙别为东北天向速度的一阶导数；[0046]步骤2、系统初始化[0047]对惯导系统进行对准，并开始惯性导航解算，获得姿态角速度和同时通过里程计获得位移ΔS;[0048]为标量，Kd为里程计刻度系数，Npluse为里程计输出脉冲；[0049]所述对准方法采用静基座对准或动基座对准方法；[0050]步骤3、天向和水平位移分量计算[0051]根据步骤2导航解算得到的坐标变换矩阵计算里程计位移的天向分量和水平分量；[0052]为3X3矩阵，则里程计输出位移的天向分量为：[0053][0054]水平分量为：[0055][0056]其中'为'的第3行第2列的元素；[0057]步骤4、东向和北向位移分量计算[0058]根据步骤2导航解算得到的速度计算里程计位移的东向分量和北向分量;具体方法如下：[0059][0060][0061]其中，为程计位移的东向分量，为程计位移的北向分量；[0062]步骤5、等效速度计算，并更新卡尔曼滤波模型信息[0063]计算滤波周期内的等效速度，具体公式如下：[0064][0065]其中，为等效速度，ASn⑴为ASn在时刻t的值为滤波周期，Tn为导航解算周期；[0066]更新系统矩阵A、观测矩阵H，计算量测量Z;[0067]步骤6、对系统进行卡尔曼滤波计算；[0068]步骤7、对惯导姿态阵、速度、位置误差进行修正，对里程计刻度系数和安装误差进行修正；[0069]其中，姿态误差为[0070]则惯导姿态阵误差修正公式为3单位矩阵；[0071]惯导速度误差为[0072]则速度误差修正方法为Vn=Vn4V;[0073]惯导位置误差修正方法为[0074][0075][0076][0077]里程计刻度系数误差为Kd=X15，[0078]则里程计刻度系数误差修正方法为[0079]各修正量使用完毕后相应的状态量应置为零。[0080]本发明的有益效果为：[0081]本发明的方法，在计算速度量测信息时充分考虑了在航向机动过程中造成的车辆的侧向速度，对其进行了有效的补偿，有利于减小定位定向设备在航向机动过程中出现的定位定向误差累积，提高了定位定向设备性能，保证了导航的精度。具体实施方式[0082]定义坐标系如下：[0083]η系：导航坐标系oxnynzn，为东北天地理坐标系，χη轴指向东，yn轴指向北，Zn轴指向天；[0084]b系:载体坐标系oxbybzb，为右前上右坐标系，Xb轴指向载体的右方，yb轴指向载体的前方，Zb轴指向载体的上方。[0085]本发明的方法具体包括以下步骤：[0086]步骤1、建立卡尔曼滤波模型[0087]取状态向量X为[0089]其中，[0090]为速度误差矢量，SVe3为东向速度误差、δνη为北向速度误差、SVu为天向速度误差；[0091]为姿态误差矢量，Φe为东向姿态误差角、Φη为北向姿态误差角、Φu为天向姿态误差角；[0092]为位置误差矢量，δλ为经度误差、5L为炜度误差、3h为高度误差；[0093]为加速度计零偏矢量，Vx为xb轴加速度计零偏量、Vy为yb轴加速度计零偏量、Vz为Zb轴加速度计零偏量；[0094]为陀螺漂移矢量，ex*Xb轴陀螺漂移量、^为％轴陀螺漂移量、^为抑轴陀螺漂移量；[0095]为里程计误差矢量，ΦΜ为Xb轴安装误差、为里程计刻度系数误差、Φ3Ζ为Zb轴安装误差；[0096]Δt:为惯导里程计速度间的时间延迟；[0097]为杆臂矢量，Rx为Xb轴方向杆臂、Ry为yb轴方向杆臂、Rz为zb轴方向杆臂。[0098]状态方程为：[0099][0100]其中，G为系统噪声矩阵，W为系统噪声，A为系统状杰矩阵，[0101][0102][0110]为从载体坐标系OXbybZbSJ导航坐标系OXnynZn的坐标变换矩阵，通过导航结算得到。[0111]ωie为地球自转角速率，Rm和Rn分别为地球子午圈和卯酉圈半径，L为炜度，Vu为天向速度，Vn为北向速度，Ve3为东向速度，fe、fn和fu分别为东北天向的等效加速度计测量值[0112]量测方程为：[0113][0114]其中，Z为量测量，H为量测矩阵，V为量测噪声；[0117]分别为等效速度在东向、北向和天向的分量；[0118]为坐标变换矩阵的第i行第j列元素[0119]Vd为里程计输出等效速度；[0120],为陀螺测量值向量；[0121]分别为东北天向速度的一阶导数；[0122]步骤2、系统初始化[0123]对惯导系统进行对准，并开始惯性导航解算，获得姿态角速度和[λ，Φ，h]τ，同时通过里程计获得位移ΔS。[0124],为标量，Kd为里程计刻度系数，Npiuse为里程计输出脉冲。[0125]所述对准方法采用静基座对准或动基座对准方法。[0126]步骤3、天向和水平位移分量计算[0127]根据步骤2导航解算得到的坐标变换矩阵计算里程计位移的天向分量和水平分量。[0128]为3X3矩阵，则里程计输出位移的天向分量为：[0129][0130]水平分量为：[0131][0132]其中为的第3行第2列的元素。[0133]步骤4、东向和北向位移分量计算[0134]根据步骤2导航解算得到的速度计算里程计位移的东向分量和北向分量。具体方法如下：[0135][0136][0137]其中~程计位移的东向分量，为程计位移的北向分量。[0138]步骤5、等效速度计算，并更新卡尔曼滤波模型信息[0139]计算滤波周期内的等效速度，具体公式如下：[0140][0141]其中，为等效速度，为ASn在时刻t的值％滤波周期，Tn为导航解算周期。[0142]更新系统矩阵A、观测矩阵H，计算量测量Z。[0143]步骤6、对系统进行卡尔曼滤波计算。[0144]步骤7、对惯导姿态阵、速度、位置误差进行修正，对里程计刻度系数和安装误差进行修正。[0145]其中，姿态误差为[0146]则惯导姿态阵误差修正公式为为单位矩阵。[0147]惯导速度误差为[0148]则速度误差修正方法为[0149]惯导位置误差修正方法为[0150][0151][0152][0153]里程计刻度系数误差为Kd=X15，[0154]则里程计刻度系数误差修正方法关[0155]各修正量使用完毕后相应的状态量应置为零。

权利要求：I.一种定位定向设备速度补偿方法，其特征在于:包括以下步骤：步骤1、建立卡尔曼滤波模型定义坐标系如下：η系:导航坐标系OXnynZn，为东北天地理坐标系，Xn轴指向东，yn轴指向北，Zn轴指向天；b系:载体坐标系oxbybZb，为右前上右坐标系，Xb轴指向载体的右方，yb轴指向载体的前方，Zb轴指向载体的上方；取状态向量X为RyRz]其中，PVe3JVnJVJ为速度误差矢量，δνθ为东向速度误差、δνη为北向速度误差、SVuS天向速度误差；[Φθ,Φη,ΦJ为姿态误差矢量，Φθ为东向姿态误差角、Φη为北向姿态误差角、为天向姿态误差角；[δλ，5L，3h]为位置误差矢量，δλ为经度误差、5L为炜度误差、3h为高度误差；为加速度计零偏矢量，Vx为Xb轴加速度计零偏量、Vy为yb轴加速度计零偏量、Vz为Zb轴加速度计零偏量；为陀螺漂移矢量，εχ为Xb轴陀螺漂移量、eySyb轴陀螺漂移量、ezSZb轴陀螺漂移量；为里程计误差矢量，为Xb轴安装误差、SKd为里程计刻度系数误差、Φ3Ζ为Zb轴安装误差；At:为惯导里程计速度间的时间延迟；%杆臂矢量，Rx为Xb轴方向杆臂、Ry为yb轴方向杆臂、Rz为Zb轴方向杆臂；状态方程为：其中，G为系统噪声矩阵，W为系统噪声，A为系统状态矩阵，为从载体坐标系OXbybZb到导航坐标系OXnynZn的坐标变换矩阵，通过导航结算得到；Oie为地球自转角速率，Rm和Rn分别为地球子午圈和卯酉圈半径，L为炜度，Vu为天向速度，Vn为北向速度，Ve3为东向速度，^、匕和匕分别为东北天向的等效加速度计测量值量测方程为：Z=HX+V其中，Z为量测量，H为量测矩阵，V为量测噪声；bv别为等效速度在东向、北向和天向的分量；€：,为坐标变换矩阵£|的第1行第」列元素，1=1，2,3;」=1，2,3;Vd为里程计输出等效速度；为陀螺测量值向量；分别为东北天向速度的一阶导数；步骤2、系统初始化对惯导系统进行对准，并开始惯性导航解算，获得姿态角[0，γ，βτ，速违和[λ，Φ，h]τ，同时通过里程计获得位移δs;ΔS=Kd·Npiuse为标量，Kd为里程计刻度系数，Npiuse为里程计输出脉冲；所述对准方法采用静基座对准或动基座对准方法；步骤3、天向和水平位移分量计算根据步骤2导航解算得到的坐标变换矩阵C；，计算里程计位移的天向分量和水平分量；为3X3矩阵，则里程计输出位移的天向分量为：水平分量为：其中q3,2为Cf的第3行第2列的元素；步骤4、东向和北向位移分量计算根据步骤2导航解算得到的速虔，计算里程计位移的东向分量和北向分量;具体方法如下：其中，Δ5：为程计位移的东向分量，为程计位移的北向分量；步骤5、等效速度计算，并更新卡尔曼滤波模型信息计算滤波周期内的等效速度，具体公式如下：其中，为等效速度，ΔSn⑴为ΛSn在时刻t的值L为滤波周期，Tn为导航解算周期；更新系统矩阵A、观测矩阵H，计算量测量Z;步骤6、对系统进行卡尔曼滤波计算；步骤7、对惯导姿态阵、速度、位置误差进行修正，对里程计刻度系数和安装误差进行修正；其中，姿态误差为则惯导姿态阵误差修正公式为，1为单位矩阵；惯导速度误差为.........则速度误差修正方法Svn=νη-δV;惯导位置误差修正方法为里程计刻度系数误差为Kd=X15，则里程计刻度系数误差修正方法为Kqdq=KqdqX1+Kd;各修正量使用完毕后相应的状态量应置为零。

百度查询： 北京自动化控制设备研究所 一种定位定向设备速度补偿方法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD