一种工业移动机器人的路径跟踪方法与流程

本发明属于机器人控制算法，涉及一种路径跟踪方法，具体涉及一种工业移动机器人(轮式的移动机器人)的路径跟踪方法。

背景技术：

1、随着科学技术的不断进步，目标跟踪作为机器人智能领域的一个重要分支，在军事和民用方面都有着十分广泛的应用；军事方面包括无人飞行器、精确制导、空中预警和战场监视等；民用方面包括移动机器人、智能视频监控、智能交通系统、人机交互、虚拟现实以及在安防领域中的枪球联动功能等。

2、总的来说，整个目标跟踪过程可分为目标特征提取和目标跟踪两部分，其中的核心技术是目标跟踪技术。目标跟踪的任务是获得目标在连续图像序列中位置随时间变化的轨迹，目的是让移动机器人能够自主地生成一系列连续平滑的速度指令，使其在允许的跟踪误差范围内一直沿着预先给定的路径行走，安全地到达目标点位置。

3、现有的跟踪算法有很多，常用的跟踪算法主要分为三大类：经典目标跟踪算法、相关滤波类算法和基于深度学习的目标跟踪算法。常用的经典目标跟踪算法有纯跟踪方法、kalman方法、mean-shift方法、和管道跟踪方法等，其中纯追踪是以一段圆弧来拟合机器人当前位置和前向距离的位置，通过计算路径的曲率，连续产生一组线速度和角速度来驱动机器人追寻路径上的点。相关滤波类算法简称cf(也常称为discriminative correlationfilter，简称dcf)，常用的滤波算法有粒子滤波算法和卡尔玛滤波算法。基于深度学习的目标跟踪算法是最近几年新兴的算法，按照轨迹生成顺序可以分为离线式的目标跟踪算法和在线式的目标跟踪算法；但是基于深度学习的目标跟踪算法，复杂度很高、训练过程比较费时，并且对硬件要求高，在实际落地中应用较少。

4、工业机器人是广泛用于工业领域的多关节机械手或多自由度的机器装置，具有一定的自动性，可依靠自身的动力能源和控制能力实现各种工业加工制造功能。工业机器人被广泛应用于电子、物流、化工等各个工业领域之中。轮式的移动机器人所处的环境一般是工厂车间、仓库等，工作时间通常很长，从某种程度上来说，这类机器人的安全性和稳定性大于功能性。经过大量的实践应用验证，上述算法在跟踪工业机器人(尤其是轮式的移动机器人)的直线路径和曲线路径上能够起到很好的效果，并且能够确保跟踪误差收敛于一个可容忍的小常数。但是，上述算法存在一个共同的问题：没有考虑机器人的起点和终点的位姿，这会直接或间接影响到工业机器人某些重要细节功能的实现(机械手臂抓取、云台拍照、插取货架等)。比如电动叉车等机器人要到达某个位置后进行对准插取货物的功能无法保障；另一方面，对于分段直线的拐弯点处没有做出具体的设计，即不考虑工业机器人的安全性，而是直接越弯经过，这个过程没法进行安全保障。

技术实现思路

1、基于上述缺陷，本发明提供一种工业移动机器人的路径跟踪方法，可以在保证稳定前进线速度和稳定角速度的基础上，对移动机器人到达目标点后的位姿进行控制。

2、为了达成上述目的，本发明提供一种工业移动机器人的路径跟踪方法，包括以下步骤：

3、(a)使用工业移动机器人所携带的传感器获取其周围的环境信息，进而获取到预设的全局路径，判断当前的模式是进行直线路径还是圆弧路径以对全局路径进行处理；

4、(b)读取所述工业移动机器人在全局地图上的位置信息，预估其在全局路径上的位置，获取局部目标点集；

5、所述目标点集的阈值计算如公式(ι)所示：

6、

7、式中，long和width分别表示局部代价地图的长和宽，用以说明局部目标点集永远在局部代价地图上；

8、(c)计算当前位置偏航角和局部目标点集之间的角度偏差，对其取平均值；其次将线速度的值设置为0，确保当前机器人转弯时不平移；再计算角速度使机器人旋转至局部目标点集位置；

9、(d)利用纯追踪模型跟踪预设路径；

10、(e)循环判断工业机器人到达局部目标点gultimate的位姿，计算当前时刻工业机器人偏航角和目标点偏航角之差，然后通过修改角速度的方式使工业机器人朝向旋转至任意设定的偏航角。

11、优化地，步骤(a)包括以下步骤：

12、(a1)遍历全局路径，当工业移动机器人处于直线路径时，找出当前路径的转弯点：

13、计算相邻两个节点(am,bm)和(am+1,bm+1)之间的斜率，计算方式如公式(1)所示；

14、

15、式中，m为点的位序；

16、然后计算角度差βm＝atan(km)，计算结束后开始遍历角度差；当βm＞α则认为当前点为转弯点，α是允许角度偏差的容忍度；

17、(a2)当工业移动机器人处于圆弧路径时，利用贝塞尔曲线对转弯点处进行插值处理：

18、设置转弯距离λ，根据转弯点gm的ind和λ数据，计算控制点cm,1，cm,2的ind数据，如公式(2)所示；

19、

20、式中，res为地图的分辨率，m为整个全局路径的转弯点个数，每个转弯点均有两个控制点。

21、进一步地，所述贝塞尔曲线算法步骤包括：

22、以全局路径、转弯点以及两个控制点为输入参数，所述贝塞尔曲线算法的迭代次数为每次迭代都是以相等的距离比例，即在c1,mgm线段上选取一个点t1,m，在c2,mgm线段上选取另一个点t2,m，然后再以相等的距离比例在t1,mt2,m线段上找到插值的点on,m；其中，n为当前转弯点的插值点个数，m为全局路径中的转弯点个数，选用的距离比例如公式(3)所示：

23、t1,mo1,m:t1,mt2,m＝c1,mtm:c1,mgm＝gmt2,m:gmc2,m  (3)。

24、优化地，步骤(d)包括以下步骤：

25、(d1)根据公式(5)当前线速度计算前向距离forward_away；

26、forward_away＝v×μ  (5)；

27、式中，v为当前机器人的线速度，μ为比例增益；

28、(d2)设置最大前向距离以及最小前向距离，计算公式如公式(6)所示；

29、forward_awaymax＝v×t+ψ

30、forward_awaymin＝v×t-ψ(6)；

31、式中，t为模拟时间，ψ为增益系数；此时根据前向距离forward_away和当前工业机器人在地图中所在的位置，在局部目标点集中找到当前时刻需要跟踪的局部目标点；

32、(d3)计算工业机器人当前位置到局部目标点之间拟合弧线的曲率σ，如公式(7)所示；

33、

34、式中，coordinatey为局部目标点在当前机器人位置坐标系下的纵坐标；

35、通过公式(7)计算出转弯半径r，如公式(8)所示。

36、

37、(d4)计算当前时刻工业机器人的线速度vt，如公式(9)所示；

38、

39、式中，以及为参数；

40、(d5)通过公式(10)计算当前时刻机器人的角速度的大小ωt；

41、ωt＝vt×σt   (10)。

42、进一步地，步骤(d)还包括以下步骤：

43、(d6)在计算出t时刻的角速度ωt后，在此刻角速度ωt的附近范围内的值ωt±extenti作为角速度ωt的备选值，通过ωt±extenti范围内的角速度计算出ωt±extenti条不同的备选路径，根据差值在这些备选路径中选择一条与预设路径最为符合的路径，如公式(11)所示：

44、dpi＝pathi-path(11)；

45、式中，pathi为通过ωt±extenti计算出来的不同范围内的路径，path为预设路径，通过计算其差值找到一条最优路径。

46、优化地，当工业移动机器人处于直线路径时，还循环判断工业机器人在运动的过程中是否接近当前最近的转弯点gm位置；当到达当前的转弯点的前l米位置，则命令工业机器人减速前行；在到达gm位置后停止前进，开始调整工业机器人的旋转方向，使机器人朝向新的局部目标点集，不停地判断工业机器人是否到达gm点位姿，包括位置以及角度；满足条件，则开始加速前进；在旋转的过程中也有加速和减速，使工业机器人的整体运动更加平滑；在工业机器人运动的过程中不停地判断此刻是否到达最终的目标点gultimate，当到达目标点gultimate前l米，则工业机器人开始减速前行，最后安全到达gultimate点位置。

47、进一步地，当工业移动机器人处于圆弧路径时，圆弧路径与直线路径的主要区别在于圆弧路径一直具有弧度，无需判断转弯点gm的位置。主要步骤是循环判断工业机器人是否到达最终的目标点gultimate位置，如果到达目标点gultimate前l米，则工业机器人开始减速前行，直到安全到达gultimate点位置后停止。

48、本发明工业移动机器人的路径跟踪方法，在保证稳定前进线速度和稳定角速度的基础上，对移动机器人到达目标点后的位姿进行控制；解决传统的纯跟踪算法在行走过程中可能会脱离预先设定的路径问题；针对分段直线路径的转弯点处安全性设计，避免工业机器人直接越弯经过的状况出现。