本文以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 = 1.0, k_i = 2.0k p =1.0,k i=2.0
- 滤波器增益 filter_gain: α = 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: τ m (0)
- 初始外部力矩 initial_tau_ext:τ e x t ( 0 ) = τ m ( 0 ) − g ( q ( 0 ) )
-
初始力矩偏差 tau_error_integral:τ e (0)=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 τ 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:
- 外部关节力矩 tau_ext:
- 期望关节力矩 tau_d:
- 关节力矩控制信号 tau_cmd:τ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;
这一段代码字面可以解读为如下公式:
这样看有点不明所以,其实我们可以换一种写法:
这是一个前馈项加一个PI控制器。注意,控制信号τ 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 的初始值是0,α 值越大m d值增长越快。
(未完待续……)
评论(0)
您还未登录,请登录后发表或查看评论