本文讨论串联机械臂的动力学与控制问题，在加入力矩的情况下控制机器人以期望的位姿运动。以标准的puma560为例做前馈力矩控制（robotics toolbox for matlab 10.3 工具箱中的puma560对象，simulink模型，sl_fforward）。考虑驱动串联机械臂的单个电机，其受力情况可以用一组矩阵形式的耦合微分方程表示。

其中，q,qd,qdd 分别为广义的关节位置、速度和加速度的向量，H是关节空间惯性矩阵，C是科氏力和向心力耦合矩阵，G是重力负荷，F为摩擦力，Q是广义坐标，最后一项是施加在末端执行器上的力旋量所产生的关节力。在给定关节角度、关节速度和加速度的情况下，转动惯量和耦合力矩都能够由该微分方程计算出来。
然而在整个串联机械臂的控制中，采用独立关节控制具有局限性，惯性变化和动态耦合力没有明确的处理方法。采用前馈控制加上计算力矩控制两种方法相结合，前馈控制项能够补偿由于惯性参数的不确定性、未建模的力或外部干扰引起的任何误差。力矩前馈控制表达式由下式给出(此式中，直接对位置误差和速度误差乘以比例系数作为反馈项与力矩合成在一起，而没有对误差进行对应于物理意义上处理)：

图a为前馈控制，根据期望机械臂状态计算所需的关节力；图b为计算力矩控制，其原理是：非线性系统通过他的逆被级联，从而使整个系统具有恒定的单位增益。由于非线性系统求逆进行前馈力矩控制是不完美的，加入反馈回路来处理误差能够达到更好的效果。

本文对应的matlab工具箱环境为，matlab2018b环境下Robotics Toolbox for MATLAB 10.3, 首先创建SerialLink对象：

>>mdl_puma560

载入力矩前馈控制器模型

>>sl_fforward

其中，jtraj模块在初始化过程中，即生成期望位姿序列，通过jtraj函数实现:

t=[0:100]'/100*tmax;            %仿真时间步，无因子时间序列乘以时间长度
[q,qd,qdd] = jtraj(q0, qf, t);

NF/Puma560是一个去除库仑摩擦力的puma560机器人对象，封装里你动力学模型：

p560.nofriction()

模块初始化，输出机器人对象的自由度

if isa(robot, 'SerialLink')
    n = robot.n
    rname = robot.name;
else
    n = 0;
    rname = '';
end

封装内容中，使用rne牛顿欧拉法求解逆动力学，输入对象[q, qd, qdd]为3x6的矩阵，输出torque

rne(robot, u(1:n)', u(n+1:2*n)', u(2*n+1:3*n)')

使用Zero-Order Hold模块控制采样时间，前馈力矩计算的采样时间与反馈采样时间可以不同。

输出关节角度监视结果如下：

关节角度误差监视结果如下：

对于惯性变化和动态耦合力矩的处理，采用计算力矩前馈控制能够显著提升控制性能，在前馈控制的基础上加入位置环和速度环的反馈控制项，使整个控制系统达到更好的控制效果。

references：

1. Robotics, Vision and Control ——Fundamental Algorithms in Matlab, Peter Corke