[发明专利]一种SLAM与目标跟踪方法

权利要求书

1.一种SLAM与目标跟踪方法，其特征在于：所述方法包括如下步骤：

步骤1：采集真实城市道路场景下三维点云数据，对所述三维点云数据进行滤波处理，采集真实城市道路场景下图像视频数据，对所述图像视频数据进行目标检测和车道线检测，获取目标回归框坐标和车道线坐标；

步骤2：根据所述车道线检测结果对滤波后的所述三维点云数据进行分割处理，得到静态场景和动态场景；

步骤3：对所述静态场景进行基于静态特征点的同步定位和构图，输出自身定位与静态场景子图，对所述动态场景三维点云数据进行三维目标检测，输出三维回归框坐标，将所述三维回归框坐标结合所述目标回归框坐标进行检测结果决策融合，基于融合结果进行基于贝叶斯滤波的多目标跟踪，输出动态目标位姿与轨迹，构建动态对象子图；

步骤4：采用图优化全局优化方法，输入所述自身定位，将所述静态特征点与动态语义对象作为统一的路标点，优化自身位置与路标位置，输出优化后的所述自身定位和场景地图。

2.如权利要求1所述的SLAM与目标跟踪方法，其特征在于：所述动态场景点为所述车道线内的所述三维点云数据；所述静态场景点包括所述车道线外的所述三维点云数据和被滤波的所述三维点云数据。

3.如权利要求1所述的SLAM与目标跟踪方法，其特征在于：所述步骤1中对所述三维点云数据进行的滤波处理为基于特征轴的地面点提取，将原始三维点云数据分为地面点与非地面点。

4.如权利要求1～3中任一项所述的SLAM与目标跟踪方法，其特征在于：所述三维点云数据通过激光雷达进行采集，所述图像视频数据通过摄像头采集。

5.如权利要求4所述的SLAM与目标跟踪方法，其特征在于：所述步骤2中包括预先对所述摄像头进行内参标定，获取摄像头内参；并对激光雷达与摄像头进行联合标定，获取所述三维点云数据与所述图像像素点之间的投影变换矩阵。

6.如权利要求4所述的SLAM与目标跟踪方法，其特征在于：所述步骤2中基于所述视频流的车道线检测输出的二维坐标，经过所述摄像头内参的透视变换，得到摄像头坐标系下的坐标信息；再根据联合校准的结果，对所述摄像头坐标系下的点进行旋转平移变换，得到车道线信息在激光雷达坐标系下的坐标。

7.如权利要求4所述的SLAM与目标跟踪方法，其特征在于：所述步骤3包括如下步骤：

a、对于所述静态场景，对激光雷达每一圈扫描的点计算曲率，按曲率大小对所有点进行排序，取曲率排名前四的点作为边缘点，取曲率排名最后的两个点视为平面点；

b、根据相邻两帧数据计算边缘点与线的运动变化，平面点与面的运动变化，用莱文贝格-马夸特方法进行优化，输出优化后的结果并以此作为自身相邻两帧的运动变化；

c、利用点柱编码器，对动态场景三维点云数据进行基于竖直方向上三维点云数据数量特征提取，将三维信息转换为二维信息，再使用二维卷积神经网络对二维编码信息进行检测，获取三维目标的类别与三维回归框坐标；

d、将三维目标投影到二维图像，剔除与二维检测目标类别不匹配的目标，将剩余类别匹配的对象作为融合后的决策结果，输出决策结果的位姿，对决策结果进行基于贝叶斯滤波的多目标跟踪，输出目标的运动轨迹。

8.如权利要求4所述的SLAM与目标跟踪方法，其特征在于：所述步骤3中动态目标的数学模型为：

其中，S_t表示第t帧检测到的所有语义目标，S_k表示当前语义目标，一个目标状态S_k包含三个部分：

表示检测分类结果，包括“车”、“自行车”、“行人”；

表示检测到该目标的置信度；

表示检测到该目标的矩形回归框，所述目标的矩形回归框为目标在三维点云数据中的位置。

9.如权利要求4所述的SLAM与目标跟踪方法，其特征在于：所述步骤4中所述图优化的路标点包括了静态场景特征点的几何信息与动态场景中的语义目标信息。

10.如权利要求9所述的SLAM与目标跟踪方法，其特征在于：所述几何信息和所述语义目标信息统一在期望似然测量模型中，所述模型为：

E_D[logp(Z|X，L，D)|Xⁱ，Lⁱ，Z]

其中，Xⁱ表示i时刻几何对象的位置；

Lⁱ表示i时刻语义对象的位置；

Z表示两者的观测值；

优化问题转化为给定初始估计Xⁱ和Yⁱ，期望测量似然取最大值取X和L，即为所需要的下一步状态的Xⁱ⁺¹和Lⁱ⁺¹，优化模型为：

Xⁱ⁺¹，Lⁱ⁺¹＝argmaxE_D[logp(Z|X，L，D)|Xⁱ，Lⁱ，Z]

其中，Xⁱ表示i时刻几何对象的位置，

Lⁱ表示i时刻语义对象的位置，Z表示两者的观测值，

Xⁱ⁺¹表示第i+1时刻几何对象的位置，

Lⁱ⁺¹表示第i+1时刻语义对象的位置。