1.一种无GPS跟车场景下前车路径重建方法,其特征在于,所述前车路径重建方法包括以下步骤:步骤1:基于前后车初始时刻相对位置,生成一条后车的参考路径;所述参考路径指后车坐标系中前车的历史路径,包含后车质心前方沿行驶方向,至前车当前时刻质心位置的区间内的若干路径点;步骤2:将传感器相对定位信息和里程计信息进行融合,获取相对位姿最优估计值;步骤3:根据相对位姿最优估计值构建运动学模型,并对参考路径进行筛选,生成有效路径;在步骤3中还包括以下步骤:步骤31,根据运动学关系进行参考路径的坐标系转换;具体的,将k-1时刻车辆坐标系的位姿信息转化到k时刻车辆坐标系中;步骤32,筛选出后车质心后方的路径点从序列中剔除,并将满足路径点跳动距离范围阈值的新的路径点加入路径;所述新的路径点由k时刻相对位姿最优估计值得到。
2.根据如权利要求1所述前车路径重建方法,其特征在于,在步骤1中,根据所述初始时刻的相对位姿,以后车质心为坐标系原点,车头朝向为x轴正方向,建立车辆坐标系,根据前车的相对位置和车头朝向角进行三次曲线拟合:f(x)=ax 3 +bx 2 +cx+d其中,a,b,c,d分别表示参考路径曲线的各次项系数,f(x)为前车参考路径曲线,x为参考路径曲线在坐标系中的纵轴坐标;由于该曲线经过坐标系原点,且在原点处切线斜率为0,所以c=d=0;又因为初始时刻前车在该曲线上,满足:解方程组可得出a,b值分别为:设定初始化路径包含的路径点的个数为N 0 ,对该三次曲线进行等距插值得到初始化路径序列中的路径点的位置信息 在每个点处该曲线的斜率为初始路径序列的姿态信息 为初始化路径序列中的路径点N 0 的x轴位置信息, 为初始化路径序列中的路径点N 0 的y轴位置信息, 为初始化路径序列中的路径点N 0 的斜率。
3.根据如权利要求1所述前车路径重建方法,其特征在于,在步骤2中,建立运动学方程,并进行离散化:其中,Δx表示后车坐标系中,前车质心的纵向相对位置;Δy表示后车坐标系中,前车质心的横向相对位置;Δθ表示前车相对于后车的车头朝向角;ω 1 表示前车的横摆角速度;ω 2 表示后车的横摆角速度;v x1 表示前车的纵向车速;v x2 表示后车的纵向车速;T为系统采样时间,k为k时刻,k-1为k-1时刻。
4.根据如权利要求3所述前车路径重建方法,其特征在于,在步骤2中,记状态向量为X k =[Δx k ,Δy k ,Δθ k ] T ,得到非线性状态方程,简化表示为:X k =f(X k-1 )+w k-1其中,w k-1 为k-1时刻过程噪声,服从高斯分布w k-1 ~N(0,Q),Q表示过程噪声的方差;记在k时刻传感器相对定位测量值为Z k ,则系统的测量方程表示为:Z k =h(X k )+ψ k其中,h(·)表示测量函数,由于状态变量由传感器直接测量得到,所以有h(X k )=HX k ,其中H=diag([1,1,1]),ψ k 为k时刻测量噪声,服从高斯分布v k ~N(0,R),R的大小取决于传感器相对定位测量精度;通过扩展卡尔曼滤波算法,根据k-1时刻的系统状态,利用状态方程得到系统在k时刻的状态的先验估计值,表示为:其中, 为k-1时刻的后验估计值;k时刻的状态的先验估计值对应的协方差表示为:其中, 表示k时刻的状态的先验估计值对应的协方差,P k-1 表示k-1时刻估计的协方差;J f 表示状态函数的雅可比矩阵。
5.根据如权利要求4所述前车路径重建方法,其特征在于,在步骤2中,所述状态函数的雅可比矩阵表示为:其中,v x1,k-1 表示k-1时刻前车的纵向速度。
6.根据如权利要求4所述前车路径重建方法,其特征在于,在步骤2中,系统在k时刻的最优估计值 为: 表示k时刻预测值;Z k 表示k时刻测量值;K k 表示k时刻卡尔曼增益值;Θ k 表示感知系统数据的可用状态,在传感信息不可用时,Θ k =0;当感知系统数据可用时,Θ k =1;所述最优估计值记为 分别表示后车坐标系中前车质心的纵向相对位置、横向相对位置和相对车头朝向角;所述k时刻卡尔曼增益值表示为:J h 表示测量函数的雅可比矩阵,为单位对角阵。
7.根据如权利要求6所述前车路径重建方法,其特征在于,在步骤2中,使所述扩展卡尔曼滤波算法能够持续更新,需要更新系统最优估计状态的协方差:其中,I表示单位矩阵。
8.根据如权利要求6所述前车路径重建方法,其特征在于,在步骤3中,系统采样时间T足够小时,在[(k-1)T,kT]时间间隔内,后车位姿变化表示为δq=(δx,δy,δθ) T ,根据车辆的单轨运动学模型,有:其中,v x2 表示后车纵向车速,l f ,l r 分别表示后车前轴到质心的距离和后轴到质心的距离,δ f 表示后车的前轮转角,ω 2 表示后车的横摆角速度;将k-1时刻车辆坐标系的位姿信息转化到k时刻车辆坐标系中:
9.根据如权利要求8所述前车路径重建方法,其特征在于,在步骤3中,假设在t=kT时刻,路径序列长度为 经过坐标系变换后,存在一个正整数i 0 满足 那么利用下式进行路径点的剔除:其中, 此时路径点序列记为 为k时刻第p个路径点序列对应的相对位姿, 为k时刻第p个路径点序列对应的纵向速度, 为k时刻第p个路径点序列对应的横摆角速度。
10.根据如权利要求9所述前车路径重建方法,其特征在于,在步骤3中,新加入的路径点满足以下不等关系:其中,d min 表示设计路径点跳动距离范围下限,d max 表示设计路径点跳动距离范围上限;将满足所述不等关系的路径点加入到路径序列中:其中, 分别为k时刻的纵向速度和横摆角速度的估计值;此时路径表示为 表示k时刻的相对位姿最优估计值, 表示路径的估计值, 为第N k 个路径点的信息;若不满足上述不等关系,则不加入新的路径点,路径表示为