CN107330925A

CN107330925A - 一种基于激光雷达深度图像的多障碍物检测和跟踪方法

Info

Publication number: CN107330925A
Application number: CN201710330461.9A
Authority: CN
Inventors: 李晓峰; 杨晗; 管岭; 贾利民; 秦勇
Original assignee: Beijing Jiaotong University
Current assignee: Beijing Jiaotong University
Priority date: 2017-05-11
Filing date: 2017-05-11
Publication date: 2017-11-07
Anticipated expiration: 2037-05-11
Also published as: CN107330925B

Abstract

本发明公开一种基于激光雷达深度图像的多障碍物检测和跟踪方法，所述方法包括：S1：处理激光雷达数据得到深度图像，进而得到水平深度图像和高度图像；S2：根据所述深度图像、水平深度图像、高度图像和参考数据，通过边缘检测提取障碍物数据得到障碍物图像；S3：根据连续的障碍物图像建立动态的障碍物列表；S4：根据障碍物列表中的信息预测障碍物运动特性，本发明通过将3D激光雷达数据转换为深度图像，进而进行多障碍物检测和跟踪，保证了激光雷达数据在计算中的完整性，并提高了计算速度，进而提高了机器平台对障碍物反应的灵敏度，提高执行效率。

Description

一种基于激光雷达深度图像的多障碍物检测和跟踪方法

技术领域

本发明涉及激光雷达数据处理领域。更具体地，涉及一种基于激光雷达深度图像的多障碍物检测和跟踪方法。

背景技术

环境感知是移动机器平台能够进行导航和避障的核心技术，其主要任务是提取周围环境中的障碍物，从而分辨出可通行区域。在各种类型的传感器之中，激光雷达有着测距精度高、实时性好、适用范围广等优点，因此移动机器平台将激光雷达作为主要传感器。

目前对通过激光雷达获取周围环境动态信息的研究大多集中在结构化道路障碍物检测上，尤其是对车辆的检测和跟踪。常规做法是先将点云数据聚类分割为不同的障碍物，然后将不同时刻的障碍物数据相互关联起来，通过计算障碍物的动态特性来表示周围环境的动态特性。

其中，障碍物检测和跟踪根据点云数据的聚类分割方式不同可分为直接在三维空间中聚类跟踪和映射为二维图像然后再聚类跟踪两种方法。前者可以通过INSAC算法、贝叶斯估计、三维地图对比差分等方法实现，但计算量都相对较大，不适合实时计算；后者通常是将激光雷达数据映射为方形或者扇形的栅格地图，然后对栅格地图进行障碍物检测，但该方法忽视了障碍物的高度信息，且计算量和计算精度受栅格地图的分辨率的影响较大。因此，既有的算法不能兼顾计算速度和信息的完整性，在实际应用中有较大的限制。

因此，需要提供一种兼顾计算速度和信息完整性的多障碍物检测和跟踪方法，提高计算速度以及障碍物检测和跟踪的准确性。

发明内容

本发明的一个目的在于提供一种基于激光雷达深度图像的多障碍物检测和跟踪方法，以提高计算速度以及障碍物检测和跟踪的准确性。

为达到上述目的，本发明采用下述技术方案：

本发明公开了一种基于激光雷达深度图像的多障碍物检测和跟踪方法，所述方法包括：

S1：处理激光雷达数据得到深度图像，进而得到水平深度图像和高度图像；

S2：根据所述深度图像、水平深度图像、高度图像和参考数据，通过边缘检测提取障碍物数据得到障碍物图像；

S3：根据连续的障碍物图像建立动态的障碍物列表；

S4：根据障碍物列表中的障碍物信息预测障碍物运动特性。

优选地，所述S1包括：

S11：对激光雷达数据进行滤波，去除激光雷达数据中的孤立噪点；

S12：对滤波处理后的激光雷达数据进行离散化处理得到深度图像，所述深度图像为

P＝{r_u,v|u＝1,2,…,m；v＝1,2,…,n}

m＝2π/Δα

其中，r_u,v为激光雷达数据中坐标为(u,v)的数据点到原点的距离，u为离散化后的水平角度序号，v为离散化后的垂直角度序号，Δα为离散化选取的角度间隔，为数据点相对于原点的水平角度，n为激光雷达的垂直通道数量；

S13：处理所述深度图像得到水平深度图像D和高度图像H为

其中，θ(v)为激光雷达通道序号v所对应的实际角度。

优选地，所述S11包括：

S111：计算激光雷达数据中每个数据点与邻域数据点的最小差值为

r_min＝min{|r_c,i,t-r_{c+Δc,i+Δi,t+Δt}|}

其中，r_c,i,t为数据点的坐标，c是该数据点所在的激光雷达的垂直通道数，i是数据点的水平角度序号，t是数据点的采集时刻序号，Δc,Δi,Δt分别为c，i，t的增量，取值范围分别为{-1，0，1}，且不全为零；

S112：将r_min与自适应阈值δ_r进行比较，如果r_min小于阈值δ_r，则不作改变，反之，则用r_c,i,t的邻域数据点的中值代替。

优选地，所述S2包括：

S21：激光雷达数据中标记为天空的数据点为

p_i,j＝p_max且h_i,j＞0

其中，p_i,j为深度图像中坐标为(i,j)的数据点的像素值，p_max为激光雷达的最大探测距离，h_i,j为坐标为(i,j)的数据点的高度；

S22：计算每个数据点与领域数据点的水平深度变化和高度变化的比值的绝对值，若低于阈值，则将该数据点标记为地面；

S23：通过Canny边缘检测算法计算水平深度图像的边缘图像，进而去除所述水平深度图像中的边缘图像，得到连通域图像；

S24：去除连通域图像中的天空、地面、面积小于阈值的连通域以及最低高度大于障碍影响范围的连通域得到障碍物图像。

优选地，所述连通域图像为

m＝2π/Δα

其中，D为水平深度图像，D^*为边缘图像，d′_ij为连通域图像中数据点的像素值，d_ij为水平深度图像中数据点的像素值，为边缘图像中数据点的像素值，Δα为离散化选取的角度间隔，n为激光雷达的垂直通道数量。

优选地，所述S3包括：

S31：处理激光雷达数据得到连续多帧的障碍物图像，对所述障碍物图像进行描述：

f_obj＝(u_c,v_c,ρ_obj,w_obj,h_obj,S_obj)

w_obj＝max(u_i)-min(u_i)|i＝1,…,n

h_obj＝max(v_i)-min(v_i)|i＝1,…,n

S_obj＝N_Convex(S)

其中，f_obj为特征描述函数，u_i、v_i、r_i为障碍物图像中序号为i的像素点的横坐标、纵坐标、像素值，n为连通域包含的像素点的数量，u_c、v_c分别为障碍物图像的中心位置的横、纵坐标，ρ_obj为障碍物连通域的平均深度，w_obj、h_obj为障碍物连通域的宽度和高度，max(u_i)、min(u_i)分别为u_i最大值、最小值，max(v_i)、min(v_i)分别为v_i最大值、最小值，N_Convex(S)为障碍物连通域的凸包的面积；

S32：建立用于存储障碍物信息的障碍物列表，存储初始帧中障碍物信息，计算相邻帧中连通域的差异度矩阵F为

F＝{D_i,j}_n×m

D_i,j＝(w_u·Δu²+w_v·Δv²+w_ρ·Δρ²)·K_wK_hK_S

x＝w,h或s

其中，i＝1,2,…,n为各帧中障碍物的序号，j＝1,2,…,m为障碍物列表中的障碍物的序号，Δu、Δv、Δρ分别为障碍物的横坐标均值、纵坐标均值和像素值均值之差，w_u、w_v、w_ρ分别为Δu、Δv、Δρ对应的权重，K_w、K_h、K_S分别为障碍物的宽、高、面积的变化系数；

S33：根据连续多帧的障碍物图像更新障碍物列表。

优选地，所述更新障碍物列表的原则为：

若障碍物列表中的障碍物相对于当前障碍物图像中障碍物的差异度矩阵值不在预设阈值范围内，则减少障碍物列表中该障碍物的存在置信度；

若当前障碍物图像中障碍物相对于障碍物列表中的障碍物的差异度矩阵值不在预设阈值范围内，则增加当前障碍物图像中该障碍物的信息；

若当前障碍物图像中障碍物与障碍物列表中的障碍物的差异度矩阵值在预设阈值范围内，将当前障碍物图像中障碍物的信息添加到列表中对应的障碍物上，并增加障碍物的存在置信度；

删除障碍物存在置信度低于置信度阈值的障碍物信息。

优选地，所述S4采用线性卡尔曼滤波位置估值、速度估值和位置预测值。

优选地，所述S4建立障碍物的状态方程为

其中，X_k+1、X_k分别为障碍物k+1、k时刻的位置、速度矩阵，Y_k+1、Y_k分别为障碍物k+1、k时刻的预测位置矩阵，ΔT为采样时间间隔，W代表均值为零的***噪声，是协方差为Q的高斯变量，V代表均值为零的量测噪声，是协方差为R的高斯变量。

本发明的有益效果如下：

本发明提出的基于激光雷达深度图像的多障碍物检测和跟踪方法将3D激光雷达数据映射为深度图像，直接在深度图像上对数据进行处理，因此能够在保留激光雷达数据的全部信息的同时也能够减少计算的复杂度，提高计算速度以及障碍物检测和跟踪的灵敏度，减少机器平台反应时间，保持机器平台较高的执行效率。

附图说明

下面结合附图对本发明的具体实施方式作进一步详细的说明。

图1示出本发明具体实施例的流程示意图。

图2示出本发明具体实施例动态自适应滤波的结果示意图。

图3示出本发明具体实施例深度图像、水平深度图像和高度图像的结果示意图。

图4示出本发明具体实施例提取障碍物的过程示意图。

图5示出本发明具体实施例相邻时刻障碍物匹配的结果示意图。

图6示出本发明具体实施例卡尔曼滤波输出的速度与差分计算得出的速度对比的结果示意图。

具体实施方式

为了更清楚地说明本发明，下面结合优选实施例和附图对本发明做进一步的说明。附图中相似的部件以相同的附图标记进行表示。本领域技术人员应当理解，下面所具体描述的内容是说明性的而非限制性的，不应以此限制本发明的保护范围。

图1所示根据本发明一种基于激光雷达深度图像的多障碍物检测和跟踪方法的具体实施例，所述方法包括：

S1：处理激光雷达数据得到深度图像，进而得到水平深度图像和高度图像。本步骤对激光雷达数据进行预处理，步骤S1进一步可包括：

S11：对激光雷达数据进行滤波，去除激光雷达数据中的孤立噪点。激光雷达数据中的噪声主要来源于环境中与激光雷达相近频率的光源干扰和混合像素干扰。具体表现为点云中孤立的噪点，可能会影响算法的效果，因此要对激光雷达的数据进行滤波处理。本发明采用动态自适应滤波算法剔除原始数据中的孤立噪点。所述S11对激光雷达数据进行处理的具体步骤可包括：

S111：对于激光雷达数据中每个数据点，取其在时间和空间上的邻域，计算激光雷达数据中每个数据点与邻域数据点的最小差值为

r_min＝min{|r_c,i,t-r_{c+Δc,i+Δi,t+Δt}|}

S112：将r_min与自适应阈值δ_r进行比较，如果r_min小于阈值δ_r，则不作改变，反之，则用r_c,i,t的邻域数据点的中值代替，即

自适应阈值δ_d与激光雷达测距以及标准差相关，由雷达在不同距离的测量数据统计分析获得。

S12：直接在3D空间对数据点云进行处理，数据的存储空间和计算量都相对较大，不利于数据的实时处理。因此，对滤波处理后的激光雷达数据进行离散化处理得到深度图像，所述深度图像为

P＝{r_u,v|u＝1,2,…,m；v＝1,2,…,n}

m＝2π/Δα

S13：通过将一帧激光雷达数据映射为深度图像和高度图像，可以完整而且直观地表示这一帧数据中所包含的信息，也便于利用图形算法对数据进行处理。处理所述深度图像得到水平深度图像D和高度图像H为

其中，θ(v)为激光雷达通道序号v所对应的实际角度。

S2：根据所述深度图像、水平深度图像、高度图像和参考数据，通过边缘检测提取障碍物数据得到障碍物图像。所述参考数据可包括激光雷达的最大探测距离为p_max，参考地面高度为h_ground，车辆高度为h_verhicle，有意义连通域的判断阈值为δ_n。在进行跟踪之前，首先要进行障碍物检测，将障碍物与非障碍物以及不同的障碍物区分开来。非障碍物的部分主要包括地面、天空，可以通过计算深度和高度将其快速区分开来。同一个障碍物的深度变化是始终连续的，不同的障碍物就算一时重叠，根据运动特性的不同，最终也会分离开来；如果不同的障碍物重叠在一起，运动特性也相同，也就等效于是同一个障碍物了。因此，本发明通过边缘分割对激光雷达的深度图像进行处理，将图像分割为不同的连通域，代表环境中不同的障碍物。步骤S2进一步可包括：S21：检测天空，天空的特点是不会有激光返回，表现在激光雷达数据中就是该方向的输出值为默认的最大值。搜索雷达深度图像P中的每个像素p_i,j，如果

p_i,j＝p_max且h_i,j＞0

则将点(i,j)标记为天空；

S22：检测地面，地面的特点是高度变化较小，即高度随水平距离的变化较小，计算每个数据点与领域数据点的水平深度变化和高度变化的比值的绝对值，若低于阈值δ_g，则将该数据点标记为地面；

S23：提取连通域，不同的障碍物之间的深度变化会有较大的差异，出现差异的位置就是不同障碍物之间的分界之处。通过边缘检测，可以将深度图像分割为不同的连通域，这些连通域就是候选的障碍物聚类。通过Canny边缘检测算法计算水平深度图像的边缘图像，代表深度图像的边缘的点的值为1，其他地方的值为0，进而去除所述水平深度图像中的边缘图像，得到连通域图像。所述连通域图像为

m＝2π/Δα

虽然Canny边缘检测算法会尽可能提取出封闭的边缘，但依旧可能会有部分边缘没有闭合，表现在连通域图像D′中就是本来应该分成多个连通域的区域因为细小的连接而形成了一个连通域。因此，对连通域图像D′做形态学开运算，断开这些细小的连接，使连通域的分割更加自然和平滑。

然后用两遍扫描法给不同的连通域区分开来，得到连通域标号图L。连通域标号图L中的每一个点的像素值即为该连通域的标号，同属于一个连通域的像素点的标号相同，连通域的最小标号为1，非连通域部分的标号为0。

S24：去除非障碍物的连通域得到障碍物图像，图像中的连通域并不都是障碍物，同时为了进一步减少计算量，去除连通域图像中的天空、地面、面积小于阈值的连通域以及最低高度大于障碍影响范围的连通域。搜寻每一个像素点(i,j)，如果该点被标记天空或者地面，则将(i,j)所在的连通域的标号变为0；计算每个连通域的面积和最低高度，将面积小于δ_n像素且最低高度大于h_verhicle的连通域的标号变为0。经过以上步骤，一帧图像被分割为了不同的连通域，并且每个连通域都代表了有意义的障碍物。

S3：根据连续的障碍物图像建立动态的障碍物列表。建立一个动态列表来储存障碍物的信息，该列表主要分为两个部分：测量信息和预测信息。障碍物列表记录历史各个时刻测量得到的障碍物信息，预测信息包含历史各个时刻对下一时刻的障碍物信息的预测。这些信息包括障碍物的特征描述和动态特性，其中，主要依靠对障碍物特征描述进行比较来将不同时刻的障碍物关联起来。步骤S3进一步可包括：

S31：处理激光雷达数据得到连续多帧的障碍物图像，根据二维投影的障碍物图像的特点，通过连通域的中心位置、平均深度、宽度、高度和凸包面积这五个方面对每个连通域进行描述，对于每个障碍物连通域Obj＝{u_i,v_i,r_i|i＝1,2,…,n}，进行描述：

f_obj＝(u_c,v_c,ρ_obj,w_obj,h_obj,S_obj)

w_obj＝max(u_i)-min(u_i)|i＝1,…,n

h_obj＝max(v_i)-min(v_i)|i＝1,…,n

S_obj＝N_Convex(S)

F＝{D_i,j}_n×m

D_i,j＝(w_u·Δu²+w_v·Δv²+w_ρ·Δρ²)·K_wK_hK_S

x＝w,h或s

S33：根据连续多帧的障碍物图像更新障碍物列表。所述更新障碍物列表的原则为：

若障碍物列表中的障碍物相对于当前障碍物图像中障碍物的差异度矩阵值不在预设阈值范围内，则减少障碍物列表中该障碍物的存在置信度。这部分障碍物列表中没有被匹配上的障碍物，障碍物或被遮挡、或被合并、或因太远而消失。

若当前障碍物图像中障碍物相对于障碍物列表中的障碍物的差异度矩阵值不在预设阈值范围内，则增加当前障碍物图像中该障碍物的信息。当前数据中没有被匹配上的障碍物聚类，将这部分障碍物聚类作为新的障碍物加入障碍物列表，设置障碍物的各项参数为初始默认值

若当前障碍物图像中障碍物与障碍物列表中的障碍物的差异度矩阵值在预设阈值范围内，将当前障碍物图像中障碍物的信息添加到列表中对应的障碍物上，并增加障碍物的存在置信度。这部分障碍物列表中障碍物与当前数据成功匹配的，将新的障碍物聚类的信息添加到列表中对应的障碍物上，增加障碍物的存在置信度。

删除障碍物存在置信度低于置信度阈值的障碍物信息。在实际计算中，置信度有上限和下限，并且只需要障碍物最近一段时间的信息来计算障碍物的动态特性，因此可以将不需要的旧历史信息去除，进一步节省存储空间。

S4：根据障碍物列表中的信息预测障碍物运动特性。可采用线性卡尔曼滤波位置估值、速度估值和位置预测值。

建立障碍物的状态方程为

由此，可以得到障碍物的当前时刻的位置估值、速度估值和下一时刻的位置预测值三种信息。其中，位置估计值用于计算***噪音的方差Q和观测噪音的方差R；位置预测值用于与下一时刻的测量数据进行匹配，以减少动态误差。

本实施例在计算过程中，对激光雷达数据进行滤波消噪，消除原始数据中的孤立噪点。根据选取的阈值不同，结果也有所不同。本实施例中选取的阈值为分段阈值δ_r＝max(0.07r_c,i,t，30)，原始数据与消噪结果的对比如图2所示。将消噪后的激光雷达数据离散化，计算水平深度和高度，将其映射为深度图像、水平深度图像和高度图像。本实施例中选取的离散间隔Δα＝0.4°，即水平角度的离散数量为m＝360/Δα＝900；所用激光雷达的通道数量为n＝16，因此，所得的图像中共有16×900个像素，所得的深度图像、水平深度图像和高度图像如图3所示。

按本发明方法的步骤检测天空和地面，然后用Canny算子提取边缘图像，然后计算连通域图像，用两遍扫描法提取不同连通域，得到连通域标记图，从中去除地面与天空相关的部分，剩下的就是不同的障碍物。本实施例中Canny边缘检测算法的阈值为(0.007,0.03)；所得的天空区域标记图、地面区域标记图、边缘图像、连通域图像和去除天空与地面的连通域标号图如图4所示

提取障碍物的图形特征，并计算差异矩阵，通过最近邻匹配方法处理差异矩阵，将不同时刻的障碍物同一关联起来。本实施例中权重系数w_u＝3、w_v＝3、w_ρ＝1，最大差异度阈值δ_D＝10⁴，所得的不同时刻同一障碍物的关联图像如图5所示。

更新了障碍物的测量信息之后，通过线性卡尔曼滤波计算障碍物的动态特性，预测障碍物在下一时刻的位置，更新障碍物的预测信息。图6所示为图5中左侧的人形障碍物的由帧间差分计算的速度和由卡尔曼滤波计算的速度的比较。

从实验结果来看，本发明提出的方法可以有效对周围环境中多个障碍物进行检测和跟踪，计算障碍的动态特性。在Inter Core-i7，四核2.10GHz，8G内存的实验环境下，处理一帧数据的平均时间约为95毫秒，满足一般情况下的实时性需求。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定，对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动，这里无法对所有的实施方式予以穷举，凡是属于本发明的技术方案所引伸出的显而易见的变化或变动仍处于本发明的保护范围之列。

Claims

1.一种基于激光雷达深度图像的多障碍物检测和跟踪方法，其特征在于，所述方法包括：

S3：根据连续的障碍物图像建立动态的障碍物列表；

S4：根据障碍物列表中的障碍物信息预测障碍物运动特性。

2.根据权利要求1所述的方法，其特征在于，所述S1包括：

P＝{r_u,v|u＝1,2,…,m；v＝1,2,…,n}

m＝2π/Δα

S13：处理所述深度图像得到水平深度图像D和高度图像H为

<mfenced open = "" close = "}"> <mtable> <mtr> <mtd> <mrow> <mi>D</mi> <mo>=</mo> <msub> <mi>r</mi> <mrow> <mi>u</mi> <mo>,</mo> <mi>v</mi> </mrow> </msub> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&theta;</mi> <mrow> <mo>(</mo> <mi>v</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>H</mi> <mo>=</mo> <msub> <mi>r</mi> <mrow> <mi>u</mi> <mo>,</mo> <mi>v</mi> </mrow> </msub> <mi>sin</mi> <mi>&theta;</mi> <mrow> <mo>(</mo> <mi>v</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced>

其中，θ(v)为激光雷达通道序号v所对应的实际角度。

3.根据权利要求2所述的方法，其特征在于，所述S11包括：

r_min＝min{|r_c,i,t-r_{c+Δc,i+Δi,t+Δt}|}

4.根据权利要求1所述的方法，其特征在于，所述S2包括：

S21：激光雷达数据中标记为天空的数据点为

p_i,j＝p_max且h_i,j＞0

5.根据权利要求1所述的方法，其特征在于，所述连通域图像为

<mrow> <msup> <mi>D</mi> <mo>&prime;</mo> </msup> <mo>=</mo> <mi>D</mi> <mo>/</mo> <msup> <mi>D</mi> <mo>*</mo> </msup> <mo>=</mo> <msub> <mrow> <mo>{</mo> <msubsup> <mi>d</mi> <mrow> <mi>i</mi> <mi>j</mi> </mrow> <mo>&prime;</mo> </msubsup> <mo>|</mo> <msubsup> <mi>d</mi> <mrow> <mi>i</mi> <mi>j</mi> </mrow> <mo>&prime;</mo> </msubsup> <mo>=</mo> <msub> <mi>d</mi> <mrow> <mi>i</mi> <mi>j</mi> </mrow> </msub> <mrow> <mo>(</mo> <mn>1</mn> <mo>-</mo> <msubsup> <mi>d</mi> <mrow> <mi>i</mi> <mi>j</mi> </mrow> <mo>*</mo> </msubsup> <mo>)</mo> </mrow> <mo>}</mo> </mrow> <mrow> <mi>m</mi> <mo>&times;</mo> <mi>n</mi> </mrow> </msub> </mrow>

m＝2π/Δα

6.根据权利要求1所述的方法，其特征在于，所述S3包括：

f_obj＝(u_c,v_c,ρ_obj,w_obj,h_obj,S_obj)

<mrow> <msub> <mi>u</mi> <mi>c</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mi>n</mi> </mfrac> <munderover> <mo>&Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msub> <mi>u</mi> <mi>i</mi> </msub> </mrow>

<mrow> <msub> <mi>v</mi> <mi>c</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mi>n</mi> </mfrac> <munderover> <mo>&Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msub> <mi>v</mi> <mi>i</mi> </msub> </mrow>

<mrow> <msub> <mi>&rho;</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>j</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mi>n</mi> </mfrac> <munderover> <mo>&Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msub> <mi>r</mi> <mi>i</mi> </msub> </mrow>

w_obj＝max(u_i)-min(u_i)|i＝1,…,n

h_obj＝max(v_i)-min(v_i)|i＝1,…,n

S_obj＝N_Convex(S)

F＝{D_i,j}_n×m

D_i,j＝(w_u·Δu²+w_v·Δv²+w_ρ·Δρ²)·K_wK_hK_S

x＝w,h或s

S33：根据连续多帧的障碍物图像更新障碍物列表。

7.根据权利要求6所述的方法，其特征在于，所述更新障碍物列表的原则为：

删除障碍物存在置信度低于置信度阈值的障碍物信息。

8.根据权利要求1所述的方法，其特征在于，所述S4采用线性卡尔曼滤波位置估值、速度估值和位置预测值。

9.根据权利要求1所述的方法，其特征在于，所述S4建立障碍物的状态方程为

<mrow> <mi>A</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mi>&Delta;</mi> <mi>T</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mi>&Delta;</mi> <mi>T</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow>