CN113911123A - 一种道路模型的更新方法及装置 - Google Patents
一种道路模型的更新方法及装置 Download PDFInfo
- Publication number
- CN113911123A CN113911123A CN202111528023.6A CN202111528023A CN113911123A CN 113911123 A CN113911123 A CN 113911123A CN 202111528023 A CN202111528023 A CN 202111528023A CN 113911123 A CN113911123 A CN 113911123A
- Authority
- CN
- China
- Prior art keywords
- updating
- vehicle
- lane line
- road
- model
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 39
- 238000005070 sampling Methods 0.000 claims abstract description 96
- 238000006073 displacement reaction Methods 0.000 claims abstract description 48
- 239000011159 matrix material Substances 0.000 claims description 83
- 238000012546 transfer Methods 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 238000004891 communication Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
- B60W40/10—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to vehicle motion
- B60W40/105—Speed
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
- B60W40/02—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to ambient conditions
- B60W40/06—Road conditions
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
- B60W40/10—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to vehicle motion
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
- B60W2050/0001—Details of the control system
- B60W2050/0043—Signal treatments, identification of variables or parameters, parameter estimation or state estimation
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W2520/00—Input parameters relating to overall vehicle dynamics
- B60W2520/06—Direction of travel
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W2520/00—Input parameters relating to overall vehicle dynamics
- B60W2520/10—Longitudinal speed
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W2552/00—Input parameters relating to infrastructure
- B60W2552/53—Road markings, e.g. lane marker or crosswalk
Landscapes
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Human Computer Interaction (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明公开了一种道路模型的更新方法及装置,所述方法在对道路模型进行更新时,根据位移量、方位角变化量进行第一阶段的更新;根据横向偏移量和预设的偏移量观测噪声对第一阶段更新所得到模型参数进行了第二阶段的更新;根据观测车道线上各采样点处的道路斜率和噪声对第二阶段更新所得到的模型参数进行第三阶段的更新;根据各目标车辆的航迹信息所对应的道路斜率和噪声对第三阶段更新所得的模型参数进行第四阶段的更新,得到当前时刻道路模型的最终参数完成模型的更新。通过实施本发明实施例能够提高道路模型更新的准确性。
Description
技术领域
本发明涉及自动驾驶技术领域,尤其涉及一种道路模型的更新方法及装置。
背景技术
具有自动驾驶功能的车辆在自动驾驶时,需要通过传感器对车辆所在位置的周围道路环境进行感知并重构,进而拟合得到道路模型。现有技术基于车辆视觉传感器所检测车道线特征信息(车道线位置、宽度)来构建道路模型并对道路模型进行实时更新,但若车道线被车辆遮挡或污损时车道线检测结果误差较大,进而使得道路模型的参数更新不准确。
发明内容
本发明实施例提供一种道路模型的更新方法及装置,能有提高道路模型更新的准确性。
本发明一实施例提供了一种道路模型的更新方法,包括:获取当前时刻的本车左右两侧车道线的车道线参数、各目标车辆的航迹信息、本车车速以及本车转向角速度;
根据本车左右两侧车道线的车道线参数计算本车相对车道中心的横向偏移量,根据本车转向角速度从左侧车道线和右侧车道线中选取一车道线作为观测车道线;
若当前时刻不为初始时刻,则根据本车车速以及本车转角速度计算上一时刻至当前时刻的位移量和方位角变化量;
根据所述位移量、方位角变化量对上一时刻道路模型的模型参数进行时间更新,获得第一更新模型参数;
根据所述横向偏移量以及预设的偏移量观测噪声对第一更新模型参数进行再次更新,获得第二更新模型参数;
对所述观测车道线进行采样并计算各采样点处的道路斜率和噪声,继而根据各采样点处的道路斜率以及噪声对第二更新模型参数进行再次更新,获得第三更新模型参数;
根据各目标车辆的航迹信息计算各航迹信息所对应的道路斜率和噪声,继而根据各航迹信息所对应的道路斜率和噪声对第三更新模型参数进行再次更新,获得第四更新模型参数,完成当前时刻所述道路模型的更新。
进一步的,还包括:若当前时刻为初始时刻,则根据所述横向偏移量以及所述观测车道线的车道线参数,生成初始化道路模型参数,根据所述初始化道路模型参数构建初始化道路模型。
进一步的,所述根据本车左右两侧车道线的车道线参数计算本车相对车道中心的横向偏移量,具体包括:
计算左侧车道线的车道线参数中三次多项式方程的常数项系数,与右侧车道线的车道线参数中三次多项式方程的常数项系数之间的差值,获得上述横向偏移量。
进一步的,所述根据本车转向角速度从左侧车道线和右侧车道线中选取一车道线作为观测车道线,具体包括:
若所述本车转向角速度大于预设第一阈值,则选取左侧车道线作为观测车道线;
若所述本车转角速度小于预设第二阈值,则选取右侧车道线作为观测车道线;
若所述本车转角速度在预设第一阈值与预设第二阈值之间,则从左侧车道线和右侧车道线中选取有效长度较大的一车道线,作为观测车道线。
进一步的,所述位移量包括横向位移量以及纵向位移量;所述根据本车车速以及本车转角速度计算上一时刻至当前时刻的位移量和方位角变化量,具体包括:
若本车转角速度的绝对值大于预设第三阈值,则通过以下公式计算横向位移量以及纵向位移量:
若本车转角速度的绝对值小于或等于预设第三阈值,则通过以下公式计算横向位移量以及纵向位移量:
进一步的,所述根据所述位移量、方位角变化量对上一时刻道路模型的模型参数进行时间更新,获得第一更新模型参数,具体包括:
通过以下公式对上一时刻道路模型的模型参数进行时间更新,得到第一更新模型参数:
进一步的,所述根据所述横向偏移量以及预设的偏移量观测噪声对第一更新模型参数进行再次更新,获得第二更新模型参数,具体包括:
通过以下公式对第一更新模型参数进行再次更新,获得第二更新模型参数:
其中,θy0 k|k-1为第二更新模型参数;Ky0为卡尔曼增益,;Z Offset 为横向偏移量;Hy0为预设偏移量观测矩阵;Pk|k-1为与第一更新模型参数相对应的第一更新协方差矩阵,;Pk-1|k-1为上一时刻道路模型所对应的协方差矩阵;Uk-1为预设控制噪声协方差,RzOffset为预设的偏移量观测噪声。
进一步的,对所述观测车道线进行采样并计算各采样点处的道路斜率和噪声,继而根据各采样点处的道路斜率以及噪声对第二更新模型参数进行再次更新,获得第三更新模型参数,具体包括:
对所述观测车道线进行采样,获得第一采样点集;
从所述第一采样点集中提取有效长度小于预设长度阈值的采样点,获得有效采样点集;
计算有效采样点集中各采样点处的道路斜率、噪声以及对应的观测矩阵;
逐一根据各有效采样点集中各采样点处的道路斜率、噪声以及对应的观测矩阵,对第二更新模型参数进行迭代更新,获得第三更新模型参数。
进一步的,航迹信息包括:目标车辆在本车坐标系中的横向位置、纵向位置以及纵向速度;
所述根据各目标车辆的航迹信息计算各航迹信息所对应的道路斜率和噪声,继而根据各航迹信息所对应的道路斜率和噪声对第三更新模型参数进行再次更新,获得第四更新模型参数,具体包括:
将横向位置、纵向位置以及纵向速度均满足预设数值条件的航迹信息,作为有效航迹信息;
根据各有效航迹信息的横向位置、纵向位置以及纵向速度计算各有效航迹信息所对应的道路斜率、噪声以及观测矩阵;
逐一根据各有效航迹信息所对应的道路斜率、噪声以及对应的观测矩阵,对第三更新模型参数进行迭代更新,获得第四更新模型参数。
在上述方法项实施例的基础上,本发明对应提供了装置项实施例;
本发明一实施例提供了一种道路模型的更新装置,包括:数据获取模块、第一计算模块、第二计算模块、第一参数更新模块、第二参数更新模块、第三参数更新模块以及第四参数更新模块;
所述数据获取模块,用于获取当前时刻的本车左右两侧车道线的车道线参数、各目标车辆的航迹信息、本车车速以及本车转向角速度;
所述第一计算模块,用于根据本车左右两侧车道线的车道线参数计算本车相对车道中心的横向偏移量,根据本车转向角速度从左侧车道线和右侧车道线中选取一车道线作为观测车道线;
所述第二计算模块,用于在当前时刻不为初始时刻时,根据本车车速以及本车转角速度计算上一时刻至当前时刻的位移量和方位角变化量;
所述第一参数更新模块,用于根据所述位移量、方位角变化量对上一时刻道路模型的模型参数进行时间更新,获得第一更新模型参数;
所述第二参数更新模块,用于根据所述横向偏移量以及预设的偏移量观测噪声对第一更新模型参数进行再次更新,获得第二更新模型参数;
所述第三参数更新模块,用于对所述观测车道线进行采样并计算各采样点处的道路斜率和噪声,继而根据各采样点处的道路斜率以及噪声对第二更新模型参数进行再次更新,获得第三更新模型参数;
所述第四参数更新模块,用于根据各目标车辆的航迹信息计算各航迹信息所对应的道路斜率和噪声,继而根据各航迹信息所对应的道路斜率和噪声对第三更新模型参数进行再次更新,获得第四更新模型参数,完成当前时刻所述道路模型的更新。
通过实施本发明实施例具有如下有益效果:
本发明实施例提供了一种道路模型的更新方法及装置,所述方法在对道路模型进行更新时,根据位移量、方位角变化量进行第一阶段的更新,然后根据横向偏移量和预设的偏移量观测噪声对第一阶段更新所得到模型参数进行了第二阶段的更新,紧接着根据观测车道线上各采样点处的道路斜率和噪声对第二阶段更新所得到的模型参数进行第三阶段的更新,最后根据各目标车辆的航迹信息所对应的道路斜率和噪声对第三阶段更新所得的模型参数进行第四阶段的更新,得到当前时刻道路模型的最终参数,完成模型的更新。与现有技术相比,本发明在道路模型的更新过程中,加入了本车行驶区域中其他目标车辆的航迹信息进行更新,解决现有模型更新时单独依靠车道线的特征,在车道被遮挡时出现模型参数更新不准确的问题。此外,由于车辆视觉传感器的检测距离有限,仅能实现对近距离的车道线的特征进行提取无法感知远距离的车道线信息,因此仅基于车道线特征信息进行更新的道路模型在拟合远距离的道路形状时,与真实的道路形状存在较大偏差,而由于其他目标车辆的运动受到前方道路形状的约束,目标车辆的航迹信息能够包含的远距离道路形状信息,因此采用本发明的道路模型更新方法,能够引入远距离道路形状信息,进而使得更新后的道路模型在拟合远距离的道路形状时,能够降低与真实的道路形状存在偏差。
附图说明
图1是本发明一实施例提供的一种道路模型的更新方法的流程示意图。
图2是本发明一实施例提供的一种道路模型的更新装置的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
如图1所示,本发明一实施例提供了一种道路模型的更新方法,包括:
步骤S101: 获取当前时刻的本车左右两侧车道线的车道线参数、各目标车辆的航迹信息、本车车速以及本车转向角速度。
步骤S102: 根据本车左右两侧车道线的车道线参数计算本车相对车道中心的横向偏移量,根据本车转向角速度从左侧车道线和右侧车道线中选取一车道线作为观测车道线。
步骤S103: 若当前时刻不为初始时刻,则根据本车车速以及本车转角速度计算上一时刻至当前时刻的位移量和方位角变化量。
步骤S104: 根据所述位移量、方位角变化量对上一时刻道路模型的模型参数进行时间更新,获得第一更新模型参数。
步骤S105: 根据所述横向偏移量以及预设的偏移量观测噪声对第一更新模型参数进行再次更新,获得第二更新模型参数。
步骤S106: 对所述观测车道线进行采样并计算各采样点处的道路斜率和噪声,继而根据各采样点处的道路斜率以及噪声对第二更新模型参数进行再次更新,获得第三更新模型参数。
步骤S107: 根据各目标车辆的航迹信息计算各航迹信息所对应的道路斜率和噪声,继而根据各航迹信息所对应的道路斜率和噪声对第三更新模型参数进行再次更新,获得第四更新模型参数,完成当前时刻所述道路模型的更新。
对于步骤S101、具体的,获取当前时刻的各目标车辆的航迹信息,得到当前时刻的航迹量测集;需要说明的是,上述各目标车辆为在预设距离范围内,位于本车前方的其他车辆;航迹量测集中,N表示航迹信息的总条数,i表示第i条航迹信息;每一航迹信息Xtrack =[x,y,vx,vy];X为目标车辆在本车坐标系中的纵向位置、y为目标车辆在本车坐标系中横向位置、vx为目标车辆在本车坐标系中的纵向速度(绝对速度),vy为目标车辆在本车坐标系中的横向速度(绝对速度)。获取当前时刻本车所在车道左右两侧的车道线的车道线参数、在本发明中车道线用本车坐标系下的三次多项式的车道线方程进行表示,一车道线的车道线参数是其三次多项式方程的各项系数及有效长度r;即一车道线的车道线参数可以表示为:ZLM =[a0,a1,a2,a3,r];有效长度r是视觉传感器跟随车道线的三次多项式方程的各项系数一起给出的一个值,表示该组车道线的三次多项式的各项系数在多远距离内有效,一般受道路曲率,是否遮挡,车道线清晰程度等因素影响。获取本车速度V ego 与本车转向角速度。
对于步骤S102、在一个优选的实施例中,所述根据本车左右两侧车道线的车道线参数计算本车相对车道中心的横向偏移量,具体包括:
计算左侧车道线的车道线参数中三次多项式方程的常数项系数,与右侧车道线的车道线参数中三次多项式方程的常数项系数之间的差值,获得上述横向偏移量。
具体的,横向偏移量通过以下公式进行计算:Z Offset =a0_LLM-a0_RLM;Z Offset 为横向偏移量;a0_LLM为左侧车道线的三次多项式方程的常数项系数;a0_RLM为右侧车道线的三次多项式方程的常数项系数。
在一个优选的实施例中,所述根据本车转向角速度从左侧车道线和右侧车道线中选取一车道线作为观测车道线,具体包括:
若所述本车转向角速度大于预设第一阈值,则选取左侧车道线作为观测车道线;若所述本车转角速度小于预设第二阈值,则选取右侧车道线作为观测车道线;若所述本车转角速度在预设第一阈值与预设第二阈值之间,则从左侧车道线和右侧车道线中选取有效长度较大的一车道线,作为观测车道线。
示意性的,上述预设第一阈值设置为0.05rad/s;上述预设第二阈值设置为-0.05rad/s;如果,则选取左侧车道线作为观测车道线,用于后续的道路模型更新或初始化,如果,则选取右侧车道线作为观测车道线,用于后续的道路模型更新或初始化,如果,则选取有效长度r较大的一侧车道线作为观测车道线。
在一个优选的实施例中,在执行完步骤S101和步骤S102后,紧接着判断当前时刻是否为初始时刻,若当前时刻为初始时刻,则根据所述横向偏移量以及所述观测车道线的车道线参数,生成初始化道路模型参数,根据所述初始化道路模型参数构建初始化道路模型。示意性的,道路模型建模为,模型参数θ=[y0,η,c0,c1];假设所选定的观测车道线的车道线参数中除有效长度r和三次多项式方程的常数项系数外的其他参数为a1,a2,a3;则此时,根据横向偏移量以及观测车道线的车道线参数对道路模型进行初始化后,道路模型的初始化参数可以为θ0=[Z Offset ,a1,2a2,6a3]。优选的,同时对道路模型所对应的协方差矩阵进行初始化,设定初始化协方差矩阵P0 =diag([10-2,10-3,10-4,10-5]);
如果当前时刻不为初始时刻,则根据步骤S103-步骤S107,对当前时刻的道路模型进行更新。
对于步骤S103、在一个优选的实施例中,所述位移量包括横向位移量以及纵向位移量;所述根据本车车速以及本车转角速度计算上一时刻至当前时刻的位移量和方位角变化量,具体包括:
若本车转角速度的绝对值大于预设第三阈值,则通过以下公式计算横向位移量以及纵向位移量:
若本车转角速度的绝对值小于或等于预设第三阈值,则通过以下公式计算横向位移量以及纵向位移量:
否则,则本车位移通过以下公式计算:
对于步骤S104、在一个优选的实施例中,所述根据所述位移量、方位角变化量对上一时刻道路模型的模型参数进行时间更新,获得第一更新模型参数,具体包括:
通过以下公式对上一时刻道路模型的模型参数进行时间更新,得到第一更新模型参数:
在一个优选的实施例中,还通过以下公式对道路模型的协方差矩阵进行更新,获得第一更新协方差矩阵:
对于步骤S105、在一个优选的实施例中,所述根据所述横向偏移量以及预设的偏移量观测噪声对第一更新模型参数进行再次更新,获得第二更新模型参数,具体包括:
通过以下公式对第一更新模型参数进行再次更新,获得第二更新模型参数:
其中,θy0 k|k-1为第二更新模型参数;Ky0为卡尔曼增益,;Z Offset 为横向偏移量;Hy0为预设偏移量观测矩阵;Pk|k-1为与第一更新模型参数相对应的第一更新协方差矩阵,;Pk-1|k-1为上一时刻道路模型所对应的协方差矩阵;Uk-1为预设控制噪声协方差,RzOffset为预设的偏移量观测噪声。
对于步骤S106、在一个优选的实施例中,对所述观测车道线进行采样并计算各采样点处的道路斜率和噪声,继而根据各采样点处的道路斜率以及噪声对第二更新模型参数进行再次更新,获得第三更新模型参数,具体包括:
对所述观测车道线进行采样,获得第一采样点集;
从所述第一采样点集中提取有效长度小于预设长度阈值的采样点,获得有效采样点集;
计算有效采样点集中各采样点处的道路斜率、噪声以及对应的观测矩阵;
逐一根据各有效采样点集中各采样点处的道路斜率、噪声以及对应的观测矩阵,对第二更新模型参数进行迭代更新,获得第三更新模型参数。
具体的,对选定的观测车道线进行采样,得到第一采样点集,然后从第一采样点集中提取距离小于有效长度r的点,获得有效采样点集:;在中,为有效采样点集中采样点的总数,Xi sample表示有效采样点集中第i个采样点;
紧接着根据以下公式计算有效采样点集中各采样点处的道路斜率:
fi LM=a1+2a2Xi sample+3a3Xi sample 2;
fi LM为有效采样点集中第i采样点处的道路斜率,Xi sample为本车坐标系下第i个采样点的纵向位置;
根据以下公式计算有效采样点集中各采样点处的噪声:
R i sample= Xi sample 2/1000;R i sample为有效采样点集中第i采样点处的噪声;
根据以下公式计算有效采样点集中各采样点处对应的观测矩阵:
其中,Hi LM为有效采样点集中第i采样点处对应的观测矩阵;
紧接着逐一根据得到的各采样点处的道路斜率、噪声和观测矩阵,对第二更新模型参数进行迭代更新,直至根据有效采样点集中所有采样点的道路斜率、噪声和观测矩阵更新完毕后,即可得到第三更新模型参数θLM_p k|k-1;优选的,逐一根据各采样点处的道路斜率、噪声和观测矩阵,对第二更新协方差矩阵进行迭代更新,得到第三更新协方差矩阵PLM _p k|k-1。
需要说明的是,在单独根据某一采样点i的道路斜率、噪声和观测矩阵进行模型参数以及协方差矩阵更新时,其更新方式与步骤S105的更新方式类似;
具体的,如果采样点i为第一个采样点,则其模型参数以及协方差矩阵的更新公式为:
θLM_1 k|k-1为根据第一个采样点的道路斜率、噪声和观测矩阵进行更新后的道路模型的参数;KLM_1为第一个采样点所对应的卡尔曼增益;f1 LM为第一个采样点所对应的道路斜率;H1 LM为第一个采样点所对应的观测矩阵;P y0 k|k-1为上述第二更新协方差矩阵;R1 sample第一个采样点所对应的噪声;PLM_1 k|k-1为根据第一个采样点的道路斜率、噪声和观测矩阵进行更新后的协方差矩阵。
如果采样点不为第一个采样点,则其模型参数以及协方差矩阵的更新公式为:
θLM_i k|k-1为根据第i个采样点的道路斜率、噪声和观测矩阵进行更新后的道路模型的参数;θLM_i-1 k|k-1为根据第i-1个采样点的道路斜率、噪声和观测矩阵进行更新后的道路模型的参数;KLM_i为第i个采样点所对应的卡尔曼增益;PLM_i-1 k|k-1为根据第i-1个采样点的道路斜率、噪声和观测矩阵进行更新后协方差矩阵。
通过上述公式,在根据最后一个采样点P的道路斜率、噪声和观测矩阵进行更新后得到本发明上述第三更新模型参数θLM_p k|k-1以及第三更新协方差矩阵PLM_p k|k-1。
对于步骤S107、在一个优选的实施例中,航迹信息包括:目标车辆在本车坐标系中的横向位置、纵向位置以及纵向速度;
所述根据各目标车辆的航迹信息计算各航迹信息所对应的道路斜率和噪声,继而根据各航迹信息所对应的道路斜率和噪声对第三更新模型参数进行再次更新,获得第四更新模型参数,具体包括:
将横向位置、纵向位置以及纵向速度均满足预设数值条件的航迹信息,作为有效航迹信息;
根据各有效航迹信息的横向位置、纵向位置以及纵向速度计算各有效航迹信息所对应的道路斜率、噪声以及观测矩阵;
逐一根据各有效航迹信息所对应的道路斜率、噪声以及对应的观测矩阵,对第三更新模型参数进行迭代更新,获得第四更新模型参数。
紧接着,根据以下公式将每一航迹信息上一时刻的坐标值补偿掉本车运动转化到当前坐标系中:
公式中,Xk-1为一目标车辆上一时刻的纵向位置(在本发明中当前时刻为k,上一时刻为k-1);yk-1为一目标车辆上一时刻的横向位置;
紧接着,通过以下公式计算每一航迹信息所对应的道路斜率、噪声以及观测矩阵:
公式中,fK为一航迹信息所对应的道路斜率;RK为一航迹信息所对应的噪声,HK为一航迹信息所对应的观测矩阵:vx_K为当前时刻一航迹信息中的纵向速度;紧接着逐一根据得到的各航迹信息的道路斜率、噪声和观测矩阵,对第三更新模型参数进行迭代更新,直至根据有效航迹集中所有航迹信息的道路斜率、噪声和观测矩阵更新完毕后,即可得到第四更新模型参数θtrack_M k|k-1;优选的,还可以逐一根据各航迹信息的道路斜率、噪声和观测矩阵,对第三更新协方差矩阵进行迭代更新,得到第四更新协方差矩阵Ptrack_M k|k-1。
需要说明的是,在单独根据某一航迹信息i的道路斜率、噪声和观测矩阵进行模型参数以及协方差矩阵更新时,其更新方式与步骤S105的更新方式类似;
具体的,如果航迹信息i为第一个航迹信息,则其模型参数以及协方差矩阵的更新公式为:
θtrack_1 k|k-1为根据第一个航迹信息的道路斜率、噪声和观测矩阵进行更新后的道路模型的参数;Ktrack_1为第一个航迹信息所对应的卡尔曼增益;f1 track为第一个航迹信息所对应的道路斜率;H1 track_1为第一个航迹信息所对应的观测矩阵;R1 k为第一个航迹信息所对应的噪声;Ptrack_1 k|k-1为根据第一个航迹信息所对应的道路斜率、噪声和观测矩阵进行更新后的协方差矩阵。
如果航迹信息不为第一个航迹信息,则其模型参数以及协方差矩阵的更新公式为:
θtrack_i k|k-1为根据第i个航迹信息所对应的道路斜率、噪声和观测矩阵进行更新后的道路模型的参数;θtrack_i-1 k|k-1为根据第i-1个航迹信息所对应的道路斜率、噪声和观测矩阵进行更新后的道路模型的参数;Ktrack_i为第i个航迹信息所对应的卡尔曼增益;fi track为第i个航迹信息所对应的道路斜率;Hi track为第i个航迹信息所对应的观测矩阵;Ptrack_i-1 k|k-1为根据第i-1个采样点的道路斜率、噪声和观测矩阵进行更新后协方差矩阵;Ri k为第i个航迹信息所对应的噪声;Ptrack_i k|k-1为根据第i个航迹信息的道路斜率、噪声和观测矩阵进行更新后协方差矩阵。
通过上述公式,在根据最后一个航迹信息M所对应的道路斜率、噪声和观测矩阵进行更新后得到本发明上述第四更新模型参数θtrack_M k|k-1及第四更新协方差矩阵Ptrack_M k|k-1,最后将第四更新模型参数θtrack_M k|k-1作为当前时刻道路模型的最终参数,完成当前时刻道路模型的更新。
通过本发明上述实施例所公开的道路模型的更新方法,在对当前时刻的道路模型进行更新时,引入了其他目标车辆的航迹信息,即使车道线被部分遮挡出现跳变的情况下,也能准确的进行模型参数的更新,此外远距离航迹信息中包含的远距离道路形状信息,因此引入航迹信息进行更新使得更新后的道路模型在进行较远距离的道路拟合时,所拟合出的道路跟接近实际道路形状。
在上述方法项实施例的基础上本发明对应提供了装置项实施例;
如图2所示,本发明一实施例提供了一种道路模型的更新装置,包括数据获取模块、第一计算模块、第二计算模块、第一参数更新模块、第二参数更新模块、第三参数更新模块以及第四参数更新模块;
所述数据获取模块,用于获取当前时刻的本车左右两侧车道线的车道线参数、各目标车辆的航迹信息、本车车速以及本车转向角速度;
所述第一计算模块,用于根据本车左右两侧车道线的车道线参数计算本车相对车道中心的横向偏移量,根据本车转向角速度从左侧车道线和右侧车道线中选取一车道线作为观测车道线;
所述第二计算模块,用于在当前时刻不为初始时刻时,根据本车车速以及本车转角速度计算上一时刻至当前时刻的位移量和方位角变化量;
所述第一参数更新模块,用于根据所述位移量、方位角变化量对上一时刻道路模型的模型参数进行时间更新,获得第一更新模型参数;
所述第二参数更新模块,用于根据所述横向偏移量以及预设的偏移量观测噪声对第一更新模型参数进行再次更新,获得第二更新模型参数;
所述第三参数更新模块,用于对所述观测车道线进行采样并计算各采样点处的道路斜率和噪声,继而根据各采样点处的道路斜率以及噪声对第二更新模型参数进行再次更新,获得第三更新模型参数;
所述第四参数更新模块,用于根据各目标车辆的航迹信息计算各航迹信息所对应的道路斜率和噪声,继而根据各航迹信息所对应的道路斜率和噪声对第三更新模型参数进行再次更新,获得第四更新模型参数,完成当前时刻所述道路模型的更新。
需说明的是,上述装置项实施例是与本发明方法项实施例相对应的,其可以实现本发明任意一实施例所述的道路模型的更新方法,此外以上所描述的装置实施例仅仅是示意性的,其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的,作为单元显示的部件可以是或者也可以不是物理单元,即可以位于一个地方,或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。另外,本发明提供的装置实施例附图中,模块之间的连接关系表示它们之间具有通信连接,具体可以实现为一条或多条通信总线或信号线。本领域普通技术人员在不付出创造性劳动的情况下,即可以理解并实施。
以上所述是本发明的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰也视为本发明的保护范围。
Claims (10)
1.一种道路模型的更新方法,其特征在于,包括:
获取当前时刻的本车左右两侧车道线的车道线参数、各目标车辆的航迹信息、本车车速以及本车转向角速度;
根据本车左右两侧车道线的车道线参数计算本车相对车道中心的横向偏移量,根据本车转向角速度从左侧车道线和右侧车道线中选取一车道线作为观测车道线;
若当前时刻不为初始时刻,则根据本车车速以及本车转角速度计算上一时刻至当前时刻的位移量和方位角变化量;
根据所述位移量、方位角变化量对上一时刻道路模型的模型参数进行时间更新,获得第一更新模型参数;
根据所述横向偏移量以及预设的偏移量观测噪声对第一更新模型参数进行再次更新,获得第二更新模型参数;
对所述观测车道线进行采样并计算各采样点处的道路斜率和噪声,继而根据各采样点处的道路斜率以及噪声对第二更新模型参数进行再次更新,获得第三更新模型参数;
根据各目标车辆的航迹信息计算各航迹信息所对应的道路斜率和噪声,继而根据各航迹信息所对应的道路斜率和噪声对第三更新模型参数进行再次更新,获得第四更新模型参数,完成当前时刻所述道路模型的更新。
2.如权利要求1所述的道路模型的更新方法,其特征在于,还包括:若当前时刻为初始时刻,则根据所述横向偏移量以及所述观测车道线的车道线参数,生成初始化道路模型参数,根据所述初始化道路模型参数构建初始化道路模型。
3.如权利要求1所述的道路模型的更新方法,其特征在于,所述根据本车左右两侧车道线的车道线参数计算本车相对车道中心的横向偏移量,具体包括:
计算左侧车道线的车道线参数中三次多项式方程的常数项系数,与右侧车道线的车道线参数中三次多项式方程的常数项系数之间的差值,获得上述横向偏移量。
4.如权利要求1所述的道路模型的更新方法,其特征在于,所述根据本车转向角速度从左侧车道线和右侧车道线中选取一车道线作为观测车道线,具体包括:
若所述本车转向角速度大于预设第一阈值,则选取左侧车道线作为观测车道线;
若所述本车转角速度小于预设第二阈值,则选取右侧车道线作为观测车道线;
若所述本车转角速度在预设第一阈值与预设第二阈值之间,则从左侧车道线和右侧车道线中选取有效长度较大的一车道线,作为观测车道线。
8.如权利要求7所述的道路模型的更新方法,其特征在于,对所述观测车道线进行采样并计算各采样点处的道路斜率和噪声,继而根据各采样点处的道路斜率以及噪声对第二更新模型参数进行再次更新,获得第三更新模型参数,具体包括:
对所述观测车道线进行采样,获得第一采样点集;
从所述第一采样点集中提取有效长度小于预设长度阈值的采样点,获得有效采样点集;
计算有效采样点集中各采样点处的道路斜率、噪声以及对应的观测矩阵;
逐一根据各有效采样点集中各采样点处的道路斜率、噪声以及对应的观测矩阵,对第二更新模型参数进行迭代更新,获得第三更新模型参数。
9.如权利要求8所述的道路模型的更新方法,其特征在于,航迹信息包括:目标车辆在本车坐标系中的横向位置、纵向位置以及纵向速度;
所述根据各目标车辆的航迹信息计算各航迹信息所对应的道路斜率和噪声,继而根据各航迹信息所对应的道路斜率和噪声对第三更新模型参数进行再次更新,获得第四更新模型参数,具体包括:
将横向位置、纵向位置以及纵向速度均满足预设数值条件的航迹信息,作为有效航迹信息;
根据各有效航迹信息的横向位置、纵向位置以及纵向速度计算各有效航迹信息所对应的道路斜率、噪声以及观测矩阵;
逐一根据各有效航迹信息所对应的道路斜率、噪声以及对应的观测矩阵,对第三更新模型参数进行迭代更新,获得第四更新模型参数。
10.一种道路模型的更新装置,其特征在于,包括:数据获取模块、第一计算模块、第二计算模块、第一参数更新模块、第二参数更新模块、第三参数更新模块以及第四参数更新模块;
所述数据获取模块,用于获取当前时刻的本车左右两侧车道线的车道线参数、各目标车辆的航迹信息、本车车速以及本车转向角速度;
所述第一计算模块,用于根据本车左右两侧车道线的车道线参数计算本车相对车道中心的横向偏移量,根据本车转向角速度从左侧车道线和右侧车道线中选取一车道线作为观测车道线;
所述第二计算模块,用于在当前时刻不为初始时刻时,根据本车车速以及本车转角速度计算上一时刻至当前时刻的位移量和方位角变化量;
所述第一参数更新模块,用于根据所述位移量、方位角变化量对上一时刻道路模型的模型参数进行时间更新,获得第一更新模型参数;
所述第二参数更新模块,用于根据所述横向偏移量以及预设的偏移量观测噪声对第一更新模型参数进行再次更新,获得第二更新模型参数;
所述第三参数更新模块,用于对所述观测车道线进行采样并计算各采样点处的道路斜率和噪声,继而根据各采样点处的道路斜率以及噪声对第二更新模型参数进行再次更新,获得第三更新模型参数;
所述第四参数更新模块,用于根据各目标车辆的航迹信息计算各航迹信息所对应的道路斜率和噪声,继而根据各航迹信息所对应的道路斜率和噪声对第三更新模型参数进行再次更新,获得第四更新模型参数,完成当前时刻所述道路模型的更新。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111528023.6A CN113911123B (zh) | 2021-12-15 | 2021-12-15 | 一种道路模型的更新方法及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111528023.6A CN113911123B (zh) | 2021-12-15 | 2021-12-15 | 一种道路模型的更新方法及装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113911123A true CN113911123A (zh) | 2022-01-11 |
CN113911123B CN113911123B (zh) | 2022-02-18 |
Family
ID=79249051
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111528023.6A Active CN113911123B (zh) | 2021-12-15 | 2021-12-15 | 一种道路模型的更新方法及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113911123B (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115236618A (zh) * | 2022-06-16 | 2022-10-25 | 中国第一汽车股份有限公司 | 护栏的检测方法、检测装置与车辆 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20030125871A1 (en) * | 1997-10-16 | 2003-07-03 | Kevin Cherveny | System and method for updating, enhancing, or refining a geographic database using feedback |
CN102616241A (zh) * | 2012-03-28 | 2012-08-01 | 周圣砚 | 基于车道线模型检测方法和在线学习方法的车道偏离报警系统 |
CN104778850A (zh) * | 2014-01-06 | 2015-07-15 | 集奥数字国际有限公司 | 确定道路模型需要更新的部分 |
CN109726426A (zh) * | 2018-11-12 | 2019-05-07 | 初速度(苏州)科技有限公司 | 一种车辆自动驾驶虚拟环境构建方法 |
CN110969837A (zh) * | 2018-09-30 | 2020-04-07 | 长城汽车股份有限公司 | 自动驾驶车辆的道路信息融合系统及方法 |
-
2021
- 2021-12-15 CN CN202111528023.6A patent/CN113911123B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20030125871A1 (en) * | 1997-10-16 | 2003-07-03 | Kevin Cherveny | System and method for updating, enhancing, or refining a geographic database using feedback |
CN102616241A (zh) * | 2012-03-28 | 2012-08-01 | 周圣砚 | 基于车道线模型检测方法和在线学习方法的车道偏离报警系统 |
CN104778850A (zh) * | 2014-01-06 | 2015-07-15 | 集奥数字国际有限公司 | 确定道路模型需要更新的部分 |
CN110969837A (zh) * | 2018-09-30 | 2020-04-07 | 长城汽车股份有限公司 | 自动驾驶车辆的道路信息融合系统及方法 |
CN109726426A (zh) * | 2018-11-12 | 2019-05-07 | 初速度(苏州)科技有限公司 | 一种车辆自动驾驶虚拟环境构建方法 |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115236618A (zh) * | 2022-06-16 | 2022-10-25 | 中国第一汽车股份有限公司 | 护栏的检测方法、检测装置与车辆 |
Also Published As
Publication number | Publication date |
---|---|
CN113911123B (zh) | 2022-02-18 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111272165B (zh) | 一种基于特征点标定的智能车定位方法 | |
CN110906923B (zh) | 车载多传感器紧耦合融合定位方法、系统、存储介质及车辆 | |
Gehrig et al. | Dead reckoning and cartography using stereo vision for an autonomous car | |
CN112083726B (zh) | 一种面向园区自动驾驶的双滤波器融合定位系统 | |
CN112083725B (zh) | 一种面向自动驾驶车辆的结构共用多传感器融合定位系统 | |
CN110411462B (zh) | 一种gnss/惯导/车道线约束/里程计多源融合方法 | |
CN112904395B (zh) | 一种矿用车辆定位系统及方法 | |
CN111239757A (zh) | 一种路面特征参数自动提取方法及系统 | |
CN109933056A (zh) | 一种基于slam的机器人导航方法以及机器人 | |
JP4984659B2 (ja) | 自車両位置推定装置 | |
CN113466890B (zh) | 基于关键特征提取的轻量化激光雷达惯性组合定位方法和系统 | |
CN108508471A (zh) | 一种无人驾驶车辆定位方法及装置 | |
CN114396957B (zh) | 基于视觉与地图车道线匹配的定位位姿校准方法及汽车 | |
CN111189454A (zh) | 基于秩卡尔曼滤波的无人车slam导航方法 | |
CN110763224A (zh) | 一种自动导引运输车导航方法及导航系统 | |
CN113911123B (zh) | 一种道路模型的更新方法及装置 | |
CN116337045A (zh) | 一种基于karto和teb的高速建图导航方法 | |
CN114623823A (zh) | 一种融合里程计的uwb多模态定位系统、方法及装置 | |
JP2023164553A (ja) | 位置推定装置、推定装置、制御方法、プログラム及び記憶媒体 | |
CN110637209A (zh) | 估计机动车的姿势的方法、设备和具有指令的计算机可读存储介质 | |
CN115577320A (zh) | 一种基于数据插值的多传感器异步数据融合方法 | |
CN116338719A (zh) | 基于b样条函数的激光雷达-惯性-车辆融合定位方法 | |
US11613272B2 (en) | Lane uncertainty modeling and tracking in a vehicle | |
CN113701749A (zh) | 一种基于立体视觉惯性里程计的车辆定位导航方法及系统 | |
CN114638902B (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 | ||
CP03 | Change of name, title or address |
Address after: Floor 25, Block A, Zhongzhou Binhai Commercial Center Phase II, No. 9285, Binhe Boulevard, Shangsha Community, Shatou Street, Futian District, Shenzhen, Guangdong 518000 Patentee after: Shenzhen Youjia Innovation Technology Co.,Ltd. Address before: 518051 401, building 1, Shenzhen new generation industrial park, No. 136, Zhongkang Road, Meidu community, Meilin street, Futian District, Shenzhen, Guangdong Province Patentee before: SHENZHEN MINIEYE INNOVATION TECHNOLOGY Co.,Ltd. |
|
CP03 | Change of name, title or address |