[发明专利]一种地铁车厢消毒机器人自动导航方法

权利要求书

1.一种地铁车厢消毒机器人自动导航方法，其特征在于，包括以下步骤：

步骤1，一段时间内连续扫描的激光雷达扫描帧生成一个子图，子图采用概率栅格的地图表达模型，当新的激光雷达扫描帧插入到概率栅格时，栅格状态更新；

步骤2，在把激光雷达扫描帧插入到子图之前，对激光雷达扫描帧位姿和当前的子图通过Ceres Solver求解器进行优化；

步骤3，通过稀疏位姿调整方法优化所有激光雷达扫描帧和子图的位姿：激光雷达扫描帧在插入子图时的位姿会被缓存到内存中进行回环检测；当子图不再变化时，所有的激光雷达扫描帧和子图都被进行回环检测，从而消除子图的累积误差，得到环境地图；

步骤4，在消毒机器人平台顶端安装双目摄像头，并每隔k秒拍摄一次环境信息，左摄像头拍摄到的n时刻的图像为左图像I_nl，右摄像头拍摄到的n时刻的图像为右图像I_nr；

步骤5，分别将左图像I_nl和右图像I_nr进行特征点检测及描述，并将左图像I_nl和右图像I_nr进行特征点匹配得到图像f_nlr；

步骤6，再将前后帧图像f_(n-1)lr与f_nlr进行特征点匹配；

步骤7，利用三角测量法重建前后帧图像匹配特征点的三维坐标，进行运动估计，得到机器人全局位姿。

2.根据权利要求1所述的一种地铁车厢消毒机器人自动导航方法，其特征在于，步骤1中，将激光雷达扫描帧通过自适应阈值的分割方法进行区域分割，将3D点云图处理成2D平面图。

3.根据权利要求1所述的一种地铁车厢消毒机器人自动导航方法，其特征在于，步骤3中，使用分支定界扫描匹配算法加速回环检测和相对位姿的求解过程，确定搜索窗口，采用查找的方法构建回环。

4.一种地铁车厢消毒机器人自动导航方法，其特征在于，包括以下步骤：

步骤1，一段时间内连续扫描的激光雷达扫描帧生成一个子图，子图采用概率栅格的地图表达模型，当新的激光雷达扫描帧插入到概率栅格时，栅格状态更新；

步骤2，在把激光雷达扫描帧插入到子图之前，对激光雷达扫描帧位姿和当前的子图通过Ceres Solver求解器进行优化；

步骤3，通过稀疏位姿调整方法优化所有激光雷达扫描帧和子图的位姿：激光雷达扫描帧在插入子图时的位姿会被缓存到内存中进行回环检测；当子图不再变化时，所有的激光雷达扫描帧和子图都被进行回环检测，从而消除子图的累积误差，得到环境地图；

步骤4，在基于摄像头的观测模型中，用极坐标来表示t时刻摄像头获取的环境特征i的观测量Z(t)：

其中，和分别表示环境特征i与机器人的距离关系和角度关系；

步骤5，基于摄像头的观测模型表示为：

Z(k)＝h(X(k))+V(k)

X(k)为机器人在k时刻时的位姿；Z(k)、h(·)和V(k)分别为摄像头在k时刻的观测信息、观测函数和观测噪声；

步骤6，机器人在k时刻的位姿为(x_t(k),y_t(k),θ_t(k))^T，此时摄像头观测到N个特征，每个特征的观测值为(x_i,y_i)，得如下表达式：

其中，x_i，y_i表示在k时刻传感器探测到的第i个路标特征的坐标；和分别表示第i个路标在k时刻与机器人的位置关系和角度关系；x_t(k)，y_t(k)，θ_t(k)分别表示移动机器人在k时刻的位姿；

步骤7，记录摄像头与环境特征的相对位置。

5.根据权利要求4所述的一种地铁车厢消毒机器人自动导航方法，其特征在于，步骤1中，将激光雷达扫描帧通过自适应阈值的分割方法进行区域分割，将3D点云图处理成2D平面图。

6.根据权利要求4所述的一种地铁车厢消毒机器人自动导航方法，其特征在于，步骤3中，使用分支定界扫描匹配算法加速回环检测和相对位姿的求解过程，确定搜索窗口，采用查找的方法构建回环。