CN114234972B - 用于惯性导航系统的惯性导航方法 - Google Patents

用于惯性导航系统的惯性导航方法 Download PDF

Info

Publication number
CN114234972B
CN114234972B CN202210189022.1A CN202210189022A CN114234972B CN 114234972 B CN114234972 B CN 114234972B CN 202210189022 A CN202210189022 A CN 202210189022A CN 114234972 B CN114234972 B CN 114234972B
Authority
CN
China
Prior art keywords
speed
error
axial direction
inertial navigation
equation
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
Application number
CN202210189022.1A
Other languages
English (en)
Other versions
CN114234972A (zh
Inventor
肖永平
满小三
刘勇
林佳
夏晓玲
王文艺
李元
李道坚
凌德智
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Changsha Jinwei Information Technology Co ltd
Original Assignee
Changsha Jinwei Information Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Changsha Jinwei Information Technology Co ltd filed Critical Changsha Jinwei Information Technology Co ltd
Priority to CN202210189022.1A priority Critical patent/CN114234972B/zh
Publication of CN114234972A publication Critical patent/CN114234972A/zh
Application granted granted Critical
Publication of CN114234972B publication Critical patent/CN114234972B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
    • 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
    • G01C21/165—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 combined with non-inertial navigation instruments
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种用于惯性导航系统的惯性导航方法，包括将目标车载组合导航系统安装到车辆上并初始化；构建基于向心力约束的Kalman滤波器；控制车体进行设定的运动；车体运动过程中Kalman滤波器收敛，完成用于惯性导航系统的惯性导航方法。本发明通过Kalman滤波器构建惯性导航的完整约束，增强了惯性导航方案的鲁棒性，能够有效避免现有技术的导航失效问题，同时本发明方法能够在车辆快速转弯、车体急速加减速等恶劣的场景提供形成较好的约束，有效保证了惯性导航系统的导航精度，而且本发明可靠性高、补偿精度高且简单方便。

Description

用于惯性导航系统的惯性导航方法
技术领域
本发明属于导航技术领域，具体涉及一种用于惯性导航系统的惯性导航方法。
背景技术
随着经济技术的发展和人们生活水平的提高，导航技术已经广泛应用于人们的生产和生活当中，给人们的生产和生活带来了无尽的便利。因此，保障导航过程的精确和稳定，就成为了导航系统最重要的任务之一。
近年来，伴随着自动驾驶、智能机器人、无人机为代表的智能交通技术的快速发展，城市峡谷场景、工业高楼层园区场景、隧道场景等复杂环境下导航的稳定性、可靠性成为急需解决重点和难点问题。
目前的车载组合导航系统，一般采用都是INS+GNSS的组合系统，而且能够不借助外部的传感器。车载组合导航系统在正常工作时，由卫星导航系统GNSS进行精确导航；而在卫星导航系统GNSS失效时（比如城市峡谷场景、工业高楼层园区场景、隧道场景等复杂环境下），车载组合导航系统则采用惯性导航系统INS进行临时的持续性导航。因此，在车载组合导航系统中，惯性导航系统INS的导航精度就显得非常重要。
但是，目前的车载组合导航系统中的惯性导航系统INS，一般都认为假设车辆在行驶过程中不存在漂移运动；但是，在车辆实际行驶中遇到急速转弯时，车辆会不可避免的产生漂移运动，这将会直接导致较大的误差。同时，现在的惯性导航系统INS在进行导航时并未考虑任何急加速和急减速的情形，这同样会导致导航过程中姿态预测和位置预测的偏差。
发明内容
本发明的目的在于提供一种可靠性高、补偿精度高且简单方便的用于惯性导航系统的惯性导航方法。
本发明提供的这种用于惯性导航系统的惯性导航方法，包括如下步骤：
S1. 将目标车载组合导航系统安装到车辆上，并对目标车载组合导航系统进行初始化；
S2. 构建基于向心力约束的Kalman滤波器；
S3. 控制车体进行设定的运动；
S4. 车体在步骤S3的运动过程中，Kalman滤波器收敛，完成用于惯性导航系统的惯性导航方法。
步骤S1所述的对目标车载组合导航系统进行初始化，具体为通过卫星导航系统，计算目标车载组合导航系统的初始的位置参数、初始的速度参数和初始的姿态参数。
步骤S2所述的构建基于向心力约束的Kalman滤波器，具体为以姿态误差、速度误差、位置误差、陀螺漂移和加速度计零偏作为状态变量并构建状态方程，以车辆侧向速度（车体系X轴速度）、车辆垂向速度（车体系Z轴速度）和向心力作为量测观测量并构建量测方程，从而构建基于向心力约束的Kalman滤波器。
所述的以姿态误差、速度误差、位置误差、陀螺漂移和加速度计零偏作为状态变量，具体包括如下步骤：
Kalman滤波器的状态变量为：
Figure 79830DEST_PATH_IMAGE001
式中
Figure 731392DEST_PATH_IMAGE002
为姿态误差，且
Figure 370183DEST_PATH_IMAGE003
，
Figure 765393DEST_PATH_IMAGE004
为x轴方向的姿态误差，
Figure 314186DEST_PATH_IMAGE005
为y轴方向的姿态误差，
Figure 605490DEST_PATH_IMAGE006
为z轴方向的姿态误差；
Figure 731577DEST_PATH_IMAGE007
为速度误差，且
Figure 664898DEST_PATH_IMAGE008
，
Figure 802619DEST_PATH_IMAGE009
为x轴方向的速度误差，
Figure 655037DEST_PATH_IMAGE010
为y轴方向的速度误差，
Figure 143787DEST_PATH_IMAGE011
为z轴方向的速度误差；
Figure 615220DEST_PATH_IMAGE012
为位置误差，且
Figure 873026DEST_PATH_IMAGE013
，
Figure 161925DEST_PATH_IMAGE014
为x轴方向的位置误差，
Figure 137971DEST_PATH_IMAGE015
为y轴方向的位置误差，
Figure 881936DEST_PATH_IMAGE016
为z轴方向的位置误差；
Figure 384462DEST_PATH_IMAGE017
为陀螺漂移，且
Figure 719628DEST_PATH_IMAGE018
，
Figure 182970DEST_PATH_IMAGE019
为x轴方向的陀螺漂移，
Figure 996206DEST_PATH_IMAGE020
为y轴方向的陀螺漂移，
Figure 228604DEST_PATH_IMAGE021
为z轴方向的陀螺漂移；
Figure 859305DEST_PATH_IMAGE022
为加速度计零偏，且
Figure 544365DEST_PATH_IMAGE023
，
Figure 161291DEST_PATH_IMAGE024
为x轴方向的加速度计零偏，
Figure 248195DEST_PATH_IMAGE025
为y轴方向的加速度计零偏，
Figure 49798DEST_PATH_IMAGE026
为z轴方向的加速度计零偏。
所述的构建状态方程，具体包括如下步骤：
Kalman滤波器的状态方程为：
Figure 222153DEST_PATH_IMAGE027
式中
Figure 377191DEST_PATH_IMAGE028
为
Figure 584182DEST_PATH_IMAGE002
的状态观测量；
Figure 556686DEST_PATH_IMAGE029
为
Figure 216337DEST_PATH_IMAGE007
的状态观测量；
Figure 175066DEST_PATH_IMAGE030
为
Figure 970984DEST_PATH_IMAGE012
的状态观测量；
Figure 114389DEST_PATH_IMAGE031
为
Figure 995757DEST_PATH_IMAGE017
的状态观测量；
Figure 758177DEST_PATH_IMAGE032
为
Figure 798814DEST_PATH_IMAGE022
的状态观测量；
Figure 722908DEST_PATH_IMAGE033
为姿态误差方程中姿态误差的相关部分，且
Figure 357152DEST_PATH_IMAGE034
，
Figure 516737DEST_PATH_IMAGE035
为地理坐标系相对惯性坐标系的旋转速度，×为叉乘运算符，数学意义为将符号×前面的3维向量变成3*3的反对称矩阵；
Figure 552827DEST_PATH_IMAGE036
为姿态误差方程中速度误差的相关部分，且
Figure 647822DEST_PATH_IMAGE037
，R Mh 为地球的长半轴半径，R Nh 为地球的短半轴半径，L为纬度；
Figure 769361DEST_PATH_IMAGE038
为姿态误差方程中位置误差的相关部分，且
Figure 998217DEST_PATH_IMAGE039
，M 1为第一中间变量，且
Figure 623234DEST_PATH_IMAGE040
，
Figure 154709DEST_PATH_IMAGE041
为地球自转角速度，M 2为第二中间变量，且
Figure 497966DEST_PATH_IMAGE042
，
Figure 530513DEST_PATH_IMAGE043
为导航坐标系下东北天速度中的E轴速度；
Figure 275615DEST_PATH_IMAGE044
为载体系到导航系的姿态转换矩阵；
Figure 977992DEST_PATH_IMAGE045
为速度误差方程中姿态误差的相关部分，且
Figure 808544DEST_PATH_IMAGE046
，
Figure 113624DEST_PATH_IMAGE047
为比力；M vv 为速度误差方程中速度误差的相关部分，且
Figure 713232DEST_PATH_IMAGE048
，v n 为导航坐标系下的东北天速度且
Figure 852090DEST_PATH_IMAGE049
，
Figure 28993DEST_PATH_IMAGE050
为导航坐标系下东北天速度中的N轴速度，
Figure 13130DEST_PATH_IMAGE051
为导航坐标系下东北天速度中的U轴速度，
Figure 732824DEST_PATH_IMAGE052
为地球自转角速率，
Figure 777003DEST_PATH_IMAGE053
为导航坐标系转动角速率；M vp 为速度误差方程中位置误差的相关部分，且
Figure 706782DEST_PATH_IMAGE054
；
Figure 494610DEST_PATH_IMAGE055
为位置误差方程中速度误差的相关部分，且
Figure 68810DEST_PATH_IMAGE056
；M pp 为位置误差方程中位置误差的相关部分，且
Figure 283891DEST_PATH_IMAGE057
；
Figure 435387DEST_PATH_IMAGE058
为陀螺白噪声；
Figure 292484DEST_PATH_IMAGE059
为加速度计白噪声。
所述的以车辆侧向速度（车体系X轴速度）、车辆垂向速度（车体系Z轴速度）和向心力作为量测观测量，具体包括如下步骤：
Kalman滤波器的量测观测量Z为
Figure 721192DEST_PATH_IMAGE060
；式中
Figure 107174DEST_PATH_IMAGE061
为载体系X轴速度；
Figure 886911DEST_PATH_IMAGE062
为载体系Z轴速度；A y 为载体系Y轴方向的向心力约束量测，且
Figure 141175DEST_PATH_IMAGE063
，
Figure 424388DEST_PATH_IMAGE064
为导航系到载体系的转换矩阵，
Figure 246851DEST_PATH_IMAGE065
为陀螺仪原始输出，
Figure 248305DEST_PATH_IMAGE066
为东北天坐标系下载体的速度，上标n表示东北天坐标系；
Figure 447205DEST_PATH_IMAGE067
的定义为：若*为向量则
Figure 709559DEST_PATH_IMAGE068
表示取向量*的第r个元素，若*为矩阵则
Figure 437344DEST_PATH_IMAGE067
表示取矩阵*的第r行元素；A z 为载体系Z轴方向的向心力约束量测，且
Figure 926094DEST_PATH_IMAGE069
。
所述的构建量测方程，具体包括如下步骤：
Kalman滤波器的量测方程为：
Figure 522160DEST_PATH_IMAGE070
式中H 1为第三中间变量，且
Figure 514387DEST_PATH_IMAGE071
，
Figure 537707DEST_PATH_IMAGE066
为东北天坐标系下载体的速度，
Figure 248174DEST_PATH_IMAGE065
为陀螺仪原始输出；H 2为第四中间变量且
Figure 523297DEST_PATH_IMAGE072
。
步骤S3所述的控制车体进行设定的运动，具体为控制车辆进行转弯、加减速等机动，从而辅助导航系统完成初始对准等。
本发明提供的这种用于惯性导航系统的惯性导航方法，通过Kalman滤波器构建惯性导航的完整约束，增强了惯性导航方案的鲁棒性，能够有效避免现有技术的导航失效问题，同时本发明方法能够在车辆快速转弯、车体急速加减速等恶劣的场景提供形成较好的约束，有效保证了惯性导航系统的导航精度，而且本发明可靠性高、补偿精度高且简单方便。
附图说明
图1为本发明方法的方法流程示意图。
图2为本发明方法中的车辆坐标示意图。
图3为本发明方法的实施例的跑车轨迹示意图。
图4为本发明方法的实施例中跑车轨迹在隧道部分的轨迹放大示意图。
具体实施方式
如图1所示为本发明方法的方法流程示意图：本发明提供的这种用于惯性导航系统的惯性导航方法，包括如下步骤：
S1. 将目标车载组合导航系统安装到车辆上，并对目标车载组合导航系统进行初始化；具体为通过卫星导航系统，计算目标车载组合导航系统的初始的位置参数、初始的速度参数和初始的姿态参数；
S2. 构建基于向心力约束的Kalman滤波器；
由于车辆转向行驶时，车辆的瞬时运动可以看成以某点为圆心的圆周运动；即使车辆直线行驶时，车辆运动也可以看成半径无穷大的圆周运动；车辆运动的圆心O位于车辆的x轴上（车体坐标系为右前上坐标系），示意图如图2所示；根据圆周运动理论，只有X轴方向有向心力，Y轴方向与Z轴方向的向心力为0，通过上述理论可构造向心力约束的量测A为
Figure 901189DEST_PATH_IMAGE073
；量测误差
Figure 95410DEST_PATH_IMAGE074
可表述为：
Figure 558752DEST_PATH_IMAGE075
式中
Figure 371988DEST_PATH_IMAGE076
为姿态误差，
Figure 604386DEST_PATH_IMAGE077
为陀螺漂移，
Figure 969508DEST_PATH_IMAGE078
为导航系到载体系的转换矩阵，
Figure 920147DEST_PATH_IMAGE079
为导航系下载体的速度，
Figure 537073DEST_PATH_IMAGE080
为陀螺仪原始输出；
因此，具体实施时，以姿态误差、速度误差、位置误差、陀螺漂移和加速度计零偏作为状态变量并构建状态方程，以车辆侧向速度（车体系X轴速度）、车辆垂向速度（车体系Z轴速度）和向心力作为量测观测量并构建量测方程，从而构建基于向心力约束的Kalman滤波器；
具体包括如下步骤：
Kalman滤波器的状态变量为：
Figure 623977DEST_PATH_IMAGE001
式中
Figure 425580DEST_PATH_IMAGE002
为姿态误差，且
Figure 597935DEST_PATH_IMAGE003
，
Figure 752973DEST_PATH_IMAGE004
为x轴方向的姿态误差，
Figure 959964DEST_PATH_IMAGE005
为y轴方向的姿态误差，
Figure 666889DEST_PATH_IMAGE006
为z轴方向的姿态误差；
Figure 326540DEST_PATH_IMAGE007
为速度误差，且
Figure 285269DEST_PATH_IMAGE008
，
Figure 346766DEST_PATH_IMAGE009
为x轴方向的速度误差，
Figure 490171DEST_PATH_IMAGE010
为y轴方向的速度误差，
Figure 371539DEST_PATH_IMAGE011
为z轴方向的速度误差；
Figure 133959DEST_PATH_IMAGE012
为位置误差，且
Figure 49962DEST_PATH_IMAGE013
，
Figure 364269DEST_PATH_IMAGE014
为x轴方向的位置误差，
Figure 732934DEST_PATH_IMAGE015
为y轴方向的位置误差，
Figure 33465DEST_PATH_IMAGE016
为z轴方向的位置误差；
Figure 928609DEST_PATH_IMAGE017
为陀螺漂移，且
Figure 289183DEST_PATH_IMAGE018
，
Figure 410723DEST_PATH_IMAGE019
为x轴方向的陀螺漂移，
Figure 249366DEST_PATH_IMAGE020
为y轴方向的陀螺漂移，
Figure 264595DEST_PATH_IMAGE021
为z轴方向的陀螺漂移；
Figure 61650DEST_PATH_IMAGE022
为加速度计零偏，且
Figure 404906DEST_PATH_IMAGE023
，
Figure 312819DEST_PATH_IMAGE024
为x轴方向的加速度计零偏，
Figure 57922DEST_PATH_IMAGE025
为y轴方向的加速度计零偏，
Figure 884932DEST_PATH_IMAGE026
为z轴方向的加速度计零偏；
Kalman滤波器的状态方程为：
Figure 715485DEST_PATH_IMAGE027
式中
Figure 895931DEST_PATH_IMAGE028
为
Figure 26698DEST_PATH_IMAGE002
的状态观测量；
Figure 899976DEST_PATH_IMAGE029
为
Figure 76879DEST_PATH_IMAGE007
的状态观测量；
Figure 61016DEST_PATH_IMAGE030
为
Figure 780710DEST_PATH_IMAGE012
的状态观测量；
Figure 949523DEST_PATH_IMAGE031
为
Figure 754668DEST_PATH_IMAGE017
的状态观测量；
Figure 542495DEST_PATH_IMAGE032
为
Figure 116696DEST_PATH_IMAGE022
的状态观测量；
Figure 456411DEST_PATH_IMAGE033
为姿态误差方程中姿态误差的相关部分，且
Figure 217693DEST_PATH_IMAGE034
，
Figure 809212DEST_PATH_IMAGE035
为地理坐标系相对惯性坐标系的旋转速度，×为叉乘运算符，数学意义为将符号×前面的3维向量变成3*3的反对称矩阵；
Figure 362553DEST_PATH_IMAGE036
为姿态误差方程中速度误差的相关部分，且
Figure 482956DEST_PATH_IMAGE037
，R Mh 为地球的长半轴半径，R Nh 为地球的短半轴半径，L为纬度；
Figure 262693DEST_PATH_IMAGE038
为姿态误差方程中位置误差的相关部分，且
Figure 516957DEST_PATH_IMAGE039
，M 1为第一中间变量，且
Figure 800170DEST_PATH_IMAGE040
，
Figure 622633DEST_PATH_IMAGE041
为地球自转角速度，M 2为第二中间变量，且
Figure 624087DEST_PATH_IMAGE042
，
Figure 557408DEST_PATH_IMAGE043
为导航坐标系下东北天速度中的E轴速度；
Figure 85341DEST_PATH_IMAGE044
为载体系到导航系的姿态转换矩阵；
Figure 813126DEST_PATH_IMAGE045
为速度误差方程中姿态误差的相关部分，且
Figure 36297DEST_PATH_IMAGE046
，
Figure 897942DEST_PATH_IMAGE047
为比力；M vv 为速度误差方程中速度误差的相关部分，且
Figure 155748DEST_PATH_IMAGE048
，v n 为导航坐标系下的东北天速度且
Figure 54434DEST_PATH_IMAGE049
，
Figure 30480DEST_PATH_IMAGE050
为导航坐标系下东北天速度中的N轴速度，
Figure 430238DEST_PATH_IMAGE051
为导航坐标系下东北天速度中的U轴速度，
Figure 542550DEST_PATH_IMAGE052
为地球自转角速率，
Figure 612137DEST_PATH_IMAGE053
为导航坐标系转动角速率；M vp 为速度误差方程中位置误差的相关部分，且
Figure 75480DEST_PATH_IMAGE054
；
Figure 36786DEST_PATH_IMAGE055
为位置误差方程中速度误差的相关部分，且
Figure 269185DEST_PATH_IMAGE056
；M pp 为位置误差方程中位置误差的相关部分，且
Figure 509673DEST_PATH_IMAGE057
；
Figure 584945DEST_PATH_IMAGE058
为陀螺白噪声；
Figure 201871DEST_PATH_IMAGE059
为加速度计白噪声；
Kalman滤波器的量测观测量Z为
Figure 288776DEST_PATH_IMAGE060
；式中
Figure 965745DEST_PATH_IMAGE061
为载体系X轴速度；
Figure 138100DEST_PATH_IMAGE062
为载体系Z轴速度；A y 为载体系Y轴方向的向心力约束量测，且
Figure 683351DEST_PATH_IMAGE063
，
Figure 624762DEST_PATH_IMAGE064
为导航系到载体系的转换矩阵，
Figure 472633DEST_PATH_IMAGE065
为陀螺仪原始输出，
Figure 132284DEST_PATH_IMAGE066
为东北天坐标系下载体的速度，上标n表示东北天坐标系；
Figure 950068DEST_PATH_IMAGE067
的定义为：若*为向量则
Figure 11564DEST_PATH_IMAGE068
表示取向量*的第r个元素，若*为矩阵则
Figure 30336DEST_PATH_IMAGE067
表示取矩阵*的第r行元素；A z 为载体系Z轴方向的向心力约束量测，且
Figure 36338DEST_PATH_IMAGE069
；
Kalman滤波器的量测方程为：
Figure 798758DEST_PATH_IMAGE070
式中H 1为第三中间变量，且
Figure 714761DEST_PATH_IMAGE071
，
Figure 904434DEST_PATH_IMAGE066
为东北天坐标系下载体的速度，
Figure 804257DEST_PATH_IMAGE065
为陀螺仪原始输出；H 2为第四中间变量且
Figure 229422DEST_PATH_IMAGE072
；
S3. 控制车体进行设定的运动；具体为控制车辆进行转弯、加减速等机动，从而辅助导航系统完成初始对准等；
S4. 车体在步骤S3的运动过程中，Kalman滤波器收敛，完成用于惯性导航系统的惯性导航方法。
本发明方法通过Kalman滤波器构建惯性导航的完整约束不仅可以约束现有方法的侧向速度和垂向速度，向心力约束更能在一定程度上提供前向速度的约束，使得导航系统能够在车辆快速转弯、车体急速加减速等恶劣的场景提供形成更好的约束，有效保证了惯性导航系统的导航精度。
以下，以一个长直线隧道的案例对本发明方法进行说明。
将两个模块（模块1采用本发明方法进行导航，模块2采用现有技术进行导航）固定在车辆后备厢进行跑车试验，且跑车轨迹中包含一个双向长度达3.5km的隧道，选取隧道出口点的位置与真实位置（以卫导rtk结果为参考）作差进行比较。
图3为整体的跑车轨迹示意图，其中虚线框部分为隧道部分；图4为跑车轨迹在隧道部分的轨迹放大示意图，其中左上角的虚线框部分对应于隧道口1，右下角的虚线框部对应于隧道口2。两种导航方法的误差数据如表1所示。
表1 两种导航方法的误差数据示意表
Figure 999932DEST_PATH_IMAGE082
从表1可以看出，本发明方法在隧道内的轨迹平滑度、隧道口的最大误差都优于现有方法。

Claims (5)

1.一种用于惯性导航系统的惯性导航方法，其特征在于包括如下步骤：
S1. 将目标车载组合导航系统安装到车辆上，并对目标车载组合导航系统进行初始化；
S2. 构建基于向心力约束的Kalman滤波器；具体为以姿态误差、速度误差、位置误差、陀螺漂移和加速度计零偏作为状态变量并构建状态方程，以车辆侧向速度、车辆垂向速度和向心力作为量测观测量并构建量测方程，从而构建基于向心力约束的Kalman滤波器；
所述的以车辆侧向速度、车辆垂向速度和向心力作为量测观测量，具体包括如下步骤：
Kalman滤波器的量测观测量Z为
Figure DEST_PATH_IMAGE002
；式中
Figure DEST_PATH_IMAGE004
为载体系X轴速度；
Figure DEST_PATH_IMAGE006
为载体系Z轴速度；A y 为载体系Y轴方向的向心力约束量测，且
Figure DEST_PATH_IMAGE008
，
Figure DEST_PATH_IMAGE010
为导航系到载体系的转换矩阵，
Figure DEST_PATH_IMAGE012
为陀螺仪原始输出，
Figure DEST_PATH_IMAGE014
为东北天坐标系下载体的速度，上标n表示东北天坐标系；
Figure DEST_PATH_IMAGE016
的定义为：若*为向量则
Figure DEST_PATH_IMAGE017
表示取向量*的第r个元素，若*为矩阵则
Figure DEST_PATH_IMAGE018
表示取矩阵*的第r行元素；A z 为载体系Z轴方向的向心力约束量测，且
Figure DEST_PATH_IMAGE020
；
其中，构建量测方程具体包括如下步骤：
Kalman滤波器的量测方程为：
Figure DEST_PATH_IMAGE022
式中H 1为第三中间变量，且
Figure DEST_PATH_IMAGE024
，
Figure 206616DEST_PATH_IMAGE010
为导航系到载体系的转换矩阵，
Figure 795860DEST_PATH_IMAGE014
为东北天坐标系下载体的速度，
Figure 106756DEST_PATH_IMAGE012
为陀螺仪原始输出；H 2为第四中间变量且
Figure DEST_PATH_IMAGE026
；X为Kalman滤波器的状态变量；×为叉乘运算符，数学意义为将符号×前面的3维向量变成3*3的反对称矩阵；
Figure 393643DEST_PATH_IMAGE017
的定义为：若*为向量则
Figure 739173DEST_PATH_IMAGE018
表示取向量*的第r个元素，若*为矩阵则
Figure 233740DEST_PATH_IMAGE016
表示取矩阵*的第r行元素；
S3. 控制车体进行设定的运动；
S4. 车体在步骤S3的运动过程中，Kalman滤波器收敛，完成用于惯性导航系统的惯性导航方法。
2.根据权利要求1所述的用于惯性导航系统的惯性导航方法，其特征在于步骤S1所述的对目标车载组合导航系统进行初始化，具体为通过卫星导航系统，计算目标车载组合导航系统的初始的位置参数、初始的速度参数和初始的姿态参数。
3.根据权利要求1或2所述的用于惯性导航系统的惯性导航方法，其特征在于所述的以姿态误差、速度误差、位置误差、陀螺漂移和加速度计零偏作为状态变量，具体包括如下步骤：
Kalman滤波器的状态变量为：
Figure DEST_PATH_IMAGE028
式中
Figure DEST_PATH_IMAGE030
为姿态误差，且
Figure DEST_PATH_IMAGE032
，
Figure DEST_PATH_IMAGE034
为x轴方向的姿态误差，
Figure DEST_PATH_IMAGE036
为y轴方向的姿态误差，
Figure DEST_PATH_IMAGE038
为z轴方向的姿态误差；
Figure DEST_PATH_IMAGE040
为速度误差，且
Figure DEST_PATH_IMAGE042
，
Figure DEST_PATH_IMAGE044
为x轴方向的速度误差，
Figure DEST_PATH_IMAGE046
为y轴方向的速度误差，
Figure DEST_PATH_IMAGE048
为z轴方向的速度误差；
Figure DEST_PATH_IMAGE050
为位置误差，且
Figure DEST_PATH_IMAGE052
，
Figure DEST_PATH_IMAGE054
为x轴方向的位置误差，
Figure DEST_PATH_IMAGE056
为y轴方向的位置误差，
Figure DEST_PATH_IMAGE058
为z轴方向的位置误差；
Figure DEST_PATH_IMAGE060
为陀螺漂移，且
Figure DEST_PATH_IMAGE062
，
Figure DEST_PATH_IMAGE064
为x轴方向的陀螺漂移，
Figure DEST_PATH_IMAGE066
为y轴方向的陀螺漂移，
Figure DEST_PATH_IMAGE068
为z轴方向的陀螺漂移；
Figure DEST_PATH_IMAGE070
为加速度计零偏，且
Figure DEST_PATH_IMAGE072
，
Figure DEST_PATH_IMAGE074
为x轴方向的加速度计零偏，
Figure DEST_PATH_IMAGE076
为y轴方向的加速度计零偏，
Figure DEST_PATH_IMAGE078
为z轴方向的加速度计零偏。
4.根据权利要求3所述的用于惯性导航系统的惯性导航方法，其特征在于所述的构建状态方程，具体包括如下步骤：
Kalman滤波器的状态方程为：
Figure DEST_PATH_IMAGE080
式中
Figure DEST_PATH_IMAGE082
为
Figure 451838DEST_PATH_IMAGE030
的状态观测量；
Figure DEST_PATH_IMAGE084
为
Figure 854001DEST_PATH_IMAGE040
的状态观测量；
Figure DEST_PATH_IMAGE086
为
Figure 677207DEST_PATH_IMAGE050
的状态观测量；
Figure DEST_PATH_IMAGE088
为
Figure 936150DEST_PATH_IMAGE060
的状态观测量；
Figure DEST_PATH_IMAGE090
为
Figure DEST_PATH_IMAGE091
的状态观测量；
Figure DEST_PATH_IMAGE093
为姿态误差方程中姿态误差的相关部分，且
Figure DEST_PATH_IMAGE095
，
Figure DEST_PATH_IMAGE097
为地理坐标系相对惯性坐标系的旋转速度，×为叉乘运算符，数学意义为将符号×前面的3维向量变成3*3的反对称矩阵；
Figure DEST_PATH_IMAGE099
为姿态误差方程中速度误差的相关部分，且
Figure DEST_PATH_IMAGE101
，R Mh 为地球的长半轴半径，R Nh 为地球的短半轴半径，L为纬度；
Figure DEST_PATH_IMAGE103
为姿态误差方程中位置误差的相关部分，且
Figure DEST_PATH_IMAGE105
，M 1为第一中间变量，且
Figure DEST_PATH_IMAGE107
，
Figure DEST_PATH_IMAGE109
为地球自转角速度，M 2为第二中间变量，且
Figure DEST_PATH_IMAGE111
，
Figure DEST_PATH_IMAGE113
为导航坐标系下东北天速度中的E轴速度；
Figure DEST_PATH_IMAGE115
为载体系到导航系的姿态转换矩阵；
Figure DEST_PATH_IMAGE117
为速度误差方程中姿态误差的相关部分，且
Figure DEST_PATH_IMAGE119
，
Figure DEST_PATH_IMAGE121
为比力；M vv 为速度误差方程中速度误差的相关部分，且
Figure DEST_PATH_IMAGE123
，v n 为导航坐标系下的东北天速度且
Figure DEST_PATH_IMAGE125
，
Figure DEST_PATH_IMAGE127
为导航坐标系下东北天速度中的N轴速度，
Figure DEST_PATH_IMAGE129
为导航坐标系下东北天速度中的U轴速度，
Figure DEST_PATH_IMAGE131
为地球自转角速率，
Figure DEST_PATH_IMAGE133
为导航坐标系转动角速率；M vp 为速度误差方程中位置误差的相关部分，且
Figure DEST_PATH_IMAGE135
；
Figure DEST_PATH_IMAGE137
为位置误差方程中速度误差的相关部分，且
Figure DEST_PATH_IMAGE139
；M pp 为位置误差方程中位置误差的相关部分，且
Figure DEST_PATH_IMAGE141
；
Figure DEST_PATH_IMAGE143
为陀螺白噪声；
Figure DEST_PATH_IMAGE145
为加速度计白噪声。
5.根据权利要求1所述的用于惯性导航系统的惯性导航方法，其特征在于步骤S3所述的控制车体进行设定的运动，具体为控制车辆进行机动，从而辅助导航系统完成初始对准；所述的机动包括转弯、加速和减速。
CN202210189022.1A 2022-03-01 2022-03-01 用于惯性导航系统的惯性导航方法 Active CN114234972B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210189022.1A CN114234972B (zh) 2022-03-01 2022-03-01 用于惯性导航系统的惯性导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210189022.1A CN114234972B (zh) 2022-03-01 2022-03-01 用于惯性导航系统的惯性导航方法

Publications (2)

Publication Number Publication Date
CN114234972A CN114234972A (zh) 2022-03-25
CN114234972B true CN114234972B (zh) 2022-05-24

Family

ID=80748281

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210189022.1A Active CN114234972B (zh) 2022-03-01 2022-03-01 用于惯性导航系统的惯性导航方法

Country Status (1)

Country Link
CN (1) CN114234972B (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107144284A (zh) * 2017-04-18 2017-09-08 东南大学 基于ckf滤波的车辆动力学模型辅助惯导组合导航方法
CN108828980A (zh) * 2018-05-15 2018-11-16 机科（山东）重工科技股份有限公司 一种过弯离心力的预测方法及系统
CN111678514A (zh) * 2020-06-09 2020-09-18 电子科技大学 一种基于载体运动条件约束和单轴旋转调制的车载自主导航方法
CN113805610A (zh) * 2020-06-12 2021-12-17 中移(苏州)软件技术有限公司 一种轨迹跟踪控制方法、装置及存储介质
WO2022007437A1 (zh) * 2020-07-04 2022-01-13 华为技术有限公司 传感器安装偏差角的标定方法、组合定位系统和车辆

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9453855B2 (en) * 2013-11-05 2016-09-27 ThinKom Soultions, Inc. System and method for calibrating an inertial measurement unit
US10775801B2 (en) * 2018-03-08 2020-09-15 Baidu Usa Llc Determining speeds along a path for autonomous driving vehicles

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107144284A (zh) * 2017-04-18 2017-09-08 东南大学 基于ckf滤波的车辆动力学模型辅助惯导组合导航方法
CN108828980A (zh) * 2018-05-15 2018-11-16 机科（山东）重工科技股份有限公司 一种过弯离心力的预测方法及系统
CN111678514A (zh) * 2020-06-09 2020-09-18 电子科技大学 一种基于载体运动条件约束和单轴旋转调制的车载自主导航方法
CN113805610A (zh) * 2020-06-12 2021-12-17 中移(苏州)软件技术有限公司 一种轨迹跟踪控制方法、装置及存储介质
WO2022007437A1 (zh) * 2020-07-04 2022-01-13 华为技术有限公司 传感器安装偏差角的标定方法、组合定位系统和车辆

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于车辆动力学模型辅助的低成本车载组合导航系统研究;房素素;《中国优秀硕士学位论文全文数据库工程科技Ⅱ辑》;20190915(第09期);第C035-208页 *

Also Published As

Publication number Publication date
CN114234972A (zh) 2022-03-25

Similar Documents

Publication Publication Date Title
CN109946732B (zh) 一种基于多传感器数据融合的无人车定位方法
Bonnifait et al. Data fusion of four ABS sensors and GPS for an enhanced localization of car-like vehicles
US8260483B2 (en) Guidance, navigation, and control system for a vehicle
CN108731670A (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN106476883A (zh) 车辆的行驶控制装置
CN107132563B (zh) 一种里程计结合双天线差分gnss的组合导航方法
Hogg et al. Algorithms and sensors for small robot path following
US20030216865A1 (en) Inertial navigation system for mobile objects with constraints
CN107144284A (zh) 基于ckf滤波的车辆动力学模型辅助惯导组合导航方法
Weinstein et al. Pose estimation of Ackerman steering vehicles for outdoors autonomous navigation
US7957898B2 (en) Computational scheme for MEMS inertial navigation system
CN104977002A (zh) 基于sins/双od的惯性组合导航系统及其导航方法
CN105987696A (zh) 一种低成本车辆自动驾驶设计实现方法
CN113670334A (zh) 一种飞行汽车的初始对准方法和装置
Liu et al. Multi-aided inertial navigation for ground vehicles in outdoor uneven environments
Niu et al. Camera-based lane-aided multi-information integration for land vehicle navigation
Hu et al. Kilometer sign positioning-aided INS/odometer integration for land vehicle autonomous navigation
Sasiadek et al. Sensor fusion for navigation of an autonomous unmanned aerial vehicle
CN114234972B (zh) 用于惯性导航系统的惯性导航方法
CN110864688A (zh) 一种用于车载方位开环水平姿态角闭环的航姿方法
CN116625394A (zh) 一种未知路况下机器人环境感知与路径寻优系统
US20230349699A1 (en) Absolute heading estimation with constrained motion
Chen et al. An integrated GNSS/INS/DR positioning strategy considering nonholonomic constraints for intelligent vehicle
CN211012986U (zh) 一种基于惯导技术的无人自主巡航车导航系统
Shaukat et al. Robust vehicle localization with gps dropouts

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