基于Simulink的二关节机器人独立PD控制仿真

本文是刘金琨. 机器人控制系统的设计与MATLAB仿真的学习笔记。

理论模型

对于二关节机器人系统，其动力学模型为

$$D(q)\ddot{q}+C(q,\dot{q})\dot{q}=r$$

式中，$D(q)$为$n\times n$阶正定惯性矩阵，$C(q,\dot{q})$为$n\times n$阶离心和哥氏力项，在二关节机器人系统中，二者的表达式为

$$D(q)=\left[\begin{array}{cc}{p}_1+p_2+2p_3\cos q_2& p_2+p_3\cos q_2\\ p_2+p_3\cos q_2& p_2\end{array}\right],C(q,\dot{q})=\left[\begin{array}{cc}-p_3{\dot{q}}_3\sin q_2& -p_3({\dot{q}}_1+{\dot{q}}_2)\sin q_2\\ p_3{\dot{q}}_1\sin q_2& 0\end{array}\right]$$

该式推导过程见：二关节机器人系统模型推导

独立的PD控制率为

$$\tau =K_d\dot{e}+K_{p}e,e=q_d-q$$

接下来，取下列参数进行仿真

$$p=\left[\begin{array}{c}2.90\\ 0.76\\ 0.87\\ 3.04\\ 0.87\end{array}\right],q_0=\left[\begin{array}{c}0.0\\ 0.0\end{array}\right],{\dot{q}}_0=\left[\begin{array}{c}0.0\\ 0.0\end{array}\right],K_p=\left[\begin{array}{cc}100& 0\\ 0& 100\end{array}\right],K_d=\left[\begin{array}{cc}100& 0\\ 0& 100\end{array}\right]$$

仿真窗口

首先，打开Matlab，点击Simulink按钮，双击新建空白模型，在弹出窗口中，点击库浏览器，在Simulink中选择下列模块

• Commonly Used Blocks->Demux, Mux, Scope
• Sources->Step, Clock
• User-Defined Functions->S-Function
• Sinks->To Workspace

绘制成如下形式

双击模块可更改其属性，除了变量名称之外，将t, x1, x2, tol的输出格式更改为结构体。

其中x1, x2即为两个关节所在位置，tol即$\tau$。

控制函数

双击左侧的S-Function，将其名称改为ctrlTest，点击【编辑】按钮->打开编辑器，将其保存为ctrlTest.m，确保和simulink文件在同一目录下。

S函数的输入参数为t,x,u,flag，其中t为当前仿真时间；x为状态变量；u为输入信号。flag为标志变量，与Simulink的状态有关，其值的含义如下

• flag = 0：初始化阶段，在此阶段，S-Function 需要返回模块的参数信息。
• flag = 1：计算导数，常用于连续时间系统的状态方程。
• flag = 2：更新阶段，常用于离散时间系统的更新。
• flag = 3：计算输出，通常用于计算模块的输出信号。
• flag = 4：计算下一个采样时间,通常用于离散时间系统。
• flag = 9：终止阶段，通常用于清理资源。

ctrlTest函数的完整内容如下

function [sys,x0,str,ts] = ctrlTest(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

%-------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)
e=[u(1)-u(3);u(2)-u(5)];        % 位置偏差
de=[0-u(4);0-u(6)]; % 角速度偏差

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 的驱动力矩

在【ctrlTest】中，根据系统状态来选择处理函数，当系统处于初始化状态，则调用【mdlInitializeSizes】函数，当系统计算暑促时，调用【mdlOutputs】，其他状态则输出空向量。

在初始化函数中，使用了simsizes函数，这是用于获取Simulink模块的参数，其返回的结构体中包含以下参数

• NumInputs: 输入端口的数量
• NumOutputs: 输出端口的数量
• NumStates: 状态变量的数量
• InitialState: 初始条件
• ContStates: 连续状态变量的数量
• DiscStates: 离散状态变量的数量
• DirFeedthrough: 是否允许直接馈通
• SampleTime: 采样时间

在【mdlOutputs】函数中，其输入的u即为simuTest图形中，ctrlTest的三个输入量，其中u(1), u(2)即为两个阶跃函数，u(3)到u(6)则为后面的目标函数plantTest输出的sys。

目标函数

右侧的S函数用同样的操作，命名为plantTest，同样根据系统状态执行不同的操作。

function [sys,x0,str,ts]=plantTest(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

%-------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];          % 离心力和哥氏力

tol=u(1:2);        
dq=[x(2);x(4)];    % x1的导数，x2的导数

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

%-------mdlOutputs 函数-------------------%
function sys=mdlOutputs(t,x,u)
sys = x;

在【mdlDerivatives】函数中，实现了二关节机器人的动力学模型，其输出的sys，将被传递给ctrlTest函数中，作为u变量的后四个分量。

仿真

点击Simulink的运行按钮，即可开启仿真，双击示波器，即可查看波形