我的毕设题目定为《基于机械臂触觉伺服的物体操控研究》，这个系列主要用于记录做毕设的过程。

前言：UR系列是优傲公司的代表产品，也是目前比较通用的产品级机械臂。所以我打算用该机械臂进行毕设的仿真实现。关于其动力学建模，网上有很多参考的文献，但由于六自由度的完整动力学模型很复杂，验证和移植都比较麻烦，所以我参考了以下的论文，对动力学模型进行了简化，并且在仿真环境中进行了验证。
基于UR5机械臂的轨迹跟踪控制算法研究

1. 模型简化
UR5e一共有6个驱动关节，其中前三个关节主要用于控制末端的位置，后三个关节主要用于控制末端的姿态。
因为后三个连杆较短，并且质量较小，动力学参数值较小，因此将其简化为质点，位置在第三个驱动关节对应连杆的末端。
对于前三个关节，认为其质量沿杆均匀分布，并将其简化为两个模型，一个是只有关节1的单连杆模型，一个是有关节2和关节3的双连杆模型。
注：以下模型中的c 1 
​
 表示c o s ( θ 1 )s 12 
​
 表示s i n ( θ 1 + θ 2 ) 

关节1的单连杆模型

关节2和关节3的双连杆模型

其中，m e 
​
 为后三个连杆的质量之和。
m e = m 4 + m 5 + m 6

2. 动力学建模

因为对于简化后的模型，连杆的动能和势能都比较容易求取，所以我采用了拉格朗日法进行动力学建模。

单连杆模型

双连杆模型

创建动力学模型
将上述式子代入到拉格朗日公式中，即可求得

合并结果

代码实现

template <typename T>
void Dynamics(const Vec6<T>& q, const Vec6<T>& qd, const Vec6<T>& qdd, Vec6<T>* tau){
    /* define params */
    T m1 = _upperarmLinkMass;
    T m2 = _forearmLinkMass;
    T l1 = _upperarmLinkLength;
    T l2 = _forearmLinkLength;
    T t1 = M_PI_2 - q(1);
    T t2 = -q(2);

    /* ------------------------------------ double link model -------------------------------------------- */
    Mat2<T> M;
    M(0,0) = m2*pow(l1,2) + (1./3.)*m1*pow(l1,2) + (1./3.)*m2*pow(l2,2) + m2*l1*l2*cos(t2);
    M(0,1) = (1./3.)*m2*pow(l2,2) + (1./2.)*m2*l1*l2*cos(t2);
    M(1,0) = (1./3.)*m2*pow(l2,2) + (1./2.)*m2*l1*l2*cos(t2);
    M(1,1) = (1./3.)*m2*pow(l2,2);

    Mat2<T> A = Mat2<T>::Zero();
    A(0,1) = -(1./2.)*m2*l1*l2*sin(t2);
    A(1,0) = (1./2.)*m2*l1*l2*sin(t2);

    Mat2<T> B = Mat2<T>::Zero();
    B(0,0) = -m2*l1*l2*sin(t2);

    Vec2<T> G;
    T g = 9.81;
    G(0) = ((1./2.)*m1 + m2)*g*l1*cos(t1) + (1./2.)*m2*g*l2*cos(t1+t2);
    G(1) = (1./2.)*m2*g*l2*cos(t1+t2);

    /* consider tail mass */
    T me = *links_mass[3] + *links_mass[4] + *links_mass[5];

    /* adjustment coefficient */
    M(0,0) += me*pow(l1,2) + me*pow(l2,2) + 2*l1*l2*me*cos(t2);
    M(0,1) += me*pow(l2,2) + l1*l2*me*cos(t2);
    M(1,0) += me*pow(l2,2) + l1*l2*me*cos(t2);
    M(1,1) += me*pow(l2,2);

    A(0,1) += -me*l1*l2*sin(t2);
    A(1,0) += me*l1*l2*sin(t2);

    B(0,0) += -2*me*l1*l2*sin(t2);

    G(0) += me*g*l1*cos(t1) + me*g*l2*cos(t1+t2);
    G(1) += me*g*l2*cos(t1+t2);

    /* compute tau */
    Vec2<T> qd_2(pow(qd[1],2), pow(qd[2],2));
    Vec2<T> qd_mix(qd[1]*qd[2], qd[1]*qd[2]);
    Vec2<T> _qdd(qdd[1], qdd[2]);

    Vec2<T> V = A*qd_2 + B*qd_mix;
    Vec2<T> _tau = M*_qdd + V + G;
    tau->operator()(1) = -_tau(0);
    tau->operator()(2) = -_tau(1);

    /* ------------------------------------ sigle link model -------------------------------------------- */
    T _l1 = l1*cos(t1);
    T _l2 = l2*cos(t2);

    T A_0 = (1./3.)*m1*pow(_l1,2) + (1./3.)*m2*pow(_l2,2) + m2*pow(_l1,2) + m2*_l1*_l2;

    /* consider tail mass */
    A_0 += me*pow(_l1+_l2,2);

    /* compute tau */
    T tau_0 = A_0*qdd[0];
    tau->operator()(0) = tau_0;

    /* no control data for other three joint */
    tau->operator()(3) = 0;
    tau->operator()(4) = 0;
    tau->operator()(5) = 0;
}

3. 计算雅克比矩阵

雅克比矩阵同样是针对于上面简化后的模型进行求取。
运动学关系

代码实现

template <typename T>
void computeJacobian(const Vec6<T>& q, Mat6<T>* J){
    T l2 = _upperarmLinkLength;
    T l3 = _forearmLinkLength;
    T t1 = q(0);
    T t2 = M_PI_2 - q(1);
    T t3 = -q(2);
    Mat6<T> J_ = Mat6<T>::Zero();
    Mat3<T> J_block;
    J_block(0, 0) = -l2*sin(t1)*cos(t2) - l3*sin(t1)*cos(t2+t3);
    J_block(0, 1) = -l2*cos(t1)*sin(t2) - l3*cos(t1)*sin(t2+t3);
    J_block(0, 2) = -l3*cos(t1)*sin(t2+t3);
    J_block(1, 0) = l2*cos(t1)*cos(t2) + l3*cos(t1)*cos(t2+t3);
    J_block(1, 1) = -l2*sin(t1)*sin(t2)  - l3*sin(t1)*sin(t2+t3);;
    J_block(1, 2) = -l3*sin(t1)*sin(t2+t3);    
    J_block(2, 0) = 0;
    J_block(2, 1) = l2*cos(t2) + l3*cos(t2+t3);
    J_block(2, 2) = l3*cos(t2+t3);     

    /* convert to model */
    Mat3<T> model_convert = Mat3<T>::Identity();
    model_convert(1,1) = -1;
    model_convert(2,2) = -1;
    J_block = J_block * model_convert;
    J_.block(0, 0, 3, 3) = J_block;
    *J = J_;
}

4. 优化

添加控制器
因为上述模型将末端等效为了一个节点，在仿真的时候，控制轨迹与理想轨迹是会存在有一定误差的，所以可以添加一个笛卡尔坐标系下的PID控制器，减小误差。

Kp：3×3阶对角矩阵，对角元素为各个轴的PID比例系数。
Kd：3×3阶对角矩阵，对角元素为各个轴的PID微分系数。
PD：3×1阶矩阵，末端目标位置。
V D3×1阶矩阵，末端目标速度。
p 3×1阶矩阵，末端当前位置。
v ×1阶矩阵，末端当前速度。
τ 3×1阶矩阵，上述动力学输出。

修改建模方向
在本文章建立的动力学模型中，关节2和关节3的转动的方向与上一篇运动学模型的方向相反。
【基于机械臂触觉伺服的物体操控研究】UR5e运动学建模及代码实现
这样子会导致实际应用时，需要修改部分数据的正负以适配模型。
所以读者在建立自己的动力学模型时，可以注意这个点，保证动力学，运动学模型，以及实际物理模型的坐标系方向都一一对应。