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

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


背景技术：

2.随着智能感知和智能控制技术的快速发展，机器人自主能力也随之得到提高，并在日常生产、智慧物流和军事作战等领域得到广泛的应用。而智能移动机器人的自主导航能力是其智能化的关键技术之一。
3.目前国内外关于移动机器人已经有了许多成熟的研究成果，然而大多数研究成果是在静态环境或已知环境中进行的。在静态环境或已知环境中，通过离线的全局地图可以快速的规划出一条到终点的可行驶路径，实现移动机器人的自主导航。相比而言，在存在动态障碍物且没有已知地图的未知动态环境下，实现移动机器人自主导航研究比较困难。未知动态环境即没有已知地图且环境中存在动态障碍物的环境，未知动态环境中存在动态障碍物，例如行人、车辆等，会对移动机器人的定位建图、路径规划产生负面影响。在机器人构建环境地图时，动态障碍物的位置会随时发生变化，使最终建立的地图无法正确反映所处环境，阻碍移动机器人的自主导航。在机器人进行路径规划时，动态障碍物的前进轨迹可能会与规划得到的路径交叉重合，容易导致碰撞。因此在机器人的运行过程中，不仅要检测出环境中的障碍物，也要对检测到的障碍物进行区分，面对动态障碍物执行不同的策略。
4.相机和激光雷达是机器人检测障碍物最常用的传感器。相机可以感知颜色信息，并且不会发出任何测量信号，这意味着不会影响其它的感知系统，随着图像处理与计算机视觉技术的发展，可以通过深度学习等人工智能算法实现目标检测，并识别目标的种类，以此检测动态障碍物，但深度学习依赖数据库的训练，硬件成本高。激光雷达不受光照条件的影响，通过发射激光可以有效地测量200米范围内的深度信息，即到物体的距离，从而获取三维信息。


技术实现要素：

5.本发明的目的是针对机器人在自主导航过程中出现动态障碍物的问题，提出了一种基于三维点云的机器人动态障碍物实时检测方法。
6.实现本发明目的的技术解决方案为：一种基于三维点云的动态障碍物实时检测方法，具体步骤如下：
7.步骤s1，将激光雷达固定安装于机器人上，采集原始激光雷达三维点云数据集p＝{p1,p2,...,pk,p
k 1
,...}，pk为第k次扫描周期的点云数据集，计算数据集p中每个点的曲率，按照曲率大小进行排序，选取曲率大于阈值c
max
的加入边缘特征点集ε＝{ε1,ε2,...,εk,ε
k 1
,...}，εk为第k次扫描周期的边缘特征点集，曲率小于c
min
的点加入平面特征点集ρ＝{ρ1,ρ2,...,ρk,ρ
k 1
,...}，ρk为第k次扫描周期的平面特征点集；根据边缘特征点集ε与平面特征集ρ构建关于机器人运动状态的代价函数，求解机器人第1，2，...，k，k 1次扫描周期的
位姿变换矩阵t1，t2，....，tk，t
k 1
，进入步骤s2。
8.步骤s2，对三维点云数据集p进行降采样、地面分割和非地面点云聚类，得到多个扫描周期的包含静态和动态障碍物三维包围框：d＝{d1,d2,...dk...dk}＝{{d1,d2,...,dn},{d1,d2,...,dn},...}，d为多个扫描周期检测到的静态和动态障碍物集合，dk指的是第k次扫描周期检测到的静态和动态障碍物集合，dn为其中一个扫描周期中检测到的第n个静态和动态障碍物，进入步骤s3。
9.步骤s3，结合机器人的位姿变换矩阵t
k-3
、t
k-2
、t
k-1
，将第k-3、k-2、k-1个扫描周期的静态和动态障碍物三维包围框分别投影到第k次扫描周期的静态和动态障碍物三维包围框所在坐标系中，对应得到第k-3、k-2、k-1个扫描周期融合后静态和动态障碍物三维包围框dk＝{d
k-3
,d
k-2
,d
k-1
}＝{{d1,d2,...,dn},{d1,d2,...,dn},{d1,d2,...,dn}}，进入步骤s4。
10.步骤s4，根据融合后静态和动态障碍物三维包围框，使用基于非地面三维点云的iou-tracker实现数据关联，进行静态和动态障碍物跟踪，获取静态和动态障碍物的历史轨迹，进入步骤s5。
11.步骤s5，根据静态和动态障碍物的历史轨迹，检测出动态障碍物。
12.本发明与现有技术相比，其显著优点在于：
13.(1)在没有imu与相机的辅助下，仅使用激光雷达实现动态障碍物的实时检测，减少系统的复杂度；通过激光雷达实现机器人的位姿估计，运用到移动中的机器人时仍能实现稳定的检测。
14.(2)采用基于非地面三维点云数据的iou-tracker实现数据关联，进行多个障碍物跟踪并获取每个障碍物的轨迹信息，根据障碍物轨迹信息检测出动态障碍物。相对于先识别障碍物种类再判断是否为动态障碍物的方法，本发明对机器人计算平台的要求更低，更能适应多种环境。
附图说明
15.图1为本发明的一种基于三维点云的动态障碍物实时检测方法框图。
16.图2为动态障碍物检测流程图。
17.图3为三维点云融合效果图，其中图(a)是整体点云投影，图(b)是行人点云投影，图(c)是行李箱点云投影。
18.图4为动态障碍物检测结果图。
具体实施方式
19.下面结合具体实施方式对本发明做进一步详细的说明。
20.结合图1和图2，本发明所述的基于三维点云的机器人动态障碍物实时检测方法，步骤如下：
21.步骤s1，将激光雷达固定安装于机器人上，采集原始激光雷达三维点云数据集p＝{p1,p2,...,pk,p
k 1
,...}，pk为第k次扫描周期的点云数据集，计算数据集p中每个点的曲率，按照曲率大小进行排序，选取曲率大于阈值c
max
的加入边缘特征点集ε＝{ε1,ε2,...,εk,ε
k 1
,...}，εk为第k次扫描周期的边缘特征点集，曲率小于c
min
的点加入平面特征点集ρ＝{ρ1,ρ2,...,ρk,ρ
k 1
,...}，ρk为第k次扫描周期的平面特征点集；根据边缘特征点集ε与平面
特征集ρ构建关于机器人运动状态的代价函数，求解机器人第1，2，...，k，k 1次扫描周期的位姿变换矩阵t1，t2，....，tk，t
k 1
。
22.步骤s1.1，计算原始激光雷达三维点云数据集p中每个点的曲率c：
[0023][0024]
其中，s为点i最近邻点集，为激光雷达坐标系l下的第k次扫描周期中点i的坐标，表示在激光雷达坐标系l下的第k次扫描周期中点j的坐标，j∈s，进入步骤s1.2。
[0025]
步骤s1.2，将曲率c按照大小进行排序，选取曲率大于阈值c
max
的加入边缘特征点集ε＝{ε1,ε2,...,εk,ε
k 1
,...}，εk为第k次扫描周期的边缘特征点集，曲率小于c
min
的点加入平面特征点集ρ＝{ρ1,ρ2,...,ρk,ρ
k 1
,...}，ρk为第k次扫描周期的平面特征点集，进入步骤s1.3。
[0026]
步骤s1.3，假设机器人的初始位姿变换矩阵为t0＝0，利用＝0，利用计算得到第1次扫描周期位姿变换矩阵t1；根据位姿变换矩阵t1，利用计算得到第2次扫描周期的位姿变换矩阵t2，不断迭代收敛，得到第k、k 1次扫描周期位姿变换矩阵为tk、t
k 1
，利用位姿变换矩阵tk把第k次扫描周期的三维点云pk投影至第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。
[0027]
步骤s1.4，根据边缘特征点集平面特征点集构建机器人位姿代价函数d
ε
和d
p
：
[0028][0029]
其中，点i1为中一个点，点j1为点i1最近邻点，点l1为连续两次扫描周期中点j1的最近邻点，j1≠l1，d
ε
为点i1到直线j1l1的距离，为点i1在激光雷达坐标系l下的坐标，为点j1在激光雷达坐标系l下的坐标，为点l1在激光雷达坐标系l下的坐标。
[0030][0031]
其中，点i2为中一个点，点j2、点l2为点i2的两个不同的最近邻点，j2≠l2，点m为点i2的最近邻点，点m在点j2的连续两次扫描周期中，d
ρ
为点i2到平面j2l2m的
距离，为点i2在激光雷达坐标系l下的坐标，为点j2在激光雷达坐标系l下的坐标，为点l2在激光雷达坐标系l下的坐标，为点m在激光雷达坐标系l下的坐标，进入步骤s1.5。
[0032]
步骤s1.5，利用代价函数d
ε
和d
p
构建距离模型d，d是关于位姿变换t
k 1
的非线性函数f(t
k 1
)＝d，使用最小二乘法优化直到|d|的值接近0，求解机器人的位姿变换t
k 1
：
[0033][0034]
其中，t
k 1
为机器人的位姿变换，λ为拉格朗日乘子，为雅可比矩阵。
[0035]
进入步骤s2。
[0036]
步骤s2，对三维点云数据集p进行降采样、地面分割和非地面点云聚类，得到多个扫描周期的包含静态和动态障碍物三维包围框：d＝{d1,d2,...dk...dk}＝{{d1,d2,...,dn},{d1,d2,...,dn},...}，d为多个扫描周期检测到的静态和动态障碍物集合，dk指的是第k次扫描周期检测到的静态和动态障碍物集合，dn为其中一个扫描周期中检测到的第n个静态和动态障碍物，进入步骤s3。
[0037]
步骤s3，如图3所示，结合机器人的位姿变换矩阵t
k-3
、t
k-2
、t
k-1
，将第k-3、k-2、k-1个扫描周期的静态和动态障碍物三维包围框分别投影到第k次扫描周期的静态和动态障碍物三维包围框所在坐标系中，对应得到第k-3、k-2、k-1个扫描周期融合后静态和动态障碍物三维包围框dk＝{d
k-3
,d
k-2
,d
k-1
}＝{{d1,d2,...,dn},{d1,d2,...,dn},{d1,d2,...,dn}}，进入步骤s4。
[0038]
步骤s4，根据融合后静态和动态障碍物三维包围框，使用基于非地面三维点云的iou-tracker实现数据关联，进行静态和动态障碍物跟踪，获取静态和动态障碍物的历史轨迹。
[0039]
作为对本发明的优选，具体步骤如下：
[0040]
步骤s4.1，初始化集合ta和tf，ta用于存储检测到的静态和动态障碍物所有轨迹，tf为保存ta中有效的轨迹并作为静态和动态障碍物跟踪的结果，tf＝{l
li
|l
li
》l
th
,l
li
∈ta}，进入步骤s4.2。
[0041]
步骤s4.2，将包含静态和动态障碍物的三维包围框坐标集合中定义检测到的第p个静态或动态障碍物d
p
，p∈[0,n]，计算d
p
三维包围框三维包围框与ta中第q个轨迹lq末端所对应三维包围框末端所对应三维包围框的交并比iou(d
p
,lq)：
[0042][0043]
其中，为组成d
p
三维包围框坐标点，三维包围框坐标点，组成l
p
三维包围框坐标点，若交并比iou(d
p
,lq)大于阈值σ
iou
，则加入集合d
iou
＝{d
p
|iou(d
p
,lq)≥σ
iou
，d
p
∈dk}，进入步骤s4.3。
[0044]
步骤s4.3，判断集合d
iou
的数量是否为零：
[0045]
若集合d
iou
的数量不为零，计算集合d
iou
中每个静态或动态障碍物中心坐标与轨迹lq末端中心坐标的距离，将计算得到的距离最小的静态或动态障碍物d
min_l
加入到轨迹lq，
并把该静态或动态障碍物从dk中删除，进入步骤s4.4。
[0046][0047]
其中，为集合d
iou
中第o个静态或动态障碍物中心坐标，为轨迹lq末端中心坐标；
[0048]
若集合d
iou
的数量为零，比较当前时间与该轨迹的更新时间，若时间差大于阈值t
min
，将该轨迹从集合ta中删除，进入步骤s4.4。
[0049]
步骤s4.4，重复步骤s4.3，遍历dk中的所有静态和动态障碍物，对集合dk中满足步骤s4.3的静态和动态障碍物，创建新的轨迹，并将其加入集合ta中，进入步骤s4.5。
[0050]
步骤s4.5，遍历集合ta中的每个轨迹，将轨迹长度大于阈值l
th
的轨迹加集合tf，输出静态和动态障碍物的历史轨迹集合tf，如图4所示。进入步骤s5。
[0051]
步骤s5，根据静态和动态障碍物的历史轨迹，检测出动态障碍物。
[0052]
上述实施例为本发明最佳的实施例及运用技术原理，但本发明的实施例方式并不受上述实施例的限制，其他不违背本发明的原理下所做的改变、修饰、替代、组合、简化，均应为等效的置换方式，都在本发明的保护范围之内。