[发明专利]一种基于目标跟踪的动态场景双目视觉SLAM方法

权利要求书

1.一种基于目标跟踪的动态场景双目视觉SLAM方法，其特征在于：在远端采用嵌入式设备控制安装在运动体上的随动云台相机并采集图像，在服务端使用嵌入式设备部署算法，算法采用结合目标跟踪的SLAM算法排除视野内运动体自身的遮挡机构和动态物体的干扰；所述SLAM方法包括以下步骤：

步骤1，SLAM算法双目初始化与目标检测网络读取训练模型信息；

步骤2，远程随动采集双目彩色视频图像；

步骤3，目标跟踪网络对双目彩色视频图像进行目标检测及跟踪，将结果送入SLAM线程作后续处理；

步骤4，SLAM线程对双目彩色图像进行预处理，提取角点后，使用光流匹配双目关键点并计算深度信息；

步骤5，使用步骤3中的目标跟踪结果，将关键点分类为静态点和待筛选点；

步骤6，对步骤5中的静态点进行对极几何约束去除多余外点，使用PnP算法求解位姿信息；

步骤7，对步骤5中的待筛选点，使用动态点筛选方法筛选出真正的动态点；

步骤8，对步骤7中动态点进行删除，并删除当前帧与该点的共视关系，对静态点进行合并；

步骤9，更新当前帧的信息，包括关键点、地图点以及其共视关系；基于位姿进行局部优化；如果当前帧是关键帧，则进行回环检测。

2.根据权利要求1所述的一种基于目标跟踪的动态场景双目视觉SLAM方法，其特征在于：在步骤2中，在服务端操作人员使用携带IMU的VR设备控制随动云台相机的云台旋转，在远端使用树莓派连接随动云台相机，控制随动云台相机采集双目彩色视频图像，并通过ROS传回到服务端完成双目彩色视频图像的采集；VR设备上的IMU测量模型为：

其中，为角速度，ω^b为角速度真值，b^g为角速度零偏、n^g为角速度噪声，为加速度、q_bw为world到body系的旋转矩阵、a^w为加速度真值、g^w为重力加速度、b^a为加速度零偏、n^a为加速度噪声；

角速度与欧拉角的转换关系为：

其中，φ为偏航角yaw，θ为俯仰角pitch，ψ为滚转角roll；ω_x、ω_y、ω_z分别是角速度在xyz轴分解的向量；

则随动云台相机两个舵机的输入脉冲数分别为：

其中，P_⊥和P_∥分别为随动云台相机两舵机的输入脉冲，DEG为步进角。

3.根据权利要求1所述的一种基于目标跟踪的动态场景双目视觉SLAM方法，其特征在于：在步骤3中，目标跟踪网络是基于yolov5s的目标检测网络和卡尔曼滤波，具体包括以下步骤：

3.1，目标检测网络对图像中的局部运动机构进行检测，得到一组矩形框；

3.2，对步骤3.1输出结果的新目标构建一个卡尔曼滤波器，并使用矩形框坐标信息进行初始化；

其中，状态量设置为状态转移矩阵设置为由于没有输入量，u＝0，观测量设置为观测矩阵设置为

另外，对运动体上特殊场景遮挡物体“第一人称挖掘机臂”、“第一人称消防水枪”由于其启动位置的固定性，预设其初始位置，即：

3.3，对步骤3.1输出结果的旧目标进行跟踪，使用其卡尔曼滤波器输出的预测值对步骤3.1的所有输出进行IOU匹配：

3.4，对步骤3.3中匹配结果大于阈值T的目标标记为跟踪成功，使用其矩形框作为最终结果并修正卡尔曼滤波器；

3.5，对步骤3.3中匹配结果小于阈值T的目标的跟踪失败Flag自增，若跟踪失败Flag大于阈值F，则释放该卡尔曼滤波器的内存，否则使用卡尔曼滤波器输出的预测值作为最终结果；

3.6，若步骤3.1目标检测网络未输出到任何矩形框，则所有已有的目标的跟踪失败Flag自增，若跟踪失败Flag大于F，则释放对应卡尔曼滤波器的内存，否则使用卡尔曼滤波器输出的预测值作为最终结果。

4.根据权利要求1所述的一种基于目标跟踪的动态场景双目视觉SLAM方法，其特征在于：在步骤7中，所述动态点筛选方法包括以下步骤：

7.1，使用对极几何约束对静态点求取本质矩阵E，

7.2，对步骤7.1的本质矩阵E使用SVD奇异值分解计算旋转矩阵R和平移向量t：

其中，R₁、R₂均是一种可能的旋转矩阵、均是一种可能的平移矩阵；

求得2组R₁、R₂、取其中点深度大于零的解即为待筛选点的位姿；

7.3，使用步骤7.2中的位姿信息作为基准，将步骤5中待筛选点分类为动态点和静态点：

根据本质矩阵E求得点x₁的极线l₁＝[X,Y,Z]^T＝Ex₁；

点x₂到极线l₁的距离公式为：

其中，x₁和x₂是一对匹配点，F是基础矩阵，X、Y是极线的参数，T代表矩阵的转置；

若D大于阈值D_th则被认为是动态点，若D小于阈值D_th则被认为是静态点。