Franka 力控和位置控制结合使用

在Franka Emika Panda机器人中，力控制结合位置控制是一种常见的控制方法，能够实现复杂的任务，如沿特定路径运动的同时感知和响应外部施加的力。这种控制方法在需要精确定位和同时需要感知外部力的场景中非常有用，比如在装配、抛光、打磨等工业任务中。

下面我们通过一个示例来展示如何使用 libfranka 实现力控制结合位置控制。在这个示例中，我们将实现机器人沿直线路径运动，同时对外力进行响应，使机器人在受到外力时能够做出相应调整。

示例：沿直线路径运动并实时调整响应外力

1. 设置和导入库

首先，确保已安装 libfranka 并设置好环境。创建一个C++程序来控制机器人。

#include <franka/exception.h>
#include <franka/robot.h>
#include <franka/model.h>
#include <franka/rate_limiting.h>
#include <iostream>
#include <array>
#include <cmath>

2. 定义控制任务

定义一个控制任务，机器人将沿着z轴向下移动，同时实时监控末端执行器的外力。根据外力的大小，机器人将调整其位置和姿态。

int main() {
    try {
        franka::Robot robot("192.168.0.1");  // 替换为机器人的实际IP地址

        // 定义力控制和位置控制的目标
        double desired_force_z = -10.0;  // 期望在z轴方向上的力（N）
        double max_force_deviation = 5.0;  // 最大允许的力偏差（N）
        double max_displacement_z = 0.1;  // 最大允许的位移（m）

        // 设置初始位置和阻抗控制参数
        robot.setCollisionBehavior(
            {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, 
            {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
            {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}},
            {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}
        );

        // 控制回调函数
        auto control_callback = [&](const franka::RobotState& state, franka::Duration period) -> franka::CartesianVelocities {
            // 获取当前末端执行器的外力
            std::array<double, 6> external_forces = state.K_F_ext_hat_K;
            double current_force_z = external_forces[2];

            // 计算力偏差
            double force_error = desired_force_z - current_force_z;

            // 计算位移调整（简单的比例控制）
            double displacement_z = force_error * 0.01;

            // 限制位移调整量
            if (displacement_z > max_displacement_z) {
                displacement_z = max_displacement_z;
            } else if (displacement_z < -max_displacement_z) {
                displacement_z = -max_displacement_z;
            }

            // 设置末端执行器的速度，沿z轴调整
            franka::CartesianVelocities velocities = {{0.0, 0.0, displacement_z, 0.0, 0.0, 0.0}};
            return franka::rate::limit(franka::kMaxCartesianVelocity, velocities);
        };

        // 开始控制循环
        robot.control(control_callback);

    } catch (const franka::Exception& e) {
        std::cerr << e.what() << std::endl;
        return -1;
    }

    return 0;
}

代码解释

1. 初始化机器人：通过 franka::Robot 类的构造函数创建机器人实例，并设置连接的IP地址。

2. 定义目标力和控制参数：

   • desired_force_z 设置为 -10.0 牛顿，表示希望在z轴方向上施加一个10牛顿向下的力。
   • max_force_deviation 和 max_displacement_z 分别定义了力和位移的允许最大偏差。

3. 设置阻抗控制：通过 setCollisionBehavior 函数设置机器人的阻抗控制参数，以确保机器人在受到外力时能够以一定的柔顺性响应。

4. 定义控制回调函数：

   • 在控制回调函数中，每次控制周期都会读取当前的外力数据。
   • 计算与期望力之间的偏差 force_error。
   • 通过简单的比例控制器计算出需要调整的位移量 displacement_z，并对其进行限制，防止过大的调整导致不稳定。

5. 控制机器人运动：

   • 使用 robot.control(control_callback) 函数启动控制循环，机器人将沿着z轴进行移动，并根据外力实时调整其位置。

注意事项

1. 安全性：确保设置合理的阻抗控制参数和运动限制，以防止机器人因过大外力或运动偏差导致损坏或发生危险。

2. 实时控制：libfranka 的控制循环非常高效，能够在毫秒级别的周期内执行控制任务，确保机器人能够实时响应外部力的变化。

3. 调试和优化：根据具体的应用需求和实验结果，调整控制参数以获得最佳效果。您可以通过观察机器人的实际运动和外力反馈来优化控制策略。

4. 高级控制策略：在实际应用中，可以使用更复杂的控制策略，如混合力/位置控制或自适应控制，来应对复杂的操作任务。

通过结合力控制和位置控制，Franka Emika Panda机器人能够在执行精细任务时保持高精度，同时具备对外部环境变化的敏感性和适应性。这种控制方法在许多工业和研究应用中具有广泛的应用前景。