基于Leapmotion的机械臂控制方案

本文发表于《无线电》杂志2017年第05期。
机械臂是不少人入门机器人制作的第一个项目，这也是我第一次比较近地接触机器人的控制项目，本以为和其他Arduino项目类似，只要找一个函数库，引用一下头文件，生成一个对象，就能很轻易地把机械臂操控自如了，然而真正做起来才发现机械臂有很多“坑”。

机械臂和舵机的选购

制作这个项目，首先得买一个机械臂。在网上买电子套件已经很正常了，实验室导师直接买了一个网上很常见的6自由度机械臂，我想着这东西既然都快遍地都是了，应该不至于会出大问题，然而碰到了第一个“坑”。
在机器人学中，假如你想要自如地控制机械臂或者描述你的手掌的姿态，你需要6个量，包括3个位置量和3个姿态量，也就是所谓的6个自由度。6个自由度不多不少正好能让机械臂复制你的手的姿态，这是有数学方法可以证明的。但是网上常见的机械臂所谓的6个自由度，其中5个是真的自由度，剩下一个通常是指夹子部分（见图1，这部分常被称为末端执行器）的开合，是不能算在自由度中的，也就是说6个舵机只有5个有效自由度，这最多只能算作6轴机械臂，而不是6自由度机械臂。
此外，你还要注意一下机械臂的材料，常见的有铝材、塑料、亚克力等。

其次是选购舵机（见图2）。买了机械臂之后，每个自由度一般是由一个舵机控制，而一般的舵机的旋转范围都是180°，这个范围不是大问题，问题主要在于舵机的扭力，也就是说它能转动又不会对自己造成损伤的最大的力。根据机械臂的大小和材料选择合适的舵机，最简单的原则就是：在预算范围内买最贵的舵机。

控制舵机

如果用其他单片机来写舵机控制程序，对于一般的初学者来说还是挺难的，但是在Arduino平台上实验（见图3），我们只要找到一个靠谱的Arduino库就行了，很方便的是，舵机有官方的库。

舵机这个类有6个成员函数，这里粗略地讲一下：

• Attach()：参数为一个引脚号，把一个引脚绑定到一个舵机的实例对象上去，注意这个库会限制PWM的使用，详细请看官方文档。
• Write()：参数是角度值，使舵机转到指定角度，比如0°度和180°就是两边，90°就是中间。
• WriteMicroseconds()：用微秒作参数来使舵机转到指定角度，一般舵机有1000~2000，或者700~2300的范围来转动，实际测试一下就行了，1500通常是中间值。
• Read()：返回该舵机对象当前的角度。
• Attached()：返回该舵机对象是否已经绑定了一个引脚。
• Detach()：解除这个舵机对象与引脚的绑定。

其中write函数默认就是把writeMicroseconds()函数中的500~2500映射到了角度中的0°~180°，这么一来，当你用write()来写角度时，很难对舵机进行很细致的操作，0°到1°会差很多。当我们需要它进行最小角度的转动时，修改一度的参数却远远不是它能达到的最小的变化角度，尤其是精细一点的舵机会更夸张，所以我在后面的代码中使用的都是writeMicroseconds()这个函数。

另外read函数比较“坑”，原本我以为返回的角度值是当前实时角度值，后来研究了一下这个库，发现返回的只是上次调用write()函数的最后一个角度值，真正使用时真的没什么用。
简单测试时，连上信号线、5V、GND，然后用官方例程（比如下面的sweep程序）跑一下、改一改，马上就能熟悉舵机了。

sweep程序

#include <Servo.h>
Servo myservo;  //创建一个舵机对象
int pos = 0;    //舵机位置变量
void setup() {
  myservo.attach(9);  //将舵机对象与9号引脚绑定
}
void loop() {
  for (pos = 0; pos <= 180; pos += 1) { 
    //从0°到180°变化，每个步长1°
    myservo.write(pos);  // 转动舵机至指定的位置
    delay(15);                      
  }
  for (pos = 180; pos >= 0; pos -= 1) { 
    //从180°到0°变化
    myservo.write(pos);              
    delay(15);                       
  }
}

舵机的电源和惯性

每个舵机都测试通过后，我装好6个舵机，给每个舵机都连上3条线，上传好程序，开始兴冲冲地调试，却发现舵机一抖一抖的，而且Arduino的指示灯一闪一闪的，一看就是没吃饱饭的样子。没错，这里要注意舵机的供电，因为给机械臂输出力真的是件挺费能量的事，尤其是当你给定的一些位置特别夸张时，这时你可能需要一个大功率的电源，可以考虑某些可调电源，也可以考虑某些笔记本电源加一个升压模块，注意电压在舵机允许范围内取相对高一点就行，一般是4.8~7V。

现在你可以让每个舵机按照你指定的角度转动，让机械臂做出各种酷炫的造型了。你偶尔还会发现有些角度下某几个舵机受力过大，开始发出抖动，不要紧张，这是正常现象（尤其是你买了廉价舵机时），而比较严重的问题是，你在一个程序里写下好几个造型的切换时，有几个切换动作比较大，而你设置的间隔时间又太短。机械臂还没到预想的形态，在转动过程中，就被你直接要求转动到另一个方向，这会导致它表演起来很丑，还会造成更加明显的惯性现象。想象一下，你想让舵机从0°转到180°，于是它全速运转，然而它刚转到150°时，你又让它转回0°，于是它又需要朝着另一个方向全速运转，可想而知，这之间需要克服的惯性以及对舵机造成的损伤之大。

为了解决这个问题，我首先想到，能不能得知舵机的当前角度值（当然不是通过read()函数）来判断舵机是否达到预期的姿态，等达到之后再做下一步变换。我查了点资料，发现有些贵一点的舵机会多一根线，这种舵机就能直接读取角度值，然而这种舵机也比一般的舵机贵上不少。另外，可以加装角度传感器应该也能解决问题，但我没有进行实践。

对于突然间大跨度转动造成的惯性问题，我们可以把大跨度改成小跨度，比如原来是从1°转到160°，一下子要跨越159°，我们可以把这159°分割成159份，每次转动1°，转动159次，这样确实可以有效地使机械臂转动起来更加平滑，惯性也更小。不过这个1°有时候并不是一个最小的数字，这里还是要用writeMicroseconds()这个函数，参数为从500~2500，每份小到10，就能让舵机稍微转动一个角度，使惯性更小。具体的每个小单位的值，需要在舵机和机械臂上事先调试得到，而且当这个值特别小时，转动时间也会相应变长，因此你也得在时间和惯性中做出取舍，达到最佳的流畅程度。

数据的传输格式和响应速度

对于Arduino而言，数据传输方式还是串口比较方便、可靠，测试完成后也能改成无线的Wi-Fi或蓝牙传输方式。那么问题来了，数据传输的格式应该是怎样的？每次发一个舵机的数据过去吗？这显然是不够方便也不够可靠的，我们可以想到通过一些字符来分割这6个舵机的数据，比如换行符、制表符等；Arduino串口有一个parseInt()函数用来分割串口接收到的数据，我在这里使用逗号来分割，每次计算机发送给Arduino的数据就会类似1800,1800,1500,800,1200,1200\n，后来发现一些舵机控制板用的也是类似的格式。

第一次测试时我使用的是Arduino UNO，前期测试时数据量小，性能上也没什么明显缺陷感，然而到后期，Leap Motion体感控制器实时计算的数据一发过来，Arduino UNO会有明显的卡壳感，有时候数据直接充满串口缓冲区，导致不少数据丢失，机械臂也会发生不同程度的抽风现象。于是我后来开始用Arduino MEGA，在以不太过分的速度发送串口数据的情况下，完全可以胜任机械臂的控制。我后来又买了一个以STM32为主控的舵机控制板，实际效果也没有比Arduino MEGA好很多。

考虑以上的所有的问题后，我们就能写出这个项目中的Arduino端代码了。首先在setup中，我们设定一个比较合适的初始位置(至少能让机械臂平稳地立在那个位置)；接着，在loop中，不断读取来自串口的数据，数据的格式类似1800,1800,1500,800,1200,1200\n，把给每个舵机的数据分割开之后，再用把大跨度移动分成小跨度移动的算法来控制舵机，这样就可以通过计算机实时、流畅地控制机械臂了。

#include <Servo.h> 
Servo myservo[6];	//舵机对象数组 
int i = 0; 
int oldpos = 0; 
int pos[6] = {1800,1800,1500,800,1200,1200};
//首次舵机的角度值，需要事先调试得到 
void setup() {   
    Serial.begin(9600); 
    Serial.println("start");   
    for(i=2;i<8;i++){     
        myservo[i-2].attach(i);//舵机绑定引脚   
    }   
    for(i=2;i<8;i++){     
        myservo[i-2].writeMicroseconds(pos[i-2]);//舵机角度初始化   
    } 
} 

void loop() {   
    while (Serial.available() > 0) {     
        i = 0;     
        while (i<6) {       
            pos[i] = Serial.parseInt();//获取来自串口的数据       
            i++;     
        }     
        if (Serial.read() == '\n') {       
            for(i=2;i<8;i++){         
                oldpos = myservo[i-2].readMicroseconds();//暂存一下上次的角度         
                if(oldpos > pos[i-2]){           
                    if(oldpos > pos[i-2]){//比较预期角度与上次的角度             
                        oldpos -= 50;//分割角度值             
                        myservo[i-2].writeMicroseconds(oldpos);//写入角度值             
                        delay(50);           
                    }         
                } else {           
                    if(oldpos == pos[i-2]){             
                        myservo[i-2].writeMicroseconds(oldpos);           
                    }           
                    if(oldpos < pos[i-2]) {             
                        oldpos += 50;             
                        myservo[i-2].writeMicroseconds(oldpos);             
                        delay(50);           
                    }         
                }       
            }     
        }   
    } 
}

LeapMotion

Arduino通过串口接收来自计算机的指令，由于Leap Motion体感控制器（见图4）是实时获取手势信息的，那么我们只要能够通过算法实时地计算出每个舵机的数据，然后把数据发送给Arduino，就能实现随动控制了。而且在Arduino端我们使用了分割大跨度移动为小跨度移动来驱动舵机的算法，实时地改变角度也就不会造成特别大的损伤。

那么Leap Motion作为一个体感控制器，我们究竟能从它身上获取到哪些信息呢？为方便起见，我在这里选择了它的Python库(主要是因为我当时还没开始学C++)。

Leap Motion首先会以自己为中心建立一个直角坐标系（见图5），它通过摄像头和红外传感器能得到很多细小的、相对不是很准确的数据，包括从手臂的肘部一直到手指末端所有部位（比如手肘、手指、关节、手掌）的位置和方向，位置是一个坐标量，方向就是一个向量（见图6）。同时它还可以识别手中的一些工具（比如笔）和一些简单的手势(包括画圈、点击、扫过)。

通过这些细致的数据和一些向量之间的计算，其实我们已经可以通过数学方法来完全并准确地描述使用者的手臂信息了，不过前面也说了，我的机械臂只有5个真正的自由度，无法完整地把手臂姿态复制出来，在3个姿态量和3个位置量中只能取5个量。我尽量把手肘的角度复制到机械臂上面的一段臂上，使它们的角度保持一致，同时通过计算手掌的方向和坐标系的角度控制机械臂的手掌部分跟随我的手掌转动，而这个机械臂的末端执行器，也就是那个小爪子是通过检测食指和大拇指的角度来进行控制的。

具体代码可以从《无线电》杂志网站www.radio.com.cn下载，其中很多数据是通过映射手势的角度到舵机的角度完成的，具体的数值需要事先调试才能得到。我数学不是太好，算法还有些粗糙，有机会会保持代码的更新。