机器人动力学与控制学习笔记(六)————机器人PID控制

六、机器人鲁棒自适应PD控制

6.1  问题的提出  

        对于具有强耦合性和非线性的机器人系统而言,线性PD控制是最为简单且行之有效的控制方法,在工业机器人中得到了广泛应用。但实践表明,线性PD控制往往要求驱动机构有很大的初始输出,而实际驱动机构(通常是电机)往往不可能提供过大的初始力矩,且机械臂本身所承受的最大力矩也是有限的,这将使通过增大PD控制系数来进一步提高系数的性能受到限制。鉴于此,很多非线性PD控制方法被提出了出来,但常规的非线性PD控制器只有单纯的PD项,要求比例和微分项的系数仍较大,存在输出力矩较大的问题。

        现有一种自适应鲁棒PD控制策略,避免了初始输出力矩过大的弊端。该控制器由非线性PD反馈和补偿控制两部分构成,机器人不确定动力学部分由回归矩阵构成的自适应控制器进行补偿,并针对机器人 有界扰动的上确界是否已知设计了两种不同的扰动补偿法。该控制策略的优点在于当初始误差较大时,PD反馈起主要作用,通过非线性PD控制,避免了过大初始力矩输出;当误差较小时,自适应控制器起着主要的作用,从而保证了系统具有良好的动态性能。

6.2  机器人动力学模型及其结构特性

       考虑一个N关节的机器人力臂,其动态性能可以由以下二阶非线性微分方程描述:

                                                     \tiny D\left ( q \right )\ddot{q}+C\left ( q,\dot{q} \right )\dot{q}+G\left ( q \right )+\omega =\tau                              (6.7)

式中:\tiny q\in R^{n}为关节角位移量,\tiny D\left ( q \right )\in R^{n\times n}为机器人的惯性矩阵,\tiny C\left ( q,\dot{q} \right )\in R^{n}表示离心力和哥氏力,\tiny G\left ( q \right )\in R^{n}为重力项,\tiny \tau \in R^{n}为控制力矩,\tiny \omega \in R^{n}为各种误差和扰动。

        机器人系统的动力学特性如下:

  •  特性1   D\left ( q \right )-2C\left ( q,\dot{q} \right )是一个斜对称矩阵。
  •  特性2   惯性矩阵D\left ( q \right )是对称正定矩阵,存在正数m_{1},m_{2}满足如下不等式:

                                                           m_{1}\left \| x \right \|^{2}\leqslant x^{T}D\left ( q \right )x\leqslant m_{2}\left \| x \right \|^{2}                                      (6.8)

  •  特性3   存在一个依赖于机械手参数的参数向量,使得D\left ( q \right ),C\left ( q,\dot{q} \right ),G\left ( q \right )满足线性关系:

                                                     D\left ( q \right )\theta +C\left ( q,\dot{q} \right )\rho +G\left ( q \right )=\Phi \left ( q,\dot{q},\rho ,\theta \right )P                         (6.9)

其中,\Phi \left ( q,\dot{q},\rho ,\theta \right )\in R^{n\times m}为已知关节变量函数的回归矩阵,它是机器人广义坐标及其各阶导数的已知函数矩阵;P\in R^{n}是描述机器人质量特性的未知定常参数向量。

        假设1  q_{d}\in R^{n}为期望的关节角位移,q_{d}的一阶导数和二阶导数存在。

        假设2  误差和扰动\omega的范数满足:

                                                            \left \| \omega \right \|\leqslant d_{1}+d_{2}\left \| e \right \|+d_{3}\left \| \dot{e} \right \|                                             (6.10)

其中,d_{1},d_{2},d_{3}分别为正常数,e=q=q_{d},\dot{e}=\dot{q}-\dot{q}_{d}分别为跟踪误差和跟踪误差导数。

6.3  控制器的设计

        分别引入变量\tiny y\tiny q_{r},并令:

                                                               \tiny y=\dot{e}+\gamma e                                                                 (6.11)

                                                              \tiny \dot{q_{r}}=\dot{q_{d}}-\gamma _{e}                                                              (6.12)
 其中常数\tiny \gamma >0,则可推出:

                                                                \tiny y=\dot{q}-\dot{q_{r}}                                                                 (6.13)

        由式(6.9)中的机器人线性关系特性,取\theta =\ddot{q}_{r},\rho =\dot{q}_{r},得:  

                                          D\left ( q \right )\ddot{q}_{r} +C\left ( q,\dot{q} \right )\dot{q}_{r} +G\left ( q \right )=\Phi \left ( q,\dot{q},\dot{q}_{r} ,\ddot{q}_{r} \right )P                              (6.14)

        由式(6.13)得\dot{q}_{r}=\dot{q}-y,将其代入上式,得:

                                  D\left ( q \right )\left (\ddot{q}-\dot{y} \right )+C\left ( q,\dot{q} \right )\left (\dot{q}-y \right ) +G\left ( q \right )=\Phi \left ( q,\dot{q},\dot{q}_{r} ,\ddot{q}_{r} \right )P                                   

        将上式结合式(6.7),得:

                                         D\left ( q \right )\dot{y}+C\left ( q,\dot{q} \right )y =\tau -\Phi \left ( q,\dot{q},\dot{q}_{r} ,\ddot{q}_{r} \right )P-\omega                                (6.15)

1.扰动信号的上确界已知时控制器的设计

        对于式(6.7)所示的机器人系统,在误差扰动信号的上确界已知时,采用以下控制器和自适应律,可保证系统全局渐进稳定。

                                              \tiny \tau =-K_{p}e-K_{v}\dot{e}+\Phi \left ( q,\dot{q},\dot{q}_{r},\ddot{q}_{r} \right )\hat{P}+u                         (6.16)

                                    \tiny u=\begin{bmatrix} u_{1}\cdots u_{n} \end{bmatrix}^{T}\tiny u_{i}=-\left ( d_{1}+d_{2}\left \| e \right \|+d_{3}\left \| \dot{e} \right \| \right )sgn\left ( y_{i} \right )   (6.17)

 \tiny \hat{p}的参数估计律取:

                                                                  \tiny \dot{\hat{p}}=-\Gamma \Phi ^{T}\left ( q,\dot{q},\dot{q}_{r} ,\ddot{q}_{r}\right )y                                    (6.18)

式中

                                            \tiny K_{p}=K_{p1}+K_{p2}B_{p}\left ( e \right ),K_{v}=K_{v1}+K_{v2}B_{v}\left ( \dot{e} \right )

                      \tiny K_{p1}=diag\left ( k_{p11}, k_{p12},\cdots ,k_{p1n}\right ),K_{p2}=diag\left ( k_{p21}, k_{p22},\cdots ,k_{p2n}\right )

                     \tiny K_{v1}=diag\left ( k_{v11}, k_{v12},\cdots ,k_{v1n}\right ),K_{v2}=diag\left ( k_{v21}, k_{v22},\cdots ,k_{v2n}\right )

                                     \tiny B_{p}\left ( e \right )=diag\left ( \frac{1}{\alpha _{1}+\left | e_{1} \right |} , \frac{1}{\alpha _{2}+\left | e_{2} \right |}, \cdots ,\frac{1}{\alpha _{n}+\left | e_{n} \right |}\right )

                                   \tiny B_{v}\left ( e \right )=diag\left ( \frac{1}{\beta _{1}+\left | \dot{e}_{1} \right |} , \frac{1}{\beta _{2}+\left | \dot{e}_{2} \right |}, \cdots ,\frac{1}{\beta _{n}+\left | \dot{e}_{n} \right |}\right )

其中\tiny k_{p1i},k_{p2i},k_{v1i},k_{v2i},\alpha _{i},\beta _{i}\left ( i=1,2,\cdots ,n \right )均大于零,\tiny \Gamma为正定对称阵。

证明过程略。

2.扰动信号的上确界未知时控制器的设计

定理  当误差扰动信号\tiny \omega的上确界为未知时,设计控制器为:

                                                  \tiny \tau =-K_{p}e-K_{v}\dot{e}+\Phi \left ( q,\dot{q},\dot{q}_{r} ,\ddot{q}_{r} \right )\hat{P}+u                      (6.21)

                                                                 \tiny u=-\frac{\left ( \hat{d} f\right )^{2}}{\hat{d} f\left \| y \right \|+\varepsilon ^{2}}y                                                 (6.22)

                                                              \tiny \dot{\hat{d}}=\gamma _{1}f\left \| y \right \|,\hat{d}\left ( 0 \right )=0                                               (6.23)

                                                                 \tiny \dot{\varepsilon }=-\gamma _{2}\varepsilon ,\varepsilon \left ( 0 \right )=0                                                  (6.24)

其中\tiny K_{p},K_{v}的取值同式(6.16),并保证满足式(6.19),\tiny P的估计值\tiny \hat{P}通过式(6.18)求得,\tiny d=d_{1}+d_{2}+d_{3}\tiny \tilde{d}=d-\hat{d},,\tiny \hat{d}\tiny d的估值,\tiny \gamma _{1},\gamma _{2}均为任意的正常数。

        对式(6.7)所示的机器人系统,并且误差扰动信号的上确界未知时,采用式(6.21)和式(6.22)的控制律可保证系统全局渐进稳定。

证明过程略。

6.4  机器人动态方程的线性化推导

        考虑动态方程:

                                    \tiny D\left ( q \right )\ddot{q_{r}}+C\left ( q,\dot{q} \right )\dot{q_{r}}+G\left ( q \right )=\Phi \left ( q,\dot{q},\dot{q_{r}}, \ddot{q_{r}}\right )P              

        为了实现控制律式(6.16)、式(6.17)和自适应律式(6.18),必须给出式(6.28)中\tiny \Phi \left ( q,\dot{q},\dot{q_{r}}, \ddot{q_{r}}\right )\tiny P的表达。

        针对双力臂机器人动态方程:

             \tiny \begin{bmatrix} D_{11} \left ( q_{2} \right )&D_{21} \left ( q_{2} \right ) \\ D_{12} \left ( q_{2} \right )&D_{22} \left ( q_{2} \right ) \end{bmatrix}\begin{bmatrix} \ddot{q_{1}} \\ \ddot{q_{2}} \end{bmatrix}+\begin{bmatrix} -C_{12} \left ( q_{2} \right )\dot{q_{2}}&-C_{12} \left ( q_{2} \right )\left ( \dot{q_{1}} +\dot{q_{2}} \right ) \\ C_{12} \left ( q_{2} \right )\dot{q_{1}}&0 \end{bmatrix}\begin{bmatrix} \dot{q_{1}} \\ \dot{q_{2}} \end{bmatrix}+\begin{bmatrix} G_{1} \left ( q_{1},q_{2} \right )g\\ G_{2} \left \left (q_{1},q_{2} \right )g \end{bmatrix}=\begin{bmatrix} \tau _{1} \\ \tau _{2} \end{bmatrix}             (6.26)

其中

                                 \tiny D_{11}\left ( q_{2} \right )=\left ( m_{1}+m_{2}\right )r_{1}^{2}+m_{2}r_{2}^{2}+2m_{2}r_{1}r_{2}cosq_{2} 

                                                  \tiny D_{12}\left ( q_{2} \right )=m_{2}r_{2}^{2}+m_{2}r_{1}r_{2}cosq_{2}

                                                                  \tiny D_{22}\left ( q_{2} \right )=m_{2}r_{2}^{2}

                                                         \tiny C_{12}\left ( q_{2} \right )=m_{2}r_{2}r_{2}sinq_{2}

                                \tiny G_{1}\left ( q_{1},q_{2} \right )=\left ( m_{1}+m_{2} \right )r_{1}cosq_{2}+m_{2}r_{2}cos\left ( q_{1}+q_{2} \right )

                                                  \tiny G_{2}\left ( q_{1},q_{2} \right )=m_{2}r_{2}cos\left ( q_{1}+q_{2} \right )

则式(6.25)中的第一个关节方程为:

                            \begin{bmatrix} D\left ( 1,1 \right ) & D\left ( 1,2 \right ) \end{bmatrix}\ddot{q}_{r}+\begin{bmatrix} C\left ( 1,1 \right ) & C\left ( 1,2 \right ) \end{bmatrix}\dot{q}_{r}+G\left ( 1 \right )=\begin{bmatrix} \phi _{11}& \phi _{12} & \phi _{13} \end{bmatrix}\begin{bmatrix} p_{1}\\ p_{2}\\ p_{3} \end{bmatrix}

则式(6.25)中的第二个关节方程为:

                            \begin{bmatrix} D\left ( 2,1 \right ) & D\left ( 2,2 \right ) \end{bmatrix}\ddot{q}_{r}+\begin{bmatrix} C\left ( 1,1 \right ) & C\left ( 1,2 \right ) \end{bmatrix}\dot{q}_{r}+G\left ( 2 \right )=\begin{bmatrix} \phi _{21}& \phi _{22} & \phi _{23} \end{bmatrix}\begin{bmatrix} p_{1}\\ p_{2}\\ p_{3} \end{bmatrix}

        根据机器人系统的动力学特性3,取回归矩阵\tiny \Phi \left ( q,\dot{q},\dot{q_{r}}, \ddot{q_{r}}\right )的表达式为:

                                                             \tiny \phi _{11}=\ddot{q}_{1r}+e_{1}cosq_{2}

                                                                \tiny \phi _{12}=\ddot{q}_{1r}+\ddot{q}_{2r}

              \tiny \phi _{13}=2\ddot{q}_{1r}cosq_{2}+\ddot{q}_{2r}cosq_{2}-\dot{q}_{2}\dot{q}_{1r}sinq_{2}-\left ( \dot{q_{1}}+ \dot{q_{2}}\right )\dot{q}_{2r}sinq_{2}+e_{1}cos\left ( q_{1}+q_{2} \right )

                                                                     \tiny \phi _{21}=0

                                                                   \tiny \phi _{22}=\phi _{12}

                                  \tiny \phi _{23}=\dot{q}_{1}\dot{q}_{1r}sinq_{2}+\ddot{q}_{1r}cosq_{2}+e_{1}cos(q_{1}+q_{2})

其中\tiny e_{1}=\frac{g}{r_{1}}\tiny g=9.8

        可见,由于\tiny \Phi \left ( q,\dot{q},\dot{q_{r}}, \ddot{q_{r}}\right )的表达式不含有\tiny m_{1}\tiny m_{2},故本控制算法可实现变负载的机械手控制。

6.5  仿真实例

        被控对象为双力臂机器人动态方程式(6.26),\tiny r_{1}=1,r_{2}=0.8,m_{1}=0.5,m_{2}=0.5

        误差扰动、位置指令和系统的初始状态分别为:
                                 \tiny d_{1}=2,d_{2}=3,d_{3}=6,\omega =d_{1}+d_{2}\left \| e \right \|+d_{2}\left \| \dot{e} \right \|

                                                            \tiny \left\{\begin{matrix} q_{1d}=sin(2\pi t)\\ q_{2d}=sin(2\pi t) \end{matrix}\right.,\begin{bmatrix} q_{1}\\ \dot{q}_{1}\\ q_{2}\\ \dot{q}_{2} \end{bmatrix}=\begin{bmatrix} 0.1\\0\\0.1\\0 \end{bmatrix}
        控制参数取

                                \tiny K_{p1}=diag\left ( 180,190 \right ),K_{p2}=diag\left ( 150,150 \right )

                                \tiny K_{v1}=diag\left ( 180,180 \right ),K_{v2}=diag\left ( 150,150 \right )

                                \tiny \alpha _{i}=\beta _{i}=1\left ( i=1,2 \right ),\gamma =5,\Gamma =\begin{bmatrix} 5 & 0 &0 \\ 0 & 5 &0 \\ 0 &0 & 5 \end{bmatrix}

        采用S函数进行控制器和被控对象的设计。按扰动上确界已知和未知两种情况进行仿真。

仿真之一

        仿真程序

        控制主程序:chap2_2sim.mdl

机器人动力学与控制学习笔记(六)————机器人PID控制_第1张图片

        指令信号子程序:chap2_2input.m

function[sys,x0,str,ts] = chap2_2input(t,x,u,flag)
switch flag
case 0
     [sys,x0,str,ts] = mdlInitializeSizes;
case 3
     sys = mdlOutputs(t,x,u);
case {2,4,9}
     sys = [];
otherwise
     error(['Unhandled flag = ',num2str(falg)]);
end
 
function[sys,x0,str,ts] = mdlInitializeSizes
sizes = simsizes;
sizes.NumOutputs = 6;
sizes.NumInputs = 0;
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 0;
sys = simsizes(sizes);
x0 =[];
str =[];
ts =[];
 
function sys = mdlOutputs(t,x,u)
q1_d = sin(2*pi*t);                    %关节1期望的角位移
q2_d = sin(2*pi*t);                    %关节2期望的角位移
dq1_d = 2*pi*cos(2*pi*t);              %关节1期望的角位移的导数
dq2_d = 2*pi*cos(2*pi*t);              %关节1期望的角位移的导数
ddq1_d = -(2*pi)^2*sin(2*pi*t);        %关节1期望的角位移的二阶导数
ddq2_d = -(2*pi)^2*sin(2*pi*t);        %关节1期望的角位移的二阶导数
 
sys(1) = q1_d;                         %赋值给sys,一共6个输入
sys(2) = dq1_d;                      
sys(3) = ddq1_d;
sys(4) = q2_d;
sys(5) = dq2_d;
sys(6) = ddq2_d;

控制器子程序:chap2_2ctrl.m

%本文件是控制策略
function [sys,x0,str,ts] = chap2_2ctrl(t,x,u,flag)
switch flag
case 0
     [sys, x0,str,ts] = mdlInitializeSizes;
case 3
     sys = mdlOutputs(t,x,u);
case{2,4,9}
     sys = [];
otherwise
     error(['Unhandled flag =',num2str(flag)]);
end
 
function [sys,x0,str,ts] = mdlInitializeSizes
sizes = simsizes;
sizes.NumOutputs      = 2;
sizes.NumInputs       = 13;
sizes.DirFeedthrough  = 1;
sizes.NumSampleTimes  = 0;
sys = simsizes(sizes);
x0 = [];
str =[];
ts = [];
 
function sys = mdlOutputs(t,x,u)
q1_d = u(1);dq1_d = u(2);ddq1_d = u(3);                %关节1期望的角位移,导数,二阶导数
q2_d = u(4);dq2_d = u(5);ddq2_d = u(6);                %关节2期望的角位移,导数,二阶导数
q1 = u(7);dq1 = u(8);                                  %关节1实际的角位移及导数
q2 = u(9);dq2 = u(10);                                 %关节2实际的角位移及导数
 
dq_d = [dq1_d,dq2_d]' ;                               %关节1和关节2期望的角位移的导数矩阵
ddq_d = [ddq1_d,ddq2_d].';                              %矩阵加单引号表示共轭转置,不仅交换行列,还改变虚部符号
  
e = [q1-q1_d,q2-q2_d].';                                %跟踪误差
de = [dq1-dq1_d,dq2-dq2_d].';                           %跟踪误差导数
 
gama = 5*eye(2);                                       %eye(2)表示生成2*2单位矩阵
y = gama.*e+de;                                        %变量y
 
dqr = dq_d-gama.*e;                                    %变量dqr是二维列向量
ddqr = ddq_d-gama.*de;                                  
 
%扰动信号的上确界已知时控制器的设计
d1 = 2;d2 = 3;d3 = 6;
u1 = -(d1+d2*norm([e(1),e(2)])+d3*norm([de(1),de(2)]))*sign(y(1));      %norm([e(1),e(2)])是范数,求e(1)的平方加e(2)的平方和再开根号
u2 = -(d1+d2*norm([e(1),e(2)])+d3*norm([de(1),de(2)]))*sign(y(2));
 
Kp1 = [180,0;0,190];
Kp2 = [150,0;0,150];
Kv1 = [180,0;0,190];
Kv2 = [150,0;0,150];
 
alfa1 = 1;alfa2 = 1;
beta1 = 1;beta2 = 1;
 
p1 = u(11);
p2 = u(12);
p3 = u(13);
P = [p1 p2 p3].';
r1 = 1;g = 9.8;e1 = g/r1;
fai11 = ddqr(1)+e1*cos(q2);
fai12 = ddqr(1)+ ddqr(2);
fai13 = 2*ddqr(1)*cos(q2)+ddqr(2)*cos(q2)-dq2*dqr(1)*dqr(1)*sin(q2)-(dq1+dq2)*dqr(2)*sin(q2)+e1*cos(q1+q2);
fai21 = 0;
fai22 = fai12;
fai23 = dq1*dqr(1)*sin(q2)+ddqr(1)*cos(q2)+e1*cos(q1+q2);
FAI = [fai11 fai12 fai13;fai21 fai22 fai23];
R = FAI * P;
tol(1) = -(Kp1(1,1)+Kp2(1,1)/(alfa1+abs(e(1))))*e(1)-(Kv1(1,1)+Kv2(1,1)/(beta1+abs(de(1))))*de(1)+R(1)+u1;                            %控制力矩tol
tol(2) = -(Kp1(2,2)+Kp2(2,2)/(alfa2+abs(e(2))))*e(2)-(Kv1(2,2)+Kv2(2,2)/(beta2+abs(de(2))))*de(2)+R(2)+u2;                            %控制力矩tol
sys(1) = tol(1);
sys(2) = tol(2);

自适应算法子程序:chap2_2adapt.m

%机器人自适应控制律
function [sys,x0,str,ts] = chap2_2adapt(t,x,u,flag)
switch flag
case 0
     [sys, x0,str,ts] = mdlInitializeSizes;
case 1
     sys = mdlDerivatives(t,x,u);
case 3
     sys = mdlOutputs(t,x,u);
case{2,4,9}
     sys = [];
otherwise
     error(['Unhandled flag =',num2str(flag)]);
end
 
function [sys,x0,str,ts] = mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates    = 3;
sizes.NumDiscStates   = 0;
sizes.NumOutputs      = 3;
sizes.NumInputs       = 10;
sizes.DirFeedthrough  = 1;
sizes.NumSampleTimes  = 0;
 
sys = simsizes(sizes);
x0 = [4.1,1.9,1.7];
str =[];
ts = [];
function sys = mdlDerivatives(t,x,u)
q1_d = u(1);dq1_d = u(2);ddq1_d = u(3);
q2_d = u(4);dq2_d = u(5);ddq2_d = u(6);
 
q1 = u(7);dq1 = u(8);                            
q2 = u(9);dq2 = u(10);                          
q_error  = [q1-q1_d,q2-q2_d]';
dq_error = [dq1-dq1_d,dq2-dq2_d]';
 
gama = 5*eye(2);
y = gama*q_error+dq_error;
ddq_d = [ddq1_d,ddq2_d]';
dq_d  = [dq1_d,dq2_d]';
 
dqr = dq_d-gama*q_error;
ddqr = ddq_d-gama*dq_error;
 
g = 9.8; r1 =1;
e1 = g/r1;
fai11 = ddqr(1)+e1*cos(q2);
fai12 = ddqr(1)+ddqr(2);
fai13 = 2*ddqr(1)*cos(q2)+ddqr(2)*cos(q2)-dq2*dqr(1)*sin(q2)-(dq1+dq2)*dqr(2)*sin(q2)+e1*cos(q1+q2);
 
fai21 = 0;
fai22 = fai12;
fai23 = dq1*dqr(1)*sin(q2)+ddqr(1)*cos(q2)+e1*cos(q1+q2);
FAI =[fai11 fai12 fai13;fai21 fai22 fai23];
Gama = 5*eye(3);
S = -Gama*FAI'*y;
for i=1:1:3
  sys(i)=S(i);
end
function sys = mdlOutputs(t,x,u)
sys(1) = x(1);
sys(2) = x(2);
sys(3) = x(3);

被控对象子程序:chap2_2plant.m

%机器人平台
function [sys,x0,str,ts] = chap2_2plant(t,x,u,flag)
switch flag
case 0
     [sys, x0,str,ts] = mdlInitializeSizes;
case 1
     sys = mdlDerivatives(t,x,u);
case 3
     sys = mdlOutputs(t,x,u);
case{2,4,9}
     sys = [];
otherwise
     error(['Unhandled flag =',num2str(flag)]);
end
 
function [sys,x0,str,ts] = mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates   = 4;
sizes.NumDiscStates   = 0;
sizes.NumOutputs      = 4;
sizes.NumInputs       = 2;
sizes.DirFeedthrough  = 0;
sizes.NumSampleTimes  = 0;
 
sys = simsizes(sizes);
x0 = [0.10,0,0.10,0];
str =[];
ts = [];
function sys = mdlDerivatives(t,x,u)
q1_d = sin(2*pi*t);                           %给定关节1的期望轨迹
q2_d = sin(2*pi*t);                           %给定关节2的期望轨迹
dq1_d = 2*pi*cos(2*pi*t);                     %给定关节1的期望轨迹的导数
dq2_d = 2*pi*cos(2*pi*t);                     %给定关节2的期望轨迹的导数
%ddq1_d = -(2*pi)^2*sin(2*pi*t);               %给定关节1的期望轨迹的二阶导数
%ddq2_d = -(2*pi)^2*sin(2*pi*t);               %给定关节2的期望轨迹的二阶导数
  
e(1) = x(1)-q1_d;                             %关节1角位移的跟踪误差
e(2) = x(3)-q2_d;                             %关节2角位移的跟踪误差
de(1) = x(2)-dq1_d;                           %关节1角位移的跟踪误差的导数
de(2) = x(4)-dq2_d;                           %关节2角位移的跟踪误差的导数
 
tol = [u(1);u(2)];
q1  = x(1);
dq1 = x(2);
q2  = x(3);
dq2 = x(4);
 
m1 = 0.5;m2 = 0.5;                            %机器人的双臂连杆的质量
r1 = 1;r2 = 0.8;                              %机器人的双臂连杆端点到质心之间的距离
g = 9.8;                                      %重力加速度g
 
D11 = (m1+m2)*r1^2+m2*r2^2+2*m2*r1*r2*cos(q2);           %机器人的惯性矩阵      
D12 = m2*r2^2+m2*r1*r2*cos(q2);                          %机器人的惯性矩阵   
D22 = m2*r2^2;                                           %机器人的惯性矩阵   
D = [D11 D12;D12 D22];                                   %机器人的惯性矩阵   
 
C12 = m2*r1*r2*sin(q2);                                  %机器人的离心力和哥氏力
C = [-C12*dq2 -C12*(dq1+dq2);C12*dq1 0];                 %机器人的离心力和哥氏力
  
g1 = (m1+m2)*r1*cos(q2)+m2*r2*cos(q1+q2);                %机器人的重力项
g2 = m2*r2*cos(q1+q2);                                   %机器人的重力项
G = [g1*g;g2*g];                                         %机器人的重力项
w = [1.5;1.5]+2.0*[e(1);e(2)]+5.0*[de(1);de(2)];         %其余误差和扰动
 
S = inv(D).*(tol-[dq1;dq2].*C-G-w);                      %S为给定关节的期望轨迹的二阶导数,即ddq
 
sys(1)=x(2);                                             %给定关节1的实际轨迹的导数
sys(2)=S(1);                                             %
sys(3)=x(4);                                             %给定关节2的实际轨迹的导数
sys(4)=S(2);                                             %

function sys = mdlOutputs(t,x,u)
sys(1) = x(1);                                           %给定关节1的实际轨迹
sys(2) = x(2);                                           %给定关节1的实际轨迹的导数
sys(3) = x(3);                                           %给定关节1的实际轨迹
sys(4) = x(4);                                           %给定关节1的实际轨迹的导数

作图子程序:chap2_2plot.m

close all;
 

t = out.t.Data;
qd = out.qd.Data;
q = out.q.Data; 
p = out.p.Data;

figure(1);
subplot(211);
plot(t,qd(:,1),'r',t,q(:,1),'b');
xlabel('time(s)');
ylabel('position tracking of link 1');
subplot(212);
plot(t,qd(:,2),'r',t,q(:,2),'b');
xlabel('time(s)');
ylabel('velocity tracking of link 1');
 
figure(2);
subplot(211);
plot(t,qd(:,4),'r',t,q(:,3),'b');;
xlabel('time(s)');
ylabel('position tracking of link 2');
subplot(212);
plot(t,qd(:,5),'r',t,q(:,4),'b');
xlabel('time(s)');
ylabel('velocity tracking of link 2');
 
m1 = 0.5;m2 = 0.5;
r1 = 1; r2 = 0.8;
p1 = (m1+m2)*r1^2;
p2 = m2*r2^2;
p3 = m2*r1*r2;
 
figure(3);
subplot(311);
plot(t,p(:,1),'r',t,p1,'b');
xlabel('time(s)');
ylabel('p1 estimate value');
subplot(312);
plot(t,p(:,2),'r',t,p2,'b');
xlabel('time(s)');
ylabel('p2 estimate value');
subplot(313);
plot(t,p(:,3),'r',t,p3,'b');
xlabel('time(s)');
ylabel('p3 estimate value');

 

你可能感兴趣的:(机器人学)