CN111047636B - 基于主动红外双目视觉的避障系统和避障方法 - Google Patents

Abstract

本发明涉及基于主动红外双目视觉的避障系统和避障方法，其中避障系统包括：视差图计算模块，用以计算双目图像的视差图；三维点云生成模块，所述视差图遍历所述双目图像，得到三维点云；三维点云障碍物检测模块；所述三维点云障碍物检测模块包括滤波模块和地面检测模块；所述地面检测模块对经过所述滤波模块滤波后的三维点云计算地面点云；将滤波后的三维点云减去地面点云得到障碍物点云。本发明在计算视差图时，只考虑像素梯度比较大的点，计算半稠密点云，大大减少了计算量，保证了实时性。

Description

基于主动红外双目视觉的避障系统和避障方法

技术领域

本发明涉及双目立体视觉的避障技术，具体涉及一种基于主动红外双目视觉的避障系统和避障方法。

背景技术

目前，基于双目立体视觉的障碍物检测方法，基本是由两个摄像头同步获取立体图像对，再通过立体匹配算法，计算所有像素的视差，得到视差图，根据立体视觉的几何原理，得到稠密三维点云。该过程计算量大，实时性很难保证。同时，基于双目立体视觉的避障大都是基于三维点云的，地面移动机器人的避障基本没有对点云做地面分割处理，导致避障误报。另外，现有技术在某些环境或条件下(例如，弱纹理、黑暗环境等)，双目立体视觉得到的三维点云精度特别差，容易导致避障误报或漏报。

发明内容

针对现有技术的不足，本发明公开了基于主动红外双目视觉的避障系统和避障方法。

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

一种基于主动红外双目视觉的避障系统，包括：

视差图计算模块，用以计算双目图像的视差图；

三维点云生成模块，所述视差图遍历所述双目图像，得到三维点云；

三维点云障碍物检测模块；所述三维点云障碍物检测模块包括滤波模块和地面检测模块；所述地面检测模块对经过所述滤波模块滤波后的三维点云计算地面点云；将滤波后的三维点云减去地面点云得到障碍物点云。

其进一步的技术方案为，还包括双目校正模块；双目校正模块对双目图像进行去畸变和立体矫正操作，得到校正后的双目图像，传输至视差图计算模块。

其进一步的技术方案为，所述滤波模块包括体素滤波模块和直通滤波模块。

其进一步的技术方案为，所述地面检测模块包括初步地面点云计算模块和最终地面点云计算模块；所述初步地面点云计算模块排除三维点云中法线方向与重力方向之间的夹角超过设定阈值的三维点；所述最终地面点云计算模块将初步地面点云划分为多个点云簇，并根据最大点云簇的性质对所述多个点云簇滤波；所述最大点云簇和多个滤波后点云簇组合为最终地面点云。

一种基于主动红外双目视觉的避障方法，包括：

视差图计算，计算双目图像的视差图；

三维点云生成，所述视差图遍历所述双目图像，得到三维点云；

三维点云障碍物检测；对滤波后的三维点云计算地面点云；将滤波后的三维点云减去地面点云得到障碍物点云。

其进一步的技术方案为，还包括双目校正；在视差图计算步骤之前，对双目图像进行去畸变和立体矫正操作。

其进一步的技术方案为，所述视差图的计算方法是：以第一目图像中的视差待计算点为基准选定第一图像块；在第二目图像中，计算与第一图像块相似度最大的图像块作为第二图像块；计算第一图像块和第二图像块的视差值得到视差图。

其进一步的技术方案为，在所述三维点云障碍物检测步骤中，滤波步骤依次包括体素滤波和直通滤波。

其进一步的技术方案为，在所述三维点云障碍物检测步骤中，地面点云的计算方法是：

计算初步地面点云；计算三维点云中每个三维点的法线方向，将其与重力方向比较；当所述法线方向与所述重力方向的夹角小于设定阈值时保留该三维点，认为其是地面的一部分，大于设定阈值的则过滤掉，从而得到初步地面点云；

计算最终地面点云；对初步地面点云进行欧拉聚类，将此平面点云划分为多个点云簇；选择最大的点云簇，计算其质心坐标和最大包围盒；根据质心坐标和最大包围盒对其他点云簇进行滤波，将滤波后的点云和最大点云簇组合起来作为最终地面点云。

本发明的有益效果如下：

在基于双目立体视觉的障碍物检测方法的领域内，相对于现有技术中将两张图像通过立体匹配算法计算所有像素的视差图，得到稠密点云的方法，本发明在计算视差图时，只考虑像素梯度比较大的点，计算半稠密点云，大大减少了计算量，保证实时性；

现有技术在某些环境或条件下(例如，弱纹理、黑暗环境等)，双目立体视觉得到的三维点云精度特别差，容易导致避障误报或漏报；而本发明所采用的设备加了IR，在夜间或者纹理比较弱的地方，有主动红外光的辅助，计算的三维点云也会比较准确，使避障可以正常工作；

基于双目立体视觉的避障大都是基于三维点云的，地面移动机器人的避障基本没有对点云做地面分割处理，容易导致避障误报；本发明对通过视差图得到的点云进行了后处理，分割了地面部分点云，降低了误报概率。

附图说明

图1是实施例3的系统架构图。

图2为实施例5的方法流程图。

具体实施方式

以下结合附图说明本发明的具体实施方式。

实施例1。

实施例1为基于主动红外双目视觉的避障系统，具体包括：

视差图计算模块，用以计算双目图像的视差图；

三维点云生成模块，所述视差图遍历所述双目图像，得到三维点云；

三维点云障碍物检测模块；所述三维点云障碍物检测模块包括滤波模块和地面检测模块；所述地面检测模块对经过所述滤波模块滤波后的三维点云计算地面点云；将滤波后的三维点云减去地面点云得到障碍物点云。

实施例2。

实施例2在实施例1的基础上，增加了双目校正模块。双目校正模块对双目图像进行去畸变和立体矫正操作，得到校正后的双目图像，传输至视差图计算模块，之后进行实施例1中的步骤。

实施例3。

实施例3在实施例2的基础上，增加了硬件系统架构。图1是实施例3的系统架构图。如图1所示，系统架构包括摄像头和主控板。摄像头与主控板电学连接。可采用型号为Smart3399的主控板。拍摄双目图像的摄像头的水平视场角或者HFOV为120度，摄像头通过MIPI接口与主控板相连，通过软件同步输出同步双目图像。主控板系统为Ubuntu16.04LTS，并采用了ROS(Robot Operating System)框架。主控板中的避障系统对双目图像进行处理。

主控板中的避障系统包括双目校正模块、视差图计算模块、三维点云生成模块和障碍物检测模块。双目图像作为避障系统的输入数据。信号数据依次在双目校正模块、视差图计算模块、三维点云生成模块和障碍物检测模块中处理和传输。

双目校正模块的输入数据是摄像头所拍摄的双目图像。双目校正模块对双目图像进行去畸变和立体矫正操作，得到校正后双目图像。双目校正模块所输出的校正后双目图像输入至视差图计算模块。

视差图计算模块用以计算视差图。以第一目图像中的视差待计算点为基准选定第一图像块；在第二目图像中，计算与第一图像块相似度最大的图像块作为第二图像块；计算第一图像块和第二图像块的视差值得到视差图。之后视差图计算模块将视差图传输至三维点云生成模块。

三维点云生成模块通过视差图遍历双目图像，得到三维点云。三维点云生成模块将三维点云传输至三维点云障碍物检测模块。

三维点云障碍物检测模块在三维点云中计算地面点云。对三维点云滤波，得到滤波后点云。滤波后点云减去地面点云即得到障碍物点云。三维点云障碍物检测模块具体包括坐标转换模块、滤波模块和地面检测模块。坐标转换模块将单目相机的坐标系转换为机体坐标系。滤波模块包括体素滤波模块和直通滤波模块，分别依次对三维点云进行体素滤波和直通滤波。地面检测模块通过计算点云法线和欧拉聚类的方法确定平面的最终地面点云。滤波后的三维点云减去最终地面点云即为障碍物点云。三维点云障碍物检测模块最后输出当前视角的障碍物点云。地面检测模块包括初步地面点云计算模块和最终地面点云计算模块。初步地面点云计算模块排除三维点云中法线方向与重力方向之间的夹角超过设定阈值的三维点。最终地面点云计算模块将初步地面点云划分为多个点云簇，并根据最大点云簇的性质对所述多个点云簇滤波；所述最大点云簇和多个滤波后点云簇组合为最终地面点云。

实施例4。

实施例4用于说明种基于主动红外双目视觉的避障方法，具体包括以下步骤：

S1.两颗摄像头同步采集双目图像。

S2.视差图计算，计算双目图像的视差图；

S3.三维点云生成，所述视差图遍历所述双目图像，得到三维点云；

S4.三维点云障碍物检测；对滤波后的三维点云计算地面点云；将滤波后的三维点云减去地面点云得到障碍物点云。

实施例5。

在实施例4的基础上，实施例5增加了双目校正的步骤。图2为实施例5的方法流程图。如图2所示，具体包括：

S1.两颗摄像头同步采集双目图像。

S2.双目校正。根据离线标定的双目内参和双目外参，对双目图像进行去畸变和立体矫正操作，得到校正后双目图像。双目内参具体是指相机投影矩阵K1和K2以及畸变系数D1和D2。双目外参具体是指左右双目的旋转矩阵R和平移向量t。

S3.视差图计算。以第一目图像中的视差待计算点为基准选定第一图像块；在第二目图像中，计算与第一图像块相似度最大的图像块作为第二图像块；计算第一图像块和第二图像块的视差值得到视差图。

在本实施例中，以左目图像I作为第一目图像，以右目图像r作为第二目图像，并以左目相机的光学坐标系作为参考坐标系，也即相机前视时，X轴朝右、Y轴朝下、Z轴朝前，则具体有：

S31.遍历左目图像I中的每一个像素点，针对每一个像素点，计算其x方向的梯度l_x和y方向的梯度l_y。梯度l_x和梯度l_y的计算方法如下：

I_x＝I(x+1,y)-I(x,y) (1)

I_y＝I(x,y+1)-I(x,y) (2)

S32.计算梯度幅值M(x,y)。梯度幅值M(x,y)的计算方法为：

当梯度幅值M(x,y)大于设定阈值T₁时，此梯度幅值M(x,y)所对应的像素点为视差待计算点p_l。

S33.在左目图像I中，以视差待计算点p_l为中心，取设定面积S₁的第一图像块B_l。在本实施例中，设定面积S₁为16×16个像素点。

S34.在右目图像r中，以视差待计算点p_l为基准点，以设定阈值T₂为搜索范围，向左搜索。在本实施例中，设定阈值T₂为64像素。在搜索的过程中，以路过的每一个像素点为中心，都取设定面积S₂的第二图像块B_r。设定面积S₂＝设定面积S₁，在本实施例中，均为16×16个像素点。

S35.根据SAD度量算法计算第一图像块B_l和第二图像块B_r的相似度。其计算公式为：

D(B_l,B_r)_SAD＝∑|B_l(x,y)-B_r(x,y)| (4)

S36.根据Winner Takes All(WTA)原则，取相似度最大的像素点，作为右目图像r中的对应点p_r，计算视差待计算点p_l和对应点p_r的视差值d_p，视差值d_p的计算方法是：

d_p＝p_l-p_r (5)

在步骤S32中，视差待计算点p_l没有涵盖图像所有像素点，所以其他像素点没有计算视差值，将这些没有计算视差值的像素点设置为零，得到视差图。

S4.三维点云生成。通过得到的视差图，遍历该图像，计算三维点，得到三维点云，原理如下：

S41.根据相机的内参焦距f和外参基线B，计算视差图上每个像素点对应的深度，即Z坐标，其计算公式如下：

S42.根据Z坐标和相机的内参焦距f，计算每个像素点对应的X坐标和Y坐标，其计算公式如下：

式(7)和式(8)中，u表示双目相机的像素坐标系中像素点横坐标，v表示像素坐标系中像素点纵坐标标，c_x表示在相机坐标系(也即光心坐标系)中像素点的横坐标，c_y表示在相机坐标系(也即光心坐标系)中像素点的纵坐标。

最终，根据每个像素点的三维坐标(X,Y,Z)，得到每帧图像对应的三维点云。

S5.三维点云障碍物检测。

S51.坐标转换。步骤S4得到的三维点云是以左目相机的光学坐标系作为参考坐标系的，即相机前视时，X轴朝右、Y轴朝下、Z轴朝前。在进行三维点云的障碍物检测时，需要通过欧式变换矩阵T将其转换到机体坐标系，即X轴朝前、Y轴朝左、Z轴朝上。

S52.对三维点云进行体素滤波，得到第一次滤波后三维点云。体素滤波的参数为0.02，即对于每个边长为0.02m正方体中只取一个三维点。体素滤波是在减少点的数量、减少点云数据的同时保存点云的形状特征。

S53.根据设定的最大障碍物高度h₁和最小地面高度h₂，对第一次滤波后三维点云进行直通滤波(PassThrough滤波)，截取部分点云，得到第二次滤波后三维点云。

S54.估计检测地面。计算第二次滤波后三维点云中每个三维点的法线或法向量，将其与重力方向(0,0,1)比较，当其夹角小于设定阈值T₃时保留该三维点，认为其是地面的一部分，大于设定阈值T₃的则过滤掉，从而得到初步地面点云。初步地面点云是平面点云。在本实施例中，设定阈值T₃可以为45°。

S55.对初步地面点云进行欧拉聚类，将此平面点云划分为多个点云簇。选择最大点云簇，计算其质心坐标和最大包围盒。根据质心坐标和最大包围盒，再对其他点云簇进行滤波，将滤波后的点云和最大点云簇组合起来作为最终地面点云。最大包围盒也即包围点云簇的最大立方体坐标

S56.将直通滤波后的点云，也即第二次滤波后三维点云，去除最终地面点云，到当前视角的障碍物点云。

以上描述是对本发明的解释，不是对发明的限定，本发明所限定的范围参见权利要求，在不违背本发明的基本结构的情况下，本发明可以作任何形式的修改。

Claims (7)

1.一种基于主动红外双目视觉的避障系统，其特征在于包括：

双目校正模块；双目校正模块根据离线标定的双目内参和双目外参对双目图像进行去畸变和立体矫正操作，得到校正后的双目图像，传输至视差图计算模块；双目内参具体是指相机投影矩阵K1和K2以及畸变系数D1和D2；双目外参具体是指左右双目的旋转矩阵R和平移向量t；

视差图计算模块，用以计算双目图像的视差图；

以第一目图像中的视差待计算点为基准选定第一图像块；在第二目图像中，计算与第一图像块相似度最大的图像块作为第二图像块；计算第一图像块和第二图像块的视差值得到视差图；之后视差图计算模块将视差图传输至三维点云生成模块；

以左目图像I作为第一目图像，以右目图像r作为第二目图像，并以左目相机的光学坐标系作为参考坐标系，也即相机前视时，X轴朝右、Y轴朝下、Z轴朝前，则具体有：

遍历左目图像I中的每一个像素点，针对每一个像素点，计算其x方向的梯度l_x和y方向的梯度l_y；梯度l_x和梯度l_y的计算方法如下：

I_x＝I(x+1,y)-I(x,y) (1)

I_y＝I(x,y+1)-I(x,y) (2)

计算梯度幅值M(x,y)；梯度幅值M(x,y)的计算方法为：

当梯度幅值M(x,y)大于设定阈值T₁时，此梯度幅值M(x,y)所对应的像素点为视差待计算点p_l；

在左目图像I中，以视差待计算点p_l为中心，取设定面积S₁的第一图像块B_l；设定面积S₁为16×16个像素点；

在右目图像r中，以视差待计算点p_l为基准点，以设定阈值T₂为搜索范围，向左搜索；设定阈值T₂为64像素；在搜索的过程中，以路过的每一个像素点为中心，都取设定面积S₂的第二图像块B_r；设定面积S₂＝设定面积S₁，均为16×16个像素点；

根据SAD度量算法计算第一图像块B_l和第二图像块B_r的相似度；其计算公式为：

D(B_l,B_r)_SAD＝∑|B_l(x,y)-B_r(x,y)| (4)

根据Winner Takes All(WTA)原则，取相似度最大的像素点，作为右目图像r中的对应点p_r，计算视差待计算点p_l和对应点p_r的视差值d_p，视差值d_p的计算方法是：

d_p＝p_l-p_r (5)

视差待计算点p_l没有涵盖图像所有像素点，所以其他像素点没有计算视差值，将这些没有计算视差值的像素点设置为零，得到视差图；

三维点云生成模块，通过视差图遍历所述双目图像，得到三维点云；

三维点云障碍物检测模块；所述三维点云障碍物检测模块包括滤波模块和地面检测模块；所述地面检测模块对经过所述滤波模块滤波后的三维点云计算地面点云；将滤波后的三维点云减去地面点云得到障碍物点云；

根据相机的内参焦距f和外参基线B，计算视差图上每个像素点对应的深度，即Z坐标，其计算公式如下：

根据Z坐标和相机的内参焦距f，计算每个像素点对应的X坐标和Y坐标，其计算公式如下：

式(7)和式(8)中，u表示双目相机的像素坐标系中像素点横坐标，v表示像素坐标系中像素点纵坐标标，c_x表示在相机坐标系(也即光心坐标系)中像素点的横坐标，c_y表示在相机坐标系(也即光心坐标系)中像素点的纵坐标；

根据每个像素点的三维坐标(X,Y,Z)，得到每帧图像对应的三维点云。

2.根据权利要求1所述的基于主动红外双目视觉的避障系统，其特征在于：所述滤波模块包括体素滤波模块和直通滤波模块。

3.根据权利要求1所述的基于主动红外双目视觉的避障系统，其特征在于：所述地面检测模块包括初步地面点云计算模块和最终地面点云计算模块；所述初步地面点云计算模块排除三维点云中法线方向与重力方向之间的夹角超过设定阈值的三维点；所述最终地面点云计算模块将初步地面点云划分为多个点云簇，并根据最大点云簇的性质对所述多个点云簇滤波；所述最大点云簇和多个滤波后点云簇组合为最终地面点云。

4.一种基于主动红外双目视觉的避障方法，应用如权利要求1所述的基于主动红外双目视觉的避障系统，其特征在于包括：

视差图计算，计算双目图像的视差图；

所述视差图的计算方法是：以第一目图像中的视差待计算点为基准选定第一图像块；在第二目图像中，计算与第一图像块相似度最大的图像块作为第二图像块；计算第一图像块和第二图像块的视差值得到视差图；

以左目图像I作为第一目图像，以右目图像r作为第二目图像，并以左目相机的光学坐标系作为参考坐标系，也即相机前视时，X轴朝右、Y轴朝下、Z轴朝前，则具体有：

S31.遍历左目图像I中的每一个像素点，针对每一个像素点，计算其x方向的梯度l_x和y方向的梯度l_y；梯度l_x和梯度l_y的计算方法如下：

I_x＝I(x+1,y)-I(x,y) (1)

I_y＝I(x,y+1)-I(x,y) (2)

S32.计算梯度幅值M(x,y)；梯度幅值M(x,y)的计算方法为：

当梯度幅值M(x,y)大于设定阈值T₁时，此梯度幅值M(x,y)所对应的像素点为视差待计算点p_l；

S33.在左目图像I中，以视差待计算点p_l为中心，取设定面积S₁的第一图像块B_l；设定面积S₁为16×16个像素点；

S34.在右目图像r中，以视差待计算点p_l为基准点，以设定阈值T₂为搜索范围，向左搜索；设定阈值T₂为64像素；在搜索的过程中，以路过的每一个像素点为中心，都取设定面积S₂的第二图像块B_r；设定面积S₂＝设定面积S₁，均为16×16个像素点；

S35.根据SAD度量算法计算第一图像块B_l和第二图像块B_r的相似度；其计算公式为：

D(B_l,B_r)_SAD＝∑|B_l(x,y)-B_r(x,y)| (4)

S36.根据Winner Takes All(WTA)原则，取相似度最大的像素点，作为右目图像r中的对应点p_r，计算视差待计算点p_l和对应点p_r的视差值d_p，视差值d_p的计算方法是：

d_p＝p_l-p_r (5)

在步骤S32中，视差待计算点p_l没有涵盖图像所有像素点，所以其他像素点没有计算视差值，将这些没有计算视差值的像素点设置为零，得到视差图；

三维点云生成，通过视差图遍历所述双目图像，得到三维点云；

三维点云障碍物检测；对滤波后的三维点云计算地面点云；将滤波后的三维点云减去地面点云得到障碍物点云。

5.根据权利要求4所述的基于主动红外双目视觉的避障方法，其特征在于包括：还包括双目校正；在视差图计算步骤之前，对双目图像进行去畸变和立体矫正操作。

6.根据权利要求4所述的基于主动红外双目视觉的避障方法，其特征在于，在所述三维点云障碍物检测步骤中，滤波步骤依次包括体素滤波和直通滤波。

7.根据权利要求4所述的基于主动红外双目视觉的避障方法，其特征在于，在所述三维点云障碍物检测步骤中，地面点云的计算方法是：

计算初步地面点云；计算三维点云中每个三维点的法线方向，将其与重力方向比较；当所述法线方向与所述重力方向的夹角小于设定阈值时保留该三维点，认为其是地面的一部分，大于设定阈值的则过滤掉，从而得到初步地面点云；

计算最终地面点云；对初步地面点云进行欧拉聚类，将此平面点云划分为多个点云簇；选择最大的点云簇，计算其质心坐标和最大包围盒；根据质心坐标和最大包围盒对其他点云簇进行滤波，将滤波后的点云和最大点云簇组合起来作为最终地面点云。