动力学

1、动力学方程

其中：在关节空间中， [公式] 分别表示关节处角度、角速度、角加速度； [公式] 表示关节力距， [公式]为nxn的质量矩阵， [公式] 是nx1的离心力和科氏力矢量； [公式] 是nx1的重力矢量。笛卡尔空间与关节空间类似。（注，上诉动力学方程只包含了刚体力学中的那些力，并未考虑摩擦力）

在机器人工具箱中，我们有以下函数求解上诉一些相关矩阵

  • R.gravload（） %重力载荷
  • R.inertia（） %关节空间惯性矩阵
  • R.coriolis（） %有效载荷（作用 在末端执行器处）的耦合矩阵
  • R.payload（） %有效载荷（作用在末端执行器处）

例：

mdl_puma560  %调用puma560模型
q = qn                       %设置关节角度
qd = [0.2,0.1,0,0.1,0,0]     %设置一个初始速度
G = p560.gravload(q)         %重力矢量
M = p560.inertia(q)          %关节空间质量矩阵
V = p560.coriolis(q,qn)      %计算离心力和科氏力矢量
p560.payload(1,[0 0 0])      %添加有效载荷
M1 = p560.inertia(q)         %计算添加载荷后的质量矩阵

2、在搭建机器人模型是，每个连杆都有许多的动力学参数（质量、惯性距等等），我们可以利用dyn函数显示这些参数。

  • R.dyn( ) %显示模型中所有连杆的动力学属性
  • R.dyn( i ) %显示第i根连杆的属性参数，也相当于R.links(2).dyn

（m（关节质量），r（3*1的关节齿轮向量），I（3*3对称惯性矩阵），Jm（电机惯性），Bm（粘性摩擦），Tc（库仑摩擦），G（齿轮传动比），qlim（关节变量上下限））

mdl_puma560        %调用puma560模型
p560.dyn( );       %显示所有关节动力学参数
p560.dyn(1);       %显示第一个关节属性参数
p560.links(1).dyn  %显示第一个关节属性参数
  • R.rne（q，qd，qdd） %（角度/角速度/角加速度→力/力矩）
  • R.rne（q，qd，qdd，grav，fext） %grav：重力加速度; fext=[Fx Fy Fz MxMy Mz]
  • [T，q，qd]=R.fdyn（T，torqfun） %（力/力矩→角度/角速度/角加速度）
  • qdd=R.accel（q，qd，torqfun） %计算角加速度
mdl_puma560          %调用puma560模型
q = qn;              %设置初始关节角度
qd = [5 1 0 0 0 0];  %设置关节加速度
qdd = [0 0 0 0 0 0]; %设置关节角加速度
T = p560.rne(q,qd,qdd,[0 0 0])   %求解逆向动力学
torqfun=[1 2 3 4 5 6];           %设置初始力矩
p560=p560.nofriction( );         %没有摩擦的动力学模型
[T,q,qd]=p560.fdyn(1,torqfun);   %求解正向动力学
for kk=1:65
qdd(kk,:)=p560.accel(q(kk,:),qd(kk,:),torqfun);    %求解角加速度
end