CN114692731A - 基于单目视觉与激光测距阵列的环境感知融合方法及系统 - Google Patents

Abstract

本发明公开了一种基于单目视觉与激光测距阵列的环境感知融合方法及系统，包括MCU电路、N个摄像头及N个激光测距阵列，所述摄像头及激光测距阵列分别与MCU电路连接，每个激光测距阵列的上方设置一个摄像头，N个激光测距阵列实现移动机器人的360度环境感知，相邻摄像头间隔角度差为360/N度，每个激光测距阵列包括M个激光测距模块，相邻激光测距模块的角度差为360/N×M。本发明测量精度高、相应速度快、测量范围广，能够很好地与视觉信息相融合，完成对目标和障碍物的感知。

Description

基于单目视觉与激光测距阵列的环境感知融合方法及系统

技术领域

本发明涉及移动机器人环境感知技术领域，具体涉及基于单目视觉与激光测距阵列的环境感知融合方法及系统。

背景技术

移动机器人环境感知技术是指移动机器人能够根据自身携带的传感器对所处环境进行信息的获取，提取环境中的有效信息，加以理解，最终得到所处环境的模型。移动机器人环境感知技术是实现机器人定位与导航的前提，视觉传感器是移动机器人环境感知系统中使用最为广泛的传感器，它获取的信息丰富、结构简单，通常与激光雷达、超声波传感器等传感器组合成为视觉系统。激光雷达拥有测量距离范围广、精度高、响应速度快等优点，能够给视觉提供较好的环境深度信息，但是激光雷达的价格昂贵；超声波传感器价格低，但是它的分辨率低、响应速度慢，无法与视觉信息较好地融合。

发明内容

为了克服现有技术存在的缺点与不足，本发明提供一种基于单目视觉与激光测距阵列的环境感知融合方法及系统。

本发明采用如下技术方案：

一种基于单目视觉与激光测距阵列的环境感知融合系统，包括MCU电路、N个摄像头及N个激光测距阵列，所述摄像头及激光测距阵列分别与MCU电路连接，每个激光测距阵列的上方设置一个摄像头，N个激光测距阵列实现移动机器人的360度环境感知，相邻摄像头间隔角度差为360/N度，每个激光测距阵列包括M个激光测距模块，相邻激光测距模块的角度差为360/N×M。

进一步，每个激光测距阵列的测量范围大于对应摄像头的视野范围。

进一步，包括四个摄像头及四个激光测距阵列，相邻摄像头间隔90度角，每个激光测距阵列包括五个激光测距模块实现前方90度的探测，相邻两个激光测距模块的平面夹角18°。

进一步，所述激光测距模块包括激光测距传感器，型号为VL53L1X。

进一步，所述摄像头的型号OV5640，其视场角为63°，其中三个激光测距模块位于摄像头的视野范围内。

进一步，所述融合方法包括如下：

S1初始化摄像头及激光测距模块，所述激光测距模块为五个，分别为第一、第二、第三、第四及第五激光测距模块；

S2摄像头获取一帧图像，输入MCU电路，得到位于摄像头视野范围内的第二、第三及第四激光测距模块的测距信息D2、D3及D4，进一步得到三个测距信息中的最小距离值d_min；

S3最小距离值与设定的避障距离阈值进行比较，若小于避障距离阈值，则执行S6避障流程，若大于避障距离阈值，则执行下一步骤；

S4将获得的障碍物与地板间的边界从世界坐标系投影至像素坐标系，得到障碍物与地板间的边界在像素坐标系中的v轴坐标v₁；

S5从图像中分割出含有障碍物的子图像，并处理后进行障碍物边缘提取，得到视野内障碍物在像素图像中的左右边界信息；

S6执行避障流程。

进一步，执行避障流程之前还包括设置优先级步骤，具体为：

将机器人前方45°到135°的区域划分为五个扇区，每个扇区内都包含一个偏转方向，相邻两个偏转方向间隔18°，每个扇区占有的角度为18°，五个扇区分别映射到像素坐标U轴的I、II、III、IV、V五个分区，其中分区III的优先级为1，分区II和IV的优先级为2，分区I和V的优先级为3，机器人优先向高优先级区域进行偏转。

进一步，所述避障流程包括：

首先，根据障碍物边界信息判断五个分区中是否含有障碍物，对于分区II、III、IV，根据障碍物左右边界的u轴坐标来判断分区内是否含有障碍物；对于分区I和V，根据第一及第五激光测距模块测得的距离值来判断，若激光测距模块的距离值小于避障阈值d_th，则代表分区内有障碍物；

接着获取最优的偏转方向，若分区III内没有障碍物，系统保持原方向不变偏转角度为0°；若分区III内含有障碍物，分区II或IV内没有障碍物，则机器人向分区II或IV内的偏转方向旋转，若分区II和IV内都不含障碍物，则需要根据分区I和分区V的信息做判断，若分区I和分区V都含有或都未含有障碍物，则机器人随机向分区II或IV内的偏转方向旋转，若分区I内含有障碍物并且分区V内没有障碍物，机器人向分区II内的偏转方向旋转，反之，向分区IV内的偏转方向旋转；若分区I、II、III内都含有障碍物，并且分区I或V内没有障碍物，机器人向分区I或分区V内的偏转方向旋转，否则随机向其中一个方向旋转。

进一步，所述融合方法还包括：

摄像机参数标定，确定摄像机倾斜角及得到被测物体在像素坐标v轴上的近似投影位置。

进一步，S5中得到视野内障碍物在像素图像中的左右边界信息，具体为：

对于左边界，从子图像第一列开始，遍历到最后一列，得到的第i个左边界，满足以下条件：列数为第i列并且该列所有像素点的值不全为零，或者该列所有像素点的值不全为零并且前一列所有像素点的值都为零，记录下该左边界的列数L_i，最终得到一个左边界点的集合L＝{L₁,...,L_i}；

对于右边界，从子图像的最后一列开始，遍历到第一列，得到的第i个右边界满足以下条件：列数为最后一列并且该列所有像素点的值不全为零，或者该列所有像素点的值不全为零并且后一列所有像素点的值都为零，记录下该左边界所在的列数R_i，最终得到一个右边界的集合R＝{R₁,...,R_i}，最后将集合L中的L_j和集合R中的R_i-j进行配对，得到视野中多个障碍物的左右边界信息。

本发明的有益效果：

(1)本发明把单目相机和激光测距传感器阵列结合起来，成本低、精度高，可广泛应用于自主机器人(如扫地机器人等)的组成部件中。

(2)机器视觉的软件部分采用OpenMV方式来实现，通过激光测距阵列对视觉处理区域的提取，大大减少视频图像数据量，使得采用通用STM32H7单片机就可以完成机器视觉的软件运行，实现了低成本。

(3)将激光测距阵列的信息与视觉信息相融合，结合坐标系转换，估算出被测物体在像素坐标的投影，无需处理整幅图像，提高了障碍物检测的速度，特别适合高度较低的贴着地面运行的扫地机器人等自主系统。

(4)采用MCU完成整个系统的激光测距和图像处理，功耗低，不到1.5W，具有良好的应用前景。

附图说明

图1是本发明的结构示意图；

图2是本发明五个激光测距模块的支架安装示意图；

图3是本发明环境感知融合系统的俯视图；

图4是本发明环境感知融合系统的侧视图；

图5是本发明环境感知融合系统的工作流程图；

图6是本发明的避障流程示意图。

具体实施方式

下面结合实施例及附图，对本发明作进一步地详细说明，但本发明的实施方式不限于此。

实施例

如图1所示，一种基于单目视觉与激光测距阵列的环境感知融合系统，包括MCU电路、N个摄像头、N个激光测距阵列、电源和通信接口电路，所述摄像头及激光测距阵列分别与MCU电路连接，每个激光测距阵列的上方设置一个摄像头，N个激光测距阵列实现移动机器人的360度环境感知，相邻摄像头间隔角度差为360/N度，每个激光测距阵列包括M个激光测距模块，相邻激光测距模块的角度差为360/N×M。

一个激光测距阵列对应一个摄像头，摄像头安装在对应激光测距阵列的上方，每个激光测距阵列的测量范围大于对应摄像头的视野范围。

上述参数N和M均为自然数。

具体地，本实施例中，MCU电路包括MCU单片机，Flash存储、SDRAM动态存储器和SD卡和外围阻容元件，MCU单片机选用STM32H743IIT6，Flash存储选用W25Q256JV，SDRAM动态存储器选用IS42S32160F。W25Q256JV用于保存用户编辑的脚本文件，SD卡用于保存系统脚本文件、实时采集到的图片和视频；IS42S32160F具有64MB数据存储空间，使系统能够处理较大的图像和运行较复杂的机器视觉算法。

摄像头采集图像信息并通过并口传输给MCU电路，摄像头选用OV5640摄像头，视场角为63°。电源和通信接口电路从外部主机获取电能并通过RS422接口与主机通信，RS422接口芯片选用SP3490EN，从主机过来的+12V电源经过DC/DC转换器TPSM84209芯片降为+3.3V的电源电压为系统供电。

本实施例中，包括四个激光测距阵列及四个摄像头，相邻摄像头间隔90度，每个激光测距阵列探测前方90度范围。每个激光测距阵列包括五个激光测距模块，每个激光测距模块包括一个激光测距传感器和外围电路，所述激光测距传感器为VL53L1X型号。

如图2所示，所述五个激光测距模块安装在一个定制支架上，使得相邻两个激光测距模块平面夹角为18°，五个激光测距模块的组合能够探测前方90°视野范围内物体的距离。MCU电路通过I/O引脚PH2和PH3模拟I2C总线连接VL53L1X，获取其测量范围中物体的距离值。

如图3所示，以摄像头的光心为坐标原点，建立世界坐标系X_wY_wZ_w，V1～V5表示第一、第二……第五激光测距模块，测量得到的距离值为D1～D5，设置每个激光测距模块的视场角为γ＝20°，相邻两个激光测距模块平面相隔18°，五个激光测距模块的测距范围能够覆盖β＝90°的范围；摄像头的视场角为α＝63°，能够覆盖第二激光测距模块V2、第三激光测距模块V3及第四激光测距模块V4三个激光测距模块的测距范围，d1为激光测距模块到摄像头光心的偏移，D为其中一个激光测距模块的距离值，LD为激光测距模块测得的障碍物的距离在Z_w轴投影。

如图4所示，α为摄像头的视场角，δ为摄像头的倾斜角，h为摄像头高度，LD为激光测距模块所测物体的距离在Z_w轴的投影，d为激光测距模块到摄像头光心距离，以摄像头的光心为坐标原点，建立坐标系，X_cY_cZ_c为相机坐标系，X_wY_wZ_w为世界坐标系。

物体检测与定位流程实现的功能是获取物体在图片中的边界与相对自身的距离。为了提高物体的检测速率与正确率，提出了一种摄像头与激光测距阵列信息相融合的物体检测方法，通过融合激光测距阵列的信息，得到一张包含边界信息的子图像，它远远小于原图像，大大缩小了图像处理时间并去除了多余的干扰。

摄像机参数标定，选取一张9×6大小的标定板，其中每个方格的大小为25mm,将标定板置于摄像头的正前方，将标定板向不同的方向旋转一个小角度，保存总计100张不同角度的图片，将这100张图片放入Matlab的标定工具箱中完成标定，最终得到摄像机的内参矩阵K，如式(1)所示，其中，(u₀,v₀)为摄像机的光学中心，k_s是倾斜参数，k_x和k_y分别表示X轴和Y轴的放大系数。

确定摄像头摄像机倾斜角δ，摄像机的世界坐标系和像极坐标系之间的转换需要摄像机的倾斜角度信息。从图4可以看出，δ与S、d、h存在如式(2)的关系：

其中，h为摄像头光心到地面的距离，d为激光测距模块中心到摄像头光心的水平距离，S为激光测距模块测得的距离在Z_w轴的投影。h、d是已知的，因此只需知道S即可得到倾斜角δ的值。S值的获取过程如下：使能摄像头和激光测距模块V3，在图像中绘制一条平行于像素坐标u且平分图像的红线，接着，将一矩形障碍物置于摄像头的正前方，调矩形障碍物的位置，直到矩形障碍物的长边与图像中的红线相重合，此时LD＝S，记录此时激光测距模块V3测得的距离值D3，此时S＝D3，结合式(5)计算得到倾斜角δ。

像素坐标系到世界坐标系的转换。式(3)为像素坐标系uv到相机坐标系X_cY_cZ_c的转换，其中(u₁,v₁)为像素坐标系中的一点，(X_c0,Y_c0,Z_c0)为相机坐标系中的一点，式(4)为相机坐标系X_cY_cZ_c到世界坐标系X_wY_wZ_w的转换，(X_w0,Y_w0,Z_w0)为世界坐标系中的一点。

结合式(3)和(4)，像素坐标中的v₁与世界坐标系中的Y_w0存在式(5)的关系，根据图4的几何关系，Y_w0＝-h，Z_w0＝LD+d，Z_c0由式6给出。对于LD，当激光测距模块V2-V4的测量距离d_min＝Min{D2,D3,D4}较小时，可以近似的认为被测物体与光轴的角度为18°，因此LD的值可以由式(7)近似的给出。至此，结合式(1)～(7)即可得到被测物体在像素坐标v轴上的近似投影位置。

Z_c0v₁＝k_y(Y_w0cosδ-Z_w0sinδ)+v₀(Y_w0sinδ+Z_w0cosδ) (5)

如图5所示，一种环境感知融合方法，包括如下步骤：

Step 51为各模块初始化，初始化每个模块的摄像头和激光测距传感器，复位感光元件，开启自动曝光模式，设置像素格式为RGB565，图像大小为QVGA，初始化激光测距传感器VL53L1X，实例化5个I2C对象，频率为400KHZ，5对SCL和SDA引脚分别为(PE3，PI8)、(PC3，P13)、(PH2，PH3)、(P10，P2)和(PD5，PD4)，然后使用这5个I2C对象实例化5个VL53L1X对象，实例化串口UART1，波特率为115200，采用1位停止位，至此，各模块初始化完成；

Step 52获取一帧图像，调用Python函数sensor.snapshot()获取一帧图像，保存于变量img中。

Step 53 MCU电路获取摄像头视野内的激光测距模块V2～V4的测距信息D2～D4，调用VL53L1X对象的read方法读取激光测距模块V2～V4测量的距离值，并求出三者中的最小距离值d_min。

Step 54对该最小距离d_min与避障距离阈值d_th进行判断，若d_min大于d_th，直接执行Step58的避障算法；若d_min小于d_th，则执行Step55；

Step55根据式(1)～(7)和d_min，将障碍物与地板间的边界从世界坐标系投影至像素坐标系，得到障碍物与地板间的边界在像素坐标中的近似v轴坐标v₁；

Step56进行图像分割和滤波处理，从原图像中分割出以像素坐标(0,max(0,v₁-20))为起点，长为40，宽度为320的子图像，并调用Python函数img.to_grayscale()、img.binary()、img.open()和img.find_edges()对子图像进行灰度化、二值化、开运算处和边缘滤波处理；

Step57执行障碍物边界提取算法，具体为：对于左边界，从子图像第一列开始，遍历到最后一列，得到的第i个左边界需满足以下条件：列数为第i列并且该列所有像素点的值不全为零，或者该列所有像素点的值不全为零并且前一列所有像素点的值都为零，记录下该左边界的列数L_i，最终得到一个左边界点的集合L＝{L₁,...,L_i}；

对于右边界，从子图像的最后一列开始，遍历到第一列，得到的第i个右边界满足以下条件：列数为最后一列并且该列所有像素点的值不全为零，或者该列所有像素点的值不全为零并且后一列所有像素点的值都为零，记录下该左边界所在的列数R_i，最终得到一个右边界的集合R＝{R₁,...,R_i}，最后将集合L中的L_j和集合R中的R_i-j进行配对，得到视野中多个障碍物的左右边界信息。

Step58执行避障算法，之后回到Step52,循环继续进行下一幅图像的处理。

图6给出了感知系统避障过程示意图，包括如下步骤：

将感知系统前方45°到135°的区域划分为5个扇区，每个扇区内都包含一个偏转方向，相邻两个偏转方向间隔18°，每个扇区占有的角度为18°，五个扇区分别映射到像素坐标U轴的I、II、III、IV、V5个分区，其中分区III的优先级为1，分区II和IV的优先级为2，分区I和V的优先级为3，感知系统优先向高优先级区域进行偏转，优先级别为1的是最高级别。

II、III、IV分区是在摄像头视野范围内的，它的障碍物信息是图像中的边界，判断障碍物是根据边界的长度是否大于某个阈值，阈值的单位是像素；1和5分区是在摄像头范围外的，它们判断障碍物是根据激光测距模块1和5的测距值是否小于某个值，单位是mm。

感知系统的避障算法实现具体如下：

首先，根据上一步得到的障碍物边界信息判断5个分区中是否含有障碍物，对于分区II、III、IV，根据上一步得到的障碍物左右边界的u轴坐标来判断分区内是否含有障碍物，对于分区I和V，根据激光测距传感器V1和V5测得的距离值来判断，若激光测距传感器的值小于避障阈值d_th，则代表分区内有障碍物；接着获取最优的偏转方向，若分区III内没有障碍物，系统保持原方向不变偏转角度为0°；若分区III内含有障碍物，分区II或IV内没有障碍物，则感知系统向段II或IV内的偏转方向旋转，若分区II和IV内都不含障碍物，则需要根据分区I和V的信息做判断，若分区I和V都含有或都未含有障碍物，则感知系统随机向段II或IV内的偏转方向旋转，若分区I内含有障碍物并且段V内没有障碍物，感知系统向分区II内的偏转方向旋转，反之，向分区IV内的偏转方向旋转；若分区I、II、III内都含有障碍物，并且分区I或V内没有障碍物，感知系统向分区I或V内的偏转方向旋转，否则随机向其中一个方向旋转。

因此，本发明提出了一种基于单目视觉与激光测距阵列相融合的环境感知系统，系统主要由视觉传感器和激光测距阵列构成，激光测距阵列由5个激光测距模块组成，它的测量精度高、相应速度快、测量范围广，能够很好地与视觉信息相融合，完成对目标和障碍物的感知，同时该系统成本低、运行功耗低、扩展性好，一个移动机器人可以配备一个或多个该系统。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受所述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (10)

1.一种基于单目视觉与激光测距阵列的环境感知融合系统，其特征在于，包括MCU电路、N个摄像头及N个激光测距阵列，所述摄像头及激光测距阵列分别与MCU电路连接，每个激光测距阵列的上方设置一个摄像头，N个激光测距阵列实现移动机器人的360度环境感知，相邻摄像头间隔角度差为360/N度，每个激光测距阵列包括M个激光测距模块，相邻激光测距模块的角度差为360/N×M。

2.根据权利要求1所述的环境感知融合系统，其特征在于，每个激光测距阵列的测量范围大于对应摄像头的视野范围。

3.根据权利要求1或2所述的环境感知融合系统，其特征在于，包括四个摄像头及四个激光测距阵列，相邻摄像头间隔90度角，每个激光测距阵列包括五个激光测距模块,每个激光测距模块实现前方90度的探测，相邻两个激光测距模块的平面夹角18°。

4.根据权利要求3所述的环境感知融合系统，其特征在于，所述激光测距模块包括激光测距传感器，型号为VL53L1X。

5.根据权利要求3所述的环境感知融合系统，其特征在于，所述摄像头的型号OV5640，其视场角为63°，其中三个激光测距模块位于摄像头的视野范围内。

6.一种实现权利要求3所述的环境感知融合系统的融合方法，其特征在于，包括如下：

S1初始化摄像头及激光测距模块，所述激光测距模块为五个，分别为第一、第二、第三、第四及第五激光测距模块；

S2摄像头获取一帧图像，输入MCU电路，得到位于摄像头视野范围内的第二、第三及第四激光测距模块的测距信息D2、D3及D4，进一步得到三个测距信息中的最小距离值d_min；

S3最小距离值与设定的避障距离阈值进行比较，若小于避障距离阈值，则执行S6避障流程，若大于避障距离阈值，则执行下一步骤；

S4将获得的障碍物与地板间的边界从世界坐标系投影至像素坐标系，得到障碍物与地板间的边界在像素坐标系中的v轴坐标v₁；

S5从图像中分割出含有障碍物的子图像，并处理后进行障碍物边缘提取，得到视野内障碍物在像素图像中的左右边界信息；

S6执行避障流程。

7.根据权利要求6所述的融合方法，其特征在于，执行避障流程之前还包括设置优先级步骤，具体为：

将机器人前方45°到135°的区域划分为五个扇区，每个扇区内都包含一个偏转方向，相邻两个偏转方向间隔18°，每个扇区占有的角度为18°，五个扇区分别映射到像素坐标U轴的I、II、III、IV、V五个分区，其中分区III的优先级为1，分区II和IV的优先级为2，分区I和V的优先级为3，机器人优先向高优先级区域进行偏转。

8.根据权利要求7所述的融合方法，其特征在于，所述避障流程包括：

首先，根据障碍物边界信息判断五个分区中是否含有障碍物，对于分区II、III、IV，根据障碍物左右边界的u轴坐标来判断分区内是否含有障碍物；对于分区I和V，根据第一及第五激光测距模块测得的距离值来判断，若激光测距模块的距离值小于避障阈值d_th，则代表分区内有障碍物；

接着获取最优的偏转方向，若分区III内没有障碍物，系统保持原方向不变偏转角度为0°；若分区III内含有障碍物，分区II或IV内没有障碍物，则机器人向分区II或IV内的偏转方向旋转，若分区II和IV内都不含障碍物，则需要根据分区I和分区V的信息做判断，若分区I和分区V都含有或都未含有障碍物，则机器人随机向分区II或IV内的偏转方向旋转，若分区I内含有障碍物并且分区V内没有障碍物，机器人向分区II内的偏转方向旋转，反之，向分区IV内的偏转方向旋转；若分区I、II、III内都含有障碍物，并且分区I或V内没有障碍物，机器人向分区I或分区V内的偏转方向旋转，否则随机向其中一个方向旋转。

9.根据权利要求6所述的融合方法，其特征在于，还包括：

摄像机参数标定，确定摄像机倾斜角及得到被测物体在像素坐标v轴上的近似投影位置。

10.根据权利要求9所述的融合方法，其特征在于，S5中得到视野内障碍物在像素图像中的左右边界信息，具体为：

对于左边界，从子图像第一列开始，遍历到最后一列，得到的第i个左边界，满足以下条件：列数为第i列并且该列所有像素点的值不全为零，或者该列所有像素点的值不全为零并且前一列所有像素点的值都为零，记录下该左边界的列数L_i，最终得到一个左边界点的集合L＝{L₁,...,L_i}；

对于右边界，从子图像的最后一列开始，遍历到第一列，得到的第i个右边界满足以下条件：列数为最后一列并且该列所有像素点的值不全为零，或者该列所有像素点的值不全为零并且后一列所有像素点的值都为零，记录下该左边界所在的列数R_i，最终得到一个右边界的集合R＝{R₁,...,R_i}，最后将集合L中的L_j和集合R中的R_i-j进行配对，得到视野中多个障碍物的左右边界信息。

Images