0
点赞
收藏
分享

微信扫一扫

平衡小车的控制算法(PID,LQR,MPC)及arduino程序导航贴


目录

​​平衡小车电机位置测试小实验​​

​​1.编码器脉冲计数​​

​​PID控制算法​​

​​平衡小车PID调参实验​​

​​位置环​​

​​2.编码器计数转换角度​​

​​小车整体的动力学建模​​

​​通过特征值判断系统动态特性​​

​​龙伯格观测器​​

平衡小车电机位置测试小实验

1.编码器脉冲计数

const byte LeftMotorInterruptP = 22;  //左电机编码器中断引脚
const byte LeftMotorCountP = 23; //左电机编码器计数引脚
const byte RightMotorInterruptP = 18; //右电机编码器中断引脚
const byte RightMotorCountP = 19; //右电机编码器计数引脚

const byte MotorDriverEn = 5; //电机驱动器使能
const byte LeftMotorP1 = 15; //左电机控制io口1
const byte LeftMotorP2 = 13; //左电机控制io口2
const byte RightMotorP1 = 16; //右电机控制io口1
const byte RightMotorP2 = 17; //右电机控制io口2

volatile long LeftMotorCounter = 0; //左电机中断计数位置
volatile long RightMotorCounter = 0; //右电机中断计数位置

portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED; //声明一个portMUX_TYPE类型的变量,利用其对主代码和中断之间的同步进行处理
portMUX_TYPE mux_1 = portMUX_INITIALIZER_UNLOCKED;

//左电机中断函数
void LeftMotorInterruptF() {
portENTER_CRITICAL_ISR(&mux_1);
delayMicroseconds(10); //延时20ms作为消抖,如果是很稳定的中断可以不加或者加很少的消抖时间
if(digitalRead(LeftMotorInterruptP) == LOW) //因为是上拉触发,所以在消抖时间完后读取引脚高低电平,如果还是为低那么就代表出现了一次稳定的中断
{
if(digitalRead(LeftMotorCountP)==LOW)
LeftMotorCounter++;
else
LeftMotorCounter--;
Serial.print("左边电机位置:");Serial.println(LeftMotorCounter);
}
portEXIT_CRITICAL_ISR(&mux_1);
}
//右电机中断函数
void RightMotorInterruptF() {
portENTER_CRITICAL_ISR(&mux);
delayMicroseconds(10); //延时2ms作为消抖,如果是很稳定的中断可以不加或者加很少的消抖时间
if(digitalRead(RightMotorInterruptP) == LOW) //因为是下拉触发,所以在消抖时间完后读取引脚高低电平,如果还是为低那么就代表出现了一次稳定的中断
{
if(digitalRead(RightMotorCountP)==LOW)
RightMotorCounter--;
else
RightMotorCounter++;
Serial.print("右边电机位置:");Serial.println(RightMotorCounter);
}
portEXIT_CRITICAL_ISR(&mux);
}


void setup(){
Serial.begin(115200);
Serial.println("中断测试实验");

//off motor enable
pinMode(MotorDriverEn,OUTPUT);
digitalWrite(MotorDriverEn,LOW);

pinMode(LeftMotorP1,OUTPUT);
pinMode(LeftMotorP2,OUTPUT);
pinMode(RightMotorP1,OUTPUT);
pinMode(RightMotorP2,OUTPUT);
digitalWrite(LeftMotorP1,LOW);
digitalWrite(LeftMotorP2,LOW);
digitalWrite(RightMotorP1,LOW);
digitalWrite(RightMotorP2,LOW);

pinMode(LeftMotorInterruptP, INPUT_PULLUP); //先把引脚设置为上拉输入模式
pinMode(RightMotorInterruptP, INPUT_PULLUP); //先把引脚设置为上拉输入模式
pinMode(LeftMotorCountP,INPUT);
pinMode(RightMotorCountP,INPUT);

attachInterrupt(digitalPinToInterrupt(LeftMotorInterruptP), LeftMotorInterruptF, FALLING);
attachInterrupt(digitalPinToInterrupt(RightMotorInterruptP), RightMotorInterruptF, FALLING);
}

void loop(){

}

PID控制算法

电机速度环和位置环请看这篇文章

2.编码器计数转换角度

const int CountperCircuit = 390;      //390个脉冲为一圈
const double pi = 3.141592653; //pi

const byte LeftMotorInterruptP = 22; //左电机编码器中断引脚
const byte LeftMotorCountP = 23; //左电机编码器计数引脚
const byte RightMotorInterruptP = 18; //右电机编码器中断引脚
const byte RightMotorCountP = 19; //右电机编码器计数引脚

const byte MotorDriverEn = 5; //电机驱动器使能
const byte LeftMotorP1 = 15; //左电机控制io口1
const byte LeftMotorP2 = 13; //左电机控制io口2
const byte RightMotorP1 = 16; //右电机控制io口1
const byte RightMotorP2 = 17; //右电机控制io口2

volatile long LeftMotorCounter = 0; //左电机中断计数位置
volatile long RightMotorCounter = 0; //右电机中断计数位置
volatile double LeftMotordeg = 0.0; //左电机中断计数位置
volatile double RightMotordeg = 0.0; //右电机中断计数位置

portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED; //声明一个portMUX_TYPE类型的变量,利用其对主代码和中断之间的同步进行处理
portMUX_TYPE mux_1 = portMUX_INITIALIZER_UNLOCKED;

//左电机中断函数
void LeftMotorInterruptF() {
portENTER_CRITICAL_ISR(&mux_1);
delayMicroseconds(10); //延时20ms作为消抖,如果是很稳定的中断可以不加或者加很少的消抖时间
if(digitalRead(LeftMotorInterruptP) == LOW) //因为是上拉触发,所以在消抖时间完后读取引脚高低电平,如果还是为低那么就代表出现了一次稳定的中断
{
if(digitalRead(LeftMotorCountP)==LOW)
LeftMotorCounter++;
else
LeftMotorCounter--;
LeftMotordeg = double(LeftMotorCounter)/double(CountperCircuit)*360.0;
Serial.print("左边电机位置:");Serial.print(LeftMotordeg);Serial.println("'C");
}
portEXIT_CRITICAL_ISR(&mux_1);
}
//右电机中断函数
void RightMotorInterruptF() {
portENTER_CRITICAL_ISR(&mux);
delayMicroseconds(10); //延时2ms作为消抖,如果是很稳定的中断可以不加或者加很少的消抖时间
if(digitalRead(RightMotorInterruptP) == LOW) //因为是下拉触发,所以在消抖时间完后读取引脚高低电平,如果还是为低那么就代表出现了一次稳定的中断
{
if(digitalRead(RightMotorCountP)==LOW)
RightMotorCounter--;
else
RightMotorCounter++;
RightMotordeg = double(RightMotorCounter)/double(CountperCircuit)*360.0;
Serial.print("右边电机位置:");Serial.print(RightMotordeg);Serial.println("'C");
}
portEXIT_CRITICAL_ISR(&mux);
}


void setup(){
Serial.begin(115200);
Serial.println("中断测试实验");

//off motor enable
pinMode(MotorDriverEn,OUTPUT);
digitalWrite(MotorDriverEn,LOW);

pinMode(LeftMotorP1,OUTPUT);
pinMode(LeftMotorP2,OUTPUT);
pinMode(RightMotorP1,OUTPUT);
pinMode(RightMotorP2,OUTPUT);
digitalWrite(LeftMotorP1,LOW);
digitalWrite(LeftMotorP2,LOW);
digitalWrite(RightMotorP1,LOW);
digitalWrite(RightMotorP2,LOW);

pinMode(LeftMotorInterruptP, INPUT_PULLUP); //先把引脚设置为上拉输入模式
pinMode(RightMotorInterruptP, INPUT_PULLUP); //先把引脚设置为上拉输入模式
pinMode(LeftMotorCountP,INPUT);
pinMode(RightMotorCountP,INPUT);

attachInterrupt(digitalPinToInterrupt(LeftMotorInterruptP), LeftMotorInterruptF, FALLING);
attachInterrupt(digitalPinToInterrupt(RightMotorInterruptP), RightMotorInterruptF, FALLING);
}

void loop(){

}

小车整体的动力学建模

平衡小车的控制算法(PID,LQR,MPC)及arduino程序导航贴_引脚

平衡小车的控制算法(PID,LQR,MPC)及arduino程序导航贴_平衡小车_02

这个模型不是很严谨,因为

平衡小车的控制算法(PID,LQR,MPC)及arduino程序导航贴_git_03

不总是比较小,而且我们的目的也不是仅仅为了保持小车平衡,在保持平衡的同时也要使得

平衡小车的控制算法(PID,LQR,MPC)及arduino程序导航贴_引脚_04

趋于0。不过暂时粗略先用这个动力学模型来做,后面再重新建立更加准确的模型进行优化。

经过测量,我的平衡小车

质量:m=0.748kg

质心高度约为40mm:L=0.04m

重力加速度用9.8:g=9.8m/s^2

那么动力学模型即为:

平衡小车的控制算法(PID,LQR,MPC)及arduino程序导航贴_引脚_05

通过特征值判断系统动态特性

 计算特征值

平衡小车的控制算法(PID,LQR,MPC)及arduino程序导航贴_git_06

可以看到,两个特征值并不都小于0.也就是说系统不稳定。

这跟我们的经验也一样,如果平衡小车没有输入的话,它将发生倾倒。

然后我们看一下能控性

也就是Co矩阵的秩,如果Co矩阵是满秩的话,就意味着这个系统是可控的。

平衡小车的控制算法(PID,LQR,MPC)及arduino程序导航贴_编码器_07

 可以看到Co矩阵是满秩的,也就意味着我们的系统是可控的。

然后我们就可以设计控制器进行控制了。

 我们设计一个线性控制器

平衡小车的控制算法(PID,LQR,MPC)及arduino程序导航贴_引脚_08

这时,我们的控制器就设计好了,输入u就如上图最后一行一样。

可是我们的z并不知道啊。我的平衡小车上面是有一个mpu6050模块,可以测得小车的倾角

平衡小车的控制算法(PID,LQR,MPC)及arduino程序导航贴_引脚_04

,也就是平衡小车的控制算法(PID,LQR,MPC)及arduino程序导航贴_平衡小车_10但是我们的

平衡小车的控制算法(PID,LQR,MPC)及arduino程序导航贴_编码器_11

 。现在我们可以用mpu6050这个传感平衡小车的控制算法(PID,LQR,MPC)及arduino程序导航贴_平衡小车_10,也就是倾角,但是倾角的角加速度我们没有传感器可以测得。也就是说我们现在无法知道z的准确值。

由于我们的控制器u是z的函数,因此我们要想办法得知z。那么接下来我们将使用一个观测器来得到平衡小车的控制算法(PID,LQR,MPC)及arduino程序导航贴_平衡小车_13

龙伯格观测器

推导

平衡小车的控制算法(PID,LQR,MPC)及arduino程序导航贴_git_14

求出L以后,我们得出了观测器的表达式。

平衡小车的控制算法(PID,LQR,MPC)及arduino程序导航贴_编码器_15

 其中平衡小车的控制算法(PID,LQR,MPC)及arduino程序导航贴_git_16为估计的初值,可以任意估计,因为随着时间增加,这个值会收敛到跟实际值几乎一样。

u为输入,这里我们的input为平衡小车的控制算法(PID,LQR,MPC)及arduino程序导航贴_编码器_17,也就是平衡小车轮子在水平方向的加速度。

y就是倾角

平衡小车的控制算法(PID,LQR,MPC)及arduino程序导航贴_引脚_04


那么我们的观测器可以写为具体的表达形式为:

平衡小车的控制算法(PID,LQR,MPC)及arduino程序导航贴_git_19

举报

相关推荐

0 条评论