CN108088443B - 一种定位定向设备速度补偿方法 - Google Patents
一种定位定向设备速度补偿方法 Download PDFInfo
- Publication number
- CN108088443B CN108088443B CN201611037069.7A CN201611037069A CN108088443B CN 108088443 B CN108088443 B CN 108088443B CN 201611037069 A CN201611037069 A CN 201611037069A CN 108088443 B CN108088443 B CN 108088443B
- Authority
- CN
- China
- Prior art keywords
- error
- speed
- odometer
- navigation
- displacement
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 39
- 238000006073 displacement reaction Methods 0.000 claims abstract description 30
- 238000005259 measurement Methods 0.000 claims abstract description 23
- 238000001914 filtration Methods 0.000 claims abstract description 16
- 238000009434 installation Methods 0.000 claims abstract description 10
- 239000011159 matrix material Substances 0.000 claims description 26
- 230000009466 transformation Effects 0.000 claims description 6
- 230000003068 static effect Effects 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 238000003491 array Methods 0.000 claims description 2
- 238000009825 accumulation Methods 0.000 abstract description 3
- 230000009286 beneficial effect Effects 0.000 abstract description 3
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
- Other Investigation Or Analysis Of Materials By Electrical Means (AREA)
Abstract
本发明属于车辆定位导航技术领域,具体涉及一种定位定向设备速度补偿方法。本发明的速度补偿方法,包括以下步骤:建立卡尔曼滤波模型;***初始化;天向和水平位移分量计算;东向和北向位移分量计算;等效速度计算,并更新卡尔曼滤波模型信息;对***进行卡尔曼滤波计算;对惯导姿态阵、速度、位置误差进行修正,对里程计刻度系数和安装误差进行修正。本发明需要解决现有定位定向设备在航向机动过程中会出现定位定向误差的累积,最终影响导航定位的精度的技术问题,在计算速度量测信息时考虑了侧向速度,对其进行了有效的补偿,有利于减小在航向机动过程中出现的定位定向误差累积,提高了定位定向设备性能,保证了导航的精度。
Description
技术领域
本发明属于车辆定位导航技术领域,具体涉及一种定位定向设备速度补偿方法。
背景技术
在陆用车辆定位导航时,采用惯性导航与里程计组成定位定向设备是一种十分通用的手段,能够获得较为满意的性能。相比于采用卫星导航组成的组合导航***,具有全自主,全天候,不受外界信息干扰的优点。在广泛应用的同时,对定位定向设备性能要求也随之提升。
现有定位定向方法中,普遍采用里程计速度信息与惯导信息进行滤波计算,兼容零速修正技术,以期获得较高的定位定向精度。但是实际上,在航向机动过程中经常会出现意外的定位定向误差,定位定向误差会逐渐积累,最终影响导航定位的精度。
发明内容
本发明需要解决的技术问题为:现有定位定向方法中,定位定向设备在航向机动过程中会出现定位定向误差的累积,最终影响导航定位的精度。
本发明的技术方案如下所述:
一种定位定向设备速度补偿方法,包括以下步骤:
步骤1、建立卡尔曼滤波模型
定义坐标系如下:
n系:导航坐标系oxnynzn,为东北天地理坐标系,xn轴指向东,yn轴指向北,zn轴指向天;
b系:载体坐标系oxbybzb,为右前上右坐标系,xb轴指向载体的右方,yb轴指向载体的前方,zb轴指向载体的上方;
取状态向量X为
X=[δVe,δVn,δVu,φe,φn,φu,δλ,δL,δh,▽x,▽y,▽z,εx,εy,εz,φax,δKD,φaz,Δt,Rx,Ry,Rz]T
其中,
[δVe,δVn,δVu]为速度误差矢量,δVe为东向速度误差、δVn为北向速度误差、δVu为天向速度误差;
[φe,φn,φu]为姿态误差矢量,φe为东向姿态误差角、φn为北向姿态误差角、φu为天向姿态误差角;
[δλ,δL,δh]为位置误差矢量,δλ为经度误差、δL为纬度误差、δh为高度误差;
[▽x,▽y,▽z]为加速度计零偏矢量,▽x为xb轴加速度计零偏量、▽y为yb轴加速度计零偏量、▽z为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为***状态矩阵,
ωie为地球自转角速率,RM和RN分别为地球子午圈和卯酉圈半径,L为纬度,Vu为天向速度,Vn为北向速度,Ve为东向速度,fe、fn和fu分别为东北天向的等效加速度计测量值
量测方程为:
Z=HX+V
其中,Z为量测量,H为量测矩阵,V为量测噪声;
VD为里程计输出等效速度;
步骤2、***初始化
ΔS=KD·Npluse为标量,KD为里程计刻度系数,Npluse为里程计输出脉冲;
所述对准方法采用静基座对准或动基座对准方法;
步骤3、天向和水平位移分量计算
水平分量为:
步骤4、东向和北向位移分量计算
步骤5、等效速度计算,并更新卡尔曼滤波模型信息
计算滤波周期内的等效速度,具体公式如下:
更新***矩阵A、观测矩阵H,计算量测量Z;
步骤6、对***进行卡尔曼滤波计算;
步骤7、对惯导姿态阵、速度、位置误差进行修正,对里程计刻度系数和安装误差进行修正;
其中,姿态误差为φ=[X(3) X(4) X(5)]T,
惯导速度误差为δV=[X(0) X(1) X(2)]T,
则速度误差修正方法为Vn=Vn-δV;
惯导位置误差修正方法为
λ=λ-X(6)
L=L-X(7)
h=h-X(8)
里程计刻度系数误差为Kd=X(15),
则里程计刻度系数误差修正方法为KODO=KODO×(1+Kd);
各修正量使用完毕后相应的状态量应置为零。
本发明的有益效果为:
本发明的方法,在计算速度量测信息时充分考虑了在航向机动过程中造成的车辆的侧向速度,对其进行了有效的补偿,有利于减小定位定向设备在航向机动过程中出现的定位定向误差累积,提高了定位定向设备性能,保证了导航的精度。
具体实施方式
定义坐标系如下:
n系:导航坐标系oxnynzn,为东北天地理坐标系,xn轴指向东,yn轴指向北,zn轴指向天;
b系:载体坐标系oxbybzb,为右前上右坐标系,xb轴指向载体的右方,yb轴指向载体的前方,zb轴指向载体的上方。
本发明的方法具体包括以下步骤:
步骤1、建立卡尔曼滤波模型
取状态向量X为
X=[δVe,δVn,δVu,φe,φn,φu,δλ,δL,δh,▽x,▽y,▽z,εx,εy,εz,φax,δKD,φaz,Δt,Rx,Ry,Rz]T
其中,
[δVe,δVn,δVu]为速度误差矢量,δVe为东向速度误差、δVn为北向速度误差、δVu为天向速度误差;
[φe,φn,φu]为姿态误差矢量,φe为东向姿态误差角、φn为北向姿态误差角、φu为天向姿态误差角;
[δλ,δL,δh]为位置误差矢量,δλ为经度误差、δL为纬度误差、δh为高度误差;
[▽x,▽y,▽z]为加速度计零偏矢量,▽x为xb轴加速度计零偏量、▽y为yb轴加速度计零偏量、▽z为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为***状态矩阵,
ωie为地球自转角速率,RM和RN分别为地球子午圈和卯酉圈半径,L为纬度,Vu为天向速度,Vn为北向速度,Ve为东向速度,fe、fn和fu分别为东北天向的等效加速度计测量值
量测方程为:
Z=HX+V
其中,Z为量测量,H为量测矩阵,V为量测噪声;
VD为里程计输出等效速度;
步骤2、***初始化
ΔS=KD·Npluse为标量,KD为里程计刻度系数,Npluse为里程计输出脉冲。
所述对准方法采用静基座对准或动基座对准方法。
步骤3、天向和水平位移分量计算
水平分量为:
步骤4、东向和北向位移分量计算
步骤5、等效速度计算,并更新卡尔曼滤波模型信息
计算滤波周期内的等效速度,具体公式如下:
更新***矩阵A、观测矩阵H,计算量测量Z。
步骤6、对***进行卡尔曼滤波计算。
步骤7、对惯导姿态阵、速度、位置误差进行修正,对里程计刻度系数和安装误差进行修正。
其中,姿态误差为φ=[X(3) X(4) X(5)]T,
惯导速度误差为δV=[X(0) X(1) X(2)]T,
则速度误差修正方法为Vn=Vn-δV。
惯导位置误差修正方法为
λ=λ-X(6)
L=L-X(7)
h=h-X(8)
里程计刻度系数误差为Kd=X(15),
则里程计刻度系数误差修正方法为KODO=KODO×(1+Kd)。
各修正量使用完毕后相应的状态量应置为零。
Claims (1)
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为高度误差;
[ε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为***状态矩阵,
ωie为地球自转角速率,RM和RN分别为地球子午圈和卯酉圈半径,L为纬度,Vu为天向速度,Vn为北向速度,Ve为东向速度,fe、fn和fu分别为东北天向的等效加速度计测量值;
量测方程为:
Z=HX+V
其中,Z为量测量,H为量测矩阵,V为量测噪声;
VD为里程计输出等效速度;
步骤2、***初始化
ΔS=KD·Npluse为标量,KD为里程计刻度系数,Npluse为里程计输出脉冲;
对准方法采用静基座对准或动基座对准方法;
步骤3、天向和水平位移分量计算
水平分量为:
步骤4、东向和北向位移分量计算
步骤5、等效速度计算,并更新卡尔曼滤波模型信息
计算滤波周期内的等效速度,具体公式如下:
更新***状态矩阵A、量测矩阵H,计算量测量Z;
步骤6、对***进行卡尔曼滤波计算;
步骤7、对惯导姿态阵、速度、位置误差进行修正,对里程计刻度系数和安装误差进行修正;
其中,姿态误差为φ=[X(3) X(4) X(5)]T,
惯导速度误差为δV=[X(0) X(1) X(2)]T,
则速度误差修正方法为Vn=Vn-δV;
惯导位置误差修正方法为
λ=λ-X (6)
L=L-X (7)
h=h-X (8)
里程计刻度系数误差为δKD=X(15),
则里程计刻度系数误差修正方法为KD=KD×(1+δKD);
各修正量使用完毕后相应的状态量置为零。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201611037069.7A CN108088443B (zh) | 2016-11-23 | 2016-11-23 | 一种定位定向设备速度补偿方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201611037069.7A CN108088443B (zh) | 2016-11-23 | 2016-11-23 | 一种定位定向设备速度补偿方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108088443A CN108088443A (zh) | 2018-05-29 |
CN108088443B true CN108088443B (zh) | 2021-06-08 |
Family
ID=62169960
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201611037069.7A Active CN108088443B (zh) | 2016-11-23 | 2016-11-23 | 一种定位定向设备速度补偿方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108088443B (zh) |
Families Citing this family (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110657788B (zh) * | 2018-06-29 | 2022-07-15 | 北京自动化控制设备研究所 | 一种起重机轨道平顺性动态检测方法 |
CN109059913B (zh) * | 2018-08-27 | 2021-08-03 | 立得空间信息技术股份有限公司 | 一种用于车载导航***的零延迟组合导航初始化方法 |
CN110873563B (zh) * | 2018-08-30 | 2022-03-08 | 杭州海康机器人技术有限公司 | 一种云台姿态估计方法及装置 |
CN109143304B (zh) * | 2018-09-30 | 2020-12-29 | 百度在线网络技术(北京)有限公司 | 用于确定无人驾驶车辆位姿的方法和装置 |
CN109827572B (zh) * | 2019-03-12 | 2021-05-28 | 北京星网宇达科技股份有限公司 | 一种检测车位置预测的方法及装置 |
CN109974697B (zh) * | 2019-03-21 | 2022-07-26 | 中国船舶重工集团公司第七0七研究所 | 一种基于惯性***的高精度测绘方法 |
CN111912405A (zh) * | 2019-05-10 | 2020-11-10 | 中国人民解放***箭军工程大学 | 一种基于车载惯组与多普勒雷达的组合导航方法及*** |
CN110514221B (zh) * | 2019-08-13 | 2023-03-14 | 中国航空工业集团公司西安飞行自动控制研究所 | 一种里程仪初始参数快速计算方法 |
CN112284415B (zh) * | 2020-10-19 | 2023-01-03 | 株洲菲斯罗克光电科技股份有限公司 | 里程计标度误差标定方法、***及计算机存储介质 |
CN115773751B (zh) * | 2023-02-13 | 2023-07-18 | 中国航空工业集团公司西安飞行自动控制研究所 | 一种修正等效天向加计零位造成对准误差的方法 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101672650A (zh) * | 2009-09-29 | 2010-03-17 | 北京航空航天大学 | 一种电磁干扰环境下的定向定位导航*** |
CN103217157A (zh) * | 2012-01-18 | 2013-07-24 | 北京自动化控制设备研究所 | 一种惯导/里程计自主组合导航方法 |
CN104977004A (zh) * | 2015-07-13 | 2015-10-14 | 湖北航天技术研究院总体设计所 | 一种激光惯组与里程计组合导航方法及*** |
-
2016
- 2016-11-23 CN CN201611037069.7A patent/CN108088443B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101672650A (zh) * | 2009-09-29 | 2010-03-17 | 北京航空航天大学 | 一种电磁干扰环境下的定向定位导航*** |
CN103217157A (zh) * | 2012-01-18 | 2013-07-24 | 北京自动化控制设备研究所 | 一种惯导/里程计自主组合导航方法 |
CN104977004A (zh) * | 2015-07-13 | 2015-10-14 | 湖北航天技术研究院总体设计所 | 一种激光惯组与里程计组合导航方法及*** |
Non-Patent Citations (2)
Title |
---|
"Local Feedback Compensation Method for INS/GPS/OD Land Navigation System";Pengxiang Yang;《Proceedings of the 2008 IEEE International Conference on Information and Automation》;20081231;正文第1422-1426页 * |
"一种里程计辅助车载捷联惯导行进间对准方法";陈鸿跃等;《导弹与航天运载技术》;20130531(第5期);正文第44-50页 * |
Also Published As
Publication number | Publication date |
---|---|
CN108088443A (zh) | 2018-05-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108088443B (zh) | 一种定位定向设备速度补偿方法 | |
CN107655476B (zh) | 基于多信息融合补偿的行人高精度足部导航方法 | |
CN110031882B (zh) | 一种基于sins/dvl组合导航***的外量测信息补偿方法 | |
CN105203129B (zh) | 一种惯导装置初始对准方法 | |
CN109459044B (zh) | 一种gnss双天线辅助的车载mems惯导组合导航方法 | |
CN108051866B (zh) | 基于捷联惯性/gps组合辅助水平角运动隔离的重力测量方法 | |
CN108458725B (zh) | 捷联惯导***晃动基座上的***级标定方法 | |
CN107588769B (zh) | 一种车载捷联惯导、里程计及高程计组合导航方法 | |
CN101949703B (zh) | 一种捷联惯性/卫星组合导航滤波方法 | |
CN107806874B (zh) | 一种视觉辅助的捷联惯导极区初始对准方法 | |
CN104501838B (zh) | 捷联惯导***初始对准方法 | |
CN106767797B (zh) | 一种基于对偶四元数的惯性/gps组合导航方法 | |
CN109612460B (zh) | 一种基于静止修正的垂线偏差测量方法 | |
CN108132060B (zh) | 一种捷联惯导***无基准的***级标定方法 | |
CN111024074B (zh) | 一种基于递推最小二乘参数辨识的惯导速度误差确定方法 | |
CN106507913B (zh) | 用于管道测绘的组合定位方法 | |
CN114061623B (zh) | 一种基于双天线测向的惯性传感器零偏误差辨识方法 | |
CN112880669B (zh) | 一种航天器星光折射和单轴旋转调制惯性组合导航方法 | |
CN107677292B (zh) | 基于重力场模型的垂线偏差补偿方法 | |
CN107202578B (zh) | 一种基于mems技术的捷联式垂直陀螺仪解算方法 | |
CN109708663B (zh) | 基于空天飞机sins辅助的星敏感器在线标定方法 | |
CN111121766A (zh) | 一种基于星光矢量的天文与惯性组合导航方法 | |
CN103604428A (zh) | 基于高精度水平基准的星敏感器定位方法 | |
CN108225312B (zh) | 一种gnss/ins松组合中杆臂估计以及补偿方法 | |
CN111722295B (zh) | 一种水下捷联式重力测量数据处理方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |