一种基于SLAM的环卫机器人路径跟踪控制系统及方法

技术领域

本发明涉及环卫机器人技术领域，具体为一种基于SLAM的环卫机器人路径跟踪控制系统及方法。

背景技术

据统计，目前我们国家环卫工人群体达到了近800万人，而且从业人员基本上年龄较大，超过50岁的居多。受城镇化、垃圾分类、美丽乡村建设等因素的影响，未来三年仍然面临着近300万人的缺口。目前，已有部分厂商在进行环卫机器人的开发与测试，正处于起步测试阶段，传统环卫设备还是占主导地们，存在成本高、效率低、管理难等痛点；且对于大多数环卫机器人的开发，存在着机器人行驶路径与系统设定路径存在偏差以及无法快速有效分析障碍物，做到避障处理。

发明内容

本发明的目的在于提供一种基于SLAM的环卫机器人路径跟踪控制系统及方法，以解决上述背景技术中提出的问题。

为了解决上述技术问题，本发明提供如下技术方案：一种基于SLAM的环卫机器人路径跟踪控制方法，包括以下过程：

将环卫机器人放置于起始位置，环卫机器人按照间隔时间扫描起始位置的周围环境，并获取间隔时间和角度数据，环卫机器人根据间隔时间和角度数据对起始位置进行环境轮廓的建立；

设定环卫机器人前进的目标位置，将起始位置和目标位置之间的路程设置等距的位置节点，获取环卫机器人在前进到目标位置过程中的所有位置节点的环境轮廓，并将所有环境轮廓构建成行驶的场景地图；

基于建立完成的场景地图，环卫机器人根据扫描获取的环境轮廓与场景地图进行匹配得到当前实时的坐标位置，环卫机器人根据任务指令进行路径规划，并在当前的坐标位置控制速度和角度，且在前进过程中对机身进行纠偏控制；

在环卫机器人前进路程中对路面的垃圾进行图像的捕捉与分析，当识别到垃圾后启动自动清扫装置，当识别到不属于垃圾的障碍物时，进行避障处理。

进一步的，环卫机器人根据间隔时间和角度数据对起始位置进行环境轮廓的建立，包括以下过程：

获取相邻间隔时间下不同角度数据对应的点云数据集合，设置点云数据集合中的目标点云U中取点集ui∈U，以及找出源点云V中对应点集vi∈V，构成最临近点(ui,vi)，使得||vi-ui||＝min；

利用误差函数公式：

/>

计算旋转矩阵R和平移矩阵t，使得误差函数最小；其中n为最临近点对的个数，ui0为目标点云U的点集ui中的一点，vi0为源点云V的点集vi中与ui0对应的最近点；

对ui使用求得的旋转矩阵R和平移矩阵t进行旋转和平移变换，得到新的对应点集ui’＝{ui’＝Rvi+t，ui∈U}；

计算ui’与对应点集vi的平均距离：

如果d小于环卫机器人设定的距离阈值或者大于环卫机器人设定的最大迭代次数，则停止迭代计算；否则返回寻找源点云V中对应点集vi∈V，使得||vi-ui||＝min，直到满足收敛条件为止。

进一步的，在前进过程中对机身进行纠偏控制，包括以下过程：

设置第一PID控制器调整环卫机器人路径的位置偏差和第二PID控制器调整环卫机器人路径的角度偏差，第一PID控制器和第二PID控制器并联；

获取环卫机器人的实时位置为[xt，yt，βt]，目标位置为[xG，yG，βG]，计算出环卫机器人与目标位置的实时距离差：

和角度差：

第一PID控制器根据实时距离差herr对输出线速度进行分析求解，第二PID控制器根据角度差βerr对输出角速度进行分析求解。

进一步的，第一PID控制器根据实时距离差herr对输出线速度进行分析求解，第二PID控制器根据角度差βerr对输出角速度进行分析求解，包括以下过程：

第一PID控制器输入距离差herr，输出为线速度vc，且将第一PID控制器输出先限幅，即令||vpidout||≤vmax，其中vpidout表示实时输出线速度，vmax表示输出线速度的最大值；

再将第一PID控制器的线速度进行平滑处理，即||v(c-1)-vpidout||＜α，其中v(c-1)为输出上一时间间隔的线速度，α为系统设定的平滑因子；最后输出vc＝vpidout；因为距离决定速度，距离远速度大，距离近速度小，这里进行先限幅再平滑是为了保持速度的连续性，如起步时距离远，线速度由0跳变为较大值，且距离无穷远速度无穷大；

第二PID控制器输入角度差βerr，输出为角速度wc，对输入的角度差进行归一化(-π，π]，即βerr大于π减2π，βerr小于等于-π加2π；这里角度差决定转速，正偏左转，负偏右转，偏差多转得快，偏差小转得慢，进行归一化是要主要角度表示方式带来的过界问题；

根据求出的vc和wc，得到转速zc，环卫机器人根据转速zc控制机身前进速度，当环卫机器人距离目标小于第一距离时，第一距离为以目标为圆心、m为目标半径的圆内herr≤m时，判定环卫机器人到达目标位置。

进一步的，将所有的环境轮廓构建成行驶的场景地图，包括以下过程：

获取初始位置到目标位置中所有位置节点的环境轮廓图像集合，提取环境轮廓图像集合中第j组相邻环境轮廓的第s个角度对应的图像特征kjs，然后计算图像特征kjs的相似度为Dkjs；

若相似度Dkjs大于等于预设的第一相似度阈值，则将相邻两张环境轮廓图像以第s个角度对应的图像特征为重合点进行重合直至完成所有轮廓图像的构建；

若相似度Dkjs小于预设的相似度阈值，则获取第j组相邻环境轮廓的s+a个角度对应的图像特征集合{kj(s+1),kj(s+2),...,kj(s+a)}，计算图像特征集合中的相邻两组集合的相似度集合{Fkj(S+a-1)}，将相似度集合{Fkj(S+a-1)}进行排序，获取相似度集合的最大值{Fkj(S+a-1)}max，并以最大值对应的两个环境轮廓图像为初始拼接图像，依次按照相似度集合{Fkj(S+a-1)}排序后的顺序进行拼接直至完成所有轮廓图像的构建。

进一步的，当识别到不属于垃圾的障碍物时，进行避障处理，包括以下过程：

获取环卫机器人存储的垃圾图像数据，当环卫机器人识别到的图像数据不属于垃圾图像数据时，并记此时的图像数据为第一障碍物图像，环卫机器人获取第一障碍物图像所属的环境轮廓图，并提取环境轮廓图中未被遮挡的图像特征；

根据未被遮挡的图像特征，获取环境轮廓图像集合中对应图像特征的目标环境轮廓图，且目标环境轮廓图中不包含障碍物；获取第一障碍物图像中图像特征对应的角度，即{kjs→s}，以及对应角度下的目标环境轮廓图，即{s→j0}，其中j0表示第j组目标环境轮廓图；

分析第一障碍物图像与第j组目标环境轮廓在第s个角度处的相似度，若相似度大于等于第一相似度阈值，则获取第s个角度对应的线速度vc和角速度wc，若相似度小于第一相似度阈值，则继续获取第一障碍物图像中图像特征对应的其他角度和目标环境轮廓图{kjs+a→s+a}和{s+a→j0}，直至比较分析第一障碍物图像与第j组目标环境轮廓在第s+a个角度处的相似度满足第一相似度阈值，环卫机器人根据线速度vc和角速度wc得到的转速按照第s个角度方向进行避障前进。

一种基于SLAM的环卫机器人路径跟踪控制系统，包括环境轮廓建立模块、场景地图完善模块、纠偏控制模块和避障处理模块；

环境轮廓建立模块用于扫描环卫机器人起始位置的周围环境，并根据间隔时间和角度数据建立环境轮廓；

场景地图完善模块用于根据起始位置到目标位置中所有位置节点对应的环境轮廓构建行驶的场景地图；

纠偏控制模块用于在环卫机器人根据任务指令进行路径规划时，结合前进过程中的坐标位置以及对应的速度和角度对机身进行纠偏控制；

避障处理模块用于处理环卫机器人识别到不属于垃圾图像的障碍物图像时进行避障处理。

进一步的，环境轮廓建立模块包括点云数据获取模块和点云数据标整模块；场景地图完善模块包括图像特征提取模块、相似度分析模块和图像拼接模块；纠偏控制模块包括第一PID控制器模块和第二PID控制器模块；避障处理模块包括障碍物角度分析模块和遍历比较模块；

点云数据获取模块获取相邻间隔时间和不同角度数据对应的点云数据；点云数据标整模块用于设定目标点云和源点云，并将不同的坐标的点云数据进行整合为同一坐标系；

图像特征提取模块用于提取初始位置到目标位置中所有位置节点的环境轮廓图像集合中不同角度的图像特征；相似度分析模块用于对比集合中图像特征的相似度；图像拼接模块用于对满足相似度阈值的图像特征进行重叠拼接；

第一PID控制器模块用于在纠偏控制中对环卫机器人的线速度进行控制，第二PID控制器模块用于在纠偏控制中对环卫机器人的角速度进行控制；

障碍物角度分析模块用于分析识别到障碍物时的图像数据中不同角度对应环境轮廓的相似度，遍历比较模块用于遍历不同角度下图像的相似度以获取满足条件的方向角度进行避障前进。

与现有技术相比，本发明所达到的有益效果是：本发明基于SLAM，通过分析环卫机器人在陌生环境下对场景的构建，以及利用PID控制原理对机器人在前进过程中的角速度和线速度进行纠偏控制防止路径出现偏离，同时在避障处理过程中结合角速度和线速度分析相同环境轮廓下的可选择角度路径，从而实现满足纠偏情况下的避障路径；本发明可以为公园景区、机场高铁站、工业园区、生活社区、商业街区、校园区域等应用场景进行无人驾驶环卫机器人的开发，解决部分环卫工人缺口问题，并且通过环卫机器人替代部分人工，可减少人员的直接接触，对疫情的传播扩散也能起到一定的抑制作用。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。在附图中：

图1是本发明一种基于SLAM的环卫机器人路径跟踪控制系统的结构示意图；

图2是本发明一种基于SLAM的环卫机器人路径跟踪控制方法的PID控制框图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图1-图2，本发明提供技术方案：一种基于SLAM的环卫机器人路径跟踪控制方法，包括以下过程：

将环卫机器人放置于起始位置，环卫机器人按照间隔时间扫描起始位置的周围环境，并获取间隔时间和角度数据，环卫机器人根据间隔时间和角度数据对起始位置进行环境轮廓的建立；

设定环卫机器人前进的目标位置，将起始位置和目标位置之间的路程设置等距的位置节点，获取环卫机器人在前进到目标位置过程中的所有位置节点的环境轮廓，并将所有环境轮廓构建成行驶的场景地图；

基于建立完成的场景地图，环卫机器人根据扫描获取的环境轮廓与场景地图进行匹配得到当前实时的坐标位置，环卫机器人根据任务指令进行路径规划，并在当前的坐标位置控制速度和角度，且在前进过程中对机身进行纠偏控制；

在环卫机器人前进路程中对路面的垃圾进行图像的捕捉与分析，当识别到垃圾后启动自动清扫装置，当识别到不属于垃圾的障碍物时，进行避障处理。

环卫机器人根据间隔时间和角度数据对起始位置进行环境轮廓的建立，包括以下过程：

获取相邻间隔时间下不同角度数据对应的点云数据集合，设置点云数据集合中的目标点云U中取点集ui∈U，以及找出源点云V中对应点集vi∈V，构成最临近点(ui,vi)，使得||vi-ui||＝min；

利用误差函数公式：

计算旋转矩阵R和平移矩阵t，使得误差函数最小；其中n为最临近点对的个数，ui0为目标点云U的点集ui中的一点，vi0为源点云V的点集vi中与ui0对应的最近点；

对ui使用求得的旋转矩阵R和平移矩阵t进行旋转和平移变换，得到新的对应点集ui’＝{ui’＝Rvi+t，ui∈U}；

计算ui’与对应点集vi的平均距离：

如果d小于环卫机器人设定的距离阈值或者大于环卫机器人设定的最大迭代次数，则停止迭代计算；否则返回寻找源点云V中对应点集vi∈V，使得||vi-ui||＝min，直到满足收敛条件为止。

在前进过程中对机身进行纠偏控制，包括以下过程：

设置第一PID控制器调整环卫机器人路径的位置偏差和第二PID控制器调整环卫机器人路径的角度偏差，第一PID控制器和第二PID控制器并联；

获取环卫机器人的实时位置为[xt，yt，βt]，目标位置为[xG，yG，βG]，计算出环卫机器人与目标位置的实时距离差：

和角度差：

第一PID控制器根据实时距离差herr对输出线速度进行分析求解，第二PID控制器根据角度差βerr对输出角速度进行分析求解。

第一PID控制器根据实时距离差herr对输出线速度进行分析求解，第二PID控制器根据角度差βerr对输出角速度进行分析求解，包括以下过程：

第一PID控制器输入距离差herr，输出为线速度vc，且将第一PID控制器输出先限幅，即令||vpidout||≤vmax，其中vpidout表示实时输出线速度，vmax表示输出线速度的最大值；

再将第一PID控制器的线速度进行平滑处理，即||v(c-1)-vpidout||＜α，其中v(c-1)为输出上一时间间隔的线速度，α为系统设定的平滑因子；最后输出vc＝vpidout；因为距离决定速度，距离远速度大，距离近速度小，这里进行先限幅再平滑是为了保持速度的连续性，如起步时距离远，线速度由0跳变为较大值，且距离无穷远速度无穷大；

第二PID控制器输入角度差βerr，输出为角速度wc，对输入的角度差进行归一化(-π，π]，即βerr大于π减2π，βerr小于等于-π加2π；这里角度差决定转速，正偏左转，负偏右转，偏差多转得快，偏差小转得慢，进行归一化是要主要角度表示方式带来的过界问题；

例如：当

根据求出的vc和wc，得到转速zc，环卫机器人根据转速zc控制机身前进速度，当环卫机器人距离目标小于第一距离时，第一距离为以目标为圆心、m为目标半径的圆内herr≤m时，判定环卫机器人到达目标位置。

将所有的环境轮廓构建成行驶的场景地图，包括以下过程：

获取初始位置到目标位置中所有位置节点的环境轮廓图像集合，提取环境轮廓图像集合中第j组相邻环境轮廓的第s个角度对应的图像特征kjs，然后计算图像特征kjs的相似度为Dkjs；

若相似度Dkjs大于等于预设的第一相似度阈值，则将相邻两张环境轮廓图像以第s个角度对应的图像特征为重合点进行重合直至完成所有轮廓图像的构建；

若相似度Dkjs小于预设的相似度阈值，则获取第j组相邻环境轮廓的s+a个角度对应的图像特征集合{kj(s+1),kj(s+2),...,kj(s+a)}，计算图像特征集合中的相邻两组集合的相似度集合{Fkj(S+a-1)}，将相似度集合{Fkj(S+a-1)}进行排序，获取相似度集合的最大值{Fkj(S+a-1)}max，并以最大值对应的两个环境轮廓图像为初始拼接图像，依次按照相似度集合{Fkj(S+a-1)}排序后的顺序进行拼接直至完成所有轮廓图像的构建。

当识别到不属于垃圾的障碍物时，进行避障处理，包括以下过程：

获取环卫机器人存储的垃圾图像数据，当环卫机器人识别到的图像数据不属于垃圾图像数据时，并记此时的图像数据为第一障碍物图像，环卫机器人获取第一障碍物图像所属的环境轮廓图，并提取环境轮廓图中未被遮挡的图像特征；

根据未被遮挡的图像特征，获取环境轮廓图像集合中对应图像特征的目标环境轮廓图，且目标环境轮廓图中不包含障碍物；获取第一障碍物图像中图像特征对应的角度，即{kjs→s}，以及对应角度下的目标环境轮廓图，即{s→j0}，其中j0表示第j组目标环境轮廓图；

分析第一障碍物图像与第j组目标环境轮廓在第s个角度处的相似度，若相似度大于等于第一相似度阈值，则获取第s个角度对应的线速度vc和角速度wc，若相似度小于第一相似度阈值，则继续获取第一障碍物图像中图像特征对应的其他角度和目标环境轮廓图{kjs+a→s+a}和{s+a→j0}，直至比较分析第一障碍物图像与第j组目标环境轮廓在第s+a个角度处的相似度满足第一相似度阈值，环卫机器人根据线速度vc和角速度wc得到的转速按照第s个角度方向进行避障前进。

一种基于SLAM的环卫机器人路径跟踪控制系统，包括环境轮廓建立模块、场景地图完善模块、纠偏控制模块和避障处理模块；

环境轮廓建立模块用于扫描环卫机器人起始位置的周围环境，并根据间隔时间和角度数据建立环境轮廓；

场景地图完善模块用于根据起始位置到目标位置中所有位置节点对应的环境轮廓构建行驶的场景地图；

纠偏控制模块用于在环卫机器人根据任务指令进行路径规划时，结合前进过程中的坐标位置以及对应的速度和角度对机身进行纠偏控制；

避障处理模块用于处理环卫机器人识别到不属于垃圾图像的障碍物图像时进行避障处理。

环境轮廓建立模块包括点云数据获取模块和点云数据标整模块；场景地图完善模块包括图像特征提取模块、相似度分析模块和图像拼接模块；纠偏控制模块包括第一PID控制器模块和第二PID控制器模块；避障处理模块包括障碍物角度分析模块和遍历比较模块；

点云数据获取模块获取相邻间隔时间和不同角度数据对应的点云数据；点云数据标整模块用于设定目标点云和源点云，并将不同的坐标的点云数据进行整合为同一坐标系；

图像特征提取模块用于提取初始位置到目标位置中所有位置节点的环境轮廓图像集合中不同角度的图像特征；相似度分析模块用于对比集合中图像特征的相似度；图像拼接模块用于对满足相似度阈值的图像特征进行重叠拼接；

第一PID控制器模块用于在纠偏控制中对环卫机器人的线速度进行控制，第二PID控制器模块用于在纠偏控制中对环卫机器人的角速度进行控制；

障碍物角度分析模块用于分析识别到障碍物时的图像数据中不同角度对应环境轮廓的相似度，遍历比较模块用于遍历不同角度下图像的相似度以获取满足条件的方向角度进行避障前进。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。

最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。