巡线机器人 - PID控制 - 安卓设置_巡线pid

巡线机器人 - PID控制 - 安卓设置

原文

该项目的目的是构建具有PID控制的巡线机器人。我们还将使用Android设备轻松设置主要控制参数，以便更好，更快地进行调谐。项目中用到的舵机也可用MG996代替，关于MG996等舵机的介绍可以看这里

这个项目是一个由两部分组成的复杂项目中的第一个，我的目的是探索线路跟随机器人的潜力。在第 2 部分：迷宫求解器机器人，使用带有 Arduino 的人工智能，机器人将使用简单的人工智能技术探索和解决迷宫。

第 1 步：物料清单

所需材料清单非常简单：

• 车身（可根据您的需要进行调整）：
  • 2 个木方块 （80X80mm）
  • 3 活页夹
  • 2 木轮（直径：50mm）
  • 1 个脚轮
  • 9 松紧带
  • 3M 条
  • 用于传感器固定的塑料接头
  • 面包板和布线
• 2套电池（每套5V）
• 2 x SM-S4303R 连续旋转 360 度舵机
• Arduino Nano
• HC-06 蓝牙模块
• 5 个巡线传感器（TCRT5000 4 通道红外线跟踪跟踪传感器模块 + 1 个独立轨道传感器）
• 1 个发光二极管
• 1 个按钮

第 2 步：设置电机

使用了2个连续旋转伺服器（SM-S4303R）。它们将被“粘合”在一起，形成一个单一的实心块。这些伺服系统将以给定的速度运行，该速度由其数据输入接收的脉冲宽度定义。对于这种伺服，脉冲宽度从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 引脚 5
• 右伺服：Arduino 引脚 3

全部连接后，必须做的第一件事是发送1.5ms脉冲以验证电机是否“停止”（未运行）。如果没有，必须将舵机调整到完全停止（寻找黄色螺栓，在舵机下方）。注意：如果您的伺服没有这种物理调整，请尝试更改函数内部的参数“1500”微秒（向上或向下），直到获得完全停止。

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

#include <Servo.h> // Servo library 
Servo leftServo;
Servo rightServo;

Void setup()
{
  leftServo.attach(5);
  rightServo.attach(3);
  leftServo.writeMicroseconds(1500);
  rightServo.writeMicroseconds(1500);
}

void loop()
{
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

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

1. 使用 3M 条，将 2 个舵机固定在其中一个方形木片上。
2. 使用活页夹将第二个方形的木材固定在上面的木头上。根据您的需要调整平台的长度。
3. 使用活页夹固定滚珠脚轮。
4. 电机的电源将来自 5V 电池组之一。这组电池将安装在面包板和车身框架之间。
5. 连接与舵机一起使用的电池：左边是专用于舵机电源
6. 将 Arduino Nano 连接到面包板
7. 将电源的GND连接到Arduino GND。
8. 将舵机连接到 Arduino：左 ==> 引脚 5;右 ==> 引脚 3
9. 将 LED 连接到 Arduino 引脚 13
10. 将按钮连接到 Arduino 引脚 9

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

• 右伺服前进速度从 1，500us（停止）到 2，000us（全速）
• 左伺服前进速度从 1，500us（停止）增加到 1，000（全速）

引脚 13 中添加了一个外部 LED，用于信号化和测试目的。

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

例如：

while(digitalRead(buttonPin)) 
{ 
}
motorTurn (LEFT, 500);
motorTurn (RIGHT, 500);
1
2
3
4
5

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

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

附件

Motor_Test.ino下载

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

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

在 HC-06 引脚连接下方：

• TX到 Arduino 针脚 10 （Rx）
• RX 引脚到 Arduino 引脚 11 （Tx）
• VCC/GND 至 Arduino 5V/GND

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

步骤 5：添加巡线传感器

1. 将5个传感器固定在塑料条上，如照片所示
2. 建议为传感器贴上标签以进行测试。传感器名称从“0”（更多向左）到“4”（更多向右）
3. 将电缆放在框架下方，使用松紧带固定它们（注意不要与车轮或脚轮混淆。
4. 将电缆连接到 Arduino 引脚，如图所示：
   • 传感器 0 = 12
   • 传感器 1 = 18
   • 传感器 2 = 17
   • 传感器 3 = 16
   • 传感器 4 = 19
5. 第二组5V电池并将其连接到Arduino Vin。

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

步骤 6：红外传感器逻辑

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

传感器模块上的集成电路产生一个简单的数字信号作为输出（高：暗;低：浅）。安装在模块上的电位计（见图）将调整正确的光水平，将其视为“暗”或“亮”。它的工作方式是，当反射光颜色为黑色/深色时，在其输出端生成高（“1”）数字电平，为另一种较浅的颜色生成低电平（“0”）。

我在这里使用了一个带有 4 个传感器的集成模块和一个带有唯一传感器的额外模块（不同的形状，但逻辑相同）。正如下面所解释的那样，该组合是一个由 5 个传感器组成的阵列，我发现这些传感器有利于实现良好而流畅的控制。


5个传感器阵列的安装方式是，如果只有一个传感器相对于黑线居中，则只有该传感器会产生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，在这种情况下，“error”将为“零”。如果机器人开始向左行驶（线“似乎向右移动”），则误差必须随着正信号的增加而增加。如果机器人开始向右移动（"线"似乎向左移动”），以同样的方式，误差必须增加，但现在带有负信号。

与传感器状态相关的误差变量为：

• 0 0 0 0 1 ==> error = 4
• 0 0 0 1 1 ==> error = 3
• 0 0 0 1 0 ==> error = 2
• 0 0 1 1 0 ==> error = 1
• 0 0 1 0 0 ==> error = 0
• 0 1 1 0 0 ==> error = -1
• 0 1 0 0 0 ==> error = -2
• 1 1 0 0 0 ==> error = -3
• 1 0 0 0 0 ==> error = -4

查看Arduino代码，每个传感器都将使用特定名称进行定义（左侧的线跟随传感器分配标签“0”）：

const int lineFollowSensor0 = 12;
const int lineFollowSensor1 = 18;
const int lineFollowSensor2 = 17;
const int lineFollowSensor3 = 16;
const int lineFollowSensor4 = 19;
1
2
3
4
5

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

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

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

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

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

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;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

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

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

简单的比例控制：

假设机器人在一条线上运行，传感器阵列输出为：0 01 0 0。相应的error为“0”。在这种情况下，两个电机都应以恒定速度向前运行。

例如：

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

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

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

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

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

电机功能将是：

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

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

• 传感器阵列：0 0 1 0 0 ==> error = 0 ==> 右伺服脉冲长度 = 1,750us ==> 左舵机脉冲长度 = 1,250us（两个电机速度相同）
• 传感器阵列：0 0 1 1 0 ==> error = 1 ==> 右舵机脉冲长度 = 1,700us（较慢）==> 左舵机脉冲长度 = 1,200us（较快）

如果情况相反，机器人向右驱动，则error将是“负数”，并且伺服器的速度应该改变：

• 传感器阵列：0 0 1 0 0 ==>error = 0 ==> 右伺服脉冲长度 = 1,750us ==> 左舵机脉冲长度 = 1,250us（两个电机速度相同）
• 传感器阵列：0 1 1 0 0 ==> error = -1 ==> 右舵机脉冲长度 = 1,800us（更快）==> 左舵机脉冲长度 = 1，300us（较慢）

在这一点上很明显，机器人被驱动到一侧，error越大，它必须更快地返回中心。机器人对error的反应速度将与之成正比。这被称为“比例控制”，即更复杂的控制PDI（比例，微分，积分）的**“P”**分量。

第 8 步：PID 控制（可选）

如果你想跳这部分，也可以。您可以继续使用最后一步中解释的比例控制，或者花一些脑筋在您的机器人中实现更复杂的控制系统，我们继续吧！

PID（比例，导数和积分）是最常见的控制方案之一。大多数工业控制回路使用某种形式的PID控制。有许多方法可以调整 PID 环路，包括此示例中使用的手动技术。

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

PID如何工作？

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

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

实施 PID

1. error项 （e）：

这等于设定点与被控制量的当前值之间的差值。

error = set_point – current_value（在我们的例子中是error变量从机器人在线上的位置获取）

2. 比例期限（P）：

此项与error成正比。

P = error

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

3. 积分项（I）：

此术语是所有先前error值的总和。

I = I + error

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

4. 微分或导数项（D）：

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

D = error - previousError

该值负责减慢物理量接近设定点时的变化率。派生项用于减少过冲或系统“过度调整”的程度。

方程：

PIDvalue = （Kp*P） + （Ki*I） + （Kd*D）

其中：

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

调整循环的一种方法是尝试error暂定方法：

1. 将 Kd 变量设置为 0，并首先单独调整 Kp 项。在我们的案例中，Kp = 25 是一个很好的起点。最后，我们使用了 Kp=50，它与我的机器人配合得很好。
2. 如果机器人反应太慢，请增加该值。
3. 如果机器人似乎反应迅速变得不稳定，请降低该值。
4. 一旦机器人做出合理的响应，调整控制回路的导数部分（Kd）。首先将 Kp 和 Kd 值分别设置为 Kp 值的 $ \frac {1}{2}$。例如，如果机器人响应合理，Kp = 50，则设置 Kp = 25 和 Kd = 25 开始。增加Kd（导数）增益以减少过冲，如果机器人变得不稳定，则减小过冲。

要考虑的另一个部分是实际的采样/环路速率。加快或减慢此参数可以显着影响机器人的性能。这是由代码中的delay 语句设置的。这是一个尝试error的试探性方法，以获得最佳结果。

基于上述方法，实现PID功能：

void calculatePID()
{
  P = error;
  I = I + error;
  D = error-previousError;
  PIDvalue = (Kp*P) + (Ki*I) + (Kd*D);
  previousError = error;
}
1
2
3
4
5
6
7
8

最后一步中使用的简单 Kp 常数将替换为此更完整的 PID值：

void motorPIDcontrol()
{
  int leftMotorSpeed = 1500 - iniMotorPower - PIDvalue;
  int rightMotorSpeed = 1500 + iniMotorPower - PIDvalue;
  
  leftServo.writeMicroseconds(leftMotorSpeed);
  rightServo.writeMicroseconds(rightMotorSpeed);
}
1
2
3
4
5
6
7
8

但请注意，如果您有 Kd 和 Ki =0，则 PID值只是使用 Kp*error

第 9 步：最终代码

在这一步中，机器人可以遵循一个恒定的循环，并在没有停止的情况下完成。

循环程序将是：

void loop ()
{
  readLFSsensors(); // read sensors, storage values at Sensor Array and calculate "error"
  calculatePID(); 
  motorPIDcontrol();
}
1
2
3
4
5
6

但是为了更完整和实际的操作，需要添加至少几个基本的与线运动相关的命令。让我们引入一个新变量：“mode”。我们将为此变量定义 3 种状态：

模式：

- #define STOPPED  0
- #define FOLLOWING_LINE 1
- #define NO_LINE 2
1
2
3

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

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

巡线机器人的其他常见情况是它发现“无线”，或者传感器阵列输出为：0 0 0 0 0。在这种情况下，我们可以将其编程为向后转 180度或以小角度转动，直到找到一条线并恢复正常的线跟随条件。

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

完整的loop() 将是：

void loop() 
{
  readLFSsensors();
  switch (mode)
  {
    case STOPPED:
       motorStop();
       break;
    case NO_LINE:
       motorStop();
       motorTurn(LEFT, 180);
       break;
    case FOLLOWING_LINE:
       calculatePID();
       motorPIDcontrol();
       break;
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

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

步骤 10：使用 Android 应用程序调整 PID 控件

在前面的代码中，您可以在“robotDefines.h”中找到与 PID 控件一起使用的常量的以下定义：

float Kp=50;
float Ki=0;
float Kd=0;
1
2
3

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

我专门为此开发了一个Android应用程序。总之：

• 传统的手动命令：
  • FW, BW, Left, Right and Stop，应用程序将分别发送到BT模块：“f”，“b”，“l”，“r”和“s”。
• 还包括 3 个滑块，每个 PID 常量一个：
  • Kp： “p/XXX”
  • Ki： “i/XXX”
  • Kd： “d/XXX”
    • 其中“XXX”是 0 到 100 之间的数字。
• 包括一个额外的按钮，它将与Arduino Pin9上连接的按钮完全相同。您可以使用其中之一。

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

附件

MJRoBot_Line_Follower_PID_Control.aia

下载
MJRoBot_Line_Follower_PID_Control.apk

下载

步骤 11：更改 PID 远程调谐的代码

在void setup()过程中，我们将引入一个循环，您可以在将小车放到线路上之前将 PID 参数发送到机器人：

  while (digitalRead(buttonPin) && !mode)
  {  
    checkBTcmd();  // verify if a comand is received from BT remote control
    manualCmd ();    
    command = "";  
  }
  checkPIDvalues();
  mode = STOPPED;
1
2
3
4
5
6
7
8

手动命令将是：

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;
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44

以下是最终代码，包括通过Android进行的PID设置：

附件

generalFunctions.ino

下载

motorFuntions.ino

下载

robotDefines.h

下载

sensorFuntions.ino

下载

smart_MJRoBot_Line_Follower_PID.ino

下载

第 12 步：结论

这是复杂的项目的第一个，探索线路跟随机器人的潜力。在下一部分中，我将开发一个迷宫求解机器人，基于这个项目在迷宫求解机器人。

该项目的更新文件可以在GITHUB上找到：https://github.com/Mjrovai/MJRoBot-Line-Follower

有关更多教程，请访问博客：MJRoBot.org