前方目标历史轨迹生成方法及介质与流程

本发明涉及汽车领域，特别是涉及一种车辆智能驾驶或先进驾驶辅助系统中的前方目标历史轨迹生成方法。

背景技术：

1、在优化智能驾驶或先进驾驶辅助系统性能中，环境感知系统是一个关键部分，而利用前方各目标(包含但不仅限于各类车辆)的历史运动轨迹提取相关道路信息用于环境感知系统中车道线融合部分则是一种低成本，可动态更新的方法。

2、前方各目标历史轨迹的稳定性和精准性会直接影响到环境感知系统的性能，现有对前方目标历史轨迹生成的方法往往是对历史采样点直接进行多项式拟合获得，但这种方法无法处理目标历史位置采样存在噪点的情况，在本身采样点较少的情况下，仅一个采样点的偏移都会对轨迹拟合有很大影响。

3、中国专利cn202211248285.1公开了车辆轨迹预测方法包括：当车辆进入路口时，基于路口的地图信息确定车辆规范行驶的第一层出口候选点和车辆不规范行驶的第二层出口候选点，预测车辆行驶至第一层出口候选点以及第二层出口候选点的多条预测轨迹；基于预置的代价函数确定多条预测轨迹各自的转弯幅度代价；从多条预测轨迹各自的转弯幅度代价中，优先选择数值最低的转弯幅度代价所对应的预测轨迹作为车辆的轨迹预测结果。该解决无法克服目标历史位置采样存在噪点的情况，在本身采样点较少的情况下，仅一个采样点的偏移都会对轨迹拟合有很大影响。

4、中国专利cn202210956957.8公开了一种车辆轨迹预测深度学习模型的鲁棒性增强方法，首先训练得到鲁棒性增强模型，其次以车辆轨迹的历史信息作为鲁棒性增强模型的输入，输出下一帧车辆轨迹作为重构轨迹，计算真实轨迹与重构轨迹之间距离，并根据实验所得检测阈值判断是否存在噪声，以此决定是否替换下一帧轨迹，达到增强车辆轨迹预测深度学习模型鲁棒性的目的，方法在ngsim数据集上得到验证。其需要通过已有的深度学习车辆轨迹预测模型，利用生成的噪声包括：pgd(投影梯度下降)算法生成对抗噪声，对车辆历史轨迹点进行逐点噪声检测与重构，其方案较为复杂对系统性能要求较高，且仅能解决噪声问题无法解决轨迹点偏移的问题。

技术实现思路

1、在
技术实现要素：
部分中引入了一系列简化形式的概念,该简化形式的概念均为本领域现有技术简化，这将在具体实施方式部分中进一步详细说明。本发明的发明内容部分并不意味着要试图限定出所要求保护的技术方案的关键特征和必要技术特征，更不意味着试图确定所要求保护的技术方案的保护范围。

2、本发明要解决的技术问题是提供一种能避免前方目标的历史轨迹生成稳定性(采样存在噪点)和准确率(轨迹点偏移)对前方目标历史轨迹生成产生影响的前方目标历史轨迹生成方法。

3、为解决上述技术问题，本发明提供的前方目标历史轨迹生成方法，包括以下步骤：

4、s1，通过上周期历史轨迹点状态及当前全部有效目标，对当前各目标对应的历史轨迹点及其状态做相应动态更新；历史轨迹点的状态包括：有效、无效和未使用；

5、有效，表示该历史轨迹点对应的目标当前有效；

6、无效，表示该历史轨迹点对应的目标当前无效；

7、未使用，表示该历史轨迹点当前未使用；

8、s2，基于指定坐标原点，例如自车后轴中心为坐标原点，随着自车运动对更新后状态为有效或无效的轨迹点位置(即含有有效道路信息)做运动补偿，计算运动补偿后历史轨迹点位置；

9、s3，根据历史轨迹点状态判断整条历史轨迹点可行性并更新历史轨迹点；

10、s4，对整条历史轨迹点进行局部筛选，获得自车后轴后方的轨迹点纵向距离阈值；

11、s5,判断保留轨迹点的拟合适宜性，确保每条保留的历史轨迹点数量和轨迹点两端距离能用于后续拟合；

12、s6，计算轨迹点的权重；

13、s7，利用加权最小二乘法拟合三次多项式的同时加上l2正则项构建损失函数，将计算轨迹点的权重带入损失函数计算得到各目标的历史行驶轨迹模型。

14、可选择的，进一步改进所述的前方目标历史轨迹生成方法，还包括以下步骤：

15、s8，通过均方差计算出历史行驶轨迹模型拟合误差，若拟合误差小于最大拟合误差阈值则判断该历史行驶轨迹模型拟合成功，否则判断拟合失败。

16、可选择的，进一步改进所述的前方目标历史轨迹生成方法，采用下述公式计算计算运动补偿后历史轨迹点位置；

17、基于变量均在同一坐标系下先旋转后平移(默认顺时针旋转为正)的坐标转换公式，目标历史轨迹点新位置为：

18、

19、

20、由于坐标系先旋转后平移，故产生速度分量初始速度分量最终速度分量vx＝vi cos(ωiti),vy＝vi sin(ωiti)，

21、故平均速度分量为

22、利用泰勒展开近似三角函数公式为：

23、根据(2)和(3)中的展开到一阶项，坐标增量δxi，δyi近似后分别为：

24、

25、

26、将(3)(4)代入至坐标转换公式(1)中近似后可得：

27、

28、

29、目标历史轨迹点新位置分别目标历史轨迹点位置坐标分别xi、yi，横摆角速度为ωi，采样时间为ti，采样车速vi。

30、由公式(5)可得基于自车运动更新，以自车后轴中心为坐标原点运动补偿后的目标历史轨迹点位置。

31、可选择的，进一步改进所述的前方目标历史轨迹生成方法，实施步骤s3时，当历史轨迹点状态为无效时，删除整条历史轨迹点均在自车后轴后方最远距离阈值maxrngrear(用于判断整条历史轨迹中是否所有轨迹点的纵向位置均小于这个最远距离阈值)外的各条历史轨迹点；

32、例如，当前设置的自车后轴后方最远距离阈值maxrngrear值为-5，那么若该条历史轨迹中的所有历史轨迹点纵向距离均小于-5，则删除该条历史轨迹；若并非所有历史轨迹点均小于-5，则不做删除动作。

33、当历史轨迹点状态为有效且达到有效轨迹点数量阈值时，则需根据最新历史轨迹点到对应目标当前位置的直线距离与最新轨迹点位置到当前目标车辆位置直线距离最大阈值maxobjdist(若距离大于等于该阈值，则通过将当前目标车辆位置插入最新历史轨迹点以此延长该条历史轨迹；若距离小于该阈值，则维持该条历史轨迹现状)的关系判断是否需要通过将当前目标位置插入最新历史轨迹点以此延长该条历史轨迹点。

34、可选择的，进一步改进所述的前方目标历史轨迹生成方法，实施步骤s4时，局部筛选包括；

35、考虑到当目标最新历史轨迹点处于自车前保处，即将可能丢失目标时，其对应历史轨迹点需要达到最短纵向距离阈值mindist用于后续拟合，故保留前保后方mindis后最近点以内的所有轨迹点。即：为方便判断轨迹点位置，给定自车后轴到前保距离，可得自车后轴后方的轨迹点纵向距离阈值londistrear。

36、可选择的，进一步改进所述的前方目标历史轨迹生成方法，实施步骤s5时，为确保保留的轨迹点可以有效用于拟合三次多项式，需要判断保留轨迹点的拟合适宜性包括：

37、根据上周期该条轨迹点的适宜性，确保每条保留的历史轨迹点数量满足有效轨迹点数量阈值minfitpts，并且轨迹点两端距离满足有效轨迹距离阈值minfitl，若均满足则可用于后续拟合。

38、可选择的，进一步改进所述的前方目标历史轨迹生成方法，实施步骤s6时，采用下述方式计算轨迹点的权重；

39、考虑到检测的历史轨迹点位置采样的不确定性，每个轨迹点的权重取其方差的倒数，而对位于自车后轴后方的轨迹点，它们的权重则通过线性插值重新进行计算来处理其不确定性，公式如下：

40、如果xi＞＝0则

41、如果xi＜0，

42、wi是各轨迹点权重，londistrear是自车后轴后方的轨迹点纵向距离阈值。

43、可选择的，进一步改进所述的前方目标历史轨迹生成方法，对筛选出的目标对应历史轨迹点，设目标的历史行驶轨迹为三次多项式。为解决轨迹点采样不确定性及模型过拟合现象，利用加权最小二乘法拟合三次多项式的同时加上l2正则项通过收缩系数控制模型复杂度防止模型过拟合，故损失函数为：

44、

45、wi是各轨迹点权重，c0、c1、c2、c3别对应该三次多项式的各项系数，n是筛选出的历史轨迹中有效轨迹点总数量。示例性的当前模型中该数量范围为大于等于4的同时，小于等于32，λi为惩罚项参数。

46、将公式(6)代入公式(7)式中可计算得到各目标的历史行驶轨迹。

47、可选择的，进一步改进所述的前方目标历史轨迹生成方法，实施步骤s8时，均方差公式：

48、通过解出上述均方差公式多项式系数计算出历史轨迹模型拟合误差。

49、本发明还提供一种计算机可读存储介质，其内部存储有一程序，所述程序被执行时，实现上述任意一项所述前方目标历史轨迹生成方法中的步骤。

50、针对智能驾驶辅助系统，为了保证实时性，本发明对各目标对应的历史轨迹点通过状态属性和运动补偿持续更新相关信息。为保证用于后续拟合的历史轨迹点有效性，对更新后的历史轨迹点通过位置和数量的维度进行整体可行性和局部筛选的判断。最后基于筛选出的有效轨迹点，利用对加权最小二乘法添加正则化的方式进行三次多项式拟合，以处理轨迹点位置的不确定性及规避模型过拟合的情况。本发明通能提高智能驾驶辅助系统中目标历史轨迹拟合的鲁棒性。