本发明涉及移动机器人，特别是移动机器人的路径跟踪方法。
背景技术：
随着人工智能技术及先进制造技术的不断发展，移动机器人的应用越来越广泛。
为了保证能够安全、高效的完成工作，在移动机器人工作之前，用户需要确认移动机器人的工作边界或设定移动机器人的工作路径。对于割草机器人等覆盖式机器人来说，可通过设定割草范围边界，使其尽可能地覆盖草坪边界，提高割草覆盖率；对于安防巡逻机器人来说，通过设定工作路径，使其尽可能按设定工作路径行走，避免监控盲区。
然后，现有的移动机器人(如割草机器人或安防巡逻机器人)在按规划的路径工作时，是通过惯性导航系统结合传感器来实现的，移动机器人的实际移动轨迹与设定工作边界或工作路径存在偏差。
因此有必要提供一种解决上述问题的方案。
技术实现要素：
本发明的目的之一在于克服背景技术中的缺陷，提供一种移动机器人的安路径跟踪方法，其具体方案如下：
一种移动机器人的路径跟踪方法，所述移动机器人包括左、右驱动轮和万向轮，包括以下步骤：s1：设定移动机器人的工作路径；s2：根据移动机器人当前方向θi、当前速度vi及沿运动方向的尺寸l设置前瞻点pi；s3：确定工作路径上距离前瞻点pi最近的相邻离散点ai、bi；s4：计算前瞻点pi与直线aibi之间的距离di；s5：计算当前方向θi与直线aibi之间的夹角δθi；s6：根据距离di及夹角δθi计算左、右驱动轮转速差δvi；s7：根据vi、δvi对移动机器人的左、右驱动轮进行速度调节。
进一步地，在本发明的优化方案中，还包括步骤s8：间隔预定时间后再返回步骤s2直到移动机器人完成工作路径跟踪。
进一步地，在本发明的一个实施例中，所述前瞻点pi位于移动机器人的正前方。
进一步地，所述前瞻点pi距离移动机器人的距离d＝k1*vi*l，其中，k1为基于速度的比例系数。
进一步地，在本发明的一个实施例中，所述步骤s4包括：s41：获取前瞻点pi、点ai及点bi的坐标；s42：计算前瞻点pi、点ai及点bi构成的三角形面积si；s43：计算点ai、点bi之间的距离li；s44：计算前瞻点pi到直线aibi的距离：di＝2*si/li。
进一步地，所述步骤s42和步骤s43可相互交换。
进一步地，在本发明的一个实施例中，所述步骤s6是通过设计轮速模糊控制器计算左、右驱动轮转速差δvi的，包括：s61：将距离di及夹角δθi进行模糊化处理；s62：通过模糊控制表查询得到期望转向轮速差；s63：对期望转向轮速差去模糊化处理得到左、右驱动轮转速差δvi。
进一步地，所述步骤s4和步骤s5可相互交换。
进一步地，在本发明的一个实施例中，所述移动机器人为割草机器人。
与现有技术相比，本发明技术方案通过设置与移动机器人当前方向、当前速度及移动机器人沿运动方向的长度相关的前瞻点，再通过该前瞻点确定工作路径上与其最近的两个相邻离散点，再通过计算该两个相邻离散点确定的直线与前瞻点之间的距离和与移动机器人当前方向的角度差来确定移动机器人的左右驱动轮转速差，然后根据该转速差对左右轮转速进行调节，使得移动机器人实际运行轨迹与设定的工作路径误差更小。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单的介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明方法的较佳实施例的流程图；
图2是图1中的步骤s4的子步骤流程图；
图3是图1中的步骤s6的子步骤流程图；
图4是本发明方法的路径跟踪示意图。
具体实施方式
下面将结合附图和具体实施例对本发明技术方案进行清楚、完整地描述，显然，这里所描述的实施例仅仅是发明的一部分实施例，而不是全部的实施例。基于本发明描述的具体实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明权利要求所限定的保护范围内。
本发明中的移动机器人包括控制系统、传感器系统、驱动系统和电源系统，控制系统用于根据预定程序或传感器系统返回的信号控制移动机器人执行特定的任务或动作，传感器系统用于检测移动机器人外部环境或移动机器人本身部件工作状态并发送相关信息给控制系统，驱动系统用于驱动移动机器人发生运动，电源系统为移动机器人的各系统提供电能，此外移动机器人还包括功能模块以执行特定任务。
如图1至图4所示，为本发明一较佳实施例。在本实施例中，移动机器人以割草机器人为例进行说明，本实施例中的割草机器人的控制系统为stm32单片机；传感器系统包括陀螺仪和uwb(ultrawideband，超宽带)；驱动系统为位于割草机器人后方的大小相同的左、右驱动轮和位于割草机前方中心位置处的万向轮，左、右驱动轮由无刷电机驱动芯片mc33035dw驱动；电源系统为可充电锂氢电池。
下面对本实施例中的割草机器人的路径跟踪过程进行详细说明。
s1：设定移动机器人的工作路径。
本发明方法基于设定的工作路径，可通过割草机器人的自建地图，然后设定其工作路径(如工作区域边界)，或者是将规划好的地图及工作路径发送至割草机器人的控制系统。其中，本实施例中的工作路径是由一系列离散点构成，每个离散点在地图中的坐标已确定。
s2：根据移动机器人当前方向θi、当前速度vi及沿运动方向的尺寸l设置前瞻点pi。
割草机器人本身尺寸l为已知量，具体地，当其为圆形时，l为其直径，当其为矩形时，l为沿当前运动方向的长度。割草机器人通过陀螺仪获取其在地图中的当前方向θi，通过uwb方式得到其在地图中的坐标(x0,yo)，通过测速传感器(如光栅测速)分别得到左、右驱动轮的旋转速度，然后选取左、右驱动轮的平均转速作为割草机器人当前的速度vi，然后在割草机器人的正前方设置前瞻点pi(即左右驱动轮连线的中心点与万向轮的连线上)，前瞻点pi距离割草机器人的前瞻距离d＝k1*vi*l，其中，前瞻距离d为左、右驱动轮连线中心点到前瞻点pi的距离，k1为基于速度的比例系数，k1是通过仿真或实验获得的经验值，其取值范围一般为20-25。
s3：确定工作路径上距离前瞻点pi最近的相邻离散点ai、bi。
首先需确定前瞻点pi在地图坐标系中的坐标(xp，yp)，如图4所示，根据移动机器人当前点o及其对应的前瞻点pi的相对关系可以得到前瞻点pi的坐标为(x0+d*cosθi,yo+d*sinθi)，然后根据就近原则得到与前瞻点pi最近的两个相邻离散点ai、bi。最简单的方法为：a、选择工作路径上的两个相邻离散点，满足其中一个离散点的横坐标大于xp，而另一个离散点的横坐标小于xp；b、或选择工作路径上的两个相邻离散点，满足其中一个离散点的纵坐标大于yp，而另一个离散点的纵坐标小于yp。
s4：计算前瞻点pi与直线aibi之间的距离di。
如图2所示，本实施例中，获取前瞻点pi与直线aibi之间的距离di的具体步骤如下：s41：获取前瞻点pi、点ai及点bi的坐标，具体地，前瞻点pi的坐标通过步骤3中可以得到，而相邻离散点ai、bi的坐标也可以直接读取；s42：计算前瞻点pi、点ai及点bi构成的三角形面积si，具体地，si＝(xa*yb+xb*yp+xp*ya-xa*yp-xp*yb-xb*ya)/2；s43：计算点ai、点bi之间的距离li，具体地，点ai、点bi之间的距离li＝((xa-xb)2+(ya-yb)2)1/2；s44：计算前瞻点pi到直线aibi的距离：di＝2*si/li，具体地，根据三角形面积公式，可以得出：di＝2*si/li。
s5：计算当前方向θi与直线aibi之间的夹角δθi。
由于点ai及点bi的坐标都是已知的，直线aibi的斜率为(ya-yb)/(xa-xb),再通过三角函数可以得到直线aibi与坐标系中x轴的角度，然后与割草机器人当前方向角度θi相减，即可得到割草机器人的当前方向θi与直线aibi之间的夹角δθi。
s6：根据距离di及夹角δθi计算左、右驱动轮转速差δvi。
如图3所示，本实施例中，通过设计轮速模糊控制器计算左、右驱动轮转速差δvi的，具体包括：s61：将距离di及夹角δθi进行模糊化处理，采用连续域离散化的均匀量化方法，将连续域中的连续值经量化因子比例变换之后，四舍五入变为离散域的整数值，即对距离di进行取整int(kdi*di)，对夹角δθi进行取整int(kδθi*δθi)，其中，kdi和kδθi的参数为比例变化系数，其值可通过仿真或实验获得，取值范围分别是，kdi：2-10，kδθi：5-20；s62：通过模糊控制表查询得到期望转向轮速差，具体地，输入不同的离散输入量di和δθi，结合模糊控制表，得到清晰的控制量即期望转向轮速差δvi；s63：对期望转向轮速差δvi去模糊化处理得到左、右驱动轮转速差δvi，转换公式为δvi＝kδv*δvi，其中，kδv为比例变化系数，其值可通过仿真或实验获得，kδv的取值范围是：0.05-0.95。
s7：根据vi、δvi对移动机器人的左、右驱动轮进行速度调节。
如图4所示，当割草机器人左转时，左驱动轮的转速vleft＝vi-δvi，右驱动轮的转速vright＝vi+δvi；当割草机器人右转弯时，左驱动轮的转速vleft＝vi+δvi，右驱动轮的转速vright＝vi-δvi。其中，δvi可为向量，其值可为正负。
s8：间隔预定时间后再返回步骤s2直到移动机器人完成工作路径跟踪。
割草机器人的左、右驱动轮按调整后的转速运行一段时间δt后，返回步骤s2。即当前方向θi+1、当前速度vi+1及沿运动方向的尺寸l设置另一前瞻点pi+1。其中，本实施例中，间隔δt与割草机器人当前速度(即左右驱动轮转速的平均值)负相关，即割草机器人当前速度越大，δt越小，当前速度越小，δt越大，其一般表达式为：δt＝k2*l/vi，其中k2为比例变化系数，可根据割草机器人沿运动方向的长度进行调整。
以上所揭露的仅为本发明技术方案的实施例而已，不能以此来限定本发明之权利范围，因此依本发明权利要求所作的等同变化，仍属本发明所涵盖的范围。