机器人末端力/力矩控制实用简述——以Franka机器人为例

最新推荐文章于 2026-06-15 10:03:57 发布

原创最新推荐文章于 2026-06-15 10:03:57 发布 · 4.9k 阅读

22 ·

本内容遵循CC 4.0 BY-SA版权协议

标签

#硬件工程 #c++

Hardwares 专栏收录该内容

6 篇文章

订阅专栏

本文详细介绍了如何使用libfranka库在Franka机器人上实现末端力/力矩控制，特别是阻抗控制方法。通过C++编程实现一个使机器人在z轴方向施加9.8N力的PI控制器。文章涵盖了控制器参数设定、观测值初始化、核心PI控制器的工作原理，以及期望力矩的线性插值更新。文中强调了平滑过渡和重力补偿在确保平稳运行中的重要性。

frankapanda
本文以Franka机器人为例简述基本的机器人末端力/力矩控制方法，以及阻抗控制方法。本文假设读者具有一定的机器人学与C++程序设计基础。笔者基于libfranka 0.8.0 版本进行开发调试。除了编程技巧外，本文还将在一定程度上讨论Franka 机器人官方运动生成与阻抗控制方法的基本特征。所有实例代码来自libranka官方手册。更多相关内容建议读者参考此文。

笔者开发配置：

OS: Ubuntu 18.04 LTS x64
RT kernel: 5.6.14-rt6
libfranka 0.8.0

文章目录

机器人末端力控实例

机器人末端力控实例

下面代码的作用是使机器人末端在z轴方向产生一个1kg的力，即9.8N，采用PI控制器实现。
（代码来自libranka官方手册实例 force_control.cpp）

// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <array>
#include <iostream>
#include <Eigen/Core>
#include <franka/duration.h>
#include <franka/exception.h>
#include <franka/model.h>
#include <franka/robot.h>
#include "examples_common.h"
int main(int argc, char** argv) {
  // Check whether the required arguments were passed
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
    return -1;
  }
  // parameters
  double desired_mass{0.0};
  constexpr double target_mass{1.0};    // NOLINT(readability-identifier-naming)
  constexpr double k_p{1.0};            // NOLINT(readability-identifier-naming)
  constexpr double k_i{2.0};            // NOLINT(readability-identifier-naming)
  constexpr double filter_gain{0.001};  // NOLINT(readability-identifier-naming)
  try {
    // connect to robot
    franka::Robot robot(argv[1]);
    setDefaultBehavior(robot);
    // load the kinematics and dynamics model
    franka::Model model = robot.loadModel();
    // 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}});
    franka::RobotState initial_state = robot.readOnce();
    Eigen::VectorXd initial_tau_ext(7), tau_error_integral(7);
    // Bias torque sensor
    std::array<double, 7> gravity_array = model.gravity(initial_state);
    Eigen::Map<Eigen::Matrix<double, 7, 1>> initial_tau_measured(initial_state.tau_J.data());
    Eigen::Map<Eigen::Matrix<double, 7, 1>> initial_gravity(gravity_array.data());
    initial_tau_ext = initial_tau_measured - initial_gravity;
    // init integrator
    tau_error_integral.setZero();
    // define callback for the torque control loop
    Eigen::Vector3d initial_position;
    double time = 0.0;
    auto get_position = [](const franka::RobotState& robot_state) {
      return Eigen::Vector3d(robot_state.O_T_EE[12], robot_state.O_T_EE[13],
                             robot_state.O_T_EE[14]);
    };
    auto force_control_callback = [&](const franka::RobotState& robot_state,
                                      franka::Duration period) -> franka::Torques {
      time += period.toSec();
      if (time == 0.0) {
        initial_position = get_position(robot_state);
      }
      if (time > 0 && (get_position(robot_state) - initial_position).norm() > 0.01) {
        throw std::runtime_error("Aborting; too far away from starting pose!");
      }
      // get state variables
      std::array<double, 42> jacobian_array =
          model.zeroJacobian(franka::Frame::kEndEffector, robot_state);
      Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
      Eigen::Map<const Eigen::Matrix<double, 7, 1>> tau_measured(robot_state.tau_J.data());
      Eigen::Map<const Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data());
      Eigen::VectorXd tau_d(7), desired_force_torque(6), tau_cmd(7), tau_ext(7);
      desired_force_torque.setZero();
      desired_force_torque(2) = desired_mass * -9.81;
      tau_ext << tau_measured - gravity - initial_tau_ext;
      tau_d << jacobian.transpose() * desired_force_torque;
      tau_error_integral += period.toSec() * (tau_d - tau_ext);
      // FF + PI control
      tau_cmd << tau_d + k_p * (tau_d - tau_ext) + k_i * tau_error_integral;
      // Smoothly update the mass to reach the desired target value
      desired_mass = filter_gain * target_mass + (1 - filter_gain) * desired_mass;
      std::array<double, 7> tau_d_array{};
      Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_cmd;
      return tau_d_array;
    };
    std::cout << "WARNING: Make sure sure that no endeffector is mounted and that the robot's last "
                 "joint is "
                 "in contact with a horizontal rigid surface before starting. Keep in mind that "
                 "collision thresholds are set to high values."
              << std::endl
              << "Press Enter to continue..." << std::endl;
    std::cin.ignore();
    // start real-time control loop
    robot.control(force_control_callback);
  } catch (const std::exception& ex) {
    // print exception
    std::cout << ex.what() << std::endl;
  }
  return 0;
}

笔者假设读者对libfranka有基本的了解，如若没有，请参考此文。下面针对代码的核心部分进行分析。

  // parameters
  double desired_mass{0.0};
  constexpr double target_mass{1.0};    // NOLINT(readability-identifier-naming)
  constexpr double k_p{1.0};            // NOLINT(readability-identifier-naming)
  constexpr double k_i{2.0};            // NOLINT(readability-identifier-naming)
  constexpr double filter_gain{0.001};  // NOLINT(readability-identifier-naming)

这一段是任务与控制器参数设定。这里定义了：

期望质量 desired_mass: $m_{d} = 0.0$
目标质量 target_mass: $m_{t} = 1.0$
PI控制器参数 k_p, k_i: $k_p = 1.0, k_i = 2.0$
滤波器增益 filter_gain: $\alpha = 0.001$

这里需要注意的是期望质量与目标质量不同。原因很简单，为了机器人能够平稳运行，不产生抖动，任何控制目标都需要插值。即让期望质量平滑过渡到目标质量，保证不出现突然的大输出增益。

    franka::RobotState initial_state = robot.readOnce();
    Eigen::VectorXd initial_tau_ext(7), tau_error_integral(7);
    // Bias torque sensor
    std::array<double, 7> gravity_array = model.gravity(initial_state);
    Eigen::Map<Eigen::Matrix<double, 7, 1>> initial_tau_measured(initial_state.tau_J.data());
    Eigen::Map<Eigen::Matrix<double, 7, 1>> initial_gravity(gravity_array.data());
    initial_tau_ext = initial_tau_measured - initial_gravity;
    // init integrator
    tau_error_integral.setZero();

这一段是观测值初始化。这里涉及几个变量：

初始重力补偿力矩 initial_graivity: $g (q (0))$
初始力矩测量值 initial_tau_measured: $\tau_{m}(0)$
初始外部力矩 initial_tau_ext: $\tau_{ext}(0) = \tau_{m}(0)-g(q(0))$
初始力矩偏差 tau_error_integral: $\tau_{e}(0) = \mathbf{0}$

值得注意的是，读取的关节力矩为实时测量值，即包含了重力补偿部分。

    auto force_control_callback = [&](const franka::RobotState& robot_state,
                                      franka::Duration period) -> franka::Torques {
      time += period.toSec();
      if (time == 0.0) {
        initial_position = get_position(robot_state);
      }
      if (time > 0 && (get_position(robot_state) - initial_position).norm() > 0.01) {
        throw std::runtime_error("Aborting; too far away from starting pose!");
      }
      // get state variables
      std::array<double, 42> jacobian_array =
          model.zeroJacobian(franka::Frame::kEndEffector, robot_state);
      Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
      Eigen::Map<const Eigen::Matrix<double, 7, 1>> tau_measured(robot_state.tau_J.data());
      Eigen::Map<const Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data());
      Eigen::VectorXd tau_d(7), desired_force_torque(6), tau_cmd(7), tau_ext(7);
      desired_force_torque.setZero();
      desired_force_torque(2) = desired_mass * -9.81;
      tau_ext << tau_measured - gravity - initial_tau_ext;
      tau_d << jacobian.transpose() * desired_force_torque;
      tau_error_integral += period.toSec() * (tau_d - tau_ext);
      // FF + PI control
      tau_cmd << tau_d + k_p * (tau_d - tau_ext) + k_i * tau_error_integral;
      // Smoothly update the mass to reach the desired target value
      desired_mass = filter_gain * target_mass + (1 - filter_gain) * desired_mass;
      std::array<double, 7> tau_d_array{};
      Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_cmd;
      return tau_d_array;
    };

核心部分：PI控制器。该控制器输出是关节力矩。这里假设机器人末端已经与某个刚性平面接触，即不产生位移。此段代码我们拆分来看。
首先是初始化与安全问题，如下：

      time += period.toSec();
      if (time == 0.0) {
        initial_position = get_position(robot_state);
      }
      if (time > 0 && (get_position(robot_state) - initial_position).norm() > 0.01) {
        throw std::runtime_error("Aborting; too far away from starting pose!");
      }

period存储了控制器执行的时间信息，第一次执行时period.toSec输出为0.0。也就是说，第一次调用该控制器时，我们读取当前时刻的机器人位置作为初始位置。而当当前位置与初始时刻位置偏差大于0.01米时，我们抛出实时错误终止该控制器。

之后时读取机器人动力学模型：

      // get state variables
      std::array<double, 42> jacobian_array =
          model.zeroJacobian(franka::Frame::kEndEffector, robot_state);
      Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
      Eigen::Map<const Eigen::Matrix<double, 7, 1>> tau_measured(robot_state.tau_J.data());
      Eigen::Map<const Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data());

这里读取了当前时刻的雅可比矩阵（jacobian） $J (q)$ ，当前关节力矩测量值tau_measured $\tau_{m}(t)$ 和当前时刻的重力补偿值gravity $g (q)$ 。注意这里代码中读取的不是实时的重力补偿力矩，因为我们假设机器人时不动的。

之后，准备控制器状态值：

      Eigen::VectorXd tau_d(7), desired_force_torque(6), tau_cmd(7), tau_ext(7);
      desired_force_torque.setZero();
      desired_force_torque(2) = desired_mass * -9.81;
      tau_ext << tau_measured - gravity - initial_tau_ext;
      tau_d << jacobian.transpose() * desired_force_torque;

这里声明了多个变量，我们逐一分析：

期望力/力矩 desired_force_torque: $f_{d}$ ， $f_{d} = [0.0, 0.0, -9.8m_{d}, 0.0, 0.0, 0.0 ]^{\top}$
外部关节力矩 tau_ext: $\tau_{ext}$ ， $\tau_{ext} = \tau_{m} - g(q) - \tau_{ext}(0)$
期望关节力矩 tau_d: $\tau_{d}$ ， $J^{\top}(q) \cdot f_{d}$
关节力矩控制信号 tau_cmd: $\tau_{c}$

控制律：

      tau_error_integral += period.toSec() * (tau_d - tau_ext);
      // FF + PI control
      tau_cmd << tau_d + k_p * (tau_d - tau_ext) + k_i * tau_error_integral;

这一段代码字面可以解读为如下公式： $\begin{aligned} \tau_{e} &= \tau_{e} + t\cdot(\tau_{d} - \tau_{ext}) \\ \tau_{c} &= \tau_{d} + k_p\cdot(\tau_{d} - \tau_{ext}) + k_i \cdot \tau_{e} \end{aligned}$ 这样看有点不明所以，其实我们可以换一种写法： $\tau_{c}(t) = \tau_{d} + k_p \cdot(\tau_d - \tau_{ext}(t)) + k_{i}*\int_{0}^{t}(\tau_d - \tau_{ext}(s))\mathrm{d}s$ 这是一个前馈项加一个PI控制器。注意，控制信号 $\tau_{c}$ 中并不需要包含重力补偿部分。

期望力矩插值更新（线性插值）：

      // Smoothly update the mass to reach the desired target value
      desired_mass = filter_gain * target_mass + (1 - filter_gain) * desired_mass;

这一段代码实现了一个从 $0$ 到 $m_{t}$ 的插值，即： $m_{d} = \alpha m_{t} + (1-\alpha)m_{d}$ 注意 $m_{d}$ 的初始值是0， $\alpha$ 值越大 $m_{d}$ 值增长越快。