CN115973166A - 道路坡度自适应融合估计方法、装置、车辆及存储介质 - Google Patents

道路坡度自适应融合估计方法、装置、车辆及存储介质 Download PDF

Info

Publication number
CN115973166A
CN115973166A CN202211678630.5A CN202211678630A CN115973166A CN 115973166 A CN115973166 A CN 115973166A CN 202211678630 A CN202211678630 A CN 202211678630A CN 115973166 A CN115973166 A CN 115973166A
Authority
CN
China
Prior art keywords
slope
vehicle
gradient
speed
road
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.)
Pending
Application number
CN202211678630.5A
Other languages
English (en)
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.)
Tsinghua University
Original Assignee
Tsinghua University
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 Tsinghua University filed Critical Tsinghua University
Priority to CN202211678630.5A priority Critical patent/CN115973166A/zh
Publication of CN115973166A publication Critical patent/CN115973166A/zh
Pending legal-status Critical Current

Links

Images

Landscapes

  • Navigation (AREA)

Abstract

本申请涉及参数估计技术领域，特别涉及一种道路坡度自适应融合估计方法、装置、车辆及存储介质，其中，方法包括：识别车辆的定位信息得到速度矢量和俯仰角，分别根据速度矢量和俯仰角构建卡尔曼滤波器，设计相应的噪声计算方法，实现车辆所在道路第一坡度和第二坡度的自适应估计；基于预设动力学方程/运动学方程和车辆状态信息，以车辆俯仰角速度作为坡度变化率，建立坡度状态方程，并对噪声进行了合理的建模，实现车辆所在道路第三坡度和第四坡度的自适应估计；计算最优融合权重融合第一坡度至第四坡度中多个坡度，得到车辆当前所在道路最终坡度。由此，解决了相关技术在复杂路况，坡度变化较为频繁时难以实现准确快速的坡度估计的问题。

Description

道路坡度自适应融合估计方法、装置、车辆及存储介质
技术领域
本申请涉及参数估计技术领域，特别涉及一种道路坡度自适应融合估计方法、装置、车辆及存储介质。
背景技术
道路坡度是道路的基本信息之一，对于车辆的准确安全控制和道路系统的构建具有重要意义。对于车辆而言，道路坡度是构建高精度车辆动力学模型所必须的信息，也是现代汽车坡道辅助系统、起步换挡控制等众多安全系统的关键信息，尤其在复杂多变的道路工况下，对于道路坡度的快速准确估计可以提升车辆控制的稳定性，改善燃油经济性和驾驶舒适性等。
近年来随着智能电动汽车的发展，一些传统车辆上不具备的新型传感器系统给坡度估计带来了新的方案，典型代表为全球定位系统(Global Positioning System，GPS)。GPS独立于车辆系统之外，不受车辆固有特性影响，可实现较高精度的估计。但是GPS信号易受环境干扰，在一些场景如隧道、城市高楼间等地方信号质量下降甚至中断。因此，仅仅依赖GPS也是不可取的。此外，相关技术通常是基于动力学和运动学的方法实现道路坡度的快速估计，然而这两类方法均存在一定局限性，基于动力学的方法需要足够的激励，在车速较低或静止的工况下难以实现准确的坡度估计，基于运动学的方法对加速度信号依赖较大，易产生累计误差，并且这两类方法大多假设道路坡度是定值，并没有考虑道路坡度的变化率，在坡度变化较为频繁的工况下难以实现准确快速的坡度估计。
发明内容
本申请提供一种道路坡度自适应融合估计方法、装置、车辆及存储介质，以解决相关技术在坡度变化较为频繁的工况下难以实现准确快速的坡度估计，存在精度低、鲁棒性差等问题。
本申请第一方面实施例提供一种道路坡度自适应融合估计方法，包括以下步骤：
获取车辆的车辆状态信息和定位系统的定位信息；识别定位信息得到速度矢量和俯仰角，分别根据速度矢量和俯仰角构建第一和第二卡尔曼滤波器，设计相应的噪声计算方法，实现车辆当前所在道路第一坡度和第二坡度的自适应估计；
基于预设动力学方程和车辆状态信息，以车辆俯仰角速度作为坡度变化率，建立坡度状态方程，进而构建第三卡尔曼滤波器，并考虑车速、轮速和转向的影响对噪声进行建模，同时对低速工况进行处理，实现车辆当前所在道路第三坡度的自适应估计；
基于预设运动学方程和车辆状态信息，以车辆俯仰角速度作为坡度变化率，建立坡度状态方程，进而构建第四卡尔曼滤波器，并考虑弯道工况对噪声进行建模，实现车辆当前所在道路第四坡度的自适应估计；
根据定位系统的信号强度和第一坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差等，计算线性最小方差意义下的最优融合权重，融合第一坡度至第四坡度中的多个坡度，得到车辆当前所在道路最终坡度。
可选地，在本申请的一个实施例中，所述根据定位系统的信号强度和第一坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差等，计算线性最小方差意义下的最优融合权重，融合第一坡度至第四坡度中的多个坡度，得到所述车辆当前所在道路最终坡度，包括：若所述信号强度大于预设强度，则根据根据定位系统的信号强度和第一坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差等，计算线性最小方差意义下的最优融合权重，根据所述第一坡度至第四坡度和各自权重进行最优加权融合，得到所述最终坡度；若所述信号强度小于或等于所述预设强度，则根据定位系统的信号强度和第三坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差等，计算线性最小方差意义下的最优融合权重，根据所述第三坡度、所述第四坡度和各自权重进行加权融合，得到所述最终坡度。
可选地，在本申请的一个实施例中，所述最终坡度的计算公式为：
Figure BDA0004018173270000021
其中，θFus为最终坡度，λi为第i坡度的权重，
Figure BDA0004018173270000022
为第i坡度；
所述第i坡度的权重λi的计算公式为：
λ＝[λ1，λ2，λ3，λ4]＝Λ-1e(eTΛ-1e)-1
其中，e＝[In，...，In]T为4n×n的矩阵，n为估计的状态量的个数，Λ由下式计算：
Figure BDA0004018173270000023
可选地，在本申请的一个实施例中，所述分别根据所述速度矢量和所述俯仰角计算所述车辆当前所在道路的第一坡度和第二坡度，包括：分别根据速度矢量和俯仰角构建第一和第二卡尔曼滤波器，设计相应的噪声计算方法，实现车辆当前所在道路第一坡度和第二坡度的自适应估计。
可选地，在本申请的一个实施例中，所述第三坡度的计算公式为：
Figure BDA0004018173270000031
其中，g为重力加速度，Fx为轮胎所受的地面纵向力，m为整车质量，Vx为纵向车速，Vy为侧向车速，ψ为横摆角，Froll为滚动阻力，Fair为空气阻力。
所述以车辆俯仰角速度作为坡度变化率，建立坡度状态方程为：
Figure BDA0004018173270000032
其中，
Figure BDA0004018173270000033
为车辆俯仰角速度，k为采样时刻，x3为第三坡度，dT为采样间隔，w3为随机噪声。
所述低速处理方法为：
θ3＝θ3*tanh(|Vx|)
所述噪声计算方法为：
Figure BDA0004018173270000034
其中，RDyn是第三坡度卡尔曼滤波器的测量噪声，C1，C2，C3，C4为标定常数，δ为方向盘转角，Wij，i∈{F，R}，j∈{L，R}是轮速，ε3是与随机噪声相关的常数。
可选地，在本申请的一个实施例中，所述第四坡度的计算公式为：
Figure BDA0004018173270000035
其中，ax为整车纵向加速度的量测值，ξ1为噪声。
所述噪声计算方法为：
RKnt＝P2*tanh(P1*(|δ|+ξ2))+ε4
其中，RKnt为第四坡度卡尔曼滤波器的测量噪声，δ为方向盘转角，P1，P2为标定常数，ξ2和ε4是为保证计算稳定而设置的常数。
所述以车辆俯仰角速度作为坡度变化率，建立坡度状态方程为：
Figure BDA0004018173270000036
其中，x4为第四坡度，w4为随机噪声。
本申请第二方面实施例提供一种道路坡度自适应融合估计装置，包括：获取模块，用于获取车辆的车辆状态信息和定位系统的定位信息；识别模块，识别定位信息得到速度矢量和俯仰角，分别根据速度矢量和俯仰角构建第一和第二卡尔曼滤波器，设计相应的噪声计算方法，实现车辆当前所在道路第一坡度和第二坡度的自适应估计；计算模块，基于预设动力学方程和车辆状态信息，以车辆俯仰角速度作为坡度变化率，建立坡度状态方程，进而构建第三卡尔曼滤波器，并考虑车速、轮速和转向的影响对噪声进行建模，同时对低速工况进行处理，实现车辆当前所在道路第三坡度的自适应估计；基于预设运动学方程和车辆状态信息，以车辆俯仰角速度作为坡度变化率，建立坡度状态方程，进而构建第四卡尔曼滤波器，并考虑弯道工况对噪声进行建模，实现车辆当前所在道路第四坡度的自适应估计；融合模块，根据定位系统的信号强度和第一坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差等，计算线性最小方差意义下的最优融合权重，融合第一坡度至第四坡度中的多个坡度，得到车辆当前所在道路最终坡度。
可选地，在本申请的一个实施例中，所述融合模块进一步用于若所述信号强度大于预设强度，则根据根据定位系统的信号强度和第一坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差等，计算线性最小方差意义下的最优融合权重，根据所述第一坡度至第四坡度和各自权重进行最优加权融合，得到所述最终坡度；若所述信号强度小于或等于所述预设强度，则根据定位系统的信号强度和第三坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差等，计算线性最小方差意义下的最优融合权重，根据所述第三坡度、所述第四坡度和各自权重进行加权融合，得到所述最终坡度。
可选地，在本申请的一个实施例中，所述最终坡度的计算公式为：
Figure BDA0004018173270000041
其中，θFus为最终坡度，λi为第i坡度的权重，
Figure BDA0004018173270000042
为第i坡度，第i坡度的权重λi的计算公式为：
λ＝[λ1，λ2，λ3，λ4]＝Λ-1e(eTΛ-1e)-1
其中，e＝[In，...，In]T为4n×n的矩阵，n为估计的状态量的个数，Λ由下式计算：
Figure BDA0004018173270000043
可选地，在本申请的一个实施例中，所述计算模块进一步用于获取多个方向的速度矢量和总的空间矢量；根据所述多个方向的速度矢量合成水平速度矢量，根据所述水平速度矢量和所述总的空间矢量计算所述第一坡度，并将所述俯仰角作为所述第二坡度；
所述噪声计算方法为将噪声σ近似为地向速度噪声σD与水平速度的比值：
Figure BDA0004018173270000044
其中，VN，VE分别为北向速度和东向速度，ε2为一较小常数，σ为标准差。
可选地，在本申请的一个实施例中，所述第三坡度的计算公式为：
Figure BDA0004018173270000051
其中，g为重力加速度，Fx为轮胎所受的地面纵向力，m为整车质量，Vx为纵向车速，Vy为侧向车速，ψ为横摆角，Froll为滚动阻力，Fair为空气阻力。
所述以车辆俯仰角速度作为坡度变化率，建立坡度状态方程为：
Figure BDA0004018173270000052
其中，
Figure BDA0004018173270000053
为车辆俯仰角速度，k为采样时刻，x3为第三坡度，dT为采样间隔，w3为随机噪声。
所述低速处理方法为：
θ3＝θ3*tanh(|Vx|)
所述噪声计算方法为：
Figure BDA0004018173270000054
其中，RDyn是第三坡度卡尔曼滤波器的测量噪声，C1，C2，C3，C4为标定常数，δ为方向盘转角，Wij，i∈{F，R｝，j∈{L，R｝是轮速，ε3是与随机噪声相关的常数。
可选地，在本申请的一个实施例中，所述第四坡度的计算公式为：
Figure BDA0004018173270000055
其中，ax为整车纵向加速度的量测值，ξ1为噪声。
所述噪声计算方法为：
RKnt＝P2*tanh(P1*(|δ|+ξ2))+ε4
其中，RKnt为第四坡度卡尔曼滤波器的测量噪声，δ为方向盘转角，P1，P2为标定常数，ξ2和ε4是为保证计算稳定而设置的常数。
所述以车辆俯仰角速度作为坡度变化率，建立坡度状态方程为：
Figure BDA0004018173270000056
其中，x4为第四坡度，w4为随机噪声。
本申请第三方面实施例提供一种车辆，包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述程序，以实现如上述实施例所述的道路坡度自适应融合估计方法。
本申请第四方面实施例提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行，以用于实现如上述实施例所述的道路坡度自适应融合估计方法。
由此，本申请至少具有如下有益效果：
通过识别定位信息得到速度矢量和俯仰角，利用速度矢量和俯仰角并计算车辆当前所在道路的第一坡度和第二坡度，有效发挥其高精度、不受车辆系统影响的优势，并通过动力学和运动学的方法，计算车辆当前所在道路的第三坡度和第四坡度，能够在定位不稳定的情况下保证足够精度、不中断的坡度估计；基于多传感器卡尔曼滤波最优融合理论和定位系统的信号强度，对第一坡度至第四坡度中的多个坡度进行融合，有效应对坡度多变的工况，在保证坡度的高精度的同时，实现坡度高实时性、强鲁棒性的估计。由此，解决了相关技术在复杂路况，坡度变化较为频繁时难以实现准确快速的坡度估计的问题，充分发挥了每种方法的优势区间，扩展了总体算法的工作范围，从而在复杂工况下可以提供合理的坡度估计。本申请附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本申请的实践了解到。
附图说明
本申请上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：
图1为根据本申请实施例提供的一种道路坡度自适应融合估计方法的流程图；
图2为根据本申请实施例提供的坡度估计系统框图；
图3为根据本申请实施例提供的一种道路坡度自适应融合估计装置的方框示意图；
图4为根据本申请实施例提供的车辆的结构示意图。
附图标记说明：获取模块-100、识别模块-200、计算模块-300、融合模块-400、存储器-401、处理器-402、通信接口-403。
具体实施方式
下面详细描述本申请的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本申请，而不能理解为对本申请的限制。
目前道路坡度估计方法主要分为两大类：基于动力学的方法和基于运动学的方法。
基于动力学的方法主要基于整车动力学模型，利用卡尔曼滤波、扩展卡尔曼滤波、递归最小二乘等方法对道路坡度进行估计。基于车辆动力学的方法在激励足够、车辆状态较为稳定时可获得精度较高的估计值。但是，由于依赖较高精度的动力学模型，通常需要较多信号引入以构建动力学模型，这带来了较多的问题。一方面，模型需求的信号可能无法直接从传感器获得例如轮胎力等，需要进行额外的估计，这加大了计算负担；另一方面，任意传感器信号的故障都可能导致整体算法的崩溃，较多信号的引入降低了鲁棒性，进而导致基于动力学的方法易受外界干扰，可靠性较差。此外，基于动力学的方法需要足够的激励，在车速较低或静止的工况下难以实现准确的坡度估计。
基于运动学的方法主要基于运动学方程，通过对信号进行滤波处理等即可用于坡度计算，一般只需要IMU(Inertial Measurement Unit)信号、车速信号等少量信号，且这些信号容易获得。信号少、模型简单使得基于运动学的方法具有较强的鲁棒性，计算负担小，可实现一定精度的坡度估计。但是基于运动学的方法问题也十分突出，例如，在基于加速度方程的方法中，其对加速度信号依赖较大，而加速度传感器通常噪声较大，尤其是低速车辆振动、俯仰等运动均可对其造成影响；在基于俯仰角速度积分的方法中，车辆加减速时产生的俯仰运动以及随机噪声和IMU传感器零偏的存在使得此方法易产生累计误差。因此，基于运动学的方法一般精度较低。
下面参考附图描述本申请实施例的道路坡度自适应融合估计方法、装置、车辆及存储介质。针对上述背景技术中提到的问题，本申请提供了一种道路坡度自适应融合估计方法，在该方法中，通过识别定位信息得到速度矢量和俯仰角，利用速度矢量和俯仰角并计算车辆当前所在道路的第一坡度和第二坡度，有效发挥其高精度、不受车辆系统影响的优势，并通过动力学和运动学的方法，计算车辆当前所在道路的第三坡度和第四坡度，能够在定位不稳定的情况下保证足够精度、不中断的坡度估计；基于多传感器卡尔曼滤波最优融合理论和定位系统的信号强度，对第一坡度至第四坡度中的多个坡度进行融合，有效应对坡度多变的工况，在保证坡度的高精度的同时，实现坡度高实时性、强鲁棒性的估计。由此，解决了相关技术在坡度变化较为频繁的工况下难以实现准确快速的坡度估计，存在精度低、鲁棒性差等问题。
具体而言，图1为本申请实施例所提供的一种道路坡度自适应融合估计方法的流程示意图。
如图1所示，该道路坡度自适应融合估计方法包括以下步骤：
在步骤S101中，获取车辆的车辆信息和定位系统的定位信息。
其中，定位系统可以根据实际情况具体选择，例如GPS、北斗等，不作具体限定，以下实施例中以GPS为例进行阐述。
在本申请实施例中，车辆信息可以为方向盘转角、轮速、车速、制动力矩、驱动力矩等。一方面综合考虑车速、轮速和转向对动力学方程的影响，对测量噪声进行了合理的建模；另一方面针对低速工况进行了处理，通过这些处理确定了动力学方法的正常工作区间，有效利用其高精度区间并方便后续融合算法处理。
在步骤S102中，识别定位信息得到速度矢量和俯仰角，分别根据速度矢量和俯仰角构建第一和第二卡尔曼滤波器，设计相应的噪声计算方法，实现车辆当前所在道路第一坡度和第二坡度的自适应估计。
可以理解的是，本申请实施例可以引入GPS并构建速度矢量法和俯仰角法两种方法，有效发挥其高精度、不受车辆系统影响的优势，填充运动学和动力学方法某些工况下无法正常工作的空白。
在本申请的一个实施例中，分别根据速度矢量和俯仰角计算车辆当前所在道路的第一坡度和第二坡度，包括：分别根据速度矢量和俯仰角构建第一和第二卡尔曼滤波器，设计相应的噪声计算方法，实现车辆当前所在道路第一坡度和第二坡度的自适应估计。以GPS为例，GPS是独立于车辆系统之外的系统，不受车辆固有特性、车辆运动等影响，且由于GPS速度精度较高，可以获得较为准确的坡度估计，并且计算简单，开销小。GPS在导航坐标系(又称“地理坐标系”，通常包括“北东地(NED)”和“东北天(ENU)”两类)下输出三个方向的速度矢量，以NED导航坐标系为例，北东速度合成矢量(即水平速度矢量)与总的空间矢量之间的夹角即第一坡度，计算公式为：
Figure BDA0004018173270000081
其中，θ1表示GPS速度矢量法估计的坡度，VD表示地向速度，VE和VN分别表示东向和北向速度，ε1是一个数值较小的常数，为保证数值计算稳定而设置。负号的设置是由于NED导航坐标系中，VD正方向朝下，而ISO车体垂向坐标系正方向朝上。
考虑到低速工况下GPS信号相对误差增大，此时第一坡度估计的精度也会下降，因此需对其噪声进行建模。本申请实施例可以将GPS速度矢量法的噪声σ(标准差)近似为地向速度噪声σD与水平速度的比值：
Figure BDA0004018173270000082
采用卡尔曼滤波算法对第一坡度值进行最优滤波处理，GPS速度矢量法计算值作为传感器测量值输入卡尔曼滤波器，构建的系统可以表示为：
x1(k)＝x1(k-1)+w1(k)
θ1＝x1(k)+v1(k)
其中，w1为过程噪声，v1是测量噪声，其方差为：
R＝E[v1(k)v1(k)T]＝σ2
进一步地，考虑到GPS天线一般安装在车身，因此，GPS输出的俯仰角可作为坡度估计的参考，其输出的角度是道路坡度角和车身俯仰角的叠加。由于IMU一般安装在车辆簧上部分，其测量的加速度信号受到坡度和车身俯仰的双重影响，因此GPS输出的俯仰角正适合作为加速度的补偿信号，此方法输出的第二坡度为：
θ2＝θGPS
其中，θ2为GPS俯仰角法估计的坡度，θGPS为GPS输出的俯仰角信号。
同样，本申请实施例可以将GPS俯仰角法输出的第二坡度经过一个卡尔曼滤波处理后即可输入至融合算法模块中，其中卡尔曼滤波系统方程与GPS速度矢量法一致：
x2(k)＝x2(k-1)+w2(k)
θ2＝x2(k)+v2(k)
其中，w2和v2分别为过程噪声和测量噪声，这两者数值较小，且跟其他因素关联不大，其方差可设置为一个较小的固定值，经过滤波后的坡度等将输入融合算法做融合估计。
在步骤S103中，基于预设动力学方程和车辆状态信息，以车辆俯仰角速度作为坡度变化率，建立坡度状态方程，进而构建第三卡尔曼滤波器，并考虑车速、轮速和转向的影响对噪声进行建模，同时对低速工况进行处理，实现车辆当前所在道路第三坡度的自适应估计；基于预设运动学方程和车辆状态信息，以车辆俯仰角速度作为坡度变化率，建立坡度状态方程，进而第四构建卡尔曼滤波器，并考虑弯道工况对噪声进行建模，实现车辆当前所在道路第四坡度的自适应估计。
可以理解的是，由于GPS信号易受干扰，不能保证全天候提供信号，因此采用传统车辆上的信号进行坡度估计是必要的。动力学方法作为坡度估计重要方法之一，在模型精度足够时坡度估计值准确度较高，但是需求信号较多且信号噪声叠加都难以实现较好的滤波处理，例如轮加速度的噪声较大，轮胎力的计算可能不准确；在静止或极低车速等激励不足的工况下无法估计。
本申请实施例考虑到动力学方法的这些缺陷，添加低速处理，综合考虑车速、轮速和转向对动力学方程的影响，对噪声进行合理建模，引入俯仰角速度作为坡度变化率，进而实现坡度的自适应、高实时性估计。对于动力学坡度估计方法，其基本依据为整车纵向动力学方程(预设动力学方式)：
Figure BDA0004018173270000091
其中，m为整车质量，Vx为纵向车速，Vy为侧向车速，ψ为横摆角。Fx为轮胎所受的地面纵向力，可以根据单轮模型计算：
Figure BDA0004018173270000092
其中，Td为驱动力矩，Tb是制动力矩，Iw为车轮转动惯量，Rw是车轮半径。
Fslope为坡度阻力，也是坡度估计的根源：
Fslope＝mgsinθD
其中，θD表示动力学方法估计的坡度。
Froll为滚动阻力，由于其值很小，一般忽略不计，η为滚动阻力系数：
Froll＝mgηcosθD≈mgη
Fair为空气阻力，在高速工况较为明显，CD是空气阻力系数，A为车辆迎风面积，ρair为空气密度：
Figure BDA0004018173270000101
因此，基于动力学方法计算得到车辆当前所在道路的第三坡度可以表示为：
Figure BDA0004018173270000102
进一步地，考虑到利用动力学方式估计车辆当前所在道路的第三坡度的方法依赖纵向激励，在低速或静止工况下十分不准确，噪声极大，需进行低速处理，本申请实施例可以采用双曲正切函数进行处理：
θ3＝θ3*tanh(|Vx|)
由于tanh函数在自变量大于0时函数值始终保持在0-1之间，且最终趋于1，因此在车速较低时，tanh函数值极小，此时动力学坡度也将被约束在较小值，从而减小对后续融合估计算法的影响，同时保证了估计的连续性，减轻估计结果跳动现象。经过计算后的坡度值作为一个卡尔曼滤波器的测量值，滤波后的结果将输入到融合算法中。考虑到此方法测量值包含车辆俯仰运动，因此，坡度的变化率应等于IMU输出的俯仰角速度的相反数，故滤波系统方程可以建模为：
Figure BDA0004018173270000103
θ3(k)＝x3(k)+v3(k)
其中，
Figure BDA0004018173270000104
为IMU输出的车辆俯仰角速度；x2为状态，即坡度；v2是测量噪声。
由于此方法参与信号众多，受噪声影响较大，因此需对其测量噪声进行建模，测量噪声的大小会影响滤波协方差的大小，进而影响其在融合算法中的权重，因此合理的建模是十分重要的。
在本申请实施例中，动力学方法测量噪声依据不同工况中车辆状态的不同，被建模为车速、方向盘转角、轮速的函数，其中，车速和轮速的影响主要体现在滑移率，滑移率较大时，轮胎力计算误差增大，动力学模型精度下降；此外，车速较小时，纵向激励较小，动力学特性体现不足；方向盘转角主要用于衡量车辆是否是直线行驶，在方向盘转角较大时，纵向动力学方程失真，测量噪声应急剧增大，以降低此方法的置信度，综合考虑这几方面，测量噪声设置为：
Figure BDA0004018173270000111
其中，中，C1，C2，C3，C4为标定常数，δ为方向盘转角，Wij，i∈{F，R}，j∈{L，R}是轮速，ε3是与随机噪声相关的常数。
进一步考虑，运动学方法也是坡度估计的重要方法之一，其主要优点是模型简单、信号容易获得，在静止工况下也可以实现坡度估计，并且具有较好的鲁棒性。但是作为其核心信号的加速度或俯仰角速度均含有复杂的噪声和传感器零偏，且这些问题难以处理，因此限制了运动学方法的精度，此外对于弯道工况，纵向运动学将失效。
考虑到运动学方法的这些缺陷，本申请实施例首先采用卡尔曼滤波进行噪声处理，并基于方向盘转角对弯道工况进行处理，在转角较大时赋予较大的测量噪声，从而降低此方法的置信度，减少其在后文融合算法中的权重；此外引入俯仰角速度作为坡度变化率，进而实现坡度的自适应、高实时性估计。
运动学坡度估计的基本依据为纵向加速度运动学方程(预设运动学方式)：
Figure BDA0004018173270000112
其中，ax为整车纵向加速度的量测值(即IMU输出的加速度，包含重力加速度的坡度分量影响)，ξ1为噪声，θ4表示运动学坡度的观测值。
故基于运动学方法计算车辆当前所在道路的第四坡度通过下式计算：
Figure BDA0004018173270000113
同理，计算后的第四坡度值作为卡尔曼滤波的测量值，同样需要将滤波后的结果输入到融合算法中，本申请实施例中利用运动学方式估计车辆当前所在道路的第四坡度的方法和动力学方法一样，运动学坡度中也包含车辆俯仰运动，因此其滤波系统方程与动力学方法一致：
Figure BDA0004018173270000114
θ4(k)＝x4(k)+v4(k)
运动学坡度估计同样需要对噪声进行建模，以增强算法的鲁棒性。由于IMU与车辆固定，因此测量的纵向加速度方向始终与车体坐标系的X轴正方向一致，这意味着运动学坡度估计对车辆直行要求较高，因此，本申请实施例可以将测量噪声设置为与方向盘转角相关，其建模如下：
RKnt＝P2*tanh(P1*(|δ|+ξ2))+ε4
其中，P1，P2为标定常数，ξ2和ε4是为保证计算稳定而设置的常数。
在步骤S104中，根据定位系统的信号强度和第一坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差等，计算线性最小方差意义下的最优融合权重，融合第一坡度至第四坡度中的多个坡度，得到车辆当前所在道路最终坡度。由于GPS信号易受干扰，无法保证全天候提供信号，本申请实施例可以根据定位系统的信号强度和车辆的当前道路工对运动学方法、动力学方法、GPS速度矢量法、GPS俯仰角法进行了最优融合，实现了基于GPS、运动学、动力学等方法的道路坡度自适应融合估计。一方面，每种方法的测量噪声大小影响其在融合算法中的权重，如果一种方法的测量噪声很大，则最终该方法在融合算法中的权重将会很小，反之则权重较大。这样使得进入失真区域的方法将获得很大的测量噪声，在融合算法中占比很小，在合适工作区间的方法则会获得一个最优权重，从而可以获得合理的、最优的融合估计结果；另一方面，最终的融合估计结果是多方法的加权融合，而非不同方法的切换，这避免了不同方法之间频繁切换导致的估计结果跳变问题。
在本申请的一个实施例中，根据定位系统的信号强度和第一坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差等，计算线性最小方差意义下的最优融合权重，融合第一坡度至第四坡度中的多个坡度，得到车辆当前所在道路最终坡度，包括：若信号强度大于预设强度，则根据根据定位系统的信号强度和第一坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差等，计算线性最小方差意义下的最优融合权重，根据第一坡度至第四坡度和各自权重进行最优加权融合，得到最终坡度；若信号强度小于或等于预设强度，则根据定位系统的信号强度和第三坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差等，计算线性最小方差意义下的最优融合权重，根据第三坡度、第四坡度和各自权重进行加权融合，得到最终坡度。
其中，预设强度可以根据实际情况进行设置，不做具体限定；本申请实施例可以在信号强度大于预设强度时，判定GPS可用；在信号强度小于或等于预设强度时，判定GPS失效。在实际执行过程中，本申请实施例可以在GPS可用时，融合动力学、运动学、GPS速度矢量法、俯仰角法四种方法，并以GPS方法为主，实现坡度的高精度融合估计；考虑到GPS易受环境干扰的特点，在GPS失效时只采用运动学方法和动力学方法进行融合估计，保证足够精度、不中断的坡度估计，并通过设计基于GPS方法的道路坡度观测值的接入、关断切换策略，充分发挥每种方法的优势区间，最大程度扩展总体算法工作范围，从而在各种复杂工况下均可以提供合理的坡度估计。
具体而言，如图2所示，本申请实施例的融合算法可以采用一种多传感器卡尔曼滤波融合算法，可实现线性最小方差意义下的最优融合估计。在GPS可用的情况下，将上文四种方法视为四个不同的传感器，分别采用卡尔曼滤波进行处理，利用卡尔曼滤波输出的估计状态、协方差、卡尔曼增益等进行最优融合，此融合具有两个特性：无偏性，即估计坡度的期望值等于实际坡度期望值；最优性，即融合滤波误差方差最小。对于GPS速度矢量法、GPS俯仰角法、运动学法、动力学法分别构建了相应的卡尔曼滤波方程，其可以表示为：
θ(k+1)＝Aθ(k)+Bu(k)+w(k)
yi(k)＝Hiθ(k)+vi(k)，i＝1,2,3,4
其中，A，B，H分别是系统矩阵、输入矩阵和测量矩阵，yi为第i个传感器测量值，w为过程噪声，均值为0，协方差为Q，vi为测量噪声，均值为0，协方差为Ri。
令θi，i＝1,2,3,4，分别为GPS速度矢量法、GPS俯仰角法、运动学法、动力学法的最优卡尔曼滤波值，可以由线性卡尔曼滤波方程组计算得到：
θi(k|k-1)＝Aθi(k-1|k-1)+Bu(k)
Pi(k|k-1)＝APi(k-1|k-1)AT+Q
Figure BDA0004018173270000131
θi(k|k)＝θi(k|k-1)+Ki(k)(yi(k)-Hi(k)x(k|k-1))
Pi(k|k)＝[I-Ki(k)Hi]Pi(k|k-1)
其中，Ki是第i个传感器的卡尔曼增益，Pi为第i个传感器的误差协方差。
为方便计算，假设上述4个传感器的w和vi互不相关，则第i和j个传感器的协方差可以由下式计算：
Pij(k|k)＝[I-Ki(k)Hi][APij(k-1|k-1)A+Q(k-1)ΓT][I-Kj(k)Hj]T
根据上式可以得出融合的权重，令λi为第i个传感器在融合估计中的权重，则λi可由下式计算：
λ＝[λ1，λ2，λ3，λ4]＝Λ-1e(eTΛ-1e)-1
式中，e＝[In，...，In]T是4n×n的矩阵，n为估计的状态量的个数，此处为1。
Λ可由下式计算：
Figure BDA0004018173270000132
最终，融合估计的坡度为：
Figure BDA0004018173270000133
此外，融合滤波器的误差协方差为：
Figure BDA0004018173270000141
综上，在本申请实施例的坡度融合估计中，有四种估计坡度来源，分别为GPS速度矢量法、GPS俯仰角法、运动学法、动力学法，每一种方法都建立了对应的卡尔曼滤波器并输出估计的状态、协方差、卡尔曼增益，并据此进行融合权重计算。例如，在弯道工况下，运动学方法和动力学方法无法准确计算坡度，这时通过赋予两者较大的测量噪声，使其在融合算法中的权重很小，而GPS的两种方法不受影响，可以获得较大的权重，最终实现弯道工况坡度的正常估计。
此外，在GPS信号可用时，由于GPS卡尔曼滤波算法的测量噪声较小，其在融合算法中权重较大，此时融合估计结果接近GPS方法，其精度较高；考虑到GPS信号易受干扰，可能在某些工况下中断，因此在GPS不可用时仅对运动学方法和动力学方法进行融合，此时这两种方法融合既可以保证坡度估计不中断，也可以输出一定精度的估计结果。
根据本申请实施例提出的道路坡度自适应融合估计方法，通过识别定位信息得到速度矢量和俯仰角，利用速度矢量和俯仰角并计算车辆当前所在道路的第一坡度和第二坡度，有效发挥其高精度、不受车辆系统影响的优势，并通过动力学和运动学的方法，计算车辆当前所在道路的第三坡度和第四坡度，能够在定位不稳定的情况下保证足够精度、不中断的坡度估计；基于多传感器卡尔曼滤波最优融合理论和定位系统的信号强度，对第一坡度至第四坡度中的多个坡度进行融合，有效应对坡度多变的工况，在保证坡度的高精度的同时，实现坡度高实时性、强鲁棒性的估计。由此，解决了相关技术在复杂路况，坡度变化较为频繁时难以实现准确快速的坡度估计的问题，充分发挥了每种方法的优势区间，扩展了总体算法的工作范围，从而在复杂工况下可以提供合理的坡度估计。
其次参照附图描述根据本申请实施例提出的一种道路坡度自适应融合估计装置。
图3是本申请实施例的一种道路坡度自适应融合估计装置的方框示意图。
如图3所示，该道路坡度自适应融合估计装置10包括：获取模块100、识别模块200、计算模块300和融合模块400。
其中，获取模块100用于获取车辆的车辆信息和定位系统的定位信息；识别模块200用于识别定位信息得到速度矢量和俯仰角，分别根据速度矢量和俯仰角构建第一和第二卡尔曼滤波器，设计相应的噪声计算方法，实现车辆当前所在道路第一坡度和第二坡度的自适应估计；计算模块300用于基于预设动力学方程和车辆状态信息，以车辆俯仰角速度作为坡度变化率，建立坡度状态方程，进而构建第三卡尔曼滤波器，并考虑车速、轮速和转向的影响对噪声进行了合理的建模，同时对低速工况进行了处理，实现车辆当前所在道路第三坡度的自适应估计；基于预设运动学方程和车辆状态信息，以车辆俯仰角速度作为坡度变化率，建立坡度状态方程，进而构建第四卡尔曼滤波器，并考虑弯道工况对噪声进行了合理的建模，实现车辆当前所在道路第四坡度的自适应估计；融合模块400用于根据定位系统的信号强度和第一坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差等，计算线性最小方差意义下的最优融合权重，融合第一坡度至第四坡度中的多个坡度，得到车辆当前所在道路最终坡度。在本申请的一个实施例中，融合模块400进一步根据定位系统的信号强度和第一坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差等，计算线性最小方差意义下的最优融合权重，融合第一坡度至第四坡度中的多个坡度，得到车辆当前所在道路最终坡度，包括：若信号强度大于预设强度，则根据根据定位系统的信号强度和第一坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差等，计算线性最小方差意义下的最优融合权重，根据第一坡度至第四坡度和各自权重进行最优加权融合，得到最终坡度；若信号强度小于或等于预设强度，则根据定位系统的信号强度和第三坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差等，计算线性最小方差意义下的最优融合权重，根据第三坡度、第四坡度和各自权重进行加权融合，得到最终坡度。
在本申请的一个实施例中，融合模块进一步用于若信号强度大于预设强度，则根据定位系统的信号强度和第一坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差，计算线性最小方差意义下的最优融合权重，根据第一坡度至第四坡度和各自权重进行最优加权融合，得到最终坡度；若信号强度小于或等于预设强度，则根据定位系统的信号强度和第三坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差，计算线性最小方差意义下的最优融合权重，根据第三坡度、第四坡度和各自权重进行加权融合，得到最终坡度。
在本申请的一个实施例中，最终坡度的计算公式为：
Figure BDA0004018173270000151
其中，θFus为最终坡度，λi为第i坡度的权重，
Figure BDA0004018173270000152
为第i坡度，第i坡度的权重λi的计算公式为：
λ＝[λ1，λ2，λ3，λ4]＝Λ-1e(eTΛ-1e)-1
其中，e＝[In，...，In]T为4n×n的矩阵，n为估计的状态量的个数，Λ由下式计算：
Figure BDA0004018173270000153
在本申请的一个实施例中，计算模块300进一步用于获取多个方向的速度矢量和总的空间矢量；根据多个方向的速度矢量合成水平速度矢量，根据水平速度矢量和总的空间矢量计算第一坡度，并将俯仰角作为第二坡度；所述噪声计算方法为将噪声σ近似为地向速度噪声σD与水平速度的比值：
Figure BDA0004018173270000161
其中，VN，VE分别为北向速度和东向速度，ε2为一较小常数，σ为标准差。
在本申请的一个实施例中，第三坡度的计算公式为：
Figure BDA0004018173270000162
其中，g为重力加速度，Fx为轮胎所受的地面纵向力，m为整车质量，Vx为纵向车速，Vy为侧向车速，ψ为横摆角，Froll为滚动阻力，Fair为空气阻力。
以车辆俯仰角速度作为坡度变化率，建立坡度状态方程为：
Figure BDA0004018173270000163
其中，
Figure BDA0004018173270000164
为车辆俯仰角速度，k为采样时刻，x3为第三坡度，dT为采样间隔，w3为随机噪声。
低速处理方法为：
θ3＝θ3*tanh(|Vx|)
噪声计算方法为：
Figure BDA0004018173270000165
其中，RDyn是第三坡度卡尔曼滤波器的测量噪声，C1，C2，C3，C4为标定常数，δ为方向盘转角，Wij，i∈{F，R}，j∈{L，R}是轮速，ε3是与随机噪声相关的常数。
在本申请的一个实施例中，第四坡度的计算公式为：
Figure BDA0004018173270000166
其中，ax为整车纵向加速度的量测值，ξ1为噪声。
噪声计算方法为：
RKnt＝P2*tanh(P1*(|δ|+ξ2))+ε4
其中，RKnt为第四坡度卡尔曼滤波器的测量噪声，δ为方向盘转角，P1，P2为标定常数，ξ2和ε4是为保证计算稳定而设置的常数。
以车辆俯仰角速度作为坡度变化率，建立坡度状态方程为：
Figure BDA0004018173270000167
其中，x4为第四坡度，w4为随机噪声。
需要说明的是，前述对道路坡度自适应融合估计方法实施例的解释说明也适用于该实施例的道路坡度自适应融合估计装置，此处不再赘述。
根据本申请实施例提出的道路坡度自适应融合估计装置，通过识别定位信息得到速度矢量和俯仰角，利用速度矢量和俯仰角并计算车辆当前所在道路的第一坡度和第二坡度，有效发挥其高精度、不受车辆系统影响的优势，并通过动力学和运动学的方法，计算车辆当前所在道路的第三坡度和第四坡度，能够在定位不稳定的情况下保证足够精度、不中断的坡度估计；基于多传感器卡尔曼滤波最优融合理论和定位系统的信号强度，对第一坡度至第四坡度中的多个坡度进行融合，有效应对坡度多变的工况，在保证坡度的高精度的同时，实现坡度高实时性、强鲁棒性的估计。由此，解决了相关技术在复杂路况，坡度变化较为频繁时难以实现准确快速的坡度估计的问题，充分发挥了每种方法的优势区间，扩展了总体算法的工作范围，从而在复杂工况下可以提供合理的坡度估计。
图4为本申请实施例提供的车辆的结构示意图。该车辆可以包括：
存储器401、处理器402及存储在存储器401上并可在处理器402上运行的计算机程序。
处理器402执行程序时实现上述实施例中提供的道路坡度自适应融合估计方法。
进一步地，车辆还包括：
通信接口403，用于存储器401和处理器402之间的通信。
存储器401，用于存放可在处理器402上运行的计算机程序。
存储器401可能包含高速RAM(Random Access Memory，随机存取存储器)存储器，也可能还包括非易失性存储器，例如至少一个磁盘存储器。
如果存储器401、处理器402和通信接口403独立实现，则通信接口403、存储器401和处理器402可以通过总线相互连接并完成相互间的通信。总线可以是ISA(IndustryStandard Architecture，工业标准体系结构)总线、PCI(Peripheral Component，外部设备互连)总线或EISA(Extended Industry Standard Architecture，扩展工业标准体系结构)总线等。总线可以分为地址总线、数据总线、控制总线等。为便于表示，图4中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。
可选的，在具体实现上，如果存储器401、处理器402及通信接口403，集成在一块芯片上实现，则存储器401、处理器402及通信接口403可以通过内部接口完成相互间的通信。
处理器402可能是一个CPU(Central Processing Unit，中央处理器)，或者是ASIC(Application Specific Integrated Circuit，特定集成电路)，或者是被配置成实施本申请实施例的一个或多个集成电路。
本申请实施例还提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如上的道路坡度自适应融合估计方法。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不是必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或N个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本申请的描述中，“N个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更N个用于实现定制逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本申请的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本申请的实施例所属技术领域的技术人员所理解。
应当理解，本申请的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，N个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。如，如果用硬件来实现和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列，现场可编程门阵列等。
本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。

Claims (10)

1.一种道路坡度自适应融合估计方法，其特征在于，包括以下步骤：
获取车辆的车辆状态信息和定位系统的定位信息；
识别所述定位信息得到速度矢量和俯仰角，分别根据所述速度矢量和所述俯仰角构建卡第一和第二卡尔曼滤波器，设计相应的噪声计算方法，实现车辆当前所在道路第一坡度和第二坡度的自适应估计；
基于预设动力学方程和所述车辆的状态信息，以所述车辆俯仰角速度作为坡度变化率，建立坡度状态方程，进而构建第三卡尔曼滤波器，并考虑车速、轮速和转向的影响对噪声进行建模，同时对低速工况进行处理，实现车辆当前所在道路第三坡度的自适应估计；
基于预设运动学方程和所述车辆的状态信息，以所述车辆俯仰角速度作为坡度变化率，建立坡度状态方程，进而构建第四卡尔曼滤波器，并考虑弯道工况对噪声进行建模，实现车辆当前所在道路第四坡度的自适应估计；
根据所述定位系统的信号强度和第一坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差，计算线性最小方差意义下的最优融合权重，融合所述第一坡度至第四坡度中的多个坡度，得到所述车辆当前所在道路最终坡度。
2.根据权利要求1所述的方法，其特征在于，所述根据所述定位系统的信号强度和第一坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差，计算线性最小方差意义下的最优融合权重，融合所述第一坡度至第四坡度中的多个坡度，得到所述车辆当前所在道路最终坡度，包括：
若所述信号强度大于预设强度，则根据所述定位系统的信号强度和所述第一坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差，计算线性最小方差意义下的最优融合权重，根据所述第一坡度至第四坡度和各自权重进行最优加权融合，得到所述最终坡度；
若所述信号强度小于或等于所述预设强度，则根据所述定位系统的信号强度和第三坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差，计算线性最小方差意义下的最优融合权重，根据所述第三坡度、所述第四坡度和各自权重进行加权融合，得到所述最终坡度。
3.根据权利要求2所述的方法，其特征在于，一种线性最小方差意义下的多传感器最优融合，所述最终坡度的计算公式为：
Figure FDA0004018173260000011
其中，θFus为最终坡度，λi为第i坡度的权重，
Figure FDA0004018173260000012
为第i坡度；
所述第i坡度的权重λi的计算公式为：
λ＝[λ1,λ2，λ3，λ4]＝Λ-1e(eTΛ-1e)-1
其中，e＝[In,…,In]T为4n×n的矩阵，n为估计的状态量的个数，Λ由下式计算：
Figure FDA0004018173260000021
4.根据权利要求1所述的方法，其特征在于，所述分别根据所述速度矢量和所述俯仰角构建第一和第二卡尔曼滤波器，设计相应的噪声计算方法，实现车辆当前所在道路第一坡度和第二坡度的自适应估计，包括：
获取多个方向的速度矢量和总的空间矢量；
根据所述多个方向的速度矢量合成水平速度矢量，根据所述水平速度矢量和所述总的空间矢量计算所述第一坡度，并将所述俯仰角作为所述第二坡度；
所述噪声计算方法为将噪声σ近似为地向速度噪声σD与水平速度的比值：
Figure FDA0004018173260000022
其中，VN,VE分别为北向速度和东向速度，ε2为一较小常数，σ为标准差。
5.根据权利要求1所述的方法，其特征在于，所述第三坡度的计算公式为：
Figure FDA0004018173260000023
其中，g为重力加速度，Fx为轮胎所受的地面纵向力，m为整车质量，Vx为纵向车速，Vy为侧向车速，ψ为横摆角，Froll为滚动阻力，Fair为空气阻力；
所述以所述车辆俯仰角速度作为坡度变化率，建立坡度状态方程为：
Figure FDA0004018173260000024
其中，
Figure FDA0004018173260000025
为车辆俯仰角速度，k为采样时刻，x3为第三坡度，dT为采样间隔，w3为随机噪声；
所述低速处理方法为：
θ3＝θ3*tanh(|Vx|)
所述噪声计算方法为：
Figure FDA0004018173260000026
其中，RDyn是第三坡度卡尔曼滤波器的测量噪声，C1,C2,C3,C4为标定常数，δ为方向盘转角，Wij,i∈{F,R},j∈{L,R}是轮速，ε3是与随机噪声相关的常数。
6.根据权利要求1所述的方法，其特征在于，所述第四坡度的计算公式为：
Figure FDA0004018173260000031
其中，ax为整车纵向加速度的量测值，ξ1为噪声；
所述噪声计算方法为：
RKnt＝P2*tanh(P1*(|δ|+ξ2))+ε4
其中，RKnt为第四坡度卡尔曼滤波器的测量噪声，δ为方向盘转角，P1,P2为标定常数，ξ2和ε4是为保证计算稳定而设置的常数；
所述以所述车辆俯仰角速度作为坡度变化率，建立坡度状态方程为：
Figure FDA0004018173260000032
其中，x4为第四坡度，w4为随机噪声。
7.一种道路坡度自适应融合估计装置，其特征在于，包括：
获取模块，用于获取车辆的车辆状态信息和定位系统的定位信息；
识别模块，识别所述定位信息得到速度矢量和俯仰角，分别根据所述速度矢量和所述俯仰角构建第一和第二卡尔曼滤波器，设计相应的噪声计算方法，实现车辆当前所在道路第一坡度和第二坡度的自适应估计；
计算模块，基于预设动力学方程和所述车辆的状态信息，以所述车辆俯仰角速度作为坡度变化率，建立坡度状态方程，进而构建第三卡尔曼滤波器，并考虑车速、轮速和转向的影响对噪声进行建模，同时对低速工况进行处理，实现车辆当前所在道路第三坡度的自适应估计；基于预设运动学方程和所述车辆的状态信息，以所述车辆俯仰角速度作为坡度变化率，建立坡度状态方程，进而构建第四卡尔曼滤波器，并考虑弯道工况对噪声进行建模，实现车辆当前所在道路第四坡度的自适应估计；
融合模块，根据所述定位系统的信号强度和第一坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差，计算线性最小方差意义下的最优融合权重，融合所述第一坡度至第四坡度中的多个坡度，得到车辆当前所在道路最终坡度。
8.根据权利要求7所述的装置，其特征在于，所述融合模块进一步用于：
若所述信号强度大于预设强度，则根据所述定位系统的信号强度和所述第一坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差，计算线性最小方差意义下的最优融合权重，根据所述第一坡度至第四坡度和各自权重进行最优加权融合，得到所述最终坡度；
若所述信号强度小于或等于所述预设强度，则根据所述定位系统的信号强度和第三坡度至第四坡度卡尔曼滤波器的卡尔曼增益、误差协方差，计算线性最小方差意义下的最优融合权重，根据所述第三坡度、所述第四坡度和各自权重进行加权融合，得到所述最终坡度。
9.一种车辆，其特征在于，包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述程序，以实现如权利要求1-6任一项所述的道路坡度自适应融合估计方法。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行，以用于实现如权利要求1-6任一项所述的道路坡度自适应融合估计方法。
CN202211678630.5A 2022-12-26 2022-12-26 道路坡度自适应融合估计方法、装置、车辆及存储介质 Pending CN115973166A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211678630.5A CN115973166A (zh) 2022-12-26 2022-12-26 道路坡度自适应融合估计方法、装置、车辆及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211678630.5A CN115973166A (zh) 2022-12-26 2022-12-26 道路坡度自适应融合估计方法、装置、车辆及存储介质

Publications (1)

Publication Number Publication Date
CN115973166A true CN115973166A (zh) 2023-04-18

Family

ID=85973511

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211678630.5A Pending CN115973166A (zh) 2022-12-26 2022-12-26 道路坡度自适应融合估计方法、装置、车辆及存储介质

Country Status (1)

Country Link
CN (1) CN115973166A (zh)

Similar Documents

Publication Publication Date Title
Liu et al. Automated vehicle sideslip angle estimation considering signal measurement characteristic
CN113819914B (zh) 一种地图构建方法及装置
CN107247824A (zh) 考虑刹车和转弯影响的汽车质量‑道路坡度联合估计方法
CN112505737B (zh) 一种gnss/ins组合导航方法
CN109521454A (zh) 一种基于自学习容积卡尔曼滤波的gps/ins组合导航方法
CN110346824B (zh) 一种车辆导航方法、系统、装置及可读存储介质
CN110987470B (zh) 一种基于模型迭代的汽车质量在线估计方法
CN108345021B (zh) 一种多普勒雷达辅助gps/ins车辆测速方法
CN107076559B (zh) 用于匹配导航系统的方法和系统
US11707952B2 (en) Method and system for determining tire effective radius
CN113196109B (zh) 用于确定完整性范围的方法
CN115265551A (zh) 一种无人驾驶复杂场景下的多传感器融合定位方法和系统
CN111750865A (zh) 一种用于双功能深海无人潜器导航系统的自适应滤波导航方法
CN115407376A (zh) 车辆定位校准方法、装置、计算机设备、存储介质
CN115060257A (zh) 一种基于民用级惯性测量单元的车辆变道检测方法
JP7069624B2 (ja) 位置演算方法、車両制御方法及び位置演算装置
CN112046491B (zh) 车轮侧偏刚度的估算方法、装置、车辆及可读存储介质
CN115973166A (zh) 道路坡度自适应融合估计方法、装置、车辆及存储介质
CN114043986B (zh) 一种考虑质量失配的轮胎路面附着系数多模型融合估计方法
CN114370872A (zh) 车辆姿态确定方法和车辆
CN113985466A (zh) 一种基于模式识别的组合导航方法及系统
Phanomchoeng et al. Real-time automotive slip angle estimation with extended H∞ circle criterion observer for nonlinear output system
CN117804475B (zh) 基于惯性卫星里程计和运动学约束的车载融合导航方法
CN115235453B (zh) 一种移动机器人定位方法及系统
CN111060944B (zh) 一种基于current-ellipse模型的车辆动态跟踪方法

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