CN110346824B - 一种车辆导航方法、系统、装置及可读存储介质 - Google Patents

一种车辆导航方法、系统、装置及可读存储介质 Download PDF

Info

Publication number
CN110346824B
CN110346824B CN201910635871.3A CN201910635871A CN110346824B CN 110346824 B CN110346824 B CN 110346824B CN 201910635871 A CN201910635871 A CN 201910635871A CN 110346824 B CN110346824 B CN 110346824B
Authority
CN
China
Prior art keywords
vehicle
navigation
zero
speed
information
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
CN201910635871.3A
Other languages
English (en)
Other versions
CN110346824A (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.)
Guangdong University of Technology
Original Assignee
Guangdong University of Technology
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 Guangdong University of Technology filed Critical Guangdong University of Technology
Priority to CN201910635871.3A priority Critical patent/CN110346824B/zh
Publication of CN110346824A publication Critical patent/CN110346824A/zh
Application granted granted Critical
Publication of CN110346824B publication Critical patent/CN110346824B/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/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
    • 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/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

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

本申请公开了一种车辆导航方法、系统、装置及可读存储介质，包括：判断GPS卫星信号是否达到有效强度；如果否，判断车辆是否运动；如果否，根据车辆的状态信息建立第一系统模型并进行零速修正，以辅助车辆的定位导航；如果是，根据状态信息和约束条件建立第二系统模型并进行零速修正，以辅助车辆的定位导航；其中，状态信息包括位置信息、速度信息、姿态信息、三维杆臂信息、加表零偏误差和陀螺仪漂移误差；约束条件具体包括：车辆的横向速度与竖直于地面的法向速度均为零。本发明中获取状态信息不需借助额外的传感器，在GPS信号长时间失锁情况下，车辆零速修正不需停车，在运动、静止两种状态下均可实现可靠、有效的定位导航。

Description

一种车辆导航方法、系统、装置及可读存储介质
技术领域
本发明涉及车载导航系统领域，特别涉及一种车辆导航方法、系统、装置及可读存储介质。
背景技术
目前，车载导航的方法，通常是根据卫星导航系统与惯导系统间具有很好的互补性，进行INS/GPS组合导航。组合导航可以充分发挥两种系统的优点，利用GPS(GlobalPosition System，全球定位系统)抑制INS的累积误差，同时使系统具有INS(InertialNavigation System，惯性导航系统)稳定性、高数据输出率以及GPS系统的高精度。对于INS/GPS系统来说，正常工作情况下可以利用卡尔曼滤波算法对INS解算得到的数据和高精度GPS接收机的输出进行处理得到最优的状态误差估计值，进而来修正惯性导航系统的位置、速度输出，但系统长时间运行的精度更多依赖获取GPS观测信息，当山区、城市等复杂路况环境下卫星导航信号衰弱、受到干扰断续甚至GPS信号不能覆盖出现失效而无法实现数据融合时，此时由于INS误差随时间积累且得不到校正，导航系统的精度会迅速下降，必须依靠零速修正技术来抑制惯性导航误差。
但是，传统的零速修正技术只能在车辆静止的状态下提供约束辅助信息，对于动态场景不适用，要求车辆每隔一定时间停车一次，频繁停车会比较容易造成车辆的机动性能下降，并且车辆的高频抖动容易导致车辆的运动状态的判断错误。
因此，如何提供一种解决上述技术问题的方案是目前本领域技术人员需要解决的问题。
发明内容
有鉴于此，本发明的目的在于提供一种不需频繁停车、适用于动态场景的车辆导航方法、系统、装置及可读存储介质。其具体方案如下：
一种车辆导航方法，包括：
判断GPS卫星信号是否达到有效强度；
如果是，通过INS系统与GPS系统的组合模式对车辆定位导航；
如果否，判断所述车辆是否运动；
如果否，根据所述车辆的状态信息建立第一系统模型，然后利用所述第一系统模型进行零速修正，以辅助所述车辆的定位导航；
如果是，根据所述状态信息和约束条件建立第二系统模型，然后利用所述第二系统模型进行零速修正，以辅助所述车辆的定位导航；
其中，所述状态信息包括位置信息、速度信息、姿态信息、IMU装置的几何中心到GPS接收机天线的相位中心的三维杆臂信息、加表零偏误差和陀螺仪漂移误差；所述约束条件具体包括：所述车辆的横向速度与竖直于地面的法向速度均为零。
优选的，所述利用所述第一系统模型进行零速修正的过程，具体包括：
通过卡尔曼滤波对所述第一系统模型进行状态估计，以进行零速修正；
相应的，所述利用所述第二系统模型进行零速修正的过程，具体包括：
通过卡尔曼滤波对所述第二系统模型进行状态估计，以进行零速修正。
优选的，所述车辆导航方法还包括：
当所述GPS卫星信号达到所述有效强度，训练对应所述车辆状态信息的神经网络模型；
相应的，所述利用所述第一系统模型进行零速修正的过程，具体包括：
通过所述神经网络模型对所述第一系统模型进行状态估计，以进行零速修正；
相应的，所述利用所述第二系统模型进行零速修正的过程，具体包括：
通过所述神经网络模型对所述第二系统模型进行状态估计，以进行零速修正。
优选的，所述通过INS系统与GPS系统的组合模式对车辆定位导航的过程，具体包括：
通过INS系统与GPS系统的松组合模式对车辆定位导航。
优选的，所述通过INS系统与GPS系统的组合模式对车辆定位导航的过程，具体包括：
通过INS系统与GPS系统的紧组合模式对车辆定位导航。
优选的，所述判断所述车辆是否运动的过程，具体包括：
判断所述车辆的比力的幅值和方差、角速度的幅值、行车速度是否均达到相应的阈值条件；
如果是，则判定所述车辆未运动，如果否，则判定所述车辆运动。
相应的，本发明还公开了一种车辆导航系统，包括：
第一判断模块，用于判断GPS卫星信号是否达到有效强度；如果是，触发第一导航模块，如果否，触发第二判断模块；
所述第一导航模块，用于通过INS系统与GPS系统的组合模式对车辆定位导航；
所述第二判断模块，用于判断所述车辆是否运动；如果否，触发第二导航模块，如果是，触发第三导航模块；
所述第二导航模块，用于根据所述车辆的状态信息建立第一系统模型，然后利用所述第一系统模型进行零速修正，以辅助所述车辆的定位导航；
所述第三导航模块，用于根据所述状态信息和约束条件建立第二系统模型，然后利用所述第二系统模型进行零速修正，以辅助所述车辆的定位导航；
其中，所述状态信息包括位置信息、速度信息、姿态信息、IMU装置的几何中心到GPS接收机天线的相位中心的三维杆臂信息、加表零偏误差和陀螺仪漂移误差；所述约束条件具体包括：所述车辆的横向速度与竖直于地面的法向速度均为零。
优选的，所述第二判断模块具体用于：判断所述车辆的比力的幅值和方差、角速度的幅值、行车速度是否均达到相应的阈值条件；如果是，则判定所述车辆未运动并触发所述第二导航模块，如果否，则判定所述车辆运动并触发所述第三导航模块。
相应的，本发明还公开了一种车辆导航装置，包括：
存储器，用于存储计算机程序；
处理器，用于执行所述计算机程序时实现如上文任一项所述车辆导航方法的步骤。
相应的，本发明还公开了一种可读存储介质，所述可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如上文任一项所述车辆导航方法的步骤。
本发明公开了一种车辆导航方法，包括：判断GPS卫星信号是否达到有效强度；如果是，通过INS系统与GPS系统的组合模式对车辆定位导航；如果否，判断所述车辆是否运动；如果否，根据所述车辆的状态信息建立第一系统模型，然后利用所述第一系统模型进行零速修正，以辅助所述车辆的定位导航；如果是，根据所述状态信息和约束条件建立第二系统模型，然后利用所述第二系统模型进行零速修正，以辅助所述车辆的定位导航；其中，所述状态信息包括位置信息、速度信息、姿态信息、IMU装置的几何中心到GPS接收机天线的相位中心的三维杆臂信息、加表零偏误差和陀螺仪漂移误差；所述约束条件具体包括：所述车辆的横向速度与竖直于地面的法向速度均为零。本发明中状态信息包括位置信息、速度信息、姿态信息、三维杆臂信息、加表零偏误差和陀螺仪漂移误差，不需借助额外的传感器，即可建立第一系统模型、结合约束条件建立第二系统模型进行零速修正；在GPS信号长时间失锁情况下，车辆不需停车进行零速修正，在运动、静止两种状态下均可实现可靠、有效的定位导航。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。
图1为本发明实施例中一种车辆导航方法的步骤流程图；
图2为本发明实施例中一种松组合的原理示意图；
图3为本发明实施例中一种具体的车辆导航方法的步骤流程图；
图4为本发明实施例中一种零速修正的原理示意图；
图5为本发明实施例中另一种零速修正的原理示意图；
图6为本发明实施例中另一种具体的车辆导航方法的步骤流程图；
图7为本发明实施例中一种车辆导航系统的结构分布图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明实施例公开了一种车辆导航方法，参见图1所示，包括：
S11：判断GPS卫星信号是否达到有效强度；
S12：如果是，通过INS系统与GPS系统的组合模式对车辆定位导航；
具体的，这里的组合模式包括INS系统和GPS系统的松组合模式、INS系统和GPS系统的紧组合模式，根据所需的目标精度进行相应的组合模式选择。
图2所示为松组合模式的原理示意图，在以松组合模式进行定位导航时，一般都将GPS惯性导航系统作为主要的导航系统。在实际应用中通常应用线性卡尔曼滤波进行数据融合，在实现组合导航前需要进行系统初始化，选择适当的模型，并进行模型一步预测。通常选择惯导误差方程作为组合导航系统的状态方程，而以GPS解算得到的位置和速度与以INS解算得到的位置和速度之差作为外部观测值输入到卡尔曼滤波器中；卡尔曼滤波器将解算出来的INS状态误差修正信息反馈并修正惯导的导航输出参数，即用来修正INS的累积误差，抑制INS误差的发散；与此同时卡尔曼滤波器还将计算得到的加表、陀螺仪常值零偏反馈并修正惯性导航的原始输出，实现对惯导的在线标定。
具体的，采用紧组合模式时，根据GPS接收机收到的星历信息和INS系统输出的位置和速度信息，计算相应于INS位置的伪距和伪距率，与GPS接收机测量得到的伪距和伪距率的差值作为组合滤波器的输入，估计INS系统误差并进行校正。紧组合模式在可见GPS卫星个数少于4颗的情况下也可以使用，而且当GPS信号较好时，如果车辆行进过程中出现侧滑、跳跃现象对零速修正产生影响，组合滤波器的观测信息会对速度约束信息进行很好的修正作用。
S13：如果否，判断所述车辆是否运动；
具体的，判断车辆是否运动的考虑因素包括车辆的比力的幅值和方差、角速度的幅值、行车速度等是否达到相应的阈值条件，如果这些参数都达到了预设条件，例如比力幅值处于某一阈值范围、方差小于方差阈值、角速度幅值小于幅值阈值且行车速度小于速度参考阈值，则判定所述车辆未运动，如果有任一参数没达到对应的阈值条件，则判定所述车辆运动。
S14：如果否，根据所述车辆的状态信息建立第一系统模型，然后利用所述第一系统模型进行零速修正，以辅助所述车辆的定位导航；
S15：如果是，根据所述状态信息和约束条件建立第二系统模型，然后利用所述第二系统模型进行零速修正，以辅助所述车辆的定位导航；
其中，所述状态信息包括位置信息、速度信息、姿态信息、IMU(InertialMeasurement Unit，惯性测量单元)装置的几何中心到GPS接收机天线的相位中心的三维杆臂信息、加表零偏误差和陀螺仪漂移误差；所述约束条件具体包括：所述车辆的横向速度与竖直于地面的法向速度均为零。
可以理解的是，IMU装置包括加表和陀螺仪，本实施例中加表也称为加速度计。
具体的，在进行零速修正时，使用的算法包括但不限于卡尔曼滤波算法、训练神经网络模型预测的算法，具体的算法类型根据实际需求和计算精度、速度确定。
本发明实施例公开了一种车辆导航方法，包括：判断GPS卫星信号是否达到有效强度；如果是，通过INS系统与GPS系统的组合模式对车辆定位导航；如果否，判断所述车辆是否运动；如果否，根据所述车辆的状态信息建立第一系统模型，然后利用所述第一系统模型进行零速修正，以辅助所述车辆的定位导航；如果是，根据所述状态信息和约束条件建立第二系统模型，然后利用所述第二系统模型进行零速修正，以辅助所述车辆的定位导航；其中，所述状态信息包括位置信息、速度信息、姿态信息、IMU装置的几何中心到GPS接收机天线的相位中心的三维杆臂信息、加表零偏误差和陀螺仪漂移误差；所述约束条件具体包括：所述车辆的横向速度与竖直于地面的法向速度均为零。本发明实施例中状态信息包括位置信息、速度信息、姿态信息、三维杆臂信息、加表零偏误差和陀螺仪漂移误差，不需借助额外的传感器，即可建立第一系统模型、结合约束条件建立第二系统模型进行零速修正；在GPS信号长时间失锁情况下，车辆不需停车进行零速修正，在运动、静止两种状态下均可实现可靠、有效的定位导航。
本发明实施例公开了一种具体的车辆导航方法，相对于上一实施例，本实施例对技术方案作了进一步的说明和优化。
需要注意的是，本实施例中定义车辆直角坐标系m系，指向车辆右侧的X轴，指向车辆正前方的Y轴，指向车辆上方的Z轴；定义惯导坐标系为b系，为右前上坐标系，定义导航坐标系为n系,为东-北-天坐标系，坐标系之间根据转换矩阵进行转换，每个参量的参考坐标系以该参量右上角标为准。当然，除了本实施例中如此规定外，以其他规定方法同样可以实现本实施例中的方法。
由于惯导系统在车辆安装时不可避免的存在安装误差角(αθ αγ αψ)T，则b系与m系之间存在转换矩阵
Figure BDA0002130315460000071
其由安装误差角构成。由于运动约束条件下的辅助量为车体右侧方向的速度分量Vx m和车体向上方向的速度分量Vz m，所以车体前向方向的横滚安装误差角对运动约束没有影响，所以状态量中只增加了安装误差角，可视为随机常数，对应误差方程为
Figure BDA0002130315460000072
安装误差角在3°以内可以不改正，误差方程需要注意的是若安装角αθ和αψ角度过大，应事先标定。使用车辆运动约束辅助约束信息时，还应考虑到惯导系统在车辆安装时与卫星天线之间的杆臂效应。因为杆臂会随着系统的震动等发生变化。
参见图3所示，具体的，本实施例中车辆导航方法包括：
S21：判断GPS卫星信号是否达到有效强度；
S22：如果是，通过INS系统与GPS系统的组合模式对车辆定位导航；
S23：如果否，判断所述车辆的比力的幅值和方差、角速度的幅值、行车速度是否均达到相应的阈值条件；
其中，所述比力的幅值实际为合成比力幅值，具体根据
Figure BDA0002130315460000073
计算，其中fk为比力的幅值，fx、fy、fz分别为加速度计在X轴、Y轴、Z轴测得的比力值；
进一步有
Figure BDA0002130315460000074
其中1表示静止状态，0表示运动状态，Tmin和Tmax均为根据IMU元件的原始静态数据确定的参考阈值。一般来说，合成比力幅值一般在9.8-10.5m/s2之间时，可认为车辆处于静止状态。
其中，比力的方差计算具体为
Figure BDA0002130315460000081
Figure BDA0002130315460000082
为所求方差，
Figure BDA0002130315460000083
为比力均值，m为滑动窗口大小，与输出频率有关。
进一步有
Figure BDA0002130315460000084
其中1表示静止状态，0表示运动状态，Tσmax同样为根据原始静态数据确定的参考阈值。一般来说，比力的方差最大可达到0.4m/s2，低于此值，可认为车辆处于静止状态。
需要注意的是，之所以可通过比例的方差来判断车辆状态，是因为静止时车辆的振动通常很小，进一步还可采用频域滤波器消除大多数引擎引起的振动，这种处理增加了基于加速度的静止检测算法的灵敏度。
类似的，角速度的幅值计算具体为
Figure BDA0002130315460000085
|w|为所求角速度的幅值，wx、wy、wz分别为陀螺仪在X轴、Y轴、Z轴测得的角速度量；
进一步有
Figure BDA0002130315460000086
其中1表示静止状态，0表示运动状态，Twmax为根据原始静态数据确定的参考数据。一般来说，角速度的幅值最大可到45°/s，低于该值，可认为车辆处于静止状态。
与上述几个参量相似，在一个合适的时间窗口内，引入速度参考阈值。该阈值必须反应传感器的性能。考虑采用消费级传感器导航的实际情况，速度参考阈值可确定为0.5m/s，当行车速度低于该速度参考阈值，可认为车辆处于静止状态。
进一步的，根据以上四个约束条件，对判定结果进行逻辑与的运算，只有最终结果为1才认为满足了步骤S24的判断要求。
可以理解的是，上文中各类根据原始静态数据确定参考阈值的过程包括：
在车辆初对准阶段，采集一段时间如100s的IMU装置的原始静态数据，然后对原始静态数据进行以下计算：
Figure BDA0002130315460000087
Figure BDA0002130315460000088
其中，|wi|为第i(i＝1,2,..100)次测量得到的陀螺角增量幅值，|vi|为第i次测量得到的加速度计速度增量幅值，g0为当地重力分量的速度增量；根据λg和λa来确定上文判断所需的参考阈值Tmin、Tmax、Tσmax和Twmax。
S24：如果是，则判定所述车辆未运动，根据所述车辆的状态信息建立第一系统模型，然后通过卡尔曼滤波对所述第一系统模型进行状态估计，以进行零速修正，进而辅助所述车辆的定位导航；
本实施例中的状态信息包括位置信息、速度信息、姿态信息、三维杆臂信息、加表零偏误差和陀螺仪漂移误差等，建立第一系统模型，通过卡尔曼滤波进行状态估计，具体原理示意图参见图4所示。
当车辆未运动，设这个时刻为t-1，状态信息具体包括东北天三个轴向上IMU装置的位置信息
Figure BDA0002130315460000091
同理得速度信息
Figure BDA0002130315460000092
姿态信息
Figure BDA0002130315460000093
三维杆臂信息
Figure BDA0002130315460000094
当到达时刻t，位置信息为Pt E、Pt N、Pt U，此时理论位置信息应当与t-1时刻相同，从而通过差分运算可得到对应的位置误差，同理可得其他状态信息的误差，进而确定误差状态量X：
X＝[δPT δVT δα δβ δγ δlT δfT δwT]T；
其中，
Figure BDA0002130315460000095
Figure BDA0002130315460000096
需要注意的是，INS系统一般以IMU装置的几何中心作为导航定位或者测速的参考基准，而GPS系统则以接收机天线的相位中心作为参考基准，在实际运行中同时使用两种甚至多种导航系统时，他们安装位置上往往会存在一定的偏差。本实施例考虑三维杆臂信息，继而成为一种改进的零速修正方法。
与上文位置信息类似，δf和δw分别为加表零偏误差和陀螺仪漂移误差，也即零速时刻加速度计和陀螺仪在初始开机0时刻与当前t时刻的输出误差量：
Figure BDA0002130315460000101
进一步的，通过卡尔曼滤波对第一系统模型进行状态估计，以进行零速修正，主要包括两个过程，一是时间更新过程，在时间更新过程中，卡尔曼滤波通过对上一时刻的状态推算出对当前时刻的状态，二是状态更新过程，通过结合新的观测值构成新的信息对一步预测状态值进行修正，进而获得一个更加优化的状态估计值。
具体的，由上文已知18维误差状态量，建立状态方程Xt＝GXt-1+Wt-1，G为转移矩阵：
Figure BDA0002130315460000102
其中Wt-1为等效系统噪声，I为3阶单位矩阵，
Figure BDA0002130315460000103
fn和fb分别比力幅值在n系和b系的表示方式；×为三维向量的反对称矩阵；
然后建立观测方程为Zt＝HXt+Vt，其中H为观测矩阵：
Figure BDA0002130315460000104
按照图4即可实现未运动的车辆的零速修正和定位导航。
S25：如果否，则判定所述车辆运动，根据所述状态信息和约束条件建立第二系统模型，然后通过卡尔曼滤波对所述第二系统模型进行状态估计，以进行零速修正，进而辅助所述车辆的定位导航。
当前文步骤中检测到车辆处于运动状态，此时采用速度约束辅助INS系统进行零速约束修正，其原理示意图如图5所示。本步骤中考虑了车辆在道路上平稳行驶时，理想情况下侧滑为零，即在非大幅度转弯条件下的横向和法向速度为0m/s的约束条件对INS系统的导航误差进行修正。
也即认为车体X轴和Z轴方向上的加速度计不会敏感到加速度信息，车辆的横向速度Vx m和竖直于地面的法向速度Vz m均为零。实际情况中当车辆转向或侧滑，传感器以及系统误差会使得车辆在X轴和Z轴速度都不为零，所以将Vx m和Vz m两个方向的速度误差量视为观测量且满足零均值的高斯白噪声，通过调整噪声的方差来反映量测违反约束的程度。
与上一步骤类似，步骤S25确定了20维误差状态量：
Figure BDA0002130315460000111
此时对应的状态方程为：
Figure BDA0002130315460000112
其中W为等效系统噪声，F为状态转移矩阵：
Figure BDA0002130315460000113
其中F1时INS系统误差方程对应的状态转移矩阵。
由于多个坐标系之间的姿态转换矩阵包括：
Figure BDA0002130315460000114
同时考虑式
Figure BDA0002130315460000115
可得m系下的速度分量为
Figure BDA0002130315460000116
利用式
Figure BDA0002130315460000117
的约束条件构成量测值：
Figure BDA0002130315460000118
如果安装偏差δαθ和δαψ为小角度(如小于30)，
Figure BDA0002130315460000119
可视为单位矩阵，则b系速度Vb可直接作为量测值：
Figure BDA00021303154600001110
对式
Figure BDA00021303154600001111
进行全微分，且忽略了高阶小量，得到下式：
Figure BDA00021303154600001112
将上式改写成：δVm＝M1δVn+M2φn+M3δα+M4Lb；
式中：
Figure BDA00021303154600001113
其中
Figure BDA0002130315460000121
为载体坐标系相对于地球坐标系的运动角速度在载体坐标系的投影，Lb是安装杆臂在载体坐标系上的投影；
符号“×”依旧表示取3维向量的反对称矩阵。
由上面所有式子可以推出量测方程：Z＝HX+V；
其中，V表示量测噪声，H表示量测矩阵，且有
Figure BDA0002130315460000122
该式中M1(1，*)表示取3×3矩阵M1的第一行，M3(1,3)表示矩阵M3的第1行3列元素，其余符号含义类似。
由于惯导系统在载车存在安装杆臂等因素的影响，车辆在转向过程中横向速度量测误差大，所以应考虑在快速转向过程中不使用横向速度进行量测更新。由此本实施例提出了一种方法，当车辆转向行驶时，车辆的瞬时运动可以看作是以车辆中心上的某点为圆心的圆周运动，可以引入向心加速度观测误差作为新的观测量，也是一种行之有效的完整的运动学约束辅助的INS/GPS组合导航系统。
本实施例不借助外部其他额外的传感器，无需任何外部输入，不增加原有的INS/GPS系统成本，仅通过增加软件模块实现。因为速度约束算法在运动过程中即可进行量测更新，间隔时间短，可抑制模型非线性影响。在GPS长时间丢失场景，可为车辆在行驶、停止两种运行状态下，提高可靠，有效，高精度的约束辅助信息，保证组合导航系统的精度。
本发明实施例公开了一种具体的车辆导航方法，相对于上一实施例，本实施例对技术方案作了进一步的说明和优化。参见图6所示，具体的：
S31：判断GPS卫星信号是否达到有效强度；
S32：如果是，通过INS系统与GPS系统的组合模式对车辆定位导航，训练对应所述车辆状态信息的神经网络模型；
S33：如果否，判断所述车辆是否运动；
S34：如果否，根据所述车辆的状态信息建立第一系统模型，通过所述神经网络模型对所述第一系统模型进行状态估计，以进行零速修正，进而辅助所述车辆的定位导航；
S35：如果是，根据所述状态信息和约束条件建立第二系统模型，然后通过所述神经网络模型对所述第二系统模型进行状态估计，以进行零速修正，进而辅助所述车辆的定位导航。
可以理解的是，本实施例在步骤S32中，利用神经网络算法对GPS系统和INS系统输出的位置数据进行训练，生成用于预测INS误差的神经网络模型，在GPS卫星信号强度不够时，利用该神经网络模型来预测INS系统的误差，进行零速修正和系统参数更新。
本发明实施例公开了一种具体的车辆导航方法，相对于上一实施例，本实施例对技术方案作了进一步的说明和优化。参见图7所示，具体的：
第一判断模块01，用于判断GPS卫星信号是否达到有效强度；如果是，触发第一导航模块02，如果否，触发第二判断模块03；
所述第一导航模块02，用于通过INS系统与GPS系统的组合模式对车辆定位导航；
所述第二判断模块03，用于判断所述车辆是否运动；如果否，触发第二导航模块04，如果是，触发第三导航模块05；
所述第二导航模块04，用于根据所述车辆的状态信息建立第一系统模型，然后利用所述第一系统模型进行零速修正，以辅助所述车辆的定位导航；
所述第三导航模块05，用于根据所述状态信息和约束条件建立第二系统模型，然后利用所述第二系统模型进行零速修正，以辅助所述车辆的定位导航；
其中，所述状态信息包括位置信息、速度信息、姿态信息和IMU装置的几何中心到GPS接收机天线的相位中心的三维杆臂信息、加表零偏误差和陀螺仪漂移误差；所述约束条件具体包括：所述车辆的横向速度与竖直于地面的法向速度均为零。
进一步的，所述第二判断模块03具体用于：判断所述车辆的比力的幅值和方差、角速度的幅值、行车速度是否均达到相应的阈值条件；如果是，则判定所述车辆未运动并触发所述第二导航模块04，如果否，则判定所述车辆运动并触发所述第三导航模块05。
本实施例不需借助额外的传感器，即可建立第一系统模型、结合约束条件建立第二系统模型进行零速修正；在GPS信号长时间失锁情况下，车辆不需停车进行零速修正，在运动、静止两种状态下均可实现可靠、有效的定位导航。
相应的，本发明实施例还公开了一种车辆导航装置，包括：
存储器，用于存储计算机程序；
处理器，用于执行所述计算机程序时实现如上文任一项所述车辆导航方法的步骤。
其中，具体有关车辆导航方法的内容，可以参照上文实施例中车辆导航方法的相关描述，此处不再赘述。
其中，本实施例中车辆导航装置具有与上文中车辆导航方法相同的有益效果，此处不再赘述。
相应的，本发明还公开了一种可读存储介质，所述可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如上文任一项所述车辆导航方法的步骤。
其中，具体有关车辆导航方法的内容，可以参照上文实施例中车辆导航方法的相关描述，此处不再赘述。
其中，本实施例中可读存储介质具有与上文中车辆导航方法相同的有益效果，此处不再赘述。
最后，还需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
以上对本发明所提供的一种车辆导航方法、系统、装置及可读存储介质进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (7)

1.一种车辆导航方法，其特征在于，包括：
判断GPS卫星信号是否达到有效强度；
如果是，通过INS系统与GPS系统的组合模式对车辆定位导航；
如果否，判断所述车辆是否运动；
如果否，根据所述车辆的状态信息建立第一系统模型，然后利用所述第一系统模型进行零速修正，以辅助所述车辆的定位导航；
如果是，根据所述状态信息和约束条件建立第二系统模型，然后利用所述第二系统模型进行零速修正，以辅助所述车辆的定位导航；
当所述GPS卫星信号达到所述有效强度，训练对应所述车辆状态信息的神经网络模型；
相应的，所述利用所述第一系统模型进行零速修正的过程，具体包括：
通过所述神经网络模型对所述第一系统模型进行状态估计，以进行零速修正；
相应的，所述利用所述第二系统模型进行零速修正的过程，具体包括：
通过所述神经网络模型对所述第二系统模型进行状态估计，以进行零速修正；
所述判断所述车辆是否运动的过程，具体包括：
判断所述车辆的比力的幅值和方差、角速度的幅值、行车速度是否均达到相应的阈值条件；
如果是，则判定所述车辆未运动，如果否，则判定所述车辆运动；
其中，所述状态信息包括位置信息、速度信息、姿态信息、IMU装置的几何中心到GPS接收机天线的相位中心的三维杆臂信息、加表零偏误差和陀螺仪漂移误差；所述约束条件具体包括：所述车辆的横向速度与竖直于地面的法向速度均为零。
2.根据权利要求1所述车辆导航方法，其特征在于，所述利用所述第一系统模型进行零速修正的过程，具体包括：
通过卡尔曼滤波对所述第一系统模型进行状态估计，以进行零速修正；
相应的，所述利用所述第二系统模型进行零速修正的过程，具体包括：
通过卡尔曼滤波对所述第二系统模型进行状态估计，以进行零速修正。
3.根据权利要求1所述车辆导航方法，其特征在于，所述通过INS系统与GPS系统的组合模式对车辆定位导航的过程，具体包括：
通过INS系统与GPS系统的松组合模式对车辆定位导航。
4.根据权利要求1所述车辆导航方法，其特征在于，所述通过INS系统与GPS系统的组合模式对车辆定位导航的过程，具体包括：
通过INS系统与GPS系统的紧组合模式对车辆定位导航。
5.一种车辆导航系统，其特征在于，包括：
第一判断模块，用于判断GPS卫星信号是否达到有效强度；如果是，触发第一导航模块，如果否，触发第二判断模块；
所述第一导航模块，用于通过INS系统与GPS系统的组合模式对车辆定位导航；
所述第二判断模块，用于判断所述车辆是否运动；如果否，触发第二导航模块，如果是，触发第三导航模块；
所述第二判断模块具体用于：判断所述车辆的比力的幅值和方差、角速度的幅值、行车速度是否均达到相应的阈值条件；如果是，则判定所述车辆未运动并触发所述第二导航模块，如果否，则判定所述车辆运动并触发所述第三导航模块；
所述第二导航模块，用于根据所述车辆的状态信息建立第一系统模型，然后利用所述第一系统模型进行零速修正，以辅助所述车辆的定位导航；
所述第三导航模块，用于根据所述状态信息和约束条件建立第二系统模型，然后利用所述第二系统模型进行零速修正，以辅助所述车辆的定位导航；
其中，所述状态信息包括位置信息、速度信息、姿态信息、IMU装置的几何中心到GPS接收机天线的相位中心的三维杆臂信息、加表零偏误差和陀螺仪漂移误差；所述约束条件具体包括：所述车辆的横向速度与竖直于地面的法向速度均为零。
6.一种车辆导航装置，其特征在于，包括：
存储器，用于存储计算机程序；
处理器，用于执行所述计算机程序时实现如权利要求1至4任一项所述车辆导航方法的步骤。
7.一种可读存储介质，其特征在于，所述可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1至4任一项所述车辆导航方法的步骤。
CN201910635871.3A 2019-07-15 2019-07-15 一种车辆导航方法、系统、装置及可读存储介质 Active CN110346824B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910635871.3A CN110346824B (zh) 2019-07-15 2019-07-15 一种车辆导航方法、系统、装置及可读存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910635871.3A CN110346824B (zh) 2019-07-15 2019-07-15 一种车辆导航方法、系统、装置及可读存储介质

Publications (2)

Publication Number Publication Date
CN110346824A CN110346824A (zh) 2019-10-18
CN110346824B true CN110346824B (zh) 2021-11-09

Family

ID=68176298

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910635871.3A Active CN110346824B (zh) 2019-07-15 2019-07-15 一种车辆导航方法、系统、装置及可读存储介质

Country Status (1)

Country Link
CN (1) CN110346824B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110794437B (zh) * 2019-10-31 2021-10-22 深圳中科保泰科技有限公司 卫星定位信号强度预测方法和装置
CN110763229B (zh) * 2019-11-12 2022-04-15 武汉大学 一种便携式惯性导航定位杆及其定位定姿方法
CN111829555A (zh) * 2020-07-24 2020-10-27 中国人民解放军火箭军工程大学 一种基于运动约束的车载航姿系统误差补偿方法及系统
CN112595313A (zh) * 2020-11-25 2021-04-02 北京海达星宇导航技术有限公司 基于机器学习的车载导航方法、装置和计算机设备
CN113758502B (zh) * 2021-09-24 2024-02-20 广东汇天航空航天科技有限公司 组合导航处理方法及装置

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104132662A (zh) * 2014-07-25 2014-11-05 辽宁工程技术大学 基于零速修正的闭环卡尔曼滤波惯性定位方法
CN104406586A (zh) * 2014-12-04 2015-03-11 南京邮电大学 基于惯性传感器的行人导航装置和方法
CN105021192A (zh) * 2015-07-30 2015-11-04 华南理工大学 一种基于零速校正的组合导航系统的实现方法
CN109855617A (zh) * 2019-02-28 2019-06-07 深圳市元征科技股份有限公司 一种车辆定位方法、车辆定位装置及终端设备

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104132662A (zh) * 2014-07-25 2014-11-05 辽宁工程技术大学 基于零速修正的闭环卡尔曼滤波惯性定位方法
CN104406586A (zh) * 2014-12-04 2015-03-11 南京邮电大学 基于惯性传感器的行人导航装置和方法
CN105021192A (zh) * 2015-07-30 2015-11-04 华南理工大学 一种基于零速校正的组合导航系统的实现方法
CN109855617A (zh) * 2019-02-28 2019-06-07 深圳市元征科技股份有限公司 一种车辆定位方法、车辆定位装置及终端设备

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
基于运动约束辅助的车载组合导航算法;王立兵 等;《兵器装备工程学报》;20190228;第40卷(第2期);第120~124页 *
激光陀螺捷联惯性导航系统零速修正技术研究;李小芳;《中国优秀硕士学位论文全文数据库信息科技辑》;20150715(第07期);第五章 *
王立兵 等.基于运动约束辅助的车载组合导航算法.《兵器装备工程学报》.2019,第40卷(第2期), *

Also Published As

Publication number Publication date
CN110346824A (zh) 2019-10-18

Similar Documents

Publication Publication Date Title
CN110346824B (zh) 一种车辆导航方法、系统、装置及可读存储介质
Liu et al. Automated vehicle sideslip angle estimation considering signal measurement characteristic
CN110274589B (zh) 一种定位方法及装置
US9921065B2 (en) Unit and method for improving positioning accuracy
US9753144B1 (en) Bias and misalignment compensation for 6-DOF IMU using GNSS/INS data
US9645250B2 (en) Fail operational vehicle speed estimation through data fusion of 6-DOF IMU, GPS, and radar
Ryu et al. Vehicle sideslip and roll parameter estimation using GPS
EP1585939B1 (en) Attitude change kalman filter measurement apparatus and method
Park et al. Integrated observer approach using in-vehicle sensors and GPS for vehicle state estimation
Fakharian et al. Adaptive Kalman filtering based navigation: An IMU/GPS integration approach
US20100030471A1 (en) Position detecting apparatus and method used in navigation system
US20100007550A1 (en) Positioning apparatus for a mobile object
JP7036080B2 (ja) 慣性航法装置
JP6409346B2 (ja) 移動距離推定装置
CN107076559B (zh) 用于匹配导航系统的方法和系统
WO2016203744A1 (ja) 測位装置
US20080243384A1 (en) Azimuth determination apparatus, azimuth determination method and azimuth determination program
Anderson et al. Estimation of tire cornering stiffness using GPS to improve model based estimation of vehicle states
CN113029139B (zh) 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法
CN111536972A (zh) 一种基于里程计刻度系数修正的车载dr导航方法
JP6248559B2 (ja) 車両用走行軌跡算出装置
CN112762944B (zh) 零速区间检测及零速更新方法
JP5164645B2 (ja) カルマンフィルタ処理における繰り返し演算制御方法及び装置
Brunker et al. GNSS-shortages-resistant and self-adaptive rear axle kinematic parameter estimator (SA-RAKPE)
JP2019184566A (ja) 車両および車両位置推定装置

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