CN111381248B - 一种考虑车辆颠簸的障碍物检测方法及系统 - Google Patents

Abstract

本发明专利提出一种考虑车辆颠簸的障碍物检测方法及系统。其步骤：使用智能车辆车载激光雷达对车辆周围环境进行扫描，利用IMU信息对单帧内不同旋转角度点云依次进行姿态修正；基于单帧内不同旋转角度的点云对应的车辆速度信息，对激光雷达点云进行纵向畸变修正；基于激光雷达前后帧对应的IMU姿态变化信息，对多帧点云姿态进行修真；利用前后帧点云之间的背景匹配关系信息对多帧点云进行修正；结合IMU与前后帧关系对多帧点云进行融合修正。本发明对激光雷达点云进行单帧和多帧的修正，实现了考虑车辆颠簸的障碍物检测，提升了智能车辆障碍物检测系统的鲁棒性，提高了车辆行驶过程中的安全性。

Description

一种考虑车辆颠簸的障碍物检测方法及系统

技术领域

本发明涉及智能车辆环境感知领域，特别是一种考虑车辆颠簸的障碍物检测方法及系统。

背景技术

智能车辆技术作为人工智能应用领域重要一部分，近几年来不断受到了人们的关注。约94％的交通事故是由驾驶人员分心驾驶、疲劳驾驶和不良情绪驾驶等个人因素引起的，而智能车辆可以在一定程度上替代驾驶人员的操作，自动对障碍物进行检测并进行规避，因此其发展有利于减少由碰撞引起的交通事故。智能车辆技术一般由感知，定位建图，路径规划，决策以及控制等部分组成，车辆感知的任务为使用传感器对车辆周围环境进行连续扫描和监控，使得智能车辆能够像驾驶员一样获得车辆周围的障碍物等环境信息。

然而，智能车辆在行驶过程中经常会遇到不平路面，智能车辆在这些路面上行驶时经常会出现颠簸强度大导致激光雷达等车载环境感知传感器抖动从而出现数据失真的问题，为传统障碍物检测方法及系统带来了挑战。在同一帧数据，车辆颠簸会使得环境感知数据是障碍物数据出现离散失真的情况，从而导致在障碍物检测时难以进行聚类以及判别障碍物大小特征；在不同帧数据中，由于车辆的颠簸会使得障碍物在不同帧中的位置难以定位，前后帧数据关联信息难以提取，对于障碍物检测以及跟踪重识别都有一定的影响。因此，实现考虑车辆颠簸的障碍物检测，对于提升智能车辆障碍物检测算法鲁棒性以及提高车辆安全性有着一定程度的影响。

目前关于考虑车辆颠簸的障碍物检测方法及系统的相关专利尚不存在。与之类似的已有的技术主要是针对越野条件下负障碍检测提出的相应解决方法。该越野条件下的负障碍物提取方法使用激光雷达同一条扫描线中相邻扫描点的之间的距离信息作为障碍物检测的依据，该方法在一定程度上可以降低颠簸对于能否检测出负障碍物的影响。但是这种方法对于车辆颠簸条件下障碍物检测信息的进一步完善并不能发挥作用：(1)相邻点距离也会随车辆颠簸而变化，不能解决车辆颠簸时导致的障碍物点云大小尺度失真情况；(2)相邻点信息只能应用在单帧障碍物检测中，对于基于前后帧信息的障碍物检测方法不适用。

因此，现有技术中考虑车辆颠簸的障碍物检测方法及系统较少，且多数仅考虑障碍物是否能够被检测出，对于颠簸条件下障碍物检测的尺度大小失真和前后关系失真问题考虑较少，难以满足障碍物检测的目标和任务。

发明内容

针对现有技术存在的不足，本发明的目的在于提供一种考虑车辆颠簸的障碍物检测方法及系统，该方法与系统从颠簸产生的点云误差出发，基于车载激光雷达与车辆惯性导航单元(IMU)数据，对车载激光雷达获得的点云数据进行实时修正，利用修正点云对障碍物进行检测，从而实现了考虑车辆颠簸的障碍物检测，提高了智能车辆障碍物检测的鲁棒性，进而提升了智能车辆的安全性。为实现上述目的，本发明提供了如下技术方案：一种考虑车辆颠簸的障碍物检测方法，包括如下步骤：

步骤1，利用智能车辆车载惯性导航单元数据频率高于激光雷达数据频率的特性，基于IMU获得姿态数据对单帧内的点云进行修正，完成对于单帧内点云姿态进行修正；

步骤2，使用智能车辆车载IMU或里程计提供的车辆速度信息，对步骤1中单帧内由车辆速度较快导致的点云畸变进行修正，完成对于单帧点云纵向畸变修正；

步骤3，利用激光雷达多帧间的IMU姿态变化信息，对激光雷达点云数据关键帧进行点云旋转修正，完成对于多帧间基于IMU的点云修正；

步骤4，利用激光雷达多帧点云间的匹配关系，将前帧点云作为基准点云，并依据此基准对后帧点云进行修正，完成对于多帧间基于背景匹配的点云修正；

步骤5，结合步骤3与步骤4中获得的点云修正信息，构建融合修正方程完成对多帧点云的修正，完成基于IMU与激光雷达融合的多帧点云修正；

步骤6，将经过步骤1～5处理后的点云数据进行背景点云去除与地面点云去除，并进行点云聚类获得障碍物点云簇，进行障碍物聚类检测，从而完成考虑车辆颠簸的障碍物检测。

作为本发明的进一步改进，所述步骤1中对单帧内的点云进行修正的具体步骤如下：

步骤11，令θ₀与θ_t分别表示激光雷达开始扫描的旋转角与t时刻的旋转角(各个点对应的旋转角可由激光雷达原始数据获得)，从θ₀旋转到θ_t的时间t为：

其中，T_liadr为激光雷达的扫描周期，可由激光雷达固定频率计算获得；

步骤12，令Δα,Δβ,Δγ分别表示0～t时刻的车辆俯仰角、横滚角和航向角变化量，ψ_αi,ψ_βi,ψ_γi分别表示i时刻IMU测得的俯仰角、横滚角和航向角加速度，T_imu为IMU的测量周期，则姿态变化矩阵为：

其中，k为0～t时刻内IMU按固定周期测量的次数，即对t/T_imu取整，可以表示为：

k＝[t/T_imu].；

步骤13，根据姿态变化量Δα,Δβ,Δγ获得单帧点云坐标旋转修正方程R₁：

令原始激光雷达点坐标为P₀＝[x₀,y₀,z₀]^T，经过单帧内点云姿态修正获得的点坐标P₁＝[x₁,y₁,z₁]^T，则P₁＝R₁P₀，通过对激光雷达一帧中不同旋转角度上的点依次进行修正最终获得姿态修正后的单帧点云。

作为本发明的进一步改进，所述步骤2中进行点云畸变进行修正的方式为通过车载IMU获得纵向加速度值并进行积分获得车辆0～t时刻的行驶距离s，对不同t时刻的激光雷达点P₁进行修正，具体的为：令单帧点云纵向畸变修正后点坐标P₂＝[x₂,y₂,z₂]^T，则P₁与P₂之间的关系为：

P₂＝P₁+T_x,

其中T_x为纵向修正矩阵，T_x＝[s,0,0]^T。

作为本发明的进一步改进，所述步骤3中对多帧间点云进行修正的具体步骤如下：

步骤31，记第k与第k+n帧间时间差为t_n，根据激光雷达固定频率特性，t_n可以表示为：

t_n＝nT_liadr,

其中T_liadr为激光雷达的扫描周期；

步骤32，根据t_n计算多帧间姿态变化矩阵：

步骤33，根据姿态变化量Δα,Δβ,Δγ与步骤13公式获得多帧间基于IMU的修正矩阵R₂，完成对于多帧间点云的修正。

作为本发明的进一步改进，所述步骤4中基于背景匹配的点云修正的具体步骤如下

步骤41，对S_k与S_k+n进行粗配准，即S_k+n为待配准点集，S_k为目标点集；

步骤42，使用经过粗配准旋转矩阵变换后的点集S′_k+n与目标点集S_k进行精配准，构造点云精配准能量函数：

J＝‖S_k-(R₃S′_k+n+T₃)‖²,

其中，R₃与T₃分别为精配准的旋转与平移矩阵；

步骤43，通过迭代可以计算出最小能量函数值对应的R₃与T₃；

步骤44，设定能量阈值ε_J与最大迭代次数K_max，当J≤ε_J或迭代次数大于K_max时停止迭代，并选取停止迭代时的R₃作为修正矩阵对第k+n帧点云进行修正，完成点云修正。

作为本发明的进一步改进，所述步骤41中粗匹配的具体步骤如下：

步骤411，从S_k+n中随机选取N_s个满足两两点距离大于ε_d的点，并计算这N_s个点的特征直方图；

步骤412，在目标点集S_k中选取相似特征直方图的点作为对应点；

步骤413，计算对应点之间的变换旋转矩阵，并依据此旋转矩阵对第k+n帧点云进行旋转获得点集S′_k+n。

作为本发明的进一步改进，所述步骤5中基于IMU与激光雷达融合的多帧点云修正的具体步骤如下：

步骤51，构建融合方程获得融合修正矩阵R₄：

其中，‖R₂‖₂代表IMU多帧修正矩阵的二范数值，

为该二范数值阈值，ω₁与ω₂为IMU与激光雷达的置信权重，依据IMU与激光雷达的测量精度选取。/>

本发明另一方面提供一种运行上述方法的系统，包括执行运算所述步骤1至6方法的智能运算处理器。

本发明的有益效果，1)考虑车辆颠簸对障碍物检测的影响问题，并提供相应的解决措施，提升了智能车辆在颠簸情况下障碍物检测的鲁棒性，提高了智能车辆的安全性；2)本方法及系统中使用的单帧点云的姿态修正和纵向畸变修正方法可以解决车辆在颠簸情况下获得的数据失真问题；3)本方法中IMU对多帧点云修正和激光雷达帧间关系对多帧点云修正的方法可以为障碍物的精确定位与跟踪重识别提供良好的数据保证；4)本方法中融合IMU与激光雷达帧间关系的方法降低了由于单传感器误差带来的修正误差，鲁棒性强；5)本方法中使用修正时的点云关系对背景点云进行剔除并利用聚类方法完成障碍物检测，降低了障碍物检测的计算量与复杂度。

附图说明

图1是本发明一种考虑车辆颠簸的障碍物检测方法及系统的流程图；

图2是本发明激光雷达单帧旋转角示意图；

图3是本发明姿态变化对点云坐标影响示意图；

图4是本发明多帧间基于背景匹配的点云修正流程图。

具体实施方式

下面将结合附图所给出的实施例对本发明做进一步的详述。

如图1所示，本实施例所提供的考虑车辆颠簸的障碍物检测方法及系统包括以下步骤：

1)单帧内点云姿态修正。利用智能车辆车载惯性导航单元(IMU)数据频率高于激光雷达数据频率的特性，基于IMU获得姿态数据对单帧内的点云进行修正；

2)单帧点云纵向畸变修正。使用智能车辆车载IMU或里程计提供的车辆速度信息，对步骤1中单帧内由车辆速度较快导致的点云畸变进行修正；

3)多帧间基于IMU的点云修正。利用激光雷达多帧间的IMU姿态变化信息，对激光雷达点云数据关键帧进行点云旋转修正；

4)多帧间基于背景匹配的点云修正。利用激光雷达多帧点云间的匹配关系，将前帧点云作为基准点云，并依据此基准对后帧点云进行修正；

5)基于IMU与激光雷达融合的多帧点云修正。为防止单个传感器测量误差影响以及背景点云匹配失败情况，结合步骤3与步骤4中获得的点云修正信息，构建融合修正方程完成对多帧点云的修正；

6)障碍物聚类检测。将经过步骤1～5处理后的点云数据进行背景点云去除与地面点云去除，并进行点云聚类获得障碍物点云簇，从而完成考虑车辆颠簸的障碍物检测。

所述步骤1中，使用的车载激光雷达数据为经过标定后并转换到笛卡尔坐标系下的三维点云数据，单帧点云为激光雷达在固定频率周期内扫描周围环境一周获得的点云数据。所使用的IMU数据标定至车辆车坐标，可以获得车辆的俯仰角、横滚角、航向角这三个姿态的加速度。通过计算单帧激光雷达数据中各个时间t对应的姿态变化信息，可以对单帧点云进行修正，从而避免由颠簸产生的单帧障碍物点云离散或障碍物特征失真情况。如图2所示，令θ₀与θ_t分别表示激光雷达开始扫描的旋转角与t时刻的旋转角(各个点对应的旋转角可由激光雷达原始数据获得)，从θ₀旋转到θ_t的时间t为：

其中，T_liadr为激光雷达的扫描周期，可由激光雷达固定频率计算获得。

IMU测量频率较高，一般可达200Hz以上，短时间内的测量误差可忽略，因此取0～t时刻的IMU测得的姿态加速度与测量周期的乘积和作为车辆姿态变化量，令Δα,Δβ,Δγ分别表示0～t时刻的车辆俯仰角、横滚角和航向角变化量，ψ_αi,ψ_βi,ψ_γi分别表示i时刻IMU测得的俯仰角、横滚角和航向角加速度，T_imu为IMU的测量周期，则姿态变化矩阵为：

其中，k为0～t时刻内IMU按固定周期测量的次数，即对t/T_imu取整，可以表示为：

k＝[t/T_imu].

如图3所示，根据姿态变化量Δα,Δβ,Δγ获得单帧点云坐标旋转修正方程R₁：

令原始激光雷达点坐标为P₀＝[x₀,y₀,z₀]^T，经过单帧内点云姿态修正获得的点坐标P₁＝[x₁,y₁,z₁]^T，则P₁＝R₁P₀，通过对激光雷达一帧中不同旋转角度上的点依次进行修正最终获得姿态修正后的单帧点云。

所述步骤2中，激光雷达在获取一帧点云数据时，智能车辆的位置会发生运动，从而导致单帧内每个点云的基准坐标不固定，当激光雷达采集数据频率较低，车辆行驶速度较快时尤为严重。这种情况会使得激光雷达测得的障碍物点云在纵向方向发生畸变或拉长，因此要进行单帧点云纵向畸变修正。所采用的方法与步骤1类似，通过车载IMU获得纵向加速度值并进行积分获得车辆0～t时刻的行驶距离s，对不同t时刻的激光雷达点P₁进行修正。令单帧点云纵向畸变修正后点坐标P₂＝[x₂,y₂,z₂]^T，则P₁与P₂之间的关系为：

P₂＝P₁+T_x,

其中T_x为纵向修正矩阵，T_x＝[s,0,0]^T。

通过对激光雷达一帧中不同旋转角度上的点依次进行纵向畸变修正最终获得考虑纵向畸变修正的单帧点云数据。

所述步骤3中，当车辆处于颠簸情况下，由于传感器的抖动会使得激光雷达相对于地面的视角发生变化，相同的障碍物在不同帧中的位置会因颠簸产生突变，对于障碍物的检测和跟踪都会产生严重的影响，因此需要对多帧间点云进行修正。为减少修正计算量，选取激光雷达第k与第k+n帧为所选关键帧进行多帧间姿态修正，即隔n帧对点云进行修正，其中n＝1,2,3…，当系统计算能力足够时可适当减小n的值来增加修正精度。为获得记第k与第k+n帧间的IMU姿态变化量，记第k与第k+n帧间时间差为t_n，根据激光雷达固定频率特性，t_n可以表示为：

t_n＝nT_liadr,

其中T_liadr为激光雷达的扫描周期。

根据t_n计算多帧间姿态变化矩阵：

根据姿态变化量Δα,Δβ,Δγ与公式(4)获得多帧间基于IMU的修正矩阵R₂，每一关键帧点云对应的修正矩阵R₂是不变的，因此令关键帧点云基于IMU修正后的坐标为P₃＝[x₃,y₃,z₃]^T，则P₃＝R₂P₂。

所述步骤4中，多帧点云之间有重叠背景区域，因此可以依据多帧点云之间的关系对多帧点云进行修正。令第k与第k+n帧点云点集分别为S_k与S_k+n，如图4所示，首先对S_k与S_k+n进行粗配准，即S_k+n为待配准点集，S_k为目标点集。粗配准的具体步骤为：(1)从S_k+n中随机选取N_s个满足两两点距离大于ε_d的点，并计算这N_s个点的特征直方图；(2)在目标点集S_k中选取相似特征直方图的点作为对应点；(3)计算对应点之间的变换旋转矩阵，并依据此旋转矩阵对第k+n帧点云进行旋转获得点集S′_k+n。

使用经过粗配准旋转矩阵变换后的点集S′_k+n与目标点集S_k进行精配准，构造点云精配准能量函数：

J＝‖S_k-(R₃S′_k+n+T₃)‖²,

其中，R₃与T₃分别为精配准的旋转与平移矩阵。

通过迭代可以计算出最小能量函数值对应的R₃与T₃，为提高计算速度，本方法及系统设定了能量阈值ε_J与最大迭代次数K_max，当J≤ε_J或迭代次数大于K_max时停止迭代，并选取停止迭代时的R₃作为修正矩阵对第k+n帧点云进行修正。令多帧间基于背景匹配修正后的点云坐标为P₄＝[x₄,y₄,z₄]^T，则P₄＝R₃P₂。

所述步骤5中，为防止出现IMU误差大和多帧点云未匹配上的情况，使用IMU与激光雷达融合的方法对点云姿态变化进行估算并对多帧点云进行修正。构建融合方程获得融合修正矩阵R₄：

其中，‖R₂‖₂代表IMU多帧修正矩阵的二范数值，

为该二范数值阈值。ω₁与ω₂为IMU与激光雷达的置信权重，依据IMU与激光雷达的测量精度选取。

最终，多帧间通过IMU与激光融合修正的方法得到的点云坐标为P₅＝R₄P₂。

所述步骤6中，经过步骤1～步骤5修正，点云障碍物失真与离散的问题基本消除，为避免地面点云与背景点云对障碍物检测产生干扰，将步骤4中匹配获得的背景点云进行剔除，并使用点云聚类的方法获得各个障碍物的点云簇，从而完成了考虑车辆颠簸的障碍物检测。

以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (7)

1.一种考虑车辆颠簸的障碍物检测方法，其特征在于：包括如下步骤：

步骤1，利用智能车辆车载惯性导航单元数据频率高于激光雷达数据频率的特性，基于IMU获得姿态数据对单帧内的点云进行修正，完成对于单帧内点云姿态进行修正；

步骤2，使用智能车辆车载IMU或里程计提供的车辆速度信息，对步骤1中单帧内由车辆速度较快导致的点云畸变进行修正，完成对于单帧点云纵向畸变修正；

步骤3，利用激光雷达多帧间的IMU姿态变化信息，对激光雷达点云数据关键帧进行点云旋转修正，完成对于多帧间基于IMU的点云修正；

步骤4，利用激光雷达多帧点云间的匹配关系，将前帧点云作为基准点云，并依据此基准对后帧点云进行修正，完成对于多帧间基于背景匹配的点云修正；

步骤5，结合步骤3与步骤4中获得的点云修正信息，构建融合修正方程完成对多帧点云的修正，完成基于IMU与激光雷达融合的多帧点云修正；

步骤6，将经过步骤1～5处理后的点云数据进行背景点云去除与地面点云去除，并进行点云聚类获得障碍物点云簇，进行障碍物聚类检测，从而完成考虑车辆颠簸的障碍物检测；

所述步骤5中基于IMU与激光雷达融合的多帧点云修正的具体步骤如下：

步骤51，构建融合方程获得融合修正矩阵R₄：

其中，||R₂||₂代表IMU多帧修正矩阵的二范数值，

为该二范数值阈值，ω₁与ω₂为IMU与激光雷达的置信权重，依据IMU与激光雷达的测量精度选取。

2.根据权利要求1所述的考虑车辆颠簸的障碍物检测方法，其特征在于：所述步骤1中对单帧内的点云进行修正的具体步骤如下：

步骤11，令θ₀与θ_t分别表示激光雷达开始扫描的旋转角与t时刻的旋转角，从θ₀旋转到θ_t的时间t为：

其中，T_liadr为激光雷达的扫描周期，可由激光雷达固定频率计算获得；

步骤12，令Δα，Δβ，Δγ分别表示0～t时刻的车辆俯仰角、横滚角和航向角变化量，ψ_αi，ψ_βi，ψ_γi分别表示i时刻IMU测得的俯仰角、横滚角和航向角加速度，T_imu为IMU的测量周期，则姿态变化矩阵为：

其中，k为0～t时刻内IMU按固定周期测量的次数，即对t/T_imu取整，表示为：

k＝[t/T_imu]；

步骤13，根据姿态变化量Δα，Δβ，Δγ获得单帧点云坐标旋转修正方程R₁：

令原始激光雷达点坐标为P₀＝[x₀，y₀，z₀]^T，经过单帧内点云姿态修正获得的点坐标P₁＝[x₁，y₁，z₁]^T，则P₁＝R₁P₀，通过对激光雷达一帧中不同旋转角度上的点依次进行修正最终获得姿态修正后的单帧点云。

3.根据权利要求2所述的考虑车辆颠簸的障碍物检测方法，其特征在于：所述步骤2中进行点云畸变进行修正的方式为通过车载IMU获得纵向加速度值并进行积分获得车辆0～t时刻的行驶距离s，对不同t时刻的激光雷达点P₁进行修正，具体的为：令单帧点云纵向畸变修正后点坐标P₂＝[x₂，y₂，z₂]^T，则P₁与P₂之间的关系为：

P₂＝P₁+T_x，

其中T_x为纵向修正矩阵，T_x＝[s，0，0]^T。

4.根据权利要求3所述的考虑车辆颠簸的障碍物检测方法，其特征在于：所述步骤3中对多帧间点云进行修正的具体步骤如下：

步骤31，记第k与第k+n帧间时间差为t_n，根据激光雷达固定频率特性，t_n表示为：

t_n＝nT_liadr

其中T_liadr为激光雷达的扫描周期；

步骤32，根据t_n计算多帧间姿态变化矩阵：

步骤33，根据姿态变化量Δα，Δβ，Δγ与步骤13公式获得多帧间基于IMU的修正矩阵R₂，完成对于多帧间点云的修正。

5.根据权利要求4所述的考虑车辆颠簸的障碍物检测方法，其特征在于：所述步骤4中基于背景匹配的点云修正的具体步骤如下

步骤41，对S_k与S_k+n进行粗配准，即S_k+n为待配准点集，S_k为目标点集；

步骤42，使用经过粗配准旋转矩阵变换后的点集S′_k+n与目标点集S_k进行精配准，构造点云精配准能量函数：

J＝||S_k-(R₃S′_k+n+T₃)||²，

其中，R₃与T₃分别为精配准的旋转与平移矩阵；

步骤43，通过迭代计算出最小能量函数值对应的R₃与T₃；

步骤44，设定能量阈值ε_J与最大迭代次数K_max，当J≤ε_J或迭代次数大于K_max时停止迭代，并选取停止迭代时的R₃作为修正矩阵对第k+n帧点云进行修正，完成点云修正。

6.根据权利要求5所述的考虑车辆颠簸的障碍物检测方法，其特征在于：所述步骤41中粗匹配的具体步骤如下：

步骤411，从S_k+n中随机选取N_s个满足两两点距离大于ε_d的点，并计算这N_s个点的特征直方图；

步骤412，在目标点集S_k中选取相似特征直方图的点作为对应点；

步骤413，计算对应点之间的变换旋转矩阵，并依据此旋转矩阵对第k+n帧点云进行旋转获得点集S′_k+n。

7.一种应用权利要求1至6任意一项所述方法的系统，其特征在于：包括执行运算所述方法的智能运算处理器。

Images