關於在 matlab 中使用 ode45 算出拉格朗日方程中的關節加速度

1. 引言

注意：這裏討論二自由度的機械臂。

在機械臂動力學中，有時候會用到ode45計算朗格朗日方程中的關節空間軌跡，關節速度，即求出方程中的q與$\dot{q}$：
$\tau = M(\dot{q})\ddot{q} + B(q,\dot{q})\dot{q} + G(q)$

2. 方法

1. mainFun.m主函數片段

xk = [q(1);q(2);d_q(1);d_q(2)];  % 分別是 關節空間和關節轉速 
[t,x] = ode45('subFun',tspan,x0);
xk = x(length(x),:);
q = [xk(1);xk(2)];
d_q = [xk(3);xk(4)];

2. subFun.m子函數

function dx = subFun(tspan,x0)
dx = zeros(4,1);
dx = [x(3);x(4);inv(M)*(tol-B*[x(3);x(4)]-G)];
end

3. 如何寫子函數(對subFun.m子函數的解釋)

約定：$\left\{\begin{matrix} &dx = \dot{x}\\ & ddx = \ddot{x} \end{matrix}\right.$
則：
由方程：
$\tau = M(\dot{q})\ddot{q} + B(q,\dot{q})\dot{q} + G(q)$
得：
$\ddot{q} = inv(M(\dot{q}))*(\tau - B(q,\dot{q})\dot{q} - G(q)) \tag{1-1}$
令；
$x= \begin{bmatrix} x(1) \\ x(2) \end{bmatrix}= \begin{bmatrix} q \\ \dot{q} \end{bmatrix} \tag{1-2}$
求導，得：
$dx= \begin{bmatrix} dx(1) \\ d x(2) \end{bmatrix}= \begin{bmatrix} \dot{q} \\ \ddot{q} \end{bmatrix}= \begin{bmatrix} x(2) \\ \ddot{q} \end{bmatrix} \tag{1-3}$
將式（1-1）帶入式（1-3），得：
$dx= \begin{bmatrix} x(2) \\ inv(M)*(\tau - B*x(2) - G) \end{bmatrix} \tag{1-4}$
又因爲是二自由度機械臂，故：
$x= \begin{bmatrix} x(1) \\ x(2) \end{bmatrix}= \begin{bmatrix} q \\ \dot{q} \end{bmatrix}= \begin{bmatrix} q_1 \\ q_2 \\ \dot{q_1}\\ \dot{q_2} \end{bmatrix}= \begin{bmatrix} x(1) \\ x(2)\\ x(3)\\ x(4) \end{bmatrix} \tag{1-5}$
將式（1-5）帶入式（1-4）可得：
$dx_{4\times 1}= \begin{bmatrix} x(3) \\ x(4) \\ inv(M)*(\tau - B*[x(3);x(4)] - G) \end{bmatrix}= \begin{bmatrix} \dot{q_1} \\ \dot{q_2} \\ \ddot{q_1}\\ \ddot{q_2} \end{bmatrix}$
以上，則是子函數的代碼邏輯。

4. 參考文章

1.matlab求解常微分方程（組）—dsolve、ode系列函數詳解（含例程）
2. MATLAB / SIMULINK通信系統建模與仿真實例分析
3. ode45官方文檔