一种结构化道路下关键障碍物筛选方法与流程

[0001]
本发明属于智能汽车技术领域，涉及一种关键障碍物筛选方法，更加具体的来讲，涉及一种结构化道路下关键障碍物筛选方法。


背景技术：

[0002]
随着汽车智能化水平的提升，用户对车载智能驾驶辅助系统和无人驾驶系统的接受度越来越高，高性能的智能化系统开发依赖多种传感手段，目前许多传感融合手段最终输出的结果都是主车周围的障碍物列表，但是智能化系统的开发只需要对主车行驶影响最为显著的关键障碍物信息，关键障碍物即是指每个车道距离主车最近的障碍物，其余的非关键障碍物对车辆行驶的影响比较微弱，如何准确筛选出关键障碍物的信息成为开发高性能驾驶辅助系统以及无人驾驶系统的关键性问题。


技术实现要素：

[0003]
为了解决现有技术存在的问题，本发明提供了一种结构化道路下关键障碍物筛选方法，基于行车场景判断模式，判断出车辆所处的行车场景，基于传感模块输出的障碍物列表，筛选出对主车行驶影响最为显著的关键障碍物，关键障碍物即是指每个车道距离主车最近的障碍物，方便智能驾驶辅助系统和无人驾驶技术功能的开发。
[0004]
本发明是采用如下技术方案实现的：
[0005]
一种结构化道路下关键障碍物筛选方法，基于行车场景判断模式，判断出主车所处的行车场景，基于传感模块输出的障碍物列表，筛选出对主车行驶影响最为显著的关键障碍物，关键障碍物即是指每个车道距离主车最近的障碍物，方便主车驾驶辅助策略和主车无人驾驶系统决策与控制策略开发，本方法具体步骤如下：
[0006]
步骤一、所需传感信息的定义：
[0007]
行车场景的判断以及障碍物的筛选都是在结构化道路环境，结构化道路下车道线清晰可见，同时关键障碍物筛选都在车辆坐标系下进行，因此定义车辆坐标系，车辆坐标系的坐标原点在主车质心o处，x轴朝向主车前进的方向，y轴的正方向为x轴逆时针旋转90度方向；
[0008]
关键障碍物筛选基于单一或者多传感融合后的目标级障碍物传感信息以及车道线信息，因此定义关键障碍物筛选所需要的传感信息，首先定义传感模块输出的障碍物的总数量为n，每个障碍物相对主车的坐标(x
i
,y
i
),i＝1,
…
,n，其中i代表障碍物的编号，同时将所有障碍物的信息赋值到一个m
×
n矩阵里面，矩阵的前三行依次是障碍物的编号i、障碍物相对主车的纵向坐标x
i
和障碍物相对主车的侧向坐标y
i
，矩阵的第四行至矩阵的第m-1行可以根据用户的定义用于输出障碍物的其他信息，矩阵的最后一行即第m行留作障碍物在车道定位的标志位，当障碍物未定位之前矩阵的最后一行赋值为0，则障碍物矩阵命名为obj，矩阵的每一列代表的是所对应障碍物的全部信息,具体如下所示：
[0009][0010]
定义主车获取的车道线信息，主车能够获取的车道线信息有四条，分别为左侧两条，右侧两条，四条车道线标识从左至右依次被命名为ll,l,r,rr，主车获取的车道线信息是三次插值函数，车道线获取的插值参数信息为d
j
,c
j
,b
j
,a
j
,j＝ll,l,r,rr，其中下标j代表所属标识车道线的参数，因此可以获取车道线方程如式(2)所示
[0011][0012]
式中x
j
为车道线在车辆坐标系下的纵向坐标，y
j
为车道线在车辆坐标系下的侧向坐标；
[0013]
定义传感模块获取的每条车道线类型为s
j
,j＝ll,l,r,rr，其中s
j
取1或0，当车道线类型为虚线时定义s
j
＝1，当车道线类型为非虚线时s
j
＝0；
[0014]
步骤二、主车行驶场景判断：
[0015]
结构化道路下的行车场景可以划分为四类，第一类行车场景为三车道行车场景且主车行驶在中间车道，主车车道以及左右相邻车道均可行驶，即使车道数量大于三，但是除了主车所在车道以及主车所在车道相邻车道外其余车道的车辆对主车影响较小，不予考虑，因此该第一类行车场景下每个车道内距离主车最近的关键障碍物需要筛选；第二类行车场景为双车道场景且主车行驶在左侧车道，只需要筛选主车所在车道以及主车右侧相邻车道关键障碍物，主车所在车道左侧认为不可行驶区域；第三类行车场景为双车道场景且主车行驶在右侧车道，只需要筛选主车所在车道以及主车相邻左侧车道关键障碍物，主车所在车道右侧为不可行驶区域；第四类为单车道行车场景，只需要筛选本车道关键障碍物即可，主车所在车道左右均为不可行使区域；
[0016]
进行关键障碍物筛选之前首先需要判断主车所行驶的场景，主车行驶场景的判断主要依据紧挨着主车左右两侧的两条车道线类型，当左右车道线均为虚线时即s
l
＝1,s
r
＝1，主车的行驶场景为上述第一类行车场景；仅右侧车道线为虚线时即s
l
＝0,s
r
＝1，主车的行驶场景为上述第二类行车场景；仅左侧车道线为虚线时即s
l
＝1,s
r
＝0，主车的行驶场景为上述第三类行车场景；当左右两侧均不为虚线时即s
l
＝0,s
r
＝0，主车的行驶场景为上述第四类行车场景；
[0017]
步骤三、判定障碍物所在车道：
[0018]
在判断行车场景的基础上，将所有障碍物在车道上进行定位，对障碍物在车道上进行定位主要通过对比障碍物与车道之间的相对位置关系，首先定义不同场景下的车道编号，主车所在车道编号为1，主车左侧相邻车道为2,主车右侧相邻车道为3，当障碍物不属于主车车道以及主车左右相邻两个车道内时认为障碍物所属车道编号为0，代表此障碍物不予考虑，判定障碍物所在车道后将矩阵obj最后一行赋值为所对应的车道编号值；
[0019]
1)当处于第一类行车场景下，需要对n个障碍物分别进行所在车道判定，编号为i
的障碍物，将障碍物纵向坐标x
i
带入式(2)，即可获取从左到右四条车道线在此障碍物纵向坐标处的位移，分别为y
lli
,y
li
,y
ri
,y
rri
，当y
lli
≥y
i
＞y
li
，则矩阵obj第i列第m行赋值为2；当y
li
≥y
i
＞y
ri
，则矩阵obj第i列第m行赋值为1；当y
ri
≥y
i
≥y
rri
，则矩阵obj第i列第m行赋值为3；当y
rri
＞y
i
或y
i
＞y
lli
，则矩阵obj第i列第m行赋值为0；按照上述方式依次对n个障碍物进行所属车道判定；
[0020]
2)当障碍物处于第二类行车场景下，需要对n个障碍物分别进行所在车道判定，由于第二类行车场景下主车相邻左侧车道线左侧均为不可行使区域，因此只需要判定障碍物是否属于主车车道或主车车道相邻右侧车道即可，编号为i的障碍物，将障碍物纵向坐标x
i
带入式(2)，获取此障碍物纵向坐标处左侧相邻主车的车道线侧向位移以及右侧两条车道线在障碍物纵向坐标处侧向位移即可，分别为y
li
,y
ri
,y
rri
，当y
li
≥y
i
＞y
ri
，则矩阵obj第i列第m行赋值为1；当y
ri
≥y
i
≥y
rri
，则矩阵obj第i列第m行赋值为3；当y
rri
＞y
i
或y
i
＞y
li
，则矩阵obj第i列第m行赋值为0；按照上述方式依次对n个障碍物进行所属车道判定；
[0021]
3)当障碍物处于第三行车类场景下，需要对n个障碍物分别进行所在车道判定，由于第三类行车场景下主车相邻右侧车道线右侧均为不可行使区域，因此只需要判定障碍物是否属于主车车道或主车车道相邻左侧车道即可，编号为i的障碍物，将障碍物纵向坐标x
i
带入式(2)，获取此障碍物纵向坐标处右侧相邻主车的车道线侧向位移以及左侧两条车道线在障碍物纵向坐标处侧向位移即可，分别为y
lli
,y
li
,y
ri
，当y
lli
≥y
i
＞y
li
，则矩阵obj第i列第m行赋值为2；当y
li
≥y
i
≥y
ri
，则矩阵obj第i列第m行赋值为1；当y
ri
＞y
i
或y
i
＞y
lli
，则矩阵obj第i列第m行赋值为0；按照上述方式依次对n个障碍物进行所属车道判定；
[0022]
4)当障碍物处于第四类行车场景下，需要对n个障碍物分别进行所在车道判定，，由于第四类行车场景下主车相邻右侧车道线右侧与主车相邻左侧车道线左侧均为不可行使区域，因此只需要判定障碍物是否属于主车车道即可，编号为i的障碍物，将障碍物纵向坐标x
i
带入式(2)，获取此障碍物纵向坐标处右侧相邻主车的车道线侧向位移以及左侧相邻主车的车道线侧向位移即可，分别为y
li
,y
ri
，当y
li
≥y
i
≥y
ri
，则矩阵obj第i列第m行赋值为1；当y
ri
＞y
i
或y
i
＞y
li
，则矩阵obj第i列第m行赋值为0；按照上述方式依次对n个障碍物进行所属车道判定；
[0023]
步骤四、关键障碍物筛选：
[0024]
关键障碍物能够直接影响主车驾驶辅助策略以及主车无人驾驶系统决策与控制策略开发，步骤四对障碍物所在车道进行判定，在步骤四基础上进行关键障碍物的筛选；
[0025]
1)当处于第一类行车场景下，需要分别对主车所在车道以及主车相邻左右车道的关键障碍物进行筛选，首先提取主车相邻左侧车道障碍物，即提取矩阵obj最后一行为2的列重新组成两个新的矩阵obj
l+
和obj
l-，obj
l+
是由矩阵obj最后一行为2且第2行障碍物纵向坐标为正的列组成的，代表主车相邻左侧车道内位于主车前方的障碍物，obj-是由矩阵obj最后一行为2且第2行障碍物纵向坐标为负的列组成的，代表主车相邻左侧车道内位于主车后方的障碍物，矩阵obj
l+
和obj
l-如式(3)所示：
[0026][0027]
其中n
l+
为矩阵obj最后一行为2且第2行障碍物纵向坐标为正的列的个数，n
l-为矩阵obj最后一行为2且第2行障碍物纵向坐标为负的列的个数；主车相邻左侧车道关键障碍物分别选取前方距离主车最近的障碍物和后方距离主车最近的障碍物即可，主车相邻左侧车道前方距离主车最近的障碍物信息为obj
l+
，obj
l+
即为矩阵obj
l+
中第2行障碍物纵向坐标最小值所对应的列，当obj
l+
为空矩阵时obj
l+
中所有元素为0，代表主车相邻左侧车道前方不存在障碍物；主车相邻左侧车道后方距离主车最近的障碍物信息为obj
l-，obj
l-即为矩阵obj
l-中第2行障碍物纵向坐标最大值所对应的列，当obj
l-为空矩阵时obj
l-中所有元素为0，代表主车相邻左侧车道后方不存在障碍物；
[0028]
主车所在车道关键障碍物方法：提取矩阵obj最后一行为1的列重新组成两个新的矩阵obj
h+
和obj
h-，obj
h+
是由矩阵obj最后一行为1且第2行障碍物纵向坐标为正的列组成的，代表主车所在车道内位于主车前方的障碍物，obj
h-是由矩阵obj最后一行为1且第2行障碍物纵向坐标为负的列组成的，代表主车所在车道内位于主车后方的障碍物，矩阵obj
h+
和obj
h-如式(4)所示：
[0029][0030]
其中n
h+
为矩阵obj最后一行为1且第2行障碍物纵向坐标为正的列的个数，n
h-为矩阵obj最后一行为1且第2行障碍物纵向坐标为负的列的个数；主车相邻左侧车道关键障碍物分别选取前方距离主车最近的障碍物和后方距离主车最近的障碍物即可，主车所在车道前方距离主车最近的障碍物信息为obj
h+
，obj
h+
即为矩阵obj
h+
中第2行最小元素所对应的列，当obj
h+
为空矩阵时obj
h+
中所有元素为0，代表主车所在车道前方不存在障碍物；主车所在车道后方距离主车最近的障碍物信息为obj
h-，obj
h-即为矩阵obj
h-中第2行障碍物纵向坐标最大值所对应的列，当obj
h-为空矩阵时obj
h-中所有元素为0，代表主车所在车道后方不存在障碍物；
[0031]
主车相邻右侧车道关键障碍物方法：提取矩阵obj最后一行为3的列重新组成两个新的矩阵obj
r+
和obj
r-，obj
r+
是由矩阵obj最后一行为3且第2行障碍物纵向坐标为正的列组成的，代表主车相邻右侧车道内位于主车前方的障碍物，obj
r-是由矩阵obj最后一行为3且第2行障碍物纵向坐标为负的列组成的，代表主车相邻右侧车道内位于主车后方的障碍物，
矩阵obj
r+
和obj
r-如式(5)所示：
[0032][0033]
其中n
r+
为矩阵obj最后一行为3且第2行障碍物纵向坐标为正的列的个数，n
r-为矩阵obj最后一行为3且第2行障碍物纵向坐标为负的列的个数；主车相邻右侧车道关键障碍物分别选取前方距离主车最近的障碍物和后方距离主车最近的障碍物即可，主车所在车道前方距离主车最近的障碍物信息为obj
r+
，obj
r+
即为矩阵obj
r+
中第2行最小元素所对应的列，当obj
r+
为空矩阵时obj
r+
中所有元素为0，代表主车相邻右侧车道前方不存在障碍物；主车相邻右侧车道后方距离主车最近的障碍物信息为obj
r-，obj
r-即为矩阵obj
r-中第2行障碍物纵向坐标最大值所对应的列，当obj
r-为空矩阵时obj
r-中所有元素为0，代表主车相邻右侧车道后方不存在障碍物；定义关键障碍物矩阵为obj，其从左至右六列分别表示主车相邻左侧车道前方关键障碍物、主车相邻左侧车道后方关键障碍物、主车所在车道前方关键障碍物、主车所在车道后方关键障碍物、主车相邻右侧车道前方关键障碍物、主车相邻右侧车道后方关键障碍物信息，当对应位置关键障碍物不存在时则令矩阵obj对应的列为零即可，则第一类行车场景下最终输出的关键障碍物矩阵obj＝[obj
l+ obj
l
- obj
h+ obj
h
- obj
r+ obj
r-]；
[0034]
2)当处于第二类行车场景下，需要分别对主车所在车道以及主车相邻右侧车道的关键障碍物进行筛选，主车所在车道左侧为不可行使区域，首先提取矩阵obj最后一行为1的列重新组成两个新的矩阵obj
h+
和obj
h-，obj
h+
是由矩阵obj最后一行为1且第2行障碍物纵向坐标为正的列组成的，代表主车所在车道内位于主车前方的障碍物，obj
h-是由矩阵obj最后一行为1且第2行障碍物纵向坐标为负的列组成的，代表主车所在车道内位于主车后方的障碍物，矩阵obj
h+
和obj
h-如式(6)所示：
[0035][0036]
其中n
h+
为矩阵obj最后一行为1且第2行障碍物纵向坐标为正的列的个数，n
h-为矩阵obj最后一行为1且第2行障碍物纵向坐标为负的列的个数；主车相邻左侧车道关键障碍物分别选取前方距离主车最近的障碍物和后方距离主车最近的障碍物即可，主车所在车道前方距离主车最近的障碍物信息为obj
h+
，obj
h+
即为矩阵obj
h+
中第2行最小元素所对应的列，当obj
h+
为空矩阵时obj
h+
中所有元素为0，代表主车所在车道前方不存在障碍物；主车所
在车道后方距离主车最近的障碍物信息为obj
h-，obj
h-即为矩阵obj
h-中第2行障碍物纵向坐标最大值所对应的列，当obj
h-为空矩阵时obj
h-中所有元素为0，代表主车所在车道后方不存在障碍物；
[0037]
主车相邻右侧车道关键障碍物方法：提取矩阵obj最后一行为3的列重新组成两个新的矩阵obj
r+
和obj
r-，obj
r+
是由矩阵obj最后一行为3且第2行障碍物纵向坐标为正的列组成的，代表主车相邻右侧车道内位于主车前方的障碍物，obj
r-是由矩阵obj最后一行为3且第2行障碍物纵向坐标为负的列组成的，代表主车相邻右侧车道内位于主车后方的障碍物，矩阵obj
r+
和obj
r-如式(7)所示：
[0038][0039]
其中n
r+
为矩阵obj最后一行为3且第2行障碍物纵向坐标为正的列的个数，n
r-为矩阵obj最后一行为3且第2行障碍物纵向坐标为负的列的个数；主车相邻右侧车道关键障碍物分别选取前方距离主车最近的障碍物和后方距离主车最近的障碍物即可，主车所在车道前方距离主车最近的障碍物信息为obj
r+
，obj
r+
即为矩阵obj
r+
中第2行最小元素所对应的列，当obj
r+
为空矩阵时obj
r+
中所有元素为0，代表主车相邻右侧车道前方不存在障碍物；主车相邻右侧车道后方距离主车最近的障碍物信息为obj
r-，obj
r-即为矩阵obj
r-中第2行障碍物纵向坐标最大值所对应的列，当obj
r-为空矩阵时obj
r-中所有元素为0，代表主车相邻右侧车道后方不存在障碍物；则第二类行车场景下最终输出的关键障碍物矩阵obj＝[0 0 obj
h+ obj
h
- obj
r+ obj
r-]；
[0040]
3)当处于第三类行车场景下，需要分别对主车所在车道以及主车相邻左车道的关键障碍物进行筛选，首先提取主车相邻左侧车道障碍物，即提取矩阵obj最后一行为2的列重新组成两个新的矩阵obj
l+
和obj
l-，obj
l+
是由矩阵obj最后一行为2且第2行障碍物纵向坐标为正的列组成的，代表主车相邻左侧车道内位于主车前方的障碍物，obj-是由矩阵obj最后一行为2且第2行障碍物纵向坐标为负的列组成的，代表主车相邻左侧车道内位于主车后方的障碍物，矩阵obj
l+
和obj
l-如式(8)所示：
[0041][0042]
其中n
l+
为矩阵obj最后一行为2且第2行障碍物纵向坐标为正的列的个数，n
l-为矩阵obj最后一行为2且第2行障碍物纵向坐标为负的列的个数；主车相邻左侧车道关键障碍
物分别选取前方距离主车最近的障碍物和后方距离主车最近的障碍物即可，主车相邻左侧车道前方距离主车最近的障碍物信息为obj
l+
，obj
l+
即为矩阵obj
l+
中第2行障碍物纵向坐标最小值所对应的列，当obj
l+
为空矩阵时obj
l+
中所有元素为0，代表主车相邻左侧车道前方不存在障碍物；主车相邻左侧车道后方距离主车最近的障碍物信息为obj
l-，obj
l-即为矩阵obj
l-中第2行障碍物纵向坐标最大值所对应的列，当obj
l-为空矩阵时obj
l-中所有元素为0，代表主车相邻左侧车道后方不存在障碍物；
[0043]
主车所在车道关键障碍物方法：提取矩阵obj最后一行为1的列重新组成两个新的矩阵obj
h+
和obj
h-，obj
h+
是由矩阵obj最后一行为1且第2行障碍物纵向坐标为正的列组成的，代表主车所在车道内位于主车前方的障碍物，obj
h-是由矩阵obj最后一行为1且第2行障碍物纵向坐标为负的列组成的，代表主车所在车道内位于主车后方的障碍物，矩阵obj
h+
和obj
h-如式(9)所示：
[0044][0045]
其中n
h+
为矩阵obj最后一行为1且第2行障碍物纵向坐标为正的列的个数，n
h-为矩阵obj最后一行为1且第2行障碍物纵向坐标为负的列的个数；主车相邻左侧车道关键障碍物分别选取前方距离主车最近的障碍物和后方距离主车最近的障碍物即可，主车所在车道前方距离主车最近的障碍物信息为obj
h+
，obj
h+
即为矩阵obj
h+
中第2行最小元素所对应的列，当obj
h+
为空矩阵时obj
h+
中所有元素为0，代表主车所在车道前方不存在障碍物；主车所在车道后方距离主车最近的障碍物信息为obj
h-，obj
h-即为矩阵obj
h-中第2行障碍物纵向坐标最大值所对应的列，当obj
h-为空矩阵时obj
h-中所有元素为0，代表主车所在车道后方不存在障碍物；则第三类行车场景下最终输出的关键障碍物矩阵obj＝[obj
l+ obj
l
- obj
h+ obj
h
- 0 0]；
[0046]
4)当处于第四类行车场景下，只需要对主车所在车道关键障碍物进行筛选，首先提取矩阵obj最后一行为1的列重新组成两个新的矩阵obj
h+
和obj
h-，obj
h+
是由矩阵obj最后一行为1且第2行障碍物纵向坐标为正的列组成的，代表主车所在车道内位于主车前方的障碍物，obj
h-是由矩阵obj最后一行为1且第2行障碍物纵向坐标为负的列组成的，代表主车所在车道内位于主车后方的障碍物，矩阵obj
h+
和obj
h-如式(10)所示：
[0047]
[0048]
其中n
h+
为矩阵obj最后一行为1且第2行障碍物纵向坐标为正的列的个数，n
h-为矩阵obj最后一行为1且第2行障碍物纵向坐标为负的列的个数；主车相邻左侧车道关键障碍物分别选取前方距离主车最近的障碍物和后方距离主车最近的障碍物即可，主车所在车道前方距离主车最近的障碍物信息为obj
h+
，obj
h+
即为矩阵obj
h+
中第2行最小元素所对应的列，当obj
h+
为空矩阵时obj
h+
中所有元素为0，代表主车所在车道前方不存在障碍物；主车所在车道后方距离主车最近的障碍物信息为obj
h-，obj
h-即为矩阵obj
h-中第2行障碍物纵向坐标最大值所对应的列，当obj
h-为空矩阵时obj
h-中所有元素为0，代表主车所在车道后方不存在障碍物；则第四类行车场景下最终输出的关键障碍物矩阵obj＝[0 0 obj
h+ obj
h
- 0 0]；
[0049]
步骤五、输出关键障碍物信息：
[0050]
将步骤四中获取的关键障碍物矩阵obj信息输出，即完成了关键障碍物的信息的筛选和输出。
[0051]
与现有技术相比，本发明的有益效果为：
[0052]
本发明提供了一种结构化道路下关键障碍物筛选方法，本方法提出了基于目标级传感融合结果下的关键障碍物筛选，所以可以为智能驾驶辅助系统以及无人驾驶系统的开发提供关键障碍物信息；因为本方法设计了基于主车左右相邻车道线类型的场景分类方法，所以能够实现结构化道路下主车行驶场景的分割；因为本方法提出了先将障碍物在车道定位，然后再对比障碍与主车相对纵向距离的方式进行关键障碍物筛选的方法，所以能够遍历所有传感获取的障碍物信息最终实现关键障碍物信息准确筛选。
附图说明
[0053]
下面结合附图对发明作进一步的说明：
[0054]
图1为本发明所述的结构化道路下关键障碍物筛选方法的流程图；
[0055]
图2为本发明所述的车辆坐标系示意图；
[0056]
图3为本发明所述的行车场景分类示意图；
具体实施方式
[0057]
下面结合附图对本发明作详细的描述：
[0058]
本发明提供了一种结构化道路下关键障碍物筛选方法，基于行车场景判断模式，判断出主车所处的行车场景，基于传感模块输出的障碍物列表，筛选出对主车行驶影响最为显著的关键障碍物，关键障碍物即是指每个车道距离主车最近的障碍物，方便主车驾驶辅助策略和主车无人驾驶系统决策与控制策略开发，本方法具体步骤如下：
[0059]
步骤一、所需传感信息的定义：
[0060]
行车场景的判断以及障碍物的筛选都是在结构化道路环境，结构化道路下车道线清晰可见，同时关键障碍物筛选都在车辆坐标系下进行，因此定义车辆坐标系，车辆坐标系的坐标原点在主车质心o处，x轴朝向主车前进的方向，y轴的正方向为x轴逆时针旋转90度方向，如图2所示；
[0061]
关键障碍物筛选基于单一或者多传感融合后的目标级障碍物传感信息以及车道线信息，因此定义关键障碍物筛选所需要的传感信息，首先定义传感模块输出的障碍物总
数量为n，每个障碍物的相对主车的坐标(x
i
,y
i
),i＝1,
…
,n，其中i代表障碍物的编号，同时将所有障碍物的信息赋值到一个m
×
n矩阵里面，矩阵的前三行依次是障碍物的编号i、障碍物相对主车的纵向坐标x
i
和障碍物相对主车的侧向坐标y
i
，矩阵的第四行至矩阵的第m-1行可以根据用户的定义用于输出障碍物的其他信息，矩阵的最后一行即第m行留作障碍物在车道定位的标志位，当障碍物未定位之前矩阵的最后一行赋值为0，则障碍物矩阵命名为obj，矩阵的每一列代表的是所对应障碍物的全部信息,具体如下所示：
[0062][0063]
定义主车获取的车道线信息，主车能够获取的车道线信息有四条，分别为左侧两条，右侧两条，四条车道线标识从左至右依次被命名为ll,l,r，rr，主车获取的车道线信息是三次插值函数，车道线获取的插值参数信息为d
j
,c
j
,b
j
,a
j
,j＝ll,l,r,rr，其中下标j代表所属标识车道线的参数，因此可以获取车道线方程如式(2)所示：
[0064][0065]
式中x
j
为车道线在车辆坐标系下的纵向坐标，y
j
为车道线在车辆坐标系下的侧向坐标；
[0066]
定义传感模块获取的每条车道线类型为s
j
,j＝ll,l,r，rr，其中s
j
取1或0，当车道线类型为虚线时定义s
j
＝1，当车道线类型为非虚线时s
j
＝0；
[0067]
步骤二、主车行驶场景判断：
[0068]
结构化道路下的行车场景可以划分为四类，如图3所示，第一类行车场景为三车道行车场景且主车行驶在中间车道，主车车道以及左右相邻车道均可行驶，即使车道数量大于三，但是除了主车所在车道以及主车所在车道相邻车道外其余车道的车辆对主车影响较小，不予考虑，因此该第一类行车场景下每个车道内距离主车最近的关键障碍物需要筛选；第二类行车场景为双车道场景且主车行驶在左侧车道，只需要筛选主车所在车道以及主车右侧相邻车道关键障碍物，主车所在车道左侧认为不可行驶区域；第三类行车场景为双车道场景且主车行驶在右侧车道，只需要筛选主车所在车道以及主车相邻左侧车道关键障碍物，主车所在车道右侧为不可行驶区域；第四类为单车道行车场景，只需要筛选本车道关键障碍物即可，主车所在车道左右均为不可行使区域；
[0069]
进行关键障碍物筛选之前首先需要判断主车所行驶的场景，主车行驶场景的判断主要依据紧挨着主车左右两侧的两条车道线类型，当左右车道线均为虚线时即s
l
＝1,s
r
＝1，主车的行驶场景为上述第一类行车场景；仅右侧车道线为虚线时即s
l
＝0,s
r
＝1，主车的行驶场景为上述第二类行车场景；仅左侧车道线为虚线时即s
l
＝1,s
r
＝0，主车的行驶场景为上述第三类行车场景；当左右两侧均不为虚线时即s
l
＝0,s
r
＝0，主车的行驶场景为上述第四类行车场景；
[0070]
步骤三、判定障碍物所在车道：
[0071]
在判断行车场景的基础上，将所有障碍物在车道上进行定位，对障碍物在车道上进行定位主要通过对比障碍物与车道之间的相对位置关系，首先定义不同场景下的车道编号，主车所在车道编号为1，主车左侧相邻车道为2,主车右侧相邻车道为3，当障碍物不属于主车车道以及主车左右相邻两个车道内时认为障碍物所属车道编号为0，代表此障碍物不予考虑，判定障碍物所在车道后将矩阵obj最后一行赋值为所对应的车道编号值；
[0072]
1)当处于第一类行车场景下，需要对n个障碍物分别进行所在车道判定，编号为i的障碍物，将障碍物纵向坐标x
i
带入式(2)，即可获取从左到右四条车道线在此障碍物纵向坐标处的位移，分别为y
lli
,y
li
,y
ri
,y
rri
，当y
lli
≥y
i
＞y
li
，则矩阵obj第i列第m行赋值为2；当y
li
≥y
i
＞y
ri
，则矩阵obj第i列第m行赋值为1；当y
ri
≥y
i
≥y
rri
，则矩阵obj第i列第m行赋值为3；当y
rri
＞y
i
或y
i
＞y
lli
，则矩阵obj第i列第m行赋值为0；按照上述方式依次对n个障碍物进行所属车道判定；
[0073]
2)当障碍物处于第二类行车场景下，需要对n个障碍物分别进行所在车道判定，由于第二类行车场景下主车相邻左侧车道线左侧均为不可行使区域，因此只需要判定障碍物是否属于主车车道或主车车道相邻右侧车道即可，编号为i的障碍物，将障碍物纵向坐标x
i
带入式(2)，获取此障碍物纵向坐标处左侧相邻主车的车道线侧向位移以及右侧两条车道线在障碍物纵向坐标处侧向位移即可，分别为y
li
,y
ri
,y
rri
，当y
li
≥y
i
＞y
ri
，则矩阵obj第i列第m行赋值为1；当y
ri
≥y
i
≥y
rri
，则矩阵obj第i列第m行赋值为3；当y
rri
＞y
i
或y
i
＞y
li
，则矩阵obj第i列第m行赋值为0；按照上述方式依次对n个障碍物进行所属车道判定；
[0074]
3)当障碍物处于第三行车类场景下，需要对n个障碍物分别进行所在车道判定，由于第三类行车场景下主车相邻右侧车道线右侧均为不可行使区域，因此只需要判定障碍物是否属于主车车道或主车车道相邻左侧车道即可，编号为i的障碍物，将障碍物纵向坐标x
i
带入式(2)，获取此障碍物纵向坐标处右侧相邻主车的车道线侧向位移以及左侧两条车道线在障碍物纵向坐标处侧向位移即可，分别为y
lli
,y
li
,y
ri
，当y
lli
≥y
i
＞y
li
，则矩阵obj第i列第m行赋值为2；当y
li
≥y
i
≥y
ri
，则矩阵obj第i列第m行赋值为1；当y
ri
＞y
i
或y
i
＞y
lli
，则矩阵obj第i列第m行赋值为0；按照上述方式依次对n个障碍物进行所属车道判定；
[0075]
4)当障碍物处于第四类行车场景下，需要对n个障碍物分别进行所在车道判定，，由于第四类行车场景下主车相邻右侧车道线右侧与主车相邻左侧车道线左侧均为不可行使区域，因此只需要判定障碍物是否属于主车车道即可，编号为i的障碍物，将障碍物纵向坐标x
i
带入式(2)，获取此障碍物纵向坐标处右侧相邻主车的车道线侧向位移以及左侧相邻主车的车道线侧向位移即可，分别为y
li
,y
ri
，当y
li
≥y
i
≥y
ri
，则矩阵obj第i列第m行赋值为1；当y
ri
＞y
i
或y
i
＞y
li
，则矩阵obj第i列第m行赋值为0；按照上述方式依次对n个障碍物进行所属车道判定；
[0076]
步骤四、关键障碍物筛选：
[0077]
关键障碍物能够直接影响主车驾驶辅助策略以及主车无人驾驶系统决策与控制策略开发，步骤四对障碍物所在车道进行判定，在步骤四基础上进行关键障碍物的筛选；
[0078]
1)当处于第一类行车场景下，需要分别对主车所在车道以及主车相邻左右车道的关键障碍物进行筛选，首先提取主车相邻左侧车道障碍物，即提取矩阵obj最后一行为2的列重新组成两个新的矩阵obj
l+
和obj
l-，obj
l+
是由矩阵obj最后一行为2且第2行障碍物纵向坐标为正的列组成的，代表主车相邻左侧车道内位于主车前方的障碍物，obj-是由矩阵obj
最后一行为2且第2行障碍物纵向坐标为负的列组成的，代表主车相邻左侧车道内位于主车后方的障碍物，矩阵obj
l+
和obj
l-如式(3)所示：
[0079][0080]
其中n
l+
为矩阵obj最后一行为2且第2行障碍物纵向坐标为正的列的个数，n
l-为矩阵obj最后一行为2且第2行障碍物纵向坐标为负的列的个数；主车相邻左侧车道关键障碍物分别选取前方距离主车最近的障碍物和后方距离主车最近的障碍物即可，主车相邻左侧车道前方距离主车最近的障碍物信息为obj
l+
，obj
l+
即为矩阵obj
l+
中第2行障碍物纵向坐标最小值所对应的列，当obj
l+
为空矩阵时obj
l+
中所有元素为0，代表主车相邻左侧车道前方不存在障碍物；主车相邻左侧车道后方距离主车最近的障碍物信息为obj
l-，obj
l-即为矩阵obj
l-中第2行障碍物纵向坐标最大值所对应的列，当obj
l-为空矩阵时obj
l-中所有元素为0，代表主车相邻左侧车道后方不存在障碍物；
[0081]
主车所在车道关键障碍物方法：提取矩阵obj最后一行为1的列重新组成两个新的矩阵obj
h+
和obj
h-，obj
h+
是由矩阵obj最后一行为1且第2行障碍物纵向坐标为正的列组成的，代表主车所在车道内位于主车前方的障碍物，obj
h-是由矩阵obj最后一行为1且第2行障碍物纵向坐标为负的列组成的，代表主车所在车道内位于主车后方的障碍物，矩阵obj
h+
和obj
h-如式(4)所示：
[0082]
其中n
h+
为矩阵obj最后一行为1且第2行障碍物纵向坐标为正的列的个数，n
h-为矩阵obj最后一行为1且第2行障碍物纵向坐标为负的列的个数；主车相邻左侧车道关键障碍物分别选取前方距离主车最近的障碍物和后方距离主车最近的障碍物即可，主车所在车道前方距离主车最近的障碍物信息为obj
h+
，obj
h+
即为矩阵obj
h+
中第2行最小元素所对应的列，当obj
h+
为空矩阵时obj
h+
中所有元素为0，代表主车所在车道前方不存在障碍物；主车所在车道后方距离主车最近的障碍物信息为obj
h-，obj
h-即为矩阵obj
h-中第2行障碍物纵向坐标最大值所对应的列，当obj
h-为空矩阵时obj
h-中所有元素为0，代表主车所在车道后方不存在障碍物；
[0083]
主车相邻右侧车道关键障碍物方法：提取矩阵obj最后一行为3的列重新组成两个新的矩阵obj
r+
和obj
r-，obj
r+
是由矩阵obj最后一行为3且第2行障碍物纵向坐标为正的列组
成的，代表主车相邻右侧车道内位于主车前方的障碍物，obj
r-是由矩阵obj最后一行为3且第2行障碍物纵向坐标为负的列组成的，代表主车相邻右侧车道内位于主车后方的障碍物，矩阵obj
r+
和obj
r-如式(5)所示：
[0084][0085]
其中n
r+
为矩阵obj最后一行为3且第2行障碍物纵向坐标为正的列的个数，n
r-为矩阵obj最后一行为3且第2行障碍物纵向坐标为负的列的个数；主车相邻右侧车道关键障碍物分别选取前方距离主车最近的障碍物和后方距离主车最近的障碍物即可，主车所在车道前方距离主车最近的障碍物信息为obj
r+
，obj
r+
即为矩阵obj
r+
中第2行最小元素所对应的列，当obj
r+
为空矩阵时obj
r+
中所有元素为0，代表主车相邻右侧车道前方不存在障碍物；主车相邻右侧车道后方距离主车最近的障碍物信息为obj
r-，obj
r-即为矩阵obj
r-中第2行障碍物纵向坐标最大值所对应的列，当obj
r-为空矩阵时obj
r-中所有元素为0，代表主车相邻右侧车道后方不存在障碍物；定义关键障碍物矩阵为obj，其从左至右六列分别表示主车相邻左侧车道前方关键障碍物、主车相邻左侧车道后方关键障碍物、主车所在车道前方关键障碍物、主车所在车道后方关键障碍物、主车相邻右侧车道前方关键障碍物、主车相邻右侧车道后方关键障碍物信息，当对应位置关键障碍物不存在时则令矩阵obj对应的列为零即可，则第一类行车场景下最终输出的关键障碍物矩阵obj＝[obj
l+ obj
l
- obj
h+ obj
h
- obj
r+ obj
r-]；
[0086]
2)当处于第二类行车场景下，需要分别对主车所在车道以及主车相邻右侧车道的关键障碍物进行筛选，主车所在车道左侧为不可行使区域，首先提取矩阵obj最后一行为1的列重新组成两个新的矩阵obj
h+
和obj
h-，obj
h+
是由矩阵obj最后一行为1且第2行障碍物纵向坐标为正的列组成的，代表主车所在车道内位于主车前方的障碍物，obj
h-是由矩阵obj最后一行为1且第2行障碍物纵向坐标为负的列组成的，代表主车所在车道内位于主车后方的障碍物，矩阵obj
h+
和obj
h-如式(6)所示：
[0087][0088]
其中n
h+
为矩阵obj最后一行为1且第2行障碍物纵向坐标为正的列的个数，n
h-为矩阵obj最后一行为1且第2行障碍物纵向坐标为负的列的个数；主车相邻左侧车道关键障碍物分别选取前方距离主车最近的障碍物和后方距离主车最近的障碍物即可，主车所在车道
前方距离主车最近的障碍物信息为obj
h+
，obj
h+
即为矩阵obj
h+
中第2行最小元素所对应的列，当obj
h+
为空矩阵时obj
h+
中所有元素为0，代表主车所在车道前方不存在障碍物；主车所在车道后方距离主车最近的障碍物信息为obj
h-，obj
h-即为矩阵obj
h-中第2行障碍物纵向坐标最大值所对应的列，当obj
h-为空矩阵时obj
h-中所有元素为0，代表主车所在车道后方不存在障碍物；
[0089]
主车相邻右侧车道关键障碍物方法：提取矩阵obj最后一行为3的列重新组成两个新的矩阵obj
r+
和obj
r-，obj
r+
是由矩阵obj最后一行为3且第2行障碍物纵向坐标为正的列组成的，代表主车相邻右侧车道内位于主车前方的障碍物，obj
r-是由矩阵obj最后一行为3且第2行障碍物纵向坐标为负的列组成的，代表主车相邻右侧车道内位于主车后方的障碍物，矩阵obj
r+
和obj
r-如式(7)所示：
[0090][0091]
其中n
r+
为矩阵obj最后一行为3且第2行障碍物纵向坐标为正的列的个数，n
r-为矩阵obj最后一行为3且第2行障碍物纵向坐标为负的列的个数；主车相邻右侧车道关键障碍物分别选取前方距离主车最近的障碍物和后方距离主车最近的障碍物即可，主车所在车道前方距离主车最近的障碍物信息为obj
r+
，obj
r+
即为矩阵obj
r+
中第2行最小元素所对应的列，当obj
r+
为空矩阵时obj
r+
中所有元素为0，代表主车相邻右侧车道前方不存在障碍物；主车相邻右侧车道后方距离主车最近的障碍物信息为obj
r-，obj
r-即为矩阵obj
r-中第2行障碍物纵向坐标最大值所对应的列，当obj
r-为空矩阵时obj
r-中所有元素为0，代表主车相邻右侧车道后方不存在障碍物；则第二类行车场景下最终输出的关键障碍物矩阵obj＝[0 0 obj
h+ obj
h
- obj
r+ obj
r-]；
[0092]
3)当处于第三类行车场景下，需要分别对主车所在车道以及主车相邻左车道的关键障碍物进行筛选，首先提取主车相邻左侧车道障碍物，即提取矩阵obj最后一行为2的列重新组成两个新的矩阵obj
l+
和obj
l-，obj
l+
是由矩阵obj最后一行为2且第2行障碍物纵向坐标为正的列组成的，代表主车相邻左侧车道内位于主车前方的障碍物，obj-是由矩阵obj最后一行为2且第2行障碍物纵向坐标为负的列组成的，代表主车相邻左侧车道内位于主车后方的障碍物，矩阵obj
l+
和obj
l-如式(8)所示：
[0093]
[0094]
其中n
l+
为矩阵obj最后一行为2且第2行障碍物纵向坐标为正的列的个数，n
l-为矩阵obj最后一行为2且第2行障碍物纵向坐标为负的列的个数；主车相邻左侧车道关键障碍物分别选取前方距离主车最近的障碍物和后方距离主车最近的障碍物即可，主车相邻左侧车道前方距离主车最近的障碍物信息为obj
l+
，obj
l+
即为矩阵obj
l+
中第2行障碍物纵向坐标最小值所对应的列，当obj
l+
为空矩阵时obj
l+
中所有元素为0，代表主车相邻左侧车道前方不存在障碍物；主车相邻左侧车道后方距离主车最近的障碍物信息为obj
l-，obj
l-即为矩阵obj
l-中第2行障碍物纵向坐标最大值所对应的列，当obj
l-为空矩阵时obj
l-中所有元素为0，代表主车相邻左侧车道后方不存在障碍物；
[0095]
主车所在车道关键障碍物方法：提取矩阵obj最后一行为1的列重新组成两个新的矩阵obj
h+
和obj
h-，obj
h+
是由矩阵obj最后一行为1且第2行障碍物纵向坐标为正的列组成的，代表主车所在车道内位于主车前方的障碍物，obj
h-是由矩阵obj最后一行为1且第2行障碍物纵向坐标为负的列组成的，代表主车所在车道内位于主车后方的障碍物，矩阵obj
h+
和obj
h-如式(9)所示：
[0096][0097]
其中n
h+
为矩阵obj最后一行为1且第2行障碍物纵向坐标为正的列的个数，n
h-为矩阵obj最后一行为1且第2行障碍物纵向坐标为负的列的个数；主车相邻左侧车道关键障碍物分别选取前方距离主车最近的障碍物和后方距离主车最近的障碍物即可，主车所在车道前方距离主车最近的障碍物信息为obj
h+
，obj
h+
即为矩阵obj
h+
中第2行最小元素所对应的列，当obj
h+
为空矩阵时obj
h+
中所有元素为0，代表主车所在车道前方不存在障碍物；主车所在车道后方距离主车最近的障碍物信息为obj
h-，obj
h-即为矩阵obj
h-中第2行障碍物纵向坐标最大值所对应的列，当obj
h-为空矩阵时obj
h-中所有元素为0，代表主车所在车道后方不存在障碍物；则第三类行车场景下最终输出的关键障碍物矩阵obj＝[obj
l+ obj
l
- obj
h+ obj
h
- 0 0]；
[0098]
4)当处于第四类行车场景下，只需要对主车所在车道关键障碍物进行筛选，首先提取矩阵obj最后一行为1的列重新组成两个新的矩阵obj
h+
和obj
h-，obj
h+
是由矩阵obj最后一行为1且第2行障碍物纵向坐标为正的列组成的，代表主车所在车道内位于主车前方的障碍物，obj
h-是由矩阵obj最后一行为1且第2行障碍物纵向坐标为负的列组成的，代表主车所在车道内位于主车后方的障碍物，矩阵obj
h+
和obj
h-如式(10)所示：
[0099][0100]
其中n
h+
为矩阵obj最后一行为1且第2行障碍物纵向坐标为正的列的个数，n
h-为矩阵obj最后一行为1且第2行障碍物纵向坐标为负的列的个数；主车相邻左侧车道关键障碍物分别选取前方距离主车最近的障碍物和后方距离主车最近的障碍物即可，主车所在车道前方距离主车最近的障碍物信息为obj
h+
，obj
h+
即为矩阵obj
h+
中第2行最小元素所对应的列，当obj
h+
为空矩阵时obj
h+
中所有元素为0，代表主车所在车道前方不存在障碍物；主车所在车道后方距离主车最近的障碍物信息为obj
h-，obj
h-即为矩阵obj
h-中第2行障碍物纵向坐标最大值所对应的列，当obj
h-为空矩阵时obj
h-中所有元素为0，代表主车所在车道后方不存在障碍物；则第四类行车场景下最终输出的关键障碍物矩阵obj＝[0 0 obj
h+ obj
h
- 0 0]；
[0101]
步骤五、输出关键障碍物信息：
[0102]
将步骤四中获取的关键障碍物矩阵obj信息输出，即完成了关键障碍物的信息的筛选和输出。

起点商标作为专业知识产权交易平台，可以帮助大家解决很多问题，如果大家想要了解更多知产交易信息请点击 【在线咨询】或添加微信 【19522093243】与客服一对一沟通，为大家解决相关问题。

此文章来源于网络,如有侵权,请联系删除