平衡车初步完成~~https://v.youku.com/v_show/id_XODgyOTI4OTU2.html


硬件资料：

控制板：         NucleoF072RB
传感器：         MPU6050
电源：             聚合物锂电池12V 4000mAh
电机：             直流减速电机+编码器
电机驱动：      TB6612
DC-DC降压：  LM2596S 、AMS1117-5
电机参数.pdf (15.67 KB, 下载次数: 218)

2015-1-30 19:30 上传

点击文件名下载附件
TB6612FNG简介及其与单片机的直流电机控制系统设计.pdf (347.08 KB, 下载次数: 296)

2015-1-30 19:30 上传

点击文件名下载附件

两轮自平衡控制系统是一种两轮左右平行布置的,像传统的倒立摆一样,本身是一个自然不稳定体,必须施加强有力的控制手段才能使之稳定。
平衡车工作原理是系统以姿态传感器(陀螺仪、加速度计)来监测车身所处的俯仰状态和状态变化率,通过高速微控制器计算出适当数据和指令后,驱动电动机产生前进或后退的加速度来达到车体前后平衡的效果。假设我们以车上重心纵轴作为参考线。当这条轴往前倾斜时，车身内的内置电动马达会产生往前的力量，一方面平衡车往前倾倒的扭矩，一方面产生让车辆前进的加速度，相反的，当陀螺仪发现重心往后倾时，也会产生向后的力量达到平衡效果。

控制器包括车体状态运算和平衡控制运算2个部分。
       车体状态运算主要是将传感器测量的数据加以融合得出车体倾斜角度值、倾斜角速度值以及行车速度等。

       平衡控制运算根据车体状态数据，计算保持平衡需要的行车速度和加速度，或者转弯所需要的左右电机速度变化值，向电机控制驱动模块发送控制指令。

       运算模块相当于两轮自平衡电动车的大脑，它主要负责的工作是：控制电机的起停，向控制模块发出加速、减速、电机正反转和制动等速度控制信号，接收电机光电信号进行车速度计算。

       加速度传感器能过输出以其芯片为中心的三轴加速度，通过这三个轴的重力加速度便可以计算出芯片的倾角，即车体的倾角。该方案的优点是重力加速度的静态性能很好，在车体静态下能测出准确稳定的倾角，而在动态下，三轴加速度各轴会受到其它加速度的影响，导致其数据并不稳定可靠。

       陀螺仪传感器能输出围绕以芯片为中心的三个轴的角速度，通过读角速度的积分，即可得出倾角。优点是陀螺仪的动态性能很好，在动态下测出的角速度没有太多的混杂成分，缺点是陀螺仪具有静态漂移，即静态下，陀螺仪仍然会输出数值，而积分却一直在进行，因此静态时，测出来的角度并不是0°。

       加速度传感器与陀螺仪传感器结合，通过融合算法，提取出加速度传感器的静态效果和陀螺仪的动态效果。优点是能测出准确稳定的倾角，但融合算法比较复杂。

芯片选择：

芯片：MPU-6050供电电源：3-5v（内部低压差稳压）
通信方式：标准IIC通信协议
芯片内置16bit AD转换器,16位数据输出
陀螺仪范围：±250   500    1000    2000  °/s
加速度范围：±2   ±4   ±8   ±16g   
    直接读取MPU6050的原始数据，可以得到xyz3轴加速度和3轴角加速度的值。
两种方案，一是自己对数据处理，把加速度计和角加速度的值进行互补滤波，然后计算倾角。
               二是使用芯片内部的DMP，获取融合好的四元数，然后转换成欧拉角。

六轴MPU6050官方DMP移植到F072：详见帖子尾部链接。



********************P   I   D******************************

PID控制器的输入输出关系：

其中e(t)是实际值与设定值的偏差。
比如这里我们假设车子直立的角度为0（设定值），实际车子向前倾斜了10°（实际值），偏差就是10°
Kp Ki Kd 是系数。



看起来容易吧，实际上参数的整定是很麻烦的过程，要有耐心。

*************************************************************


********************调试******意外******************************
活动买的stm32f072 nucleo板，本来做平衡车的的，调试电机时，给接外部5v电源，错接到PD2脚，然后不幸听到了微弱的嗞嗞声，板子就挂掉了，现在一通电就开始发热发烫，但是程序还能正常跑。。。看来今天晚上可以休息休息洗洗睡了接上USB，stlink指示灯红灯闪烁，说明有电压，没信号。stlink和板子电源不通。
JP5跳帽换到E5V，外部5V供电，U4输出2V左右电压。
给板子接3.3V电源，主芯片发烫，程序能正常跑。。。
多谢版主重新发了一块。。


我的电源分析：
https://www.stmcu.org.cn/module/forum/thread-599203-1-1.html
下面说说调试中遇到的问题和心得吧，。。
*********************************************************************
SWDIO被占用的问题
PA13 和 PA14好像是，如果被 配置成GPIO口，就无法连接下载了，
     解决方法:
   1.Flash Loader Demonstrator串口擦除程序
   2.连上电脑有U盘符，直接用mbed生成的bin文件下载正常配置的程序覆盖。

HAL库的使用
不太习惯用HAL库，简单的串口通信做的好复杂。。。中断、定时器看了好久。
不过使用STM32CubeMX对工程进行初始化配置很方便，层次很清楚，
阅读固件库对我们这种新手养成良好的编程习惯帮助很大。
中断用回调函数实现、串口通信数据接收要建立缓冲区，有必要最好封装成数据帧。
******************************************************************************
希望大家爱护板子。。不要像我一样。。。
让我去好好反省一会儿。。。
******************************************************************************
要抓紧时间了，一月都过去一半了！！
给自己写个调试进程：
2015.01.15
调试完成减速直流电机的驱动
模拟IIC对着示波器调了好久，终于OK了。用的内部上拉电阻，阻值太大，上升沿不太好。终于读到取MPU6050 寄存器的3轴加速度和3轴角速度还有温度了。

**************************************************************************
2015.01.23
放假了回家好好调小车了！！一个礼拜不知道来不来得及~
早上调好了PWM，用的STM32cubeMX自动配置，真是傻瓜操作了= =
下午继续做，晚上回来汇报。
**************************************************************************
整合了一下以前的程序，用mpu6050原始数据的变化，通过TIM3的PWM控制电机通过。
关于6050的教程：第一个是野火的，讲解怎么用，第二个就是原始数据的含义以及一些处理方法。
MPU6050教程.pdf (1015.02 KB, 下载次数: 1455)

2015-1-25 22:56 上传

点击文件名下载附件
MPU6050原始数据分析.pdf (1.93 MB, 下载次数: 1161)

2015-1-25 22:56 上传

点击文件名下载附件
**************************************************************************
2015.01.28
原始数据处理确实麻烦，互补滤波出来数据漂漂漂。。还是移植官方的DMP吧。
**************************************************************************
2015.01.29
折腾了3天终于把DMP移植好了！本来觉得好累，不过看到出来的Pitch 和Roll 那么稳定，哇咔咔！！
FIFO溢出问题没解决，不过被我机智地注释掉了~~

**************************************************************************
2015.01.30
月的最后两天了，平衡车已经能站起来了，暂时只加上了简单的PID参数慢慢调整吧，现在看起来效果还不错的。推都不倒~

***********************************************************************2015.02.02
角度闭环搞定了，下个目标：速度环。后期考虑加上无线通信模块，载人什么的电机肯定不行了，要不装个机械臂。。
***********************************************************************
先传了个短的10秒视频：
https://v.youku.com/v_show/id_XODgyOTI4OTU2.html


2015.02.08
前几天不在家，下午刚回来，编码器调试好了，开始正式调节PID的速度环。

简单说一下这里我对PID的理解，如有错误的地方请大家指出来哈！
现在的被控量，也就是这里电机上电压的                   占空比=Kp*角度+Kd*角加速度+K(speed)*速度+Ki(x)*位移

之前已经通过角度（比例P）和角加速度（微分D），让小车能够在0°左右平衡站立不倒了，现在要通过安装在电机上的光栅编码器，读出转过的角度，转化成小车的速度。我的编码器是AB相的，相位相差90°，AB相先后表示正转和反转。

PD调节小车站立，可是如果用手去推，小车就会向一边加速倒去，这时原先的Kp和Kd参数产生的调节力度就不足以使它保持平衡。现在获得了车的速度，那么就可以在控制方程里加上K(speed),同样做为一个比例系数，反馈到处理器，来增大调节力度。进一步的，我们把速度进行积分，就可以得到车的位移，从而又能产生一个系数Ki(x)，让小车位置保持在原来的位置。

这样两个控制环：角度环和速度环，其中包括了两个比例环节和一个微分环节和一个积分环节，最终就可以实现平衡车的基本功能了。
前进、后退、转弯、旋转等功能的实现也依赖于此，比如说前进的实现，只要在平衡的基础上，适当增加PWM的输出就可以了。

说起来比较简单，调试起来就很麻烦了，先从比例环节开始调，让系统开始振荡，然后调节积分环节。。。有时间上位机写个画图的软件，PID调节就方便多了。
一直调到电池没有电了，本来准备通宵吧参数都做完了，现在可以洗洗睡了。。。等这两天参数调好了就让平衡车跑起来！！

***********************************************************************

***********************************************************************
六轴MPU6050官方DMP移植到F072：
https://www.stmcu.org.cn/module/forum/thread-599730-1-1.html

***********************************************************************

DMP读取四元数，转化成欧拉角~~

Nocleo_soft_i2c_test1 - 移植失败1失败2失败3成功4.zip (4.07 MB, 下载次数: 352)

2015-1-29 16:17 上传

点击文件名下载附件

烧多了经验就来了，哈哈

我用printf("%0.2f    %0.2f    %0.2f\r\n",Angle,Angle_ax,Gyro_y);函数分别读取的加速度，角速度和倾角，我发现当我快速的改变板子的倾角的时候，比如快速变化10度，Angle（卡尔曼滤波后的倾角）瞬时变化非常快，可能会瞬间变化几十度然后回到正常角度，而当我缓慢变化10度的时候，Angle变化是正常线性变化到10度，在这两种变化中，Angle_ax（从MPU6050读取的值经过处理后的陀螺仪的Y轴数据）的变化一直都是线性正常的，并且Angle的值特别接近Angle_ax的值
问题：1，我快速改变板子倾角时Angle的变化正常吗？
      2，Angle正常变化的时候是应该与Angle_ax的值相近吗？

现在情况就是，就算我是在减小倾角，只要我快速地改变，它显示的倾角都会先增大再减小，而如果我慢速改变的话，倾角就会缓慢减小而不会出现中间的角度增大


*************读取数据********************
//定义MPU6050内部地址
#define        SMPLRT_DIV                0x19        //陀螺仪采样率 典型值 0X07 125Hz
#define        CONFIG                          0x1A        //低通滤波频率 典型值 0x00
#define        GYRO_CONFIG                0x1B        //陀螺仪自检及测量范围                 典型值 0x18 不自检 2000deg/s
#define        ACCEL_CONFIG        0x1C        //加速度计自检及测量范围及高通滤波频率 典型值 0x01 不自检 2G 5Hz
#define INT_PIN_CFG     0x37
#define INT_ENABLE      0x38
#define INT_STATUS      0x3A    //只读
#define        ACCEL_XOUT_H        0x3B
#define        ACCEL_XOUT_L        0x3C
#define        ACCEL_YOUT_H        0x3D
#define        ACCEL_YOUT_L        0x3E
#define        ACCEL_ZOUT_H        0x3F
#define        ACCEL_ZOUT_L        0x40
#define        TEMP_OUT_H                0x41
#define        TEMP_OUT_L                0x42
#define        GYRO_XOUT_H    0x43
#define        GYRO_XOUT_L                0x44       
#define        GYRO_YOUT_H        0x45
#define        GYRO_YOUT_L                0x46
#define        GYRO_ZOUT_H        0x47
#define        GYRO_ZOUT_L                0x48

//读取寄存器原生数据
           
        MPU6050_Raw_Data.Accel_X = (buf[0]<<8 | buf[1]);
        MPU6050_Raw_Data.Accel_Y = (buf[2]<<8 | buf[3]);
        MPU6050_Raw_Data.Accel_Z = (buf[4]<<8 | buf[5]);
        MPU6050_Raw_Data.Temp =    (buf[6]<<8 | buf[7]);  
        MPU6050_Raw_Data.Gyro_X = (buf[8]<<8 | buf[9]);
        MPU6050_Raw_Data.Gyro_Y = (buf[10]<<8 | buf[11]);
        MPU6050_Raw_Data.Gyro_Z = (buf[12]<<8 | buf[13]);
      
      
        //将原生数据转换为实际值，计算公式跟寄存器的配置有关
        MPU6050_Real_Data.Accel_X = -(float)(MPU6050_Raw_Data.Accel_X)/8192.0;
        MPU6050_Real_Data.Accel_Y = -(float)(MPU6050_Raw_Data.Accel_Y)/8192.0;
        MPU6050_Real_Data.Accel_Z = (float)(MPU6050_Raw_Data.Accel_Z)/8192.0;
              MPU6050_Real_Data.Gyro_X=-(float)(MPU6050_Raw_Data.Gyro_X - gyroADC_X_offset)/65.5;   
        MPU6050_Real_Data.Gyro_Y=-(float)(MPU6050_Raw_Data.Gyro_Y - gyroADC_Y_offset)/65.5;   
        MPU6050_Real_Data.Gyro_Z=(float)(MPU6050_Raw_Data.Gyro_Z - gyroADC_Z_offset)/65.5;   
    }
   


//******卡尔曼参数************
               
const float Q_angle=0.001;  
const float Q_gyro=0.003;
const float R_angle=0.5;
const float dt=0.01;                          //dt为kalman滤波器采样时间;
const 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;         //后验估计
        Gyro_y   = Gyro - Q_bias;         //输出值(后验估计)的微分=角速度

}

******************倾角计算*****************
void Angle_Calculate(void)
{

/****************************加速度****************************************/
       
        Accel_x  =  MPU6050_Real_Data.Accel_X;          //读取X轴加速度
        Angle_ax = Accel_x*1.2*180/3.14;     //弧度转换为度

/****************************角速度****************************************/
       
         Gyro_y = MPU6050_Real_Data.Gyro_Y;             
时间dt，所以此处不用积分
/***************************卡尔曼融合*************************************/
        Kalman_Filter(Angle_ax,Gyro_y);       //卡尔曼滤波计算倾角
       

简单 去淘宝或者代理商那 弄几篇072RBT6就解决了

harvardx 发表于 2015-1-10 17:18
简单 去淘宝或者代理商那 弄几篇072RBT6就解决了

主要是某觉得电源管理芯片也可能挂掉了。。

那个应该不怕, 主要是看看U4 那个要是坏掉了 焊接有麻烦.

先把电源部分检查下看有没问题，芯片未必有问题

770781327 发表于 2015-1-10 19:34
先把电源部分检查下看有没问题，芯片未必有问题

检查过了芯片肯定是坏了，电表测了下，GND 和VDD短路了。
电机和板子没有光耦隔离，就知道会这样。。。
芯片这边估计还买不到，淘宝10块钱一片，快递10块钱，伤心，，，
U4那块电源芯片很难焊，希望是好的吧，，，

平衡车是个好东西啊，又是个考算法的题目

xyc2690 发表于 2015-1-10 19:47
检查过了芯片肯定是坏了，电表测了下，GND 和VDD短路了。
电机和板子没有光耦隔离，就知道会这样。。。
...

恩，芯片智能买了，u4确实不大好焊，可以考虑这部分自己搭电路替换

GND和VCC短路不一定就说明芯片坏了 也有可能是电源保护了 仔细检测一下

z00 发表于 2015-1-10 20:37
GND和VCC短路不一定就说明芯片坏了 也有可能是电源保护了 仔细检测一下

哦？请教下，电源是怎么保护的啊？
板子的所有的3.3和地都通了，万用表测试过的。。上电就发烫，感觉是没有救了。。

从电源开始排查吧，运气好的话芯片不会坏。只要找到坏掉的器件。
以我烧芯片的心得，看到芯片冒青烟了，那才是真的坏，其他的情况大多是某些外围挂了。
一般都是迷迷糊糊的接错电源，就烧了：前面许多板子使用12V，后来忘记把稳压电源调到5V了。

短路的话，也可能是挂LDO啊、电路保护啊。走背字的才会挂的没救。

xyc2690 发表于 2015-1-10 20:48
哦？请教下，电源是怎么保护的啊？
板子的所有的3.3和地都通了，万用表测试过的。。上电就发烫，感觉是没 ...

先不要拆LDO 先把稳压管 TVS 等之类的外部元件拆除 再进行测试

Dylan疾风闪电 发表于 2015-1-10 21:24
从电源开始排查吧，运气好的话芯片不会坏。只要找到坏掉的器件。
以我烧芯片的心得，看到芯片冒青烟了，那 ...

谢啦！！我去慢慢查。。