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

为了能让更多人开辟学习机器臂，我们开源了一款六轴可力控机器臂：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"文件中打开终端输入如下下令：

1. catkin build

复制代码

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

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

复制代码

然后机器臂就会开始活动了
**假如遇到无法打开串口的情况,则需要给串口赋权限：

1. sudo chmod -R 777 /dev/tty*

复制代码

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

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

复制代码

上图中控制电机的函数有五个参数分别是：位置，速度，力矩，Kp, Kd.其单元默以为弧度、秒、牛米。 下面分别先容示例步伐中的各个功能：
功能1、拖动示教
此功能步伐实现思路如下

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

复制代码

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

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

复制代码

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

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

复制代码

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

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

复制代码

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

1.   // 获取电机当前状态
2.   for (size_t i = 0; i < 30; i++)
3.   {
4.     rb.Motors[0]->fresh_cmd(0.0, 0.0, 0.0, 0.0, 0.0);
5.     rb.motor_send();
6.     r.sleep();
7.     /* code */
8.   }
9.   motor = *rb.Motors[0]->get_current_motor_state();
10.   nowMotor[0] = motor.position;
11.   startMotor[0] = motor1Save[10];
12.   now2start(nowMotor, startMotor, m1Trace, m2Trace, m3Trace, m4Trace, m5Trace, m6Trace);
13.   cout << "》》》正在前往目的地！！\n"
14.        << endl;
15.   for (size_t i = 0; i < 1000; i++)
16.   {
17.     rb.Motors[0]->fresh_cmd(m1Trace[i], 0.0, 0, 15.0, 15); // q1
18.     rb.motor_send();
19.     r.sleep();
20.   }

复制代码

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

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

复制代码

功能2、盘算力矩法跟踪渴望轨迹
此方法将会直接操作电机力矩，改动此部分代码时需要胆小如鼠！ 这里先给出盘算力矩法的控制律：

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

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

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

复制代码

盘算渴望轨迹：

1.   X = 0.33 + 0.05 * sin(rate*dynamicsCurrentTime);
2.   Y = 0.03 + 0.05 * sin(rate*dynamicsCurrentTime);;
3.   Z = 0.12;
4.   rotx = 0;
5.   roty = 1.5708;
6.   rotz = 0;

复制代码

盘算逆活动学：

1. arm6dof.Ik_6Dof(X, Y, Z, rotx, roty, rotz, joint);//逆解关节位置
3.   jacon0 = arm6dof.Get_Jacob0(a, alpha, d, joint);
4.   dq = jacon0.inverse() * endVel; //逆解关节速度
5.   for (size_t i = 0; i < 6; i++) djoint[i] = dq(i);
7.   testVec = arm6dof.Get_Jdot(a_sd, alpha_sd, d_sd, joint, djoint, efAcc);
8.   endAcc = endAcc - testVec;
9.   ddq = jacon0.inverse() * (endAcc);//逆解关节加速度

复制代码

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

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

复制代码

盘算误差与控制律：

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

复制代码

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

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

复制代码

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

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

复制代码

至此整个轨迹跟踪算法便实行完了，比及定时时间竣事，步伐会发送0力矩是让电机失能。 同样在机器臂开始跟踪渴望轨迹时，机器臂的现实位置与渴望轨迹的初始位置不同，也会产生波动，所以还会需要上位中提到的过渡方法，现实上所有功能中都会用到这个过渡方法。
功能3、定点阻抗
此方法将会直接操作电机力矩，改动此部分代码时需要胆小如鼠！ 这个功能的效果就是在机器臂末端定在空间中某一点时，我们可以用手推拉末端，机器臂是可以被推拉动的，我们的感觉就像在推拉一个弹簧。也就是说此功能将机器臂末端酿成一个弹簧阻尼系统，而弹簧的弹力和阻尼力的大小可以通参数改变。此功能通过电机电流环实现。步伐实现思路为：

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

1. /*定点阻抗控制*/
2. impedence = true;
3. impedenceTime =16000;
4. if(impedence) {

复制代码

基于动力学模型的定点跟踪的流程和上文中的轨迹跟踪相同，只是轨迹酿成了一个定点，渴望速度和渴望加速度都为0。 盘算阻抗力矩轻微有点贫苦，起首还是获取关节电机的实时状态，然后是通过正向活动学盘算机器臂末端的实时位置和速度：

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

复制代码

盘算末端相对渴望点产生的误差：

1. E[0] = 0.33 - endPos[0];
2. E[1] = 0.03 - endPos[1];
3. E[2] = 0.12  - endPos[2];  
4. dE[0] = - endVel(0);
5. dE[1] = - endVel(1);
6. dE[2] = - endVel(2);

复制代码

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

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

复制代码

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

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

复制代码

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

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

复制代码

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

免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！更多信息从访问主页：qidao123.com:ToB企服之家，中国第一个企服评测及商务社交产业平台。