带有PID控制的线跟随机器人

描述

该项目的目的是构建一个带有 PID 控制的线跟随机器人。我们还将使用 Android 设备轻松设置主要控制参数，以便更好、更快地进行调整。

这个项目是一个由两部分组成的更复杂项目中的第一个，我的目的是探索 Line Follower Robots 的潜力。第二部分：迷宫求解机器人，使用 Arduino 的人工智能，机器人将使用简单的人工智能威廉希尔官方网站 探索和解决迷宫。

第 1 步：物料清单

所需材料清单非常简单，最终的机器人非常便宜（约 75.00 美元）：

车身（可根据您的需要进行调整）：

• 2个木方格（80X80mm）

• 3 个活页夹

• 2个木轮（直径：50mm）

• 1个球脚轮

• 9 松紧带

• 3M 命令框条

• 用于传感器固定的塑料接头

• 面包板和接线

• 2组4XNi金属氢化物电池（每组5V）

• 2 X SM-S4303R 连续旋转 360 度塑料伺服

• Arduino纳米

• HC-06 蓝牙模块

• 5 X 线传感器（TCRT5000 4CH 红外线跟踪传感器模块 + 1 个独立的跟踪传感器）

• 1 个 LED

• 1 个按钮

第 2 步：设置电机

对于电机，使用了 2 个连续伺服系统 (SM-S4303R)。正如您在照片中看到的那样，它们将被“粘合”在一起，形成一个单一的实心块（使用 3M 命令条、胶水或双面胶带）。这些伺服系统将以给定的速度运行，由其数据输入上接收到的脉冲宽度定义。对于这个特定的伺服，脉冲宽度从 1.0ms（1,000 微秒）到 2.0ms（2,000 微秒）。其他伺服系统可以使用不同的脉冲宽度。

细看：

• 1.5ms 的脉冲将伺服定位在中性位置，或“停止”。

• 1.0ms 的脉冲将命令伺服在一个方向上全速（约 70 RPM）

• 反方向的 2.0ms 全速脉冲。

• 1.0 到 1.5ms 或 1.5ms 到 2.0ms 之间的脉冲，将产生比例速度。

将两个伺服系统物理连接后，按照上面的绘图电路为它们提供源（外部 5V 或 6V），并为它们提供 Arduino 的信号：

• 左舵机：Arduino Pin 5

• 右舵机：Arduino Pin 3

全部连接后，必须做的第一件事是发送一个 1.5 毫秒的脉冲来验证电机是否“停止”（未运行）。如果不是，则必须将伺服系统调整到完全停止（寻找黄色螺栓，在伺服系统下方）。

注意：如果您的舵机没有此物理调整，请尝试更改函数内部的参数“1500”微秒（向上或向下），直到完全停止。

下面的 Arduino 代码可以完成这项工作：

#include  // Servo library 
Servo leftServo;
Servo rightServo;
Void setup()
{
  leftServo.attach(5);
  rightServo.attach(3);
  leftServo.writeMicroseconds(1500);
  rightServo.writeMicroseconds(1500);
}
void loop()
{
}

第 3 步：组装车身和电机进行运动测试

• 使用 3M Command 框架条，将 2 个舵机固定到其中一个方形木片上。

• 使用活页夹将第二块方形木头固定在上面的木头上。根据您的需要调整平台的长度。

• 使用活页夹固定球脚轮。

• 电机的电源将来自一组 5V 电池。这套电池将安装在面包板和车身框架之间。

• 连接舵机使用的电池：左侧一侧电网专用于舵机源

• 将 Arduino Nano 连接到面包板

• 将电网的 GND 连接到 Arduino GND。

• 将伺服系统连接到 Arduino：左 ==> 引脚 5；右 ==> 引脚 3

• 将 LED 连接到 Arduino 引脚 13

• 将按钮连接到 Arduino 引脚 9

请注意，由于伺服系统的安装方式（相反），速度范围为：

• 右舵机前进速度从 1,500us（停止）到 2,000us（全速）

• 左舵机前进速度从 1,500us（停止）到 1,000（全速）

一个外部 LED 被添加到 pin13，用于信号化和测试目的（如果需要，您可以使用内部 Arduino LED，而不是外部 LED，但考虑到在电缆中间很难看到它）。

还有一个按钮连接到引脚 9。这个按钮对于测试目的和机器人的启动非常有用。

例如：

while(digitalRead(buttonPin)) 
{ 
}
motorTurn (LEFT, 500);
motorTurn (RIGHT, 500);

请注意，将命令机器人向左转、等待 500 毫秒并向右转的 2 行仅在您按下按钮 (buttonPin = 0) 后才会发生。在此之前，程序将在无限循环中停止。

下面的代码可用作完整电机测试（前进、后退、句号、左转、右转）的基础。如有必要，您必须根据您的电机调整所需转角的延迟（此外，有时左右脉冲值应该略有不同，以补偿电机的任何缺乏平衡。

第四步：蓝牙模块（可选）

蓝牙模块 HC-06 应安装在面包板上，如图所示。将使用 Arduino 库 SoftSerial。

HC-06 引脚连接下方：

• Tx 引脚到 Arduino引脚 10 (Rx)

• RX 引脚到 Arduino引脚 11 (Tx)

• VCC/GND到 Arduino 5V/GND

机器人将使用或不使用蓝牙。该代码的构建方式是，如果您不激活 BT，则默认参数将是机器人使用的参数。因此，如果您不想安装 HC-06 模块，请不要担心，代码仍然可以正常工作。在本教程的最后一部分，我将探讨如何使用 Android 应用程序发送数据以更好地调整机器人参数和/或在手动模式下移动机器人。例如，如果有人想更多地探索使用 Line Follower Robot 进行比赛，我会将蓝牙和应用程序的使用保留为可选项。

第 5 步：添加线传感器

将电缆连接到 Arduino 引脚，如下所示：

• 传感器 0 = 12

• 传感器 1 = 18

• 传感器 2 = 17

• 传感器 3 = 16

• 传感器 4 = 19

• 如图所示，将 5 个传感器固定在塑料条上

• 建议对传感器进行标记以进行测试。传感器名称从“0”（更靠左）到“4”（更靠右）

• 将电缆穿过框架下方，使用松紧带固定它们。注意不要与轮子或脚轮混淆。

• 固定第二组 5V 电池并将其连接到 Arduino Vin。

就我而言，我使用了一个集成了 4 个传感器 + 1 个额外传感器的模块。它们都是兼容的。为简单起见，在图中我包括了 5 个连接在一起的独立传感器。两种配置的最终结果相同。

第 6 步：实现 IR 传感器逻辑

IR 传感器由一个单独的 IR LED 和一个 IR 光电二极管组成。LED 发出的红外光照射到表面并反射回红外光电二极管。然后，光电二极管产生与表面反射率水平成比例的输出电压（“亮表面”的值较高，“黑/暗表面”的值较低）。

在使用传感器的情况下，模块上的集成电路会生成一个简单的数字信号（HIGH：暗；LOW：亮）作为输出。安装在模块上的电位器（见照片）将调整正确的光线水平以被认为是“暗”或“亮”。它的工作方式是，当反射光颜色为黑色/深色时，在其输出端生成一个高（“1”）数字电平，并为另一种较浅的颜色生成一个低（“0”）数字电平。我在这里使用了一个带有 4 个传感器的集成模块和一个带有唯一传感器的额外模块（不同的形状，但相同的逻辑）。该组合是由 5 个传感器组成的阵列，我发现它们有利于实现良好而平稳的控制，如下所述。

5 个传感器阵列的安装方式是，如果只有一个传感器相对于黑线居中，则只有该特定传感器会产生 HIGH。另一方面，应计算传感器之间的空间，以允许 2 个传感器可以同时覆盖黑线的整个宽度，同时在两个传感器上产生一个 HIGH（见上图）。

跟随线时可能的传感器阵列输出是：

• 0 0 0 0 1

• 0 0 0 1 1

• 0 0 0 1 0

• 0 0 1 1 0

• 0 0 1 0 0

• 0 1 1 0 0

• 0 1 0 0 0

• 1 1 0 0 0

• 1 0 0 0 0

拥有 5 个传感器，可以生成一个“误差变量”，这将有助于控制机器人在线上的位置，如下所示。

让我们考虑一下最佳条件是机器人居中时，这条线正好位于“中间传感器”（传感器 2）下方。数组的输出将是：0 0 1 0 0，在这种情况下，“错误”将是“零”。如果机器人开始向左移动（线“似乎向右移动”），则误差必须随着正信号而增加。如果机器人开始向右移动（线“似乎向左移动”），在同样，误差必须增加，但现在是负信号。

与传感器状态相关的错误变量将是：

0 0 1 0 0 ==> 错误 = 0

• 0 0 0 0 1 ==> 错误 = 4

• 0 0 0 1 1 ==> 错误 = 3

• 0 0 0 1 0 ==> 错误 = 2

• 0 0 1 1 0 ==> 错误 = 1

• 0 1 1 0 0 ==> 错误 = -1

• 0 1 0 0 0 ==> 错误 = -2

• 1 1 0 0 0 ==> 错误 = -3

• 1 0 0 0 0 ==> 错误 = -4

查看 Arduino 代码，每个传感器都将定义一个特定的名称（考虑到更靠左的 Line Follow Sensor 必须分配一个标签“0”）：

const int lineFollowSensor0 = 12;
const int lineFollowSensor1 = 18;
const int lineFollowSensor2 = 17;
const int lineFollowSensor3 = 16;
const int lineFollowSensor4 = 19;

为了存储每个传感器的值，将创建一个数组变量：

int LFSensor[5]={0, 0, 0, 0, 0};

阵列的每个位置都将随着每个传感器的输出不断更新：

LFSensor[0] = digitalRead(lineFollowSensor0);
LFSensor[1] = digitalRead(lineFollowSensor1);
LFSensor[2] = digitalRead(lineFollowSensor2);
LFSensor[3] = digitalRead(lineFollowSensor3);
LFSensor[4] = digitalRead(lineFollowSensor4);

有了每个传感器的值，必须实现一个逻辑来生成错误变量：

if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 1 )) error = 4;
else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 1 )&&(LFSensor[4]== 1 )) error = 3; 
else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 1 )&&(LFSensor[4]== 0 )) error = 2;
else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 1 )&&(LFSensor[3]== 1 )&&(LFSensor[4]== 0 )) error = 1;
else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 1 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) error = 0;
else if((LFSensor[0]== 0 )&&(LFSensor[1]== 1 )&&(LFSensor[2]== 1 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) error =- 1;
else if((LFSensor[0]== 0 )&&(LFSensor[1]== 1 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) error = -2;
else if((LFSensor[0]== 1 )&&(LFSensor[1]== 1 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) error = -3;
else if((LFSensor[0]== 1 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) error = -4;

第 7 步：控制方向（比例控制 - P）

完美的！至此，我们的机器人已经组装完毕并可以运行了。您应该对电机进行一些基本测试，读取传感器的输出并通过一条线对其进行测试。缺少的是真正的“大脑”，即“人工智能”的第一步。我们将得到这一点，实现一个控制逻辑，以保证机器人将保持跟随线。

简单的比例控制：

假设机器人在一条线上运行，传感器阵列输出为：“0 0 1 0 0” 。对应错误为“0”。在这种情况下，两台电机都应该匀速向前运行。

例如：

定义变量：iniMotorSpeed = 250 ; 表示左舵机将接收 1,250us 的脉冲，右舵机将接收 1,750us 的脉冲。使用这些参数，机器人将以半速前进。请记住，右舵机前进速度的脉冲长度范围从 1,500us（停止）到 2,000us（全速），左舵机从 1,500us（停止）到 1,000us（全速）。

rightServo.writeMicroseconds(1500 + iniMotorPower);
leftServo.writeMicroseconds(1500 - iniMotorPower);

现在假设机器人向左行驶（就像“线向右”）并且还覆盖了传感器 3。阵列输出将为：“0 0 1 1 0”并且错误 = 1。在这种情况下，您需要将机器人向右转。为此，您必须降低 RIGHT 伺服的速度，这意味着减少脉冲的长度。此外，左舵机的速度必须增加，这意味着减少左舵机脉冲的长度。为此，我们需要更改电机控制功能：

rightServo.writeMicroseconds(1500 + iniMotorPower - error); ==> Positive error: decrease velocity
leftServo.writeMicroseconds(1500 - iniMotorPower - error); ==> Positive error: increase velocity

上述逻辑是正确的，但很容易理解，在脉冲长度上添加或减去“1”微秒不会在实际时间产生所需的校正。很直观，要加减的数字应该更大，例如 50、100 等。为此，“误差”必须乘以一个常数（我们称其为“K”）。一旦这个常数的影响将与误差成正比，我们将其命名为“比例常数：Kp” 。

运动功能将是：

int Kp = 50;
rightServo.writeMicroseconds(1500 + iniMotorPower - Kp*error);
leftServo.writeMicroseconds(1500 - iniMotorPower - Kp*error);

我们可以恢复电机将发生的情况，如下所示：

• 传感器阵列：0 0 1 0 0 ==>错误 = 0 ==> 右伺服脉冲长度 = 1,750us ==> 左伺服脉冲长度 = 1,250us（两个电机速度相同）

• 传感器阵列：0 0 1 1 0 ==>错误 = 1 ==> 右侧伺服脉冲长度 = 1,700us（较慢）==> 左侧伺服脉冲长度 = 1,200us（较快）

如果情况相反并且机器人向右驱动，则错误将为“负”并且伺服系统的速度应该改变：

• 传感器阵列：0 0 1 0 0 ==>错误 = 0 ==> 右伺服脉冲长度 = 1,750us ==> 左伺服脉冲长度 = 1,250us（两个电机速度相同）

• 传感器阵列：0 1 1 0 0 ==>错误 = -1 ==> 右舵机脉冲长度 = 1,800us（更快）==> 左舵机脉冲长度 = 1,300us（更慢）

在这一点上，很明显，机器人向一侧行驶的次数越多，误差越大，它必须更快地返回中心。机器人的速度将对误差做出反应，与误差成正比。这称为“比例控制” ，即更复杂的控制网络 PDI（比例、微分、积分）的“P”组件。

第 8 步：PID 控制（可选）

如果你想跳过这部分，也可以。您可以继续使用上一步解释的比例控制，也可以烧一些脑筋在您的机器人中实现更复杂的控制系统，这是您的选择。

如果你下定决心，那就走吧！

PID（比例、微分和积分）是最常见的控制方案之一。大多数工业控制回路使用某种 PID 控制。调整 PID 回路的方法有很多，包括本例中使用的手动威廉希尔官方网站 。

将 PID 视为一个简单的弹簧。弹簧具有原始长度，当受到膨胀或收缩的干扰时，它往往会在尽可能短的时间内恢复其原始长度。类似地，系统中的 PID 算法具有要控制的特定物理量的设定值，称为“ set point ”，当由于某种原因改变时，系统控制其中的其他必要特征，以回到在尽可能短的时间内达到初始设定点。PID 控制器用于需要控制物理量并使其等于指定值的任何地方。例如，汽车中的巡航控制器、机器人、温度调节器、电压调节器等。

PID是如何工作的？

系统通过使用传感器测量该物理量的当前值，从设定点计算物理量的“ error”或“ ”。deviation为了回到设定点，这个' error'应该被最小化，并且应该理想地等于零。此外，这个过程应该尽快发生。理想情况下，系统对其设定点变化的响应应该为零滞后。

更多信息可以在许多书籍和网站中找到，包括这里。

实施 PID

i) 误差项 (e)：

这等于设定点与被控制量的当前值之间的差值。Error = set_point– current_value（在我们的例子中是从 Robot 线上的位置获取的误差变量

ii) 比例项 (P)：

该项与误差成正比。

P = 错误

该值负责物理量达到设定点所需的变化幅度。比例项决定了控制回路的上升时间或达到设定点的速度。

iii) 积分学期 (I)：

该项是所有先前误差值的总和。

我 = 我 + 错误

该值负责系统对设定点变化的响应速度。积分项用于消除比例项所需的稳态误差。通常，小型机器人不使用积分项，因为我们不关心稳态误差，它会使“ loop tuning”复杂化。

iv) 微分或微分项 (D)：

该项是设定点的瞬时误差与前一时刻的误差之差。

D = 错误 - previousError

当物理量接近设定点时，该值负责减慢物理量的变化率。微分项用于减少过冲或系统应“ over correct”的程度。

方程：

PID 值 = (Kp*P) + (Ki*I) + (Kd*D)

在哪里：

Kp是用于改变达到设定点所需的变化幅度的常数。Ki是用于改变物理量的变化率以达到设定点的常数。Kd是用于改变系统稳定性的常数。

调整循环的一种方法是Try-error tentative 方法：

将Kd 变量设置为 0并首先单独调整 Kp 项。在我们的案例中， Kp 为 25是一个很好的起点。在最后一步，我们使用了 50 的 Kp，这与我的机器人非常配合。一旦机器人做出合理响应，调整控制回路的微分部分 ( Kd )。首先将 Kp 和 Kd 值分别设置为 Kp 值的 1/2。例如，如果机器人响应合理，Kp = 50，则设置 Kp = 25 和 Kd = 25 开始。增加 Kd（微分）增益以减少过冲，如果机器人变得不稳定，则减少它。

• 如果机器人反应太慢，请增加该值。

• 如果机器人似乎反应迅速变得不稳定，请减小该值。

要考虑的循环的另一个组成部分是实际的Sample/Loop Rate。加快或减慢此参数可以显着提高机器人的性能。这是由代码中的延迟语句设置的。这是一种尝试错误的尝试方法，以获得最佳结果。

基于上述方法，实现了以下功能：

void calculatePID()
{
  P = error;
  I = I + error;
  D = error-previousError;
  PIDvalue = (Kp*P) + (Ki*I) + (Kd*D);
  previousError = error;
}

最后一步使用的简单 Kp 常数将被替换为更完整PIDvalue ：

void motorPIDcontrol()
{
  int leftMotorSpeed = 1500 - iniMotorPower - PIDvalue;
  int rightMotorSpeed = 1500 + iniMotorPower - PIDvalue;
  
  leftServo.writeMicroseconds(leftMotorSpeed);
  rightServo.writeMicroseconds(rightMotorSpeed);
}

但请注意，如果您有 Kd 和Ki =0 ，PIDvalue 则仅在最后一步中使用 Kp*error。

第 9 步：最终代码

在这一步，机器人可以遵循一个恒定的循环，并且不会停止。循环程序将是：

void loop ()
{
  readLFSsensors(); // read sensors, storage values at Sensor Array and calculate "error"
  calculatePID(); 
  motorPIDcontrol();
}

但是为了更完整和真实的操作，添加至少几个基础commands “完成”是很重要的with the line 。例如，让我们引入一个新变量：“ mode ”。我们将为这个变量定义 3 个状态：

模式：

• #define STOPPED 0

• #define FOLLOWING_LINE 1

• #define NO_LINE 2

如果所有传感器都找到一条黑线，则传感器阵列输出将为：1 1 1 1 1。在这种情况下，我们可以将模式定义为“STOPPED”，机器人应该执行“ full stop”。

if((LFSensor[0]== 1 )&&(LFSensor[1]== 1 )&&(LFSensor[2]== 1 )&&(LFSensor[3]== 1 )&&(LFSensor[4]== 1 )) 
{ 
  mode = STOPPED;
}

Follower Line Robots 的其他常见情况是它找到“ no line ”的位置，或者 Sensor Array 输出为：0 0 0 0 0。在这种情况下，我们可以将其编程为返回 180o 或以小角度转动，直到找到一条线并且恢复正常的线路跟随条件。

else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) 
{ 
  mode = NO_LINE;
}

完整的loop ()将是：

void loop() 
{
  readLFSsensors();
  switch (mode)
  {
    case STOPPED:
       motorStop();
       break;
    case NO_LINE:
       motorStop();
       motorTurn(LEFT, 180);
       break;
    case FOLLOWING_LINE:
       calculatePID();
       motorPIDcontrol();
       break;
  }
}

真正的最终代码将集成一些额外的逻辑以及一些必须初始化的变量等。在上面的解释中，为了简单起见，我将它们省略了，但看最终代码应该一切都清楚。

下面是最终的 Arduino 代码：

第 10 步：使用 Android 应用程序调整 PID 控制

在前面的代码中，您可以在“ robotDefines.h”选项卡中找到以下用于 PID 控制的常数定义：

float Kp=50;
float Ki=0;
float Kd=0;

如上一步所述，定义与 PID 控制器一起使用的正确常数的最佳方法是使用“试错”方法。不好的一面是每次必须更改程序时都必须重新编译程序。加速该过程的一种方法是使用 Android 应用程序在“设置阶段”发送常量。

我专门为此开发了一个 Android 应用程序。简而言之，有传统的手动命令：

• FW、BW、Left、Right 和 Stop，应用程序将分别发送到 BT 模块：'f'、'b'、'l'、'r' 和 's'。

还包括 3 个滑块，每个 PID 常数一个：

• Kd：“d/XXX”，其中“XXX”是一个从 0 到 100 的数字。

• Kp：“p/XXX”

• Ki：“我/XXX”

包括一个额外的按钮，它与 Arduino Pin9 上连接的按钮完全一样。您可以使用其中一种，没关系。

您可以在下面找到.aia 可以在MIT AppInventor修改的.apk 文件以及要直接安装在您的 Android 设备中的文件。

第 11 步：更改 PID 远程调整的代码

在设置过程中，我们将引入一个循环，您可以在其中将 PID 参数发送给机器人，然后再将其置于线路上：

 while (digitalRead(buttonPin) && !mode)
  {  
    checkBTcmd();  // verify if a comand is received from BT remote control
    manualCmd ();    
    command = "";  
  }
  checkPIDvalues();
  mode = STOPPED;

手动命令功能将是：

void manualCmd()
{
  switch (command[0])
  {
    case 'g':
      mode = FOLLOWING_LINE;
      break;
    
    case 's': 
      motorStop(); //turn off both motors
      break;
    case 'f':  
      motorForward();  
      break;
    case 'r':     
      motorTurn(RIGHT, 30);
      motorStop();
      
      break;
   case 'l': 
      motorTurn(LEFT, 30);
      motorStop();
      break;
    
    case 'b':  
      motorBackward();
      break;
      
    case 'p':
      Kp = command[2];
      break;
    
    case 'i':
      Ki = command[2];
      break; 
    
    case 'd':
      Kd = command[2];
      break;
  }
}

在视频中，您可以看到一些使用 Android 应用程序的测试。在最终代码下方，包括通过 Android 设置的 PID：

第 12 步：结论

这是一个更复杂项目的第一部分，探索线跟随机器人的潜力。在下一部分中，我将基于这个项目在这里开发一个迷宫求解机器人。希望我可以为其他人做出贡献，以了解更多关于电子、机器人、Arduino 等的知识。

这个项目的更新文件可以在GITHUB找到。更多教程请访问我的博客：MJRoBot.org