CN111993417A - 一种基于rbf神经网络的机械臂自适应阻抗控制方法 - Google Patents

Abstract

本发明公开了一种基于RBF神经网络的机械臂自适应阻抗控制方法，包括：建立机械臂的D‑H参数，并推导机械臂的正运动学；利用kane方法建立机械臂系统的名义动力学模型；结合二阶阻抗方程设计阻抗控制器；设计动力学模型误差补偿控制器；将阻抗控制器与动力学模型误差补偿控制器结合，并根据李雅普诺夫函数设计RBF神经网络权值矩阵的变化率，构成自适应阻抗控制器，并证明自适应阻抗控制器的稳定性；根据所设计的控制器，判断机械臂柔顺控制的效果。本方法能在线补偿机械臂动力学误差的阻抗控制的方法，对机械臂模动力学的不确定部分进行在线补偿，使补偿后的阻抗控制器能够实现对机械臂的精准柔顺控制。

Description

一种基于RBF神经网络的机械臂自适应阻抗控制方法

技术领域

本发明属于机械臂柔顺控制领域，涉及一种基于径向基函数(RBF)神经网络的机械臂自适应阻抗控制方法。

背景技术

随着航天技术的发展，航天领域的研究焦点开始向航天器的在轨应用转移。但航天器在轨运行时可能会出现航天器失控、零件失效、燃料用尽等问题，导致航天器无法正常工作甚至被迫退役。由于太空环境的复杂性以及科学技术的限制，许多空间任务尚不能通过航天员出舱进行，因此空间机械臂成为了执行空间任务中不可或缺的部分。空间机械臂主要应用于在轨维护、目标捕获、飞行器组装与回收等操作，在执行任务时会与环境发生接触，空间机械臂末端与目标会存在较大的接触力。为了确保目标或机械臂本身不被损坏以及任务的顺利执行，这就需要在空间机械臂上安装力传感器实现力感知，再设计相应的控制算法将接触力控制在一个合理的范围之内。机械臂对接触力具有响应的能力，则称机械臂具有柔顺性能，相应的控制方法称为柔顺控制方法。在柔顺控制算法中，阻抗控制凭借将力与位置纳入统一控制体系等优点，成为空间机械臂柔顺控制应用领域的一大热点。在机械臂阻抗控制的研究和应用中，通常使用计算力矩控制方法，即结合机械臂逆动力学与阻抗方程设计阻抗控制器。通常机械臂执行任务的目标刚度较大，较小的控制力拒误差就会导致较大的接触力。控制力拒的精准度由逆动力学模型的精准度决定。

因此，要实现精准的柔顺控制，让机械臂具有良好的柔顺性能，需要对机械臂进行精准的动力学建模。机械臂系统具有高度的不确定性和非线性，包括模型的不确定性和参数的不确定性以及外部干扰，这些不确定性所造成的影响会直接反映为动力学模型的不确定性，从而影响机械臂的柔顺性能。机械臂系统的名义动力学模型通常可由机械臂参数求出，但实际动力学模型与名义动力学模型的误差无法通过计算求出。

因此，期望设计一种能在线补偿机械臂动力学误差的自适应阻抗控制方法。

发明内容

针对机械臂模型存在的不确定性问题，本发明提出的一种基于RBF神经网络的机械臂自适应阻抗控制方法，本方法能在线补偿机械臂动力学误差的阻抗控制的方法，对机械臂动力学模型的不确定部分进行在线补偿，使补偿后的阻抗控制器能够实现对机械臂的精准柔顺控制。

本发明提供了一种基于RBF神经网络的机械臂自适应阻抗控制方法，包括如下步骤：

S1：建立机械臂的D-H参数，并推导机械臂的正运动学；

S2：利用kane方法建立机械臂系统的名义动力学模型；

S3：根据步骤S2中建立的机械臂系统的名义动力学模型，结合二阶阻抗方程设计阻抗控制器；

S4：设定RBF神经网络的初始参数，设计动力学模型误差补偿控制器；

S5：将步骤S3中设计的阻抗控制器与步骤S4中建立的动力学模型误差补偿控制器结合，并根据李雅普诺夫函数设计RBF神经网络权值矩阵的变化率，构成自适应阻抗控制器，并证明所构成的自适应阻抗控制器的稳定性；

S6：根据步骤S5中所设计的自适应阻抗控制器，判断机械臂柔顺控制的效果，在相同的控制器参数的条件下设置期望接触力为0，以接触力大小为指标，机械臂接触力越小则柔顺控制效果越好。

进一步，步骤S1具体过程为：

假定机械臂的自由度为n，即把机械臂看作由n个连杆，n-1个关节连接组成，关节1为第1根连杆和第2根连杆的连接关节，首先根据如下步骤建立机械臂的D-H坐标系：

1)建立基座坐标系：以基座上感兴趣的位置为原点、关节1的运动轴正方向为z₀轴，按右手定则建立右手正交坐标系(x₀,y₀,z₀)，其中，x₀轴和y₀轴与z₀轴垂直，方向任选；

2)建立连杆i坐标系的z_i轴：以连杆i与连杆i-1连接关节的运动轴正向为z_i轴；

3)建立连杆i坐标系的原点O_i：若z_i轴和连杆i-1坐标系的z_i-1轴相交，则以两轴交点为原点O_i；若z_i轴和z_i-1轴异面或平行，则以两轴的公垂线与z_i轴的交点为原点O_i；

4)建立连杆i坐标系的x_i轴：按x_i＝±(z_i-1×z_i)/||z_i-1×z_i||建立x_i轴，即，使x_i轴与z_i-1轴及z_i轴同时垂直；若z_i-1轴与z_i轴平行，则以它们的公垂线为x_i轴；

5)建立连杆i坐标系的y_i轴：根据已建立的x_i轴和z_i轴，按右手定则建立y_i轴，即令y_i＝(z_i×x_i)/||z_i×x_i||；

6)建立机械臂末端坐标系(x_n,y_n,z_n)：z_n轴与连杆n-1坐标系的z_n-1轴平行，但指向机器人本体外方向；x_n轴与z_n-1轴及z_n轴同时垂直；y_n轴由右手定则确定，由此完成机械臂的D-H坐标系的建立；

根据建立的连杆坐标系，计算从一个关节到下一个相邻关节的变换矩阵，变换通过旋转-平移-旋转-平移四个子变换得到：绕连杆i-1的x_i-1轴转角度量α_i-1；沿x_i-1轴移动量a_i-1；绕连杆i的z_i轴转动角度量θ_i；沿z_i轴移动量d_i；

第i-1根连杆相对于第i根连杆的位姿变换矩阵A_i定义如下：

^i-1T_i＝A_i＝Rot(x,α_i-1)Trans(x,a_i-1)Rot(z,θ_i)Trans(z,d_i) (1)

其中，^i-1T_i用来描述连杆末端在基座的位姿态，表示第i-1根连杆的连杆坐标系相对于第i根连杆的连杆坐标系的变换；Rot表示旋转；Trans表示移动，

计算得到第i-1根连杆相对于第i根连杆的位姿变化矩阵A_i的通式为：

其中，C表示余弦三角函数；S表示正弦三角函数，

将各连杆相对于其前一根连杆的位姿变换矩阵相乘，得到机械臂总变换矩阵⁰T_n，即得到机械臂的正运动学：

⁰T_n＝⁰T₁ ¹T₂…^n-1T_n＝A₁A₂…A_n (3)。

进一步，步骤S2中利用kane方法建立机械臂系统的名义动力学模型，得到机械臂与环境接触时的名义动力学方程：

其中，τ为机械臂收到的驱动关节的控制力矩；M_c(q)为机械臂的惯性矩阵，并且为正定对称矩阵；

为离心力和哥氏力矢量；g_c(q)为重力矢量；q为关节旋转角矩阵；

为关节旋转角速度矩阵；

为关节旋转角加速度矩阵；J^T(q)为雅克比矩阵的转置矩阵；F_e为机械臂末端与环境的接触力。

进一步，步骤S3具体过程如下：

二阶阻抗方程式为

其中，M、B、K为阻抗参数，；x_d为机械臂末端的期望位置；x为机械臂末端位置；

为机械臂末端的期望加速度；

为为机械臂末端加速度；

为机械臂末端的期望速度；

为机械臂末端速度；

利用逆动力学求得机械臂的操作律为：

其中，τ_c为阻抗控制器输出的机械臂控制力矩，

将机械臂与环境接触时的名义运动学方程式(4)左右两边同时求二阶导求得关节旋转角加速度矩阵

其中，J^-1(q)为雅克比矩阵的逆矩阵；

为雅可比矩阵的变化率，

将关节旋转角加速度矩阵

代入逆动力学得到阻抗控制律：

进一步，步骤S4中，RBF神经网络的初始参数包括输入层节点数、输出层节点数、隐含层节点数、隐含层节点中心向量和高斯函数宽度。

进一步，步骤S4具体过程如下：

动力学模型误差为计算得到的模型与真实的模型之差，则真实的机械臂动力学模型为：

其中，ΔM(q),

Δg(q)为动力学误差项；g(q)为计算得到的动力学模型中的重力项；d为干扰项；

将干扰项与动力学误差项合并得到：

其中，动力学误差函数

动力学模型误差补偿控制器输出的补偿力矩为Δτ：

其中，

为由RBF神经网络估计的动力学误差函数；

为估计权值矩阵；h(x)为高斯函数矩阵。

进一步，步骤S5具体过程如下：

将步骤S3中设计的阻抗控制器与步骤S4中建立的动力学模型误差补偿控制器结合构成机械臂自适应阻抗控制器，该自适应阻抗控制器输出的控制力矩τ_out为：

τ_out＝τ_c+Δτ (10)

将自适应阻抗控制器输出的控制力矩代入真实的机械臂动力学模型即可得到机械臂系统状态方程，

其中，

为最佳动力学误差函数；

为最佳逼近权值；

为w^*的估计权值；w^*为最佳权值逼近误差；

为逼近误差，

h(x)为高斯函数矩阵；

根据李雅普诺夫函数设计RBF神经网络权值矩阵的变化率，并证明控制器的稳定性，构造如下李雅普诺夫函数V：

其中，γ＞0；矩阵P是对称正定矩阵，且满足李雅普诺夫方程：

PA+A^TP＝-Q (13)

其中，Q>0；

对李雅普诺夫函数V求导得到：

其中，tr()为求矩阵的迹；

为最佳权值逼近误差的变化率；

设计自适应律为：

则

假设逼近误差是有限的，

其中，η₀为逼近误差上界；sup为上确界，即最小上界；

根据逼近误差上界得到：

其中，λ_min(Q)为矩阵Q的最小特征值；λ_max(P)为矩阵P的最大特征值。为保证控制系统问题，由李雅普诺夫第二法则需要李雅普诺夫的导数小于0，即满足以下条件：

由式(19)得到结论：增大Q的特征值和减小P的特征值可以提高收敛效果。

本发明的有益效果：

1)本发明将动力学模型的不确定部分与名义动力学部分分开考虑，机械臂的控制力矩分别由两个控制器提供，保证了控制器的有效性；

2)本发明使用RBF神经网络在线逼近动力学模型的不确定部分，使机械臂获得更好的柔顺控制效果；

3)本发明简单易于实施，是一种具有高鲁棒性的柔顺控制方法。

附图说明

图1为本发明提供的基于RBF神经网络的机械臂自适应阻抗控制方法流程图；

图2为本发明的D-H坐标系的建立机械臂的构型参数；

图3为本发明的自适应阻抗控制器原理示意图；

图4为本发明的2自由度机械臂结构及碰撞墙面示意图；

图5为本发明的机械臂末端位置图；

图6为本发明的机械臂末端接触力图。

具体实施方式

下面结合附图对本发明作进一步详细说明。

如图1所示，本发明的基于RBF神经网络的机械臂自适应阻抗的控制方法，包括如下步骤：

S1：建立机械臂的D-H参数并推导机械臂的正运动学。将机械臂看作多根连杆连接，连杆与连杆之间的连接点叫做关节，在每一根连杆上建立一个坐标系多次使用齐次坐标变换即可得到机械臂末端坐标系和基座坐标系之间的关系，获得机械臂的正运动学。

D-H坐标系是描述机械臂运动的惯用坐标系，其示意图如图2所示，结合图2按照以下步骤建立D-H坐标系，其中假定机械臂的自由度为n，即机械臂由n个连杆，n-1个关节连接组成，建立D-H坐标系具体过程如下：

1)建立基座坐标系：以基座上感兴趣的位置为原点、关节1(即第1根连杆和第2根连杆的连接关节)的运动轴正方向为z₀轴，按右手定则建立右手正交坐标系(x₀,y₀,z₀)，其中，x₀轴和y₀轴与z₀轴垂直，方向任选；

对每个连杆i(i＝1、…，n)，完成步骤2)至步骤5)：

2)建立连杆i坐标系的z轴(即z_i轴)：以连杆i与连杆i-1连接关节的运动(转动或移动)轴正向为z_i轴；

3)建立连杆i坐标系的原点O_i：若z_i轴和z_i-1轴相交，则以两轴交点为原点O_i；若z_i轴和z_i-1轴异面或平行，则以两轴的公垂线与z_i轴的交点为原点O_i；

4)建立连杆i坐标系的x_i轴：按x_i＝±(z_i-1×z_i)/||z_i-1×z_i||建立x_i轴，即，使x_i轴与z_i-1轴及z_i轴同时垂直；若z_i-1轴与z_i轴平行，则以它们的公垂线为x_i轴；

5)建立连杆i坐标系的y_i轴：根据已建立的x_i轴和z_i轴，按右手定则建立y_i轴，即令y_i＝(z_i×x_i)/||z_i×x_i||；

6)建立机械臂末端坐标系(x_n,y_n,z_n)：z_n轴与z_n-1轴平行，但指向机器人本体外方向；x_n轴与z_n-1轴及z_n轴同时垂直；y_n轴由右手定则确定。

通过各关节位置和连杆几何参数求解机械臂末端相对于基座的位置与姿态，即为机械臂正运动学。其基本思路为：根据建立的连杆体坐标系，计算从一个关节到下一个相邻关节的变换矩阵，最后将所有变换矩阵相乘即得到机械臂总体变换矩阵。

用^i-1T_i来描述连杆末端在基座的位姿态，^i-1T_i表示第i-1根连杆的连杆坐标系相对于第i根连杆的连杆坐标系的变换。其变换通过“旋转-平移-旋转-平移”四个子变换得到：绕x_i-1轴转α_i-1角；沿x_i-1轴移动a_i-1；绕z_i轴转θ_i角；沿z_i轴移动d_i。连杆变换矩阵定义如下：

^i-1T_i＝A_i＝Rot(x,α_i-1)Trans(x,a_i-1)Rot(z,θ_i)Trans(z,d_i) (1)

其中，A_i表示第i-1根连杆相对于第i根连杆的位姿变化矩阵；Rot表示旋转；Trans表示移动。

计算得到第i-1根连杆相对于第i根连杆的位姿变化矩阵的通式为：

其中，C表示余弦三角函数；S表示正弦三角函数。

将各连杆相对于其前一根连杆的位姿变换矩阵相乘，得到机械臂总变换矩阵⁰T_n，即得到机械臂的正运动学：

⁰T_n＝⁰T₁ ¹T₂…^n-1T_n＝A₁A₂…A_n (3)。

S2：利用kane方法建立机械臂系统的名义动力学模型。kane方法是利用广义速率代替广义坐标作为独立变量来描述系统的运动，并将广义主动力和广义惯性力直接向特定的基矢量方向投影，以消除理想约束力。该方法中不出现相邻体之间的内力项，使推导过程更加系统化，适用于计算机编程。本发明为了计算的快速性和编程的简便性，基于kane方程，建立多体系统的动力学模型。且将Kane方程中偏速度和偏角速度的概念推广为偏速度矩阵和偏角速度矩阵，把各体的广义惯性力统一为相同的、简洁的数学形式，使系统动力学方程易于通过计算机符号或者数值的建立，避免了建模中繁琐的公式推导。本发明利用Kane方法建立机械臂系统的名义动力学模型，是将机械臂相邻连杆通过单自由度的转动连接，建立了该多体系统的动力学模型，该模型不包含机械臂动力学模型的不确定项，故称为机械臂系统的名义动力学模型，最后可以得到机械臂与环境接触时的名义动力学方程:

其中，τ为机械臂收到的驱动关节的控制力矩；M_c(q)为机械臂的惯性矩阵，并且为正定对称矩阵；

为离心力和哥氏力矢量；g_c(q)为重力矢量；q为关节旋转角矩阵；

为关节旋转角速度矩阵；

为关节旋转角加速度矩阵；J^T(q)为雅克比矩阵的转置矩阵；F_e为机械臂末端与环境的接触力。

S3：根据步骤S2中建立的机械臂系统的名义动力学模型，结合二阶阻抗方程设计阻抗控制器。

二阶阻抗方程式为

其中，M、B、K为阻抗参数，；x_d为机械臂末端的期望位置；x为机械臂末端位置；

为机械臂末端的期望加速度；

为为机械臂末端加速度；

为机械臂末端的期望速度；

为机械臂末端速度。

利用逆动力学求得机械臂的操作律为

将机械臂与环境接触时的名义运动学方程左右两边同时求二阶导求得关节角加速度

其中，J^-1(q)为雅克比矩阵的逆矩阵；

为雅克比矩阵的变化率。

将关节角加速度代入逆动力学得到阻抗控制律：

其中，τ_c为控制器输出的机械臂控制力矩。本发明利用逆动力学设计阻抗控制器，其稳定性非常容易证明。

S4：设定RBF神经网络的初始参数，包括输入层节点数、输出层节点数、隐含层节点数、隐含层节点中心向量和高斯函数宽度，设计动力学模型误差补偿控制器。RBF神经网络对非线性函数具有较强的逼近能力，所以可以基于RBF神经网络设计一个补偿控制器，对机械臂的动力学模型误差进行补偿。再设计李雅普诺夫函数，确定RBF神经网络权值矩阵的变化率。

动力学模型误差为计算得到的模型与真实的模型之差，则真实的机械臂动力学模型为：

其中，ΔM(q),

Δg(q)为动力学误差项；g(q)为计算得到的动力学模型中的重力项；d为干扰项。

将干扰项与动力学误差项合并可以得到：

其中，动力学误差函数

RBF神经网络便是在机械臂工作时在线对该误差函数进行逼近，并通过动力学模型误差补偿控制器对控制力矩进行补偿。动力学模型误差补偿控制器输出的补偿力矩为

其中，Δτ为补偿控制器输出的补偿力矩；

为由RBF神经网络估计的动力学误差函数；

为估计权值矩阵；h(x)为高斯函数矩阵。

S5：将阻抗控制器与动力学模型误差补偿控制器结合构成机械臂自适应阻抗控制器，证明自适应阻抗控制器的稳定性，自适应阻抗控制器原理示意图如图3所示。其中将阻抗控制器与动力学模型误差补偿控制器结合，是指自适应阻抗控制器输出的控制力矩由两部分相加组成，一是由名义模型决定的阻抗控制器输出的控制力矩，二是由RBF神经网络逼近动力学不确定项后误差补偿控制器输出的补偿控制力矩。

自适应阻抗控制器输出的控制力矩为

τ_out＝τ_c+Δτ (10)

将自适应阻抗控制器输出的控制力矩代入真实的机械臂动力学模型即可得到机械臂系统状态方程，

其中，

为最佳动力学误差函数；

为最佳逼近权值；

为w^*的估计权值；w^*为最佳权值逼近误差；

为逼近误差，

h(x)为高斯函数矩阵。

根据李雅普诺夫函数设计RBF神经网络权值矩阵的变化率，并证明控制器的稳定性。构造如下李雅普诺夫函数V：

其中，γ＞0；矩阵P是对称正定矩阵，且满足李雅普诺夫方程：

PA+A^TP＝-Q (13)

其中，Q>0。对李雅普诺夫函数V求导可以得到：

其中，tr()为求矩阵的迹；

为最佳权值逼近误差的变化率。

设计自适应律为：

则

假设逼近误差是有限的，

其中，η₀为逼近误差上界；sup为上确界，即最小上界。

根据逼近误差上界可得

其中，λ_min(Q)为矩阵Q的最小特征值；λ_max(P)为矩阵P的最大特征值。为保证控制系统问题，由李雅普诺夫第二法则需要李雅普诺夫的导数小于0，即满足以下条件：

由该条件可以得到结论：增大Q的特征值和减小P的特征值可以提高收敛效果。

S6：根据步骤S5所设计的控制器，判断机械臂柔顺控制的效果。设置自适应阻抗控制器的期望接触力为0，则机械臂末端接触力的大小即可反应机械臂的柔顺性能。接触力的值越小，则柔顺性能越好。

实施例

本实施例通过对一个动力学模型存在误差的2自由度机械臂进行柔顺控制，来验证本发明方法的有效性。

(1)2自由度机械臂结构及碰撞墙面示意图如图4所示，墙面(x_e表示墙面的位置)距离坐标远点0.18m其正运动学方程为

其中，x为机械臂末端x方向的位移；y为机械臂末端y方向的位移；l₁为2自由度机械臂第一根连杆长度；l₁为第二根连杆长度；θ₁为关节1旋转的角度，θ₂为关节2旋转的角度。

将正运动学方程(20)两边求导可获得雅克比矩阵J(q)：

(2)利用Kane(凯恩)方法求得2自由度机械臂动力学方程为

其中，τ₁为关节1的驱动力矩，τ₂为关节2的驱动力矩，m₁为第一根连杆的质量，m₂为第二根连杆的质量，g为重力加速度。

将式(22)写为机械臂名义动力学方程的形式：

其中，

(3)如本发明步骤S3-步骤S5设计自适应阻抗控制器，并与不含补偿控制器的阻抗控制器进行对比。

自适应阻抗控制器设置参数为

动力学误差项为名义项的0.2，RBF神经网络初始阈值和权值随机赋值。机械臂末端位移如图5所示，机械臂末端接触力如图6所示。通过相应曲线可以看出，在机械臂模型存在误差的情况下，基于RBF神经网络自适应阻抗控制器的柔顺控制效果要好于传统阻抗控制器，且该控制器可以在控制的同时在线逼近模型误差部分。

综上，本发明针对模型存在不确定性的机械臂给出了一种基于RBF神经网络的自适应阻抗控制，通过本发明控制的机械臂能够表现良好的柔顺性能，故该方法具有良好的推广前景。

对于本领域的普通技术人员来说，在不脱离本发明创造构思的前提下，还可以对本发明的实施例做出若干变型和改进，这些都属于本发明的保护范围。

Claims (7)

1.一种基于RBF神经网络的机械臂自适应阻抗控制方法，其特征在于，包括如下步骤：

S1：建立机械臂的D-H参数，并推导机械臂的正运动学；

S2：利用kane方法建立机械臂系统的名义动力学模型；

S3：根据步骤S2中建立的机械臂系统的名义动力学模型，结合二阶阻抗方程设计阻抗控制器；

S4：设定RBF神经网络的初始参数，设计动力学模型误差补偿控制器；

S5：将步骤S3中设计的阻抗控制器与步骤S4中建立的动力学模型误差补偿控制器结合，并根据李雅普诺夫函数设计RBF神经网络权值矩阵的变化率，构成自适应阻抗控制器，并证明所构成的自适应阻抗控制器的稳定性；

S6：根据步骤S5中所设计的自适应阻抗控制器，判断机械臂柔顺控制的效果，在相同的控制器参数的条件下设置期望接触力为0，以接触力大小为指标，机械臂接触力越小则柔顺控制效果越好。

2.根据权利要求要求1所述的方法，其特征在于，步骤S1具体过程为：

假定机械臂的自由度为n，即把机械臂看作由n个连杆，n-1个关节连接组成，关节1为第1根连杆和第2根连杆的连接关节，首先根据如下步骤建立机械臂的D-H坐标系：

1)建立基座坐标系：以基座上感兴趣的位置为原点、关节1的运动轴正方向为z₀轴，按右手定则建立右手正交坐标系(x₀,y₀,z₀)，其中，x₀轴和y₀轴与z₀轴垂直，方向任选；

2)建立连杆i坐标系的z_i轴：以连杆i与连杆i-1连接关节的运动轴正向为z_i轴；

3)建立连杆i坐标系的原点O_i：若z_i轴和连杆i-1坐标系的z_i-1轴相交，则以两轴交点为原点O_i；若z_i轴和z_i-1轴异面或平行，则以两轴的公垂线与z_i轴的交点为原点O_i；

4)建立连杆i坐标系的x_i轴：按x_i＝±(z_i-1×z_i)/||z_i-1×z_i||建立x_i轴，即，使x_i轴与z_i-1轴及z_i轴同时垂直；若z_i-1轴与z_i轴平行，则以它们的公垂线为x_i轴；

5)建立连杆i坐标系的y_i轴：根据已建立的x_i轴和z_i轴，按右手定则建立y_i轴，即令y_i＝(z_i×x_i)/||z_i×x_i||；

6)建立机械臂末端坐标系(x_n,y_n,z_n)：z_n轴与连杆n-1坐标系的z_n-1轴平行，但指向机器人本体外方向；x_n轴与z_n-1轴及z_n轴同时垂直；y_n轴由右手定则确定，由此完成机械臂的D-H坐标系的建立；

根据建立的连杆坐标系，计算从一个关节到下一个相邻关节的变换矩阵，变换通过旋转-平移-旋转-平移四个子变换得到：绕连杆i-1的x_i-1轴转角度量α_i-1；沿x_i-1轴移动量a_i-1；绕连杆i的z_i轴转动角度量θ_i；沿z_i轴移动量d_i；

第i-1根连杆相对于第i根连杆的位姿变换矩阵A_i定义如下：

^i-1T_i＝A_i＝Rot(x,α_i-1)Trans(x,a_i-1)Rot(z,θ_i)Trans(z,d_i) (1)

其中，^i-1T_i用来描述连杆末端在基座的位姿态，表示第i-1根连杆的连杆坐标系相对于第i根连杆的连杆坐标系的变换；Rot表示旋转；Trans表示移动，

计算得到第i-1根连杆相对于第i根连杆的位姿变化矩阵A_i的通式为：

其中，C表示余弦三角函数；S表示正弦三角函数，

将各连杆相对于其前一根连杆的位姿变换矩阵相乘，得到机械臂总变换矩阵⁰T_n，即得到机械臂的正运动学：

⁰T_n＝⁰T₁ ¹T₂...^n-1T_n＝A₁A₂...A_n (3)。

3.根据权利要求1所述的方法，其特征在于，步骤S2中利用kane方法建立机械臂系统的名义动力学模型，得到机械臂与环境接触时的名义动力学方程：

其中，τ为机械臂收到的驱动关节的控制力矩；M_c(q)为机械臂的惯性矩阵，并且为正定对称矩阵；

为离心力和哥氏力矢量；g_c(q)为重力矢量；q为关节旋转角矩阵；

为关节旋转角速度矩阵；

为关节旋转角加速度矩阵；J^T(q)为雅克比矩阵的转置矩阵；F_e为机械臂末端与环境的接触力。

4.根据权利要求3所述的方法，其特征在于，步骤S3具体过程如下：

二阶阻抗方程式为

其中，M、B、K为阻抗参数；x_d为机械臂末端的期望位置；x为机械臂末端位置；

为机械臂末端的期望加速度；

为为机械臂末端加速度；

为机械臂末端的期望速度；

为机械臂末端速度；

利用逆动力学求得机械臂的操作律为：

其中，τ_c为控制器输出的机械臂控制力矩,

将机械臂与环境接触时的名义运动学方程式(4)左右两边同时求二阶导求得关节旋转角加速度矩阵

其中，J^-1(q)为雅克比矩阵的逆矩阵；

为雅克比矩阵的变化率，

将关节旋转角加速度矩阵

代入逆动力学得到阻抗控制律：

5.根据权利要求1所述的方法，其特征在于，步骤S4中，RBF神经网络的初始参数包括输入层节点数、输出层节点数、隐含层节点数、隐含层节点中心向量和高斯函数宽度。

6.根据权利要求1所述的方法，其特征在于，步骤S4具体过程如下：

动力学模型误差为计算得到的模型与真实的模型之差，则真实的机械臂动力学模型为：

其中，ΔM(q),

Δg(q)为动力学误差项；g(q)为计算得到的动力学模型中的重力项；d为干扰项；

将干扰项与动力学误差项合并得到：

其中，动力学误差函数

动力学模型误差补偿控制器输出的补偿力矩为Δτ：

其中，

为由RBF神经网络估计的动力学误差函数；

为估计权值矩阵；h(x)为高斯函数矩阵。

7.根据权利要求1所述的方法，其特征在于，步骤S5具体过程如下：

将步骤S3中设计的阻抗控制器与步骤S4中建立的动力学模型误差补偿控制器结合构成机械臂自适应阻抗控制器，该自适应阻抗控制器输出的控制力矩τ_out为：

τ_out＝τ_c+Δτ (10)

将自适应阻抗控制器输出的控制力矩代入真实的机械臂动力学模型即得到机械臂系统状态方程，

其中，

为最佳动力学误差函数；

为最佳逼近权值；

为w^*的估计权值；w^*为最佳权值逼近误差；

为逼近误差，

h(x)为高斯函数矩阵；

根据李雅普诺夫函数设计RBF神经网络权值矩阵的变化率，并证明控制器的稳定性，构造如下李雅普诺夫函数V：

其中，γ＞0；矩阵P是对称正定矩阵，且满足李雅普诺夫方程：

PA+A^TP＝-Q (13)

其中，Q>0；

对李雅普诺夫函数V求导得到：

其中，tr()为求矩阵的迹；

为最佳权值逼近误差的变化率；

设计自适应律为：

则

假设逼近误差是有限的，

其中，η₀为逼近误差上界；sup为上确界，即最小上界；

根据逼近误差上界得到：

其中，λ_min(Q)为矩阵Q的最小特征值；λ_max(P)为矩阵P的最大特征值；为保证控制系统问题，由李雅普诺夫第二法则需要李雅普诺夫的导数小于0，即满足以下条件：

由式(19)得到结论：增大Q的特征值和减小P的特征值可以提高收敛效果。

Images