轻擎机械臂电流环力控：通过雅可比矩阵实现导纳（低配版）

晴

本文介绍轻擎2机械臂中实现控制末端力的方法。
项目地址： https://gitee.com/qingqing-gaq/light_lift_arm_3dof.git
演示视频：【【开源】轻擎2；仿真不了一点，真男人就得上实物！！！】 https://www.bilibili.com/video/BV1RVPveRE1d/?share_source=copy_web&vd_source=54c454ce8d52dd206aad5a0bba0e3c2c

c++上位机中的模式的配置代码：

            //     AutoServo,      //电机内部mit
            //     ManualServo,    //手动通过纯力矩控制电机
            //     Impdence,       //阻抗
            //     Admittance,     //导纳
            //     Gravity,        //重力补偿
            //     JointAutoServo, //关节自动控制,注意要初始化关节位置
            //     Zero            //设置零点
            //     Teach           //示教

            control_mode.modeTransition(State::Admittance);

首先说明我们要实现什么效果

相对刚度和阻尼，现在我们要是实现的功能是非常简单的：让机械臂在某一个轴或多个轴向上输出一个力，而剩余的轴则保持位置控制，继续跟踪期望轨迹。
这种效果个人理解，也可以称为力位混控，因这两种控制是同时存在的。
当然力控部分仍然是“开环”的，没有反馈的接触力，也不考虑环境的阻抗。

思路
用这个篇文章：轻擎机械臂电流环力控：通过雅可比矩阵实现末端阻抗（刚度+阻尼）中的方法也可以实现，但是位置精度不高，所以这里介绍另一种比较复杂的方法：
1、提供一个期望轨迹，这里只需要位置就行。提供一个期望的力（包括大小和方向）。
2、实时通过正运动学计算末端位置（三轴机械臂没有姿态），根据期望力和期望位置更新下一次的期望位置。
3、把末端期望的力通过雅可比矩阵映射成关节力矩，把期望末端位置通过逆运动学映射成关节位置。
4、把期望的关节力矩和关节位置，通过mit模式输入到电机，注意要调整一个合适的pid，不让位置控制会抖振。

具体实现

1、
期望轨迹：

arm_3dof.desir_end_efect_pos[0] = 0.21 + amplitude * sin(rate * realtime);
arm_3dof.desir_end_efect_pos[1] = 0.0 + amplitude * cos(rate * realtime);
arm_3dof.desir_end_efect_pos[2] = 0.0 + amplitude * cos(rate * realtime);

期望力：

    float end_force_value = -1.0;//大小
    float end_force_orientation[3] = {0, 0, 1};//方向,只能是正数

2、
实时通过正运动学计算末端位置

 fk(motor_pos_bk, current_end_pos); // 获取当前末端位置

通过期望力和期望位置更新下一次的期望位置：

for (size_t i = 0; i < 3; i++)
{
  refer_mask[i] = 1 - abs(end_force_orien[i]);
}
    // std::cout << "desir_motor_pos  : " << current_end_pos[0] << " " << current_end_pos[1] << " " << current_end_pos[2]<< std::endl;
imp_desir_pos[0] = reference_point[0] * refer_mask[0] + current_end_pos[0] * abs(end_force_orien[0]); 
imp_desir_pos[1] = reference_point[1] * refer_mask[1] + current_end_pos[1] * abs(end_force_orien[1]); 
imp_desir_pos[2] = reference_point[2] * refer_mask[2] + current_end_pos[2] * abs(end_force_orien[2]);

这一部分代码最有意思：
其中 reference_point 表示期望轨迹（1中给出的）， current_end_pos 表示当前的末端位置（2中正运动学计算的结果）。那么 reference_point + current_end_pos 的意思是期望位置要受当前位置的影响，具体哪个轴向的期望位置受当前位置的影响，由期望力的方向决定。比如期望力的方向是（0,0,1）也就是z轴上有一个期望力，此时refer_mask就是（1，1,，0），reference_point[2] * refer_mask[2] 这一项就是0，current_end_pos[2] * abs(end_force_orien[2]) 这一项就等于 current_end_pos[2] 。这个意思就是z轴的期望位置就是当前的位置。

3、
末端期望的力通过雅可比矩阵映射成关节力矩：

    end_force[0] = end_force_orien[0] * end_force_value;
    end_force[1] = end_force_orien[1] * end_force_value;
    end_force[2] = end_force_orien[2] * end_force_value;
    end2joint_force(motor_pos_bk, end_force, tao_dan); // 期望末端力，对应的关节的力矩

把期望末端位置通过逆运动学映射成关节位置：

Ik_3Dof(imp_desir_pos, motor_pos);

4、
把期望的关节力矩和关节位置，通过mit模式输入到电机，这里注意kp kd要调好哈。

        motor.ControlMotors(port,
                            motor.desir_motor_pos,
                            motor.current_motor_vel, // 使用实时速度是最好的！！！！
                            motor.kp,
                            motor.kd,
                            motor.motor_control_tau);

还有一些小细节

4中输入电机的期望速度也可以，通过类似上述过程计算，但是实际效果不好，所以在4中使用力电机的实时速度。但是导纳函数 admittance() 中会有相关代码，这些代码的计算结果并没有用到，大家不要疑惑哈。
因为机械臂有重量，所以还需要加上重力补偿的力：

        arm.joint_control_tau[0] = arm.joint_admittance_tau[0] + arm.joint_gravity_tau[0];
        arm.joint_control_tau[1] = arm.joint_admittance_tau[1] + arm.joint_gravity_tau[1];
        arm.joint_control_tau[2] = arm.joint_admittance_tau[2] + arm.joint_gravity_tau[2];_tau[2];

下面一段是通过关节速度对关节的摩擦力做补偿的代码，效果是有一点的。

      for (size_t i = 0; i < 3; i++)
        {
            if (abs(arm.current_joint_vel[i]) <= 0.01)
                arm.joint_admittance_tau[i] += signum(arm.joint_admittance_tau[i]) * 0.1;
        }

实话实说

算法很简单，效果也挺好（个人感觉）。
但是毕竟是电流环，也没力矩传感器，这个输出的力精度是没保证的，程序上输出1n，实际可能大点或小点。
当然也可以做电机的标定，机械臂的摩擦辨识。我嫌麻烦就没深入搞。
不同的轴向力的输出效果也略不同，这个可能跟摩擦和重力有关。
因为这个方法用到力电机内部的pid控制，所以要把pid调好。

机器人小伊

你的文章很好地概述了在轻擎2机械臂上实现基于雅可比矩阵的末端力控（导纳控制的一种简化形式）的过程。这种方法结合了位置控制和力控制，使得机械臂在某些轴上能够施加期望的力，而在其他轴上则继续跟踪期望的位置轨迹。以下是对你文章的一些补充和澄清，以及对潜在读者的进一步解释：

1. 雅可比矩阵的作用

雅可比矩阵（Jacobian Matrix）在机器人学中非常重要，因为它描述了机器人关节空间速度到末端执行器空间速度的线性映射。在力控中，雅可比矩阵的转置被用来将末端执行器空间的力映射到关节空间的力矩。这种映射是实现力控的关键步骤之一。

2. 力位混控

力位混控（Hybrid Force/Position Control）允许机器人在同一时间对不同的自由度进行不同的控制。在你的例子中，机械臂在特定轴上施加力，同时在其他轴上跟踪位置轨迹。这种控制策略对于需要与环境进行交互的机器人任务非常有用。

3. 实时性

由于你的控制算法依赖于实时数据（如当前末端位置和速度），因此确保系统的实时性至关重要。这意味着从传感器读取数据、计算控制输入以及将控制信号发送到电机都需要在非常短的时间内完成。

4. 摩擦力补偿

摩擦力是机器人系统中常见的非线性干扰因素。在你的代码中，你通过检查关节速度并对非常小的速度应用一个小的力来尝试补偿摩擦力。这种方法虽然简单，但在某些情况下可能有效。然而，更精确的方法可能涉及动态摩擦模型的识别和补偿。

5. 重力补偿

对于多关节机器人来说，重力是一个重要的考虑因素。你的代码中包括了重力补偿的步骤，这是确保机械臂在自由空间中运动时不会因重力而偏离期望轨迹的关键。

6. 电流环控制

你提到的方法依赖于电机的电流环控制。这意味着控制算法输出的力矩（或电流）指令被直接发送到电机驱动器，驱动器负责将其转换为实际的电机电流。电流环控制通常具有较快的响应时间，但力控制的精度可能受到电机特性、驱动器性能和机械系统动态特性的影响。

7. 调试和校准

尽管你的算法在概念上相对简单，但实现一个稳定且有效的力控系统可能需要大量的调试和校准工作。这包括调整PID控制器的参数、验证雅可比矩阵的正确性、以及确保所有传感器和执行器都正常工作。

结论

你的文章为那些希望在机械臂上实现简单力控功能的读者提供了一个很好的起点。尽管这种方法在精度和鲁棒性方面可能无法与更高级的控制策略相媲美，但它为理解和实现更复杂的控制算法提供了宝贵的基础。对于那些希望将机械臂用于交互任务的研究人员和工程师来说，这是一个非常有价值的技能。

以上为生成回答，仅供参考～