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

Abstract

本发明涉及一种非时间参考的差速驱动机器人定点跟踪控制方法，属于差速驱动机器人的定点跟踪控制技术领域，该方法包括：控制器与外界监测系统实时通信，获取目标点信息；首先，根据机器人航向角与目标点处目标航向角的差值，选择机器人自转控制模式；接着，根据机器人与目标点的相对距离判断机器人定点跟踪控制模式；然后，根据机器人位置和航向角及目标点位置和目标点处目标航向角，计算车轮行驶速度并由驱动电机执行；若机器人到达最终目标点，则控制结束，否则跟踪一下目标点。本方法不需要规划路径，设计的车速控制律与时间因素无关，便于驱动电机实现计算出的车轮行驶速度，易实现机器人跟踪单个目标点和连续依次跟踪多个目标点。

Description

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

技术领域

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

背景技术

差速驱动机器人是通过2个驱动电机控制两侧车轮行驶速度，实现直线或曲线运动的轮式机器人，因结构简单、传动效率高和控制灵活被广泛使用。随着轮式机器人的智能化程度不断提高，这种智能机器人已被用在多个领域，如生产车间货物运输、危险环境勘察、室内清洁等。

机器人的物品运输、室内清洁等任务，可通过控制机器人跟踪单个目标点或依次跟踪多个目标点最终到达指定目的地来完成。移动机器人定点跟踪控制问题是机器人位置控制的基本问题，是机器人运动控制研究课题之一，目前机器人定点跟踪控制方法有误差比例法、路径规划与路径跟踪法、轨迹规划与轨迹跟踪法。误差比例控制法简单地将机器人与目标点的位置偏差和航向角偏差乘以一个比例系数，来计算驱动轮行驶速度，不适用于差速驱动机器人这种强非线性运动系统。通过规划机器人起点到目标点的路径然后采用路径跟踪实现机器人定点跟踪的方法，需要设计机器人行驶路径和路径跟踪控制方法。轨迹规划与轨迹跟踪法通过设计轮速随时间变化函数，实现机器人定点跟踪的方法，对车轮速度控制精度要求较高，当驱动电机出现速度控制误差时，导致机器人不能到达目标点，导致定点跟踪失败。

本发明适用的已有轮式机器人包括，图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的定子252固接于机器人底盘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)机器人的控制器启动，机器人进入定点跟踪控制状态；

2)控制器与外界监测系统实时通信，获取当前目标点P的位置和当前目标点P处目标航向角θ_r的信息，若机器人航向角θ与当前目标点P处目标航向角θ_r之差的绝对值小于90°或大于270°，则进入步骤10)，否则进入步骤3)；

3)若机器人航向角θ与当前目标点P处目标航向角θ_r之差的绝对值大于等于90°且小于等于180°，则进入步骤4)；若机器人航向角θ与当前目标点P处目标航向角θ_r之差的绝对值大于180°且小于等于270°，则进入步骤5)；

4)若机器人航向角θ大于当前目标点P处目标航向角θ_r，则进入步骤6)，否则，进入步骤7)；

5)若机器人航向角θ大于当前目标点P处目标航向角θ_r，则进入步骤8)，否则，进入步骤9)；

6)控制器控制机器人顺时针自转，根据公式(1)计算机器人左驱动轮、右驱动轮速度，同时，实时与外界监测系统通信，获取并计算机器人航向角θ与目标点P处目标航向角θ_r之差；控制器控制机器人自转过程中，当机器人航向角θ与当前目标点P处目标航向角θ_r之差小于90°时，进入步骤10)；

公式(1)中：v_r为机器人右驱动轮速度，若v_r＞0，右驱动轮相对机器人底盘前进行驶，若v_r＜0，右驱动轮相对机器人底盘后退行驶；v_l为机器人左驱动轮速度，若v_l＞0，左驱动轮相对机器人底盘前进行驶，若v_l＜0，左驱动轮相对机器人底盘后退行驶；v_o为用户设定的机器人运动速度，v_o≥0；k_s为机器人速度比例系数；

7)控制器控制机器人逆时针自转,根据公式(2)计算机器人左驱动轮、右驱动轮速度，同时，实时与外界监测系统通信，获取并计算机器人航向角与目标点处目标航向角之差；控制器控制机器人自转过程中，若当前目标点P处目标航向角θ_r与机器人航向角θ之差小于90°，进入步骤10)；

8)控制器控制机器人逆时针自转，根据公式(2)计算机器人左驱动轮、右驱动轮行驶速度，同时，实时与外界监测系统通信，获取并计算机器人航向角θ与目标点P处目标航向角θ_r之差；控制器控制机器人自转过程中，当机器人航向角θ与当前目标点P处目标航向角θ_r之差大于270°时，进入步骤10)；

9)控制器控制机器人顺时针自转，根据公式(1)计算机器人左驱动轮、右驱动行驶速度，同时，实时与外界监测系统通信，获取并计算机器人航向角θ与目标点P处目标航向角θ_r之差；控制器控制机器人自转过程中，若当前目标点P处目标航向角θ_r与机器人航向角θ之差大于270°时，进入步骤10)；

10)计算在当前目标点P处目标航向角θ_r方向上，机器人与当前目标点P的距离L，计算公式如公式(3)所示，若L＜0，则进入步骤12)；若L＞0，则进入步骤13)；若L＝0且当前目标点P与机器人两驱动轮轮心连线中点o不重合，则进入步骤11)，若L＝0且当前目标点P与机器人两驱动轮轮心连线中点o重合，则进入步骤15)；

L＝(x-x_r)cosθ_r+(y-y_r)sinθ_r (3)

公式(3)中，L为在当前目标点P处目标航向角θ_r方向上，机器人与当前目标点P的距离，x为机器人的X轴坐标，y为机器人的Y轴坐标，x_r为当前目标点P的X轴坐标，y_r为当前目标点P的Y轴坐标，θ_r为当前目标点P处目标航向角；

11)控制器采用公式(4)计算机器人左驱动轮、右驱动轮的行驶速度，控制机器人沿当前目标航向角θ直线前进行驶一段距离S,S>0，然后返回步骤10)；

12)当步骤(10)中采用公式(3)计算的L＜0时，控制器采用前进式定点跟踪控制方法，即机器人前进行驶跟踪目标点的方法，根据公式(5)计算机器人左驱动轮、右驱动轮速度，控制机器人跟踪当前目标点P；同时，控制器采用公式(3)，实时计算在当前目标点P处目标航向角θ_r方向上，机器人与当前目标点P的距离L，若L＜0，则继续执行步骤12)，若L≥0，则进入步骤14)；

公式(5)中：D为机器人左驱动轮与右驱动轮的轮心距离；θ是机器人航向角；k₁为位置偏差增益，k₁＞0，k₂为航向角偏差增益，k₂＞0；

13)当步骤(10)中采用公式(3)计算的L＞0时，控制器采用后退式定点跟踪控制方法，即机器人后退行驶跟踪目标点的方法，根据公式(6)计算机器人左驱动轮、右驱动行驶速度，控制机器人跟踪当前目标点P；同时，控制器采用公式(3)，实时计算当前目标点P处目标航向角θ_r方向上，机器人与当前目标点P的距离L，若L＞0，则继续执行步骤13)，若L≤0，则进入步骤14)；

14)若机器人到达当前目标点P，则进入步骤15)，否则，返回步骤10)；

15)若当前目标点P为最终目标点，则进入步骤17)，否则，进入步骤16)；

16)选取下一目标点为新的当前目标点，重新返回步骤2)；

17)机器人到达最终目标点，定点跟踪控制结束。

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

该方法在非时间区域中，设计用于差速机器人定点跟踪的轮速控制律，设计出的车轮行驶速度与时间因素无关，摆脱了时间对速度的约束，便于驱动电机实现计算出的车轮行驶速度；定点跟踪过程中，不需要规划机器人运动路径，便于实现机器人单个定点跟踪和连续依次对多个定点跟踪。

附图说明

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

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

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

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

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

图6是本实施例中前进式定点跟踪示意图。

图7是本实施例中后退式定点跟踪示意图。

图8是本实施例机器人多定点连续跟踪效果示意图。

具体实施方式

本发明提出的一种非时间参考的差速驱动机器人定点跟踪控制方法，下面结合附图及具体实施例进一步说明如下：

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

1)机器人的控制器启动，机器人进入定点跟踪控制状态；

2)控制器与外界监测系统实时通信，获取当前目标点P的位置和当前目标点P处目标航向角θ_r的信息，若机器人航向角θ与当前目标点P处目标航向角θ_r之差的绝对值小于90°或大于270°，则进入步骤10)，否则进入步骤3)；

3)若机器人航向角θ与当前目标点P处目标航向角θ_r之差的绝对值大于等于90°且小于等于180°，则进入步骤4)；若机器人航向角θ与当前目标点P处目标航向角θ_r之差的绝对值大于180°且小于等于270°，则进入步骤5)；

4)若机器人航向角θ大于当前目标点P处目标航向角θ_r，则进入步骤6)，否则，进入步骤7)；

5)若机器人航向角θ大于当前目标点P处目标航向角θ_r，则进入步骤8)，否则，进入步骤9)；

6)控制器控制机器人顺时针自转，根据公式(1)计算机器人左驱动轮、右驱动轮速度，同时，实时与外界监测系统通信，获取并计算机器人航向角θ与目标点P处目标航向角θ_r之差；控制器控制机器人自转过程中，当机器人航向角θ与当前目标点P处目标航向角θ_r之差小于90°时，进入步骤10)；

公式(1)中：v_r为机器人右驱动轮速度，若v_r＞0，右驱动轮相对机器人底盘前进行驶，若v_r＜0，右驱动轮相对机器人底盘后退行驶；v_l为机器人左驱动轮速度，若v_l＞0，左驱动轮相对机器人底盘前进行驶，若v_l＜0，左驱动轮相对机器人底盘后退行驶；v_o为用户设定的机器人运动速度，v_o≥0；k_s为机器人速度比例系数，建议k_s的取值范围为(0,1]；

7)控制器控制机器人逆时针自转,根据公式(2)计算机器人左驱动轮、右驱动轮速度，同时，实时与外界监测系统通信，获取并计算机器人航向角与目标点处目标航向角之差；控制器控制机器人自转过程中，若当前目标点P处目标航向角θ_r与机器人航向角θ之差小于90°时，进入步骤10)；

公式(2)中各符号定义与公式(1)相同；

8)控制器控制机器人逆时针自转，根据公式(2)计算机器人左驱动轮、右驱动轮行驶速度，同时，实时与外界监测系统通信，获取并计算机器人航向角θ与目标点P处目标航向角θ_r之差；控制器控制机器人自转过程中，当机器人航向角θ与当前目标点P处目标航向角θ_r之差大于270°时，进入步骤10)；

9)控制器控制机器人顺时针自转，根据公式(1)计算机器人左驱动轮、右驱动行驶速度，同时，实时与外界监测系统通信，获取并计算机器人航向角θ与目标点P处目标航向角θ_r之差；控制器控制机器人自转过程中，若当前目标点P处目标航向角θ_r与机器人航向角θ之差大于270°时，进入步骤10)；

10)计算在当前目标点P处目标航向角θ_r方向上，机器人与当前目标点P的距离L，计算公式如公式(3)所示，若L＜0，则进入步骤12)；若L＞0，则进入步骤13)；若L＝0且当前目标点P与机器人两驱动轮轮心连线中点o不重合(如图6中，当前目标点P69在左驱动轮63和右驱动轮64的轮心连线上，但当前目标点P69与两轮心连线中点o不重合的情况；如图7中，当前目标点P79在左驱动轮73和右驱动轮74的轮心连线上，但当前目标点P79与两轮心连线中点o不重合的情况)，则进入步骤11)，若L＝0且当前目标点P与机器人两驱动轮轮心连线中点o重合(如图6中，当前目标点P69在左驱动轮63和右驱动轮64的轮心连线上，且与两轮心连线中点o重合的情况；图7中，当前目标点P79在左驱动轮73和右驱动轮74的轮心连线上，且与两轮心连线中点o重合的情况)，则进入步骤15)；

L＝(x-x_r)cosθ_r+(y-y_r)sinθ_r (3)

公式(3)中，L为在当前目标点P处目标航向角θ_r方向上，机器人与当前目标点P的距离，x为机器人的X轴坐标，y为机器人的Y轴坐标，x_r为当前目标点P的X轴坐标，y_r为当前目标点P的Y轴坐标，θ_r为当前目标点P处目标航向角；

11)控制器采用公式(4)计算机器人左驱动轮、右驱动轮的行驶速度，控制机器人沿当前目标航向角θ直线前进行驶一段距离S，S>0，(建议S≥0.1米)，然后返回步骤10)；

公式(4)中各符号定义同公式(1)、(2)；

12)当步骤(10)中采用公式(3)计算的L＜0时，控制器采用前进式定点跟踪控制方法(即机器人前进行驶跟踪目标点的方法)，根据公式(5)计算机器人左驱动轮、右驱动轮速度，控制机器人跟踪当前目标点P；同时，控制器采用公式(3)，实时计算在当前目标点P处目标航向角θ_r方向上，机器人与当前目标点P的距离L，若L＜0，则继续执行步骤12)，若L≥0，则进入步骤14)；

公式(5)中：D为机器人左驱动轮与右驱动轮的轮心距离；θ是机器人航向角；k₁为位置偏差增益，k₁＞0，k₂为航向角偏差增益，k₂＞0；其他符号定义与公式(1)、(2)、(3)、(4)相同；

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

前进式定点跟踪如图6所示。图6中：o点为机器人62左驱动轮63与右驱动轮64的轮心连线中点；x、y分别为o点在坐标系61的X轴坐标和Y轴坐标；θ是机器人航向角，为沿逆时针方向上，坐标系X轴到机器人62行驶方向的夹角；x_r、y_r分别为当前目标点P点69在坐标系61中的X轴坐标、Y轴坐标；θ_r为当前目标P点69的航向角；

公式(5)中，v_l为控制器67计算出的左驱动轮63行驶速度，控制器67将该速度指令发送给左轮驱动电机65，左轮驱动电机65执行该速度指令；v_r为控制器67计算出的右驱动轮64行驶速度，控制器67将该速度指令发送给右轮驱动电机66，右轮驱动电机66执行该速度指令；v_o为使用者设定的机器人62的行驶速度，即机器人62o点的运动速度，o点为机器人62左驱动轮63与右驱动轮64的轮心连线中心；D为机器人62的左驱动轮63与右驱动轮64的轮心距离；x为机器人62o点的X轴坐标；y为机器人62o点的Y轴坐标；θ是机器人航向角，为逆时针方向上，坐标系61的X轴到机器人62行驶方向的夹角；P点69为当前目标点；x_r是P点69的X轴坐标；y_r为P点69的Y轴坐标；θ_r为当前目标点P点69的航向角；k₁为距离偏差增益，k₁＞0，根据经验建议k₁的取值范围为[0.1，30]；k₂为航向角偏差增益，k₂＞0，根据经验建议k₂的取值范围为[0.1，50]。

13)当步骤(10)中采用公式(3)计算的L＞0时，控制器采用后退式定点跟踪控制方法，即机器人后退行驶跟踪目标点的方法,，根据公式(6)计算机器人左驱动轮、右驱动行驶速度，控制机器人跟踪当前目标点P；同时，控制器采用公式(3)，实时计算当前目标点P处目标航向角θ_r方向上，机器人与当前目标点P的距离L，若L＞0，则继续执行步骤13)，若L≤0，则进入步骤14)；

公式(6)中各符号定义与公式(1)、(2)、(3)、(4)、(5)相同；

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

后退式定点跟踪如图7所示。图7中：o点为机器人72左驱动轮73与右驱动轮74的轮心连线中点；x、y分别为o点在坐标系71的X轴坐标和Y轴坐标；θ是机器人航向角，为沿逆时针方向上，坐标系X轴到机器人72行驶方向的夹角；x_r、y_r分别为当前目标点P点79在坐标系71中的X轴坐标、Y轴坐标；θ_r为当前目标P点79的航向角；

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

14)若机器人到达当前目标点P，则进入步骤15)，否则，返回步骤10)；

15)若当前目标点P为最终目标点，则进入步骤17)，否则，进入步骤16)；

16)选取下一目标点为新的当前目标点，重新返回步骤2)；

17)机器人到达最终目标点，定点跟踪控制结束。

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

选择任意一条路线，选取路线上的一系列点，采用上述定点跟踪控制方法进行跟踪控制，定点跟踪仿真结果如图8所示。图8中，控制器87控制机器人82沿建筑物80围成的道路81到达目的地88；图8中的五角星为控制器87通过与外界监测系统通信获取的道路81中心线上的多个点，控制器87通过控制机器人82依次跟踪这些标示点最终到达目的地88。在跟踪图8所示五角星时，控制器87根据上述定点跟踪控制方法计算左驱动轮83和右驱动轮84的行驶速度，并由左轮驱动电机85和右轮驱动电机86执行计算出的速度，控制机器人依次跟踪图8五角星标示的定点，最终到达目的地88，整个过程中机器人82的实际运动轨迹如图8中实线89所示。由仿真结果图8可知，本发明提出的差速机器人定点跟踪方法，可以实现单个定点跟踪或多个定点连续跟踪，使机器人最终到达目的地88。

Claims (1)

1.一种非时间参考的差速驱动机器人定点跟踪控制方法，该方法包括以下步骤：

1)机器人的控制器启动，机器人进入定点跟踪控制状态；

2)控制器与外界监测系统实时通信，获取当前目标点P的位置和当前目标点P处目标航向角θ_r的信息，若机器人航向角θ与当前目标点P处目标航向角θ_r之差的绝对值小于90°或大于270°，则进入步骤10)，否则进入步骤3)；

3)若机器人航向角θ与当前目标点P处目标航向角θ_r之差的绝对值大于等于90°且小于等于180°，则进入步骤4)；若机器人航向角θ与当前目标点P处目标航向角θ_r之差的绝对值大于180°且小于等于270°，则进入步骤5)；

4)若机器人航向角θ大于当前目标点P处目标航向角θ_r，则进入步骤6)，否则，进入步骤7)；

5)若机器人航向角θ大于当前目标点P处目标航向角θ_r，则进入步骤8)，否则，进入步骤9)；

6)控制器控制机器人顺时针自转，根据公式(1)计算机器人左驱动轮、右驱动轮速度，同时，实时与外界监测系统通信，获取并计算机器人航向角θ与当前目标点P处目标航向角θ_r之差；控制器控制机器人自转过程中，当机器人航向角θ与当前目标点P处目标航向角θ_r之差小于90°时，进入步骤10)；

公式(1)中：v_r为机器人右驱动轮速度，若v_r＞0，右驱动轮相对机器人底盘前进行驶，若v_r＜0，右驱动轮相对机器人底盘后退行驶；v_l为机器人左驱动轮速度，若v_l＞0，左驱动轮相对机器人底盘前进行驶，若v_l＜0，左驱动轮相对机器人底盘后退行驶；v_o为用户设定的机器人运动速度，v_o≥0；k_s为机器人速度比例系数；

7)控制器控制机器人逆时针自转,根据公式(2)计算机器人左驱动轮、右驱动轮速度，同时，实时与外界监测系统通信，获取并计算机器人航向角与目标点处目标航向角之差；控制器控制机器人自转过程中，若当前目标点P处目标航向角θ_r与机器人航向角θ之差小于90°，进入步骤10)；

8)控制器控制机器人逆时针自转，根据公式(2)计算机器人左驱动轮、右驱动轮行驶速度，同时，实时与外界监测系统通信，获取并计算机器人航向角θ与当前目标点P处目标航向角θ_r之差；控制器控制机器人自转过程中，当机器人航向角θ与当前目标点P处目标航向角θ_r之差大于270°时，进入步骤10)；

9)控制器控制机器人顺时针自转，根据公式(1)计算机器人左驱动轮、右驱动行驶速度，同时，实时与外界监测系统通信，获取并计算机器人航向角θ与当前目标点P处目标航向角θ_r之差；控制器控制机器人自转过程中，若当前目标点P处目标航向角θ_r与机器人航向角θ之差大于270°，进入步骤10)；

10)计算在当前目标点P处目标航向角θ_r方向上，机器人与当前目标点P的距离L，计算公式如公式(3)所示，若L＜0，则进入步骤12)；若L＞0，则进入步骤13)；若L＝0且当前目标点P与机器人两驱动轮轮心连线中点o不重合，则进入步骤11)，若L＝0且当前目标点P与机器人两驱动轮轮心连线中点o重合，则进入步骤15)；

L＝(x-x_r)cosθ_r+(y-y_r)sinθ_r (3)

公式(3)中，L为在当前目标点P处目标航向角θ_r方向上，机器人与当前目标点P的距离，x为机器人的X轴坐标，y为机器人的Y轴坐标，x_r为当前目标点P的X轴坐标，y_r为当前目标点P的Y轴坐标，θ_r为当前目标点P处目标航向角；

11)控制器采用公式(4)计算机器人左驱动轮、右驱动轮的行驶速度，控制机器人沿当前目标航向角θ直线前进行驶一段距离S,S>0，然后返回步骤10)；

12)当步骤(10)中采用公式(3)计算的L＜0时，控制器采用前进式定点跟踪控制方法，即机器人前进行驶跟踪目标点的方法，根据公式(5)计算机器人左驱动轮、右驱动轮速度，控制机器人跟踪当前目标点P；同时，控制器采用公式(3)，实时计算在当前目标点P处目标航向角θ_r方向上，机器人与当前目标点P的距离L，若L＜0，则继续执行步骤12)，若L≥0，则进入步骤14)；

公式(5)中：D为机器人左驱动轮与右驱动轮的轮心距离；θ是机器人航向角；k₁为位置偏差增益，k₁＞0，k₂为航向角偏差增益，k₂＞0；

13)当步骤(10)中采用公式(3)计算的L＞0时，控制器采用后退式定点跟踪控制方法，即机器人后退行驶跟踪目标点的方法，根据公式(6)计算机器人左驱动轮、右驱动行驶速度，控制机器人跟踪当前目标点P；同时，控制器采用公式(3)，实时计算当前目标点P处目标航向角θ_r方向上，机器人与当前目标点P的距离L，若L＞0，则继续执行步骤13)，若L≤0，则进入步骤14)；

14)若机器人到达当前目标点P，则进入步骤15)，否则，返回步骤10)；

15)若当前目标点P为最终目标点，则进入步骤17)，否则，进入步骤16)；

16)选取下一目标点为新的当前目标点，重新返回步骤2)；

17)机器人到达最终目标点，定点跟踪控制结束。