CN108648219B - 一种基于双目的障碍物与可行区域检测方法 - Google Patents

Abstract

本发明提供了一种基于双目的障碍物与可行区域检测方法，包括：步骤1：标定双目系统；步骤2：通过视觉SLAM算法构建环境地图；步骤3：进行点云拼接；步骤4：将Set_w进行降采样，并将点云转到移动底盘坐标系下；步骤5：在转换后的Set_w中提取地面，得到可行区域Set_traversable；步骤6：在Set_traversable中提取障碍物区域，得到障碍物区域Set_obs。本发明在视觉SLAM系统构建完成环境地图的基础上，进一步结合双目匹配算法，得到大范围场景点云。该点云包含场景的连续位姿信息，同时由于多帧融合，有效提高点云精度，减少噪声，并进一步在融合后的点云中检测障碍物与可行区域，实现了基于双目的障碍物与可行区域检测。

Description

一种基于双目的障碍物与可行区域检测方法

技术领域

本发明涉及障碍物与可行区域检测技术领域，具体涉及一种基于双目的障碍物与可行区域检测方法。

背景技术

双目相机由于成本低廉，信息量大的优势在机器人构图、定位导航等应用中获得广泛应用。基于图像的构图通常使用视觉SLAM(simultaneous localization andmapping)系统，得到的地图形式为一系列关键帧和空间点的集合。该地图仅包含离散可行位姿，在实际应用中具有很大的局限性。双目相机可以通过左右图像的匹配得到真实世界的点云信息。然而图像匹配得到的点云存在噪声大、精度不高等问题，同样也制约了实际应用。

本发明在视觉SLAM系统构建完成环境地图的基础上，进一步结合双目匹配算法，得到大范围场景点云。该点云包含场景的连续位姿信息，同时由于多帧融合，有效提高点云精度，减少噪声，克服了上述两个缺点，并进一步在融合后的点云中检测障碍物与可行区域。

发明内容

本发明提供了一种基于双目的障碍物与可行区域检测方法，在视觉SLAM系统得到关键帧和空间点的基础上，融合双目系统得到场景稠密点云，在该点云中检测障碍物与可行区域信息。

本发明所述地图为关键帧集合

N_k表示关键帧数目。所述关键帧表示为K_i，

其中t_i表示关键帧K_i在世界坐标系中的位置，为3×1矩阵，R_i表示关键帧K_i的朝向，为3×3矩阵，

为双目相机系统在K_i位置时得到的图像。

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

一种基于双目的障碍物与可行区域检测方法，包括以下步骤：

步骤1：标定双目系统；

步骤1中，标定双目系统，分别得到两个相机的内参和外参，并且标定双目系统与移动底盘之间的旋转矩阵R_b和平移矩阵t_b。

步骤2：通过视觉SLAM算法构建环境地图；

所述的环境地图为关键帧集合

N_k表示关键帧数目，K_l、K_i、

表示不同的关键帧，关键帧

其中t_i表示关键帧K_i在世界坐标系中的位置，为3×1矩阵，R_i表示关键帧K_i的朝向，为3×3矩阵，

为双目相机系统在K_i位置时得到的图像。

步骤3：进行点云拼接；

步骤3.1：对于关键帧集合Set_K中的每个元素K_i，使用StereoBM算法(opencv开源库中的代码)计算双目视差图，并进一步将双目视差图转化为基于左相机的三维点云，形成点云集合Set_P＝{P₁...P_ip...P_mp}，其中，mp为K_i形成的点云总数目，

(u_ip,v_ip)为三维点对应的图像坐标，

为三维点相对于左相机的坐标，

为该点云在世界坐标系中的坐标，并建立以下关系式：

步骤3.2：对于P_ip∈Set_P，将P_ip投影到相邻关键帧K_i-1或者K_i+1，以投影到K_i-1为例：

R'＝R_i-1 ^T

t'＝-R_i-1 ^T×t_i-1

其中，ρ表示相机投影函数，由相机模型决定，(u',v')即为P_ip在K_i-1中的投影坐标，其中R^T表示对矩阵R进行转置，R_i-1 ^T表示对矩阵R_i-1进行转置。(x',y',z')为中间变量坐标。

步骤3.3：利用图像灰度值误差判定P_ip有效性；

I(u,v)记为获取图像I在(u,v)处的灰度值，

是指图像

在(u_ip+i,v_ip+j)处的灰度值，

是指图像

在(u'+i,v'+j)处的灰度值，如果I_err大于阈值ε，则P_ip无效，否则判定P_ip有效，将点

加入到集合Set_w，构建三维点集合Set_w＝{Pw₁...Pw_i...Pw_N}，

w_N为三维点的个数；

步骤4：将Set_w进行降采样，并将点云转到移动底盘坐标系下。

步骤4中，使用PCL点云库中的VoxelGridFilter对Set_w进行降采样处理，对于任意点(x_i,y_i,z_i)∈Set_w，执行下列公式将其转换到移动底盘坐标系下：

其中，R_b为旋转矩阵，t_b为平移矩阵，公式中左边的(x_i,y_i,z_i)为转换到移动底盘坐标系下的坐标，得到转换后的Set_w。公式中右边的(x_i,y_i,z_i)为转换前Set_w中点的坐标；

步骤5：在转换后的Set_w中提取地面，得到可行区域Set_traversable。对于任一点Pw_i＝(x_i,y_i,z_i)∈Set_w，在Set_w中查找距离Pw_i最近的三个点，{Pw¹＝(x¹,y¹,z¹),Pw²＝(x²,y²,z²),Pw³＝(x³,y³,z³)}，求得点Pw_i法向量

同理，通过在Set_w中查找距离Pw¹最近的三个点，得到Pw¹法向量n¹，通过在Set_w中查找距离Pw²最近的三个点，得到Pw²法向量n²，通过在Set_w中查找距离Pw³最近的三个点，得到Pw³法向量n³。

计算法向量误差n_error：

n_error＝3-|n_i·n¹|-|n_i·n²|-|n_i·n³|

如果Pw_i中z_i小于设定阈值z_thres，并且n_error小于阈值n_thres，则判定点Pw_i为可行区域，加入Set_traversable。

步骤6：在Set_traversable中提取障碍物区域，得到障碍物区域Set_obs。设置障碍物高度阈值h_thres。对于任意一点Pt_i∈Set_traversable，

检测转换后的Set_w中是否存在点Pw_j，

满足以下关系：

如果存在Pw_j满足上述关系，则判定Pt_i为障碍物区域，加入到Seto_bs中，并将该点在Set_traversable中删除。

综上所述，由于采用了上述技术方案，本发明的有益效果是：

本发明在视觉SLAM系统构建完成环境地图的基础上，进一步结合双目匹配算法，得到大范围场景点云。该点云包含场景的连续位姿信息，同时由于多帧融合，有效提高点云精度，减少噪声，并进一步在融合后的点云中检测障碍物与可行区域，实现了基于双目的障碍物与可行区域检测。

附图说明

图1为本发明基于双目的障碍物与可行区域检测方法的流程示意图。

具体实施方式

本说明书中公开的所有特征，或公开的所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以以任意方式组合。

实施例一：

步骤1：标定系统。标定双目系统，分别得到两个相机的内参和外参。标定双目系统与移动底盘之间的旋转矩阵R_b和平移矩阵t_b。

步骤2：通过视觉SLAM算法构建环境地图。所述的环境地图为关键帧集合

N_k表示关键帧数目，K₁、K_i、

表示不同的关键帧，关键帧

其中t_i表示关键帧K_i在世界坐标系中的位置，为3×1矩阵，R_i表示关键帧K_i的朝向，为3×3矩阵，

为双目相机系统在K_i位置时得到的图像。

步骤3：进行点云拼接。构建三维点集合Set_w＝{Pw₁...Pw_i...Pw_N}，Pw_i＝{x_i,y_i,z_i}，对于关键帧集合Set_K中的每个元素K_i，执行下列操作。

步骤3.1：使用StereoBM算法(opencv开源库中的代码)算法计算双目视差图。并进一步将双目视差图转化为基于左相机的三维点云。形成点云集合Set_P＝{P₁...P_ip...P_mp}，其中mp为K_i形成的点云总数目。

(u_ip,v_ip)为三维点对应的图像坐标，

为三维点相对于左相机的坐标，

为该点云在世界坐标系中的坐标。

步骤3.2：对于P_ip∈Set_P，将P_ip投影到相邻关键帧K_i-1或者K_i+1。以投影到K_i-1为例。

R'＝R_i-1 ^T

t'＝-R_i-1 ^T×t_i-1

其中，ρ表示相机投影函数，由相机模型决定，(u',v')即为P_ip在K_i-1中的投影坐标，其中R^T表示对矩阵R进行转置，R_i-1 ^T表示对矩阵R_i-1进行转置。(x',y',z')为中间变量坐标。

步骤3.3：利用图像灰度值误差判定P_ip有效性。

I(u,v)记为获取图像I在(u,v)处的灰度值。如果I_err大于阈值ε，则P_ip无效。否则判定P_ip有效，将点

加入到集合Set_w。I(u,v)记为获取图像I在(u,v)处的灰度值，

是指图像

在(u_ip+i,v_ip+j)处的灰度值，

是指图像

在(u'+i,v'+j)处的灰度值，如果I_err大于阈值ε，则P_ip无效，否则判定P_ip有效，将点

加入到集合Set_w，构建三维点集合Set_w＝{Pw₁...Pw_i...Pw_N}，

w_N为三维点的个数；

步骤4：将Set_w进行降采样，并将点云转到移动底盘坐标系下。使用PCL点云库中的VoxelGridFilter对点云进行降采样处理，设置降采样后的点云为Set_w。使用下列公式将点云转换到移动底盘坐标系下。

其中，R_b为旋转矩阵，t_b为平移矩阵，公式中左边的(x_i,y_i,z_i)为转换到移动底盘坐标系下的坐标，得到转换后的Set_w。公式中右边的(x_i,y_i,z_i)为转换前Set_w中点的坐标；

步骤5：在Set_w中提取地面，得到可行区域Set_traversable。对于任一点Pw_i∈Set_w，在Set_w中查找距离Pw_i最近的三个点，{Pw¹＝(x¹,y¹,z¹),Pw²＝(x²,y²,z²),Pw³＝(x³,y³,z³)}，求得点Pw_i法向量

同理，通过在Set_w中查找距离Pw¹最近的三个点，得到Pw¹法向量n¹，通过在Set_w中查找距离Pw²最近的三个点，得到Pw²法向量n²，通过在Set_w中查找距离Pw³最近的三个点，得到Pw³法向量n³。

计算法向量误差n_error：

n_error＝3-|n_i·n¹|-|n_i·n²|-|n_i·n³|

如果z_i小于阈值z_thres，并且n_error小于阈值n_thres，则判定点Pw_i为可行区域，加入Set_traversable。

步骤6：在Set_traversable中提取障碍物区域，得到障碍物区域Set_obs。设置障碍物高度阈值h_thres。对于任意一点Pt_i∈Set_traversable，检测Set_w中是否存在点Pw_j，满足以下关系：

则判定Pt_i为障碍物区域，加入到Set_obs中，并将该点在Set_traversable中删除。从而计算得到障碍物区域。

Claims (5)

1.一种基于双目的障碍物与可行区域检测方法，其特征在于，包括以下步骤：

步骤1：标定双目系统；

步骤2：通过视觉SLAM算法构建环境地图；

步骤3：进行点云拼接；

步骤4：将三维点集合Set_w进行降采样，并将点云转到移动底盘坐标系下；

步骤5：在转换后的Set_w中提取地面，得到可行区域Set_traversable，对于任一点Pw_i＝(x_i,y_i,z_i)∈Set_w，在Set_w中查找距离Pw_i最近的三个点，{Pw¹＝(x¹,y¹,z¹),Pw²＝(x²,y²,z²),Pw³＝(x³,y³,z³)}，求得点Pw_i法向量

同理，通过在Set_w中查找距离Pw¹最近的三个点，得到Pw¹法向量n¹，通过在Set_w中查找距离Pw²最近的三个点，得到Pw²法向量n²，通过在Set_w中查找距离Pw³最近的三个点，得到Pw³法向量n³；

计算法向量误差n_error：

n_error＝3-|n_i·n¹|-|n_i·n²|-|n_i·n³|

如果Pw_i中z_i小于设定阈值z_thres，并且n_error小于阈值n_thres，则判定点Pw_i为可行区域，加入Set_traversable；

步骤6：在Set_traversable中提取障碍物区域，得到障碍物区域Set_obs，设置障碍物高度阈值h_thres，对于任意一点Pt_i∈Set_traversable，

检测转换后的Set_w中是否存在点Pw_j，

满足以下关系：

如果存在Pw_j满足上述关系，则判定Pt_i为障碍物区域，将Pw_j加入到Set_obs中，并将该点Pw_j在Set_traversable中删除。

2.根据权利要求1所述的基于双目的障碍物与可行区域检测方法，其特征在于，步骤1中，标定双目系统，分别得到两个相机的内参和外参，并且标定双目系统与移动底盘之间的旋转矩阵R_b和平移矩阵t_b。

3.根据权利要求1所述的基于双目的障碍物与可行区域检测方法，其特征在于，步骤2中，所述的环境地图为关键帧集合

N_k表示关键帧数目，K₁、K_i、

表示不同的关键帧，关键帧

其中t_i表示关键帧K_i在世界坐标系中的位置，为3×1矩阵，R_i表示关键帧K_i的朝向，为3×3矩阵，

为双目相机系统在K_i位置时得到的图像。

4.根据权利要求3所述的基于双目的障碍物与可行区域检测方法，其特征在于，步骤3中，进行点云拼接，具体包括：

步骤3.1：对于关键帧集合Set_K中的每个元素K_i，使用StereoBM算法计算双目视差图，并进一步将双目视差图转化为基于左相机的三维点云，形成点云集合Set_P＝{P₁...,P_ip...,P_mp}，其中，mp为K_i形成的点云总数目，

(u_ip,v_ip)为三维点对应的图像坐标，

为三维点相对于左相机的坐标，

为该点云在世界坐标系中的坐标，并建立以下关系式：

步骤3.2：对于P_ip∈Set_P，将P_ip投影到相邻关键帧K_i-1或者K_i+1，以投影到K_i-1为例：

R'＝R_i-1 ^T

t'＝-R_i-1 ^T×t_i-1

其中，ρ表示相机投影函数，由相机模型决定，(u',v')即为P_ip在K_i-1中的投影坐标，其中R_i-1 ^T表示对矩阵R_i-1进行转置；

步骤3.3：利用图像灰度值误差I_err判定P_ip有效性；

I(u,v)记为获取图像I在(u,v)处的灰度值，

是指图像

在(u_ip+i,v_ip+j)处的灰度值，

是指图像

在(u'+i,v'+j)处的灰度值，如果I_err大于阈值ε，则P_ip无效，否则判定P_ip有效，将点

加入到集合Set_w，构建三维点集合Set_w＝{Pw₁...,Pw_i...,Pw_N}，

w_N为三维点的个数。

5.根据权利要求1所述的基于双目的障碍物与可行区域检测方法，其特征在于，步骤4中，对Set_w进行降采样处理，对于任意点(x_i,y_i,z_i)∈Set_w，执行下列公式将其转换到移动底盘坐标系下：

其中，R_b为旋转矩阵，t_b为平移矩阵，公式中左边的(x_i,y_i,z_i)为转换到移动底盘坐标系下的坐标，得到转换后的Set_w。

Images