[发明专利]一种结构化道路下关键障碍物筛选方法

权利要求书

1.一种结构化道路下关键障碍物筛选方法，基于行车场景判断模式，判断出主车所处的行车场景，基于传感模块输出的障碍物列表，筛选出对主车行驶影响最为显著的关键障碍物，关键障碍物即是指每个车道距离主车最近的障碍物，方便主车驾驶辅助策略和主车无人驾驶系统决策与控制策略开发，其特征在于，本方法具体步骤如下：

步骤一、所需传感信息的定义：

行车场景的判断以及障碍物的筛选都是在结构化道路环境，结构化道路下车道线清晰可见，同时关键障碍物筛选都在车辆坐标系下进行，因此定义车辆坐标系，车辆坐标系的坐标原点在主车质心o处，x轴朝向主车前进的方向，y轴的正方向为x轴逆时针旋转90度方向；

关键障碍物筛选基于单一或者多传感融合后的目标级障碍物传感信息以及车道线信息，因此定义关键障碍物筛选所需要的传感信息，首先定义传感模块输出的障碍物的总数量为n，每个障碍物相对主车的坐标(x_i,y_i),i＝1,…,n，其中i代表障碍物的编号，同时将所有障碍物的信息赋值到一个m×n矩阵里面，矩阵的前三行依次是障碍物的编号i、障碍物相对主车的纵向坐标x_i和障碍物相对主车的侧向坐标y_i，矩阵的第四行至矩阵的第m-1行可以根据用户的定义用于输出障碍物的其他信息，矩阵的最后一行即第m行留作障碍物在车道定位的标志位，当障碍物未定位之前矩阵的最后一行赋值为0，则障碍物矩阵命名为OBJ，矩阵的每一列代表的是所对应障碍物的全部信息,具体如下所示：

定义主车获取的车道线信息，主车能够获取的车道线信息有四条，分别为左侧两条，右侧两条，四条车道线标识从左至右依次被命名为ll,l,r,rr，主车获取的车道线信息是三次插值函数，车道线获取的插值参数信息为d_j,c_j,b_j,a_j,j＝ll,l,r,rr，其中下标j代表所属标识车道线的参数，因此可以获取车道线方程如式(2)所示：

式中x_j为车道线在车辆坐标系下的纵向坐标，y_j为车道线在车辆坐标系下的侧向坐标；

定义传感模块获取的每条车道线类型为s_j,j＝ll,l,r,rr，其中s_j取1或0，当车道线类型为虚线时定义s_j＝1，当车道线类型为非虚线时s_j＝0；

步骤二、主车行驶场景判断：

结构化道路下的行车场景可以划分为四类，第一类行车场景为三车道行车场景且主车行驶在中间车道，主车车道以及左右相邻车道均可行驶，即使车道数量大于三，但是除了主车所在车道以及主车所在车道相邻车道外其余车道的车辆对主车影响较小，不予考虑，因此该第一类行车场景下每个车道内距离主车最近的关键障碍物需要筛选；第二类行车场景为双车道场景且主车行驶在左侧车道，只需要筛选主车所在车道以及主车右侧相邻车道关键障碍物，主车所在车道左侧认为不可行驶区域；第三类行车场景为双车道场景且主车行驶在右侧车道，只需要筛选主车所在车道以及主车相邻左侧车道关键障碍物，主车所在车道右侧为不可行驶区域；第四类为单车道行车场景，只需要筛选本车道关键障碍物即可，主车所在车道左右均为不可行使区域；

进行关键障碍物筛选之前首先需要判断主车所行驶的场景，主车行驶场景的判断主要依据紧挨着主车左右两侧的两条车道线类型，当左右车道线均为虚线时即s_l＝1,s_r＝1，主车的行驶场景为上述第一类行车场景；仅右侧车道线为虚线时即s_l＝0,s_r＝1，主车的行驶场景为上述第二类行车场景；仅左侧车道线为虚线时即s_l＝1,s_r＝0，主车的行驶场景为上述第三类行车场景；当左右两侧均不为虚线时即s_l＝0,s_r＝0，主车的行驶场景为上述第四类行车场景；

步骤三、判定障碍物所在车道：

在判断行车场景的基础上，将所有障碍物在车道上进行定位，对障碍物在车道上进行定位主要通过对比障碍物与车道之间的相对位置关系，首先定义不同场景下的车道编号，主车所在车道编号为1，主车左侧相邻车道为2,主车右侧相邻车道为3，当障碍物不属于主车车道以及主车左右相邻两个车道内时认为障碍物所属车道编号为0，代表此障碍物不予考虑，判定障碍物所在车道后将矩阵OBJ最后一行赋值为所对应的车道编号值；

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个障碍物进行所属车道判定；

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个障碍物进行所属车道判定；

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个障碍物进行所属车道判定；

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个障碍物进行所属车道判定；

步骤四、关键障碍物筛选：

关键障碍物能够直接影响主车驾驶辅助策略以及主车无人驾驶系统决策与控制策略开发，步骤四对障碍物所在车道进行判定，在步骤四基础上进行关键障碍物的筛选；

1)当处于第一类行车场景下，需要分别对主车所在车道以及主车相邻左右车道的关键障碍物进行筛选，首先提取主车相邻左侧车道障碍物，即提取矩阵OBJ最后一行为2的列重新组成两个新的矩阵OBJ_l+和OBJ_l-，OBJ_l+是由矩阵OBJ最后一行为2且第2行障碍物纵向坐标为正的列组成的，代表主车相邻左侧车道内位于主车前方的障碍物，OBJ_-是由矩阵OBJ最后一行为2且第2行障碍物纵向坐标为负的列组成的，代表主车相邻左侧车道内位于主车后方的障碍物，矩阵OBJ_l+和OBJ_l-如式(3)所示：

其中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，代表主车相邻左侧车道后方不存在障碍物；

主车所在车道关键障碍物方法：提取矩阵OBJ最后一行为1的列重新组成两个新的矩阵OBJ_h+和OBJ_h-，OBJ_h+是由矩阵OBJ最后一行为1且第2行障碍物纵向坐标为正的列组成的，代表主车所在车道内位于主车前方的障碍物，OBJ_h-是由矩阵OBJ最后一行为1且第2行障碍物纵向坐标为负的列组成的，代表主车所在车道内位于主车后方的障碍物，矩阵OBJ_h+和OBJ_h-如式(4)所示：

其中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最后一行为3的列重新组成两个新的矩阵OBJ_r+和OBJ_r-，OBJ_r+是由矩阵OBJ最后一行为3且第2行障碍物纵向坐标为正的列组成的，代表主车相邻右侧车道内位于主车前方的障碍物，OBJ_r-是由矩阵OBJ最后一行为3且第2行障碍物纵向坐标为负的列组成的，代表主车相邻右侧车道内位于主车后方的障碍物，矩阵OBJ_r+和OBJ_r-如式(5)所示：

其中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-]；

2)当处于第二类行车场景下，需要分别对主车所在车道以及主车相邻右侧车道的关键障碍物进行筛选，主车所在车道左侧为不可行使区域，首先提取矩阵OBJ最后一行为1的列重新组成两个新的矩阵OBJ_h+和OBJ_h-，OBJ_h+是由矩阵OBJ最后一行为1且第2行障碍物纵向坐标为正的列组成的，代表主车所在车道内位于主车前方的障碍物，OBJ_h-是由矩阵OBJ最后一行为1且第2行障碍物纵向坐标为负的列组成的，代表主车所在车道内位于主车后方的障碍物，矩阵OBJ_h+和OBJ_h-如式(6)所示：

其中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最后一行为3的列重新组成两个新的矩阵OBJ_r+和OBJ_r-，OBJ_r+是由矩阵OBJ最后一行为3且第2行障碍物纵向坐标为正的列组成的，代表主车相邻右侧车道内位于主车前方的障碍物，OBJ_r-是由矩阵OBJ最后一行为3且第2行障碍物纵向坐标为负的列组成的，代表主车相邻右侧车道内位于主车后方的障碍物，矩阵OBJ_r+和OBJ_r-如式(7)所示：

其中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-]；

3)当处于第三类行车场景下，需要分别对主车所在车道以及主车相邻左车道的关键障碍物进行筛选，首先提取主车相邻左侧车道障碍物，即提取矩阵OBJ最后一行为2的列重新组成两个新的矩阵OBJ_l+和OBJ_l-，OBJ_l+是由矩阵OBJ最后一行为2且第2行障碍物纵向坐标为正的列组成的，代表主车相邻左侧车道内位于主车前方的障碍物，OBJ_-是由矩阵OBJ最后一行为2且第2行障碍物纵向坐标为负的列组成的，代表主车相邻左侧车道内位于主车后方的障碍物，矩阵OBJ_l+和OBJ_l-如式(8)所示：

其中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，代表主车相邻左侧车道后方不存在障碍物；

主车所在车道关键障碍物方法：提取矩阵OBJ最后一行为1的列重新组成两个新的矩阵OBJ_h+和OBJ_h-，OBJ_h+是由矩阵OBJ最后一行为1且第2行障碍物纵向坐标为正的列组成的，代表主车所在车道内位于主车前方的障碍物，OBJ_h-是由矩阵OBJ最后一行为1且第2行障碍物纵向坐标为负的列组成的，代表主车所在车道内位于主车后方的障碍物，矩阵OBJ_h+和OBJ_h-如式(9)所示：

其中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- 00]；

4)当处于第四类行车场景下，只需要对主车所在车道关键障碍物进行筛选，首先提取矩阵OBJ最后一行为1的列重新组成两个新的矩阵OBJ_h+和OBJ_h-，OBJ_h+是由矩阵OBJ最后一行为1且第2行障碍物纵向坐标为正的列组成的，代表主车所在车道内位于主车前方的障碍物，OBJ_h-是由矩阵OBJ最后一行为1且第2行障碍物纵向坐标为负的列组成的，代表主车所在车道内位于主车后方的障碍物，矩阵OBJ_h+和OBJ_h-如式(10)所示：

其中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]；

步骤五、输出关键障碍物信息：

将步骤四中获取的关键障碍物矩阵obj信息输出，即完成了关键障碍物的信息的筛选和输出。