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

[0001]
本发明涉及车辆导航技术领域，尤其涉及一种车辆行驶盲区校准方法、装置、设备及存储介质。


背景技术：

[0002]
随着城市化进程加速，越来越多的高层建筑及立体交通开始出现，随之也为全球定位系统(global positioning system，gps)定位导航的车辆带来了更多的信号盲区，由于建筑物的遮挡，使用gps定位导航的车辆在通过一些高架桥等立体交叉路段时，gps接收机无法接收到卫星信号或者接受信号质量较差；即便对于高精度的ins-gps组合导航定位方法，如果在高大建筑、高架桥等遮挡gps信号的情况下，只能单独采用惯性导航对定位结果进行修正；惯性导航设备在长时间内将无可避免地出现累计误差的情况，使导航精度降低，甚至出现无法导航状态，无法为车辆驾驶系统提供准确的平面及高程信息输入。
[0003]
现有方案是通过对基于超宽带(ultra wide band，uwb)的短距离高精度无线定位信息、北斗卫星导航系统(beidou navigation satellite system，bds)+gps的双模全球导航卫星系统(global navigation satellite system，gnss)定位信息融合航位推算信息，不同信号源定位数据不断融合进行定位，或者通过导航模组进行定位讯号状态的确定，但是存在对外围基础设施要求高，成本较高，由于各个定位技术的时间不同步存在，定位数据缺乏有效性，定位精度较差的缺陷。


技术实现要素：

[0004]
本发明的主要目的在于提供一种车辆行驶盲区校准方法、装置、设备及存储介质，旨在解决现有技术中依赖外围通信基础设施，成本较高，数据缺乏有效性，导致定位精度较差的技术问题。
[0005]
第一方面，本发明提供一种车辆行驶盲区校准方法，所述车辆行驶盲区校准方法包括以下步骤：
[0006]
在检测到当前车辆进入到信号盲区时，将全球定位系统gps数据、惯性导航系统ins数据和车身传感器数据进行时间同步；
[0007]
在时间同步后，基于预设车辆动力学模型和ins建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数；
[0008]
根据交互多模型kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正。
[0009]
可选地，所述在检测到当前车辆进入到信号盲区时，将全球定位系统gps、惯性导航系统ins和车身传感器进行时间同步，包括：
[0010]
在检测到当前车辆进入到信号盲区时，采集全球定位系统gps的gps数据中协调世界时utc的时间序列；
[0011]
采集惯性导航系统ins和车身传感器对应的计算机时间，获取所述时间序列与所
述计算机时间在相同时刻的时间偏差序列；
[0012]
根据所述时间偏差序列对gps、ins和车身传感器的时间同步。
[0013]
可选地，所述根据所述时间偏差序列对gps、ins和车身传感器的时间同步，包括：
[0014]
根据所述时间偏差序列通过下式对gps、ins和车身传感器的时间同步：
[0015]
t
pc
＝t
utc-δt
[0016]
其中，t
pc
为所述计算机时间，t
utc
为utc时间，δt为所述时间偏差序列。
[0017]
可选地，所述在时间同步后，基于预设车辆动力学模型和ins建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数，
[0018]
在时间同步后，获取车辆车重；
[0019]
将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程，并获得对应的动力状态估计函数；
[0020]
根据ins获得车辆俯仰角，根据所述车辆俯仰角获得惯性导航高程状态方程，并获得对应的惯性状态估计函数。
[0021]
可选地，所述将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程，并获得对应的动力状态估计函数，包括：
[0022]
将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程：
[0023][0024]
其中，h
k1
为基于预设车辆动力学模型的高程，v
k1
为基于预设车辆动力学模型的纵向速度，β
k1
为基于预设车辆动力学模型的坡度，δt为采样时间间隔，为纵向加速度，为坡度变化率，为当前高程下的车辆车重，为当前车速下的车辆车重，为当前坡度下的车辆车重；
[0025]
获取动力模型混合概率，根据所述动力模型混合概率通过下式获得动力状态估计函数：
[0026][0027]
其中，为预设车辆动力学模型的状态估计值，k为时刻，μ
i1
(k-1)为动力模型混合概率，为车辆运动状态的动力状态估计函数。
[0028]
可选地，所述根据ins获得车辆俯仰角，根据所述车辆俯仰角获得惯性导航高程状态方程，并获得对应的惯性状态估计函数，包括：
[0029]
获得ins提供的车辆俯仰角，根据所述车辆俯仰角通过下式获得惯性导航高程状态方程：
[0030][0031]
其中，h
k2
为基于ins的高程，v
k2
为基于ins的纵向速度，β
k2
为基于ins的坡度，k为时刻，a
imux,k-1
为k-1时刻的ins测量的x方向的加速度，g为重力加速度，δt为采样时间间隔，θ
k-1
为所述车辆俯仰角，为当前高程下的车辆车重，为当前车速下的车辆车重，为当前坡度下的车辆车重；
[0032]
获取ins混合概率，根据所述ins混合概率通过下式获得惯性状态估计函数：
[0033][0034]
其中，为ins的状态估计值，k为时刻，μ
i2
(k-1)为ins混合概率，为惯性状态估计函数。
[0035]
可选地，所述根据交互多模型kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正，包括：
[0036]
根据交互多模型kalman滤波器和所述状态估计函数对当前车辆数据进行筛选，确定有效测量值；
[0037]
根据所述有效测量值通过下式确定最终状态估计函数：
[0038][0039]
其中，n(k)为在第一个循环中计算有效测量值的个数，k为时刻，z
k
为累计到k-1时刻的观测值，θ
i
为第i个来自实际目标的事件的有效度量，p为事件θ
i
(k)的概率；为最终状态估计函数；
[0040]
根据所述最终状态估计函数通过下式确定车辆最终估计参数：
[0041][0042]
其中，k为时刻，为最终状态估计函数，c
i
(k)为预测概率，为估计的最终车速、高程及坡度信息车辆最终估计参数；
t
为时间，h
k
为当前高程，v
k
为当前车速，β
k
为当前坡度，为车辆最终估计参数。
[0043]
第二方面，为实现上述目的，本发明还提出一种车辆行驶盲区校准装置，所述车辆行驶盲区校准装置包括：
[0044]
时间同步模块，用于在检测到当前车辆进入到信号盲区时，将全球定位系统gps数据、惯性导航系统ins数据和车身传感器数据进行时间同步；
[0045]
函数构建模块，用于在时间同步后，基于预设车辆动力学模型和ins建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数；
[0046]
修正模块，用于根据交互多模型kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正。
[0047]
第三方面，为实现上述目的，本发明还提出一种车辆行驶盲区校准设备，所述车辆
行驶盲区校准设备包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的车辆行驶盲区校准程序，所述车辆行驶盲区校准程序配置为实现如权利要求上文所述的车辆行驶盲区校准方法的步骤。
[0048]
第四方面，为实现上述目的，本发明还提出一种存储介质，所述存储介质上存储有车辆行驶盲区校准程序，所述车辆行驶盲区校准程序被处理器执行时实现如上文所述的车辆行驶盲区校准方法的步骤。
[0049]
本发明提出的车辆行驶盲区校准方法，通过在检测到当前车辆进入到信号盲区时，将全球定位系统gps数据、惯性导航系统ins数据和车身传感器数据进行时间同步；在时间同步后，基于预设车辆动力学模型和ins建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数；根据交互多模型kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正，不会受到外界环境的影响进行高程信号的修正，并且信号的传输具有实时性，避免了长时间处于盲区下惯性导航系统出现信号漂移的问题；避免了在坡度变化率较大时导航估计结果表现出较为严重的延迟现象，保证了导航估计结果的估计精度和时间精度，导航修正数据具有实时性和有效性，可以在全工况道路条件下工作，提高了车辆导航的精度，并且实现成本较低，不需要搭建额外的基础设施。
附图说明
[0050]
图1为本发明实施例方案涉及的硬件运行环境的设备结构示意图；
[0051]
图2为本发明车辆行驶盲区校准方法第一实施例的流程示意图；
[0052]
图3为本发明车辆行驶盲区校准方法第二实施例的流程示意图；
[0053]
图4为本发明车辆行驶盲区校准方法第三实施例的流程示意图；
[0054]
图5为本发明车辆行驶盲区校准装置第一实施例的功能模块图。
[0055]
本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
[0056]
应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
[0057]
本发明实施例的解决方案主要是：通过在检测到当前车辆进入到信号盲区时，将全球定位系统gps数据、惯性导航系统ins数据和车身传感器数据进行时间同步；在时间同步后，基于预设车辆动力学模型和ins建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数；根据交互多模型kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正，能够不会受到外界环境的影响进行高程信号的修正，并且信号的传输具有实时性，避免了长时间处于盲区下惯性导航系统出现信号漂移的问题；避免了在坡度变化率较大时导航估计结果表现出较为严重的延迟现象，保证了导航估计结果的估计精度和时间精度，导航修正数据具有实时性和有效性，可以在全工况道路条件下工作，提高了车辆导航的精度，并且实现成本较低，不需要搭建额外的基础设施，解决了现有技术中依赖外围通信基础设施，成本较高，数据缺乏有效性，导致定位精度较差的技术问题。
[0058]
参照图1，图1为本发明实施例方案涉及的硬件运行环境的设备结构示意图。
[0059]
如图1所示，该车辆行驶盲区校准设备可以包括：处理器1001，例如cpu，通信总线1002、用户接口1003，网络接口1004，存储器1005；其中，通信总线1002用于实现这些组件之间的连接通信；用户接口1003可以包括显示屏(display)、输入单元比如键盘(keyboard)，可选用户接口1003还可以包括标准的有线接口、无线接口；网络接口1004可选的可以包括标准的有线接口、无线接口(如wi-fi接口)；存储器1005可以是高速ram存储器，也可以是稳定的存储器(non-volatile memory)，例如磁盘存储器；存储器1005可选的还可以是独立于前述处理器1001的存储装置。
[0060]
本领域技术人员可以理解，图1中示出的设备结构并不构成对该设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。
[0061]
在具体实现中，所述车辆行驶盲区校准设备可以为esp控制器，也可以为车载电脑，还可以为实现车辆行驶盲区校准功能的其他设备或终端，例如中控单元或远端服务器，本实施例对此不加以限制。
[0062]
如图1所示，作为一种存储介质的存储器1005中可以包括操作系统、网络通信模块、用户接口模块以及车辆行驶盲区校准程序。
[0063]
本发明设备通过处理器1001调用存储器1005中存储的车辆行驶盲区校准程序，并执行以下操作：
[0064]
在检测到当前车辆进入到信号盲区时，将全球定位系统gps数据、惯性导航系统ins数据和车身传感器数据进行时间同步；
[0065]
在时间同步后，基于预设车辆动力学模型和ins建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数；
[0066]
根据交互多模型kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正。
[0067]
进一步地，处理器1001可以调用存储器1005中存储的车辆行驶盲区校准程序，还执行以下操作：
[0068]
在检测到当前车辆进入到信号盲区时，采集全球定位系统gps的gps数据中协调世界时utc的时间序列；
[0069]
采集惯性导航系统ins和车身传感器对应的计算机时间，获取所述时间序列与所述计算机时间在相同时刻的时间偏差序列；
[0070]
根据所述时间偏差序列对gps、ins和车身传感器的时间同步。
[0071]
进一步地，处理器1001可以调用存储器1005中存储的车辆行驶盲区校准程序，还执行以下操作：
[0072]
根据所述时间偏差序列通过下式对gps、ins和车身传感器的时间同步：
[0073]
t
pc
＝t
utc-δt
[0074]
其中，t
pc
为所述计算机时间，t
utc
为utc时间，δt为所述时间偏差序列。
[0075]
进一步地，处理器1001可以调用存储器1005中存储的车辆行驶盲区校准程序，还执行以下操作：
[0076]
在时间同步后，获取车辆车重；
[0077]
将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程，并获得对应的动力状态估计函数；
[0078]
根据ins获得车辆俯仰角，根据所述车辆俯仰角获得惯性导航高程状态方程，并获得对应的惯性状态估计函数。
[0079]
进一步地，处理器1001可以调用存储器1005中存储的车辆行驶盲区校准程序，还执行以下操作：
[0080]
将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程：
[0081][0082]
其中，h
k1
为基于预设车辆动力学模型的高程，v
k1
为基于预设车辆动力学模型的纵向速度，β
k1
为基于预设车辆动力学模型的坡度，δt为采样时间间隔，为纵向加速度，为坡度变化率，为当前高程下的车辆车重，为当前车速下的车辆车重，为当前坡度下的车辆车重；
[0083]
获取动力模型混合概率，根据所述动力模型混合概率通过下式获得动力状态估计函数：
[0084][0085]
其中，为预设车辆动力学模型的状态估计值，k为时刻，μ
i1
(k-1)为动力模型混合概率，为车辆运动状态的动力状态估计函数。
[0086]
进一步地，处理器1001可以调用存储器1005中存储的车辆行驶盲区校准程序，还执行以下操作：
[0087]
获得ins提供的车辆俯仰角，根据所述车辆俯仰角通过下式获得惯性导航高程状态方程：
[0088][0089]
其中，h
k2
为基于ins的高程，v
k2
为基于ins的纵向速度，β
k2
为基于ins的坡度，k为时刻，a
imux,k-1
为k-1时刻的ins测量的x方向的加速度，g为重力加速度，δt为采样时间间隔，θ
k-1
为所述车辆俯仰角，为当前高程下的车辆车重，为当前车速下的车辆车重，为当前坡度下的车辆车重；
[0090]
获取ins混合概率，根据所述ins混合概率通过下式获得惯性状态估计函数：
[0091][0092]
其中，为ins的状态估计值，k为时刻，μ
i2
(k-1)为ins混合概率，为惯性状态估计函数。
[0093]
进一步地，处理器1001可以调用存储器1005中存储的车辆行驶盲区校准程序，还执行以下操作：
[0094]
根据交互多模型kalman滤波器和所述状态估计函数对当前车辆数据进行筛选，确定有效测量值；
[0095]
根据所述有效测量值通过下式确定最终状态估计函数：
[0096][0097]
其中，n(k)为在第一个循环中计算有效测量值的个数，k为时刻，z
k
为累计到k-1时刻的观测值，θ
i
为第i个来自实际目标的事件的有效度量，p为事件θ
i
(k)的概率；为最终状态估计函数；
[0098]
根据所述最终状态估计函数通过下式确定车辆最终估计参数：
[0099][0100]
其中，k为时刻，为最终状态估计函数，c
i
(k)为预测概率，为估计的最终车速、高程及坡度信息车辆最终估计参数；
t
为时间，h
k
为当前高程，v
k
为当前车速，β
k
为当前坡度，为车辆最终估计参数。
[0101]
本实施例通过上述方案，通过在检测到当前车辆进入到信号盲区时，将全球定位系统gps数据、惯性导航系统ins数据和车身传感器数据进行时间同步；在时间同步后，基于预设车辆动力学模型和ins建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数；根据交互多模型kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正，能够不会受到外界环境的影响进行高程信号的修正，并且信号的传输具有实时性，避免了长时间处于盲区下惯性导航系统出现信号漂移的问题；避免了在坡度变化率较大时导航估计结果表现出较为严重的延迟现象，保证了导航估计结果的估计精度和时间精度，导航修正数据具有实时性和有效性，可以在全工况道路条件下工作，提高了车辆导航的精度，并且实现成本较低，不需要搭建额外的基础设施。
[0102]
基于上述硬件结构，提出本发明车辆行驶盲区校准方法实施例。
[0103]
参照图2，图2为本发明车辆行驶盲区校准方法第一实施例的流程示意图。
[0104]
在第一实施例中，所述车辆行驶盲区校准方法包括以下步骤：
[0105]
步骤s10、在检测到当前车辆进入到信号盲区时，将全球定位系统gps数据、惯性导航系统ins数据和车身传感器数据进行时间同步。
[0106]
需要说明的是，在检测到当前车辆进入到信号盲区时，可以通过将gps数据，惯性导航系统(inertial navigation system，ins)数据，和车身控制器局域网络(controller area network，can)总线采集的车身传感器数据进行数据融合综合性判断当前车辆的定位，为了保证数据融合的精度，需要将这几种数据对应的传感器进行时间同步。
[0107]
步骤s20、在时间同步后，基于预设车辆动力学模型和ins建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数。
[0108]
可以理解的是，所述预设车辆动力学模型为预先设置的车辆动力学模型，在时间同步后，可以基于预设车辆动力学模型和ins建立高程、车速和道路坡度相关的状态方程，
同时可以获得对应的状态估计函数；所述状态方程反映不同高程、车速和道路坡度对应不同车身运动状态的方程，所述状态估计函数为每个状态对应的状态估计值初始条件函数。
[0109]
步骤s30、根据交互多模型kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正。
[0110]
应当理解的是，所述交互多模型kalman滤波器为用于并行处理不同的状态模型带来的目标运行模式的滤波器，每个模型通过混合前一时刻所有滤波器的状态估计函数来确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正。
[0111]
进一步的，所述步骤s30具体包括以下步骤：
[0112]
根据交互多模型kalman滤波器和所述状态估计函数对当前车辆数据进行筛选，确定有效测量值；
[0113]
根据所述有效测量值通过下式确定最终状态估计函数：
[0114][0115]
其中，n(k)为在第一个循环中计算有效测量值的个数，k为时刻，z
k
为累计到k-1时刻的观测值，θ
i
为第i个来自实际目标的事件的有效度量，p为事件θ
i
(k)的概率；为最终状态估计函数；
[0116]
根据所述最终状态估计函数通过下式确定车辆最终估计参数：
[0117][0118]
其中，k为时刻，为最终状态估计函数，c
i
(k)为预测概率，为估计的最终车速、高程及坡度信息车辆最终估计参数；t为时间，h
k
为当前高程，v
k
为当前车速，β
k
为当前坡度，为车辆最终估计参数。
[0119]
需要说明的是，在状态模型构建模块分别建立了基于动力学的状态模型及基于惯性导航系统的状态模型；最终的估计结果是对不同模型所得估计的混合，而不仅仅是在每一个时刻选择完全正确的模型来完成估计；在概率数据关联滤波模块中，要对有效量测进行筛选，将有效量测用于状态推断；并确定合理的混合原则，以最终得到精确的状态更新。
[0120]
在具体实现中，概率数据关联kalman滤波
[0121]
每个阶段有效的测量值被定义为：
[0122][0123]
其中，n(k)是在第一个循环中计算有效测量值的个数。
[0124]
针对动力学模型及惯性导航模型均可产生高程的量测值，模型的量测新息为
[0125][0126]
而只有满足下条件量测新息才能被当作有效值被保存下来。
[0127]
ε
i
(k)
t
s
j
(k)-1
ε
i
(k)＜g2[0128]
其中s
j
(k)是新息协方差，g是跟踪门的门限，对应跟踪门的体积。
[0129]
v(k)＝g2π|s
j
(k)|
1/2
[0130]
在上式中，g在“chi-square distribution”是可信度为98％的警戒值；如果利用
有效领域内的测量值，通过“有条件的平均”来推测状态，就可以做出更加值得信赖的推测。模型的状态更新为
[0131][0132]
z
k
是累积到k-1的观测值，θ
i
是第i个来自实际目标的事件的有效度量。所有有效的测量都是集群事件；假设上面一个目标只有一个测量值；而且，其他有效区域内的测量值是目标附近的封闭区域；θ
i
(k)，(i＝0，1，...，n(k))的事件是互斥事件；事件θ
i
(k)的概率，即第i个量测来自实际目标的概率，以及实际目标不是来自实际目标的概率，由以下等式定义：
[0133][0134][0135][0136]
b(k)＝n(k)(1-p
g
p
d
)[p
g
p
d
v(k)]-1
[0137]
n{ε
i
(k)；0，s(k)}表示正态概率密度函数，均值为0，方差为s(k)；p
g
是正确测量值落在有效范围内的概率，p
d
是gps检测到正确测量值的概率。
[0138]
kalman增益
[0139][0140][0141]
状态更新为：
[0142][0143][0144]
模型的协方差更新
[0145][0146]
高程识别值是两个模型状态估计值的加权结果，每个模型的加权系数是当前时刻模型正确描述高程信息状态的概率，即模型概率。
[0147]
在时刻模型基于量测计算模型概率为
[0148]
[0149][0150]
式中，是模型j的预测概率，λ
j
(k)是多量测新息的联合概率密度函数
[0151]
利用利用前述结果，在两个状态模型的滤波器估计结果的基础上加权叠加，得到总的状态估计和总的协方差估计。
[0152]
为所估计的最终车速、高程及坡度信息
[0153][0154][0155]
在获得了所述车辆最终估计参数后，可以通过显示输出模块进行输出显示，显示输出模块可包括显示器和高程信号输出模块，高程信号输入车载控制器，参与预测巡航控制系统对车身姿态和位置高程的判断。
[0156]
在上一个步骤得到了校准之后的高程信息
[0157]
x＝[h
k v
k β
k
]
t
[0158]
即，获知了当前车辆所处的高程、车速及坡度信息。
[0159]
本发明原义是将得到的精确数据信息输入到车载控制器，辅助进行巡航预测或高程位置的判断。本模块只是输出显示。
[0160]
在获得上述信息的精确值之后，易于获知车辆位置信息，
[0161]
例如，设车辆的起始位置是h0，由上述步骤得到的从t
k
到t
k+1
时刻的行驶距离δs＝v
k
δt信息坡度值为β
k
,则可以推算出t
k+1
时刻的车辆位置：
[0162]
h
k+1
＝h
k
+δs
·
sinβ
k
[0163]
同样，可由当前位置的坡度信息，参考动力学模型，获知车身俯仰角信息。
[0164]
本实施例只涉及高程信息的修正和校准，针对的是城市路况中的高架桥上，gps信号出现扰动的情况，本实施例基于交互多模型的gps高程盲区校准系统能准确实时地融合车身can提供的车辆动力学信息及惯性导航系统提供的加速度信息，来完成对gps高程信号的修正校准；通过对车身can信号的引入，对高程信号的修正不会受到外界环境的影响，而且信号的传输具有实时性，避免了长时间处于盲区下惯性导航系统出现信号漂移的问题；通过引入对量测新息有效性的判断，能够及时形成对高程定位信息不同模式的处理方式，在有效量测下完成了高程的校准，提高了结果的可信；传感器的时间同步能够增加最终的融合精度；在构建状态方程时，考虑了道路坡度自身变化率信息，避免了在坡度变化率较大时估计结果表现出较为严重的延迟现象，使估计结果在估计精度和时间精度两个方面同时取得较好的效果，并且为配备组合导航系统的车辆对于车身姿态及位置高程信息的获取提供一个新的框架与接口，如需要获知上述信息，只需将车身can信号及组合导航接口引入，加快了算法在智能驾驶方程的推进；该系统可以在全工况道路条件下工作，提高了车辆姿态及位置高程的识别范围及工作效果；本发明通过实车实验，对利用车辆高程信息的预测巡航系统的节油效果产生了有益影响
[0165]
本实施例通过上述方案，通过在检测到当前车辆进入到信号盲区时，将全球定位
系统gps数据、惯性导航系统ins数据和车身传感器数据进行时间同步；在时间同步后，基于预设车辆动力学模型和ins建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数；根据交互多模型kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正，能够不受到外界环境的影响进行高程信号的修正，并且信号的传输具有实时性，避免了长时间处于盲区下惯性导航系统出现信号漂移的问题；避免了在坡度变化率较大时导航估计结果表现出较为严重的延迟现象，保证了导航估计结果的估计精度和时间精度，导航修正数据具有实时性和有效性，可以在全工况道路条件下工作，提高了车辆导航的精度，并且实现成本较低，不需要搭建额外的基础设施。
[0166]
进一步地，图3为本发明车辆行驶盲区校准方法第二实施例的流程示意图，如图3所示，基于第一实施例提出本发明车辆行驶盲区校准方法第二实施例，在本实施例中，所述步骤s10具体包括以下步骤：
[0167]
步骤s11、在检测到当前车辆进入到信号盲区时，采集全球定位系统gps的gps数据中协调世界时utc的时间序列。
[0168]
可以理解的是，由于受不同传感器采用的时间系统不相同、采样频率不相同和不同传感器在无线电传输过程中存在不同时间延迟等因素的影响，导致运动高程测量结果出现因时间不同步引起的失真现象，因此为了实现gps信号盲区高程实时重建，必须要求gps、惯性导航系统和车辆动力模型时间同步，在检测到当前车辆进入到信号盲区时，由于gps是有时标的系统，可以采集gps数据中的协调世界时(universal time coordinated，utc)。
[0169]
步骤s12、采集惯性导航系统ins和车身传感器对应的计算机时间，获取所述时间序列与所述计算机时间在相同时刻的时间偏差序列。
[0170]
需要说明的是，所述ins和车身传感器对应有计算机时间，通过将所述时间序列与所述计算机时间进行对比能够确定在相同时刻的时间偏差序列。
[0171]
步骤s13、根据所述时间偏差序列对gps、ins和车身传感器的时间同步。
[0172]
应当理解的是，利用所述实际偏差序列能够消除时间序列与计算机时间的偏差，从而实现时间数据的统一。
[0173]
进一步的，所述s13具体包括以下步骤：
[0174]
根据所述时间偏差序列通过下式对gps、ins和车身传感器的时间同步：
[0175]
t
pc
＝t
utc-δt
[0176]
其中，t
pc
为所述计算机时间，t
utc
为utc时间，δt为所述时间偏差序列。
[0177]
可以理解的是，利用时间偏差序列，可以初步实现时间数据的统一，将偏差序列保存为δt，通过消除时间偏差可以实现gps数据、ins数据及车身can采集的传感器数据的时间统一。
[0178]
本实施例通过上述方案，通过在检测到当前车辆进入到信号盲区时，采集全球定位系统gps的gps数据中协调世界时utc的时间序列；采集惯性导航系统ins和车身传感器对应的计算机时间，获取所述时间序列与所述计算机时间在相同时刻的时间偏差序列；根据所述时间偏差序列对gps、ins和车身传感器的时间同步，能够提高数据融合的精度，实现了时间的统一，保证了导航估计结果的估计精度和时间精度，导航修正数据具有实时性和有效性，可以在全工况道路条件下工作，提高了车辆导航的精度。
[0179]
进一步地，图4为本发明车辆行驶盲区校准方法第三实施例的流程示意图，如图4所示，基于第一实施例提出本发明车辆行驶盲区校准方法第三实施例，在本实施例中，所述步骤s20具体包括以下步骤：
[0180]
步骤s21、在时间同步后，获取车辆车重。
[0181]
需要说明的是，所述车辆车重为车辆的当前质量估计，因为只有对车辆质量做出有效估计，才能保证后期在车辆动力学模型中对定位高程估计的准确性。
[0182]
在具体实现中，可以在基于惯性导航系统的汽车质量估计模块利用惯性导航系统及车辆动力学特性，对车辆质量做出估计；即惯性导航系统可以提供三个轴向的加速度及角加速度的值，一般令汽车行驶方向为x轴，垂直于行驶路面为z轴；沿坡道方向测得的分量(x轴)是车辆运动加速度a与重力加速度常量沿坡道分量的和，沿垂直于坡道方向上测得的分量(z轴)是重力加速度垂直于坡道的分量；则
[0183][0184]
a
imuz
＝g cosβ
[0185]
其中，a
imux
和a
imuz
分别是惯性导航系统测量的x方向和z方向的加速度，为纵向加速度，g是重力加速度，β是道路坡度值。
[0186]
在实际道路坡度中，可以假设sinβ＝β，
[0187][0188]
利用惯性导航系统测量的车辆纵向加速度及车重可以表示为：
[0189][0190][0191]
式中，为发动机力矩驱动下的驱动加速度，m为整车质量、η
t
为传动效率、r
w
为车轮半径、i
g
为从发动机到车轮的总变速比(包括变速器速比和主减速比)，为风阻带来的减速度，c
d
，a
f
，ρ分别为风阻系数、车辆有效迎风面积和空气密度；f为摩擦阻力系数。
[0192]
步骤s22、将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程，并获得对应的动力状态估计函数。
[0193]
可以理解的是，将所述车辆车重代入预设车辆动力学模型中，可以获得高程、车速和道路坡度相关的动力学高程状态方程，所述动力学高程状态方程主要是运用车辆can总线采集驱动信息，即发动机力矩、档位等、车速等信息；该模型建立的最大优点是无需额外传感器信息，信号均由can总线提供；既能实时匹配车身信息，也能够避免额外的生产成本，所述动力状态估计函数为动力状态估计值初始条件函数。
[0194]
进一步的，所述步骤s22具体包括以下步骤：
[0195]
将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程：
[0196][0197]
其中，h
k1
为基于预设车辆动力学模型的高程，v
k1
为基于预设车辆动力学模型的纵向速度，β
k1
为基于预设车辆动力学模型的坡度，δt为采样时间间隔，为纵向加速度，为坡度变化率，为当前高程下的车辆车重，为当前车速下的车辆车重，为当前坡度下的车辆车重；
[0198]
获取动力模型混合概率，根据所述动力模型混合概率通过下式获得动力状态估计函数：
[0199][0200]
其中，为预设车辆动力学模型的状态估计值，k为时刻，μ
i1
(k-1)为动力模型混合概率，为车辆运动状态的动力状态估计函数。
[0201]
需要说明的是，在车辆行驶过程中，目标存在机动性，并且盲区的存在也使得基本的kalman滤波的结果并不精确；使用两个模型来描述行驶过程中可能的状态，并且通过有效的加权融合进行系统的状态估计，能够很好地克服单模型估计误差加大的问题；在状态模型定义模块中，分别利用车身can信号建立基于车辆动力学的状态模型，利用惯性导航系统测量信息建立惯性导航状态模型。
[0202]
在具体实现中，车辆所处的高程h(t)变化表现为车速和坡度值β
k
的组合：
[0203][0204]
由车辆动力学模型可知：
[0205][0206]
其中，g为重力加速度，c
r
是滚动阻力系数。
[0207][0208]
相对道路坡度而言，假设令sinβ＝β
[0209]
即
[0210][0211]
构建动力学模型的状态方程的状态变量：
[0212]
x1＝[x
k1
]＝[h
k1 v
k1 β
k1
]
t
[0213]
δt为采样时间间隔，为纵向加速度，为坡度变化率。
[0214]
[0215]
对上式两边求导，同时假设在相邻的采样时间间隔内车辆加速度变化率为零，且档位不变，可以得到坡度的变化率：
[0216][0217]
步骤s23、根据ins获得车辆俯仰角，根据所述车辆俯仰角获得惯性导航高程状态方程，并获得对应的惯性状态估计函数。
[0218]
应当理解的是，所述车辆俯仰角由ins采集提供，可以反映道路坡度的变化率，根据所述车辆俯仰角获得惯性导航高程状态方程，即基于ins的高程状态模型，所述惯性状态估计函数为惯性状态估计值初始条件函数。
[0219]
进一步的，所述步骤s23具体包括以下步骤：
[0220]
获得ins提供的车辆俯仰角，根据所述车辆俯仰角通过下式获得惯性导航高程状态方程：
[0221][0222]
其中，h
k2
为基于ins的高程，v
k2
为基于ins的纵向速度，β
k2
为基于ins的坡度，k为时刻，a
imux，k-1
为k-1时刻的ins测量的x方向的加速度，g为重力加速度，δt为采样时间间隔，θ
k-1
为所述车辆俯仰角，为当前高程下的车辆车重，为当前车速下的车辆车重，为当前坡度下的车辆车重；
[0223]
获取ins混合概率，根据所述ins混合概率通过下式获得惯性状态估计函数：
[0224][0225]
其中，为ins的状态估计值，k为时刻，μ
i2
(k-1)为ins混合概率，为惯性状态估计函数。
[0226]
需要说明的是，由于利用车辆动力学建立状态模型也存在弊端：比如转弯时的侧向运动和加速或减速工况下的车轮滑移也会使状态方程产生误差；在车辆上加装惯性导航系统可以增加可观测的量，提高状态估计的精度和稳定性；惯性导航系统安装在车辆重心位置可以提供车身运动状态及姿态信息，基于惯性导航系统可以获得另一个高程状态模型，即惯性导航高程状态模型。
[0227]
在具体实现中，状态变量：
[0228]
x2＝[x
k2
]＝[h
k2 v
k2 β
k2
]
t
[0229]
gps可以用来记录纬度、经度、旅行距离、高度和交流卫星的数量。其中，gps提供的高度信号恰是本发明中需要校准和修正的量；车辆的纵向速度即为车辆前进的车速信息，可以通过车身can直接获取；观测矩阵选取直观观测量车辆纵向速度及车辆高程信息：
[0230]
z
j
(k-1)＝h
j
(k-1)x
j
(k-1)+v
j
(k-1)
[0231]
其中，
[0232][0233][0234][0235]
相应地，应用车辆动力学模型中对车辆本身状态的描述及惯性导航对车身运动及姿态的测量，可以通过两个模型对车辆运动的高程和状态表示；设第j个模型表示的目标状态方程为：
[0236]
x
j
(k)＝φ
j
(k-1)x
j
(k-1)+g
j
(k-1)w
j
(k-1)
[0237]
式中，j＝1表示动力学模型，j＝2表示惯性导航模型，φ
j
为状态转移矩阵；g
j
为噪声驱动矩阵；h为观测矩阵；w
j
(k-1)和v(k-1)为均值为零、协方差矩阵各为q
j
和r
j
的白噪声序列。
[0238]
模型之间的转移由马尔科夫概率转移矩阵确定，概率转移矩阵如下：
[0239][0240]
概率转移矩阵中的元素pij(i，j＝1，2)表示由第i个模型转移到第j个模型的概率；由高程目标的两个状态的估计与上述动力学模型和惯性导航模型的模型概率μ
i
(k-1)得到交互模型的混合估计值和协方差。详细步骤如下：
[0241]
对模型1(动力学模型)来说：
[0242]
模型1的预测概率为：
[0243][0244]
其中，μ
i
(k-1
)是模型i在k-1时刻的概率。
[0245]
作用到模型1的混合概率为：
[0246][0247][0248]
于是，可以得到模型1的混合状态估计为：
[0249][0250]
是模型i的状态估计，μ
i1
(k-1)是模型i到模型1的混合概率。
[0251]
然后得到模型1的初始混合协方差估计为：
[0252][0253]
同理，对模型2来说，可以得到交互之后的模型2的初始条件：
[0254][0255][0256][0257][0258]
得到模型j的混合状态估计及混合协方差估计p
0j
(k-1|k-1)，之后便可将它们作为输入进行kalman滤波，来更新预测状态和滤波协方差p
j
。
[0259]
模型j的状态预测：
[0260][0261]
误差协方差预测：
[0262][0263]
新息协方差：
[0264][0265]
本实施例通过上述方案，通过在时间同步后，获取车辆车重；将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程，并获得对应的动力状态估计函数；根据ins获得车辆俯仰角，根据所述车辆俯仰角获得惯性导航高程状态方程，并获得对应的惯性状态估计函数；能够不受到外界环境的影响进行高程信号的修正，并且信号的传输具有实时性，避免了长时间处于盲区下惯性导航系统出现信号漂移的问题；避免了在坡度变化率较大时导航估计结果表现出较为严重的延迟现象，保证了导航估计结果的估计精度和时间精度，导航修正数据具有实时性和有效性，可以在全工况道
路条件下工作，提高了车辆导航的精度，并且实现成本较低，不需要搭建额外的基础设施。
[0266]
相应地，本发明进一步提供一种车辆行驶盲区校准装置。
[0267]
参照图5，图5为本发明车辆行驶盲区校准装置第一实施例的功能模块图。
[0268]
本发明车辆行驶盲区校准装置第一实施例中，该车辆行驶盲区校准装置包括：
[0269]
时间同步模块10，用于在检测到当前车辆进入到信号盲区时，将全球定位系统gps数据、惯性导航系统ins数据和车身传感器数据进行时间同步。
[0270]
函数构建模块20，用于在时间同步后，基于预设车辆动力学模型和ins建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数。
[0271]
修正模块30，用于根据交互多模型kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正。
[0272]
其中，车辆行驶盲区校准装置的各个功能模块实现的步骤可参照本发明车辆行驶盲区校准方法的各个实施例，此处不再赘述。
[0273]
此外，本发明实施例还提出一种存储介质，所述存储介质上存储有车辆行驶盲区校准程序，所述车辆行驶盲区校准程序被处理器执行时实现如下操作：
[0274]
在检测到当前车辆进入到信号盲区时，将全球定位系统gps数据、惯性导航系统ins数据和车身传感器数据进行时间同步；
[0275]
在时间同步后，基于预设车辆动力学模型和ins建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数；
[0276]
根据交互多模型kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正。
[0277]
进一步地，所述车辆行驶盲区校准程序被处理器执行时还实现如下操作：
[0278]
在检测到当前车辆进入到信号盲区时，采集全球定位系统gps的gps数据中协调世界时utc的时间序列；
[0279]
采集惯性导航系统ins和车身传感器对应的计算机时间，获取所述时间序列与所述计算机时间在相同时刻的时间偏差序列；
[0280]
根据所述时间偏差序列对gps、ins和车身传感器的时间同步。
[0281]
进一步地，所述车辆行驶盲区校准程序被处理器执行时还实现如下操作：
[0282]
根据所述时间偏差序列通过下式对gps、ins和车身传感器的时间同步：
[0283]
t
pc
＝t
utc-δt
[0284]
其中，t
pc
为所述计算机时间，t
utc
为utc时间，δt为所述时间偏差序列。
[0285]
进一步地，所述车辆行驶盲区校准程序被处理器执行时还实现如下操作：
[0286]
在时间同步后，获取车辆车重；
[0287]
将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程，并获得对应的动力状态估计函数；
[0288]
根据ins获得车辆俯仰角，根据所述车辆俯仰角获得惯性导航高程状态方程，并获得对应的惯性状态估计函数。
[0289]
进一步地，所述车辆行驶盲区校准程序被处理器执行时还实现如下操作：
[0290]
将所述车辆车重代入预设车辆动力学模型中，获得高程、车速和道路坡度相关的动力学高程状态方程：
[0291][0292]
其中，h
k1
为基于预设车辆动力学模型的高程，v
k1
为基于预设车辆动力学模型的纵向速度，β
k1
为基于预设车辆动力学模型的坡度，δt为采样时间间隔，为纵向加速度，为坡度变化率，为当前高程下的车辆车重，为当前车速下的车辆车重，为当前坡度下的车辆车重；
[0293]
获取动力模型混合概率，根据所述动力模型混合概率通过下式获得动力状态估计函数：
[0294][0295]
其中，为预设车辆动力学模型的状态估计值，k为时刻，μ
i1
(k-1)为动力模型混合概率，为车辆运动状态的动力状态估计函数。
[0296]
进一步地，所述车辆行驶盲区校准程序被处理器执行时还实现如下操作：
[0297]
获得ins提供的车辆俯仰角，根据所述车辆俯仰角通过下式获得惯性导航高程状态方程：
[0298][0299]
其中，h
k2
为基于ins的高程，v
k2
为基于ins的纵向速度，β
k2
为基于ins的坡度，k为时刻，a
imux,k-1
为k-1时刻的ins测量的x方向的加速度，g为重力加速度，δt为采样时间间隔，θ
k-1
为所述车辆俯仰角，为当前高程下的车辆车重，为当前车速下的车辆车重，为当前坡度下的车辆车重；
[0300]
获取ins混合概率，根据所述ins混合概率通过下式获得惯性状态估计函数：
[0301][0302]
其中，为ins的状态估计值，k为时刻，μ
i2
(k-1)为ins混合概率，为惯性状态估计函数。
[0303]
进一步地，所述车辆行驶盲区校准程序被处理器执行时还实现如下操作：
[0304]
根据交互多模型kalman滤波器和所述状态估计函数对当前车辆数据进行筛选，确定有效测量值；
[0305]
根据所述有效测量值通过下式确定最终状态估计函数：
[0306][0307]
其中，n(k)为在第一个循环中计算有效测量值的个数，k为时刻，z
k
为累计到k-1时
刻的观测值，θ
i
为第i个来自实际目标的事件的有效度量，p为事件θ
i
(k)的概率；为最终状态估计函数；
[0308]
根据所述最终状态估计函数通过下式确定车辆最终估计参数：
[0309][0310]
其中，k为时刻，为最终状态估计函数，c
i
(k)为预测概率，为估计的最终车速、高程及坡度信息车辆最终估计参数；
t
为时间，h
k
为当前高程，v
k
为当前车速，β
k
为当前坡度，为车辆最终估计参数。
[0311]
本实施例通过上述方案，通过在检测到当前车辆进入到信号盲区时，将全球定位系统gps数据、惯性导航系统ins数据和车身传感器数据进行时间同步；在时间同步后，基于预设车辆动力学模型和ins建立高程、车速和道路坡度相关的状态方程，并获得对应的状态估计函数；根据交互多模型kalman滤波器和所述状态估计函数确定车辆最终估计参数，根据所述车辆最终估计参数对当前导航结果进行修正，能够不会受到外界环境的影响进行高程信号的修正，并且信号的传输具有实时性，避免了长时间处于盲区下惯性导航系统出现信号漂移的问题；避免了在坡度变化率较大时导航估计结果表现出较为严重的延迟现象，保证了导航估计结果的估计精度和时间精度，导航修正数据具有实时性和有效性，可以在全工况道路条件下工作，提高了车辆导航的精度，并且实现成本较低，不需要搭建额外的基础设施。
[0312]
需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者系统不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者系统所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括该要素的过程、方法、物品或者系统中还存在另外的相同要素。
[0313]
上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。
[0314]
以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。