基于单线激光雷达与视觉融合的目标测距和追踪的方法

：本发明属于水面目标测距和追踪，特别是涉及基于单线激光雷达与视觉融合的目标测距和追踪的方法。

背景技术

0、
背景技术：

1、对于水面垃圾，传统清理方式就是通过人为驾驶救生筏。随着人工智能的普及以及算法的不断提升，越来越多的繁杂、危险的工作可以通过机器来代替完成，由此水面无人垃圾清理船应运而生，而水面无人垃圾清理船的关键技术就是对水面目标垃圾的识别以及跟踪；

2、水面目标的测距和追踪技术已经成为多个领域如海洋科学、船舶导航和渔业监测等的核心技术。传统的测距方法主要基于视觉传感技术，如单目或双目视觉系统，这些系统通常利用图像的特征点匹配或深度估计算法来估计目标的距离。然而，这些传统方法在处理远距离、复杂环境或部分遮挡的目标时会面临一些挑战。

3、中国发明专利cn202310234432.8，公开了一种多传感器融合的近岸无人船slam方法。该方法成功地将激光雷达惯性里程计(lio)和视觉惯性里程计(vio)紧密耦合，并巧妙地利用了水面和水岸线的语义信息来联合处理数据。此方法在某些场景中，如强反光的水面环境，确实提供了较高的鲁棒性和准确性。然而，该方法仍然存在一些局限性。首先，它在目标追踪和特征点识别方面仍然面临一些困难。其次，尽管其能够较好地处理近距离的测距和追踪，但在远距离和复杂环境中，其性能仍有待提高。

4、中国发明专利cn201810267075.4，公开了一种无人船的目标识别追踪系统及其工作方法。该系统结合了3d激光雷达、矢量水听器等技术来实现对水面和水下障碍物的扫描和避障。虽然这种方法在处理水下障碍物上有所突破，但它对于长距离测距和高精度追踪的需求仍然不能完全满足。

5、现有的目标追踪算法都只适用于摄像头可视距离范围内的追踪(一般摄像头的可视距离在2-4m)，对于远距离追踪效果不佳，在近年来，随着无人船技术的快速发展，实现精确的水面目标测距和追踪成为了研究的重要课题。为了提高测距和追踪的精度，传统方法往往依赖于高成本、高功耗的设备，如多线激光雷达。然而，这种方法不仅成本高昂，而且在某些情况下可能不适合小型无人船。

6、为了解决这些问题，市场上已经公开了基于单线激光雷达的方法，与多线激光雷达相比，单线激光雷达具有成本低、功耗少的优势，但是，单纯依赖单线激光雷达进行目标测距和追踪仍然存在一定的局限性，尤其是在复杂的水面环境中。

7、为了进一步提高测距和追踪的精度和稳定性，本发明将单线激光雷达与其他传感器，如视觉传感器，进行融合的方法。通过这种融合方法，可以充分利用每种传感器的优势，从而实现更为精确和稳定的目标测距和追踪，利用融合检测追踪算法，通过融合单线雷达的传感距离(一般雷达的传感距离在8-10m)，实现远距离目标追踪，并且由于本算法是通过追踪特征点，对于障碍物被部分遮盖时，也可以有效的追踪目标。

技术实现思路

0、
技术实现要素：

1、针对现有技术的不足，本发明提出了基于单线激光雷达与视觉的水面目标测距追踪方法，其核心是融合单线激光雷达与视觉传感器的数据，以实现对水面目标的精确测距和稳定追踪。该方法涵盖了以下几个关键步骤：单线激光雷达进行目标的初步三维空间定位、视觉传感器进行目标的二维图像获取、通过预先标定得到的内外参数矩阵进行信息融合，以及利用klt算法与卡尔曼滤波进行目标运动状态的预测和修正。为了实现该方法，本发明提供基于单线激光雷达与视觉融合的目标测距和追踪的方法，该方法包含以下步骤：

2、s1：通过单线激光雷达进行目标的初步三维空间定位，获取目标相对于雷达的粗略距离和方向信息；

3、s2：利用视觉传感器进行目标的二维图像获取，并通过图像处理算法识别目标特征点；

4、s3：通过预先标定得到的内外参数矩阵，将单线激光雷达的三维空间信息与视觉传感器的二维图像信息进行融合；

5、s4：应用数据融合结果，在目标特征点上进行精确的距离测量和追踪，其中包括应用多模态数据融合算法和卡尔曼滤波器；

6、s5：输出目标在三维空间中的精确位置和运动状态。

7、作为本发明的一种优先技术方案，所述步骤s1中通过单线激光雷达进行目标的初步三维空间定位的特征在于：

8、t1：单线激光雷达采用的激光波长为905纳米，以确保在各种光照条件下的高测距精度；

9、t2：雷达的扫描角度范围设定为±30度，用于捕获目标在水平面上的方向信息；

10、t3：利用雷达获取的时间飞行数据，计算目标与雷达之间的粗略距离θ，通过小孔成像原理公式，计算目标在三维空间中的x和y的坐标，

11、

12、其中相机上的点p(xc,yc,zc)在图像坐标成像的点是p`(x,y)，其中(u0,v0)为像素坐标系和物理成像坐标系原点位置的偏移量，(1/dx,1/dy)为偏导数；

13、t4：结合雷达平台的高度z，完成目标在三维空间x,y,z中的初步位置定位。

14、作为本发明的一种优先技术方案，所述步骤s2中二维图像获取具备以下特征：

15、p1：使用cmos图像传感器，具有200万像素分辨率和60帧/秒的帧率，以实现高清晰度的目标图像捕获；

16、p2：图像传感器配置有自动曝光和自动白平衡功能，以适应不同光照条件下的目标图像获取；

17、p3：利用harris角点检测算法进行目标特征点的提取，该算法能够有效识别目标图像中的角点作为特征点；

18、p4：结合图像中的特征点和雷达获取的初步三维空间定位信息，通过对极几何约束进行特征点匹配；

19、p5：将图像坐标系下的雷达坐标点转换为像素坐标系下的像素点，以实现数据融合和精确目标定位。

20、作为本发明的一种优先技术方案，所述步骤s3中二维图像信息进行融合包含以下步骤：

21、r1：通过棋盘格标定板作为标定对象，其中棋盘格由黑白相间的正方形组成，用于单线激光雷达和视觉传感器的联合标定；

22、r2：利用标定方法获取视觉传感器的内参数矩阵k和畸变系数d；

23、r3：同时获取单线激光雷达对标定板的距离数据，进一步利用最小二乘法求解雷达和视觉传感器之间的外参数矩阵t；

24、r4：结合内参数矩阵k、畸变系数d和外参数矩阵t，通过线性三角化方法进行数据融合；

25、r5：通过以下公式将雷达坐标系的点映射到相机坐标系下，进一步进行目标的定位和追踪

26、

27、其中，[xc,yc,zc]w是映射后图像坐标系下的点，是相机内参矩阵，是雷达和相机联合标定的4*4的外参矩阵，[xp,yp,zp]w是雷达坐标系下的点。

28、作为本发明的一种优先技术方案，所述步骤s4中距离测量和追踪使用的是klt算法，其包含以下步骤：

29、y1：选择上一帧图像中的特征点集合，用于在下一帧图像中进行匹配；

30、y2：利用视差(d1,d2)的变化，在以坐标(i,j)为中心的(n1×n2)的yolo v5检测框内，从时间tk+1中寻找时间tk的匹配点，其中，n1，n2代表检测框的高度和宽度，i和j表示中心点的像素坐标；

31、y3：采用欧式距离准则，评估两帧之间特征点的匹配程度，设xp是上一帧图像f(xi,yi,tk)中的一个特征点，xq为下一帧图像内的特征点，计算欧氏距离如下：

32、

33、其中，n1和n1分别表示在两帧图像中考虑的区域的高度和宽度；

34、y4:根据上述欧氏距离，估算两点间的位移：

35、

36、其中，k1和k2分别为欧氏距离的阈值。

37、作为本发明的一种优先技术方案，所述步骤s4中卡尔曼滤波器主要用于预测目标的运动状态，其包含以下步骤：

38、u1：基于假设，认为运动目标以恒定的速度进行运动；

39、u2：构建目标的状态方程如下：

40、x(k+1)＝φ(k)+gw(k)

41、其中，w(k)＝[wx，wy]t是均值为0互不相关的随机加速噪声，x(k+1)及y(k+1)是特征点在k+1时刻的坐标，及是特征点在k+1时刻的速度，t是采样间隔；

42、u3:根据上述状态方程，对特征点在k+1时刻的位置x(k+1)和y(k+1)及其速度和进行预测。

43、作为本发明的一种优先技术方案，所述卡尔曼滤波器还包括一种klt算法的输出进行修正和优化的方法，具体步骤如下：

44、m1：设定观测方程如下：

45、z(k+1)＝hx(k+1)+v(k+1)

46、其中，v(k+1)＝[vx(k+1)，vy(k+1)]t是均值为0互不相关的观测高斯白噪声；

47、m2：利用klt算法提供的目标特征点信息，结合上述观测方程，应用卡尔曼滤波器对目标的运动状态进行估计；

48、m3：基于卡尔曼滤波器的估计结果，对klt算法的输出进行修正，确保在连续的图像帧中对目标的跟踪更为稳定和准确。

49、与相关的现有技术相比，本技术提案相比现有技术，有以下主要技术优点：本发明的有益效果是：

50、成本效益：通过采用单线激光雷达代替传统的多线激光雷达，本发明显著降低了设备和维护的成本，使其更适合于广泛的应用，尤其是小型无人船。

51、高精度和稳定性：通过融合单线激光雷达和视觉传感器的数据，本发明提供了更高的测距和追踪精度。结合klt算法和卡尔曼滤波，进一步增强了对目标的稳定追踪能力，特别是在复杂的水面环境中。

52、灵活性：本发明不仅适用于小型无人船，还可以轻松地扩展到其他平台或应用，如大型船舶或固定观测站。

53、实时性能：利用本发明的方法，可以实时地获取和处理数据，从而实现对水面目标的即时测距和追踪。

54、自适应性：通过内外参数矩阵的预先标定和信息融合，本发明能够自动适应不同的环境和场景，从而提供持续、稳定的测距和追踪性能。