CN113911123A - 一种道路模型的更新方法及装置 - Google Patents

Abstract

本发明公开了一种道路模型的更新方法及装置，所述方法在对道路模型进行更新时，根据位移量、方位角变化量进行第一阶段的更新;根据横向偏移量和预设的偏移量观测噪声对第一阶段更新所得到模型参数进行了第二阶段的更新;根据观测车道线上各采样点处的道路斜率和噪声对第二阶段更新所得到的模型参数进行第三阶段的更新;根据各目标车辆的航迹信息所对应的道路斜率和噪声对第三阶段更新所得的模型参数进行第四阶段的更新，得到当前时刻道路模型的最终参数完成模型的更新。通过实施本发明实施例能够提高道路模型更新的准确性。

Description

一种道路模型的更新方法及装置

技术领域

本发明涉及自动驾驶技术领域，尤其涉及一种道路模型的更新方法及装置。

背景技术

具有自动驾驶功能的车辆在自动驾驶时，需要通过传感器对车辆所在位置的周围道路环境进行感知并重构，进而拟合得到道路模型。现有技术基于车辆视觉传感器所检测车道线特征信息（车道线位置、宽度）来构建道路模型并对道路模型进行实时更新，但若车道线被车辆遮挡或污损时车道线检测结果误差较大，进而使得道路模型的参数更新不准确。

发明内容

本发明实施例提供一种道路模型的更新方法及装置，能有提高道路模型更新的准确性。

本发明一实施例提供了一种道路模型的更新方法，包括：获取当前时刻的本车左右两侧车道线的车道线参数、各目标车辆的航迹信息、本车车速以及本车转向角速度；

根据本车左右两侧车道线的车道线参数计算本车相对车道中心的横向偏移量，根据本车转向角速度从左侧车道线和右侧车道线中选取一车道线作为观测车道线；

若当前时刻不为初始时刻，则根据本车车速以及本车转角速度计算上一时刻至当前时刻的位移量和方位角变化量；

根据所述位移量、方位角变化量对上一时刻道路模型的模型参数进行时间更新，获得第一更新模型参数；

根据所述横向偏移量以及预设的偏移量观测噪声对第一更新模型参数进行再次更新，获得第二更新模型参数；

对所述观测车道线进行采样并计算各采样点处的道路斜率和噪声，继而根据各采样点处的道路斜率以及噪声对第二更新模型参数进行再次更新，获得第三更新模型参数；

根据各目标车辆的航迹信息计算各航迹信息所对应的道路斜率和噪声，继而根据各航迹信息所对应的道路斜率和噪声对第三更新模型参数进行再次更新，获得第四更新模型参数，完成当前时刻所述道路模型的更新。

进一步的，还包括：若当前时刻为初始时刻，则根据所述横向偏移量以及所述观测车道线的车道线参数，生成初始化道路模型参数，根据所述初始化道路模型参数构建初始化道路模型。

进一步的，所述根据本车左右两侧车道线的车道线参数计算本车相对车道中心的横向偏移量，具体包括：

计算左侧车道线的车道线参数中三次多项式方程的常数项系数，与右侧车道线的车道线参数中三次多项式方程的常数项系数之间的差值，获得上述横向偏移量。

进一步的，所述根据本车转向角速度从左侧车道线和右侧车道线中选取一车道线作为观测车道线，具体包括：

若所述本车转向角速度大于预设第一阈值，则选取左侧车道线作为观测车道线；

若所述本车转角速度小于预设第二阈值，则选取右侧车道线作为观测车道线；

若所述本车转角速度在预设第一阈值与预设第二阈值之间，则从左侧车道线和右侧车道线中选取有效长度较大的一车道线，作为观测车道线。

进一步的，所述位移量包括横向位移量以及纵向位移量；所述根据本车车速以及本车转角速度计算上一时刻至当前时刻的位移量和方位角变化量，具体包括：

通过以下公式计算方位角变化量：

；

若本车转角速度的绝对值大于预设第三阈值，则通过以下公式计算横向位移量以及纵向位移量：

；

若本车转角速度的绝对值小于或等于预设第三阈值，则通过以下公式计算横向位移量以及纵向位移量：

；

其中，

为方位角变化量；

为本车转角速度；△t为当前时刻与上一时刻的时间间隔；△x_k为纵向位移量；V _ego 为本车车速；△y_k为横向位移量。

进一步的，所述根据所述位移量、方位角变化量对上一时刻道路模型的模型参数进行时间更新，获得第一更新模型参数，具体包括：

通过以下公式对上一时刻道路模型的模型参数进行时间更新，得到第一更新模型参数：

；

其中，F_K为状态转移矩阵；θ_k|k-1为第一更新模型参数；θ_k-1|k-1为前一时刻道路模型的模型参数；G为预设控制传递矩阵；

；

；Q_k-1为状态噪声协方差。

进一步的，所述根据所述横向偏移量以及预设的偏移量观测噪声对第一更新模型参数进行再次更新，获得第二更新模型参数，具体包括：

通过以下公式对第一更新模型参数进行再次更新，获得第二更新模型参数：

；

其中，θ^y0 _k|k-1为第二更新模型参数；K_y0为卡尔曼增益，

；Z _Offset 为横向偏移量；H_y0为预设偏移量观测矩阵；P_k|k-1为与第一更新模型参数相对应的第一更新协方差矩阵，

；P_k-1|k-1为上一时刻道路模型所对应的协方差矩阵；U_k-1为预设控制噪声协方差，R_zOffset为预设的偏移量观测噪声。

进一步的，对所述观测车道线进行采样并计算各采样点处的道路斜率和噪声，继而根据各采样点处的道路斜率以及噪声对第二更新模型参数进行再次更新，获得第三更新模型参数，具体包括：

对所述观测车道线进行采样，获得第一采样点集；

从所述第一采样点集中提取有效长度小于预设长度阈值的采样点，获得有效采样点集；

计算有效采样点集中各采样点处的道路斜率、噪声以及对应的观测矩阵；

逐一根据各有效采样点集中各采样点处的道路斜率、噪声以及对应的观测矩阵，对第二更新模型参数进行迭代更新，获得第三更新模型参数。

进一步的，航迹信息包括：目标车辆在本车坐标系中的横向位置、纵向位置以及纵向速度；

所述根据各目标车辆的航迹信息计算各航迹信息所对应的道路斜率和噪声，继而根据各航迹信息所对应的道路斜率和噪声对第三更新模型参数进行再次更新，获得第四更新模型参数，具体包括：

将横向位置、纵向位置以及纵向速度均满足预设数值条件的航迹信息，作为有效航迹信息；

根据各有效航迹信息的横向位置、纵向位置以及纵向速度计算各有效航迹信息所对应的道路斜率、噪声以及观测矩阵；

逐一根据各有效航迹信息所对应的道路斜率、噪声以及对应的观测矩阵，对第三更新模型参数进行迭代更新，获得第四更新模型参数。

在上述方法项实施例的基础上，本发明对应提供了装置项实施例；

本发明一实施例提供了一种道路模型的更新装置，包括：数据获取模块、第一计算模块、第二计算模块、第一参数更新模块、第二参数更新模块、第三参数更新模块以及第四参数更新模块；

所述数据获取模块，用于获取当前时刻的本车左右两侧车道线的车道线参数、各目标车辆的航迹信息、本车车速以及本车转向角速度；

所述第一计算模块，用于根据本车左右两侧车道线的车道线参数计算本车相对车道中心的横向偏移量，根据本车转向角速度从左侧车道线和右侧车道线中选取一车道线作为观测车道线；

所述第二计算模块，用于在当前时刻不为初始时刻时，根据本车车速以及本车转角速度计算上一时刻至当前时刻的位移量和方位角变化量；

所述第一参数更新模块，用于根据所述位移量、方位角变化量对上一时刻道路模型的模型参数进行时间更新，获得第一更新模型参数；

所述第二参数更新模块，用于根据所述横向偏移量以及预设的偏移量观测噪声对第一更新模型参数进行再次更新，获得第二更新模型参数；

所述第三参数更新模块，用于对所述观测车道线进行采样并计算各采样点处的道路斜率和噪声，继而根据各采样点处的道路斜率以及噪声对第二更新模型参数进行再次更新，获得第三更新模型参数；

所述第四参数更新模块，用于根据各目标车辆的航迹信息计算各航迹信息所对应的道路斜率和噪声，继而根据各航迹信息所对应的道路斜率和噪声对第三更新模型参数进行再次更新，获得第四更新模型参数，完成当前时刻所述道路模型的更新。

通过实施本发明实施例具有如下有益效果：

本发明实施例提供了一种道路模型的更新方法及装置，所述方法在对道路模型进行更新时，根据位移量、方位角变化量进行第一阶段的更新，然后根据横向偏移量和预设的偏移量观测噪声对第一阶段更新所得到模型参数进行了第二阶段的更新，紧接着根据观测车道线上各采样点处的道路斜率和噪声对第二阶段更新所得到的模型参数进行第三阶段的更新，最后根据各目标车辆的航迹信息所对应的道路斜率和噪声对第三阶段更新所得的模型参数进行第四阶段的更新，得到当前时刻道路模型的最终参数，完成模型的更新。与现有技术相比，本发明在道路模型的更新过程中，加入了本车行驶区域中其他目标车辆的航迹信息进行更新，解决现有模型更新时单独依靠车道线的特征，在车道被遮挡时出现模型参数更新不准确的问题。此外，由于车辆视觉传感器的检测距离有限，仅能实现对近距离的车道线的特征进行提取无法感知远距离的车道线信息，因此仅基于车道线特征信息进行更新的道路模型在拟合远距离的道路形状时，与真实的道路形状存在较大偏差，而由于其他目标车辆的运动受到前方道路形状的约束，目标车辆的航迹信息能够包含的远距离道路形状信息，因此采用本发明的道路模型更新方法，能够引入远距离道路形状信息，进而使得更新后的道路模型在拟合远距离的道路形状时，能够降低与真实的道路形状存在偏差。

附图说明

图1是本发明一实施例提供的一种道路模型的更新方法的流程示意图。

图2是本发明一实施例提供的一种道路模型的更新装置的结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1所示，本发明一实施例提供了一种道路模型的更新方法，包括：

步骤S101: 获取当前时刻的本车左右两侧车道线的车道线参数、各目标车辆的航迹信息、本车车速以及本车转向角速度。

步骤S102: 根据本车左右两侧车道线的车道线参数计算本车相对车道中心的横向偏移量，根据本车转向角速度从左侧车道线和右侧车道线中选取一车道线作为观测车道线。

步骤S103: 若当前时刻不为初始时刻，则根据本车车速以及本车转角速度计算上一时刻至当前时刻的位移量和方位角变化量。

步骤S104: 根据所述位移量、方位角变化量对上一时刻道路模型的模型参数进行时间更新，获得第一更新模型参数。

步骤S105: 根据所述横向偏移量以及预设的偏移量观测噪声对第一更新模型参数进行再次更新，获得第二更新模型参数。

步骤S106: 对所述观测车道线进行采样并计算各采样点处的道路斜率和噪声，继而根据各采样点处的道路斜率以及噪声对第二更新模型参数进行再次更新，获得第三更新模型参数。

步骤S107: 根据各目标车辆的航迹信息计算各航迹信息所对应的道路斜率和噪声，继而根据各航迹信息所对应的道路斜率和噪声对第三更新模型参数进行再次更新，获得第四更新模型参数，完成当前时刻所述道路模型的更新。

对于步骤S101、具体的，获取当前时刻的各目标车辆的航迹信息，得到当前时刻的航迹量测集

；需要说明的是，上述各目标车辆为在预设距离范围内，位于本车前方的其他车辆；航迹量测集

中，N表示航迹信息的总条数，i表示第i条航迹信息；每一航迹信息X_track =[x,y,v_x,v_y]；X为目标车辆在本车坐标系中的纵向位置、y为目标车辆在本车坐标系中横向位置、v_x为目标车辆在本车坐标系中的纵向速度（绝对速度），v_y为目标车辆在本车坐标系中的横向速度（绝对速度）。获取当前时刻本车所在车道左右两侧的车道线的车道线参数、在本发明中车道线用本车坐标系下的三次多项式

的车道线方程进行表示，一车道线的车道线参数是其三次多项式方程的各项系数及有效长度r；即一车道线的车道线参数可以表示为：Z_LM =[a₀,a₁,a₂,a₃,r]；有效长度r是视觉传感器跟随车道线的三次多项式方程的各项系数一起给出的一个值，表示该组车道线的三次多项式的各项系数在多远距离内有效，一般受道路曲率，是否遮挡，车道线清晰程度等因素影响。获取本车速度V _ego 与本车转向角速度

。

对于步骤S102、在一个优选的实施例中，所述根据本车左右两侧车道线的车道线参数计算本车相对车道中心的横向偏移量，具体包括：

计算左侧车道线的车道线参数中三次多项式方程的常数项系数，与右侧车道线的车道线参数中三次多项式方程的常数项系数之间的差值，获得上述横向偏移量。

具体的，横向偏移量通过以下公式进行计算：Z _Offset =a_{0_LLM}-a_{0_RLM}；Z _Offset 为横向偏移量；a_{0_LLM}为左侧车道线的三次多项式方程的常数项系数；a_{0_RLM}为右侧车道线的三次多项式方程的常数项系数。

在一个优选的实施例中，所述根据本车转向角速度从左侧车道线和右侧车道线中选取一车道线作为观测车道线，具体包括：

若所述本车转向角速度大于预设第一阈值，则选取左侧车道线作为观测车道线；若所述本车转角速度小于预设第二阈值，则选取右侧车道线作为观测车道线；若所述本车转角速度在预设第一阈值与预设第二阈值之间，则从左侧车道线和右侧车道线中选取有效长度较大的一车道线，作为观测车道线。

示意性的，上述预设第一阈值设置为0.05rad/s；上述预设第二阈值设置为-0.05rad/s；如果

,则选取左侧车道线作为观测车道线，用于后续的道路模型更新或初始化，如果

，则选取右侧车道线作为观测车道线，用于后续的道路模型更新或初始化，如果

，则选取有效长度r较大的一侧车道线作为观测车道线。

在一个优选的实施例中，在执行完步骤S101和步骤S102后，紧接着判断当前时刻是否为初始时刻，若当前时刻为初始时刻，则根据所述横向偏移量以及所述观测车道线的车道线参数，生成初始化道路模型参数，根据所述初始化道路模型参数构建初始化道路模型。示意性的，道路模型建模为

，模型参数θ=[y₀,η,c₀,c₁]；假设所选定的观测车道线的车道线参数中除有效长度r和三次多项式方程的常数项系数外的其他参数为a₁,a₂,a₃；则此时，根据横向偏移量以及观测车道线的车道线参数对道路模型进行初始化后，道路模型的初始化参数可以为θ₀=[Z _Offset ,a₁,2a₂,6a₃]。优选的，同时对道路模型所对应的协方差矩阵进行初始化，设定初始化协方差矩阵P₀ =diag([10^-2,10^-3,10^-4,10^-5])；

如果当前时刻不为初始时刻，则根据步骤S103-步骤S107，对当前时刻的道路模型进行更新。

对于步骤S103、在一个优选的实施例中，所述位移量包括横向位移量以及纵向位移量；所述根据本车车速以及本车转角速度计算上一时刻至当前时刻的位移量和方位角变化量，具体包括：

通过以下公式计算方位角变化量：

；

若本车转角速度的绝对值大于预设第三阈值，则通过以下公式计算横向位移量以及纵向位移量：

；

若本车转角速度的绝对值小于或等于预设第三阈值，则通过以下公式计算横向位移量以及纵向位移量：

；

其中，

为方位角变化量；

为本车转角速度；△t为当前时刻与上一时刻的时间间隔；△x_k为纵向位移量；V _ego 为本车车速；△y_k为横向位移量。

示意性的，预设第三阈值可以为10^-3rad/s；即，如果

，则本车位移通过以下公式计算：

；

否则，则本车位移通过以下公式计算：

；

对于步骤S104、在一个优选的实施例中，所述根据所述位移量、方位角变化量对上一时刻道路模型的模型参数进行时间更新，获得第一更新模型参数，具体包括：

通过以下公式对上一时刻道路模型的模型参数进行时间更新，得到第一更新模型参数：

；

其中，F_K为状态转移矩阵；θ_k|k-1为第一更新模型参数；θ_k-1|k-1为前一时刻道路模型的模型参数；G为预设控制传递矩阵；

；

；Q_k-1为状态噪声协方差;

示意性的,G可以设定为diag([10^-4,10^-6,10^-8,10^-10]);即：

；

在一个优选的实施例中，还通过以下公式对道路模型的协方差矩阵进行更新，获得第一更新协方差矩阵：

；P_k|k-1为第一更新协方差矩阵，P_k-1|k-1为上一时刻道路模型所对应的协方差矩阵；U_k-1为预设控制噪声协方差；示意性的，U_k-1=diag([10^-3,10^-6])；

对于步骤S105、在一个优选的实施例中，所述根据所述横向偏移量以及预设的偏移量观测噪声对第一更新模型参数进行再次更新，获得第二更新模型参数，具体包括：

通过以下公式对第一更新模型参数进行再次更新，获得第二更新模型参数：

；

其中，θ^y0 _k|k-1为第二更新模型参数；K_y0为卡尔曼增益，

；Z _Offset 为横向偏移量；H_y0为预设偏移量观测矩阵；P_k|k-1为与第一更新模型参数相对应的第一更新协方差矩阵，

；P_k-1|k-1为上一时刻道路模型所对应的协方差矩阵；U_k-1为预设控制噪声协方差，R_zOffset为预设的偏移量观测噪声。

具体的，使用横向偏移量Z _Offset ，利用标准卡尔曼更新方法再次更新道路模型参数得到第二更新模型参数θ^y0 _k|k-1；优选的，可设定

；R_zOffset=10^-2；

在一个优选的实施例中，还通过以下公式对一次更新协方差矩阵进行再次更新，获得二次更新协方差矩阵：

；P^y0 _k|k-1为二次更新协方差矩阵；

为单位矩阵；

对于步骤S106、在一个优选的实施例中，对所述观测车道线进行采样并计算各采样点处的道路斜率和噪声，继而根据各采样点处的道路斜率以及噪声对第二更新模型参数进行再次更新，获得第三更新模型参数，具体包括：

对所述观测车道线进行采样，获得第一采样点集；

从所述第一采样点集中提取有效长度小于预设长度阈值的采样点，获得有效采样点集；

计算有效采样点集中各采样点处的道路斜率、噪声以及对应的观测矩阵；

逐一根据各有效采样点集中各采样点处的道路斜率、噪声以及对应的观测矩阵，对第二更新模型参数进行迭代更新，获得第三更新模型参数。

具体的，对选定的观测车道线进行采样，得到第一采样点集，然后从第一采样点集中提取距离小于有效长度r的点，获得有效采样点集：

；在

中，

为有效采样点集中采样点的总数，Xⁱ _sample表示有效采样点集中第i个采样点；

紧接着根据以下公式计算有效采样点集中各采样点处的道路斜率：

fⁱ _LM=a₁+2a₂Xⁱ _sample+3a₃Xⁱ _sample ²；

fⁱ _LM为有效采样点集中第i采样点处的道路斜率，Xⁱ _sample为本车坐标系下第i个采样点的纵向位置；

根据以下公式计算有效采样点集中各采样点处的噪声：

R ⁱ _sample= Xⁱ _sample ²/1000；R ⁱ _sample为有效采样点集中第i采样点处的噪声；

根据以下公式计算有效采样点集中各采样点处对应的观测矩阵：

；

其中，Hⁱ _LM为有效采样点集中第i采样点处对应的观测矩阵；

紧接着逐一根据得到的各采样点处的道路斜率、噪声和观测矩阵，对第二更新模型参数进行迭代更新，直至根据有效采样点集中所有采样点的道路斜率、噪声和观测矩阵更新完毕后，即可得到第三更新模型参数θ^LM_p _k|k-1；优选的，逐一根据各采样点处的道路斜率、噪声和观测矩阵，对第二更新协方差矩阵进行迭代更新，得到第三更新协方差矩阵P^{LM _p} _k|k-1。

需要说明的是，在单独根据某一采样点i的道路斜率、噪声和观测矩阵进行模型参数以及协方差矩阵更新时，其更新方式与步骤S105的更新方式类似；

具体的，如果采样点i为第一个采样点，则其模型参数以及协方差矩阵的更新公式为：

;

;

;

θ^LM_1 _k|k-1为根据第一个采样点的道路斜率、噪声和观测矩阵进行更新后的道路模型的参数；K_{LM_1}为第一个采样点所对应的卡尔曼增益；f¹ _LM为第一个采样点所对应的道路斜率；H¹ _LM为第一个采样点所对应的观测矩阵；P ^y0 _k|k-1为上述第二更新协方差矩阵；R¹ _sample第一个采样点所对应的噪声;P^LM_1 _k|k-1为根据第一个采样点的道路斜率、噪声和观测矩阵进行更新后的协方差矩阵。

如果采样点不为第一个采样点，则其模型参数以及协方差矩阵的更新公式为：

;

;

;

θ^LM_i _k|k-1为根据第i个采样点的道路斜率、噪声和观测矩阵进行更新后的道路模型的参数；θ^LM_i-1 _k|k-1为根据第i-1个采样点的道路斜率、噪声和观测矩阵进行更新后的道路模型的参数;K_{LM_i}为第i个采样点所对应的卡尔曼增益；P^LM_i-1 _k|k-1为根据第i-1个采样点的道路斜率、噪声和观测矩阵进行更新后协方差矩阵。

通过上述公式，在根据最后一个采样点P的道路斜率、噪声和观测矩阵进行更新后得到本发明上述第三更新模型参数θ^LM_p _k|k-1以及第三更新协方差矩阵P^LM_p _k|k-1。

对于步骤S107、在一个优选的实施例中，航迹信息包括：目标车辆在本车坐标系中的横向位置、纵向位置以及纵向速度；

所述根据各目标车辆的航迹信息计算各航迹信息所对应的道路斜率和噪声，继而根据各航迹信息所对应的道路斜率和噪声对第三更新模型参数进行再次更新，获得第四更新模型参数，具体包括：

将横向位置、纵向位置以及纵向速度均满足预设数值条件的航迹信息，作为有效航迹信息；

根据各有效航迹信息的横向位置、纵向位置以及纵向速度计算各有效航迹信息所对应的道路斜率、噪声以及观测矩阵；

逐一根据各有效航迹信息所对应的道路斜率、噪声以及对应的观测矩阵，对第三更新模型参数进行迭代更新，获得第四更新模型参数。

具体的，在所获取的各航迹信息中，提取满足以下预设数值条件的航迹信息，作为有效航迹信息，获得有效航迹集

；有效航迹集

中，M为航迹信息的总数量，

为第i个航迹信息；

紧接着，根据以下公式将每一航迹信息上一时刻的坐标值补偿掉本车运动转化到当前坐标系中：

；

公式中，X_k-1为一目标车辆上一时刻的纵向位置（在本发明中当前时刻为k,上一时刻为k-1）；y_k-1为一目标车辆上一时刻的横向位置；

紧接着，通过以下公式计算每一航迹信息所对应的道路斜率、噪声以及观测矩阵：

；

；

；

公式中，f_K为一航迹信息所对应的道路斜率；R_K为一航迹信息所对应的噪声，H_K为一航迹信息所对应的观测矩阵：v_{x_K}为当前时刻一航迹信息中的纵向速度；紧接着逐一根据得到的各航迹信息的道路斜率、噪声和观测矩阵，对第三更新模型参数进行迭代更新，直至根据有效航迹集中所有航迹信息的道路斜率、噪声和观测矩阵更新完毕后，即可得到第四更新模型参数θ^track_M _k|k-1；优选的，还可以逐一根据各航迹信息的道路斜率、噪声和观测矩阵，对第三更新协方差矩阵进行迭代更新，得到第四更新协方差矩阵P^track_M _k|k-1。

需要说明的是，在单独根据某一航迹信息i的道路斜率、噪声和观测矩阵进行模型参数以及协方差矩阵更新时，其更新方式与步骤S105的更新方式类似；

具体的，如果航迹信息i为第一个航迹信息，则其模型参数以及协方差矩阵的更新公式为：

;

;

;

θ^track_1 _k|k-1为根据第一个航迹信息的道路斜率、噪声和观测矩阵进行更新后的道路模型的参数；K_{track_1}为第一个航迹信息所对应的卡尔曼增益；f¹ _track为第一个航迹信息所对应的道路斜率；H¹ _{track_1}为第一个航迹信息所对应的观测矩阵；R¹ _k为第一个航迹信息所对应的噪声;P^track_1 _k|k-1为根据第一个航迹信息所对应的道路斜率、噪声和观测矩阵进行更新后的协方差矩阵。

如果航迹信息不为第一个航迹信息，则其模型参数以及协方差矩阵的更新公式为：

;

;

;

θ^track_i _k|k-1为根据第i个航迹信息所对应的道路斜率、噪声和观测矩阵进行更新后的道路模型的参数；θ^track_i-1 _k|k-1为根据第i-1个航迹信息所对应的道路斜率、噪声和观测矩阵进行更新后的道路模型的参数;K_{track_i}为第i个航迹信息所对应的卡尔曼增益；fⁱ _track为第i个航迹信息所对应的道路斜率；Hⁱ _track为第i个航迹信息所对应的观测矩阵；P^track_i-1 _k|k-1为根据第i-1个采样点的道路斜率、噪声和观测矩阵进行更新后协方差矩阵；Rⁱ _k为第i个航迹信息所对应的噪声；P^track_i _k|k-1为根据第i个航迹信息的道路斜率、噪声和观测矩阵进行更新后协方差矩阵。

通过上述公式，在根据最后一个航迹信息M所对应的道路斜率、噪声和观测矩阵进行更新后得到本发明上述第四更新模型参数θ^track_M _k|k-1及第四更新协方差矩阵P^track_M _k|k-1，最后将第四更新模型参数θ^track_M _k|k-1作为当前时刻道路模型的最终参数，完成当前时刻道路模型的更新。

通过本发明上述实施例所公开的道路模型的更新方法，在对当前时刻的道路模型进行更新时，引入了其他目标车辆的航迹信息，即使车道线被部分遮挡出现跳变的情况下，也能准确的进行模型参数的更新，此外远距离航迹信息中包含的远距离道路形状信息，因此引入航迹信息进行更新使得更新后的道路模型在进行较远距离的道路拟合时，所拟合出的道路跟接近实际道路形状。

在上述方法项实施例的基础上本发明对应提供了装置项实施例；

如图2所示，本发明一实施例提供了一种道路模型的更新装置，包括数据获取模块、第一计算模块、第二计算模块、第一参数更新模块、第二参数更新模块、第三参数更新模块以及第四参数更新模块；

所述数据获取模块，用于获取当前时刻的本车左右两侧车道线的车道线参数、各目标车辆的航迹信息、本车车速以及本车转向角速度；

所述第一计算模块，用于根据本车左右两侧车道线的车道线参数计算本车相对车道中心的横向偏移量，根据本车转向角速度从左侧车道线和右侧车道线中选取一车道线作为观测车道线；

所述第二计算模块，用于在当前时刻不为初始时刻时，根据本车车速以及本车转角速度计算上一时刻至当前时刻的位移量和方位角变化量；

所述第一参数更新模块，用于根据所述位移量、方位角变化量对上一时刻道路模型的模型参数进行时间更新，获得第一更新模型参数；

所述第二参数更新模块，用于根据所述横向偏移量以及预设的偏移量观测噪声对第一更新模型参数进行再次更新，获得第二更新模型参数；

所述第三参数更新模块，用于对所述观测车道线进行采样并计算各采样点处的道路斜率和噪声，继而根据各采样点处的道路斜率以及噪声对第二更新模型参数进行再次更新，获得第三更新模型参数；

所述第四参数更新模块，用于根据各目标车辆的航迹信息计算各航迹信息所对应的道路斜率和噪声，继而根据各航迹信息所对应的道路斜率和噪声对第三更新模型参数进行再次更新，获得第四更新模型参数，完成当前时刻所述道路模型的更新。

需说明的是，上述装置项实施例是与本发明方法项实施例相对应的，其可以实现本发明任意一实施例所述的道路模型的更新方法，此外以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。另外，本发明提供的装置实施例附图中，模块之间的连接关系表示它们之间具有通信连接，具体可以实现为一条或多条通信总线或信号线。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。

以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。

Claims (10)

1.一种道路模型的更新方法，其特征在于，包括：

获取当前时刻的本车左右两侧车道线的车道线参数、各目标车辆的航迹信息、本车车速以及本车转向角速度；

根据本车左右两侧车道线的车道线参数计算本车相对车道中心的横向偏移量，根据本车转向角速度从左侧车道线和右侧车道线中选取一车道线作为观测车道线；

若当前时刻不为初始时刻，则根据本车车速以及本车转角速度计算上一时刻至当前时刻的位移量和方位角变化量；

根据所述位移量、方位角变化量对上一时刻道路模型的模型参数进行时间更新，获得第一更新模型参数；

根据所述横向偏移量以及预设的偏移量观测噪声对第一更新模型参数进行再次更新，获得第二更新模型参数；

对所述观测车道线进行采样并计算各采样点处的道路斜率和噪声，继而根据各采样点处的道路斜率以及噪声对第二更新模型参数进行再次更新，获得第三更新模型参数；

根据各目标车辆的航迹信息计算各航迹信息所对应的道路斜率和噪声，继而根据各航迹信息所对应的道路斜率和噪声对第三更新模型参数进行再次更新，获得第四更新模型参数，完成当前时刻所述道路模型的更新。

2.如权利要求1所述的道路模型的更新方法，其特征在于，还包括：若当前时刻为初始时刻，则根据所述横向偏移量以及所述观测车道线的车道线参数，生成初始化道路模型参数，根据所述初始化道路模型参数构建初始化道路模型。

3.如权利要求1所述的道路模型的更新方法，其特征在于，所述根据本车左右两侧车道线的车道线参数计算本车相对车道中心的横向偏移量，具体包括：

计算左侧车道线的车道线参数中三次多项式方程的常数项系数，与右侧车道线的车道线参数中三次多项式方程的常数项系数之间的差值，获得上述横向偏移量。

4.如权利要求1所述的道路模型的更新方法，其特征在于，所述根据本车转向角速度从左侧车道线和右侧车道线中选取一车道线作为观测车道线，具体包括：

若所述本车转向角速度大于预设第一阈值，则选取左侧车道线作为观测车道线；

若所述本车转角速度小于预设第二阈值，则选取右侧车道线作为观测车道线；

若所述本车转角速度在预设第一阈值与预设第二阈值之间，则从左侧车道线和右侧车道线中选取有效长度较大的一车道线，作为观测车道线。

5.如权利要求1所述的道路模型的更新方法，其特征在于，所述位移量包括横向位移量以及纵向位移量；所述根据本车车速以及本车转角速度计算上一时刻至当前时刻的位移量和方位角变化量，具体包括：

通过以下公式计算方位角变化量：

；

若本车转角速度的绝对值大于预设第三阈值，则通过以下公式计算横向位移量以及纵向位移量：

；

若本车转角速度的绝对值小于或等于预设第三阈值，则通过以下公式计算横向位移量以及纵向位移量：

；

其中，

为方位角变化量；

为本车转角速度；△t为当前时刻与上一时刻的时间间隔；△x_k为纵向位移量；V _ego 为本车车速；△y_k为横向位移量。

6.如权利要求5所述的道路模型的更新方法，其特征在于，所述根据所述位移量、方位角变化量对上一时刻道路模型的模型参数进行时间更新，获得第一更新模型参数，具体包括：

通过以下公式对上一时刻道路模型的模型参数进行时间更新，得到第一更新模型参数：

；

其中，F_K为状态转移矩阵;θ_k|k-1为第一更新模型参数;θ_k-1|k-1为前一时刻道路模型的模型参数；G为预设控制传递矩阵；

；

；Q_k-1为状态噪声协方差。

7.如权利要求6所述的道路模型的更新方法，其特征在于，所述根据所述横向偏移量以及预设的偏移量观测噪声对第一更新模型参数进行再次更新，获得第二更新模型参数，具体包括：

通过以下公式对第一更新模型参数进行再次更新，获得第二更新模型参数：

；

其中，θ^y0 _k|k-1为第二更新模型参数；K_y0为卡尔曼增益，

；Z _Offset 为横向偏移量；H_y0为预设偏移量观测矩阵；P_k|k-1为与第一更新模型参数相对应的第一更新协方差矩阵，

；P_k-1|k-1为上一时刻道路模型所对应的协方差矩阵；U_k-1为预设控制噪声协方差，R_zOffset为预设的偏移量观测噪声。

8.如权利要求7所述的道路模型的更新方法，其特征在于，对所述观测车道线进行采样并计算各采样点处的道路斜率和噪声，继而根据各采样点处的道路斜率以及噪声对第二更新模型参数进行再次更新，获得第三更新模型参数，具体包括：

对所述观测车道线进行采样，获得第一采样点集；

从所述第一采样点集中提取有效长度小于预设长度阈值的采样点，获得有效采样点集；

计算有效采样点集中各采样点处的道路斜率、噪声以及对应的观测矩阵；

逐一根据各有效采样点集中各采样点处的道路斜率、噪声以及对应的观测矩阵，对第二更新模型参数进行迭代更新，获得第三更新模型参数。

9.如权利要求8所述的道路模型的更新方法，其特征在于，航迹信息包括：目标车辆在本车坐标系中的横向位置、纵向位置以及纵向速度；

所述根据各目标车辆的航迹信息计算各航迹信息所对应的道路斜率和噪声，继而根据各航迹信息所对应的道路斜率和噪声对第三更新模型参数进行再次更新，获得第四更新模型参数，具体包括：

将横向位置、纵向位置以及纵向速度均满足预设数值条件的航迹信息，作为有效航迹信息；

根据各有效航迹信息的横向位置、纵向位置以及纵向速度计算各有效航迹信息所对应的道路斜率、噪声以及观测矩阵；

逐一根据各有效航迹信息所对应的道路斜率、噪声以及对应的观测矩阵，对第三更新模型参数进行迭代更新，获得第四更新模型参数。

10.一种道路模型的更新装置，其特征在于，包括：数据获取模块、第一计算模块、第二计算模块、第一参数更新模块、第二参数更新模块、第三参数更新模块以及第四参数更新模块；

所述数据获取模块，用于获取当前时刻的本车左右两侧车道线的车道线参数、各目标车辆的航迹信息、本车车速以及本车转向角速度；

所述第一计算模块，用于根据本车左右两侧车道线的车道线参数计算本车相对车道中心的横向偏移量，根据本车转向角速度从左侧车道线和右侧车道线中选取一车道线作为观测车道线；

所述第二计算模块，用于在当前时刻不为初始时刻时，根据本车车速以及本车转角速度计算上一时刻至当前时刻的位移量和方位角变化量；

所述第一参数更新模块，用于根据所述位移量、方位角变化量对上一时刻道路模型的模型参数进行时间更新，获得第一更新模型参数；

所述第二参数更新模块，用于根据所述横向偏移量以及预设的偏移量观测噪声对第一更新模型参数进行再次更新，获得第二更新模型参数；

所述第三参数更新模块，用于对所述观测车道线进行采样并计算各采样点处的道路斜率和噪声，继而根据各采样点处的道路斜率以及噪声对第二更新模型参数进行再次更新，获得第三更新模型参数；

所述第四参数更新模块，用于根据各目标车辆的航迹信息计算各航迹信息所对应的道路斜率和噪声，继而根据各航迹信息所对应的道路斜率和噪声对第三更新模型参数进行再次更新，获得第四更新模型参数，完成当前时刻所述道路模型的更新。

Images