[发明专利]一种用于商场超市拣货机器人的综合控制方法

权利要求书

1.一种用于商场超市拣货机器人的综合控制方法，其特征在于，该方法包含以下内容：

一、采用激光雷达和单目相机结合的方式构建环境地图；利用gmapping算法增量式构建环境地图，并对当前帧的环境图像进行回环检测，在构建环境地图的过程中利用贝叶斯滤波算法计算货架的位置坐标；

S1、上位机采用gmapping算法增量式构建环境地图，具体步骤为：

步骤一：将环境信息抽象为粒子状态信息，粒子状态信息用公式(1)描述为：

p(x_1：t，m|z_1：t，u_1：t-1)＝p(m|x_1：t，z_1：t)·p(x_1：t|z_1：t，u_1：t-1) (1)

式(1)中，x_1：t为机器人从初始状态到t时刻的位姿；m为环境地图的状态数据；z_1：t表示激光雷达从初始状态到t时刻的观测数据；u_1：t-1表示从初始状态到t-1时刻控制系统发给机器人的控制数据；p(m|x_1：t，z_1：t)表示环境地图在t时刻的状态分布；p(x_1：t|z_1：t，u_1：t-1)表示从初始状态到t时刻机器人的位姿；p(x_1：t，m|z_1：t，u_1：t-1)为用从初始状态到t-1时刻的控制系统控制机器人进行的数据和激光雷达的观测数据表示机器人位姿和环境地图的状态分布；

步骤二：计算每个粒子的权重，以供步骤三的重采样使用；

由于粒子要覆盖机器人的全部运动状态空间，而其中只有一小部分粒子符合要求，采用公式(2)计算每个粒子的权重；

式(2)中，表示t时刻任意粒子的权重；表示t-1时刻任意粒子的权重；x_j表示运动状态空间峰值附近(接近真实状态)的机器人位姿；Z_t表示t-1时刻激光雷达的观测数据；表示t-1时刻环境地图的状态数据；表示t-1时刻机器人的位姿；u_t-1表示t-1时刻控制系统给机器人的控制数据；K表示运动状态空间峰值附近粒子的个数；为用t-1时刻环境地图的状态数据、机器人位姿和控制机器人的控制数据表示运动状态空间峰值附近激光雷达的观测数据；为用t-1时刻环境地图的状态数据表示运动状态空间峰值附近激光雷达的观测数据；为用t-1时刻机器人的位姿和机器人的控制数据表示运动状态空间峰值附近的机器人位姿；

步骤三：对粒子进行重采样，筛选符合实际场景环境的粒子；采用粒子表示环境地图状态分布时，需要把权重小的粒子丢弃，保留权重大的粒子并复制，使粒子收敛到真实状态附近；为了筛选出与真实状态相近的粒子，需对权重变化超过阈值的粒子进行重采样，重采样的标准满足公式(3)；阈值为N/2，N为采集到的粒子个数；

式(3)中，表示任意粒子的权重；N表示采集到的粒子个数；当Neff大于阈值时，表示粒子权重与真实值的差距越小，当所有粒子权重都一样时这些粒子恰好可以表示真实分布；当Neff小于阈值时，表示粒子权重与真实值的差距很大，需要对粒子进行重采样；

通过步骤一至步骤三获得t时刻的环境地图，重复执行步骤一至步骤三，完成增量式构建室内环境地图；在此过程中机器人实时更新自身位姿，实现了自定位的目的；

S2、采用ORB-SLAM2算法的LOOPCLOSING线程对当前帧的环境图像进行回环检测；

利用单目相机采集每一帧的环境图像，上位机采用ORB特征描述算法分别对每帧的环境图像提取特征点，当同一环境图像的特征点满足一定数量(至少为30个)为关键帧，并将所有关键帧加入词袋模型；

上位机采用ORB-SLAM2算法的LOOPCLOSING线程匹配单目相机采集的当前帧和词袋模型中的关键帧，对当前帧进行回环检测，即对当前帧与词袋模型中的关键帧进行匹配，将当前帧生成的地图与上一帧生成的地图进行匹配；若回环检测成功，上位机引导gmapping算法匹配回环检测成功的关键帧对应的机器人位置点，实现机器人路径的闭合，以消除机器人在构建环境地图中的累计误差，实现环境地图的校正，为拣货机器人更加精准的避障和导航提供基础；若回环检测失败，上位机继续对回环检测失败的帧进行回环检测；

S3、采用贝叶斯滤波算法计算货架的位置坐标；在构建环境地图的过程中，上位机采用SSD算法检测货架；检测出货架后，结合单目相机与激光雷达的相对安装位置，利用贝叶斯滤波算法计算货架的位置坐标，贝叶斯滤波算法满足公式(4)：

x_t～p(x_t|w_1：t，v_1：t) (4)

式(4)中，x_t为货架在t时刻的位置坐标；w_1：t表示从初始状态到t时刻货架相对于激光雷达的位置坐标；v_1：t表示从初始状态到t时刻货架相对于单目相机的位置坐标；p(x_t|w_1：t，v_1：t)为用货架相对于激光雷达以及单目相机的位置表示货架的位置坐标；

二、采用全局最优路径规划和局部匀速路径规划相结合的方式进行路径规划；利用融合A*算法和DWA算法进行全局最优路径规划，采用匀速直线行进的方式进行局部匀速路径规划；

三、基于超声波传感器的实时动态避障，包括非拣货避障和拣货避障，保证机器人不会因避障绕行而错捡漏捡；

四、基于单目相机的目标识别，包括图像标注和数据集的训练。