本文是劉金琨. 機器人控制系統的設計與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 flagcase 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);    % 確定參數賦值給 sysx0  = [];                 % 初始值為空
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中作為輸出變量，輸出到 workspacesys(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 flagcase 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的運行按鈕，即可開啟仿真，雙擊示波器，即可查看波形