CN103399575B - 一种二维定常风速场中多机器人的寻迹编队控制方法 - Google Patents

Abstract

本发明是一种二维定常风速场中多机器人的寻迹编队控制方法，包括如下步骤：a)计算风速场中机器人的总偏航角度和总线速度；b)由轨道函数计算寻迹误差；c)由轨道函数和弧长参数计算机器人沿轨道运动的广义弧长和速度；d)由邻居信息设计总偏航角速度和总线加速度使得寻迹误差和队形达到设计要求，并保证机器人本身的速率大于风速；e）由总偏航角速度和总线加速度求解机器人本身期望角速度和控制力；f）计算真实与期望角速度的误差，设计机器人的控制力矩；g)由伺服系统来完成机器人的运动控制。该方法简单可靠、精度较高，对与时间无关的二维风速矢量描述的欧拉风速场和轨道函数描述的轨道尤其适用，可用于野外信息采集等。

Description

一种二维定常风速场中多机器人的寻迹编队控制方法

技术领域

本发明涉及一种二维定常风速场中多机器人的寻迹编队控制方法。

背景技术

近些年来，协作采集技术因其较好的鲁棒性、可扩展性以及广泛的应用前景受到控制和工程界的广泛关注。该技术主要通过将各种传感器安装在多个移动载体从而构成移动传感器网络来协作采集某一区域的信息。为了优化协作采集，实际应用中通常先根据所测区域时间和空间的变化来规划每个移动载体的轨道，再在规划好的轨道上协调移动载体间的编队运动，这也是协作信息采集技术核心控制问题之一，即寻迹编队控制问题。随着移动机器人产品日趋成熟，利用搭载传感器设备的多移动机器人系统越来越多的被运用于军事和民用领域中。例如：多轮式机器人按预定的路径协作搜救幸存者、探测污染源，多水下机器人运动于各自的椭圆轨道协作采集海洋信息等。

当前，已有的寻迹编队控制方法大都忽略流体运动（如风速场）对多机器人系统的影响，如中国专利ZL201010552508.4“基于轨道扩展的多机器人的寻迹编队控制方法”，以及中国专利ZL200910184547.0“三维空间中多运动体的寻迹编队控制方法”。马里兰大学的Paley副教授指导的团队给出了海洋中以单位速率运动的牛顿质点（机器人模型）均匀分布于圆轨道的设计方法（D.A.PaleyandC.Peterson,“Stabilizationofcollectivemotioninatime-invariantflowfield,”J.Guidance,Contr.,andDynamics,2009）。虽然该方法考虑了定常水流速度对质点运动的影响，由于机器人的模型过于简单以及目标轨道是圆曲线，在一定程度上也限制了该方法的实际应用。

外界环境中运动的机器人一般都会受到风速场的影响，这就使得忽略风速对机器人运动影响而设计出的寻迹编队控制律在实际操作中会经常失效。由于实验室和工程邻域的流体力学变量的测量方法通常与欧拉方法一致，本发明中所述的风速场是指风速矢量描述的欧拉风速场，并且风速矢量是不随时间变化的。实际中流动变化非常缓慢风场通常可以视为定常风速场。此外，对于特殊情况（匀速运动机器人引起的空气流动）我们还可以通过适当选择参考系将非定常流动可以转化为定常流动来处理。考虑到垂直于机器人机身高度方向的各个平面上（忽略机器人机身端部）的流动几乎相同，这里二维风速矢量来描述风速场。与此同时，实际中机器人的运动通常是满足非完整约束的动力学模型，因此非常有必要研究二维定常风速场中多个满足非完整约束的机器人寻迹编队控制方法。但是目前还不存在此类控制方法。

发明内容

发明目的：本发明的目的是提供一种二维定常风速场中多机器人的寻迹编队控制方法，该方法简单可靠、精度较高，可用于野外信息采集等。

技术方案：一种二维定常风速场中多机器人的寻迹编队控制方法，定常风速场是指与时间无关的二维风速矢量描述的欧拉风速场，并且机器人的运动轨道是用轨道函数描述的，该方法包括如下步骤：

a).由风速矢量计算风速场中机器人的总偏航角度和总线速度；

b).由轨道函数计算寻迹误差；

c).由轨道函数和弧长参数计算机器人沿轨道运动的广义弧长和在轨速度；

d).由通信得到的邻居信息，设计总偏航角速度和总线加速度使得寻迹误差和队形达到设计要求，并保证机器人本身的速率大于风速；

e).由总偏航角速度和总线加速度联列求解机器人本身期望角速度和控制力；

f).计算真实与期望角速度的误差，设计机器人的控制力矩；

g).由伺服系统来完成机器人的运动控制。

其中，所述步骤a）包括如下步骤：

a1)由机器人本身的线速度、转角以及风速矢量，计算风速场中机器人的总偏航角度；

a2)由风速场中机器人总偏航角度，计算机器人总运动方向以及与运动方向垂直的方向；

a3)由机器人本身的线速度、风速矢量、机器人总运动方向以及与运动方向垂直的方向，计算风速场中机器人的总线速度。

其中，所述步骤b）包括如下步骤：

b1)由风速场中机器人的位置和轨道函数，计算轨道函数值与期望值间的位置寻迹误差；

b2)由轨道函数计算轨道的单位法向量和单位切向量；

b3)由风速场中机器人的总运动方向和轨道的单位切方向，计算角度寻迹误差。

其中，所述步骤c）包括如下步骤：

c1)规定好目标轨道的弧长参数和弧长计算的起始点，由此确定关于轨道函数的等值曲线簇中其他曲线的弧长计算的起始点；

c2)由轨道函数和弧长参数计算机器人沿轨道从起始点到当前位置间的弧长；

c3)由队形要求确定广义弧长与弧长间的函数关系，计算广义弧长和在轨速度。

其中，所述步骤d）包括如下步骤：

d1)由位置寻迹误差、角度寻迹误差以及通信得到的相邻机器人的广义弧长，设计风速场中机器人总偏航角速度使寻迹误差达到设计要求；

d2)由通信得到的相邻机器人的广义弧长和在轨速度，设计风速场中机器人总线加速度在实现编队同时保证机器人本身的速率大于风速。

其中，所述步骤e）包括如下步骤：

e1)对总偏航角度求导得到总偏航角速度与机器人本身的角速度和控制力间的关系式；

e2)对总线速度求导得到总线加速度与机器人本身的角速度和控制力间的关系式；

e3)由步骤d)设计出的总偏航角速度和总线加速度，根据推导出的关系式联列求解出机器人本身期望角速度和控制力。

其中，所述步骤f）包括如下步骤：

f1)由机器人真实角速度，计算其与步骤e)得出的期望角速度间的角速度误差；

f2)由总偏航角速度和总线加速度对时间的导数，计算期望角速度对时间的导数；f3)根据角速度误差和期望角速度的导数，设计机器人的力矩使得角速度误差减少到满足的设计要求。

有益效果：本发明与现有技术相比，其有益效果是：该方法简单可靠、精度较高，考虑了风速场的影响，设计了二维定常风速场中多个满足非完整约束的机器人寻迹编队控制方法，可用于野外信息采集。

附图说明

图1为定常风速场中运动的机器人；

图2为机器人在等值曲线上的运动；

图3为通信拓扑连通图；

图4为多机器人以三角形队形编队运动在目标轨道上；

图5为多机器人在同心圆轨道上的编队运动；

图6为定常风速场中多机器人寻迹编队控制设计流程图。

以上的图中有：o：惯性坐标系的坐标原点；z^x：惯性坐标系的横轴；z^y：惯性坐标系的纵轴；z_i：机器人i的位置坐标；θ_i：机器人i本身的偏航角度；υ_i：机器人i本身的线速度；[cosθ_i,sinθ_i]^T：无风速影响下的机器人i的运动方向；f_i：风速矢量；γ_i：风速场中机器人i的总偏航角度；：风速场中机器人i的总线速度；x_i：风速场中机器人的总运动方向；机器人i的位置随时间的变化；λ_i：轨道函数；C_i,-1：轨道函数值为-1的等值曲线；C_i,0：轨道函数值为0的等值曲线，即目标轨道；C_i,0.5：轨道函数值为0.5的等值曲线；y_i：与x_i垂直的方向；T_i：轨道的单位切向量；N_i：轨道的单位法向量；α_i：x_i与T_i间的夹角，即角度寻迹误差；d_i：位置寻迹误差；s_i：弧长计算的起始点到机器人当前位置间的距离；v₁：机器人1；v₂：机器人2；v₃：机器人3；v₄：机器人4；v₅：机器人5；C_1,0：机器人1的目标轨道；C_2,0：机器人2的目标轨道；C_3,0：机器人3的目标轨道；h₁：机器人1对应的期望的队形向量；h₂：机器人2对应的期望的队形向量；h₃：机器人3对应的期望的队形向量；z₁：机器人1的位置坐标；z₂：机器人2的位置坐标；z₃：机器人3的位置坐标；C_1,0上弧长计算的起始点；C_2,0上弧长计算的起始点；C_3,0上弧长计算的起始点；s₁：C_1,0上的弧长；s₂：C_2,0上的弧长；s₃：C_3,0上的弧长；ξ₁：C_1,0上的广义弧长；ξ₂：C_2,0上的广义弧长；ξ₃：C_3,0上的广义弧长；η₁：机器人1在轨速度；η₂：机器人2在轨速度；η₃：机器人3在轨速度；R₁：C_1,0对应的半径；R₂：C_2,0对应的半径；R₃：C_3,0对应的半径。

具体实施方式

下面结合附图对本发明做更进一步的解释。

本发明的一种二维定常风速场中多机器人的寻迹编队控制方法，特别适用于与时间无关的二维风速矢量描述欧拉风速场和轨道函数描述的轨道。考虑用二维风速矢量（||f_i||≤f_M＜∞）描述的定常风速场中运动n个机器人，每个机器人i的动力学方程可以写成：

${\stackrel{\·}{z}}_i^x={\mathrm{\υ}}_i\cos {\mathrm{\θ}}_i+{f_i}^x$

${\stackrel{\·}{z}}_i^y={\mathrm{\υ}}_i\sin {\mathrm{\θ}}_i+{f_i}^y$

${\stackrel{\·}{\mathrm{\θ}}}_i={\mathrm{\ω}}_i$

$m_i{\stackrel{\·}{\mathrm{\υ}}}_i=F_i$

$I_i{\stackrel{\·}{\mathrm{\ω}}}_i=N_i$

其中：表示机器人i重心在惯性坐标系中的位置坐标，θ_i为机器人i本身的转角，υ_i和ω_i分别是机器人i本身的线速度和角速度，m_i为机器人的质量，I_i为转动惯量，F_i为机器人的控制力输入，N_i为控制力矩输入，i＝1,...,n。

机器人i的目标轨道C_i,0是平面中一条曲率为正的简单闭曲线。由中国专利ZL201010552508.4—“基于轨道扩展的多机器人的寻迹编队控制方法”提出的同心压缩曲线扩展方法，可以将C_i,0扩展为一组关于轨道函数：

λ_i(z):Ω_i→(-1,ε₁)

的等值曲线簇，并且满足▽λ_i≠0，ε₁为正常数。其中，λ_i(z)＝c,(c为常数)表示等值曲线簇中的一条等值曲线C_i,c，目标轨道对应的轨道函数值为0，即λ_i(z)＝0，如图2所示。

多机器人在寻迹编队运动中，机器人间的通信是必不可少的，这里我们用双向图G＝(v,ε)来描述，其中v＝{v₁,v₂,...,v_n}为节点集，为有向边的集合。如果存在一条边连接节点v_i和v_k表明机器人i和k可以交换信息，它们互为相邻节点，即邻居。机器人i的相邻节点集合用表示。当图中任意两个节点间都存在着一条路径，那么图是连通的。这里的两个节点和间的路径是指由不同节点和边构成的图。图的邻接矩阵A＝[a_ij]可以定义为a_ij＞0当且仅当时，其他a_ij＝0。图3为5个机器人间的通信拓扑对应的连通图。设计时，一旦规定好多机器人间通信关系，那么以后每一个时刻机器人i的都是不变的，且对应的双向图是连通的。本发明的任务就是根据通信得到的相邻机器人的信息，设计每个机器人的控制力和控制力矩使其运动在目标轨道的同时机器人间保持一定的队形。

在本发明中，对于沿着目标轨道运动的各机器人之间的队形位置关系采用如下方式规定：设目标轨道C_i,0上的固定点为弧长计算的起始点，s_i是从起始点沿着目标轨道到机器人位置间的路程（即弧长），广义弧长ξ_i＝τ_i(s_i)是关于s_i一个具有二阶连续导数的函数，并且对于所有的s_i满足 $\∞>{\mathrm{\ϵ}}_3\≥\∂{\mathrm{\ξ}}_i/\∂s_i\≥{\mathrm{\ϵ}}_2>0$ 以及 $\∞>{\mathrm{\ϵ}}_4\≥|{\∂}^2{\mathrm{\ξ}}_i/{\∂s}_i^2|$ ，其中ε₂，ε₃和ε₄为正常数。各机器人之间保持队形位置关系是指：

τ_i(s_i)-τ_j(s_j)＝0。

图4所示的是机器人的目标轨道是由参考轨道平移期望的队形向量h_i得到的曲线，如果规定的弧长计算的起始点满足那么我们可以简单的定义弧长s_i即为广义弧长ξ_i，弧长s_i和机器人沿目标轨道的在轨速度达到一致就能保证多机器人以期望的队形z_i(t)-z_j(t)＝h_i-h_j编队运动。对于如图5所示的更复杂的情况，三个机器人运动在不同半径R_i的同心圆轨道上并且保持与圆心在同一条直线上，显然只有当ξ_i＝s_iR_i及在轨速度达到一致时才能保证多机器人以期望的队形编队运动。因此，本方法选择用广义弧长ξ_i达到一致、在轨速度η_i达到给定值η^*(t)来描述多机器人的编队运动，这里我们要求η^*(t)是一个对时间t一致有界且满足η^*＞η^m＝2ε₂f_M信号。

本发明中，控制器的设计思想是先由风速矢量确定机器人在定常风速场中总偏航角度和总线速度。为了设计机器人的控制力，本发明中我们是分三步来完成：第一步，通过设计风速场中机器人的总偏航角速度使得初始位于Ω_i中的机器人i始终在Ω_i中运动，与此同时，轨道函数值λ_i(z_i)与期望值间的位置寻迹误差d_i以及机器人在风速场中的总运动方向与轨道的切方向间的夹角（即角度寻迹误差）α_i减少到满足设计的要求；第二步，当机器人运动在各自的目标轨道上，多机器人间的编队运动就退化为机器人沿轨道运动的位置与速度达到一致。根据通信得到相邻机器人的信息，设计风速场中机器人的总加速度使得沿轨道运动的广义弧长ξ_i达到一致、在轨速度η_i达到期望值η^*来实现多机器人的在轨编队运动；第三步，联列前两步得到的风速场中机器人总偏航角速度和总加速度，根据总偏航角速度和总加速度与机器人本身的控制力和角速度的关系，一方面求解出机器人的控制力，一方面求解出机器人本身期望的角速度为下面设计机器人的控制力矩作好准备。最后，设计机器人的控制力矩使得真实角速度和期望角速度间的误差减少到满足设计的要求。

根据下述实施例，可以更好的理解本发明。

如图6所示为本发明的设计流程图，由模块P1、P2、P3、P4、P5、P6和P7构成，各模块叙述如下：

1）模块P1

由于本发明是设计二维定常风速场中运动的机器人寻迹编队控制律，因此首先需要确定机器人在二维定常风速场中总线速度和总偏航角度，具体按下列步骤实现：

第一步：由机器人的线速度υ_i、转角θ_i以及风速矢量f_i(z_i)＝[f_i ^x(z_i),f_i ^y(z_i)]^T计算风速场中机器人的总偏航角度γ_i∈(-π/2,π/2)：

${\mathrm{\γ}}_i=\arctan \left(\frac{{\mathrm{\υ}}_i\sin {\mathrm{\θ}}_i+{f_i}^y}{{\mathrm{\υ}}_i\cos {\mathrm{\θ}}_i+{f_i}^x}\right);$

第二步：由总偏航角度γ_i，计算机器人在风速场中的总运动方向x_i以及垂直总运动方向的方向y_i：

x_i＝[cosγ_i,sinγ_i]^T，

y_i＝[-sinγ_i,cosγ_i]^T；

第三步：由机器人的线速度υ_i、风速矢量f_i、机器人在风速场中的总运动方向x_i以及垂直总运动方向的方向yi，计算机器人在风速场中的总线速度：

${\mathrm{\υ}}_{f_i}=f_i\·x_i+\sqrt{{\mathrm{\υ}}_i^2-{(f_i\·y_i)}^2}.$

2）模块P2

模块P2根据轨道函数以及风速场中机器人的位置和总运动方向计算寻迹误差和编队误差，包括如下步骤：

第一步：由轨道函数λ_i(z_i)和机器人的当前位置z_i计算位置寻迹误差d_i：

d_i＝λ_i(z_i)-0＝λ_i(z_i)；

第二步：由轨道函数λ_i(z_i)计算轨道的法向量N_i和切向量T_i：

$N_i=-\frac{\▿{\mathrm{\λ}}_i}{\left|\right|\▿{\mathrm{\λ}}_i\left|\right|},$

$T_i=\left[\begin{array}{cc}0& 1\\ -1& 0\end{array}\right]N_i;$

第三步：由总运动方向x_i和轨道的单位切方向T_i计算角度寻迹误差α_i(t)∈(-π,π]：

α_i＝arccos(x_i·T_i)。

3）模块P3

如图2所示，机器人i沿轨道切方向的运动可以改变机器人沿当前轨道运动的弧长s_i。与此同时，机器人i投影到轨道法向量上的运动可以改变机器人所在的轨道（即轨道函数值λ_i(z_i)发生变化），同样也可以改变弧长。因此，本方法选择目标轨道对应的参数σ_i来刻画机器人运动在Ω_i中所有轨道上的弧长。

模块P3根据机器人当前所在的轨道、弧长参数以及队形要求来计算广义弧长及其导数，具体的设计步骤如下：

第一步：规定好目标轨道上弧长计算的起始点和弧长参数σ_i，由此可以选择关于轨道函数的等值曲线簇中其他曲线的弧长计算的起始点满足其对应的弧长参数值与对应的弧长参数值相同。

第二步：由轨道函数λ_i和弧长参数σ_i，计算沿着机器人当前所在的轨道从起始点到机器人当前位置z_i间的弧长s_i：

$s_i({\mathrm{\λ}}_i,{\mathrm{\σ}}_i)\stackrel{\mathrm{\Δ}}{=}{\∫}_{{\mathrm{\σ}}_i^{*}}^{{\mathrm{\σ}}_i}\frac{\∂s_i({\mathrm{\λ}}_i,\mathrm{\τ})}{\∂\mathrm{\τ}}\mathrm{d\τ};$

第三步：由队形的要求设计好广义弧长函数ξ_i，即ξ_i是关于s_i一个具有二阶连续导数的函数，并且对于所有的s_i满足 $\∞>{\mathrm{\ϵ}}_3\≥\∂{\mathrm{\ξ}}_i/\∂s_i\≥{\mathrm{\ϵ}}_2>0$ 且 $\∞>{\mathrm{\ϵ}}_4\≥|{\∂}^2{\mathrm{\ξ}}_i/{\∂s}_i^2|$ 。由弧长s_i计算广义弧长ξ_i(s_i)。于是，广义弧长对时间的导数为

${\stackrel{\·}{\mathrm{\ξ}}}_i=\frac{\∂{\mathrm{\ξ}}_i}{\∂s_i}{\mathrm{\υ}}_{f_i}(\cos {\mathrm{\α}}_i+\frac{\∂s_i({\mathrm{\λ}}_i,{\mathrm{\σ}}_i)}{\∂f_i}||\▿{\mathrm{\λ}}_i|\left|\sin {\mathrm{\α}}_i\right).$

考虑到机器人最终会运动于目标轨道，因此选择在轨速度η_i为α_i＝0时的即：

${\mathrm{\η}}_i=\frac{\∂{\mathrm{\ξ}}_i}{\∂s_i}{\mathrm{\υ}}_{f_i}.$

这样做的好处就是可以简化计算量和控制律的设计。

4）模块P4

模块P4根据通信得到的相邻机器人的广义弧长ξ_j和在轨速度η_j信息，设计总偏航角速度使寻迹误差达到设计要求，设计总线加速度在实现编队的同时保证机器人本身的速率大于风速。具体设计步骤如下：

第一步：根据位置寻迹误差d_i、角度寻迹误差α_i以及通信得到的邻居的广义弧长ξ_j，设计总偏航角速度：

${\mathrm{\ω}}_{{\mathrm{\γ}}_i}={\mathrm{\υ}}_{f_i}({\mathrm{\κ}}_{{\mathrm{\α}}_i}^1\cos {\mathrm{\α}}_i+{\mathrm{\κ}}_{{\mathrm{\α}}_i}^2\sin {\mathrm{\α}}_i)+2{\mathrm{\υ}}_{f_i}\▿h_i\left|\right|\▿{\mathrm{\λ}}_i\left|\right|{\cos }^2\frac{{\mathrm{\α}}_i}{2}$

$+k_0{\mathrm{\η}}_i(-\sin {\mathrm{\α}}_i+2\frac{\∂s_i}{\∂{\mathrm{\λ}}_i}||\▿{\mathrm{\λ}}_i|\left|{\cos }^2\frac{{\mathrm{\α}}_i}{2}\right)\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{a}_{\mathrm{ij}}({\mathrm{\ξ}}_i-{\mathrm{\ξ}}_j)+k_1\sin \frac{{\mathrm{\α}}_i}{2}$

其中，控制参数k₀,k₁＞0，

${\mathrm{\κ}}_{{\mathrm{\α}}_i}^1=\frac{1}{\left|\right|\▿{\mathrm{\λ}}_i\left|\right|}{T}_i\·{\▿}^2{\mathrm{\λ}}_i{T}_i,$

${\mathrm{\κ}}_{{\mathrm{\α}}_i}^2=\frac{1}{\left|\right|\▿{\mathrm{\λ}}_i\left|\right|}{T}_i\·{\▿}^2{\mathrm{\λ}}_i{N}_i,$

$\▿h_i=\frac{1}{{\mathrm{\ϵ}}_1-d_i}-\frac{1}{d_i+1}+\ln (1+d_i)-\ln ({\mathrm{\ϵ}}_1-d_i)-\frac{1}{{\mathrm{\ϵ}}_1}+\ln {\mathrm{\ϵ}}_1.$

第二步：由通信得到的相邻机器人的广义弧长ξ_j和在轨速度η_j信息，设计总线加速度：

$a_{f_i}=-{({\mathrm{\η}}_i-{\mathrm{\η}}^m)}^2{\left(\frac{\∂{\mathrm{\ξ}}_i}{\∂s_i}\right)}^{-1}({({\mathrm{\η}}^{*}-{\mathrm{\η}}^m)}^{-2}\frac{{\∂}^2{\mathrm{\ξ}}_i}{{\∂s}_i^2}{\mathrm{\υ}}_{f_i}({\mathrm{\η}}_i+{\mathrm{\η}}_i(-2{\sin }^2\frac{{\mathrm{\α}}_i}{2}+\frac{\∂s_i}{\∂{\mathrm{\λ}}_i}||\▿{\mathrm{\λ}}_i|\left|\sin {\mathrm{\α}}_i\right))$

${-({\mathrm{\η}}_i-{\mathrm{\η}}^m)}^{-1}{({\mathrm{\η}}^{*}-{\mathrm{\η}}^m)}^{-1}{\stackrel{\·}{\mathrm{\η}}}^{*}+k_0\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{a}_{\mathrm{ij}}({\mathrm{\ξ}}_i-{\mathrm{\ξ}}_j)+k_2({\mathrm{\η}}_i-{\mathrm{\η}}^{*})+k_3\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{a}_{\mathrm{ij}}({\mathrm{\η}}_i-{\mathrm{\η}}_j)$

其中，控制参数k₂,k₃＞0。

5）模块P5

由于总偏航角速度并不是机器人本身的角速度ω_i，总线加速度并不是机器人的控制力F_i（或者说机器人本身的加速度F_i/m_i），但是与{ω_i,F_i}具有固定的转换关系，模块P5就是根据该转换关系由求解出机器人本身期望的角速度和控制力F_i。具体设计步骤如下：

第一步：对总偏航角度求导得到总偏航角速度与机器人本身的角速度ω_i和控制力F_i间的关系式：

${\mathrm{\ω}}_{{\mathrm{\γ}}_i}={\mathrm{\κ}}_{{\mathrm{\γ}}_i}^F{F}_i+{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}}{\mathrm{\ω}}_i,$

其中：

${\mathrm{\κ}}_{{\mathrm{\γ}}_i}^F=-\frac{{\mathrm{\υ}}_i^{-1}{\mathrm{\υ}}_{f_i}^{-1}(f_i\·y_i)}{m_i},$

${\mathrm{\κ}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}}=1-{\mathrm{\υ}}_{f_i}^{-1}(f_i\·x_i);$

第二步：对总线速度求导得到总线加速度与机器人本身的角速度ω_i和控制力F_i间的关系式

${\stackrel{\·}{\mathrm{\υ}}}_{f_i}=a_{f_i}={\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^F{F}_i+{\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^{\mathrm{\ω}}{\mathrm{\ω}}_i,$

其中：

${\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^F=\frac{{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^F}{m_i}[(f_i\·y_i)-{({\mathrm{\υ}}_i^2-{(f_i\·y_i)}^2)}^{-\frac{1}{2}}(f_i\·y_i)(f_i\·x_i)]+\frac{{\mathrm{\υ}}_i}{m_i}{({\mathrm{\υ}}_i^2-{(f_i\·y_i)}^2)}^{-\frac{1}{2}},$

${\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^{\mathrm{\ω}}={\mathrm{\κ}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}}[(f_i\·y_i)-{({\mathrm{\υ}}_i^2-{(f_i\·y_i)}^2)}^{-\frac{1}{2}}(f_i\·y_i)(f_i\·x_i)];$

第三步：由步骤d)设计出的总偏航角速度和总线加速度，根据推导出的关系式联列求解出机器人本身的期望角速度和控制力F_i

$\left[\begin{array}{c}{\stackrel{\‾}{\mathrm{\ω}}}_i\\ F_i\end{array}\right]={({\mathrm{\κ}}_{{\mathrm{\γ}}_i}^F{\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^{\mathrm{\ω}}-{\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^F{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}})}^{-1}\left[\begin{array}{c}{\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^{\mathrm{\ω}}{\mathrm{\ω}}_{{\mathrm{\γ}}_i}-{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}}{a}_{f_i}\\ {\mathrm{\κ}}_{{\mathrm{\γ}}_i}^F{\mathrm{\ω}}_{{\mathrm{\γ}}_i}-{\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^F{a}_{f_i}\end{array}\right].$

6）模块P6

模块P6根据真实与期望角速度的误差，设计机器人本身的控制力矩使得两者间的角速度误差减少到满足设计的要求。设计按下列步骤实现：

第一步：由机器人的真实角速度ω_i，计算其与模块P5得出的期望的角速度的角速度误差

${\mathrm{\ω}}_{e_i}={\mathrm{\ω}}_i-{\stackrel{\‾}{\mathrm{\ω}}}_i;$

第二步：由总偏航角速度对时间的导数和总线速度变化律对时间的导数，计算期望角速度对时间的导数

${\stackrel{\·}{\stackrel{\‾}{\mathrm{\ω}}}}_i=-{({\mathrm{\κ}}_{{\mathrm{\γ}}_i}^F{\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^{\mathrm{\ω}}-{\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^F{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}})}^{-2}({\stackrel{\·}{\mathrm{\κ}}}_{{\mathrm{\γ}}_i}^F{\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^{\mathrm{\ω}}+{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^F{\stackrel{\·}{\mathrm{\κ}}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^{\mathrm{\ω}}-{\stackrel{\·}{\mathrm{\κ}}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^F{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}}-{\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^F{\stackrel{\·}{\mathrm{\κ}}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}})({\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^{\mathrm{\ω}}{\mathrm{\ω}}_{{\mathrm{\γ}}_i}-{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}}{a}_{\mathrm{fi}})$

$+{({\mathrm{\κ}}_{{\mathrm{\γ}}_i}^F{\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^{\mathrm{\ω}}-{\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^F{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}})}^{-1}({\stackrel{\·}{\mathrm{\κ}}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^{\mathrm{\ω}}{\mathrm{\ω}}_{{\mathrm{\γ}}_i}+{\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^{\mathrm{\ω}}{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{\γ}}_i}-{\stackrel{\·}{\mathrm{\κ}}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}}{a}_{f_i}-{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}}{\stackrel{\·}{a}}_{f_i});$

其中：

${\stackrel{\·}{\mathrm{\κ}}}_{{\mathrm{\γ}}_i}^F=\frac{{\mathrm{\υ}}_i^{-1}{\mathrm{\υ}}_{f_i}^{-1}}{m_i}[(\frac{F_i}{m_i}{\mathrm{\υ}}_i^{-1}+({\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^F{F}_i+{\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^{\mathrm{\ω}}{\mathrm{\ω}}_i){\mathrm{\υ}}_{f_i}^{-1})(f_i\·y_i)+({\mathrm{\κ}}_{{\mathrm{\γ}}_i}^F{F}_i+{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}}{\mathrm{\ω}}_i)(f_i\·x_i)],$

${\stackrel{\·}{\mathrm{\κ}}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}}={\mathrm{\υ}}_{f_i}^{-2}({\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^F{F}_i+{\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^{\mathrm{\ω}}{\mathrm{\ω}}_i)(f_i\·x_i)-{\mathrm{\υ}}_{f_i}^{-1}({\mathrm{\κ}}_{{\mathrm{\γ}}_i}^F{F}_i+{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}}{\mathrm{\ω}}_i)(f_i\·y_i),$

${\stackrel{\·}{\mathrm{\κ}}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^{\mathrm{\ω}}={\stackrel{\·}{\mathrm{\κ}}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}}[(f_i\·y_i)-{({\mathrm{\υ}}_i^2-{(f_i\·y_i)}^2)}^{-\frac{1}{2}}(f_i\·y_i)(f_i\·x_i)]+\frac{{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^F}{m_i}[-({\mathrm{\κ}}_{{\mathrm{\γ}}_i}^F{F}_i+{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}}{\mathrm{\ω}}_i)(f_i\·x_i)$

$+{({\mathrm{\υ}}_i^2-{(f_i\·y_i)}^2)}^{-\frac{1}{2}}({({\mathrm{\υ}}_i^2-{(f_i\·y_i)}^2)}^{-1}(\frac{{\mathrm{\υ}}_i{F}_i}{m_i}+({\mathrm{\κ}}_{{\mathrm{\γ}}_i}^F{F}_i+{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}}{\mathrm{\ω}}_i)(f_i\·x_i)(f_i\·y_i))(f_i\·y_i)(f_i\·x_i)$

$+({\mathrm{\κ}}_{{\mathrm{\γ}}_i}^F{F}_i+{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}}{\mathrm{\ω}}_i){(f_i\·x_i)}^2-({\mathrm{\κ}}_{{\mathrm{\γ}}_i}^F{F}_i+{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}}{\mathrm{\ω}}_i){(f_i\·y_i)}^2\left)\right],$

${\stackrel{\·}{\mathrm{\κ}}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^F=\frac{{\stackrel{\·}{\mathrm{\κ}}}_{{\mathrm{\γ}}_i}^F}{m_i}[(f_i\·y_i)-{({\mathrm{\υ}}_i^2-{(f_i\·y_i)}^2)}^{-\frac{1}{2}}(f_i\·y_i)(f_i\·x_i)]+\frac{{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^F}{m_i}[-({\mathrm{\κ}}_{{\mathrm{\γ}}_i}^F{F}_i+{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}}{\mathrm{\ω}}_i)(f_i\·x_i)$

$+{({\mathrm{\υ}}_i^2-{(f_i\·y_i)}^2)}^{-\frac{1}{2}}({({\mathrm{\υ}}_i^2-{(f_i\·y_i)}^2)}^{-1}(\frac{{\mathrm{\υ}}_i{F}_i}{m_i}+({\mathrm{\κ}}_{{\mathrm{\γ}}_i}^F{F}_i+{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}}{\mathrm{\ω}}_i)(f_i\·x_i)(f_i\·y_i))(f_i\·y_i)(f_i\·x_i)$

$+({\mathrm{\κ}}_{{\mathrm{\γ}}_i}^F{F}_i+{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}}{\mathrm{\ω}}_i){(f_i\·x_i)}^2-({\mathrm{\κ}}_{{\mathrm{\γ}}_i}^F{F}_i+{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}}{\mathrm{\ω}}_i){(f_i\·y_i)}^2\left)\right]+\frac{F_i}{m_i^2}{({\mathrm{\υ}}_i^2-{(f_i\·y_i)}^2)}^{-\frac{1}{2}}$

$-\frac{{\mathrm{\υ}}_i}{m_i}{({\mathrm{\υ}}_i^2-{(f_i\·y_i)}^2)}^{-\frac{3}{2}}(\frac{{\mathrm{\υ}}_i{F}_i}{m_i}+({\mathrm{\κ}}_{{\mathrm{\γ}}_i}^F{F}_i+{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}}{\mathrm{\ω}}_i)(f_i\·x_i)(f_i\·y_i)),$

${\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{\γ}}_i}=({\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^F{F}_i+{\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^{\mathrm{\ω}}{\mathrm{\ω}}_i)({\mathrm{\κ}}_{{\mathrm{\α}}_i}^1\cos {\mathrm{\α}}_i+{\mathrm{\κ}}_{{\mathrm{\α}}_i}^2\sin {\mathrm{\α}}_i+2\▿h_i||\▿{\mathrm{\λ}}_i|\left|{\cos }^2\frac{{\mathrm{\α}}_i}{2}\right)+\frac{k_1}{2}{\stackrel{\·}{\mathrm{\α}}}_i\cos \frac{{\mathrm{\α}}_i}{2}$

$+{\mathrm{\υ}}_{f_i}[{\stackrel{\·}{\mathrm{\κ}}}_{{\mathrm{\α}}_i}^1\cos {\mathrm{\α}}_i+{\stackrel{\·}{\mathrm{\κ}}}_{{\mathrm{\α}}_i}^2\sin {\mathrm{\α}}_i+{\stackrel{\·}{\mathrm{\α}}}_i({\mathrm{\κ}}_{{\mathrm{\α}}_i}^1\cos {\mathrm{\α}}_i-{\mathrm{\κ}}_{{\mathrm{\α}}_i}^2\sin {\mathrm{\α}}_i-{\▿h}_i|\left|{\▿\mathrm{\λ}}_i\right|\left|\sin {\mathrm{\α}}_i\right)$

$+2{\stackrel{\·}{d}}_i{\▿}^2{h}_i\left|\right|\▿{\mathrm{\λ}}_i\left|\right|{\cos }^2\frac{{\mathrm{\α}}_i}{2}+2\▿h_i{\left|\right|\▿{\mathrm{\λ}}_i\left|\right|}^{-1}{\mathrm{\υ}}_{f_i}\▿{\mathrm{\λ}}_i\·{\▿}^2{\mathrm{\λ}}_i{x}_i{\cos }^2\frac{{\mathrm{\α}}_i}{2}]$

$+(k_0\frac{\∂{\mathrm{\ξ}}_i}{\∂s_i}({\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^F{F}_i+{\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^{\mathrm{\ω}}{\mathrm{\ω}}_i)+k_0\frac{{\∂}^2{\mathrm{\ξ}}_i}{{\∂s}_i^2}{\mathrm{\η}}_i{\mathrm{\υ}}_{f_i}(\cos {\mathrm{\α}}_i+\frac{\∂s_i({\mathrm{\λ}}_i,{\mathrm{\σ}}_i)}{\∂f_i}||\▿{\mathrm{\λ}}_i|\left|\sin {\mathrm{\α}}_i\right))\×$

$(-\sin {\mathrm{\α}}_i+2\frac{\∂s_i}{\∂{\mathrm{\λ}}_i}||\▿{\mathrm{\λ}}_i|\left|{\cos }^2\frac{{\mathrm{\α}}_i}{2}\right)\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{a}_{\mathrm{ij}}({\mathrm{\ξ}}_i-{\mathrm{\ξ}}_j)-k_0{\mathrm{\η}}_i({\stackrel{\·}{\mathrm{\α}}}_i\cos {\mathrm{\α}}_i+\frac{\∂s_i}{\∂{\mathrm{\λ}}_i}||\▿{\mathrm{\λ}}_i||{\stackrel{\·}{\mathrm{\α}}}_i\sin {\mathrm{\α}}_i$

$+2(\frac{{\∂}^2{s}_i}{{{\∂\mathrm{\λ}}_i}^2}{\stackrel{\·}{d}}_i+\frac{{\∂}^2{s}_i}{{\∂\mathrm{\λ}}_i\∂{\mathrm{\σ}}_i}{\stackrel{\·}{\mathrm{\σ}}}_i)\left|\right|\▿{\mathrm{\λ}}_i\left|\right|{\cos }^2\frac{{\mathrm{\α}}_i}{2}+2\frac{\∂s_i}{\∂{\mathrm{\λ}}_i}{\left|\right|\▿{\mathrm{\λ}}_i\left|\right|}^{-1}{\mathrm{\υ}}_{f_i}\▿{\mathrm{\λ}}_i\·{\▿}^2{\mathrm{\λ}}_i{x}_i{\cos }^2\frac{{\mathrm{\α}}_i}{2})\×$

$\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{a}_{\mathrm{ij}}({\mathrm{\ξ}}_i-{\mathrm{\ξ}}_j)+k_0{\mathrm{\η}}_i(-\sin {\mathrm{\α}}_i+2\frac{\∂s_i}{\∂{\mathrm{\λ}}_i}||\▿{\mathrm{\λ}}_i|\left|{\cos }^2\frac{{\mathrm{\α}}_i}{2}\right)\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{a}_{\mathrm{ij}}({\mathrm{\η}}_i-{\mathrm{\η}}_j+$

${\mathrm{\η}}_i(-2{\sin }^2\frac{{\mathrm{\α}}_i}{2}+\frac{\∂s_i}{\∂{\mathrm{\λ}}_i}||\▿{\mathrm{\λ}}_i|\left|\sin {\mathrm{\α}}_i\right)-{\mathrm{\η}}_j(-2{\sin }^2\frac{{\mathrm{\α}}_j}{2}+\frac{\∂s_j}{\∂{\mathrm{\λ}}_j}||\▿{\mathrm{\λ}}_j|\left|\sin {\mathrm{\α}}_j\right)),$

${\stackrel{\·}{d}}_i={\mathrm{\υ}}_{f_{\text{i}}}\left|\right|\▿{\mathrm{\λ}}_i\left|\right|\sin {\mathrm{\α}}_i,$

${\stackrel{\·}{\mathrm{\α}}}_i={\mathrm{\υ}}_{f_i}({\mathrm{\κ}}_{{\mathrm{\α}}_i}^1\cos {\mathrm{\α}}_i+{\mathrm{\κ}}_{{\mathrm{\α}}_i}^2\sin {\mathrm{\α}}_i)-({\mathrm{\κ}}_{{\mathrm{\γ}}_i}^u{u}_i+{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}}{\mathrm{\ω}}_i),$

${\stackrel{\·}{\mathrm{\κ}}}_{{\mathrm{\α}}_i}^1=\frac{{\mathrm{\κ}}_{{\mathrm{\α}}_i}^1}{\left|\right|\▿f_i\left|\right|}(N_i\·{\▿}^2{\mathrm{\λ}}_i{\mathrm{\υ}}_{f_i}{x}_i)-\frac{1}{{\left|\right|\▿{\mathrm{\λ}}_i\left|\right|}^2}\left(\left[\begin{array}{cc}0& 1\\ -1& 0\end{array}\right]({\▿}^2{\mathrm{\λ}}_i{\mathrm{\υ}}_{f_i}{x}_i-(N_i\·{\▿}^2{\mathrm{\λ}}_i{\mathrm{\υ}}_{f_i}{x}_i)N_i)\right)\·{\▿}^2{\mathrm{\λ}}_i{T}_i$

$+\frac{1}{\left|\right|\▿{\mathrm{\λ}}_i\left|\right|}(T_i\·({\▿}^3{\mathrm{\λ}}_i{\stackrel{\·}{d}}_i{T}_i-\frac{{\▿}^2{\mathrm{\λ}}_i}{\left|\right|\▿{\mathrm{\λ}}_i\left|\right|})\left[\begin{array}{cc}0& 1\\ -1& 0\end{array}\right]({\▿}^2{\mathrm{\λ}}_i{\mathrm{\υ}}_{f_i}{x}_i-(N_i\·{\▿}^2{\mathrm{\λ}}_i{\mathrm{\υ}}_{f_i}{x}_i)N_i))),$

${\stackrel{\·}{\mathrm{\κ}}}_{{\mathrm{\α}}_i}^2=\frac{{\mathrm{\κ}}_{{\mathrm{\α}}_i}^2}{\left|\right|\▿f_i\left|\right|}(N_i\·{\▿}^2{\mathrm{\λ}}_i{\mathrm{\υ}}_{f_i}{x}_i)-\frac{1}{{\left|\right|\▿{\mathrm{\λ}}_i\left|\right|}^2}\left(\left[\begin{array}{cc}0& 1\\ -1& 0\end{array}\right]({\▿}^2{\mathrm{\λ}}_i{\mathrm{\υ}}_{f_i}{x}_i-(N_i\·{\▿}^2{\mathrm{\λ}}_i{\mathrm{\υ}}_{f_i}{x}_i)N_i)\right)\·{\▿}^2{\mathrm{\λ}}_i{N}_i$

$+\frac{1}{\left|\right|\▿{\mathrm{\λ}}_i\left|\right|}(T_i\·({\▿}^3{\mathrm{\λ}}_i{\stackrel{\·}{d}}_i{N}_i-\frac{{\▿}^2{\mathrm{\λ}}_i}{\left|\right|\▿{\mathrm{\λ}}_i\left|\right|})({\▿}^2{\mathrm{\λ}}_i{\mathrm{\υ}}_{f_i}{x}_i-(N_i\·{\▿}^2{\mathrm{\λ}}_i{\mathrm{\υ}}_{f_i}{x}_i)N_i))),$

${\stackrel{\·}{a}}_{f_i}=({\mathrm{\η}}_i-{\mathrm{\η}}^m){\left(\frac{\∂{\mathrm{\ξ}}_i}{\∂s_i}\right)}^{-1}[({\mathrm{\η}}_i-{\mathrm{\η}}^m){\left(\frac{\∂{\mathrm{\ξ}}_i}{\∂s_i}\right)}^{-1}\frac{{\∂}^2{\mathrm{\ξ}}_i}{\∂s_i^2}{\mathrm{\υ}}_{f_i}(\cos {\mathrm{\α}}_i+\frac{\∂s_i({\mathrm{\λ}}_i,{\mathrm{\σ}}_i)}{\∂f_i}||\▿{\mathrm{\λ}}_i|\left|\sin {\mathrm{\α}}_i\right)-2{\stackrel{\·}{\mathrm{\η}}}_t]$

$\×({({\mathrm{\η}}^{*}-{\mathrm{\η}}^m)}^{-2}\frac{{\∂}^2{\mathrm{\ξ}}_i}{\∂s_i^2}{\mathrm{\υ}}_{f_i}({\mathrm{\η}}_i+{\mathrm{\η}}_i(-2{\sin }^2\frac{{\mathrm{\α}}_i}{2}+\frac{\∂s_i}{\∂{\mathrm{\λ}}_i}||\▿{\mathrm{\λ}}_i|\left|\sin {\mathrm{\α}}_i\right))-{({\mathrm{\η}}_i-{\mathrm{\η}}^m)}^{-1}{({\mathrm{\η}}^{*}-{\mathrm{\η}}^m)}^{-1}{\stackrel{\·}{\mathrm{\η}}}^{*}$

$+k_0\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{a}_{\mathrm{ij}}({\mathrm{\ξ}}_i-{\mathrm{\ξ}}_j)+k_2({\mathrm{\η}}_i-{\mathrm{\η}}^{*})+k_3\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{a}_{\mathrm{ij}}({\mathrm{\η}}_i-{\mathrm{\η}}_j))$

$-{({\mathrm{\η}}_i-{\mathrm{\η}}^m)}^2{\left(\frac{\∂{\mathrm{\ξ}}_i}{\∂s_i}\right)}^{-1}((-2{({\mathrm{\η}}^{*}-{\mathrm{\η}}^m)}^{-3}{\stackrel{\·}{\mathrm{\η}}}^{*}\frac{{\∂}^2{\mathrm{\ξ}}_i}{\∂s_i^2}{\mathrm{\υ}}_{f_i}+{({\mathrm{\η}}^{*}-{\mathrm{\η}}^m)}^{-2}\frac{{\∂}^3{\mathrm{\ξ}}_i}{\∂s_i^3}{\mathrm{\υ}}_{f_i}^2\×$

$(\cos {\mathrm{\α}}_i+\frac{\∂s_i({\mathrm{\λ}}_i,{\mathrm{\σ}}_i)}{\∂f_i}||\▿{\mathrm{\λ}}_i|\left|\sin {\mathrm{\α}}_i\right)+{({\mathrm{\η}}^{*}-{\mathrm{\η}}^m)}^{-2}\frac{{\∂}^2{\mathrm{\ξ}}_i}{\∂s_i^2}({\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^F{F}_i+{\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^{\mathrm{\ω}}{\mathrm{\ω}}_i))\×$

$({\mathrm{\η}}_i+{\mathrm{\η}}_i(-2{\sin }^2\frac{{\mathrm{\α}}_i}{2}+\frac{\∂s_i}{\∂{\mathrm{\λ}}_i}||\▿{\mathrm{\λ}}_i|\left|\sin {\mathrm{\α}}_i\right))+{({\mathrm{\η}}^{*}-{\mathrm{\η}}^m)}^{-2}\frac{{\∂}^2{\mathrm{\ξ}}_i}{\∂s_i^2}{\mathrm{\υ}}_{f_i}\×$

$({\stackrel{\·}{\mathrm{\η}}}_i(1+(-2{\sin }^2\frac{{\mathrm{\α}}_i}{2}+\frac{\∂s_i}{\∂{\mathrm{\λ}}_i}||\▿{\mathrm{\λ}}_i|\left|\sin {\mathrm{\α}}_i\right))+{\mathrm{\η}}_i(-{\stackrel{\·}{\mathrm{\α}}}_i\sin {\mathrm{\α}}_i+(\frac{{\∂}^2{s}_i}{\∂{\mathrm{\λ}}_i^2}{\stackrel{\·}{d}}_i+\frac{{\∂}^2{s}_i}{\∂{\mathrm{\λ}}_i\∂{\mathrm{\σ}}_i}{\stackrel{\·}{\mathrm{\σ}}}_i)\×$

$\left|\right|\▿{\mathrm{\λ}}_i\left|\right|\sin {\mathrm{\α}}_i+\frac{\∂s_i}{\∂{\mathrm{\λ}}_i}{\left|\right|\▿{\mathrm{\λ}}_i\left|\right|}^{-1}{\▿}^2{\mathrm{\λ}}_i{\mathrm{\υ}}_{f_i}\▿{\mathrm{\λ}}_i\·{\▿}^2{\mathrm{\λ}}_i{x}_i\sin {\mathrm{\α}}_i+{\stackrel{\·}{\mathrm{\α}}}_i\frac{\∂s_i}{\∂{\mathrm{\λ}}_i}\left|\right|\▿{\mathrm{\λ}}_i\left|\right|\sin {\mathrm{\α}}_i\left)\right)$

$+{({\mathrm{\η}}_i-{\mathrm{\η}}^m)}^{-2}{\stackrel{\·}{\mathrm{\η}}}_i{({\mathrm{\η}}^{*}-{\mathrm{\η}}^m)}^{-1}{\stackrel{\·}{\mathrm{\η}}}^{*}+{({\mathrm{\η}}_i-{\mathrm{\η}}^m)}^{-1}{({\mathrm{\η}}^{*}-{\mathrm{\η}}^m)}^{-2}{\left({\stackrel{\·}{\mathrm{\η}}}^{*}\right)}^2-{({\mathrm{\η}}_i-{\mathrm{\η}}^m)}^{-1}{({\mathrm{\η}}^{*}-{\mathrm{\η}}^m)}^{-1}{\stackrel{\·\·}{\mathrm{\η}}}^{*}$

$+k_0\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{a}_{\mathrm{ij}}({\mathrm{\η}}_i-{\mathrm{\η}}_j+{\mathrm{\η}}_i(-2{\sin }^2\frac{{\mathrm{\α}}_i}{2}+\frac{\∂s_i}{\∂{\mathrm{\λ}}_i}||\▿{\mathrm{\λ}}_i|\left|\sin {\mathrm{\α}}_i\right)-{\mathrm{\η}}_j(-2{\sin }^2\frac{{\mathrm{\α}}_j}{2}+\frac{\∂s_j}{\∂{\mathrm{\λ}}_j}||\▿{\mathrm{\λ}}_j|\left|\sin {\mathrm{\α}}_j\right))$

$+k_2({\stackrel{\·}{\mathrm{\η}}}_i-{\stackrel{\·}{\mathrm{\η}}}^{*})+k_3\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{a}_{\mathrm{ij}}({\stackrel{\·}{\mathrm{\η}}}_i-{\stackrel{\·}{\mathrm{\η}}}_j)),$

${\stackrel{\·}{\mathrm{\η}}}_i=\frac{\∂{\mathrm{\ξ}}_i}{\∂s_i}({\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^u{u}_i+{\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^{\mathrm{\ω}}{\mathrm{\ω}}_i)+\frac{{\∂}^2{\mathrm{\ξ}}_i}{\∂s_i^2}{\mathrm{\υ}}_{f_i}({\mathrm{\η}}_i+{\mathrm{\η}}_i(-2{\sin }^2\frac{{\mathrm{\α}}_i}{2}+\frac{\∂s_i}{\∂{\mathrm{\λ}}_i}||\▿{\mathrm{\λ}}_i|\left|\sin {\mathrm{\α}}_i\right)),$

${\stackrel{\·}{\mathrm{\σ}}}_i={\mathrm{\υ}}_{f_i}\cos {\mathrm{\α}}_i{\left(\frac{\∂s_i}{\∂{\mathrm{\σ}}_i}\right)}^{-1}.$

第三步：设计机器人的控制力矩N_i

$N_i=I_i({\stackrel{\·}{\stackrel{\‾}{\mathrm{\ω}}}}_i-{\mathrm{\κ}}_{{\mathrm{\γ}}_i}^{\mathrm{\ω}}\tan \frac{{\mathrm{\α}}_i}{2}+{\mathrm{\κ}}_{{\mathrm{\υ}}_{\mathrm{fi}}}^{\mathrm{\ω}}\frac{\∂{\mathrm{\ξ}}_i}{\∂s_i}{({\mathrm{\η}}_i-{\mathrm{\η}}^m)}^{-2}({\mathrm{\η}}_i-{\mathrm{\η}}^{*})-k_4{\mathrm{\ω}}_{e_i})$

其中，控制参数k₄＞0。

7）模块P7

模块P7将模块P5和P6的得到机器人的控制力和力矩发送给下位机，通过伺服系统完成机器人的运动控制，具体的按照以下步骤实现：

由上位机将机器人的控制力和力矩发送给下位机中，通过伺服系统来完成机器人的运动控制，并返回到模块P1。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (7)

1.一种二维定常风速场中多机器人的寻迹编队控制方法，其特征在于：定常风速场是指与时间无关的二维风速矢量描述的欧拉风速场，并且机器人的运动轨道是用轨道函数描述的，该方法包括如下步骤：

a).由风速矢量计算风速场中机器人的总偏航角度和总线速度；

b).由轨道函数计算寻迹误差；

c).由轨道函数和弧长参数计算机器人沿轨道运动的广义弧长和在轨速度；

d).由通信得到的邻居信息，设计总偏航角速度和总线加速度使得寻迹误差和队形达到设计要求，并保证机器人本身的速率大于风速；

e).由总偏航角速度和总线加速度联立求解机器人本身期望角速度和控制力；

f).计算真实与期望角速度的误差，设计机器人的控制力矩；

g).由伺服系统来完成机器人的运动控制。

2.根据权利要求1所述的二维定常风速场中多机器人的寻迹编队控制方法，其特征在于：

其中所述步骤a)包括如下步骤：

a1)由机器人本身的线速度、转角以及风速矢量，计算风速场中机器人的总偏航角度；

a2)由风速场中机器人总偏航角度，计算机器人总运动方向以及与总运动方向垂直的方向；

a3)由机器人本身的线速度、风速矢量、机器人总运动方向以及与总运动方向垂直的方向，计算风速场中机器人的总线速度。

3.根据权利要求1所述的二维定常风速场中多机器人的寻迹编队控制方法，其特征在于：

其中所述步骤b)包括如下步骤：

b1)由风速场中机器人的位置和轨道函数，计算轨道函数值与期望值间的位置寻迹误差；

b2)由轨道函数计算轨道的单位法向量和单位切向量；

b3)由风速场中机器人的总运动方向和轨道的单位切方向，计算角度寻迹误差。

4.根据权利要求1所述的二维定常风速场中多机器人的寻迹编队控制方法，其特征在于：

其中所述步骤c)包括如下步骤：

c1)规定好目标轨道的弧长参数和弧长计算的起始点，由此确定关于轨道函数的等值曲线簇中其他曲线的弧长计算的起始点；

c2)由轨道函数和弧长参数计算机器人沿轨道从起始点到当前位置间的弧长；

c3)由队形要求确定广义弧长与弧长间的函数关系，计算广义弧长和在轨速度。

5.根据权利要求1所述的二维定常风速场中多机器人的寻迹编队控制方法，其特征在于：

其中所述步骤d)包括如下步骤：

d1)由位置寻迹误差、角度寻迹误差以及通信得到的相邻机器人的广义弧长，设计风速场中机器人总偏航角速度使寻迹误差达到设计要求；

d2)由通信得到的相邻机器人的广义弧长和在轨速度，设计风速场中机器人总线加速度在实现编队同时保证机器人本身的速率大于风速。

6.根据权利要求1所述的二维定常风速场中多机器人的寻迹编队控制方法，其特征在于：

其中所述步骤e)包括如下步骤：

e1)对总偏航角度求导得到总偏航角速度与机器人本身的角速度和控制力间的关系式；

e2)对总线速度求导得到总线加速度与机器人本身的角速度和控制力间的关系式；

e3)由步骤d)设计出的总偏航角速度和总线加速度，根据推导出的关系式联立求解出机器人本身期望角速度和控制力。

7.根据权利要求1所述的二维定常风速场中多机器人的寻迹编队控制方法，其特征在于：

其中所述步骤f)包括如下步骤：

f1)由机器人真实角速度，计算其与步骤e)得出的期望角速度间的角速度误差；

f2)由总偏航角速度和总线加速度对时间的导数，计算期望角速度对时间的导数；

f3)根据角速度误差和期望角速度对时间的导数，设计机器人的力矩使得角速度误差减少到满足的设计要求。