[发明专利]一种无GPS跟车场景下前车路径重建方法

说明书

本发明提出一种无GPS跟车场景下前车路径重建方法，包括步骤1：基于前后车初始时刻相对位置，生成一条参考路径；步骤2：将传感器相对定位信息和里程计信息进行融合，获取相对位姿最优估计值；步骤3：根据相对位姿最优估计值构建运动学模型，并对后车路径初始化并进行筛选，生成有效路径。本发明提供的无GPS自动驾驶跟车场景下前车路径重建方法，不依靠绝对定位信息及其他辅助基础设施，适用于非结构化道路上跟随行驶场景；同时，融合传感器、里程计多源信息，进一步提高相对定位的精度，并最终生成可靠的参考路径。

技术领域

本发明涉及自动驾驶技术领域，具体涉及一种无GPS跟车场景下前车路径重建方法。

背景技术

随着自动汽车技术与计算机技术的快速发展，自动驾驶自动化程度不断提高，适用的场景不断增加。队列行驶和跟随行驶场景，由于其能够提高道路容量、降低燃油消耗的优点，近年来受到广泛研究。

当前车辆队列行驶以及跟随驾驶场景下自动驾驶功能的实现，绝大多数需要依赖于GPS(全球定位系统)设备提供两车的准确位置信息，从而实现后车对前车历史轨迹的跟随，这种方式虽然跟踪效果好，但是GPS设备昂贵且在遮挡区域效果不稳定，难以大规模推广。此外，基于车辆相对位置的跟车行驶方式仅考虑前后车瞬时时刻的相对位置关系，使后车不能准确地沿着前车历史路径行驶，且在转向时可能会发生内切现象。

发明内容

本发明旨在提供一种无GPS信号的自动驾驶跟车场景，通过相对定位以及车间通信设备获取的定位与车辆信息对前车历史路径进行重建，作为后车的参考路径。

具体的，一种无GPS跟车场景下前车路径重建方法，其特征在于，所述前车路径重建方法包括以下步骤：

步骤1：基于前后车初始时刻相对位置，生成一条参考路径；

步骤2：将传感器相对定位信息和里程计信息进行融合，获取相对位姿最优估计值；

步骤3：根据相对位姿最优估计值构建运动学模型，并对后车路径初始化并进行筛选，生成有效路径。

更进一步地，在步骤1中，根据所述初始时刻的相对位姿，以后车质心为坐标系原点，车头朝向为x轴正方向，建立车辆坐标系，根据前车的相对位置和车头朝向角进行三次曲线拟合：

f(x)＝ax³+bx²+cx+d

其中，a,b,c,d分别表示参考路径曲线的各次项系数；由于该曲线经过坐标系原点，且在原点处切线斜率为0，所以c＝d＝0；又因为初始时刻前车在该曲线上，满足：

解方程组可得出a,b值分别为：

设定初始化路径包含的路径点的个数为N₀，对该三次曲线进行等距插值得到初始化路径序列中的路径点的位置信息在每个点处该曲线的斜率为初始路径序列的姿态信息

更进一步地，在步骤2中，建立运动学方程，并进行离散化：

其中，Δx表示后车坐标系中，前车质心的纵向相对位置；Δy表示后车坐标系中，前车质心的横向相对位置；Δθ表示前车相对于后车的车头朝向角；ω₁表示前车的横摆角速度；ω₂表示后车的横摆角速度；v_x1表示前车的纵向车速；v_x2表示后车的纵向车速。

更进一步地，在步骤2中，通过扩展卡尔曼滤波算法，根据k-1时刻的系统状态，利用状态方程可以得到系统在k时刻的状态的先验估计值，表示为：