CN105479460A - 一种非时间参考的差速驱动机器人路线跟踪控制方法 - Google Patents

Abstract

本发明提出一种非时间参考的差速驱动机器人路线跟踪控制方法，属于差速驱动机器人的路线跟踪控制技术领域，该方法包括：在全局目标路线相对应的全局坐标系中，若该全局目标路线的X轴坐标值单调变化，机器人控制器直接切换到路线跟踪控制状态，否则将全局目标路线划分为多个局部路线，并建立各局部路线相对应的局部坐标系，使局部路线的X轴坐标在该局部坐标系内单调变化，机器人控制器切换到路线跟踪控制；实时获取目标路线信息及机器人位置和方位角信息，计算得到的速度指令控制机器人的速度前进，直到机器人达到全局路线终点，路线跟踪控制结束。本方法便于驱动电机实现计算出的车轮行驶速度，易实现控制机器人沿目标路线行驶。

Description

一种非时间参考的差速驱动机器人路线跟踪控制方法

技术领域

本发明属于差速驱动机器人的路线跟踪控制技术领域，特别涉及适用于两轮、三轮、四轮、六轮等，基于差速驱动的轮式机器人和轮式走行装置的路线跟踪控制。

背景技术

随着人类社会的进步和科学技术的发展，轮式移动机器人已在人们生产和生活的多个领域得到了广泛的应用，如生产车间中的货物搬运机器人、医院中的药品传输机器人、室内地面清洁机器人、智能轮椅机器人等。差速驱动机器人采用2个伺服电机控制车轮速度，实现目标路线或目标点的跟踪，因结构简单，传动效率高和控制灵活被广泛使用。机器人的路线跟随能力是其实现特定任务的关键。

目前机器人路线控制多采用PID控制、模糊逻辑等方法，控制机器人沿目标路线运动，或设计轮速随时间变化的函数，来实现机器人的路线跟踪。机器人运动学模型为强非线性系统，传统的PID控制方法不适用于强非线性系统。基于模糊控制逻辑的路线跟踪控制器结构不易确定，且需大量实验操作数据训练模糊控制器。采用设计轮速随时间变化函数实现机器人路线跟踪的方法，对车轮速度控制精度要求较高，当驱动电机出现速度控制偏差时，导致路线跟踪失败。

本发明适用的已有轮式机器人包括，图1所示的三轮机器人、图2所示的四轮机器人、图3所示的六轮机器人等基于两轮差速驱动行驶的多轮机器人。

图1中的三轮机器人包括：机器人底盘11、驱动轮12、驱动轮13、驱动电机14、驱动电机15、控制器16、电源设备17、万向轮18，其中驱动电机14包括转子141和定子142，驱动电机15又包括转子151和定子152。图1中：驱动轮12与机器人底盘11通过驱动电机14连接，驱动电机14的转子141与驱动轮12固接，驱动电机14的定子142固接于机器人底盘11；驱动轮13与机器人底盘11通过驱动电机15连接，驱动电机15的转子151与驱动轮13固接，驱动电机15的定子152固接于机器人底盘11；控制器16固定在机器人底盘11上，与外界定位系统进行通信，实时获取路线信息及机器人的位置和方位角信息，根据目标路线信息及机器人的位置和方位角计算驱动轮12和驱动轮13的运动速度，并将速度指令发送给驱动电机14和驱动电机15，然后驱动电机14和驱动电机15执行接收到的速度指令；电源设备17固定在机器人底盘11上，给驱动电机14和驱动电机15及控制器16供电。万向轮18固定在机器人底盘11的纵向对称线上，可在底盘11所在平面内转动，用于支撑机器人重量。

图2中的三轮机器人包括：机器人底盘21、驱动轮22、驱动轮23、驱动电机24、驱动电机25、控制器26、电源设备27、万向轮281、万向轮282，其中驱动电机24包括转子241和定子242，驱动电机25包括转子251和定子252。图2中：驱动轮22与机器人底盘21通过驱动电机24连接，驱动电机24的转子241与驱动轮22固接，驱动电机24的定子242固接于机器人底盘21；驱动轮23与机器人底盘21通过驱动电机25连接，驱动电机25的转子251与驱动轮23固接，驱动电机25的定子固接于机器人底盘21；控制器26固定在机器人底盘21上，与外界定位系统进行通信，实时获取路线信息及机器人的位置和方位角信息，根据目标路线信息及机器人的位置和方位角计算驱动轮22和驱动轮23运动速度，并将速度指令发送给驱动电机24和驱动电机25，然后驱动电机24和驱动电机25执行接收到的速度指令；电源设备27固定在机器人底盘21上，给驱动电机24和驱动电机25及控制器26供电。万向轮281和万向轮282固定在机器人底盘21的对称线上，且分别布置底盘21的前端和后端，万向轮281和万向轮282可在底盘21所在平面内转动，用于支撑机器人重量。

图3中的三轮机器人包括：机器人底盘31、驱动轮32、驱动轮33、驱动电机34、驱动电机35、控制器36、电源设备37、万向轮381、万向轮382、万象轮383、万向轮384，其中驱动电机34包括转子341和定子342，驱动点机35包括转子351和定子352。图3中：驱动轮32与机器人底盘31通过驱动电机34连接，驱动电机34的转子341与驱动轮32固接，驱动电机34的定子342固接于机器人底盘31；驱动轮33与机器人底盘31通过驱动电机35连接，驱动电机35的转子351与驱动轮33固接，驱动电机35的定子352固接于机器人底盘31；控制器36固定在机器人底盘31上，与外界定位系统进行通信获取路线信息和机器人状态信息，根据目标路线信息及机器人的位置和方位角计算驱动轮32和驱动轮23运动速度，并将速度指令发送给驱动电机34和驱动电机35，然后驱动电机34和驱动电机35执行接收到的速度指令；电源设备37固定在机器人底盘31上，给驱动电机34和驱动电机35及控制器36供电。万向轮381、万向轮382、万向轮383、万向轮384均固定在机器人底盘31上，分别布置在底盘31的四个端点处，用于支撑机器人重量，全部万向轮可在底盘31所在平面内转动。

图1所示的万向轮18，图2所示的万向轮281、万向轮282，图3中所示万向轮381、万向轮382、万向轮383、万向轮384只启平衡载荷作用，对机器人的运动和路线跟踪控制不产生影响，因此将图1、图2、图3所示机器人简化为图4所示两轮机器人模型。

图4所示两轮机器人结构包括：机器人底盘41、驱动轮42、驱动轮43、驱动电机44、驱动电机45，控制器46，电源设备47，其中驱动电机44包括转子441和定子442，驱动电机45包括转子451和定子452。图4中：驱动轮42与机器人底盘41通过驱动电机44连接，驱动电机44的转子441与驱动轮42固接，驱动电机44的定子442与机器人底盘41固接；驱动轮43与机器人底盘41通过驱动电机45连接，驱动电机45的转子451与驱动轮43固接，驱动电机45的定子与机器人底盘41固接；控制器46固定在机器人底盘41上，与外界定位系统进行通信，实时获取路线信息和机器人的位置和方位角信息，根据获得的目标路线信息和机器人的位置和方位角计算驱动轮42和驱动轮43运动速度，并将速度指令发送给驱动电机44和驱动电机45，然后驱动电机44和驱动电机45执行接收到的速度指令；电源设备47固定在机器人底盘41上，给驱动电机44和驱动电机45及控制器46供电。

发明内容

本发明的目的是为克服已有技术的不足之处，提出一种非时间参考的差速驱动机器人路线跟踪控制方法，本方法便于驱动电机实现计算出的车轮行驶速度，易实现控制机器人沿目标路线行驶。

本发明的方法包括：

为实现上述机器人路线跟踪问题，本发明采用的技术方案为：

一种非时间参考的差速驱动机器人路线跟踪控制方法，其特征在于，包括以下步骤：

1)对全局目标路线进行分析：在全局目标路线相对应的全局坐标系中，若该全局目标路线的X轴坐标值单调变化，机器人控制器直接切换到路线跟踪控制状态，直接进入步骤2)，否则将全局目标路线划分为多个局部路线，并建立各局部路线相对应的局部坐标系，使局部路线的X轴坐标在该局部坐标系内单调变化，机器人控制器切换到路线跟踪控制，进入步骤2)；

2)若在当前坐标系中，当前目标路线的X轴坐标若单调递增，则进入步骤3)，若单调递减，则进入步骤4)；

3)实时获取目标路线信息及机器人位置和方位角信息，采用非时间参考的差速驱动机器人路线跟踪驱动轮行驶速度计算方法,如公式(1)得到的速度指令控制机器人的速度前进，当机器人已到当前目标路线终点，则进入步骤5)；

$\left\{\begin{array}{c}{v}_r=v_o+\frac{v_o{\mathrm{Dcos}}^3\mathrm{\θ}}{2}(\frac{{\mathrm{\ρ}}_r}{{\cos }^3{\mathrm{\θ}}_r}-k_1(y-y_r)-k_2(\tan \mathrm{\θ}-{\mathrm{tan\θ}}_r))\\ v_l=v_o-\frac{v_o{\mathrm{Dcos}}^3\mathrm{\θ}}{2}(\frac{{\mathrm{\ρ}}_r}{{\cos }^3{\mathrm{\θ}}_r}-k_1(y-y_r)-k_2(\tan \mathrm{\θ}-{\mathrm{tan\θ}}_r))\end{array}\right.---\left(1\right)$

公式(1)中，v_l为机器人控制器计算出的左驱动轮行驶速度指令，v_r为控制器计算出的右驱动轮行驶速度指令；v_o为使用者预先设定的该机器人o点的行驶速度，o点为机器人的左驱动轮与右驱动轮的轮心连线中点；D为机器人的左驱动轮与右驱动轮的轮心距离；x为机器人o点在当前坐标系的X轴坐标；y为机器人o点在当前坐标系的Y轴上坐标；θ是机器人方位角，为逆时针方向上，机器人所在的当前坐标系X轴到机器人行驶方向的夹角；记P点为o点沿当前坐标系Y轴方向在当前目标路线上的投影；y_r是P点在当前坐标系Y轴上的坐标；θ_r是当前目标路线在P点处的切线沿顺时针方向到当前坐标系X轴的夹角；ρ_r是当前目标路线在P点处的曲率；k₁为距离偏差增益，k₁＞0；k₂为方位角偏差增益，k₂＞0；

4)实时获取目标路线信息及机器人位置和方位角信息，采用非时间参考的差速驱动机器人路线跟踪驱动轮行驶速度计算方法,如公式(2)得到的速度指令控制机器人速度，当机器人到达当前目标路线终点，则进入步骤5)；

$\left\{\begin{array}{c}{v}_r=v_o+\frac{v_o{\mathrm{Dcos}}^3\mathrm{\θ}}{2}(\frac{{\mathrm{\ρ}}_r}{{\cos }^3{\mathrm{\θ}}_r}+k_3(y-y_r)-k_4(\tan \mathrm{\θ}-{\mathrm{tan\θ}}_r))\\ v_l=v_o-\frac{v_o{\mathrm{Dcos}}^3\mathrm{\θ}}{2}(\frac{{\mathrm{\ρ}}_r}{{\cos }^3{\mathrm{\θ}}_r}+k_3(y-y_r)-k_4(\tan \mathrm{\θ}-{\mathrm{tan\θ}}_r))\end{array}\right.---\left(2\right)$

公式(2)中，v_l为机器人控制器计算出的左驱动轮行驶速度指令；v_r为控制器计算出的右驱动轮行驶速度指令；v_o为使用者预先设定的该机器人o点的行驶速度，o点为机器人的左驱动轮与右驱动轮的轮心连线中点；D为机器人左驱动轮与右驱动轮轮心距离；x为机器人o点在当前坐标系X轴上的坐标；y为机器人o点在当前坐标系Y轴上的坐标；θ是机器人方位角，为逆时针方向上，机器人所在的当前坐标系X轴到机器人行驶方向的夹角；记P点为o点沿当前坐标系Y轴方向在当前目标路线上的投影；y_r是P点在当前坐标系Y轴上的坐标；θ_r是当前目标路线在P点处的切线沿顺时针方向到当前坐标系X轴的夹角；ρ_r为当前目标路线在P点处的曲率；k₃为距离偏差增益，k₃＞0，k₄为方位角偏差增益，k₄＞0；

5)若机器人达到全局路线终点，转步骤7)，否则，进入步骤6)；

6)进入下一个局部坐标系，转步骤2)；

7)路线跟踪控制结束。

本发明的特点及有益效果：

该方法在非时间区域中设计轮速控制律，设计出的车轮行驶速度与时间因素无关，摆脱了时间对速度的约束，便于驱动电机实现计算出的车轮行驶速度，易实现控制机器人沿目标路线行驶。

附图说明

图1是三轮机器人结构示意图。

图2是四轮机器人机构示意图。

图3是六轮机器人结构示意图。

图4是结构简化后的两轮机器人结构示意图。

图5是本发明的两轮机器人路线跟踪控制方法实施流程示意图。

图6是本实施例中任意路线局部坐标系设置方法示例图。

图7是本实施例在当前坐标系中目标路线的X轴坐标值递增时，路线跟踪示意图。

图8是本实施例在当前坐标系中目标路线的X轴坐标值递减时，路线跟踪示意图。

图9是本实施例在当前坐标系中目标路线的X轴坐标值递增时，机器人的路线跟随效果。

图10是本实施例在当前坐标系中目标路线的X轴坐标值递增时，机器人的行驶速度。

图11是本实施例在当前坐标系中目标路线的X轴坐标值递减时，机器人的路线跟随效果。

图12是本实施例在当前坐标系中目标路线的X轴坐标值递减时，机器人的行驶速度。

具体实施方式

本发明提出的一种非时间参考的差速驱动机器人路线跟踪控制方法结合附图及实施实例说明如下：

本发明提出的一种非时间参考的差速驱动机器人路线跟踪控制方法及用于两轮机器人路线跟踪控制的实施例，两轮机器人路线跟踪控制器的工作流程如图5所示，包括以下步骤：

1)对全局目标路线进行分析：在全局目标路线相对应的全局坐标系中，若该全局目标路线的X轴坐标值单调变化(即单调递增或单调递减)，机器人控制器直接切换到路线跟踪控制状态，直接进入步骤2)，否则将全局目标路线划分为多个局部路线，并建立各局部路线相对应的局部坐标系，使局部路线的X轴坐标在该局部坐标系内单调变化(即单调递增或单调递减)，机器人控制器切换到路线跟踪控制，进入步骤2)；

本实施例如图6所示，在全局坐标系61中，包括四条目标路线：目标路线62、目标路线63、目标路线64、目标路线65，其中目标路线62的起点为O₆₂₀，终点为O₆₂₁；目标路线63的起点为O₆₃₀，终点为O₆₃₁；目标路线64的起点为O₆₄₁，终点为O_64x；目标路线65的起点为O₆₅₀，终点为O₆₅₁。

由图6可知，在全局坐标系61中，目标路线62在全局坐标系61中的X轴坐标值沿路线单调递增，目标路线63的X轴在全局坐标系61中的X轴坐标值沿路线单调递减，对这两条目标路线62、63可直接进入步骤2)；图6中的目标路线64与目标路线65在全局坐标系中的X轴坐标不单调变化，在全局坐标系中，建立局部坐标系，使局部路线在所在的局部坐标系中的X轴坐标单调变化(即单调递增，或单调递减)；

如图6所示目标路线64曲线，该路线在全局坐标系61的X轴坐标和Y轴坐标均不单调变化，因此在目标路线64上选取拐点O₆₄₁、O₆₄₂、O₆₄₃、O₆₄₄、O₆₄₅、O₆₄₆，此时目标路线64可看作由局部路线O₆₄₁O₆₄₂、局部路线O₆₄₂O₆₄₃、局部路线O₆₄₃O₆₄₄、局部路线O₆₄₄O₆₄₅、局部路线O₆₄₅O₆₄₆、局部路线O₆₄₆O_64x依次连接组成。分别在各拐点建立局部坐标系，即在O₆₄₁建立局部坐标系641，在O₆₄₂建立局部坐标系642，在O₆₄₃建立局部坐标系643、在O₆₄₄建立局部坐标系644、在O₆₄₅建立局部坐标系645、在O₆₄₆建立局部坐标系646。此时，就实现了目标路线64的局部路线在所在的局部坐标系的X轴坐标值单调变化。机器人控制器将控制机器人依次在局部坐标系641中跟踪局部目标路线O₆₄₁O₆₄₂，在局部坐标系642中跟踪局部目标路线O₆₄₂O₆₄₃，在局部坐标系643中跟踪局部目标路线O₆₄₃O₆₄₄，在局部坐标系644中跟踪局部目标路线O₆₄₄O₆₄₅，在局部坐标系645中跟踪局部目标路线O₆₄₅O₆₄₆、在局部坐标系646中跟踪局部路标路线O₆₄₆O_64x。

若目标路线在全局坐标系中的X轴坐标值非单调变化，在全局坐标系中的Y轴坐标值单调变化，如本实施例图6所示目标路线65，在全局坐标系61中将X、Y坐标轴互换，将全局坐标系61中坐标原点O移到目标路线65的起点O₆₅₀后转换成新的全局坐标系650，使目标路线65在全局坐标系650中的X轴坐标单调变化。

2)若在当前坐标系中(即，机器人此时要跟踪的全局目标路线所在的全局坐标系，或者此时要跟踪的局部目标路线所在的局部坐标系)，当前目标路线(即，机器人此时要跟踪的满足在全局坐标系中X轴坐标单调变化的全局目标路线，或机器人此时要跟踪的满足在所在局部坐标系中单调变化的局部目标路线)的X轴坐标若单调递增，则进入步骤3)，若单调递减，则进入步骤4)；

3)实时获取目标路线信息及机器人位置和方位角信息，采用非时间参考的差速驱动机器人路线跟踪驱动轮行驶速度计算方法,如公式(1)所示，得到的速度指令控制机器人的速度前进，当机器人已到当前目标路线终点，则进入步骤5)；

$\left\{\begin{array}{c}{v}_r=v_o+\frac{v_o{\mathrm{Dcos}}^3\mathrm{\θ}}{2}(\frac{{\mathrm{\ρ}}_r}{{\cos }^3{\mathrm{\θ}}_r}-k_1(y-y_r)-k_2(\tan \mathrm{\θ}-{\mathrm{tan\θ}}_r))\\ v_l=v_o-\frac{v_o{\mathrm{Dcos}}^3\mathrm{\θ}}{2}(\frac{{\mathrm{\ρ}}_r}{{\cos }^3{\mathrm{\θ}}_r}-k_1(y-y_r)-k_2(\tan \mathrm{\θ}-{\mathrm{tan\θ}}_r))\end{array}\right.---\left(1\right)$

公式(1)中，v_l为机器人控制器计算出的左驱动轮行驶速度指令，v_r为控制器计算出的右驱动轮行驶速度指令；v_o为使用者预先设定的该机器人o点的行驶速度，o点为机器人的左驱动轮与右驱动轮的轮心连线中点；D为机器人的左驱动轮与右驱动轮的轮心距离；x为机器人的o点在当前坐标系X轴的坐标；y为机器人的o点在当前坐标系Y轴上的坐标；θ是机器人方位角，为沿逆时针方向上，机器人所在的当前坐标系X轴到机器人行驶方向的夹角；记P点为o点沿当前坐标系Y轴方向在当前目标路线上的投影；y_r是P点在当前坐标系Y轴上的坐标；θ_r是当前目标路线在P点处的切线沿顺时针方向到当前坐标系X轴的夹角；ρ_r为当前目标路线在P点处的曲率；k₁为距离偏差增益，k₁＞0，k₂为方位角偏差增益，k₂＞0；

本实施例步骤3)中，机器人控制器与外界路线检测系统和机器人定位系统实时通信，实时获取当前目标路线信息及机器人位置和方位角信息，并采用公式(1)计算驱动轮的行驶速度，并将速度指令信息发送给车轮驱动电机，驱动电机执行接收到的速度指令。

当当前目标路线在当前坐标系X轴的坐标值单调递增时，路线跟踪如图7所示。图7中：o点为机器人72左驱动轮73与右驱动轮74的轮心连线中点；x、y为o点分别在当前坐标系71的X轴坐标和Y轴坐标；θ是机器人方位角，为沿逆时针方向上，当前坐标系71的X轴到机器人72行驶方向的夹角；曲线78为当前目标路线，A点为当前目标路线78的起点，B点为当前目标路线78的终点；P点79为o点沿当前坐标系71Y轴方向在当前目标路线78上的投影；x_r、y_r分别为P点79在当前坐标系71中的X轴坐标、Y轴坐标；θ_r为当前目标路线78在P点79的处切线沿顺时针方向到当前坐标系71X轴的夹角；ρ_r为当前目标路线78在P点79处的曲率。

公式(1)中，v_l为控制器77计算出的左驱动轮73行驶速度，控制器77将该速度指令发送给驱动电机75，驱动电机75执行该速度指令；v_r为控制器77计算出的右驱动轮74行驶速度，控制器77将该速度指令发送给驱动电机76，驱动电机76执行该速度指令；v_o为使用者设定的机器人72的行驶速度，即机器人72的o点运动速度，o点为机器人72左驱动轮73与右驱动轮74的轮心连线中心；D为机器人72的左驱动轮73与右驱动轮74的轮心距离；x为机器人72的o点在当前坐标系71X轴的坐标；y为机器人72的o点在当前坐标系71Y轴上的坐标；θ是机器人方位角，为逆时针方向上，机器人72所在的当前坐标系71X轴到机器人72行驶方向的夹角；P点79为o点沿当前坐标系71Y轴方向在当前目标路线78上的投影；y_r为P点79在当前坐标系Y轴上的坐标；θ_r为当前目标路线78在P点79处的切线沿顺时针方向到当前坐标系71X轴的夹角；ρ_r为当前目标路线78在P点79处的曲率；k₁为距离偏差增益，k₁＞0，根据经验建议k₁的取值范围为[0.1，30]；k₂为方位角偏差增益，k₂＞0，根据经验建议k₂的取值范围为[0.1，50]。

4)实时获取目标路线信息及机器人位置和方位角信息，采用非时间参考的差速驱动机器人路线跟踪驱动轮行驶速度计算方法(如公式(2)所示)，得到的速度指令控制机器人速度，当机器人到达当前目标路线终点，则进入步骤5)；

$\left\{\begin{array}{c}{v}_r=v_o+\frac{v_o{\mathrm{Dcos}}^3\mathrm{\θ}}{2}(\frac{{\mathrm{\ρ}}_r}{{\cos }^3{\mathrm{\θ}}_r}+k_3(y-y_r)-k_4(\tan \mathrm{\θ}-{\mathrm{tan\θ}}_r))\\ v_l=v_o-\frac{v_o{\mathrm{Dcos}}^3\mathrm{\θ}}{2}(\frac{{\mathrm{\ρ}}_r}{{\cos }^3{\mathrm{\θ}}_r}+k_3(y-y_r)-k_4(\tan \mathrm{\θ}-{\mathrm{tan\θ}}_r))\end{array}\right.---\left(2\right)$

公式(2)中，v_l为机器人控制器计算出的左驱动轮行驶速度指令；v_r为控制器计算出的右驱动轮行驶速度指令；v_o为使用者预先设定的该机器人o点的行驶速度，o点为机器人的左驱动轮与右驱动轮的轮心连线中点；D为机器人左驱动轮与右驱动轮轮心距离；x为机器人o点在当前坐标系X轴上的坐标；y为机器人o点在当前坐标系Y轴上的坐标；θ是机器人方位角，为沿逆时针方向上，机器人所在的当前坐标系X轴到机器人行驶方向的夹角；记P点为o点沿当前坐标系Y轴方向在当前目标路线上的投影；y_r为P点在当前坐标系Y轴上的坐标；θ_r为当前目标路线在P点处的切线与当前坐标系X轴的夹角；ρ_r为当前目标路线在P点处的曲率；k₃为距离偏差增益，k₃＞0，k₄为方位角偏差增益，k₄＞0。

本实施例中步骤4)中，机器人控制器与外界路线检测系统和机器人定位系统实时通信，实时获取当前目标路线信息及机器人位置和方位角信息，并采用公式(2)计算驱动轮的运动速度，并将速度指令信息发送给车轮驱动电机，驱动电机执行接收到的速度指令。

当当前目标路线在当前局部坐标系X轴的坐标值单调递减时，路线跟踪示意图如图8所示。图8中：o点为机器人82的左驱动轮83与右驱动轮84轮心连线中点；x、y为o点分别在当前坐标系81中的X轴坐标、Y轴坐标；θ是机器人方位角，为沿逆时针方向，当前坐标系81的X轴到机器人82行驶方向的夹角；曲线88为当前目标路线，A点为当前目标路线88的起点，B点为当前目标路线88的终点；P点为o点沿当前坐标系81Y轴方向在当前目标路线88上的投影；x_r为P点89在当前坐标系81中的X轴坐标；y_r为P点89在当前坐标系81中的Y轴坐标；θ_r为当前目标路线88在P点89处的切线沿顺时针方向到当前坐标系81X轴夹角；ρ_r为当前目标路线88在P点89处的曲率。

公式(2)中，v_l为控制器87计算出的机器人82的左驱动轮83行驶速度，控制器87将该速度指令发送至驱动电机85，由驱动电机85执行该速度指令；v_r为控制器87计算出的右驱动轮84行驶速度，控制器87将该速度指令发送至驱动电机86，驱动电机86执行该速度指令；v_o为使用者设定的机器人82行驶速度，即机器人82o点的行驶速度，o点为机器人82左驱动轮83与右驱动轮84轮心连线中点；D为机器人82左驱动轮83与右驱动轮84轮心距离；x为机器人82的o点在X轴81上的坐标；y为机器人82的o点在当前坐标系81Y轴上的坐标；θ是机器人82方位角，为逆时针方向上，机器人82所在的当前坐标系81X轴到机器人82行驶方向的夹角；P点为o点沿当前坐标系81Y轴方向在当前目标路线88上的投影；y_r为图8所示P点89在当前坐标系81Y轴上的坐标；θ_r为图8所示当前目标路线88在P点89处的切线与当前坐标系81X轴的夹角；ρ_r为当前目标路线88在P点89处的曲率；k₃为距离偏差增益，k₃＞0，根据经验建议k₃的取值范围为[0.1，30]；k₄为方位角偏差增益，k₄＞0，根据经验建议k₄的取值范围为[0.1，50]。

5)若机器人达到全局路线终点，转步骤7)，否则，进入步骤6)；

6)进入下一个局部坐标系，转步骤2)；

7)路线跟踪控制结束。

上述方法同样可用于三轮、四轮、六轮机器人路线跟踪控制。用于如图1所示三轮机器人时，控制器17只需采用上述方法计算驱动轮12和驱动轮13的行驶速度，并将速度指令分别发送给驱动轮12的驱动电机14和驱动轮13的驱动电机15，驱动电机14与驱动电机15执行接收到的速度指令，完成该三轮机器人的路线跟踪任务，对万向轮18不做任何控制。用于如图2所示的四轮机器人，控制器27只需采用上述方法计算驱动轮22和驱动轮23的行驶速度，并将速度指令分别发送给驱动轮22的驱动电机24和驱动轮23的驱动电机25，驱动电机24与驱动电机25执行接收到的速度指令，完成该四轮机器人的路线跟踪任务，对万向轮281、万向轮282不做任何控制。用于如图3中所示的六轮机器人，控制器37只需采用上述流程计算驱动轮32和驱动轮33的行驶速度，并将速度指令分别发给驱动轮32的驱动电机34和驱动轮33的驱动电机35，驱动电机34和驱动电机35执行接收的速度指令，完成六轮机器人的路线跟踪任务，对万向轮381、万向轮382、万向轮383、万向轮384不做任何控制。

以一种正弦曲线为目标路线采用上述路线跟踪控制方法进行路线跟踪，跟踪结果如图9和图11所示。

图9中：坐标系90为机器人91所在的当前坐标系；在当前坐标系90中，机器人91的o点在初始位置处的X轴坐标为0米，Y轴坐标为5米，机器人91在初始位置处的方位角为0度，o点为机器人91左驱动轮92和右驱动轮93轮心连线中点；实线98表示预先设置的目标路线，该实线98的X轴坐标值在当前坐标系91中单调递增；虚线99表示采用本方法机器人91o点在当前坐标系90中的实际路线。机器人91包括：左驱动轮92、右驱动轮93、左轮驱动电机94、右轮驱动电机95、控制器96、机器人底盘97，其中左驱动轮92通过左轮驱动电机94与机器人底盘97连接，右驱动轮93通过右轮驱动电机95与机器人底盘97连接，控制器96固定在机器人底盘97上。由图9可看出本方法的机器人路线跟踪效果非常理想，即使机器人91起初不在目标路线98上，控制器96采用上述路线跟踪控制方法使机器人91靠近目标路线98并沿目标路线98运动(图9中，机器人91在X轴坐标为16m时跟踪到目标路线98上，然后沿目标路线98运动)。图9所示机器人91跟踪当前目标路线98过程中，左驱动轮92运动速度、右驱动轮93运动速度及o点运动速度随时间变化如图10所示。图10中：横轴101为时间轴，单位为秒；纵轴102为速度轴，单位为m.s^-1；实线103为图9所示机器人91的o点运动速度随时间变化曲线；点虚线104为图9所示机器人91左驱动轮92行驶速度随时间变化曲线；点划线105为图9所示机器人91右驱动轮93行驶速度随时间变化曲线。

图11中：坐标系110为机器人111所在的当前坐标系；在当前坐标系110中，机器人111的o点在初始位置处的X轴坐标为125.66米，Y轴坐标为5米，机器人在初始位置处的方位角为180度，o点为机器人111左驱动轮112和右驱动轮113轮心连线中点；实线118表示预先设置的目标路线，该目标路线118的X轴坐标值在当前坐标系110中单调递减；虚线119表示采用本方法机器人111o点在当前坐标系110中的实际路线。机器人111包括：左驱动轮112、右驱动轮113、左轮驱动电机114、右轮驱动电机115、控制器116、机器人底盘117，其中左驱动轮112通过左轮驱动电机114与机器人底盘117连接，右驱动轮113通过右轮驱动电机115与机器人底盘117连接，控制器116固定在机器人底盘117上。由图11可看出本方法的机器人路线跟踪效果非常理想，即使机器人111起初不在目标路线118上，控制器116采用上述机器人路线跟踪方法使机器人111靠近目标路线118并沿目标路线118运动(图9中，机器人111在X轴坐标为116m时跟踪到目标路线118上，然后沿目标路线118运动)。图11所示机器人111跟踪当前目标路线118过程中，左驱动轮112运动速度、右驱动轮113运动速度及o点运动速度随时间变化如图12所示。图12中：横轴121为时间轴，单位为秒；纵轴122为速度轴，单位为m.s^-1；实线123为图11所示机器人111的o点运动速度随时间变化曲线；点虚线124为图11所示机器人111左驱动轮112行驶速度随时间变化曲线；点划线125为图11所示机器人111右驱动轮113行驶速度随时间变化曲线。

由图9和图11可知，不论目标路线的X轴坐标在所在坐标系中单调递增还是单调递减，机器人控制器都可以采用上述方法控制机器人沿目标路线运动。

Claims (1)

1.一种非时间参考的差速驱动机器人路线跟踪控制方法，用于两轮机器人路线跟踪控制，其特征在于，该方法包括以下步骤：

1)对全局目标路线进行分析：在全局目标路线相对应的全局坐标系中，若该全局目标路线的X轴坐标值单调变化，机器人控制器直接切换到路线跟踪控制状态，直接进入步骤2)，否则将全局目标路线划分为多个局部路线，并建立各局部路线相对应的局部坐标系，使局部路线的X轴坐标在该局部坐标系内单调变化，机器人控制器切换到路线跟踪控制，进入步骤2)；

2)若在当前坐标系中，当前目标路线的X轴坐标若单调递增，则进入步骤3)，若单调递减，则进入步骤4)；

3)实时获取目标路线信息及机器人位置和方位角信息，采用非时间参考的差速驱动机器人路线跟踪驱动轮行驶速度计算方法,如公式(1)，得到的速度指令控制机器人的速度前进，当机器人已到当前目标路线终点，则进入步骤5)；

$\left\{\begin{array}{c}{v}_r=v_o+\frac{v_o{\mathrm{Dcos}}^3\mathrm{\θ}}{2}(\frac{{\mathrm{\ρ}}_r}{{\cos }^3{\mathrm{\θ}}_r}-k_1(y-y_r)-k_2(tan\mathrm{\θ}-{\mathrm{tan\θ}}_r))\\ v_l=v_o-\frac{v_o{\mathrm{Dcos}}^3\mathrm{\θ}}{2}(\frac{{\mathrm{\ρ}}_r}{{\cos }^3{\mathrm{\θ}}_r}-k_1(y-y_r)-k_2(tan\mathrm{\θ}-{\mathrm{tan\θ}}_r))\end{array}\right.---\left(1\right)$

公式(1)中，v_l为机器人控制器计算出的左驱动轮行驶速度指令，v_r为控制器计算出的右驱动轮行驶速度指令；v_o为使用者预先设定的该机器人o点的行驶速度，o点为机器人的左驱动轮与右驱动轮的轮心连线中点；D为机器人左驱动轮与右驱动轮的轮心距离；x为机器人的o点在当前坐标系X轴的坐标；y为机器人o点在当前坐标系Y轴上的坐标；θ是机器人方位角，为逆时针方向上，机器人所在的当前坐标系X轴到机器人行驶方向的夹角；记P点为o点沿当前坐标系Y轴方向在当前目标路线上的投影；y_r是P点在当前坐标系Y轴上的坐标；θ_r是当前目标路线在P点处的切线沿顺时针方向到当前坐标系X轴的夹角；ρ_r是当前目标路线在P点处的曲率；k₁为距离偏差增益，k₁＞0，k₂为方位角偏差增益，k₂＞0；

4)实时获取目标路线信息及机器人位置和方位角信息，采用采用非时间参考的差速驱动机器人路线跟踪驱动轮行驶速度计算方法，如公式(2)得到的速度指令控制机器人速度，当机器人到达当前目标路线终点，则进入步骤5)；

$\left\{\begin{array}{c}{v}_r=v_o+\frac{v_o{\mathrm{Dcos}}^3\mathrm{\θ}}{2}(\frac{{\mathrm{\ρ}}_r}{{\cos }^3{\mathrm{\θ}}_r}+k_3(y-y_r)-k_4(tan\mathrm{\θ}-{\mathrm{tan\θ}}_r))\\ v_l=v_o-\frac{v_o{\mathrm{Dcos}}^3\mathrm{\θ}}{2}(\frac{{\mathrm{\ρ}}_r}{{\cos }^3{\mathrm{\θ}}_r}+k_3(y-y_r)-k_4(tan\mathrm{\θ}-{\mathrm{tan\θ}}_r))\end{array}\right.---\left(2\right)$

公式(2)中，v_l为机器人控制器计算出的机器人的左驱动轮行驶速度指令；v_r为控制器计算出的右驱动轮行驶速度指令；v_o为使用者预先设定的该机器人o点的行驶速度，o点为机器人左驱动轮与右驱动轮的轮心连线中点；D为机器人左驱动轮与右驱动轮轮心距离；x为机器人的o点在当前坐标系X轴上的坐标；y为机器人的o点在当前坐标系Y轴上的坐标；θ是机器人方位角，为逆时针方向上，机器人所在的当前坐标系X轴到机器人行驶方向的夹角；记P点为o点沿当前坐标系Y轴方向在当前目标路线上的投影；y_r是P点在当前坐标系Y轴上的坐标；θ_r是当前目标路线在P点处的切线沿顺时针方向到当前坐标系X轴的夹角；ρ_r是当前目标路线在P点处的曲率，k₃为距离偏差增益，k₃＞0，k₄为方位角偏差增益，k₄＞0；

5)若机器人达到全局路线终点，转步骤7)，否则，进入步骤6)；

6)进入下一个局部坐标系，转步骤2)；

7)路线跟踪控制结束。