关于在 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子函数的解释)

约定：$$\{\begin{array}{cc}& dx=\dot{x}\\ & ddx=\ddot{x}\end{array}$$
则：
由方程：
$$\tau =M(\dot{q})\ddot{q}+B(q,\dot{q})\dot{q}+G(q)$$
得：
$$\ddot{q}=inv(M(\dot{q}))\ast (\tau -B(q,\dot{q})\dot{q}-G(q))\text{\hspace{1em}(1-1)}$$
令；
$$x=\left[\begin{array}{c}x(1)\\ x(2)\end{array}\right]=\left[\begin{array}{c}q\\ \dot{q}\end{array}\right]\text{\hspace{1em}(1-2)}$$
求导，得：
$$dx=\left[\begin{array}{c}dx(1)\\ dx(2)\end{array}\right]=\left[\begin{array}{c}\dot{q}\\ \ddot{q}\end{array}\right]=\left[\begin{array}{c}x(2)\\ \ddot{q}\end{array}\right]\text{\hspace{1em}(1-3)}$$
将式（1-1）带入式（1-3），得：
$$dx=\left[\begin{array}{c}x(2)\\ inv(M)\ast (\tau -B\ast x(2)-G)\end{array}\right]\text{\hspace{1em}(1-4)}$$
又因为是二自由度机械臂，故：
$$x=\left[\begin{array}{c}x(1)\\ x(2)\end{array}\right]=\left[\begin{array}{c}q\\ \dot{q}\end{array}\right]=\left[\begin{array}{c}{q}_1\\ q_2\\ \dot{q_1}\\ \dot{q_2}\end{array}\right]=\left[\begin{array}{c}x(1)\\ x(2)\\ x(3)\\ x(4)\end{array}\right]\text{\hspace{1em}(1-5)}$$
将式（1-5）带入式（1-4）可得：
$$d{x}_{4\times 1}=\left[\begin{array}{c}x(3)\\ x(4)\\ inv(M)\ast (\tau -B\ast [x(3);x(4)]-G)\end{array}\right]=\left[\begin{array}{c}\dot{q_1}\\ \dot{q_2}\\ \ddot{q_1}\\ \ddot{q_2}\end{array}\right]$$
以上，则是子函数的代码逻辑。

4. 参考文章

1.matlab求解常微分方程（组）—dsolve、ode系列函数详解（含例程）
2. MATLAB / SIMULINK通信系统建模与仿真实例分析
3. ode45官方文档