方框图

伪代码

核心代码：

//本章的两个功能集中在计算力矩控制器的使用上//计算力矩控制//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 errEigen::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