深度解析Franka机器人的运动生成与控制——libfranka

最新推荐文章于 2024-09-19 20:01:02 发布

止于至玄

最新推荐文章于 2024-09-19 20:01:02 发布

阅读量7.9k

点赞数 10

分类专栏： Hardwares 文章标签： c++

本文链接：https://blog.csdn.net/philthinker/article/details/106766194

版权

本文深入剖析了使用Franka机器人C++库libfranka进行运动控制的方法，包括关节和笛卡尔空间的运动生成，以及阻抗控制和力控策略。强调了运动信号的连续性和平滑过渡的重要性，并提供了关节空间和笛卡尔空间的控制实例。此外，还介绍了外力估计与控制以及阻抗控制的实现，展示了如何通过libfranka实现精确的力矩控制。

摘要由CSDN通过智能技术生成

Franka
本文主要分析如何使用Franka机器人C++代码库——libfranka来进行运动生成与控制。包含机器人学相关知识、Franka机器人的特性以及笔者在使用过程中的一点心得体会。笔者基于libfranka 0.8.0 版本进行开发调试。除了编程技巧外，本文还将在一定程度上讨论Franka 机器人官方运动生成与阻抗控制方法的基本特征，以及一些实际使用技巧。重点在于介绍FCI手册中没有将的一些实用技术细节以及笔者对Franka机器人的分析。

这里假设读者已具备以下基础知识：

C++基础
Eigen基础
机器人学基础

最新版libfranka可从这里下载。编译过程参考此文。

libfranka综述

libfranka是采用C++开发的工具包，用以开发Franka机器人的应用。libfranka提供了实时的控制机制，使能用户开发复杂的运动规划/控制算法，十分方便。Franka机器人的控制频率为 1kHz ，但是实际留给用户代码的执行时间 <300 $\mu$ s。切记不要让代码过于冗长，也不要太多额外的外部读写操作。

本文进讨论Franka机器人的实时运动生成与控制部分，假设机器人未安装官方电爪。用户可以自行拆解机器人的电爪，并在Desk界面的Settings中将末端执行器设置为None。
control
玩转franka机器人其实指的是掌握libfranka提供的运动生成策略以及通过直接控制关节力矩设计控制策略。如上图所示，实时控制信号为关节力矩，用户也可以实时指定关节位置及速度、笛卡尔空间位置及速度、肘部位置。libfranka非实时指令的内容比较简单易懂，本文不做专门介绍。

以下内容中，我们假设读者已经对franka::Robot::Control的工作机理有初步的了解，至少看过FCI手册或者此文。

运动生成

我们从官方例程说起。按照FCI手册的说法，如上图所示，用户给定的指令信号（下标为c，command）为关节空间位置、速度，笛卡尔空间位置、速度。注意运动信号务必要“连续”，即变化不能超限，限制阈值参考FCI手册。随后，机器人控制器根据运动学特性换算出期望信号（下标为d，desired）。也就是说，实际控制机器人的其实是期望信号。

关节空间运动生成

关节空间的运动生成不需要运动学计算，FCI手册上这样说：

When you are using a joint motion generator, the Robot kinematics completion block will not modify the commanded joint values and therefore $q_d, \dot{q}_d, \ddot{q}_d$ and $q_c, \dot{q}_c, \ddot{q}_c$ are equivalent. Note that you will only find the $d$ signals in the robot state.

也就是说 $q_{d} = q_{c}$ ， $\dot{q}_{d} = \dot{q}_{c}$ 。其实不是这么简单，我们先看官方例程：

std::array<double, 7> initial_position;
double time = 0.0;
robot.control([&initial_position, &time](const franka::RobotState& robot_state,
                                             franka::Duration period) -> franka::JointPositions {
   
      time += period.toSec();
      if (time == 0.0) {
   
        initial_position = robot_state.q_d;
      }
      double delta_angle = M_PI / 8.0 * (1 - std::cos(M_PI / 2.5 * time));
      franka::JointPositions output = {
   {
   initial_position[0], initial_position[1],
                                        initial_position[2], initial_position[3] + delta_angle,
                                        initial_position[4] + delta_angle, initial_position[5],
                                        initial_position[6] + delta_angle}};
      if (time >= 5.0) {
   
        std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
        return franka::MotionFinished(output);
      }
  return output;
});

关于franka::Robot::control方法的使用读者请自行参考frankaemika.github.io上的介绍，其它就是C++ 基础，此处不做赘述。该例程完成了关节4、5、7的运动规划。根据变量delta_angle（此处记作 $\delta\theta$ ）的公式：
$\delta\theta = \frac{\pi}{8} \left(1-\cos\left(\frac{2\pi}{5}t\right)\right)$ 可以看出，这是一个S形曲线运动规划。使用正余弦函数可以保证轨迹在任何位置无穷阶可微，即保证了光滑性。此时只要限制振幅（此处为 $\pi/8$ ）就可以确保速度、加速度、加加速度不超限。

为了理解问题，我们简化程序：

double delta_angle = M_PI / 64.0;      
franka::JointPositions output = {
   {
   initial_position[0], initial_position[1],                  
                                  initial_position[2], initial_position[3],
                                  initial_position[4], initial_position[5],
                                  initial_position[6] + delta_angle}};

给定一个固定值，结果会怎么样？我们读取7轴的期望信号与指令信号共同作图：
qcqd
有意思了，这酷似一个欠阻尼二阶控制系统的瞬态响应曲线。我们再把速度、和加速度曲线画出来：
dqd
ddqd
这下更明显了，开始时控制系统将加加速度设置为最大，进而将加速度加到最大，反复调节，最终系统稳定。注意 franka::Robot::control 函数参数中除了运动生成器外还包含一个控制器，如果不指定控制器，控制系统会调用内部的阻抗控制器实现。

那么，如果读取官方例程中指令信号与期望信号作比较，会得到什么样的结果呢？这个问题留给读者自行检验。（提示：一个周期内差的积分为0）

综上，我们可以看出一个有用的技巧是：不要直接给定阶跃控制目标。平滑过渡对于实际操作十分重要。原因或许是缺少一个内部插值机制，偏差过大会导致控制器输出达到峰值。

笛卡尔空间运动生成

如果想要单纯输出**位置（3维）而非位姿（6维）**运动轨迹，那么可以参考官方例程；否则时常会报出加速度过大的错误。因此笔者不建议采用这种方式生成笛卡尔空间位姿轨迹。官方例程中代码核心部分如下：

    // First move the robot to a suitable joint configuration
    std::array<double, 7> q_goal = {
   {
   0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
    MotionGenerator motion_generator(0.5, q_goal);
    std::cout << "WARNING: This example will move the robot! "
              << "Please make sure to have the user stop button at hand!" << std::endl
              << "Press Enter to continue..." << std::endl;
    std::cin.ignore();
    robot.control(motion_generator);
    std::cout << "Finished moving to initial joint configuration." << std::endl;
    // Set additional parameters always before the control loop, NEVER in the control loop!
    // Set collision behavior.
    robot.setCollisionBehavior(
        {
   {
   20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {
   {
   20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
        {
   {
   20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {
   {
   20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
        {
   {
   20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {
   {
   20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
        {
   {
   20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {
   {
   20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});
    std::array<double, 16> initial_pose;
    double time = 0.0;
    robot.control([&time, &initial_pose](const franka::RobotState& robot_state,
                                         franka::Duration period) -> franka::CartesianPose {
   
      time += period.toSec();
      if (time == 0.0) {
   
        initial_pose = robot_state.O_T_EE_c;
      }
      constexpr double kRadius = 0.3;
      double angle =