CN101791800B - 一种双轮差动式机器人运动控制方法 - Google Patents

Abstract

本发明涉及一种双轮差动式机器人运动控制方法，技术特征在于：以机器人的几何中心为坐标原点在世界坐标系XOY中建立本体坐标系xoy；基于运动学的双轮差动式机器人运动控制方法，该方法针对所有轮式机器人运动中最基本、最主要的两个动作，即轮式机器人原地转角动作和运动到定点动作，实现了对这两个动作的运动控制。双轮差动式机器人的所有复杂动作都可以由这两个基本动作合成。本发明提出采用基于误差比例控制法对其中的线速度函数作了合理的改进，同时结合弦切法，对机器人运动到定点进行综合控制。综合控制极大发挥了这两种方法各自的优点，避免了各自缺陷对机器人运动的影响。发明中还针对循环震荡现象，提出了其相应的解决策略。

Description

一种双轮差动式机器人运动控制方法

技术领域

本发明涉及一种双轮差动式机器人运动控制方法，具体地说是一种基于运动学的双轮差动式机器人运动控制方法，可用于任何要求具有高精度、高稳定性的双轮差动式机器人的运动控制。

背景技术

机器人技术是一种面向未来的现代化技术，它和网络技术、基因技术、通信技术、计算机技术等科学技术一样，也属于高新技术。早在1920年时，“ROBOT”这个词就开始在捷克作家K.凯比克的一本科幻剧本中出现。在1950年，美国作家I.阿西莫夫又提出了机器人学(Roboties)这一概念，并提出了所谓的“机器人三原则”。在21世纪的今天，可以说机器人在当今社会对于人类的作用和影响越来越大，如在军事领域、医学领域、环保产业领域、农业领域、太空和深海探测等诸多方面都有着不同性能和不同种类的机器人从事着相关工作，并且随着各项技术的提高，机器人将对人类的进步和发展产生深远的影响。

业内周知，机器人要完成某个任务，都是通过其具体动作来实现的。而机器人的动作则由机器人最底层运动控制决定的，底层运动控制是完成各种复杂动作的基本保证。双轮差动机器人也不例外，所有的双轮差动机器人动作归根结底是根据场上的不同情况由决策系统自主直接指定其左、右轮速度，实现机器人的直线、曲线、和原地旋转等基本动作，再由这些基本动作组合成较为复杂的动作。由于对于双轮差动式机器人模型，从运动学角度讲它不能同时满足非完全约束和动力学约束的条件，是一个非完整系统。为了实现机器人快速而准确的运动控制，国内外学者在这方面提出了许多方法。例如在原地转角控制方面，现在普遍采用的是比例线性控制算法，即由当前方向角与目标方向角之间的差值Δθ_r作为控制量，然后通过大量测试确定一个比例系数a，将它们的乘积a·Δθ_r作为数字控制器的输出，作用到电机上以驱动车轮。这种方法原理简单直观，且容易实现，但在实际应用中，由于物理惯性的影响，会使机器人产生强烈的震荡，不能快速、稳定的达到目标角度。而在跑定点动作控制中，现在普遍采用的有弦切法、误差比例控制法等，这些方法虽然都有比较明确的物理或几何意义，但都由于没有对于物理惯性或机器人的实际应用环境给与特殊考虑，故在实际应用中都存在各个方面的问题。比如应用弦切法控制，就容易使机器人运动轨迹过长，从而延长了运动时间；而采用误差比例控制法，就容易使机器人在目标点附近产生强烈的震荡，不能有效的定位于目标点，而震荡现象也容易引起电机故障，从而减少电机的使用寿命。

发明内容

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种双轮差动式机器人运动控制方法，以实现双轮差动式机器人在各种复杂工作环境下快速、稳定、精确的运动控制。

技术方案

一种双轮差动式机器人运动控制方法，其特征在于：以机器人的几何中心为坐标原点在世界坐标系XOY中建立本体坐标系xoy，转角运动的控制步骤和跑位运动的控制步骤如下：

转角运动的控制步骤：

步骤1计算机器人的当前方向角和目标方向角角度差：Δθ_r＝|θ_d-θ_n|，其中θ_d为机器人的目标方向角，θ_n为机器人的当前方向角；

步骤2计算左右轮速值：

$\left\{\begin{array}{c}{V}_L=1+\ln a\·{\mathrm{\Δ\θ}}_r+\frac{{\ln }^{2}a}{2}{{\mathrm{\Δ\θ}}_r}^2+\frac{{\ln }^{3}a}{6}{{\mathrm{\Δ\θ}}_r}^3+...+\frac{{\ln }^{n}a}{n!}{{\mathrm{\Δ\θ}}_r}^n\\ V_R=-(1+\ln a\·{\mathrm{\Δ\θ}}_r+\frac{{\ln }^{2}a}{2}{{\mathrm{\Δ\θ}}_r}^2+\frac{{\ln }^{3}a}{6}{{\mathrm{\Δ\θ}}_r}^3+...+\frac{{\ln }^{n}a}{n!}{{\mathrm{\Δ\θ}}_r}^n)\end{array}\right.$

V_L为左轮速值，V_R为右轮速值，项数n和底数a的参数选择策略如下：

当Δθ_r的区间为[0，5]时，n取为2、a取为1.8；

当Δθ_r的区间为(5，15]时，n取为3、a取为1.4；

当Δθ_r的区间为(15，30]时，n取为4、a取为1.21；

当Δθ_r的区间为(30，90]时，n取为5、a取为1.12；

步骤3：将步骤2得到的轮速分别作用于机器人左右轮的伺服电机上，驱动机器人左右轮作相应的转动；

跑位运动的控制步骤：

步骤1计算机器人当前位置坐标与目标点位置坐标的距离：

$\mathrm{dis}=\sqrt{{(x_{\mathrm{sta}}-x_{\mathrm{des}})}^2+{(y_{\mathrm{sya}}-y_{\mathrm{des}})}^2}$

其中：x_sta、y_sta为机器人当前的位置坐标，x_des、y_des为目标点的位置坐标；

步骤2计算机器人当前方向角与当前位置和目标点位置连线的夹角：

${\mathrm{\Δ\θ}}_m=\arctan \left(\frac{y_{\mathrm{des}}-y_{\mathrm{sta}}}{x_{\mathrm{des}}-x_{\mathrm{sta}}}\right)-{\mathrm{\θ}}_n$

其中：θ_n为机器人的当前方向角；

步骤3进行跑位震荡检测：比较机器人运动最近三个仿真节拍的目标点坐标和机器人的位置坐标，当两拍的数据不相等时执行下一步骤；

当两拍的数据相等时，断定机器人当前处在目标点附近的震荡状态，将机器人正面朝向与X轴正方向夹角设为θ_p，机器人目标位置和当前位置的连线与X轴正方向的夹角设为θ_q，则机器人执行转角运动控制步骤、做一个由θ_p转向θ_q的原地转角运动，然后执行下一步骤；

步骤4：以误差比例控制方法产生的最大震荡距离dis_vib为分界线：

当dis＞＝dis_vib时，左右轮速值为

其中f(dis·cos(Δθ_m))和g(Δθ_m)采用拟合方法得到；

当dis＜dis_vib时，左右轮速值为其中

V的值取f(dis·cos(Δθ_m))，为指定的机器人运动的线速度，L是机器人两轮之间的距离；

所述的dis_vib为机器人的最大速度所对应的运动距离为其值；

所述的拟合方法为：将若干组机器人运动距离dis、转角量Δθ和运动速度v的关系dis-v和Δθ-v的数据，采用拟合方法得到如下公式：

f(dis·cos(Δθ_m))＝0.14284×(dis·cos(Δθ_m))³-0.66979×(dis·cos(Δθ_m))²

                  +12.519×(dis·cos(Δθ_m))-0.23125

g(Δθ_m)＝0.01178×(Δθ_m)²+0.1989×(Δθ_m)+1

步骤5：将步骤4得到的轮速分别作用于机器人左右轮的伺服电机上，驱动机器人左右轮作相应的转动。

有益效果

本发明提出的双轮差动式机器人运动控制方法，相比现有技术的优越性在于：

(1)本发明由于基于运动学模型，因而在具体实现时，控制系统分为两层。上层是基于运动学模型的机器人运动控制算法，下层是电机速度控制器控制驱动电机旋转以带动车轮运动。上层在实现机器人的运动控制时，可以不考虑机器人的动力学特性，即下层的电机速度控制对上层来讲是透明的，这样的建模思想直接反映了机器人的主要运动特性，便于建立合理的运动模型，而且易于设计相应的控制器，同时也使系统开发模块化，便于以后控制算法的改进。

(2)本发明由于考虑了机器人运动中惯性的影响，因而机器人的运动控制更加精确和全面。

(3)本发明的指数型原地转角控制算法中，由于可以按角度差Δθ_r的大小分成不同的区间，在各区间上分别确定待定参数，因而扩大了该算法的适用性，同时也提高了机器人的控制效果。

(4)本发明综合运用了误差比例控制法和弦切法，从而充分发挥了其各自的优点，同时又避免了其各自算法的不足。

(5)本发明对误差比例控制法中的线速度函数f(dis)做了部分改进，从而提高了机器人运动的精确性。

(6)本发明参考计算机操作系统中死锁的防治策略，提出了应用于机器人运动控制中的震荡检测方法，并说明了一种打破振荡条件的方法。

附图说明

图1是双轮差动式机器人运动控制方法流程图

图2是本发明用于说明机器人系统组成的示意图

图3是本发明为了数学建模所建立的坐标系示意图

图4是本发明的描述机器人曲线运动的示意图

图5是本发明所建立的指数型原地转角模型的原理说明图

图6是本发明用于建立弦切法定点运动模型的原理说明图

具体实施方式

现结合实施例、附图对本发明作进一步描述：

一、本发明所应用的数学理论、控制原理及技术术语说明

1、数据拟合

所谓数据拟合是指已知某函数的若干离散函数值{f1，f2，...，fn}，通过调整该函数中若干待定系数f(λ1，λ2，...，λn)，使得该函数与已知点集的差别(最小二乘意义)最小。如果待定函数是线性，就叫线性拟合或者线性回归，否则叫作非线性拟合或者非线性回归。表达式也可以是分段函数，这种情况下叫作样条拟合。数据拟合的工程意义在与它可以通过若干离散的数据得到一个自变量与因变量之间的连续的函数，由该函数得到的函数值与已知点集的差别最小。

2、循环震荡

循环震荡就是到定点运动控制中机器人在目标点附近作永不停歇的震荡运动，不能静止在目标点的一种现象，不同于普通的震荡现象，同时这种现象还具有机器人的位置坐标在若干拍内形成循环的特点。这是由于双轮差动式机器人不能同时满足非完全约束和动力学约束的条件，是一个非完整系统，因此当机器人处于某种初始状态，再向某一目标点运动时，通过控制算法的计算，会导致机器人永远不能到达目标点，而只能在目标点附近不断运动，最终形成循环。

3、伺服电机

伺服电机是在伺服系统中控制机械元件运转的发动机，是一种补助马达间接变速装置。它可以使控制速度，位置精度非常准确。在自动控制系统中，它用作执行元件，把所收到的电信号转换成电动机轴上的角位移或角速度输出以驱动控制对象的。

二、本发明的双轮差动式机器人运动控制方法具体实现

本发明主要包括双轮差动式机器人的原地转角和机器人运动到定点的控制方法。具体步骤如下：

步骤一、在机器人所处环境中建立一个总的坐标系，称之为世界坐标系，设为XOY，然后以机器人的几何中心为坐标原点建立一个坐标系，称之为本体坐标系xoy，建立坐标系后的机器人如图3所示，在建立的坐标系里构建机器人的控制模型。

1、设(x，y)为机器人在所建立的世界坐标系下的位置坐标；θ为机器人的方向角，即X轴到x轴转过的角度，并以逆时针方向为正方向；V为机器人质心的线速度大小；w为机器人质心作曲线运动时的角速度大小；r为机器人轮子的半径；L为机器人两轮之间的距离；V_L、V_R为机器人左右轮转动的线速度大小；θ_L、θ_R为机器人左右轮转动的角速度大小。

2、假定机器人左右轮子和地面之间没有滑动，且沿直线作纯滚动运动，即每瞬时与地面接触点的速度等于零，则由钢体运动规律可得如下方程：

$w=\frac{V_R-V_L}{L}$

$V=\frac{V_R+V_L}{2}$

V_L＝r·θ_L

V_R＝r·θ_R

3、当机器人作曲线运动时，由于机器人运动的轨迹是由一系列绕瞬时圆心旋转的小段圆弧组成，如图4所示。

根据图三的几何关系可得到方程：

$\frac{V_R-V_L}{L}=\frac{V_R}{R+\frac{L}{2}}$

其中R为瞬时圆弧的半径。对该方程作等价变换，可得到机器人作圆周运动时的圆弧半径R为：

$R=\frac{L}{2}\·\frac{V_R+V_L}{V_R-V_L}$

由该方程可以得到，当V_L＝V_R时，圆弧半径R趋向无穷大，则机器人作直线运动；当V_L＝-V_R时，圆弧半径R等于0，则机器人以质心为圆心作原地旋转；当V_L≠V_R时，则圆弧半径R为一个固定值，即机器人围绕瞬时圆心作半径为R的圆周运动。

视觉子系统采集机器人的图像信息，通过图像处理后将机器人在已建立的世界坐标系中的坐标信息，以及机器人的方向信息等传递到智能决策子系统。智能决策子系统对视觉子系统传递的预处理信息做相应的处理，并对机器人当前应做的动作作出判断。如果当前拍机器人需要作转角控制，则转向步骤(2)；如果当前拍机器人需要作精确跑位动作，则转向步骤(3)。

步骤二、由机器人当前的方向角和目标方向角作为数字控制器的输入参数，经指数型转角控制算法得到机器人当前拍的左轮速和右轮速大小，然后执行步骤(4)。

1、用目标方向角减去当前方向角，得到其差值Δθ_r，即：

Δθ_r＝|θ_d-θ_n|

其中θ_d为机器人的目标方向角，θ_n为机器人的当前方向角。

2、考虑到机器人运动时的惯性现象，并为了保证机器人运动的快速性，则当Δθ_r较大时，机器人应有较大的左右轮速|V_L|，以达到快速转角的目的，当Δθ_r减小时，轮速大小|V_L|应急剧减小，以缓冲机器人惯性的影响，达到消除震荡现象，使机器人稳定的目的。如图5为实际测试的某双轮机器人静态转角时|V_L|和Δθ_r之间的关系图(对于其他的双轮式机器人也有类似关系)，这正符合指数曲线的特点：当x较小时y的值较小，且变化较为平缓，当x增大时，y的值将随之急剧增大。因而建立机器人的转角模型为：

V_L＝-V_R

$\left|V_L\right|=a^{{\mathrm{\Δ\θ}}_r}$

其中参数a的范围为a＞1。

3、为了便于数字控制器的实现，由麦克劳林公式：

$f\left(x\right)=\underset{n=0}{\overset{\∞}{\mathrm{\Σ}}}\frac{f^{\left(n\right)}\left(0\right)}{n!}{x}^n$

及(a^x)⁽ⁿ⁾|_x＝0＝a^xlnⁿa|_x＝0＝lnⁿa

可得 $a^{{\mathrm{\Δ\θ}}_r}=\underset{n=0}{\overset{\∞}{\mathrm{\Σ}}}\frac{{\ln }^{n}a}{n!}{{\mathrm{\Δ\θ}}_r}^n=1+\ln a\·{\mathrm{\Δ\θ}}_r+\frac{{\ln }^{2}a}{2}{{\mathrm{\Δ\θ}}_r}^2+\frac{{\ln }^{3}a}{6}{{\mathrm{\Δ\θ}}_r}^3+...+\frac{{\ln }^{n}a}{n!}{{\mathrm{\Δ\θ}}_r}^n+...$

故只要确定合适的项数n和底数a即可由该模型确定机器人左右轮轮速|V_L|和角度差Δθ_r之间的函数关系式。

4、在具体实现中，选定项数n和底数a可参照如下策略：

随着项数n或底数a的增大，机器人的Δθ_r--|V_L|曲线将逐渐变得比较陡峭，即机器人的转角速度加快，但机器人容易产生震荡现象。

随着项数n或底数a的减小，机器人的Δθ_r--|V_L|曲线将逐渐变得比较平缓，即机器人的转角速度减慢，但转角过程平稳，机器人不易出现震荡现象。

为了达到较好的控制效果，通常可以按Δθ_r的变化将Δθ_r分成几个不同的区间，然后在不同的区间上确定不同的参数。对于不同的区间Δθ_r，选定参数时，应在以上策略的基础之上，先选定项数n，然后从某一个不会引起机器人震荡的底数a开始，在尽可能稳定的前提下逐渐增大a的取值以提高机器人转角的速度。

对于不同区间上项数n的确定，由于当Δθ_r较小时，机器人的惯性对机器人的运动影响较为显著，因而转角就越需要平稳；当Δθ_r较大时，机器人的惯性对机器人的运动影响不是很明显，且为了达到快速性的要求，转角速度需要提高。因而对于不同的区间，随着Δθ_r的增大，该区间上项数n的值也应该逐渐增大。

项数n和底数a的参数选择策略如下：

当Δθ_r的区间为[0，5]时，n取为2、a取为1.8；

当Δθ_r的区间为(5，15]时，n取为3、a取为1.4；

当Δθ_r的区间为(15，30]时，n取为4、a取为1.21；

当Δθ_r的区间为(30，90]时，n取为5、a取为1.12；

5、在确定了转角模型的所有参数后，即可由该模型得到确定的机器人左右轮轮子的速度大小|V_L|，将该值作为数字控制器的输出。

步骤三、由机器人当前的位置坐标、目标点位置坐标和机器人的当前方向角作为数字控制器的输入参数，经改进后的误差比例控制算法得到机器人当前拍的左轮速和右轮速大小，然后执行步骤(4)。

1、由机器人当前的位置坐标和目标点的位置坐标，得到这两点之间的欧式距离，即

$\mathrm{dis}=\sqrt{{(x_{\mathrm{sta}}-x_{\mathrm{des}})}^2+{(y_{\mathrm{sya}}-y_{\mathrm{des}})}^2}$

其中x_sta、y_sta为机器人当前的位置坐标，x_des、y_des为目标点的位置坐标。

2、由数字控制器的输入参数，得到机器人当前方向角与当前位置和目标点位置连线的夹角，即

${\mathrm{\Δ\θ}}_m=\arctan \left(\frac{y_{\mathrm{des}}-y_{\mathrm{sta}}}{x_{\mathrm{des}}-x_{\mathrm{sta}}}\right)-{\mathrm{\θ}}_n$

其中θ_n为机器人当前的方向角。

3、作震荡检测。在实践中发现，机器人到达目标点附近时，由于惯性等各方面因素的影响，机器人总是不能有效的停止在目标点上，而是不停的来回震荡，这无论是对机器人的有效运动控制还是对机器人自身的物理器件都有较为严重的影响。因此可以保存机器人运动过程中前三拍的目标点坐标和机器人的位置坐标，只要检测到这三拍中有两拍的数据完全相等，则可以断定机器人当前处在目标点附近的震荡状态。当检测到机器人处于震荡状态时，可以改变机器人的运动策略，即将朝向目标点的到定点运动改为指向目标点的原地转角运动，这样就可以打破机器人的震荡平衡，而使机器人下一拍可以有效地朝向目标点运动。

4、采用误差比例控制算法，即通过到目标点的距离dis来控制机器人的线速度V，而通过与目标方向的夹角Δθ_m来控制机器人的角速度w，建立机器人的到定点运动模型：

V_L＝f(dis)+g(Δθ_m)

V_R＝f(dis)-g(Δθ_m)

其中f(dis)是以距离dis为自变量的函数，g(Δθ_m)是以角度差Δθ_m为自变量的函数。

5、在上述基于误差比例控制方法建立的模型中，其中g(Δθ_m)函数可以通过多次实验，对机器人付以不同的轮速，得到机器人在不同轮速下一拍可以转到的角度曲线Δθ，然后用拟合的方法，得到Δθ-|V_L|曲线g(Δθ_m)。f(dis)可以基于同样的方法得到，同时考虑到小车的速度方向始终与其指向一致，只有距离在连线方向上的投影才是小车运动的有效距离，因此对得到的f(dis)函数稍作修改，即可得到改进后的基于误差比例控制方法的到定点运动模型：

V_L＝f(dis·cos(Δθ_m))+g(Δθ_m)；

V_R＝f(dis·cos(Δθ_m))-g(Δθ_m)

6、由于误差比例控制方法没有考虑到机器人运动过程中的路径规划，因此在短距离的跑定点运动中，该方法显得过于简单，会使机器人产生较为剧烈的震荡现象。因此，以误差比例控制方法产生的最大震荡距离dis_vib为分界线，当dis＞＝dis_vib时采用上述所述的改进后的误差比例控制方法。当dis＜dis_vib时，建立如下的基于弦切法的到定点运动模型：

如图6所示，由几何关系可以得到如下等式：

$R=\frac{\mathrm{dis}}{2\sin \left({\mathrm{\Δ\θ}}_m\right)}$

又由步骤(一)中所建立的运动模型，可以得到：

$R=\frac{L}{2}\·\frac{V_R+V_L}{V_R-V_L}$

$V=\frac{V_R+V_L}{2}$

$w=\frac{V_R-V_L}{L}$

由以上公式联立即可以解得机器人的左右轮轮速分别为：

$V_L=V-\frac{L\·V}{2R}$

$V_R=V+\frac{L\·V}{2R}$

所以只要指定机器人运动的线速度V，并确定机器人两轮之间的距离L就可以由该控制方法得到机器人运动的左右轮轮速。该控制方法在机器人的运动过程中有了相应的路径规划，因此在短距离内可以使机器人的运动更加准确，但由于双轮差动式机器人不能同时满足非完全约束和动力学约束的条件，是一个非完整系统，因而该控制方法会使机器人产生循环震荡现象，这也就是为什么第三步要先进行循环检测的原因。

7、由以上模型确定了机器人左右轮轮速V_L和V_R后，就可将该值作为数字控制器的输出。

步骤四、将数字控制器的输出分别作用于机器人左右轮的伺服电机上，以驱动机器人左右轮作相应的转动，以达到期望的运动结果。

Claims (1)

1.一种双轮差动式机器人运动控制方法，其特征在于：以机器人的几何中心为坐标原点在世界坐标系XOY中建立本体坐标系xoy，转角运动的控制步骤和跑位运动的控制步骤如下：

转角运动的控制步骤：

步骤1计算机器人的当前方向角和目标方向角角度差：Δθ_r＝|θ_d-θ_n|，其中θ_d为机器人的目标方向角，θ_n为机器人的当前方向角；

步骤2计算左右轮速值：

$\left\{\begin{array}{c}{V}_L=1+\ln a\·{\mathrm{\Δ\θ}}_r+\frac{{\ln }^{2}a}{2}{{\mathrm{\Δ\θ}}_r}^2+\frac{{\ln }^{3}a}{6}{{\mathrm{\Δ\θ}}_r}^3+...+\frac{{\ln }^{n}a}{n!}{{\mathrm{\Δ\θ}}_r}^n\\ V_R=-(1+\ln a\·{\mathrm{\Δ\θ}}_r+\frac{{\ln }^{2}a}{2}{{\mathrm{\Δ\θ}}_r}^2+\frac{{\ln }^{3}a}{6}{{\mathrm{\Δ\θ}}_r}^3+...+\frac{{\ln }^{n}a}{n!}{{\mathrm{\Δ\θ}}_r}^n)\end{array}\right.$

V_L为左轮速值，V_R为右轮速值，项数n和底数a的参数选择策略如下：

当Δθ_r的区间为[0，5]时，n取为2、a取为1.8；

当Δθ_r的区间为(5，15]时，n取为3、a取为1.4；

当Δθ_r的区间为(15，30]时，n取为4、a取为1.21；

当Δθ_r的区间为(30，90]时，n取为5、a取为1.12；

步骤3：将步骤2得到的轮速分别作用于机器人左右轮的伺服电机上，驱动机器人左右轮作相应的转动；

跑位运动的控制步骤：

步骤1计算机器人当前位置坐标与目标点位置坐标的距离：

$\mathrm{dis}=\sqrt{{(x_{\mathrm{sta}}-x_{\mathrm{des}})}^2+{(y_{\mathrm{sya}}-y_{\mathrm{des}})}^2}$

其中：x_sta、y_sta为机器人当前的位置坐标，x_des、y_des为目标点的位置坐标；

步骤2计算机器人当前方向角与当前位置和目标点位置连线的夹角：

${\mathrm{\Δ\θ}}_m=\arctan \left(\frac{y_{\mathrm{des}}-y_{\mathrm{sta}}}{x_{\mathrm{des}}-x_{\mathrm{sta}}}\right)-{\mathrm{\θ}}_n$

其中：θ_n为机器人的当前方向角；

步骤3进行跑位震荡检测：比较机器人运动最近三个仿真节拍的目标点坐标和机器人的位置坐标，当两拍的数据不相等时执行下一步骤；

当两拍的数据相等时，断定机器人当前处在目标点附近的震荡状态，将机器人正面朝向与X轴正方向夹角设为θ_p，机器人目标位置和当前位置的连线与X轴正方向的夹角设为θ_q，则机器人执行转角运动控制步骤、做一个由θ_p转向θ_q的原地转角运动，然后执行下一步骤；

步骤4：以误差比例控制方法产生的最大震荡距离dis_vib为分界线：

当dis＞＝dis_vib时，左右轮速值为

其中f(dis·cos(Δθ_m))和g(Δθ_m)采用拟合方法得到；

当dis＜dis_vib时，左右轮速值为

其中

V的值取f(dis·cos(Δθ_m))，为指定的机器人运动的线速度，L是机器人两轮之间的距离；

所述的dis_vib为机器人的最大速度所对应的运动距离为其值；

所述的拟合方法为：将若干组机器人运动距离dis、转角量Δθ和运动速度v的关系dis-v和Δθ-v的数据，采用拟合方法得到如下公式：

f(dis·cos(Δθ_m))＝0.14284×(dis·cos(Δθ_m))³-0.66979×(dis·cos(Δθ_m))²

                  +12.519×(dis·cos(Δθ_m))-0.23125

g(Δθ_m)＝0.01178×(Δθ_m)²+0.1989×(Δθ_m)+1

步骤5：将步骤4得到的轮速分别作用于机器人左右轮的伺服电机上，驱动机器人左右轮作相应的转动。

Images