libfranka--cartesian_impedance_control分析

cartesian_impedance_control.cpp

部分内容来自链接，我个人以初学者的角度对代码进行了更加细致的解释。

先上笛卡尔空间阻抗控制官方例程（核心部分）：

  // Compliance parameters
  const double translational_stiffness{150.0};
  const double rotational_stiffness{10.0};
  Eigen::MatrixXd stiffness(6, 6), damping(6, 6);
  stiffness.setZero();
  stiffness.topLeftCorner(3, 3) << translational_stiffness * Eigen::MatrixXd::Identity(3, 3);
  stiffness.bottomRightCorner(3, 3) << rotational_stiffness * Eigen::MatrixXd::Identity(3, 3);
  damping.setZero();
  damping.topLeftCorner(3, 3) << 2.0 * sqrt(translational_stiffness) *
                                     Eigen::MatrixXd::Identity(3, 3);
  damping.bottomRightCorner(3, 3) << 2.0 * sqrt(rotational_stiffness) *
  Eigen::MatrixXd::Identity(3, 3);
// connect to robot
franka::Robot robot(argv[1]);
setDefaultBehavior(robot);
// load the kinematics and dynamics model
franka::Model model = robot.loadModel();
franka::RobotState initial_state = robot.readOnce();
// equilibrium point is the initial position
Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
Eigen::Vector3d position_d(initial_transform.translation());
Eigen::Quaterniond orientation_d(initial_transform.linear());
// set collision behavior
robot.setCollisionBehavior({{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                            {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                            {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                            {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});
// define callback for the torque control loop
std::function<franka::Torques(const franka::RobotState&, franka::Duration)> impedance_control_callback = [&](const franka::RobotState& robot_state, franka::Duration /*duration*/) -> franka::Torques {
  // get state variables
  std::array<double, 7> coriolis_array = model.coriolis(robot_state);
  std::array<double, 42> jacobian_array =
      model.zeroJacobian(franka::Frame::kEndEffector, robot_state);
  // convert to Eigen
  Eigen::Map<const Eigen::Matrix<double, 7, 1>> coriolis(coriolis_array.data());
  Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
  Eigen::Map<const Eigen::Matrix<double, 7, 1>> q(robot_state.q.data());
  Eigen::Map<const Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());
  Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
  Eigen::Vector3d position(transform.translation());
  Eigen::Quaterniond orientation(transform.linear());
  // compute error to desired equilibrium pose
  // position error
  Eigen::Matrix<double, 6, 1> error;
  error.head(3) << position - position_d;
  // orientation error
  // "difference" quaternion
  if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) {
    orientation.coeffs() << -orientation.coeffs();
  }
  // "difference" quaternion
  Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d);
  error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
  // Transform to base frame
  error.tail(3) << -transform.linear() * error.tail(3);
  // compute control
  Eigen::VectorXd tau_task(7), tau_d(7);
  // Spring damper system with damping ratio=1
  tau_task << jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq));
  tau_d << tau_task + coriolis;
  std::array<double, 7> tau_d_array{};
  Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
  return tau_d_array;
};

我们依然拆分开解析：

• 这个程序实现了什么功能?——固定末端位姿的阻抗控制，即机器人末端模拟一个弹簧阻尼机构。
• 理论上是如何实现的？

$${\mathbf{\tau }}_d={\mathbf{J}}^T(\mathbf{q})(-\mathbf{\alpha }\cdot \mathbf{e}-\mathbf{\beta }\cdot \mathbf{J}(\mathbf{q})\dot{\mathbf{q}})+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})$$

• 其中$\mathbf{\alpha }$、$\mathbf{\beta }$分别为刚度（弹簧）和阻尼，$\mathbf{e}$是位姿偏差。科氏力$\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})$比较小，影响不大。注意机器人本身自带重力补偿。
• 本例中雅可比矩阵$J$和科氏力均是通过libfranka提供的franka::Model类获取，注意构造函数输入可以是franka::RobotState结构体。
• 这里多说一点偏差$\mathbf{e}$，这个偏差是位置与姿态的混合偏差（$\mathbf{e}\in {\mathbb{R}}^6$），此处位置偏差用欧式距离，姿态偏差用四元数虚部之差。
  理清上述问题后，再看程序就没有那么复杂。

1. 首先定义刚度阻尼参数：

此处刚度$\mathbf{\alpha }\in {\mathbb{R}}^{6\times 6}$ ，阻尼$\mathbf{\beta }\in {\mathbb{R}}^{6\times 6}$ ，且左上角$3\times 3$ 区块为位置刚度/阻尼，右下角$3\times 3$区块为姿态刚度/阻尼。这里设定每个维度上阻尼相同。

$$\alpha =\beta =\left[\begin{array}{cccccc}translation& 0& 0& 0& 0& 0\\ 0& translation& 0& 0& 0& 0\\ 0& 0& translation& 0& 0& 0\\ 0& 0& 0& rotation& 0& 0\\ 0& 0& 0& 0& rotation& 0\\ 0& 0& 0& 0& 0& rotation\end{array}\right]$$

  const double translational_stiffness{150.0}; // 位置刚度或阻尼
  const double rotational_stiffness{10.0}; // 姿态刚度或阻尼
  // 刚度矩阵、阻尼矩阵
  Eigen::MatrixXd stiffness(6, 6), damping(6, 6);
  
  stiffness.setZero(); // 刚度矩阵初始化为0
  stiffness.topLeftCorner(3, 3) << translational_stiffness * Eigen::MatrixXd::Identity(3, 3); // 左上角3X3区块为位置刚度
  stiffness.bottomRightCorner(3, 3) << rotational_stiffness * Eigen::MatrixXd::Identity(3, 3); // 右下角3X3区块为姿态刚度

  damping.setZero(); // 阻尼矩阵初始化为0
  damping.topLeftCorner(3, 3) << 2.0 * sqrt(translational_stiffness) * Eigen::MatrixXd::Identity(3, 3); // 左上角3X3区块为位置阻尼
  damping.bottomRightCorner(3, 3) << 2.0 * sqrt(rotational_stiffness) * Eigen::MatrixXd::Identity(3, 3); // 右下角3X3区块为姿态阻尼

2. 加载机器人运动学与动力学模型，并读取机器人当前状态：

    // 加载机器人运动学与动力学模型
    franka::Model model = robot.loadModel();
    // 获取当前机器人状态信息
    franka::RobotState initial_state = robot.readOnce();

设置机器人最后平衡点为当前初始位姿。对当前末端执行器的笛卡尔位置、姿态进行提取。
我们知道齐次变换矩阵${}_O^{EE}T$中包含了关节坐标相对于基座标的平移和旋转，在数学中这种矩阵变换被称为仿射变换，即在三维空间中仿射变换 = 线性变换 + 平移 = 齐次变换矩阵。其中线性变换包含旋转、缩放、推移，线性变换不移动其坐标原点。注意姿态由四元数表示，更准确地说应该是单位四元数。
$$positio{n}_d=\left[\begin{array}{c}{p}_x\\ p_y\\ p_z\end{array}\right]$$

$$orientatio{n}_d=\left[\begin{array}{c}x\\ y\\ z\\ w\end{array}\right]$$

通过上述的解释，下面这几行代码理解起来就简单多了:

  // 设置机器人最后平衡点为当前初始位姿
  Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data())); // 将齐次变换矩阵转换为仿射变换
  Eigen::Vector3d position_d(initial_transform.translation()); // 提取仿射变换(或齐次变换矩阵)中的平移向量,"_d"代表"desire",即期望的位置
  Eigen::Quaterniond orientation_d(initial_transform.linear()); // 提取仿射变换(或齐次变换矩阵)中的旋转矩阵,并转换为四元数表达,"_d"代表"desire",即期望的姿态

3. 定义力矩控制循环的回调函数

控制策略开始后，实时读取当前状态，即反馈信号：位置、速度、末端姿态、当前雅可比矩阵、当前科氏力。

  // get state variables
  std::array<double, 7> coriolis_array = model.coriolis(robot_state);
  std::array<double, 42> jacobian_array = model.zeroJacobian(franka::Frame::kEndEffector, robot_state);

注意通过 franka::RobotState& robot_state 读取的数据都是 std::array 模板类的对象，我们需要将其转化成 Eigen::Matrix 模板类的对象:

  // convert to Eigen
  Eigen::Map<const Eigen::Matrix<double, 7, 1>> coriolis(coriolis_array.data());
  Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
  Eigen::Map<const Eigen::Matrix<double, 7, 1>> q(robot_state.q.data());
  Eigen::Map<const Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());

同上述一样,对运动过程中末端执行器的笛卡尔位置、姿态进行提取。：

  Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data())); 
  Eigen::Vector3d position(transform.translation()); // position后面没有"_d"，为当前运动的位置
  Eigen::Quaterniond orientation(transform.linear()); // orientation后面没有"_d"，为当前运动的姿态

4. 计算位姿误差

位置偏差采用欧式距离，直接用当前末端位置减平衡点位置即可。姿态偏差采用四元数减法。这里稍微详细地说一下：由于同一个姿态可以用两个单位四元数表示，即 $\mathbf{Q}$ 与 $-\mathbf{Q}$ 表达的是同一个姿态，但是旋转的方向完全相反。为了计算两个四元数之间最小差值，需要在计算差之前首先调整符号（此处类似于0$°$与60$°$的差值，可以是$60°-0°=60°$，也可以是$60°-360°=-300°$。但要取最小差值，即$60°$）；另外，此处采用四元数虚部来表示距离，这个定义在小范围的偏差上可用。

  // position error(位置误差)
  Eigen::Matrix<double, 6, 1> error;
  error.head(3) << position - position_d; // 将位置误差储存到error向量中前三位中

  // orientation error(姿态误差)
  // "difference" quaternion
  if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) {
    orientation.coeffs() << -orientation.coeffs();
  }
  Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d);
  error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
  // Transform to base frame(转换到基坐标)
  error.tail(3) << 0 - transform.linear() * error.tail(3); // 将姿态误差储存到error向量中后三位中

5. 计算关节力矩控制律

$${\mathbf{\tau }}_d={\mathbf{J}}^T(\mathbf{q})(-\mathbf{\alpha }\cdot \mathbf{e}-\mathbf{\beta }\cdot \mathbf{J}(\mathbf{q})\dot{\mathbf{q}})+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})$$

  // compute control
  Eigen::VectorXd tau_task(7), tau_d(7);

  // Spring damper system with damping ratio = 1(阻尼比为1的弹簧-阻尼系统)
  tau_task << jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq));
  tau_d << tau_task + coriolis; // 补偿科氏力

注意返回值的类型不能是 Eigen::Matrix ，需要变回 std::array 。

  std::array<double, 7> tau_d_array{};
  Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
  return tau_d_array;