CN103728988B - 基于内模的scara机器人轨迹跟踪控制方法 - Google Patents

Abstract

提供了一种基于内模的SCARA机器人轨迹跟踪控制方法。不需要知道被控对象精确的数学模型，具有强鲁棒性、高跟踪精度、快速跟踪速度，并且控制结构简单，参数调节单一。在输入信号为时变且存在外界干扰的情况下，为使系统的稳态误差为零，需要在控制回路中添加二阶滤波器f(s)，滤波器中的参数λ是整个系统的调节参数，可以调节系统的响应时间和控制精度，参数调整简单明了，系统性能优越。

Description

基于内模的SCARA机器人轨迹跟踪控制方法

技术领域

本发明涉及SCARA机器人轨迹跟踪控制领域，具体是指为了使SCARA机器人在外界干扰大的情况下具有优良的轨迹跟踪性能，提出一种用内模控制原理设计机器人控制器的方法。此方法结构简单，控制性能优越，可以在时变信号和外界干扰的情况下保持稳态误差为零。

背景技术

机器人技术集机械、电子、计算机、自动控制、人工智能等基础和高新学科领域的理论和技术于一体，机器人本体的设计需要考虑材料选择、质量分配、尺寸优化等问题，需要应用三维造型、有限元分析、运动学和动力学分析、最优化理论等手段。机器人的控制系统涉及到伺服驱动、运动控制、计算机软件等。机器人的人机交互系统需要采用高性能嵌入式系统，需要考虑安全性和易操作性。

SCARA机器人系统是一个复杂的多输入多输出的非线性系统，具有时变、强耦合和非线性的动力学特性。轨迹跟踪控制是工业机器人控制中的一个重要内容。机器人轨迹跟踪控制是指通过给定各关节的驱动力矩，使机器人的位置、速度等状态变量跟踪给定的理想轨迹，对于整个轨迹来说，都需要严格控制。因此，轨迹跟踪控制是十分复杂与困难，但也是工业生产中应用最为广泛的控制方式。研究机器人轨迹跟踪控制以及提高轨迹跟踪控制的精度对机器人技术有着重要的意义。

对于自由运动的SCARA机器人来说，其控制器设计可以按是否考虑机器人的动力学特性而分为两类：(1)完全不考虑机器人的动力学特性，只是按照机器人实际轨迹与期望轨迹间的偏差进行负反馈控制。这类方法通常被称为运动控制。主要优点是控制律简单，易于实现。但对于控制高速高精度机器人来说，这类方法有两个明显的缺点：一是难于保证受控机器人具有良好的动态和静态品质，二是需要较大的控制能量。(2)考虑动力学的控制器设计方法，根据机器人动力学模型的性质设计出精细的非线性控制律。这类控制通常称为动态控制。用这种方法设计的控制器可使被控机器人具有良好的动态和静态品质，克服了运动控制方法的缺点。

虽然一些智能控制算法能达到高精度控制要求，但控制结构复杂，算法计算时间长，有时需要调节多个参数才能到达控制的要求。因此希望能设计一个控制结构简单，调节参数单一，同时满足控制精度的控制器。

发明内容

本发明的目的是针对SCARA机器人在输入信号时变且具有外界干扰的情况下能否满足轨迹跟踪精度且控制器结构设计简单，调节参数单一的问题，设计一种基于内模原理的SCARA机器人控制器设计策略。

为达此目的，本发明技术方案如下：建立SCARA机器人的动力学模型，根据动力学方程估算各关节的惯性力矩、向心力和哥氏力矩、重力矩，最后得出各关节的力矩估算公式，将其作为内模对对象的估计模型。然后，为确保系统的稳定性和鲁棒性使系统的稳态误差为零，需要设计内模滤波器f(s)，最后通过调整参数λ，使系统达到机器人精度要求。整个流程包括：动力学估算模块、建立内模模型模块、设计内模控制器模块、控制律计算模块。

第一步，建立SCARA机器人各连杆坐标系，确定各连杆的D-H参数(α_i，α_i，d_i，θ_i)。由拉格朗日函数方程：推导出动力学方程：其中τ_i为关节i处的广义力矩，q_i为关节i处的广义位移，n为机器人的连杆数，D_ij表示关节i，j之间的惯量，D_ijk表示关节i处的向心力和哥氏力，D_i表示关节i处的重力载荷。

根据动力学方程计算出惯性力项、向心力和哥氏力项以及重力项最后得出力矩估算公式：

第二步，根据上一步估计的动力学各项参数建立内模模型M(S)。M(S)可分为两项：M₊(S)和M_-(S)，即M(S)＝M₊(S)M_-(S)。其中，M₊(S)为模型中包含纯滞后和不稳定零点的部分，M_-(S)为模型中的最小相位部分。

第三步，内模控制器的设计。为确保系统的稳定性和鲁棒性使系统的稳态误差为零，需在最小相位M_-(S)的逆上添加滤波器f(s)，则此时的内模控制器为：

当系统输入为时变信号时，通常选用形式为式中，λ为滤波器时间常数，是内模控制器仅有的调节参数，同时也是整个系统仅有的调节参数。

第四步，将以上几部分组合，关节的力矩控制输入为：

$\mathrm{\τ}=\frac{2\hat{H}\left(q\right)}{\mathrm{\λ}}\stackrel{\·}{e}+(\frac{\hat{H}\left(q\right)}{{\mathrm{\λ}}^2}+\frac{2\hat{C}(q,\stackrel{\·}{q})}{\mathrm{\λ}})e+\frac{\hat{C}(q,\stackrel{\·}{q})}{{\mathrm{\λ}}^2}\∫edt+G\left(q\right)$

其中，分别为H(q)，的估计值，G(q)为重力项，e分别为速度跟踪误差和位置跟踪误差，λ为内模控制器的调节参数，由此可以看出设计出来的控制器参数调节简单。

本发明的有益效果：提供了一种基于内模原理的SCARA机器人轨迹跟踪控制方法，用于提高SCARA机器人的轨迹跟踪精度和抗干扰能力。内模控制具有结构简单、参数整定直观明了和在线调整容易等优点，对于鲁棒及抗扰性的改善效果尤为显著。只要对单一参数λ进行调节就可以对整个系统的稳定性和鲁棒性进行调整，系统参数调整简单，性能优越。

附图说明

图1 SCARA机器人模型示意图；

图2本发明内模控制框图；

图3本发明将内模控制框图等效成常规控制机器人框图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，下面结合具体实施例，并参照附图，对本发明作进一步详细说明。

本发明的基本思路是：提供了一种基于内模的SCARA机器人的轨迹跟踪控制方法：它不需要知道被控对象的精确的数学模型；而且具有强鲁棒性、高跟踪精度、快速跟踪速度；并且控制结构简单，参数调节单一。本发明首先对SCARA机器人进行动力学模型建模，获得SCARA机器人惯性力矩、向心力和哥氏力矩、重力矩的估计值；将其作为内模对对象的估计模型。然后，为确保系统的稳定性和鲁棒性使系统的稳态误差为零，需要设计内模滤波器f(s)，最后通过调整参数λ，使系统达到机器人精度要求。整个流程包括：动力学估算模块、建立内模模型模块、设计内模控制器、控制律计算模块。

附图2为本发明的内模控制框图。其中P(S)为被控对象，M(S)为被控对象的数学模型即为P(S)的估计模型，Q(S)为内模控制器，q_d(S)、q(S)、D(S)、Z(S)分别为控制系统的输入信号、输出信号、干扰信号和反馈信号。控制目标是保持输出q(t)逼近参考值(设定值)q_d(t)。

图3为本发明将内模控制框图等效成常规控制框图。滤波器设计为内模控制器将其等效成常规控制器 参数λ为整个控制器的调节参数，同时也是整个系统仅有的调节参数。

进一步，具体实现步骤为：

步骤一，设计动力学估算模块。

SCARA机器人系统的结构及坐标系如图1所示，系统动力学方程式，即拉格朗日函数方程如下：机器人的动力学模型为：

${\mathrm{\τ}}_i=\underset{j=1}{\overset{n}{\Σ}}{D}_{ij}{\stackrel{\·\·}{q}}_j+I_{ai}{\stackrel{\·\·}{q}}_i+\underset{j=1}{\overset{n}{\Σ}}\underset{k=1}{\overset{n}{\Σ}}{D}_{ijk}{\stackrel{\·}{q}}_j{\stackrel{\·}{q}}_k+D_i$

对于SCARA机器人：

$D_{ij}=\underset{p=\max i,j}{\overset{4}{\Σ}}Trace\left(\frac{\∂T_p}{\∂q_j}{J}_p\frac{\∂T_p^T}{\∂q_i}\right)$

$D_{ijk}=\underset{p=\max i,j,k}{\overset{4}{\Σ}}Trace\left(\frac{{\∂}^2{T}_p}{\∂q_j\∂q_k}{J}_p\frac{\∂T_p^T}{\∂q_i}\right)$

$D_i=\underset{p=i}{\overset{4}{\Σ}}-m_p{g}^T\frac{\∂T_p}{\∂q_i}{r_p}_p$

$J_i=\left[\begin{array}{cccc}\frac{-I_{ixx}+I_{iyy}+I_{izz}}{2}& I_{ixy}& I_{ixz}& m_i{\stackrel{\‾}{z}}_i\\ I_{ixy}& \frac{I_{ixx}-I_{iyy}+I_{izz}}{2}& I_{iyz}& m_i{\stackrel{\‾}{z}}_i\\ I_{ixz}& I_{iyz}& \frac{I_{ixx}+I_{iyy}-I_{izz}}{2}& m_i{\stackrel{\‾}{z}}_i\\ m_i{\stackrel{\‾}{x}}_i& m_i{\stackrel{\‾}{y}}_i& m_i{\stackrel{\‾}{z}}_i& m_i\end{array}\right]$

根据SCARA机器人的结构及坐标关系变换，结合以上公式可以估算出各关节惯性力项、哥氏力项和重力项H(q)，G(q)。

$H\left(q\right)=\left[\begin{array}{cccc}{D}_{11}& D_{12}& 0& D_{14}\\ D_{21}& D_{22}& 0& D_{24}\\ 0& 0& D_{33}& 0\\ D_{41}& D_{42}& 0& D_{44}\end{array}\right]$

$D_{11}=\frac{1}{3}{m}_1{l}_1^2+m_2(\frac{1}{3}{l}_2^2+l_1^2+l_1{l}_2{C}_2)+\frac{1}{2}{m}_4{r}^2+(m_3+m_4)(l_1^2+l_2^2+2l_1{l}_2{C}_2)$

$D_{22}=\frac{1}{3}{m}_2{l}_2+m_3{l}_2^2+m_4{l}_2^2+\frac{1}{3}{m}_4{r}^2$

$D_{33}=m_3+m_4,D_{44}=\frac{1}{2}{m}_4{r}^2,D_{14}=-\frac{1}{2}{m}_4{r}^2$

$D_{12}=\frac{1}{3}{m}_2{l}_2^2+\frac{1}{2}{m}_2{l}_1{l}_2{C}_2+m_3{l}_2^2+m_3{l}_1{l}_2{C}_2+m_4{l}_2^2+m_4{l}_1{l}_2{C}_2+\frac{1}{2}{m}_4{r}^2$

$D_{21}=\frac{1}{3}{m}_2{l}_2^2+\frac{1}{2}{m}_2{l}_1{l}_2{C}_2+m_3{l}_2+m_3{l}_1{l}_2{C}_2+m_4{l}_2+m_4{l}_1{l}_2{C}_2+\frac{1}{2}{m}_{4}r$

$D_{24}=-\frac{1}{2}{m}_4{r}^2,D_{41}=-\frac{1}{2}{m}_4{r}^2,D_{42}=-\frac{1}{2}{m}_4{r}^2$

$C(q,\stackrel{\·}{q})=\left[\begin{array}{cccc}2D_{112}{\stackrel{\·}{q}}_2& D_{122}{\stackrel{\·}{q}}_2& 0& 0\\ D_{211}{\stackrel{\·}{q}}_1& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]$

$D_{112}=-(\frac{1}{2}{m}_2+m_3+m_4)l_1{l}_2{s}_2$

$D_{122}=-(\frac{1}{2}{m}_2+m_3+m_4)l_1{l}_2{s}_2$

$D_{211}=(\frac{1}{2}{m}_2+m_3+m_4)l_1{l}_2{s}_2$

$G\left(q\right)=\left[\begin{array}{c}0\\ 0\\ -(m_3+m_4)g\\ 0\end{array}\right]$

其中，m_i代表各连杆的质量，l_i代表各连杆长度，r为旋转导轨的半径，c_i，s_i是cos(q_i)，sin(q_i)的简写。

SCARA机器人的估算动力学模型为：

$\left[\begin{array}{c}{\mathrm{\τ}}_1\\ {\mathrm{\τ}}_2\\ {\mathrm{\τ}}_3\\ {\mathrm{\τ}}_4\end{array}\right]=\left[\begin{array}{cccc}{D}_{11}& D_{12}& 0& D_{14}\\ D_{21}& D_{22}& 0& D_{24}\\ 0& 0& D_{33}& 0\\ D_{41}& D_{42}& 0& D_{44}\end{array}\right]\left[\begin{array}{c}{\stackrel{\·\·}{q}}_1\\ {\stackrel{\·\·}{q}}_2\\ {\stackrel{\·\·}{q}}_3\\ {\stackrel{\·\·}{q}}_4\end{array}\right]+\left[\begin{array}{c}2D_{112}{\stackrel{\·}{q}}_1{\stackrel{\·}{q}}_2+D_{122}{\stackrel{\·}{q}}_2^2\\ D_{211}{\stackrel{\·}{q}}_1^2\\ -(m_3+m_4)g\\ 0\end{array}\right]$

步骤二，建立内模模型模块。

SCARA机器人的动力学模型为：

$H\left(q\right)\stackrel{\·\·}{q}+C(q,\stackrel{\·}{q})\stackrel{\·}{q}+G\left(q\right)=\mathrm{\τ}$

H(q)为惯性力项，为哥氏力和向心力项，G(q)为重力项。

对上式式变换：

$H\left(q\right)\stackrel{\·\·}{q}+C(q,\stackrel{\·}{q})\stackrel{\·}{q}=\mathrm{\τ}-G\left(q\right)$

令u＝τ-G(q)得

$H\left(q\right)\stackrel{\·\·}{q}+C(q,\stackrel{\·}{q})\stackrel{\·}{q}=u$

将其写成传递函数的形式：

$P\left(S\right)=\frac{q\left(S\right)}{U\left(S\right)}=\frac{1}{H\left(q\right)S^2+C(q,\stackrel{\·}{q})S}$

内模模型为

$M\left(S\right)=\frac{1}{\hat{H}\left(q\right)S^2+\hat{C}(q,\stackrel{\·}{q})S}$

其中和为被控对象的估计值。

由于M(S)中不包含纯滞后和不稳定零点的部分，所以M(S)＝M_-(S)。

步骤三，设计内模控制器模块。

当输入为时变信号时，取滤波器为

$f\left(s\right)=\frac{2\mathrm{\λ}s+1}{{(\mathrm{\λ}s+1)}^2}$

则内模控制器为

$Q\left(S\right)=\frac{f\left(S\right)}{M\left(S\right)}=\frac{\left(\hat{H}\right(q)s^2+\hat{C}(q,\stackrel{\·}{q}\left)s\right)(2\mathrm{\λ}s+1)}{{(1+\mathrm{\λ}s)}^2}$

满足

$\frac{d}{dS}\left(M\right(S\left)Q\right(S\left)\right){|}_{S=0}=\frac{d}{dS}\left(f\right(S\left)\right){|}_{S=0}=\frac{2\mathrm{\λ}{(\mathrm{\λ}S+1)}^2-2\mathrm{\λ}(\mathrm{\λ}S+1)(2\mathrm{\λ}S+1)}{{(\mathrm{\λ}S+1)}^2}{|}_{S=0}=0$

和Q(0)M(0)＝1

由终值定理得

$e\left(\mathrm{\∞}\right)=\underset{s\→0}{\lim }SE\left(S\right)=0$

将内模控制器等效成常规控制器得

$C\left(S\right)=\frac{Q\left(S\right)}{1-Q\left(S\right)M\left(S\right)}=\frac{2\hat{H}\left(q\right)S}{\mathrm{\λ}}+\frac{\hat{C}(q,\stackrel{\·}{q})}{{\mathrm{\λ}}^{2}S}+(\frac{\hat{H}\left(q\right)}{{\mathrm{\λ}}^2}+\frac{2\hat{C}(q,\stackrel{\·}{q})}{\mathrm{\λ}})$

步骤四，设计控制律计算模块

由常规控制器C(S)和式u＝τ-G可推导控制率的传递函数为：

$\mathrm{\τ}\left(S\right)=C\left(S\right)E\left(S\right)+G\left(q\right)=\[\frac{2\hat{H}\left(q\right)S}{\mathrm{\λ}}+\frac{\hat{C}(q,\stackrel{\·}{q})}{{\mathrm{\λ}}^{2}S}+(\frac{\hat{H}\left(q\right)}{{\mathrm{\λ}}^2}+\frac{2\hat{C}(q,\stackrel{\·}{q})}{\mathrm{\λ}})\]E\left(S\right)+G\left(q\right)$

所以SCARA机器人的控制率为：

$\mathrm{\τ}=\frac{2\hat{H}\left(q\right)}{\mathrm{\λ}}\stackrel{\·}{e}+(\frac{\hat{H}\left(q\right)}{{\mathrm{\λ}}^2}+\frac{2\hat{C}(q,\stackrel{\·}{q})}{\mathrm{\λ}})e+\frac{\hat{C}(q,\stackrel{\·}{q})}{{\mathrm{\λ}}^2}\∫edt+G\left(q\right)$

其中，τ关节的力矩，分别为H(q)，的估计值，G(q)为重力项，e分别为位置跟踪误差速度和位置跟踪误差，λ为内模控制器的调节参数，同时也是整个系统仅有的调节参数，由此可以看出设计出来的控制器参数调节简单。从运算时间上来看，由于没有引入智能算法，所以计算时间要比引入智能算法的控制器短很多。

Claims (3)

1.一种基于内模的SCARA机器人轨迹跟踪控制方法：它不需要知道被控对象的具体数学模型，在输入信号为时变且具有外界干扰的情况下，具有强鲁棒性，并且控制结构简单，参数调节单一；首先对SCARA机器人进行动力学模型建模，获得SCARA机器人惯性力矩、向心力和哥氏力矩、重力矩的估计值；将其作为内模对对象的估计模型，然后在输入信号时变且具有外界干扰的情况下，为确保系统的稳定性和鲁棒性使系统的稳态误差为零，需要设计内模滤波器f(s)，最后通过调整参数λ，使系统达到机器人精度要求，系统主要包括以下几个模块：

(1)、动力学估算模块：通过建立SCARA机器人动力学方程，根据机器人坐标变换估算各关节的惯性力矩、向心力和哥氏力矩、重力矩的估算值；

(2)、建立内模模型模块：根据得到的SCARA机器人动力学数学模型建立内模的估计模型M(S)，M(S)可分为两项：M₊(S)和M_-(S)，即M(S)＝M₊(S)M_-(S)，其中，M₊(S)为模型中包含纯滞后和不稳定零点的部分，M_-(S)为模型中的最小相位部分；

(3)、设计内模控制器模块：为确保系统的稳定性和鲁棒性使系统的稳态误差为零，需在最小相位M_-(S)的逆上添加滤波器则此时的内模控制器为λ为内模控制器的调节参数，同时也是整个系统仅有的调节参数；

(4)、控制律计算模块：根据设计出的内模控制器，计算出各关节的输入力矩τ_i，将其作为伺服控制器的输入来实现SCARA机器人快速，高精度的轨迹跟踪。

2.根据权利要求1所述基于内模的SCARA机器人轨迹跟踪控制方法，其特征在于，所述模块(2)中建立的内模模型模块为：

$M\left(S\right)=\frac{1}{\hat{H}\left(q\right)S^2+\hat{C}(q,\stackrel{\·}{q})S}$

其中，和分别为被控对象惯性力项H(q)的估计值与向心力和哥氏力项的估计值；

由于M(S)中不包含纯滞后和不稳定零点的部分，所以M(S)＝M_-(S)。

3.根据权利要求1所述基于内模的SCARA机器人轨迹跟踪控制方法，其特征在于，所述模块(4)中控制律计算模块，根据计算可以解得SCARA机器人控制率为：

$\mathrm{\τ}=\frac{2\hat{H}\left(q\right)}{\mathrm{\λ}}\stackrel{\·}{e}+(\frac{\hat{H}\left(q\right)}{{\mathrm{\λ}}^2}+\frac{2\hat{C}(q,\stackrel{\·}{q})}{\mathrm{\λ}})e+\frac{\hat{C}(q,\stackrel{\·}{q})}{{\mathrm{\λ}}^2}\∫edt+G\left(q\right)$

其中，τ为机器人关节的输入力矩，和分别为被控对象惯性力项H(q)的估计值与向心力和哥氏力项的估计值，G(q)为重力项补偿，分别为速度跟踪误差和位置跟踪误差。