CN112378410B - 车辆行驶盲区校准方法、装置、设备及存储介质 - Google Patents

车辆行驶盲区校准方法、装置、设备及存储介质 Download PDF

Info

Publication number
CN112378410B
CN112378410B CN202011408240.7A CN202011408240A CN112378410B CN 112378410 B CN112378410 B CN 112378410B CN 202011408240 A CN202011408240 A CN 202011408240A CN 112378410 B CN112378410 B CN 112378410B
Authority
CN
China
Prior art keywords
vehicle
time
elevation
ins
model
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
CN202011408240.7A
Other languages
English (en)
Other versions
CN112378410A (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.)
Dongfeng Motor Corp
Original Assignee
Dongfeng Motor Corp
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 Dongfeng Motor Corp filed Critical Dongfeng Motor Corp
Priority to CN202011408240.7A priority Critical patent/CN112378410B/zh
Publication of CN112378410A publication Critical patent/CN112378410A/zh
Application granted granted Critical
Publication of CN112378410B publication Critical patent/CN112378410B/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/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
    • 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/20—Instruments for performing navigational calculations
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

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)

Abstract

本发明公开了一种车辆行驶盲区校准方法、装置、设备及存储介质，所述方法通过在检测到当前车辆进入到信号盲区时，将全球定位系统GPS数据、惯性导航系统INS数据和车身传感器数据进行时间同步；在时间同步后，基于预设车辆动力学模型和INS建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数；根据交互多模型Kalman滤波器和状态估计函数确定车辆最终估计参数，根据车辆最终估计参数对当前导航结果进行修正，避免了长时间处于盲区下惯性导航系统出现信号漂移的问题，保证了导航估计结果的估计精度和时间精度，导航修正数据具有实时性和有效性，提高了车辆导航的精度，并且实现成本较低，不需要搭建额外的基础设施。

Description

车辆行驶盲区校准方法、装置、设备及存储介质
技术领域
本发明涉及车辆导航技术领域，尤其涉及一种车辆行驶盲区校准方法、装置、设备及存储介质。
背景技术
随着城市化进程加速，越来越多的高层建筑及立体交通开始出现，随之也为全球定位系统(Global Positioning System，GPS)定位导航的车辆带来了更多的信号盲区，由于建筑物的遮挡，使用GPS定位导航的车辆在通过一些高架桥等立体交叉路段时，GPS接收机无法接收到卫星信号或者接受信号质量较差；即便对于高精度的INS-GPS组合导航定位方法，如果在高大建筑、高架桥等遮挡GPS信号的情况下，只能单独采用惯性导航对定位结果进行修正；惯性导航设备在长时间内将无可避免地出现累计误差的情况，使导航精度降低，甚至出现无法导航状态，无法为车辆驾驶系统提供准确的平面及高程信息输入。
现有方案是通过对基于超宽带(Ultra Wide Band，UWB)的短距离高精度无线定位信息、北斗卫星导航系统(BeiDou Navigation Satellite System，BDS)+GPS的双模全球导航卫星系统(Global Navigation Satellite System，GNSS)定位信息融合航位推算信息，不同信号源定位数据不断融合进行定位，或者通过导航模组进行定位讯号状态的确定，但是存在对外围基础设施要求高，成本较高，由于各个定位技术的时间不同步存在，定位数据缺乏有效性，定位精度较差的缺陷。
发明内容
本发明的主要目的在于提供一种车辆行驶盲区校准方法、装置、设备及存储介质，旨在解决现有技术中依赖外围通信基础设施，成本较高，数据缺乏有效性，导致定位精度较差的技术问题。
第一方面，本发明提供一种车辆行驶盲区校准方法，所述车辆行驶盲区校准方法包括以下步骤：
在检测到当前车辆进入到信号盲区时，将全球定位系统GPS数据、惯性导航系统INS数据和车身传感器数据进行时间同步；
在时间同步后，基于预设车辆动力学模型和INS建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数；
根据交互多模型Kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正。
可选地，所述在检测到当前车辆进入到信号盲区时，将全球定位系统GPS、惯性导航系统INS和车身传感器进行时间同步，包括：
在检测到当前车辆进入到信号盲区时，采集全球定位系统GPS的GPS数据中协调世界时UTC的时间序列；
采集惯性导航系统INS和车身传感器对应的计算机时间，获取所述时间序列与所述计算机时间在相同时刻的时间偏差序列；
根据所述时间偏差序列对GPS、INS和车身传感器的时间同步。
可选地，所述根据所述时间偏差序列对GPS、INS和车身传感器的时间同步，包括：
根据所述时间偏差序列通过下式对GPS、INS和车身传感器的时间同步：
TPC＝TUTC-ΔT
其中，TPC为所述计算机时间，TUTC为UTC时间，ΔT为所述时间偏差序列。
可选地，所述在时间同步后，基于预设车辆动力学模型和INS建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数，
在时间同步后，获取车辆车重；
将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程，并获得对应的动力状态估计函数；
根据INS获得车辆俯仰角，根据所述车辆俯仰角获得惯性导航高程状态方程，并获得对应的惯性状态估计函数。
可选地，所述将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程，并获得对应的动力状态估计函数，包括：
将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程：
Figure BDA0002814974570000031
其中，hk1为基于预设车辆动力学模型的高程，vk1为基于预设车辆动力学模型的纵向速度，βk1为基于预设车辆动力学模型的坡度，Δt为采样时间间隔，
Figure BDA0002814974570000032
为纵向加速度，
Figure BDA0002814974570000033
为坡度变化率，
Figure BDA0002814974570000034
为当前高程下的车辆车重，
Figure BDA0002814974570000035
为当前车速下的车辆车重，
Figure BDA0002814974570000036
为当前坡度下的车辆车重；
获取动力模型混合概率，根据所述动力模型混合概率通过下式获得动力状态估计函数：
Figure BDA0002814974570000037
其中，
Figure BDA0002814974570000038
为预设车辆动力学模型的状态估计值，k为时刻，μi1(k-1)为动力模型混合概率，
Figure BDA0002814974570000039
为车辆运动状态的动力状态估计函数。
可选地，所述根据INS获得车辆俯仰角，根据所述车辆俯仰角获得惯性导航高程状态方程，并获得对应的惯性状态估计函数，包括：
获得INS提供的车辆俯仰角，根据所述车辆俯仰角通过下式获得惯性导航高程状态方程：
Figure BDA00028149745700000310
其中，hk2为基于INS的高程，vk2为基于INS的纵向速度，βk2为基于INS的坡度，k为时刻，aIMUX,k-1为k-1时刻的INS测量的X方向的加速度，g为重力加速度，Δt为采样时间间隔，θk-1为所述车辆俯仰角，
Figure BDA00028149745700000311
为当前高程下的车辆车重，
Figure BDA00028149745700000312
为当前车速下的车辆车重，
Figure BDA00028149745700000313
为当前坡度下的车辆车重；
获取INS混合概率，根据所述INS混合概率通过下式获得惯性状态估计函数：
Figure BDA00028149745700000314
其中，
Figure BDA00028149745700000315
为INS的状态估计值，k为时刻，μi2(k-1)为INS混合概率，
Figure BDA0002814974570000041
为惯性状态估计函数。
可选地，所述根据交互多模型Kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正，包括：
根据交互多模型Kalman滤波器和所述状态估计函数对当前车辆数据进行筛选，确定有效测量值；
根据所述有效测量值通过下式确定最终状态估计函数：
Figure BDA0002814974570000042
其中，n(k)为在第一个循环中计算有效测量值的个数，k为时刻，Zk为累计到K-1时刻的观测值，θi为第i个来自实际目标的事件的有效度量，P为事件θi(k)的概率；
Figure BDA0002814974570000043
为最终状态估计函数；
根据所述最终状态估计函数通过下式确定车辆最终估计参数：
Figure BDA0002814974570000044
其中，k为时刻，
Figure BDA0002814974570000045
为最终状态估计函数，Ci(k)为预测概率，
Figure BDA0002814974570000046
为估计的最终车速、高程及坡度信息车辆最终估计参数；T为时间，hk为当前高程，vk为当前车速，βk为当前坡度，
Figure BDA0002814974570000047
为车辆最终估计参数。
第二方面，为实现上述目的，本发明还提出一种车辆行驶盲区校准装置，所述车辆行驶盲区校准装置包括：
时间同步模块，用于在检测到当前车辆进入到信号盲区时，将全球定位系统GPS数据、惯性导航系统INS数据和车身传感器数据进行时间同步；
函数构建模块，用于在时间同步后，基于预设车辆动力学模型和INS建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数；
修正模块，用于根据交互多模型Kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正。
第三方面，为实现上述目的，本发明还提出一种车辆行驶盲区校准设备，所述车辆行驶盲区校准设备包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的车辆行驶盲区校准程序，所述车辆行驶盲区校准程序配置为实现如权利要求上文所述的车辆行驶盲区校准方法的步骤。
第四方面，为实现上述目的，本发明还提出一种存储介质，所述存储介质上存储有车辆行驶盲区校准程序，所述车辆行驶盲区校准程序被处理器执行时实现如上文所述的车辆行驶盲区校准方法的步骤。
本发明提出的车辆行驶盲区校准方法，通过在检测到当前车辆进入到信号盲区时，将全球定位系统GPS数据、惯性导航系统INS数据和车身传感器数据进行时间同步；在时间同步后，基于预设车辆动力学模型和INS建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数；根据交互多模型Kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正，不会受到外界环境的影响进行高程信号的修正，并且信号的传输具有实时性，避免了长时间处于盲区下惯性导航系统出现信号漂移的问题；避免了在坡度变化率较大时导航估计结果表现出较为严重的延迟现象，保证了导航估计结果的估计精度和时间精度，导航修正数据具有实时性和有效性，可以在全工况道路条件下工作，提高了车辆导航的精度，并且实现成本较低，不需要搭建额外的基础设施。
附图说明
图1为本发明实施例方案涉及的硬件运行环境的设备结构示意图；
图2为本发明车辆行驶盲区校准方法第一实施例的流程示意图；
图3为本发明车辆行驶盲区校准方法第二实施例的流程示意图；
图4为本发明车辆行驶盲区校准方法第三实施例的流程示意图；
图5为本发明车辆行驶盲区校准装置第一实施例的功能模块图。
本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
本发明实施例的解决方案主要是：通过在检测到当前车辆进入到信号盲区时，将全球定位系统GPS数据、惯性导航系统INS数据和车身传感器数据进行时间同步；在时间同步后，基于预设车辆动力学模型和INS建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数；根据交互多模型Kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正，能够不会受到外界环境的影响进行高程信号的修正，并且信号的传输具有实时性，避免了长时间处于盲区下惯性导航系统出现信号漂移的问题；避免了在坡度变化率较大时导航估计结果表现出较为严重的延迟现象，保证了导航估计结果的估计精度和时间精度，导航修正数据具有实时性和有效性，可以在全工况道路条件下工作，提高了车辆导航的精度，并且实现成本较低，不需要搭建额外的基础设施，解决了现有技术中依赖外围通信基础设施，成本较高，数据缺乏有效性，导致定位精度较差的技术问题。
参照图1，图1为本发明实施例方案涉及的硬件运行环境的设备结构示意图。
如图1所示，该车辆行驶盲区校准设备可以包括：处理器1001，例如CPU，通信总线1002、用户接口1003，网络接口1004，存储器1005；其中，通信总线1002用于实现这些组件之间的连接通信；用户接口1003可以包括显示屏(Display)、输入单元比如键盘(Keyboard)，可选用户接口1003还可以包括标准的有线接口、无线接口；网络接口1004可选的可以包括标准的有线接口、无线接口(如Wi-Fi接口)；存储器1005可以是高速RAM存储器，也可以是稳定的存储器(Non-Volatile Memory)，例如磁盘存储器；存储器1005可选的还可以是独立于前述处理器1001的存储装置。
本领域技术人员可以理解，图1中示出的设备结构并不构成对该设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。
在具体实现中，所述车辆行驶盲区校准设备可以为ESP控制器，也可以为车载电脑，还可以为实现车辆行驶盲区校准功能的其他设备或终端，例如中控单元或远端服务器，本实施例对此不加以限制。
如图1所示，作为一种存储介质的存储器1005中可以包括操作系统、网络通信模块、用户接口模块以及车辆行驶盲区校准程序。
本发明设备通过处理器1001调用存储器1005中存储的车辆行驶盲区校准程序，并执行以下操作：
在检测到当前车辆进入到信号盲区时，将全球定位系统GPS数据、惯性导航系统INS数据和车身传感器数据进行时间同步；
在时间同步后，基于预设车辆动力学模型和INS建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数；
根据交互多模型Kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正。
进一步地，处理器1001可以调用存储器1005中存储的车辆行驶盲区校准程序，还执行以下操作：
在检测到当前车辆进入到信号盲区时，采集全球定位系统GPS的GPS数据中协调世界时UTC的时间序列；
采集惯性导航系统INS和车身传感器对应的计算机时间，获取所述时间序列与所述计算机时间在相同时刻的时间偏差序列；
根据所述时间偏差序列对GPS、INS和车身传感器的时间同步。
进一步地，处理器1001可以调用存储器1005中存储的车辆行驶盲区校准程序，还执行以下操作：
根据所述时间偏差序列通过下式对GPS、INS和车身传感器的时间同步：
TPC＝TUTC-ΔT
其中，TPC为所述计算机时间，TUTC为UTC时间，ΔT为所述时间偏差序列。
进一步地，处理器1001可以调用存储器1005中存储的车辆行驶盲区校准程序，还执行以下操作：
在时间同步后，获取车辆车重；
将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程，并获得对应的动力状态估计函数；
根据INS获得车辆俯仰角，根据所述车辆俯仰角获得惯性导航高程状态方程，并获得对应的惯性状态估计函数。
进一步地，处理器1001可以调用存储器1005中存储的车辆行驶盲区校准程序，还执行以下操作：
将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程：
Figure BDA0002814974570000081
其中，hk1为基于预设车辆动力学模型的高程，vk1为基于预设车辆动力学模型的纵向速度，βk1为基于预设车辆动力学模型的坡度，Δt为采样时间间隔，
Figure BDA0002814974570000082
为纵向加速度，
Figure BDA0002814974570000083
为坡度变化率，
Figure BDA0002814974570000084
为当前高程下的车辆车重，
Figure BDA0002814974570000085
为当前车速下的车辆车重，
Figure BDA0002814974570000086
为当前坡度下的车辆车重；
获取动力模型混合概率，根据所述动力模型混合概率通过下式获得动力状态估计函数：
Figure BDA0002814974570000087
其中，
Figure BDA0002814974570000088
为预设车辆动力学模型的状态估计值，k为时刻，μi1(k-1)为动力模型混合概率，
Figure BDA0002814974570000089
为车辆运动状态的动力状态估计函数。
进一步地，处理器1001可以调用存储器1005中存储的车辆行驶盲区校准程序，还执行以下操作：
获得INS提供的车辆俯仰角，根据所述车辆俯仰角通过下式获得惯性导航高程状态方程：
Figure BDA00028149745700000810
其中，hk2为基于INS的高程，vk2为基于INS的纵向速度，βk2为基于INS的坡度，k为时刻，aIMUX,k-1为k-1时刻的INS测量的X方向的加速度，g为重力加速度，Δt为采样时间间隔，θk-1为所述车辆俯仰角，
Figure BDA0002814974570000091
为当前高程下的车辆车重，
Figure BDA0002814974570000092
为当前车速下的车辆车重，
Figure BDA0002814974570000093
为当前坡度下的车辆车重；
获取INS混合概率，根据所述INS混合概率通过下式获得惯性状态估计函数：
Figure BDA0002814974570000094
其中，
Figure BDA0002814974570000095
为INS的状态估计值，k为时刻，μi2(k-1)为INS混合概率，
Figure BDA0002814974570000096
为惯性状态估计函数。
进一步地，处理器1001可以调用存储器1005中存储的车辆行驶盲区校准程序，还执行以下操作：
根据交互多模型Kalman滤波器和所述状态估计函数对当前车辆数据进行筛选，确定有效测量值；
根据所述有效测量值通过下式确定最终状态估计函数：
Figure BDA0002814974570000097
其中，n(k)为在第一个循环中计算有效测量值的个数，k为时刻，Zk为累计到K-1时刻的观测值，θi为第i个来自实际目标的事件的有效度量，P为事件θi(k)的概率；
Figure BDA0002814974570000098
为最终状态估计函数；
根据所述最终状态估计函数通过下式确定车辆最终估计参数：
Figure BDA0002814974570000099
其中，k为时刻，
Figure BDA00028149745700000910
为最终状态估计函数，Ci(k)为预测概率，
Figure BDA00028149745700000911
为估计的最终车速、高程及坡度信息车辆最终估计参数；T为时间，hk为当前高程，vk为当前车速，βk为当前坡度，
Figure BDA00028149745700000912
为车辆最终估计参数。
本实施例通过上述方案，通过在检测到当前车辆进入到信号盲区时，将全球定位系统GPS数据、惯性导航系统INS数据和车身传感器数据进行时间同步；在时间同步后，基于预设车辆动力学模型和INS建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数；根据交互多模型Kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正，能够不会受到外界环境的影响进行高程信号的修正，并且信号的传输具有实时性，避免了长时间处于盲区下惯性导航系统出现信号漂移的问题；避免了在坡度变化率较大时导航估计结果表现出较为严重的延迟现象，保证了导航估计结果的估计精度和时间精度，导航修正数据具有实时性和有效性，可以在全工况道路条件下工作，提高了车辆导航的精度，并且实现成本较低，不需要搭建额外的基础设施。
基于上述硬件结构，提出本发明车辆行驶盲区校准方法实施例。
参照图2，图2为本发明车辆行驶盲区校准方法第一实施例的流程示意图。
在第一实施例中，所述车辆行驶盲区校准方法包括以下步骤：
步骤S10、在检测到当前车辆进入到信号盲区时，将全球定位系统GPS数据、惯性导航系统INS数据和车身传感器数据进行时间同步。
需要说明的是，在检测到当前车辆进入到信号盲区时，可以通过将GPS数据，惯性导航系统(Inertial Navigation System，INS)数据，和车身控制器局域网络(ControllerArea Network，CAN)总线采集的车身传感器数据进行数据融合综合性判断当前车辆的定位，为了保证数据融合的精度，需要将这几种数据对应的传感器进行时间同步。
步骤S20、在时间同步后，基于预设车辆动力学模型和INS建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数。
可以理解的是，所述预设车辆动力学模型为预先设置的车辆动力学模型，在时间同步后，可以基于预设车辆动力学模型和INS建立高程、车速和道路坡度相关的状态方程，同时可以获得对应的状态估计函数；所述状态方程反映不同高程、车速和道路坡度对应不同车身运动状态的方程，所述状态估计函数为每个状态对应的状态估计值初始条件函数。
步骤S30、根据交互多模型Kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正。
应当理解的是，所述交互多模型Kalman滤波器为用于并行处理不同的状态模型带来的目标运行模式的滤波器，每个模型通过混合前一时刻所有滤波器的状态估计函数来确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正。
进一步的，所述步骤S30具体包括以下步骤：
根据交互多模型Kalman滤波器和所述状态估计函数对当前车辆数据进行筛选，确定有效测量值；
根据所述有效测量值通过下式确定最终状态估计函数：
Figure BDA0002814974570000111
其中，n(k)为在第一个循环中计算有效测量值的个数，k为时刻，Zk为累计到K-1时刻的观测值，θi为第i个来自实际目标的事件的有效度量，P为事件θi(k)的概率；
Figure BDA0002814974570000112
为最终状态估计函数；
根据所述最终状态估计函数通过下式确定车辆最终估计参数：
Figure BDA0002814974570000113
其中，k为时刻，
Figure BDA0002814974570000114
为最终状态估计函数，Ci(k)为预测概率，
Figure BDA0002814974570000115
为估计的最终车速、高程及坡度信息车辆最终估计参数；T为时间，hk为当前高程，vk为当前车速，βk为当前坡度，
Figure BDA0002814974570000116
为车辆最终估计参数。
需要说明的是，在状态模型构建模块分别建立了基于动力学的状态模型及基于惯性导航系统的状态模型；最终的估计结果是对不同模型所得估计的混合，而不仅仅是在每一个时刻选择完全正确的模型来完成估计；在概率数据关联滤波模块中，要对有效量测进行筛选，将有效量测用于状态推断；并确定合理的混合原则，以最终得到精确的状态更新。
在具体实现中，概率数据关联Kalman滤波
每个阶段有效的测量值被定义为：
Figure BDA0002814974570000117
其中，n(k)是在第一个循环中计算有效测量值的个数。
针对动力学模型及惯性导航模型均可产生高程的量测值，模型的量测新息为
Figure BDA0002814974570000118
而只有满足下条件量测新息才能被当作有效值被保存下来。
εi(k)TSj(k)-1εi(k)＜g2
其中Sj(k)是新息协方差，g是跟踪门的门限，对应跟踪门的体积。
v(k)＝g2π|Sj(k)|1/2
在上式中，g在“Chi-square distribution”是可信度为98％的警戒值；如果利用有效领域内的测量值，通过“有条件的平均”来推测状态，就可以做出更加值得信赖的推测。模型的状态更新为
Figure BDA0002814974570000121
Zk是累积到k-1的观测值，θi是第i个来自实际目标的事件的有效度量。所有有效的测量都是集群事件；假设上面一个目标只有一个测量值；而且，其他有效区域内的测量值是目标附近的封闭区域；θi(k)，(i＝0，1，...，n(k))的事件是互斥事件；事件θi(k)的概率，即第i个量测来自实际目标的概率，以及实际目标不是来自实际目标的概率，由以下等式定义：
Figure BDA0002814974570000122
Figure BDA0002814974570000123
Figure BDA0002814974570000124
b(k)＝n(k)(1-PGPD)[PGPDV(k)]-1
N{εi(k)；0，S(k)}表示正态概率密度函数，均值为0，方差为S(k)；PG是正确测量值落在有效范围内的概率，PD是GPS检测到正确测量值的概率。
Kalman增益
Figure BDA0002814974570000125
Figure BDA0002814974570000126
状态更新为：
Figure BDA0002814974570000127
Figure BDA0002814974570000128
模型的协方差更新
Figure BDA0002814974570000129
高程识别值是两个模型状态估计值的加权结果，每个模型的加权系数是当前时刻模型正确描述高程信息状态的概率，即模型概率。
在时刻模型基于量测计算模型概率为
Figure BDA0002814974570000131
Figure BDA0002814974570000132
式中，
Figure BDA0002814974570000133
是模型j的预测概率，Λj(k)是多量测新息的联合概率密度函数
利用利用前述结果，在两个状态模型的滤波器估计结果的基础上加权叠加，得到总的状态估计和总的协方差估计。
Figure BDA0002814974570000134
为所估计的最终车速、高程及坡度信息
Figure BDA0002814974570000135
Figure BDA0002814974570000136
在获得了所述车辆最终估计参数后，可以通过显示输出模块进行输出显示，显示输出模块可包括显示器和高程信号输出模块，高程信号输入车载控制器，参与预测巡航控制系统对车身姿态和位置高程的判断。
在上一个步骤得到了校准之后的高程信息
X＝[hk vk βk]T
即，获知了当前车辆所处的高程、车速及坡度信息。
本发明原义是将得到的精确数据信息输入到车载控制器，辅助进行巡航预测或高程位置的判断。本模块只是输出显示。
在获得上述信息的精确值之后，易于获知车辆位置信息，
例如，设车辆的起始位置是h0，由上述步骤得到的从tk到tk+1时刻的行驶距离Δs＝vkΔt信息坡度值为βk,则可以推算出tk+1时刻的车辆位置：
hk+1＝hk+Δs·sinβk
同样，可由当前位置的坡度信息，参考动力学模型，获知车身俯仰角信息。
本实施例只涉及高程信息的修正和校准，针对的是城市路况中的高架桥上，GPS信号出现扰动的情况，本实施例基于交互多模型的GPS高程盲区校准系统能准确实时地融合车身CAN提供的车辆动力学信息及惯性导航系统提供的加速度信息，来完成对GPS高程信号的修正校准；通过对车身CAN信号的引入，对高程信号的修正不会受到外界环境的影响，而且信号的传输具有实时性，避免了长时间处于盲区下惯性导航系统出现信号漂移的问题；通过引入对量测新息有效性的判断，能够及时形成对高程定位信息不同模式的处理方式，在有效量测下完成了高程的校准，提高了结果的可信；传感器的时间同步能够增加最终的融合精度；在构建状态方程时，考虑了道路坡度自身变化率信息，避免了在坡度变化率较大时估计结果表现出较为严重的延迟现象，使估计结果在估计精度和时间精度两个方面同时取得较好的效果，并且为配备组合导航系统的车辆对于车身姿态及位置高程信息的获取提供一个新的框架与接口，如需要获知上述信息，只需将车身CAN信号及组合导航接口引入，加快了算法在智能驾驶方程的推进；该系统可以在全工况道路条件下工作，提高了车辆姿态及位置高程的识别范围及工作效果；本发明通过实车实验，对利用车辆高程信息的预测巡航系统的节油效果产生了有益影响
本实施例通过上述方案，通过在检测到当前车辆进入到信号盲区时，将全球定位系统GPS数据、惯性导航系统INS数据和车身传感器数据进行时间同步；在时间同步后，基于预设车辆动力学模型和INS建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数；根据交互多模型Kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正，能够不受到外界环境的影响进行高程信号的修正，并且信号的传输具有实时性，避免了长时间处于盲区下惯性导航系统出现信号漂移的问题；避免了在坡度变化率较大时导航估计结果表现出较为严重的延迟现象，保证了导航估计结果的估计精度和时间精度，导航修正数据具有实时性和有效性，可以在全工况道路条件下工作，提高了车辆导航的精度，并且实现成本较低，不需要搭建额外的基础设施。
进一步地，图3为本发明车辆行驶盲区校准方法第二实施例的流程示意图，如图3所示，基于第一实施例提出本发明车辆行驶盲区校准方法第二实施例，在本实施例中，所述步骤S10具体包括以下步骤：
步骤S11、在检测到当前车辆进入到信号盲区时，采集全球定位系统GPS的GPS数据中协调世界时UTC的时间序列。
可以理解的是，由于受不同传感器采用的时间系统不相同、采样频率不相同和不同传感器在无线电传输过程中存在不同时间延迟等因素的影响，导致运动高程测量结果出现因时间不同步引起的失真现象，因此为了实现GPS信号盲区高程实时重建，必须要求GPS、惯性导航系统和车辆动力模型时间同步，在检测到当前车辆进入到信号盲区时，由于GPS是有时标的系统，可以采集GPS数据中的协调世界时(Universal Time Coordinated，UTC)。
步骤S12、采集惯性导航系统INS和车身传感器对应的计算机时间，获取所述时间序列与所述计算机时间在相同时刻的时间偏差序列。
需要说明的是，所述INS和车身传感器对应有计算机时间，通过将所述时间序列与所述计算机时间进行对比能够确定在相同时刻的时间偏差序列。
步骤S13、根据所述时间偏差序列对GPS、INS和车身传感器的时间同步。
应当理解的是，利用所述实际偏差序列能够消除时间序列与计算机时间的偏差，从而实现时间数据的统一。
进一步的，所述S13具体包括以下步骤：
根据所述时间偏差序列通过下式对GPS、INS和车身传感器的时间同步：
TPC＝TUTC-ΔT
其中，TPC为所述计算机时间，TUTC为UTC时间，ΔT为所述时间偏差序列。
可以理解的是，利用时间偏差序列，可以初步实现时间数据的统一，将偏差序列保存为ΔT，通过消除时间偏差可以实现GPS数据、INS数据及车身CAN采集的传感器数据的时间统一。
本实施例通过上述方案，通过在检测到当前车辆进入到信号盲区时，采集全球定位系统GPS的GPS数据中协调世界时UTC的时间序列；采集惯性导航系统INS和车身传感器对应的计算机时间，获取所述时间序列与所述计算机时间在相同时刻的时间偏差序列；根据所述时间偏差序列对GPS、INS和车身传感器的时间同步，能够提高数据融合的精度，实现了时间的统一，保证了导航估计结果的估计精度和时间精度，导航修正数据具有实时性和有效性，可以在全工况道路条件下工作，提高了车辆导航的精度。
进一步地，图4为本发明车辆行驶盲区校准方法第三实施例的流程示意图，如图4所示，基于第一实施例提出本发明车辆行驶盲区校准方法第三实施例，在本实施例中，所述步骤S20具体包括以下步骤：
步骤S21、在时间同步后，获取车辆车重。
需要说明的是，所述车辆车重为车辆的当前质量估计，因为只有对车辆质量做出有效估计，才能保证后期在车辆动力学模型中对定位高程估计的准确性。
在具体实现中，可以在基于惯性导航系统的汽车质量估计模块利用惯性导航系统及车辆动力学特性，对车辆质量做出估计；即惯性导航系统可以提供三个轴向的加速度及角加速度的值，一般令汽车行驶方向为X轴，垂直于行驶路面为Z轴；沿坡道方向测得的分量(X轴)是车辆运动加速度a与重力加速度常量沿坡道分量的和，沿垂直于坡道方向上测得的分量(Z轴)是重力加速度垂直于坡道的分量；则
Figure BDA0002814974570000161
aIMUZ＝g cosβ
其中，aIMUX和aIMUZ分别是惯性导航系统测量的X方向和Z方向的加速度，
Figure BDA0002814974570000167
为纵向加速度，g是重力加速度，β是道路坡度值。
在实际道路坡度中，可以假设sinβ＝β，
Figure BDA0002814974570000162
利用惯性导航系统测量的车辆纵向加速度及车重可以表示为：
Figure BDA0002814974570000163
Figure BDA0002814974570000164
式中，
Figure BDA0002814974570000165
为发动机力矩驱动下的驱动加速度，M为整车质量、ηt为传动效率、rw为车轮半径、Ig为从发动机到车轮的总变速比(包括变速器速比和主减速比)，
Figure BDA0002814974570000166
为风阻带来的减速度，cd，Af，ρ分别为风阻系数、车辆有效迎风面积和空气密度；f为摩擦阻力系数。
步骤S22、将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程，并获得对应的动力状态估计函数。
可以理解的是，将所述车辆车重代入预设车辆动力学模型中，可以获得高程、车速和道路坡度相关的动力学高程状态方程，所述动力学高程状态方程主要是运用车辆CAN总线采集驱动信息，即发动机力矩、档位等、车速等信息；该模型建立的最大优点是无需额外传感器信息，信号均由CAN总线提供；既能实时匹配车身信息，也能够避免额外的生产成本，所述动力状态估计函数为动力状态估计值初始条件函数。
进一步的，所述步骤S22具体包括以下步骤：
将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程：
Figure BDA0002814974570000171
其中，hk1为基于预设车辆动力学模型的高程，vk1为基于预设车辆动力学模型的纵向速度，βk1为基于预设车辆动力学模型的坡度，Δt为采样时间间隔，
Figure BDA0002814974570000172
为纵向加速度，
Figure BDA0002814974570000173
为坡度变化率，
Figure BDA0002814974570000174
为当前高程下的车辆车重，
Figure BDA0002814974570000175
为当前车速下的车辆车重，
Figure BDA0002814974570000176
为当前坡度下的车辆车重；
获取动力模型混合概率，根据所述动力模型混合概率通过下式获得动力状态估计函数：
Figure BDA0002814974570000177
其中，
Figure BDA0002814974570000178
为预设车辆动力学模型的状态估计值，k为时刻，μi1(k-1)为动力模型混合概率，
Figure BDA0002814974570000179
为车辆运动状态的动力状态估计函数。
需要说明的是，在车辆行驶过程中，目标存在机动性，并且盲区的存在也使得基本的Kalman滤波的结果并不精确；使用两个模型来描述行驶过程中可能的状态，并且通过有效的加权融合进行系统的状态估计，能够很好地克服单模型估计误差加大的问题；在状态模型定义模块中，分别利用车身CAN信号建立基于车辆动力学的状态模型，利用惯性导航系统测量信息建立惯性导航状态模型。
在具体实现中，车辆所处的高程h(t)变化表现为车速和坡度值βk的组合：
Figure BDA0002814974570000181
由车辆动力学模型可知：
Figure BDA0002814974570000182
其中，g为重力加速度，cr是滚动阻力系数。
Figure BDA0002814974570000183
相对道路坡度而言，假设令sinβ＝β
即
Figure BDA0002814974570000184
构建动力学模型的状态方程的状态变量：
X1＝[xk1]＝[hk1 vk1 βk1]T
Δt为采样时间间隔，
Figure BDA0002814974570000185
为纵向加速度，
Figure BDA0002814974570000186
为坡度变化率。
Figure BDA0002814974570000187
对上式两边求导，同时假设在相邻的采样时间间隔内车辆加速度变化率为零，且档位不变，可以得到坡度的变化率：
Figure BDA0002814974570000188
步骤S23、根据INS获得车辆俯仰角，根据所述车辆俯仰角获得惯性导航高程状态方程，并获得对应的惯性状态估计函数。
应当理解的是，所述车辆俯仰角由INS采集提供，可以反映道路坡度的变化率，根据所述车辆俯仰角获得惯性导航高程状态方程，即基于INS的高程状态模型，所述惯性状态估计函数为惯性状态估计值初始条件函数。
进一步的，所述步骤S23具体包括以下步骤：
获得INS提供的车辆俯仰角，根据所述车辆俯仰角通过下式获得惯性导航高程状态方程：
Figure BDA0002814974570000191
其中，hk2为基于INS的高程，vk2为基于INS的纵向速度，βk2为基于INS的坡度，k为时刻，aIMUX，k-1为k-1时刻的INS测量的X方向的加速度，g为重力加速度，Δt为采样时间间隔，θk-1为所述车辆俯仰角，
Figure BDA0002814974570000192
为当前高程下的车辆车重，
Figure BDA0002814974570000193
为当前车速下的车辆车重，
Figure BDA0002814974570000194
为当前坡度下的车辆车重；
获取INS混合概率，根据所述INS混合概率通过下式获得惯性状态估计函数：
Figure BDA0002814974570000195
其中，
Figure BDA0002814974570000196
为INS的状态估计值，k为时刻，μi2(k-1)为INS混合概率，
Figure BDA0002814974570000197
为惯性状态估计函数。
需要说明的是，由于利用车辆动力学建立状态模型也存在弊端：比如转弯时的侧向运动和加速或减速工况下的车轮滑移也会使状态方程产生误差；在车辆上加装惯性导航系统可以增加可观测的量，提高状态估计的精度和稳定性；惯性导航系统安装在车辆重心位置可以提供车身运动状态及姿态信息，基于惯性导航系统可以获得另一个高程状态模型，即惯性导航高程状态模型。
在具体实现中，状态变量：
X2＝[xk2]＝[hk2 vk2 βk2]T
GPS可以用来记录纬度、经度、旅行距离、高度和交流卫星的数量。其中，GPS提供的高度信号恰是本发明中需要校准和修正的量；车辆的纵向速度即为车辆前进的车速信息，可以通过车身CAN直接获取；观测矩阵选取直观观测量车辆纵向速度及车辆高程信息：
Zj(k-1)＝Hj(k-1)Xj(k-1)+Vj(k-1)
其中，
Figure BDA0002814974570000198
Figure BDA0002814974570000199
Figure BDA00028149745700001910
相应地，应用车辆动力学模型中对车辆本身状态的描述及惯性导航对车身运动及姿态的测量，可以通过两个模型对车辆运动的高程和状态表示；设第j个模型表示的目标状态方程为：
Xj(k)＝Φj(k-1)Xj(k-1)+Gj(k-1)Wj(k-1)
式中，j＝1表示动力学模型，j＝2表示惯性导航模型，Φj为状态转移矩阵；Gj为噪声驱动矩阵；H为观测矩阵；Wj(k-1)和V(k-1)为均值为零、协方差矩阵各为Qj和Rj的白噪声序列。
模型之间的转移由马尔科夫概率转移矩阵确定，概率转移矩阵如下：
Figure BDA0002814974570000201
概率转移矩阵中的元素pij(i，j＝1，2)表示由第i个模型转移到第j个模型的概率；由高程目标的两个状态的估计
Figure BDA0002814974570000202
与上述动力学模型和惯性导航模型的模型概率μi(k-1)得到交互模型的混合估计值和协方差。详细步骤如下：
对模型1(动力学模型)来说：
模型1的预测概率为：
Figure BDA0002814974570000203
其中，μi(k-1)是模型i在k-1时刻的概率。
作用到模型1的混合概率为：
Figure BDA0002814974570000204
Figure BDA0002814974570000205
于是，可以得到模型1的混合状态估计为：
Figure BDA0002814974570000206
Figure BDA0002814974570000207
是模型i的状态估计，μi1(k-1)是模型i到模型1的混合概率。
然后得到模型1的初始混合协方差估计为：
Figure BDA0002814974570000211
同理，对模型2来说，可以得到交互之后的模型2的初始条件：
Figure BDA0002814974570000212
Figure BDA0002814974570000213
Figure BDA0002814974570000214
Figure BDA0002814974570000215
得到模型j的混合状态估计
Figure BDA0002814974570000216
及混合协方差估计P0j(k-1|k-1)，之后便可将它们作为输入进行Kalman滤波，来更新预测状态
Figure BDA0002814974570000217
和滤波协方差Pj。
模型j的状态预测：
Figure BDA0002814974570000218
误差协方差预测：
Figure BDA0002814974570000219
新息协方差：
Figure BDA00028149745700002110
本实施例通过上述方案，通过在时间同步后，获取车辆车重；将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程，并获得对应的动力状态估计函数；根据INS获得车辆俯仰角，根据所述车辆俯仰角获得惯性导航高程状态方程，并获得对应的惯性状态估计函数；能够不受到外界环境的影响进行高程信号的修正，并且信号的传输具有实时性，避免了长时间处于盲区下惯性导航系统出现信号漂移的问题；避免了在坡度变化率较大时导航估计结果表现出较为严重的延迟现象，保证了导航估计结果的估计精度和时间精度，导航修正数据具有实时性和有效性，可以在全工况道路条件下工作，提高了车辆导航的精度，并且实现成本较低，不需要搭建额外的基础设施。
相应地，本发明进一步提供一种车辆行驶盲区校准装置。
参照图5，图5为本发明车辆行驶盲区校准装置第一实施例的功能模块图。
本发明车辆行驶盲区校准装置第一实施例中，该车辆行驶盲区校准装置包括：
时间同步模块10，用于在检测到当前车辆进入到信号盲区时，将全球定位系统GPS数据、惯性导航系统INS数据和车身传感器数据进行时间同步。
函数构建模块20，用于在时间同步后，基于预设车辆动力学模型和INS建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数。
修正模块30，用于根据交互多模型Kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正。
其中，车辆行驶盲区校准装置的各个功能模块实现的步骤可参照本发明车辆行驶盲区校准方法的各个实施例，此处不再赘述。
此外，本发明实施例还提出一种存储介质，所述存储介质上存储有车辆行驶盲区校准程序，所述车辆行驶盲区校准程序被处理器执行时实现如下操作：
在检测到当前车辆进入到信号盲区时，将全球定位系统GPS数据、惯性导航系统INS数据和车身传感器数据进行时间同步；
在时间同步后，基于预设车辆动力学模型和INS建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数；
根据交互多模型Kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正。
进一步地，所述车辆行驶盲区校准程序被处理器执行时还实现如下操作：
在检测到当前车辆进入到信号盲区时，采集全球定位系统GPS的GPS数据中协调世界时UTC的时间序列；
采集惯性导航系统INS和车身传感器对应的计算机时间，获取所述时间序列与所述计算机时间在相同时刻的时间偏差序列；
根据所述时间偏差序列对GPS、INS和车身传感器的时间同步。
进一步地，所述车辆行驶盲区校准程序被处理器执行时还实现如下操作：
根据所述时间偏差序列通过下式对GPS、INS和车身传感器的时间同步：
TPC＝TUTC-ΔT
其中，TPC为所述计算机时间，TUTC为UTC时间，ΔT为所述时间偏差序列。
进一步地，所述车辆行驶盲区校准程序被处理器执行时还实现如下操作：
在时间同步后，获取车辆车重；
将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程，并获得对应的动力状态估计函数；
根据INS获得车辆俯仰角，根据所述车辆俯仰角获得惯性导航高程状态方程，并获得对应的惯性状态估计函数。
进一步地，所述车辆行驶盲区校准程序被处理器执行时还实现如下操作：
将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程：
Figure BDA0002814974570000231
其中，hk1为基于预设车辆动力学模型的高程，vk1为基于预设车辆动力学模型的纵向速度，βk1为基于预设车辆动力学模型的坡度，Δt为采样时间间隔，
Figure BDA0002814974570000241
为纵向加速度，
Figure BDA0002814974570000242
为坡度变化率，
Figure BDA0002814974570000243
为当前高程下的车辆车重，
Figure BDA0002814974570000244
为当前车速下的车辆车重，
Figure BDA0002814974570000245
为当前坡度下的车辆车重；
获取动力模型混合概率，根据所述动力模型混合概率通过下式获得动力状态估计函数：
Figure BDA0002814974570000246
其中，
Figure BDA0002814974570000247
为预设车辆动力学模型的状态估计值，k为时刻，μi1(k-1)为动力模型混合概率，
Figure BDA0002814974570000248
为车辆运动状态的动力状态估计函数。
进一步地，所述车辆行驶盲区校准程序被处理器执行时还实现如下操作：
获得INS提供的车辆俯仰角，根据所述车辆俯仰角通过下式获得惯性导航高程状态方程：
Figure BDA0002814974570000249
其中，hk2为基于INS的高程，vk2为基于INS的纵向速度，βk2为基于INS的坡度，k为时刻，aIMUX,k-1为k-1时刻的INS测量的X方向的加速度，g为重力加速度，Δt为采样时间间隔，θk-1为所述车辆俯仰角，
Figure BDA00028149745700002410
为当前高程下的车辆车重，
Figure BDA00028149745700002411
为当前车速下的车辆车重，
Figure BDA00028149745700002412
为当前坡度下的车辆车重；
获取INS混合概率，根据所述INS混合概率通过下式获得惯性状态估计函数：
Figure BDA00028149745700002413
其中，
Figure BDA00028149745700002414
为INS的状态估计值，k为时刻，μi2(k-1)为INS混合概率，
Figure BDA00028149745700002415
为惯性状态估计函数。
进一步地，所述车辆行驶盲区校准程序被处理器执行时还实现如下操作：
根据交互多模型Kalman滤波器和所述状态估计函数对当前车辆数据进行筛选，确定有效测量值；
根据所述有效测量值通过下式确定最终状态估计函数：
Figure BDA0002814974570000251
其中，n(k)为在第一个循环中计算有效测量值的个数，k为时刻，Zk为累计到K-1时刻的观测值，θi为第i个来自实际目标的事件的有效度量，P为事件θi(k)的概率；
Figure BDA0002814974570000252
为最终状态估计函数；
根据所述最终状态估计函数通过下式确定车辆最终估计参数：
Figure BDA0002814974570000253
其中，k为时刻，
Figure BDA0002814974570000254
为最终状态估计函数，Ci(k)为预测概率，
Figure BDA0002814974570000255
为估计的最终车速、高程及坡度信息车辆最终估计参数；T为时间，hk为当前高程，vk为当前车速，βk为当前坡度，
Figure BDA0002814974570000256
为车辆最终估计参数。
本实施例通过上述方案，通过在检测到当前车辆进入到信号盲区时，将全球定位系统GPS数据、惯性导航系统INS数据和车身传感器数据进行时间同步；在时间同步后，基于预设车辆动力学模型和INS建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数；根据交互多模型Kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正，能够不会受到外界环境的影响进行高程信号的修正，并且信号的传输具有实时性，避免了长时间处于盲区下惯性导航系统出现信号漂移的问题；避免了在坡度变化率较大时导航估计结果表现出较为严重的延迟现象，保证了导航估计结果的估计精度和时间精度，导航修正数据具有实时性和有效性，可以在全工况道路条件下工作，提高了车辆导航的精度，并且实现成本较低，不需要搭建额外的基础设施。
需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者系统不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者系统所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者系统中还存在另外的相同要素。
上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。
以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (8)

1.一种车辆行驶盲区校准方法，其特征在于，所述车辆行驶盲区校准方法包括：
在检测到当前车辆进入到信号盲区时，将全球定位系统GPS数据、惯性导航系统INS数据和车身传感器数据进行时间同步；
在时间同步后，基于预设车辆动力学模型和INS建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数；
根据交互多模型Kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正；
其中，所述在时间同步后，基于预设车辆动力学模型和INS建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数，包括：
在时间同步后，获取车辆车重；
将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程，并获得对应的动力状态估计函数；
根据INS获得车辆俯仰角，根据所述车辆俯仰角获得惯性导航高程状态方程，并获得对应的惯性状态估计函数；
其中，所述将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程，并获得对应的动力状态估计函数，包括：
将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程：
Figure FDA0003936628030000011
其中，hk1为基于预设车辆动力学模型的高程，vk1为基于预设车辆动力学模型的纵向速度，βk1为基于预设车辆动力学模型的坡度，Δt为采样时间间隔，
Figure FDA0003936628030000012
为纵向加速度，
Figure FDA0003936628030000013
为坡度变化率，
Figure FDA0003936628030000014
为当前高程下的车辆车重，
Figure FDA0003936628030000015
为当前车速下的车辆车重，
Figure FDA0003936628030000016
为当前坡度下的车辆车重；
获取动力模型混合概率，根据所述动力模型混合概率通过下式获得动力状态估计函数：
Figure FDA0003936628030000021
其中，
Figure FDA0003936628030000022
为预设车辆动力学模型的状态估计值，k为时刻，μi1(k-1)为动力模型混合概率，
Figure FDA0003936628030000023
为车辆运动状态的动力状态估计函数。
2.如权利要求1所述的车辆行驶盲区校准方法，其特征在于，所述在检测到当前车辆进入到信号盲区时，将全球定位系统GPS、惯性导航系统INS和车身传感器进行时间同步，包括：
在检测到当前车辆进入到信号盲区时，采集全球定位系统GPS的GPS数据中协调世界时UTC的时间序列；
采集惯性导航系统INS和车身传感器对应的计算机时间，获取所述时间序列与所述计算机时间在相同时刻的时间偏差序列；
根据所述时间偏差序列对GPS、INS和车身传感器的时间同步。
3.如权利要求2所述的车辆行驶盲区校准方法，其特征在于，所述根据所述时间偏差序列对GPS、INS和车身传感器的时间同步，包括：
根据所述时间偏差序列通过下式对GPS、INS和车身传感器的时间同步：
TPC＝TUTC-ΔT
其中，TPC为所述计算机时间，TUTC为UTC时间，ΔT为所述时间偏差序列。
4.如权利要求1所述的车辆行驶盲区校准方法，其特征在于，所述根据INS获得车辆俯仰角，根据所述车辆俯仰角获得惯性导航高程状态方程，并获得对应的惯性状态估计函数，包括：
获得INS提供的车辆俯仰角，根据所述车辆俯仰角通过下式获得惯性导航高程状态方程：
Figure FDA0003936628030000031
其中，hk2为基于INS的高程，vk2为基于INS的纵向速度，βk2为基于INS的坡度，k为时刻，aIMUX，k-1为k-1时刻的INS测量的X方向的加速度，g为重力加速度，Δt为采样时间间隔，θk-1为所述车辆俯仰角，
Figure DEST_PATH_IMAGE002
为当前高程下的车辆车重，
Figure DEST_PATH_IMAGE004
为当前车速下的车辆车重，
Figure FDA0003936628030000034
为当前坡度下的车辆车重；
获取INS混合概率，根据所述INS混合概率通过下式获得惯性状态估计函数：
Figure FDA0003936628030000035
其中，
Figure FDA0003936628030000036
为INS的状态估计值，k为时刻，μi2(k-1)为INS混合概率，
Figure FDA0003936628030000037
为惯性状态估计函数。
5.如权利要求1-4中任一项所述的车辆行驶盲区校准方法，其特征在于，所述根据交互多模型Kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正，包括：
根据交互多模型Kalman滤波器和所述状态估计函数对当前车辆数据进行筛选，确定有效测量值；
根据所述有效测量值通过下式确定最终状态估计函数：
Figure FDA0003936628030000038
其中，n(k)为在第一个循环中计算有效测量值的个数，k为时刻，Zk为累计到k-1时刻的观测值，θi为第i个来自实际目标的事件的有效度量，P为事件θi(k)的概率；
Figure FDA0003936628030000039
为最终状态估计函数；
根据所述最终状态估计函数通过下式确定车辆最终估计参数：
Figure FDA00039366280300000310
其中，k为时刻，
Figure FDA00039366280300000311
为最终状态估计函数，Ci(k)为预测概率，
Figure FDA00039366280300000312
为估计的最终车速、高程及坡度信息车辆最终估计参数；T为转置，hk为当前高程，vk为当前车速，βk为当前坡度。
6.一种车辆行驶盲区校准装置，其特征在于，所述车辆行驶盲区校准装置包括：
时间同步模块，用于在检测到当前车辆进入到信号盲区时，将全球定位系统GPS数据、惯性导航系统INS数据和车身传感器数据进行时间同步；
函数构建模块，用于在时间同步后，基于预设车辆动力学模型和INS建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数；
修正模块，用于根据交互多模型Kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正；
所述函数构建模块，还用于在时间同步后，获取车辆车重；将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程，并获得对应的动力状态估计函数；根据INS获得车辆俯仰角，根据所述车辆俯仰角获得惯性导航高程状态方程，并获得对应的惯性状态估计函数；
所述函数构建模块，还用于将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程：
Figure FDA0003936628030000041
其中，hk1为基于预设车辆动力学模型的高程，vk1为基于预设车辆动力学模型的纵向速度，βk1为基于预设车辆动力学模型的坡度，Δt为采样时间间隔，
Figure FDA0003936628030000042
为纵向加速度，
Figure FDA0003936628030000043
为坡度变化率，
Figure FDA0003936628030000044
为当前高程下的车辆车重，
Figure FDA0003936628030000045
为当前车速下的车辆车重，
Figure FDA0003936628030000046
为当前坡度下的车辆车重；
获取动力模型混合概率，根据所述动力模型混合概率通过下式获得动力状态估计函数：
Figure FDA0003936628030000047
其中，
Figure FDA0003936628030000048
为预设车辆动力学模型的状态估计值，k为时刻，μi1(k-1)为动力模型混合概率，
Figure FDA0003936628030000049
为车辆运动状态的动力状态估计函数。
7.一种车辆行驶盲区校准设备，其特征在于，所述车辆行驶盲区校准设备包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的车辆行驶盲区校准程序，所述车辆行驶盲区校准程序配置为实现如权利要求1至5中任一项所述的车辆行驶盲区校准方法的步骤。
8.一种存储介质，其特征在于，所述存储介质上存储有车辆行驶盲区校准程序，所述车辆行驶盲区校准程序被处理器执行时实现如权利要求1至5中任一项所述的车辆行驶盲区校准方法的步骤。
CN202011408240.7A 2020-12-03 2020-12-03 车辆行驶盲区校准方法、装置、设备及存储介质 Active CN112378410B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011408240.7A CN112378410B (zh) 2020-12-03 2020-12-03 车辆行驶盲区校准方法、装置、设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011408240.7A CN112378410B (zh) 2020-12-03 2020-12-03 车辆行驶盲区校准方法、装置、设备及存储介质

Publications (2)

Publication Number Publication Date
CN112378410A CN112378410A (zh) 2021-02-19
CN112378410B true CN112378410B (zh) 2023-03-24

Family

ID=74590452

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011408240.7A Active CN112378410B (zh) 2020-12-03 2020-12-03 车辆行驶盲区校准方法、装置、设备及存储介质

Country Status (1)

Country Link
CN (1) CN112378410B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113160589B (zh) * 2021-03-04 2024-05-28 腾讯科技（深圳）有限公司 车辆预警方法及装置、电子设备、计算机可读存储介质
CN113242104B (zh) * 2021-04-30 2022-11-15 广州文远知行科技有限公司 基于无信号区域的时钟控制方法、装置、设备及存储介质
CN113820732A (zh) * 2021-08-06 2021-12-21 荣耀终端有限公司 一种导航方法及装置
CN114125721A (zh) * 2021-11-16 2022-03-01 长江师范学院 基础设施受限场景下基于车辆运动状态的车辆定位方法
CN114526746B (zh) * 2022-03-15 2024-09-06 智道网联科技(北京)有限公司 高精地图车道线的生成方法、装置、设备及存储介质

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5906655A (en) * 1997-04-02 1999-05-25 Caterpillar Inc. Method for monitoring integrity of an integrated GPS and INU system
TW201126429A (en) * 2010-01-25 2011-08-01 Mingchi Inst Of Technology A method and system for processing GPS navigation information and INS navigation information by utilization of artificial neural network and Kalman wave filter
CN103983997A (zh) * 2014-05-09 2014-08-13 北京航空航天大学 一种抗gps失效的车载组合导航方法
CN107247824A (zh) * 2017-05-23 2017-10-13 重庆大学 考虑刹车和转弯影响的汽车质量‑道路坡度联合估计方法
CN108957496A (zh) * 2018-04-18 2018-12-07 广州市中海达测绘仪器有限公司 Uav抗gnss失效定位定向接收机及其应用方法
CN110285804A (zh) * 2019-06-26 2019-09-27 南京航空航天大学 基于相对运动模型约束的车辆协同导航方法
CN110727994A (zh) * 2019-10-28 2020-01-24 吉林大学 参数解耦的电动汽车质量与坡度估计方法
CN110985651A (zh) * 2019-12-04 2020-04-10 北京理工大学 一种基于预测的自动变速器多参数融合换挡策略

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP4140720B2 (ja) * 2004-01-14 2008-08-27 三菱電機株式会社 車両挙動再現システム
US7139651B2 (en) * 2004-03-05 2006-11-21 Modular Mining Systems, Inc. Multi-source positioning system for work machines

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5906655A (en) * 1997-04-02 1999-05-25 Caterpillar Inc. Method for monitoring integrity of an integrated GPS and INU system
TW201126429A (en) * 2010-01-25 2011-08-01 Mingchi Inst Of Technology A method and system for processing GPS navigation information and INS navigation information by utilization of artificial neural network and Kalman wave filter
CN103983997A (zh) * 2014-05-09 2014-08-13 北京航空航天大学 一种抗gps失效的车载组合导航方法
CN107247824A (zh) * 2017-05-23 2017-10-13 重庆大学 考虑刹车和转弯影响的汽车质量‑道路坡度联合估计方法
CN108957496A (zh) * 2018-04-18 2018-12-07 广州市中海达测绘仪器有限公司 Uav抗gnss失效定位定向接收机及其应用方法
CN110285804A (zh) * 2019-06-26 2019-09-27 南京航空航天大学 基于相对运动模型约束的车辆协同导航方法
CN110727994A (zh) * 2019-10-28 2020-01-24 吉林大学 参数解耦的电动汽车质量与坡度估计方法
CN110985651A (zh) * 2019-12-04 2020-04-10 北京理工大学 一种基于预测的自动变速器多参数融合换挡策略

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
MIMU/GPS Integrated Navigation Filtering Algorithm under the Condition of Satellite Missing;Fujian Zhang 等;《 2018 IEEE CSAA Guidance, Navigation and Control Conference (CGNCC)》;20200502;第1-5页 *
城市环境下车辆智能融合定位技术研究;徐启敏;《中国优秀博硕士学位论文全文数据库(博士)工程科技Ⅱ辑》;20200315(第03期);第C035-13页 *

Also Published As

Publication number Publication date
CN112378410A (zh) 2021-02-19

Similar Documents

Publication Publication Date Title
CN112378410B (zh) 车辆行驶盲区校准方法、装置、设备及存储介质
CN113945206B (zh) 一种基于多传感器融合的定位方法及装置
EP4382864A1 (en) Positioning method and positioning apparatus
CN106840179B (zh) 一种基于多传感器信息融合的智能车定位方法
CN107132563B (zh) 一种里程计结合双天线差分gnss的组合导航方法
CN111595592B (zh) 一种自适应巡航控制系统性能测评方法
CN101099076B (zh) 基于运动学模型的组合惯性导航系统
US8775063B2 (en) System and method of lane path estimation using sensor fusion
CN112505737B (zh) 一种gnss/ins组合导航方法
CN109946730A (zh) 一种车路协同下基于超宽带的车辆高可靠融合定位方法
CN107247275B (zh) 基于公交车的城市gnss脆弱性监测系统及其方法
US11117590B2 (en) Method and system for determining effective wind speeds for motor vehicles
US20090018772A1 (en) Position Sensing Device And Method
CN102680002B (zh) 汽车用微机械陀螺零点电压的在线标定方法
US11946746B2 (en) Method for satellite-based detection of a vehicle location by means of a motion and location sensor
CN112346103A (zh) 基于v2x的智能网联汽车动态协同定位方法与装置
CN107274721A (zh) 一种智能交通系统中多车辆协作定位方法
CN113219507A (zh) 基于rt3000的自动驾驶车辆感知融合数据的精度测量方法
Baek et al. Accurate vehicle position estimation using a Kalman filter and neural network-based approach
CN112188386A (zh) 一种基于etc信号强度的车辆定位方法
Jensen et al. Mass estimation of ground vehicles based on longitudinal dynamics using loosely coupled integrated navigation system and CAN-bus data with model parameter estimation
CN110617815A (zh) 一种自动驾驶监控告警的方法和装置
Tao et al. Comparative evaluation of Kalman filters and motion models in vehicular state estimation and path prediction
CN113115230B (zh) 一种基于信息物理系统的车辆广播通信控制方法
CN102706364B (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