一种移动机器人的高效性局部路径控制方法

本申请属于电子控制技术领域，具体涉及一种移动机器人的高效性局部路径控制方法。

背景技术：

常规的移动机器人需要利用激光雷达、imu等实现移动机器人在复杂环境下的路径规划。作为移动机器人的重要组成部分，移动机器人的底盘可以看作是机器人的“足”，在移动机器人底盘的开发过程中，为了调试移动机器人底盘的运动性能，在简单环境下给定一个基于当前移动机器人的偏航角和距离的目标点，通常情况下移动机器人依靠陀螺仪和里程计信息在向目标点移动时，假如当前移动机器人处于静止状态，则需要先原地旋转固定角度，然后再前行固定距离；假如当前移动机器人处于运行状态，则需要先停车，再旋转固定角度，最后前行固定距离。

由此可见，不论哪种情况下的运动控制方法都需要频繁地执行停车动作，这导致移动机器人控制存在迟钝的缺点。并且如果在移动机器人运行过程中给定一系列目标点，那么按照常规的处理方法，移动机器人在每次到达当前目标点后，在向下一个目标点运动时都将执行停车动作，然后执行旋转动作，最后执行前进固定距离的动作。显然，在整个运动过程中，运动路径不流畅并且还会大大降低移动机器人的运动效率。

技术实现要素：

本申请的目的在于一种移动机器人的高效性局部路径控制方法，提高移动机器人局部路径控制的准确性和时效性。

为实现上述目的，本申请所采取的技术方案为：

一种移动机器人的高效性局部路径控制方法，所述移动机器人安装有陀螺仪以及相对布置的左右轮，并且左右轮上分别安装有独立的霍尔传感器，所述移动机器人的高效性局部路径控制方法包括：

步骤s1、获取移动机器人转弯时的旋转中心到左右轮的轮轴中心的距离作为移动机器人的转弯半径；

步骤s2、基于霍尔传感器计算移动机器人的左右轮的里程值，并通过陀螺仪采集移动机器人的航向角；

步骤s3、根据移动机器人的左右轮的里程值和航向角实现局部路径控制，包括以下步骤：

步骤s31、将移动机器人等效为左右轮的轮轴中心，且令移动机器人目前处于o0点，获取移动机器人的目标点t点，t点与o0点偏航角的差为α0，距离为d0；

步骤s32、建立以机器人朝向为y轴正方向、左右轮的轴线为x轴，且x轴的正方向为从左轮指向右轮的坐标系，以o0点为移动机器人的运动起始点，以旋转中心为圆心c，以转弯半径为半径r作移动机器人的虚拟转弯圆，连接t点与o0点记为线l0，作线l0的平行线l1交虚拟转弯圆于点o1，控制移动机器人沿虚拟转弯圆由o0点运行至o1点，连接虚拟转弯圆的圆心c与o1点记为线lr，由于线l1是虚拟转弯圆的切线，因此线l1垂直于线lr，同时又因为线l1平行于线l0，因此线l0垂直于线lr，则o0点至o1点的旋转角度为α0，因此可得：

线l0与线lr的交点o到起始点o0的距离为l0：

l0＝sinα0·r

连接t点与o点的线段to的长度l1为：

l1＝d0-l0＝d0-sinα0·r

连接o1点与o点的线段o1o的长度l2为：

l2＝r-cosα0·r

因此连接t点与o1点的线段to1的距离d1为：

由于线l0平行于线l1，因此线l1与线段to1的夹角α1等于线段to1的与线段to的夹角β，有：

步骤s33、判断夹角α1是否小于阈值δα，若是则控制移动机器人基于里程值由o1点起直线运动距离d1到达目标点t点；否则将o1点作为新的o0点、夹角α1作为t点与新的o0点偏航角的差α0、距离d1作为新的距离d0重新执行步骤s32直至夹角α1小于阈值δα。

以下还提供了若干可选方式，但并不作为对上述总体方案的额外限定，仅仅是进一步的增补或优选，在没有技术或逻辑矛盾的前提下，各可选方式可单独针对上述总体方案进行组合，还可以是多个可选方式之间进行组合。

作为优选，所述获取机器人转弯时的旋转中心到左右轮的轮轴中心的距离作为移动机器人的转弯半径，包括：

令g为旋转中心，在k时刻，左右轮位置分别为a1、b1，左右轮的轮轴中心的位置为z1，在k+1时刻，左右轮位置分别为a2、b2，左右轮的轮轴中心的位置为z2，则k+1时刻移动机器人相对k时刻的位置绕旋转中心g旋转了α度，左右轮前进距离分别为l1、l2，定义转弯半径r为旋转中心g和左右轮的轮轴中心z1的距离，则有：

两式相加得：

两式相减得：

因此得到转弯半径r为：

式中，w为移动机器人的底盘的宽，即为a1和b1的距离。

作为优选，所述基于霍尔传感器计算移动机器人的左右轮的里程值，包括：

定时读取霍尔传感器上传的累计的霍尔信号变化次数m；

根据累计的霍尔信号变化次数m计算里程值l为：

式中，m为左右轮的电机旋转一圈对应的霍尔信号变化次数，k为左右轮的减速比，r为左右轮的轮胎的半径。

作为优选，所述控制移动机器人沿虚拟转弯圆由o0点运行至o1点，包括：

令目标点t点的偏航角为δm，控制左右轮的电机输出速度使移动机器人由o0点起沿虚拟转弯圆开始运动，同时在运动过程中实时采集陀螺仪测量得到的航向角，直至陀螺仪测量的航向角与偏航角δm相同则认为移动机器人到达o1点。

作为优选，控制移动机器人由o1点起直线运动距离d1到达目标点t点，包括：

令目标点t点的偏航角为δm，当前陀螺仪采集到的航向角为δn，若δm＞δn时，即移动机器人左偏，此刻加大左轮对应的电机输出速度；若δm＜δn时，即移动机器人右偏，此刻加大右轮对应的电机输出速度，若δm＝δn时，则保持左轮和右轮对应的电机输出速度不变。

本申请提供的移动机器人的高效性局部路径控制方法，经过对移动机器人左右轮霍尔信号数据的采集计算出机器人前进的距离以及当前的航向角，在移动机器人向目标点移动过程中，不断地对当前的位置进行判断，并且根据里程值计算剩余距离和航向角得到是否达到目标角度，使机器人完成局部路径控制。即本申请根据底盘的里程和航向角信息实现从一个点到另一个点的运动，由于直接从安装在底盘上的传感器获取的里程等信息更加实时，且无需上传至上位机进行局部路径控制，因此可以提高局部路径控制的准确性，提高局部路径控制的时效性。

附图说明

图1为本申请的移动机器人的高效性局部路径控制方法的流程图；

图2为移动机器人旋转半径示意图；

图3为移动机器人路径控制示意图；

图4为移动机器人最终的移动路径示意图；

图5为本申请的仿真结果示意图。

具体实施方式

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

除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施例的目的，不是在于限制本申请。

其中一个实施例中，提供一种移动机器人的高效性局部路径控制方法，本实施例中移动机器人的底盘采用前后两个万向轮、左右轮差速驱动控制的方法，其中左右轮采用无刷直流电机驱动，并且底盘上装有陀螺仪。

容易理解的是，本实施例重点在于对移动机器人的控制，对于移动机器人的具体结构不展开描述，所使用的移动机器人具有常规结构，例如运动机构、驱动机构、计算机构等。

如图1所示，本实施例的移动机器人的高效性局部路径控制方法，包括以下步骤：

步骤s1、获取移动机器人转弯时的旋转中心到左右轮的轮轴中心的距离作为移动机器人的转弯半径。如图2所示(图2中以左旋为例)，令g为旋转中心，在k时刻，左右轮位置分别为a1、b1，左右轮的轮轴中心的位置为z1，在k+1时刻，左右轮位置分别为a2、b2，左右轮的轮轴中心的位置为z2，则k+1时刻移动机器人相对k时刻的位置绕旋转中心g旋转了α度，左右轮前进距离分别为l1、l2，定义转弯半径r为旋转中心g和左右轮的轮轴中心z1的距离，则有：

(1)和(2)两式相加得：

(2)和(1)两式相减得：

将式(4)代入式(3)得到转弯半径r为：

式中，w为移动机器人的底盘的宽，即为a1和b1的距离。

由此可知，当匀速运行时，在单位时间内，左右轮各自的l1和l2都不变，因此可以认为，在匀速运行过程中，移动机器人(或等效为移动机器人的底盘)在绕某一点，以固定的旋转半径作圆周运动。

在另一种情况下，原本移动机器人的底盘处于左右轮速度相等的直线行驶状态，现在需要向左或者向右运动，那么必将至少有一个轮子的速度将发生变化，但是为了减小旋转半径，一般都是变化其中一个轮子的速度，变化策略为：把移动机器人的底盘看成一个质点，建立以底盘质点为圆心，底盘朝向为y轴，两轮轴线为x轴，方向为从左轮指向右轮的坐标系，当目标点位于坐标系二、三象限时，可以提高右轮速度，例如原本左右轮的电机转速都是500rmp，现在左轮的电机转速依旧保持500rmp，而将右轮的电机转速提高到600rmp；当目标点位于坐标系一、四象限时，同理可以提高左轮速度。根据(5)式的推导，在加速过程中，其中一个轮子的里程值会出现一个里程增量δl，在减速时同理，那么此时旋转半径变为：

δl为轮子速度变化时带来的里程增减量，但是由于单位时间内δl量很小，因此可得r′≈r。综述，移动机器人在向目标点移动过程中，在进行转弯运动时为绕某一点作近似圆周运动，因此本申请移动机器人的转弯运动等效为圆周运动，以大大减少计算量。

本实施例中转弯半径为一个预设值，根据移动机器人的机械结构或者运动约束进行设置，并且在控制移动机器人转弯过程中根据预设的转弯半径计算转弯时左右轮分别的速度值。

步骤s2、基于霍尔传感器计算移动机器人的左右轮的里程值，并通过陀螺仪采集移动机器人的航向角。

对于机器人局部路径控制而言，里程值和航向角信息尤为重要，本实施例中对于里程值和航向角的采集包括以下步骤：

为了控制移动机器人计算量，并且为了反映移动机器人的状态变化，本实施例定时读取霍尔传感器的状态，并在每次改变时记录霍尔信号的变化次数，即本实施例设有数据采集周期。例如本实施例设置霍尔传感器每0.05ms进行一次数据采集计算，每10ms进行一次里程计算，陀螺仪每1ms进行一次数据采集计算。

本实施例记录霍尔传感器自移动机器人开始运行以来累计的霍尔信号的变化次数，由于该数值与实际物理量不对应，若需要知道机器人当前里程信息，则需要将霍尔信号累计次数转换为实际的移动距离。

本实施例提供的一种里程计算方法如下：

首先读取霍尔信号的值，在每次霍尔信号发生变化，即换向时将霍尔信号变化次数加一，每隔一段时间计算里程值，定时读取霍尔传感器上传的累计的霍尔信号变化次数m。

根据累计的霍尔信号变化次数m计算里程值l为：

式中，m为左右轮的电机旋转一圈对应的霍尔信号变化次数，该值与电机的极对数有关，k为左右轮的减速比，r为左右轮的轮胎的半径。

步骤s3、根据移动机器人的左右轮的里程值和航向角实现局部路径控制，如图3所示，包括：

步骤s31、将移动机器人等效为左右轮的轮轴中心(即等效为一个质点)，且令移动机器人目前处于o0点，获取移动机器人的目标点t点，t点与o0点偏航角的差为α0，距离为d0。

步骤s32、建立以机器人朝向为y轴正方向、左右轮的轴线为x轴，且x轴的正方向为从左轮指向右轮的坐标系，以o0点为移动机器人的运动起始点，以旋转中心为圆心c，以转弯半径为半径r作移动机器人的虚拟转弯圆，连接t点与o0点记为线l0，作线l0的平行线l1交虚拟转弯圆于点o1，控制移动机器人沿虚拟转弯圆由o0点运行至o1点。

在生成虚拟转弯圆时，根据目标点t相对于移动机器人起始点o0点的位置，确定移动机器人为左转或右转，然后在o0点建立坐标系，坐标系的y轴为底盘当前朝向，x轴为与y轴垂直，然后再以此坐标系建立虚拟转弯圆。当移动机器人为左转时，圆心c位于x轴负半轴，距离坐标原点的距离为转弯半径的长度，当移动机器人为右转时，圆心c位于x轴正半轴，距离坐标原点的距离为转弯半径的长度。

并且在做平行线l1交虚拟转弯圆于点o1时，由于与线l0平行且与虚拟转弯圆相切的点有两个，在移动机器人左转时，两个交点分别位于第二象限和第三象限；在移动机器人右转时，两个交点分别位于第一象限和第四象限。但是根据移动机器人的运动规律，仅选取第二象限和第一象限的交点。

控制移动机器人沿虚拟转弯圆由o0点运行至o1点时，令目标点t点的偏航角为δm，控制左右轮的电机输出速度使移动机器人由o0点起沿虚拟转弯圆开始运动，同时在运动过程中实时采集陀螺仪测量得到的航向角，直至陀螺仪测量的航向角与偏航角δm相同则认为移动机器人到达o1点。需要说明的是，这里的航向角与偏航角δm相同可以是两者相等，也可以是两者在允许的误差范围内。

当到达点o1时，说明已经达到了与给定点t点相对于底盘起始点o0相同偏航角，但是如果此刻执行直线前进动作的话，一定不可能到达t点，为了能够达到t点，作如下推导计算：

连接虚拟转弯圆的圆心c与o1点记为线lr，由于线l1是虚拟转弯圆的切线，因此线l1垂直于线lr，同时又因为线l1平行于线l0，因此线l0垂直于线lr，则o0点至o1点绕c点的旋转角度为α0，因此可得：

线l0与线lr的交点o到起始点o0的距离为l0：

l0＝sinα0·r(8)

连接t点与o点的线段to的长度l1为：

l1＝d0-l0＝d0-sinα0·r(9)

连接o1点与o点的线段o1o的长度l2为：

l2＝r-cosα0·r(10)

因此连接t点与o1点的线段to1的距离d1为：

由于线l0平行于线l1，因此线l1与线段to1的夹角α1等于线段to1的与线段to的夹角β，有：

步骤s33、判断夹角α1是否小于阈值δα，若是则控制移动机器人基于里程值由o1点起直线运动距离d1到达目标点t点；否则将o1点作为新的o0点、夹角α1作为t点与新的o0点偏航角的差为α0、距离d1作为新的距离d0重新执行步骤s32直至夹角α1小于阈值δα。

在上面推导过程中，首先得到了点t相对于点o1的信息，即t点与o1点偏航角差为α1，距离为d1，因此可以通过上述信息，将o1点重新作为起始点继续运行到偏航角值为α0+α1为止，即再一次到达规定的偏航角位置点o2，再重复上述计算，得到点t相对于点o2的信息，相对于经过n次的计算，最终得到点t相对于点on的信息，对比此时角度αn是否小于阈值δα，如果满足条件，即认为在点on处时目标点t在移动机器人朝向上，则执行前进固定距离的动作，前进的固定距离为dn。其中阈值δα为用户设定的一个较小的阈值，阈值越小，精度越高但迭代次数也随之增加；阈值越大，迭代次数会大大减少，但是可能无法精确到达目标点。

需要说明的是，本实施例的局部路径控制实现方法，可以是规划一个位置点后控制移动机器人运动至规划的位置点后，然后再进行下一个点的规划；也可以是规划一个位置点后控制移动机器人运行至规划的位置点的过程中进行下一个位置点的规划，以提高机器人的运行效率。

在最后执行前进固定距离的直线行驶的过程中，为了保持直线行驶，根据最终已经知道了目标点的偏航角，因此可以通过读取陀螺仪的航向角来进行直线矫正，使得底盘保持直线行驶，最终到达目标点处。控制方法如下：

令目标点t点的偏航角为δm，当前陀螺仪采集到的航向角为δn，若δm＞δn时，即移动机器人左偏，此刻加大左轮对应的电机输出速度；若δm＜δn时，即移动机器人右偏，此刻加大右轮对应的电机输出速度，若δm＝δn时，则保持左轮和右轮对应的电机输出速度不变。

其中向左时陀螺仪的航向角值减小，向右时陀螺仪的航向角值增大。最终控制移动机器人运动的规划路径如图4所示，其中加粗为控制移动机器人运动的规划路径，整个规划路径的目的就是为了找到点ta，该点就是目标点t与运动轨迹的切点，到达点ta后即可基于里程值直线运动一定距离到达目标点t。

为了验证本申请将移动移动机器人的转弯运动等效为圆周运动进行移动机器人的路径规划控制具有实用性以及准确性，本申请以相同的起始点、目标点、转弯半径进行路径规划和实际仿真试验，最终得到的仿真图如图5所示，图中实线为根据本申请的控制方法规划得到的规划路径，虚线为根据本申请的控制方法得到的规划路径控制移动机器人仿真运动得到的仿真实际路径。根据图示可以看出，规划路径与仿真实际路径在转弯阶段由于移动机器人初始运动存在加速阶段导致两者存在一定误差，但是两者的误差很小，在移动机器人的规划控制误差允许范围内，最终移动机器人能够准确到达目标点。因此本申请将转弯运动等效为圆周运动进行移动机器人的路径规划控制是可行的，且能够根据规划路径准确控制移动机器人运动至目标点，也极大的降低了路径规划的计算量，提高移动机器人运动控制的时效性。

需要说明的是，本实施例的局部路径控制方法适用于简单环境下的路径规划控制，即从一个点到另一个点的路径控制(也视作局部的路径控制)。对于其余的复杂环境，需要先将复杂环境进行分段，确保每一段为一个点到另一个点的路径，再依据本实施例的局部路径控制方法完成具体的路径控制。

现有的移动机器人路径规划控制主要工作在上位机，底盘通过上位机下发的速度指令实现移动机器人的路径控制。在此过程中上位机需要定时获取底盘运行数据，而底盘需要等待上位机规划后下发的指令进行运行，存在实时性不高等缺点。本实施例公开的方法，可以由上位机通过建图把整条路径分成多个目标点一次性下发到底盘，底盘就能够根据底盘的里程和航向角信息实现从一个点到另一个点的运动，由于底盘获取的里程等信息更加实时，因此可以提高路径控制的准确性和时效性。

应该理解的是，虽然图1的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图1中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。