自动驾驶场景下具有多目标追踪能力的动态SLAM方法

自动驾驶场景下具有多目标追踪能力的动态slam方法
技术领域
1.本发明属于人工智能与机器人和计算机视觉领域的一种自动驾驶场景下的slam方法，具体涉及到一种自动驾驶场景下基于语义分割和多传感数据的多目标追踪slam定位方法。


背景技术：

2.同时定位和建图(simultaneous localization and mapping,以下简称slam)是一种在未知环境下，通过携带传感器的个体的观测作为输入，实时估计自身运动状态并构建周围环境模型的方法。slam目前在无人驾驶、机器人、高精度地图构建、无人机和ar等领域有着广泛的应用。
3.slam按照使用传感器的不同可主要分为雷达slam和视觉slam，雷达slam目前以激光雷达和毫米波雷达为主，发展较为成熟，可以实现稳定的定位建图效果且能够提供较为准确的深度信息，但是无法像视觉一样给出丰富的语义信息；视觉slam以单目、双目和rgbd相机为主，其成本低廉、实时性好、应用前景广泛，但是会受到光照等条件的影响。两种方法存在自己的优点，将两者结合可以得到更好的效果；
4.目前的slam方法大多存在静态环境假设，即传感器观测的结果都为静态成分，算法在此基础上通过几何等方法实现定位和建图效果。然而真实的场景往往不满足静态环境假设，当面对存在动态要素的场景时，现有的方法会受到动态元素的影响产生误匹配并随着时间不断累积误差，导致slam算法失效，因而在实际应用时无法满足精度和鲁棒性要求。同时，由于现有的方法专注于构建静态要素地图，故无法满足诸如自动驾驶场景下更进一步的需求。因为作为前端的slam算法需要为后端规划层提供更加丰富的信息，如：周围车辆的位置和速度等，以完成更好的决策行为。
5.现有应对动态场景的slam算法有通过几何信息和语义信息两种。利用几何信息的方法将前一帧的特征点投影到当前帧并计算重投影误差，通过重投影误差的大小判断是否是动态特征点，从而剔除动态部分，该方法能够对动态和静态做出筛选但是由于重投影误差同样受到动态要素的影响因此提升的精度有限；结合语义信息的方法利用事先训练好的语义神经网络处理图像，利用目标分割或者语义分割的结果获取图片中的语义信息，再根据语义信息对图像中的信息做有选择的使用，该方法能够很大程度减弱动态要素的影响，但存在无法剔除潜在运动物体以及某些时候图片可用信息过少的问题。剔除动态要素是目前的slam方法处理动态的主要思路，这导致了动态语义信息的低效利用。


技术实现要素：

6.本发明的目的是提供一种自动驾驶场景下基于语义分割和多传感器数据的多目标追踪slam定位方法。该方法以激光雷达和相机采集的数据作为输入，结合了目标检测技术，利用雷达的深度信息和目标检测得到的语义信息构建3d物体，对得到的静态成分和3d物体分别计算位姿变换，并通过图优化方法结合计算得到的信息优化位姿结果，得到可追
踪动态物体的激光相机融合slam系统，经验证，该算法能够在含有动态元素的自动驾驶场景下完成有效定位并追踪图像中的车辆，具有较高的实际应用价值。
7.上述的目的通过以下的技术方案实现：
8.步骤1：将车载相机采集的图像序列输入到语义分割网络中，获得图像序列中各图像帧的语义分割掩膜，同时通过光流方法对车载相机采集的图像序列进行处理后，获得当前图像序列的光流结果；
9.步骤2：将车载激光雷达采集的初始点云序列与车载相机采集的图像序列之间进行点云-图像投影，获得3d点云序列；
10.步骤3：根据图像序列中各图像帧的语义分割掩膜和3d点云序列中的2d信息，对3d点云序列中各帧3d点云进行点云语义分割，获得各帧3d点云的车辆点云和静态点云，对各帧3d点云的车辆点云进行聚类后，获得各帧3d点云中不同车辆对应的点云；
11.步骤4：根据图像序列中第一个相邻的两帧图像帧、3d点云序列中对应帧的3d点云的静态点云和不同车辆对应的点云确定当前两帧的静态特征点和动态特征点，进而根据当前两帧对应的光流结果确定车载相机所在车辆在当前相邻帧间的初始位姿变换以及不同车辆在当前相邻帧间的初始相对变换矩阵；
12.步骤5：根据当前两帧图像帧的静态特征点与动态特征点、车载相机所在车辆在当前相邻帧间的初始位姿变换和不同车辆在当前相邻帧间的初始相对变换矩阵构建图优化问题并求解，获得车载相机所在车辆在当前相邻帧间的优化后的位姿变换以及不同车辆在当前两帧的后一帧时的优化后的相对变换矩阵；
13.步骤6：将车载相机所在车辆的优化后的位姿变换作为车载相机的最终位姿变换，根据车载相机所在车辆在当前相邻帧间的优化后的位姿变换以及不同车辆在当前相邻帧间的优化后的相对变换矩阵，计算获得当前相邻帧间不同车辆的位姿信息，再根据当前相邻帧间不同车辆的位姿信息计算对应车辆的速度；
14.步骤7：重复步骤4-步骤6，根据图像序列中剩余图像帧、3d点云序列中对应帧3d点云的静态点云和不同车辆对应的点云以及光流结果依次进行相邻帧的位姿计算以及优化，获得各帧中车载相机的位姿变换、不同车辆的位姿信息以及对应的速度。
15.所述步骤1中，各图像帧的语义分割掩膜中属于车辆的像素标记为1，含有除车辆外的动态元素的像素标记为-1，剩余像素被标记为0。
16.所述步骤2中，根据车载激光雷达到车载相机的外参将雷达坐标系下的初始点云序列转换到相机坐标系下，再根据相机的内参将相机坐标系下的初始点云序列转换到像素坐标系下，从而获得3d点云序列。
17.所述步骤3中，首先将各帧3d点云的车辆点云构建为对应的k-d树，接着设定搜索半径后对各帧k-d树进行欧式聚类分割，获得各帧3d点云中不同车辆对应的点云。
18.所述步骤4具体为：
19.步骤4.1：将当前两帧3d点云的静态点云在对应图像帧上的像素位置作为当前两帧图像帧的静态特征点，将当前两帧3d点云中不同车辆对应的点云在对应图像帧上的像素位置作为当前两帧图像帧的动态特征点；
20.步骤4.2：利用光流结果将当前两帧图像帧的静态特征点进行匹配，获得当前两帧2d光流静态匹配结果，利用光流结果将当前两帧图像帧的动态特征点进行匹配，获得当前
两帧2d光流动态匹配结果；
21.步骤4.3：将当前相邻的两帧记为t-1时刻和t时刻，进行t-1时刻的静态特征点到t时刻的2d光流静态匹配结果的2d坐标匹配，得到t-1时刻静态特征点到t时刻2d坐标的匹配关系并作为3d-2d静态匹配结果，基于3d-2d静态匹配结果通过pnp和随机抽样一致算法求解得到车载相机所在车辆的在t-1时刻到t时刻之间的位姿变换t；
22.步骤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。
23.所述步骤5具体为：
24.步骤5.1：根据当前两帧图像帧的静态特征点和动态特征点、车载相机所在车辆的在当前相邻帧间的初始位姿变换和不同车辆在当前相邻帧间的初始相对变换矩阵构建图优化问题，图优化问题具体为：
25.根据车载相机所在车辆的在t-1时刻到t时刻之间的位姿变换t和t-1时刻的位姿确定车载相机所在车辆的t时刻的位姿，由时刻t-k到时刻t构成长度为k的时间窗口，在当前时间窗口中，将车载相机所在车辆在各个时刻的位姿、各图像帧的静态特征点、各图像帧的动态特征点以及各图像帧中不同车辆的初始相对变换矩阵h均构建为图优化的顶点；
26.在当前时间窗口中，将车载相机所在车辆在各个时刻的位姿和对应图像帧的静态特征点之间的投影误差构建成对应顶点之间的边；将车载相机所在车辆在相邻时刻的位姿之间的位姿变换t构成对应顶点之间的边；将车载相机所在车辆在每一时刻的位姿和对应时刻动态特征点之间的投影误差构建成对应顶点之间的边；将相邻图像帧的动态特征点及对应车辆的初始相对变换矩阵h之间的变换误差构成对应顶点之间的边；
27.步骤5.2：对当前时间窗口中构建得到的图优化问题进行求解，获得车载相机所在车辆在当前相邻帧间的优化后的位姿变换以及不同车辆在当前两帧的后一帧时的优化后的相对变换矩阵。
28.所述步骤6中，当前相邻帧间各个车辆的位姿信息的计算公式如下：
29.t
car
＝h-1
*t
30.其中，h表示每个车辆的优化后的相对变换矩阵，t表示t时刻车载相机所在车辆的优化后的位姿变换，t
car
表示t-1时刻到t时刻中当前车辆的位姿信息。
31.所述步骤6中，各个车辆的速度的计算公式如下：
32.v＝t
car-(i-r
car
)c
t-1
33.其中，t
car
表示每个车辆的位姿信息t
car
中的平移部分，i表示3*3的单位矩阵，r
car
表示当前车辆的位姿信息t
car
中的旋转部分，c
t-1
表示当前车辆在t-1时刻的3d点云的质心坐标。
34.本发明的有益效果为：
35.针对传统slam无法应对动态场景的情况，本发明提出了一种自动驾驶场景下具有多目标追踪能力的动态slam方法，有效地克服了传统slam方法在动态目标干扰下不稳定，精度低的缺点，且能够准确估计自动驾驶场景下动态车辆的运动。同时引入光流方法和深
度学习中的语义分割方法，结合了自动驾驶车辆搭载的雷达和图片信息，完成了鲁棒的信息融合。本发明不同于主流的利用语义去除动态信息的动态slam研究，其利用了场景中的动态物体信息并为自动驾驶研究后续的规划控制部分提供了诸如目标位姿和目标速度等更丰富的场景信息。
附图说明
36.图1为本发明方法的流程图。
37.图2为本发明方法的输入的一张原始图片。
38.图3为本发明方法的光流处理结果图片。
39.图4为本发明方法的语义处理结果图片。
40.图5为本发明方法的点云聚类结果图片。
41.图6为本发明方法的动态/静态特征点特征点处理结果图片。
42.图7为本发明方法的多目标追踪动态slam的几何关系图片。
43.图8为本发明方法使用kitti tracking数据集的train 0003序列对图像中车辆的速度估计的结果图。
44.图9为本发明方法使用kitti tracking数据集的train 0003序列对携带传感器车辆和画面中车辆的追踪定位效果图。
具体实施方式
45.下面将结合本发明给出的附图，对本发明实施例中的技术方案进行清楚、完整地描述。显然，所给出地实施例只是本发明一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
46.相比与传统的slam，该算法结合了激光点云的深度信息和图片的语义信息，利用点云聚类方法对图像中的目标进行建模，并纳入slam的计算流程，实现携带传感器车辆和图像中目标车辆的同时定位。
47.如图1所示，本发明包括以下步骤：
48.步骤1：将车载相机采集的图像序列输入到语义分割网络中，获得图像序列中各图像帧的语义分割掩膜，同时通过光流方法对车载相机采集的图像序列进行处理后，获得当前图像序列的光流结果；具体地，将每一时刻图像帧与下一时刻图像帧一起输入光流网络或者传统光流算法中后，获得当前时刻图像帧中各个像素的光流向量，由所有图像帧中各个像素的光流向量组成当前图像序列的光流结果，如图3所示。
49.步骤1中，对于如图2所示的一张rbg图片，将其输入语义分割网络，如图4所示，各图像帧的语义分割掩膜中属于车辆的像素标记为1，含有除车辆外的动态元素的像素标记为-1，剩余像素被标记为0。
50.步骤2：将车载激光雷达采集的初始点云序列与车载相机采集的图像序列之间进行点云-图像投影，获得3d点云序列；
51.步骤2中，根据车载激光雷达到车载相机的外参将雷达坐标系下的初始点云序列转换到相机坐标系下，再根据相机的内参将相机坐标系下的初始点云序列转换到像素坐标
系下，从而获得3d点云序列，3d点云序列中的点包含深度信息和图像的像素坐标(即2d信息)。
52.具体地：激光雷达每次扫描会获得n个雷达坐标系下的数据点，每个数据点在激光雷达坐标系下的坐标p
l
为p
l
＝[x
l
,y
l
,z
l
]，x
l
,y
l
,z
l
分别表示数据点在雷达坐标系下x，y，z轴上的数值，通过激光雷达到相机的外参rt可以转换到相机坐标系，当前数据点在相机坐标系下的坐标pc表示为：
[0053]
pc＝rt*p
l
[0054]
对于当前数据点在相机坐标系下的坐标pc＝[xc,yc,zc]，xc,yc,zc分别表示数据点在相机坐标系下x，y，z轴上的数值，通过相机的内参可以转换到像素坐标系，表示为：
[0055][0056]
其中，u表示数据点在像素坐标系下的横向数值，v表示数据点在像素坐标系下的纵向数值，f
x
表示相机内参中横向的缩放量，c
x
表示表示相机内参中横向的平移量，fy表示相机内参中纵向的缩放量，cy表示表示相机内参中纵向的平移量。
[0057]
步骤3：根据图像序列中各图像帧的语义分割掩膜和3d点云序列中的2d信息，对3d点云序列中各帧3d点云进行点云语义分割，获得各帧3d点云的车辆点云和静态点云，具体地，首先根据各图像帧的语义分割掩膜对对应的各图像帧进行语义分割后，获得3d点云序列中各个点在图像上的2d坐标对应的语义信息，再根据2d坐标对应的语义信息确定3d点云序列的车辆点云和静态点云；对各帧3d点云的车辆点云进行聚类后，获得各帧3d点云中不同车辆对应的点云；
[0058]
步骤3中，如图5所示，首先将各帧3d点云的车辆点云构建为对应的k-d树，接着设定搜索半径后对各帧k-d树进行欧式聚类分割，获得各帧3d点云中不同车辆对应的点云。其中，每帧中每个车辆对应的点云可表示为pi＝{p
i1
,
…
,p
in
}(i》0)，p
i1
表示当前帧中车辆i的第一个数据点，n表示当前帧中车辆i的数据点数量；各帧3d点云的静态点云可表示为p0＝{p
01
,
…
,p
0m
}，p
01
表示静态点云的第一个数据点，m表示静态点云的数据点数量。
[0059]
步骤4：根据图像序列中第一个相邻的两帧图像帧、3d点云序列中对应帧的3d点云的静态点云和不同车辆对应的点云确定当前两帧的静态特征点和动态特征点，进而根据当前两帧对应的光流结果确定车载相机(即车载激光雷达)所在车辆在当前相邻帧间的初始位姿变换以及不同车辆在当前相邻帧间的初始相对变换矩阵；
[0060]
步骤4具体为：
[0061]
步骤4.1：如图6所示，将当前两帧3d点云的静态点云在对应图像帧上的像素位置作为当前两帧图像帧的静态特征点，将当前两帧3d点云中不同车辆对应的点云在对应图像帧上的像素位置作为当前两帧图像帧的动态特征点；
[0062]
步骤4.2：利用光流结果将当前两帧图像帧的静态特征点进行匹配，获得当前两帧2d光流静态匹配结果，利用光流结果将当前两帧图像帧的动态特征点进行匹配，获得当前两帧2d光流动态匹配结果；
[0063]
步骤4.3：如图7所示，将当前相邻的两帧记为t-1时刻和t时刻(即t-1帧和t帧)，进
行t-1时刻的静态特征点到t时刻的2d光流静态匹配结果的2d坐标匹配，得到t-1时刻静态特征点到t时刻2d坐标的匹配关系并作为3d-2d静态匹配结果，基于3d-2d静态匹配结果通过pnp(pespective-n-point)和随机抽样一致算法(ransac，random sample consensus)求解得到车载相机所在车辆的在t-1时刻到t时刻之间的位姿变换t；
[0064]
步骤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。根据车载相机所在车辆的初始位姿变换以及不同车辆的初始相对变换矩阵可以通过步骤7中的公式计算获得各个车辆的初始位姿信息。
[0065]
步骤5：根据当前两帧图像帧的静态特征点与动态特征点、车载相机所在车辆在当前相邻帧间的初始位姿变换和不同车辆在当前相邻帧间的初始相对变换矩阵构建图优化问题并求解，获得车载相机所在车辆在当前相邻帧间的优化后的位姿变换以及不同车辆在当前两帧的后一帧时的优化后的相对变换矩阵；
[0066]
步骤5具体为：
[0067]
步骤5.1：根据当前两帧图像帧的静态特征点和动态特征点、车载相机所在车辆的在当前相邻帧间的初始位姿变换和不同车辆在当前相邻帧间的初始相对变换矩阵构建图优化问题，图优化问题具体为：
[0068]
根据车载相机所在车辆的在t-1时刻到t时刻之间的位姿变换t和t-1时刻的位姿确定车载相机所在车辆的t时刻的位姿，由时刻t-k到时刻t构成长度为k的时间窗口，在当前时间窗口中，将车载相机所在车辆在各个时刻的位姿、各图像帧的静态特征点、各图像帧的动态特征点以及各图像帧中不同车辆的初始相对变换矩阵h均构建为图优化的顶点；
[0069]
在当前时间窗口中，将车载相机所在车辆在各个时刻的位姿和对应图像帧的静态特征点之间的投影误差构建成对应顶点之间的边；将车载相机所在车辆在相邻时刻的位姿之间的位姿变换t构成对应顶点之间的边；将车载相机所在车辆在每一时刻的位姿和对应时刻动态特征点之间的投影误差构建成对应顶点之间的边；将相邻图像帧的动态特征点及对应车辆的初始相对变换矩阵h之间的变换误差构成对应顶点之间的边；
[0070]
步骤5.2：对当前时间窗口中构建得到的图优化问题进行求解，获得车载相机所在车辆在当前相邻帧间的优化后的位姿变换以及不同车辆在当前两帧的后一帧时的优化后的相对变换矩阵。
[0071]
步骤6：将车载相机所在车辆的优化后的位姿变换作为车载相机的最终位姿变换，根据车载相机所在车辆在当前相邻帧间的优化后的位姿变换以及不同车辆在当前相邻帧间的优化后的相对变换矩阵，计算获得当前相邻帧间不同车辆的位姿信息，如图9所示，左边的轨迹线是车载相机所在车辆的轨迹，右边的轨迹线是图像中所有车辆的轨迹。
[0072]
步骤6中，t时刻的图像帧中，各个车辆的位姿信息的计算公式如下：
[0073]
t
car
＝h-1
*t
[0074]
其中，h表示每个车辆的优化后的相对变换矩阵，t表示t时刻车载相机所在车辆的优化后的位姿变换，t
car
表示t-1时刻到t时刻中当前车辆的位姿信息。
[0075]
再根据当前相邻帧间不同车辆的位姿信息计算对应车辆的速度，各个车辆的速度的计算公式如下：
[0076]
v＝t
car-(i-r
car
)c
t-1
[0077]
其中，t
car
表示每个车辆的位姿信息t
car
中的平移部分，i表示3*3的单位矩阵，r
car
表示当前车辆的位姿信息t
car
中的旋转部分，c
t-1
表示当前车辆在t-1时刻的3d点云的质心坐标。
[0078]
具体而言，根据计算得到的车辆位姿变换得到的车辆速度估计，如图8所示，图中的数字“57.11”以及“52.50”表示对应车辆的速度，单位为km/h。
[0079]
步骤7：重复步骤4-步骤6，根据图像序列中剩余图像帧、3d点云序列中对应帧3d点云的静态点云和不同车辆对应的点云以及光流结果依次进行相邻帧的位姿计算以及优化，获得各帧中车载相机的位姿变换、不同车辆的位姿信息以及对应的速度。
[0080]
还需要说明的是，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包含至少一个该特征，在本发明地描述中，“多个”地含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
[0081]
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表达不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
[0082]
尽管上面以及示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解对本发明的限制，本领域的普通技术人员在本发明范围内可以对上述实施例进行变化、修改、替换和变型。