CN113870316A - 一种无gps跟车场景下前车路径重建方法 - Google Patents

一种无gps跟车场景下前车路径重建方法 Download PDF

Info

Publication number
CN113870316A
CN113870316A CN202111215298.4A CN202111215298A CN113870316A CN 113870316 A CN113870316 A CN 113870316A CN 202111215298 A CN202111215298 A CN 202111215298A CN 113870316 A CN113870316 A CN 113870316A
Authority
CN
China
Prior art keywords
vehicle
path
representing
time
coordinate system
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202111215298.4A
Other languages
English (en)
Other versions
CN113870316B (zh
Inventor
李惠乾
黄晋
贾一帆
苏炎召
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Qingdao Dezhi Automobile Technology Co ltd
Original Assignee
Qingdao Dezhi Automobile Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Qingdao Dezhi Automobile Technology Co ltd filed Critical Qingdao Dezhi Automobile Technology Co ltd
Priority to CN202111215298.4A priority Critical patent/CN113870316B/zh
Publication of CN113870316A publication Critical patent/CN113870316A/zh
Application granted granted Critical
Publication of CN113870316B publication Critical patent/CN113870316B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G06T7/251—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments involving models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G06T7/75—Determining position or orientation of objects or cameras using feature-based methods involving models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Multimedia (AREA)
  • Navigation (AREA)

Abstract

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

Description

一种无GPS跟车场景下前车路径重建方法
技术领域
本发明涉及自动驾驶技术领域，具体涉及一种无GPS跟车场景下前车路径重建方法。
背景技术
随着自动汽车技术与计算机技术的快速发展，自动驾驶自动化程度不断提高，适用的场景不断增加。队列行驶和跟随行驶场景，由于其能够提高道路容量、降低燃油消耗的优点，近年来受到广泛研究。
当前车辆队列行驶以及跟随驾驶场景下自动驾驶功能的实现，绝大多数需要依赖于GPS(全球定位***)设备提供两车的准确位置信息，从而实现后车对前车历史轨迹的跟随，这种方式虽然跟踪效果好，但是GPS设备昂贵且在遮挡区域效果不稳定，难以大规模推广。此外，基于车辆相对位置的跟车行驶方式仅考虑前后车瞬时时刻的相对位置关系，使后车不能准确地沿着前车历史路径行驶，且在转向时可能会发生内切现象。
发明内容
本发明旨在提供一种无GPS信号的自动驾驶跟车场景，通过相对定位以及车间通信设备获取的定位与车辆信息对前车历史路径进行重建，作为后车的参考路径。
具体的，一种无GPS跟车场景下前车路径重建方法，其特征在于，所述前车路径重建方法包括以下步骤：
步骤1：基于前后车初始时刻相对位置，生成一条参考路径；
步骤2：将传感器相对定位信息和里程计信息进行融合，获取相对位姿最优估计值；
步骤3：根据相对位姿最优估计值构建运动学模型，并对后车路径初始化并进行筛选，生成有效路径。
更进一步地，在步骤1中，根据所述初始时刻的相对位姿，以后车质心为坐标系原点，车头朝向为x轴正方向，建立车辆坐标系，根据前车的相对位置和车头朝向角进行三次曲线拟合：
f(x)＝ax3+bx2+cx+d
其中，a,b,c,d分别表示参考路径曲线的各次项系数；由于该曲线经过坐标系原点，且在原点处切线斜率为0，所以c＝d＝0；又因为初始时刻前车在该曲线上，满足：
Figure BDA0003310540660000021
解方程组可得出a,b值分别为：
Figure BDA0003310540660000022
设定初始化路径包含的路径点的个数为N0，对该三次曲线进行等距插值得到初始化路径序列中的路径点的位置信息
Figure BDA0003310540660000023
在每个点处该曲线的斜率为初始路径序列的姿态信息
Figure BDA0003310540660000024
更进一步地，在步骤2中，建立运动学方程，并进行离散化：
Figure BDA0003310540660000025
其中，Δx表示后车坐标系中，前车质心的纵向相对位置；Δy表示后车坐标系中，前车质心的横向相对位置；Δθ表示前车相对于后车的车头朝向角；ω1表示前车的横摆角速度；ω2表示后车的横摆角速度；vx1表示前车的纵向车速；vx2表示后车的纵向车速。
更进一步地，在步骤2中，通过扩展卡尔曼滤波算法，根据k-1时刻的***状态，利用状态方程可以得到***在k时刻的状态的先验估计值，表示为：
Figure BDA0003310540660000031
其中，
Figure BDA0003310540660000032
为k-1时刻的后验估计值；k时刻的状态的先验估计值对应的协方差可以表示为：
Figure BDA0003310540660000033
其中，
Figure BDA0003310540660000034
表示k时刻的状态的先验估计值对应的协方差，Pk-1表示k-1时刻估计的协方差；Jf表示状态函数的雅可比矩阵。
更进一步地，在步骤2中，所述状态函数的雅可比矩阵表示为：
Figure BDA0003310540660000035
其中，vx,t-1表示k-1时刻后车的纵向速度。
更进一步地，在步骤2中，***在该时刻的最优估计值
Figure BDA0003310540660000036
为：
Figure BDA0003310540660000037
Figure BDA0003310540660000038
表示k时刻预测值；Zk表示k时刻测量值；Kk表示k时刻卡尔曼增益值；Θk表示感知***数据的可用状态，在传感信息不可用时，Θk＝0；当感知***数据可用时，Θk＝1；
所述最优估计值记为
Figure BDA0003310540660000039
Figure BDA00033105406600000310
分别表示后车坐标系中前车质心的纵向相对位置、横向相对位置和相对车头朝向角；
所述k时刻卡尔曼增益值可以表示为:
Figure BDA00033105406600000311
Jh表示测量函数的雅可比矩阵，为单位对角阵。
更进一步地，在步骤2中，使所述扩展卡尔曼滤波算法能够持续更新，需要更新***最优估计状态的协方差：
Figure BDA0003310540660000041
其中，I表示单位矩阵；
获取k时刻的相对位置最优估计值，记为
Figure BDA0003310540660000042
Figure BDA0003310540660000043
分别表示后车坐标系中前车质心的纵向相对位置、横向相对位置和相对车头朝向角。
更进一步地，在步骤3中，***采样时间T足够小时，在[(k-1)T，kT]时间间隔内，后车位姿变化可以表示为δq＝(δx,δy,δθ)T，根据车辆的单轨运动学模型，有：
Figure BDA0003310540660000044
其中，vx表示后车纵向车速，lf,lr分别表示后车前轴到质心的距离和后轴到质心的距离，δf表示后车的前轮转角，ω表示后车的横摆角速度；将k-1时刻车辆坐标系的位姿信息转化到k时刻车辆坐标系中：
Figure BDA0003310540660000045
更进一步地，在步骤3中，假设在t＝kT时刻，路径序列长度为
Figure BDA0003310540660000046
经过坐标系变换后，存在一个正整数i0满足
Figure BDA0003310540660000047
Figure BDA0003310540660000048
那么可以利用下式进行路径点的剔除：
Figure BDA0003310540660000049
Figure BDA00033105406600000410
Figure BDA0003310540660000051
其中，
Figure BDA0003310540660000052
此时路径点序列记为
Figure BDA0003310540660000053
更进一步地，在步骤3中，新加入的路径点满足以下不等关系：
Figure BDA0003310540660000054
其中，dmin表示设计路径点跳动距离范围下限，dmax表示设计路径点跳动距离范围上限；
将满足所述不等关系的路径点加入到路径序列中：
Figure BDA0003310540660000055
Figure BDA0003310540660000056
Figure BDA0003310540660000057
其中，
Figure BDA0003310540660000058
Figure BDA0003310540660000059
分别为该时刻的纵向速度和横摆角速度的估计值；此时路径表示为
Figure BDA00033105406600000510
若不满足上述不等关系，则不加入新的路径点，路径表示为
Figure BDA00033105406600000511
本发明的有益效果包括：
本发明提供的无GPS自动驾驶跟车场景下前车路径重建方法，不依靠绝对定位信息及其他辅助基础设施，适用于非结构化道路上跟随行驶场景。
本发明提供的无GPS自动驾驶跟车场景下前车路径重建方法，融合传感器、里程计多源信息，进一步提高相对定位的精度，并最终生成可靠的参考路径。
附图说明
图1是本发明实施例提供的一种无GPS跟车场景下前车路径重建方法的流程示意图；
图2是本发明实施例提供的一种无GPS跟车场景下前车路径重建方法中前后车相对运动模型的示意图。
具体实施方式
下面结合附图对本发明的技术方案进行更详细的说明，本发明包括但不仅限于下述实施例。
如附图1所示，本发明提供了一种无GPS跟车场景下前车路径重建方法，该前车路径重建方法包括以下步骤：
步骤1，在***开始运行时，基于前后车初始时刻相对位置，生成一条参考路径，作为后续路径重建的基础。
本发明涉及两车队列或者跟随驾驶场景，两车分别称为“前车”和“后车”，均不装备GPS设备但装有车间通信设备，能够实现前车向后车的信号传输。两车均装有能够测量纵向速度、纵向加速度和横摆角速度等状态的传感器；两车之间通过传感器，如激光雷达，摄像头；或者相对定位模块，如UWB定位模块获取两车的相对位姿q＝(Δx,Δy,Δθ)；另外，后车应具备线控驱动、线控制动、线控转向***。实际运行中，后车可以根据重建路径设计控制器自主跟随前车。
重建路径指后车坐标系中前车的历史路径，包含后车质心前方沿行驶方向，至前车当前时刻质心位置的区间内的若干路径点。在t＝kT(k＝0,1,2…)时刻，其中，T是***采样时间，该路径可以表示为一个长度为Nk的路径点序列Mk，序列初始长度为N0，可以表示为：
Figure BDA0003310540660000061
其中，第i个路径点的信息
Figure BDA0003310540660000062
可以描述为：
Figure BDA0003310540660000063
其中，
Figure BDA0003310540660000064
为k时刻前车的位姿，
Figure BDA0003310540660000065
为k时刻前车的纵向速度，
Figure BDA0003310540660000066
为k时刻前车的横摆角速度。
根据初始时刻的相对位姿，以后车质心为坐标系原点，车头朝向为x轴正方向，建立车辆坐标系，根据前车的相对位置和车头朝向角进行三次曲线拟合：
f(x)＝ax3+bx2+cx+d
其中，a,b,c,d分别表示参考路径曲线的各次项系数；由于该曲线经过坐标系原点，且在原点处切线斜率为0，所以c＝d＝0；又因为初始时刻前车在该曲线上，满足以下模型：
Figure BDA0003310540660000071
解方程组可得出a,b值分别为：
Figure BDA0003310540660000072
设定初始化路径包含的路径点的个数为N0，对该三次曲线进行等距插值得到初始化路径序列中的路径点的位置信息
Figure BDA0003310540660000073
在每个点处该曲线的斜率为初始路径序列的姿态信息
Figure BDA0003310540660000074
步骤2，将传感器相对定位信息和里程计信息进行融合，以提高相对定位精度，并且能够在传感器相对定位失效的情况下利用里程计信息提供相对定位信息。
步骤21，首先，根据如附图2所示的相对运动关系，建立运动学方程，并进行离散化，可表示为：
Figure BDA0003310540660000075
其中，Δx表示后车坐标系中，前车质心的纵向相对位置；Δy表示后车坐标系中，前车质心的横向相对位置；Δθ表示前车相对于后车的车头朝向角；ω1表示前车的横摆角速度；ω2表示后车的横摆角速度；vx1表示前车的纵向车速；vv2表示后车的纵向车速。
步骤22，其次，扩展卡尔曼滤波算法融合传感信息和里程计信息。
记状态向量为Xk＝[Δxk,Δyk,Δθk]T，得到非线性状态方程，可以简化表示为：
Xk＝f(Xk-1)+wk-1
其中，wk-1为k-1时刻过程噪声，服从高斯分布wk-1～N(0,Q)，Q表示过程噪声的方差。记在k时刻传感器相对定位测量值为Zk，则***的测量方程可以表示为：
Zk＝h(Xk)+ψk
其中，h(·)表示测量函数，由于状态变量可以由传感器直接测量得到，所以有h(Xk)＝HXk，其中H＝diag([1,1,1])，ψk为k时刻测量噪声，服从高斯分布vk～N(0,R)，R的大小取决于传感器相对定位测量精度。
在具体实现扩展卡尔曼滤波算法时，首先根据k-1时刻的***状态，利用状态方程可以得到***在k时刻的状态的先验估计值，表示为：
Figure BDA0003310540660000081
其中，
Figure BDA0003310540660000082
为k-1时刻状态X的后验估计值。k时刻的状态的先验估计值对应的协方差可以表示为：
Figure BDA0003310540660000083
其中，
Figure BDA0003310540660000084
表示k时刻的状态的先验估计值对应的协方差，Pk-1表示k-1时刻估计的协方差；Jf表示状态函数的雅可比矩阵，可以通过对状态方程求偏导获得：
Figure BDA0003310540660000085
其中，vx,t-1表示k-1时刻沿车辆前进方向的速度。
***的卡尔曼增益Kk可以表示为：
Figure BDA0003310540660000091
Jh表示测量函数的雅可比矩阵，为单位对角阵。
最后需要更新***最优估计状态的协方差，以使扩展卡尔曼滤波算法能够持续更新：
Figure BDA0003310540660000092
其中，I表示单位矩阵。
根据k时刻预测值
Figure BDA0003310540660000093
测量值Zk以及卡尔曼增益值Kk可以求出***在该时刻的最优估计值
Figure BDA0003310540660000094
为：
Figure BDA0003310540660000095
为了防止相对定位传感信息丢失，引入一个参数Θk表征传感信息的可用状态。在传感信息不可用时，Θk＝0，表示仅用里程计信息进行预测；当感知***数据可用时，Θk＝1，将两信息进行融合。
通过以上步骤可以得到k时刻的相对位姿最优估计值，记为
Figure BDA0003310540660000096
Figure BDA0003310540660000097
Figure BDA0003310540660000098
分别表示后车坐标系中前车质心的纵向相对位置、横向相对位置和相对车头朝向角。
步骤3，对路径点进行筛选，生成有效路径。
步骤31，根据运动学关系进行历史路径点的坐标系转换，当***采样时间T足够小时，在[(k-1)T，kT]时间间隔内，后车位姿变化可以表示为δq＝(δx,δy,δθ)T，根据车辆的单轨运动学模型，有：
Figure BDA0003310540660000099
其中，vx表示后车纵向车速，lf,lr分别表示后车前轴到质心的距离和后轴到质心的距离，δf表示后车的前轮转角，ω表示后车的横摆角速度。
通过以下坐标系变换方程，将k-1时刻车辆坐标系的位姿信息转化到k时刻车辆坐标系中：
Figure BDA0003310540660000101
步骤32，沿车辆前进方向，在质心后方的路径点的重建对后续路径跟踪没有意义，因此将这些路径点从序列中剔除。假设在t＝kT时刻，路径序列长度为
Figure BDA0003310540660000102
经过坐标系变换后，存在一个正整数i0满足
Figure BDA0003310540660000103
那么可以利用下式进行路径点的剔除：
Figure BDA0003310540660000104
Figure BDA0003310540660000105
Figure BDA0003310540660000106
其中，
Figure BDA0003310540660000107
此时路径点序列记为
Figure BDA0003310540660000108
同时，对于新加入的路径点数据，为了避免由于前车静止造成的路径点堆叠，以及传感数据异常造成的路径点位置突变，设计路径点跳动距离范围上下限dmin,dmax，当新加入的路径点满足以下不等关系：
Figure BDA0003310540660000109
将该路径点加入到路径序列中：
Figure BDA00033105406600001010
Figure BDA00033105406600001011
Figure BDA00033105406600001012
其中
Figure BDA00033105406600001013
Figure BDA00033105406600001014
分别为该时刻的纵向速度和横摆角速度的估计值。此时路径表示为
Figure BDA00033105406600001015
若不满足上述不等关系，则不加入新的路径点，路径表示为
Figure BDA00033105406600001016
本发明不仅局限于上述具体实施方式，本领域一般技术人员根据实施例和附图公开内容，可以采用其它多种具体实施方式实施本发明，因此，凡是采用本发明的设计结构和思路，做一些简单的变换或更改的设计，都落入本发明保护的范围。

Claims (10)

1.一种无GPS跟车场景下前车路径重建方法，其特征在于，所述前车路径重建方法包括以下步骤：
步骤1：基于前后车初始时刻相对位置，生成一条参考路径；
步骤2：将传感器相对定位信息和里程计信息进行融合，获取相对位姿最优估计值；
步骤3：根据相对位姿最优估计值构建运动学模型，并对后车路径初始化并进行筛选，生成有效路径。
2.根据如权利要求1所述前车路径重建方法，其特征在于，在步骤1中，根据所述初始时刻的相对位姿，以后车质心为坐标系原点，车头朝向为x轴正方向，建立车辆坐标系，根据前车的相对位置和车头朝向角进行三次曲线拟合：
f(x)＝ax3+bx2+cx+d
其中，a,b,c,d分别表示参考路径曲线的各次项系数；由于该曲线经过坐标系原点，且在原点处切线斜率为0，所以c＝d＝0；又因为初始时刻前车在该曲线上，满足：
Figure FDA0003310540650000011
解方程组可得出a,b值分别为：
Figure FDA0003310540650000012
设定初始化路径包含的路径点的个数为N0，对该三次曲线进行等距插值得到初始化路径序列中的路径点的位置信息
Figure FDA0003310540650000013
在每个点处该曲线的斜率为初始路径序列的姿态信息
Figure FDA0003310540650000021
3.根据如权利要求1所述前车路径重建方法，其特征在于，在步骤2中，建立运动学方程，并进行离散化：
Figure FDA0003310540650000022
其中，Δx表示后车坐标系中，前车质心的纵向相对位置；Δy表示后车坐标系中，前车质心的横向相对位置；Δθ表示前车相对于后车的车头朝向角；ω1表示前车的横摆角速度；ω2表示后车的横摆角速度；vx1表示前车的纵向车速；vx2表示后车的纵向车速。
4.根据如权利要求3所述前车路径重建方法，其特征在于，在步骤2中，通过扩展卡尔曼滤波算法，根据k-1时刻的***状态，利用状态方程可以得到***在k时刻的状态的先验估计值，表示为：
Figure FDA0003310540650000023
其中，
Figure FDA0003310540650000024
为k-1时刻的后验估计值；k时刻的状态的先验估计值对应的协方差可以表示为：
Figure FDA0003310540650000025
其中，
Figure FDA0003310540650000026
表示k时刻的状态的先验估计值对应的协方差，Pk-1表示k-1时刻估计的协方差；Jf表示状态函数的雅可比矩阵。
5.根据如权利要求4所述前车路径重建方法，其特征在于，在步骤2中，所述状态函数的雅可比矩阵表示为：
Figure FDA0003310540650000027
其中，vx,t-1表示k-1时刻后车的纵向速度。
6.根据如权利要求4所述前车路径重建方法，其特征在于，在步骤2中，***在该时刻的最优估计值
Figure FDA0003310540650000028
为：
Figure FDA0003310540650000031
Figure FDA0003310540650000032
表示k时刻预测值；Zk表示k时刻测量值；Kk表示k时刻卡尔曼增益值；Θk表示感知***数据的可用状态，在传感信息不可用时，Θk＝0；当感知***数据可用时，Θk＝1；
所述最优估计值记为
Figure FDA0003310540650000033
Figure FDA0003310540650000034
分别表示后车坐标系中前车质心的纵向相对位置、横向相对位置和相对车头朝向角；
所述k时刻卡尔曼增益值可以表示为：
Figure FDA0003310540650000035
Jh表示测量函数的雅可比矩阵，为单位对角阵。
7.根据如权利要求6所述前车路径重建方法，其特征在于，在步骤2中，使所述扩展卡尔曼滤波算法能够持续更新，需要更新***最优估计状态的协方差：
Figure FDA0003310540650000036
其中，I表示单位矩阵；
获取k时刻的相对位置最优估计值，记为
Figure FDA0003310540650000037
Figure FDA0003310540650000038
分别表示后车坐标系中前车质心的纵向相对位置、横向相对位置和相对车头朝向角。
8.根据如权利要求6所述前车路径重建方法，其特征在于，在步骤3中，***采样时间T足够小时，在[(k-1)T，kT]时间间隔内，后车位姿变化可以表示为δq＝(δx,δy,δθ)T，根据车辆的单轨运动学模型，有：
Figure FDA0003310540650000039
其中，vx表示后车纵向车速，lf,lr分别表示后车前轴到质心的距离和后轴到质心的距离，δf表示后车的前轮转角，ω表示后车的横摆角速度；将k-1时刻车辆坐标系的位姿信息转化到k时刻车辆坐标系中：
Figure FDA0003310540650000041
9.根据如权利要求8所述前车路径重建方法，其特征在于，在步骤3中，假设在t＝kT时刻，路径序列长度为
Figure FDA0003310540650000042
经过坐标系变换后，存在一个正整数i0满足
Figure FDA0003310540650000043
那么可以利用下式进行路径点的剔除：
Figure FDA0003310540650000044
Figure FDA0003310540650000045
Figure FDA0003310540650000046
其中，
Figure FDA0003310540650000047
此时路径点序列记为
Figure FDA0003310540650000048
10.根据如权利要求9所述前车路径重建方法，其特征在于，在步骤3中，新加入的路径点满足以下不等关系：
Figure FDA0003310540650000049
其中，dmin表示设计路径点跳动距离范围下限，dmax表示设计路径点跳动距离范围上限；
将满足所述不等关系的路径点加入到路径序列中：
Figure FDA00033105406500000410
Figure FDA00033105406500000411
Figure FDA00033105406500000412
其中，
Figure FDA00033105406500000413
Figure FDA00033105406500000414
分别为该时刻的纵向速度和横摆角速度的估计值；此时路径表示为
Figure FDA00033105406500000415
若不满足上述不等关系，则不加入新的路径点，路径表示为
Figure FDA00033105406500000416
CN202111215298.4A 2021-10-19 2021-10-19 一种无gps跟车场景下前车路径重建方法 Active CN113870316B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111215298.4A CN113870316B (zh) 2021-10-19 2021-10-19 一种无gps跟车场景下前车路径重建方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111215298.4A CN113870316B (zh) 2021-10-19 2021-10-19 一种无gps跟车场景下前车路径重建方法

Publications (2)

Publication Number Publication Date
CN113870316A true CN113870316A (zh) 2021-12-31
CN113870316B CN113870316B (zh) 2023-08-15

Family

ID=79000236

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111215298.4A Active CN113870316B (zh) 2021-10-19 2021-10-19 一种无gps跟车场景下前车路径重建方法

Country Status (1)

Country Link
CN (1) CN113870316B (zh)

Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107015238A (zh) * 2017-04-27 2017-08-04 睿舆自动化（上海）有限公司 基于三维激光雷达的无人车自主定位方法
CN108519773A (zh) * 2018-03-07 2018-09-11 西安交通大学 一种结构化环境下无人驾驶车辆的路径规划方法
CN108646763A (zh) * 2018-07-18 2018-10-12 扬州大学 一种自主行驶轨迹跟踪控制方法
CN109668569A (zh) * 2018-12-08 2019-04-23 华东交通大学 一种智能驾驶中路径快速生成方法
CN109991972A (zh) * 2017-12-29 2019-07-09 长城汽车股份有限公司 控制车辆行驶的方法、装置、车辆及可读存储介质
CN110806215A (zh) * 2019-11-21 2020-02-18 北京百度网讯科技有限公司 车辆定位的方法、装置、设备及存储介质
CN111231965A (zh) * 2020-01-14 2020-06-05 北京小马智行科技有限公司 车辆控制模式的调整方法及装置、无人驾驶车辆
US20200180611A1 (en) * 2018-12-07 2020-06-11 Volkswagen Aktiengesellschaft Driver Assistance System For A Motor Vehicle, Motor Vehicle And Method For Operating A Motor Vehicle
CN111338340A (zh) * 2020-02-21 2020-06-26 天津大学 基于模型预测的无人驾驶汽车局部路径规划方法
CN111707272A (zh) * 2020-06-28 2020-09-25 湖南大学 一种地下车库自动驾驶激光定位***
CN111750887A (zh) * 2020-06-11 2020-10-09 上海交通大学 降低事故严重程度的无人驾驶车辆轨迹规划方法及***
CN112130559A (zh) * 2020-08-21 2020-12-25 同济大学 一种基于uwb与激光雷达的室内行人跟随与避障方法
CN112904849A (zh) * 2021-01-18 2021-06-04 北京科技大学 一体式自动驾驶汽车换道轨迹规划和跟踪控制方法及***
CN113386795A (zh) * 2021-07-05 2021-09-14 西安电子科技大学芜湖研究院 一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策***
CN113433947A (zh) * 2021-07-15 2021-09-24 天津大学 基于障碍车估计与预测的交叉路口轨迹规划与控制方法

Patent Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107015238A (zh) * 2017-04-27 2017-08-04 睿舆自动化（上海）有限公司 基于三维激光雷达的无人车自主定位方法
CN109991972A (zh) * 2017-12-29 2019-07-09 长城汽车股份有限公司 控制车辆行驶的方法、装置、车辆及可读存储介质
CN108519773A (zh) * 2018-03-07 2018-09-11 西安交通大学 一种结构化环境下无人驾驶车辆的路径规划方法
CN108646763A (zh) * 2018-07-18 2018-10-12 扬州大学 一种自主行驶轨迹跟踪控制方法
US20200180611A1 (en) * 2018-12-07 2020-06-11 Volkswagen Aktiengesellschaft Driver Assistance System For A Motor Vehicle, Motor Vehicle And Method For Operating A Motor Vehicle
CN109668569A (zh) * 2018-12-08 2019-04-23 华东交通大学 一种智能驾驶中路径快速生成方法
CN110806215A (zh) * 2019-11-21 2020-02-18 北京百度网讯科技有限公司 车辆定位的方法、装置、设备及存储介质
CN111231965A (zh) * 2020-01-14 2020-06-05 北京小马智行科技有限公司 车辆控制模式的调整方法及装置、无人驾驶车辆
CN111338340A (zh) * 2020-02-21 2020-06-26 天津大学 基于模型预测的无人驾驶汽车局部路径规划方法
CN111750887A (zh) * 2020-06-11 2020-10-09 上海交通大学 降低事故严重程度的无人驾驶车辆轨迹规划方法及***
CN111707272A (zh) * 2020-06-28 2020-09-25 湖南大学 一种地下车库自动驾驶激光定位***
CN112130559A (zh) * 2020-08-21 2020-12-25 同济大学 一种基于uwb与激光雷达的室内行人跟随与避障方法
CN112904849A (zh) * 2021-01-18 2021-06-04 北京科技大学 一体式自动驾驶汽车换道轨迹规划和跟踪控制方法及***
CN113386795A (zh) * 2021-07-05 2021-09-14 西安电子科技大学芜湖研究院 一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策***
CN113433947A (zh) * 2021-07-15 2021-09-24 天津大学 基于障碍车估计与预测的交叉路口轨迹规划与控制方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
HANZHANG XUE等: "IMU-Aided High-Frequency Lidar Odometry for Autonomous Driving", 《APPLIED SCIENCES》, vol. 9, pages 1 - 20 *
化祖旭: "自动驾驶汽车路径跟踪控制算法综述", 《装备制造技术》, no. 5, pages 100 - 103 *
肖军浩等: "单目相机-3维激光雷达的外参标定及融合里程计研究", 《机器人》, vol. 43, no. 1, pages 17 - 28 *
谭光兴等: "基于扩展卡尔曼滤波的汽车行驶状态估计", 《广西科技大学学报》, vol. 31, no. 1, pages 18 - 24 *
赵凌峰: "高速无人驾驶车辆横纵向控制算法的研究", 《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》, no. 06, pages 035 - 200 *

Also Published As

Publication number Publication date
CN113870316B (zh) 2023-08-15

Similar Documents

Publication Publication Date Title
CN106476728B (zh) 用于车载的车辆传感器的运动补偿
Son et al. Robust multirate control scheme with predictive virtual lanes for lane-keeping system of autonomous highway driving
EP3678110B1 (en) Method for correcting positional error and device for correcting positional error in a drive-assisted vehicle
US20210403032A1 (en) Two-level path planning for autonomous vehicles
Kang et al. Comparative evaluation of dynamic and kinematic vehicle models
CN112083726B (zh) 一种面向园区自动驾驶的双滤波器融合定位***
US20160299234A1 (en) Fail operational vehicle speed estimation through data fusion of 6-dof imu, gps, and radar
JP6594589B1 (ja) 走行計画生成装置および自動運転システム
CN112486156B (zh) 一种车辆自动循迹控制***及控制方法
Xia et al. Advancing estimation accuracy of sideslip angle by fusing vehicle kinematics and dynamics information with fuzzy logic
KR102024986B1 (ko) 차로 유지 제어 장치 및 방법
Liu et al. Slip-aware motion estimation for off-road mobile robots via multi-innovation unscented Kalman filter
CN112433531A (zh) 一种自动驾驶车辆的轨迹跟踪方法、装置及计算机设备
CN112099378B (zh) 考虑随机测量时滞的前车侧向运动状态实时估计方法
CN114912061B (zh) 一种面向商用车车道保持辅助***的精准测评方法
CN115923839A (zh) 一种车辆路径规划方法
Zindler et al. Real-time ego-motion estimation using Lidar and a vehicle model based Extended Kalman Filter
Berntorp et al. Bayesian sensor fusion of GNSS and camera with outlier adaptation for vehicle positioning
Katriniok Optimal vehicle dynamics control and state estimation for a low-cost GNSS-based collision avoidance system
CN113870316A (zh) 一种无gps跟车场景下前车路径重建方法
Zhou et al. Vision-based control of an industrial vehicle in unstructured environments
CN117146838A (zh) 一种路径规划方法及装置、相关产品
EP2364891B1 (en) Method for threat assessment in a vehicle
Han et al. The Leader-Follower Formation Control of Nonholonomic Vehicle With Follower-Stabilizing Strategy
CN113048987A (zh) 一种车载导航***定位方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant