【基於Simulink+UG NX MCD 一級倒立擺控制系統仿真】建模和分析(一)

前言

倒立擺是比較典型的系統，可以看出火箭發射的簡化模型，國內外學者常常通過在倒立擺上開發和測試控制算法。

對倒立擺的控制分爲兩大任務：

起擺
穩擺

所以本文想通過此項目對自動控制原理進行一個複習與學習的過程，具體目標設想建立單擺模型，通過simulink進行仿真，開發相關算法，同時，利用UG NX MCD建立單擺機電模型，可以進行更爲直觀的仿真，但利用MCD進行控制算法的開發還未看到有相關研究。

視頻資料：【倒立擺合集】一、二、三階倒立擺

建立數學模型

Step 0：問題描述

如圖所示，倒立擺一端通過關節連接水平移動的小車上，小車由電機輪驅動並且只能在 $x$ 方向移動，電機水平驅動力爲 $F$ ，小車質量爲 $M$ ，擺杆質量爲 $m$ ，擺杆長度爲 $2l$ ，要研究怎樣的外力 $F$ 輸入才能控制擺杆保持垂直。

Step 1：採用牛頓力學定律進行建模

設小車移動記錄爲 $x$ ，擺杆偏離垂直方向角度爲 $θ$ ，小車對擺杆垂直方向和水平方向的反作用力分別爲 $P$ 和 $N$

對於小車，根據牛頓定律可以寫出水平方向的力平衡方程：
$M\ddot{x}=F-B_p\dot{x}-N$

對於擺杆，根據牛頓定律可以寫出水平方向和垂直方向的力平衡方程：
$N=m\frac{d^{2} x_{1}}{dt^{2}} =\frac{d^{2} }{dt^{2}}(x+lsinθ) =m(\dot{x}+\dot{θ}lcosθ)'$ $=m\ddot{x}+ml\ddot{θ}cosθ-ml\dot{θ^2}sinθ$

$P-mg=\frac{d^{2}y_1}{dt^{2}}=\frac{d^{2}}{dt^{2}}(lcosθ)=m(-\dot{θ}lsinθ)'$ $=-ml\ddot{θ}sinθ-ml\dot{θ}cosθ$
其中： $x_1$ 爲擺杆質心水平移動距離， $y_1$ 爲垂直移動距離。

擺杆對質心轉動慣量爲 $I$ ，則擺杆質心的力矩方程爲：
$I\ddot{θ}=Plsinθ-Nlcosθ$
聯立上述四式，消去變量 $H$ 和 $N$ ，得:
$I\ddot{θ}=mglsinθ-ml^{2}\ddot{θ}-ml\ddot{x}cosθ$

$M\ddot{x}=F-B_p\dot{x}-m(\ddot{x}+l\ddot{θ}cosθ-l\dot{θ^2}sinθ)$

爲使得倒單擺保持垂直狀態，假定 $θ$ 很小，則有 $cosθ≈1，sinθ=0，\dot{θ}^2=0$ ，因此上面兩式可化簡爲：
$(I-ml^2)\ddot{θ}+ml\ddot{x}=mglθ$ $(M+m)\ddot{x}+B_p\dot{x}+ml\ddot{θ}=F$

設擺杆質量分佈均勻,則 $I=\frac{m(2l)^2}{12}$ ，同時忽略摩擦， $B_p=0$ ，因此上面兩式可化簡爲數學模型方程：
$\frac{4}{3}ml^2\ddot{θ}+ml\ddot{x}=mglθ$ $(M+m)\ddot{x}+ml\ddot{θ}=F$

Step 2：傳遞函數

對上面兩式做拉式變換，並設初始狀態爲 $x(0)=0，θ(0)=0$ ，得：
$\frac{4}{3}ml^2Θ(s)s^2+mlX(s)s^2=mglΘ(s)$ $(M+m)X(s)s^2+mlΘ(s)s^2=F(s)$

設定系統輸出用 $θ$ 角表示，則聯立上面兩式，意消去 $X(s)$ ，得
$X(s)=(\frac{g}{s^2}-\frac{4l}{3})Θ(s)$ $(M+m)(\frac{g}{s^2}-\frac{4l}{3})Θ(s)s^2+mlΘ(s)s^2=F(s)$
整理得傳遞函數：
$\frac{Θ(s)}{F(s)}=\frac{1}{[(M+m)(\frac{g}{s^2}-\frac{4l}{3})+ml]s^2}$
$=\frac{-\frac{3}{(4M+m)l}}{s^2-\frac{3(M+m)g}{(4M+m) l}}$

使用matlab構造傳遞函數

取 $M=0.1kg，m=0.5kg，l=0.5m，g=9.81$ ，使用matlab計算，則有

%% 清理
clear;close;clc;

%% 模型數據
M=1;%kg，小車質量
m=0.5;%kg,擺杆質量
l=0.5;%m,擺杆長
g=9.81;%% 傳遞函數

num = [-3/((4*M+m)*l)];  %分子多項式係數行向量
den = [1 0 -(3*(M+m)*g)/((4*M+m)*l)];   %分母多項式係數行向量
GS = tf(num,den); %建立傳遞函數模型

得到傳遞函數爲：

關於狀態空間模型

狀態空間方法是現代控制中的內容，適合於多輸入多輸出系統，時變系統等，並且可以知道內部變量的變化，而傳遞函數模型是經典控制中的內容，只適用於單輸入單輸出的線性定常系統。

狀態空間方程標準形式和方框圖作法如下

Step 3：狀態空間模型

選取系統狀態變量：

$x_1=θ，x_2=\dot{θ}，x_3=x，x_4=\dot{x}$

根據上述數學模型方程，於是得到

$\dot{x_1}=x_2$

$\dot{x_2}=\frac{3(M+m)g}{(4M+m)l}x_1-\frac{3}{(4M+m)l}F$

$\dot{x_3}=x_4$

$\dot{x_4}=-\frac{3mg}{4M+m}x_1+\frac{4}{4M+m}F$

則狀態方程爲：
$\begin{bmatrix}\dot{x_1} \\\dot{x_2} \\\dot{x_3} \\\dot{x_4}\end{bmatrix}=\begin{bmatrix}0 & 1 &0 &0 \\\frac{3(M+m)g}{(4M+m)l} &0 &0 &0 \\0 &0 &0 & 1\\-\frac{3mg}{4M+m} &0 &0 &0\end{bmatrix}\begin{bmatrix}x_1 \\x_2 \\x_3 \\x_4\end{bmatrix}+\begin{bmatrix}0 \\-\frac{3}{(4M+m)l} \\0 \\\frac{4}{4M+m}\end{bmatrix}F$

輸出變量選擇爲

$y_1=x$

$y_2=θ$

則輸出方程爲
$y=\begin{bmatrix}x\\θ\end{bmatrix}=\begin{bmatrix}0 &0 &1 &0\\1&0&0&0\end{bmatrix} \begin{bmatrix}x_1\\x_2\\x_3\\x_4\end{bmatrix}$

有：

$A=\begin{bmatrix}0 & 1 &0 &0 \\\frac{3(M+m)}{(4M+m)l}g &0 &0 &0 \\0 &0 &0 & 1\\-\frac{3mg}{4M+m} &0 &0 &0\end{bmatrix}$

$B=\begin{bmatrix}0 \\-\frac{3}{(4M+m)l} \\0 \\\frac{4}{4M+m}\end{bmatrix}$

$C=\begin{bmatrix}0 &0 &1 &0\\1&0&0&0\end{bmatrix}$

$D=0$

傳遞矩陣

傳遞函數和狀態空間方程之間可以進行轉換。

由狀態空間方程，根據公式，可以算得傳遞矩陣

可得
$G(s)=\begin{bmatrix}0 &0 &1 &0\\1&0&0&0\end{bmatrix}\left(\begin{bmatrix}s &0 &0 &0 \\0 &s &0 &0 \\0 & 0 & s &0 \\0 &0 &0 &s\end{bmatrix}-\begin{bmatrix}0 & 1 &0 &0 \\\frac{3(M+m)}{(4M+m)l}g &0 &0 &0 \\0 &0 &0 & 1\\-\frac{3mg}{4M+m} &0 &0 &0\end{bmatrix}\right)^{-1}\begin{bmatrix}0 \\-\frac{3}{(4M+m)l} \\0 \\\frac{4}{4M+m}\end{bmatrix}$
使用matlab計算

%% 狀態空間模型
syms M m l g;
A = [0 1 0 0 ;
    3*(M+m)/(4*M+m)/l*g 0 0 0;
    0 0 0 1;
    -3*m*g/(4*M+m) 0 0 0;]
B = [0;
   -3/(4*M+m)/l;
    0;
    4/(4*M+m);]
C = [0 0 1 0;
    1 0 0 0;];
D = 0;

syms s;
si = s*eye(4);
%det(si-A);%行列式
%inv(si-A)%逆矩陣
Gs_z = C * inv(si-A) * B+D %轉遞矩陣

結果爲：

即小車位移和力的傳遞函數爲第一行，擺杆擺角和驅動力的傳遞函數爲第二行，我們關心擺角，則有
$\frac{Θ(s)}{F(s)}=\frac{-\frac{3}{(4M+m)l}}{s^2-\frac{3(M+m)g}{(4M+m) l}}$

可見和通過拉氏變換得到的傳遞函數一樣。

Step 4：模型分析

分析模型是否穩定，可不可控，以及能不能觀測。

穩定性

a. 奈奎斯特判據
使用matlab做出開環傳遞函數的零極點圖和nyquist圖

%% 模型分析
figure() ;
subplot(121);
pzmap(GS)%零極點圖
grid;

subplot(122);
nyquist(GS)%nyquist圖 由開環傳遞函數判斷閉環系統穩定性 是圖解分析的方法
grid;

由經典控制理論奈奎斯特判據，穩定系統在 $s$ 右半平面不能有閉環極點，公式爲 $Z=N+P=0$ ，從圖中看到開環傳遞函數在 $s$ 右半平面內的極點數爲 $P=1$ ，對 $-1+j0$ 順時針包圍的次數 $N=0$ ，所以 $Z=1$ ，即閉環傳遞函數特徵方程的零點在s右半平面的個數爲1，因此係統不穩定。

b. 李雅普諾夫第一法（間接法）

參考文獻：現代控制理論 6-1 概念 6-2 李雅普諾夫第一法(間接法)
由現代控制理論李雅普諾夫第一法，通過判斷系統矩陣A的特徵值來判斷穩定性，令 $|λE-A|=0$ ，解得特徵值 $λ=[0, 0, 4.4294, -4.4294]$ ，有大於0的特徵值，所以系統不穩定。matlab代碼如下：

%李雅普諾夫第一法（間接法）
sys lm;%矩陣A特徵值
da=det(lm*eye(4)-A) %A特徵方程行列式
lm=solve(da)%求解da零點

%公式求解 和上面一樣
eig(A) %公式求解A特徵值

可控性

根據現代控制理論，完全可控的條件是 $n\times n$ 維的可控性矩陣 $[B, AB, ...,A^{n-1} B]$ 的秩爲 $n$ ，秩就是矩陣的非零子式的最高階次。
本系統中，可控性矩陣爲
$Q_c=[A，AB，A^2B，A^3B]$

在Matlab中計算

%% 可控可觀性分析
qc=[B A*B A^2*B A^3*B]
rank(qc)%求秩

所以說明系統是完全可控的。

可觀性

根據現代控制理論，完全可觀的條件爲 $n\times mn$ 維的可觀測矩陣 $[C,CA,...,CA^{n-1}]^T$ 的秩等於 $n$ ，則本系統可觀性矩陣爲
$G_c=\begin{bmatrix}\\C\\CA\\CA^2\\CA^3\end{bmatrix}$
在Matlab中計算

gc=[C C*A C*A^2 C*A^3]';
rank(gc)%可觀性矩陣求秩

所以說明系統是完全可觀測的。

Step 5： Simulink模型

根據狀態空間方程作圖法建立模型

開環階躍信號響應曲線

代碼分享

%% 清理
clear;
close;
clc;
%% 模型數據
M=1;%kg，小車質量
m=0.5;%kg,擺杆質量
l=0.5;%m,擺杆長
g=9.81;

%% 傳遞函數模型
num = [-3/((4*M+m)*l)];  %分子多項式係數行向量
den = [1 0 -(3*(M+m)*g)/((4*M+m)*l)];   %分母多項式係數行向量
GS = tf(num,den); %建立傳遞函數模型

%% 狀態空間模型
%syms M m l g; %算符號傳遞矩陣時用上
A = [0 1 0 0 ;
    3*(M+m)/(4*M+m)/l*g 0 0 0;
    0 0 0 1;
    -3*m*g/(4*M+m) 0 0 0;];
B = [0;
   -3/(4*M+m)/l;
    0;
    4/(4*M+m);];
C = [0 0 1 0;
    1 0 0 0;];
D = [0;0];

syms s;
si = s*eye(4);
%det(si-A);%行列式
%inv(si-A)%逆矩陣
Gs_z = C * inv(si-A) * B+D; %轉遞矩陣
%[num,den] = ss2tf(A,B,C,D,1)%也可使用此函數進行轉換

GSS=ss(A,B,C,D)%由狀態空間構造傳遞函數
%% 模型相關分析作圖

sys1=feedback(GS,1);%閉環傳遞函數
figure()
rlocus(sys1)%根軌跡圖 %由閉環傳遞函數特徵方程的根隨開環增益K從0-anf變化在S平面上
                    %的變化軌跡，優點在於不必求解特徵方程
                    %全爲復
%rlocus(A,B,C,D)%用戶自定義K可以使用rlocus(sys1,K),rlocus(A,B,C,D,K)

figure();
bode(sys1);%伯德圖
grid;

figure();
step(GS);%階躍響應曲線
grid;
%% 穩定性分析
%nyquist判據方法
figure() ;
subplot(121);
pzmap(GS)%零極點圖
grid;
subplot(122);
nyquist(GS)%nyquist圖 由開環傳遞函數判斷閉環系統穩定性 是圖解分析的方法
grid;

%李雅普諾夫第一法（間接法）
syms lm;%矩陣A特徵值
da=det(lm*eye(4)-A); %A特徵方程行列式
lm=solve(da);%求解da零點

%公式求解 和上面一樣
eigs(A) %公式求解A特徵值 

%% 可控可觀性分析
qc=[B A*B A^2*B A^3*B];
rank(qc)%可控性矩陣求秩

gc=[C C*A C*A^2 C*A^3]';
rank(gc)%可觀性矩陣求秩;

%%

交流羣

感興趣同學可加機械&自動化攻城獅交流羣（羣號：1105076200）

參考文獻

[1] 倒立擺狀態反饋控制——分析、建模與仿真(matlab)
[2] 《現代控制工程》第五版盧伯英譯
[3] 《Matlab/Simulink 機電系統建模與仿真》宋志安
[4]王強. 直線倒立擺的起擺和穩擺智能控制研究[D].天津理工大學,2013.

【基於Simulink+UG NX MCD 一級倒立擺控制系統仿真】建模和分析(一)

前言

建立數學模型

Step 0：問題描述

Step 1：採用牛頓力學定律進行建模

Step 2：傳遞函數

使用matlab構造傳遞函數

關於狀態空間模型

Step 3：狀態空間模型

傳遞矩陣

Step 4：模型分析

穩定性

可控性

可觀性

Step 5： Simulink模型

代碼分享

交流羣

參考文獻

【CNC——第12篇】用Turbo PMAC驅動一個松下交流伺服電機以及I變量知識

【運動控制】梯形加減速三角加減速柔性S型加減速曲線的認識

【UG NX MCD 機電一體化概念設計】NX MCD中加減速控制算法的實現

【Plant Simulation】GA遺傳算法

【CNC——第6篇】PMAC上位機編程基礎篇(上位機和下位機如何通信)

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結

【基於Simulink+UG NX MCD 一級倒立擺控制系統仿真】建模和分析(一)

前言

建立數學模型

Step 0： 問題描述

Step 1： 採用牛頓力學定律進行建模

Step 2： 傳遞函數

使用matlab構造傳遞函數

關於狀態空間模型

Step 3： 狀態空間模型

傳遞矩陣

Step 4：模型分析

穩定性

可控性

可觀性

Step 5： Simulink模型

代碼分享

交流羣

參考文獻

Step 0：問題描述

Step 1：採用牛頓力學定律進行建模

Step 2：傳遞函數

Step 3：狀態空間模型