(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210667977.3 (22)申请日 2022.06.14 (71)申请人 中国矿业大 学 地址 221116 江苏省徐州市铜山区大 学路 一号 (72)发明人 吴保磊　赵子豪　郭义鑫　张文琪　 王军　 (74)专利代理 机构 南京理工大 学专利中心 32203 专利代理师 朱沉雁 (51)Int.Cl. G06T 7/00(2017.01) G06T 3/00(2006.01) G06T 7/246(2017.01) G06V 10/762(2022.01) (54)发明名称 一种基于三维点云的机器人动态障碍物实 时检测方法 (57)摘要 本发明公开了一种基于三维点云的动态障 碍物实时检测方法， 属于人工智 能领域， 首先根 据三维点云数据的曲率大小提取三维点云数据 中特征点， 构建机器人运动状态代 价函数并进行 非线性优化； 然后， 对三维点云数据进行降采样、 地面分割和非地面三维点云聚类， 获得多个扫描 周期非地面三维点云数据中的障碍物 位置信息， 并结合机器人的位姿将障碍物位置信息投影至 当前扫描周期中非地面三维点云所对应的坐标 系； 其次， 使用基于非地面三维点云数据的IoU ‑ Tracker实现数据关联， 进行多个障碍物跟踪并 获取每个障碍物的轨迹信息； 最后， 根据获取每 个障碍物轨迹信息检测出动态障碍物。 本发明使 用激光雷达实现动态障碍物的检测， 实时性强， 准确率高。 权利要求书3页 说明书5页 附图3页 CN 114998276 A 2022.09.02 CN 114998276 A 1.一种基于三维点云的动态障碍物实时检测方法， 其特 征在于， 步骤如下： 步骤S1， 将激光雷达固定安装于机器人上， 采集原始激光雷达三维点云数据集P＝{P1， P2， ...， Pk， Pk+1， ...}， Pk为第k次扫描周期的点云数据集， 计算数据集P中每个点的曲率， 按 照曲率大小进行排序， 选取曲率大于阈值cmax的加入边缘特征点集ε＝{ε1， ε2， ...， εk， εk+1， ...}， εk为第k次扫描周 期的边缘特征点集， 曲率小于cmin的点加入平面特征点集ρ＝ {ρ1， ρ2， ...， ρk， ρk+1， ...}， ρk为第k次扫描周期的平面特征点集； 根据边缘特征点集ε与平面 特征集ρ 构建关于机器人运动状态的代 价函数， 求解机器人第1， 2， ...， k， k +1次扫描周期的 位姿变换矩阵T1， T2， ....， Tk， Tk+1， 进入步骤S2； 步骤S2， 对三维点云数据集P进行降采样、 地面分割和 非地面点云聚类， 得到多个扫描 周期的包含静态和动态障碍物三维包围框： D＝{D1， D2， ...Dk...DK}＝{{d1， d2， ...， dn}， {d1， d2， ...， dn}， ...}， D为多个扫描周期检测到的静态和动态障碍物集合， Dk指的是第k次扫描 周期检测到 的静态和动态障碍物集合， dn为其中一个扫描周期中检测到的第n个静态和动 态障碍物， 进入步骤S3； 步骤S3， 结合机器人的位姿变换矩阵Tk‑3、 Tk‑2、 Tk‑1， 将第k‑3、 k‑2、 k‑1个扫描周期的静 态和动态障碍物三维包围框 分别投影到第k次扫描周期的静态和 动态障碍物三维包围框所 在坐标系中， 对应得到第k ‑3、 k‑2、 k‑1个扫描周期融合后静态和动态障碍物三维包围框Dk ＝{Dk‑3， Dk‑2， Dk‑1}＝{{d1， d2， ...， dn}， {d1， d2， ...， dn}， {d1， d2， ...， dn}}， 进入步骤S4； 步骤S4， 根据融合后静态和动态障碍物三维包围框， 使用基于非地面三维点云的IoU ‑ Tracker实现数据关联， 进行静态和动态障碍物跟踪， 获取静态和动态障碍物的历史轨迹， 进入步骤S5； 步骤S5， 根据静态和动态障碍物的历史轨 迹， 检测出动态障碍物。 2.根据权利要求1所述的一种基于三维点云的机器人动态障碍物实时检测方法， 其特 征在于， 步骤S1中， 将激光雷达固定安装于机器人上， 采集原始激光雷达三维点云数据集P ＝{P1， P2， ...， Pk， Pk+1， ...}， Pk为第k次扫描周期的点云数据集， 计算数据集P中每个点的曲 率， 按照曲率大小进行排序， 选取曲率大于阈值cmax的加入边缘特征点集 ε＝{ ε1， ε2， ...， εk， εk+1， ...}， εk为第k次扫描周 期的边缘特征点集， 曲率小于cmin的点加入平面特征点集ρ＝ {ρ1， ρ2， ...， ρk， ρk+1， ...}， ρk为第k次扫描周期的平面特征点集； 根据边缘特征点集ε与平面 特征集ρ 构建关于机器人运动状态的代 价函数， 求解机器人第1， 2， ...， k， k +1次扫描周期的 位姿变换矩阵T1， T2， ....， Tk， Tk+1， 具体如下： 步骤S1.1， 计算原 始激光雷达三维点云数据集P中每 个点的曲率c： 其中， S为点i最近邻点集， 为激光雷达坐标系L下的第k次扫描周期中点i的坐标， 表示在激光雷达坐标系L下的第k次扫描周期中点j的坐标， j∈S， 进入步骤S1.2； 步骤S1.2， 将曲率c按照大小进行排序， 选取曲率大于阈值cmax的加入边缘特征点集ε＝ { ε1， ε2， ...， εk， εk+1， ...}， εk为第k次扫描周期的边缘特征点集， 曲率小于cmin的点加入平面 特征点集ρ＝{ρ1， ρ2， ...， ρk， ρk+1， ...}， ρk为第k次扫描周期的平面特征点集， 进入步骤 S1.3；权　利　要　求　书 1/3 页 2 CN 114998276 A 2步骤S1.3， 假设机器人的初始位姿变换矩阵为T0＝0， 利用 计算得到第1次扫描周期位姿变换矩阵T1； 根据位姿变换矩阵T1， 利用 计算得到第 2次扫描周期的位姿变换矩阵T2， 不断迭代收敛， 得到第k、 k+1次扫描周期位姿变换矩阵为Tk、 Tk+1， 利用位姿变换矩阵Tk把第k 次扫描周期的三维点云Pk投影至第k+1周期初始时刻tk+1， 得到第k次扫描周期的三维投影 点云 第k次扫描周期的边缘特征点集 第k次扫描周期的平面特征点集 利用位姿变 换矩阵Tk+1把第k+1次扫描周期的三维点 云Pk+1投影第k+1周期初始时刻tk+1， 得到第k +1次扫 描周期的三维投影点云 第k+1次扫描周期的边缘特征点集 第k+1次扫描周期的平 面特征点集 进入步骤S1.4； 步骤S1.4， 根据边缘特征点集 平面特征点集 构建机器人位姿代 价函数 dε和dp： 其中， 点i1为 中一个点， 点j1为点i1最近邻点， 点l1为连续两次扫 描周期中点j1的最 近邻点， j1≠l1， dε为点i1到直线j1l1的距离， 为点i1在激光雷达坐标系L下 的坐标， 为点j1在激光雷达坐标系L下的坐标， 为点l1在激光雷达坐标系L下的坐 标； 其中， 点i2为 中一个点， 点j2、 点l2为点i2的两个不同的最近邻点， j2≠l2， 点m为点i2 的最近邻点， 点m在点j2的连续两次扫描周期中， dρ为点i2到平面j2l2m的距离， 为点i2在激光雷达坐标系L下的坐标， 为点j2在激光雷达坐标系L下的坐标， 为点l2在激光雷达坐标系L下的坐标， 为点m在激光雷达坐标系L下的坐标， 进入 步骤S1.5； 步骤S1.5， 利用代价函数dε和dp构建距离模型d， d是关于位姿变换Tk+1的非线性函数f (Tk+1)＝d， 使用最小二乘法优化 直到|d|的值接 近0， 求解机器人的位姿变换Tk+1： 其中， Tk+1为机器人的位姿变换， λ为拉格朗日乘子， 为雅可比矩阵。 3.根据权利要求2所述的一种基于三维点云的机器人动态障碍物实时检测方法， 其特 征在于， 步骤S4， 根据融合后静态和动态障碍物三维包围框， 使用基于非地面三维点云的 IoU‑Tracker实现数据关联， 进行静态和 动态障碍物跟踪， 获取静态和 动态障碍物的历史轨 迹， 具体如下： 步骤S4.1， 初始化 集合Ta和Tf， Ta用于存储检测到的静态和动态障碍物所有轨 迹， Tf为保存Ta中有效的轨迹并作为静态和动态障碍物跟踪的结 果， Tf＝{lli|lli＞lth， lli∈Ta}， 进入步骤S4.2；权　利　要　求　书 2/3 页 3 CN 114998276 A 3