前轮驱动车式移动机器人的动态编队控制方法
【专利摘要】本发明提出一种前轮驱动车式移动机器人的动态编队控制方法，多移动机器人在自主编队运动中，领航机器人负责自主导航、目标搜索和主动避障等任务，其它机器人作为跟随者与领航机器人一起自主组成既定的几何队形，并在运动过程中保持该队形。跟随机器人的运动控制器采用基于反步法和链式系统的混合控制算法。当领航机器人的位姿参数发生变化时，跟随机器人实时检测其与领航机器人之间的距离d和夹角并将自身状态向量与期望队形的状态向量进行比较，获得跟随机器人实际位姿与理想位姿之间的误差，再依据该位姿误差求得各跟随机器人对应的运动控制器，实现对跟随机器人的速度和角速度的有效控制，最终达到多移动机器人的编队成形和编队保持的目的。
【专利说明】
前轮驱动车式移动机器人的动态编队控制方法
技术领域
[0001] 本发明属于移动机器人技术领域，具体涉及多个自主车式移动机器人的动态编队 控制方法。基于"前轮导向与驱动一体化"的车式移动机器人驱动新模式，提出一种适用于 领航跟随编队的机器人运动控制方法，为多移动机器人在联合侦查、群体盯梢、协作救援、 合作搬运等领域的工程应用提供一种解决方法。
【背景技术】
[0002] 移动机器人作为一种自主、智能的运动平台和运输载体，尤其适用于野外侦查、目 标搜索、排雷抢险、空间探测等危险、污染、极端等场合。传统的单移动机器人存在结构复 杂、造价昂贵、作业范围受限等弊端，而近年来出现的多移动机器人技术则具有群体协作、 高鲁棒性、单体造价低等独特优势。
[0003] 移动机器人动态编队是多移动机器人技术领域的一个重要分支，在联合侦查、群 体盯梢、协作救援、合作搬运、传感网络等军事和民用领域具有广阔的应用背景。由于受传 感器感知范围、单体作业范围、设备搭载能力、通讯距离等因素制约，在执行大范围、长距 离、动态复杂等环境下的野外作业任务时，亟需多个移动机器人协作构成一个较为精准的 编队或图形以完成特定的目标任务，并在执行作业任务、目标跟踪、躲避障碍物时尽可能保 持编队队形不变。因此，如何形成编队、如何保持编队队形是移动机器人动态编队的两个核 心问题。
[0004] 目前，移动机器人动态编队包含三种方法:基于行为法(Behavior-based)、虚拟结 构法(Virtual-structure)和领航跟随法(Leader-follower)。基于行为的编队控制方法无 法准确描述系统整体行为、难以进行数学分析，且系统稳定性无法保证，而虚拟结构法存在 系统灵活性差的缺陷。领航跟随法以其易于保持队形、数学分析简单等优势，广泛应用于多 移动机器人编队、无人机编队、舰船编队等领域。
[0005] 领航跟随法在执行编队任务时，由领航机器人(Leader)负责目标跟踪、轨迹生成、 主动避障等任务，而跟随机器人(follower)主要任务是跟随领航机器人的运动轨迹，并同 时保持预先设定的编队队形。因此，跟随机器人的驱动方式、运动控制器设计则成为领航跟 随法形成编队并保持队形的关键所在。由于跟随机器人在编队运动中处于被动跟随的角 色，现有基于"前轮导向与后轮驱动"的车式移动机器人在跟随运动时，有可能因为前轮转 向角过大而致使前轮卡死，从而导致跟随机器人运动失效、整个编队队形失真等问题。
【发明内容】
[0006] 针对现有"前轮导向与后轮驱动"车式移动机器人存在的前轮易卡死、编队队形易 失真等问题，本发明基于一种"前轮导向与驱动一体化"的车式移动机器人驱动新模式;在 此新驱动模式基础上，设计了一种基于反步法(Backstepping)和链式系统的跟随机器人运 动控制器，以确保多个车式移动机器人编队运动的精准性和稳定性，从而为多自主移动机 器人在联合侦查、协作救援、合作搬运、空间探索等领域的工程应用提供理论支撑与解决思 路。
[0007] 技术方案
[0008] 本发明中的车式移动机器人采用"前轮导向与驱动一体化"运动模式，多个移动机 器人的动态编队控制采用领航跟随法。多移动机器人在自主编队运动并执行任务过程中， 领航机器人负责自主导航、目标搜索和主动避障等任务，而其它机器人则作为跟随者与领 航机器人一起自主组成既定的几何队形，并在运动过程中尽可能保持该队形不变。
[0009] 跟随机器人的运动控制器采用基于反步法和链式系统的混合控制算法。当领航机 器人的位姿参数发生变化时，跟随机器人实时检测其与领航机器人之间的距离d和夹角沪， 并将自身状态向量与期望队形的状态向量进行比较，获得跟随机器人实际位姿与理 想位姿之间的误差，再依据该位姿误差求得各跟随机器人对应的运动控制器，实现对跟随 机器人的速度和角速度的有效控制，最终达到多移动机器人的编队成形和编队保持的目 的。
[0010]本发明的技术方案为：
[0011] 所述一种前轮驱动车式移动机器人的动态编队控制方法，其特征在于:包括以下 步骤：
[0012] 步骤1:对非完整移动机器人系统中的每个前轮驱动车式移动机器人进行设置，确 保非完整移动机器人系统中的每个前轮驱动车式移动机器人都在各自的通讯范围内；
[0013] 步骤2:根据普法夫约束方程
[0015]建立非完整移动机器人系统中的每个前轮驱动车式移动机器人的运动学模型为：
[0017] 其中x，y，0，φ，l，v，w分别表示前轮驱动车式移动机器人在广义坐标下的位姿横 坐标、位姿纵坐标、方向角、前轮方向角、车身长度、速度和角速度；
[0018] 步骤3:将非完整移动机器人系统的运动学模型通过链式变换转换为可控的链式 系统：
[0020]同时控制输入的变换方程为：
[0023]通过上述链式变换非完整移动机器人的运动学模型表示为：
[0025]其中Vl，Wl分别表示第i个跟随机器人在广义坐标下的速度和角速度，Q=[ qia， 91,2冲,3沖,4]7为变换后第1个跟随机器人的状态量，11=[111, 1，111,2]7为变换后第1个跟随机 器人的控制输入；
[0026]步骤4:计算第i个跟随机器人在广义坐标下理想位姿
[0028] 其中^^^，^(^分别表示前领航移动机器人在广义坐标下的位姿横坐标"立姿 纵坐标、方向角、前轮方向角3^7^91￥，巾 1￥，(1，分别表示第1个跟随机器人在广义坐标 下的理想位姿横坐标、位姿纵坐标、方向角、前轮方向角、第i个跟随机器人与领航机器人之 间的距离、第i个跟随机器人与领航机器人之间的夹角；
[0029] 步骤5:构建广义坐标系下跟随机器人的误差模型：
[0030] 实时采集各跟随机器人的实际位姿，并与各自对应的期望位姿进行比较并形成误 差模型，设计运动控制器使得误差趋近于零，实现跟随机器人的实际位姿趋近于预期的期 望位姿：
[0032] 其中1^，5^，0^，(})^分别表示第1个跟随机器人的位姿横坐标误差，位姿纵坐标误 差，方向角误差，前轮方向角误差; Xl，yi，，Φ i分别表示第i个跟随机器人的实际位姿横坐 标，位姿纵坐标，方向角，前轮方向角；
[0033] 将上述在广义坐标系下跟随机器人的位姿误差转换到链式系统下的跟随机器人 的位姿误差：
[0037] 其中：啦,1(3，(^2(3，(^3(3，(^4(3分别表示第1个跟随机器人在链式系统下的位姿误差； 9 1,1^1>冲,3^1,扣分别表示第1个跟随机器人在链式系统下的理想位姿参数;11=[1 11山， m,2v]T分别表示第i个跟随机器人在链式系统下的理想控制输入；
[0038] 步骤6:设计跟随机器人的运动控制器：
[0041 ] 有益效果
[0042] 本发明采用"前轮导向与驱动一体化"的车式移动机器人驱动模式，有助于解决现 有"前轮导向与后轮驱动"方式存在的车式移动机器人前轮卡死、编队队形失真等问题；另 外，采用反步法(Backstepping)方法和李雅普诺夫(Lyapunouv)稳定性理论，所设计的跟随 机器人运动控制器具有可控性和稳定性;本发明在车式移动机器人驱动方式改进、运动控 制器设计两方面的解决思路和研究方法，在多移动机器人编队控制、无人机编队飞行、舰船 编队航行等应用领域具有一定的适用性和推广性。
[0043] 本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变 得明显，或通过本发明的实践了解到。
【附图说明】
[0044] 本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得 明显和容易理解，其中：
[0045] 图1.前轮驱动车式移动机器人模型；
[0046] 图2 .Leader-follower编队运动模型（以三角形为例）；
[0047]图3.编队运动过程中跟随机器人的前轮卡死现象（Φ2ν=Φ3ν=90°);
[0048]图4.编队直线轨迹仿真结果；
[0049]图5.编队类正弦轨迹仿真结果；
[0050]图6.编队圆轨迹仿真结果。
【具体实施方式】
[0051] 下面详细描述本发明的实施例，所述实施例是示例性的，旨在用于解释本发明，而 不能理解为对本发明的限制。
[0052] 本发明中的车式移动机器人采用"前轮导向与驱动一体化"运动模式，多个移动机 器人的动态编队控制采用领航跟随法。多移动机器人在自主编队运动并执行任务过程中， 领航机器人负责自主导航、目标搜索和主动避障等任务，而其它机器人则作为跟随者与领 航机器人一起自主组成既定的几何队形，并在运动过程中尽可能保持该队形不变。
[0053]跟随机器人的运动控制器采用基于反步法和链式系统的混合控制算法。当领航机 器人的位姿参数发生变化时，跟随机器人实时检测其与领航机器人之间的距离d和夹角炉， 并将自身状态向量(40与期望队形的状态向量进行比较，获得跟随机器人实际位姿与理 想位姿之间的误差，再依据该位姿误差求得各跟随机器人对应的运动控制器，实现对跟随 机器人的速度和角速度的有效控制，最终达到多移动机器人的编队成形和编队保持的目 的。
[0054]具体包括以下步骤：
[0055] 步骤1:对非完整移动机器人系统中的每个前轮驱动车式移动机器人进行设置，机 器人的初始位置和姿态在传感器的通讯范围随机设置，确保非完整移动机器人系统中的每 个前轮驱动车式移动机器人都在各自的通讯范围内；机器人通过蓝牙模块启动，利用传感 器进行相互之间的通讯。
[0056] 步骤2:为防止非完整移动机器人系统中每个移动机器人在运动过程中出现打滑 现象，对其进行普法夫(Pfaffian)约束，根据普法夫约束方程
[0058]建立非完整移动机器人系统中的每个前轮驱动车式移动机器人的运动学模型为：
[0060] 其中x，y，0，Φ，l，v，w分别表示前轮驱动车式移动机器人在广义坐标下的位姿横 坐标、位姿纵坐标、方向角、前轮方向角、车身长度、速度和角速度。
[0061] 步骤3:根据步骤2所建立的基于普法夫(Pfaffian)约束非完整移动机器人系统， 为了便于控制器的设计，将非完整移动机器人系统的运动学模型通过链式变换转换为可控 的链式系统：
[0063]同时控制输入的变换方程为：
[0066]通过上述链式变换非完整移动机器人的运动学模型表示为：
[0068]其中Vl，Wl分别表示第i个跟随机器人在广义坐标下的速度和角速度，Q=[ qia， 91,2冲,3沖,4]7为变换后第1个跟随机器人的状态量，11=[111, 1，111,2]7为变换后第1个跟随机 器人的控制输入。
[0069]步骤4:为了完成"前轮导向与驱动一体化"运动新模式下多移动机器人的编队成 形以及编队保持的任务，计算第i个跟随机器人在广义坐标下理想位姿
[0071] 其中^^^，^(^分别表示前领航移动机器人在广义坐标下的位姿横坐标"立姿 纵坐标、方向角、前轮方向角;^，7^9^(^，(1，分别表示第1个跟随机器人在广义坐标 下的理想位姿横坐标、位姿纵坐标、方向角、前轮方向角、第i个跟随机器人与领航机器人之 间的距离、第i个跟随机器人与领航机器人之间的夹角。
[0072] 步骤5:构建广义坐标系下跟随机器人的误差模型：
[0073] 实时采集各跟随机器人的实际位姿，并与各自对应的期望位姿进行比较并形成误 差模型，设计运动控制器使得误差趋近于零，实现跟随机器人的实际位姿趋近于预期的期 望位姿：
[0075]其中xie，yie，0ie，φ ie分别表示第i个跟随机器人的位姿横坐标误差，位姿纵坐标误 差，方向角误差，前轮方向角误差;Xl，yi，，Φ i分别表示第i个跟随机器人的实际位姿横坐 标，位姿纵坐标，方向角，前轮方向角；
[0076]将上述在广义坐标系下跟随机器人的位姿误差转换到链式系统下的跟随机器人 的位姿误差：
[0080] 其中：啦,1(3，(^2(3，(^3(3，(^4(3分别表示第1个跟随机器人在链式系统下的位姿误差； 9 1,1^1>冲,3^1,扣分别表示第1个跟随机器人在链式系统下的理想位姿参数;11=[1 11山， m,2v]T分别表示第i个跟随机器人在链式系统下的理想控制输入。
[0081] 步骤6:设计跟随机器人的运动控制器：
[0082] 采用反步法(Backstepping)方法和李雅普诺夫(Lyapunouv)稳定性理论，通过递 归方式构造出李雅普诺夫(Lyapunov)函数，推导并设计出使得整个系统具有李雅普诺夫 (Lyapunov)稳定的跟随机器人运动控制器;可计算出跟随机器人的速度和角速度：
[0085] 利用所求得的闭环运动控器(Vl，Wl)来控制编队中第i跟随机器人，使得整个编队 系统的动态特性和稳态特性都得到改善，尤其解决了多移动机器人在动态编队中，跟随机 器人前轮卡死的状况，使得动态编队可以顺利进行。
[0086] 基于上述原理，本实施例过程为：
[0087] 1.针对单个车式移动机器人建立运动学模型。见图1。
[0089] 2 ·根据Leader-f 〇1 lower方法，计算第i个跟随移动机器人的理想位姿，见图2 〇
[0091] 其中，d代表编队中跟随机器人与领航机器人保持的理想距离，识代表编队中跟随 机器人与领航机器人保持的理想角度。
[0092] 3.将运动学模型转化成链式系统。通过链式变换：
[0099] 4.在链式系统下，对跟随移动机器人的理想位姿与实际位姿进行分析，见图2。
[0101 ] 5.利用反步法(backstepping)设计第i个跟随车式移动机器人运动控制器。为了 使用(backstepping)，先将上述误差模型映射为三角形式，如下：
[01 02] yi,l - Qi, 4e-( Qi, Se+Qi, 3v) * Qi,le
[01 03] yi,2 - Qi, 3e-( Qi, 2e'l'Qi, 2v) * Qi,le
[0104] yi,3 = qi,2e
[0105] yi,4 = qi,ie
[0106] 对上式微分：
[0107] =Wu-^.2(?i.i-wu,.)v.4
[0108] Λ.2 ，.少),3-"他八4
[0109]
[0110]
[0111] ⑴考虑九1子系统，将y1>2视为虚拟控制量，madPyu视为时变函数，令又广仏， 选取李雅普诺夫(Lyapunouv)函数：
[0113] = Μ/,1,. (?/,1 -?/,1ν ) ν,·,4
[0114] 因为y1>2是虚拟控制量，令其期望值为f1;1，并定义误差变量瓦2=为 12-/,+ |^,,卜观 察到当yi,4=0时，fi,i(yi,i)=0是系统的稳定方程。
[0115] (2)考虑系统，选取李雅普诺夫(Lyapunouv)函数：
[0118]其中凡2二乃>2 -又2 (知)，将y i, 3视为虚拟控制量，令其期望值为f i, 2，定义 另,3 =乃,3 - Λ2 (乃,1，乃,2 )，对八2求导，可得到：
[0120] 可知乜,2^,1#,2)=11,1，对无,3求导，可得：
[0122] (3)考虑(ju,九2，九3)子系统，选取李雅普诺夫(Lyapunouv)函数，
[0125] 为了使负定，选取如下控制输入：
[0126] -kxyui - uiJryLi + u^ - ??；2ν + uUvy. ,
[0127] 其中，1α>0,可得：
[0128] Ui,2 = Ui,2v-2Ui,lvyi,2-kl(yi,l+yi,3)
[0129] (4)考虑(，允.2, )系统，选取李雅普诺夫(Lyapunouv)函数，
[0131]为使ft 4负定，选取如下控制输入：
[0147] 利用上述方法所求得的闭环运动控器(Vl Wl)来控制编队中的第i个跟随机器人， 并选取控制参数(khks)，使得整个编队系统的动态特性和稳态特性都得到改善，尤其解决 了多移动机器人在动态编队中，跟随机器人前轮卡死的状况，使得动态编队可以顺利进行。
[0148] 通过直线、类正弦、圆三类运动轨迹的三组仿真实验验证了上述动态编队控制方 法的有效性。
[0149] 在直线编队仿真实验中，三个前轮驱动车式移动机器人，先完成三角形编队，然后 保持队形做直线运动。机器人的初始位姿和编队过程如图4(a)所示，实际行运动轨迹如图4 (b)所示，运动过程中跟随机器人的跟随速度如图4(c)所示，跟随机器人与领航机器人的距 离误差le和角度误差θβ如图4(d)和图4(e)。由于跟随机器人起始位姿远离期望位姿，初始时 会有较大误差，前40s机器人迅速完成编队，40s之后机器人以较小误差保持队形前进。 [0150]在类正弦编队仿真实验中，三个前轮驱动车式移动机器人，先完成三角形编队，然 后保持队形做类正弦运动。机器人的初始位置和编队过程如图5(a)所示，实际行驶编队轨 迹如图5(b)所示，运动过程中跟随机器人跟随速度如图5(c)所示，跟随机器人与领航机器 人的距离误差l e和角度误差如图5(d)和图5(e)。由于正弦轨迹的周期性，跟随机器人的速 度v以及距离误差le和角度误差9 6都成周期性变化。跟随机器人在前10s迅速完成编队，10s 之后以较小误差保持队形前进。
[0151]在圆编队仿真实验中，三个前轮驱动车式移动机器人，先完成三角形编队，然后保 持队形做圆运动。机器人的初始位置和编队过程如图6(a)所示，实际行驶编队轨迹如图6 (b)所示，运动过程中跟随机器人的跟随速度如图6(c)所示，跟随机器人的距离误差l e和角 度误差9e如图6(d)和图6(e)。其中在t = 100和250s时出现奇异点两个跟随机器人前轮导向 角〇1=(|)2 = 90°使得跟随机器人的速度产生突变，由于本发明采用前轮驱动在该奇异点跟 随机器人仍能继续跟领航机器人保持队形前进。跟随机器人在前l〇s迅速完成编队，l〇s之 后以较小误差保持队形行进。
[0152] 尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例 性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨 的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。
【主权项】
1. 一种前轮驱动车式移动机器人的动态编队控制方法，其特征在于:包括W下步骤： 步骤1:对非完整移动机器人系统中的每个前轮驱动车式移动机器人进行设置，确保非 完整移动机器人系统中的每个前轮驱动车式移动机器人都在各自的通讯范围内； 步骤2:根据普法夫约束方程建立非完整移动机器人系统中的每个前轮驱动车式移动机器人的运动学模型为：其中x，y，0, Φ ,l,v，w分别表示前轮驱动车式移动机器人在广义坐标下的位姿横坐标、 位姿纵坐标、方向角、前轮方向角、车身长度、速度和角速度. 步骤3:将非完整移动机器人系统的运动学模型通过链式变换转换为可控的链式系统：其中vi，wi分别表示第i个跟随机器人在广义坐标下的速度和角速度，Q=[qi,i，qi,2, 91,3,〇1,4^为变换后第1个跟随机器人的状态量，11=[111,1，111,2]%变换后第1个跟随机器人 的控制输入； 步骤4:计算第i个跟随机器人在广义坐标下理想位姿其中：^1，71，01，(1)1分别表示前领航移动机器人在广义坐标下的位姿横坐标、位姿纵坐 标、方向角、前轮方向角;Xlv，ylv，θlv，Φlv，d，巧分别表示第i个跟随机器人在广义坐标下的 理想位姿横坐标、位姿纵坐标、方向角、前轮方向角、第i个跟随机器人与领航机器人之间的 距离、第i个跟随机器人与领航机器人之间的夹角； 步骤5:构建广义坐标系下跟随机器人的误差模型： 实时采集各跟随机器人的实际位姿，并与各自对应的期望位姿进行比较并形成误差模 型，设计运动控制器使得误差趋近于零，实现跟随机器人的实际位姿趋近于预期的期望位 姿：其中Xie，yie，0ie，Φ le分别表示第i个跟随机器人的位姿横坐标误差，位姿纵坐标误差， 方向角误差，前轮方向角误差;^1，71，91，(1)1分别表示第1个跟随机器人的实际位姿横坐标， 位姿纵坐标，方向角，前轮方向角. 将上述在广义坐标系下跟随机器人的位姿误差转换到链式系统下的跟随机器人的位 姿误差：其中：91,16,91,26,91,36,91,46分别表不第1个跟随机器人在链式系统下的位姿误差；91,山， 91,2巾沖,3巾沖,如分别表示第1个跟随机器人在链式系统下的理想位姿参数;11=[化叫，化2乂^ 分别表示第i个跟随机器人在链式系统下的理想控制输入； 步骤6 :设计跟随机器人的运动控制器：
【文档编号】G05D1/02GK106094835SQ201610618966
【公开日】2016年11月9日
【申请日】2016年8月1日 公开号201610618966.0, CN 106094835 A, CN 106094835A, CN 201610618966, CN-A-106094835, CN106094835 A, CN106094835A, CN201610618966, CN201610618966.0
【发明人】杨宏安, 张婷, 王经国, 张亚, 陈豪杰, 白洛玉
【申请人】西北工业大学