力学系统的平衡控制

基本原理

信号与系统

自动控制理论

开环控制与闭环控制

PID控制系统

一个例子(from陶鑫):自平衡小车

实验仪器

STM32 单片机

感谢沈金辉学长整理的资料:STM32 Nucleo 单片机开发与应用

STM32F401RE管脚图:

补充几个在mbed环境下常用的预处理变量(针对STM32F401RE):

Variable Name Physical Pin Alias
LED2 绿色LED(标志为LD2)对应端口
USER_BUTTON 蓝色按钮对应端口
SERIAL_RX I2C通讯接收端(Receive) USBRX
SERIAL_TX I2C通讯接收端(Transmit) USBTX
HIGH (int) 1
LOW (int) 0

参考来源:http://www.st.com/content/st_com/en/products/evaluation-tools/product-evaluation-tools/mcu-eval-tools/stm32-mcu-eval-tools/stm32-mcu-nucleo/nucleo-f401re.html

MPU-6050 六轴传感器

MPU6050模块端口:

名称功能
VCC 3.3V电压输入
GND 接地
RX 串口通讯接收端(receive)
TX 串口通讯发送端(transmit)
SDA I2C通讯数据线
SCL I2C通讯时钟线

MPU6050的性能参数:

电压:3V~6V
电流:<10mA
体积:15.24mm X 15.24mm X 2mm
焊盘间距:
    上下100mil(2.54mm)
    左右600mil(15.24mm)
测量维度:
    加速度:3维
    角速度:3维
    姿态角:3维
量程:
    加速度:±16g
    角速度:±2000°/s
分辨率:
    加速度:6.1e-5g
    角速度:7.6e-3°/s
稳定性:
    加速度:0.01g
    角速度:0.05°/s
姿态测量稳定度:0.01° 
数据输出频率:100Hz(波特率115200)/20Hz(波特率9600)
数据接口:串口(TTL电平),I2C(直接连MPU6050,无姿态输出)
波特率:115200kps/9600kps

实验设置

本实验的初步构想如下: 实用STM32F401RE单片机作为信号处理器,MPU-6050作为角度传感器,将采集的信号通过I2C通讯的方式传入单片机。 经过数据处理,得到即时的角度\theta、角度积分delim{}{\sum{i}{}{\theta_i}}{}、和角速度值\omega,按照PID方法求和得到最终的重物位置输出 delim{}
{x = K_p  \theta + K_i  ( \sum{i}{}{\theta_i} ) + K_d  \omega
}{}

可以通过机器学习的方法,让单片机自己优化参数,得到保持系统平衡的最优解delim{\lbrace}{ K_p \, K_i \, K_d }{\lbrace}

实验进展

09192016 - 电子学PID模拟实验


搭建了电子PID控制器,其电路图如图1。容易计算,其等效的PID系数为:
delim{lbrace}{
    matrix{3}{1}{
        { K_p = \alpha{\tau_{11}+\tau_{22} }/{ \tau_{12} } }
        { K_i = \alpha 1/{ \tau_{12} } }
        { K_d = \alpha \tau_{21}  }
    }
}{} 其中: delim{lbrace}{
    matrix{2}{2}{
        {{\alpha} = -{R_4}/{R_3}} {}
        {\tau_{ij} = R_i C_j} {i,j\in{1,2}}
    }
}{}

在一些典型参数下的输入信号(紫色)、输出信号(黄色)如图2、3所示。其中有一部分由于运放输出电压饱和,呈被“截断”状。

图1
图1 电子学PID控制器电路图

图2 matrix{1}{5}{
    {\alpha=1}
    {R_1=82k\Omega}
    {C_1=0.0210{\mu}F}
    {R_2=82k\Omega}
    {C_2=9.6nF}
}
图2 PID控制器的输出信号(黄色)在输入的方波信号上升沿(紫色)附近的响应(1)

图3 matrix{1}{5}{
    {\alpha=1}
    {R_1=82k\Omega}
    {C_1=0.1010{\mu}F}
    {R_2=82k\Omega}
    {C_2=9.6nF}
}
图3 PID控制器的输出信号(黄色)在输入的方波信号上升沿(紫色)附近的响应(2)

在固定其他参数,调节R_1时,K_i会改变而K_d不变,而K_i与信号稳态的斜率{du}/{dt}成正比:

{du}/{dt}\propto K_i \propto 1/{R_1} ,可以按照{du}/{dt} = k 1/{R_1}+b对其拟合。

固定其他参数,调节C_1时,K_d会改变而K_i不变,而K_d与信号瞬态振荡衰减的振荡周期T成正比:

T \propto K_d \propto C_1 ,可以按照T = k C_1 + b对其拟合。

FIXME See the comment. 1)

图4
图4 其他条件不变时,信号斜率{du}/{dt}随电阻的倒数{R_{1}}^{-1}的变化关系图
拟合结果: delim{lbrace}{
    matrix{3}{1}{
        { k = 9.815 \times 10^4 }
        { b = 1.824 }
        { R^2 = 0.9996 }
    }
}{}

图5
图5 其他条件不变时,信号振荡周期 T随电容C_{1}的变化关系图
拟合结果: delim{lbrace}{
    matrix{3}{1}{
        { k = 0.6695 }
        { b = 58.93 }
        { R^2 = 0.9772 }
    }
}{}

10122016 - 学习使用STM32单片机与MPU6050六轴传感器


电路部分的连接

将传感器MPU6050的管脚焊接完成,并连接到了STM32上。

MPU6050与STM32F401RE

MPU6050的性能参数见:mpu-6050_六轴传感器

STM32单片机的编程学习

利用”mbed.h”头文件,可以使用已定义好的C/C++变量编写程序。通过mbed编译程序,可以得到对应的二进制文件”*.bin”。

在已经安装好硬件驱动的情况下,将二进制文件复制到单片机所在磁盘内即可完成烧录。需要注意:单片机联建电脑后的第一次复制常常会失败,之后的复制均可成功。

利用mbed库编写了测试单片机数字输出(LED1)、按钮(USER_BUTTON)与USB串口通讯的C++程序(如下),经测试可以正常读取数据。

 
#include "mbed.h" // Use library "mbed-os" for Thread CLASS 
 
//------------------------------------
// Hyperterminal configuration
// 9600 bauds, 8-bit data, no parity
//------------------------------------
 
class State {
public:
    State();
    bool isPressedDown(); 
    void refresh();
    void switchLed();
    void count(int n);
    Serial* pc; // A pointer to Serial object
    DigitalOut led;
    DigitalIn btn;
    bool lastInput;
    int pressCount;
};
 
State setup();
void loop(State& state);
 
int main() {
    State state = setup();
    while(true) { 
        loop(state);
    }
}
 
State setup(){
    State state;
    state.pc->printf("Hello World !\n");
    return state;
}
 
void loop(State& state) {
    state.refresh();
    Thread::wait(5); // wait for 5 ms
    if ( state.isPressedDown() ) {
        if ( (state.pressCount)%2!=0 )
            state.pc->printf("Button pressed for %d times. LED status: OFF\n", state.pressCount);
        else
            state.pc->printf("Button pressed for %d times. LED status: ON\n", state.pressCount);
        state.count(1);
        state.switchLed(); 
    }
}
 
// ********************************************************
// ******** BELOW IS DEFINITIONS OF CLASS "State" *********
// ********************************************************
 
// constructor of class State
State::State():
    //pc(TX, RX),
    led(LED1),
    btn(USER_BUTTON),
    lastInput(false),
    pressCount(1) {
    pc = new Serial(SERIAL_TX, SERIAL_RX);
    pc->baud(9600);
    led = 1;
}
 
bool State::isPressedDown(){
    return ( lastInput != btn ) && ( lastInput == false );
}
 
void State::refresh(){
    lastInput = btn;
}
 
void State::switchLed() {
    led = !led;
}
 
void State::count(int n) {
    this->pressCount += n;
}

参考资料:STM32F401RE相关资料

MPU-6050的特性与数据读取

使用开源的函数库”i2c_mpu6050.lib”,成功地直接读取传感器的三个加速度与三个角速度分量,以及经过计算的方位角。

由于角度为直接积分角速度得到的,会有很大偏差,所以接下来将使用kalman filter完成程序来解析MPU6050的输出数据。

Kalman filter是现在多用于动态系统的一种方法,能对实时数据进行更新,有效减少噪声误差。

参考资料: kalman filter

10172016 - 深入学习MPU6050与STM32的特性


MPU6050传感器的响应

利用”mpu6050.h”中自带的补偿滤波方法,将传感器绕手肘为中心正、反翻转,输出起角度测量值,可得如下图所示输出:

图中可看出:在翻转较慢时(约90^{\circ}每秒),角度输出可以达到0^{\circ}180^{\circ};而在运动较快时(约270^{\circ}每秒),其角度输出幅度明显减小,小于180^{\circ}。这是因为(近似)圆周运动的向心加速度影响了加速度传感器的测量。

在设计仪器的机械部分时,可以适当增加平衡杆的转动惯量,使其偏离平衡时的角速度较小,增加传感器输出的精度,也便于电机的响应、调节。

深入学习STM32的特性

相比Arduino,STM32拥有许多更为先进的硬件、软件特性。个人以为,其中最令人瞩目的一点是它支持并行运算(多线程任务)。下面介绍一些实现并行运算的方法

在Arduino等单线程单片机中,实现电位的读取、输出通常需要借助wait()函数,例如如下伪代码实现两个电平的交错闪烁:

int main() {
    DigitalOut led1(PIN1);
    DigitalOut led2(PIN2);
    led1 = HIGH;
    led2 = LOW;
    while (true) {
        wait(0.5);
        led1 = LOW;
        led2 = HIGH;
        wait(0.5);
        led1 = HIGH;
        led2 = LOW;
    }
    return 0;
}

若希望两个LED以不同频率闪烁,则较为复杂。在STM32 Nucleo中,可以使用多种方法完成这个任务:

方法1:Ticker类

Ticker类的实例可以按照一定的时间间隔反复执行某一个函数。例如我们希望led1每pi=3.14159秒闪烁一次,同时led2每e=2.71828秒闪烁一次,可以通过如下方法实现(伪代码):

#include "mbed.h"
 
const double pi = 3.14159;
const double e = 2.71828;
 
DigitalOut led1(PIN1);
DigitalOut led2(PIN2);
 
void toggleLED1() {led1 = !led1;}
void toggleLED2() {led2 = !led2;}
 
int main() {
    Ticker tic, toc; // construct two independent Tickers
    tic.attach(&toggleLED1, pi);
    toc.attach(&toggleLED2, e);
    while (true) { wait(10); }
    return 0;
}

方法2:Thread类

使用函数指针作为参数初始化Thread类,可以为该函数开启一个独立的线程并开始执行。不同线程之间可以通过Queue、Signal、 Semaphore等工具通信。使用Thread类完成上述相同的工作:

#include "mbed.h"
#include "rtos.h"
 
const double pi = 3.14159;
const double e = 2.71828;
 
DigitalOut led1(PIN1);
DigitalOut led2(PIN2);
 
void twinkleLED1() {
    while (true) {
        led1 = !led1;
        Thread::wait(pi*1000); // unit: ms
    }
}
 
void twinkleLED2() {
    while (true) {
        led2 = !led2;
        Thread::wait(e*1000); // unit: ms
    }
}
 
int main() {
    Thread thread1(twinkleLED1);
    Thread thread2(twinkleLED2);
    while (true) { wait(10); }
    return 0;
}

使用Thread配合Queue,可以实现生产者-消费者结构,达到数据处理、储存与采样独立的效果,以实现精确的采样频率。

参考资料 mbed online ref.

11012016 - 仪器机械部分的制作与STM32控制程序的架构



机械部分的制作


初步计划:使用“跷跷板”结构作为系统的大致结构,在上面安置步进电机、传感器、重物等器件。传感器安装在较靠近转轴处,可有效避免较大加速度引起的角度计算误差。 使用经过切割、打磨的木板作为“跷跷板”的“板”;将圆柱状木条粘在木板上,作为转轴;在转轴两端安装轴承,并固定在架子上,即构成了机械部分的大致结构。

STM32控制程序的架构与初步测试


改程序实现了生产者-消费者的书记处理模式,可以并发、异步地完成信号的读取、数据的处理和信号的输出。
其具体功能为:

  • 生产者(producer)在PC串口读取输入,将原信息发送至消息邮箱(mail)中,等待消费者读取。
  • 消费者(consumer)读取消息邮箱中的信息,并且将读取后的数据加以处理,传入PC串口。

在实际运用中,我们需要实现 producerMode2()consumerMode2() 两个函数分别达成角速度数据采集和PID计算、变频方波脉冲输出。

主程序:

/************** main.cpp **************/
#include "main.h"
 
int main() {
	Thread producerTread (producer);
	Thread consumerThread (consumer);
	producerTread.join();
	consumerThread.join();
	return 0;
}

主程序简单明了:开启生产者消费者两个线程,两线程并行执行,到两者都执行完成时,结束程序。

头文件:

/************** main.h **************/
#include "mbed.h"
#include "rtos.h"
#include "Serial.h"
 
// Definitions of pins
/** defs left for motor
#define PUL D5
#define DIR D4
#define P5V D3
#define ENBL D2
*/
 
// Setting of communication
const int boxSize = 32;
struct MessageT {
    float frequency;
};
Mail<MessageT, boxSize> messageQueue;
// Serial should be accessed by only one thread each time
Mutex serialLock; 
 
///////////////////////////////////////////////
// Set consumer mode
//     mode 1 : display on PC (default)
//     mode 2 : transfer on motor
// DONE
const int consumerMode = 1;
 
///////////////////////////////////////////////
// Set producer mode
//     mode 1 : get from pc (default)
//     mode 2 : get from PID output
// DONE
const int producerMode = 1;
 
///////////////////////////////////////////////
// Declarations of p/c model
 
void producer ();
void consumer ();
void consumerMode1 ();
void consumerMode2 ();
void producerMode1 ();
void producerMode2 ();
 
///////////////////////////////////////////////
// implements of p/c model
 
void producer () {
	switch (producerMode) {
		case 1 :
			producerMode1 ();
			break;
		case 2 :
			producerMode2 ();
			break;
		default:
			producerMode1 ();
	}
}
 
void producerMode1 () {
	// read pulse frequency input from PC serial
    Serial pc (SERIAL_TX, SERIAL_RX);
    float tmpD = 0.0;
    int ret = 0;
    Thread::wait(500);
    serialLock.lock ();
    pc.printf("\n\rHello from PROCUCER loop!\n\r");
    serialLock.unlock ();
    while (true) {
        // wait for PC input
        //serialLock.lock ();
        Thread::wait(233); // Avoid conflictions between two
		serialLock.lock();
        pc.printf ("\n\rPlease input a float number : ");
		serialLock.unlock();
        ret = pc.scanf("%f", &tmpD);
        serialLock.lock ();
        pc.printf("\n\r");
        serialLock.unlock ();
        if (ret == 1) {
            serialLock.lock ();
            pc.printf("scanf() succeeded!!!\n\r");
            serialLock.unlock ();
            MessageT* messagePtr = messageQueue.alloc();
            messagePtr->frequency = tmpD;
            messageQueue.put(messagePtr);
        }
    }
}
 
void producerMode2 () {
	// get data from PID calculations
	// to be completed
}
 
void consumer () {
    // get pulse frequency input from producer loop
    // refresh the output frequency
    switch (consumerMode == 1){
        case 1:
            consumerMode1 ();
            break;
        case 2:
            consumerMode2 ();
            break;
        default:
            consumerMode1 ();
            break;
    }
}
 
void consumerMode1 () {
    // print the data read
    Serial pc(SERIAL_TX, SERIAL_RX);
    serialLock.lock();
    pc.printf("Hello from the CONSUMER loop!\n\r");
    serialLock.unlock ();
    while (true) {
        osEvent event = messageQueue.get ();
        if (event.status == osEventMail) {
            MessageT* messagePtr = (MessageT*) event.value.p;
            if  (serialLock.trylock ()) {
                pc.printf ("%.6f\n\r", messagePtr->frequency);
                serialLock.unlock ();
            }
            messageQueue.free (messagePtr);
        }
        else {
            serialLock.lock ();
            pc.printf("NOT A FLOAT NUMBER!\r\n");
            serialLock.unlock ();
        }
    }
}
 
void consumerMode2 () {
    // implement data to motor
	// to be completed
}

程序运行示例:

:!:注意!使用Arduino IDE测试本程序时,需要将结束符设为“NL和CR”,在每个数字输入后添加\n\r,以满足scanf()函数的读取格式要求。

程序成功执行,数据处理架构搭建成功!以前用多个Arduino单片机实现的功能,仅需一块STM32即可实现。

参考资料

  1. Katsuhiko Ogata. 现代控制工程(第五版). 北京:电子工业出版社. 2011.8
  2. Gene F. Franklin, J. David Powell, Abbas Emani-Naeini. 自动控制原理与设计(第六版). 北京:电子工业出版社. 2014.7
  3. 刘金琨. 先进PID控制MATLAB仿真(第4版). 北京:电子工业出版社. 2016.6

讨论区

1) * ATTENTION! 在\LaTeX中,正比例符号为\propto,还没有找到dokuwiki上的对应符号。 *
 
course/modern2/balance_control/start.txt · 最后更改: 2016/12/22 19:05 由 jeepee
 
除额外注明的地方外,本维基上的内容按下列许可协议发布:CC Attribution-Noncommercial-Share Alike 3.0 Unported
Recent changes RSS feed Donate Powered by PHP Valid XHTML 1.0 Valid CSS Driven by DokuWiki