simulink入门2--机器人控制系统仿真

文章目录

• 1. 概述
• 2. 控制律设计
• 3. 收敛性
• 4. 仿真实例步骤
• 1. 仿真概述
• 2. simulink窗口
• 3. 新建"Blank Model",
• 4. 构建simulink仿真图
• 5. 编辑第一个s函数
• 6. 编辑第二个s函数
• 7. 新建m文件
• 8. 未完待续

1. 概述

详细介绍《机器人控制系统的设计与MATLAB仿真》--刘金琨 编著 的仿真程序Simulink 主程序：chap2_1sim.mdl 的仿真步骤。

2. 控制律设计

忽略在重力和外加干扰，采用独立PD控制，实现机器人的定点控制要求。
设 n 关节机械手方程为:
$D(q) \ddot{q} + C(q,\dot {q}) \dot {q} = \tau \tag{1}$D(q)q¨​+C(q,q˙​)q˙​=τ(1)
式中 D(q) 为 n×n 阶正定惯性矩阵，C(q,$\dot{q}$q˙​) 为 n×n 阶离心力和哥氏力，$\tau$τ为关节力矩，q 为位置，$\ddot{q}$q¨​为角加速度，$\dot{q}$q˙​为角速度。
独立的PD控制律为：
$\tau = K_d\dot e+K_pe \tag{2}$τ=Kd​e˙+Kp​e(2)
跟踪误差为 e = q_d - q,当采用定点方式控制时，q_d 为常数项(要求机械手达到的位置)，$\dot{q}$q˙​_d = $\ddot{q}$q¨​_d = 0

3. 收敛性

由李雅普诺夫判据及 LaSalle 定理可知，(e,$\dot e$e˙) = (0,0)是受控机器人全局渐进稳定的平衡点，即从任意初始条件 (q₀,$\dot q$q˙​₀) 出发，均有 q→q_d,$\dot q$q˙​_d→0.

4. 仿真实例步骤

选取二关节机械臂(不考虑重力、摩擦力和干扰)，其动力学模型为：
D(q) $\ddot{q}$q¨​ + C(q,$\dot {q}$q˙​) $\dot {q}$q˙​ = $\tau$τ
其中
$D(q) = \begin{bmatrix} p_1+p_2+2p_3cosq_{2} & p_2+p_3cosq_2\\ p_2+p_3cosq_2 & p_2 \end{bmatrix}$D(q)=[p1​+p2​+2p3​cosq2​p2​+p3​cosq2​​p2​+p3​cosq2​p2​​]
$C(q,\dot q) = \begin{bmatrix} -p_3\dot q_2sinq_2 &-p_3(\dot q_1+\dot q_2)sinq_2 \\ p_3\dot q_1sinq_2 & 0 \end{bmatrix}$C(q,q˙​)=[−p3​q˙​2​sinq2​p3​q˙​1​sinq2​​−p3​(q˙​1​+q˙​2​)sinq2​0​]
程序中关于D(q)和C(q,$\dot q$q˙​)d的写法

D0=[p(1)+p(2)+2*p(3)*cos(x(3)) p(2)+p(3)*cos(x(3));
    p(2)+p(3)*cos(x(3)) p(2)];
C0=[-p(3)*x(4)*sin(x(3)) -p(3)*(x(2)+x(4))*sin(x(3));
     p(3)*x(2)*sin(x(3))  0];

取 p = [2.90   0.76   0.87   3.04   0.87]^T，q₀ = [0 0]^T，$\dot q$q˙​₀ = [0 0]^T。
位置指令为q_d(0) = [1 1]^T，在控制器式（2）中，取K_p = [100  0; 0  100],K_d = [100  0; 0  100]。

1. 仿真概述

详细介绍《机器人控制系统的设计与MATLAB仿真》--刘金琨 编著 的仿真程序Simulink 主程序：chap2_1sim.mdl 仿真实例的仿真步骤。

2. simulink窗口

3. 新建"Blank Model",

4. 构建simulink仿真图

需要指出的是:position,position1,position2 是 sinks 中的 to workspace 模块

5. 编辑第一个s函数

• 双击 S-Function,选择 Edit,然后点击Open Editor

• 在编辑器打开后，首先选择保存，保存名为chap2_1ctrl,与simulink文件保存在同一目录下，并在编辑器中输入如下代码，然后回到Block Parameters S-Function(第4步)点击 ok
  

function [sys,x0,str,ts] = spacemodel(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      = 6;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
x0  = [];
str = [];
ts  = [0 0];

function sys=mdlOutputs(t,x,u)
R1=u(1);dr1=0;
R2=u(2);dr2=0;

x(1)=u(3);
x(2)=u(4);
x(3)=u(5);
x(4)=u(6);

e1=R1-x(1);
e2=R2-x(3);
e=[e1;e2];

de1=dr1-x(2);
de2=dr2-x(4);
de=[de1;de2];

Kp=[50 0;0 50];
Kd=[50 0;0 50];

tol=Kp*e+Kd*de;

sys(1)=tol(1);
sys(2)=tol(2);

6. 编辑第二个s函数

%S-function for continuous state equation
function [sys,x0,str,ts]=s_function(t,x,u,flag)

switch flag,
%Initialization
  case 0,
    [sys,x0,str,ts]=mdlInitializeSizes;
case 1,
    sys=mdlDerivatives(t,x,u);
%Outputs
  case 3,
    sys=mdlOutputs(t,x,u);
%Unhandled flags
  case {2, 4, 9 }
    sys = [];
%Unexpected flags
  otherwise
    error(['Unhandled flag = ',num2str(flag)]);
end

%mdlInitializeSizes
function [sys,x0,str,ts]=mdlInitializeSizes
global p g
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 0 0 0];
str=[];
ts=[];

p=[2.9 0.76 0.87 3.04 0.87];
g=9.8;
function sys=mdlDerivatives(t,x,u)
global p g

D0=[p(1)+p(2)+2*p(3)*cos(x(3)) p(2)+p(3)*cos(x(3));
    p(2)+p(3)*cos(x(3)) p(2)];
C0=[-p(3)*x(4)*sin(x(3)) -p(3)*(x(2)+x(4))*sin(x(3));
     p(3)*x(2)*sin(x(3))  0];
tol=u(1:2);
dq=[x(2);x(4)];

S=inv(D0)*(tol-C0*dq);

sys(1)=x(2);
sys(2)=S(1);
sys(3)=x(4);
sys(4)=S(2);
function sys=mdlOutputs(t,x,u)
sys(1)=x(1);
sys(2)=x(2);
sys(3)=x(3);
sys(4)=x(4);

7. 新建m文件

• 编辑器选择新建文件，保存为’chap2_1plot’，目录与上述文件目录相同
  
• 输入程序如下

close all;

figure(1);
subplot(211);
plot(t,x1(:,1),'r',t,x1(:,2),'b');
xlabel('time(s)');ylabel('position tracking of link 1');
subplot(212);
plot(t,x2(:,1),'r',t,x2(:,2),'b');
xlabel('time(s)');ylabel('position tracking of link 2');

figure(2);
subplot(211);
plot(t,tol(:,1),'r');
xlabel('time(s)');ylabel('tol1');
subplot(212);
plot(t,tol(:,2),'r');
xlabel('time(s)');ylabel('tol2');

8. 未完待续

后续插入仿真调试，程序的运行。