未分类 | 创元素

使用 Arduino 进行杆球平衡系统的 PID 控制

通过研究杆和球系统并使用Arduino作为控制器，很容易理解PID 控制。目标是通过使用闭合控制回路方便地倾斜球，将球放置在杆的中心。

套件淘宝购买链接:

Arduino PID 控制球 PID算法控制原理控制工程经典item.taobao.com

5分钟总结视频（点击即可观看）

杆和球系统

它是控制工程中的经典系统。

使用距离传感器，我们测量球的位置。
使用Controller，通过 PID 控制，我们计算应该倾斜杆以将球定位和稳定在杆中心的角度。
一个执行机构修改栏的倾向。

球位感应

我们通过使用红外光和PSD 检测器的距离传感器来做到这一点：SHARP GP2Y0A21。

它的测量范围为 6 至 80 厘米。它在 5V 下工作，它的输出是与此特性曲线测量的距离相关的电压：

如果球距离传感器的距离小于 6cm，则测量结果错误。我们限制球在那个距离内的运动。

传感器信号调理

为了过滤（低通）传感器信号并获得更准确和可重复的信号，我们将在传感器输出和地之间连接一个 10μF 电解电容器。

由于我们要测量的最大电压为 3.1V，我们将使用指令将 Arduino 的电压参考设置为 3.3V：

analogReference(EXTERNAL);

我们将 AREF 引脚与 Arduino 的 3.3V 输出连接：

这样，Arduino 的 10 位数模转换器提供的 1024 个点将具有 3.3V 的满量程，而不是默认的 5V。因此，我们将分辨率从 5mV/ADC 提高到 3mV/ADC。

传感器校准

为了将传感器提供的张力与以厘米为单位的距离联系起来，我们将沿着杆移动球，注意 ADC 中的读数。在为 Arduino 开发的软件中，包含一种操作模式，它通过串行端口连续传输传感器读数：

    if(0){// Para calibrar sensor de Distancia
      Serial.print(dist);
      Serial.print("mm     ADC: ");    
      Serial.println(measure); 
    }

沿着横杆有 9 个点就足够了。我们获得了传感器的校准曲线。

在软件中定义为：

int dcal [] = { // Calibracion de ADC a Distancia
  -193, -160, -110, -60, 0, 40, 60, 90, 120};
int ADCcal [] = {
  177, 189, 231, 273, 372, 483, 558, 742, 970};

并且为了将存储在测量变量中的传感器的 ADC 读数转换为dist变量中以毫米为单位的位置，我们应用以下算法：

   for(int i =0; i<8; i++){ // Aplicamos curva de Calibracion de ADC a mm 
      if (measure >= ADCcal[i] && measure< ADCcal[i+1]){
        dist = map(measure,ADCcal[i],ADCcal[i+1],dcal[i],dcal[i+1]);
      }

变量dist有负值和正值：条形左端为 -193mm，右侧为 120mm，中心为 0。由于我们的目标是将球留在中心点，因此该变量dist相当于PID 控制系统文献中使用的误差。

执行器

我们将使用扭矩为 6.9 kg.cm 的 HEXTRONIK HX5010 Servo和固定在杆一端的玻璃纤维连杆倾斜杆。

正如维基百科所说，我们用可变持续时间的脉冲控制伺服的旋转：

在这个特定的伺服系统中，0º 位置是用 0.5ms 脉冲获得的，180º 转动是用 2.3ms 获得的。

标准库（包含在 Arduino IDE 中）舵机，包括写入（角度）指令，角度是0 到 180 之间的整数，它允许我们调整舵机的位置。这是常用的指令，但由于我们希望在转动伺服时获得最大精度，因此我们将使用writeMicroseconds代替。它的语法是：servo.writeMicroseconds (μS)，其中μS 是脉冲持续时间的微秒。我们将有 500（向上位置）和 2300（向下位置）之间的值，有 1800 个不同的点，而不是使用最基本的指令：write（角度）只有 180 个。

在调试和调整过程中，我们会借助水平仪计算出静止位置（水平杆）。

控制器

我们将使用带有 ATMEL ATMEGA328-PU 微控制器的 Arduino 克隆，类似于 Arduino UNO 或旧的原始 Duemilanove。

它在其模拟输入 A0 处接收球位置的测量值。
它发出脉冲以控制其数字输出 12 上的伺服。
通过其 USB 连接，它发送不同的数据集用于调试或以下形式的帧：

173,173, -5, -5 $ // dist, dist, vel, vel $

由运行在Processing 中开发的应用程序的 PC 接收，这将使我们能够欣赏球的位置和速度随时间变化的图表，如下所示：

作为奖励，当球距离中心小于 8 毫米时，它会打开连接到输出 13 的 LED。

5V电源

Arduino 通过其 USB 连接接收其运行所需的 5V。它的5V脚的电源不足以给舵机供电，所以我们会用一个辅助电源给它供电。不要忘记将辅助电源的地与Arduino的GND地相连！否则，伺服控制信号将没有公共参考，将无法工作。

完整系统示意图

PID 控制。ARDUINO 软件。

一旦我们实现了物理系统，就该为控制器提供必要的智能，以实现我们的目标：将球留在杆的中心。

测量和反应期

测量和反应序列（程序周期）不会像微控制器那样快，而是每50 毫秒（存储在周期变量中的值）。我们这样做是因为如果测量和反应周期总是相同的持续时间，PID 控制系统会更好地工作。

主频为 16MHz 的微控制器有足够的速度在不到 10ms 的时间内完成程序周期。但是在如此快速的循环中，球速度的测量会失去精度，因为循环之间球的位置差异可以忽略不计，我们将球的速度计算为连续 2 个程序循环中的位置差异。

如果我们将周期延长到 100 毫秒，速度测量会更准确，但伺服以明显的间歇方式工作。

测试后，50ms 的周期提供了可接受的球速测量值和平滑的伺服操作。

球速计算

在Arduino 中实现 PID 控制的软件中，我们将速度计算为球的当前位置（变量dist）与其在前一个循环中的位置（变量lastDist）之间的差异。我们将通过数字低通滤波器（平均值）来提高此测量的精度，该滤波器包括获得最后 5 个测量速度的平均值。我们将它们存储在矩阵v [] 中，并在每个程序周期中使用以下算法处理它们：

  for (int i = 0; i <level-1; i ++) {// 我们全部向左移动 
      v [i] = v [i + 1];
    }
    v [nvel-1] = (dist - lastDist); // 我们放入一个新数据
    vel = 0;
    for (int i = 0; i <nvel; i ++) {// 我们计算平均值
      vel = vel + v [i];
    }
    vel = vel / nvel;

我们将使用速度值来计算 PID 控制的微分分量。我们还通过串行端口/USB 电缆发送它，以便处理软件接收它并以图形方式表示它。

第一个近似值：比例项

如果是将球带到杆的中心，那么我们应该将杆倾斜得越多，球离中心越远，这似乎是合乎逻辑的。我们用取负值（向上）的变量pos的值来确定舵机的旋转，它决定了杆的倾斜度：

和正（向下）从水平条左侧的值 0 开始：

由于我们在变量dist中有球的位置，“球离中心越远，杆越倾斜”在软件中写为：

pos = Kp * dist

其中 Kp 是一个常数。

因此，我们实现了 PID 控制的比例项。

– 别告诉我这不容易！

– 是的，当然……我们为 Kp 分配什么值。

为了给 Kp 赋值（对于后面的 Kdifferential 和 Kintegral），我们首先给它赋值：1、100、235、0.01 或任何你想要的值，我们将观察系统的行为。

我们正在寻找当球接近目标但尚未到达时足以倾斜杆的最小值。大数值让球跑得太快，然后不得不停在中间！

对于构建的系统，合适的值是 2。只有比例项，球永远不会稳定。

以下视频显示了以下效果：

– Kp = 1。太低。在点 0 附近几乎没有影响。
– Kp = 100。太高了。球加速太多。
– Kp = 2。它适用于我们的系统。

术语差异

术语“差异”作用于当前周期和前一个周期之间的位置差异。也就是说，关于球的速度，因为一次测量和下一次测量之间经过的时间总是相同的。这个时间是测量和反应周期，在我们的系统中是 50ms。

微分项在软件中写成：

pos = Kd * vel

在程序的前 3 行中，我们定义了 PID 控制的 3 个常量的值：

float Kp =0; 
float Kd = 100;
float Ki =0;

我们正在做的是使杆倾斜与球的速度相反。我们首先为 Kd 赋予任何值。

– 我可以给它值 10，这是我最喜欢的数字吗？

– 是的。前进。

点击这里即可观看

我们已经设置 Kp = 0 以便比例项不参与，并且 Kd = 10 作为起点。据观察，这是一个不足的值，因为它没有足够的反应来停止球。

我们上升到 Kd = 100：反应过度，系统不稳定。

正确的值在 10 到 100 之间。我们用 50、25 进行测试……观察系统在微分控制下的行为。可接受的值为 Kd = 35。

我们设法非常有效地阻止了球！

我们的目标是将球停在中点。通过将杆倾斜得越远，比例控制使球更靠近中心。球移动得越快，导数控制使杆倾斜得越多，并设法阻止它。现在观察到2个学期的共同作用下，定义POS伺服作为银行足球比赛：

    pos = Kp * dist + Kd * vel；

这已经有效了！

比例项和导数项的组合作用是 PD 控制，足以满足许多应用。它的弱点之一是当球停在中心点附近时，它不再做出反应。由于速度为 0，微分项不起作用。由于它靠近中心点，按比例项的杆倾斜很小，可能不足以移动球。这就是这次发生的事情：

这永远不会发生在远离中心的位置，因为比例项已经有足够的实体，因此它的倾斜度会使球移动。

– 我们应该增加比例项吗？

正如我们在之前的视频中看到的 Kp = 100，高 Kp 值会导致系统不稳定。

解决方案是我们需要有一个真正的 PID 控制的字母：积分的 I。

精度：积分项

积分项考虑了球的位置（如比例项）以及它在那里的时间。更严格地说：就像微分项作用于速度（球的位置随时间的导数）一样，积分项作用于位置曲线下随时间变化的区域。

该区域取决于积分区间。如果我们连续积分曲线下的面积，积分项只会导致系统不稳定。我们只会在球距杆中心小于 4 厘米时对球的位置进行积分。当距离中心小于 8 mm 时，我们认为目标已实现，我们停止积分。如果我们超出一个间隔，我们将重置 I (I = 0)。我们在软件中写成：

int Rint = 8;
int Rext = 40;

    if(abs(dist)>Rint && abs(dist)<Rext){
      I=I+dist*Ki;
    } 
    else {
      I=0;
    }
 pos=Kp*dist+Kd*vel+I;

也就是说，当球距离中心小于 4 厘米且大于 8 毫米时，我们将取其与中心的距离，将其乘以 Ki 并将结果累积在 I 中。我们在那里呆的时间越多，我就会变得越大。
总之，积分项提供了更高的精度，但必须方便地限制其作用，否则会带来太多的不稳定性。

Arduino总程序

#include <Servo.h>
//#include <Wire.h>
//#include "IICLiquidCrystal.h"

// Connect via i2c, default address #0 (A0-A2 not jumpered)
//LiquidCrystal lcd(0);

float Kp = 3;   //2
float Kd = 1;  //35
float Ki = 0.5; //0.1
int Rint = 8;   //
int Rext = 40;  //
int aim = 0;
unsigned long time = 0; //execution time of the last cycle
unsigned long timeSerial = 0;
int period = 50;        //Sampling period in ms
int sensorPin = 0;        //Analog Pin where the Distance Sensor signal is connected
int measure;            //What the sensor measures. They are ADCs.
int dcal [] = {         //Remote ADC calibration
  -193, -160, -110, -60, 0, 40, 60, 90, 120
};
int ADCcal [] = {
  177, 189, 231, 273, 372, 483, 558, 742, 970
};
int lastDist;     //Previous value of Distance to calculate Speed
int dist;         //distance in mm with 0 in the center of the bar
int nvel = 5;       //number of velocity values over which we calculate the average
int v[5];
int vel;          //mean value of the last speed levels
float I;          //Integral Value


Servo myservo;    //create servo object to control a servo
float pos;
float reposo = 1350; //value held by horizontal bar

int ledPin = 13; //Green led pin.

void setup()
{
  analogReference(EXTERNAL);  //AREF connected to 3.3V
  myservo.attach(3);         //attaches the servo on pin X to the servo object
  Serial.begin(115200);
  pinMode(ledPin, OUTPUT);

  myservo.writeMicroseconds(reposo);
  delay(5000);

  //lcd.begin(16, 2);

}

void loop()
{
  if (millis() > timeSerial + 200)
  {
    timeSerial = millis();
//    Kp = map(analogRead(A1), 0, 1023, 0, 5000) / 100.0;
//    Kd = map(analogRead(A2), 0, 1023, 0, 400) / 100.0;
//    Ki = map(analogRead(A3), 0, 1023, 0, 300) / 100.0;

    //aim = map(analogRead(A5), 0, 1023, -20, 20);

    Serial.println();
    Serial.print("Kp:");
    Serial.println(Kp);

    Serial.print("Kd:");
    Serial.println(Kd);

    Serial.print("Ki:");
    Serial.println(Ki);

    Serial.print("Aim:");
    Serial.println(aim);

    Serial.print("Pos:");
    Serial.println(dist);
  }

  
  if (millis() > time + period) { //
    time = millis();

    //    lcd.setCursor(0, 0);
    //    lcd.print("Kp:");
    //    lcd.print(Kp);
    //
    //    lcd.setCursor(8, 0);
    //    lcd.print("Kd:");
    //    lcd.print(Kd);
    //
    //    lcd.setCursor(0, 1);
    //    lcd.print("Ki:");
    //    lcd.print(Ki);
    //
    //    lcd.setCursor(8, 1);
    //    lcd.print("Pos:");
    //    lcd.print(dist);






    //We measure DISTANCE
    measure = analogRead(sensorPin);
    measure = constrain(measure, ADCcal[0], ADCcal[8]);
    lastDist = dist; //We save the previous value of dist to calculate the speed
    for (int i = 0; i < 8; i++) { //We apply Calibration curve from ADC to mm
      if (measure >= ADCcal[i] && measure < ADCcal[i + 1]) {
        dist = map(measure, ADCcal[i], ADCcal[i + 1], dcal[i], dcal[i + 1]);
      }
    }
    //Average SPEED calculation
    for (int i = 0; i < nvel - 1; i++) { //We all move to the left to free the last one.
      v[i] = v[i + 1];
    }
    v[nvel - 1] = (dist - lastDist); //We put a new data
    vel = 0;
    for (int i = 0; i < nvel; i++) { //We calculate the mean
      vel = vel + v[i];
    }
    vel = vel / nvel;
    // Integral
    if (abs(dist - aim) > Rint && abs(dist - aim) < Rext) { //Only if it is inside (-Rext, Rext) and outside (-Rint, Rint)
      I = I + dist * Ki;
    }
    else {
      I = 0;
    }
    //We calculate servo position
    pos = Kp * (dist - aim) + Kd * vel + I;
    myservo.writeMicroseconds(reposo + pos);

    if (abs(dist) < Rint) { //If we are inside Rint turn on Led
      digitalWrite(ledPin, HIGH);
    }
    else {
      digitalWrite(ledPin, LOW);
    }

    if (1) { //Shipping for PROCESSING
      Serial.print(dist + 200);
      Serial.print(",");
      Serial.print(dist + 200);
      Serial.print(",");
      Serial.print(vel);
      Serial.print(",");
      Serial.print(vel);
      Serial.print("$");
    }
    if (0) { //Debug
      Serial.print(millis());
      Serial.print(" ms|dist: ");
      Serial.print(dist);
      Serial.print("|vel: ");
      Serial.print(vel);
      Serial.print("|Kp*dist: ");
      Serial.print(Kp * dist);
      Serial.print("|Kd*vel: ");
      Serial.print(Kd * vel);
      Serial.print("|Int: ");
      Serial.print(I);
      Serial.print("|pos: ");
      Serial.println(pos);
    }
    if (0) { //To calibrate Distance sensor
      Serial.print(dist);
      Serial.print("mm     ADC: ");
      Serial.println(measure);
    }
    if (0) { //DeBug Speeds
      for (int i = 0; i < (nvel); i++) {
        Serial.print(v[i]);
        Serial.print(",");
      }
      Serial.print("       vel:");
      Serial.println(vel);
    }
  }
}

Processing总程序

// Arduino: _14_HV_logger_2Ch
// VOLTAGE IN PIN Analog0
// CURRENT IN PIN Analog1
import processing.serial.*;
Serial USB;
String message = null;
int jmax = 100000; // Stored readings
int[] VminADC; // From 0 to jmax
int[] VmaxADC; // Raw values 
int[] IminADC;
int[] ImaxADC;
float[] Vmin; // From 0 to jmax
float[] Vmax; // in kV and uA
float[] Imin;
float[] Imax;
int j = 0; // Whole Register
int x = 0; // On Screen
int xpant = 1000; // Dimensions of the screen
int ypant = 800;
int mSup = 60; // Margin Sup and Inf of the trace
int mInf = 50;
int mLat = 50;
int xgraf = xpant - 2 * mLat;
int xDisplay = 30; // Beginning of the text
int yDisplay = 30;
float Cal0 = 1;
float Cal1 = 1;
float CalVEL =  300; // 
float CalPOS = 400; // 
PFont fontVerdana;
String [] com = new String [3];
void setup() {
  size(1000, 800, P3D);
  println(Serial.list());
  String portName = Serial.list()[0]; //    Puerto COM 14 
  USB = new Serial(this, portName, 115200);
  VminADC = new int [jmax];
  VmaxADC = new int [jmax];
  IminADC = new int [jmax];
  ImaxADC = new int [jmax];
  Vmin = new float [jmax];
  Vmax = new float [jmax];
  Imin = new float [jmax];
  Imax = new float [jmax];
  com = new String [3];
  fontVerdana = loadFont("Verdana-20.vlw");
}
void draw() {
  background(190);
  // New Data ***************************************************************************
  while (USB.available () > 0) {
    message = USB.readStringUntil(36); // 644,659,725,733$
    if (message != null) {    
      if (j < jmax - 2) {
        j++;
      } 
      else {
        j = 0;
      }
      message = message.substring(0, message.length()-1); // 644,659,725,733
      String[] com = splitTokens(message, ",");
      VminADC [j] = int(com[0])-200;   // Stored in VminADC[j] as ADC 
      VmaxADC [j] = int(com[1])-200;
      IminADC [j] = int(com[2]);
      ImaxADC [j] = int(com[3]);
      print(VminADC [j]);
      print("\t");
      print(VmaxADC [j]);
      print("\t");
      print(IminADC [j]);
      print("\t");
      println(ImaxADC [j]);
      Vmin [j] = VminADC [j] * Cal0;  // Stored in Vmin[j] as kV
      Vmax [j] = VmaxADC [j] * Cal0;
      Imin [j] = IminADC [j] * Cal1;
      Imax [j] = ImaxADC [j] * Cal1;
    }
  }

  //  Axis ********************************************************************************
  stroke(255);
  strokeWeight(2);
  line(mLat-10, ypant-mInf, xpant-mLat, ypant-mInf);
  line(mLat, ypant-mInf+10, mLat, mSup);
  // Reference Axis *************************************************************************************
  /*  fill(#FF1C20); // Red
   //  text("500uA", 10, (ypant-mInf)-500*(ypant-mSup-mInf)/660+4);
   stroke(#FF1C20, 20); // Red
   line(mLat, (ypant-mInf)-500*(ypant-mSup-mInf)/660, xpant-mLat, (ypant-mInf)-500*(ypant-mSup-mInf)/660);
   */
  fill(#321CFF); // Blue
  text("0", 30, mSup+(ypant-mInf-mSup)/2+4);
  stroke(#321CFF, 20); // Blue
  line(mLat, mSup+(ypant-mInf-mSup)/2, xpant-mLat, mSup+(ypant-mInf-mSup)/2);

  // Draw before 1st scroll ********************************************************************************************
  if (j <= xgraf) {
    for ( int i = 0; i < j; i ++) {
      strokeWeight(1);
      stroke(#FF1C20); // Red VELOCIDAD
      line(i+mLat, int(mSup+(ypant-mInf-mSup)/2)-Imin[i]*(ypant-mInf-mSup)/CalVEL-2, i+mLat, int(mSup+(ypant-mInf-mSup)/2)-Imax[i]*(ypant-mInf-mSup)/CalVEL+2);

      stroke(#321CFF); // Blue POSICIÓN
      line(i+mLat, int(mSup+(ypant-mInf-mSup)/2)-Vmin[i]*(ypant-mInf-mSup)/CalPOS-2, i+mLat, int(mSup+(ypant-mInf-mSup)/2)-Vmax[i]*(ypant-mInf-mSup)/CalPOS+2);
    }
  }
  // Draw with scroll **************************************************************************************************************
  if (j > xgraf) {
    for ( int i = 0; i <= xgraf; i ++) {
      strokeWeight(1);
      stroke(#FF1C20); // Red
      //      line(i+mLat, int((ypant-mInf)-Imin [j-xgraf+i]*(ypant-mSup-mInf)/660), i+mLat, int((ypant-mInf)-Imax [j-xgraf+i]*(ypant-mSup-mInf)/660));      
      line(i+mLat, int(mSup+(ypant-mInf-mSup)/2)-Imin[j-xgraf+i]*(ypant-mInf-mSup)/CalVEL-2, i+mLat, int(mSup+(ypant-mInf-mSup)/2)-Imax[j-xgraf+i]*(ypant-mInf-mSup)/CalVEL+2);
      stroke(#321CFF); // Blue
      // line(i+mLat, int((ypant-mInf)-Vmin [j-xgraf+i]*(ypant-mSup-mInf)/15), i+mLat, int((ypant-mInf)-Vmax [j-xgraf+i]*(ypant-mSup-mInf)/15));
      line(i+mLat, int(mSup+(ypant-mInf-mSup)/2)-Vmin[j-xgraf+i]*(ypant-mInf-mSup)/CalPOS-2, i+mLat, int(mSup+(ypant-mInf-mSup)/2)-Vmax[j-xgraf+i]*(ypant-mInf-mSup)/CalPOS+2);
    }
  }
  // Text Channels Readings ***************************************************************
  stroke(190);
  fill(190); // Blue
  rect(30, 12, 330, 20);
  textFont(fontVerdana, 20);
  fill(#321CFF); // Blue
  text(int(Vmin[j]) +" mm", xDisplay, yDisplay);
  textFont(fontVerdana, 10);
  //  text("+/-" + nf((Vmax[j] - Vmin[j]), 0, 1) +" kV", xDisplay + 80, yDisplay);
  textFont(fontVerdana, 20);
  fill(#FF1C20); // Red
  text(int(Imax[j]) +" mm/s", xDisplay + 200, yDisplay);
  textFont(fontVerdana, 10);
  //  text("+/-" + int((Imax[j] - Imin[j])) +" uA", xDisplay + 200 + 80, yDisplay);
}
/*void keyPressed() {
 if (key == 's' || key =='S') {
 grabar();
 }
 if (key == 'v' || key =='V') {
 Cal0 = 9.0/((VminADC [j]+VmaxADC [j])/2); // 9 kV equals 489 ADC
 println("Calibración de Tensión: "+ Cal0);
 }
 if (key == 'i' || key =='I') {
 Cal1 = 660.0/((IminADC [j]+ImaxADC [j])/2);
 println("Calibración de Intensidad: "+ Cal1);
 }
 }
 */
void grabar() {
  String[] lines = new String[j];
  for (int i = 0; i < j; i++) {
    lines[i] = str(Vmin [i+1]) + "\t" + str(Vmax [i+1]) + "\t" + str(Imin [i+1]) + "\t" + str(Imax [i+1]); // Vmin  Vmax  Imin  Imax
  }
  saveStrings("Registro.txt", lines);
  exit();
}

创元素

广州大学城的创客空间

分类目录归档：未分类

使用 Arduino 进行杆球平衡系统的 PID 控制

使用 Arduino 进行杆球平衡系统的 PID 控制

套件淘宝购买链接:

杆和球系统

球位感应

传感器信号调理

传感器校准

执行器

控制器

5V电源

完整系统示意图

PID 控制。ARDUINO 软件。

测量和反应期

球速计算

第一个近似值：比例项

术语差异

精度：积分项

Arduino总程序

Processing总程序