CN105068421A - 多移动机器人的二自由度协同控制方法 - Google Patents

Abstract

多移动机器人的二自由度协同控制方法，步骤如下：1)根据拉格朗日方法获取单个移动机器人的模型2)根据模型参数确定能够保证单个机器人稳定的个体控制器的稳定集合；3)选取个体控制器稳定域内合适的一点，将个体控制器和原来的模型结合成新的模型；4)在所求取的PID控制器稳定集合中选取合适的值，并执行PID控制程序，使多移动机器人系统完成协同控制。本发明适用于各种包含有单输入单输出移动机器人的多移动机器人系统，通过选取求得的两类控制器稳定域内的参数，可以有效地解决不稳定移动机器人的协同控制问题。

Description

多移动机器人的二自由度协同控制方法

技术领域

本发明涉及多移动机器人系统的协同控制方法。

背景技术

多移动机器人系统是分布式人工智能系统领域的一个相当重要的分支。多移动机器人系统在20世纪80年代后期已经成为分布式人工智能研究中的主要研究对象。研究多移动机器人系统的主要目的是功能相对简单的多个移动机器人之间进行分布式协同控制，完成复杂任务，为在极端环境下的作业提供了可靠的支持。

多移动机器人系统是由多个简单的移动机器人有机组合而成，它包含了两方面的基本特点：一方面需要多个移动机器人模块，设计由单个移动机器人形成的模块相对容易，这也保证了整个系统的良好的模块性并且方便扩展，同时也有效地降低了应用成本；另一方面，多移动机器人系统是一个注重协调的系统，在这个系统中的各个移动机器人的功能可以比较简单，但是它们通过互相之间的通信、合作、协调、调度、管理和控制能完成复杂的整体功能。

关于多移动机器人的研究主要包括移动机器人的知识、目标、技能、规划等，以及移动机器人在解决问题时的协调行动。研究者主要研究移动机器人个体之间的信息交互、协同合作、冲突解决等方面，注重多个移动机器人之间的紧密群体合作，而不是单个移动机器人能力的自治和发挥，说明如何分析、设计和集成多个移动机器人，从而构成互相协作的系统是主要目标。

所谓一致性，即在一个多移动机器人系统中，所有的移动机器人最终状态能够趋于一致。一致性问题的出现主要源于合作控制问题。对于多移动机器人系统的合作控制问题，移动机器人之间共享信息是保证合作的一个前提条件，共享信息可以以多种形式出现，比如说一个共同的目标，一种共同的控制算法，或者相对的位置信息。当一组移动机器人要合作共同去完成一项任务，合作控制策略的有效性表现在，多移动机器人必须能够应对各种不可预知的形势和环境的改变，这就要求移动机器人随着环境的改变能够达到一致。因此，多移动机器人达到一致是实现协调合作控制的一个首要条件。

近年来，针对多移动机器人控制的分析与研究，已在一致性和协同控制方面取得了很大的进展。Li在文献Distributedconsensusoflinearmulti-agentsystemswithadaptivedynamicprotocols(Automatica,2013,49(7):1986-1995.)中对具有领导和没有领导的线性多智能体系统，分析了系统一致性问题。Philip在文献AdaptiveConsensusControlforaClassofNonlinearMultiagentTime-DelaySystemsUsingNeuralNetworks(IEEEtransactionsonneuralnetworksandlearningsystems,2014,25(6):1217-1226.)中对于一类具有时滞的非线性多智能体系统，介绍了自适应神经网络一致性控制方法。Yu在文献Distributedcontrolgainsdesignforconsensusinmulti-agentsystemswithsecond-ordernonlineardynamics(Automatica,2013,49(7):2107-2115.)中基于相邻智能体的局部信息对每个跟随者提出了一个分布式自适应律。然而上述研究都集中于移动机器人个体间的网络拓扑特性，通过改变网络拓扑参数及移动机器人间的控制协议达到控制目标，而将单个移动机器人认为是具有一定动态特性的质点，无法从根本上进行系统设计使整个多移动机器人系统满足一定的性能指标要求，也很难灵活地实现不同的全局控制目标。

发明内容

本发明要克服现有技术的上述缺点，提供一种多移动机器人的二自由度协同控制方法，改善多移动机器人的一致性和协同控制。

本发明提出了二自由度控制器稳定集的求法。二自由度控制器包括了两类控制器，一类是与移动机器人自身动态有关的个体控制器，其目的是控制移动机器人，使其达到稳定状态；另一类是影响网络拓扑动态的耦合控制器，其目的在于多移动机器人系统通过拓扑结构的信息交互使各个移动机器人的终值能够趋向一致。使用这两类控制器能分别有效地控制单个移动机器人的稳定性和整个多移动机器人系统的一致性，使得控制更加灵活，更加精确。这两类控制器的有机结合，能有效地提高多移动机器人系统的鲁棒性、快速性。

本发明的目的在于：针对执行军事任务或者在狭小空间中搬运大物件的多移动机器人系统中，只考虑环境对整个系统中移动机器人状态的调整，而不考虑单个移动机器人当自身状态改变时的稳定性的状况，提出二自由度的控制器设计方法。首先基于Hermite-Biehler稳定判据的拓展，设计能使单个移动机器人稳定的PID(比例-积分-微分)控制器，即个体控制器。然后在所求得的个体控制器稳定参数范围内选取合适的控制器参数，与原来的移动机器人结合成新的模型。接着运用矩阵原理，分解拓扑结构，得到能使新的模型达成协同控制的条件。最后通过所得的条件求取控制器参数，即耦合控制器。只要在所得到的个体控制器稳定的范围内选取参数值，就能使单个移动机器人达到稳定状态；只要在所得到的耦合控制器可取范围内选取参数值，就能使整个系统达到一致，完成协同控制。

本发明是通过以下技术方案实现的：先基于刚体运动学原理，考虑移动机器人的输入输出等时滞的影响，利用拉格朗日方法建立单个移动机器人的模型，为了使其能够不受外界干扰按照设定的速度前进，在现有的鲁棒控制器设计方法以及控制系统稳定性分析结果的基础上，采用单位反馈控制结构，结合改进的Nyquist稳定判据和推广的Hermite定理，基于移动机器人模型参数计算出个体控制器中控制参数的稳定域；将移动机器人的模型和个体控制器结合成一个新的模型，针对这个模型，运用矩阵原理，分解拓扑结构，得到能使新的模型达成协同控制的条件。通过所得的条件求取耦合控制器参数。最后，通过在算法获得的稳定集合中进行控制参数选取和调节，选取能够满足控制要求的控制参数并得到控制信号，使多移动机器人系统达到一致，实现对多移动机器人的协同控制。具体步骤如下：多移动机器人的二自由度协同控制方法，包括如下步骤：

步骤1，先基于刚体运动学原理，考虑移动机器人的输入输出等时滞的影响，利用拉格朗日方法建立具有如下传递函数形式的移动机器人模型g(s)：

若令g(s)的分子和分母分别为V(s)和U(s)，则式(1)中，s为复平面上的一个变量，u和v分别表示U(s)和V(s)中s项的最高阶次，u>v，e为数学常数，sⁱ为s的i次方，在U(s)中，i为整数且i＝0,…,u-1，j为整数且j＝1,…,h_i，h_i是U(s)中sⁱ项所对应的时滞块的个数，τ_ij和α_ij是U(s)中sⁱ项所对应的时滞和系数，在V(s)中，i为整数且i＝0,…,v-1，j为整数且j＝1,…,f_i，f_i是V(s)中sⁱ项所对应的时滞块的个数，θ_ij,β_ij和是V(s)中sⁱ项所对应的时滞和系数，β_v为V(s)的最高阶次项的系数。然后，将辨识出的模型参数送到主机的存储单元RAM中。

步骤2，建立多移动机器人系统的二自由度控制结构图如图2所示，图中，g(s)为单个移动机器人，c₁(s)和c₂(s)是具有以下形式的PID控制器：

$c_1\left(s\right)=c_2\left(s\right)=k_p+\frac{k_i}{s}+k_{d}s---\left(2\right)$

其中，k_p、k_i和k_d分别为PID控制器的比例、积分和微分增益。将图2转化成系统控制框图如图3所示，图中，为控制单个移动机器人状态的个体反馈控制器矩阵，为控制系统达到一致性的耦合控制器矩阵，为具有时滞的移动机器人模型矩阵，为与单个移动机器人相邻的邻居个数矩阵，k为单个移动机器人的邻居个数，并且E为单位矩阵，为克罗内克积，A为邻接矩阵，r为系统输入，y为系统输出。

步骤3，根据以下步骤确定能使移动机器人稳定的个体控制器c₁(s)的参数范围单个移动机器人的系统闭环结构图如图4所示，图中，r1为系统输入，y1为系统输出：

(1)确定系统闭环特征函数δ(s)为

δ(s)＝sU(s)+kV(s)(k_ds²+k_ps+k_i)(3)

令ω为频率，且令s＝jω，U_r(ω)和U_i(ω)为U(jω)的实部和虚部，V_r(ω)和V_i(ω)为V(jω)的实部和虚部，令δ'(jω)＝δ(jω)*V(-jω)，得到

δ'(jω)＝p(ω,k_i,k_d)+jq(ω,k_p)

其中p(ω,k_i,k_d)表示δ'(jω)的实部，q(ω,k_p)表示δ'(jω)的虚部，

p(ω,k_i,k_d)＝p₁(ω)+k(k_i-ω²k_d)[V_r ²(ω)+V_i ²(ω)](4)

q(ω,k_p)＝ω{q₁(ω)+kk_p[V_r ²(ω)+V_i ²(ω)]}(5)

令p₁(ω)＝p(ω,k_i,k_d)-k(k_i-ω²k_d)[V_r ²(ω)+V_i ²(ω)]，q₁(ω)＝q(ω,k_p)/ω-kk_p[V_r ²(ω)+V_i ²(ω)]，则p₁(ω)和q₁(ω)分别为：

p₁(ω)＝ω[U_r(ω)V_i(ω)-U_i(ω)V_r(ω)](6)

q₁(ω)＝[U_r(ω)V_r(ω)+U_i(ω)V_i(ω)](7)

(2)选取一个足够大的频率值ω^*。

(3)令Re[V(jω)]和Im[V(jω)]分别为V(jω)的实部与虚部，根据下式计算ω由0变化到ω^*时所对应的V(jω)的幅角变化范围

这里，当v为偶数时，w₀<w₁<w₂<…<w_e是Im[V(jω)]在[0,ω^*)的零点；反之当v为奇数时，w₀<w₁<w₂<…<w_e是Re[V(jω)]在[0,ω^*)的零点,e+1表示零点的个数，sgn(x)为符号函数，其中x为实数，当x>0时，sgn(x)＝1，当x＝0时，sgn(x)＝0，当x<0时，sgn(x)＝-1。

(4)确定k_p的最大可允许稳定范围：

令Q表示f₁(ω)＝k_p与f₂(ω)＝-q₁(ω)/{k[V_r ²(ω)+V_i ²(ω)]}在(0,ω^*)上的交点数，给出满足下式的k_p范围，即为k_p的最大可允许稳定范围。

其中，j(V)表示V(s)在正虚轴上的零点数，为ω由0变化到ω^*时所对应的V(jω)的幅角变化范围。

(5)令k_p的最大可允许稳定范围为[k_pmin,k_pmax]，k_pmin为k_p允许的最小值，k_pmax为k_p允许的最大值，将k_p值在该范围内进行等间隔的遍历，即每个遍历点为其中F为遍历点之间的间隔，z为采样频率，z＝0,1,…,F。

(6)对于其中一个遍历点根据以下步骤确定能够保证闭环系统稳定的(k_d,k_i)二维稳定域：

(a)计算方程q(ω,k_p)＝0在区间[0,ω^*)内的实零点，将这些实零点按照从小到大的顺序表示为ω₀,ω₁,ω₂,…,ω_c-1，其中，c为实零点的个数；

(b)令t＝0,1,2,…,c-1，选择整数i_t的值：

(i)如果V(-jω_t)＝0，那么i_t＝0；

(ii)如果V(-s)在原点处存在零点，那么

$i_0=\{sgn\left(\frac{d}{d\mathrm{\&omega;}}\right\{\mathrm{\&omega;}\&lsqb;U_r\left(\mathrm{\&omega;}\right)V_i\left(\mathrm{\&omega;}\right)-U_i\left(\mathrm{\&omega;}\right)V_r\left(\mathrm{\&omega;}\right)\&rsqb;\}{|}_{\mathrm{\&omega;}=0})$

(iii)若不满足(i)和(ii)，i_t＝1或-1，其准确值需根据下述稳定性条件而定；

(iv)令I＝{i₀,i₁,…}，I为i_t(t＝0,1,2,…c-1)的序列，确定能够满足下述等式的所有I：

$\frac{u+1}{2}-\underset{s=j\mathrm{\&omega;},\mathrm{\&omega;}\&Element;(0,\mathrm{\&omega;}*)}{\mathrm{\&Delta;}\arg \&lsqb;V\left(s\right)\&rsqb;}=\frac{\mathrm{\&pi;}}{2}\mathrm{\&gamma;}\left(I\right)---\left(10\right)$

其中，这里ω_c-1是q(ω,k_p)＝0在区间[0,ω^*)内的第c个实零点，ε是一足够小的正实数，为大于ω_c-1且无限接近ω_c-1的值，γ(I)为i_t(t＝0,1,2,…c-1)的字符组合，由下式给出：

(c)对于遍历点(k_d,k_i)二维稳定域由下式决定:

[k_i-A(ω_t)k_d+B(ω_t)]i_t>0(12)

其中，ω_t为式(5)在区间[0,ω^*)内的实零点，t＝0,1,…c-1，令式(4)等于零，则 $A\left({\mathrm{\&omega;}}_t\right)={\mathrm{\&omega;}}_t^2$ 为k_d的系数的绝对值， $B\left({\mathrm{\&omega;}}_t\right)=p_1\left({\mathrm{\&omega;}}_t\right)/\{k\&lsqb;V_r^2\left({\mathrm{\&omega;}}_t\right)+V_i^2\left({\mathrm{\&omega;}}_t\right)\&rsqb;\}$ 为常数项。通过求取所有ω_t所对应的由式(12)所决定的不等式组的交集，即可确定具有凸多边形特性的(k_d,k_i)二维稳定域。

(7)对于步骤(5)中所给出的k_p的每个遍历点，都重复步骤(6)，确定能使闭环系统稳定的所有个体控制器c₁(s)的参数集合。

步骤4，根据以下步骤确定能使多移动机器人系统一致的耦合控制器c₂(s)的参数范围：

41.选取步骤3得到的稳定范围内合适的一点作为个体控制器c₁(s)的参数，将个体控制器c₁(s)与原移动机器人模型g(s)进行整合得到新的模型g'(s)，其中由此可将复杂的多反馈系统简化为具有多时滞单反馈的多输入多输出多移动机器人系统，如图5所示，则系统的特征方程为：

$\widetilde{\mathrm{\&delta;}}\left(s\right)=\det \&lsqb;E+(-A)G^{\&prime;}\left(s\right)C_2\left(s\right)\&rsqb;$

其中，E为单位矩阵，A表示邻接矩阵。因为-A是可对角化矩阵，一定存在一个可逆矩阵P使得-A＝P^-1ΛP，其中，Λ为对角矩阵。对特征方程进行变形，得到其特征方程为：

$\widetilde{\mathrm{\&delta;}}\left(s\right)=\det \&lsqb;E+P^{-1}{\mathrm{\&Lambda;PG}}^{\&prime;}\left(s\right)C_2\left(s\right)\&rsqb;=\underset{i=1}{\overset{N}{\&Pi;}}\det \&lsqb;1+{\mathrm{\&lambda;}}_i{g}^{\&prime;}\left(s\right)c_2\left(s\right)\&rsqb;---\left(13\right)$

其中，表示新的传递函数矩阵，C₂(s)表示耦合控制器矩阵，g'(s)为各个子系统的传递函数，c₂(s)为各个子系统上的PID控制器，λ_i为矩阵-A的特征值，N为特征值个数。

42.对于不同的λ值，重复步骤3的方法，确定能使整个系统达到一致，完成系统协同控制的所有PID控制器的集合。

步骤5，将移动机器人的模型参数输入个体控制器c₁(s)参数的计算单元，由步骤3计算个体控制器c₁(s)的稳定集合，根据需要在个体控制器的稳定集合中选取控制参数，将控制参数输入监控模块执行预调控制程序：经模拟量输入信号，经A/D装换模块将模拟信号转化为数字信号输入，将输入值与设定值进行比较可得到不同的跟踪误差按照离散域PID控制算式计算控制信号增量Δu(n₁)的值，与前一时刻的控制信号u(n₁-1)通过加法器进行加法运算就得到当前时刻的输出控制信号u(n₁)，其中，n₁为当前时刻的采样步数。Δu(n₁)计算公式如下：

Δu(n₁)＝a₁e(n₁)+a₂e(n₁-1)+a₃e(n₁-2)(14)

其中，a₁＝(k_p1R₁+k_d1+R₁ ²k_i1)/R₁，a₂＝-(k_p1R₁+2k_d1)/R₁，a₃＝k_d1/R₁，R₁为系统采样周期，Δu(n₁)为当前采样步数为n₁时控制器输出信号增量，e(n₁)为当前采样步数为n₁时的跟踪误差，e(n₁-1)为采样步数为n₁-1时的跟踪误差，e(n₁-2)为采样步数为n₁-2时的跟踪误差。通过对PID控制器的调节减少误差以确保移动机器人的稳定运行。

步骤6，将步骤5中经过预调系统镇定的个体控制器施加于每个移动机器人，以便于对稳定的移动机器人进行协同控制。

步骤7，将个体控制器与原模型结合形成新的模型，并将模型参数输入耦合控制器c₂(s)参数的计算单元，由步骤4计算耦合控制器c₂(s)的稳定集合。然后由监控模块执行事先编制好的控制程序：经模拟量输入通道传输信号，并将信号接入检测变送装置，再经A/D转换后得到数字量输入信号与此时的系统设定值比较后得到不同时刻的跟踪误差，基于跟踪误差，在所获得的c₂(s)稳定集合中选取控制参数，然后按照控制器c₂(s)的离散域PID控制算式计算控制信号增量Δu(n₂)的值，与前一时刻的控制信号u(n₂-1)通过加法器进行加法运算就得到当前时刻的输出控制信号u(n₂)，其中，n₂为当前时刻的采样步数。Δu(n₂)计算公式如下：

Δu(n₂)＝b₁e(n₂)+b₂e(n₂-1)+b₃e(n₂-2)(15)

其中，b₁＝(k_pR₂+k_d+R₂ ²k_i)/R₂，b₂＝-(k_pR₂+2k_d)/R₂，b₃＝k_d/R₂，R₂为系统采样周期，Δu(n₂)为当前采样步数为n₂时控制器输出信号增量，e(n₂)为当前采样步数为n₂时的跟踪误差，e(n₂-1)为采样步数为n₂-1时的跟踪误差，e(n₂-2)为采样步数为n₂-2时的跟踪误差。通过c₂(s)控制器程序调节各移动机器人之间的速度差来使整个系统达到一致，完成协同控制。

对多移动机器人的协同控制采用本发明提出的将个体控制器和耦合控制器结合的二自由度控制器的稳定集的求法，最大的特点就是：通过分别设计两类控制器，个体控制器只需要控制移动机器人自身的稳定性，耦合控制器只需要控制整个系统的一致性，将单个移动机器人的稳定性控制和整个系统的协同控制有机的结合，完成对不稳定多移动机器人的协同控制。

本发明的优点是：只要在求取的稳定集合中选取控制参数，均能保证移动机器人自身的稳定和多移动机器人系统的一致性。两类控制器分别控制单个移动机器人的稳定性和整个多移动机器人系统的一致性使得控制更加灵活、精确，调节更加方便。使用二自由度控制方法不仅能达到全局的控制目标，而且使系统具有良好的全局性能和局部性能，提高多移动机器人系统的鲁棒性和稳定性，系统的抗干扰能力也得到显著改善。

附图说明

图1为本发明方法采用的工作流程图。

图2为本发明采用的多移动机器人系统结构图。

图3为本发明采用的闭环控制框图。其中C₁(s)为控制单个移动机器人状态的个体控制器矩阵，C₂(s)为控制系统达到一致性的耦合控制器矩阵，G(s)为具有时滞的移动机器人矩阵，r为系统输入，y为系统输出，K为与单个移动机器人相邻的邻居个数矩阵，A为邻接矩阵。

图4为本发明采用的控制移动机器人稳定的闭环控制框图，其中r1为系统输入，y1为系统输出。

图5为本发明采用的系统闭环控制框图的简化图。

图6为本发明实施例中个体控制器c₁(s)的k_p1的可取范围。

图7为本发明实施例中k_p1＝2时，(k_i1,k_d1)的可取范围。

图8为本发明实施例中个体控制器稳定的三维范围。

图9为本发明实施例中选取个体控制器稳定域内的控制参数(k_p1,k_i1,k_d1)为(k_p1,k_i1,k_d1)＝(2,15,0.1)时的单位阶跃响应曲线。

图10为本发明实施例中选取在个体控制器稳定域边界上的控制参数(k_p1,k_i1,k_d1)＝(2,27,0.1)时的单位阶跃响应曲线。

图11为本发明实施例中当λ＝2时，耦合控制器c₂(s)的k_p2可取范围。

图12为本发明实施例中当λ＝1时，耦合控制器c₂(s)的k_p2可取范围。

图13为本发明实施例中k_p2＝1时，(k_i2,k_d2)的可取范围。

图14为本发明实施例中耦合控制器c₂(s)对于不同λ值的稳定域交集。

图15为本发明实施例中耦合控制器c₂(s)关于(k_p2,k_i2,k_d2)的三维稳定域。

图16为本发明实施例中选取耦合控制器稳定域中的点(k_p2,k_i2,k_d2)＝(1,1,0.1)的系统响应。

图17为本发明实施例中选取耦合控制器稳定域边界内的点(k_p2,k_i2,k_d2)＝(1,-19,-0.1)的系统响应。

图18为本发明实施例中选取耦合控制器稳定域边界外的点(k_p2,k_i2,k_d2)＝(1,-20,-0.1)的系统响应。

具体实施方式

以下结合附图和实施例对本发明的技术方案作进一步描述。

先基于刚体运动学原理，考虑移动机器人的输入输出等时滞的影响，利用拉格朗日方法建立单个移动机器人的模型。通过改进的Nyquist稳定判据和推广的Hermite定理，得到个体控制器c₁(s)，通过自身加载个体控制器，反馈误差使单个移动机器人保持稳定。然后在所得的个体控制器参数范围内选取合适的控制器参数，与原模型结合成新的模型。接着运用矩阵原理，分解拓扑结构，得到能使新个体达成协同控制的条件。最后通过所得的条件求取耦合控制器c₂(s)的参数。给定单个移动机器人的初速度，通过PID控制程序调节各移动机器人的速度，使整个系统达到一致。

实施例：

1.先基于刚体运动学原理，考虑移动机器人的输入输出等时滞的影响，利用拉格朗日方法建立具有如下传递函数形式的移动机器人模型：

$g\left(s\right)=\frac{1}{s+1}{e}^{-0.1s}$

2.建立多移动机器人系统的二自由度控制结构图如图2所示，图中，g(s)为单个移动机器人，c₁(s)和c₂(s)是具有以下形式的PID控制器：

$c_1\left(s\right)=c_2\left(s\right)=k_p+\frac{k_i}{s}+k_{d}s$

其中，k_p、k_i和k_d分别为PID控制器的比例、积分和微分增益。将图2转化成系统控制框图如图3所示，图中，C₁(s)为控制单个移动机器人状态的个体控制器矩阵，C₂(s)为控制系统达到一致性的耦合控制器矩阵，G(s)为具有时滞的移动机器人矩阵，K为与单个移动机器人相邻的邻居个数矩阵，其对角线元素用k表示，A为邻接矩阵，r为系统输入，y为系统输出。

3.求解个体控制器c₁(s)的稳定范围：

(a)由模型参数得到

U(s)＝s+1，V(s)＝e^-0.1s

闭环特征函数为

δ(s)＝s(s+1)+ke^-0.1s(k_ds²+k_ps+k_i)

(b)选取一个足够大的频率值ω^*＝100，计算幅角的变化范围计算Re[V(jω)]的实零点可得如下结果:

w₀＝15.7080，w₁＝47.1239，w₂＝78.5398

则对应sgn(Im[V(jw_t)])的值为(-1,1,-1)，sgn[V(0)]＝1。

$\underset{s=j\mathrm{\&omega;},\mathrm{\&omega;}\&Element;(0,{\mathrm{\&omega;}}^{*})}{\mathrm{\&Delta;}\arg \&lsqb;V\left(s\right)\&rsqb;}=\frac{\mathrm{\&pi;}}{2}\mathrm{sgn}\&lsqb;V\left(0\right)\&rsqb;\&CenterDot;\{2\mathrm{sgn}\&lsqb;\mathrm{Im}\left(V\right({\mathrm{jw}}_0))\&rsqb;+\mathrm{...}+{(-1)}^{e}2\mathrm{sgn}\&lsqb;\mathrm{Im}\left(V\right({\mathrm{jw}}_e))\&rsqb;\}=\frac{\mathrm{\&pi;}}{2}\&CenterDot;1\&CenterDot;(-2-2-2)=-3\mathrm{\&pi;}$

(c)由图6可得，k_p最大允许的可稳定的范围为(-0.5,9.3269)，将k_p值在该范围内进行等间隔的遍历，每个遍历点为其中k_pmin为k_p允许的最小值，k_pmax为k_p允许的最大值，z为采样频率，z＝0,1,…,F，F＝0.1为遍历点之间的间隔。

(d)选取一遍历点求取能保证闭环系统稳定的(k_d,k_i)二维稳定域：

A.计算q(ω,k_p)＝0在区间[0,ω^*)上不同的非负实零点。求得满足要求的非负实零点为

ω₀＝0，ω₁＝7.2033，ω₂＝30.4267，ω₃＝63.6181，ω₄＝93.9283

B.确定i_t的值:

由式 $i_0=\mathrm{sgn}(\frac{d}{d\mathrm{\&omega;}}\&lsqb;V_r(\mathrm{\&omega;})U_r\left(\mathrm{\&omega;}\right)+V_i\left(\mathrm{\&omega;}\right)U_i\left(\mathrm{\&omega;}\right)\&rsqb;{|}_{\mathrm{\&omega;}=0})$ 可知：i₀＝-1。

先求γ(I)：

因为u+v＝1为奇数，则有

$\mathrm{\&gamma;}\left(I\right)=\{i_0-2i_1+2i_2-2i_3+i_4\}\&CenterDot;\left\{{(-1)}^4\mathrm{sgn}\right\{\&lsqb;q({\mathrm{\&omega;}}_4^{+},k_p)\&rsqb;\left\}\right\}$

由于 ${(-1)}^4\mathrm{sgn}\{\&lsqb;q({\mathrm{\&omega;}}_4^{+},k_p)\&rsqb;=-1,$

从而由式(11)可以得到：

γ(I)＝{-i₀+2i₁-2i₂+2i₃-i₄}

移动机器人稳定的充分必要条件为：

$\frac{\mathrm{\&pi;}}{2}\mathrm{\&gamma;}\left(I\right)=\frac{u+1}{2}\mathrm{\&pi;}-\underset{s=j\mathrm{\&omega;},\mathrm{\&omega;}\&Element;(0,+\mathrm{\&infin;})}{\mathrm{\&Delta;}\arg \&lsqb;V\left(s\right)\&rsqb;}=4\mathrm{\&pi;}$

因此，可得

$\frac{u+1}{2}\mathrm{\&pi;}-\underset{s=j\mathrm{\&omega;},\mathrm{\&omega;}\&Element;(0,+\mathrm{\&infin;})}{\mathrm{\&Delta;}\arg \&lsqb;V\left(s\right)\&rsqb;}=\frac{\mathrm{\&pi;}}{2}\{-i_0+2i_1-2i_2+2i_3-i_4\}\&DoubleRightArrow;\{i_0,i_1,i_2,i_3,i_4\}=\{-1,1,-1,1,-1\}$

C.确定个体控制器对应k_p＝2的不等式集合：

k_i<0，k_i>51.8882k_d-21.9k_i<925.7860k_d+459.1

k_i>4047.3k_d-2019.9k_i<8822.5k_d+4407.5

依据以上不等式可以得到，对于个体控制器c₁(s)，当k_p1＝2时，相应的(k_i1,k_d1)的稳定集合，如图7所示，其中，k_p1,k_i1,k_d1分别为个体控制器c₁(s)的参数k_p,k_i,k_d。

(e)遍历k_p1，重复步骤(c)-(d)得到个体控制器c₁(s)的稳定集合(k_p1,k_i1,k_d1)如图8所示。

(f)验证个体控制器稳定域的正确性。选取稳定范围内的点(k_p1,k_i1,k_d1)＝(2,15,0.1)，单位阶跃响应如图9所示，个体趋于稳定。选取边界点(k_p1,k_i1,k_d1)＝(2,27,0.1)，单位阶跃响应如图10所示近似等幅振荡，可见个体控制器稳定域正确。

4.求解耦合控制器c₂(s)的稳定范围：

a)选取个体控制器c₁(s)稳定域范围内的点(k_p1,k_i1,k_d1)＝(4,10,0.1)，将个体控制器和原来的模型结合成新的模型。接着分解系统，求得特征值分别为λ＝-2和λ＝1。根据步骤3中的(a)-(c),对于不同的λ值求得c₂(s)的比例系数k_p2的可取范围:当λ＝-2时，k_p2的取值范围为(-5.3269,4.5)，如图11所示；当λ＝1时，k_p2的取值范围为(-9,10.6537)，如图12所示。因此，k_p2的取值范围是(-5.3269,4.5000)。

b)取k_p2＝1，根据步骤3中的(d)求得耦合控制器c₂(s)对应的(k_i2,k_d2)的取值范围如图13所示。

c)对于不同λ值的稳定域交集如图14所示。

d)遍历k_p2，根据步骤b)得到耦合控制器c₂(s)关于(k_p2,k_i2,k_d2)的三维稳定域如图15所示，其中，k_p2,k_i2,k_d2分别为耦合控制器c₂(s)的参数k_p,k_i,k_d。

5.将移动机器人的模型参数输入个体控制器c₁(s)参数的计算单元，由步骤3计算个体控制器c₁(s)的稳定集合，根据需要在个体控制器的稳定集合中选取控制参数，将控制参数输入监控模块执行预调控制程序：经模拟量输入信号，经A/D装换模块将模拟信号转化为数字信号输入，将输入值与设定值进行比较可得到不同的跟踪误差按照离散域PID控制算式计算控制信号增量Δu(n₁)的值，与前一时刻的控制信号u(n₁-1)通过加法器进行加法运算就得到当前时刻的输出控制信号u(n₁)，其中，n₁为当前时刻的采样步数。Δu(n₁)计算公式如下：

Δu(n₁)＝a₁e(n₁)+a₂e(n₁-1)+a₃e(n₁-2)

其中，a₁＝(k_pR₁+k_d+R₁ ²k_i)/R₁，a₂＝-(k_pR₁+2k_d)/R₁，a₃＝k_d/R₁，R₁为系统采样周期，Δu(n₁)为当前采样步数为n₁时控制器输出信号增量，e(n₁)为当前采样步数为n₁时的跟踪误差，e(n₁-1)为采样步数为n₁-1时的跟踪误差，e(n₁-2)为采样步数为n₁-2时的跟踪误差。通过对PID控制器的调节减少误差以确保移动机器人的稳定运行。

6.将步骤5中经过预调系统镇定的个体控制器施加于每个移动机器人，以便于对稳定的移动机器人进行协同控制。

7.将个体控制器与原模型结合形成新的模型，并将模型参数输入耦合控制器c₂(s)参数的计算单元，由步骤4计算耦合控制器的c₂(s)的稳定集合。然后由监控模块执行事先编制好的控制程序：经模拟量输入通道传输信号，并将信号接入检测变送装置，再经A/D转换后得到数字量输入信号与此时的系统设定值比较后得到不同时刻的跟踪误差，基于跟踪误差，在所获得的c₂(s)稳定集合中选取控制参数，然后按照控制器c₂(s)的离散域PID控制算式计算控制信号增量Δu(n₂)的值，与前一时刻的控制信号u(n₂-1)通过加法器进行加法运算就得到当前时刻的输出控制信号u(n₂)，其中，n₂为当前时刻的采样步数。Δu(n₂)计算公式如下：

Δu(n₂)＝b₁e(n₂)+b₂e(n₂-1)+b₃e(n₂-2)

其中，b₁＝(k_pR₂+k_d+R₂ ²k_i)/R₂，b₂＝-(k_pR₂+2k_d)/R₂，b₃＝k_d/R₂，R₂为系统采样周期，Δu(n₂)为当前采样步数为n₂时控制器输出信号增量，e(n₂)为当前采样步数为n₂时的跟踪误差，e(n₂-1)为采样步数为n₂-1时的跟踪误差，e(n₂-2)为采样步数为n₂-2时的跟踪误差。通过PID控制器程序调节各移动机器人之间的速度差来使整个系统达到一致，完成协同控制。

在耦合控制器的稳定集合内选取(k_p2,k_i2,k_d2)为(1,1,0.1)时系统的输出曲线如图16所示，以验证该稳定方法确实能够使得多移动机器人的协同控制。又分别在稳定域边界处与稳定域外各取一点，分别为(k_p2,k_i2,k_d2)＝(1,-19,-0.1)和(k_p2,k_i2,k_d2)＝(1,-20,-0.1)，系统的输出曲线如图17和图18所示。从而，验证了两类控制器的有效性。

Claims (1)

1.多移动机器人的二自由度协同控制方法，包括如下步骤：

步骤1，先基于刚体运动学原理，考虑移动机器人的输入输出等时滞的影响，利用拉格朗日方法建立具有如下传递函数形式的移动机器人模型g(s)：

若令g(s)的分子和分母分别为V(s)和U(s)，则式(1)中，s为复平面上的一个变量，u和v分别表示U(s)和V(s)中s项的最高阶次，u>v，e为数学常数，sⁱ为s的i次方，在U(s)中，i为整数且i＝0,…,u-1，j为整数且j＝1,…,h_i，h_i是U(s)中sⁱ项所对应的时滞块的个数，τ_ij和α_ij是U(s)中sⁱ项所对应的时滞和系数，在V(s)中，i为整数且i＝0,…,v-1，j为整数且j＝1,…,f_i，f_i是V(s)中sⁱ项所对应的时滞块的个数，θ_ij,β_ij和是V(s)中sⁱ项所对应的时滞和系数，β_v为V(s)的最高阶次项的系数。然后，将辨识出的模型参数送到主机的存储单元RAM中。

步骤2，建立具有个体反馈控制器集合C₁(s)，耦合反馈控制器集合C₂(s)，带有时滞控制模型集合G(s)的多移动机器人反馈控制系统，其中 并且E为单位矩阵，为克罗内克积，g(s)为单个移动机器人模型，c₁(s)和c₂(s)是具有以下形式的PID控制器：

$c_1\left(s\right)=c_2\left(s\right)=k_p+\frac{k_i}{s}+k_{d}s---\left(2\right)$

其中，k_p、k_i和k_d分别为PID控制器的比例、积分和微分增益。用表示单个移动机器人的邻居个数矩阵，k为单个移动机器人的邻居个数，用A表示邻接矩阵，用r表示系统输入，用y表示系统输出。

步骤3，根据以下步骤确定能使移动机器人稳定的个体控制器c₁(s)的参数范围：

(31)确定系统闭环特征函数δ(s)为

δ(s)＝sU(s)+kV(s)(k_ds²+k_ps+k_i)(3)

令ω为频率，且令s＝jω，U_r(ω)和U_i(ω)为U(jω)的实部和虚部，V_r(ω)和V_i(ω)为V(jω)的实部和虚部，令δ'(jω)＝δ(jω)*V(-jω)，得到

δ'(jω)＝p(ω,k_i,k_d)+jq(ω,k_p)

其中j²＝-1，p(ω,k_i,k_d)表示δ'(jω)的实部，q(ω,k_p)表示δ'(jω)的虚部，

$p(\mathrm{\&omega;},k_i,k_d)=p_1\left(\mathrm{\&omega;}\right)+k(k_i-{\mathrm{\&omega;}}^2{k}_d)\&lsqb;V_r^2\left(\mathrm{\&omega;}\right)+V_i^2\left(\mathrm{\&omega;}\right)\&rsqb;---\left(4\right)$

$q(\mathrm{\&omega;},k_p)=\mathrm{\&omega;}\{q_1\left(\mathrm{\&omega;}\right)+{\mathrm{kk}}_p\&lsqb;V_r^2\left(\mathrm{\&omega;}\right)+V_i^2\left(\mathrm{\&omega;}\right)\&rsqb;\}---\left(5\right)$

令 $p_1\left(\mathrm{\&omega;}\right)=p(\mathrm{\&omega;},k_i,k_d)-k(k_i-{\mathrm{\&omega;}}^2{k}_d)\&lsqb;V_r^2\left(\mathrm{\&omega;}\right)+V_i^2\left(\mathrm{\&omega;}\right)\&rsqb;,$ $q_1\left(\mathrm{\&omega;}\right)=q(\mathrm{\&omega;},k_p)/\mathrm{\&omega;}-{\mathrm{kk}}_p\&lsqb;V_r^2\left(\mathrm{\&omega;}\right)+V_i^2\left(\mathrm{\&omega;}\right)\&rsqb;,$ 则p₁(ω)和q₁(ω)分别为：

p₁(ω)＝ω[U_r(ω)V_i(ω)-U_i(ω)V_r(ω)](6)

q₁(ω)＝[U_r(ω)V_r(ω)+U_i(ω)V_i(ω)](7)

(32)选取一个足够大的频率值ω^*。

(33)令Re[V(jω)]和Im[V(jω)]分别为V(jω)的实部与虚部，根据下式计算ω由0变化到ω^*时所对应的V(jω)的幅角变化范围

这里，当v为偶数时，w₀<w₁<w₂<…<w_e是Im[V(jω)]在[0,ω^*)的零点；反之当v为奇数时，w₀<w₁<w₂<…<w_e是Re[V(jω)]在[0,ω^*)的零点,e+1表示零点的个数，sgn(x)为符号函数，其中x为实数，当x>0时，sgn(x)＝1，当x＝0时，sgn(x)＝0，当x<0时，sgn(x)＝-1。

(34)确定k_p的最大可允许稳定范围：

令Q表示f₁(ω)＝k_p与在(0,ω^*)上的交点数，给出满足下式的k_p范围，即为k_p的最大可允许稳定范围。

其中，j(V)表示V(s)在正虚轴上的零点数，为ω由0变化到ω^*时所对应的V(jω)的幅角变化范围。

(35)令k_p的最大可允许稳定范围为[k_pmin,k_pmax]，k_pmin为k_p允许的最小值，k_pmax为k_p允许的最大值，将k_p值在该范围内进行等间隔的遍历，即每个遍历点为其中F为遍历点之间的间隔，z为采样频率，z＝0,1,…,F。

(36)对于其中一个遍历点根据以下步骤确定能够保证闭环系统稳定的(k_d,k_i)二维稳定域：

(a)计算方程q(ω,k_p)＝0在区间[0,ω^*)内的实零点，将这些实零点按照从小到大的顺序表示为ω₀,ω₁,ω₂,…，ω_c-1，其中，c为实零点的个数；

(b)令t＝0,1,2,…,c-1，选择整数i_t的值：

(i).如果V(-jω_t)＝0，那么i_t＝0；

(ii).如果V(-s)在原点处存在零点，那么

$i_0=\{sgn\left(\frac{d}{d\mathrm{\&omega;}}\right\{\mathrm{\&omega;}\&lsqb;U_r\left(\mathrm{\&omega;}\right)V_i\left(\mathrm{\&omega;}\right)-U_i\left(\mathrm{\&omega;}\right)V_r\left(\mathrm{\&omega;}\right)\&rsqb;\}{|}_{\mathrm{\&omega;}=0})$

(iii).若不满足(i)和(ii)，i_t＝1或-1，其准确值需根据下述稳定性条件而定；

(iv).令I＝{i₀,i₁,…}，I为i_t(t＝0,1,2,…c-1)的序列，确定能够满足下述等式的所有I：

$\frac{u+1}{2}-\underset{s=j\mathrm{\&omega;},\mathrm{\&omega;}\&Element;(0,\mathrm{\&omega;}*)}{\mathrm{\&Delta;}\arg \&lsqb;V\left(s\right)\&rsqb;}=\frac{\mathrm{\&pi;}}{2}\mathrm{\&gamma;}\left(I\right)---\left(10\right)$

令这里ω_c-1是q(ω,k_p)＝0在区间[0,ω^*)内的第c个实零点，ε是一足够小的正实数，为大于ω_c-1且无限接近ω_c-1的值，γ(I)为i_t(t＝0,1,2,…c-1)的字符组合，由下式给出：

(c)对于遍历点(k_d,k_i)二维稳定域由下式决定:

[k_i-A(ω_t)k_d+B(ω_t)]i_t>0(12)

其中，ω_t为式(5)在区间[0,ω^*)内的实零点，t＝0,1,…c-1，令式(4)等于零，则为k_d的系数的绝对值，为常数项。通过求取所有ω_t所对应的由式(12)所决定的不等式组的交集，即可确定具有凸多边形特性的(k_d,k_i)二维稳定域。

(37)对于步骤(5)中所给出的k_p的每个遍历点，都重复步骤(6)，确定能使闭环系统稳定的所有个体控制器c₁(s)的参数集合。

步骤4，根据以下步骤确定能使多移动机器人系统一致的耦合控制器c₂(s)的参数范围：

41.选取步骤3得到的稳定范围内合适的一点作为个体控制器c₁(s)的参数，将个体控制器c₁(s)与原移动机器人模型g(s)进行整合得到新的模型g'(s)，其中由此可将复杂的多反馈系统简化为具有多时滞单反馈的多输入多输出多移动机器人系统，则系统的特征方程为：

$\widetilde{\mathrm{\&delta;}}\left(s\right)=\det \&lsqb;E+(-A)G^{\&prime;}\left(s\right)C_2\left(s\right)\&rsqb;$

其中，E为单位矩阵，A表示邻接矩阵。因为-A是可对角化矩阵，一定存在一个可逆矩阵P使得-A＝P^-1ΛP，其中，Λ为对角矩阵。对特征方程进行变形，得到其特征方程为：

$\widetilde{\mathrm{\&delta;}}\left(s\right)=\det \&lsqb;E+P^{-1}{\mathrm{\&Lambda;PG}}^{\&prime;}\left(s\right)C_2\left(s\right)\&rsqb;=\underset{i=1}{\overset{N}{\&Pi;}}\det \&lsqb;1+{\mathrm{\&lambda;}}_i{g}^{\&prime;}\left(s\right)c_2\left(s\right)\&rsqb;---\left(13\right)$

其中，表示新的传递函数矩阵，C₂(s)表示耦合控制器矩阵，g'(s)为各个子系统的传递函数，c₂(s)为各个子系统上的PID控制器，λ_i为矩阵-A的特征值，N为特征值个数。

42.对于不同的λ值，重复步骤3的方法，确定能使整个系统达到一致，完成系统协同控制的所有PID控制器的集合。

步骤5，将移动机器人的模型参数输入个体控制器c₁(s)参数的计算单元，由步骤3计算个体控制器c₁(s)的稳定集合，根据需要在个体控制器的稳定集合中选取控制参数，将控制参数输入监控模块执行预调控制程序：经模拟量输入信号，经A/D装换模块将模拟信号转化为数字信号输入，将输入值与设定值进行比较可得到不同的跟踪误差按照离散域PID控制算式计算控制信号增量Δu(n₁)的值，与前一时刻的控制信号u(n₁-1)通过加法器进行加法运算就得到当前时刻的输出控制信号u(n₁)，其中，n₁为当前时刻的采样步数。Δu(n₁)计算公式如下：

Δu(n₁)＝a₁e(n₁)+a₂e(n₁-1)+a₃e(n₁-2)(14)

其中，a₁＝(k_pR₁+k_d+R₁ ²k_i)/R₁，a₂＝-(k_pR₁+2k_d)/R₁，a₃＝k_d/R₁，R₁为系统采样周期，Δu(n₁)为当前采样步数为n₁时控制器输出信号增量，e(n₁)为当前采样步数为n₁时的跟踪误差，e(n₁-1)为采样步数为n₁-1时的跟踪误差，e(n₁-2)为采样步数为n₁-2时的跟踪误差。通过对PID控制器的调节减少误差以确保移动机器人的稳定运行。

步骤6，将步骤5中经过预调系统镇定的个体控制器施加于每个移动机器人，以便于对稳定的移动机器人进行协同控制。

步骤7，将个体控制器与原模型结合形成新的模型，并将模型参数输入耦合控制器c₂(s)参数的计算单元，由步骤4计算耦合控制器c₂(s)的稳定集合。然后由监控模块执行事先编制好的控制程序：经模拟量输入通道传输信号，并将信号接入检测变送装置，再经A/D转换后得到数字量输入信号与此时的系统设定值比较后得到不同时刻的跟踪误差，基于跟踪误差，在所获得的c₂(s)稳定集合中选取控制参数，然后按照控制器c₂(s)的离散域PID控制算式计算控制信号增量Δu(n₂)的值，与前一时刻的控制信号u(n₂-1)通过加法器进行加法运算就得到当前时刻的输出控制信号u(n₂)，其中，n₂为当前时刻的采样步数。Δu(n₂)计算公式如下：

Δu(n₂)＝b₁e(n₂)+b₂e(n₂-1)+b₃e(n₂-2)(15)

其中，b₁＝(k_pR₂+k_d+R₂ ²k_i)/R₂，b₂＝-(k_pR₂+2k_d)/R₂，b₃＝k_d/R₂，R₂为系统采样周期，Δu(n₂)为当前采样步数为n₂时控制器输出信号增量，e(n₂)为当前采样步数为n₂时的跟踪误差，e(n₂-1)为采样步数为n₂-1时的跟踪误差，e(n₂-2)为采样步数为n₂-2时的跟踪误差。通过c₂(s)控制器程序调节各移动机器人之间的速度差来使整个系统达到一致，完成协同控制。