一种非时间参考的差速驱动机器人定点跟踪控制方法
【专利摘要】本发明涉及一种非时间参考的差速驱动机器人定点跟踪控制方法，属于差速驱动机器人的定点跟踪控制技术领域，该方法包括：控制器与外界监测系统实时通信，获取目标点信息；首先，根据机器人航向角与目标点处目标航向角的差值，选择机器人自转控制模式；接着，根据机器人与目标点的相对距离判断机器人定点跟踪控制模式；然后，根据机器人位置和航向角及目标点位置和目标点处目标航向角，计算车轮行驶速度并由驱动电机执行；若机器人到达最终目标点，则控制结束，否则跟踪一下目标点。本方法不需要规划路径，设计的车速控制律与时间因素无关，便于驱动电机实现计算出的车轮行驶速度，易实现机器人跟踪单个目标点和连续依次跟踪多个目标点。
【专利说明】
一种非时间参考的差速驱动机器人定点跟踪控制方法
技术领域
[0001] 本发明属于差速驱动机器人的定点跟踪控制技术领域，特别涉及适用于两轮、三 轮、四轮、六轮等基于差速驱动的轮式机器人和轮式走行装置的定点跟踪控制。
【背景技术】
[0002] 差速驱动机器人是通过2个驱动电机控制两侧车轮行驶速度，实现直线或曲线运 动的轮式机器人，因结构简单、传动效率高和控制灵活被广泛使用。随着轮式机器人的智能 化程度不断提高，这种智能机器人已被用在多个领域，如生产车间货物运输、危险环境勘 察、室内清洁等。
[0003] 机器人的物品运输、室内清洁等任务，可通过控制机器人跟踪单个目标点或依次 跟踪多个目标点最终到达指定目的地来完成。移动机器人定点跟踪控制问题是机器人位置 控制的基本问题，是机器人运动控制研究课题之一，目前机器人定点跟踪控制方法有误差 比例法、路径规划与路径跟踪法、轨迹规划与轨迹跟踪法。误差比例控制法简单地将机器人 与目标点的位置偏差和航向角偏差乘以一个比例系数，来计算驱动轮行驶速度，不适用于 差速驱动机器人这种强非线性运动系统。通过规划机器人起点到目标点的路径然后采用路 径跟踪实现机器人定点跟踪的方法，需要设计机器人行驶路径和路径跟踪控制方法。轨迹 规划与轨迹跟踪法通过设计轮速随时间变化函数，实现机器人定点跟踪的方法，对车轮速 度控制精度要求较高，当驱动电机出现速度控制误差时，导致机器人不能到达目标点，导致 定点跟踪失败。
[0004] 本发明适用的已有轮式机器人包括，图1所示的三轮机器人、图2所示的四轮机器 人、图3所示的六轮机器人等基于两轮差速驱动行驶的多轮机器人。
[0005] 图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所在平面内转 动，用于支撑机器人重量。
[0006] 图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所在平面内转动，用于 支撑机器人重量。
[0007]图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所在平面内转动。
[0008]图1所示的万向轮18,图2所示的万向轮281、万向轮282,图3中所示万向轮381、万 向轮382、万向轮383、万向轮384只启平衡载荷作用，对机器人的运动和定点跟踪控制不产 生影响，因此将图1、图2、图3所示机器人简化为图4所示两轮机器人模型。
[0009]图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供电。
【发明内容】
[0010] 本发明的目的是为克服已有技术的不足之处，提出一种非时间参考的差速驱动机 器人定点跟踪控制方法，本方法便于驱动电机实现计算出的车轮行驶速度，易实现机器人 对单个定点跟踪和连续依次对多个定点跟踪。
[0011] 本发明提出的一种非时间参考的差速驱动机器人定点跟踪控制方法，其特征在 于，该方法包括以下步骤：
[0012] 1)机器人的控制器启动，机器人进入定点跟踪控制状态；
[0013] 2)控制器与外界监测系统实时通信，获取当前目标点P的位置和当前目标点P处目 标航向角t的信息，若机器人航向角9与当前目标点P处目标航向角t之差的绝对值小于90° 或大于270°，则进入步骤10)，否则进入步骤3);
[0014] 3)若机器人航向角0与当前目标点P处目标航向角0r之差的绝对值大于等于90°且 小于等于180°，则进入步骤4);若机器人航向角0与当前目标点P处目标航向角0 r之差的绝 对值大于180°且小于等于270°，则进入步骤5);
[0015] 4)若机器人航向角0大于当前目标点P处目标航向角0r，则进入步骤6)，否则，进入 步骤7);
[0016] 5)若机器人航向角0大于当前目标点P处目标航向角0r，则进入步骤8)，否则，进入 步骤9);
[0017] 6)控制器控制机器人顺时针自转，根据公式（1)计算机器人左驱动轮、右驱动轮速 度，同时，实时与外界监测系统通信，获取并计算机器人航向角9与目标点P处目标航向角I 之差;控制器控制机器人自转过程中，当机器人航向角9与当前目标点P处目标航向角I之 差小于90°时，进入步骤10);
[0018] i ' (1)
[Vl =ks'V0
[0019] 公式（1)中：vr为机器人右驱动轮速度，若vr>0，右驱动轮相对机器人底盘前进行 驶，若Vr<0，右驱动轮相对机器人底盘后退行驶;VI为机器人左驱动轮速度，若VI >〇，左驱 动轮相对机器人底盘前进行驶，若Vl<〇，左驱动轮相对机器人底盘后退行驶;V。为用户设定 的机器人运动速度，V。彡0 ;ks为机器人速度比例系数；
[0020] 7)控制器控制机器人逆时针自转，根据公式(2)计算机器人左驱动轮、右驱动轮速 度，同时，实时与外界监测系统通信，获取并计算机器人航向角与目标点处目标航向角之 差;控制器控制机器人自转过程中，若当前目标点P处目标航向角I与机器人航向角9之差 小于90°，进入步骤10); \ \
[0021] ； (2)
[vr=-^-v0
[0022] 8)控制器控制机器人逆时针自转，根据公式(2)计算机器人左驱动轮、右驱动轮行 驶速度，同时，实时与外界监测系统通信，获取并计算机器人航向角9与目标点P处目标航向 角I之差;控制器控制机器人自转过程中，当机器人航向角9与当前目标点P处目标航向角t 之差大于270°时，进入步骤10);
[0023] 9)控制器控制机器人顺时针自转，根据公式（1)计算机器人左驱动轮、右驱动行驶 速度，同时，实时与外界监测系统通信，获取并计算机器人航向角9与目标点P处目标航向角 I之差;控制器控制机器人自转过程中，若当前目标点P处目标航向角t与机器人航向角9之 差大于270°时，进入步骤10);
[0024] 10)计算在当前目标点P处目标航向角0r方向上，机器人与当前目标点P的距离L， 计算公式如公式(3)所示，若L<0,则进入步骤12);若L>0,则进入步骤13);若L = 0且当前 目标点P与机器人两驱动轮轮心连线中点〇不重合，则进入步骤11 )，若L = 0且当前目标点P 与机器人两驱动轮轮心连线中点〇重合，则进入步骤15);
[0025] L= (x-Xr)cos9r+(y-yr)sin9r (3)
[0026] 公式(3)中，L为在当前目标点P处目标航向角0r方向上，机器人与当前目标点P的 距离，x为机器人的X轴坐标，y为机器人的Y轴坐标， Xr为当前目标点P的X轴坐标，yr为当前目 标点P的Y轴坐标，为当前目标点P处目标航向角；
[0027] 11)控制器采用公式(4)计算机器人左驱动轮、右驱动轮的行驶速度，控制机器人 沿当前目标航向角9直线前进行驶一段距离S，S>0,然后返回步骤10);
[0_ 。⑷
[Vl^ks-Vo
[0029] 12)当步骤(10)中采用公式(3)计算的L<0时，控制器采用前进式定点跟踪控制方 法，即机器人前进行驶跟踪目标点的方法，根据公式(5)计算机器人左驱动轮、右驱动轮速 度，控制机器人跟踪当前目标点P;同时，控制器采用公式(3)，实时计算在当前目标点P处目 标航向角I方向上，机器人与当前目标点P的距离L，若L<0,则继续执行步骤12)，若L多0， 则进入步骤14);
[0031]公式（5)中：D为机器人左驱动轮与右驱动轮的轮心距离；0是机器人航向角；1^为 位置偏差增益，ki>0，k2为航向角偏差增益，k2>0;
[0032] 13)当步骤（10)中采用公式(3)计算的L>0时，控制器采用后退式定点跟踪控制方 法，即机器人后退行驶跟踪目标点的方法，根据公式(6)计算机器人左驱动轮、右驱动行驶 速度，控制机器人跟踪当前目标点P;同时，控制器采用公式(3)，实时计算当前目标点P处目 标航向角I方向上，机器人与当前目标点P的距离L，若L>0,则继续执行步骤13)，若LS0， 则进入步骤14);
[0034] 14)若机器人到达当前目标点P，则进入步骤15)，否则，返回步骤10);
[0035] 15)若当前目标点P为最终目标点，则进入步骤17)，否则，进入步骤16);
[0036] 16)选取下一目标点为新的当前目标点，重新返回步骤2);
[0037] 17)机器人到达最终目标点，定点跟踪控制结束。
[0038]本发明的特点及有益效果：
[0039] 该方法在非时间区域中，设计用于差速机器人定点跟踪的轮速控制律，设计出的 车轮行驶速度与时间因素无关，摆脱了时间对速度的约束，便于驱动电机实现计算出的车 轮行驶速度;定点跟踪过程中，不需要规划机器人运动路径，便于实现机器人单个定点跟踪 和连续依次对多个定点跟踪。
【附图说明】
[0040] 图1是三轮机器人结构示意图。
[0041 ]图2是四轮机器人机构示意图。
[0042]图3是六轮机器人结构示意图。
[0043] 图4是结构简化后的两轮机器人结构示意图。
[0044] 图5是本发明的两轮机器人定点跟踪控制方法实施流程示意图。
[0045] 图6是本实施例中前进式定点跟踪示意图。
[0046] 图7是本实施例中后退式定点跟踪示意图。
[0047] 图8是本实施例机器人多定点连续跟踪效果示意图。
【具体实施方式】
[0048] 本发明提出的一种非时间参考的差速驱动机器人定点跟踪控制方法，下面结合附 图及具体实施例进一步说明如下：
[0049] 本发明提出的一种非时间参考的差速驱动机器人定点跟踪控制方法实施例，用于 两轮机器人的定点跟踪控制。该两轮机器人定点跟踪控制方法的实施流程如图5所示，包括 以下步骤：
[0050] 1)机器人的控制器启动，机器人进入定点跟踪控制状态；
[0051 ] 2)控制器与外界监测系统实时通信，获取当前目标点P的位置和当前目标点P处目 标航向角t的信息，若机器人航向角9与当前目标点P处目标航向角t之差的绝对值小于90° 或大于270°，则进入步骤10)，否则进入步骤3);
[0052] 3)若机器人航向角0与当前目标点P处目标航向角0r之差的绝对值大于等于90°且 小于等于180°，则进入步骤4);若机器人航向角0与当前目标点P处目标航向角0 r之差的绝 对值大于180°且小于等于270°，则进入步骤5);
[0053] 4)若机器人航向角0大于当前目标点P处目标航向角0r，则进入步骤6)，否则，进入 步骤7);
[0054] 5)若机器人航向角0大于当前目标点P处目标航向角0r，则进入步骤8)，否则，进入 步骤9);
[0055] 6)控制器控制机器人顺时针自转，根据公式（1)计算机器人左驱动轮、右驱动轮速 度，同时，实时与外界监测系统通信，获取并计算机器人航向角9与目标点P处目标航向角I 之差;控制器控制机器人自转过程中，当机器人航向角9与当前目标点P处目标航向角I之 差小于90°时，进入步骤10); fv,. =-A?v0
[0056] , ⑴
[0057] 公式（1)中：Vr为机器人右驱动轮速度，若Vr> 0，右驱动轮相对机器人底盘前进行 驶，若vr<〇，右驱动轮相对机器人底盘后退行驶;vi为机器人左驱动轮速度，若vi>〇，左驱 动轮相对机器人底盘前进行驶，若vi<〇，左驱动轮相对机器人底盘后退行驶;V。为用户设定 的机器人运动速度，V。多0 ; ks为机器人速度比例系数，建议ks的取值范围为(0，1 ];
[0058] 7)控制器控制机器人逆时针自转，根据公式(2)计算机器人左驱动轮、右驱动轮速 度，同时，实时与外界监测系统通信，获取并计算机器人航向角与目标点处目标航向角之 差;控制器控制机器人自转过程中，若当前目标点P处目标航向角I与机器人航向角9之差 小于90°时，进入步骤10);
[_卜:" ⑵
[0060] 公式(2)中各符号定义与公式（1)相同；
[0061] 8)控制器控制机器人逆时针自转，根据公式(2)计算机器人左驱动轮、右驱动轮行 驶速度，同时，实时与外界监测系统通信，获取并计算机器人航向角9与目标点P处目标航向 角I之差;控制器控制机器人自转过程中，当机器人航向角9与当前目标点P处目标航向角t 之差大于270°时，进入步骤10);
[0062] 9)控制器控制机器人顺时针自转，根据公式（1)计算机器人左驱动轮、右驱动行驶 速度，同时，实时与外界监测系统通信，获取并计算机器人航向角9与目标点P处目标航向角 I之差;控制器控制机器人自转过程中，若当前目标点P处目标航向角t与机器人航向角9之 差大于270°时，进入步骤10);
[0063] 10)计算在当前目标点P处目标航向角0r方向上，机器人与当前目标点P的距离L， 计算公式如公式(3)所示，若L<0,则进入步骤12);若L>0,则进入步骤13);若L = 0且当前 目标点P与机器人两驱动轮轮心连线中点〇不重合(如图6中，当前目标点P69在左驱动轮63 和右驱动轮64的轮心连线上，但当前目标点P69与两轮心连线中点〇不重合的情况；如图7 中，当前目标点P79在左驱动轮73和右驱动轮74的轮心连线上，但当前目标点P79与两轮心 连线中点〇不重合的情况），则进入步骤11)，若L = 0且当前目标点P与机器人两驱动轮轮心 连线中点〇重合(如图6中，当前目标点P69在左驱动轮63和右驱动轮64的轮心连线上，且与 两轮心连线中点〇重合的情况；图7中，当前目标点P79在左驱动轮73和右驱动轮74的轮心连 线上，且与两轮心连线中点〇重合的情况），则进入步骤15);
[0064] L= (x-Xr)cos9r+(y-yr)sin9r (3)
[0065] 公式(3)中，L为在当前目标点P处目标航向角0r方向上，机器人与当前目标点P的 距离，x为机器人的X轴坐标，y为机器人的Y轴坐标， Xr为当前目标点P的X轴坐标，yr为当前目 标点P的Y轴坐标，为当前目标点P处目标航向角；
[0066] 11)控制器采用公式(4)计算机器人左驱动轮、右驱动轮的行驶速度，控制机器人 沿当前目标航向角9直线前进行驶一段距离S，S>0,（建议S彡0.1米），然后返回步骤10); \vr -ks-V〇.
[0067] / (4)
[0068] 公式(4)中各符号定义同公式(1)、（2);
[0069] 12)当步骤(10)中采用公式(3)计算的L<0时，控制器采用前进式定点跟踪控制方 法（即机器人前进行驶跟踪目标点的方法），根据公式(5)计算机器人左驱动轮、右驱动轮速 度，控制机器人跟踪当前目标点P;同时，控制器采用公式(3)，实时计算在当前目标点P处目 标航向角I方向上，机器人与当前目标点P的距离L，若L<0,则继续执行步骤12)，若L多0， 则进入步骤14);
[0071] 公式(5)中：D为机器人左驱动轮与右驱动轮的轮心距离；0是机器人航向角;1^为 位置偏差增益为航向角偏差增益，k2>0;其他符号定义与公式（1)、（2)、（3)、（4) 相同；
[0072] 本实施例步骤12)中，机器人控制器与外界监测系统实时通信，实时获取当前目标 点信息及机器人位置和航向角信息，并采用公式(5)计算驱动轮的行驶速度，并将速度指令 信息发送给车轮驱动电机，驱动电机执行接收到的速度指令。
[0073]前进式定点跟踪如图6所示。图6中：〇点为机器人62左驱动轮63与右驱动轮64的轮 心连线中点；x、y分别为〇点在坐标系61的X轴坐标和Y轴坐标;0是机器人航向角，为沿逆时 针方向上，坐标系X轴到机器人62行驶方向的夹角；x r、yr分别为当前目标点P点69在坐标系 61中的X轴坐标、Y轴坐标;0r为当前目标P点69的航向角；
[0074]公式(5)中，^为控制器67计算出的左驱动轮63行驶速度，控制器67将该速度指令 发送给左轮驱动电机65,左轮驱动电机65执行该速度指令;vr为控制器67计算出的右驱动 轮64行驶速度，控制器67将该速度指令发送给右轮驱动电机66,右轮驱动电机66执行该速 度指令;v。为使用者设定的机器人62的行驶速度，即机器人62〇点的运动速度，〇点为机器人 62左驱动轮63与右驱动轮64的轮心连线中心;D为机器人62的左驱动轮63与右驱动轮64的 轮心距离;x为机器人62〇点的X轴坐标;y为机器人62〇点的Y轴坐标；0是机器人航向角，为逆 时针方向上，坐标系61的X轴到机器人62行驶方向的夹角;P点69为当前目标点；x r是P点69 的X轴坐标;yr为P点69的Y轴坐标；9r为当前目标点P点69的航向角;ki为距离偏差增益，ki> 0，根据经验建议h的取值范围为[0.1，30]; k2为航向角偏差增益，k2>0，根据经验建议k2的 取值范围为[0.1，50]。
[0075] 13)当步骤（10)中采用公式(3)计算的L>0时，控制器采用后退式定点跟踪控制方 法，即机器人后退行驶跟踪目标点的方法，，根据公式(6)计算机器人左驱动轮、右驱动行驶 速度，控制机器人跟踪当前目标点P;同时，控制器采用公式(3)，实时计算当前目标点P处目 标航向角L方向上，机器人与当前目标点P的距离L，若L>0,则继续执行步骤13)，若LS0， 则进入步骤14);
[0077] 公式(6)中各符号定义与公式(1)、（2)、（3)、（4)、（5)相同；
[0078] 本实施例步骤13)中，机器人控制器与外界监测系统实时通信，实时获取当前目标 点位置及目标点处目标航向角信息与机器人位置和航向角信息，并采用公式(6)计算驱动 轮的行驶速度，并将速度指令信息发送给车轮驱动电机，驱动电机执行接收到的速度指令。 [0079]后退式定点跟踪如图7所示。图7中 ：〇点为机器人72左驱动轮73与右驱动轮74的轮 心连线中点；x、y分别为〇点在坐标系71的X轴坐标和Y轴坐标;0是机器人航向角，为沿逆时 针方向上，坐标系X轴到机器人72行驶方向的夹角; Xr、yr分别为当前目标点P点79在坐标系 71中的X轴坐标、Y轴坐标;0r为当前目标P点79的航向角；
[0080] 公式(6)中，^为控制器77计算出的左驱动轮73行驶速度，控制器77将该速度指令 发送给左轮驱动电机75,左轮驱动电机75执行该速度指令;v r为控制器77计算出的右驱动 轮74行驶速度，控制器77将该速度指令发送给右轮驱动电机76,右轮驱动电机76执行该速 度指令;v。为使用者设定的机器人72的行驶速度，即机器人72〇点的运动速度，〇点为机器人 72左驱动轮73与右驱动轮74的轮心连线中心;D为机器人72的左驱动轮73与右驱动轮74的 轮心距离;x为机器人72〇点的X轴坐标;y为机器人72〇点的Y轴坐标;0是机器人航向角，为逆 时针方向上，坐标系71的X轴到机器人72行驶方向的夹角;P点79为当前目标点； Xr是P点79 的X轴坐标;yr为P点79的Y轴坐标;0r为当前目标点P点79的航向角;h为位置偏差增益， 0，根据经验建议h的取值范围为[0.1，30]; k2为航向角偏差增益，k2>0，根据经验建议k2的 取值范围为[0.1，50]。
[0081] 14)若机器人到达当前目标点P，则进入步骤15)，否则，返回步骤10);
[0082] 15)若当前目标点P为最终目标点，则进入步骤17)，否则，进入步骤16);
[0083] 16)选取下一目标点为新的当前目标点，重新返回步骤2);
[0084] 17)机器人到达最终目标点，定点跟踪控制结束。
[0085] 上述方法同样可用于三轮、四轮、六轮机器人定点跟踪控制。用于如图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不做任何控制。
[0086] 选择任意一条路线，选取路线上的一系列点，采用上述定点跟踪控制方法进行跟 踪控制，定点跟踪仿真结果如图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可知，本发明提出的差速机器人定点跟踪方法，可以实现 单个定点跟踪或多个定点连续跟踪，使机器人最终到达目标地。
【主权项】
1. 一种非时间参考的差速驱动机器人定点跟踪控制方法，该方法包括以下步骤： 1) 机器人的控制器启动，机器人进入定点跟踪控制状态； 2) 控制器与外界监测系统实时通信，获取当前目标点P的位置和当前目标点P处目标航 向角的信息，若机器人航向角Θ与当前目标点P处目标航向角0 r之差的绝对值小于90°或大 于270°，则进入步骤10)，否则进入步骤3); 3) 若机器人航向角Θ与当前目标点P处目标航向角差的绝对值大于等于90°且小于 等于180°，则进入步骤4);若机器人航向角Θ与当前目标点P处目标航向角0r之差的绝对值 大于180°且小于等于270°，则进入步骤5); 4) 若机器人航向角Θ大于当前目标点P处目标航向角0r，则进入步骤6)，否则，进入步骤 7)； 5) 若机器人航向角Θ大于当前目标点P处目标航向角0r，则进入步骤8)，否则，进入步骤 9)； 6) 控制器控制机器人顺时针自转，根据公式（1)计算机器人左驱动轮、右驱动轮速度， 同时，实时与外界监测系统通信，获取并计算机器人航向角Θ与目标点P处目标航向角1之 差;控制器控制机器人自转过程中，当机器人航向角Θ与当前目标点P处目标航向角θ τ之差 小于90°时，进入步骤10); ⑴ I V/ ^ks-Vo 公式（1 )中：Vr为机器人右驱动轮速度，若Vr > 〇，右驱动轮相对机器人底盘前进行驶，若 Vr <0，右驱动轮相对机器人底盘后退行驶;Vl为机器人左驱动轮速度，若Vl >0，左驱动轮相 对机器人底盘前进行驶，若Vl<〇，左驱动轮相对机器人底盘后退行驶;V。为用户设定的机器 人运动速度，V。多〇;k s为机器人速度比例系数； 7) 控制器控制机器人逆时针自转，根据公式(2)计算机器人左驱动轮、右驱动轮速度， 同时，实时与外界监测系统通信，获取并计算机器人航向角与目标点处目标航向角之差;控 制器控制机器人自转过程中，若当前目标点P处目标航向角0 1与机器人航向角Θ之差小于 90°，进入步骤10); \ν··-κ·ν0 ⑵ Iv/ 二-夂 _ vO 8) 控制器控制机器人逆时针自转，根据公式(2)计算机器人左驱动轮、右驱动轮行驶速 度，同时，实时与外界监测系统通信，获取并计算机器人航向角Θ与目标点P处目标航向角0 r 之差;控制器控制机器人自转过程中，当机器人航向角Θ与当前目标点P处目标航向角 差大于270°时，进入步骤10); 9) 控制器控制机器人顺时针自转，根据公式（1)计算机器人左驱动轮、右驱动行驶速 度，同时，实时与外界监测系统通信，获取并计算机器人航向角Θ与目标点P处目标航向角0 r 之差;控制器控制机器人自转过程中，若当前目标点P处目标航向角91与机器人航向角Θ之 差大于270°，进入步骤10); 10) 计算在当前目标点P处目标航向角0r方向上，机器人与当前目标点P的距离L，计算公 式如公式(3)所示，若L<0,则进入步骤12);若L>0,则进入步骤13);若L = O且当前目标点P 与机器人两驱动轮轮心连线中点0不重合，则进入步骤11)，若L=O且当前目标点P与机器人 两驱动轮轮心连线中点〇重合，则进入步骤15); (3) 公式(3)中，L为在当前目标点P处目标航向角θτ方向上，机器人与当前目标点P的距离，X 为机器人的X轴坐标，y为机器人的Y轴坐标，Xr为当前目标点P的X轴坐标，yr为当前目标点P 的γ轴坐标，Or为当前目标点P处目标航向角； 11) 控制器采用公式(4)计算机器人左驱动轮、右驱动轮的行驶速度，控制机器人沿当 前目标航向角Θ直线前进行驶一段距离S，S>0,然后返回步骤10);12) 当步骤（10)中采用公式(3)计算的L<0时，控制器采用前进式定点跟踪控制方法， 即机器人前进行驶跟踪目标点的方法，根据公式(5)计算机器人左驱动轮、右驱动轮速度， 控制机器人跟踪当前目标点P;同时，控制器采用公式(3)，实时计算在当前目标点P处目标 航向角方向上，机器人与当前目标点P的距离L，若L<0,则继续执行步骤12)，若L^OJlJ 进入步骤14);公式(5)中:D为机器人左驱动轮与右驱动轮的轮心距离;Θ是机器人航向角;1^为位置偏 差增益，ki>0，k2为航向角偏差增益，k2>0; 13) 当步骤（10)中采用公式(3)计算的L>0时，控制器采用后退式定点跟踪控制方法， 即机器人后退行驶跟踪目标点的方法，根据公式（6)计算机器人左驱动轮、右驱动行驶速 度，控制机器人跟踪当前目标点P;同时，控制器采用公式(3)，实时计算当前目标点P处目标 航向角方向上，机器人与当前目标点P的距离L，若L>0,则继续执行步骤13)，若LSOJlJ 进入步骤14);14) 若机器人到达当前目标点P，则进入步骤15)，否则，返回步骤10); 15) 若当前目标点P为最终目标点，则进入步骤17)，否则，进入步骤16); 16) 选取下一目标点为新的当前目标点，重新返回步骤2); 17) 机器人到达最终目标点，定点跟踪控制结束。
【文档编号】G05D1/02GK105892459SQ201610204701
【公开日】2016年8月24日
【申请日】2016年4月1日
【发明人】李红, 王文军, 李克强
【申请人】清华大学