CN111319036B - 基于自适应算法的移动机械臂位置/力自抗扰控制方法 - Google Patents

Abstract

基于自适应算法的移动机械臂位置/力自抗扰控制方法，该方法依据性能函数利用梯度下降法及改进过程方法，设计参数自适应估计算法来实时调节目标阻抗参数，使目标阻抗模型能够适应外界环境。然后设计了关节空间的阻抗控制器，利用改进扩张状态观测器对系统的扰动进行估计并在阻抗控制器中进行补偿。仿真实验表明，该方法能够在外界环境不确定的情况下实现位置/力的准确控制。

Description

基于自适应算法的移动机械臂位置/力自抗扰控制方法

技术领域

本发明属于一种移动机械臂的位置/力控制领域，尤其涉及一种基于自适应算法的移动机械臂位置/力自抗扰控制方法。

背景技术

移动机械臂是将机械臂固定在移动平台上而构成的一类移动机器人系统。它是一类典型的强耦合、非线性的多输入多输出系统。在实际的工程应用中，一些复杂任务的执行要求移动机械臂末端执行器与外界进行交互。当与外界环境进行交互时，通常采用阻抗控制策略对移动机械臂的位置/力进行控制。但是在移动机械臂末端与环境的接触作用过程中，环境的变化会使得目标阻抗的参数发生变化；即使在同一环境下，移动机械臂与环境间的接触力不同或移动机械臂末端处于不同姿态时其等效阻尼、刚度、惯性矩阵也不同，因此，为了实现位置和力的精确跟踪，阻抗控制在实际应用中需要实时调整阻抗模型参数以适应外界环境的变化。目前，目标阻抗模型的参数调节问题是环境不确定情况下力控制研究中的难点问题。

针对移动机械臂与外界交互对象模型参数不确定性问题，很多学者进行了研究。国外方面，Blauer等依据力的测量值，采用最优估计算法来估计加工表面的参数及状态，并考虑了执行器末端与加工表面之间的摩擦力。(期刊：IEEE Transactions onAutomaticControl；著者:Blauer.M.andP.Belanger；出版年月:1987；文章题目：State andparameterestimation for robotic manipulators using force measurements,页码：1055-1066)。Natale等采用了基于无源性理论的自适应控制算法来进行机械臂的位置/力控制，并利用基于操作力误差的自适应算法估计交互对象模型中的未知参数，进行了实验验证。(会议：The 37th IEEE Conference on Decision and Control；著者:Natale C,Villani L；出版年月:1998；文章题目：Passivity-based design and experimental validationofadaptive force/position controllers for robot manipulators,页码：427-432)。Jung等人基于阻抗控制，设计了阻抗参数自适应调节控制算法，使得目标阻抗能够根据外界的变化来调节阻抗参数以解决上述问题。(期刊：IEEE Transactions on ControlSystems Technology；著者:Jung,Seul,eta；出版年月:2004；文章题目：Force trackingimpedance control ofrobot manipulators under unknown envi-ronment,页码：474-483)。Capisani等人依据力传感器测量值来估计外界交互对象的参数，并采用二阶滑模控制算法实现位置/力的控制。(期刊：I IEEE Transactions on Industrial Electronics；著者:Capisani,Luca Massimiliano and A.Ferrara；出版年月:2012；文章题目：Trajectory planning and second-order sliding mode motion/interaction controlfor robot manipulators in unknown environments,页码：3189-3198)。Yanan等人利用迭代学习的方法调节目标阻抗的参数以适应外界环境的变化，并根据阻抗关系来确定移动机械臂末端的期望位置，之后设计了位置控制算法对末端的位置进行了控制，通过实验验证了该算法的有效性。(期刊：IEEE Transactions on Control Systems Technology；著者:Li Y,Ge SS；出版年月:2014；文章题目：Impedance learning for robotsinteracting with unknown environments,页码：1422-1432)。国内方面，国防科技大学的祝晓才等针对外部交互环境接触面为不确定曲面的问题，设计了鲁棒镇定控制律，在设计过程中使用参数有界但未知的二次曲面来近似不确定曲面，并进行了仿真验证。(期刊：动力学与控制学报；著者:祝晓才，董国华，蔡自兴等；出版年月:2006；文章题目：不确定曲面上非完整移动机器人的鲁棒镇定,页码：299-307)。华南理工大学的李智军等[35]针对交互环境表面可变形条件下的力控制问题，设计了基于两个解耦子空间并行的模糊控制器进行位置/力的混合控制。(期刊：Neural Computing&Applications；著者:Li Zhijun,et al.；出版年月:2006；文章题目：Intelligent compliant force/motion controlofnonholonomic mobile manipula-tor working on the nonrigid surface,页码：204)。华中科技大学的李正义等针对外界环境刚度系数未知的情况，提出了利用神经网络近似估计外界环境的刚度系数方法。还有一些其它学者针对上述问题设计了一系列目标阻抗参数的自适应调节算法，以达到适应外界环境的目的。(学位论文：华中科技大学；著者:李正义；出版年月:2011；文章题目：机器人与环境间力/位置控制技术研究与应用,页码：28-50)。

上述解决方案大多数是基于神经网络、迭代学习、模糊理论来估计环境模型参数或调整阻抗模型参数。但是在实际应用中外界环境是实时变化的，这些算法难以依据外界环境的变化进行实时调整。

发明内容

为克服现有技术的不足，本发明基于自适应算法的移动机械臂位置/力自抗扰控制方法，首先通过定义控制系统的性能函数并利用梯度下降法及改进过程方法设计阻抗参数自适应估计算法，该自适应估计算法根据目标阻抗模型中阻抗参数与位置、末端力传感器的测量值之间的关系调节目标阻抗参数，以使目标阻抗模型能够适应外界环境的变化；然后利用力的测量值得到末端参考位置并依据阻抗模型设计移动机械臂在关节空间下的阻抗控制器，同时在控制器中对改进扩张状态观测器所估计的扰动进行补偿，从而提高移动机械臂末端的轨迹追踪性能，实现接触力的精确控制。本方法能够根据反应外界环境变化的性能函数实时调整阻抗参数，从而保证当外界环境发生变化时，目标阻抗模型仍然可以适应外界环境，最终实现移动机械臂的位置/力的精确控制。

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

步骤一：建立动力学模型

定义惯性坐标系{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)，接下来设计阻抗参数自适应算法；

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

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

其中，γ，λ为常系数，

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

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

式中，

e_x(t)＝X(t)-X_d(t)；

最后，利用PD过程改进(the PD-type betterment process)算法对

的值进行估计；利用阻抗模型，设定操作空间的控制输入为：

定义如下表达式：

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

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

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

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

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

1、改进扩张状态观测器

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

定义控制系统的状态变量

可以得到系统的状态方程为：

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

其中，

α，η为常系数，β₂₁，β₃₁为观测器增益；由式(13)可知，利用观测误差

来调节z₂，观测误差

来调节z₃，加快了z₂、z₃的调节速度；且根据已有文献的分析，扩张状态观测器的观测误差是有界的；

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

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

2、阻抗控制器

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

F_ε＝K_ε(X-X_ε) (16)

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

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

其中，k_d，k_ε为K_d，K_ε中在力控制方向的元素；

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

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

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

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

式中，

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

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

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

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

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

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

下面对所设计的闭环控制系统的稳定性进行分析，取控制器追踪误差为：

可以得到：

取e_μ＝[e₁ e₂]^T，则式(25)整理可得：

其中，

定理：在控制系统中，若D(t)是有界的，则存在一个常向量σ＝[σ₁ … σ₆]^T且σ_i＞0，使得在有限时间内，向量e_μ中的每个元素满足|e_μi(t)|≤σ_i，i＝1，2，…，6。

证明：解方程(26)可得：

在已知改进扩张状态观测器估计误差有界的基础上，可得：

定义Γ＝[0 0 0 ρ₄ ρ₅ ρ₆]^T，得到：

由式(26)可知存在常数矩阵A_εmin和A_εmax，使得A_εmin≤A_ε≤A_εmax成立。则对于t＞T₁，下式成立：

由此可知，下式成立：

其中，ρ_max＝max{ρ₄：ρ₅：ρ₆}。对于所有：

令T＝max{T₁，T₂}，则对于t≥T：

令e_max(0)＝max(|e_μ1(0)|，|e_μ2(0)|，...|e_μ5(0)|)，则：

因此，对于所有t≥T，满足：

由式(35)可知，本方案中的控制系统是有界输入有界输出稳定。

基于自适应算法的移动机械臂位置/力自抗扰控制方法，该方法依据性能函数利用梯度下降法及改进过程方法，设计参数自适应估计算法来实时调节目标阻抗参数，使目标阻抗模型能够适应外界环境。然后设计了关节空间的阻抗控制器，利用改进扩张状态观测器对系统的扰动进行估计并在阻抗控制器中进行补偿。仿真实验表明，该方法能够在外界环境不确定的情况下实现位置/力的准确控制。

附图说明

图1是本发明中移动机械臂的工作示意图；

图2是本发明所设计的控制系统的结构框图；

图3a是全方位移动机械臂位置/力控制的第一组仿真末端接触力控制曲线；

图3b是全方位移动机械臂位置/力控制的第一组仿真阻尼、刚度参数实时变化曲线；图3c是全方位移动机械臂位置/力控制的第一组仿真末端运动轨迹曲线；

图3d是全方位移动机械臂位置/力控制的第一组仿真末端运动轨迹误差曲线；

图3e是全方位移动机械臂位置/力控制的第一组仿真移动机械臂的各个关节的运动轨迹曲线；

图4a是全方位移动机械臂位置/力控制的第二组仿真末端接触力控制曲线；

图4b是全方位移动机械臂位置/力控制的第二组仿真阻尼、刚度参数实时变化曲线；图4c是全方位移动机械臂位置/力控制的第二组仿真末端运动轨迹曲线；

图4d是全方位移动机械臂位置/力控制的第二组仿真末端运动轨迹误差曲线。

具体实施方式

本发明所设计的控制系统的结构框图如图2所示。为验证本发明所设计的控制算法的有效性，以MATLAB作为仿真平台，以具有5自由度的两连杆全方位移动机械臂(图1所示)为控制对象进行了移动机械臂位置/力的控制仿真实验的验证。下面结合仿真实验和附图对本发明针对在外界环境不确定的条件下，移动机械臂位置/力的控制方法作出详细说明。

本发明针对在外界环境不确定的条件下，全方位移动机械臂位置/力的控制问题，利用梯度下降法和过程改进方法设计了阻抗参数自适应算法，然后设计了基于阻抗参数自适应算法的自抗扰控制方法，最终实现了移动机械臂控制系统在外界环境不确定情况下的稳定控制。

设定两组对比仿真，两组仿真分别设定不同的环境变化形式，其它设定值均相同。通过对比仿真来验证所设计的自适应算法适用于不同形式的环境变化。

如图1所示，仿真中移动机械臂的任务为按照给定接触力与平面接触并按照给定轨迹在平面上运动。仿真中动力学模型中各参数：平台质量m₀＝30kg，连杆1质量m₁＝4kg，连杆2质量m₂＝35kg，连杆1长度l₁＝05m，连杆2长度l₂＝035m，平台转动惯量J₀＝532kgm²，连杆1转动惯量J₁＝034kg.m²，连杆2转动惯量J₂＝015kg.m²。自适应算法中常系数γ，λ的取值为：γ＝60，λ＝180。阻抗参数M_d，B_d，K_d的初始值为：

观测器参数设置为ω₀＝8，b₁＝5，b₂＝2，α＝0.75，η＝2。

在第一组仿真中，仿真中环境刚度如下所示：

即在前第10s(‘s’为秒，下同)、20s、30s分别改变环境的刚度系数。

设定期望接触力为：

移动机械臂末端按照以下轨迹运动：

式中，X_E为移动机械臂末端姿态X在X_w方向的分量，X_d＝[X_Ed Y_Ed Z_Ed]^T为移动机械臂末端位置变量的期望轨迹；

为X_E的初始位置。

系统仿真时间为80s，采样时间为0.01s。全方位移动机械臂的初始位置、接触力的初始值分别设定为：X_ε＝[1 0.35 0]^T，F_ε＝[0 0 0]^T。仿真结果分别如图3(a)、3(b)、3(c)、3(d)、3(e)所示。图3(a)表示采用固定阻抗参数(IMC)的控制系统与采用阻抗参数自适应算法(AIMC)的控制系统的控制效果。阻抗参数采用固定值时，当外界环境发生变化时会使得实际接触力无法跟踪期望接触力，两者之间存在较大误差；而采用阻抗参数自适应算法时，外界环境发生变化后实际接触力能够迅速调整并跟踪上期望接触力；图3(b)表示力控制方向的b_d：k_d两个参数的变化曲线，当外界环境发生变化时，两个参数快速调整使得目标阻抗模型能够适应外界环境的变化；图3(c)表示移动机械臂末端的运动轨迹，由仿真曲线可以看出末端实际运动轨迹能够精确的追踪期望运动轨迹；图3(d)表示移动机械臂末端轨迹三个方向的误差，当外界环境发生变化时力控制方向的轨迹追踪误差突然增大，之后快速收敛到零而其它两个方向的轨迹追踪误差并没有明显的变化，是因为在阻抗控制模型中力控制方向与位置控制方向是完全解耦的；图3(e)表示移动机械臂各关节运行轨迹，每个关节运动轨迹都是光滑且连续的没有出现奇异值。

第二组仿真中设定外界环境的刚度为：

k_ε＝5000+2500sin(π10t)

第二组仿真中移动机械臂末端的位置及接触力的初始值、末端期望轨迹、控制器参数均与第一组仿真相同。仿真结果分别如图4(a)、4(b)、4(c)、4(d)所示。图4(a)表示接触力控制曲线，当移动机械臂末端在接触外界环境的时刻，末端接触力出现了相对较大的误差，但之后末端接触力能够比较精确的追踪其期望值；图4(b)表示表示力控制方向的两个参数的变化曲线；图4(c)表示移动机械臂末端位置方向运动轨迹；图4(d)表示末端三个方向的位置控制误差，控制误差在合理范围内。对比两组仿真结果可以得出以下结论：对于不同形式的外界环境变化，本方案中的参数自适应算法都能够使得实际接触力快速追踪期望接触力，证明所设计的阻抗参数自适应算法对外界环境的变化形式具有很强的适应能力。

经过上述分析，证明了本发明算法的有效性。

Claims (1)

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的零空间中的任意向量；

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

Images