使用 Arduino 进行杆球平衡系统的 PID 控制
通过研究杆和球系统并使用Arduino作为控制器，很容易理解PID 控制。目标是通过使用闭合控制回路方便地倾斜球，将球放置在杆的中心。
套件淘宝购买链接:
Arduino PID 控制 球 PID算法 控制原理 控制工程 经典
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();
}
