CN109901193A - 近距离障碍物的光达侦测装置及其方法 - Google Patents

Abstract

本发明公开了一种近距离障碍物的光达侦测装置及其方法，其利用四个二维光达扫描障碍物，以取得障碍物对应的原始点云数据，其包含障碍物相对车辆的相对距离、相对角度与相对速度。接着，将点云数据分成对应障碍物的点云群组，并依据点云群组的轮廓取得障碍物的边界长度。最后，利用卡尔曼滤波法与外插法预测并追踪动态障碍物的动作路径，且传送对应动态障碍物的相对速度与边界长度予自动驾驶控制装置使用，又根据相对距离取得最接近车辆的动态障碍物的坐标，以传送坐标予自动驾驶控制装置使用，进而有效侦测障碍物。

Description

近距离障碍物的光达侦测装置及其方法

技术领域

本发明涉及一种障碍物侦测技术，且特别关于一种近距离障碍物的光达侦测装置及其方法。

背景技术

自动驾驶车是一种智能车辆，其通过感测系统感测道路环境，并利用感测获得的道路、车辆位置与障碍物信息，控制车辆的行驶参数，进而使车辆能够安全可靠地在道路上行驶并到达预定目的地。自动驾驶车应用了计算器、感测、通讯、信息融合、人工智能与自动控制技术，已经成为世界各国的研究焦点。

环境传感器是自动驾驶车上不可或缺的组件，包含77千兆赫兹(G Hz)雷达、摄像机、24千兆赫兹(G Hz)雷达与三维光达(Lidar)、二维光达(Lidar)，每一种传感器都有其优缺点，并可依照不同辅助驾驶系统需求进行配置。举例来说，77G Hz雷达负责远距侦测，视野(Field Of View，FOV)为34度，2公尺内为盲区，可侦测距离77G Hz雷达100公尺的位置，但噪声多，FOV有限，难以精准地建立障碍物的模型。摄像机负责远距侦测，FOV为90度，10公尺内为盲区，可侦测距离摄像机80公尺的位置，但易受大雨、浓雾、强光与天气影响。24G Hz雷达负责中距侦测，FOV为80度，可侦测距离24G Hz雷达40公尺的位置，但无法撷取车体的侧边(B柱)的障碍物，且侦测的障碍物的数量受限，数据误差大等限制。三维光达负责中距侦测，FOV为360度，可侦测距离三维光达35公尺的位置，3公尺内为盲区。综合上述所言，目前自驾车普遍采用雷达及摄像机，但由于其各自有噪声多、易受环境因素影响、无法应用于近距侦测等缺点，故可靠度高的三维光达逐渐普及化，但三维光达的数据量庞大，需较高的效能平台进行处理对应的数据量，再者，目前一般皆会通过采用多种类的传感器来搭配，以相互补足使用上的限制，造成整体成本较为昂贵。

因此，本发明针对上述的困扰，提出一种近距离障碍物的光达侦测装置及其方法，以解决现有技术所产生的问题。

发明内容

本发明的主要目的，在于提供一种近距离障碍物的光达侦测装置及其方法，其利用卡尔曼(Kalman)滤波法与外插法预测并追踪动态的障碍物的动作路径，以减少运算资源，并快速补足二维光达当下未扫描到障碍物所产生的失误。此外，提供动态的障碍物的边界长度，相较仅提供障碍物的中心点的坐标，更利于估算障碍物碰撞机率。

本发明的另一目的，在于提供一种近距离障碍物的光达侦测装置及其方法，其采用二维光达，二维光达具有数据精准、广视野(FOV)与盲区小，藉此提升车辆的近距侦测能力。此外，二维光达设于车辆四个角落，并位于两个不同高度，针对近距离盲区进行障碍物侦测，提升感测精准度。

为达上述目的，本发明提供一种近距离障碍物光达侦测装置，其设于一车辆中，车辆周围有至少一障碍物，近距离障碍物光达侦测装置包含四个二维光达(Lidar)、一噪声滤波器与一处理器。二维光达的其中两个设于车辆的前端，另两个设于车辆的后端，所有二维光达扫描障碍物，以取得障碍物对应的原始点云数据，原始点云数据包含障碍物相对车辆的相对距离、相对角度与相对速度。噪声滤波器电性连接所有二维光达，并接收原始点云数据，以滤除原始点云数据的噪声，进而产生滤除点云数据。处理器电性连接噪声滤波器与设于车辆中的一自动驾驶控制装置，并接收滤除点云数据。处理器以一预设长度将滤除点云数据分成对应障碍物的至少一点云群组，并依据点云群组的轮廓取得障碍物的边界长度。在相对速度于一预设时段内变动时，处理器判断障碍物为动态障碍物，处理器利用卡尔曼(Kalman)滤波法与外插法预测并追踪动态障碍物的动作路径，且传送对应动态障碍物的相对速度与边界长度给自动驾驶控制装置，又处理器根据动态障碍物的相对距离取得最接近车辆的动态障碍物的坐标，以传送此坐标予自动驾驶控制装置。

在本发明的一实施例中，外插法为拉格朗日插值法(Lagrangepolynomialinterpolation)与最小平方法(least squares)。

在本发明的一实施例中，相对速度于预设时段内固定时，处理器判断障碍物为静态障碍物，处理器传送最接近车辆的静态障碍物的相对距离予自动驾驶控制装置。

在本发明的一实施例中，障碍物与车辆的距离小于20公尺且大于0.3公尺。

在本发明的一实施例中，车辆的前端的二维光达设置于不同高度，车辆的后端的二维光达设置于不同高度。

本发明亦提供一种近距离障碍物光达侦测方法，其侦测一车辆周围的至少一障碍物。首先，利用四个二维光达(Lidar)扫描障碍物，以取得障碍物对应的原始点云数据，原始点云数据包含障碍物相对车辆的相对距离、相对角度与相对速度。接着，接收原始点云数据，以滤除原始点云数据的噪声，进而产生滤除点云数据。再来，接收滤除点云数据，且以一预设长度将滤除点云数据分成对应障碍物的至少一点云群组，并依据点云群组的轮廓取得障碍物的边界长度。最后，判断相对速度于一预设时段内是否变动：若是，判断障碍物为动态障碍物，并利用卡尔曼(Kalman)滤波法与外插法预测并追踪动态障碍物的动作路径，且传送对应动态障碍物的相对速度与边界长度给电性连接二维光达的自动驾驶控制装置，又根据动态障碍物的相对距离取得最接近车辆的动态障碍物的坐标，以传送此坐标予自动驾驶控制装置；以及若否，判断障碍物为静态障碍物，并传送最接近车辆的静态障碍物的相对距离予自动驾驶控制装置。

附图说明

图1为本发明的二维光达设于车辆的示意图。

图2为本发明的光达侦测装置的一实施例的装置方块图。

图3为本发明的滤除点云数据的分布图。

图4为本发明的光达侦测方法的一实施例的流程图。

附图标记说明：10-光达侦测装置；12-车辆；14-二维光达；16-噪声滤波器；18-处理器；20-自动驾驶控制装置。

具体实施方式

本发明的实施例将藉由下文配合相关图式进一步加以解说。尽可能的，于图式与说明书中，相同标号代表相同或相似构件。于图式中，基于简化与方便标示，形状与厚度可能经过夸大表示。可以理解的是，未特别显示于图式中或描述于说明书中的组件，为所属技术领域中具有通常技术者所知的形态。本领域的通常技术者可依据本发明的内容而进行多种的改变与修改。

以下请参阅图1、图2与图3。以下介绍本发明的近距离障碍物的光达侦测装置10，其设于一车辆12中，车辆12周围有至少一障碍物，障碍物的数量以多个为例。每一障碍物与车辆12的距离小于20公尺且大于0.3公尺。光达侦测装置10包含至少四个二维光达(Lidar)14、一噪声滤波器16与一处理器18，其中处理器18包含计算器与分类器，噪声滤波器16可为线性滤波器或非线性滤波器，线性滤波器例如为高斯(Gussan)滤波器，非线性滤波器例如为中值(Median)滤波器，二维光达14的数量以四个为例。二维光达14具有高数据精准度、广视野(FOV)与盲区小，0.3公尺为盲区，FOV为270度，且可侦测距离二维光达14约20公尺的位置，藉此提升车辆的近距侦测能力。因此，二维光达能侦测到三维光达的盲区，并提供比雷达更可靠的数据。二维光达14的其中二者设于车辆12的前端，其余设于车辆12的后端。为了避免当所有二维光达14皆设置在同一高度时，会出现侦测失误，例如二维光达14扫描到其他车辆的轮胎之间的空间，使光达侦测装置10以为周边没有障碍物。因此，车辆12的前端的二维光达14分别设置于不同高度，车辆12的后端的二维光达14亦分别设置于不同高度，以进行全局侦测。二维光达14设于车辆四个角落。举例来说，车辆的前端的二维光达14分别设置于离地面75公分与25公分的位置，车辆的后端的二维光达14设置于离地面75公分与25公分的位置。所有二维光达14扫描所有障碍物，以取得所有障碍物对应的原始点云数据OD，原始点云数据OD包含所有障碍物相对车辆12的相对距离、相对角度与相对速度。噪声滤波器16电性连接所有二维光达14，并接收原始点云数据OD，以滤除原始点云数据OD的噪声，进而产生滤除点云数据FD，以提高判断出障碍物的正确率。处理器18接收滤除点云数据FD，并以一预设长度将滤除点云数据FD分成分别对应所有障碍物的多个点云群组，例如图3中有五个点云群组，其中预设长度以50公分为例，X代表横轴距离，Y代表纵轴距离。因为属于同一个障碍物的滤除点云数据FD之间的距离会相当接近，通常小于50公分，若属于不同障碍物的滤除点云数据FD之间的距离通常会大于50公分。换言之，五个点云群组代表有五个障碍物在车辆12周围。处理器18依据每一点云群组的轮廓取得每一障碍物的边界长度，藉此分辨所有障碍物的大小。在障碍物的相对速度于一预设时段内变动时，处理器18判断此障碍物为动态障碍物，处理器18利用卡尔曼(Kalman)滤波法与外插法预测，并追踪动态障碍物的动作路径，且传送对应动态障碍物的相对速度与边界长度予自动驾驶控制装置20使用。处理器18根据动态障碍物的相对距离取得最接近车辆12的动态障碍物的坐标，以传送此坐标予自动驾驶控制装置20使用，以避免车辆与障碍物发生碰撞。在障碍物的相对速度于预设时段内固定时，处理器18判断此障碍物为静态障碍物，处理器18传送静态障碍物最接近车辆12的相对距离给自动驾驶控制装置20使用，以避免车辆与障碍物发生碰撞。

本发明利用卡尔曼滤波法与外插法，提前预测并追踪动态的障碍物的动作路径，以减少运算资源，并快速补足二维光达14当下未扫描到障碍物所产生的失误。此外，假设障碍物的密度相同，则障碍物的体积与质量成正相关，又由于障碍物的体积与长度亦成正相关，故障碍物的质量与体积成正相关；根据牛顿第二运动定律，可得知障碍物的质量与加速度成正相关；由上述可知，障碍物的加速度与长度为相关的，因此障碍物的移动状态与长度亦为相关的；换言之，由于大型障碍物与小型障碍物的加速度不一样，此将使评估的障碍物的位置与实际位置不一样；因此，提供动态障碍物的边界长度与所在区间，相较于障碍物的中心点坐标，更利于估算障碍物碰撞机率，以避免车辆12与障碍物发生碰撞。

以下介绍本发明的近距离障碍物的光达侦测装置10的运作方法。首先，如步骤S10所示，利用四个二维光达14扫描所有障碍物，以取得所有障碍物对应的原始点云数据OD，原始点云数据OD包含所有障碍物相对车辆12的相对距离、相对角度与相对速度。接着，如步骤S12所示，噪声滤波器16接收原始点云数据OD，以滤除原始点云数据OD的噪声，进而产生滤除点云数据FD。再来，如步骤S14所示，处理器18接收滤除点云数据FD，并以预设长度将滤除点云数据FD分成分别对应所有障碍物的多个点云群组，且依据每一点云群组的轮廓取得每一障碍物的边界长度。然后，如步骤S16所示，处理器18判断障碍物的相对速度于预设时段内是否变动，若是，进行步骤S18，若否，进行步骤S20。在步骤S18中，处理器18判断障碍物为动态障碍物，并利用卡尔曼滤波法与外插法预测并追踪动态障碍物的动作路径，且传送对应动态障碍物的相对速度与边界长度予电性连接二维光达14的自动驾驶控制装置20，又根据动态障碍物的相对距离取得最接近车辆12的动态障碍物的坐标，以传送此坐标予自动驾驶控制装置20。在步骤S20中，处理器18判断障碍物为静态障碍物，并传送最接近车辆12的静态障碍物的相对距离予自动驾驶控制装置20。

以下说明本发明所使用的卡尔曼滤波法，其以下列公式(1)、公式(2)、公式(3)、公式(4)与公式(5)实现。

X_k|(k-1)＝AX_(k-1)+Bu_(k-1) (1)

P_k|(k-1)＝AP_(k-1)A^T+Q (2)

K_k＝P_k|(k-1)H^T(HP_k|(k-1)H^T+R)^-1 (3)

X_k＝X_k|(k-1)+K_k(z_k-HX_k|(k-1)) (4)

P_k＝(1-K_kH)P_k|(k-1) (5)

k以大于或等于5的整数为例，公式(1)为状态预测公式。第(k-1)时刻早于第k时刻，X_k|(k-1)为利用第(k-1)时刻的障碍物的位置预估第k时刻的障碍物的位置，X_k为第k时刻的障碍物的位置，X_(k-1)为第(k-1)时刻的障碍物的位置，A为状态转移矩阵，A^T为状态转移矩阵的转置(transpose)矩阵，B为控制矩阵，u_(k-1)为第(k-1)时刻的控制向量，P_k|(k-1)为利用第(k-1)时刻的协方差矩阵预估第k时刻的协方差(covariance)矩阵，Q为系统噪声，P_(k-1)为第(k-1)时刻的协方差矩阵，K_k为第k时刻的卡尔曼常数，H为观测模型，H^T为观测模型的转置，R为观测误差矩阵，z_k为第k时刻的障碍物的速度，P_k为第k时刻的协方差矩阵，P_k|(k-1)为利用第(k-1)时刻的协方差矩阵预估第k时刻的协方差矩阵。

本发明所使用的外插法例如为拉格朗日插值法(Lagrangepolynomialinterpolation)与最小平方法(least squares)。以三阶插值法为例，障碍物的位置f(t)如公式(6)所示：

t为时间，当t＝0，f(0)＝X_k。当t＝1，f(1)为X_(k+1)。当t＝2，f(2)为X_(k+2)。X_(k-2)为第(k-2)时刻的障碍物的位置，X_(k+1)为第(k+1)时刻的障碍物的位置，X_(k+2)为第(k+2)时刻的障碍物的位置。

以最小平方法为例。障碍物的真实位置F(t)如公式(7)所示：

F(t)＝at²+bt+c (7)

t为时间，误差值||F(t)-X_t||＝{[F(t)-X_t]²}^1/2。当t＝k,(k-1),(k-2),(k-3)与(k-4)，且||F(t)-X_t||的总和为最小值时，F(t)为最佳曲线。

因此，依序得到公式(8)、公式(9)与公式(10)。

由于X_k、X_(k-1)、X_(k-2)、X_(k-3)与X_(k-4)皆为已知，故根据公式(10)可得到a、b与c及F(t)。当t＝(k+1)与(k+2)时，可得到障碍物的未来的位置。

综上所述，本发明采用二维光达，二维光达具有高数据精准度、广视野与盲区小，藉此提升车辆的近距侦测能力，同时利用位于不同高度的二维光达，达到全局侦测的目的。

以上所述仅为本发明一较佳实施例而已，并非用来限定本发明实施的范围，故举凡依本发明权利要求范围所述的形状、构造、特征及精神所为的均等变化与修饰，均应包括于本发明的保护范围内。

Claims (14)

1.一种近距离障碍物的光达侦测装置，其设于一车辆中，该车辆周围有至少一障碍物，其特征在于，该近距离障碍物的光达侦测装置包含：

四个二维光达，其中两个该二维光达设于该车辆的前端，另外两个该二维光达设于该车辆的后端，该四个二维光达扫描该至少一障碍物障碍物，以取得该至少一障碍物对应的原始点云数据，该原始点云数据包含该至少一障碍物相对该车辆的相对距离、相对角度与相对速度；

一噪声滤波器，电性连接该四个二维光达，并接收该原始点云数据，以滤除该原始点云数据的噪声，进而产生滤除点云数据；以及

一处理器，电性连接该噪声滤波器与设于该车辆中的一自动驾驶控制装置，并接收该滤除点云数据，该处理器以一预设长度将该滤除点云数据分成对应该至少一障碍物的至少一点云群组，并依据该至少一点云群组的轮廓取得该至少一障碍物的边界长度，在该相对速度于一预设时段内变动时，该处理器判断该至少一障碍物为动态障碍物，该处理器利用卡尔曼滤波法与外插法预测并追踪该动态障碍物的动作路径，且传送对应该动态障碍物的该相对速度与该边界长度予该自动驾驶控制装置，又该处理器根据该动态障碍物的该相对距离取得最接近该车辆的该动态障碍物的坐标，以传送该坐标予该自动驾驶控制装置。

2.如权利要求1所述的近距离障碍物的光达侦测装置，其特征在于，该噪声滤波器为线性滤波器或非线性滤波器。

3.如权利要求2所述的近距离障碍物的光达侦测装置，其特征在于，该线性滤波器为高斯滤波器。

4.如权利要求2所述的近距离障碍物的光达侦测装置，其特征在于，该非线性滤波器为中值滤波器。

5.如权利要求1所述的近距离障碍物的光达侦测装置，其特征在于，该相对速度于该预设时段内固定时，该处理器判断该至少一障碍物为静态障碍物，该处理器传送最接近该车辆的该静态障碍物的该相对距离予该自动驾驶控制装置。

6.如权利要求1所述的近距离障碍物的光达侦测装置，其特征在于，该外插法为拉格朗日插值法与最小平方法。

7.如权利要求1所述的近距离障碍物的光达侦测装置，其特征在于，该至少一障碍物与该车辆的距离小于20公尺且大于0.3公尺。

8.如权利要求1所述的近距离障碍物的光达侦测装置，其特征在于，该车辆的该前端的二维光达设置于不同高度，该车辆的该后端的二维光达设置于不同高度。

9.如权利要求8所述的近距离障碍物的光达侦测装置，其特征在于，该车辆的该前端的二维光达分别设置于离地面75公分与25公分的位置，该车辆的该后端的二维光达设置于离该地面75公分与25公分的位置。

10.如权利要求1所述的近距离障碍物光达侦测装置，其特征在于，该预设长度为50公分。

11.一种近距离障碍物的光达侦测方法，其用于侦测一车辆周围的至少一障碍物，其特征在于，该近距离障碍物的光达侦测方法包含下列步骤：

利用四个二维光达扫描该至少一障碍物，以取得该至少一障碍物对应的原始点云数据，该原始点云数据包含该至少一障碍物相对该车辆的相对距离、相对角度与相对速度；

接收该原始点云数据，以滤除该原始点云数据的噪声，进而产生滤除点云数据；

接收该滤除点云数据，且以一预设长度将该滤除点云数据分成对应该至少一障碍物的至少一点云群组，并依据该至少一点云群组的轮廓取得该至少一障碍物的边界长度；

判断该相对速度于一预设时段内是否变动：

若是，判断变动的该至少一障碍物为动态障碍物，利用卡尔曼滤波法与外插法预测，并追踪该动态障碍物的动作路径，且传送对应该动态障碍物的该相对速度与该边界长度给电性连接二维光达的自动驾驶控制装置，又根据该动态障碍物的该相对距离取得最接近该车辆的该动态障碍物的坐标，以传送该坐标予该自动驾驶控制装置；以及

若否，判断该至少一障碍物为静态障碍物，并传送最接近该车辆的该静态障碍物的该相对距离予该自动驾驶控制装置。

12.如权利要求11所述的近距离障碍物的光达侦测方法，其特征在于，该外插法为拉格朗日插值法与最小平方法。

13.如权利要求11所述的近距离障碍物的光达侦测方法，其特征在于，该至少一障碍物与该车辆的距离小于20公尺且大于0.3公尺。

14.如权利要求11所述的近距离障碍物的光达侦测方法，其特征在于，该预设长度为50公分。