[发明专利]基于深度相机的真实障碍物地图构建和导航方法及系统

权利要求书

1.一种基于深度相机的真实障碍物地图构建和导航方法，其特征在于，包括：

通过深度相机获取各个角度下的环境障碍物的深度图像；

选取所述深度图像中位于深度相机平面以下的任意三个像素点构建平面，获取平面的法向量并转化为单位向量；

计算所述深度图像中每一个像素点与所述单位向量的内积，获得与所述平面共面的像素点；

分析所述深度图像中与所述平面共面的像素点是否超过设置的阈值；若所述深度图像中与所述平面共面的像素点超过设置的阈值，则将所述平面定义为地面平面；

统计所述深度相机平面与所述地面平面之间各个像素点相对于所述深度相机的深度后进行排序，得到各个角度离障碍物的最小距离；

将所述深度图像左右范围内的所有角度的像素点进行排序，得到各个角度下各个像素列所对应的最小障碍物距离；

将深度相机数据转换为激光数据；

获取里程计数据，根据所述激光数据和所述里程计数据进行地图构建。

2.根据权利要求1所述的方法，其特征在于，所述方法还包括：使用ROS系统的导航包根据所述激光数据对机器人进行导航。

3.根据权利要求1所述的方法，其特征在于，所述选取所述深度图像中位于深度相机平面以下的任意三个像素点构建平面，获取平面的法向量并转化为单位向量的步骤实现方式为：

T₁＝Y₁Y₂∧Y₁Y₃；

其中，Y₁、Y₂、Y₃为选取的像素点；∧为叉乘运算，求向量的外积；T₁为三个像素点组成的平面的法向量；T为单位向量。

4.根据权利要求1所述的方法，其特征在于，所述将深度相机数据转换为激光数据的步骤包括：

获取所述深度图像中各个像素列距离障碍物最小的像素点的相机坐标，并计算该相机坐标对应的世界坐标；

计算所述世界坐标与相机坐标原点的夹角；

根据所述深度相机的视野范围计算所述世界坐标所对应的伪激光点的计数；

计算伪激光点到相机坐标原点的距离；

根据所述世界坐标与相机坐标原点的夹角、所述伪激光点的计数和所述伪激光点到相机坐标原点的距离建立对应的激光数据。

5.根据权利要求1所述的方法，其特征在于，所述根据所述激光数据和所述里程计数据进行地图构建的步骤所使用的算法为gmapping建图算法中改进的Rao-Blackwellized粒子滤波算法。

6.一种基于深度相机的真实障碍物地图构建和导航系统，其特征在于，包括：控制器，与所述控制器连接的深度相机；与所述控制器连接的里程计；

所述深度相机用于获取各个角度下的环境障碍物的深度图像；

所述控制器用于平面构建模块、地面平面分析模块、障碍物距离分析模块、数据转换模块和地图构建模块；

所述平面构建模块用于选取所述深度图像中位于深度相机平面以下的任意三个像素点构建平面，获取平面的法向量并转化为单位向量；

所述地面平面分析模块用于计算所述深度图像中每一个像素点与所述单位向量的内积，获得与所述平面共面的像素点；分析所述深度图像中与所述平面共面的像素点是否超过设置的阈值；若所述深度图像中与所述平面共面的像素点超过设置的阈值，则将所述平面定义为地面平面；

所述障碍物距离分析模块用于统计所述深度相机平面与所述地面平面之间各个像素点相对于所述深度相机的深度后进行排序，得到各个角度离障碍物的最小距离；将所述深度图像左右范围内的所有角度的像素点进行排序，得到各个角度下各个像素列所对应的最小障碍物距离；

所述数据转换模块用于将深度相机数据转换为激光数据；

所述地图构建模块用于获取里程计数据，根据所述激光数据和所述里程计数据进行地图构建。

7.根据权利要求6所述的系统，其特征在于，所述控制器还包括导航模块，所述导航模块用于使用ROS系统的导航包根据所述激光数据对机器人进行导航。

8.根据权利要求6所述的系统，其特征在于，所述系统还包括无线通信装置；所述无线通信装置用于建立机器人无线通信网络。