CN114312840A - 自动驾驶障碍目标轨迹拟合方法、系统、车辆及存储介质 - Google Patents

Abstract

本发明请求保护一种自动驾驶障碍目标轨迹拟合方法，涉及自动驾驶技术领域。根据障碍物目标的位置，横纵坐标变化及横纵向车速变化性、目标出现的时长，目标的稳定性确定目标的置信度；当目标的置信度低于标定阈值时，将目标筛除；根据本车的横纵向车速及航向角构建障碍目标上一周期本车坐标系到本周期本车坐标系中的变换矩阵，计算并描绘障碍物目标的历史点迹，并根据变换矩阵将目标历史点迹转换至当前本车坐标系下，用最小二乘法拟合障碍物目标的历史轨迹，构建单个障碍物目标的轨迹方程，根据障碍物目标出现的时长、置信度，运动轨迹给不同障碍物目标的轨迹方程系数分配权重，获得最终车流轨迹方程，确定自动驾驶行车轨迹。

Description

自动驾驶障碍目标轨迹拟合方法、系统、车辆及存储介质

技术领域

本发明属于无人驾驶技术领域，具体涉及一种自动驾驶车辆运动轨迹拟合方法。

背景技术

当前工程应用领域，无人驾驶车辆决策控制会综合传感输入的车道线信息，周围环境大的障碍物信息进行横纵向控制，为了避免本车车辆与障碍物车辆发生碰撞，根据车辆的历史运动信息推测障碍物车辆的行驶意图，以及当传感检测的道路信息失效时，更好的参考障碍物车辆的行驶路径，从而更好的控制本车安全行驶，拟合周围环境车辆的历史轨迹就显得尤为重要。当本车行驶在没有车道线的路段或者传感丢失道路信息时，适当参考前方目标车辆的行驶轨迹，使得车辆正常行驶，降低自动驾驶过程中接管次数甚至安全事故发生的概率。

中国发明专利申请CN110962847A车道居中辅助自适应巡航的轨迹规划方法和系统，公开了一种车道居中辅助自适应巡航的轨迹规划方法，包括：根据获取的车道线数据对车道线进行拟合，得到车道线方程，生成车辆的感兴趣区域；通过对感兴趣区域内障碍物进行代价计算，得到目标障碍物信息；根据左右车道线数据计算当前所在车道的道路中心线即最佳的行驶路径；根据目标障碍物信息结合自车的车辆状态信息进行自车的纵向速度规划，得到车辆纵向速度曲线，完成轨迹规划，所述目标障碍物信息包括障碍物与自车的相对位置和相对自车的速度信息。当无RTK信号或信号丢失时，基于摄像头反馈数据和毫米波数据得到横向和纵向速度规划曲线，最终完成横纵向解耦控制，实现低成本硬件架构在结构化道路下的自动驾驶。

申请号CN201811609007.8的中国发明专利申请，自动驾驶车辆的行驶轨迹规划系统及方法，适用于自动驾驶车辆，根据所述车辆的当前位置,当前航向角,当前速度,当前加速度,多个路径终点,当前道路宽度,及当前道路曲率产生多个目标轨迹规划结果,每一目标轨迹规划结果包括轨迹路径,及速度变化的曲线；估算出与所述车辆相距预定距离范围内的每一障碍物在行驶期间中的多个单位时间点的多个预估移动范围及多个预估移动速度；及根据所述目标轨迹规划结果,及每一障碍物的所述预估移动范围与所述预估移动速度,从所述目标轨迹规划结果决定出最佳轨迹规划结果。借此进一步考虑每一障碍物的移动意图区域,以提升轨迹规划的精确性及安全性。

上述专利文献重在阐述如何根据周围环境规划本车实行驶路径，规划过程中虽涉及周围障碍物的运动状态，考虑每一障碍物的移动意图区域，当并未将障碍物的历史运动以轨迹的行驶表征出来，如当本车行驶在没有车道线的路段或者传感丢失道路信息时，不能为自动驾驶提供准确导航运动轨迹。

发明内容

本发明针对现有技术存在的上述问题，旨在更好的利用周围环境障碍物车辆的历史信息，对本车的控制提供更多可用信息，提供了一种障碍物车辆历史轨迹的拟合方法，当本车在行驶过程中，传感输入的车道信息发生失效时，可适当参考周围障碍物车辆的历史行驶轨迹来控制本车的行驶轨迹。

本发明解决上述技术问题的技术方案是，接收来自传感器采集的障碍物目标位置信息，进行预处理，包括筛除目标信息中不符合场景或者可信度过低的目标；根据本车运动状态，将目标车辆的历史坐标点转换至当前车辆坐标系下；挑选可参考轨迹的目标车辆，筛除正在变道的目标车辆；最小二乘法拟合单障碍车目标轨迹方程，并筛除正在换到的目标；给有效目标的轨迹方程系数分配权重，求解当前车流轨迹方程。具体为：

一方面，本发明提出一种自动驾驶障碍目标轨迹拟合方法，包括步骤：接收传感器采集的障碍物目标位置信息，根据障碍物目标的位置，横纵坐标变化及横纵向车速变化性、目标出现的时长，目标的稳定性确定目标的置信度；当目标的置信度低于标定阈值时，将目标筛除；根据本车的横纵向车速及航向角构建障碍目标上一周期本车坐标系到本周期本车坐标系中的变换矩阵，计算并描绘障碍物目标的历史点迹，并根据变换矩阵将目标历史点迹转换至当前本车坐标系下，用最小二乘法拟合障碍物目标的历史轨迹，构建单个障碍物目标的轨迹方程，根据障碍物目标出现的时长、置信度，运动轨迹给不同障碍物目标的轨迹方程系数分配权重，获得最终车流轨迹方程，确定自动驾驶行车轨迹。

进一地，确定目标的置信度具体包括：如障碍物目标出现的时间越长，目标的置信度越高；障碍物目标的横纵坐标点、横纵向车速没有数值突变，目标的置信度高；根据传感器的感知特性，探测到障碍物的属性为目标可信度高区域的目标，则目标置信度高；多传感器同时感知到同一目标则目标置信度高；根据目标出现时长、目标坐标及速度突变、目标区域范围、感知目标的传感器数量通过加权计算确定标定阈值。

进一步地，根据本车坐标系坐标原点在纵坐标发生的偏移量Dy、坐标系发生的旋转量angle，构建在上一周期本车坐标系到本周期本车坐标系中障碍目标横纵坐标的平移变换矩阵：和旋转转换矩阵：

通过上述矩阵变换将目标的历史坐标点转换到当前时刻本车坐标系。

进一步地，采集本车当前行驶的纵向车速ESP_VehicleSpeed，航向角ESP_YawRate，上一周期至本周期间的时间间隔interval，调用公式：Dy＝-ESP_VehicleSpeed*interval计算当前时刻相对于上一时刻本车坐标系坐标原点在纵坐标发生的偏移量Dy，调用公式angle＝ESP_YawRate*interval计算坐标系发生的旋转量。

进一步地，传感器采集并存储障碍物目标的历史点迹，根据上一时刻坐标P_t-1调用公式：P_t＝P_t-1*transMatrix*rotateMatrix确定障碍物目标当前时刻坐标P_t，通过平移转换矩阵和旋转转换矩阵将障碍物目标历史点迹转换至当前本车坐标系下。

进一步地，记录障碍物目标历史点迹，当目标历史点迹出现时间超过预定周期时长，若障碍物目标最先出现的点迹坐标位于本车车头以后的位置，则丢弃出现的点迹，当障碍物目标随时间变换坐标点集中，将密集处点迹更新为新的坐标。

进一步地，用最小二乘法拟合单个目标障碍物的历史轨迹，构建当前时刻本车坐标系下单个目标的轨迹方程为：Y＝A₀+A₁*X+A₂*X²+A₃*X³，拟合障碍目标运动轨迹，其中，A0为轨迹方程常数项，A1为轨迹方程一次项系数，A2为轨迹方程二次项系数，A3为轨迹方程三次项系数，根据目标出现时长、目标置信度、轨迹点与轨迹距离的方差给不同目标障碍物的轨迹方程系数分配权重，构建最终目标轨迹方程：Lane_Y＝Lane_A₁*X+Lane_A₂*X²+Lane_A₃*X³。

第二方面，本发明提出一种一种自动驾驶障碍目标轨迹拟合系统，包括：目标信息捕捉单元、识别确定单元、轨迹计算单元、坐标变换单元、，目标信息捕捉单元接收传感器采集的障碍物目标位置信息；识别确定单元根据障碍物目标的位置，横纵坐标变化及横纵向车速变化性、目标出现的时长，目标的稳定性确定目标的置信度，当目标的置信度低于标定阈值时，将目标筛除；坐标变换单元根据本车的横纵向车速及航向角构建障碍目标上一周期本车坐标系到本周期本车坐标系中的变换矩阵，计算并描绘障碍物目标的历史点迹，并根据变换矩阵将目标历史点迹转换至当前本车坐标系下；轨迹计算单元采用最小二乘法拟合障碍物目标的历史轨迹，构建单个障碍物目标的轨迹方程，根据障碍物目标出现的时长、置信度，运动轨迹给不同障碍物目标的轨迹方程系数分配权重，获得最终车流轨迹方程，确定自动驾驶行车轨迹。

第三方面本发明提出一种车辆，包括上述用于自动驾驶障碍目标轨迹拟合系统。

第四方面，本发明还提出一种计算机可读存储介质，其上存储有计算机程序，该程序能够被处理器加载和运行以执行上面所述的自动驾驶障碍目标轨迹拟合方法。

本发明采用传感信息预处理，获取周围道路信息、车辆位置信息，本车运动状态处理，目标车辆坐标转换，有效目标轨迹点选取，目标轨迹拟合等。利用周围环境障碍物车辆的历史信息，对本车的控制提供更多可用信息，当本车行驶在没有车道线的路段或者传感丢失道路信息时，参考前方目标车辆的行驶轨迹，使得车辆正常行驶，降低自动驾驶过程中接管次数甚至安全事故发生的概率。

附图说明

图1本车坐标系变化示意图；

图2障碍车历史轨迹示意图。

具体实施方式

下面结合附图和具体实例对本发明的实施方案作进一步详细说明。本实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式，此处所述的具体实施例不限制本发明的使用方案。

根据障碍物目标的位置，横纵坐标变化及横纵向车速变化的合理性、目标出现的时长，目标的稳定性计算目标的置信度，当目标的置信度低于标定阈值时，将目标筛除；根据本车的横纵向车速及航向角计算障碍车辆上一周期本车坐标系到本周期本车坐标系中障碍车辆横纵坐标的变换矩阵，存储障碍物车辆目标的历史点迹，并将历史点迹转换至当前本车坐标系下，用最小二乘法拟合单个目标的历史轨迹，得到单个目标的轨迹方程，根据目标出现的时长，以及目标可信度，轨迹的优劣给不同车辆轨迹的轨迹方程系数分配权重，求得最终车流轨迹方程。具体包括：

接收来自传感器采集的目标位置信息，并进行预处理，包括筛除目标信息中不符合场景或者可信度过低的目标。例如：如信号显示当前道路只有3条车道，但是目标横向距离远大于3条车道所覆盖的横向距离范围；如传感器显示一段时间内视野范围内都没有目标，但突然距离本车很近的地方出现一个目标，且目标存在的时间很短，则认为目标无效；

根据本车运动状态，将目标车辆的历史坐标点转换至当前车辆坐标系下；

挑选可参考轨迹的目标车辆，筛除正在变道的目标车辆；最小二乘法拟合单障碍车目标轨迹方程，并筛除正在换道的目标车辆；给有效目标的轨迹方程系数分配权重，求解当前车辆运动轨迹方程。接收来自传感器采集的目标信息，根据障碍物目标的位置，横纵坐标变化及横纵向车速变化的合理性、目标出现的时长，目标的稳定性计算目标的置信度，当目标的置信度低于标定阈值时，将目标筛除。目标剔除可考虑以下几种情况：

如目标出现的时间越长，目标的置信度越高；目标的坐标点越符合物体运动规律，(即目标没有横纵坐标点，横纵向车速没有数值突变的情况)，认为目标的置信度越高；考虑传感器的感知特性，探测到障碍物的属性为目标可信度高的区域的目标，则目标置信度高，否则目标置信度低；.多传感器同时感知到同一目标则目标置信度高，否则目标置信度低。标定阈值根据目标出现时长、目标坐标及速度突变、目标区域范围、感知目标的传感器数量通过加权计算确定。

如图1所示为本车坐标系变化示意图，根据本车的横纵向车速及航向角计算障碍车辆上一周期本车坐标系(X_t-1,O_t,Y_t-1)到本周期本车坐标系(X_t,O_t,Y_t)中障碍车辆横纵坐标的变换矩阵，若本车当前行驶的纵向车速为ESP_VehicleSpeed，航向角为ESP_YawRate，上一周期至本周期间的时间间隔为interval，则本周期相对上一周期本车坐标系发生的变化可调用公式：

Dy＝-ESP_VehicleSpeed*interval

计算当前T时刻相对于T-1时刻，本车坐标系坐标原点在纵坐标发生的偏移量Dy。调用公式Angle＝ESP_YawRate*interval计算坐标系发生的旋转量，其中angle表征上一时刻至本时刻周期本车航向变化的角度。

建立T-1时刻的坐标点转换到T时刻本车坐标系下的平移转换矩阵为：

T-1时刻的坐标点转换到T时刻本车坐标系下的旋转转换矩阵为：

上述矩阵为T-1时刻目标在本车坐标系下坐标到T时刻本车坐标系下的坐标转换矩阵。因为本车随时在运动，本车坐标系也在时刻移动，通过上述矩阵变换将目标的历史坐标点转换到当前时刻本车坐标系下。

通过传感器采集并存储目标障碍物车辆的历史点迹，通过平移转换矩阵和旋转转换矩阵将历史点迹转换至当前本车坐标系下，根据上一时刻坐标P_t-1确定当前时刻Pt坐标位置，具体为：

根据前一时刻坐标位置调用公式：

P_t＝P_t-1*transMatrix*rotateMatrix

确定当前时刻障碍目标坐标位置。若T-1时刻障碍目标在本车坐标系下的坐标为P_t-1[lat,long,0]，其中lat为横坐标，long为纵坐标。

公式中Pt-1代表[lat,long,0],Pt-1时刻坐标*transMatrix*rotateMatrix后即得障碍物T-1时刻在T时刻本车坐标系下的坐标点。

在更新坐标点的同时将上一周期(周期时长由算法或者传感器采集数据的周期决定)的历史点迹坐标转换至当前本车坐标系下。记录预定个周期，每周期重复上述转换，记录目标历史点迹，当目标障碍物出现时间超过预定周期时长，若障碍物目标最先出现的点迹坐标位于本车车头以后的位置，则丢弃最早出现的点迹，当障碍物目标车辆运动速度过低时，随时间变换会出现坐标点很集中，将点迹过于密集处的点更新为新的坐标点。

最小二乘要求坐标点在同一坐标系下，坐标变化的目的就是为了将历史时刻本车坐标系下的点迹转换至当前时刻本车坐标系下。

用最小二乘法拟合单个目标障碍物车辆的历史轨迹，构建当前时刻本车坐标系下单个目标的轨迹方程为：

Y＝A0+A1*X+A2*X^2+A3*X^3

其中，A0为轨迹方程常数项(可为车道宽度)，A1为轨迹方程一次项系数(可为轨迹曲线斜率)，A2为轨迹方程二次项系数(可为轨迹曲率)，A3为轨迹方程三次项系数(可为轨迹最远端曲率)，上述系数可分别为：本车传感器采集并融合得到的车道宽度、轨迹曲线斜率、曲率、轨迹最远端的曲率等。

当环境中车辆较多，且同段纵向位置段中某个目标的轨迹曲率A2与其它目标的轨迹曲率相差较大时(如当与其他目标轨迹二次项系数A2相减差值的绝对值大于标定值时认为轨迹曲率相差较大)则认为该目标为变道目标，变道目标在拟合道路车流曲线时分配权重为0。

如图2所示为障碍目标历史轨迹示意图，障碍目标车辆RT₂目标的轨迹曲率A2与其它目标的轨迹曲率相差较大，认为RT2为换道目标，在拟合道路车流曲线时分配权重为0；根据目标出现的时长，以及目标置信度，轨迹的优劣给不同车辆轨迹的轨迹方程系数分配权重，可设置为：目标置信度越高，轨迹的权重越高；轨迹点与轨迹距离的方差越小，轨迹权重越高；

构建最终车流轨迹方程：

Lane_Y＝Lane_A1*X+Lane_A2*X^2+Lane_A3*X^3该方程系数由单个目标轨迹方程系数加权获得。其中，Lane_A1、Lane_A2、Lane_A3分别为加权后的轨迹方程一次项系数、轨迹方程二次项系数、轨迹方程三次项系数。

通过求解上述方程，获得目标轨迹之后计算目标与本车是否有碰撞可能，在没有车道线时可修正本车运动轨迹，用于自动驾驶过程的目标选择。

Claims (10)

1.一种自动驾驶障碍目标轨迹拟合方法，其特征在于，包括步骤：接收传感器采集的障碍物目标位置信息，根据障碍物目标的位置，横纵坐标变化及横纵向车速变化性、目标出现的时长，目标的稳定性确定目标的置信度；当目标的置信度低于标定阈值时，将目标筛除；根据本车的横纵向车速及航向角构建障碍目标上一周期本车坐标系到本周期本车坐标系中的变换矩阵，计算并描绘障碍物目标的历史点迹，并根据变换矩阵将目标历史点迹转换至当前本车坐标系下，用最小二乘法拟合障碍物目标的历史轨迹，构建单个障碍物目标的轨迹方程，根据障碍物目标出现的时长、置信度，运动轨迹给不同障碍物目标的轨迹方程系数分配权重，获得最终车流轨迹方程，确定自动驾驶行车轨迹。

2.根据权利要求1所述的方法，其特征在于，确定目标的置信度具体包括：如障碍物目标出现的时间越长，目标的置信度越高；障碍物目标的横纵坐标点、横纵向车速没有数值突变，目标的置信度高；根据传感器的感知特性，探测到障碍物的属性为目标可信度高区域的目标，则目标置信度高；多传感器同时感知到同一目标则目标置信度高；根据目标出现时长、目标坐标及速度突变、目标区域范围、感知目标的传感器数量通过加权计算确定标定阈值。

3.根据权利要求1或2所述的方法，其特征在于，根据本车坐标系坐标原点在纵坐标发生的偏移量Dy、坐标系发生的旋转量angle，构建在上一周期本车坐标系到本周期本车坐标系中障碍目标横纵坐标的平移变换矩阵：

和旋转转换矩阵：

通过上述矩阵变换将目标的历史坐标点转换到当前时刻本车坐标系。

4.根据权利要求3所述的方法，其特征在于，采集本车当前行驶的纵向车速ESP_VehicleSpeed，航向角ESP_YawRate，上一周期至本周期间的时间间隔interval，调用公式：Dy＝-ESP_VehicleSpeed*interval计算当前时刻相对于上一时刻本车坐标系坐标原点在纵坐标发生的偏移量Dy，调用公式angle＝ESP_YawRate*interval计算坐标系发生的旋转量。

5.根据权利要求1-4其中之一所述的方法，其特征在于，传感器采集并存储障碍物目标的历史点迹，根据上一时刻坐标P_t-1调用公式：

P_t＝P_t-1*transMatrix*rotateMatrix确定障碍物目标当前时刻坐标P_t，通过平移转换矩阵和旋转转换矩阵将障碍物目标历史点迹转换至当前本车坐标系下。

6.根据权利要求1-4其中之一所述的方法，其特征在于，记录障碍物目标历史点迹，当目标历史点迹出现时间超过预定周期时长，若障碍物目标最先出现的点迹坐标位于本车车头以后的位置，则丢弃出现的点迹，当障碍物目标随时间变换坐标点集中，将密集处点迹更新为新的坐标。

7.根据权利要求1-4其中之一所述的方法，其特征在于，用最小二乘法拟合单个目标障碍物的历史轨迹，构建当前时刻本车坐标系下单个目标的轨迹方程为：Y＝A₀+A₁*X+A₂*X²+A₃*X³，拟合障碍目标运动轨迹，其中，A0为轨迹方程常数项，A1为轨迹方程一次项系数，A2为轨迹方程二次项系数，A3为轨迹方程三次项系数，根据目标出现时长、目标置信度、轨迹点与轨迹距离的方差给不同目标障碍物的轨迹方程系数分配权重，构建最终目标轨迹方程：Lane_Y＝Lane_A₁*X+Lane_A₂*X²+Lane_A₃*X³。

8.一种自动驾驶障碍目标轨迹拟合系统，其特征在于，包括：目标信息捕捉单元、识别确定单元、轨迹计算单元、坐标变换单元、，目标信息捕捉单元接收传感器采集的障碍物目标位置信息；识别确定单元根据障碍物目标的位置，横纵坐标变化及横纵向车速变化性、目标出现的时长，目标的稳定性确定目标的置信度，当目标的置信度低于标定阈值时，将目标筛除；坐标变换单元根据本车的横纵向车速及航向角构建障碍目标上一周期本车坐标系到本周期本车坐标系中的变换矩阵，计算并描绘障碍物目标的历史点迹，并根据变换矩阵将目标历史点迹转换至当前本车坐标系下；轨迹计算单元采用最小二乘法拟合障碍物目标的历史轨迹，构建单个障碍物目标的轨迹方程，根据障碍物目标出现的时长、置信度，运动轨迹给不同障碍物目标的轨迹方程系数分配权重，获得最终车流轨迹方程，确定自动驾驶行车轨迹。

9.一种车辆，其特征在于，包括权利要求8所述的用于自动驾驶障碍目标轨迹拟合系统。

10.一种计算机可读存储介质，其特征在于，其上存储有计算机程序，该程序能够被处理器加载和运行以执行权利要求1至7中任一项所述的自动驾驶障碍目标轨迹拟合方法。

Images