力学系统的平衡控制

指导教师：乐永康
课题参与者：刘知平吴雪君

开环控制与闭环控制

PID控制系统

一个例子（from陶鑫）：自平衡小车

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

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}$ 。

搭建了电子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 电子学PID控制器电路图

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

$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 \propto K_d \propto C_1$ ,可以按照 T = k C_1 + b 对其拟合。

See the comment. ¹⁾

图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 其他条件不变时，信号振荡周期随电容 $C_{1}$ 的变化关系图
拟合结果: $delim{lbrace}{ matrix{3}{1}{ { k = 0.6695 } { b = 58.93 } { R^2 = 0.9772 } } }{}$

电路部分的连接

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

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

STM32单片机的编程学习

利用“mbed.h”头文件，可以使用已定义好的C/C变量编写程序。通过[[https://developer.mbed.org 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

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.

机械部分的制作

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

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即可实现。

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

¹⁾

* ATTENTION! 在\LaTeX中，正比例符号为\propto，还没有找到dokuwiki上的对应符号。 *

力学系统的平衡控制

基本原理

信号与系统

自动控制理论

开环控制与闭环控制

PID控制系统

实验仪器

STM32 单片机

MPU-6050 六轴传感器

实验设置

实验进展

09192016 - 电子学PID模拟实验

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

电路部分的连接

STM32单片机的编程学习

MPU-6050的特性与数据读取

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

MPU6050传感器的响应

深入学习STM32的特性

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

机械部分的制作

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

参考资料

讨论区

Fudan Physics Teaching Lab