高擎机电-开源六轴可力控机械臂

为了能让更多人开发学习机械臂，我们开源了一款六轴可力控机械臂：3d模型、代码全部开源！ 我们开源的机械臂控制算法示例中提供了：运动学、动力学、动力学控制算法（计算力矩法）、拖动示教、定点阻抗等功能。因为是基于ROS的，所以大家可以在此基础上做自己的开发应用，创造无限的可能！

代码链接：https://github.com/HighTorque-Robotics/livelybot_arm.git

视频链接：【开源-高擎生态可力控六轴机械臂】 开源-高擎生态可力控六轴机械臂_哔哩哔哩_bilibili

1、环境介绍

控制机械臂是在linux系统中，使用ros环境，我们已经写好了sdk，直接使用。

首先，确保有一个Linux系统。

其次，ros的版本为ros1-noetic。

然后，就可以使用我们的sdk了。

2、代码组成

下载下sdk文件后将会获得如下文件夹

文件结构如上图，我们需要打开的运行文件为：“testArm.cpp”，这个文件调用的文件为“6Dof_Model_dyna.h”和eigen库，也就是高擎机械臂所有算法都在这三个文件里面。其他文件是ros中编译相关的文件在此不做介绍。

运行程序的方法： 在"livelybot_new"文件中打开终端输入如下命令：

catkin build

每次打开新终端都要添加环境变量，然后再执行roslaunch

source ./devel/setup.bash 
roslaunch livelybot_bringup tstArm.launch

然后机械臂就会开始运动了

**如果遇到无法打开串口的情况,则需要给串口赋权限：

sudo chmod -R 777 /dev/tty*

3、代码介绍

高擎机械臂提供的示例代码中已有逆运动学、动力学、计算力矩法、拖动示教、定点阻抗的算法。 如果各位想用其他的机器人库可以自行下载配置，只需使用sdk中控制电机的函数即可:

rb.Motors[0]->fresh_cmd(Pos, Vel, Tor, Kp, Kd);
rb.motor_send();

上图中控制电机的函数有五个参数分别是：位置，速度，力矩，Kp, Kd.其单位默认为弧度、秒、牛米。 下面分别介绍示例程序中的各个功能：

功能1、拖动示教

此功能程序实现思路如下

*重力补偿和拖动示教，*/
teachTime = 11000; // 11秒
teach = false;
if (teach)
{

teachTime 表示运行时间，给teach 变量赋“true”时就会运行拖动示教算法。 重力补偿是在拖动的时候运行的，目的是让拖动机械臂时更具轻松。其实现方法是通过实时的获取电机位置和速度，然后带入动力学计算出补偿力矩，最后将补偿力矩输入电机。 下面是获取实时的电机位置速度的代码。

    motor = *rb.Motors[0]->get_current_motor_state();
    motor1Save[teachCount] = motor.position;
    motord1Save[teachCount] = motor.velocity; 

下面是计算补偿力矩和将力矩输入给电机的代码。

torque = arm6dof.Get_Tor(a_sd, alpha_sd, d_sd, joint, jointd, jointdd);
rb.Motors[0]->fresh_cmd(0.0, 0.0, torque[0], 0.0, 0.0);
rb.motor_send();

拖动机械臂时一方面将关节的位置速度数据输入动力学计算用于重力补偿，一方面储存起来，当进入重播阶段时只需要将储存的数据输入电机即可。在重播阶段只需要执行“motor*save”数组里面的轨迹即可。代码如下：

    rb.Motors[0]->fresh_cmd(motor1Save[i], motord1Save[i], 0, 15.0, 15); // q1
    rb.Motors[1]->fresh_cmd(motor2Save[i], motord2Save[i], 0, 15.0, 15); // q2
    rb.Motors[2]->fresh_cmd(motor3Save[i], motord1Save[i], 0, 15.0, 15); // q3
    rb.Motors[3]->fresh_cmd(motor4Save[i], motord4Save[i], 0, 8.0, 4);   // q4
    rb.Motors[4]->fresh_cmd(motor5Save[i], motord5Save[i], 0, 8.0, 4);   // q5
    rb.Motors[5]->fresh_cmd(motor6Save[i], motord6Save[i], 0, 8.0, 4);   // q6
    rb.motor_send();

此时示教的整个过程就执行完了，但是有一个问题：当拖动的轨迹的初始位置和结束位置不相同，重播时就需要瞬间让机械臂从结束位置到达初始位置，这会导致机械臂产生巨大的波动。那么就需要一个方法将机械臂从结束位置缓慢的过渡到初始位置。所以就有了下面一段代码

  // 获取电机当前状态
  for (size_t i = 0; i < 30; i++)
  {
    rb.Motors[0]->fresh_cmd(0.0, 0.0, 0.0, 0.0, 0.0);
    rb.motor_send();
    r.sleep();
    /* code */
  }
  motor = *rb.Motors[0]->get_current_motor_state();
  nowMotor[0] = motor.position;
  startMotor[0] = motor1Save[10];
  now2start(nowMotor, startMotor, m1Trace, m2Trace, m3Trace, m4Trace, m5Trace, m6Trace);
  cout << "》》》正在前往目的地！！\n"
       << endl;
  for (size_t i = 0; i < 1000; i++)
  {
    rb.Motors[0]->fresh_cmd(m1Trace[i], 0.0, 0, 15.0, 15); // q1
    rb.motor_send();
    r.sleep();
  }

为了简洁只贴上了其中一个电机的处理方法。其大体思路是获得电机当前位置和期望到达的位置，对应拖动轨迹的结束位置和开始位置，然后做一个简单的线性插值就是函数“now2start”，也就是获得关节空间的一个轨迹，然后关节执行这个轨迹即可，如上面程序的第18行。这里设定是1秒内到达期望位置，也就是到达拖动的初始位置，之后再重播拖动的轨迹就可以无缝连接了。 当拖动示教的时间结束后，程序会向电机发生0力矩从而失能电机，防止电机持续负载或发生意外。

  rb.Motors[0]->fresh_cmd(0.0, 0.0, 0.0, 0.0, 0.0);
  rb.Motors[1]->fresh_cmd(0.0, 0.0, 0.0, 0.0, 0.0);
  rb.Motors[2]->fresh_cmd(0.0, 0.0, 0.0, 0.0, 0.0);
  rb.Motors[3]->fresh_cmd(0.0, 0.0, 0.0, 0.0, 0.0);
  rb.Motors[4]->fresh_cmd(0.0, 0.0, 0.0, 0.0, 0.0);
  rb.Motors[5]->fresh_cmd(0.0, 0.0, 0.0, 0.0, 0.0);
  rb.motor_send();

功能2、计算力矩法跟踪期望轨迹

此方法将会直接操作电机力矩，改动此部分代码时需要小心谨慎！ 这里先给出计算力矩法的控制律：

方程左边的力矩就是要输入电机的力矩。其中e表示期望关节位置与实际关节位置的差。程序实现的思路为：

同理，设置运行时间，设置bool值，即可运行动力学算法。注意要把其他功能的bool值设置为false。

/*动力学控制，计算力矩法,纯力矩控制*/
dynamicsTime = 12000;
dynamics = false;
if (dynamics)
{

计算期望轨迹：

  X = 0.33 + 0.05 * sin(rate*dynamicsCurrentTime);
  Y = 0.03 + 0.05 * sin(rate*dynamicsCurrentTime);;
  Z = 0.12;
  rotx = 0;
  roty = 1.5708;
  rotz = 0;

计算逆运动学：

 arm6dof.Ik_6Dof(X, Y, Z, rotx, roty, rotz, joint);//逆解关节位置

  jacon0 = arm6dof.Get_Jacob0(a, alpha, d, joint);
  dq = jacon0.inverse() * endVel; //逆解关节速度
  for (size_t i = 0; i < 6; i++) djoint[i] = dq(i);

  testVec = arm6dof.Get_Jdot(a_sd, alpha_sd, d_sd, joint, djoint, efAcc);
  endAcc = endAcc - testVec;
  ddq = jacon0.inverse() * (endAcc);//逆解关节加速度

这里逆运动学计算出来的关节状态为期望值，此外我们还需要关节反馈的实时状态也就是更新关节是实时状态，代码如下：

  motor = *rb.Motors[0]->get_current_motor_state();
  motorRecv[0] = motor.position;
  motordRecv[0] = motor.velocity; // q6 and dq6

计算误差与控制律：

    for (size_t i = 0; i < 6; i++)
    {
      e[i] = joint[i] - jointRecv[i];
      de[i] = djoint[i] - jointdRecv[i];
      u[i] = ddjoint[i] + Kp[i]*e[i] + Kd[i]*de[i];
    }
  //5，带入动力学模型
  torque = arm6dof.Get_Tor(a_sd, alpha_sd, d_sd, jointRecv, jointdRecv, u);

然后将计算出的力矩输入给电机就好了。但是由于某些关节的转动方向和电机的转动方向不同所以需要：

 motorTor[0] = torque(0); motorTor[1] = -torque(1);    
 motorTor[2] = torque(2); motorTor[3] = -torque(3);     
 motorTor[4] = torque(4); motorTor[5] = torque(5);

也就是说关节2和关节4的转动方向和电机的转动方向相反，需要给模型计算出的力矩加个负号才能让机械臂的电机正常运行：

  //6，输入关节力矩      
rb.Motors[0]->fresh_cmd(0.0, 0.0, 1*motorTor[0], 0.0, 0.0);      
rb.Motors[1]->fresh_cmd(0.0, 0.0, 1*motorTor[1], 0.0, 0.0);      
rb.Motors[2]->fresh_cmd(0.0, 0.0, 1*motorTor[2], 0.0, 0.0);      
rb.Motors[3]->fresh_cmd(0.0, 0.0, 1*motorTor[3], 0.0, 0.0);      
rb.Motors[4]->fresh_cmd(0.0, 0.0, 1*motorTor[4], 0.0, 0.0);      
rb.Motors[5]->fresh_cmd(0.0, 0.0, 1*motorTor[5], 0.0, 0.0);      
rb.motor_send();

至此整个轨迹跟踪算法便执行完了，等到定时时间结束，程序会发送0力矩是让电机失能。 同样在机械臂开始跟踪期望轨迹时，机械臂的实际位置与期望轨迹的初始位置不同，也会产生波动，所以还会需要上位中提到的过渡方法，实际上所有功能中都会用到这个过渡方法。

功能3、定点阻抗

此方法将会直接操作电机力矩，改动此部分代码时需要小心谨慎！ 这个功能的效果就是在机械臂末端定在空间中某一点时，我们可以用手推拉末端，机械臂是可以被推拉动的，我们的感觉就像在推拉一个弹簧。也就是说此功能将机械臂末端变成一个弹簧阻尼系统，而弹簧的弹力和阻尼力的大小可以通参数改变。此功能通过电机电流环实现。程序实现思路为：

这个功能有两个部分组成：基于动力学模型的定点跟踪算法和计算阻抗力矩。 首先还是要设置时间和bool值：

/*定点阻抗控制*/ 
impedence = true; 
impedenceTime =16000; 
if(impedence) {

基于动力学模型的定点跟踪的流程和上文中的轨迹跟踪相同，只是轨迹变成了一个定点，期望速度和期望加速度都为0。 计算阻抗力矩稍微有点麻烦，首先还是获取关节电机的实时状态，然后是通过正向运动学计算机械臂末端的实时位置和速度：

 jacon0 = arm6dof.Get_Jacob0(a, alpha, d, jointRecv);  
arm6dof.Get_endPos(a, alpha, d, jointRecv, endPos);//获取末端实时位置  
endVel = jacon0*jointVel;//获取末端实时速度

计算末端相对期望点产生的误差：

E[0] = 0.33 - endPos[0]; 
E[1] = 0.03 - endPos[1]; 
E[2] = 0.12  - endPos[2];  
dE[0] = - endVel(0); 
dE[1] = - endVel(1); 
dE[2] = - endVel(2);

带入阻抗方程计算对应的力：

 FX = elasticX*E[0] + dampingX*dE[0]; 
 FY = elasticY*E[1] + dampingY*dE[1];  
FZ = elasticZ*E[2] + dampingZ*dE[2];  
 endTor_V(0) = FX;  endTor_V(1) = FY; 
 endTor_V(2) = FZ;  
endTor_V(3) = 0;  
endTor_V(4) = 0;  
endTor_V(5) = 0;

这里我们只操作力，不操作末端力矩。所以在末端力向量中后三位为0。然后通过雅可比矩阵将末端力映射到关节力矩

impTor = jacon0.transpose()*endTor_V;//获取末端阻抗对应的关节阻抗

最后将这个力矩与定点控制计算出的力矩相加，输入电机即可。 为了能体现出阻尼的效果，给定点控制的力矩增加了限制，只要控制力矩超过这个限制（limitTor）就不会增加了：

for (size_t i = 0; i < 6; i++) 
{   if(motorTor[i]>=(torRef(i)+limitTor[i])) motorTor[i] = torRef(i)+limitTor[i]; 
  if(motorTor[i]<=(torRef(i)-limitTor[i])) motorTor[i] = torRef(i)-limitTor[i];  
 /* code */ }

以上功能就是使用我们提供的算法实现的内容，大家可以使用自己的算法实现更多更好的功能！