1. 引言
注意:这里讨论二自由度的机械臂
。
在机械臂动力学中,有时候会用到ode45
计算朗格朗日方程中的关节空间轨迹,关节速度,即求出方程中的q与q˙:
τ=M(q˙)q¨+B(q,q˙)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
子函数的解释)
约定:{dx=x˙ddx=x¨
则:
由方程:
τ=M(q˙)q¨+B(q,q˙)q˙+G(q)
得:
q¨=inv(M(q˙))∗(τ−B(q,q˙)q˙−G(q))(1-1)
令;
x=[x(1)x(2)]=[qq˙](1-2)
求导,得:
dx=[dx(1)dx(2)]=[q˙q¨]=[x(2)q¨](1-3)
将式(1-1)带入式(1-3),得:
dx=[x(2)inv(M)∗(τ−B∗x(2)−G)](1-4)
又因为是二自由度机械臂,故:
x=[x(1)x(2)]=[qq˙]=⎣⎢⎢⎡q1q2q1˙q2˙⎦⎥⎥⎤=⎣⎢⎢⎡x(1)x(2)x(3)x(4)⎦⎥⎥⎤(1-5)
将式(1-5)带入式(1-4)可得:
dx4×1=⎣⎡x(3)x(4)inv(M)∗(τ−B∗[x(3);x(4)]−G)⎦⎤=⎣⎢⎢⎡q1˙q2˙q1¨q2¨⎦⎥⎥⎤
以上,则是子函数的代码逻辑。
4. 参考文章
1.matlab求解常微分方程(组)—dsolve、ode系列函数详解(含例程)
2. MATLAB / SIMULINK通信系统建模与仿真实例分析
3. ode45官方文档