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

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

Obstacle detection and feasible region detection method based on binocular vision

The present invention provides a method for detecting obstacles and feasible areas based on binocular objects, including: step 1: calibrating binocular system; step 2: constructing environment map by visual SLAM algorithm; step 3: splicing point clouds; step 4: downsampling Setw and transferring point clouds to moving chassis coordinate system; step 5: turning around The ground is extracted from the replaced Setw to get the feasible area Settraversable; Step 6: The obstacle area is extracted from Settraversable to get the obstacle area Setobs. On the basis of constructing the environment map by the visual SLAM system, the invention further combines the binocular matching algorithm to obtain a large range of scene point clouds. The point cloud contains the continuous pose information of the scene. Because of multi-frame fusion, the accuracy of the point cloud is improved and noise is reduced effectively. Obstacles and feasible regions are detected in the fused point cloud, which realizes the detection of obstacles and feasible regions.

【技术实现步骤摘要】
一种基于双目的障碍物与可行区域检测方法
本专利技术涉及障碍物与可行区域检测
，具体涉及一种基于双目的障碍物与可行区域检测方法。
技术介绍
双目相机由于成本低廉，信息量大的优势在机器人构图、定位导航等应用中获得广泛应用。基于图像的构图通常使用视觉SLAM(simultaneouslocalizationandmapping)系统，得到的地图形式为一系列关键帧和空间点的集合。该地图仅包含离散可行位姿，在实际应用中具有很大的局限性。双目相机可以通过左右图像的匹配得到真实世界的点云信息。然而图像匹配得到的点云存在噪声大、精度不高等问题，同样也制约了实际应用。本专利技术在视觉SLAM系统构建完成环境地图的基础上，进一步结合双目匹配算法，得到大范围场景点云。该点云包含场景的连续位姿信息，同时由于多帧融合，有效提高点云精度，减少噪声，克服了上述两个缺点，并进一步在融合后的点云中检测障碍物与可行区域。
技术实现思路
本专利技术提供了一种基于双目的障碍物与可行区域检测方法，在视觉SLAM系统得到关键帧和空间点的基础上，融合双目系统得到场景稠密点云，在该点云中检测障碍物与可行区域信息。本专利技术所述地图为关键帧集合Nk表示关键帧数目。所述关键帧表示为Ki，其中ti表示关键帧Ki在世界坐标系中的位置，为3×1矩阵，Ri表示关键帧Ki的朝向，为3×3矩阵，为双目相机系统在Ki位置时得到的图像。本专利技术采用的技术方案如下：一种基于双目的障碍物与可行区域检测方法，包括以下步骤：步骤1：标定双目系统；步骤1中，标定双目系统，分别得到两个相机的内参和外参，并且标定双目系统与移动底盘之间的旋转矩阵Rb和平移矩阵tb。步骤2：通过视觉SLAM算法构建环境地图；所述的环境地图为关键帧集合Nk表示关键帧数目，Kl、Ki、表示不同的关键帧，关键帧其中ti表示关键帧Ki在世界坐标系中的位置，为3×1矩阵，Ri表示关键帧Ki的朝向，为3×3矩阵，为双目相机系统在Ki位置时得到的图像。步骤3：进行点云拼接；步骤3.1：对于关键帧集合SetK中的每个元素Ki，使用StereoBM算法(opencv开源库中的代码)计算双目视差图，并进一步将双目视差图转化为基于左相机的三维点云，形成点云集合SetP＝{P1...Pip...Pmp}，其中，mp为Ki形成的点云总数目，(uip,vip)为三维点对应的图像坐标，为三维点相对于左相机的坐标，为该点云在世界坐标系中的坐标，并建立以下关系式：步骤3.2：对于Pip∈SetP，将Pip投影到相邻关键帧Ki-1或者Ki+1，以投影到Ki-1为例：R'＝Ri-1Tt'＝-Ri-1T×ti-1其中，ρ表示相机投影函数，由相机模型决定，(u',v')即为Pip在Ki-1中的投影坐标，其中RT表示对矩阵R进行转置，Ri-1T表示对矩阵Ri-1进行转置。(x',y',z')为中间变量坐标。步骤3.3：利用图像灰度值误差判定Pip有效性；I(u,v)记为获取图像I在(u,v)处的灰度值，是指图像在(uip+i,vip+j)处的灰度值，是指图像在(u'+i,v'+j)处的灰度值，如果Ierr大于阈值ε，则Pip无效，否则判定Pip有效，将点加入到集合Setw，构建三维点集合Setw＝{Pw1...Pwi...PwN}，wN为三维点的个数；步骤4：将Setw进行降采样，并将点云转到移动底盘坐标系下。步骤4中，使用PCL点云库中的VoxelGridFilter对Setw进行降采样处理，对于任意点(xi,yi,zi)∈Setw，执行下列公式将其转换到移动底盘坐标系下：其中，Rb为旋转矩阵，tb为平移矩阵，公式中左边的(xi,yi,zi)为转换到移动底盘坐标系下的坐标，得到转换后的Setw。公式中右边的(xi,yi,zi)为转换前Setw中点的坐标；步骤5：在转换后的Setw中提取地面，得到可行区域Settraversable。对于任一点Pwi＝(xi,yi,zi)∈Setw，在Setw中查找距离Pwi最近的三个点，{Pw1＝(x1,y1,z1),Pw2＝(x2,y2,z2),Pw3＝(x3,y3,z3)}，求得点Pwi法向量同理，通过在Setw中查找距离Pw1最近的三个点，得到Pw1法向量n1，通过在Setw中查找距离Pw2最近的三个点，得到Pw2法向量n2，通过在Setw中查找距离Pw3最近的三个点，得到Pw3法向量n3。计算法向量误差n_error：n_error＝3-|ni·n1|-|ni·n2|-|ni·n3|如果Pwi中zi小于设定阈值z_thres，并且n_error小于阈值n_thres，则判定点Pwi为可行区域，加入Settraversable。步骤6：在Settraversable中提取障碍物区域，得到障碍物区域Setobs。设置障碍物高度阈值h_thres。对于任意一点Pti∈Settraversable，检测转换后的Setw中是否存在点Pwj，满足以下关系：如果存在Pwj满足上述关系，则判定Pti为障碍物区域，加入到Setobs中，并将该点在Settraversable中删除。综上所述，由于采用了上述技术方案，本专利技术的有益效果是：本专利技术在视觉SLAM系统构建完成环境地图的基础上，进一步结合双目匹配算法，得到大范围场景点云。该点云包含场景的连续位姿信息，同时由于多帧融合，有效提高点云精度，减少噪声，并进一步在融合后的点云中检测障碍物与可行区域，实现了基于双目的障碍物与可行区域检测。附图说明图1为本专利技术基于双目的障碍物与可行区域检测方法的流程示意图。具体实施方式本说明书中公开的所有特征，或公开的所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以以任意方式组合。实施例一：步骤1：标定系统。标定双目系统，分别得到两个相机的内参和外参。标定双目系统与移动底盘之间的旋转矩阵Rb和平移矩阵tb。步骤2：通过视觉SLAM算法构建环境地图。所述的环境地图为关键帧集合Nk表示关键帧数目，K1、Ki、表示不同的关键帧，关键帧其中ti表示关键帧Ki在世界坐标系中的位置，为3×1矩阵，Ri表示关键帧Ki的朝向，为3×3矩阵，为双目相机系统在Ki位置时得到的图像。步骤3：进行点云拼接。构建三维点集合Setw＝{Pw1...Pwi...PwN}，Pwi＝{xi,yi,zi}，对于关键帧集合SetK中的每个元素Ki，执行下列操作。步骤3.1：使用StereoBM算法(opencv开源库中的代码)算法计算双目视差图。并进一步将双目视差图转化为基于左相机的三维点云。形成点云集合SetP＝{P1...Pip...Pmp}，其中mp为Ki形成的点云总数目。(uip,vip)为三维点对应的图像坐标，为三维点相对于左相机的坐标，为该点云在世界坐标系中的坐标。步骤3.2：对于Pip∈SetP，将Pip投影到相邻关键帧Ki-1或者Ki+1。以投影到Ki-1为例。R'＝Ri-1Tt'＝-Ri-1T×ti-1其中，ρ表示相机投影函数，由相机模型决定，(u',v')即为Pip在Ki-1中的投影坐标，其中RT表示对矩阵R进行转置，Ri-1T表示对矩阵Ri-1进行转置。(x',y',z')为中间变量坐标。步骤3.3：利用图像灰度值误差判定Pip有效性。I(u,本文档来自技高网...

【技术保护点】
1.一种基于双目的障碍物与可行区域检测方法，其特征在于，包括以下步骤：步骤1：标定双目系统；步骤2：通过视觉SLAM算法构建环境地图；步骤3：进行点云拼接；步骤4：将Setw进行降采样，并将点云转到移动底盘坐标系下；步骤5：在转换后的Setw中提取地面，得到可行区域Settraversable，对于任一点Pwi＝(xi,yi,zi)∈Setw，在Setw中查找距离Pwi最近的三个点，{Pw

【技术特征摘要】
1.一种基于双目的障碍物与可行区域检测方法，其特征在于，包括以下步骤：步骤1：标定双目系统；步骤2：通过视觉SLAM算法构建环境地图；步骤3：进行点云拼接；步骤4：将Setw进行降采样，并将点云转到移动底盘坐标系下；步骤5：在转换后的Setw中提取地面，得到可行区域Settraversable，对于任一点Pwi＝(xi,yi,zi)∈Setw，在Setw中查找距离Pwi最近的三个点，{Pw1＝(x1,y1,z1),Pw2＝(x2,y2,z2),Pw3＝(x3,y3,z3)}，求得点Pwi法向量同理，通过在Setw中查找距离Pw1最近的三个点，得到Pw1法向量n1，通过在Setw中查找距离Pw2最近的三个点，得到Pw2法向量n2，通过在Setw中查找距离Pw3最近的三个点，得到Pw3法向量n3；计算法向量误差n_error：n_error＝3-|ni·n1|-|ni·n2|-|ni·n3|如果Pwi中zi小于设定阈值z_thres，并且n_error小于阈值n_thres，则判定点Pwi为可行区域，加入Settraversable；步骤6：在Settraversable中提取障碍物区域，得到障碍物区域Setobs，设置障碍物高度阈值h_thres，对于任意一点Pti∈Settraversable，检测转换后的Setw中是否存在点Pwj，满足以下关系：如果存在Pwj满足上述关系，则判定Pti为障碍物区域，加入到Setobs中，并将该点在Settraversable中删除。2.根据权利要求1所述的基于双目的障碍物与可行区域检测方法，其特征在于，步骤1中，标定双目系统，分别得到两个相机的内参和外参，并且标定双目系统与移动底盘之间的旋转矩阵Rb和平移矩阵tb。3.根据权利要求1所述的基于双目的障碍物与可行区域检测方法，其特征在于，步骤2中，所述的环境地图为关键帧集合Nk...

【专利技术属性】
技术研发人员：张清泉，王军南，余饶东，唐杭生，
申请(专利权)人：浙江大承机器人科技有限公司，
类型：发明
国别省市：浙江,33