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

文章目录

      • 1. 概述
      • 2. 控制律设计
      • 3. 收敛性
      • 4. 仿真实例步骤
          • 4.1 仿真概述
          • 4.2 simulink窗口
          • 4.3 新建"Blank Model",
          • 4.4 构建simulink仿真图
            • 4.4.1 注意
            • 4.4.2 关于4.4.1节内容的解释
          • 4.5 编辑第一个s函数chap2_1ctrl
          • 4.6 编辑第二个s函数chap2_1plant
          • 4.7 新建m文件chap2_1plot
          • 4.8 仿真
      • 5 注意事项

1. 概述

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

2. 控制律设计

忽略在重力和外加干扰,采用独立 PD 控制,实现机器人的定点控制要求。
设 n 关节机械手方程为:
D ( q ) q ¨ + C ( q , q ˙ ) q ˙ = τ (1) 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, q ˙ \dot{q} q˙) 为 n×n 阶离心力和哥氏力, τ \tau τ为关节力矩,q 为关节角度(要实现的位置), q ¨ \ddot{q} q¨为角加速度, q ˙ \dot{q} q˙为角速度。
独立的PD控制律为:
τ = K d e ˙ + K p e (2) \tau = K_d\dot e+K_pe \tag{2} τ=Kde˙+Kpe(2)
其中:跟踪误差为 e = qd - q,当采用定点方式控制时,qd 为常数项(要求机械手达到的位置), q ˙ \dot{q} q˙d = q ¨ \ddot{q} q¨d = 0

3. 收敛性

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

4. 仿真实例步骤

选取二关节机械臂(不考虑重力、摩擦力和干扰),利用拉格朗日动力学方程,求得动力学模型为:
D(q) q ¨ \ddot{q} q¨ + C(q, q ˙ \dot {q} q˙) q ˙ \dot {q} q˙ = τ \tau τ
其中:
D ( q ) = [ p 1 + p 2 + 2 p 3 c o s q 2 p 2 + p 3 c o s q 2 p 2 + p 3 c o s q 2 p 2 ] 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+2p3cosq2p2+p3cosq2p2+p3cosq2p2]
C ( q , q ˙ ) = [ − p 3 q ˙ 2 s i n q 2 − p 3 ( q ˙ 1 + q ˙ 2 ) s i n q 2 p 3 q ˙ 1 s i n q 2 0 ] 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˙)=[p3q˙2sinq2p3q˙1sinq2p3(q˙1+q˙2)sinq20]
程序中关于D(q)和C(q, 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
q0 = [0 0]T
q ˙ \dot q q˙0 = [0 0]T
关节角度位置指令为:
qd(0) = [1 1]T
在控制器式(2)中,取
Kp = [100  0; 0  100],
Kd = [100  0; 0  100]。

4.1 仿真概述

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

4.2 simulink窗口

simulink入门2--机器人控制系统仿真_第1张图片

4.3 新建"Blank Model",

simulink入门2--机器人控制系统仿真_第2张图片

4.4 构建simulink仿真图

simulink入门2--机器人控制系统仿真_第3张图片
simulink入门2--机器人控制系统仿真_第4张图片
需要指出的是:position,position1,position2 是 sinks 中的 to workspace 模块.

4.4.1 注意
  1. 双击t模块,将Save format更改为Structure
    simulink入门2--机器人控制系统仿真_第5张图片
    simulink入门2--机器人控制系统仿真_第6张图片
  2. 方法如上,将 tol x1 x2Save format均更改为Structure
    simulink入门2--机器人控制系统仿真_第7张图片
4.4.2 关于4.4.1节内容的解释

参见如下文章:

  1. 将simulink的Scope波形数据保存到workspace
  2. MATLAB将simulink中波形数据输出到工作空间
  3. matlab2018中simulink scope数据导出到matlab变量中;
  4. matlab2018中simulink scope数据导出到matlab变量中;
4.5 编辑第一个s函数chap2_1ctrl
  • 双击 S-Function,选择 Edit,然后点击Open Editor

simulink入门2--机器人控制系统仿真_第8张图片
simulink入门2--机器人控制系统仿真_第9张图片

  • 在编辑器打开后,首先选择保存,保存名为chap2_1ctrl,与simulink文件保存在同一目录下,并在编辑器中输入如下代码,然后回到Block Parameters S-Function(第4步)点击 ok
    simulink入门2--机器人控制系统仿真_第10张图片
function [sys,x0,str,ts] = chap2_1ctrl(t,x,u,flag)
% 新定义函数 chap2_1ctrl,四个参数为 t,x,u,flag,另外 sys 是输出
% 参看 sim 仿真图,这个函数是控制器 chap2_1ctrl 控制器的函数
% 实现 τ = Kd*e + Kp*E ,其中:e 是角速度误差,E 是位置误差(2020.3.25 更新)
% 参数说明:(2020.3.25 更新)
%   x1:关节 1 的位置,其导数为其角速度
%   x2:关节 2 的位置,其导数为其角速度
%   tol:关节力矩,为 2x1 的矩阵,其中 tol(1) 为关节 1 的力矩,tol(2)同理。
% 输入输出:
%   输入:6 个输入,依次分别为 u(1),u(2),u(3),u(4),u(5),u(6),分别对应
%        sim 图中的 step1,step2, x1 ,x1 的倒数,x2, x2 的倒数(x1, x2为实际
%        关节1,关节2 的位置,其导数则为角速度)
%   输出:tol:关节力矩,为 2x1 的矩阵,其中 tol(1) 为关节 1 的力矩,tol(2)同理。
switch flag,  % 判断flag
case 0,       % flag == 0,调用 mdlInitializeSizes 函数
    [sys,x0,str,ts]=mdlInitializeSizes;
case 3,       % flag == 3,调用 mdlOutputs 函数
    sys=mdlOutputs(t,x,u);
case {2,4,9}
    sys=[];   % 输出为空
otherwise  
    error(['Unhandled flag = ',num2str(flag)]);
end

%-------mdlInitializeSizes 函数-------------------%
function [sys,x0,str,ts]=mdlInitializeSizes
sizes = simsizes;         % 结构体模块的赋值
sizes.NumOutputs     = 2; % 输出参数为控制力矩, tol 为 2x1 的矩阵(2020.3.25 更新)
sizes.NumInputs      = 6; % 输入参数 6 个,两个 step,四个chap2_1plant的输出
sizes.DirFeedthrough = 1; % 输入直接控制输出
sizes.NumSampleTimes = 1; % 采样时间为 1
sys = simsizes(sizes);    % 确定参数赋值给 sys

x0  = [];                 % 初始值为空
str = [];                 % 默认设置为空
ts  = [0 0];              % 采样时间与偏移量


%-------mdlOutputs(t,x,u) 函数-------------------%
function sys=mdlOutputs(t,x,u)  % 函数有三个输入参数(结合sim图观察)
R1=u(1);      % step 是 u1,即前文分析的位置指令 qd(0)[1;1]的(1,1)元素
dr1=0;        % 微分,对 R1 的微分,即关节角 1 的角速度(2020.3.25 更新)
R2=u(2);      % step 是 u2,即前文分析的位置指令 qd(0)[1;1]的(1,2)元素
dr2=0;        % 微分,对 R2 的微分,即关节角 2 的角速度 (2020.3.25 更新)

x(1)=u(3);    % 状态变量,反馈环节chap2_1plant的输出作为控制器的输入x1
x(2)=u(4);    % 状态变量,反馈环节chap2_1plant的输出作为控制器的输入 x1的导数
x(3)=u(5);    % 状态变量,反馈环节chap2_1plant的输出作为控制器的输入x2
x(4)=u(6);    % 状态变量,反馈环节chap2_1plant的输出作为控制器的输入 x2的导数

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; % PD 控制,在sim中作为输出变量,输出到 workspace

sys(1)=tol(1);  % 关节 1 的驱动力矩
sys(2)=tol(2);  % 关节 2 的驱动力矩

4.6 编辑第二个s函数chap2_1plant
function [sys,x0,str,ts]=chap2_1plant(t,x,u,flag)
% S-function for continuous state equation
% 此为 sim 图中的被控对象 char2_1plant 的函数
switch flag,  % 判断 flag
%Initialization
  case 0,     % flag == 0,执行 mdlInitializeSizes 子函数
    [sys,x0,str,ts]=mdlInitializeSizes;
case 1,       % flag == 2,执行 mdlDerivatives 子函数
    sys=mdlDerivatives(t,x,u);
%Outputs
  case 3,% flag == 3,执行 mdlOutputs 子函数
    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                  % 定义全局变量 p g 
sizes = simsizes;
sizes.NumContStates  = 4;   % 连续变量 4 个
sizes.NumDiscStates  = 0;   % 离散变量 0 个
sizes.NumOutputs     = 4;   % 输出 4 个
sizes.NumInputs      =2;    % 输入 2 个
sizes.DirFeedthrough = 0;   % 输入不控制输出
sizes.NumSampleTimes = 0;   % 采样时间为 0,被控对象,无需采样
sys=simsizes(sizes);
x0=[0 0 0 0];
str=[];
ts=[];

p=[2.9 0.76 0.87 3.04 0.87];  % p 是D(q)正定惯性矩阵的各项系数
g=9.8;      % 重力系数 9.8

%-------mdlDerivatives 函数-------------------%
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];  % 离心力和哥氏力
 
% 被控对象 chap2_1plant的输入是控制器的输出,此时的u(1:2)是
% chap2_1ctrl 的输出 sys,即经过 PD 调节的力矩
%
tol=u(1:2);        
dq=[x(2);x(4)];    % x1的导数,x2的导数

S=inv(D0)*(tol-C0*dq);  % 动力学方程的反向应用,用于求出角加速度
% 这里的sys 为中间变量,S(1),S(2)为关节角1,2的角加速度
sys(1)=x(2);
sys(2)=S(1);
sys(3)=x(4);
sys(4)=S(2);

%-------mdlOutputs 函数-------------------%
function sys=mdlOutputs(t,x,u)
sys(1)=x(1);  % 状态变量 x1,作为被控对象的输出,同时反馈到控制器
sys(2)=x(2);  % 状态变量 x1的导数,作为被控对象的输出,同时反馈到控制器
sys(3)=x(3);  % 状态变量 x2,作为被控对象的输出,同时反馈到控制器
sys(4)=x(4);  % 状态变量 x2的导数,作为被控对象的输出,同时反馈到控制器

4.7 新建m文件chap2_1plot
  • 编辑器选择新建文件,保存为’chap2_1plot’,目录与上述文件目录相同
    simulink入门2--机器人控制系统仿真_第11张图片
  • 输入程序如下
close all;

% 绘制关节位置与关节角速度
figure(1);
subplot(211);
plot(t.signals(1).values(:,1), x1.signals(1).values(:,2),'r', ...
     t.signals(1).values(:,1), x1.signals(1).values(:,3),'b');
xlabel('time(s)');ylabel('position tracking of link 1');
subplot(212);
plot(t.signals(1).values(:,1), x2.signals(1).values(:,2),'r', ...
     t.signals(1).values(:,1), x2.signals(1).values(:,3),'b');
xlabel('time(s)');ylabel('position tracking of link 2');

% 关节力矩
figure(2);
subplot(211);
plot(t.signals(1).values(:,1), tol.signals(1).values(:,1),'r');
xlabel('time(s)');ylabel('tol1');
subplot(212);
plot(t.signals(1).values(:,1), tol.signals(1).values(:,2),'r');
xlabel('time(s)');ylabel('tol2');

4.8 仿真
  1. 在m函数编辑器处点击运行
    simulink入门2--机器人控制系统仿真_第12张图片
  2. 选择“添加到路径”
    simulink入门2--机器人控制系统仿真_第13张图片
  3. 忽略上述关于参数的提示,运行simulink(书中没有示波器,参照上文添加示波器)
    3.1 忽略下图的提示
    simulink入门2--机器人控制系统仿真_第14张图片

3.2 运行仿真,点击示波器,观察图形:
simulink入门2--机器人控制系统仿真_第15张图片
simulink入门2--机器人控制系统仿真_第16张图片
3.3 与此同时,在matlab的工作区出现参数:
simulink入门2--机器人控制系统仿真_第17张图片

5 注意事项

  1. 在搭建好simulink,完成m函数的编辑之后,首先运行m函数(忽略matlab给出的错误提示),然后再运行simulink。分析原因:在添加到路径这一步骤中,matlab将m函数读入,从而在simulink的运行时,不会在报错(找不到函数)
  2. chap2_1plant·实际输出了 4 个信号,分别为x1, x1 的导数,x2,x2 的导数。在经过一个demux分离后,x1 与 x1 的导数上行与 step1信号经过mux到达示波器,故示波器显示三根线,x2 与 x2 的导数同理与step2mux到达示波器,示波器显示三根线。
  3. simulink的搭建中,x1,x2,tol都是多个信号的混合,可以采用demux将信号分开。
  4. 这本书作为入门不太好,推荐先看看《用MATLAB玩转机器人》(电子版或实体书)
    simulink入门2--机器人控制系统仿真_第18张图片

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