(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211123948.7 (22)申请日 2022.09.15 (71)申请人 浙江大学 地址 310058 浙江省杭州市西湖区余杭塘 路866号 (72)发明人 刘之涛　洪峰　苏宏业　 (74)专利代理 机构 杭州求是专利事务所有限公 司 33200 专利代理师 林超 (51)Int.Cl. G06T 7/73(2017.01) G06V 10/26(2022.01) G06V 10/762(2022.01) G06V 20/58(2022.01) G01C 21/00(2006.01) (54)发明名称 自动驾驶场景下具有多目标追踪能力的动 态SLAM方法 (57)摘要 本发明公开了一种自动驾驶场景下具有多 目标追踪能力的动态SLAM 方法。 包括： 首先， 提取 相机采集的图像序列的各帧语义 分割掩膜， 同时 通过光流方法得到对应的光流结果； 接着对雷达 采集的点云进行点云 ‑图像投影， 将图像对应的 点云帧中的点通过坐标变换转换到像素坐标系； 再提取各帧中的静态点云和不同车辆对应的点 云； 再进行车载相机所在车辆的初始位姿序列以 及各图像帧中不同车辆的初始相对变换矩阵； 进 而利用已有信息构建图优化问题进一步优化自 身和图像中车辆的位姿结果； 最后获得目标位姿 信息。 本发明采用激光雷达和相机作为传感器， 结合了语义 分割信息， 能够在动态环 境下稳定运 行并输出画面中车辆的运动状态， 具有较高的实 际运用价 值。 权利要求书3页 说明书7页 附图4页 CN 115482282 A 2022.12.16 CN 115482282 A 1.一种自动驾驶场景下具有多目标追踪能力的动态SLAM方法， 其特征在于， 包括以下 步骤： 步骤1： 将车载相机采集的图像序列输入到语义分割网络 中， 获得图像序列中各图像帧 的语义分割掩膜， 同时通过光流方法对车载相 机采集的图像序列进行处理后， 获得当前图 像序列的光 流结果； 步骤2： 将车载激光雷达采集的初始点云序列与车载相机采集的图像序列之间进行点 云‑图像投影， 获得3D点云序列； 步骤3： 根据图像序列中各图像帧的语义分割掩膜和3D点云序列中的2D信息， 对3D点云 序列中各帧3D点云进行点云语义分割， 获得各帧3D点云的车辆点云和静态点云， 对各帧3D 点云的车辆点云进行聚类后， 获得 各帧3D点云中不同车辆对应的点云； 步骤4： 根据图像序列中第一个相邻的两帧图像帧、 3D点云序列中对应帧的3D点云的静 态点云和不同车辆对应的点云确定 当前两帧的静态特征点和 动态特征点， 进而根据当前两 帧对应的光流结果确定车载相机所在车辆在当前相邻帧间的初始位姿变换以及不同车辆 在当前相邻帧间的初始相对变换矩阵； 步骤5： 根据当前两帧图像帧的静态特征点与动态特征点、 车载相机所在车辆在 当前相 邻帧间的初始位姿变换和 不同车辆在当前相邻帧间的初始相对变换矩阵构建图优化问题 并求解， 获得车载相机所在车辆在当前相 邻帧间的优化后的位姿变换以及不同车辆在当前 两帧的后一帧时的优化后的相对变换矩阵； 步骤6： 将车载相机所在车辆的优化后的位姿变换作为车载相机的最终位姿变换， 根据 车载相机所在车辆在当前相邻帧间的优化后的位姿变换以及不同车辆在当前相邻帧间的 优化后的相对变换矩阵， 计算获得当前相邻帧间不同车辆的位姿信息， 再根据当前相邻帧 间不同车辆的位姿信息计算对应车辆的速度； 步骤7： 重 复步骤4‑步骤6， 根据图像序列中剩余图像帧、 3D点云序列中对应帧3D点云的 静态点云和不同车辆对应的点云以及光流结果依次进 行相邻帧的位姿计算以及 优化， 获得 各帧中车 载相机的位姿变换、 不同车辆的位姿信息以及对应的速度。 2.根据权利要求1所述的一种自动驾驶场景下具有多目标追踪能力的动态SLAM方法， 其特征在于， 所述步骤1中， 各图像帧的语义分割掩膜中属于车辆的像素标记 为1， 含有除车 辆外的动态元 素的像素 标记为‑1， 剩余像素被标记为0 。 3.根据权利要求1所述的一种自动驾驶场景下具有多目标追踪能力的动态SLAM方法， 其特征在于， 所述步骤2中， 根据车载激光雷达到车载相机的外参将雷达坐标系下的初始 点 云序列转换到相机坐标系下， 再根据相机的内参将相机坐标系下的初始 点云序列转换到像 素坐标系下， 从而获得3D点云序列。 4.根据权利要求1所述的一种自动驾驶场景下具有多目标追踪能力的动态SLAM方法， 其特征在于， 所述步骤3中， 首先将各帧3D点云的车辆点云构建为对应的K ‑D树， 接着设定搜 索半径后对各帧K ‑D树进行欧式聚类分割， 获得 各帧3D点云中不同车辆对应的点云。 5.根据权利要求1所述的一种自动驾驶场景下具有多目标追踪能力的动态SLAM方法， 其特征在于， 所述 步骤4具体为： 步骤4.1： 将当前两帧3D点云的静态点云在对应图像帧上的像素位置作为当前两帧图 像帧的静态特征点， 将当前两帧3D点云中不同车辆对应的点云在 对应图像帧上的像素位置权　利　要　求　书 1/3 页 2 CN 115482282 A 2作为当前两帧图像帧的动态特 征点； 步骤4.2： 利用光流结果将当前两帧图像帧的静态特征点进行匹配， 获得当前两帧2D光 流静态匹配结果， 利用光流结果将当前两帧图像 帧的动态特征点进行匹配， 获得当前两 帧 2D光流动态匹配结果； 步骤4.3： 将 当前相邻的两帧记为t ‑1时刻和t时刻， 进行t ‑1时刻的静态特征点到t时刻 的2D光流静态匹配结果的2D坐标匹配， 得到t ‑1时刻静态特征点到t时刻2D坐标的匹配 关系 并作为3D ‑2D静态匹配结果， 基于3D ‑2D静态匹配结果通过PnP和随机抽样一致算法求解得 到车载相机所在车辆的在t ‑1时刻到t时刻之间的位姿变换T； 步骤4.4： 利用t时刻的2D光流动态匹配结果检查将t ‑1时刻不同车辆对应的点云与t时 刻的不同车辆对应的点云是否构成匹配， 若构成匹配后再进 行t‑1时刻的动态特征点到t时 刻的2D光流动态匹配结果的2D坐标匹配， 得到t ‑1时刻动态特征点到t时刻2D坐标的匹配关 系并作为3D ‑2D动态匹配结果， 基于3D ‑2D动态匹配结果通过PnP和随机抽样一致算法求解 得到不同车辆在 在t‑1时刻到t时刻之间的初始相对变换矩阵H 。 6.根据权利要求1所述的一种自动驾驶场景下具有多目标追踪能力的动态SLAM方法， 其特征在于， 所述 步骤5具体为： 步骤5.1： 根据当前两帧图像帧的静态特征点和动态特征点、 车载相机所在车辆的在 当 前相邻帧间的初始位姿变换和 不同车辆在当前相邻帧间的初始相对变换矩阵构建图优化 问题， 图优化问题具体为： 根据车载相机所在车辆的在t ‑1时刻到t时刻之间的位姿变换T和t ‑1时刻的位姿确定 车载相机所在车辆的t时刻的位姿， 由时刻t ‑k到时刻t构成长度为k的时间窗口， 在当前时 间窗口中， 将车载相机所在车辆在各个时刻的位姿、 各图像帧的静态特征点、 各图像帧的动 态特征点以及各图像帧中不同车辆的初始相对变换矩阵H均构建为图优化的顶点； 在当前时间窗口中， 将车载相机所在车辆在各个时刻的位姿和对应图像帧的静态特征 点之间的投影误差构建成对应顶点之 间的边； 将车载相机所在车辆在相 邻时刻的位姿之间 的位姿变换T构成对应顶点之间的边； 将车载相 机所在车辆在每一时刻的位姿和对应时刻 动态特征点之 间的投影误差构建成对应顶点之 间的边； 将相 邻图像帧的动态特征点及 对应 车辆的初始相对变换矩阵H之间的变换误差构成对应顶点之间的边； 步骤5.2： 对当前时间窗口中构建得到的图优化问题进行求解， 获得车载相机所在车辆 在当前相邻帧间的优化后的位姿变换以及不同车辆在当前两 帧的后一帧时的优化后的相 对变换矩阵。 7.根据权利要求1所述的一种自动驾驶场景下具有多目标追踪能力的动态SLAM方法， 其特征在于， 所述 步骤6中， 当前相邻帧间各个车辆的位姿信息的计算公式如下： Tcar＝H‑1*T 其中， H表示每个车辆的优化后的相对变换矩阵， T表示t时刻车载相机所在车辆的优化 后的位姿变换， Tcar表示t‑1时刻到t时刻中当前 车辆的位姿信息 。 8.根据权利要求1所述的一种自动驾驶场景下具有多目标追踪能力的动态SLAM方法， 其特征在于， 所述 步骤6中， 各个车辆的速度的计算公式如下： v＝tcar‑(I‑Rcar)ct‑1 其中， tcar表示每个车辆的位姿信息Tcar中的平移部分， I表示3*3的单位矩阵， Rcar表示权　利　要　求　书 2/3 页 3 CN 115482282 A 3