CN113345008B - 考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法 - Google Patents

Abstract

本发明公开了一种考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法，该方法包括：步骤1，通过激光点云数据的几何特性，提取符合预设曲率特征的点云作为特征点，建立同一特征点在两帧相邻时刻点云数据中的匹配关系，并构建代价函数，以轮式机器人位姿为变量构造ICP问题，获得轮式机器人位姿信息；步骤2，检测候选动态障碍物；步骤3，估计动态障碍物状态。本发明不需要结合多传感器，在使用单一传感器的前提下进行动态障碍物检测目的，使系统更加高效，而且安全性更高。

Description

考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法

技术领域

本发明涉及激光雷达障碍物检测技术领域，特别是关于一种考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法。

背景技术

现如今，在人工智能高速发展的推动下，机器人技术正从传统工业制造向教育娱乐、医疗服务和勘探勘测方向迅速扩展，甚至也影响着自动驾驶等交叉领域。这些科技应用不断进步使得人们生活越发便捷。在众多驱动方式不同的机器人中，轮式机器人(WMR)由于其机械结构成熟，被广泛应用在了各行各业，例如物流机器人、仓储机器人和扫地机器人等。其中，智能机器人系统便为其核心组成部分，具有重要的研究意义。

智能机器人系统主要由三大技术部分组成，即环境感知、决策规划和运动控制，是一种综合性的智能控制系统。为保证安全，完成作业任务，机器人的避障功能必不可少，如扫地机器人智能避障功能和自动驾驶汽车主动防撞技术等，而这些需求都离不开障碍物检测这一关键技术。

障碍物检测技术通常基于视觉传感器或激光传感器所开展，但由于单目相机在尺度上的模糊性以及双目相机在深度估计方面的范围局限性，不适用于机器人的实时避撞过程。而激光雷达由于测量精度高、测距范围广的特性常被作为主要感知传感器，应用到移动机器人及无人驾驶汽车上。

随着机器人应用场景的复杂性不断提升，在机器人运动过程中不仅需要对障碍物进行准确检测，同时也需要对障碍物的运动状态做进一步预测，根据障碍物的预测轨迹，从而使机器人在路径规划过程中更新安全路线以避免碰撞。根据运动状态不同，可将障碍物分成两类，一是静态障碍物，如建筑物和绿化带等，机器人与静态障碍物的相对速度来源于机器人自身运动；而机器人与动态障碍物的相对速度不仅取决于自身速度，还取决于动态障碍物的运动状态。如何最大限度地估计障碍物状态，以实现最高效安全的避障，仍是一个待研究的课题。现有方案主要有两种：一是结合深度学习技术，在提取出环境语义信息的基础上，判断障碍物是否为可能运动的物体，如行人、车辆，从而进行避障优先级决策。但其无法在不知晓机器人自身运动状态前提下仅利用单帧信息估计出障碍物运动状态，此外，深度学习技术强依赖于其训练数据库，需要并行计算加速，硬件成本高，环境适应性低，并非是障碍物检测技术落地的首要之选。二是将多源传感器信息结合，如激光雷达结合轮式编码器或惯性导航单元在推算出机器人自身运动状态的同时，对障碍物进行有效跟踪，从而进行避障规划。

为降低系统硬件拓扑复杂度，提高系统可靠性，通常希望复用少量传感器实现多目标功能。目前基于单一传感器的动态障碍物识别方法仍不成熟，相关理论仍有待进一步完善。

发明内容

本发明的目的在于提供一种考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法来克服或至少减轻现有技术的上述缺陷中的至少一个。

为实现上述目的，本发明提供一种考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法，该方法包括：

步骤1，通过激光点云数据的几何特性，提取符合预设曲率特征的点云作为特征点，建立同一特征点在两帧相邻时刻点云数据中的匹配关系，并构建代价函数，以轮式机器人位姿为变量构造ICP问题，获得轮式机器人位姿信息；

步骤2，根据步骤1获得的轮式机器人的位姿信息，检测候选动态障碍物；

步骤3，根据步骤2检测得到的候选动态障碍物，估计动态障碍物状态。

进一步地，步骤1中，所述提取符合预设曲率特征的点云作为特征点的方法具体包括：

步骤a1，对点云数据进行体素滤波；

步骤b1，利用下式(1)计算点云数据中各点云的曲率值r，并按照曲率值r的大小排序，曲率值最大的点云定义为特征角点，曲率值最小的点云定义为特征平面点：

式中，X_(k，i)表示第k帧点云数据P_k中的第i个点云在雷达坐标系中的坐标，X_(k，j)表示P_k中的第j个点云在雷达坐标系中的坐标，S表示X_(k，i)的邻域。

进一步地，所述特征点为特征角点的情形下，步骤1中，所述建立同一特征点在两帧相邻时刻点云数据中的匹配关系的方法具体包括：

在第k-1帧点云数据P_k-1中寻找P_k中的第a个特征角点X_(k，a)对应的特征线段，该线段上具有点云b和点云c，其中，点云b为P_k-1中与特征角点X_(k，a)距离最近的点云，点云c为P_k-1中在特征角点X_(k，a)相邻扫描线上与特征角点X_(k，a)距离最近的点云，点云线距离

表示为下式(2)：

式中，X_(k-1，b)表示P_k-1中的第b个特征角点在雷达坐标系中的坐标，X_(k-1，c)表示P_k-1中的第c个特征角点在雷达坐标系中的坐标；

所述特征点为特征平面点的情形下，步骤1中，所述建立同一特征点在两帧相邻时刻点云数据中的匹配关系的方法具体包括：

在P_k-1中寻找第d个特征平面点X_(k，d)所对应的特征面，该特征面具有点云e、点云f和点云g，点云e为P_k-1中与特征平面点X_(k，d)距离最近的点云，点云f为P_k-1中在点云e同一相邻扫描线上与点云d距离最近的点云，点云g为在相邻扫描线上与特征平面点X_(k，d)距离最近的点云，点云面距离

表示为下式(3)：

式中，X_(k-1，e)表示P_k-1中的第e个特征平面点在雷达坐标系中的坐标，X_(k-1，f)表示P_k-1中的第f个特征平面点在雷达坐标系中的坐标，X_(k-1，g)表示P_k-1中的第g个特征平面点在雷达坐标系中的坐标。

进一步地，

步骤1中，构建代价函数，以轮式机器人位姿为变量构造ICP问题，获得轮式机器人位姿信息的方法具体包括：

步骤b1，考虑其水平方向平移及偏航方向旋转对应的二维变换矩阵

根据对应点云的距离关系，建立代价函数，将位姿解算问题转化成一个非线性最小二乘的求解问题，如下式(5)：

其中，

表示最优变换矩阵估计值，

表示P_k中第p个特征点在雷达坐标系中去除z轴维度后的坐标，q表示特征点的数量，

为

在P_k-1中的去除z轴维度后的齐次坐标，

表示

在P_k中的去除z轴维度后的齐次坐标；

步骤b2，定义第k帧特征点的质心在雷达坐标系中去除z轴维度后的坐标

以及第k-1帧特征点的质心在雷达坐标系中去除z轴维度后的坐标

步骤b3，将去质心坐标定义为

将上述问题(5)解耦，转化为式(6)表示的问题：

式中，

表示最优的轮式机器人偏航方向旋转矩阵估计值，

表示单次迭代的旋转矩阵，

表示最优的水平方向平移估计值；

步骤b4，通过SVD分解求出上述问题(6)中最优

定义矩阵W为式(7)：

W为一个2×2矩阵，对其进行SVD分解，可得式(8)：

W＝U∑V^T (8)

其中，∑为奇异值对角矩阵，U和V为正交矩阵；

当W满秩时，

表示为式(9)：

若计算得到的

行列式为负，则取

为当前迭代最优值；

步骤b5，按照

得到轮式机器人在x方向和y方向上的平移量进一步地，步骤2具体包括：

步骤21，根据步骤1输出的轮式机器人位姿信息，提取出可能属于动态障碍物点云；

步骤22，分割步骤21提取出的可能属于动态障碍物点云中的地面点云与非地面点云，去除地面点云，得到非地面点云；

步骤23，将整体的非地面点云按照特定策略聚类为多个点云簇，一个点云簇对应一个障碍物；

步骤24，估计步骤23获得的每个障碍物的位置、形状和尺寸信息；

其中，步骤21具体包括：

步骤211，根据里程计得到的位姿信息，将当前帧P_k左乘变换矩阵

变换到P_k-1的坐标系中；

步骤212，根据最近邻关系，P_k与P_k-1的交集部分确定为静态点云部分；

步骤213，将P_k与P_k-1的非交集部分的点云进行障碍物检测操作，通过观测多帧数据的方式去除属于静态障碍物的目标。

进一步地，步骤23具体包括：

步骤231，从步骤22获得的非地面点云中取出一个点云作为聚类种子点云，并对其进行栅格邻域搜索，若在栅格邻域半径ρ内存有点云，则将这些点云存入聚类生长点云队列

步骤232，遍历

中每个点云，进行半径阈值为ρ的邻域栅格搜索，若邻域内存在点云，则将不属于

的点云存入聚类生长过渡队列

遍历完所有点云后，将属于

中点云存入聚类容器

m为障碍物类别；

步骤233，将

中所有点云存入

重复以上步骤直至某次生长时

为空集，则该类别聚类结束；

重复上述步骤，直至所有点云处理完毕，得到所有障碍物聚类信息

将不满足足够阈值数量的类别进行剔除。

进一步地，步骤24具体包括：

步骤241，将步骤23得到的点云簇的质心定义为该障碍物的坐标；

步骤242，将移除z轴维度后的点云进行随机采样一致性直线分割，将分割出的直线方向作为该障碍物主方向；

步骤243，利用步骤242确定的主方向建立二维坐标系，再根据障碍物所对应点云簇中每个点云的空间信息，计算点云在该坐标系x′、y′两个维度上的极大、极小值，并将极大值与相应的极小值的差值作为障碍物的长和宽。

进一步地，步骤3具体包括：

步骤31，利用候选动态障碍物信息根据多帧数据中障碍物的差异程度计算差异度函数，并建立关联矩阵；

步骤32，根据动态障碍物的关联情况，结合激光里程计计算得到的帧间相对运动，判断障碍物运动状态，从而得到动态障碍物的运动方向及速度。

进一步地，步骤31具体包括：

步骤311，根据第k帧第u个障碍物的位置为

第k-1帧第v个障碍物位置为

障碍物的长和宽，以及步骤1计算得到的两帧点云数据相对运动为

将差异度函数

定义为下式(10)：

其中，

为机器人在二维空间中位置的齐次坐标表现形式，α为距离差异权重，β为长宽比差异权重；

步骤312，根据P_k-1中共检测到的障碍物的总数量为m个，记为

其中，u＝1，2，…，m；P_k中共检测到的障碍物的总数量为n个，记为

其中，v＝1，2，…，n，将关联矩阵

表示为下式(11)：

式中，

表示由式(10)计算的差异度；

步骤313，根据步骤312中建立的关联矩阵(11)，关联当前帧和上一帧点云数据中的动态障碍物，若最小差异度所对应的P_k-1中元素的下标u＞v，则表明发生障碍物丢失；若最小差异度所对应的P_k-1中元素的下标u＜v，则表明有新动态障碍物出现。

进一步地，步骤32具体包括：

有步骤31关联的障碍物在P_k-1中的坐标为t^k-1＝[x^k-1，y^k-1]，P_k中的坐标为t^k＝[x^k，y^k]，步骤1计算得到的两帧点云数据相对运动为

则利用式(12)计算障碍物的运动距离：

根据激光雷达传感器的帧率f，则动态障碍物的速度为V＝Δd×f，方向为v＝t^k-t^k-1。

本发明由于采取以上技术方案，具有以下优点：1)不需要结合多传感器，在使用单一传感器的前提下进行动态障碍物检测目的，使系统更加高效；2)激光里程计在短时内的位姿估计较为准确，且不会出现零漂现象，也不需额外的标定工作，所以使用激光里程计代替轮速计或惯导可以使系统更简单和稳定，因为轮速计偏差较大，容易导致误匹配，而惯导会出现零漂现象，且需要和激光雷达进行外参标定，同样会使得系统变复杂，不确定因素增多；3)基于轮式机器人的运动特性将障碍物检测问题转化为二维问题，在达到动态避障目的前提下使算法复杂度降低，从而可得到更高的数据处理频率；4)充分利用激光里程计信息，在提取出潜在动态点云基础上再进行动态障碍物检测，大大减少了由分割聚类带来的冗余计算；5)由于障碍物跟踪是在估计了自身位姿的基础上进行的，相比于不考虑自身位姿的跟踪算法，本发明的障碍物跟踪效果更好。

附图说明

图1为动态障碍物检测算法架构示意图。

具体实施方式

下面结合附图和实施例对本发明进行详细的描述。

如图1所示，本发明实施例提供的考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法包括：

步骤1，通过激光点云数据的几何特性，提取符合预设曲率特征的点云作为特征点，建立同一特征点在两帧相邻时刻点云数据中的匹配关系，并构建代价函数，以轮式机器人位姿为变量构造ICP(Iterative Closest Point；迭代最近点)问题，并使用SVD(Singular Value Decomposition；奇异值分解)分解在雷达坐标系的x和y二维平面解空间中进行迭代，直至位姿收敛，获得轮式机器人的位姿信息。

步骤2，根据步骤1获得的轮式机器人的位姿信息，检测候选动态障碍物。

步骤3，根据步骤2检测得到的候选动态障碍物，估计动态障碍物的运动方向及速度。

本发明实施例利用多帧激光点云数据进行特征点配准，解算机器人位姿，以实现局部里程计功能，进而替代轮式编码器或惯性导航单元。由于轮式机器人仅具有三个自由度，故本发明将激光点云的三维数据经算法处理后转化为二维结果进行描述，进而在达到动态避障目的前提下进一步降低算法复杂度。

在一个实施例中，步骤1中，所述提取符合预设曲率特征的点云作为特征点的方法具体包括：

步骤a1，对点云数据进行体素滤波，以在保证形状特征不变的前提下大幅度降低点云的数据量，为降低算法复杂度提供有利条件。

步骤b1，计算点云数据中各点云的曲率值，比如利用下式(1)，并按照曲率值大小排序，曲率值最大的点云定义为特征角点，曲率值最小的点云定义为特征平面点。选择特征角点和特征平面点作为特征点，为后续点云配准做准备。

式中，X_(k，i)表示第k帧点云数据P_k中的第i个点云在雷达坐标系中的坐标，描述为：X_(k，i)＝[x y z]^T；X_(k，j)表示P_k中的第j个点云在雷达坐标系中的坐标；S表示X_(k，i)的邻域，其可以通过KD树等现有方法快速获得。

进一步地，步骤1中，通过分区域提取所述特征点，这样可以保证特征点分布均匀，即将点云云数据划分为不同区域，在每个区域内提取一定阈值数量的特征点。此外，在选取特征点过程中，应当避免将符合以下三种情况的点云被选作为特征点：1)局部平面与激光束大致平行的点云，此类点云可靠性较低，其中的局部平面可以理解为周围(一定邻域内)的点云与该点云共面而形成的平面；2)一定半径范围内(根据经验而定)已存在特征点的点云；3)在该点云位于遮挡区域的边界上。

步骤1中的“建立同一特征点在两帧相邻时刻激光点云数据中的匹配关系”具体包括特征角点和特征线之间的点云线距离

和/或特征平面点和特征面之间的点云面距离

通过

和

得到同一特征点在两帧相邻时刻激光点云数据中的匹配关系，以便计算特征点的相对运动关系。

设定：当前第k帧点云数据表示为P_k，其上一帧(k-1)点云数据表示为P_k-1，对于两类特征点处理分别如下：

对于P_k中的每一个特征角点，下面以第a个特征角点X_(k，a)为例，对步骤1中的建立同一特征点在两帧相邻时刻点云数据中的匹配关系的方法进行说明。

在P_k-1中寻找特征角点X_(k，a)所对应的特征线段，该线段可以由两点云(b，c)表示，点云b为P_k-1中与特征角点X_(k，a)距离最近的点云，点云c是P_k-1中在特征角点X_(k，a)相邻扫描线上与特征角点X_(k，a)距离最近的点云，相邻扫描线即为雷达扫描线中在径向上与该扫描线最为接近的一条扫描线，其中，

表示为下式(2)：

式中，X_(k，a)表示P_k中的第a个特征角点在雷达坐标系中的坐标，X_(k-1，b)表示P_k-1中的第b个特征角点在雷达坐标系中的坐标，X_(k-1，c)表示P_k-1中的第c个特征角点在雷达坐标系中的坐标。

对于P_k中的每一个特征平面点，下面以P_k中的第d个特征平面点X_(k，d)为例，对步骤1中的建立同一特征点在两帧相邻时刻点云数据中的匹配关系的方法进行说明。

在P_k-1中寻找该特征平面点所对应的特征面，特征面可由三点云(e，f，g)表示，点云e为P_k-1中与特征平面点X_(k，d)距离最近的点云，点云f为P_k-1中在点云e同一相邻扫描线上与点云d距离最近的点云，点云g为在相邻扫描线上与特征平面点X_(k，d)距离最近的点云，其中，

表示为下式(3)：

式中，X_(k-1，e)表示P_k-1中的第e个特征平面点在雷达坐标系中的坐标，X_(k-1，f)表示P_k-1中的第f个特征平面点在雷达坐标系中的坐标，X_(k-1，g)表示P_k-1中的第g个特征平面点在雷达坐标系中的坐标。

在一个实施例中，步骤1中，轮式机器人位姿信息可以通过轮式机器人的水平方向平移及偏航方向旋转，来表示相对运动。由于轮式机器人仅有三个自由度，本实施例仅考虑其水平方向平移及偏航方向旋转对应的二维变换矩阵SE(2)，其表示为式(4)：

式中，q_k表示位姿参数，t_x表示轮式机器人在惯性坐标系下x方向的平移量，t_y表示轮式机器人在惯性坐标系下y方向的平移量，θ_z表示轮式机器人在惯性坐标系下绕z轴旋转的角度，

表示结合上述状态量的二维刚体变换矩阵，即SE(2)。

文中采用上标L表示原三维数据去除z轴维度后的二维数据，变换矩阵等同理。

因此，步骤1中，构建代价函数，即：特征角点和特征线以及特征平面点和特征面之间对应几何关系误差函数，以轮式机器人位姿为变量构造ICP问题，获得轮式机器人位姿信息，该方法具体包括：

步骤b1，根据对应点云的距离关系，如上式(2)和式(3)所示，建立代价函数，将位姿解算问题转化成一个非线性最小二乘的求解问题，如下式(5)：

其中，

表示最优变换矩阵估计值，

表示P_k中第p个特征点在雷达坐标系中去除z轴维度后的坐标，q表示特征点的数量；

为

在P_k-1中的去除z轴维度后的齐次坐标，即

表示

在P_k中的去除z轴维度后的齐次坐标。

步骤b2，定义第k帧特征点的质心在雷达坐标系中去除z轴维度后的坐标

以及第k-1帧特征点的质心在雷达坐标系中去除z轴维度后的坐标

步骤b3，为方便表达，将去质心坐标定义为

即将每个特征点都减去质心后的坐标，于是可将上述问题(5)解耦，转化为式(6)表示的问题：

式中，

表示最优的轮式机器人偏航方向旋转矩阵估计值，

表示单次迭代的旋转矩阵，

表示最优的水平方向平移估计值。

步骤b4，通过SVD分解求出上述问题(6)中最优

定义矩阵W为式(7)：

W为一个2×2矩阵，对其进行SVD分解，可得式(8)：

W＝U∑V^T (8)

其中，∑为奇异值对角矩阵，U和V为正交矩阵；

当W满秩时，

表示为式(9)：

若计算得到的

行列式为负，则取

为当前迭代最优值。

步骤b5，按照

得到轮式机器人在x方向和y方向上的平移量。需要说明的是，对于单传感器的轮式机器人而言，可以将雷达坐标系的x方向和y方向作为其方向。

在一个实施例中，步骤2具体包括：

步骤21，根据步骤1输出的轮式机器人位姿信息，提取出可能属于动态障碍物点云。其可以通过如下步骤实现，例如：

步骤211，根据轮式机器人位姿信息，即：q_k＝[t_x，t_y，θ_z]^T，或者，

将当前帧P_k左乘变换矩阵

变换到P_k-1的坐标系中。

步骤212，根据最近邻关系，P_k与P_k-1的交集部分确定为静态点云部分，P_k与P_k-1的非交集部分主要由三部分点云组成：1)属于动态障碍物的点云；2)在上一帧数据中被动态障碍物遮挡的点云；3)新观测到的点云，即首次进入雷达捕获范围物体上的点云。

步骤213，将P_k与P_k-1的非交集部分的点云进行障碍物检测操作，通过观测多帧数据的方式去除属于静态障碍物的目标，从而达到降低算法计算量目的。

步骤22，分割步骤21提取出的可能属于动态障碍物点云中的地面点云与非地面点云，去除地面点云，得到非地面点云。例如，可以将步骤21提取出的可能属于动态障碍物点云放入俯视方向上预设的栅格中，进行点云结构化操作，同时使点云二维化处理，这样可以避免障碍物之间由于地面点云数据造成牵连和噪声干扰。其中，俯视方向可以理解为雷达坐标系Z轴的反方向，即从上到下看。点云结构化操作可以采用比如深度图、KD树或八叉树等现有方法。点云二维化处理可以理解为将原本x、y、z三维信息中的z轴信息归一或者丢掉，由立体三维结构的转化成二维平面点云。

步骤23，将整体的非地面点云按照特定策略聚类为多个点云簇，一个点云簇对应一个障碍物，以区分为各自独立的障碍物。例如：步骤23可以通过如下步骤实现，也可以通过本领域公知的DBSCAN、超体聚类或K-means聚类方法等获得：

步骤231，从步骤22获得的非地面点云中取出一个点云作为聚类种子点云，并对其进行栅格邻域搜索，若在栅格邻域半径ρ内存有点云，则将这些点云存入聚类生长点云队列

步骤232，遍历

中每个点云，进行半径阈值为ρ的邻域栅格搜索，若邻域内存在点云，则将不属于

的点云存入聚类生长过渡队列

遍历完所有点云后，将属于

中点云存入聚类容器

m为障碍物类别。

步骤233，将

中所有点云存入

重复以上步骤直至某次生长时

为空集，则该类别聚类结束。

重复上述步骤，直至所有点云处理完毕，得到所有障碍物聚类信息

将不满足足够阈值数量的类别进行剔除。

步骤24，估计步骤23获得的每个障碍物的位置、形状和尺寸信息。本实施例采用一个二维包围框将每个障碍物框出来，而二维包围框的长宽由其主方向和点云簇中的点云分布共同决定，以描述障碍物的位置、大小和主方向等信息。

例如：步骤24可以通过如下步骤实现，也可以通过本领域公知的最小面积法等方法获得：

步骤241，将步骤23得到的点云簇的质心定义为该障碍物的坐标，第k帧第u个障碍物的位置表示为

步骤242，将移除z轴维度后的障碍物进行随机采样一致性直线分割，将分割出的直线方向作为该障碍物的主方向。

步骤243，利用步骤242确定的主方向建立二维坐标系，比如，设置所述主方向为x′轴，然后设置垂直该x′轴、并且垂直雷达坐标系的z轴y′轴。再根据障碍物所对应点云簇中每个点云的空间信息，计算点云在该坐标系x′、y′两个维度上的极大、极小值。最后将极大值与相应的极小值的差值作为二维包围框的长和宽，即

其中，

表示二维包围框的长，即障碍物的长度；

表示二维包围框的宽，即障碍物的宽度。

在一个实施例中，步骤3具体包括：

步骤31，利用候选动态障碍物信息根据多帧数据中障碍物的差异程度计算差异度函数，并建立关联矩阵，最后通过关联矩阵，可以将多帧点云数据中障碍物关联起来，并成功在多帧数据中关联起来的障碍物即为动态障碍物。

例如：步骤31可以通过如下步骤实现，也可以通过本领域公知的卡尔曼滤波跟踪或基于深度学习等障碍物跟踪方法获得：

步骤311，根据第k帧第u个障碍物的位置为

第k-1帧第v个障碍物位置为

障碍物的长和宽分别由

以及步骤1计算得到的两帧点云数据相对运动为

将差异度函数

定义为下式(10)：

其中，

为机器人在二维空间中位置的齐次坐标表现形式，由于不考虑垂直方向平移，即

α为距离差异权重，β为长宽比差异权重，均取经验值，比如α＝0.6，β＝0.4。

步骤312，根据P_k-1中共检测到的障碍物的总数量为m个，记为

其中，u＝1，2，…，m；P_k中共检测到的障碍物的总数量为n个，记为

其中，v＝1，2，…，n，将关联矩阵

表示为下式(11)：

式中，

表示由式(10)计算的差异度，其中的每一行的第二个下标相同，这意味每一行对应着当前帧的同一个障碍物，那么，第一行的下标均为1，则表示第一障碍物；第二行的下标均为2，则表示第二障碍物；……；第v行的下标均为v，则表示第v障碍物。

步骤313，根据步骤312中建立的关联矩阵(11)，关联当前帧P_k和上一帧P_k-1中的动态障碍物。具体地，关联某一障碍物时，将当前帧P_k的关联矩阵的选定行所对应的障碍物作为选定障碍物，再计算选定行中的各元素分别与上一帧P_k-1中关联矩阵中所有的元素的差异度，并以计算得到的最小差异度所对应的P_k-1中元素所对应的障碍物作为选定障碍物的关联障碍物，此时判定选定障碍物与关联障碍物是同一障碍物。

重复上述步骤，直至当前帧P_k的关联矩阵的每一行对应的障碍物关联完毕。

由于以上操作是在潜在动态点云基础上实现的，故关联成功的障碍物一定为动态障碍物。

进一步地，若最小差异度所对应的P_k-1中元素的下标u＞v，则表明发生障碍物丢失；若最小差异度所对应的P_k-1中元素的下标u＜v，则表明有新动态障碍物出现。

比如：P_k中有10个障碍物，上一帧P_k-1有10个障碍物，并且这10个障碍物在这两帧点云数据中能够一一对应，那么，该关联矩阵为10行10列的矩阵，将第一行的10个元素对应的是当前帧P_k的第一个障碍物作为选定障碍物。而步骤313是将第一行的10个元素与P_k-1的关联矩阵中的所有元素，利用式(10)进行差异度计算，其中，计算得到的最小差异度所对应的P_k-1中元素所对应的障碍物作为选定障碍物的关联障碍物，二者被认定为同一个障碍物。

步骤32，根据步骤31关联的动态障碍物，结合步骤1计算得到的两帧点云数据相对运动为

判断障碍物运动状态，从而得到动态障碍物的运动方向及速度。

例如：步骤32可以通过如下步骤实现，也可以通过本领域公知的其它方法获得：

若某障碍物在连续两帧点云数据中被关联，其在P_k-1中的坐标为t^k-1＝[x^k-1，y^k-1]，P_k中的坐标为t^k＝[x^k，y^k]，步骤1计算得到的两帧点云数据相对运动为

则利用式(12)计算障碍物的运动距离：

根据激光雷达传感器的帧率f，则动态障碍物的速度为V＝Δd×f，方向为v＝t^k-t^k-1，

表示t^k去除z轴维度后的齐次坐标，

表示t^k-1去除z轴维度后的齐次坐标。

经上述步骤可以得到点云数据中动态障碍物信息。

若要得到静态障碍物信息，可建立栅格地图，根据轮式机器人高度将相应范围点云存入栅格地图，若栅格被占据，即表示为轮式机器人不可通过，从而达到在动态环境下高效安全避障目的。

本发明通过障碍物检测及跟踪技术，结合两帧点云数据相对运动，达到检测动态障碍物、提高系统安全性目的。

最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (9)

1.一种考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法，其特征在于，包括：

步骤1，通过激光点云数据的几何特性，提取符合预设曲率特征的点云作为特征点，建立同一特征点在两帧相邻时刻点云数据中的匹配关系，并构建代价函数，以轮式机器人位姿为变量构造ICP问题，获得轮式机器人位姿信息；

步骤2，根据步骤1获得的轮式机器人的位姿信息，检测候选动态障碍物；

步骤3，根据步骤2检测得到的候选动态障碍物，估计动态障碍物状态；

步骤1中，构建代价函数，以轮式机器人位姿为变量构造ICP问题，获得轮式机器人位姿信息的方法具体包括：

步骤b1，考虑其水平方向平移及偏航方向旋转对应的二维变换矩阵

根据对应点云的距离关系，建立代价函数，将位姿解算问题转化成一个非线性最小二乘的求解问题，如下式(5)：

其中，

表示最优变换矩阵估计值，

表示P_k中第p个特征点在雷达坐标系中去除z轴维度后的坐标，q表示特征点的数量，

为

在P_k-1中的去除z轴维度后的齐次坐标，

表示

在P_k中的去除z轴维度后的齐次坐标；

步骤b2，定义第k帧特征点的质心在雷达坐标系中去除z轴维度后的坐标

以及第k-1帧特征点的质心在雷达坐标系中去除z轴维度后的坐标

步骤b3，将去质心坐标定义为

将上述问题(5)解耦，转化为式(6)表示的问题：

式中，

表示最优的轮式机器人偏航方向旋转矩阵估计值，

表示单次迭代的旋转矩阵，

表示最优的水平方向平移估计值；

步骤b4，通过SVD分解求出上述问题(6)中最优

定义矩阵W为式(7)：

W为一个2×2矩阵，对其进行SVD分解，可得式(8)：

W＝UΣV^T (8)

其中，Σ为奇异值对角矩阵，U和V为正交矩阵；

当W满秩时，

表示为式(9)：

若计算得到的

行列式为负，则取

为当前迭代最优值；

步骤b5，按照

得到轮式机器人在x方向和y方向上的平移量。

2.如权利要求1所述的考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法，其特征在于，步骤1中，所述提取符合预设曲率特征的点云作为特征点的方法具体包括：

步骤a1，对点云数据进行体素滤波；

步骤b1，利用下式(1)计算点云数据中各点云的曲率值r，并按照曲率值r的大小排序，曲率值最大的点云定义为特征角点，曲率值最小的点云定义为特征平面点：

式中，X_(k,i)表示第k帧点云数据P_k中的第i个点云在雷达坐标系中的坐标，X_(k,j)表示P_k中的第j个点云在雷达坐标系中的坐标，S表示X_(k,i)的邻域。

3.如权利要求2所述的考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法，其特征在于，所述特征点为特征角点的情形下，步骤1中，所述建立同一特征点在两帧相邻时刻点云数据中的匹配关系的方法具体包括：

在第k-1帧点云数据P_k-1中寻找P_k中的第a个特征角点X_(k,a)对应的特征线段，该线段上具有点云b和点云c，其中，点云b为P_k-1中与特征角点X_(k,a)距离最近的点云，点云c为P_k-1中在特征角点X_(k,a)相邻扫描线上与特征角点X_(k,a)距离最近的点云，点云线距离

表示为下式(2)：

式中，X_(k-1，b)表示P_k-1中的第b个特征角点在雷达坐标系中的坐标，X_(k-1，c)表示P_k-1中的第c个特征角点在雷达坐标系中的坐标；

所述特征点为特征平面点的情形下，步骤1中，所述建立同一特征点在两帧相邻时刻点云数据中的匹配关系的方法具体包括：

在P_k-1中寻找第d个特征平面点X_(k，d)所对应的特征面，该特征面具有点云e、点云f和点云g，点云e为P_k-1中与特征平面点X_(k，d)距离最近的点云，点云f为P_k-1中在点云e同一相邻扫描线上与点云d距离最近的点云，点云g为在相邻扫描线上与特征平面点X_(k，d)距离最近的点云，点云面距离

表示为下式(3)：

式中，X_(k-1，e)表示P_k-1中的第e个特征平面点在雷达坐标系中的坐标，X_(k-1，f)表示P_k-1中的第f个特征平面点在雷达坐标系中的坐标，X_(k-1，g)表示P_k-1中的第g个特征平面点在雷达坐标系中的坐标。

4.如权利要求3所述的考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法，其特征在于，步骤2具体包括：

步骤21，根据步骤1输出的轮式机器人位姿信息，提取出可能属于动态障碍物点云；

步骤22，分割步骤21提取出的可能属于动态障碍物点云中的地面点云与非地面点云，去除地面点云，得到非地面点云；

步骤23，将整体的非地面点云按照特定策略聚类为多个点云簇，一个点云簇对应一个障碍物；

步骤24，估计步骤23获得的每个障碍物的位置、形状和尺寸信息；

其中，步骤21具体包括：

步骤211，根据里程计得到的位姿信息，将当前帧点云数据P_k左乘变换矩阵

变换到P_k-1的坐标系中；

步骤212，根据最近邻关系，P_k与P_k-1的交集部分确定为静态点云部分；

步骤213，将P_k与P_k-1的非交集部分的点云进行障碍物检测操作，通过观测多帧数据的方式去除属于静态障碍物的目标。

5.如权利要求4所述的考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法，其特征在于，步骤23具体包括：

步骤231，从步骤22获得的非地面点云中取出一个点云作为聚类种子点云，并对其进行栅格邻域搜索，若在栅格邻域半径ρ内存有点云，则将这些点云存入聚类生长点云队列

步骤232，遍历

中每个点云，进行半径阈值为ρ的邻域栅格搜索，若邻域内存在点云，则将不属于

的点云存入聚类生长过渡队列

遍历完所有点云后，将属于

中点云存入聚类容器

m为障碍物类别；

步骤233，将

中所有点云存入

重复以上步骤直至某次生长时

为空集，则该类别聚类结束；

重复上述步骤，直至所有点云处理完毕，得到所有障碍物聚类信息

将不满足足够阈值数量的类别进行剔除。

6.如权利要求4所述的考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法，其特征在于，步骤24具体包括：

步骤241，将步骤23得到的点云簇的质心定义为该障碍物的坐标；

步骤242，将移除z轴维度后的点云进行随机采样一致性直线分割，将分割出的直线方向作为该障碍物主方向；

步骤243，利用步骤242确定的主方向建立二维坐标系，再根据障碍物所对应点云簇中每个点云的空间信息，计算点云在该坐标系x′、y′两个维度上的极大、极小值，并将极大值与相应的极小值的差值作为障碍物的长和宽。

7.如权利要求4所述的考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法，其特征在于，步骤3具体包括：

步骤31，利用候选动态障碍物信息根据多帧数据中障碍物的差异程度计算差异度函数，并建立关联矩阵；

步骤32，根据动态障碍物的关联情况，结合激光里程计计算得到的帧间相对运动，判断障碍物运动状态，从而得到动态障碍物的运动方向及速度。

8.如权利要求7所述的考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法，其特征在于，步骤31具体包括：

步骤311，根据第k帧第u个障碍物的位置为

第k-1帧第v个障碍物位置为

障碍物的长和宽分别由

以及步骤1计算得到的两帧点云数据相对运动为

将差异度函数

定义为下式(10)：

其中，

为机器人在二维空间中位置的齐次坐标表现形式，α为距离差异权重,β为长宽比差异权重；

步骤312，P_k-1中共检测到的障碍物的总数量为m个，记为

其中，u＝1,2,…,m；P_k中共检测到的障碍物的总数量为n个，记为

其中，v＝1,2,…,n，将关联矩阵

表示为下式(11)：

式中，

表示由式(10)计算的差异度；

步骤313，根据步骤312中建立的关联矩阵(11)，关联当前帧和上一帧点云数据中的动态障碍物，若最小差异度所对应的P_k-1中元素的下标u＞v，则表明发生障碍物丢失；若最小差异度所对应的P_k-1中元素的下标u＜v，则表明有新动态障碍物出现。

9.如权利要求8所述的考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法，其特征在于，步骤32具体包括：

有步骤31关联的障碍物在P_k-1中的坐标为t^k-1＝[x^k-1,y^k-1]，P_k中的坐标为t^k＝[x^k,y^k]，步骤1计算得到的两帧点云数据相对运动为

则利用式(12)计算障碍物的运动距离：

根据激光雷达传感器的帧率f，则动态障碍物的速度为V＝Δd×f，方向为v＝t^k-t^k-1，

表示t^k去除z轴维度后的齐次坐标，

表示t^k-1去除z轴维度后的齐次坐标。

Images