arduino 的研究筆記: 平衡小車

Ref : https://world.taobao.com/item/45347924687.htm?fromSite=main&spm=a1z10.5-c.w4002-9258381263.27.iKfjZu

針對普通的用戶

MiniBalanceV3.5 平衡小车源码（DMP 版）
MiniBalanceV3.5 平衡小车源码（互补滤波版）
MiniBalanceV3.5 平衡小车源码（卡尔曼滤波版）

MiniBalanceV3.5 平衡小车源码（精简入门版）

MiniBalanceV3.5 平衡小车源码（顶配版含无线模块和线性CCD）

另外还有【1.5KG 超强负载版】这个代码中PID 参数比较大，可以适应更重的负
载，一般400g~1500g 都可以让小车平衡，比如让小车背负一片矿泉水。小车载着一瓶
矿泉水之后，重心升高了，重量也增加了，转动惯量和惯性都增大了。根据我们
调试平衡小车的经验，小车转动惯量越大，需要的阻尼力越大，所以需要增大微
分控制系数。小车越重，需要的回复力就越大，所以需要增大比例控制系数。
【1.5KG 超强负载版】相比其他的代码，其实就是PID 参数增大了！所以，这说
明了不同的负载情况下，小车应该使用不同的PID 参数。

ADC 模块：采集电阻分压后的电池电压,采集模拟CCD 摄像头数据
TIM1：初始化为PWM 输出，CH1，CH4 输出双路10KHZ 的PWM 控制电机

TIM2：初始化为正交编码器模式，硬件采集编码器1 数据
TIM3：CH3 初始化为超声波的回波采集接口。
TIM4：初始化为正交编码器模式，硬件采集编码器2 数据
USART1：通过串口1 把数据发到串口调试助手
USART3：通过串口3 接收蓝牙遥控的数据，接收方式为中断接收。并发送数据给app。
IIC：利用IO 模拟IIC 去读取MPU6050 的数据，原理图上MPU6050 链接的是STM32 的硬
件IIC 接口，但是因为STM32 硬件IIC 不稳定，所以默认使用模拟IIC，大家可以自行拓展。
SPI：利用IO 模拟SPI 去驱动OLED 显示屏,硬件SPI 驱动NRF24L01
GPIO：读取按键输入，控制LED，控制电机使能和正反转
SWD：提供用于在线调试的SWD 接口
EXTI:由MPU6050 的INT 引脚每5ms 触发一次中断，作为控制的时间基准

程序主要用户文件说明如下：
 Source Group1
 Startup_stm32f10x_md.s :stm32 的启动文件
 User
 Minibalance.c：放置主函数，并把超声波读取和人机交互等工作放在死循环里面。
 SYSTEM
 Delay.c：提供系统延时初始化函数及相关的函数
 Sys.c:提供时钟、中断、系统初始化函数
 Usart1.c：提供串口1 初始化函数及相关的函数
 HARDWARE
 Led.c：提供LED 初始化函数及相关的函数
 Key.c：提供按键初始化函数及相关的函数，如单击、双击、长按检测。
 Oled.c：提供OLED 初始化函数及相关的函数
 adc.c：提供ADC 初始化函数及相关的函数，如电池电压检测,线性CCD 摄像头初始化
和驱动程序。
 Timer.c：提供超声波的初始化代码和采集代码。
 Motor.c 提供电机控制初始化函数
 Encoder.c 提供编码器采集相关函数
 Ioi2c.c：提供模拟IIC 初始化函数及相关的函数
 Usart3.c：提供串口3 初始化函数及相关的函数，其中蓝牙遥控使用的串口3 接收中断
函数在这里面。
 Exti.c：提供外部中断初始化代码。
 Balance
 Control.c 提供全部的控制函数，并放在由MPU6050 触发的外部中断里面执行
 Inv_mpu.c：MPU6050 内置DMP 的相关库文件
 Inv_mpu_dmp_motion_driver.c：MPU6050 内置DMP 的相关库文件
 Fitler.c：提供平衡小车常用的滤波算法，如卡尔曼滤波，互补滤波
 Mpu6050.c：提供MPU6050 初始化函数及相关的函数
 Show.c：提供用于数显和APP 使用的相关函数
5.滤波算法
平衡小车获取姿态角的滤波算法一般为卡尔曼滤波和一阶互补滤波。但是MPU6050 内
置的DMP 可以直接输出和姿态相关的四元数。所以，常用的有三种方法可以获取角度。
本程序内置了多种滤波算法，获取四元数的算法放在DMP 相关的库文件里面，卡尔曼
滤波和互补滤波放在filter.c 里面。除了精简版代码外，其他每个代码都提供了Way_Angle
变量控制滤波算法。
Way_Angle=1：通过DMP 获取四元数，并算出角度
Way_Angle=2：通过卡尔曼滤波对陀螺仪和重加进行数据融合
Way_Angle=3：通过互补滤波对陀螺仪和重加进行数据融合
根据长时间的实践，以DMP 输出的四元数表示的角度和卡尔曼滤波最为稳定。互补滤
波的效果稍差，但也是很不错的。
6.控制算法
 直立控制：PD 控制，这是最核心的控制，其他的控制都是相对直立控制而言都是干扰。
 速度控制：PI 控制对编码器信息进行低通滤波可以削弱电机控制的比重，提高系统稳
定性。
 转向控制：PD 控制结合了Z 轴陀螺仪PD 控制;

程式碼的研究
=================================================================

/**************************************************************************
函数功能：简易卡尔曼滤波
入口参数：加速度、角速度
返回值：无
**************************************************************************/

float K1 =0.02;
float angle, angle_dot;
float Q_angle=0.001;// 过程噪声的协方差
float Q_gyro=0.003;//0.003 过程噪声的协方差过程噪声的协方差为一个一行两列矩阵
float R_angle=0.5;// 测量噪声的协方差既测量偏差
float dt=0.005;//
char C_0 = 1;
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };

void Kalman_Filter(float Accel,float Gyro)
{
angle+=(Gyro - Q_bias) * dt; //先验估计
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分

Pdot[1]=-PP[1][1];
Pdot[2]=-PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;

Angle_err = Accel - angle; //zk-先验估计

PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];

E = R_angle + C_0 * PCt_0;

K_0 = PCt_0 / E;
K_1 = PCt_1 / E;

t_0 = PCt_0;
t_1 = C_0 * PP[0][1];

PP[0][0] -= K_0 * t_0; //后验估计误差协方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;

angle += K_0 * Angle_err; //后验估计
Q_bias += K_1 * Angle_err; //后验估计
angle_dot = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
}

這個式另外一版的

void Kalman_Filter(double angle_m,double gyro_m)
{
angle+=(gyro_m-q_bias) * dtt;
Pdot[0]=Q_angle - P[0][1] - P[1][0];
Pdot[1]=- P[1][1];
Pdot[2]=- P[1][1];
Pdot[3]=Q_gyro;
P[0][0] += Pdot[0] * dtt;
P[0][1] += Pdot[1] * dtt;
P[1][0] += Pdot[2] * dtt;
P[1][1] += Pdot[3] * dtt;
angle_err = angle_m - angle;
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * P[0][1];
P[0][0] -= K_0 * t_0;
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;
angle+= K_0 * angle_err;
q_bias += K_1 * angle_err;
angle_dot = gyro_m-q_bias;//也许应该用last_angle-angle

// Serial.print(" filter: ");
// Serial.print(angle_dot);
}

看起來一模一樣...應該都是抄來的吧...我猜...要不然就是作者是同一個人....

/**************************************************************************
函数功能：一阶互补滤波
入口参数：加速度、角速度
返回值：无
**************************************************************************/

float K1 =0.02;

void Yijielvbo(float angle_m, float gyro_m)
{
angle = K1 * angle_m+ (1-K1) * (angle + gyro_m * 0.005);
}

另外一版的

   float dt = (now - preTime) / 1000.0;
float K = 0.8;
float A = K / (K + dt);

f_angle = A * (f_angle + omega * dt) + (1-A) * angleA; // 互补滤波算法

/**************************************************************************
函数功能：直立PD控制
入口参数：角度、角速度
返回值：直立控制PWM
作者：平衡小车之家
**************************************************************************/
int balance(float Angle,float Gyro)
{
float Bias,kp=100,kd=0.40;
int balance;
Bias=Angle-ZHONGZHI; //===求出平衡的角度中值和机械相关
balance=kp*Bias+Gyro*kd; //===计算平衡控制的电机PWM PD控制 kp是P系数 kd是D系数
return balance;
}

/**************************************************************************
函数功能：获取角度三种算法经过我们的调校，都非常理想
入口参数：获取角度的算法 1：DMP 2：卡尔曼 3：互补滤波
返回值：无
**************************************************************************/
void Get_Angle(u8 way)
{
float Accel_Y,Accel_X,Accel_Z,Gyro_Y,Gyro_Z;
Temperature=Read_Temperature();
//===读取MPU6050内置温度传感器数据，近似表示主板温度。
if(way==1) //===DMP的读取在数据采集中断提醒的时候，严格遵循时序要求
   {
Read_DMP(); //===读取加速度、角速度、倾角
Angle_Balance=Pitch; //===更新平衡倾角
Gyro_Balance=gyro[1]; //===更新平衡角速度
Gyro_Turn=gyro[2]; //===更新转向角速度
   Acceleration_Z=accel[2]; //===更新Z轴加速度计
}
else
{
Gyro_Y=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_L); //读取Y轴陀螺仪
Gyro_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_L); //读取Z轴陀螺仪
Accel_X=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_L); //读取X轴加速度计
Accel_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_L); //读取Z轴加速度计
if(Gyro_Y>32768) Gyro_Y-=65536; //数据类型转换也可通过short强制类型转换
if(Gyro_Z>32768)
Gyro_Z-=65536; //数据类型转换
if(Accel_X>32768)
Accel_X-=65536; //数据类型转换
if(Accel_Z>32768)
Accel_Z-=65536; //数据类型转换
Gyro_Balance=-Gyro_Y; //更新平衡角速度

Accel_Y=atan2(Accel_X,Accel_Z)*180/PI; //计算倾角
Gyro_Y=Gyro_Y/16.4; //陀螺仪量程转换

if(Way_Angle==2)
Kalman_Filter(Accel_Y,-Gyro_Y);//卡尔曼滤波
else if(Way_Angle==3)
Yijielvbo(Accel_Y,-Gyro_Y); //互补滤波

Angle_Balance=angle; //更新平衡倾角
Gyro_Turn=Gyro_Z; //更新转向角速度
Acceleration_Z=Accel_Z; //===更新Z轴加速度计
}
}

/**************************************************************************
函数功能：直立PD控制
入口参数：角度、角速度
返回值：直立控制PWM
作者：平衡小车之家
**************************************************************************/
int balance(float Angle,float Gyro)
{
float Bias,kp=100,kd=0.40;
int balance;
Bias=Angle-ZHONGZHI; //===求出平衡的角度中值和机械相关
balance=kp*Bias+Gyro*kd; //===计算平衡控制的电机PWM PD控制 kp是P系数 kd是D系数
return balance;
}

/**************************************************************************
函数功能：速度PI控制修改前进后退遥控速度，请修Target_Velocity，比如，改成60就比较慢了
入口参数：左轮编码器、右轮编码器
返回值：速度控制PWM
作者：平衡小车之家
**************************************************************************/
int velocity(int encoder_left,int encoder_right)
{
static float Velocity,Encoder_Least,Encoder,Movement;
static float Encoder_Integral,Target_Velocity=130;
float kp=50,ki=kp/200;
//=============遥控前进后退部分=======================//
if(1==Flag_Qian)
Movement=-Target_Velocity; //===前进标志位置1
else if(1==Flag_Hou)
   Movement=Target_Velocity; //===后退标志位置1
else
Movement=0;
if(Bi_zhang==1&&Distance<500&&Flag_Left!=1&&Flag_Right!=1) //避障标志位置1且非遥控转弯的时候，进入避障模式
Movement=Target_Velocity;
//=============速度PI控制器=======================//
Encoder_Least =(Encoder_Left+Encoder_Right)-0; //===获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零）
Encoder *= 0.7; //===一阶低通滤波器
Encoder += Encoder_Least*0.3; //===一阶低通滤波器
Encoder_Integral +=Encoder; //===积分出位移积分时间：10ms
Encoder_Integral=Encoder_Integral-Movement; //===接收遥控器数据，控制前进后退
if(Encoder_Integral>15000) Encoder_Integral=15000; //===积分限幅
if(Encoder_Integral<-15000) Encoder_Integral=-15000; //===积分限幅
Velocity=Encoder*kp+Encoder_Integral*ki; //===速度控制
if(Turn_Off(Angle_Balance,Voltage)==1||Flag_Stop==1) Encoder_Integral=0; //===电机关闭后清除积分
return Velocity;
}

==============================================================

int EXTI9_5_IRQHandler(void)
{
if(PBin(5)==0)
{
EXTI->PR=1<<5; //清除LINE5上的中断标志位
Flag_Target=!Flag_Target;
if(delay_flag==1)
{
if(++delay_50==10) delay_50=0,delay_flag=0; //给主函数提供精准延时
}
if(Flag_Target==1) //5ms读取一次陀螺仪和加速度计的值，更高的采样频率可以改善卡尔曼滤波和互补滤波的效果
{
Get_Angle(Way_Angle); //===更新姿态
return 0;
} //10ms控制一次，为了保证M法测速的时间基准，首先读取编码器数据
Encoder_Left=-Read_Encoder(2); //===读取编码器的值，因为两个电机的旋转了180度的，所以对其中一个取反，保证输出极性一致
Encoder_Right=Read_Encoder(4); //===读取编码器的值
Get_Angle(Way_Angle); //===更新姿态
Read_Distane(); //读取超声波测量得到的距离
if(Bi_zhang==0)
Led_Flash(100); //===LED闪烁;常规模式 1s改变一次指示灯的状态
if(Bi_zhang==1)
Led_Flash(0); //===LED闪烁;避障模式指示灯常亮
Voltage=Get_battery_volt(); //===获取电池电压
Key(); //===扫描按键状态单击双击可以改变小车运行状态
Balance_Pwm =balance(Angle_Balance,Gyro_Balance); //===平衡PID控制
Velocity_Pwm=velocity(Encoder_Left,Encoder_Right); //===速度环PID控制记住，速度反馈是正反馈，就是小车快的时候要慢下来就需要再跑快一点
Turn_Pwm =turn(Encoder_Left,Encoder_Right,Gyro_Turn); //===转向环PID控制
Moto1=Balance_Pwm+Velocity_Pwm-Turn_Pwm; //===计算左轮电机最终PWM
Moto2=Balance_Pwm+Velocity_Pwm+Turn_Pwm; //===计算右轮电机最终PWM
Xianfu_Pwm(); //===PWM限幅
if(Pick_Up(Acceleration_Z,Angle_Balance,Encoder_Left,Encoder_Right))//===检查是否小车被那起
Flag_Stop=1; //===如果被拿起就关闭电机
if(Put_Down(Angle_Balance,Encoder_Left,Encoder_Right)) //===检查是否小车被放下
Flag_Stop=0; //===如果被放下就启动电机
if(Turn_Off(Angle_Balance,Voltage)==0) //===如果不存在异常
Set_Pwm(Moto1,Moto2); //===赋值给PWM寄存器
}
return 0;
}

/**************************************************************************
函数功能：赋值给PWM寄存器
入口参数：左轮PWM、右轮PWM
返回值：无
**************************************************************************/
void Set_Pwm(int moto1,int moto2)
{
int siqu=400;
if(moto1<0)
  AIN2=1, AIN1=0;
else
AIN2=0, AIN1=1;

PWMA=myabs(moto1)+siqu;

if(moto2<0)
  BIN1=0, BIN2=1;
else
BIN1=1, BIN2=0;

PWMB=myabs(moto2)+siqu;
}