🤛🏻 👩🏻‍🏭 🧝🏼 ROS和神经网格乞eg机器人 🛀🏾 🕺🏾 👨🏿‍🌾

通常，此类工艺品会出现两个这样的问题：“如何？”为了什么？”出版物本身专门针对第一个问题，我将立即回答第二个问题：

我开始这个项目是为了掌握机器人技术，从Raspberry Pi和相机开始。如您所知，最好的学习方法之一就是提出一项技术任务并尝试完成它，同时获得必要的技能。

那时，我在机器人技术领域还没有聪明的主意，因此我决定做一个非常有趣的项目-乞robot机器人。结果是在Raspberry Pi和ROS上创建了一个独立的机器人，使用Movidius神经采样棒检测面部。他在房间里徘徊，寻找人，并在他们面前摇摇罐子。这是此机器人的外观：

机器人会在房间内随机移动，如果发现有人，它就会卷起并摇晃罐子里的小东西。为了娱乐，我给他添加了一个小表情-他知道如何移动眉毛：

第一次尝试后，机器人试图再次发现他的脸，然后转向该人，然后再次摇晃银行。但是，如果您此时离开，将会发生什么：

机器人

我从《大众力学》杂志上想到了一个乞讨机器人。克里斯·埃克特（Chris Eckert）的原型作者叫金姆（Gimme），在美学上看起来非常令人满意。

我想将更多精力放在功能上，因此该机壳是用即兴材料组装而成的。特别是，PVC角被证明是用途最广泛的材料，几乎可以将其用于连接任何两个部分。目前看来，该机器人的百分之五由PVC角和M3螺丝组成。外壳本身由三个层压板平台组成，在其上安装了头部和所有电子设备。

该机器人的基础是Raspberry Pi 2B，代码是用C ++编写的，位于GitHub上。

视力

为了感知现实，机器人使用了Paspberry Pi Camera Module v2相机，可以使用RaspiCam库对其进行控制。

对于面部检测，我尝试了几种不同的方法。 OpenCV的经典检测器的质量令我不满意，因此最终我得到了一个非标准的解决方案。在控制框架OpenVINO上运行Movidius神经计算棒（NCS）的设备上检测参与神经网络的人员。

NCS是用于有效启动神经网络的硬件，其中有专门针对此设计的多个矢量处理器。该设备通过USB连接，仅消耗1瓦的功率。因此，NCS充当Raspberry Pi的协处理器，Raspberry Pi不拉神经网络。当NCS处理下一帧时，Paspberry处理器可用于其他操作。值得注意的是，为了使设备达到最佳操作，需要USB 3.0接口，而在较早版本的Raspberry中则不可用；使用USB 2.0也可以，但速度较慢。另外，为了不阻塞Raspberry USB连接器，我通过短USB电缆将NCS连接到它。在上一篇文章中，我详细介绍了如何使用神经计算棒。

一开始我想训练拥有基于MobileNet + SSD架构的开放式数据集的人脸检测器。检测器确实可以工作，但不是很稳定：随着拍摄条件的不可避免的恶化（曝光和模糊的拍摄），检测器的质量大大下降。但是，经过一段时间后，OpenVINO中出现了现成的人脸检测器，我切换到了具有SqueezeNet light + SSD架构的检测器，该检测器不仅在各种拍摄条件下都工作得更好，而且速度更快。

在将图像上传到NCS以获得探测器的预测之前，必须对图像进行预处理。我选择的检测器适用于彩色图像

300 \times 300

$300 \times 300$ ，因此需要首先压缩图像。为此，我使用了最轻的缩放算法-最近的邻居方法（OpenCV库中的INTER_NEAREST）。它比插值方法快一点，并且几乎不影响结果。还要注意图像通道的顺序：检测器期望BGR顺序，因此您需要为摄像机设置相同的顺序。

我还尝试将视频处理分为两个流，一个流从摄像机接收下一帧并对其进行处理，另一个在那个时候将前一帧上载到NCS并等待检测器结果。通过该方案，从技术上讲，处理速度提高了，但是接收帧和接收对其的检测之间的延迟也增加了。由于这种落后于现实，监视面部变得更加困难，因此最终我拒绝了该方案。

除了实际检测人脸外，还需要对其进行跟踪以避免检测器错误。为此，我使用了轻量级的跟踪器Simple Online Realtime Tracker（SORT）。这个简单的跟踪器由两部分组成：匈牙利算法用于匹配相邻帧上的对象，并预测物体的轨迹（如果它突然消失的话）- 卡尔曼滤波器。当我玩脸跟踪时，我发现卡尔曼滤波器预测的轨迹在突然移动的情况下是非常难以置信的，这再次使过程变得复杂。

因此，我关闭了卡尔曼滤波器，只留下了面部匹配算法和检测到面部的连续帧数的计数器-这样，我就摆脱了检测器的误报。

上平台，从左至右：摄像头，伺服器，用于控制头部和眉毛，开关，电源端子，红色大按钮。

交通

为了运动，该机器人具有五个伺服器：两个FS5103R连续旋转伺服器使车轮旋转；还有另外两种普通的FS5109M，其中一种旋转磁头，另一种旋转罐子。最后，SG90小型机动了动眉。

老实说，SG90微型伺服器对我来说就像是垃圾-我的一个伺服器的控制脉冲宽度错误，在其他四个伺服器中只有一个幸免了。公平地讲，我不小心抓住了其中一个仆人的肘部，而另外两个仆人根本无法承受重担（我曾经用它们来做头部和罐头）。即使最后一个完成最简单工作的伺服器-移动眉毛，也必须时不时地戳一根棍子，以使其不会楔入。对于其他伺服器，我没有发现任何问题。确实，有时必须对连续旋转伺服器进行校准，以使它们不会在非活动状态下旋转-为此，在其上有一个小型调节器，可以使用带时钟的螺丝刀将其旋转。

事实证明，使用Raspberry管理伺服器并不是那么简单。首先，他们受到脉宽调制（PWM / PWM），并且在Raspberry上，只有两个引脚受硬件支持PWM。其次，当然，Raspberry将无法为伺服器供电，它将无法承受。幸运的是，使用外部PWM控制器可以解决这些问题。

Adafruit PCA9685是一个16通道PWM控制器，可以通过I2C接口进行控制。它具有用于为伺服器供电的端子也非常方便。此外，[理论上]可以链接多达62个控制器，同时最多接收992个控制引脚-为此，您需要使用特殊的跳线为每个控制器分配一个唯一的地址。因此，如果您突然需要大量的伺服器-您就会知道该怎么做。

为了控制PCA9685，有一个充当WiringPi扩展的高级库。。使用此工具非常方便-在初始化期间，它将创建16个虚拟引脚，您可以在这些虚拟引脚中写入PWM信号，但首先必须计算滴答声的数量。要将伺服杆旋转到[0，180]范围内的某个角度，您必须首先将该角度转换为以毫秒为单位的控制脉冲长度范围[SERVO_MS_MIN，SERVO_MS_MAX]。对于我所有的伺服器，这些值分别约为0.6 ms和2.4 ms。通常，这些值可以在伺服数据表中找到，但实践表明它们可能有所不同，因此可能需要选择它们。然后将结果值除以20 ms（控制周期长度的标准值），然后乘以最大滴答数PCA9685（4096）：

void driveDegs(float angle, int pin) {
    int ticks = (int) (PCA_MAX_PWM * (angle/180.0f*(SERVO_MS_MAX-SERVO_MS_MIN) + SERVO_MS_MIN) / 20.0f); 
    pwmWrite(pin, ticks);
}

同样，这是通过连续旋转伺服器完成的-代替角度，我们将速度设置为[-1,1]。

我用简易的方法组装了机器人的底盘和车身：将家具轮放在连续旋转的伺服驱动器上，并将家具球支撑架用作第三个轮子。以前，轮子代替轮子，而是靠在旋转的支架上，但是有了这样的底盘，很难精确地转弯，所以我不得不更换轮子。罐子下方还有一个小轮子，可将部分重量从伺服器传递到壳体。一开始对我来说不明显的简单事情是，伺服杆必须用螺钉固定，尤其是对于车轮，以免它们沿途脱落。由于如此愚蠢，我不得不重做一次机箱。我还使机器人成为了一个由PVC角制成的宽保险杠，这样它就不会经常卡住了。

现在介绍您可以采取的措施。首先，您可以摇动罐子并移动眉毛-为此，您只需要将伺服杆旋转到预先选择的角度即可。

其次，您可以旋转头。我不希望磁头以伺服器的最大速度旋转，因为它上面装有摄像头。因此，我决定以编程方式降低速度：我需要将操纵杆旋转一个小角度，然后等待几毫秒-依此类推，直到达到所需的角度为止。在这种情况下，有必要记住磁头的当前绝对位置，并且每次检查磁头的绝对位置是否已超过允许的限制（在我的机器人上，该绝对位置在[10，90]度范围内）。

第三，您可以通过改变轮子的旋转速度来改变运动方向。以相同的方式，您可以旋转平台，例如，跟随面部。旋转角速度既取决于伺服器本身，也取决于其在机架上的位置，因此更容易进行一次测量，然后在转弯时将其考虑在内。要找到打开电动机以使其旋转和关闭电动机之间的必要延迟，需要将角度模块除以角速度。

最后，您可以同时异步旋转头部和底盘，以免浪费时间。我这样做是这样的：

auto waitRotation = std::async(std::launch::async, rotatePlatform, platformAngle);
success = driveHead(headAngle);
waitRotation.wait();

中央平台，从左至右：PCA9685，电源总线，Raspberry Pi，MCP3008 ADC

然后，我没有使任何事情复杂化，因此该机器人仅使用两个Sharp GP2Y0A02YK红外测距仪进行导航。这也不是那么简单，因为传感器是模拟的，但是与Arduino不同，Raspberry 没有模拟输入。该问题由模数转换器（ADC / ADC）解决-我使用10位，8通道MCP3008。它作为单独的微电路出售，因此必须焊接到印刷电路板上，并且引脚也要焊接在那里，以使其连接起来更加方便。另外，根据我对电路更加困惑的巴蒂的建议，我在电源的两脚与地面之间焊接了两个电容器（陶瓷和电解电容器），以吸收整个电路数字部分的噪声。传感器在输出端输出的电压不超过3伏，因此可以将3.3v Raspberry连接为基准ADC电压（VREF）-与MCP3008（VDD）电源相同。

MCP3008可以通过SPI接口进行控制，为此，甚至可以在GitHub上轻松找到现成的代码。

尽管如此，为了方便使用ADC，您将需要一些手鼓跳舞。

unsigned int analogRead(mcp3008Spi &adc, unsigned char channel)
{
    unsigned char spi_data[3];
    unsigned int val = 0;

    spi_data[0] = 1;  // start bit
    spi_data[1] = 0b10000000 | ( channel << 4); // mode and channel
    spi_data[2] = 0; // anything
    adc.spiWriteRead(spi_data, sizeof(spi_data));
  
    // read value, combine last two bits of second byte with whole third byte
    val = (spi_data[1]<< 8) & 0b1100000000; 
    val |= (spi_data[2] & 0xff);
    return val;
}

必须将三个字节发送到MCP3008，第一个字节写入起始位，第二个字节写入模式和通道号（0-7）。我们还返回了三个字节，之后我们需要将第二个字节的两个最低有效位与第三个字节的所有位粘合在一起。

现在我们可以从传感器获取值，我们需要对其进行校准，因为两个传感器可能会略有不同。通常，由于这些传感器的信号强度，从远处显示是非线性的，并且不是很简单（有关更多详细信息，请参见数据表，pdf）。因此，只要乘以两个系数就足够了，传感器乘以两个有意义的相等距离后的值将为1.0。

传感器读数可能非常嘈杂，尤其是在困难的障碍物上，因此我使用指数加权移动平均值（EWMA）来平滑每个传感器的信号。我通过肉眼选择了平滑参数，以使信号不会产生噪声，也不会远远落后于实际情况。

前视图：银行，测距仪和保险杠。

营养

首先，让我们评估一下机器人将消耗的电流（大约是Raspberry和外围设备的电流消耗）：

Raspberry Pi 2B：不小于350 mA，但在负载下更大（高达750-820 mA（？））；
相机：约250 mA；
神经计算棒：声明的功耗为1瓦，USB上的电压为5伏时为200 mA；
红外传感器：每个33 mA（数据表，pdf）;
MCP3008: , 0.5 (, pdf);
PCA9685: , 6 (, pdf);
: ~150-200 1500-2000 (stall current), ( FS5109M, pdf)
HDMI ( ): 50 ;
+ ( ): ~200 .

总体而言，只要所有伺服器在重负载下都不同时运动，则可以估计1.5-2.5安培就足够了。同时，Raspberry需要有条件的5伏电压，而对于伺服器则需要4.8-6伏。仍然需要找到满足这些要求的电源。

结果，我决定使用18650格式电池为机器人供电，如果您使用两节ROBITON 3.4 / Li18650电池（3.6伏，3400毫安，最大放电电流4875毫安）并串联连接，它们可以在7.2伏的电压下产生高达4.8安培的电流。在1.5-2.5安培的消耗电流下，它们应该足够一两个小时。

顺便说一句，电池有一个问题：尽管有18650的外形规格，但它们的尺寸还是远远不够。

18 \times 650

$18 \times 650$ 毫米-由于内置的充电控制电路，它们长了几毫米。因此，我不得不用刀刺电池盒，使其适合那里。

只能将电压降低到5伏。为此，我使用两个单独的降压DC-DC转换器DFRobot电源模块。这块铁可以让您降低输入电压3.6-25伏和电压差至少0.6伏时的电压。为方便起见，它具有一个开关，可让您在输出端选择恰好5伏的电压，或者您可以使用特殊的调节器配置任意输出电压。我将两个转换器都设为5伏；其中一个通过Micro-USB连接器馈入Raspberry，第二个通过PCA9685端子馈入伺服。为了使机器人的逻辑部分和电源部分的电源最大化，以便它们不会相互干扰，这是必需的。

在调试阶段，我使用了9伏2安的中文电源代替电池，这足以使机器人正常工作-我像电池一样将其连接到两个DC-DC转换器。因此，为方便起见，我在机器人上制作了端子，您可以在其中连接电源或电池仓以进行选择。当我完全重写了ROS上的所有代码时，这非常有用，而且我不得不调试机器人很长时间，包括伺服器。

为了方便起见，我还必须制作一个“电源总线”-实际上，只有一块板上有三排分别用于接地的3.3v和5v连接引脚。总线连接到相应的Raspberry引脚。仅红外测距仪由5v总线供电，MCP3008和PCA9685由3.3v总线供电。

当然，根据良好的传统，我在机器人上放了红色大按钮-按下机器人时，它只会中断整个电源电路。不必将其用于紧急停止，但是借助按钮打开机器人确实更加方便。

下部平台，从左到右：电池仓，NCS，DC-DC转换器，带轮的伺服驱动器，测距仪。

机械手控制

Raspberry Pi 2B上没有Wi-Fi，因此我必须通过ssh通过以太网电缆进行连接（顺便说一下，这可以直接在笔记本电脑上完成，而无需使用路由器）。事实证明是这样的方案：我们通过电缆通过ssh连接，启动机器人并拔下电缆。然后可以将其返回到其位置以再次访问Raspberry。有更多优雅的解决方案，但我决定不复杂。

为了使机器人可以容易地停止而不关闭，我添加了一个巨大的苏联开关（是从潜水艇上来的？），当您关闭它时，程序结束并且机器人停止了。

开关连接到地面和Raspberry GPIO引脚之一，您可以使用WiringPi库从中读取：

wiringPiSetup();
pinMode(PIN_SWITCH, INPUT);
pullUpDnControl(PIN_SWITCH, PUD_UP);
bool value = digitalRead(BB_PIN_SWITCH);

值得注意的是，通过这种连接，必须将引脚上的电压上拉至3.3v，同时在断开状态下将产生高信号，在闭合状态下将产生低信号。

放在一起

线程

现在，以上所有内容都需要组合成一个程序来控制机器人。在机器人的第一个版本中，我使用线程（pthread）进行了此操作。这个版本在master分支中，但是那里的代码非常可怕。

该程序有四个线程：一个线程从摄像机获取帧，然后在NCS上启动检测器；另一个线程则从NCS启动。第二个流从测距仪读取数据；第三个线程监视开关并将全局变量设置is_running为false如果关闭了；主线程负责机器人的行为和伺服控制。线程具有与主线程相同的指针，通过它们可以写入工作结果。我将存储有关检测器发现的面部信息的向量限制为互斥体，并将其他更简单的公共变量声明为atomic。为了协调人脸检测器与主线程的流程，有一个标志face_processed会在检测器收到新结果时重置，并在主线程使用此结果选择行为时升起-这是必要的，以便不处理可能不相关的旧数据移动后。带有流

的ROS

版本工作正常，但是我开始所有这些是为了学习一些东西，所以为什么不同时掌握罗斯？我已经听了很长时间这个框架了，甚至在hackathon上我都不得不使用它，所以最终我决定重写ROS上的所有代码。此版本的代码位于ros的默认分支中，并且看起来更加不错。显然，由于发送消息和其他所有事务的开销，ROS上的实现几乎肯定会比流上的实现慢-唯一的问题是多少？

ROS概念

ROS (Robot Operating System) — , , , .

, , , (node), , , .

(topic) (message) , - .

— (service). , , . « », .

.msg .srv . .

ROS .

对于我的机器人，我没有将任何现成的程序包与ROS的算法一起使用，而是仅将机器人代码设计在一个单独的程序包中，该程序包包含五个使用消息和ROS服务相互通信的节点。

最简单的节点switch_node监视交换机的状态。交换机关闭后，该节点便开始发送垃圾邮件，该垃圾邮件bool中的主题类型为terminator。这是向主节点发出的信号，该该该完成工作了。

第二个节点sensor_node定期读取两个红外测距仪的读数，并通过sensor_state一条消息将其发送到主题。同样，此节点负责信号处理：通过校准因子和移动平均值进行缩放。

第三结camera_node他负责与脸部相关的所有事情：他从相机拍摄图像，对其进行处理，接收检测器的结果，将它们通过跟踪器传递，然后找到最靠近镜框中心的脸部-机器人无论如何都不会使用其余的脸部，但是您想发出较小的消息。节点发送给主题的消息camera_state包含帧号，有脸的事实（因为您还需要知道没有脸），左上角的相对坐标，脸的宽度和高度。这就是文件中消息类型的描述的样子DetectionBox.msg：

int64 count
bool present
float32 x
float32 y
float32 width
float32 height

第四个节点servo_node负责伺服。首先，它支持一项服务servo_action，该服务允许伺服器按其编号执行一项操作：将整个节点置于其初始状态（眉毛，库，头，停止机箱）；将磁头转移到其初始状态；摇动罐子；用一根眉毛描绘三个表情（善良，中立，邪恶）之一。其次，使用该服务，servo_speed您可以通过在请求中发送两个车轮来设置新的速度。两种服务均不返回任何内容。最后，有一项服务servo_head_platform可让您相对于当前位置旋转头部和/或底盘一定角度。true如果有可能至少部分转动头部，则此服务返回，并且false否则，如果头部已经在允许角度的边界上，我们将尝试进一步旋转它。如果请求中的两个角度都不为零，则服务将异步旋转，如上所述。在主循环中，伺服节点不执行任何操作。

例如，这里是服务的描述servo_head_platform：

float32 head_delta
float32 platform_delta
---
bool head_success

列出的每个节点都支持terminate_{switch, camera, sensor, servo}带有空响应请求的服务，该服务将停止该节点的操作。它是通过以下方式实现的：

一些代码

...
std::atomic_bool is_running; // global

bool terminate_node(std_srvs::Empty::Request &req, std_srvs::Empty::Response &ignored) {
    is_running = false;
    return true;
}

int main(int argc, char **argv) {
    is_running = true;
    ...
    while (is_running && ros::ok()) {
        // do stuff
    }
    ...
}

该节点具有全局变量is_running，其值确定节点的主循环。该服务仅重置此变量，并且主循环被中断。

还有一个主要节点，beggar_bot在其中实现了机器人的基本逻辑。在主循环开始之前，它订阅主题sensor_state并将camera_state消息的内容保存在回调函数的全局变量中。他还订阅了该主题terminator，该主题的回调将重置标志is_running，从而中断主循环。同样，在周期开始之前，节点会宣布伺服节点的服务接口，并等待几秒钟，其他节点才能启动。主循环结束后，此节点调用服务terminate_{switch, camera, sensor, servo}，因此先关闭所有其他节点，然后再将其自身关闭。也就是说，当开关关闭时，所有五个节点都将完成操作。

改用ROS迫使我改变了程序的结构。例如，更早的时候可以高频改变轮速，但效果很好，但是ROS服务的运行速度慢了一个数量级，因此我不得不重写代码，以便仅在速度真正改变时才调用该服务（在``惰性模式''下）。

ROS还使您可以非常方便地运行机械手的所有节点。为此，您需要编写一个.launch启动文件，以xml格式列出该机械手的所有节点和其他属性，然后运行以下命令：

roslaunch beggar_bot robot.launch

ROS vs. pthread

现在，最后，您可以比较ROS版本和pthread版本的速度。我这样做是这样的：只要其他所有功能都可以正常工作，负责与相机一起工作的线程/节点就会考虑其FPS（是最慢的元素）。对于pthread版本，我一直获得FPS 9.99左右，而对于ROS版本，我发现它约为8.3。实际上，对于这样的玩具，这已经足够了，但是开销非常明显。

机器人行为

这个想法很简单：如果机器人看到一个人，他必须开车到他身边并摇动罐子。摇晃罐子非常简单且有趣，但是首先需要与对方联系。

有一个功能follow_face是，如果框架中有一个面，则会沿其方向旋转底盘和机器人头（仅考虑最靠近中心的面）。这是必要的，以便机器人始终在一个人身上保持行进路线（如果他在框架中），并且在摇动罐子时也可以直视面部。

使用camera_state相同的变量将此功能与主题同步。face_processed，如带有流的版本。想法是一样的-我们只想处理一次数据，因为机器人一直在移动。该函数首先等待，直到检测到主题的回调降低了已处理最后一帧的标志。在等待期间，她不断调用ros::spinOnce()以接收新消息（通常，应在程序需要新数据的任何地方进行此操作）。如果框架中有人脸，则需要计算角度，这需要旋转平台和头部-这可以通过了解人脸中心的相对坐标以及水平和垂直方向上摄像机的视场来完成。之后，您可以致电服务servo_head_platform并移动机器人。

有一个微妙的要点：有关脸部位置的信息落后于脸部的实际运动，并且可能落后于机器人本身的运动。因此，机器人可能会高估旋转角度，因此，头部开始以增加的幅度来回移动。为防止这种情况，我在移动后进行了延迟（300毫秒），并在移动后也跳过了一帧。出于相同的目的，将底盘和机头的旋转角度乘以0.8倍（PID控制器的P分量有意义）。

功能follow_face返回一个人的状态。一个人可以：不在，离中心足够近，离机器人太远；另一个选择-当我们转动机器人而又不知道脸部发生了什么（在搜索过程中）时；头部位于边界上的情况仍然很少见，这就是为什么无法转向面部的原因。

一个很简单的事情发生在主循环中：

呼叫follow_face直到此人具有某种状态（“在搜索过程中除外”）。在此步骤结束时，机器人将直接注视脸部。
如果找到该脸并且脸很近：
1. 摇罐；
2. 再找脸
3. 如果脸部位置合适，请用眉毛打个好表情，然后再次摇动罐子；
4. 如果脸消失了，用眉毛生气地表情；
5. 转身，进入循环的开始。
如果没有人（或者很远）-在房间周围导航：
1. 如果两边都没有障碍物，则向前行驶（如果发现了脸部，但事实证明它太远了，机器人将走近人）；
2. 如果两侧都靠近障碍物，则在范围内随机转弯 $[90, 180] \cup [-180, -90]$ ;
3. 如果障碍物仅在一侧，则以相反的角度向相反方向旋转 $[0, 90]$ ;
4. 如果向前运动持续的时间过长（可能卡住了），请向后拉一点并在范围内随机转一圈。 $[90, 180] \cup [-180, -90]$ ;