控制工程实验
1.控制目标简述
1.png
本次控制实验的目标是使用螺旋桨的升力完成对一个一端有重物的平衡杆相对于桌面的角度控制。平衡杆应能自动从任意位置自动到达设定的角度。
2.购买材料
我们选用航模用的铁配重块作为重物，选用普通130电机和模型螺旋桨提供升力，计划用一根长木条作为平衡杆的主体，在上面加装2mm粗的钢轴，以及微型轴承。用两块木块支撑钢轴，完成主体的搭建。另外，选用mpu6050模块读取角度，用l289n电机驱动模块驱动电机，用arduino控制以上两种芯片。
2.png
3.装配
获得材料后，我们进行了装配，钢轴如计划能相对平衡杆灵活转动，阻力较小。后来，又对基座进行固定，安装了电机、mpu6050模块。
3.jpg
4.jpg
5.jpg
值得一提的是，平衡装置十分敏感，0.2克左右的力就足以改变它的角度，普通的导线弹性太大，会给平衡杆施加较大弹力，我们把导线换成了细而轻的漆包线，解决了这个问题。
4.驱动电机
尝试用l289n模块驱动电机，取得了成功，驱动的原理是向电机发送不同占空比的PWM波，以调节转速，占空比可以在0至256之间调节。
5.测试电机的升力
为测试输入电压的占空比数值，和电机升力之间的关系，进行了如下图的实验：
6.jpg
7.png
其中重物下方是一高精度的电子秤，通过改变配重位置，把平衡杆调节至水平；把称垫高，让重物恰好接触称面，并把电子秤的示数调零。这时驱动电机产生升力，因为平衡杆两臂等长，称的示数就是电机升力所对应的质量。
为了方便这个测试过程，我们在arduino上烧录如下代码：
void setup() {
pinMode(4,OUTPUT);
pinMode(2, OUTPUT);
pinMode(3,OUTPUT);
pinMode(A0, INPUT);
Serial.begin(9600);
}
void loop() {
digitalWrite(2,HIGH);
digitalWrite(4,LOW);
for(int i = 0; i < 256; i=i+20){
analogWrite(3, i);
delay(10000);
}
Serial.println(analogRead(A0));
}
程序的基本思路是从0开始，每10秒把占空比数值提高20。10秒的间隔时间可以让称的读数稳定，让我们记下读数。最后，我们得到了这样的结果：
8.jpg
观察数据点可以发现近似的一次规律，所以我们用一次函数进行了最小二乘拟合。
6.读取角度
使用MPU6050惯性测量单元和其板载DMP单元，直接读取chip在世界坐标系内的姿态角（Yaw Pitch Roll）
值得一提的是，平衡装置十分敏感，0.2克左右的力就足以改变它的角度，普通的导线弹性太大，会给平衡杆施加较大弹力，我们把导线换成了细而轻的漆包线，解决了这个问题。
7.尝试控制
初次尝试控制时，我们从简单的开始，先采用单一的P控制，我们在arduino上烧录了如下程序：
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
#define INTERRUPT_PIN 2
#define I2CDEV_MPU_ADD_PIN 3
#define MOTER_INT_1_PIN 4
#define MOTER_PWM_PIN 5
#define MOTER_INT_2_PIN 6
#define I2CDEV_MPU_ADD_SET LOW
// CTRL_TERM in millis-second
#define CTRL_TERM 10
MPU6050 mpu;
// mpu control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
float rollAngle = 0; // roll angle in degree current
float rollAngleSet = 0; // roll angle seting
float dertAngle = rollAngle - rollAngleSet;
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
volatile bool mpuInterrupt = false;
void dmpDataReady(){
mpuInterrupt = true;
}
int dirCtrl = 2;
float pwmCtrl = 0;
unsigned long time = 0;
unsigned long timeCount = 0;
unsigned long termCount = 0;
void moter(float pwm){
if(pwm > 0){
// up rolling
digitalWrite(MOTER_INT_1_PIN, HIGH);
digitalWrite(MOTER_INT_2_PIN, LOW);
analogWrite(MOTER_PWM_PIN, (int)pwm);
}else if(pwm < 0){
// down rolling
digitalWrite(MOTER_INT_1_PIN, LOW);
digitalWrite(MOTER_INT_2_PIN, HIGH);
analogWrite(MOTER_PWM_PIN, - (int)pwm);
}else{
// stop
digitalWrite(MOTER_INT_1_PIN, LOW);
digitalWrite(MOTER_INT_2_PIN, LOW);
digitalWrite(MOTER_PWM_PIN, 0);
}
return;
}
void setup(void){
// I2C dev initialize
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock
Serial.begin(115200);
// mpu and dmp initialize
mpu.initialize();
devStatus = mpu.dmpInitialize();
if(devStatus == 0){
mpu.setDMPEnabled(true);
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
}
// modify the offset
mpu.setXAccelOffset(-316);
mpu.setYAccelOffset(983);
mpu.setZAccelOffset(1515);
mpu.setXGyroOffset(0);
mpu.setYGyroOffset(0);
mpu.setZGyroOffset(0);
// moter initialize
pinMode(I2CDEV_MPU_ADD_PIN, OUTPUT);
digitalWrite(I2CDEV_MPU_ADD_PIN, I2CDEV_MPU_ADD_SET);
pinMode(MOTER_INT_1_PIN, OUTPUT);
pinMode(MOTER_INT_2_PIN, OUTPUT);
pinMode(MOTER_PWM_PIN, OUTPUT);
return;
}
const float Kp = 0.01;
void loop(void){
rollAngleSet = 5;
while(!mpuInterrupt && fifoCount < packetSize){
}
// read package
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
if((mpuIntStatus & 0x10) || fifoCount == 1024){
mpu.resetFIFO();
}else if(mpuIntStatus & 0x02){
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
if((millis() - time) > 10){
time = millis();
termCount++;
rollAngle = ypr[2] * 180/M_PI;
dertAngle = rollAngle - rollAngleSet;
}
// PID
pwmCtrl += dertAngle * Kp;
pwmCtrl = (pwmCtrl < -255)?(- 255):pwmCtrl;
pwmCtrl = (pwmCtrl > 255)?255:pwmCtrl;
moter(pwmCtrl);
// print
if((millis() - time) > 100){
Serial.print("angle setting\t");
Serial.print(rollAngleSet);
Serial.print("\troll angle\t");
Serial.print(rollAngle);
/*
Serial.print("\tdert angle\t");
Serial.print(dertAngle);
Serial.print("\tpwmCtrl\t");
Serial.println(pwmCtrl);
*/
Serial.print("\ttime\t");
Serial.println(timeCount++ * 100);
}
}
return;
}
该程序读取mpu6050模块返回的角度，把它和设定的角度作差，得到差值dertAngle，并乘上系数Kp，便得到PWM波占空比的数值，再用此数值驱动电机。
经过对Kp的调整和实验，我们的控制取得了一定的成果。在程序中设定一角度，程序启动后，电机会自主调节速度，最终使得平衡杆稳定于设定好的角度。但暂态时间较长，长达十几秒，这可能是单一的p控制性能不够好引起的，为了缩短调整时间，还需要加入I控制的环节。
网友评论