路沿跟踪方法、装置、计算机可读存储介质及驾驶装置与流程

1.本发明涉及目标跟踪技术领域，具体提供一种路沿跟踪方法、装置、计算机可读存储介质及驾驶装置。


背景技术：

2.路沿检测是在城市地区行驶的自动驾驶车辆需要具备的一项重要感知功能，通过进行路沿检测可为车辆提供道路边界信息，约束车辆行驶区域。激光雷达作为自动驾驶重要的传感器，基于其检测路沿也是一种常用的功能。由于道路的复杂性与多变性，基于激光雷达进行检测时，点云稀疏性与不稳定性以及激光雷达量测噪音等原因，会影响单帧路沿检测的准确性与鲁棒性。通过应用跟踪算法可以提高路沿检测的稳定性以及抗干扰能力，然而，目前在路沿跟踪方面的研究较少，且基本以单纯应用卡尔曼滤波器进行路沿跟踪为主，无法有效解决检测失效以及量测跳变等问题。


技术实现要素：

3.本发明旨在解决上述技术问题，即，解决现有基于卡尔曼滤波的路沿跟踪方法无法有效解决检测失效以及量测跳变的问题。
4.在第一方面，本发明提供了一种路沿跟踪方法，其包括：
5.获取至少一侧路沿对应的路沿拟合曲线和路沿历史轨迹，其中所述路沿历史轨迹包括主轨迹和备用轨迹，所述路沿拟合曲线、所述主轨迹以及所述备用轨迹基于以行驶装置为原点的三维坐标系构建，所述三维坐标系以所述行驶装置前进方向为y轴方向，以垂直于地面方向为z轴方向以及以垂直于y轴和z轴所在平面且满足右手定则的方向为x轴方向；
6.对所述至少一侧路沿对应的路沿拟合曲线进行锚点采样；基于采样得到的多个锚点构建观测向量，所述观测向量至少包括多个锚点对应的x轴坐标；
7.将所述观测向量和所述主轨迹进行匹配，获得所述观测向量与所述主轨迹的第一匹配状态；
8.当所述第一匹配状态为小误差匹配或未匹配时，更新主轨迹状态；
9.当所述第一匹配状态为大误差匹配时，将所述观测向量和所述备用轨迹进行匹配并基于所述观测向量和所述备用轨迹的第二匹配状态更新主轨迹状态；
10.基于主轨迹状态更新时对应的更新模式更新跟踪状态量；
11.根据更新后的跟踪状态量对更新后的所述主轨迹状态进行确认；
12.基于所述主轨迹状态的确认结果输出路沿跟踪结果。
13.在一些实施例中，将所述观测向量和所述主轨迹进行匹配，获得所述观测向量与所述主轨迹的第一匹配状态包括：
14.基于所述观测向量和所述多个锚点在所述主轨迹中对应的x轴坐标获得第一匹配距离；
15.当所述第一匹配距离小于等于第一匹配距离阈值时，判定所述第一匹配状态为小
误差匹配；
16.当所述第一匹配距离大于所述第一匹配距离阈值且小于等于第二匹配距离阈值时，判定所述第一匹配状态为大误差匹配；
17.当所述第一匹配距离大于所述第二匹配距离阈值时，判定所述第一匹配状态为未匹配状态；其中所述第一匹配距离阈值小于所述第二匹配距离阈值；
18.所述将所述观测向量和所述备用轨迹进行匹配包括：
19.基于所述观测向量和所述多个锚点在所述备用轨迹中对应的x轴坐标获得第二匹配距离；
20.当所述第二匹配距离小于等于所述第一匹配距离阈值时，判定所述第二匹配状态为小误差匹配；
21.当所述第二匹配距离大于所述第一匹配距离阈值且小于等于所述第二匹配距离阈值时，判定所述第二匹配状态为大误差匹配；
22.当所述第二匹配距离大于所述第二匹配距离阈值时，判定所述第二匹配状态为未匹配状态。
23.在一些实施例中，利用以下表达式计算所述第一匹配距离和所述第二匹配距离：
24.其中，d为第一匹配距离或第二匹配距离，n为锚点数量，为主轨迹或备用轨迹中与第i个锚点对应的x轴坐标，为路沿拟合曲线中与第i个锚点对应的x轴坐标，δi为第i个锚点的匹配权重值。
25.在一些实施例中，基于所述主轨迹状态的确认结果输出路沿跟踪结果包括：
26.当所述主轨迹状态的确认结果为确认时，将进行卡尔曼滤波更新后的所述主轨迹作为目标轨迹，基于所述目标轨迹输出路沿跟踪结果；
27.当所述主轨迹状态的确认结果为删除时，将所述备用轨迹作为所述目标轨迹，基于所述目标轨迹输出路沿跟踪结果。
28.在一些实施例中，当所述第一匹配状态为小误差匹配或未匹配时，更新主轨迹状态，包括：
29.当所述第一匹配状态为小误差匹配时，以有量测模式更新主轨迹状态；当所述第一匹配状态为未匹配时，以无量测模式更新主轨迹状态；
30.基于所述观测向量和所述备用轨迹的第二匹配状态更新主轨迹状态包括：
31.当所述第二匹配状态为小误差匹配、大误差匹配或未匹配时，以无量测模式更新主轨迹状态。
32.在一些实施例中，所述跟踪状态量包括未关联次数计数器计数值和关联次数计数器计数值，所述基于主轨迹状态更新时对应的更新模式更新跟踪状态量，包括；
33.当主轨迹状态更新时对应的更新模式为所述有量测模式时，所述关联次数计数器计数值加一，所述未关联次数计数器计数值清零；
34.当主轨迹状态更新时对应的更新模式为所述无量测模式时，所述未关联次数计数器计数值加一。
35.在一些实施例中，所述根据更新后的跟踪状态量对更新后的所述主轨迹状态进行确认包括：
36.当主轨迹状态更新时对应的更新模式为所述有量测模式时，判断所述关联次数计数器计数值是否大于或等于关联次数阈值；如果是，则所述主轨迹状态的确认结果为确认；
37.当主轨迹状态更新时对应的更新模式为所述无量测模式时，判断所述未关联次数计数器计数值是否小于或等于未关联次数阈值；如果否，则所述主轨迹状态的确认结果为删除，所述关联次数计数器计数值清零。
38.在第二方面，本发明提供了一种路沿跟踪装置，其包括：
39.曲线获取模块，其用于获取至少一侧路沿对应的路沿拟合曲线和路沿历史轨迹，其中所述路沿历史轨迹包括主轨迹和备用轨迹，所述路沿拟合曲线、所述主轨迹以及所述备用轨迹基于以行驶装置为原点的三维坐标系构建，所述三维坐标系以所述行驶装置前进方向为y轴方向，以垂直于地面方向为z轴方向以及以垂直于y轴和z轴所在平面且满足右手定则的方向为x轴方向；
40.锚点采样模块，其用于对所述至少一侧路沿的路沿拟合曲线进行锚点采样；基于采样得到的多个锚点构建观测向量，所述观测向量至少包括多个锚点对应的x轴坐标；
41.距离匹配模块，其用于将所述观测向量和所述主轨迹进行匹配，获得所述观测向量与所述主轨迹的第一匹配状态；
42.轨迹状态更新模块，其用于当所述第一匹配状态为小误差匹配或未匹配时，更新主轨迹状态；
43.当所述第一匹配状态为大误差匹配时，将所述观测向量和所述备用轨迹进行匹配并基于所述观测向量和所述备用轨迹的第二匹配状态更新主轨迹状态；
44.跟踪状态量更新模块，其用于基于主轨迹状态更新时对应的更新模式更新跟踪状态量；
45.轨迹状态确认模块，其用于根据更新后的跟踪状态量对更新后的所述主轨迹状态进行确认；
46.跟踪输出模块，其用于基于所述主轨迹状态的确认结果输出路沿跟踪结果。
47.在第三方面，本发明提供了一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机程序，所述计算机程序被处理器执行时实现上述任意一项所述的路沿跟踪方法。
48.在第四方面，本发明提供了一种驾驶装置，其包括存储器和处理器，所述存储器中存储有计算机程序，所述计算机程序被所述处理器执行时实现上述任意一项所述的路沿跟踪方法。
49.在采用上述技术方案的情况下，本发明通过基于从路沿对应的路沿拟合曲线中采样得到的锚点构建观测向量，并将该观测向量与路沿历史轨迹进行匹配，根据匹配结果对应的不同匹配状态进行路沿跟踪；其中路沿历史轨迹包括主轨迹和备用轨迹，在匹配时首先基于观测向量和主轨迹进行匹配，当匹配后得到的第一匹配状态为大误差匹配时再基于观测向量和备用轨迹进行匹配并得到第二匹配状态，进而可基于第一匹配状态或第二匹配状态对主轨迹状态进行更新，并基于更新主轨迹状态时对应的更新模式更新跟踪状态量，根据跟踪状态量对更新后的主轨迹状态进行确认，以根据主轨迹状态的确认结果确定基于
主轨迹或利用备用轨迹替换主轨迹输出路沿跟踪结果，从而达到能够有效解决检测失效以及量测跳变问题，提高最终输出路沿跟踪结果的稳定性与可靠性的有益效果。
附图说明
50.下面结合附图来描述本发明的优选实施方式，附图中：
51.图1是本发明实施例提供的一种路沿跟踪方法的流程示意图；
52.图2是本发明实施例提供的对左右路沿进行路沿跟踪方法的流程示意图；
53.图3是本发明具体示例提供的一种路沿跟踪方法的示意图；
54.图4是本发明实施例提供的一种路沿跟踪装置的结构示意图；
55.图5是本发明实施例提供的驾驶装置的结构示意图。
具体实施方式
56.下面参照附图来描述本发明的一些实施方式。本领域技术人员应当理解的是，这些实施方式仅仅用于解释本发明的技术原理，并非旨在限制本发明的保护范围。
57.路沿检测是在城市地区行驶的自动驾驶车辆需要具备的一项重要感知功能，通过进行路沿检测可为车辆提供道路边界信息，约束车辆行驶区域。激光雷达作为自动驾驶重要的传感器，基于其检测路沿也是一种常用的功能。由于道路的复杂性与多变性，基于激光雷达进行检测时，点云稀疏性与不稳定性以及激光雷达量测噪音等原因，会影响单帧路沿检测的准确性与鲁棒性。通过应用跟踪算法可以提高路沿检测的稳定性以及抗干扰能力，然而，目前在路沿跟踪方面的研究较少，且基本以单纯应用卡尔曼滤波器进行路沿跟踪为主，无法有效解决检测失效以及量测跳变等问题。
58.有鉴于此，本发明提供了一种路沿跟踪方法，通过基于从路沿对应的路沿拟合曲线中采样得到的锚点构建观测向量，并将该观测向量与路沿历史轨迹进行匹配，根据匹配结果对应的不同匹配状态进行路沿跟踪；其中路沿历史轨迹包括主轨迹和备用轨迹，在匹配时首先基于观测向量和主轨迹进行匹配，当匹配后得到的第一匹配状态为大误差匹配时再基于观测向量和备用轨迹进行匹配并得到第二匹配状态，进而可基于第一匹配状态或第二匹配状态对主轨迹状态进行更新，并基于更新主轨迹状态时对应的更新模式更新跟踪状态量，根据跟踪状态量对更新后的主轨迹状态进行确认，以根据主轨迹状态的确认结果确定基于主轨迹或利用备用轨迹替换主轨迹输出路沿跟踪结果，从而达到能够有效解决检测失效以及量测跳变问题，提高最终输出路沿跟踪结果的稳定性与可靠性的有益效果。
59.参见图1所示，图1是本发明实施例提供的一种路沿跟踪方法的流程示意图，其可以包括：
60.步骤s11：获取至少一侧路沿对应的路沿拟合曲线和路沿历史轨迹，其中路沿历史轨迹包括主轨迹和备用轨迹，路沿拟合曲线、主轨迹以及备用轨迹基于以行驶装置为原点的三维坐标系构建，三维坐标系以行驶装置前进方向为y轴方向，以垂直于地面方向为z轴方向以及以垂直于y轴和z轴所在平面且满足右手定则的方向为x轴方向；
61.步骤s12：对至少一侧路沿对应的路沿拟合曲线进行锚点采样；基于采样得到的多个锚点构建观测向量，观测向量至少包括多个锚点对应的x轴坐标；
62.步骤s13：将观测向量和主轨迹进行匹配，获得观测向量与主轨迹的第一匹配状
态；
63.步骤s14：当第一匹配状态为小误差匹配或未匹配时，更新主轨迹状态；
64.步骤s15：当第一匹配状态为大误差匹配时，将观测向量和备用轨迹进行匹配并基于观测向量和备用轨迹的第二匹配状态更新主轨迹状态；
65.步骤s16：基于主轨迹状态更新时对应的更新模式更新跟踪状态量；
66.步骤s17：根据更新后的跟踪状态量对更新后的主轨迹状态进行确认；
67.步骤s18：基于主轨迹状态的确认结果输出路沿跟踪结果。
68.在一些实施例中，步骤s11可以具体为采用三维激光雷达对路沿进行检测，得到至少一侧路沿的路沿拟合曲线。路沿历史轨迹可以为至少一侧路沿在预设时间段内的位置序列。
69.其中，在基于一侧路沿对应的路沿拟合曲线和路沿历史轨迹进行路沿跟踪的初始时刻k＝0，还可以基于初始时刻路沿拟合曲线中对应的量测值对主轨迹和备用轨迹中的目标位置进行初始化，对主轨迹和备用轨迹的初始化过程不同。
70.在一些实施例中，步骤s12可以具体为针对每一侧路沿，分别根据路沿拟合曲线确定锚点个数，当路沿拟合曲线为n次曲线时，对应在路沿拟合曲线上选择n 1个锚点，并基于n 1个锚点构建观测向量。
71.在一些实施例中，基于n 1个锚点构建观测向量可以具体为在路沿拟合曲线上选择n 1个不同的y坐标，根据路沿拟合曲线确定每个y坐标对应的x坐标，并基于n 1个锚点对应的x轴坐标构建观测向量。其中，为了提高路沿跟踪输出的平滑性，可以为选择均匀分布的n 1个不同的y坐标，并在此基础上确定x坐标以及构建观测向量。
72.作为具体示例，当一侧的路沿拟合曲线为二次曲线时，可以对该侧的路沿拟合曲线进行锚点采样，得到3个锚点，基于3个锚点对应的x轴坐标构建观测向量z(k)＝(χ0,χ1,χ2)
t
，其中z(k)代表k时刻的观测向量。
73.在一些实施例中，步骤s13可以具体为：
74.基于观测向量和多个锚点在主轨迹中对应的x轴坐标获得第一匹配距离；
75.当第一匹配距离小于等于第一匹配距离阈值时，判定第一匹配状态为小误差匹配；
76.当第一匹配距离大于第一匹配距离阈值且小于等于第二匹配距离阈值时，判定第一匹配状态为大误差匹配；
77.当第一匹配距离大于第二匹配距离阈值时，判定第一匹配状态为未匹配状态；其中第一匹配距离阈值小于第二匹配距离阈值。
78.在一些实施例中，利用以下表达式1-1计算第一匹配距离：
[0079][0080]
其中，d为第一匹配距离，n为锚点数量，为主轨迹与第i个锚点对应的x轴坐标，为路沿拟合曲线中与第i个锚点对应的x轴坐标，δi为第i个锚点的匹配权重值。
[0081]
在一些实施例中，步骤s14可以具体为：
[0082]
当第一匹配状态为小误差匹配时，以有量测模式更新主轨迹状态；当第一匹配状态为未匹配时，以无量测模式更新主轨迹状态并利用量测值对备用轨迹进行初始化。
[0083]
在一些实施例中，为了提高路沿轨迹的平滑性，可以针对不同第一匹配状态利用卡尔曼滤波对主轨迹进行跟踪滤波，基于卡尔曼滤波以有量测模式更新主轨迹状态时，利用前一时刻得到的卡尔曼滤波估计向量x(k-1)预测当前时刻的预测状态向量并基于当前时刻对应的观测向量z(k)和当前时刻的预测状态向量进行融合得到当前时刻的估计向量x(k)，输出估计向量x(k)；基于卡尔曼滤波以无量测模式更新主轨迹状态时，利用前一时刻得到的卡尔曼滤波估计向量x(k-1)预测当前时刻的预测状态向量将不进行融合过程，以预测出的作为输出。
[0084]
其中，以路沿拟合曲线为二次曲线，采集到3个锚点，基于3个锚点构建的观测向量为z(k)＝(χ0,χ1,χ2)
t
为例，利用卡尔曼滤波进行跟踪滤波，主轨迹的状态向量可以表示为v1、v2和v3分别代表三个锚点x轴方向上的速度，假设测量噪声为高斯白噪声，量测模型可以表示为表达式2-1：
[0085]
z(k)＝x(k) v(k)(2-1)，其中，v(k)代表测量噪声；
[0086]
利用表达式2-2预测状态以及利用表达式2-3更新误差协方差矩阵：
[0087][0088][0089]
其中，代表预测出的下一时刻k 1的预测状态向量，χ(k)为当前k时刻的状态向量，为下一时刻的预测误差协方差，p(k)为当前时刻的误差协方差，ω(k)为过程噪声，a(k)为状态转移矩阵，b(k)为控制输入矩阵，u(k)为系统控制量，cq代表过程激励噪声协方差；
[0090]
利用表达式2-4计算卡尔曼滤波增益：
[0091][0092]
其中，gk(k 1)代表k 1时刻更新的卡尔曼滤波增益，cr代表测量噪声协方差；
[0093]
利用表达式2-5更新状态以及利用表达式2-6更新误差协方差矩阵：
[0094][0095][0096]
其中，x(k 1)为下一时刻k 1的估计向量，即卡尔曼滤波跟踪最终输出的锚点x坐标向量；p(k 1)为下一时刻k 1更新的误差协方差。
[0097]
在一些实施例中，步骤s15中将观测向量和备用轨迹进行匹配可以具体为：
[0098]
基于观测向量和多个锚点在备用轨迹中对应的x轴坐标获得第二匹配距离；
[0099]
当第二匹配距离小于等于第一匹配距离阈值时，判定第二匹配状态为小误差匹配；
[0100]
当第二匹配距离大于第一匹配距离阈值且小于等于第二匹配距离阈值时，判定第二匹配状态为大误差匹配；
[0101]
当第二匹配距离大于第二匹配距离阈值时，判定第二匹配状态为未匹配状态。
[0102]
其中，可以利用表达式1-1计算第二匹配距离，d替换为第二匹配距离，替换为备
用轨迹中与第i个锚点对应的x轴坐标。
[0103]
在一些实施例中，步骤s15中基于观测向量和备用轨迹的第二匹配状态更新主轨迹状态包括：
[0104]
当第二匹配状态为小误差匹配、大误差匹配或未匹配时，以无量测模式更新主轨迹状态。
[0105]
在一些实施例中，当第二匹配状态为大误差匹配或未匹配时，以量测值初始化备用轨迹。
[0106]
在一些实施例中，跟踪状态量可以包括未关联次数计数器计数值和关联系数计数器计数值，步骤s16可以具体为：
[0107]
当主轨迹状态更新时对应的更新模式为有量测模式时，关联次数计数器计数值加一，未关联次数计数器计数值清零；
[0108]
当主轨迹状态更新时对应的更新模式为无量测模式时，未关联次数计数器计数值加一。
[0109]
在一些实施例中，步骤s17可以具体为：
[0110]
当主轨迹状态更新时对应的更新模式为有量测模式时，判断关联次数计数器计数值是否大于或等于关联次数阈值；如果是，则主轨迹状态的确认结果为确认；如果否，则主轨迹状态的确认结果为不确认。
[0111]
当主轨迹状态更新时对应的更新模式为无量测模式时，判断未关联次数计数器计数值是否小于或等于未关联次数阈值；如果否，则主轨迹状态的确认结果为删除，关联次数计数器计数值清零；如果是，则主轨迹状态的确认结果为延续上一时刻主轨迹状态，即以上一时刻的卡尔曼滤波估计向量作为输出。
[0112]
在一些实施例中，步骤s18可以具体为：
[0113]
当主轨迹状态的确认结果为确认时，将进行卡尔曼滤波更新后的主轨迹作为目标轨迹，基于目标轨迹输出路沿跟踪结果；
[0114]
当主轨迹状态的确认结果为删除时，将备用轨迹作为目标轨迹，基于目标轨迹输出路沿跟踪结果。在另一些实施例中，还可以在第二匹配状态为小误差匹配时，对备用轨迹进行卡尔曼滤波更新，进而可在主轨迹状态的确认结果为删除时，将进行卡尔曼滤波更新后的备用轨迹作为目标轨迹。
[0115]
以上为本发明实施例提供的一种路沿跟踪方法，通过基于从路沿对应的路沿拟合曲线中采样得到的锚点构建观测向量，并将该观测向量与路沿历史轨迹进行匹配，根据匹配结果对应的不同匹配状态进行路沿跟踪；其中路沿历史轨迹包括主轨迹和备用轨迹，在匹配时首先基于观测向量和主轨迹进行匹配，当匹配后得到的第一匹配状态为大误差匹配时再基于观测向量和备用轨迹进行匹配并得到第二匹配状态，进而可基于第一匹配状态或第二匹配状态对主轨迹状态进行更新，并基于更新主轨迹状态时对应的更新模式更新跟踪状态量，根据跟踪状态量对更新后的主轨迹状态进行确认，以根据主轨迹状态的确认结果确定基于主轨迹或利用备用轨迹替换主轨迹输出路沿跟踪结果，从而达到能够有效解决检测失效以及量测跳变问题，提高最终输出路沿跟踪结果的稳定性与可靠性的有益效果。
[0116]
在另一些实施例中，可以同时获取路沿两侧的路沿拟合曲线，并针对每一次的路沿拟合曲线分别进行路沿跟踪，具体请参见下文中的描述。
[0117]
参见图2所示，图2是本发明实施例提供的对左右路沿进行路沿跟踪方法的流程示意图，其可以包括：
[0118]
步骤s21：获取左右路沿检测结果；
[0119]
针对左路沿和右路沿分别执行步骤s22-s24：
[0120]
步骤s22：路沿量测值与历史轨迹进行匹配，获得两者匹配状态；
[0121]
步骤s23：根据不同匹配状态对轨迹进行更新；
[0122]
步骤s24：确认轨迹状态；
[0123]
步骤s25：输出左右路沿跟踪结果。
[0124]
在本发明实施例中，步骤s21可以具体为分别获取左右两侧路沿对应的路沿拟合曲线和路沿历史轨迹，其中路沿历史轨迹包括主轨迹和备用轨迹，针对每一次路沿的路沿拟合曲线和路沿历史轨迹的获取可以采用和上述实施例中步骤s11相同的方式实现，具体可参见上文中的描述。
[0125]
在一些实施例中，可以对左侧路沿或右侧路沿的路沿拟合曲线进行锚点采样，基于多个锚点对应的x轴坐标构建观测向量，其中每个锚点对应的x轴坐标即为路沿量测值。其中，历史轨迹即路沿历史轨迹，包括主轨迹和备用轨迹，步骤s22可以具体为先将观测向量和主轨迹进行匹配，得到观测向量与主轨迹的第一匹配状态；当第一匹配状态为大误差匹配时，再将观测向量和备用轨迹进行匹配，得到观测向量与备用轨迹的第二匹配状态。其中，可采用和步骤s13相同的方式获取第一匹配状态，可采用和步骤s15相同的方式获取第二匹配状态。
[0126]
在一些实施例中，步骤s23可以具体为当第一匹配状态为小误差匹配或未匹配时，更新主轨迹状态；当第一匹配状态为大误差匹配，基于第二匹配状态更新主轨迹状态；基于主轨迹状态更新时对应的更新模式更新跟踪状态量。其中，更新主轨迹状态以及跟踪状态量可以采用和步骤s14-s16相同的方式，具体可参见上文中的描述。
[0127]
在一些实施例中，步骤s24可以具体为根据更新后的跟踪状态量对更新后的主轨迹状态进行确认，具体可参见步骤s17的实现方式。
[0128]
在一些实施例中，步骤s25可以具体为针对左侧路沿或右侧路沿，分别基于相应侧路沿的主轨迹状态的确认结果输出路沿跟踪结果，当主轨迹状态的确认结果为确认时，将进行卡尔曼滤波更新后的主轨迹作为目标轨迹，基于目标轨迹输出路沿跟踪结果；当主轨迹状态的确认结果为删除时，将备用轨迹作为目标轨迹，基于目标轨迹输出路沿跟踪结果，从而得到左右路沿跟踪结果。
[0129]
参见图3所示，图3是本发明具体示例提供的一种路沿跟踪方法的示意图。
[0130]
该路沿跟踪方法可基于本发明上述实施例示出的路沿跟踪方法实现，将路沿历史轨迹和对路沿量测得到的路沿拟合曲线进行匹配，其中路沿历史轨迹包括主轨迹和备用轨迹，首先进行主轨迹匹配，当主轨迹匹配的匹配状态为小误差匹配时，以有量测模式进行主轨迹滤波器更新；当主轨迹匹配的匹配状态为未匹配时，以无量测模式进行主轨迹滤波器更新并初始化备用轨迹；当主轨迹匹配的匹配状态为大误差匹配时再进行备用轨迹匹配，当备用轨迹匹配的匹配状态为小误差匹配时，进行备用轨迹滤波器更新，当备用轨迹匹配的匹配状态为大误差匹配或未匹配时，初始化备用轨迹，同时根据备用轨迹匹配的相应匹配状态以无量测模式进行主轨迹滤波器更新。进而基于主轨迹滤波器更新对应的更新模式
进行跟踪状态更新，并基于跟踪状态更新后的跟踪状态量进行轨迹确认。当主轨迹的确认结果为确认时，将进行滤波器更新后的主轨迹作为目标轨迹，基于目标轨迹输出路沿跟踪结果；当主轨迹的确认结果为删除时，将进行滤波器更新后的备用轨迹作为目标轨迹，基于目标轨迹输出路沿跟踪结果。
[0131]
参见图4所示，图4是本发明实施例提供的一种路沿跟踪装置的结构示意图，其包括：
[0132]
曲线获取模块41，其用于获取至少一侧路沿对应的路沿拟合曲线和路沿历史轨迹，其中路沿历史轨迹包括主轨迹和备用轨迹，路沿拟合曲线、主轨迹以及备用轨迹基于以行驶装置为原点的三维坐标系构建，三维坐标系以行驶装置前进方向为y轴方向，以垂直于地面方向为z轴方向以及以垂直于y轴和z轴所在平面且满足右手定则的方向为x轴方向；
[0133]
锚点采样模块42，其用于对至少一侧路沿对应的路沿拟合曲线进行锚点采样；基于采样得到的多个锚点构建观测向量，观测向量至少包括多个锚点对应的x轴坐标；
[0134]
距离匹配模块43，其用于将观测向量和主轨迹进行匹配，获得观测向量与主轨迹的第一匹配状态；
[0135]
轨迹状态更新模块44，其用于当第一匹配状态为小误差匹配或未匹配时，更新主轨迹状态；
[0136]
当第一匹配状态为大误差匹配时，将观测向量和备用轨迹进行匹配并基于观测向量和备用轨迹的第二匹配状态更新主轨迹状态；
[0137]
跟踪状态量更新模块45，其用于基于主轨迹状态更新时对应的更新模式更新跟踪状态量；
[0138]
轨迹状态确认模块46，其用于根据更新后的跟踪状态量对更新后的主轨迹状态进行确认；
[0139]
跟踪输出模块47，其用于基于主轨迹状态的确认结果输出路沿跟踪结果。
[0140]
本发明提供的路沿跟踪装置可用于执行上述路沿跟踪方法，达到与上述实施例中路沿跟踪方法相同的有益效果。进一步，应该理解的是，由于各个模块的设定仅仅是为了说明本发明的装置的功能单元，这些模块对应的物理器件可以是处理器本身，或者处理器中软件的一部分，硬件的一部分，或者软件和硬件结合的一部分。因此，图4中的各个模块的数量仅仅是示意性的。本领域技术人员能够理解的是，可以对装置中的各个模块进行适应性地拆分或合并。对具体模块的这种拆分或合并并不会导致技术方案偏离本发明的原理，因此，拆分或合并之后的技术方案都将落入本发明的保护范围内。
[0141]
本领域技术人员能够理解的是，本发明实现上述实施例的方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读存储介质可以包括：能够携带所述计算机程序代码的任何实体或装置、介质、u盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器、随机存取存储器、电载波信号、电信信号以及软件分发介质等。
[0142]
本发明的另一方面还提供了一种计算机可读存储介质，该计算机可读存储介质中存储有计算机程序，计算机程序被处理器执行时能够实现上述任一实施例中的路沿跟踪方
法。该计算机可读存储介质可以是包括各种电子设备形成的存储装置设备，可选的，本发明实施例中计算机可读存储介质是非暂时性的计算机可读存储介质。
[0143]
本发明的另一方面，还提供了一种驾驶装置，参见图5所示，图5是本发明实施例提供的驾驶装置的结构示意图，其可以包括：
[0144]
存储器51和处理器52，存储器51中存储有计算机程序，计算机程序被处理器52执行时实现上述实施例中所述的路沿跟踪方法。
[0145]
该驾驶装置可以实现上述实施例中的路沿跟踪方法，达到与上述实施例相同的有益效果。
[0146]
至此，已经结合附图所示的优选实施方式描述了本发明的技术方案，但是，本领域技术人员容易理解的是，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征作出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。