方框图

伪代码

核心代码：

//本章的两个功能集中在计算力矩控制器的使用上
  //计算力矩控制
  //thetalist :关节位置n维矢量
  //dthetalist: 关节速率n维矢量
  // eint: 关节误差的时间积分的 n 维向量
  //g: 重力矢量
  //Mlist: 在home位置时连杆坐标系{i}相对于{i−1}位姿矩阵列表。
  //Glist: 连杆的空间惯量矩阵Gi列表
  //Slist:空间坐标系中关节的螺旋轴 Si
  //thetalistd :参考关节变量 θd 的 n 维向量。目标关节位置与当前关节位置非常接近。采用插值轨迹点。
  //dthetalistd: 参考关节速度的 n 维向量 ˙θd。
  //ddthetalistd: 参考关节加速度的 n 维向量 ¨θd。
  //Kp：反馈比例增益（每个关节相同）。
  //Ki：反馈积分增益（每个关节相同）。
  //Kd：反馈微分增益（每个关节相同）。
  //输出：taulist：当前瞬间 由计算力矩控制器 计算的关节力/扭矩的矢量。
  Eigen::VectorXd ComputedTorque(const Eigen::VectorXd& thetalist, const Eigen::VectorXd& dthetalist, const Eigen::VectorXd& eint,
    const Eigen::VectorXd& g, const std::vector<Eigen::MatrixXd>& Mlist, const std::vector<Eigen::MatrixXd>& Glist,
    const Eigen::MatrixXd& Slist, const Eigen::VectorXd& thetalistd, const Eigen::VectorXd& dthetalistd, const Eigen::VectorXd& ddthetalistd,
    double Kp, double Ki, double Kd) {


    Eigen::VectorXd e = thetalistd - thetalist;  //位置误差 position err
    Eigen::VectorXd tau_feedforward = MassMatrix(thetalist, Mlist, Glist, Slist)*(Kp*e + Ki * (eint + e) + Kd * (dthetalistd - dthetalist));//前馈计算力/力矩增量 


    Eigen::VectorXd Ftip = Eigen::VectorXd::Zero(6);//末端不受力
    Eigen::VectorXd tau_inversedyn = InverseDynamics(thetalist, dthetalist, ddthetalistd, g, Ftip, Mlist, Glist, Slist);//重力补偿 


    Eigen::VectorXd tau_computed = tau_feedforward + tau_inversedyn;//计算目标力矩
    return tau_computed;
  }

// 仿真控制
  /*
  thetalist：初始关节变量的n向量。
  dthetalist：初始关节速度的n向量。
  g：实际重力矢量 g。
  Ftipmat ：一个 N × 6 矩阵，其中每一行都是 Ftip(k∆t) 形式的向量。（如果没有尖端力，用户应输入零，将使用零矩阵）。
  Mlist：在初始位置相对于 {i-1} 的连杆坐标系 {i} 的实际列表。
  Glist：连杆的实际空间惯性矩阵Gi。
  Slist：空间坐标系中关节的螺旋轴 Si。
  thetamatd：来自参考轨迹的所需关节变量 θd 的 N × n 矩阵。第一行是初始所需的关节配置，第 N 行是最终所需的关节配置。每行之间的时间是 dt，如下所示。
  dthetamatd：所需关节速度 ˙θd 的 N × n 矩阵。
  ddthetamatd：所需关节加速度 ¨θd 的 N × n 矩阵。
  gtilde：重力矢量的（可能不正确的）模型。
  Mtildelist：连杆坐标系位置的（可能不正确的）模型。
  Gtildelist：连杆空间惯性矩阵的（可能不正确的）模型。
  Kp：反馈比例增益（每个关节相同）。
  Ki：反馈积分增益（每个关节相同）。
  Kd：反馈微分增益（每个关节相同）。
  dt：参考轨迹上各点之间的时间步长 Δt。
  intRes：此输入必须是大于或等于 1 的整数。intRes 是每个时间步长 Δt 期间的欧拉积分步数。较大的值会导致模拟速度变慢，但积分误差的累积会减少。
  输出：
  taumat ：控制器命令的关节力/力矩的 N × n 矩阵，其中每行 n 个力/力矩对应一个时间点。
  thetamat ：实际关节变量的 N × n 矩阵，与 目标关节变量thetamatd 进行比较。
  Plot：实际和期望的关节变量的绘图。
  */
  std::vector<Eigen::MatrixXd> SimulateControl(const Eigen::VectorXd& thetalist, const Eigen::VectorXd& dthetalist, const Eigen::VectorXd& g,
    const Eigen::MatrixXd& Ftipmat, const std::vector<Eigen::MatrixXd>& Mlist, const std::vector<Eigen::MatrixXd>& Glist,
    const Eigen::MatrixXd& Slist, const Eigen::MatrixXd& thetamatd, const Eigen::MatrixXd& dthetamatd, const Eigen::MatrixXd& ddthetamatd,
    const Eigen::VectorXd& gtilde, const std::vector<Eigen::MatrixXd>& Mtildelist, const std::vector<Eigen::MatrixXd>& Gtildelist,
    double Kp, double Ki, double Kd, double dt, int intRes) {
    Eigen::MatrixXd FtipmatT = Ftipmat.transpose();// NX6->6XN  Tip端施加的力     一列一个力
    Eigen::MatrixXd thetamatdT = thetamatd.transpose();//关节轨迹点 dt时刻  Nxn->nXN   一列一个 关节位置n维列向量
    Eigen::MatrixXd dthetamatdT = dthetamatd.transpose();//一列一个 关节速率n维列向量
    Eigen::MatrixXd ddthetamatdT = ddthetamatd.transpose();//一列一个 关节加速度n维列向量
    int m = thetamatdT.rows(); //自由度
    int n = thetamatdT.cols();//轨迹点数
    Eigen::VectorXd thetacurrent = thetalist;//当前关节位置
    Eigen::VectorXd dthetacurrent = dthetalist;//当前目标关节位置
    Eigen::VectorXd eint = Eigen::VectorXd::Zero(m);//累积关节误差  关节误差的时间积分的 m 维向量
    Eigen::MatrixXd taumatT = Eigen::MatrixXd::Zero(m, n);//计算力矩控制器计算的控制力
    Eigen::MatrixXd thetamatT = Eigen::MatrixXd::Zero(m, n);//实际关节位置矩阵，一列一个 关节位置dof=m维向量
    Eigen::VectorXd taulist;
    Eigen::VectorXd ddthetalist;
    for (int i = 0; i < n; ++i) {//遍历所有轨迹点
      taulist = ComputedTorque(thetacurrent, dthetacurrent, eint, gtilde, Mtildelist, Gtildelist, Slist, thetamatdT.col(i),
        dthetamatdT.col(i), ddthetamatdT.col(i), Kp, Ki, Kd);//计算关节控制力/力矩
      for (int j = 0; j < intRes; ++j) {
        ddthetalist = ForwardDynamics(thetacurrent, dthetacurrent, taulist, g, FtipmatT.col(i), Mlist, Glist, Slist);//计算实际关节加速度
        EulerStep(thetacurrent, dthetacurrent, ddthetalist, dt / intRes);//更新关节位置和关节速率
      }
      taumatT.col(i) = taulist;//记录轨迹点i处的实际控制力/力矩
      thetamatT.col(i) = thetacurrent;//记录轨迹点dt时刻的实际关节位置
      eint += dt * (thetamatdT.col(i) - thetacurrent);//更新关节误差的dt累积误差
    }
    std::vector<Eigen::MatrixXd> ControlTauTraj_ret;//控制力矩轨迹
    ControlTauTraj_ret.push_back(taumatT.transpose());//前n行 控制力矩
    ControlTauTraj_ret.push_back(thetamatT.transpose());//后n行 实际关节位置
    return ControlTauTraj_ret;
  }

本书中符号含义：

---

The End