[发明专利]一种自动驾驶过程中使车辆位置平滑的装置及方法

说明书

本发明涉及一种自动驾驶过程中使车辆位置平滑的装置及方法，此装置由初始模型,车辆位置信息，车辆位置预测和车辆航向约束四大模块组成；此装置使用的方法是：结合滤波模型与车辆行进过程中最大横摆角速度，对车辆位置进行平滑处理；通过本发明降低了自动驾驶系统运行过程中，由于车身位置抖动而造成的方向盘抖动，从而使自动驾驶系统控制车辆更好地运行，有效提升用户的驾乘体验,可以广泛运用于汽车自动驾驶系统领域。

技术领域

本发明涉及汽车自动驾驶领域，特别是涉及一种自动驾驶过程中使车辆位置平滑的装置及方法。

背景技术

自动驾驶系统在运行过程中，实时获取的车辆位置信息是从高精度组合惯导获取，国内如星网宇达、迈普时空等厂商从事这类组合惯导的生产。但现有设备输出的车辆位置均存在一定的抖动现象，会造成自动驾驶系统在运行过程中车身的抖动，进而有可能会给乘客造成头晕等不适的驾乘体验。

造成这种现象的原因是车辆在直线行驶过程中，在较短的时间周期内，按照等时间间隔对车辆位置进行采样，并将这些位置按照时间顺序连接，理论上这种连接应该得到一条近似直线的折线段。但实际上，从组合惯导设备获取的位置信息计算出的折线段经常会呈现Z字形等异常形状，尤其是在车辆拐弯的时候变形更大，导致车身产生更大的抖动，致使乘客产生不适体验。

以往的算法中，直接运用历史位置与当前位置的均值或者使用一次函数进行拟合当前位置，效果并不理想，自动驾驶时车身的抖动仍然很明显。

发明内容

根据现有技术的不足，本发明的目的在于提供一种自动驾驶过程中使车辆位置平滑的装置及方法，以降低车辆位置的抖动，使得自动驾驶系统控制车辆更好地运行，有效提升用户的体验。

为了实现上述目的，本发明采取的技术方案如下：

本发明公开的一种自动驾驶过程中使车辆位置平滑的装置，所述装置包括如下部分：建立初始模型，设定参数建立滤波模型，设定最大偏航数值的初始值，由初始值赋值给偏航数值，偏航数值用来给车辆行进轨迹的航向进行约束；车辆位置信息模块，获取车辆位置数据，对滤波模型进行初始化；车辆位置预测模块，利用滤波模型对当前车辆位置进行预测，并结合惯导获取的当前车辆位置的观测值进行校正；车辆航向约束模块，对校正后的车辆位置进行修正，获取更新的车辆位置，若车辆位置出现异常跳动时偏航数值会变大，车辆位置恢复正常时偏航值逐渐恢复其初始值。

在上述技术方案中，所述初始化模块中，所述滤波模型为卡尔曼滤波模型，所述卡尔曼滤波模型使用OpenCV库提供的函数进行计算，卡尔曼滤波模型公式如下：

其中，x为车辆的经度，y为车辆的纬度，vx为车辆的东向速度，vy为车辆的北向速度，(x y vx vy)^T_k表示时刻k车辆的状态，(x y vx vy)^T_k+1表示时刻k+1车辆的状态。

在上述技术方案中，所述车辆航向约束模块中，当校正后的位置满足车辆的航向约束时，将校正后的位置输出，若校正后的位置不满足航向约束时，则对校正后的车辆位置进行修正，并将修正后的位置参数输出，利用车辆的偏航数值对车辆的行进轨迹进行航向约束。

在上述技术方案中，所述车辆航向约束模块中，偏航数值初始值MaxYaw0是最大横摆角速度与获取车辆当前时刻和上一时刻位置的时间差的乘积，MaxYaw0赋值给MaxYaw，车辆位置出现异常情况时，MaxYaw会进行更新，车辆位置出现异常情况是指上上一个时刻位置与上一个时刻位置的连线和上一个时刻位置与当前时刻位置的连线的向量夹角大于某个给定的阈值，车辆位置恢复正常时指上述向量夹角小于偏航数值且偏航数值大于偏航数值初始值。

在上述技术方案中，所述车辆位置信息模块中，从高精度组合惯导中提取经度、纬度、东向速度和北向速度参数，确定车辆位置信息。