一种三关节点焊机器人轨迹跟踪控制方法

摘要

本发明公开了一种三关节点焊机器人轨迹跟踪控制方法，包括以下步骤：1) 建立三关节点焊机器人系统的数学模型，在实际工程中要得到精确数学模型十分困难，在建立点焊机器人的数学模型时，需要对点焊机器人做合理的近似处理，忽略一些不确定性因素，利用拉格朗日方法对水平三关节点焊机器人进行动力学建模；2) 扩张状态观测器设计，本发明采用扩张状态观测器可将不确定动态作为总和扰动的一部分进行估计。其可以有效地对多关节点焊机器人进行解耦控制，此方法对多关节机械人的不确定性和系统内外扰动以及模型不确定性均具有很强的抑制能力。

说明书

技术领域

本发明应用于机器人控制领域，涉及到工业机器人在实际应用中 的轨迹跟踪问题，尤其是汽车制造所用到的点焊机器人的路径规划和 轨迹跟踪，实现一种有效的实时控制方法。

背景技术

随着计算机和自动化技术的发展，特别是第一台计算机的诞生以 来，计算机取得了巨大进步，并逐渐向着巨型化、微型化、智能化、 网络化方向发展。同时，随着工业规模不断扩大，大批量生产的迫切 需求推动了自动化技术的不断进步，也为工业机器人的研发奠定了基 础。由于工业机器人的动作灵活、运动惯性小、通用性强等优点，目 前已被广泛应用到机械制造、电子、冶金、军事和危险任务等领域。

工业机器人大致有三大部分组成，包括执行机构、驱动结构和控 制系统。其执行机构部分是用来抓持工具，也就是工业机器人的末端 执行器，根据不同的抓持工具和作业对象又分为多种结构方式，在此 不再赘述；其驱动系统部分包括动力和传动装置，使得执行机构产生 相应的动作，如常用到的伺服驱动、气动装置等；其控制系统部分则 是整个系统的核心部分，控制整个机器人系统有条不紊精确执行相应 的动作，其工作原理是将信号指令发送到驱动系统，其执行机构按照 指令完成特定动作，并接收传感器反馈回来的检测信息，它通常是以 MCU、ARM或DSP等微控制器为主控制芯片，并辅助外围电路设备， 通过控制算法的研究和编程而实现对机械手的精确控制。工业机器人 通常是多关节的，多关节机器人是一个非线性和不确定性系统，同时 又是一个与运动学和动力学原理密切相关、强耦合的多变量复杂控制 系统，因此，不管是工业机器人的数学建模，还是工业机器人的控制 方法都带来巨大挑战。

为解决点焊机器人的轨迹跟踪以及内外部扰动对点焊机器人的 性能影响问题，常用的处理方法有传统的PID控制、自适应控制、神 经网络控制、模糊控制以及迭代学习控制等。通常所用的PID控制方 法是利用误差来消除误差的过程控制原理，它不依赖于系统的模型， 而且简单实用得到广泛的应用。但是该方法存在因系统自身的惯性， 导致系统输出不能及时对跳变量跟踪而产生超调的不合理情况，即快 速性和超调之间有着不可调和的矛盾，同时由于积分环节具有滞后作 用，这就使得误差积分的反馈会使闭环系统反应迟钝以及容易产生震 荡等问题。迭代学习法是通过算法本身的学习，在机器重复过程中能 够达到有效的控制。这对于像重复作业的机器人来说是非常有效的， 然而迭代学习控制大多仅适用于那些可重复作业的系统，对收敛速度 等控制品质来说做的还是不够。

发明内容

针对现有技术的不足，本发明的目的是提供一种不仅能对三关节 的点焊机器人各关节进行很好有效地控制，也能解决各关节之间的耦 合问题的轨迹跟踪控制方法。

本发明解决其技术问题所采用的技术方案如下：

一种采用带有解耦控制抗干扰技术的三关节点焊机器人的轨迹 跟踪控制方法，所述方法包括以下步骤：

步骤1)建立三关节点焊机器人系统的数学模型。

一个五自由度的点焊机器人，其结构由一系列旋转连杆和滑动关 节顺次串联而成，由于末端执行器可以由前三个关节来控制，在不影 响对系统动力学结构和控制算法研究的情况下，在此把点焊机器人系 统近似看成一个三关节刚性串联的点焊机器人。由于在实际建模过程 中要得到精确数学模型是十分困难，所以在建立点焊机器人的数学模 型时，可以考虑对点焊机器人结构做一定的近似处理，比如忽略一些 不确定性因素。建模方法包括牛顿欧拉法和拉格朗日方法，本发明采 用拉格朗日方法对水平三关节点焊机器人进行动力学建模，得到点焊 机器人系统的动力学方程为：

$M\left(q\right)\stackrel{··}{q}+V(q,\stackrel{·}{q})+G\left(q\right)=\tau ---\left(1\right)$

式中M(d)为点焊机器人的3×3质量矩阵，是3×1的离心

力和哥氏力矢量，G为3×1的重力项，τ为输入力矩。其中

$M\left(d\right)=\left(\begin{array}{ccc}H& 0& 0\\ 0& m_2+m_3& 0\\ 0& 0& m_3\end{array}\right)---\left(2\right)$

$V(q,\stackrel{·}{q})=\left(\begin{array}{c}2(m_2+m_3)d_2{\stackrel{·}{d}}_2{\stackrel{·}{\theta }}_1\\ -(m_2+m_3)d_2{\stackrel{·}{\theta }}_1^2\\ 0\end{array}\right)---\left(3\right)$

$G=\left(\begin{array}{c}0\\ 0\\ -m_{3}g\end{array}\right)---\left(4\right)$

$H=I_{x1}+I_{y2}+I_{z3}+\frac{m_1}{4}{l_2}^2+m_2{d_2}^2+m_3{d_2}^2---\left(5\right)$

其中d＝[d₁d₂d₃]^T，d₁为旋转关节变量，d₂、d₃为两个移动关 节变量，τ＝[τ₁τ₂τ₃]^T，τ₁、τ、₂τ₃分别为三个关节的输入力矩，l₁、l、₂l₃分别为三个连杆的长度，m₁、m₂、m₃分别为三连杆的质量，I_z1、I_y2、I_z3分别为各关节惯性矩转动惯量。质量矩阵是对称正定矩阵，为便于理 解后续带有解耦控制的抗扰动控制器的设计过程，将式(1)写成如下 形式：

$\stackrel{··}{q}=-M{\left(q\right)}^{-1}V(q,\stackrel{·}{q})-M{\left(q\right)}^{-1}G\left(q\right)+M{\left(q\right)}^{-1}\tau ---\left(6\right)$

步骤2)三关节点焊机器人的解耦抗干扰控制算法的设计。

带有解耦控制的抗扰动控控制算法大致有三部分组成，包括 跟踪微分器、非线性反馈控制律和扩张状态观测器。其具体设计 过程如下所示：

2.1)安排过渡过程，设计跟踪微分器。此过程是将三关节 的参考运动轨迹v，经过跟踪微分器获得参考轨迹v的近似微分 信号v₂，同时还可以获得参考轨迹v的过渡信号v₁，此过程可将 跳变的轨迹信号平滑化，防止产生超调，式(7)给出跟踪微分器的 具体形式。

$\left(\begin{array}{c}e\left(k\right)=v_1\left(k\right)-v\left(k\right)\\ \mathrm{fh}=\mathrm{fhan}(e\left(k\right),v_2\left(k\right),r,h_0)\\ v_1\left(k\right)=v_1\left(k\right)+v_2+h\left(k\right)\\ v_2\left(k\right)=v_2\left(k\right)+\mathrm{hhf}\end{array}\right)---\left(7\right)$

其中，r为快速跟踪因子，h为积分步长，h₀为滤波因子， fhan(e(k),v₂(k),r,h₀)为最速控制综合函数，fhan(x₁,x₂,r,h)具体表达式如 下：

$\left(\begin{array}{c}d=\mathrm{rh}\\ d_0=\mathrm{hd}\\ y=x_1+h{x}_2\\ a_0=\sqrt{d^2+8r\left|y\right|}\\ a=\left(\begin{array}{cc}{x}_2+\frac{(a_0+d)}{2}\mathrm{sign}\left(y\right),& \left|y\right|>d_0---\left(8\right)\\ x_2+\frac{y}{h},& \left|y\right|\le d_0\end{array}\right)\\ \mathrm{fhan}=-\left(\begin{array}{cc}\mathrm{rsign}\left(a\right),& \left|a\right|>d\\ r\frac{a}{d},& \left|a\right|\le d\end{array}\right)\end{array}\right)$

2.2)扩张状态观测器的设计。

用于估计系统每个关节中三个状态变量的扩张状态观测器具有 如下形式：

$\left(\begin{array}{c}e\left(k\right)=z_1\left(k\right)-x_1\left(k\right),\\ \mathrm{fe}=\mathrm{fal}(e\left(k\right),0.5,\delta ),\\ \mathrm{fe}1=\mathrm{fal}(e\left(k\right),0.25,\delta ),\\ z_1(k+1)=z_1\left(k\right)+h(z_2\left(k\right)-{\beta }_{01}e\left(k\right))\\ z_2(k+1)=z_2\left(k\right)+h(z_3\left(k\right)-{\beta }_{02}\mathrm{fe}+\mathrm{bu}\left(k\right))\\ z_3(k+1)=z_3\left(k\right)+h(-{\beta }_{03}\mathrm{fe}1)\end{array}\right)---\left(9\right)$

其中，e(k)为三关节点焊机器人参考轨迹值与实际运动轨迹预估计值 之差，即关节运动轨迹的误差量，其式中z₁(k)是对运动轨迹x₁(k)的估 计，z₂(k)是对运动轨迹速度量x₂(k)的估计，z₃(k)是对新扩张状态量 x₃(k)的估计，h是积分步长。fal(e(k),0.25,δ)为非线性函数，具体如式 (10)所示。δ、、β₀₁、β₀₂β₀₃为一组待整定的参数，为保证一定的估计精 度，根据高增益状态观测器设计原则，β₀₁、、β₀₂β₀₃可取得大一些，一 般要大于噪声或扰动的上界。

$\mathrm{fal}(e\left(k\right),a,\delta )=\left(\begin{array}{cc}\frac{e\left(k\right)}{{\delta }^{1-a}}& \left|e\left(k\right)\right|\le \delta \\ {\left|e\left(k\right)\right|}^a\mathrm{sign}\left(e\left(k\right)\right)& \left|e\left(k\right)\right|>\delta \end{array}\right)---\left(10\right)$

其中，a为幂指数，δ为线性段的区间长度，sign()为符号函数，具体 表达式如式(11)所示。

$\mathrm{sign}\left(x\right)=\left(\begin{array}{cc}1& x>0\\ 0& x=0\\ -1& x<0\end{array}\right)---\left(11\right)$

2.3)非线性反馈控制律设计。

此过程得到两个误差量，即e₁(k)＝v₁(k)-z₁(k)和e₂(k)＝v₂(k)-z₂(k)。 经过非线性组合可计算出对象控制量u₀(k)，计算过程如式(12)所示。

$\left(\begin{array}{c}{e}_1\left(k\right)=v_1\left(k\right)-z_1\left(k\right),e_2\left(k\right)=v_2\left(k\right)-z_2\left(k\right)\\ u_0\left(k\right)=-\mathrm{fhan}(e_1\left(k\right),e_2\left(k\right),r,h)\\ u\left(k\right)=(u_0\left(k\right)-z_3\left(k\right))/b\end{array}\right)---\left(12\right)$

为了补偿系统中加速度项和内外扰动，在得到的控制量u₀(k)中减 去新扩张的量z₃(k)得到新的控制量，即补偿过程可 抵消系统中所有总和扰动项，从而使系统转化成了纯积分的线性系统， 同时也消除了内外扰动对系统性能的影响。

步骤3)三关节点焊机器人系统的解耦过程。

式(6)计算得出的点焊机器人模型是经过近似处理得到的没有关 节间耦合的数学模型，实际上多关节机器人是一个强耦合的多变量复 杂控制系统，所以在实际应用中必须考虑关节间的耦合问题。对于多 输入多输出系统的解耦控制方法无论是控制理论界还是控制工程界 都是追求解决的重要问题，现有的方法是采用系统模型，但是计算量 很大，本发明采用带有解耦抗扰动控制技术来解决关节之间的耦合问 题，具有很好的鲁棒性。

为了分析在有耦合情况下的解耦控制，同时为了验证设计控制器 的抗干扰能力，人为地加入扰动项w(t)，现将式(6)写成如下形式：

$\left(\begin{array}{c}{\stackrel{··}{q}}_1=f_1({\stackrel{·}{q}}_1,{\stackrel{·}{d}}_2,{\stackrel{·}{d}}_3,w\left(t\right))+b_{11}{\tau }_1\text{+}{b}_{12}{\tau }_2+b_{13}{\tau }_3\\ {\stackrel{··}{d}}_2=f_2({\stackrel{·}{q}}_1,{\stackrel{·}{q}}_2,{\stackrel{·}{d}}_3,w\left(t\right))+b_{21}{\tau }_1+b_{22}{\tau }_2+b_{23}{\tau }_3\\ {\stackrel{··}{d}}_3=f_3({\stackrel{·}{q}}_1,{\stackrel{·}{q}}_2,{\stackrel{·}{d}}_3,w\left(t\right))+b_{31}{\tau }_1+b_{32}{\tau }_2+b_{33}{\tau }_3\end{array}\right)---\left(13\right)$

其中，为含有 内外总和扰动加速度项，w(t)为噪声信号。控制量的放大系数b_ij是状 态变量的函数，即b_ij(d₁,d₂,d₃)。在这里，将系统控制量之外的模型部分 称为“动态耦合”部分，将b_ij(d₁,d₂,d₃)部分称为“静态 耦合部分”。三关节点焊机器人系统具有耦合性，在解耦控制过程中， 由于扩张的状态观测器对内外扰实时估计，并对含有总和扰动的加速 度项进行动态线性化补偿，这就使得解耦过程不必考虑“动态耦合” 部分，只需考虑“静态耦合”部分即可。

在式(13)中，取

$B=\left(\begin{array}{ccc}{b}_{11}& b_{12}& b_{13}\\ b_{21}& b_{22}& b_{23}\\ b_{31}& b_{21}& b_{33}\end{array}\right)---\left(14\right)$

并引入“虚拟控制量”U＝[U_d1U_d2U_d3]^T，则有

$\left(\begin{array}{c}{\tau }_1\\ {\tau }_2\\ {\tau }_3\end{array}\right)={\left(\begin{array}{ccc}{b}_{11}& b_{12}& b_{13}\\ b_{21}& b_{22}& b_{23}\\ b_{31}& b_{32}& b_{33}\end{array}\right)}^{-1}\left(\begin{array}{c}{U}_{d1}\\ U_{d2}\\ U_{d3}\end{array}\right)---\left(15\right)$

简写成如下形式：

τ＝B^-1U(16) 与现有技术相比，本发明的优点在于：

1、多关节机器人各关节之间解耦过程相对简单。

本发明采用的解耦方法是利用带解耦控制抗扰动控制技术，相较 于其他方法具有计算量小，有很好的鲁棒性。

2、对参考轨迹的跟踪比较平缓，不会出现很大抖动。

给定参考运动轨迹后，经过事先安排的过渡过程，可将跳变的信 号平滑化，一方面解决快速性和超调之间的矛盾，另一方面使得点焊 机器人的对参考轨迹缓慢跟踪，解决跟踪一开始出现的抖动问题。

3、具有很强的抗干扰能力

z₃(k)作为新扩张出的系统状态量，既含有不确定动态又含有内外 扰动，扩张状态观测器一并将其作为总和扰动予以估计，从而在补偿 加速度项的同时也补偿了内外扰动，消除了内外扰动对系统性能的影 响。

附图说明

图1是本发明一实施例的三关节点焊机器人的抗干扰控制器结构图。

图2是本发明一实施例的点焊机器人的三关节轨迹跟踪曲线图。

图3是本发明一实施例的点焊机器人的三关节轨迹跟踪误差曲线图。

具体实施方式

为了使本发明的技术方案、设计思路能更加清晰，下面结合附图 和实施例进行更详尽的描述。

本发明针对三关节的点焊机器人，为了克服上述提到的现有控制 方法的无法解决精确轨迹跟踪、抗干扰能力较差等问题，本发明采用 带解耦的抗干扰控制技术设计点焊机器人的抗干扰能力和精确跟踪 的控制策略。对点焊机器人的每个关节分别设计独立的带有解耦特性 的抗干扰控制器，同时对于各关节间的耦合采用算法中自带的解耦控 制，不仅能对各关节进行很好有效地控制，也解决了各关节之间的耦 合问题，同时此算法技术的动态线性化补偿，也解决了内外扰动对点 焊机器人性能的影响。

参照图1～图3，一种采用带有解耦抗干扰控制技术的点焊机器人 轨迹跟踪控制，所述方法包括以下步骤：

步骤1)建立三关节点焊机器人系统的数学模型。

一个五自由度的点焊机器人，其结构由一系列旋转连杆和滑动关 节顺次串联而成，由于末端执行器可以由前三个关节来控制，在不影 响对系统动力学结构和控制算法研究的情况下，可以把点焊机器人系 统近似看成一个三关节刚性串联的点焊机器人。由于在实际建模过程 中要得到精确数学模型是十分困难，所以在建立点焊机器人的数学模 型时，可以考虑对点焊机器人结构做一定的近似处理，比如忽略一些 不确定性因素。建模方法包括牛顿欧拉法和拉格朗日方法，本发明采 用拉格朗日方法对水平三关节点焊机器人进行动力学建模，得到点焊 机器人系统的动力学方程为：

$M\left(q\right)\stackrel{··}{q}+V(q,\stackrel{·}{q})+G\left(q\right)=\tau ---\left(17\right)$

式中M(d)为点焊机器人的3×3质量矩阵，是3×1的离心 力和哥氏力矢量，G为3×1的重力项，τ为输入力矩，具体参照实 施方案中步骤1)。

质量矩阵是对称正定矩阵，为便于说明后续带有解耦控制抗扰动 控制器的设计过程，将式(1)写成如下形式：

$\stackrel{··}{q}=-M{\left(q\right)}^{-1}V(q,\stackrel{·}{q})-M{\left(q\right)}^{-1}G\left(q\right)+M{\left(q\right)}^{-1}\tau ---\left(18\right)$

步骤2)三关节点焊机器人的抗干扰控制算法的设计。

带有解耦控制的抗扰动控控制算法大致有三部分组成，包括跟踪 微分器、非线性反馈控制律和扩张状态观测器。其具体设计过程如下 所示：

2.1)安排过渡过程。此过程是将三关节的参考运动轨迹v，经过 跟踪微分器获得参考轨迹v的近似微分信号v₂，同时还可以获得参考 轨迹v的过渡信号v₁，可将跳变的轨迹信号平滑化，防止产生超调。

2.2)扩张状态观测器的设计。

用于估计系统(18)中三个状态变量的扩张状态观测器具有如下形 式：

$\left(\begin{array}{c}e\left(k\right)=z_1\left(k\right)-x_1\left(k\right),\\ \mathrm{fe}=\mathrm{fal}(e\left(k\right),0.5,\delta ),\\ \mathrm{fe}1=\mathrm{fal}(e\left(k\right),0.25,\delta ),\\ z_1(k+1)=z_1\left(k\right)+h(z_2\left(k\right)-{\beta }_{01}e\left(k\right))\\ z_2(k+1)=z_2\left(k\right)+h(z_3\left(k\right)-{\beta }_{02}\mathrm{fe}+\mathrm{bu}\left(k\right))\\ z_3(k+1)=z_3\left(k\right)+h(-{\beta }_{03}\mathrm{fe}1)\end{array}\right)---\left(19\right)$

其中，e(k)为三关节点焊机器人参考轨迹值与实际运动轨迹估计值之 差，即关节运动轨迹的误差量，z₁(k)是对运动轨迹x₁(k)的估计，z₂(k) 是对运动轨迹速度x₂(k)的估计，z₃(k)是对新扩张状态量x₃(k)的估计， h是积分步长。fal(e(k),0.25,δ)为非线性函数，具体如式(20)所示。 δ、、β₀₁、β₀₂β₀₃为一组待整定的参数，为保证一定的估计精度，根据高 增益状态观测器设计原则，β₀₁、、β₀₂β₀₃可取得大一些，一般要大于噪 声或扰动的上界。

$\mathrm{fal}(e\left(k\right),a,\delta )=\left(\begin{array}{cc}\frac{e\left(k\right)}{{\delta }^{1-a}}& \left|e\left(k\right)\right|\le \delta \\ {\left|e\left(k\right)\right|}^a\mathrm{sign}\left(e\left(k\right)\right)& \left|e\left(k\right)\right|>\delta \end{array}\right)---\left(20\right)$

其中，a为幂指数，δ为线性段的区间长度，sign()为符号函数，具体 表达式如式(21)所示。

$\mathrm{sign}\left(x\right)=\left(\begin{array}{cc}1& x>0\\ 0& x=0\\ -1& x<0\end{array}\right)---\left(21\right)$

2.3)非线性反馈控制律设计。

此过程得到两个误差量，即e₁(k)＝v₁(k)-z₁(k)和e₂(k)＝v₂(k)-z₂(k)。 经过非线性组合模块可计算出控制量u₀(k)，计算过程如式(22)所示。

$\left(\begin{array}{c}{e}_1\left(k\right)=v_1\left(k\right)-z_1\left(k\right),e_2\left(k\right)=v_2\left(k\right)-z_2\left(k\right)\\ u_0\left(k\right)=-\mathrm{fhan}(e_1\left(k\right),e_2\left(k\right),r,h)\\ u\left(k\right)=(u_0\left(k\right)-z_3\left(k\right))/b\end{array}\right)---\left(22\right)$

为了补偿系统中加速度项和内外扰动，在得到的控制量u₀(k)中减 去z₃(k)得到新的控制量，即补偿过程可抵消系统中 所有总和扰动项，从而使系统转化成了纯积分的线性系统，同时也消 除了内外扰动对系统性能的影响。

步骤3)三关节点焊机器人系统的解耦过程。

式(18)计算得出的点焊机器人模型是经过近似处理得到的没有 关节间耦合的数学模型，实际上多关节机器人是一个强耦合的多变量 复杂控制系统，所以在实际应用中必须考虑关节间的耦合问题。对于 多输入多输出系统的解耦控制方法无论是控制理论界还是控制工程 界都是追求解决的重要问题，现有的方法是采用系统模型，但是计算 量很大，本实施例采用带解耦抗干扰控制技术来解决关节之间的耦合 问题，具有很好的鲁棒性。

为了分析在有耦合情况下的解耦控制，同时为了验证设计控制器 的抗干扰能力，人为地加入扰动项w(t)，现将式(18)写成如下形式：

$\left(\begin{array}{c}{\stackrel{··}{q}}_1=f_1({\stackrel{·}{q}}_1,{\stackrel{·}{d}}_2,{\stackrel{·}{d}}_3,w\left(t\right))+b_{11}{\tau }_1\text{+}{b}_{12}{\tau }_2+b_{13}{\tau }_3\\ {\stackrel{··}{d}}_2=f_2({\stackrel{·}{q}}_1,{\stackrel{·}{q}}_2,{\stackrel{·}{d}}_3,w\left(t\right))+b_{21}{\tau }_1+b_{22}{\tau }_2+b_{23}{\tau }_3\\ {\stackrel{··}{d}}_3=f_3({\stackrel{·}{q}}_1,{\stackrel{·}{q}}_2,{\stackrel{·}{d}}_3,w\left(t\right))+b_{31}{\tau }_1+b_{32}{\tau }_2+b_{33}{\tau }_3\end{array}\right)---\left(23\right)$

其中，为含有 内外总和扰动加速度项，w(t)为未知扰动，仿真时给定w(t)为随机噪 声信号。控制量的放大系数b_ij是状态变量的函数，即b_ij(d₁,d₂,d₃)。在这 里，将系统控制量之外的模型部分称为“动态耦合” 部分，将b_ij(d₁,d₂,d₃)部分称为“静态耦合部分”。三关节点焊机器人 系统具有耦合性，在解耦控制过程中，由于扩张的状态观测器对内外 扰实时估计，并对含有总和扰动的加速度项动态线性化补偿，使得解 耦过程不必考虑“动态耦合”部分，只需考虑“静态耦合”部分即可。

在式(23)中，取

$B=\left(\begin{array}{ccc}{b}_{11}& b_{12}& b_{13}\\ b_{21}& b_{22}& b_{23}\\ b_{31}& b_{21}& b_{33}\end{array}\right)---\left(24\right)$

并引入“虚拟控制量”U＝[U_d1U_d2U_d3]^T，则有

$\left(\begin{array}{c}{\tau }_1\\ {\tau }_2\\ {\tau }_3\end{array}\right)={\left(\begin{array}{ccc}{b}_{11}& b_{12}& b_{13}\\ b_{21}& b_{22}& b_{23}\\ b_{31}& b_{32}& b_{33}\end{array}\right)}^{-1}\left(\begin{array}{c}{U}_{d1}\\ U_{d2}\\ U_{d3}\end{array}\right)---\left(25\right)$

简写成如下形式：

τ＝B^-1U(26)

如图1所示，分别对每个关节设计独立的抗干扰控制器，每个 抗干扰控制器的参数调节根据每个参考轨迹的特性来选择合适的参 数，而且每个控制器的互不影响相互独立。

参考输入轨迹d01，经过安排过度过程得到两个信号，一个是过 渡信号d11，一个是近似微分信号d12。由扩张状态观测器得到三个状 态量估计，分别是过渡信号d11的估计z11，近似微分信号d12的估计 z12，新扩张的量z31，即含有内外扰动的总和扰动估计z13，进一步 分别求得误差后经过非线性反馈控制律得到控制量U_0d1，再由动态线 性化补偿过程得到新控制量U_d1，经过带解耦抗干扰的解耦控制得到 关节的输入力矩。另外两个关节的控制方法和原理近似，在此不作详 述。

如图2所示，从图中可以看出，应用抗干扰控制方法，点焊机器 人的三个关节分别能精确地跟踪，且跟踪误差基本为零，可以看出抗 干扰算法具有很好的控制效果和较强的鲁棒性。

如图3所示，从图中可以看出，三条误差曲线基本上为零，三关 节点焊机器人具有很好的轨迹跟踪效果，而且仿真过程中人为的加入 扰动，跟踪曲线依然能迅速跟踪，说明抗干扰控制算法具有很好的控 制效果和抗干扰能力。

上面结合优选实施方式对本发明作了详细说明，但是本发明不 限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还 可以在不脱离本发明宗旨的前提下做出各种变化。

不脱离本发明的构思和范围可以做出许多其他改变和改型。应 当理解，本发明不限于特定的实施方式，本发明的范围由所附权利要 求限定。