[发明专利]基于天花板视觉的同步定位与地图绘制的方法

权利要求书

1.基于天花板视觉的同步定位与地图绘制的方法，其特征在于，通过安装在机器人上垂直于天花板的摄像头,由摄像头对天花板进行拍摄，经系统处理后输出当前更新的地图；

所述系统包括：机器人运动模块（1）、图像处理模块（2）、视觉信息处理模块（3）、基于扩展卡尔曼滤波的SLAM模块（4）；

其中，

所述机器人运动模块（1）包括运动模块（1.1）和摄像模块（1.2），运动模块（1.1）控制机器人的运动，机器人的每次运动在接收速度和运动方向信号后，经过分析完成运动，并且给出运动的距离作为输入量传给视觉处理模块；摄像模块（1.2）由内嵌于机器人内部垂直向上的摄像头完成对天花板的拍摄任务，并将图片传输到图像处理模块；

所述图像处理模块（2）包括角点特征提取子模块（2.1），通过光流跟踪子模块（2.2）；角点特征提取子模块（2.1）接收机器人移动过程中拍摄的照片，用Harris角点检测方法提取其中的角点；角点特征提取模块（2.1）中输出的角点像素信息作为输入传给光流跟踪子模块（2.2），经处理后输出连续图片中匹配的角点对；

所述视觉信息处理模块（3）包括深度信息重构子模块（3.1）和三维重构子模块（3.2）；图像处理模块（2）得到的角点对做为输入传到深度信息子模块（3.1）中去，用三角测量法进行深度信息重构；将得到的深度信息作为输入传给三维重构子模块（3.2），根据光学相关知识进行三维坐标的重构；

所述基于扩展卡尔曼滤波的EKF-SLAM模块（4）包括预测子模块（4.1）、观测子模块（4.2）、更新子模块（4.3）和地图管理子模块（4.4）；预测子模块（4.1）根据预测模型以及先前系统状态推测出当前系统的状态；观测子模块（4.2）接收三维重构的数据，其中，如果符合观测数据的要求，则将其作为观测数据处理；更新子模块（4.3），根据扩展卡尔曼滤波的更新公式进行矩阵运算；地图管理子模块（4.4），根据当前的状态以及观测数据，对地图实施添加、修改和删除管理操作。

2.根据权利要求1所述的基于天花板视觉的同步定位与地图绘制的方法，其特征在于，在图像处理模块（2）中：

所述的角点特征提取子模块（2.1），根据Harris角点检测法提取角点，并存储角点像素坐标；

所述光流跟踪子模块（2.2），在前一帧获取的特征点周围基于光流金字塔，进行特征点匹配，如果匹配成功的特征点数目少于指定数目，则重新检测特征点；否则，将获取的特征点的中位数作为输出。

3.根据权利要求1所述的基于天花板视觉的同步定位与地图绘制的方法，其特征在于，在视觉处理模块（3）中：

所述深度信息重构子模块（3.1），采用三角测量法以及连续两祯图片中匹配的角点提取深度信息；用连续图片中匹配角点在运动方向的像素移动配合上标定出的摄像头焦距得到有误差的深度信息，推导公式如下：

Z=fTxl-xr]]>

其中f为焦距，T为拍摄这连续图片时机器人移动的位移，x_l和x_r分别为连续图片中匹配点的x像素坐标；

所述三维重构子模块（3.2），利用标定出的内参数矩阵，结合当前像素坐标，实现三维坐标重构。

4.根据权利要求1所述的基于天花板视觉的同步定位与地图绘制的方法，其特征在于，在基于扩展卡尔曼滤波的EKF-SLAM模块（4）中：所述预测模块（4.1）包括根据预测模型以及先前系统状态推测出当前系统的状态，预测部分分为机器人和角点位置的预测，机器人位置的预测由它的前进路线而定；

所述观测模块（4.2）接收三维重构的数据，如果符合观测数据的要求，则将其作为观测数据处理；观测到的点如果已经在地图中且满足观测要求，则将其作为观测值更新地图中点的位置；否则，重构出它的初始值，并将其加入到地图中去；

所述更新模块（4.3）根据扩展卡尔曼滤波的更新公式进行矩阵运算，更新部分包括机器人位置更新、路标位置更新；

所述地图管理模块（4.4）根据当前的状态以及观测数据，对地图实施添加、修改和删除管理操作。