本发明属于工业机器人领域，涉及一种路径跟踪方法，具体涉及一种基于纯追踪模型的工业机器人路径跟踪方法。
背景技术：
路径跟踪(追踪)的目的是让移动机器人能够自主地生成一系列连续平滑的速度指令，使其在允许的跟踪误差范围内一直沿着预先给定的路径行走，安全地到达目标点位置。
目前常用的跟踪算法主要分为两大类：基于控制理论和基于几何学控制。经典的基于控制理论的方法有：pid控制、模糊控制、滑模控制、李雅普诺夫控制、模型预测控制等等。这类方法的一般步骤是：分析机器人的运动学模型，设计一个合理的控制器，求解控制率。它们往往有很强的理论基础，稳定性证明，但是这些方法通常都面临调参的压力，一些关键性的参数直接影响着系统的稳定性和鲁棒性，通常需要更多的计算开销才能获得理想的效果。相对于控制理论方法，基于几何学的控制，因其思路简单，易于计算机实现，物理意义显著等特点，在移动机器人领域备受欢迎。它主要探索的是移动机器人位置和预设路径之间的几何关系，而没有复杂的控制理论知识。经典的方法主要有：纯追踪、向量追踪、cf追踪等等。其中纯追踪是以一段圆弧来拟合机器人当前位置和前向距离的位置，通过计算路径的曲率，连续产生一组线速度和角速度来驱动机器人追寻路径上的点。
上述算法都能很好地跟踪直线和曲线，且能保证跟踪误差最终收敛于一个可容忍的小常数。但是这些算法都没有考虑机器人的起点和终点的位姿，这会直接或间接影响某些重要功能的实现(机械手臂抓取、云台拍照、插取货架等)，比如电动叉车等机器人要到达某个位置后进行对准插取货物的功能无法保障；而且对于分段直线的拐弯点处也没有考虑安全性，而是直接越弯经过，这个过程没法进行安全保障。而工业机器人是面向工业领域的，它跟踪算法所处的环境一般是工厂车间、仓库等且工作时间通常很长，具有功能相对单一、稳定性和安全性要求极高。
技术实现要素：
本发明目的是为了克服现有技术的不足而提供一种基于纯追踪模型的工业机器人路径跟踪方法。
为达到上述目的，本发明采用的技术方案是：一种基于纯追踪模型的工业机器人路径跟踪方法，它包括以下步骤：
(a)获取预设的全局路径，读取工业机器人在当前路径的形走模式；所述形走模式为走直线模式或拐圆弧模式；
(b)读取所述工业机器人当前在局部代价地图上的位置信息，计算其在全局路径上的位置并获取局部目标点集；
(c)处理所述工业机器人的起点位姿，计算所述工业机器人当前位置的偏航角和局部目标点集之间的角度偏差并对其取平均值，再利用纯追踪模型跟踪预设的全局路径；
(d)不停地判断所述工业机器人有没有到达目标点位姿，计算当前机器人偏航角和目标点偏航角之差，计算角速度使所述工业机器人朝设定的偏航角旋转即可。
优化地，步骤(a)中，当所述形走模式为走直线模式时，遍历所述全局路径，计算其相邻两点(xi,yi)(xi+1,yi+1)之间的斜率
进一步地，步骤(a)中，当所述形走模式为拐圆弧模式时，遍历所述全局路径，计算其相邻两点(xi,yi)(xi+1,yi+1)之间的斜率
优化地，步骤(b)中，所述目标点集的阈值为ε＝max(l,w)/2，式中l、w分别表示所述局部代价地图的长和宽。
优化地，步骤(c)中，处理所述工业机器人的起点位姿时，将线速度设置为0；并且计算角速度使所述工业机器人旋转至局部目标点集位置。
由于上述技术方案运用，本发明与现有技术相比具有下列优点：本发明基于纯追踪模型的工业机器人路径跟踪方法，通过利用纯追踪模型跟踪预设的全局路径并不停地使工业机器人朝设定的偏航角旋转，使得线速度和角速度的控制分开，这样虽然没有同时控制的效率高，但是保证了机器人的安全性和稳定性；而且拐弯全程的速度均由工业机器人计算得到而非设定的常数，这样保证了工业机器人在拐弯时不越弯、安全可靠，而且平稳、轨迹平滑、没有速度的跳变；而且该方法控制思路简单，计算量小，易于计算机编程实现，保证了一定程度上的安全性和稳定性，适合在工业上使用。
附图说明
附图1为本发明基于纯追踪模型的工业机器人路径跟踪方法的流程图；
附图2为本发明基于纯追踪模型的工业机器人路径跟踪方法中采用的经典两轮差速机器人运动模型；
附图3为本发明纯追踪模型的几何解析图。
具体实施方式
本发明基于纯追踪模型的工业机器人路径跟踪方法，如图1所示，它包括以下步骤：
(a)获取预设的全局路径，读取工业机器人在当前路径的形走模式；所述形走模式为走直线模式或拐圆弧模式。
当所述形走模式为走直线模式时，遍历所述全局路径，计算其相邻两点(xi,yi)(xi+1,yi+1)之间的斜率
当所述形走模式为拐圆弧模式时，遍历所述全局路径，计算其相邻两点(xi,yi)(xi+1,yi+1)之间的斜率
利用贝塞尔曲线对所述转弯点处进行插值具体为：(11)设置转弯距离λ，根据转弯点gi的index和λ，计算控制点ci,1，ci,2的index，
(12)贝塞尔曲线算法步骤：输入是全局路径、转弯点和两个控制点；算法总共迭代次数
(13)采用分段读取的方法获取所有的插值点和原路径点(不包含被插值的点)，具体如下：总共段数为l＝2×i+1，迭代次数为l次。当l为偶数，则装入直线段；当l为奇数，则装入弧线段，即插值点(值得注意的是，装入直线段和曲线段时，起点和终点是相同的，只要其中一对即可)。
(b)读取所述工业机器人当前在局部代价地图上的位置信息，计算其在全局路径上的位置并获取局部目标点集；所述目标点集的阈值为ε＝max(l,w)/2，式中l、w分别表示所述局部代价地图的长和宽，说明局部目标点集永远在局部代价地图上。
(c)处理所述工业机器人的起点位姿，计算所述工业机器人当前位置的偏航角和局部目标点集之间的角度偏差并对其取平均值，将线速度设置为0，确保当前机器人转弯时不平移；计算角速度使机器人旋转至局部目标点集位置，这就确保了机器人在起步时的安全性；再利用纯追踪模型跟踪预设的全局路径，具体如下：
根据当前速度计算前向距离
当走直线时，不停地判断机器人有没有接近当前的转弯点gi位置，如果到达当前的转弯点的前η米，那么减速；等到达gi后，停止，开始旋转，使机器人朝向新的局部目标点集，不停地判断机器人有没有到达gi点位姿(位置+角度)，如果满足条件，那么开始加速前进(其中这段的旋转也有加速和减速，这样机器人的整个运动都会很平滑)。不停地判断机器人有没有到达最终的目标点gfinal，同样如果到达目标点gfinal前η米，减速，最后安全到达gfinal点位置。在走直线模式下，在转弯点处的提前减速，到达位置后旋转，然后起步加速，也将线速度和角速度分开控制，且旋转过程加入加速和减速，给予工业机器人一定的缓冲，保证了工业机器人在转弯时候的安全性。
当拐圆弧时，不停地判断机器人有没有到达最终的目标点gfinal位置，如果到达目标点gfinal前η米，那么减速，直到安全到达gfinal点位置后停止。在拐弧线模式下，机器人在拐弯处会根据当前路径的弯曲程度，计算出一组线速度和角速度驱动机器人过弯，拐弯全程的速度均由机器人计算而得，并非设定的常数，这样一方面保证了机器人在拐弯时不越弯，安全可靠，另一方面使得机器人在拐弯过程中很平稳，轨迹平滑，没有速度的跳变。
(d)不停地判断所述工业机器人有没有到达目标点位姿，计算当前机器人偏航角和目标点偏航角之差，计算角速度使所述工业机器人朝设定的偏航角旋转即可。
下面将结合附图对本发明优选实施方案进行详细说明：
实施例1
本实施例采用图2所示的工业上室内常用的两轮差速机器人底盘的运动模型(图2中中左右两个均为驱动轮，万向轮没有画出)，两个轮子都只能前后运动，不能转向，可见该运动模型为非完整性约束模型，不能全向运动。图2中点划线是左右轮经过的路径，下一时刻，机器人在世界坐标系下以转弯半径r，转了θ，其中机器人的轮间距为l，轮直径为d。由简单的几何原理可知，左轮走过的路径长δsl＝(r-l/2)×θ，右轮走过的路径长δsr＝(r+l/2)×θ，因此，机器人走过的路径长δs＝(δsl+δsr)/2，转过的角度δθ＝(δsr-δsl)/l，转弯半径
因此，在当前机器人的坐标系下，局部目标点g的坐标为(xg,yg)，机器人当前位置到局部目标点的距离为l，红虚线为拟合曲线，半径r、曲率1/γ、圆心角为α，由图3可知，yg-r＝r×cos(π-α)，xg＝r×sin(π-α)。对上述两个公式整理得，
以本公司的atx50移动机器人，具体步骤如下：
(a)获取预设的全局路径，分别针对走直线和拐圆弧两种模式对全局路径处理：
走直线：遍历全局路径，计算相邻两点之间的斜率，计算斜率对应的角度，遍历角度差值，找出大于0.1的点即为转弯点；比如有一条全局路径总共有100个点，那么斜率有99个，对应的角度差值也是99个；
拐圆弧：同上述方法找出转弯点，对转弯点进行贝塞尔插值，获取曲线点。在贝塞尔插值中，可以选择任意的距离比例，距离比例选的越小，插值点越多，路径越密。因为每个路径点之间的距离都是0.05m，故选择的距离比例为0.05除于控制点到转弯点的距离。那么，插值点的个数其实是可以手动算出来的。比如刚才那条路径有一个转弯点，设置的转弯距离为1.0m，那么距离比例就是0.05m：1m＝0.05，插值点的个数就是1/0.05+1＝21，同时贝塞尔迭代的次数也是20次。也就是说，原路径100个点，插值后就是80个点。插值后曲线点上的相邻的距离会大于0.05m，但实验证明，并不会影响机器人安全和平滑地过弯。
(b)将处理过后的全局路径交给局部路径规划器，读取当前机器人在局部代价地图上的位置，估计机器人在路径上的位置；如果机器人离起点位置很远，那么默认将起点设置为局部目标点集，这样的好处是机器人给初始位置时，不需要一定在起点位置附近，可以是任意安全的位置。
(c)计算机器人当前偏航角与局部目标点集之间的偏差，如果大于1°，旋转机器人的朝向。角度小于1°后，准备进入后续步骤；计算前向距离，使得前向距离在lookaheadmin和lookaheadmax之间，确保任意时刻都能在局部目标点集中能够找到一个需要跟踪的局部目标点。经实验，当机器人最大速度为0.3m/s时，局部目标点在机器人前方0.8m～1.2m之间。计算机器人当前位置到局部目标点拟合曲线的曲率，计算该曲率下机器人的线速度和角速度。结合走直线还是拐圆弧模式，判断的条件有所不同。走直线模式下，需要判断是否接近和到达当前转弯点，接近时减速，到达转弯点后旋转，然后再加速前进。拐圆弧模式下，直接判断是否接近或到达目标点。同样，接近就减速，到达后停止，准备进入后续步骤。
(d)计算机器人当前偏航角和目标点偏航角之间的角度差，这里设置朝向路径方向，那么机器人到达目标点位置后，旋转180°，最后判断是否到达目标点位姿。其中，旋转时的加速度和减速度都可设，同时要设置最大线速度和角速度，可以设vmax＝0.3m/s，vmin＝0.1m/s。机器人在正常行驶时会加速至最大速度。
上述实施例只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人士能够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围，凡根据本发明精神实质所作的等效变化或修饰，都应涵盖在本发明的保护范围之内。