平衡小车制作

只看该作者 · 2021-6-8 12:47

三、转向环
1.中文公式

转向环输出=系数×Z轴角速度
(Z轴角速度由陀螺仪MPU6050测得)

2.软件编程

转向环的编程比较简单，我们只需设置一个参数调节Z轴角速度即可。

/*****************
转向环：系数*Z轴角速度
******************/
int Turn(int gyro_Z)
{
  int PWM_out;

  PWM_out = (-0.6)*gyro_Z;

  return PWM_out;
}

只看该作者 · 2021-6-8 12:48

四、控制函数
1、采集编码器数据和MPU6050角度信息

编码器数据：左电机速度，右电机速度
（两个电机是相对安装，刚好相差180度，为了编码器输出极性一致，就需要对其中一个取反）
MPU6050数据：角度数据，角速度数据，角加速度数据
2、将数据压入闭环控制中，计算出控制输出量

直立环输出
速度环输出
转向环输出
3、把控制输出量加载到电机上，完成最终的控制

左电机输出（编码器放置相对）
右电机输出
限幅
赋值
4、控制中断函数
首先要判断是否接受到中断请求，即检测MPU6050的ANT引脚是否处在低电平（即为发生中断），然后清除中断标志位，进行接下来三步（即上面的三个步骤）。

void EXTI9_5_IRQHandler(void)
{
  int PWM_out;
  if(EXTI_GetITStatus(EXTI_Line5)!=0) // 一级判定
  {
if(PBin(5)==0) // 二级判断
{
   EXTI_ClearITPendingBit(EXTI_Line5); // 清除中断标志位
   // 1.采集编码器数据&MPU6050角度信息
   // 电机是相对安装，刚好相差180度，为了编码器输出极性一致，就需要对其中一个取反
   Encoder_Left  = -Read_Speed(2);
   Encoder_Right = Read_Speed(4);

   mpu_dmp_get_data(&Pitch,&Roll,&Yaw);       // 读取角度
   MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);  // 读取角速度
   MPU_Get_Accelerometer(&aacx,&aacy,&aacz); // 读取加速度
   // 2.将数据压入闭环控制中，计算出控制输出量
Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); // 速度环
   Vertical_out=Vertical(Velocity_out+Med_Angle,Roll,gyrox);    // 直立环
Turn_out=Turn(gyroz);

   PWM_out=Vertical_out;//最终输出

   // 3.把控制输出量加载到电机上，完成最终控制
   MOTO1 = PWM_out-Turn_out; // 左电机
   MOTO2 = PWM_out+Turn_out; // 右电机
   Limit(&MOTO1,&MOTO2);    // PWM限幅
   Load(MOTO1,MOTO2);       // 加载到电机上
}
  }
}

只看该作者 · 2021-6-8 12:49

五、整个控制函数源代码
1、control.c

#include "control.h"

float Med_Angle=0;    // 机械中值，能使得小车真正平衡住的角度
float Target_Speed=0;    // 期望速度。---二次开发接口，用于控制小车前进后退及其速度。
float
  Vertical_Kp=0,
  Vertical_Kd=0;    // 直立环Kp、Kd
float
  Velocity_Kp=0,    // 速度环Kp、Ki（正反馈）
  Velocity_Ki=0;
float
  Turn_Kp=0;

int Vertical_out,Velocity_out,Turn_out; // 直立环&速度环&转向环的输出变量

int Vertical(float Med,float Angle,float gyro_Y); // 函数声明
int Velocity(int Target,int encoder_left,int encoder_right);
int Turn(int gyro_Z);

void EXTI9_5_IRQHandler(void)
{
  int PWM_out;
  if(EXTI_GetITStatus(EXTI_Line5)!=0) // 一级判定
  {
if(PBin(5)==0) // 二级判断
{
   EXTI_ClearITPendingBit(EXTI_Line5); // 清除中断标志位
   // 1.采集编码器数据&MPU6050角度信息
   // 电机是相对安装，刚好相差180度，为了编码器输出极性一致，就需要对其中一个取反
   Encoder_Left  = -Read_Speed(2);
   Encoder_Right = Read_Speed(4);

   mpu_dmp_get_data(&Pitch,&Roll,&Yaw);       // 读取角度
   MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);  // 读取角速度
   MPU_Get_Accelerometer(&aacx,&aacy,&aacz); // 读取加速度
   // 2.将数据压入闭环控制中，计算出控制输出量
Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); // 速度环
   Vertical_out=Vertical(Velocity_out+Med_Angle,Roll,gyrox);    // 直立环
Turn_out=Turn(gyroz);

   PWM_out=Vertical_out;//最终输出

   // 3.把控制输出量加载到电机上，完成最终控制
   MOTO1 = PWM_out-Turn_out; // 左电机
   MOTO2 = PWM_out+Turn_out; // 右电机
   Limit(&MOTO1,&MOTO2);    // PWM限幅
   Load(MOTO1,MOTO2);       // 加载到电机上
}
  }
}

/*****************
直立环PD控制器：Kp*Ek+Kd*Ek_D

入口：Med:机械中值(期望角度)，Angle:真实角度，gyro_Y:真实角速度
出口：直立环输出
******************/
int Vertical(float Med,float Angle,float gyro_Y)
{
  int PWM_out;

  PWM_out = Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro_Y-0);

  return PWM_out;
}

/*****************
速度环PI控制器：Kp*Ek+Ki*Ek_S(Ek_S：偏差的积分)
******************/
int Velocity(int Target,int encoder_left,int encoder_right)
{
  // 定义成静态变量，保存在静态存储器，使得变量不丢掉
  static int PWM_out,Encoder_Err,Encoder_S,EnC_Err_Lowout,EnC_Err_Lowout_last;
  float a=0.7;

  // 1.计算速度偏差
  //舍去误差--我的理解：能够让速度为"0"的角度，就是机械中值。
  Encoder_Err = ((encoder_left+encoder_right)-Target);
  // 2.对速度偏差进行低通滤波
  // low_out = (1-a)*Ek+a*low_out_last
  EnC_Err_Lowout = (1-a)*Encoder_Err + a*EnC_Err_Lowout_last; // 使得波形更加平滑，滤除高频干扰，放置速度突变
  EnC_Err_Lowout_last = EnC_Err_Lowout; // 防止速度过大影响直立环的正常工作
  // 3.对速度偏差积分出位移
  Encoder_S+=EnC_Err_Lowout;
  // 4.积分限幅
  Encoder_S=Encoder_S>10000?10000:(Encoder_S<(-10000)?(-10000):Encoder_S);

  // 5.速度环控制输出
  PWM_out = Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;

  return PWM_out;
}

/*****************
转向环：系数*Z轴角速度
******************/
int Turn(int gyro_Z)
{
  int PWM_out;

  PWM_out = Turn_Kp*gyro_Z;

  return PWM_out;
}

只看该作者 · 2021-6-8 12:51

第七部分串级PID调参及平衡成果展示

一、调参步骤
确立机械中值
直立环（内环）——Kp极性、Kp大小、Kd极性、Kd大小
速度环（外环）——Kp&Ki极性、Kp&Ki大小
转向环——系数极性、系数大小
二、机械中值
把小车放在底面上，从前向后以及从后向前绕电机轴旋转平衡小车，两次向另一边倒下的角度的中值，即为机械中值。
举例：往后倒在2度，往后倒在-3度左右，这边我取的机械中值为-1度。

float Med_Angle=-1.0; // 机械中值，能使得小车真正平衡住的角度

图1 往后倒在2度左右（Y轴，即Roll）

图2 往前倒在-3度左右（Y轴，即Roll）

只看该作者 · 2021-6-8 12:52

三、直立环
1.Kp极性：

极性错误：小车往哪边倒，车轮就往反方向开，会使得小车加速倒下。
极性正确：小车往哪边倒，车轮就往哪边开，以保证小车有直立的趋势。

2.Kp大小：

Kp一直增加，直到出现大幅低频振荡。（即小车平衡时出现抖动）
注：Kp的绝对值范围大概在150至350内，当我设置为320时出现低频振荡，可以略微增大Kp，当出现大幅度低频振荡时，说明Kp已经足够大，是时候引入微分项。

3.Kd极性：

极性错误：拿起小车绕电机轴旋转，车轮反向转动，无跟随。
极性正确：拿起小车绕电机轴旋转，车轮同向转动，有跟随。

4.Kd大小：

Kd一直增加，直到出现高频振荡。（即触碰小车出现剧烈抖动）
注：Kd的绝对值范围大概在0.1至0.9内，当我设置为0.8时出现高频振荡，此时需及时关闭电机，以防电机及驱动烧毁。
直立环调试完毕后，对所有确立的参数乘以0.6作为最终参数。
原因：之前得到的参数都是Kp、Kd最大值，根据工程经验平衡小车的理想参数为最大参数乘以0.6求得。
结果：乘0.6后，小车抖动消失，但同时直立效果也变差。待下面加入速度环就能得到更好的性能。
我最终调整好的直立环Kp与Kd参数：

float
Vertical_Kp=200,
Vertical_Kd=0.5; // 直立环Kp、Kd

只看该作者 · 2021-6-8 12:53

四、速度环
1.需要注意的点

（1）
在调节【速度环参数极性】时，需要去掉【直立环运算】；
在调节【速度环参数大小】时，需要引入【直立环运算】。
（2）
【转向环运算】始终是去掉的一个状态，若转向环已提前将参数调好，未注释也影响不大。

2.Kp&Ki：

线性关系：Ki=(1/200)*Kp，仅调Kp即可。

3.Kp&Ki极性：（直立环注释）

极性错误：手动转动其中一个车轮，另一车轮会以同样速度反向旋转——典型负反馈。
极性正确：手动转动其中一个车轮，两个车轮会同向加速，直至电机最大速度——典型正反馈。

4.Kp&Ki大小：

增加Kp&Ki，直至：小车保持平衡的同时，速度接近于0，且回位效果好。
注：Kp的绝对值范围在0.1至0.9内。

我最终调整好的速度环Kp与Kd参数：

float
Velocity_Kp=0.30,
Velocity_Ki=0.0015; // 速度环Kp、Ki（正反馈）

只看该作者 · 2021-6-8 12:54

五、转向环
1.Kp极性：
极性错误：拿起小车，并将小车绕Z轴旋转，两车轮旋转的趋势与小车旋转趋势一致——典型正反馈。
极性正确：拿起小车，并将小车绕Z轴旋转，两车轮旋转的趋势与小车旋转趋势相反——典型负反馈
2.Kp大小：
加大Kp，直至走直线效果较好，且无剧烈抖动。
注：Kp大概范围在0.1至0.9内。

我最终调整好的转向环Kp与Kd参数：

float
Turn_Kp=-0.6;

只看该作者 · 2021-6-8 12:55

第八部分蓝牙遥控及平衡成果展示

一、蓝牙初始化
1.串口3初始化函数——usart3.c
这一串代码很容易理解，就是通过串口3与蓝牙连接，手机连接蓝牙将信息发送给蓝牙模块，再传至STM32获取控制信息。

#include "usart3.h"

void uart3_init(u32 bound)
{
//GPIO端口设置
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); // 时钟GPIOB，USART3
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);

  //USART3_TX PB.10
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_Init(GPIOB, &GPIO_InitStructure);
//USART3_RX    PB.11
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOB, &GPIO_InitStructure);
//USART 初始化设置
USART_InitStructure.USART_BaudRate = bound;//一般设置为9600;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;

  USART_Init(USART3, &USART_InitStructure);
USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);//开启中断
USART_Cmd(USART3, ENABLE);                   //使能串口
}

/*
0x00:刹车
0x01:前进
0x02:后退
0x03:左转
0x07:右转
*/
u8 Fore,Back,Left,Right;
void USART3_IRQHandler(void)                   // 串口1中断服务程序
{
  int Bluetooth_data;
  if(USART_GetITStatus(USART3,USART_IT_RXNE)!=0)  // 接收中断标志位拉高
  {
Bluetooth_data=USART_ReceiveData(USART3);    // 保存接收到的指令
if(Bluetooth_data==0x00)Fore=0,Back=0,Left=0,Right=0; // 刹车
else if(Bluetooth_data==0x01)Fore=1,Back=0,Left=0,Right=0; // 前进
else if(Bluetooth_data==0x05)Fore=0,Back=1,Left=0,Right=0; // 后退
else if(Bluetooth_data==0x03)Fore=0,Back=0,Left=1,Right=0; // 左转
else if(Bluetooth_data==0x07)Fore=0,Back=0,Left=0,Right=1; // 右转
else                   Fore=0,Back=0,Left=0,Right=0;
  }
}

// 发送一个
void USART3_Send_Data(char data)
{
  USART_SendData(USART3,data);
  while(USART_GetFlagStatus(USART3,USART_FLAG_TC)==0);  // 除非发送完成
}

// 发送一串
void USART3_Send_String(char *String)
{
  u16 len,j;
  len=strlen(String);
  for(j=0;j<len;j++)
  {
USART3_Send_Data(*String++);
  }
}

只看该作者 · 2021-6-8 12:56

2.串口3头文件——usart3.h

#ifndef __USART_H
#define __USART_H
#include "sys.h"

void uart3_init(u32 bound);
void USART3_IRQHandler(void);
void USART_Send_Data(char data);
void USART_Send_String(char *String);
#endif

只看该作者 · 2021-6-8 12:56

二、转向环控制
转向环约束控制，引入RC，不是严格的PD控制器，Kd针对的是转向环的约束，但Kp针对的是遥控的转向。

float
  Turn_Kd=-0.6,
  Turn_Kp=-20;

/*****************
转向环：系数*Z轴角速度+系数*遥控数据
******************/
int Turn(int gyro_Z,int RC)
{
  int PWM_out;

  // 不是严格的PD控制器，Kd针对的是转向环的约束，但Kp针对的是遥控的转向
  PWM_out = Turn_Kd*gyro_Z+Turn_Kp*RC;

  return PWM_out;
}

只看该作者 · 2021-6-8 12:57

三、修改控制函数
1.二次开发接口引入

float Target_Speed=0;    // 期望速度。---二次开发接口，用于控制小车前进后退及其速度。
float Turn_Speed=0;    // 左右遥控数据

19行至37行是控制函数编写，这一块应该比较容易理解。当对应标志位置1时，进行对应的操作，同时还需进行限幅，防止超过PWM规定范围。

void EXTI9_5_IRQHandler(void)
{
  int PWM_out;
  if(EXTI_GetITStatus(EXTI_Line5)!=0) // 一级判定
  {
if(PBin(5)==0) // 二级判断
{
   EXTI_ClearITPendingBit(EXTI_Line5); // 清除中断标志位
   // 1.采集编码器数据&MPU6050角度信息
   // 电机是相对安装，刚好相差180度，为了编码器输出极性一致，就需要对其中一个取反
   Encoder_Left  = -Read_Speed(2);
   Encoder_Right = Read_Speed(4);

   mpu_dmp_get_data(&Pitch,&Roll,&Yaw);       // 读取角度
   MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);  // 读取角速度
   MPU_Get_Accelerometer(&aacx,&aacy,&aacz); // 读取加速度
   // 2.将数据压入闭环控制中，计算出控制输出量

   /*前后*/
   if((Fore==0)&&(Back==0))Target_Speed=0; // 未接收到前进后退指令->速度清零，稳在原地
   if(Fore==1)Target_Speed++;  // 前进标志位为1->需要前进
   if(Back==1)Target_Speed--;  // 后退标志位为1->需要后退
   Target_Speed=Target_Speed>SPEED_Y?SPEED_Y:(Target_Speed<-SPEED_Y?(-SPEED_Y):Target_Speed);  // 限幅

   /*左右*/
   if((Left==0)&&(Right==0))Turn_Speed=0;
   if(Left==1)Turn_Speed++; // 左转标志位为1->需要左转
   if(Right==1)Turn_Speed--;  // 右转标志位为1->需要右转
   Turn_Speed=Turn_Speed>SPEED_Z?SPEED_Z:(Turn_Speed<-SPEED_Z?(-SPEED_Z):Turn_Speed);       // 限幅

   /*转向约束*/
   if((Left==0)&&(Right==0))Turn_Kd=-0.6; // 若无左右转向指令，则开启转向约束
   else if((Left==1)||(Right==1))Turn_Kd=0;  // 若左右转向指令接收到，则去掉转向约束

   Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); // 速度环
   Vertical_out=Vertical(Velocity_out+Med_Angle,Roll,gyrox);    // 直立环
   Turn_out=Turn(gyroz,Turn_Speed);

   PWM_out=Vertical_out;//最终输出

   // 3.把控制输出量加载到电机上，完成最终控制
   MOTO1 = PWM_out-Turn_out; // 左电机
   MOTO2 = PWM_out+Turn_out; // 右电机
   Limit(&MOTO1,&MOTO2);    // PWM限幅
   Load(MOTO1,MOTO2);       // 加载到电机上
}
  }
}

只看该作者 · 2021-6-8 12:58

四、主函数
在主函数中初始化串口3函数。

#include "stm32f10x.h"
#include "sys.h"

int PWM_MAX=7200,PWM_MIN=-7200; // PWM限幅变量
int MOTO1,MOTO2;

float Pitch,Roll,Yaw;             // Pitch：俯仰角，Roll：横滚角，Yaw：偏航角
short gyrox,gyroy,gyroz;       // 角速度
short aacx,aacy,aacz;          // 加速度
int Encoder_Left,Encoder_Right; // 编码器数据（速度）

int main(void)
{
delay_init();
NVIC_Config();
uart1_init(115200);
uart3_init(9600);
    USART3_Send_String("AT+NAME hc_05_sxwl \r\n");
OLED_Init();
OLED_Clear();

MPU_Init();
mpu_dmp_init();
MPU6050_EXTI_Init();

Encoder_TIM2_Init();
Encoder_TIM4_Init();
Motor_Init();
PWM_Init_TIM1(0,7199);
    while(1)
{
OLED_Float(0,0,Roll,3);
}
}

只看该作者 · 2021-9-4 21:50

太精彩了，楼主厉害。

只看该作者 · 2021-9-5 11:45

一直对平衡车很感兴趣，来看看制作原理把

平衡小车制作

技术新星奖章