融合视觉与激光雷达的目标定位、测量和追踪装置及方法

本发明涉及图像处理，具体地，涉及融合视觉与激光雷达的目标定位、测量和追踪装置及方法。

背景技术：

1、现如今的应急救援装置越来越智能化，各种应急救援环境感知装置层出不穷。在应急救援环境感知中，对救援目标或者救援场景中的特定目标进行定位跟踪，能够提高救援效率，减少人为参与，提高智能化水平。

2、在现有的应急救援环境感知中，目标定位方法主要依靠单一传感器来实现，单纯依靠视觉或者激光雷达的定位方法受环境条件限制大，而对于多传感器的定位和跟踪方法中，各传感器之间的标定和融合步骤复杂且不稳定，并且对于视野中存在多目标的定位和追踪存在算法复杂，检测目标慢，定位误差大等不足。

技术实现思路

1、本发明的目的在于提供融合视觉与激光雷达的目标定位、测量和追踪装置及方法，以解决上述背景技术中提出的问题。

2、为实现上述目的，本发明提供如下技术方案：融合视觉与激光雷达的目标定位、测量和追踪方法，包括以下步骤：

3、步骤1、通过相机和激光雷达的装置初始安装位置获取初始位姿，得到相机与激光雷达标定的初始标定向量，并根据初始标定向量获取相机像素坐标系和激光雷达坐标系的初始外参矩阵；

4、步骤2、相机和激光雷达均通过标定板进行标定，通过提取激光雷达标定板信息和相机标定板信息与初始外参矩阵进行匹配，建立匹配关系，根据匹配误差建立目标函数；

5、步骤3、将匹配误差最小的目标函数确定为最优标定效果，并利用最优标定效果时的平移矩阵参数以及外参矩阵，完成相机和激光雷达标定；

6、步骤4、通过相机和激光雷达标定，实现相机的像素坐标和激光雷达点云坐标的关联后，利用yolo识别算法对目标进行识别和计算，从而获取目标物体的三维坐标以及位置信息；

7、步骤5、通过三维坐标对应获取目标物体的三维点云坐标集合，并对获取的三维点云坐标集合中的数据进行优化，将优化后的坐标集合进行平均值计算，得到目标物体的三维坐标，从而完成目标定位；

8、步骤6、将激光雷达点云进行点云滤波、降采样、分割以及包围处理，获取视野中所有目标的包围框集合，根据目标物体定位获取的三维坐标值进行搜索匹配，并输出目标的尺寸大小；

9、步骤7、根据目标物体定位能够获取目标物体的位置和距离信息，通过yolo识别并计算目标物体的中心像素坐标与相机视场的中心像素坐标的标差值，根据偏差控制云台进行运动，使目标物体始终保持在视野中央，完成目标物体跟踪。

10、优选的，所述步骤1，初始标定向量包括相机和激光雷达的旋转矩阵r和平移矩阵t，根据旋转矩阵r和平移矩阵t获取相机像素坐标系和激光雷达坐标系的初始外参矩阵，相机像素坐标系和激光雷达坐标系的初始外参矩阵为：

11、

12、其中，u和v为像素坐标系下的点，fx、fy、u0、v0为相机内参矩阵参数，tx、ty、tz为平移矩阵参数。

13、优选的，在所述步骤2中，通过激光雷达点云的特征提取获得目标函数的流程为：

14、步骤21、在激光雷达点云中提取标定板点云，获取标定板的三维点云坐标集合qli；

15、步骤22、将三维点云坐标集合根据初始位姿参数进行坐标转换获取点云像素坐标集合qci；

16、步骤23、通过对比点云像素坐标集合qci与相机视角下的标定板真实像素坐标集合pci，获取匹配误差为ε的目标函数。

17、优选的，在所述步骤4-所述步骤5中，获取目标物体的三维坐标以及位置信息的流程包括以下步骤：

18、步骤51、获取目标物体的所有像素区域坐标mci，通过坐标的关联能够获取像素坐标mci对应的点云坐标集合mli；

19、步骤52、获取点云坐标集合mli中的深度信息xi，将深度信息xi进行排序并获取集合ml，剔除集合ml的前20％数据和后20％数据并获取集合ml1，设集合ml1的总数为n，求点云坐标集合坐标平均值：从而目标物体获取优化后的三维坐标以及位置信息。

20、优选的，在所述步骤6中，根据输入的三维坐标值和目标的包围框集合，计算三维点与包围框质心的距离：式中，(xl,yl,zl)为目标的三维坐标，(xci,yci,zci)为各目标包围框的质心坐标，根据距离远近对包围框信息集合进行排序，减少搜索范围，然后取距离最近框判断三维坐标点是否在包围框内，如果是，则输出目标的尺寸大小，不是则继续判断下一包围框。

21、优选的，在所述步骤7中，根据yolo算法能够获得目标的像素区域中心像素坐标o(u0,v0)，通过将目标中心像素坐标t(ui,vi)与相机视野的中心点像素坐标进行对比得到像素坐标差值：通过像素坐标差值控制云台进行移动，对每一帧目标进行识别和差值计算，将目标中心像素坐标逼近相机视野中心，进行目标跟踪。

22、本发明还提供了融合视觉与激光雷达的目标定位、测量和追踪装置，包括：

23、相机，用于图像信息的采集并获取像素坐标；

24、激光雷达，用于周围点云信息的采集并获取点云坐标；

25、防爆外壳，用于设置所述相机和所述激光雷达的安装位置并进行遮盖防护，所述相机和所述激光雷达安装于所述防爆外壳的内部；

26、云台，其两侧通过转轴安装有连接支架，防爆外壳安装于所述连接支架的顶部，所述云台上安装有存储器；以及耦接至所述存储器的处理器，所述处理器被配置为基于存储在所述存储器的指令执行如上述的融合视觉与激光雷达的目标定位、测量和追踪方法。

27、优选的，所述处理器用于处理所述相机和所述激光雷达传输的数据，并控制所述云台的运行状态。

28、与现有技术相比，本发明的有益效果是：

29、本发明通过融合装置固定相机和激光雷达的相对位置从而完成联合标定，实现相机和激光雷达的数据关联，简化了标定流程，提高了标定的准确性和快速性，同时利用相机和激光雷达融合进行多目标定位提高了定位的准确性和实时性。

30、本发明通过yolo识别算法能够对多目标进行识别，并通过进一步优化算法，能够获得多目标在激光雷达下位置，从而得到多目标的三维坐标，并对于多个目标能够根据目标的定位距离选择对指定距离的目标自动控制云台运动，实现对应急救援中的多目标的实时检测，同时得到目标的位置，完成目标的精准定位，并根据定位距离，选择特定距离的目标进行跟踪。

技术特征：

1.融合视觉与激光雷达的目标定位、测量和追踪方法，其特征在于，包括以下步骤：

2.根据权利要求1所述的融合视觉与激光雷达的目标定位、测量和追踪方法，其特征在于：所述步骤1中，初始标定向量包括相机和激光雷达的旋转矩阵r和平移矩阵t，根据旋转矩阵r和平移矩阵t获取相机像素坐标系和激光雷达坐标系的初始外参矩阵，相机像素坐标系和激光雷达坐标系的初始外参矩阵为：

3.根据权利要求1所述的融合视觉与激光雷达的目标定位、测量和追踪方法，其特征在于：在所述步骤2中，通过激光雷达点云的特征提取获得目标函数的流程为：

4.根据权利要求1所述的融合视觉与激光雷达的目标定位、测量和追踪方法，其特征在于：在所述步骤4-所述步骤5中，获取目标物体的三维坐标以及位置信息的流程包括以下步骤：

5.根据权利要求4所述的融合视觉与激光雷达的目标定位、测量和追踪方法，其特征在于：在所述步骤6中，根据输入的三维坐标值和目标的包围框集合，计算三维点与包围框质心的距离：式中，(xl,yl,zl)为目标的三维坐标，(xci,yci,zci)为各目标包围框的质心坐标，根据距离远近对包围框信息集合进行排序，减少搜索范围，然后取距离最近框判断三维坐标点是否在包围框内，如果是，则输出目标的尺寸大小，不是则继续判断下一包围框。

6.根据权利要求1所述的融合视觉与激光雷达的目标定位、测量和追踪方法，其特征在于：在所述步骤7中，根据yolo算法能够获得目标的像素区域中心像素坐标o(u0,v0)，通过将目标中心像素坐标t(ui,vi)与相机视野的中心点像素坐标进行对比得到像素坐标差值：通过像素坐标差值控制云台进行移动，对每一帧目标进行识别和差值计算，将目标中心像素坐标逼近相机视野中心，进行目标跟踪。

7.融合视觉与激光雷达的目标定位、测量和追踪装置，其特征在于，包括：

8.根据权利要求7所述的融合视觉与激光雷达的目标定位、测量和追踪装置，其特征在于：所述处理器(1)用于处理所述相机(4)和所述激光雷达(5)传输的数据，并控制所述云台(3)的运行状态。

技术总结
本发明公开了融合视觉与激光雷达的目标定位、测量和追踪装置及方法，其中，本发明通过提取激光雷达中标定板信息与相机图像中的标定板信息进行匹配，建立匹配关系，根据匹配误差建立目标函数，本发明通过融合装置固定相机和激光雷达的相对位置从而完成联合标定，实现相机和激光雷达的数据关联，简化了标定流程，提高了标定的准确性和快速性，同时利用相机和激光雷达融合进行多目标定位和追踪。

技术研发人员：李伟,胡炳帅,潘禄,刘中冠,张博,朱劲松,刘秀梅,刘庆
受保护的技术使用者：中国矿业大学
技术研发日：
技术公布日：2024/1/16