基于六维力和力矩传感器的阻抗/导纳控制

目前实验室购买了一款F/T六维力和力矩传感器，希望借此完成机械臂的力控制，一些论文将力和位置的控制关系称之为柔顺控制。也就是说阻抗或导纳控制是柔顺控制的一种，通过调节机器人末端位置与力之间的动态特性来实现柔顺性。

阻抗控制（Impedance Control）:基于内环力控制的阻抗控制，根据当前的运动状态，通过阻抗模型，决定输出的力；

导纳控制（Admittance Control）:基于内环运动控制的阻抗控制，根据当前的受力状态，以及目标导纳模型，决定输出的运动

阻抗控制的特点是通过机器人末端控制器的位置或者速度和外部环境作用力之间的关系，通过调节机械臂各个关节的反馈位置误差、速度误差来达到控制外部作用力的目的。这种力控制不外乎两种基本形式一种基于位置的，另一种基于速度的。把反馈的力信号转换为位置调整量称为刚度控制；把反馈的力信号转换为速度修正量称为阻尼控制。当把反馈的力信号同时转换为位置和速度的修正量即为阻抗控制。基于内环力控制的可以认为机器人本身是一个Admittance(输入是力矩，输出是运动)，而控制器是一个Impedance（输入是位置，输出是力矩）。在基于内环运动控制的阻抗控制中，机器人本身是Impedance （输入是速度，输出是对外的力），而控制器是Admittance（输入是外力，输出是运动）

机器人与外界环境之间的交互可以由质量-弹簧-阻尼系统来描述，如下图所示，

系统动力学方程为： 其中x为位移，m为质量，b为阻尼，k为刚度，f为作用力。

对上面的动力学方程进行拉普拉斯变换，可以得到：

 将机器人看作是相同的模型，并认为机器人和环境的接触时弹性的：

则可以将阻抗定义为 ，导纳定义为阻抗的倒数： 

导纳控制算法：

通过机器人末端六维力和力矩传感器获取表示环境对机器人系统施加的力，通过下面的公式可以计算出机器人的加速度：

    

通常，我们可以获取机器人的初始位置以及运动速度，即下面的，通过导纳模型可以求出理想运动加速度、速度、位置：

 

此时系统呈现出我们理想的阻抗。这个方法可以很好地利用现在已非常成熟的位置控制算法来实现精度和响应都比较好的导纳控制。另外，它也不需要依赖系统本身的动力学参数。但是可以看到，处在分母的项使得我们很难让系统呈现比较小的质量/惯性——很小的扰动即可能造成很大的运动指令变化，系统也容易不稳定。在导纳控制的同时对接触力进行跟踪，静态情况下，如果接触力与我们理想的力之间存在误差，我们可以通过调节位置原点，或者理想刚度来改变这个力。https://www.guyuehome.com/4538

因此实际控制的时候，有如下几个步骤

• 对力和力矩传感器数据进行过滤
• 末端负载的重力补偿
• 通过导纳控制器根据力偏差得到位置纠偏量，期望力可以自己设定

void AdmittanceController::compute_admittance() {

  Vector6d arm_desired_accelaration;

  Vector6d error;

  // 平衡位置和实际位置的之间的error
  Eigen::Quaterniond quat_rot_err(arm_real_orientation_*equilibrium_orientation_.inverse());

  Eigen::AngleAxisd err_arm_des_orient(quat_rot_err);
  error.bottomRows(3) << err_arm_des_orient.axis() *err_arm_des_orient.angle();

  // Translation error w.r.t.（with respect to） desipared equilibrium
  error.topRows(3) = arm_real_position_ - equilibrium_position_;

  // 阻抗导纳求解期望的加速度、速度
  arm_desired_accelaration = M_a_.inverse() * ( - K_a_ * error - D_a_ * arm_desired_twist_final_ +  wrench_external_);

  // x,y,z for the arm
  double a_acc_norm = (arm_desired_accelaration.segment(0, 3)).norm();

  // Integrate for velocity based interface
  ros::Duration duration = loop_rate_.expectedCycleTime();
  arm_desired_twist_final_ += arm_desired_accelaration * duration.toSec();
}

 目前程序在ROS中测试是成功的，但是没有在真实机器人上测试，所以还需要一段时间等开学去验证，这是我ROS中仿真的效果，真实情况大概如东南大学做出的效果https://zhuanlan.zhihu.com/p/31457020。当在真实机器人中测试成功之后，再给出完整的ROS程序和参考把。