道路坡度估计方法、装置及自动驾驶车辆与流程

摘要: 本技术涉及自动驾驶,更具体地,涉及一种道路坡度估计方法、装置及自动驾驶车辆。背景技术:1、在自动驾驶车辆纵向控制过程中,相关技术中通过直接采用定位模块输出的车体...
道路坡度估计方法、装置及自动驾驶车辆与流程

本技术涉及自动驾驶,更具体地,涉及一种道路坡度估计方法、装置及自动驾驶车辆。

背景技术:

1、在自动驾驶车辆纵向控制过程中,相关技术中通过直接采用定位模块输出的车体姿态四元数解算俯仰角作为道路坡度,或通过固定低通滤波、惯性测量单元角速度积分、路面地图坡度先验以及加速度计长基线倾角测量等方式获取坡度信息,但存在车身动力学俯仰,即起步抬头与制动点头与真实坡度相互混叠、固定滤波带宽在坡道跟踪与噪声抑制间无法兼顾、惯性测量单元积分依赖静止初始化而与起步工况冲突、高精地图更新周期长且无法应对临时施工路段等难以克服的技术问题。

技术实现思路

1、有鉴于此,本技术提供了一种道路坡度估计方法、装置及自动驾驶车辆。

2、本技术的一个方面提供了一种道路坡度估计方法,包括:获取车体俯仰角测量值和车辆纵向加速度;

3、根据上述车辆纵向加速度计算工况污染度,上述工况污染度指示上述车体俯仰角测量值受车身俯仰角分量污染的程度;构建上述车体俯仰角测量值的状态空间模型,其中,将上述车体俯仰角测量值中的路面坡度分量作为状态量,上述状态量的变化量服从随机分布;将上述车体俯仰角测量值中的车身动力学俯仰角分量、静态俯仰偏置分量和传感器噪声分量作为测量噪声分量;根据上述工况污染度调节卡尔曼滤波器的过程噪声方差和观测噪声方差,其中,上述过程噪声方差用于表征上述路面坡度分量在相邻时刻之间变化的不确定程度,上述观测噪声方差用于表征上述测量值中上述测量噪声分量的不确定程度;以及根据上述状态空间模型、上述过程噪声方差和上述观测噪声方差,执行卡尔曼滤波递推,以对上述车体俯仰角测量值中的上述路面坡度分量与上述测量噪声分量进行加权分离,估计上述路面坡度分量作为真实路面坡度值。

4、根据本技术的实施例,上述根据上述工况污染度调节卡尔曼滤波器的过程噪声方差和观测噪声方差包括:将上述过程噪声方差计算为上述工况污染度与过程噪声差值之积与过程噪声最小值之和,上述过程噪声差值为过程噪声最大值与上述过程噪声最小值之差;将上述观测噪声方差计算为上述工况污染度与观测噪声差值之积与观测噪声最小值之和,上述观测噪声差值为观测噪声最大值与上述观测噪声最小值之差。

5、根据本技术的实施例,根据上述车辆纵向加速度计算工况污染度包括:根据上述车辆纵向加速度的绝对值与预设加速度区间阈值的比较结果确定加速度污染度;获取颠簸标志值,上述颠簸标志值用于指示车辆当前颠簸状态信息;将上述加速度污染度与上述颠簸标志值中的最大值确定为上述工况污染度。

6、根据本技术的实施例,预设加速度区间阈值包括第一预设加速度阈值和第二预设加速度阈值,上述根据上述车辆纵向加速度的绝对值与预设加速度区间阈值的比较结果确定加速度污染度包括:将上述车辆纵向加速度的绝对值与上述第一预设加速度阈值和上述第二预设加速度阈值进行比较;在上述绝对值小于等于上述第一预设加速度阈值的情况下,判定车辆处于平稳巡航工况,确定上述加速度污染度为0;在上述绝对值大于等于上述第二预设加速度阈值的情况下,判定车辆处于剧烈加减速工况,确定上述加速度污染度为1;在上述绝对值大于上述第一预设加速度阈值且小于上述第二预设加速度阈值的情况下,判定车辆处于过渡工况,将上述绝对值与上述第一预设加速度阈值的差值占上述第一预设加速度阈值与上述第二预设加速度阈值之差的比例,作为上述加速度污染度。

7、根据本技术的实施例,上述根据上述状态空间模型、上述过程噪声方差和上述观测噪声方差,执行卡尔曼滤波递推包括:根据上一帧的状态估计值确定当前帧的状态预测值;根据上一帧的协方差值和当前帧的上述过程噪声方差确定当前帧的协方差预测值;根据上述协方差预测值和当前帧的上述观测噪声方差确定卡尔曼增益;根据上述状态预测值、上述卡尔曼增益以及上述车体俯仰角测量值与上述状态预测值的偏差,确定当前帧的状态估计值,将上述状态估计值作为上述真实路面坡度值;以及根据上述卡尔曼增益和上述协方差预测值确定当前帧的协方差值。

8、根据本技术的实施例,还包括:将当前帧的上述协方差值限制在预定协方差下界和预定协方差上界之间。

9、根据本技术的实施例,还包括:在首帧未初始化时,将上述车体俯仰角测量值作为上述真实路面坡度值的初始状态估计值,并将上述观测噪声最大值作为初始协方差值。

10、根据本技术的实施例,还包括:对上述车体俯仰角测量值进行有效性检查;当上述车体俯仰角测量值超出预定角度范围或为非数值标记时,跳过当前帧的更新并保持上一帧的输出;比较上述车体俯仰角测量值的第一时间戳与上述车辆纵向加速度的第二时间戳;当上述第一时间戳与上述第二时间戳的偏差超过预定时间阈值时,将加速度污染度设置为0。

11、本技术的另一个方面提供了一种道路坡度估计装置,包括:获取模块,用于获取车体俯仰角测量值和车辆纵向加速度;工况计算模块,用于根据上述车辆纵向加速度计算工况污染度,上述工况污染度指示上述车体俯仰角测量值受车身俯仰角分量污染的程度;模型构建模块,用于构建上述车体俯仰角测量值的状态空间模型,其中,将上述车体俯仰角测量值中的路面坡度分量作为状态量,上述状态量的变化量服从随机分布;将上述车体俯仰角测量值中的车身动力学俯仰角分量、静态俯仰偏置分量和传感器噪声分量作为测量噪声分量;噪声调节模块,用于根据上述工况污染度调节卡尔曼滤波器的过程噪声方差和观测噪声方差,其中,上述过程噪声方差用于表征上述路面坡度分量在相邻时刻之间变化的不确定程度,上述观测噪声方差用于表征上述测量值中上述测量噪声分量的不确定程度;以及滤波递推模块,用于根据上述状态空间模型、上述过程噪声方差和上述观测噪声方差,执行卡尔曼滤波递推,以对上述车体俯仰角测量值中的上述路面坡度分量与上述测量噪声分量进行加权分离,估计上述路面坡度分量作为真实路面坡度值。

12、本技术的另一个方面提供了一种自动驾驶车辆,包括:一个或多个处理器;存储装置,用于存储一个或多个程序;当上述一个或多个程序被上述一个或多个处理器执行时,使得上述一个或多个处理器实现如上述中任一项上述的方法。

13、本技术的另一个方面提供了一种电子设备,包括:一个或多个处理器;存储器,用于存储一个或多个程序,其中,当所述一个或多个程序被所述一个或多个处理器执行时,使得所述一个或多个处理器实现如上所述的方法。

14、本技术的另一方面提供了一种计算机可读存储介质,存储有计算机可执行指令,所述指令在被执行时用于实现如上所述的方法。

15、本技术的另一方面提供了一种计算机程序产品,所述计算机程序产品包括计算机可执行指令,所述指令在被执行时用于实现如上所述的方法。

16、根据本技术的实施例,通过获取车体俯仰角测量值和车辆纵向加速度,以便于后续实现车辆动力学响应的协同优化与驾驶安全性的全面提升,然后根据车辆纵向加速度计算工况污染度,并且构建车体俯仰角测量值的状态空间模型,用于对缓慢时变或未知状态量,如真实路面坡度、静态俯仰偏置或传感器零漂等的时序演化规律进行数学建模与预测;再根据工况污染度调节卡尔曼滤波器的过程噪声方差和观测噪声方差,以便于实现平稳工况强信任测量、激烈工况强信任先验的动态切换以及根据状态空间模型、过程噪声方差和观测噪声方差,执行卡尔曼滤波递推,以对车体俯仰角测量值中的路面坡度分量与测量噪声分量进行加权分离,估计路面坡度分量作为真实路面坡度值,以便于滤波器在平稳工况下倾向于信任观测以快速跟踪真实坡度变化并校准惯性积分漂移,在强机动或颠簸工况下倾向于依赖预测以隔离车身动力学俯仰对观测值的污染,实现道路坡度估计的平滑性、跟踪响应速度与抗差鲁棒性的自洽统一。