CN110434858A - 一种基于命令滤波的多机械臂系统的力/位混合控制方法 - Google Patents

Abstract

本发明属于机器人控制技术领域，具体公开了一种基于命令滤波的多机械臂系统的力/位混合控制方法。该方法基于李雅普诺夫函数，运用反步法构造中间虚拟控制信号，逐步递推得到控制律，从而对多机器臂的末端执行器进行控制；利用模糊逻辑系统处理多机械臂系统中未知的非线性函数，同时使用命令滤波技术解决期望物体的实际位置的二阶导数不存在的问题。本发明方法能够保证物体的位置跟踪误差和内力调节误差收敛到原点周围的一个足够小的邻域内。综上，本发明所提出的多机械臂系统的力/位控制方法能够在多种工况下有效，使物体准确地跟踪期望轨迹，并且将物体所受的内力控制在一个合理的范围内。

Description

一种基于命令滤波的多机械臂系统的力/位混合控制方法

技术领域

本发明属于机器人控制领域，涉及一种基于命令滤波的多机械臂系统的力/位混合控制方法。

背景技术

目前，国内工业生产中多数使用单臂机器人，单臂机器人在组装零件、搬运重物等工业生产活动中效率低下，在高精度作业工况下工作质量一般。协同多机器人是多个机械臂同时搬运、装配、打磨操作对象，在运行效率和控制精确度上都要优于单臂机器人。然而，相比于单机械臂系统，多机械臂系统状态变量繁多，高度非线性且存在复杂耦合性。多机械臂系统的力/位混合控制是一项重大挑战，目前也是该领域研究的热点之一。

作为先进控制方法的反步控制法已经被运用到多机械臂系统的控制中，并取得了较好的控制效果，但反步法存在的问题主要体现在：(1)系统的某些函数必须是线性的；(2)物体的实际位置信号的二阶导数不存在，上述问题的存在使得反步法的使用具有较大的局限性。

模糊逻辑系统或神经网络对上述第(1)个问题提供了解决思路，其中，模糊逻辑逼近系统通过近似理论处理了复杂非线性系统中的未知非线性函数。针对第(2)个问题，专家们已经提出了动态面控制方法，通过引入一阶滤波器处理传统反步控制方法中不可避免的“计算爆炸”问题的同时，解决了传统反步法的第(2)个问题。然而，动态面控制技术在实际工程中存在滤波误差，因此，会大大影响系统的位置跟踪和内力调节误差。

发明内容

本发明的目的在于提出一种基于命令滤波的多机械臂系统的力/位混合控制方法，以实现对多机械臂系统力/位混合的高精度控制。

本发明为了实现上述目的，采用如下技术方案：

一种基于命令滤波的多机械臂系统的力/位混合控制方法，包括如下步骤：

a.建立第i个机械臂的动力学模型，如公式(1)所示：

其中，q_i＝[q_i,1,q_i,2]^T，q_i,n表示第i个机械臂上的第n个关节向量，n＝1,2；

τ_i＝[τ_i,1,τ_i,2]^T，τ_i,n表示施加在第i个机械臂上的第n个关节的控制力矩，n＝1,2；

M_i(q_i)是第i个机械臂的对称正定惯性矩阵；

是第i个机械臂的科里奥利和离心力矩阵；

G_i(q_i)是第i个机械臂的重力矢量；是动态和静态摩擦矢量；

d_i(t)是外部干扰的矢量；J_m,i(q_i)是第i个机械臂的雅可比矩阵；

F_i是第i个末端执行器施加在物体上的力；

由公式(1)得到k个机械臂的协作动力学方程是：

其中，M(q)＝blockdiag[M₁(q₁),M₂(q₂),..,M_k(q_k)]；

τ＝[τ₁,τ₂,..,τ_k]^T；

J_m(q)＝blockdiag[J_m,1(q₁),J_m,2(q₂),..,J_m,k(q_k)]；

blockdiag[]表示分块对角矩阵函数；

雅克比矩阵J_m,i(q_i)由第i个机械臂的正向动力学得到：

其中，x_e,i表示每个机械臂执行器末端的位置向量，公式(3)简写为：

其中，

b.建立物体的动力学模型，如公式(5)所示：

其中，p是物体的位置向量；M_o(p)是物体的对称正定惯性矩阵；是物体的科里奥利和离心力矩阵；G_o(p)是物体的重力矢量；F_o是物体质心所受的合力矩矢量；

F_o的表达式如公式(6)所示：

其中，是从第i个末端执行器到物体的雅可比矩阵，公式(6)简写为：

其中，

F_i由内力f_i＝[f_i,1,f_i,2]^T和外力E_i＝[E_i,1,E_i,2]两部分组成，得到：

F＝f+E (8)

其中，将公式(8)代入公式(7)中得到：

f是零空间向量中的任意向量，得到：

将公式(9)代入公式(8)中得到：

其中，表示的伪逆矩阵，假设物体的运动不会受到内力的影响，内力之间互相抵消，则c.建立多机械臂协作的动态模型，如公式(12)所示：

p＝φ_i(q_i) (12)

其中，φ_i(q_i)表示p与q_i的运动学关系式，通过正向运动学获得；

对公式(12)求一阶微分得到如下雅克比矩阵：

其中，为从第i个机械臂的关节变量q_i到笛卡尔空间变量的雅可比矩阵；

由公式(12)得到：

其中， 表示的转置逆矩阵；

定义对公式(14)求一阶微分得到下面的雅克比矩阵：

将公式(11)、(14)和(15)代入公式(2)中，得到多机械臂协作的动态模型：

其中：

是斜对称矩阵，从而当

假设d(t)满足其中，是一个未知正常数；

为简化多机械臂协作的动态模型，定义新的变量为：

多机械臂系统的动态数学模型表示为：

d.构建基于模糊自适应的命令滤波反步控制器；

假设f(Z)在紧集Ω_Z中是一个连续的函数，对于任意的常数ε＞0，总存在一个模糊逻辑系统W^TS(Z)满足：

其中，输入向量Q是模糊输入维数，R^Q为实数向量集；W∈R^l是模糊权向量，模糊节点数l为正整数，且l＞1，R^l为实数向量集；

S(Z)＝[s₁(Z),...,s_l(Z)]^T∈R^l为基函数向量，选取基函数s_j(Z)为如下的高斯函数：

其中，μ_j是高斯函数分布曲线的中心位置，而η_j则为高斯函数的宽度；

对于R²表示2维实数向量集，总存在不等式：

其中，P₁＞0,r＞1,s＞1，并且：(r-1)(s-1)＝1；

定义系统误差变量为：x_1d为物体的期望轨迹信号，α为虚拟控制律；

其中，A₀,A₁,b_c1是正常数；

命令滤波器定义如公式(19)所示：

其中，φ₁和φ₂表示实数，ω_n＞0，ζ∈(0，1]；如果输入信号满足和对于所有的t≥0均成立，其中，ρ₁和ρ₂是正的常量，并且φ₁(0)＝α₁(0)，φ₂(0)＝0；

则对于任意μ＞0，必然存在ω_n＞0和ζ∈(0，1]，使得|φ₁-α₁|≤μ，和|φ₁|都是有界的；其中，φ₁(0)、α₁(0)、φ₂(0)分别为φ₁、α₁、φ₂的初始值；

多机械臂系统的力/位控制方法设计的每一步都会选取一个李雅普诺夫函数来构建一个虚拟控制函数或者真实控制律，控制方法包括如下步骤：

d1.对于物体的期望轨迹信号x_1d，定义补偿误差：v₁＝z₁-ζ₁，其中，ζ₁为误差补偿信号；

选取李雅普诺夫函数为：对V₁求导得到：

选取虚拟控制律常数k₁＞0，误差补偿信号的一阶微分x_1c表示虚拟控制律α经过命令滤波器输出的信号；

将虚拟控制律α和误差补偿信号ζ₁代入公式(20)中得到：

d2.选取李雅普诺夫函数为：

定义补偿误差：v₂＝z₂-ζ₂，其中，ζ₂为误差补偿信号；则对公式(22)求导得到：

定义误差补偿信号ζ₂的一阶导数为得到：

因为由公式(24)得到：

其中，

定义f_i(Z)＝[f_i,1(Z),f_i,2(Z)]^T，

由万能逼近定理可知，存在模糊逻辑系统使得δ_i,n表示逼近误差，对于任意小的正数ε_i,n，满足不等式|δ_i.n|≤ε_i,n，n＝1,2；

定义c＝Λ_om(q)v₂，则：

其中，c_i＝[c_i,1,c_i,2]^T，||W_i,n||为向量W_i,n的范数，是正常数，ε_i＝[ε_i,1,ε_i,2]^T；

将公式(26)代入公式(25)得：

根据杨氏不等式和公式(27)，得到：

定义e_f为内力误差变量，e_f＝f₀-f，其中，f₀为期望的内力；

定义

选取真实控制律τ：

其中，常数k₂＞0；为θ_i的估计值，θ_i将在后面定义；σ_f,d、σ_f,i是正常数；

将公式(29)代入公式(28)中得到：

由公式(4)、公式(7)和公式(14)得到：

结合公式(10)和公式(31)，得到：

将公式(32)代入公式(30)中得到：

e.定义选取李雅普诺夫函数如下：

式中，η_i是正常数，对公式(34)求导得到：

定义θ_i＝max||W_i,n||²，得到：

选取自适应律其中，m_i是正常数；

将自适应律代入公式(36)得：

其中，则：

其中：

假设初始时间为t₀，对于任意时间t满足则公式(38)表示为：

其中，V(t₀)表示初始时刻李雅普诺夫函数的值，显然有

f.定义滤波误差|x_1c-α|≤σ，σ是任意小的正常数；

构造李雅普诺夫函数对李雅普诺夫函数求导得：

由公式(40)得：其中，

系统误差变量z₁＝v₁+ζ₁，ζ₁和v₁均为有界，则有界且满足

调整参数a₀、b₀、b₁、k₀的值，使得误差变量z₁收敛于原点周围期望的邻域内；

将τ代入公式(15)中得到：

其中，σ_d表示一个正常数；

由于系统内的信号都是有界的，调整参数σ_d使得内力误差收敛到一个极小的值。

本发明具有如下优点：

(1)本发明方法充分考虑了外部干扰的影响，在外部扰动介入的情况下，仍能实现对多机械臂系统力/位混合的高精度控制。

(2)本发明方法利用模糊逻辑系统逼近多机械臂力/位混合系统中的未知非线性函数，构造了基于模糊自适应的命令滤波反步控制器，有效地处理了系统中的非线性项。

(3)本发明方法引入命令滤波技术解决物体的实际位置信号的二阶导数不存在的问题，并引入误差补偿机制减少滤波误差带来的影响，大大提高了系统的控制精度。

附图说明

图1为本发明实施例中k个机械臂控制一个共同的物体的动态模型示意图。

图2为本发明实施例中两个两连杆机械臂的模型示意图。

图3为采用本发明控制方法后物体的实际位置和期望位置的仿真图。

图4为采用本发明控制方法后物体的跟踪误差仿真图。

图5为采用本发明控制方法后物体所受的第一个内力误差的仿真图。

图6为采用本发明控制方法后物体所受的第二个内力误差的仿真图。

图7为采用本发明控制方法后物体所受的第三个内力误差的仿真图。

图8为采用本发明控制方法后物体所受的第四个内力误差的仿真图。

图9为采用本发明控制方法后控制力矩τ的仿真图。

具体实施方式

本发明的基本构思是：

基于李雅普诺夫函数，运用反步法构造中间虚拟控制信号，逐步递推得到控制律，从而对多机器臂的末端执行器进行控制；利用模糊逻辑系统处理多机械臂系统中未知的非线性函数，同时使用命令滤波技术解决期望物体的实际位置的二阶导数不存在的问题。

通过以上发明构思保证基于命令滤波的多机械臂系统的力/位控制方法在多种工况下有效，使物体准确地跟踪期望轨迹，并且将物体所受的内力控制在一个合理的范围内。

下面结合附图以及具体实施方式对本发明作进一步详细说明：

如图1所示，一种基于命令滤波的多机械臂系统的力/位混合控制方法，包括如下步骤：

a.建立第i个机械臂的动力学模型，如公式(1)所示：

其中，q_i＝[q_i,1,q_i,2]^T，q_i,n表示第i个机械臂上的第n个关节向量，n＝1,2。

τ_i＝[τ_i,1,τ_i,2]^T，τ_i,n表示施加在第i个机械臂上的第n个关节的控制力矩，n＝1,2。

M_i(q_i)是第i个机械臂的对称正定惯性矩阵。

是第i个机械臂的科里奥利和离心力矩阵。

G_i(q_i)是第i个机械臂的重力矢量；是动态和静态摩擦矢量。

d_i(t)是外部干扰的矢量；J_m,i(q_i)是第i个机械臂的雅可比矩阵。

F_i是第i个末端执行器施加在物体上的力。

由公式(1)得到k个机械臂的协作动力学方程是：

其中，M(q)＝blockdiag[M₁(q₁),M₂(q₂),..,M_k(q_k)]。

τ＝[τ₁,τ₂,..,τ_k]^T。

J_m(q)＝blockdiag[J_m,1(q₁),J_m,2(q₂),..,J_m,k(q_k)]；

blockdiag[]表示分块对角矩阵函数。

雅克比矩阵J_m,i(q_i)由第i个机械臂的正向动力学得到：

其中，x_e,i表示每个机械臂执行器末端的位置向量。公式(3)简写为：

其中，

b.建立物体的动力学模型，如公式(5)所示：

其中，p是物体的位置向量；M_o(p)是物体的对称正定惯性矩阵。是物体的科里奥利和离心力矩阵；G_o(p)是物体的重力矢量。

F_o是物体质心所受的合力矩矢量，F_o的表达式如公式(6)所示：

其中，是从第i个末端执行器到物体的雅可比矩阵。公式(6)简写为：

其中，

F_i由内力f_i＝[f_i,1,f_i,2]^T和外力E_i＝[E_i,1,E_i,2]两部分组成，得到：

F＝f+E (8)

其中，将公式(8)代入公式(7)中得到：

f是零空间向量中的任意向量，得到：

将公式(9)代入公式(8)中得到：

其中，表示的伪逆矩阵，

假设物体的运动不会受到内力的影响，内力之间互相抵消，则

c.建立多机械臂协作的动态模型，如公式(12)所示：

p＝φ_i(q_i) (12)

其中，φ_i(q_i)表示p与q_i的运动学关系式，通过正向运动学获得。

对公式(12)求一阶微分得到如下雅克比矩阵：

其中，为从第i个机械臂的关节变量q_i到笛卡尔空间变量的雅可比矩阵。

由公式(12)得到：

其中， 表示的转置逆矩阵。

定义对公式(14)求一阶微分得到下面的雅克比矩阵：

将公式(11)、(14)和(15)代入公式(2)中，得到多机械臂协作的动态模型：

其中：

是斜对称矩阵，从而当

假设d(t)满足其中，是一个未知正常数。

为简化整个系统动力学的表达式，需要设定如下条件：

1.所有机械臂都是非冗余的并且具有相同的自由度。

2.每个末端执行器与物体之间不存在相对运动，即：物体和末端执行器之间的接触是刚性的。

3.多机械臂系统的运动学方程是完全已知的。

4.每个机械臂的运动学方程都是非奇异的。

5.所有关节和物体都是刚性的。

为简化多机械臂协作的动态模型，定义新的变量为：

多机械臂系统的动态数学模型表示为：

d.构建基于模糊自适应的命令滤波反步控制器。

假设f(Z)在紧集Ω_Z中是一个连续的函数，对于任意的常数ε＞0，总存在一个模糊逻辑系统W^TS(Z)满足：

其中，输入向量Q是模糊输入维数，R^Q为实数向量集；W∈R^l是模糊权向量，模糊节点数l为正整数，且l＞1，R^l为实数向量集。

S(Z)＝[s₁(Z),...,s_l(Z)]^T∈R^l为基函数向量，选取基函数s_j(Z)为如下的高斯函数：

其中，μ_j是高斯函数分布曲线的中心位置，而η_j则为高斯函数的宽度。

对于R²表示2维实数向量集，总存在不等式：

其中，P₁＞0,r＞1,s＞1，并且：(r-1)(s-1)＝1。

定义系统误差变量为：x_1d为物体的期望轨迹信号，α为虚拟控制律。

其中，A₀,A₁,b_c1是正常数。

命令滤波器定义如公式(19)所示：

其中，φ₁和φ₂表示实数，ω_n＞0，ζ∈(0，1]；如果输入信号满足和对于所有的t≥0均成立，其中，ρ₁和ρ₂是正的常量，并且φ₁(0)＝α₁(0)，φ₂(0)＝0。

则对于任意μ＞0，必然存在ω_n＞0和ζ∈(0，1]，使得|φ₁-α₁|≤μ，和|φ₁|都是有界的；其中，φ₁(0)、α₁(0)、φ₂(0)分别为φ₁、α₁、φ₂的初始值。

多机械臂系统的力/位控制方法设计的每一步都会选取一个李雅普诺夫函数来构建一个虚拟控制函数或者真实控制律，控制方法包括如下步骤：

d1.对于物体的期望轨迹信号x_1d，定义补偿误差：v₁＝z₁-ζ₁，其中，ζ₁为误差补偿信号。

选取李雅普诺夫函数为：对V₁求导得到：

选取虚拟控制律常数k₁＞0，误差补偿信号的一阶微分x_1c表示α经过命令滤波器输出的信号。

将虚拟控制律α和误差补偿信号ζ₁代入公式(20)中得到：

d2.选取李雅普诺夫函数为：

定义补偿误差：v₂＝z₂-ζ₂，其中，ζ₂为误差补偿信号；则对公式(22)求导得到：

定义误差补偿信号ζ₂的一阶导数为得到：

因为由公式(24)得到：

其中，

定义f_i(Z)＝[f_i,1(Z),f_i,2(Z)]^T，

由万能逼近定理可知，存在模糊逻辑系统使得δ_i,n表示逼近误差，对于任意小的正数ε_i,n，满足不等式|δ_i.n|≤ε_i,n，n＝1,2。

定义c＝Λ_om(q)v₂，则：

其中，c_i＝[c_i,1,c_i,2]^T，||W_i,n||为向量W_i,n的范数，是正常数，ε_i＝[ε_i,1,ε_i,2]^T。

将公式(26)代入公式(25)得：

根据杨氏不等式和公式(27)，得到：

定义e_f为内力误差变量，e_f＝f₀-f，其中，f₀为期望的内力。

定义选取真实控制律：

其中，常数k₂＞0；为θ_i的估计值，θ_i将在后面定义；σ_f,d、σ_f,i是正常数。

将公式(29)代入公式(28)中得到：

由公式(4)、公式(7)和公式(14)得到：

结合公式(10)和公式(31)，得到：

将公式(32)代入公式(30)中得到：

e.定义选取李雅普诺夫函数如下：

式中，η_i是正常数，对公式(34)求导得到：

定义θ_i＝max||W_i,n||²，得到：

选取自适应律其中，m_i是正常数。将自适应律代入公式(36)得：

其中，则：

其中：

假设初始时间为t₀，对于任意时间t满足则公式(38)表示为：

其中，V(t₀)表示初始时刻李雅普诺夫函数的值，显然有

f.定义滤波误差|x_1c-α|≤σ，σ是任意小的正常数。

构造李雅普诺夫函数对函数求导得：

由公式(40)得：其中，

系统误差变量z₁＝v₁+ζ₁，ζ₁和v₁均为有界，则有界且满足

调整参数a₀、b₀、b₁、k₀的值，使得误差变量z₁收敛于原点周围期望的邻域内。

将τ代入公式(15)中得到：

其中，σ_d表示一个正常数。

由于系统内的信号都是有界的，调整参数σ_d使得内力误差收敛到一个极小的值。

由以上分析得到，在控制律τ的作用下，物体的位置跟踪误差和内力的跟踪误差收敛到原点的一个充分小的邻域内，保证了其他信号有界。

由上述步骤可知，本发明基于李雅普诺夫函数，通过将反步法和模糊自适应技术相结合，并且使用命令滤波技术，能够有效解决多机械臂系统的力/位混合控制的问题。

在图1中，{B}表示参考坐标系，x_b、y_b、z_b为该坐标系下三个坐标轴；{O}表示以物体质心为中心建立的坐标系，x_o、y_o、z_o为该坐标系下三个坐标轴；{E_i}是以第i个末端执行器为中心建立的直角坐标系，为该坐标系下三个坐标轴，其中1≤i≤k。

在虚拟环境下对所建立的基于模糊自适应的命令滤波反步控制器进行仿真，验证所提出的基于命令滤波的多机械臂系统的力/位混合控制方法的可行性：

在图2中，多机械臂系统参数及物体的参数：

两个机械臂的长度分别是：l_1,1＝l_2,1＝1m,l_1,2＝l_2,2＝1m；质量m_1,1＝m_2,1＝1kg,m_1,2＝m_2,2＝1kg；力矩I_1,1＝I_2,1＝1N·m,I_1,2＝I_2,2＝1N·m。

刚性物体的半径、质量、力矩分别是r₀＝1.5m,m₀＝0.3kg,I₀＝0.1N·m。

机械臂的基座分别是：(x₁,y₁)＝(-1.4,0)，(x₂,y₂)＝(1.4,0)。

选择控制律参数为：k₁＝10，k₂＝8,l_i＝20,σ_f,d＝0.1,σ_f,i＝20；

物体的期望轨迹信号为：

期望的内力f₀＝[10,10,-10,-10]^T；

x_1d＝[p_x,r,p_y,r]^T是物体的期望轨迹信号。

其中，p_x,r表示物体在X轴上的期望轨迹信号、p_y,r表示物体在Y轴上的期望轨迹信号。

模糊隶属度函数为：

在图2中，q_i＝[q_i,1,q_i,2]^T是每一个关节向量。

d_x,1，d_x,2分别表示每个机械臂的基座坐标位置，m₀,r₀分别表示物体的质量和半径。

应用本发明方法控制后，跟踪信号和期望信号如图3所示。

x₁＝[p_x,p_y]^T是物体的跟踪轨迹。

其中，p_x表示物体在X轴上的跟踪轨迹信号，p_y表示物体在Y轴上的跟踪轨迹信号。

跟踪信号和期望信号的误差如图4所示。

由图3和图4看出，多机械系统的输出能够很好的跟踪期望信号。

图5-图8是采用本发明控制方法后物体所受的四个内力误差的仿真图。

其中：e_f11，e_f12，e_f21，e_f22分别表示物体所受的四个内力误差。由图5-图8能够看出，采用发明方法控制后，内力误差e_f可以收敛到一个极小的值。

图9是采用本发明方法后每一个机械臂上的控制力矩τ_i的仿真图。

由图9看出，物体与末端执行器的接触内力误差能够收敛到原点的一个足够小的邻域内。以上仿真表明，本发明控制方法能够高效地跟踪的参考信号，具有良好实际实施意义。

当然，以上说明仅仅为本发明的较佳实施例，本发明并不限于列举上述实施例，应当说明的是，任何熟悉本领域的技术人员在本说明书的教导下，所做出的所有等同替代、明显变形形式，均落在本说明书的实质范围之内，理应受到本发明的保护。

Claims (1)

1.一种基于命令滤波的多机械臂系统的力/位混合控制方法，其特征在于，

包括如下步骤：

a.建立第i个机械臂的动力学模型，如公式(1)所示：

其中，q_i＝[q_i,1,q_i,2]^T，q_i,n表示第i个机械臂上的第n个关节向量，n＝1,2；

τ_i＝[τ_i,1,τ_i,2]^T，τ_i,n表示施加在第i个机械臂上的第n个关节的控制力矩，n＝1,2；

M_i(q_i)是第i个机械臂的对称正定惯性矩阵；

是第i个机械臂的科里奥利和离心力矩阵；

G_i(q_i)是第i个机械臂的重力矢量；是动态和静态摩擦矢量；

d_i(t)是外部干扰的矢量；J_m,i(q_i)是第i个机械臂的雅可比矩阵；

F_i是第i个末端执行器施加在物体上的力；

由公式(1)得到k个机械臂的协作动力学方程是：

其中，M(q)＝blockdiag[M₁(q₁),M₂(q₂),..,M_k(q_k)]；

τ＝[τ₁,τ₂,..,τ_k]^T；

J_m(q)＝blockdiag[J_m,1(q₁),J_m,2(q₂),..,J_m,k(q_k)]；

blockdiag[]表示分块对角矩阵函数；

雅克比矩阵J_m,i(q_i)由第i个机械臂的正向动力学得到：

其中，x_e,i表示每个机械臂执行器末端的位置向量，公式(3)简写为：

其中，

b.建立物体的动力学模型，如公式(5)所示：

其中，p是物体的位置向量；M_o(p)是物体的对称正定惯性矩阵；是物体的科里奥利和离心力矩阵；G_o(p)是物体的重力矢量；F_o是物体质心所受的合力矩矢量；

F_o的表达式如公式(6)所示：

其中，是从第i个末端执行器到物体的雅可比矩阵，公式(6)简写为：

其中，

F_i由内力f_i＝[f_i,1,f_i,2]^T和外力E_i＝[E_i,1,E_i,2]两部分组成，得到：

F＝f+E (8)

其中，将公式(8)代入公式(7)中得到：

f是零空间向量中的任意向量，得到：

将公式(9)代入公式(8)中得到：

其中，表示的伪逆矩阵，

假设物体的运动不会受到内力的影响，内力之间互相抵消，则

c.建立多机械臂协作的动态模型，如公式(12)所示：

p＝φ_i(q_i) (12)

其中，φ_i(q_i)表示p与q_i的运动学关系式，通过正向运动学获得；

对公式(12)求一阶微分得到如下雅克比矩阵：

其中，为从第i个机械臂的关节变量q_i到笛卡尔空间变量的雅可比矩阵；

由公式(12)得到：

其中， 表示的转置逆矩阵；

定义对公式(14)求一阶微分得到下面的雅克比矩阵：

将公式(11)、(14)和(15)代入公式(2)中，得到多机械臂协作的动态模型：

其中：

是斜对称矩阵，从而当

假设d(t)满足其中，是一个未知正常数；

为简化多机械臂协作的动态模型，定义新的变量为：

多机械臂系统的动态数学模型表示为：

d.构建基于模糊自适应的命令滤波反步控制器；

假设f(Z)在紧集Ω_Z中是一个连续的函数，对于任意的常数ε＞0，总存在一个模糊逻辑系统W^TS(Z)满足：

其中，输入向量Q是模糊输入维数，R^Q为实数向量集；W∈R^l是模糊权向量，模糊节点数l为正整数，且l＞1，R^l为实数向量集；

S(Z)＝[s₁(Z),...,s_l(Z)]^T∈R^l为基函数向量，选取基函数s_j(Z)为如下的高斯函数：

其中，μ_j是高斯函数分布曲线的中心位置，而η_j则为高斯函数的宽度；

对于R²表示2维实数向量集，总存在不等式：

其中，P₁＞0,r＞1,s＞1，并且：(r-1)(s-1)＝1；

定义系统误差变量为：x_1d为物体的期望轨迹信号，α为虚拟控制律；

其中，A₀,A₁,b_c1是正常数；

命令滤波器定义如公式(19)所示：

其中，φ₁和φ₂表示实数，ω_n＞0，ζ∈(0，1]；如果输入信号满足和对于所有的t≥0均成立，其中，ρ₁和ρ₂是正的常量，并且φ₁(0)＝α₁(0)，φ₂(0)＝0；

则对于任意μ＞0，必然存在ω_n＞0和ζ∈(0，1]，使得|φ₁-α₁|≤μ，和|φ₁|都是有界的；其中，φ₁(0)、α₁(0)、φ₂(0)分别为φ₁、α₁、φ₂的初始值；

多机械臂系统的力/位控制方法设计的每一步都会选取一个李雅普诺夫函数来构建一个虚拟控制函数或者真实控制律，控制方法包括如下步骤：

d1.对于物体的期望轨迹信号x_1d，定义补偿误差：v₁＝z₁-ζ₁，其中，ζ₁为误差补偿信号；

选取李雅普诺夫函数为：对V₁求导得到：

选取虚拟控制律常数k₁＞0，误差补偿信号的一阶微分x_1c表示虚拟控制律α经过命令滤波器输出的信号；

将虚拟控制律α和误差补偿信号ζ₁代入公式(20)中得到：

d2.选取李雅普诺夫函数为：

定义补偿误差：v₂＝z₂-ζ₂，其中，ζ₂为误差补偿信号；则对公式(22)求导得到：

定义误差补偿信号ζ₂的一阶导数为得到：

因为由公式(24)得到：

其中，

定义f_i(Z)＝[f_i,1(Z),f_i,2(Z)]^T，

由万能逼近定理可知，存在模糊逻辑系统使得δ_i,n表示逼近误差，对于任意小的正数ε_i,n，满足不等式|δ_i.n|≤ε_i,n，n＝1,2；

定义c＝Λ_om(q)v₂，则：

其中，c_i＝[c_i,1,c_i,2]^T，||W_i,n||为向量W_i,n的范数，是正常数，ε_i＝[ε_i,1,ε_i,2]^T；

将公式(26)代入公式(25)得：

根据杨氏不等式和公式(27)，得到：

定义e_f为内力误差变量，e_f＝f₀-f，其中，f₀为期望的内力；

定义

选取真实控制律τ：

其中，常数k₂＞0；为θ_i的估计值，θ_i将在后面定义；σ_f,d、σ_f,i是正常数；

将公式(29)代入公式(28)中得到：

由公式(4)、公式(7)和公式(14)得到：

结合公式(10)和公式(31)，得到：

将公式(32)代入公式(30)中得到：

e.定义选取李雅普诺夫函数如下：

式中，η_i是正常数，对公式(34)求导得到：

定义θ_i＝max||W_i,n||²，得到：

选取自适应律其中，m_i是正常数；

将自适应律代入公式(36)得：

其中，则：

其中：

假设初始时间为t₀，对于任意时间t满足则公式(38)表示为：

其中，V(t₀)表示初始时刻李雅普诺夫函数的值，显然有

f.定义滤波误差|x_1c-α|≤σ，σ是任意小的正常数；

构造李雅普诺夫函数对李雅普诺夫函数求导得：

由公式(40)得：其中，

系统误差变量z₁＝v₁+ζ₁，ζ₁和v₁均为有界，则有界且满足

调整参数a₀、b₀、b₁、k₀的值，使得误差变量z₁收敛于原点周围期望的邻域内；

将τ代入公式(15)中得到：

其中，σ_d表示一个正常数；

由于系统内的信号都是有界的，调整参数σ_d使得内力误差收敛到一个极小的值。