基于轨道扩展的多机器人的寻迹编队控制方法 【技术领域】
本发明涉及一种基于轨道扩展的多机器人寻迹编队控制方法。背景技术 机器人的寻迹控制技术因其控制律比较简单、 控制系统具有好的鲁棒性以及易于 工程实践等特点被广泛应用于军事、 灾难搜救以及信息采集等各种领域中。所谓机器人的 寻迹控制， 是指在控制系统的驱动下， 机器人从任意初始位置驶入预先规划好的轨迹， 并沿 此轨迹运动或者最终运动到目的地。 例如 ： 轮式机器人按预定的路径去搜救幸存者、 水下机 器人运动于椭圆轨道采集海洋信息等。 近些年来， 随着灵敏传感器的快速发展， 人们可以将 传感器安装于多个移动机器人， 从而构成移动传感器网络像生物体那样通过编队来共同采 集某一区域的信息。为了能够在时间和空间上实现移动传感器网络测量的最优化， 需要根 据所测区域时间和空间的变化来规划每个机器人的运动轨迹， 并且在规划好的轨迹上协调 机器人间的编队运动， 这样的控制任务称为寻迹编队控制问题。
当前， 已有的寻迹方法都是针对单个机器人的， 多机器人的寻迹编队控制研究 甚少。Princeton 大学的 Leonard 教授指导的团队采用沿曲线法向量扩展的方法将简单 闭曲线扩展为一组关于轨道函数的闭曲线设计了多个质点机器人在单条简单闭曲线上 的寻迹编队控制律 (F.Zhang et al，″ Control of coordinated patternsfor ocean sampling，″ Int.J.Control， vol.80， pp.1186-1199， 2007)。但是， 该方法对一些简单 闭曲线 ( 如 ： 椭圆、 圆角正方形以及圆角平行四边形等 ) 是很难求出轨道函数显示表达 式， 而轨道函数在控制律的设计中是必不可少的， 这使得某些情况下寻迹编队控制很难实 现。其次， 该方法将通信拓扑限制为双向链式结构、 运动中质点机器人的速度要求始终不 能为 0 并且控制律设计中需要用到邻居的加速度信息， 这在一定程度上也限制了该方法 在实际中的运用。中国专利 “基于投影的多运动体的协同路径跟踪控制方法” ( 专利号 ： ZL200810196368.4) 采用基于沿着横轴或纵轴投影的方法设计多质点机器人的协同路径跟 踪控制律， 由于该方法需要在目标路径上一直能够找到运动体的唯一的投影点， 因而不适 应目标路径是闭曲线的情况。 此外， 实际中应用较多的轮式机器人含有非完整约束 ( 如 ： 不 能侧滑等 )， 而上述两种方法针对的模型都是满足牛顿第二定律的质点运动， 因而不能直接 用于真实机器人的控制。
实际上， 最优化信息采集中的目标轨道多为简单凸闭曲线。 例如， 海洋探测中设定 的轨道通常为边很长且接近直线的超椭圆曲线 ( 包括椭圆、 圆角正方形以及圆角平行四边 形等 )， 同样的情况也出现于多个轮式机器人协同采集某一陆地的信息。与此同时， 机器人 的运动通常满足非完整约束的动力学方程。因此， 设计多个满足非完整约束的动力学机器 人在一组简单凸闭轨道上的寻迹编队控制方法将更加具有现实意义。 但目前还不存在此类 控制方法。
发明内容 发明目的 ： 本发明的目的在于针对现有技术的不足， 提供一种方法简单可靠、 精度 较高， 可用于多机器人最优化信息采集等复杂任务的基于轨道扩展的多机器人寻迹编队控 制方法。
技术方案 ： 本发明所述的本发明是一种基于轨道扩展的多机器人寻迹编队控制 方法， 其中所述的机器人的运动满足非完整约束的动力学方程并且目标轨道是简单凸闭曲 线， 具体包括如下步骤 ：
a) 对于平面中一组目标轨道， 将目标轨道沿其中心指向轨道上各点的向量扩展为 关于轨道函数等值轨道簇， 并确定机器人的可运动范围 ；
b) 由轨道函数计算寻迹误差， 设计机器人的虚拟角速度使寻迹误差达到设计要求 ；
c) 由轨道函数和目标轨道对应的参数计算机器人沿目标轨道运动的广义弧长及 其导数， 由通信得到的邻居信息， 设计机器人的控制力来实现编队 ；
d) 计算真实与虚拟角速度间的误差， 设计机器人的控制力矩完成寻迹 ；
e) 通过伺服系统完成机器人的运动控制。
其中所述步骤 a) 包括如下步骤 ：
a1) 将目标轨道沿其中心指向轨道上各点的向量扩展为等值轨道簇 ；
a2) 根据曲线的正则条件， 确定对应机器人的可运动范围 ；
a3) 在可运动范围上构建轨道函数， 使得每条等值轨道可由轨道函数取不同的值 来表示。
其中所述步骤 b) 包括如下步骤 ：
b1) 由机器人的位置和轨道函数， 计算轨道函数值与期望值间的位置寻迹误差 ；
b2) 由轨道函数得到轨道的切向量， 计算机器人的运动方向与轨道切方向间的角 度寻迹误差 ；
b3) 由位置寻迹误差和角度寻迹误差， 设计机器人的虚拟角速度使使得寻迹误差 减少到满足的设计要求， 同时保证机器人始终在可运动范围内运动。
其中所述步骤 c) 包括如下步骤 ：
c1) 规定好目标轨道对应的参数和弧长计算的起始点， 由此确定等值轨道簇中其 他轨道的弧长计算的起始点 ；
c2) 由轨道函数和目标轨道对应的参数， 计算机器人沿轨道从起始点到当前位置 间的弧长 ；
c3) 根据队形要求确定广义弧长与弧长间的函数关系， 计算广义弧长及其对时间 的导数 ；
c4) 由通信的相邻运动体的广义弧长及其导数， 设计机器人的控制力完成编队。
其中所述步骤 d) 包括如下步骤 ：
d1) 由机器人的真实角速度， 计算其与步骤 b) 得出的虚拟角速度间的角速度误 差；
d2) 由寻迹误差及其随时间的变化以及步骤 c) 得出的机器人的控制力， 计算虚拟 角速度对时间的导数 ；
d3) 根据角速度误差和虚拟角速度的导数， 设计机器人的控制力矩使角速度误差
减少到满足的设计要求从而最终完成寻迹。
其中所述步骤 e) 包括如下步骤 ：
由上位机将机器人的控制力和力矩发送给下位机中， 通过伺服系统来完成机器人 的运动控制。
有益效果 ： 本发明与现有技术相比， 其有益效果是 ： 本方法具有简单可靠、 精度较 高以及便于实际运用的特点， 可用于多机器人最优化信息采集等复杂任务 ； 本发明对非完 整约束的动力学机器人和简单凸闭轨道尤其适用。 附图说明
图 1 为通信拓扑对应的连通图 ；
图 2 为三个机器人以三角形队形编队运动于各自的目标轨道上 ；
图 3 为三个机器人运动于各自同心圆轨道并保持他们的位置与圆心在一条直线 上；
图 4 为由目标轨道扩展出的等值轨道簇 ；
图 5 为单个机器人在等值轨道簇中的运动 ； 图 6 为基于轨道扩展的多机器人的寻迹编队控制设计流程图。
以上的图中 ： V1、 V2、 V3 和 V4 分别表示机器人 1、 2、 3和4； Ci0、 Cj0 和 Ck0 分别表示机 器人 i、 j 和 k 对应的目标轨道 ； hi、 hj 和 hk 分别为机器人 i、 j 和 k 对应的期望队形向量 ；
和分别为轨道 Ci0、 Cj0 和 Ck0 上弧长计算的起始点 ； si、 sj 和 sk 分别为轨道 Ci0、 Cj0和 Ck0 上的弧长 ； ξi、 ξj 和 ξk 分别为 Ci0、 Cj0 和 Ck0 上的广义弧长 ； ηi、 ηj 和 ηk 分别为沿 轨道运动时 ξi、 ξj 和 ξk 对时间的导数 ； Ri、 Rj 和 Rk 分别为机器人 i、 j 和 k 的目标圆轨道 的半径 ； ρi ： 轨道 Ci0 的中心 指向轨道上各点的向量 ； λi1 和 λi2 分别为沿 ρi 平移的不同 长度 ； Ci1 和 Ci2 分别为目标轨道 Ci0 沿向量 ρi 平移 λi1 和 λi2 得到的轨道 ； 和 分别为 Ci1 和 Ci2 上弧长计算的起始点 ；是轨道切方向与横轴间的夹角 ； fi ： 轨道函数 ； 机器人 i αi ：与 间的夹角。 的运动方向 ； 与 垂直的方向 ； 轨道的切向量 ； 轨道的法向量 ； 具体实施方式 下面结合附图， 通过一个最佳实施例， 对本发明技术方案进行详细说明， 但是本发 明的保护范围不局限于所述实施例。
考虑平面中由 n 个机器人组成的寻迹编队控制系统， 机器人在 Frenet-Serret 坐 标系中的动力学方程为
其中 zi ＝ [zxi， zyi]T 表示机器人 i 重心的位置坐标， υi 为重心平移的线速度的大 表示机器人的单位速度方向， 为与单位速度方向小， ωi 是角速度的大小， 垂直的方向并且满足右手定则， mi 为机器人的质量， Ii 为转动惯量， Fi 为机器人的控制力输入， Ni 为控制力矩输入， i ＝ 1， K， n。
多机器人在寻迹编队运动中， 机器人间的通信是必不可少的， 这里我们用无向图 G ＝ (V， E) 来描述， 其中 V ＝ {V1， V2， K， Vn} 为节点集， 为有向边的集合。如果 存在一条边连接节点 Vi 和 Vk 表明机器人 i 和 k 可以交换信息， 它们互为相邻节点 ( 即， 邻居 )。机器人 i 的相邻节点集合用 Ni 表示。当图中任意两个节点间都存在着一条路 径， 那么图是连通的。这里的两个节点 和 间的路径是指由不同节点 和边构成的图。图的邻接矩阵 A ＝ [aij] 可以定义为 aij ＞ 0 当且 仅当 (Vi， Vj) ∈ E 时， 其他 aij ＝ 0。图 1 为 4 个机器人间的通信拓扑对应的连通图。设计 时， 我们一旦规定好多机器人间通信关系， 那么以后每一个时刻机器人 i 的 Ni 都是不变的， 且对应的无向图是连通的。
设 机 器 人 i 的 目 标 轨 道 Ci0 是 平 面 中 一 条 曲 率 κi0 为 正 的 简 单 凸 闭 曲 线。 该 目 标 轨 道 可 以 在 以 Ci0 的 中 心 为 原 点 的 参 考 坐 标 系 中 表 示 为 光 滑 的 映 射 ： ρi ： [0， 2π) → 2， φi ρi(φi)，其 中 光 滑 函 数 φi ： 是轨道切方向与横轴间的夹角， T 为一维环空间 ( 即 ： 如果 我们视 和 等价 )， 并且 ρi(φi) 对所有的 φi 满足 ||ρi(φi)|| ≠ 0 且 vi ＝ ||dρi(φi)/dφi|| ＞ 0。目标轨道的 Frenet 表述为
其中和分别为轨道的单位切向量和法向量。本发明的目的就是根据通信得到的相邻机器人的信息， 设计每个机器人的控制力和控制力矩使其运动在目标轨道 的同时， 机器人间保持一定的队形。
在本发明中， 对于沿着目标轨道运动的各机器人之间的队形位置关系采用如下方 式规定 ： 设目标轨道 Ci0 上的固定点 为弧长计算的起始点， si 是从起始点 沿着目标轨道 到机器人位置间的路程 ( 即弧长 )， 广义弧长 ξi ＝ τi(si) 是关于 si 一个具有二阶连续导 数的函数， 并且对于所有的 si 满足 人之间保持队形位置关系是指 ：7以及各机器102073320 A CN 102073325
说明书5/9 页τi(si)-τj(sj) ＝ 0。 如果机器人 i 的目标轨道是由参考轨道平移期望的队形向量 hi 而得到的 ( 如图 2 那么我们可以简单的定义弧长 si 即所示 )， 规定的弧长计算的起始点满足弧长 si 和机器人沿目标轨道的速度 达到一致就能保证多机器人以期望 为广义弧长 ξi， 的队形 zi(t)-zj(t) ＝ hi-hj 编队运动。对于更复杂的情况， 弧长和机器人沿着目标轨道的 速度达到一致并不能保证多机器人以期望的队形编队运动。图 3 所示的是三个机器人运动 在不同半径 Ri 的同心圆轨道上并且保持与圆心在同一条直线上的例子， 显然只有当 ξi ＝ si/Ri 及其导数 ηi 达到一致时才能保证多机器人以期望的队形编队运动。因此， 本方法选 * 择用沿着轨道运动的广义弧长 ξi 达到一致、 其导数 ηi 达到一致或给定值 η (t) 来描述多 机器人的编队运动， 这里我们要求一致值和 η*(t) 是一个对时间 t 一致有界非 0 的信号。
本发明中， 控制器的设计思想是先将目标轨道 Ci0 沿着其中心 指向轨道上各点 的向量 ρi 扩展为关于轨道函数 fi(z) 的等值轨道簇， 并由曲线的正则性确定对应机器人 i 的可运动范围 Ωi( 如图 4 所示 )。通过设计机器人的力矩使得初始位于 Ωi 中的机器人 i 始终在 Ωi 中运动， 与此同时， 轨道函数值 fi(zi) 与期望值间的位置寻迹误差 di 以及机器人 的运动方向与轨道的切方向间的夹角 ( 即， 角度寻迹误差 )αi 减少到满足设计的要求 ( 如 图 5 所示 )。为了设计机器人的控制力矩， 本发明中我们是分两步来完成 ： 第一步， 设计虚 拟角速度使得寻迹误差减少到满足设计的要求， 同时保证机器人不离开 Ωi ； 第二步， 设计 机器人的控制力矩使得真实角速度和虚拟角速度间的误差减少到满足设计的要求。 当机器 人运动在各自的目标轨道上， 多机器人间的编队运动就退化为机器人沿轨道运动的位置与 速度达到一致。根据通信得到相邻机器人的信息， 设计机器人的控制力使得沿轨道运动的 广义弧长 ξi(t) 达到一致、 其导数 ηi(t) 达到一致值或期望值 η*(t) 来实现多机器人的 在轨编队运动。
图 6 是本发明的设计流程图， 由模块 P1、 P2、 P3、 P4 和 P5 构成， 各模块叙述如下 ：
1) 模块 P1
由于本发明设计寻迹编队控制器的前提是将每条目标轨道扩展为由不同轨道函 数值表示的等值轨道簇， 模块 P1 用于得到等值轨道簇、 轨道函数以及对应机器人的可运动 范围， 具体按下列步骤实现 ：
第一步 ： 在 Ci0 附近， 将 Ci0 上的每一点沿其中心 指向轨道上各点的向量 ρi 平移实数 λik 得到扩展轨道 Cik， 即
Cik(φi， λik) ＝ Ci0(φi)+λikρi0(φi).
第二步 ： 由扩展轨道的切向量以及正则条件我们选择机器人 i 的可运动范围 Ωi 为平面中所有满足 -1 ＜ λik ＜ ε ＜∞的轨道上点的集合。
第三步 ： 由于集合 Ωi 中的每一点都属于 Ωi 中的一条扩展轨道， 我们可以在 Ωi 上构建轨道函数
fi ： Ωi → (-1， ε)
并且满足 进而， Ωi 可以表示为 Ωi ＝ {z ∈ i2|-1 ＜ fi(z) ＜ ε}。Cik 是关 即 z ＝ [zx， zy]T ∈ Cik， 则 fi(z) ＝ λik ； 当 z ∈ Ci0， 于轨道函数 fi 的一条等值平面凸闭轨道， fi(z) ＝ 0。
例如 ： ( 倾斜 ) 超椭圆曲线其中 μ ∈ i， a ≥ b ＞ 0， p ＝ 1， 3， 5...。对于 μ ＝ 0 且 a ＞ b( 特别的， a ＝ b)， p ＝ 1 表示椭圆 ( 特别的， 圆 ) 而 p ≥ 3 表示圆角矩形 ( 特别的， 圆角正方形 )。如果 μ ≠ 0 且 p ＞ 1， 则表示圆角平行四边形。 按照模块 P1 的轨道扩展方式， 上述超椭圆曲线可以扩展 为关于轨道函数
的等值轨道簇。
2) 模块 P2
模块 P2 是用来设计虚拟角速度使得寻迹误差减少到满足设计的要求， 同时保证 机器人始终在可运动范围内运动， 具体的设计步骤如下 ：
第一步 ： 由轨道函数 fi 和机器人的当前位置 zi， 计算寻迹位置误差 di(t)
di ＝ fi(zi)-0 ＝ fi(zi).
第二步 ： 由轨道函数 fi 得到轨道的切向量其中为轨道的法向量， 由此计算寻迹角度误差 αi(t) ∈ (-π，π]
第三步 ： 设计机器人的虚拟角速度其中ψi(di) ＝ 1/(ε-di)-1/(di+1)+ln(1+di)-ln(ε-di)-1/ε+1+lnε，
控制参数 k1 ＞ 0。函数 ψi(di) 是用于保证初始位于 Ωi 的机器人 i 始终在 Ωi 中 运动且最终运动于目标轨道。
3) 模块 P3
如图 5 所示， 机器人 i 沿轨道切方向的运动可以改变机器人沿当前轨道运动的弧 长 si。与此同时， 机器人 i 投影到轨道法向量上的运动可以改变平面中机器人所在的轨道 ( 即轨道函数值 fi(zi) 发生变化 )， 同样也可以改变弧长。因此， 本方法选择目标轨道对应 的参数 φi 来刻画机器人运动在 Ωi 中所有轨道上的弧长。
模块 P3 根据通信得到邻居的广义弧长 ξj(t) 和 ηj(t) 来设计机器人 i 的控制力 从而实现编队， 具体的设计步骤如下 ：
第一步 ： 规定好目标轨道对应的参数 φi 和弧长计算的起始点由此我们可以选择等值轨道簇中其他轨道上弧长计算的起始点
使得 和 对应的参数值是一致的。第二步 ： 由轨道函数和弧长参数 φi， 计算沿机器人当前轨道从起始点 到位置 zi间的弧长 si
其中 为起始点 对应的参数值 ； 第三步 ： 由队形的要求设计好广义弧长函数 ξi， 即 ξi 是关于 si 一个具有二阶连 且 由弧续导数的函数， 并且对于所有的 si 满足 长 si 计算广义弧长 ξi(si)。于是， 广义弧长对时间的导数为
考虑到机器人最终会运动于目标轨道， 因此我们选择 ηj(t) 为 αi ＝ 0 时的即：这样做的好处就是可以简化计算量和控制律的设计。
第四步 ： 根据信息交互所得到的相邻运动体的广义弧长 ξj 和 ηj， 设计机器人的 控制力 Fi(t)
其中控制参数 k2 ＞ 0。
对于运动体的沿着目标轨道的速度达到期望值 η*(t) 的编队要求， 设计机器人的 控制力 Fi(t) 为
4) 模块 P4
模块 P4 根据真实角速度和虚拟角速度来设计机器人的控制力矩使得两者间的角 速度误差减少到满足设计的要求从而最终实现寻迹。设计按下列步骤实现 ：
第一步 ： 由机器人的真实角速度 ωi(t)， 计算其与模块 P2 得出的虚拟角速度的角速度误差 ωie(t)
第二步 ： 由寻迹误差及其随时间的变化以及步骤 c) 得出的机器人的控制力 随时间的变化10Fi(t)， 计算虚拟角速度102073320 A CN 102073325
说明书8/9 页
其中
第三步 ： 设计机器人的控制力矩 Ni(t)其中控制参数 k3 ＞ 0。
5) 模块 P5
模块 P5 将模块 P3 和 P4 的得到机器人的控制力和力矩发送给下位机， 通过伺服系 统完成机器人的运动控制， 具体的按照以下步骤实现 ：
由上位机将机器人的控制力和力矩发送给下位机中， 通过伺服系统来完成机器人 的运动控制， 并返回到模块 P2。
如上所述， 尽管参照特定的优选实施例已经表示和表述了本发明， 但其不得解释
为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下， 可对 其在形式上和细节上作出各种变化。