CN114998276B - 一种基于三维点云的机器人动态障碍物实时检测方法 - Google Patents

Abstract

本发明公开了一种基于三维点云的动态障碍物实时检测方法，属于人工智能领域，首先根据三维点云数据的曲率大小提取三维点云数据中特征点，构建机器人运动状态代价函数并进行非线性优化；然后，对三维点云数据进行降采样、地面分割和非地面三维点云聚类，获得多个扫描周期非地面三维点云数据中的障碍物位置信息，并结合机器人的位姿将障碍物位置信息投影至当前扫描周期中非地面三维点云所对应的坐标系；其次，使用基于非地面三维点云数据的IoU‑Tracker实现数据关联，进行多个障碍物跟踪并获取每个障碍物的轨迹信息；最后，根据获取每个障碍物轨迹信息检测出动态障碍物。本发明使用激光雷达实现动态障碍物的检测，实时性强，准确率高。

Description

一种基于三维点云的机器人动态障碍物实时检测方法

技术领域

本发明涉及到人工智能领域，具体涉及一种基于三维点云的机器人动态障碍物实时检测方法。

背景技术

随着智能感知和智能控制技术的快速发展，机器人自主能力也随之得到提高，并在日常生产、智慧物流和军事作战等领域得到广泛的应用。而智能移动机器人的自主导航能力是其智能化的关键技术之一。

目前国内外关于移动机器人已经有了许多成熟的研究成果，然而大多数研究成果是在静态环境或已知环境中进行的。在静态环境或已知环境中，通过离线的全局地图可以快速的规划出一条到终点的可行驶路径，实现移动机器人的自主导航。相比而言，在存在动态障碍物且没有已知地图的未知动态环境下，实现移动机器人自主导航研究比较困难。未知动态环境即没有已知地图且环境中存在动态障碍物的环境，未知动态环境中存在动态障碍物，例如行人、车辆等，会对移动机器人的定位建图、路径规划产生负面影响。在机器人构建环境地图时，动态障碍物的位置会随时发生变化，使最终建立的地图无法正确反映所处环境，阻碍移动机器人的自主导航。在机器人进行路径规划时，动态障碍物的前进轨迹可能会与规划得到的路径交叉重合，容易导致碰撞。因此在机器人的运行过程中，不仅要检测出环境中的障碍物，也要对检测到的障碍物进行区分，面对动态障碍物执行不同的策略。

相机和激光雷达是机器人检测障碍物最常用的传感器。相机可以感知颜色信息，并且不会发出任何测量信号，这意味着不会影响其它的感知系统，随着图像处理与计算机视觉技术的发展，可以通过深度学习等人工智能算法实现目标检测，并识别目标的种类，以此检测动态障碍物，但深度学习依赖数据库的训练，硬件成本高。激光雷达不受光照条件的影响，通过发射激光可以有效地测量200米范围内的深度信息，即到物体的距离，从而获取三维信息。

发明内容

本发明的目的是针对机器人在自主导航过程中出现动态障碍物的问题，提出了一种基于三维点云的机器人动态障碍物实时检测方法。

实现本发明目的的技术解决方案为：一种基于三维点云的动态障碍物实时检测方法，具体步骤如下：

步骤S1，将激光雷达固定安装于机器人上，采集原始激光雷达三维点云数据集P＝{P₁,P₂,...,P_k,P_k+1,...}，P_k为第k次扫描周期的点云数据集，计算数据集P中每个点的曲率，按照曲率大小进行排序，选取曲率大于阈值c_max的加入边缘特征点集ε＝{ε₁,ε₂,...,ε_k,ε_k+1,...}，ε_k为第k次扫描周期的边缘特征点集，曲率小于c_min的点加入平面特征点集ρ＝{ρ₁,ρ₂,...,ρ_k,ρ_k+1,...}，ρ_k为第k次扫描周期的平面特征点集；根据边缘特征点集ε与平面特征集ρ构建关于机器人运动状态的代价函数，求解机器人第1，2，...，k，k+1次扫描周期的位姿变换矩阵T₁，T₂，....，T_k，T_k+1，进入步骤S2。

步骤S2，对三维点云数据集P进行降采样、地面分割和非地面点云聚类，得到多个扫描周期的包含静态和动态障碍物三维包围框：D＝{D₁,D₂,...D_k...D_K}＝{{d₁,d₂,...,d_n},{d₁,d₂,...,d_n},...}，D为多个扫描周期检测到的静态和动态障碍物集合，D_k指的是第k次扫描周期检测到的静态和动态障碍物集合，d_n为其中一个扫描周期中检测到的第n个静态和动态障碍物，进入步骤S3。

步骤S3，结合机器人的位姿变换矩阵T_k-3、T_k-2、T_k-1，将第k-3、k-2、k-1个扫描周期的静态和动态障碍物三维包围框分别投影到第k次扫描周期的静态和动态障碍物三维包围框所在坐标系中，对应得到第k-3、k-2、k-1个扫描周期融合后静态和动态障碍物三维包围框D_k＝{D_k-3,D_k-2,D_k-1}＝{{d₁,d₂,...,d_n},{d₁,d₂,...,d_n},{d₁,d₂,...,d_n}}，进入步骤S4。

步骤S4，根据融合后静态和动态障碍物三维包围框，使用基于非地面三维点云的IoU-Tracker实现数据关联，进行静态和动态障碍物跟踪，获取静态和动态障碍物的历史轨迹，进入步骤S5。

步骤S5，根据静态和动态障碍物的历史轨迹，检测出动态障碍物。

本发明与现有技术相比，其显著优点在于：

(1)在没有IMU与相机的辅助下，仅使用激光雷达实现动态障碍物的实时检测，减少系统的复杂度；通过激光雷达实现机器人的位姿估计，运用到移动中的机器人时仍能实现稳定的检测。

(2)采用基于非地面三维点云数据的IoU-Tracker实现数据关联，进行多个障碍物跟踪并获取每个障碍物的轨迹信息，根据障碍物轨迹信息检测出动态障碍物。相对于先识别障碍物种类再判断是否为动态障碍物的方法，本发明对机器人计算平台的要求更低，更能适应多种环境。

附图说明

图1为本发明的一种基于三维点云的动态障碍物实时检测方法框图。

图2为动态障碍物检测流程图。

图3为三维点云融合效果图，其中图(a)是整体点云投影，图(b)是行人点云投影，图(c)是行李箱点云投影。

图4为动态障碍物检测结果图。

具体实施方式

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

结合图1和图2，本发明所述的基于三维点云的机器人动态障碍物实时检测方法，步骤如下：

步骤S1，将激光雷达固定安装于机器人上，采集原始激光雷达三维点云数据集P＝{P₁,P₂,...,P_k,P_k+1,...}，P_k为第k次扫描周期的点云数据集，计算数据集P中每个点的曲率，按照曲率大小进行排序，选取曲率大于阈值c_max的加入边缘特征点集ε＝{ε₁,ε₂,...,ε_k,ε_k+1,...}，ε_k为第k次扫描周期的边缘特征点集，曲率小于c_min的点加入平面特征点集ρ＝{ρ₁,ρ₂,...,ρ_k,ρ_k+1,...}，ρ_k为第k次扫描周期的平面特征点集；根据边缘特征点集ε与平面特征集ρ构建关于机器人运动状态的代价函数，求解机器人第1，2，...，k，k+1次扫描周期的位姿变换矩阵T₁，T₂，....，T_k，T_k+1。

步骤S1.1，计算原始激光雷达三维点云数据集P中每个点的曲率c：

其中，S为点i最近邻点集，

为激光雷达坐标系L下的第k次扫描周期中点i的坐标，/>

表示在激光雷达坐标系L下的第k次扫描周期中点j的坐标，j∈S，进入步骤S1.2。

步骤S1.2，将曲率c按照大小进行排序，选取曲率大于阈值c_max的加入边缘特征点集ε＝{ε₁,ε₂,...,ε_k,ε_k+1,...}，ε_k为第k次扫描周期的边缘特征点集，曲率小于c_min的点加入平面特征点集ρ＝{ρ₁,ρ₂,...,ρ_k,ρ_k+1,...}，ρ_k为第k次扫描周期的平面特征点集，进入步骤S1.3。

步骤S1.3，假设机器人的初始位姿变换矩阵为T₀＝0，利用

计算得到第1次扫描周期位姿变换矩阵T₁；根据位姿变换矩阵T₁，利用/>

计算得到第2次扫描周期的位姿变换矩阵T₂，不断迭代收敛，得到第k、k+1次扫描周期位姿变换矩阵为T_k、T_k+1，利用位姿变换矩阵T_k把第k次扫描周期的三维点云P_k投影至第k+1周期初始时刻t_k+1，得到第k次扫描周期的三维投影点云/>

第k次扫描周期的边缘特征点集/>

第k次扫描周期的平面特征点集/>

利用位姿变换矩阵T_k+1把第k+1次扫描周期的三维点云P_k+1投影第k+1周期初始时刻t_k+1，得到第k+1次扫描周期的三维投影点云/>

第k+1次扫描周期的边缘特征点集/>

第k+1次扫描周期的平面特征点集/>

进入步骤S1.4。

步骤S1.4，根据边缘特征点集

平面特征点集/>

构建机器人位姿代价函数d_ε和d_p：

其中，点i₁为

中一个点，点j₁为点i₁最近邻点，点l₁为连续两次扫描周期中点j₁的最近邻点，j₁≠l₁，/>

d_ε为点i₁到直线j₁l₁的距离，/>

为点i₁在激光雷达坐标系L下的坐标，/>

为点j₁在激光雷达坐标系L下的坐标，/>

为点l₁在激光雷达坐标系L下的坐标。

其中，点i₂为

中一个点，点j₂、点l₂为点i₂的两个不同的最近邻点，j₂≠l₂，点m为点i₂的最近邻点，点m在点j₂的连续两次扫描周期中，/>

d_ρ为点i₂到平面j₂l₂m的距离，/>

为点i₂在激光雷达坐标系L下的坐标，/>

为点j₂在激光雷达坐标系L下的坐标，/>

为点l₂在激光雷达坐标系L下的坐标，/>

为点m在激光雷达坐标系L下的坐标，进入步骤S1.5。

步骤S1.5，利用代价函数d_ε和d_p构建距离模型d，d是关于位姿变换T_k+1的非线性函数f(T_k+1)＝d，使用最小二乘法优化直到|d|的值接近0，求解机器人的位姿变换T_k+1：

其中，T_k+1为机器人的位姿变换，λ为拉格朗日乘子，

为雅可比矩阵。

进入步骤S2。

步骤S2，对三维点云数据集P进行降采样、地面分割和非地面点云聚类，得到多个扫描周期的包含静态和动态障碍物三维包围框：D＝{D₁,D₂,...D_k...D_K}＝{{d₁,d₂,...,d_n},{d₁,d₂,...,d_n},...}，D为多个扫描周期检测到的静态和动态障碍物集合，D_k指的是第k次扫描周期检测到的静态和动态障碍物集合，d_n为其中一个扫描周期中检测到的第n个静态和动态障碍物，进入步骤S3。

步骤S3，如图3所示，结合机器人的位姿变换矩阵T_k-3、T_k-2、T_k-1，将第k-3、k-2、k-1个扫描周期的静态和动态障碍物三维包围框分别投影到第k次扫描周期的静态和动态障碍物三维包围框所在坐标系中，对应得到第k-3、k-2、k-1个扫描周期融合后静态和动态障碍物三维包围框D_k＝{D_k-3,D_k-2,D_k-1}＝{{d₁,d₂,...,d_n},{d₁,d₂,...,d_n},{d₁,d₂,...,d_n}}，进入步骤S4。

步骤S4，根据融合后静态和动态障碍物三维包围框，使用基于非地面三维点云的IoU-Tracker实现数据关联，进行静态和动态障碍物跟踪，获取静态和动态障碍物的历史轨迹。

作为对本发明的优选，具体步骤如下：

步骤S4.1，初始化集合T_a和T_f，T_a用于存储检测到的静态和动态障碍物所有轨迹，

T_f为保存T_a中有效的轨迹并作为静态和动态障碍物跟踪的结果，T_f＝{l_li|l_li>l_th,l_li∈T_a}，进入步骤S4.2。

步骤S4.2，将包含静态和动态障碍物的三维包围框坐标集合中定义检测到的第p个静态或动态障碍物d_p，p∈[0,n]，计算d_p三维包围框

与T_a中第q个轨迹l_q末端所对应三维包围框/>

的交并比IoU(d_p,l_q)：

其中，

为组成d_p三维包围框坐标点，/>

组成l_p三维包围框坐标点，若交并比IoU(d_p,l_q)大于阈值σ_IoU，则加入集合D_IoU＝{d_p|IoU(d_p,l_q)≥σ_IoU，d_p∈D_k}，进入步骤S4.3。

步骤S4.3，判断集合D_IoU的数量是否为零：

若集合D_IoU的数量不为零，计算集合D_IoU中每个静态或动态障碍物中心坐标与轨迹l_q末端中心坐标的距离，将计算得到的距离最小的静态或动态障碍物d_{min_l}加入到轨迹l_q，并把该静态或动态障碍物从D_k中删除，进入步骤S4.4。

其中，

为集合D_IoU中第O个静态或动态障碍物中心坐标，

为轨迹l_q末端中心坐标；

若集合D_IoU的数量为零，比较当前时间与该轨迹的更新时间，若时间差大于阈值t_min，将该轨迹从集合T_a中删除，进入步骤S4.4。

步骤S4.4，重复步骤S4.3，遍历D_k中的所有静态和动态障碍物，对集合D_k中满足步骤S4.3的静态和动态障碍物，创建新的轨迹，并将其加入集合T_a中，进入步骤S4.5。

步骤S4.5，遍历集合T_a中的每个轨迹，将轨迹长度大于阈值l_th的轨迹加集合T_f，输出静态和动态障碍物的历史轨迹集合T_f，如图4所示。进入步骤S5。

步骤S5，根据静态和动态障碍物的历史轨迹，检测出动态障碍物。

上述实施例为本发明最佳的实施例及运用技术原理，但本发明的实施例方式并不受上述实施例的限制，其他不违背本发明的原理下所做的改变、修饰、替代、组合、简化，均应为等效的置换方式，都在本发明的保护范围之内。

Claims (2)

1.一种基于三维点云的动态障碍物实时检测方法，其特征在于，步骤如下：

步骤S1，将激光雷达固定安装于机器人上，采集原始激光雷达三维点云数据集P＝{P₁，P₂，...，P_k，P_k+1，...}，P_k为第k次扫描周期的点云数据集，计算数据集P中每个点的曲率，按照曲率大小进行排序，选取曲率大于阈值c_max的加入边缘特征点集ε＝{ε₁，ε₂，...，ε_k，ε_k+1，...}，ε_k为第k次扫描周期的边缘特征点集，曲率小于c_min的点加入平面特征点集ρ＝{ρ₁，ρ₂，...，ρ_k，ρ_k+1，...}，ρ_k为第k次扫描周期的平面特征点集；根据边缘特征点集ε与平面特征集ρ构建关于机器人运动状态的代价函数，求解机器人第1，2，…，k，k+1次扫描周期的位姿变换矩阵T₁，T₂，....，T_k，T_k+1，具体如下：

步骤S1中，将激光雷达固定安装于机器人上，采集原始激光雷达三维点云数据集P＝{P₁，P₂，...，P_k，P_k+1，...}，P_k为第k次扫描周期的点云数据集，计算数据集P中每个点的曲率，按照曲率大小进行排序，选取曲率大于阈值c_max的加入边缘特征点集ε＝{ε₁，ε₂，...，ε_k，ε_k+1，...}，ε_k为第k次扫描周期的边缘特征点集，曲率小于c_min的点加入平面特征点集ρ＝{ρ₁，ρ₂，...，ρ_k，ρ_k+1，...}，ρ_k为第k次扫描周期的平面特征点集；根据边缘特征点集ε与平面特征集ρ构建关于机器人运动状态的代价函数，求解机器人第1，2，...，k，k+1次扫描周期的位姿变换矩阵T₁，T₂，....，T_k，T_k+1，具体如下：

步骤S1.1，计算原始激光雷达三维点云数据集P中每个点的曲率c：

其中，S为点i最近邻点集，

为激光雷达坐标系L下的第k次扫描周期中点i的坐标，

表示在激光雷达坐标系L下的第k次扫描周期中点j的坐标，j∈S，进入步骤S1.2；

步骤S1.2，将曲率c按照大小进行排序，选取曲率大于阈值c_max的加入边缘特征点集ε＝{ε₁，ε₂，...，ε_k，ε_k+1，...}，ε_k为第k次扫描周期的边缘特征点集，曲率小于e_min的点加入平面特征点集ρ＝{ρ₁，ρ₂，...，ρ_k，ρ_k+1，...}，ρ_k为第k次扫描周期的平面特征点集，进入步骤S1.3；

步骤S1.3，假设机器人的初始位姿变换矩阵为T₀＝0，利用

计算得到第1次扫描周期位姿变换矩阵T₁；根据位姿变换矩阵T₁，利用/>

计算得到第2次扫描周期的位姿变换矩阵T₂，不断迭代收敛，得到第k、k+1次扫描周期位姿变换矩阵为T_k、T_k+1，利用位姿变换矩阵T_k把第k次扫描周期的三维点云P_k投影至第k+1周期初始时刻t_k+1，得到第k次扫描周期的三维投影点云/>

第k次扫描周期的边缘特征点集/>

第k次扫描周期的平面特征点集/>

利用位姿变换矩阵T_k+1把第k+1次扫描周期的三维点云P_k+1投影第k+1周期初始时刻t_k+1，得到第k+1次扫描周期的三维投影点云/>

第k+1次扫描周期的边缘特征点集/>

第k+1次扫描周期的平面特征点集/>

进入步骤S1.4；

步骤S1.4，根据边缘特征点集

平面特征点集/>

构建机器人位姿代价函数d_ε和d_p：

/>

其中，点i₁为

中一个点，点j₁为点i₁最近邻点，点l₁为连续两次扫描周期中点j₁的最近邻点，/>

d_ε为点i₁到直线j₁l₁的距离，/>

为点i₁在激光雷达坐标系L下的坐标，/>

为点j₁在激光雷达坐标系L下的坐标，/>

为点l₁在激光雷达坐标系L下的坐标；

其中，点i₂为

中一个点，点j₂、点l₂为点i₂的两个不同的最近邻点，j₂≠l₂，点m为点i₂的最近邻点，点m在点j₂的连续两次扫描周期中，/>

d_ρ为点i₂到平面j₂l₂m的距离，

为点i₂在激光雷达坐标系L下的坐标，/>

为点j₂在激光雷达坐标系L下的坐标，

为点l₂在激光雷达坐标系L下的坐标，/>

为点m在激光雷达坐标系L下的坐标，进入步骤S1.5；

步骤S1.5，利用代价函数d_ε和d_p构建距离模型d，d是关于位姿变换T_k+1的非线性函数f(T_k+1)＝d，使用最小二乘法优化直到|d|的值接近0，求解机器人的位姿变换T_k+1：

其中，T_k+1为机器人的位姿变换，λ为拉格朗日乘子，

为雅可比矩阵；

进入步骤S2；

步骤S2，对三维点云数据集P进行降采样、地面分割和非地面点云聚类，得到多个扫描周期的包含静态和动态障碍物三维包围框：D＝{D₁，D₂，...D_k...D_K}＝{{d₁，d₂，...，d_n}，{d₁，d₂，...，d_n}，...}，D为多个扫描周期检测到的静态和动态障碍物集合，D_k指的是第k次扫描周期检测到的静态和动态障碍物集合，d_n为其中一个扫描周期中检测到的第n个静态和动态障碍物，进入步骤S3；

步骤S3，结合机器人的位姿变换矩阵T_k-3、T_k-2、T_k-1，将第k-3、k-2、k-1个扫描周期的静态和动态障碍物三维包围框分别投影到第k次扫描周期的静态和动态障碍物三维包围框所在坐标系中，对应得到第k-3、k-2、k-1个扫描周期融合后静态和动态障碍物三维包围框D_k＝{D_k-3，D_k-2，D_k-1}＝{{d₁，d₂，...，d_n}，{d₁，d₂，...，d_n}，{d₁，d₂，...，d_n}}，进入步骤S4；

步骤S4，根据融合后静态和动态障碍物三维包围框，使用基于非地面三维点云的IoU-Tracker实现数据关联，进行静态和动态障碍物跟踪，获取静态和动态障碍物的历史轨迹，进入步骤S5；

步骤S5，根据静态和动态障碍物的历史轨迹，检测出动态障碍物。

2.根据权利要求1所述的一种基于三维点云的动态障碍物实时检测方法，其特征在于，步骤S4，根据融合后静态和动态障碍物三维包围框，使用基于非地面三维点云的IoU-Tracker实现数据关联，进行静态和动态障碍物跟踪，获取静态和动态障碍物的历史轨迹，具体如下：

步骤S4.1，初始化集合T_a和T_f，T_a用于存储检测到的静态和动态障碍物所有轨迹，

T_f为保存T_a中有效的轨迹并作为静态和动态障碍物跟踪的结果，T_f＝{l_li|l_li＞l_th，l_li∈T_a}，进入步骤S4.2；/>

步骤S4.2，将包含静态和动态障碍物的三维包围框坐标集合中定义检测到的第p个静态或动态障碍物d_p，p∈[0，n]，计算d_p三维包围框

与T_a中第q个轨迹l_q末端所对应三维包围框/>

的交并比IoU(d_p，l_q)：

其中，

为组成d_p三维包围框坐标点，/>

组成l_p三维包围框坐标点，若交并比IoU(d_p，l_q)大于阈值σ_IoU，则加入集合D_IoU＝{d_p|IoU(d_p，l_q)≥σ_IoU，d_p∈D_k}，进入步骤S4.3；

步骤S4.3，判断集合D_IoU的数量是否为零：

若集合D_IoU的数量不为零，计算集合D_IoU中每个静态或动态障碍物中心坐标与轨迹l_q末端中心坐标的距离，将计算得到的距离最小的静态或动态障碍物d_{min_l}加入到轨迹l_q，并把该静态或动态障碍物从D_k中删除，进入步骤S4.4；

其中，

为集合D_IoU中第O个静态或动态障碍物中心坐标，/>

为轨迹l_q末端中心坐标；

若集合D_IoU的数量为零，比较当前时间与该轨迹的更新时间，若时间差大于阈值t_min，将该轨迹从集合T_a中删除，进入步骤S4.4；

步骤S4.4，重复步骤S4.3，遍历D_k中的所有静态和动态障碍物，对集合D_k中满足步骤S4.3的静态和动态障碍物，创建新的轨迹，并将其加入集合T_a中，进入步骤S4.5；

步骤S4.5，遍历集合T_a中的每个轨迹，将轨迹长度大于阈值l_th的轨迹加集合T_f，输出静态和动态障碍物的历史轨迹集合T_f。

Images