CN111998772B - 一种基于激光与单目视觉融合的像素级目标定位方法 - Google Patents

Abstract

本发明涉及物体三维定位方法领域，尤其涉及一种基于激光与单目视觉融合的像素级目标定位方法，首先安装相机和激光测距模块，构建坐标系，对单目相机和激光测距模块测距相对位置进行标定，得到标定参数，通过单目相机获取环境图像后进行预处理和确定待定位目标的像素坐标，然后通过PID反馈控制驱动执行机构带动激光测距模块转动采集数据并进行计算，最后驱动激光测距模块将测距光点准确打到目标位置后采集数据通过计算实现目标定位。本发明计算资源消耗低，能够在移动端算力有限的情况下保证实时性，具有高性能的物体定位能力，精度高、范围广，能提升智能自主作业设备的作业范围，还具有一定的成本优势，降低使用门槛，利于技术推广与应用普及。

Description

一种基于激光与单目视觉融合的像素级目标定位方法

技术领域

本发明涉及物体三维定位方法领域，尤其涉及一种基于激光与单目视觉融合的像素级目标定位方法。

背景技术

近年来，随着机器人技术和人工智能技术的发展，越来越多的智能自主作业设备应用在社会生产生活和军事国防领域中。在社会生产生活中，利用智能机器人来协助或取代人类开展部分重复性或服务性工作是社会发展的趋势，可以有效提升生产效率，提高日常生活的便捷程度；在军事国防领域中，利用智能机器人来辅助或替代人来执行特定任务可以有效提高任务执行效率，保障任务执行人员的安全。

智能自主作业设备或智能机器人强依赖于感知系统，对作业范围内物体的准确定位是其开展作业任务的前提，也是其自主作业能力的体现。目前在感知系统中对于物体的定位通常采用双目视觉、激光雷达、毫米波雷达等深度传感器，此类方案在一定范围内可实现智能自主作业设备对物体的定位，但在计算资源消耗、物体定位精度、有效定位距离及成本等方面还存在着明显的不足。由于智能自主作业设备受成本、体积及功耗等问题的制约，其搭载的终端计算设备算力较为有限，因此采用上述方案的物体定位方法造价高昂、实时性差、定位范围有限，在智能自主作业设备实际工作条件下难以有效完成物体精确定位工作。

发明内容

为了解决现有技术中存在的上述技术问题，本发明提供了一种基于激光与单目视觉融合方案的像素级目标定位方法，包括如下步骤：

S1，安装相机使得相机光轴平行于地面，安装激光测距模块使得激光测距模块光心与相机光心的连线垂直于地面且使激光测距模块与单目相机之间的垂直间距尽可能小，对单目相机和激光测距模块进行标定和联合标定，得到标定参数包括相机内参矩阵K和相机外参矩阵M；

S2，在相机像素坐标系中给出待定位物体的像素坐标，即为激光测距模块测距光点在相机像素坐标系中的目标位置；

S3，利用所述相机内参矩阵K、外参矩阵M和待定位物体像素坐标，通过PID反馈控制驱动执行机构带动激光测距模块转动，进行物体定位，反馈控制的反馈量由激光测距模块引入，根据激光测距模块当前时刻测距点的距离信息和激光测距模块在航向轴、俯仰轴实际转动角度，计算转换得到当前时刻测距点在激光测距模块坐标系即系统世界坐标系下的三维坐标(X_W,Y_W,Z_W)，利用相机外参矩阵M和内参矩阵K得到当前激光测距模块测距点在相机像素坐标系下的像素坐标，将所述坐标作为反馈量引入PID反馈控制中来实现激光测距模块的闭环控制，驱动激光测距模块将测距光点准确打到目标位置；

S4，根据S3中激光测距模块测得的目标点距离和激光测距模块在航向轴、俯仰轴实际转动角度，计算转换得到目标点在激光测距模块坐标系即系统世界坐标系下的三维坐标(X′_W,Y_W′,Z′_W)，实现物体的精确定位。

进一步的，所述S1步骤具体包括：

S11，相机标定板选取为12×9的国际象棋盘图案标定板，结合单目相机视角场范围采集50张不同位姿和距离的标定图像以覆盖相机的全部视场；

S12，利用采集到的标定图像通过张正友标定法完成单目相机的标定，得到相机的内参矩阵K；

S13，选取激光测距模块坐标系作为本系统的世界坐标系，根据单目相机和激光测距模块之间的相对位置及联合标定工具获取相机的外参矩阵M。

进一步的，所述S2步骤具体包括：

S21，单目相机获取视野中的环境原始图像；

S22，对获取到的环境原始图像进行预处理，包括图像的裁切，实现单目相机数码变焦；

S23，根据获取到的环境图像利用物体识别神经网络或其他待定位目标给定程序有效确定待定位目标像素坐标(u,v)，即为激光测距模块测距光点在相机像素坐标系中的目标位置。

进一步的，所述S22步骤具体包括：

S221，编写图像预处理程序读入单目相机获取到的环境原始图像数据；

S222，在图像预处理程序中按照一定的裁切系数以环境原始图像中心为中心点对图像进行裁切；

S223，将裁切完成的图像发布出去，动态改变目标物体在相机视野中的尺度。

进一步的，所述S3步骤具体包括：

S31，将所述的目标点像素坐标(u,v)作为控制信号输入到PID反馈控制中，驱动执行机构带动激光测距模块开始定位过程；

S32，在定位过程中连续采集激光测距模块测量点的距离d和激光测距模块的俯仰角α和航向角β，计算转换得到当前测距点在激光测距模块坐标系即系统世界坐标系下的三维坐标(X_W,Y_W,Z_W)，(X_W,Y_W,Z_W)的计算公式为

得到当前测距点在激光测距模块坐标系即系统世界坐标系下的三维坐标(X_W,Y_W,Z_W)，利用相机外参矩阵M和内参矩阵K，计算转换得到激光测距模块当前测距点在相机像素坐标系的二维坐标(u′,v′)，并将该坐标作为反馈量引入PID反馈控制中，实现测距点的闭环控制，驱动激光测距模块将测距光点准确打到目标位置，系统世界坐标系下测距点的三维坐标(X_W,Y_W,Z_W)与相机像素坐标系下二维坐标(u′,v′)的转换关系为：

进一步的，所述S4步骤具体包括：

S41，经所述激光测距模块测距点打到目标位置后，采集此时激光测距模块测距点距离d′、俯仰角α′和航向角β′；

S42，利用采集得到的激光测距模块测距点距离d′俯仰角α′和航向角β′，通过式

计算得出待定位目标点在系统坐标系下的实际三维坐标(X′_W,Y_W′,Z′_W)。

本发明可使智能自主作业设备在实际工作场景中有效得开展环境物体的定位，极大的提高了智能自主作业设备的自主作业能力，在目标定位过程中将测距模块作为传感器为系统引入有效的反馈量，实现了整个定位过程的闭环控制，充分利用了系统中感知模块的特性与性能，实现了高精度的像素级目标定位；此方法对计算资源的消耗较少，能够在移动端算力有限的情况下保证定位实时性，同时定位精度高、范围广，可实现目标的精确定位，提升智能自主作业设备的作业范围，拓宽智能自主作业设备的作业种类；此方法中使用的传感器元件成熟易采购，结构紧凑，集成度高，还具有较大的成本优势，降低了此方法的使用门槛，有利于此像素级目标定位方法的推广与应用普及。

附图说明

图1是本发明中定位方法流程图；

图2是本发明中定位方法原理图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，下面结合说明书附图对本发明做进一步的详细说明。

如图1-2所示，本发明的一种基于激光与单目视觉融合的像素级目标定位方法，包括如下步骤：

S1，相机安装位置保证相机光轴平行于地面，激光测距模块安装位置保证激光测距模块光心与相机光心的连线垂直于地面且使激光测距模块与单目相机之间的垂直间距尽可能小，其中O_WX_WY_WZ_W激光测距模块坐标系即本系统世界坐标系，O_CX_CY_CZ_C为相机坐标系，O₁xy为相机成像坐标系，O₀uv为相机像素坐标系，对单目相机和激光测距模块测距相对位置进行标定，得到标定参数，包含相机内参矩阵K和相机外参矩阵M；

S2，在相机像素坐标系中给出待定位物体A的像素坐标，即为激光测距模块测距光点在相机像素坐标系中的目标位置；

S3，利用所述相机内参矩阵K、外参矩阵M和待定位物体A的像素坐标，通过PID反馈控制驱动执行机构带动激光测距模块转动，开始物体定位过程；反馈控制的反馈量由激光测距模块引入，根据激光测距模块当前时刻测距点的距离信息和激光测距模块在航向轴、俯仰轴实际转动角度，可计算转换得到当前测距点在激光测距模块坐标系即系统世界坐标系下的三维坐标(X_W,Y_W,Z_W)，利用相机外参矩阵M和内参矩阵K，可得到当前激光测距模块测距点在相机像素坐标系下的像素坐标，此坐标作为反馈量引入PID反馈控制中来实现激光测距模块的闭环控制，驱动激光测距模块将测距光点准确打到目标位置A；

S4，根据S3中激光测距模块测得的目标点距离和激光测距模块在航向轴、俯仰轴实际转动角度，可计算转换得到目标点在激光测距模块坐标系即系统世界坐标系下的三维坐标(X′_W,Y_W′,Z′_W)，实现物体的精确定位。

上述S1步骤具体包括：

S11，相机标定板选取为12×9的国际象棋盘图案标定板，结合单目相机视角场范围采集了50张不同位姿和距离的标定图像以覆盖相机的全部视场；

S12，利用采集到的标定图像通过张正友标定法完成单目相机的标定，得到相机的内参矩阵K；

S13，选取激光测距模块坐标系作为本系统的世界坐标系，根据单目相机和激光测距模块之间的相对位置及联合标定工具获取相机的外参矩阵M。

上述步骤S2具体包括：

S21，单目相机获取视野中的环境原始图像；

S22，对获取到的环境原始图像进行预处理，包括图像的裁切，实现单目相机数码变焦；

S23，根据获取到的环境图像利用物体识别神经网络或其他待定位目标给定程序有效确定待定位目标A的像素坐标(u,v)，即为激光测距模块测距光点在相机像素坐标系中的目标位置。

所述步骤S22具体为：

S221，编写图像预处理程序读入单目相机获取到的原始图像数据；

S222，在图像预处理程序中按照一定的裁切系数以原始图像中心为中心点对图像进行裁切；

S223，将裁切完成图像发布出去，动态改变目标物体在相机视野中的尺度。

所述S3步骤具体包括：

S31，将S2获得的目标点像素坐标(u,v)作为控制信号输入到PID反馈控制中，驱动执行机构带动激光测距模块开始定位过程；

S32，在定位过程中连续采集激光测距模块测量点的距离d和激光测距模块的俯仰角α和航向角β，计算转换得到当前测距点在激光测距模块坐标系即系统世界坐标系下的三维坐标(X_W,Y_W,Z_W)，式(1)为(X_W,Y_W,Z_W)的计算公式；得到当前测距点在激光测距模块坐标系即系统世界坐标系下的三维坐标(X_W,Y_W,Z_W)，利用相机外参矩阵M和内参矩阵K，可计算转换得到激光测距模块当前测距点在相机像素坐标系的二维坐标(u′,v′)，此坐标作为反馈量引入PID反馈控制中，实现测距点的闭环控制，驱动激光测距模块将测距光点准确打到目标位置A，式(2)为系统世界坐标系下测距点的三维坐标(X_W,Y_W,Z_W)与相机像素坐标系下二维坐标(u′,v′)的转换关系。

上述S4步骤具体包括：

S41，经所述激光测距模块测距点打到目标位置后，采集此时激光测距模块测距点距离d′俯仰角α′和航向角β′；

S42，利用S41中采集得到的激光测距模块测距点距离d′俯仰角α′和航向角β′，通过式(3)可计算得出待定位目标点在系统坐标系下的实际三维坐标(X′_W,Y_W′,Z′_W)。

以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (5)

1.一种基于激光与单目视觉融合的像素级目标定位方法，其特征在于，包括如下步骤：

S1，安装相机使得相机光轴平行于地面，安装激光测距模块使得激光测距模块光心与相机光心的连线垂直于地面且使激光测距模块与单目相机之间的垂直间距尽可能小，对单目相机和激光测距模块进行标定联合标定，得到标定参数包括相机内参矩阵K和相机外参矩阵M；

S2，在相机像素坐标系中给出待定位物体的像素坐标，即为激光测距模块测距光点在相机像素坐标系中的目标位置；

S3，利用所述相机内参矩阵K、外参矩阵M和待定位物体像素坐标，通过PID反馈控制驱动执行机构带动激光测距模块转动，进行物体定位，反馈控制的反馈量由激光测距模块引入，根据激光测距模块当前时刻测距点的距离信息和激光测距模块在航向轴、俯仰轴实际转动角度，计算转换得到当前时刻测距点在激光测距模块坐标系即系统世界坐标系下的三维坐标

，利用相机外参矩阵M和内参矩阵K得到当前激光测距模块测距点在相机像素坐标系下的像素坐标，将所述坐标作为反馈量引入PID反馈控制中来实现激光测距模块的闭环控制，驱动激光测距模块将测距光点准确打到目标位置；

S4，根据S3中激光测距模块测得的目标点距离和激光测距模块在航向轴、俯仰轴实际转动角度，计算转换得到目标点在激光测距模块坐标系即系统世界坐标系下的三维坐标

，实现物体的精确定位；

所述S3步骤具体包括：

S31，将所述的目标点像素坐标

作为控制信号输入到PID反馈控制中，驱动执行机构带动激光测距模块开始定位过程；

S32，在定位过程中连续采集激光测距模块测量点的距离d和激光测距模块的俯仰角α和航向角β，计算转换得到当前测距点在激光测距模块坐标系即系统世界坐标系下的三维坐标

，

的计算公式为

；得到当前测距点在激光测距模块坐标系即系统世界坐标系下的三维坐标

，利用相机外参矩阵M和内参矩阵K，计算转换得到激光测距模块当前测距点在相机像素坐标系的二维坐标

，并将该坐标作为反馈量引入PID反馈控制中，实现测距点的闭环控制，驱动激光测距模块将测距光点准确打到目标位置，系统世界坐标系下测距点的三维坐标

与相机像素坐标系下二维坐标

的转换关系为：

。

2.如权利要求1所述的一种基于激光与单目视觉融合的像素级目标定位方法，其特征在于，所述S1步骤具体包括：

S11，相机标定板选取为12×9的国际象棋盘图案标定板，结合单目相机视角场范围采集50张不同位姿和距离的标定图像以覆盖相机的全部视场；

S12，利用采集到的标定图像通过张正友标定法完成单目相机的标定，得到相机的内参矩阵K；

S13，选取激光测距模块坐标系作为本系统的世界坐标系，根据单目相机和激光测距模块之间的相对位置及联合标定工具获取相机的外参矩阵M。

3.如权利要求1所述的一种基于激光与单目视觉融合的像素级目标定位方法，其特征在于，所述S2步骤具体包括：

S21，单目相机获取视野中的环境原始图像；

S22，对获取到的环境原始图像进行预处理，包括图像的裁切，实现单目相机数码变焦；

S23，根据获取到的环境图像利用物体识别神经网络或其他待定位目标给定程序有效确定待定位目标像素坐标

，即为激光测距模块测距光点在相机像素坐标系中的目标位置。

4.如权利要求3所述的一种基于激光与单目视觉融合的像素级目标定位方法，其特征在于，所述S22步骤具体包括：

S221，编写图像预处理程序读入单目相机获取到的环境原始图像数据；

S222，在图像预处理程序中按照一定的裁切系数以环境原始图像中心为中心点对图像进行裁切；

S223，将裁切完成的图像发布出去，动态改变目标物体在相机视野中的尺度。

5.如权利要求1所述的一种基于激光与单目视觉融合的像素级目标定位方法，其特征在于，所述S4步骤具体包括：

S41，经所述激光测距模块测距点打到目标位置后，采集此时激光测距模块测距点距离d ^， 、俯仰角α ^，和航向角β ^， ；

S42，利用采集得到的激光测距模块测距点距离d ^， 俯仰角α ^，和航向角β ^， ，通过式

计算得出待定位目标点在系统坐标系下的实际三维坐标

。

Images