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

(19)国家知识产权局(12)发明专利申请(10)申请公布号CN115963815A(43)申请公布日2023.04.14(21)申请号202210594369.4(22)申请日2022.05.27(71)申请人江苏博人智能机器人有限公司地址213164江苏省常州市武进国家高新技术产业开发区西湖路8号津通国际工业园18号楼320(72)发明人谷容辉菅坤杰周作伟(74)专利代理机构北京华际知识产权代理有限公司11676专利代理师俞璇(51)Int.Cl.G05D1/02(2020.01)权利要求书3页说明书7页附图1页(54)发明名称一种基于SLAM的环卫机器人路径跟踪控制系统及方法(57)摘要本发明公开了一种基于SLAM的环卫机器人路径跟踪控制系统及方法，包括环境轮廓建立模块、场景地图完善模块、纠偏控制模块和避障处理模块；环境轮廓建立模块用于根据间隔时间和角度数据建立环境轮廓；场景地图完善模块用于根据起始位置到目标位置中所有位置节点对应的环境轮廓构建行驶的场景地图；纠偏控制模块用于在环卫机器人根据任务指令进行路径规划时，结合前进过程中的坐标位置以及对应的速度和角度对机身进行纠偏控制；避障处理模块用于处理环卫机器人识别到不属于垃圾图像的障碍物图像时进行避障处理；本发明在避障处理过程中结合角速度和线速度分析相同环境轮廓下的可选择角度路径，从而实现满足纠偏情况下的避障路径。CN115963815ACN115963815A权利要求书1/3页1.一种基于SLAM的环卫机器人路径跟踪控制方法，其特征在于，包括以下过程：将环卫机器人放置于起始位置，所述环卫机器人按照间隔时间扫描起始位置的周围环境，并获取间隔时间和角度数据，所述环卫机器人根据间隔时间和角度数据对起始位置进行环境轮廓的建立；设定环卫机器人前进的目标位置，将起始位置和目标位置之间的路程设置等距的位置节点，获取环卫机器人在前进到目标位置过程中的所有位置节点的环境轮廓，并将所有环境轮廓构建成行驶的场景地图；基于建立完成的场景地图，环卫机器人根据扫描获取的环境轮廓与场景地图进行匹配得到当前实时的坐标位置，环卫机器人根据任务指令进行路径规划，并在当前的坐标位置控制速度和角度，且在前进过程中对机身进行纠偏控制；在环卫机器人前进路程中对路面的垃圾进行图像的捕捉与分析，当识别到垃圾后启动自动清扫装置，当识别到不属于垃圾的障碍物时，进行避障处理。2.根据权利要求1所述的一种基于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，直到满足收敛条件为止。3.根据权利要求1所述的一种基于SLAM的环卫机器人路径跟踪控制方法，其特征在于：所述在前进过程中对机身进行纠偏控制，包括以下过程：设置第一PID控制器调整环卫机器人路径的位置偏差和第二PID控制器调整环卫机器人路径的角度偏差，所述第一PID控制器和第二PID控制器并联；获取环卫机器人的实时位置为[xt，yt，βt]，目标位置为[xG，yG，βG]，计算出环卫机器人与目标位置的实时距离差：2CN115963815A权利要求书2/3页和角度差：所述第一PID控制器根据实时距离差herr对输出线速度进行分析求解，所述第二PID控制器根据角度差βerr对输出角速度进行分析求解。4.根据权利要求3所述的一种基于SLAM的环卫机器人路径跟踪控制方法，其特征在于：所述第一PID控制器根据实时距离差herr对输出线速度进行分析求解，所述第二PID控制器根据角度差βerr对输出角速度进行分析求解，包括以下过程：第一PID控制器输入距离差herr，输出为线速度vc，且将第一PID控制器输出先限幅，即令||vpidout||≤vmax，其中vpidout表示实时输出线速度，vmax表示输出线速度的最大值；再将第一PID控制器的线速度进行平滑处理，即||v(c‑1)‑vpidout||＜α，其中v(c‑1)为输出上一时间间隔