轻擎2机械臂的电流环力控实现

[2431088289]

本文介绍轻擎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::Impdence);

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

刚度：物体的的软硬程度，弹簧就是这个词的最好例子。
阻尼：就是“丝滑”的程度，速度越大阻力越大，生活中有些门就有这个效果，为了防止“摔门”的情况，就加了个阻尼器。
惯性：这个好理解，大家也经常能体会到，她和加速度有关。但是在机械臂里（仅针对轻擎哈）就不太好实现，无论是加速度还是惯量都不太好获取。

所以源代码中所谓的“阻抗”仅仅是刚度阻尼控制，且是静态的。
最终效果就是让机械臂交互起来有“弹簧”和“阻尼器”的效果。还要注意的一点，这个方法不考虑环境的刚度，可以说是一个“开环”的力控制。

思路

1、有一个末端期望轨迹（或者一个点，这样效果更好些），和轨迹的速度。期望的刚度和阻尼。
2、正运动学实时计算末端的位置（三轴机械臂，只有位置），通过雅可比矩阵计算末端速度。
3、通过“期望-实际”的方法，计算实际的末端位置和期望的末端位置误差，以及速度误差。
4、有了误差就可以通过

               力 = 刚度*位置误差 + 阻尼*速度误差![替代文字](图片地址)

计算末端在有交互的情况下应该向外界产生多少力。
5、通过雅可比矩阵的把末端力映射到关节上，然后输入电机。

具体实现

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);
    arm_3dof.desir_end_efect_vel[0] = amplitude * rate * cos(rate * realtime);
    arm_3dof.desir_end_efect_vel[1] = -amplitude * rate * sin(rate * realtime);
    arm_3dof.desir_end_efect_vel[2] = -amplitude * rate * sin(rate * realtime);
    arm_3dof.desir_end_efect_acc[0] = -amplitude * rate * rate * sin(rate * realtime);
    arm_3dof.desir_end_efect_acc[1] = -amplitude * rate * rate * cos(rate * realtime);
    arm_3dof.desir_end_efect_acc[2] = -amplitude * rate * rate * cos(rate * realtime);

刚度40,阻尼8

   arm.end_impedance_control(arm.desir_end_efect_pos,
                              arm.desir_end_efect_vel,
                              arm.current_joint_pos,
                              arm.current_joint_vel,
                              arm.joint_impedance_tau,
                              8.0, 40,
                              end_force);

阻抗函数中的内容：
先计算雅可比矩阵：

//计算雅可比矩阵，q123都是关节的时是位置
 J11 = -sin(q1) * (a2 * cos(q2) + a3 * cos(q2 + q3));
 J21 = cos(q1) * (a2 * cos(q2) + a3 * cos(q2 + q3));
 J31 = 0;
 J12 = -cos(q1) * (a2 * sin(q2) + a3 * sin(q2 + q3));
 J22 = -sin(q1) * (a2 * sin(q2) + a3 * sin(q2 + q3));
 J32 = a2 * cos(q2) + a3 * cos(q2 + q3);
 J13 = -a3 * cos(q1) * sin(q3 + q2);
 J23 = -a3 * sin(q1) * sin(q3 + q2);
 J33 = a3 * cos(q3 + q2);

2、
正运动学实时计算末端的位置和速度：

//当前的末端位置
X[0] = cos(q1) * (a2 * cos(q2) + a3 * cos(q2 + q3));
X[1] = sin(q1) * (a2 * cos(q2) + a3 * cos(q2 + q3));
X[2] = a2 * sin(q2) + a3 * sin(q2 + q3);

// 当前的末端速度
dX[0] = current_joint_vel[0] * J11 + current_joint_vel[1] * J12 + current_joint_vel[2] * J13;
dX[1] = current_joint_vel[0] * J21 + current_joint_vel[1] * J22 + current_joint_vel[2] * J23;
dX[2] = current_joint_vel[0] * J31 + current_joint_vel[1] * J32 + current_joint_vel[2] * J33;

3、
末端位置误差，以及速度误差

// 末端误差
e[0] = X_desired[0] - X[0];
e[1] = X_desired[1] - X[1];
e[2] = X_desired[2] - X[2];
de[0] = desir_end_vel[0] - dX[0];
de[1] = desir_end_vel[1] - dX[1];
de[2] = desir_end_vel[2] - dX[2];

4、
计算末端在有交互的情况下应该向外界产生多少力

Fx = 2*c * de[0] + 9*k * e[0]; //x轴
Fy = 2*c * de[1] + 9*k * e[1]; //y轴
Fz = 2*c * de[2] + 9*k * e[2]; //z轴

5、
通过雅可比矩阵的把末端力映射到关节上

//映射到关节
joint_tor[0] = J11 * Fx + J21 * Fy + J31 * Fz;
joint_tor[1] = J12 * Fx + J22 * Fy + J32 * Fz;
joint_tor[2] = J13 * Fx + J23 * Fy + J33 * Fz;

因为是mit模式，所以如果想要只输入力矩，就需要把kpkd设置为0；

motor.kp[0] = 0;
motor.kd[0] = 0.0;

motor.kp[1] = 0;
motor.kd[1] = 0.0;

motor.kp[2] = 0;
motor.kd[2] = 0.0;

motor.ControlMotors_g(port,
                              motor.desir_motor_pos,
                              motor.desir_motor_vel,
                              motor.kp,
                              motor.kd,
                              motor.motor_control_tau,//前面计算的各个关节的力矩
                              arm.desir_grp);

还有一些小细节

因为机械臂有重量，所以输入各个关节的力矩如果单纯是阻抗力的话，效果可能不太好，所以还需要加上重力补偿的力：

    // 关节的控制力矩 = 重力补偿力矩 + 阻尼力矩
    arm.joint_control_tau[0] = arm.joint_gravity_tau[0] + 1.0 * arm.joint_impedance_tau[0];
    arm.joint_control_tau[1] = arm.joint_gravity_tau[1] + 1.0 * arm.joint_impedance_tau[1];
    arm.joint_control_tau[2] = arm.joint_gravity_tau[2] + 1.0 * arm.joint_impedance_tau[2];

通过手动修改各个轴上的力可以实现不同轴的力效果，比如一个常值，或者0：

Fx = 2*c * de[0] + 9*k * e[0]; //x轴
Fy = 2*c * de[1] + 9*k * e[1]; //y轴
Fz = 1; //z轴

实话实说

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

[小伊]

您的文章详细介绍了在轻擎2机械臂中实现基于电流环的末端刚度与阻尼控制的方法。这是一个非常实用且有趣的项目，下面是对您文章的总结和一些额外的思考：

总结

1. 项目目标：

   • 实现机械臂末端的刚度与阻尼控制，模拟弹簧和阻尼器的效果。

2. 控制模式：

   • 使用阻抗控制模式（Impdance）来实现目标。

3. 实现步骤：

   • 定义期望轨迹、期望刚度和阻尼。
   • 通过正运动学计算末端位置和速度。
   • 计算末端位置误差和速度误差。
   • 使用公式 力 = 刚度*位置误差 + 阻尼*速度误差 计算所需末端力。
   • 通过雅可比矩阵将末端力映射到关节力矩。
   • 将重力补偿力矩与阻抗力矩结合，输入到电机。

4. 代码实现：

   • 包括期望轨迹的生成、雅可比矩阵的计算、末端位置和速度的计算、误差计算、力的计算和力矩的映射等。

5. 注意事项：

   • 由于是电流环控制，且没有力矩传感器，输出力的精度可能受到影响。
   • 不同的轴向力的输出效果可能略有不同，可能与摩擦和重力有关。

额外思考

1. 力的精度提升：

   • 虽然电流环控制没有力矩传感器，但可以通过电机标定和机械臂摩擦辨识来提高力的精度。
   • 考虑使用更高级的控制算法，如自适应控制或鲁棒控制，以更好地应对不确定性。

2. 动态性能优化：

   • 在实际应用中，机械臂可能会遇到各种动态干扰，如外部冲击或负载变化。
   • 可以考虑引入观测器来估计机械臂的状态，如末端位置、速度和加速度，从而更准确地计算误差和所需的控制力。

3. 安全性考虑：

   • 在实现刚度与阻尼控制时，需要确保机械臂在与外部物体交互时不会造成过大的冲击力或损坏。
   • 可以设置力/力矩限制器，当检测到过大的力/力矩时，及时采取措施保护机械臂和外部物体。

4. 实验验证：

   • 通过实验来验证控制算法的有效性和性能。
   • 可以设计一系列实验场景，如与不同刚度物体的交互、不同速度下的运动等，来全面评估机械臂的表现。

5. 软件与硬件集成：

   • 确保上位机软件与机械臂硬件之间的稳定通信和数据传输。
   • 考虑使用实时操作系统（RTOS）来提高控制算法的实时性和稳定性。

总之，您的项目为实现机械臂的末端刚度与阻尼控制提供了一个很好的范例。通过进一步的优化和实验验证，可以进一步提升机械臂的性能和实用性。

---

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

[陆门马]

真不错，大佬对阻抗控制很有研究，学习了。