[发明专利]基于自适应算法的移动机械臂位置/力自抗扰控制方法

权利要求书

1.基于自适应算法的移动机械臂位置/力自抗扰控制方法，其特征在于：步骤如下：

步骤一：建立动力学模型

定义惯性坐标系{W}，基于拉格朗日方程建立全方位移动机械臂一体化动力学模型，得到全方位移动机械臂的动力学模型：

式中，q＝[q₁，...，q_n]^T∈Rⁿ表示移动机械臂在惯性坐标系下的广义坐标，[·]^T表示矩阵的转置，∈表示集合间的“属于”关系，n表示移动机械臂的自由度，下同；M(q)∈R^n×n表示惯性矩阵，R^n×n表示n行n列的实数向量，下同；表示离心力矩和哥氏力矩；G(q)∈Rⁿ表示重力力矩；τ∈Rⁿ表示控制输入；J∈R^n×m表示几何约束雅克比矩阵，m表示几何约束的维度，τ_f∈Rⁿ表示与外界接触所产生的约束力矩；

步骤二：设计阻抗参数自适应算法

取移动机械臂末端目标阻抗模型为：

式中，M_d，B_d，K_d∈R^3×3，X_d，X，F_ed，F_e∈R^3×1；

阻抗控制是一种通过位置控制达到力控制目标的间接控制算法，基于阻抗模型式(2)，接下来设计阻抗参数自适应算法；

首先，定义一个描述控制系统性能的函数，将系统的位置、接触力控制误差函数作为描述控制系统性能的函数，如下所示：

E(t)＝(F_t(t)-F_td(t))^T(F_d(t)-F_td(t))+(X(t)-X_d(t))^T(X(t)-X_d(t)) (3)

其次，依据所定义的性能函数，利用梯度下降法调整阻尼参数、刚度参数，在线自适应的更新算法如下所示：

其中，γ，λ为常系数，由于在实际应用中，移动机械臂末端的加速度测量值中通常含有噪声难以得到精确值，因此惯性参数M_d采用固定值；

由阻抗模型可以得到自适应算法为：

式中，e_X(t)＝X(t)-X_d(t)；

最后，利用PD过程改进(the PD-type bettermentprocess)算法对的值进行估计；利用阻抗模型，设定操作空间的控制输入为：

定义如下表达式：

利用PD类型的过程改进算法，可以得到：

其中，e_f＝F_e-F_ed，Γ₀₁，Γ₀₂，Γ₀₃分别表示速度误差、位置误差、接触力误差的增益；式(8)给出了控制输入减少性能函数的方向；因此，我们利用式(8)来估计

由此得到参数更新算法为：

步骤三：设计位置/力控制算法

控制算法包括改进扩张状态观测器和阻抗控制器：

1、改进扩张状态观测器

假设控制系统所受的全部扰动为d(t)，全方位移动机械臂一体化动力学模型可以写为：

M_O＝M_d，C_O＝C_d，定义控制系统的状态变量x₁＝q，可以得到系统的状态方程为：

令z_i(i＝1，2，3)为状态变量x_i的估计值，改进扩张状态观测器设计如下所示：

其中，α，η为常系数，β₂₁，β₃₁为观测器增益；由式(13)可知，利用观测误差来调节z₂，观测误差来调节z₃，加快了z₂、z₃的调节速度；且根据已有文献的分析，改进扩张状态观测器的观测误差是有界的；

此外为了解决观测器增益引起的积分饱和现象，利用饱和函数tanh(·)来实现观测器变增益，即：

其中，b₁，b₂为常系数；利用改进扩张状态观测器，得到系统扰动的估计值为：

2、阻抗控制器

首先，计算移动机械臂末端期望轨迹，将外界环境动力学模型简化为弹性模型，则移动机械臂末端与环境之间的接触力为：

F_e＝K_e(X-X_e) 16)

其中，K_e为环境刚度，X_e为未发生接触时的环境位置，X表示移动机械臂末端实际位置变量，F_e为接触力；

移动机械臂与环境之间的系统等效刚度为k＝k_dd_e/(k_d+k_e)；由于机械臂是刚性的，我们认为k_d＞＞k_e；因此，系统等效刚度为：

其中，k_d，k_e为K_d，K_e中在力控制方向的元素；

移动机械臂末端期望轨迹X_d为：

然而在实际应用中外界接触环境的刚度难以得到精确数值，因此，我们利用接触力F_e、移动机械臂末端位置测量值X来估计末端期望轨迹X_d，如下：

其次，利用逆运动学关系将其转换到关节空间，从而实现对各个关节的控制；移动机械臂末端加速度与关节空间加速度之间的关系为：

全方位移动机械臂在操作空间是冗余的，因此雅克比矩阵J的逆矩阵不存在，需要使用伪逆矩阵J⁺来得到以下关系：

式中，为J的零空间速度，伪逆矩阵J⁺定义为满足以下条件的唯一矩阵：

当J为满秩时，可将J⁺写为：

J⁺＝J^T(JJ^T)^-1 (22)

根据式(20)可将操作空间的控制量转换到关节空间：

其中，φ_N为J的零空间中的任意向量；

最终，根据全方位移动机械臂动力学模型与目标阻抗模型，得到系统控制器为：