終於結束了自己的畢設設計，這半年的時間一直在和剛體動力學仿真硬磕，關於這方面的網絡資源不太多，因此在這里將自己不斷摸索學到的方法做一個概括闡述，希望能夠為同樣飽受困擾的讀者提供一些幫助。
程序開源地址：剛體動力學仿真示例程序
〇、問題闡述
一般情況下，剛體系統具備這樣形式的動力學方程：
其中，\(q\) 為系統廣義坐標，\(D(q)\) 為慣性矩陣，\(C(q,\dot{q})\) 為離心力及科里奧利力項，\(G(q)\) 為重力項，\(U\) 為廣義力項。
為方便闡述，這里以 一階旋轉倒立擺 這一簡單多剛體系統為研究對象，探討該系統的動力學仿真問題。參考文獻[1]，在不考慮系統摩擦等不確定因素干擾情況下，給出各項具體形式為：
其中，廣義坐標 \(q = [\theta_0,\theta_1]^T\)，\(J_0\) 為旋臂繞電機轉軸的轉動慣量，\(L_0\) 為旋臂總長，\(m_1\) 為擺桿質量，\(l_1\) 為擺臂從其質心到與旋臂轉動關節間的長度，\(J_1\) 為擺桿繞其質心的轉動慣量，\(\theta_0\) 為旋臂轉動角度，\(\theta_1\) 為擺桿作順時針運動時與垂直線間的夾角大小，\(\tau\) 為輸入力矩。
本質上該系統的這一仿真問題就是其動力學方程（常微分方程）的求解問題。借由 MATLAB 可以有如下幾種解決思路：
- （1）ODE 求解器
- 純文本編程，方便快速開發，十分適用於與其他算法（參數尋優、復雜控制算法）結合使用；
- 在一些奇異點會出現難以求解的情況，諸如一階旋轉倒立擺系統在直立位置 \([\theta_0,\theta_1]^T = [0°,0°]^T\)；
- 未找到支持求解過程進行中斷的功能，因此針對以系統狀態變量作派生的函數諸如系統能量、李雅普諾夫函數等，需要在求解過程完全結束后二次計算，不夠優美；
- （2）Simulink 初級使用 —— 基礎模塊
- 利用 Simulink 中諸如矩陣單元、微積分單元、加減乘除單元等基礎單元進行動力學方程的表述
- 工程量很大，不符合快速開發這一重要前提
- （3）Simulink 進階使用 —— S-function 模塊
- 文本編程與圖形化編程結合，功能實現支持 ｍ語言、C語言等常見編程語言，以模塊形式在 Simulink 中被調用；
- 靈活度不夠高，基本上是按照固定模板進行程序編寫，對於一些內部參數（諸如物理參數、系統能量等）沒辦法直接傳遞到模塊外（只能傳遞選定的系統狀態變量）；
- 當結合硬件在環技術（例如 QUARC）時編譯工作很麻煩；
- （4）Simulink 高階使用 —— Simscape Multibody 工具箱
- 三維仿真，完成實體、關節及變換坐標系之后，系統能自行進行動力學求解，省去了動力學方程的直接表述工作；
- 基於物理模型的動力學建模，為與物理系統保持盡量一致的運動特性，往往需要精確的三維模型參數（長度、質量、慣性系數等）；
- 沒有碰撞箱
- 手動裝配比較麻煩，特別是各個坐標系之間的轉換很繁瑣；安裝相應插件后支持從SolidWorks 導出的裝配體模型直接導入Simscape Multibody中，並自動生成相應的模型文件，但是生成算法不夠智能，對於復雜模型會存在一些怪異的裝配方式。
一、方法 ①：ODE 求解器
ODE 求解器實際是 MATLAB 提供的一系列常微分方程（Ordinary Differential Equation）的求解函數集合，根據不同求解場景，有 ode45、ode23、ode113等8種基本求解器（詳見 選擇 ODE 求解器 -- MathWorks 中國）。對於多剛體系統，一般選擇以基於顯式 Runge-Kutta (4,5) 公式的 ode45 求解器即可。該求解器的基本函數形式及各參數解釋為：
[t,x] = ode45(odefun,tspan,x0,options)，
odefun：待積分函數的句柄。亦即描述微分方程 \(\dot{x} = f(t,x)\) 的函數的句柄，函數基本形式為 dx = odefun(t,x），其中odefun亦即 \(f\)；
tspan：積分區間。亦即待求解問題的總時間跨度；
y0：變量初始狀態。亦即待求解狀態變量的初始值；
options：一些額外的求解條件構成的結構體，包含求解精度、求解方法等。
對於一階旋轉倒立擺系統，可選取系統狀態變量 \(x = [\theta_0,\dot{\theta}_0,\theta_1,\dot{\theta}_1]^T\)，而 odefun要求返回值 \(\dot{x} =[\dot{\theta}_0,\ddot{\theta}_0,\dot{\theta}_1,\ddot{\theta}_1]^T\)，必須借助系統動力學方程作為轉換手段，亦即 \(\ddot{q}=[\ddot{\theta}_0,\ddot{\theta}_1]^T = D^{-1}(U-C·\dot{q}-G)\)，因此整個求解流程可總結偽代碼為：
function main：
① 定義初始條件（包含求解時長，系統初始狀態，額外求解條件等）
② [t,x] = ode45(odefun,tspan,x0,options);
end
function dx = odefun(t,x):
① 物理參數初始化（質量、長度、慣性系數等）
② 各系數矩陣定義（D、C、G、U）
③ dq = [x(2);x(4)]
④ ddq = inv(D)*(U-C*dq-G) % 動力學方程
⑤ dx = [dq(1);ddq(1);dq(2);ddq(2)]
end
針對一階旋轉倒立擺系統在擺桿處於水平位置\([\theta_0,\dot{\theta}_0,\theta_1,\dot{\theta}_1]^T=[0°,0,90°,0]\)的零輸入響應過程，仿照上述偽代碼可撰寫核心程序如下：
%% ode 初始化
T_stop=10;% 仿真時長 s
T_sample = 0.002;% 采樣周期 s
x0 = [0;0;1/2*pi;0];% 初始狀態 [q0;dq0;q1;dq1]; 期望狀態 x_f = [0;0;0;0];
options=odeset('RelTol',1.0e-6,'AbsTol',1.0e-6,'BDF','on');%設置仿真精度,Backward Differentiation Formulas (BDFs) instead of the default Numerical Differentiation Formulas (NDFs).
%% ode 求解
[t,x]=ode45(@(t,x) RIP_Sysm(t,x),[0:T_sample:T_stop],x0,options); % ode45解常微分方程
%% 繪圖
figure(1)
plot(t,x(:,1));title('時間-旋臂角度');
xlabel('t[s]');ylabel('\theta_0[rad]')
figure(2)
plot(t,x(:,3));title('時間-擺桿角度');
xlabel('t[s]');ylabel('\theta_1[rad]')
%% 待積分函數
function dx = RIP_Sysm(t,x)
%% Constants
L_0 = 0.2159; %Total lenth of the arm
l_1 = 0.1556; % Distance from pivot to center of pendulum's mass
m_1 = 0.1270; % Mass of pendulum
J_0 = 0.0020; % Inertia of the arm about the rotation axis
J_1 = 0.0012; % Pendulum moment of intertia about center of mass
g = 9.80; % Acc of the gravity
%% D(q) 正定慣性矩陣
D_11 = J_0 + m_1*(L_0^2+l_1^2*sin(x(3))^2);
D_12 = m_1*l_1*L_0*cos(x(3));
D_21 = D_12;
D_22 = J_1 + m_1*l_1^2;
D=[D_11,D_12;D_21,D_22];
D_inv =inv(D);
% C(q,dq) 離心力和哥氏力
C_11 = 1/2*m_1*l_1^2*sin(2*x(3))*x(4);
C_12 = -m_1*l_1*L_0*sin(x(3))*x(4)+1/2*m_1*l_1^2*sin(2*x(3))*x(2);
C_21 = -1/2*m_1*l_1^2*sin(2*x(3))*x(2);
C_22 = 0;
C = [C_11,C_12;C_21,C_22];
% G(q) 重力
G = [0; -m_1*g*l_1*sin(x(3))];
U = [0;0];%零輸入響應
dq=[x(2);x(4)]; % 角度 x1的導數，x2的導數
ddq= D_inv*(U - C*dq - G ); % 動力學方程反解角加速度：D(q)*ddq + C*dq + G = U
dx = [dq(1);ddq(1);dq(2);ddq(2)];
end
旋臂及擺桿運動曲線結果如下，由於未引入摩擦阻尼等因素，因此系統作近似正弦運動（不完全是，擺桿一部分勢能轉換為旋臂動能）。
 |
 |
二、方法 ②：Simulink 初級使用 —— 基礎模塊
三、方法 ③：Simulink 進階使用 —— S-function 模塊
S-Function 是 Simulink 中一個很好的連接圖形化及文本編程的模塊工具，它能夠按仿真時間序列，依賴各項回調函數完成系統初始化、系統狀態求導及系統狀態輸出等工作[2] ，整個過程由該模塊自迭代完成。
具體說來，為滿足描述剛體動力學方程的需求，S-Function 需要定義：
主入口函數：用於根據仿真時間序列進行 S-Function 的狀態分流（類似於狀態機）。形如 rip_plant(t,x,u,flag)，其中 t 為仿真時間，x 為狀態變量，u 為控制輸入，flag 即為仿真環境自動提供的當前運行狀態標志；
初始化函數：用於對被描述系統的各變量進行初始化表述，包含連續（離散）狀態變量個數、輸入量個數、輸出量個數、采樣方式（連續 or 離散）及周期（定周期 or 變周期）等；
狀態求導函數：用於完成系統狀態變量的求導工作，即執行 \(\ddot{q}=[\ddot{\theta}_0,\ddot{\theta}_1]^T = D^{-1}(U-C·\dot{q}-G)\) 算式；
輸出函數：用於輸出系統狀態變量。
特別注意，針對一階旋轉倒立擺系統，為與 方法 ①：ODE 求解器 結果保持一致，需要設置求解器為定步長且補償為 0.002s（變步長時，有時步長過大容易產生漂移）。
運行仿真模型，其結果與方法 ① 的結果一致：
四、方法 ④：Simulink 高階使用 —— Simscape Multibody 工具箱
Simscape Multibody 提供了適用於諸如機器人、汽車懸架、建築設備等的 3D 機械系統多體仿真環境。借助表示剛體、關節、約束、力元件和傳感器的模塊，可以實現對多體系統的三維建模。Simscape Multibody 會根據使用者所創建的機械模型自動建立起整個機械系統的運動方程並進行求解。對於一些復雜的機械系統，借助一些插件工具可將完整的 CAD 裝配件（包括質量、慣性、關節、約束和 3D 幾何結構）導入到仿真模型中·，極大地簡化了復雜模型的裝配工作。
- 工具箱直接在 MATLAB 視窗主頁菜單欄的“附加功能”搜索安裝即可
- 關於將 SolidWorks 的裝配件直接導入到 Simscape Multibody 的方法可參考：
Matlab模型可視化仿真:SimMechanics Link的安裝與使用-知乎-作者：llllq
對於一階旋轉倒立擺系統，由於整個系統機械結構比較簡單，因此直接利用該工具箱中的 Solid 模塊、Joint 模塊以及 Tuansfiorm 模塊等簡單模塊作手動裝配即可表述其三維結構。
通過運行該仿真模型，可以在 MATLAB 的 Mechanics Explorer 視窗觀察到系統作自由響應的三維動畫，關節角度及角速度可由示波器 Scope 進行觀察：
 |
 |
五、注意事項
- （1）關於 ode45 如何設置定步長問題（求解非剛性微分方程 - 中階方法 - MathWorks）
- 答：一般來說 ode45 函數的變步長調用方式為
[t,x] = ode45(odefun,tspan = [0,Tf],x0,options)，注意到此時tspan = [0,Ts]，返回值[t,x]為變步長求解時刻及對應時刻的系統狀態變量組成的集合；從原理上來說，ode45 本身求解過程是變步長這一事實不能更改，但是該函數提供了一種插值的方式以使得使用者可以獲取到指定時刻的系統狀態，此時該函數的調用方式為[t,x] = ode45(odefun,tspan = [0:Ts:Tf],x0,options)，注意到此時tspan = [0:Ts:Tf]，Ts即為采樣周期。
- （2）程序開源地址：