[发明专利]一种基于双目的障碍物与可行区域检测方法

摘要

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

主权项

1.一种基于双目的障碍物与可行区域检测方法，其特征在于，包括以下步骤：步骤1：标定双目系统；步骤2：通过视觉SLAM算法构建环境地图；步骤3：进行点云拼接；步骤4：将Setw进行降采样，并将点云转到移动底盘坐标系下；步骤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法向量同理，通过在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：在Set_traversable中提取障碍物区域，得到障碍物区域Set_obs，设置障碍物高度阈值h_thres，对于任意一点Pt_i∈Set_traversable，检测转换后的Set_w中是否存在点Pw_j，满足以下关系：如果存在Pwj满足上述关系，则判定Pti为障碍物区域，加入到Setobs中，并将该点在Settraversable中删除。