基于Android 的 PID 控制巡线机器人

        在上次的博文Arduino PID 控制教程中给大家介绍了PID的原理，这里我将构建一个具有 PID 控制的巡线机器人。我们还将使用 Android 设备轻松设置主要控制参数，以便更好、更快速地进行调整。

所需材料清单：

• 车体（木板或Kt板或其他板材）
• 6v电池组
• 360度舵机马达两个+轮子两个
• Arduino Nano开发板一块
• HC-06蓝牙模块（可选，接收arduino设备传来的PID参数）
• TCRT5000   红外线传感器5个
• 1 个 LED    了解小车的状态
• 1 个按钮     启动小车

下面的 Arduino 代码可以完成舵机的测试：

#include <Servo.h> // 舵机库
Servo leftServo; 
Servo rightServo; 

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

void loop() 
{ 
}

1. 使用双面胶将 2 个舵机固定到车架之上。
2. 将球轮固定在小车前部。
3. 电机的电源将来自一组 6V 电池。这组电池将安装在面包板和车身框架之间。
4. 连接舵机使用的电池。
5. 将 Arduino Nano 连接到面包板。
6. 将 GND 连接到 Arduino GND。
7. 将舵机连接到 Arduino nano：左 ==>pin 5；右 ==>pin 3
8. 将 LED 连接到 Arduino pin 13
9. 将按钮连接到 Arduino pin 9

将外部 LED 添加到 pin13，用于信号传输和测试目的。

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

例如：

while(digitalRead(buttonPin)) //按下按钮时执行循环后的语句，否则将执行循环内的语句。
{ 
}
motorTurn (LEFT, 500);
motorTurn (RIGHT, 500);

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

以下是HC-06引脚连接：

• Tx Pin至 Arduino   Pin 10 (Rx)
• RX Pin至 Arduino   Pin 11 (Tx)
• VCC/GND至 Arduino 5V/GND

        机器人可以使用或不使用蓝牙工作。代码的构建方式是，如果您不激活蓝牙，则默认参数将是机器人使用的参数。因此，如果您不想安装蓝牙 HC-06 模块，代码仍将正常工作。

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

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

        对于使用的传感器，模块上的集成电路会生成一个简单的数字信号作为输出（高：暗；低：亮）。安装在模块上的电位器（见图）将调整正确的光级，以被视为“暗”或“亮”。它的工作原理是，当反射光颜色为黑色/暗时，其输出端会产生高（“1”）数字电平，而对于另一种较浅的颜色，则会产生低（“0”）。我在这里使用了一个带有 4 个传感器的集成模块和一个带有单个传感器的额外模块（形状不同，但逻辑相同）。我发现这种组合是 5 个传感器的阵列，可以实现良好而流畅的控制，如下所述。

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

        沿线行驶时可能的传感器阵列输出为：

• 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 个传感器，可以生成“error 变量”，这将有助于确定机器人在线上的位置，如下所示。

        假设黑线刚好位于机器人“中间传感器”（sersor 2）下方。数组的输出将是：0 0 1 0 0，在这种情况下，“error”将为“零”，也就是说没有误差。 与传感器状态相关的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  正好处在中间传感器sersor2下方
• 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;

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

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);

根据每个传感器的值来生成error变量的值：

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;
error值的绝对值
步骤 7：控制方向（比例控制 P）

        此时，我们的机器人已组装完毕并可运行。您应该对电机进行一些基本测试，读取传感器的输出并通过线路进行测试。下面我们将实现一个控制逻辑，以确保机器人能够一直沿着线路运行。

        假设机器人正在跑过一条线，传感器阵列输出为：“0 0 1 0 0 ”。对应的误差为“0”。在这种情况下，两个电机都应该以恒定速度向前运行。

        例如：iniMotorSpeed = 250；表示左舵机将接收 1250us 的脉冲右舵机将接收 1750us 的脉冲。使用这些参数，机器人将以半速向前移动。

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

        现在假设机器人行驶中传感器2和3检测到黑色，数组输出将为：“0 0 1 1 0”，error= 1。也就是小车偏左了，在这种情况下，您需要将机器人向右转。要做到这一点，您必须降低右侧舵机的速度，这意味着减少脉冲的长度。此外，必须增加左侧舵机的速度，这意味着减少左侧舵机脉冲的长度。为此，我们需要更改电机控制功能：

rightServo.writeMicroseconds(1500 + iniMotorPower - error); ==> 正误差：降低速度
leftServo.writeMicroseconds(1500 - iniMotorPower - error); ==> 正误差：增加速度

        上述逻辑是正确的，但很容易理解的是，在脉冲长度上增加或减去“1”不会产生所需的校正。下面我们需要将error调整成和速度相当的一个值，以便使用它来调整机器人两个舵机的速度。我们使用的方法是将“error”乘以一个常数（我们称之为“K”），将error映射成一个与速度相当的值。这个用来“放大error”的值我们定义成Kp。

int Kp = 50;

rightServo.writeMicroseconds（1500 + iniMotorPower-Kp*error）;

leftServo.writeMicroseconds（1500-iniMotorPower-Kp*error）;

传感器阵列的不同情况对应两个舵机的速度例如：

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

如果情况相反，机器人向右转，则误差将为“负”，并且舵机的速度应该改变：

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

此时很明显，机器人被推向一侧的程度越大，误差就越大，它必须更快地返回中心。机器人对误差的反应速度与误差成正比。这称为“比例控制”，即更复杂的控制网络 PDI（比例、微分、积分）的“P”组件。

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

        PID（比例、积分和微分）是最常见的控制方案之一。大多数工业控制回路都使用某种形式的 PID 控制。有许多方法可以调整 PID 回路，包括本例中使用的手动技术。如果你想更加详细的了解PID可以看 Arduino PID 控制教程。

        在巡线机器人的运行中如果一旦机器人与线发生了偏差，我们就要让他以最快的速度回到线上来，而不会等待误差的积累后再处理，所以需要微分项D来调整，而积分项I在巡线机器人中一般是没有用处的，一般为了保证公式的完整性我们设置KI=0。

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

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

在此步骤中，机器人可以按照恒定循环进行并且不会停止。

循环程序如下：

void loop () 
{ 
  readLFSsensors(); //读取传感器，将值存储在传感器阵列中并计算“error” 
  calculatePID(); //计算PID.
  motorPIDcontrol(); //使用PID控制舵机速度。
}

但为了实现更完整、更真实的操作，让我们引入一个新变量：“mode”。我们将为该变量定义 3 个状态：

模式：

• #define STOPPED 0        //停止
• #define FOLLOWING_LINE 1    //巡线状态
• #define NO_LINE 2        //小车脱离了线

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

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

        跟随线机器人的其他常见情况是发现“NO_LINE”也就是脱离了线，传感器检测不到黑线，传感器阵列输出为：0 0 0 0 0。在这种情况下，我们可以对其进行编程，使其原地旋转 ，直到找到线并恢复正常的巡线状态。

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; 
  } 
}

        真正的最终代码将集成一些额外的逻辑以及一些必须初始化的变量等。

        在前面的代码中，您可以在“robotDefines.h”选项卡中找到与 PID 控制一起使用的常量的以下定义：float Kp=50; float Ki=0; float Kd=0;

        定义用于 PID 控制器的正确常数的最佳方法是使用“尝试错误”方法。这种方法的缺点是每次必须更改程序时都必须重新编译程序。加快此过程的一种方法是使用 Android 应用程序在“设置阶段”利用蓝牙通讯功能给机器人发送常数，也就是首先在手机或其他的Android设备上设置kp、ki、kd然后利用蓝牙将其发送给机器人使用。

        我专门为此开发了一款 Android 应用程序。简而言之：

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

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

  while (digitalRead(buttonPin) && !mode) 
  {   
    checkBTcmd(); // 验证是否从 BT 遥控器接收到命令
    manualCmd (); //收到的字符翻译成相应的命令    
    command = ""; //清空字符串  
  } 
  checkPIDvalues(); 
  mode = STOPPED;

手动命令功能将是：

void manualCmd() 
{ 
  switch (command[0]) 
  { 
    case 'g': 
      mode = FOLLOWING_LINE; 
      break; 
    
    case 's': 
      motorStop(); //关闭两个电机
      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; 
  } 
}

程序比较长，可以自己用以上内容补充完整。