MATLAB实现六轴机械臂的机器人动力学

---

1. 质点动力学

质点动力学有两类基本问题：一是已知作用于质点上的力，求质点的运动，这是正动力学；二是已知质点的运动，求作用于质点上的力，这个属于逆动力学。

求解正向动力学问题：需要求解质点运动微分方程或求积分；
求解逆向动力学问题：对质点的运动方程取二阶导数，得到质点的加速度，代入牛顿第二定律，即可求得力。

2. 动力学参数

以UR5机械臂为例，查看它的动力学参数（需要先安装Peter Cork的机器人工具箱，可参考官网和这篇知乎教程）

>> startup_rtb;
>> mdl_ur5;
>> ur5.dyn;

结果显示出了六个关节的动力学参数，我们也可以查看单个关节的参数，比如第二个关节：ur5.dyn(2)或者ur5.links(2).dyn具体如图所示

还可能有其他动力学参数，比如puma560有对称惯性矩阵，关节极限范围等参数，如下图所示

• I：3x3的对称惯性矩阵，是刚体在主轴或其他轴上的转动惯量
• Jm：电机惯性
• qlim：关节转动的最大范围
• 忽略库仑摩擦力的情况ur5.nofriction().dyn，这样Tc都为0

获取这些属性的方法:

>> ur5.link(2).m
>> ur5.link(1).G

3. 正向运动学

根据受力，求出关节角度，角速度，角加速度

>> p560=p560.nofriction( );
>> [T,q,qd]=p560.fdyn(10,[],qz);
>> %或者
>> torqfun = [1 2 3 4 5 6]; 
>> [T,q,qd]=p560.fdyn(10,[],torqfun);

函数说明 fdyn( )

SerialLink/fdyn Integrate forward dynamics

[T,Q,QD] = R.fdyn(TMAX, FTFUN)integrates the dynamics of the robot over the time interval 0 to TMAX and returns vectors of time T (Kx1), joint position Q (KxN) and joint velocity QD (KxN). The initial joint position and velocity are zero. The torque applied to the joints is computed by the user-supplied control function FTFUN:TAU = FTFUN(ROBOT, T, Q, QD) where TAU (1xN) is the joint torques to be applied at this time instant,ROBOT is the robot object being simulated, T is the current time, and Q (1xN) and QD (1xN) are the manipulator joint coordinate and velocity state respectively.
[TI,Q,QD] = R.fdyn(T, FTFUN, Q0, QD0) as above but allows the initial joint position Q0 (1xN) and velocity QD0 (1x) to be specified.

由此产生的运动，我们将运动速度变化曲线绘制出来

set(gcf,'Position',[10,10, 800, 1000]);
subplot(6,1,1); plot(T,qd(:,1)); xlabel('Time (s)'); ylabel('Joint 1 (rad)')
subplot(6,1,2); plot(T,qd(:,2)); xlabel('Time (s)'); ylabel('Joint 2 (rad)')
subplot(6,1,3); plot(T,qd(:,3)); xlabel('Time (s)'); ylabel('Joint 3 (rad)')
subplot(6,1,4); plot(T,qd(:,4)); xlabel('Time (s)'); ylabel('Joint 4 (rad)')
subplot(6,1,5); plot(T,qd(:,5)); xlabel('Time (s)'); ylabel('Joint 5 (rad)')
subplot(6,1,6); plot(T,qd(:,6)); xlabel('Time (s)'); ylabel('Joint 6 (rad)')

如下图

或者这样画图，更直观（选1~4关节的关节角度）

figure('Position', [10, 10, 800, 1000]);
plot(T,qd(:,1:4));
legend('q1','q2','q3','q4');
text(0.9,0.28,'\leftarrow q1');
grid on

4. 逆动力学

根据物体所要达到的一定运动状态，求解作用在物体上的力，也就是已知角度、角速度、角加速度等值，求解各关节所需提供的力。

q1 = qn;%关节位姿
qd = [5 1 0 0 0 0];  %关节角速度
qdd = [0 0 0 0 0 0]; %关节角加速度
TAU = p560.rne(q1,qd,qdd);%求解逆向动力学
TAU =

   22.1585   56.8861   -3.3693   -0.0018    0.0283    0.0002

函数说明 rne( ):

SerialLink/rne Inverse dynamics

TAU = R.rne(Q, QD, QDD, OPTIONS) is the joint torque required for the robot R to achieve the specified joint position Q (1xN), velocity QD(1xN) and acceleration QDD (1xN), where N is the number of robot joints.
TAU = R.rne(X, OPTIONS) as above where X=[Q,QD,QDD] (1x3N).
[TAU,WBASE] = R.rne(X, GRAV, FEXT) as above but the extra output is the wrench on the base.

Options:
‘gravity’,G specify gravity acceleration (default [0,0,9.81])
‘fext’,W specify wrench acting on the end-effector W=[Fx Fy Fz Mx My Mz]
‘slow’ do not use MEX file

Reference：MATLAB六轴机械臂机器人的动力学