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

权利要求书

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

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

所述参考路径指后车坐标系中前车的历史路径，包含后车质心前方沿行驶方向，至前车当前时刻质心位置的区间内的若干路径点；

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

步骤3：根据相对位姿最优估计值构建运动学模型，并对参考路径进行筛选，生成有效路径；

在步骤3中还包括以下步骤：

步骤31，根据运动学关系进行参考路径的坐标系转换；

具体的，将k-1时刻车辆坐标系的位姿信息转化到k时刻车辆坐标系中；

步骤32，筛选出后车质心后方的路径点从序列中剔除，并将满足路径点跳动距离范围阈值的新的路径点加入路径；

所述新的路径点由k时刻相对位姿最优估计值得到。

2.根据如权利要求1所述前车路径重建方法，其特征在于，在步骤1中，根据所述初始时刻的相对位姿，以后车质心为坐标系原点，车头朝向为x轴正方向，建立车辆坐标系，根据前车的相对位置和车头朝向角进行三次曲线拟合：

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

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

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

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

3.根据如权利要求1所述前车路径重建方法，其特征在于，在步骤2中，建立运动学方程，并进行离散化：

其中，Δx表示后车坐标系中，前车质心的纵向相对位置；Δy表示后车坐标系中，前车质心的横向相对位置；Δθ表示前车相对于后车的车头朝向角；ω₁表示前车的横摆角速度；ω₂表示后车的横摆角速度；v_x1表示前车的纵向车速；v_x2表示后车的纵向车速；T为系统采样时间，k为k时刻，k-1为k-1时刻。