网硕互联帮助中心
MPU6050；
俯仰角
翻滚角
偏航角
TB6612
PID
1.PID基础知识
设定值和反馈值相比较就是偏差
比例项就是一个系数乘以一个偏差
比例：提高反应速度，减小静差
积分：消除静态误差
微分项：减小震荡以及超调
直立环，让角度趋于0
速度环，让速度趋于0
积分项，一个ki，积分时间常数乘EK的积分，从0到k
微分，就是一个求导，本次的偏差减去上次的偏差再除以一个单位时间
PID有两种，一个是位置式，一个是增量式
加速度反应的是位置的变化率
滤波，测速算法
关于滤波算法
方案一： 卡尔曼滤波，可以在有大量干扰的情况下，从已知测量方差的存在噪声的数据中预测估算出系统的最优状态[3]，利用卡尔曼滤波可以从MPU6050的原始数据中得到一组接近真值、较为准确的智能车姿态数据。
方案二： 经典数字滤波器，主要包括低通、高通、带通滤波器，互补滤波器属于带通滤波器的变种。计算角度时，一般通过角加速度和角速度两种方式计算得到角度，因为两种角度的得出方式不同，在融合原始数据时需要一个融合系数[4]。在将两种方式得到的角度进行互补滤波后即可得到一个反应迅速、角度跟踪较为准确的姿态数据。
在进行两种方案的对比选择中，发现MPU6050芯片内部自带运动数据处理运算单元（DMP），通过DMP可以直接输出芯片的姿态角，包括俯仰角、偏航角和滚转角，而俯仰角即芯片所在平面与水平面的夹角，也就是上文提到的角度值。这样，通过使用MPU6050的DMP既可以得到一个较为准确的角度值，又可以减轻微处理器的运算负担，但为了使最后的数据更接近真值，在速度控制上仍然使用了低通滤波算法。最后采用的方案是在方案二的基础上调用了MPU6050的DMP，虽然程序上要移植大量的MPU6050的固件库，但数据更加准确。
关于控制算法
控制算法采用的是久经工业现场考验的PID算法[5]，其控制算法较为简单但控制性能比较可靠稳定、鲁棒性较强、参数整定的方案较为成熟并且无需建立数学模型，因此为了实现智能车的自主平衡，控制算法采用的是PID算法，涉及到直立PID、速度PID和转向PID[6]。
关于测速算法
方案一：位置差分法（即M法），测量规定时间内齿轮转过的角度值
方案二：定角测时法（即T法），测量齿轮转过固定角度所用的时间
M法的程序编写简单，虽然测量低速脉冲时可能会有一些误差，但是测量高速时平稳性和精度都较高；T法一般是测量两个脉冲之间的时间间隔确定被测速度，适合测量低速情况，但车轮转一圈会产生几十上百个脉冲，使用T法不太方便，采用方案一。
3 系统的硬件设计及系统流程
3.1 单片机控制电路
ARM公司推出的 Cortex-M 系列微处理器是市场上较为主流的芯片，经多方比较，本实验平台选择 STM32F103C8T6作为核心芯片，该芯片是新一代 Cortex-M3内核的微处理器，具有 72 MHz 主频、20K字节SRAM、128K字节闪存，提供两个12位adc(16通道)，三个通用16位定时器加上一个PWM定时器，多达两个I2Cs和SPIs，三个usart[11]，一个USB,一个CAN, 2个看门狗定时器, CRC计算单元，7通道DMA控制器。而且有官方提供的专门的开发库，操作调试更加方便简单[12] 。外围电路模块部分围绕主芯片设计，主要组成及功能如下：
4.1.2 算法介绍
低通滤波器算法：Encoder_Least =(speed_left+speed_right)-0;
Encoder *= 0.8;
Encoder += Encoder_Least*0.2;
上式中Encoder_Least是最新速度偏差； 第2、3个程序可以理解为Y(n)=αX(n)+(1-α)Y(n-1)，其中X(n)等同于Encoder_Least，代表本次采样值；Y(n-1)等同于Encoder，代表上次滤波输出值；α是滤波系数，在上式中是0.2。
一阶互补滤波算法：g_fAngle = 0.02 * Angle_ax + (0.98) * (g_fAngle + GY * dt);
上式中左边的g_fAngle为实际得到的角度；Angle_ax为角加速度换算后的角度值，因为角加速度受车体震荡等因素会产生高频噪声，所以对其进行低通滤波，滤除高频信号，因此融合系数较小；右边的g_fAngle可以理解为上一次的角度；GY是角速度，dt是时间，GY *计算dt时间段内通过的角度，通过对角度的不断积分得到一个较为准确的角度，可信度和融合系数较大，有点类似高通滤波。
PID算法：balance=CarAngle.Kp*Bias+CarAngle.Kd*Gyro；
Velocity=Encoder*CarSpeed.Kp+Encoder_Integral*CarSpeed.Ki;
Turn = -Turn_Target * Kp – gyro * Kd;
上面第1个程序是直立PD算法，即角度环控制算法，属于位置PD算法[15]。通过此算法可以实现智能车短时间的直立平衡状态，其中Bias是小车偏离水平面的角度，数值为0时即与地面平行；Gyro是角速度值，通过调节Kd可以减缓小车震荡；具体调节步骤如图4-2，当小车向前倾倒时，车轮跟随小车前进的方向，使其有一个向后的力，反之亦然，调参数时的最好现象是小车对角度变化反应敏感且没有震荡。
图4-2 直立小车运动模型
第2个程序是速度PI控制，Encoder是经过低通滤波输出的当前速度值，因为速度控制和直立控制输出值是要加到一起的，低通滤波可以减缓速度差值对直立控制的干扰；Encoder_Integral是对速度的积分，即位移，积分控制的是速度的静差，对Encoder_Integra的限幅就是对速度的限幅；直立环和速度环一起调试，即可实现小车的长时间直立[16]，直立环是负反馈，速度环是正反馈，即小车因为直立环原因向一个方向加速时，速度环的作用是产生一个同方向更快的速度。
第3个程序是转向PD算法，Turn_Target是转向速度值，数值较大时转向速度块，反之慢，数值大小和转向前小车的速度有关，即小车的旋转速度可以跟随实时速度，增加小车的适应性；gyro是角速度值，gyro * Kd可以减小小车转向时的震荡；据体控制方法就是将小车的左轮PWM输出值减去转向PD输出值，小车的右轮PWM输出值加上转向PD输出值，利用小车车轮的差速进行转向。
串口通信
时钟标准是什么
RI是接受的一个标志位
一个是接收的SBUF，一个是发送的SBUF
直流电机驱动
停电时用来吸收剩余感性原件电流的二极管
编码器
PID，位置式PID
直立环，速度环
转向环，不是
#include "encoder.h"//编码器函数
/**************************************************************************
函数功能：总体来说，我们先要开启总线时钟，然后用三个函数，非别是1.时钟初始化函数2.GPIO端口初始化函数3.配置输入捕获函数，也就是编码器相关的函数
**************************************************************************/
void Encoder_Init_TIM2(void)//编码器初始化函数，形式参数写void做一个强调
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; //定义一个结构体变量成员
TIM_ICInitTypeDef TIM_ICInitStructure; //定义一个结构体变量成员
GPIO_InitTypeDef GPIO_InitStructure; //定义一个结构体变量成员
////
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);//使能定时器2的时钟，因为全部GPIO都是挂载到APB2总线上的，这是开启总线的，通用定时器，挂载到HB1上的
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);//使能PB端口时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;//端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
GPIO_Init(GPIOA, &GPIO_InitStructure); //根据设定参数初始化GPIOA，根据设定参数初始化GPIOA.8,引用函数，第一个是GPIOc，初始化GPIOA外设，第二个是结构体变量名
//前面加上取地址的符号，这里使用的是地址传递
////不配置速度是因为编码器是读取，不是输出，，，，，Note: TIM_TimeBaseStructInit 这个用于初始化结构体
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);//这是对定时器进行初始化，Note: TIM_TimeBaseStructInit 这个用于初始化结构体，最好加上，防止有其他异常
TIM_TimeBaseStructure.TIM_Prescaler = 0x00; // 4.分频系数，预分频器，这里就是0，因为不分频
TIM_TimeBaseStructure.TIM_Period = 65535; //3.设定计数器自动重装值， //不可大于65535 因为F103的定时器是16位的。
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;//1.选择时钟分频：不分频
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;////2.TIM向上计数，计数模式
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
/////////////////////////////////////////////////////////////////////////////////////////
TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);//使用编码器模式3、、这是编码器特有的，编码器配置函数，第一个参数是定时器，第二个参数是模式3，为什么是模式3啊，第三个参数是上升沿，两个都是上升沿
///////////////////////////////////////怎么输入捕获初始化就设置了一个滤波系数，边沿检测和捕获通道都没设置呢？
TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_ICFilter = 10;//这里只有一个滤波器，滤波器是设置10就可以了，ic只有滤波器有效，其他都用不上
//////////////////////////////////////////////////
TIM_ICInit(TIM2, &TIM_ICInitStructure);
TIM_ClearFlag(TIM2, TIM_FLAG_Update);//清除TIM的更新标志位，计数更新之后会拉高，然后要清除一下
TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);//配置什么中断类型，当然是溢出更新
//Reset counter
TIM_SetCounter(TIM2,0);//把0给这个定时器
TIM_Cmd(TIM2, ENABLE); //开定时器
}
/**************************************************************************
函数功能：把TIM4初始化为编码器接口模式
入口参数：无
返回 值：无
**************************************************************************/
void Encoder_Init_TIM4(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_ICInitTypeDef TIM_ICInitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);//使能定时器4的时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);//使能PB端口时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;//端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
GPIO_Init(GPIOB, &GPIO_InitStructure); //根据设定参数初始化GPIOB
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Prescaler = 0x00; // 预分频器
TIM_TimeBaseStructure.TIM_Period = 65535; //设定计数器自动重装值
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;//选择时钟分频：不分频
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;////TIM向上计数
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);//使用编码器模式3
TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_ICFilter = 10;
TIM_ICInit(TIM4, &TIM_ICInitStructure);
TIM_ClearFlag(TIM4, TIM_FLAG_Update);//清除TIM的更新标志位
TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE);
//Reset counter
TIM_SetCounter(TIM4,0);
TIM_Cmd(TIM4, ENABLE);
}
/**************************************************************************
函数功能：单位时间读取编码器计数
入口参数：定时器
返回 值：速度值
**************************************************************************/
int Read_Encoder(u8 TIMX)//编码器读取速度，这里如果用int进行强制类型转换会报错
{
int point; //读取定时器的计数值，然后再清0
switch(TIMX)
{//所以换成这个库函数，这结构体成员不会
case 2: point= TIM_GetCounter(TIM2);TIM_SetCounter(TIM2,0); break;
case 4: point= TIM_GetCounter(TIM4);TIM_SetCounter(TIM2,0); break;
default: point=0;
}
return point;//因为getcounter返回的是uint16，这个是什么，这个是短整型函数，而我们之前对函数定义的是int，所以可能会出错。所以我们要进行一次强制类型转换
}
/**************************************************************************
函数功能：TIM4中断服务函数
入口参数：无
返回 值：无
**************************************************************************/
void TIM4_IRQHandler(void)
{
if(TIM4->SR&0X0001)//溢出中断
{
}
TIM4->SR&=~(1<<0);//清除中断标志位
}
/**************************************************************************
函数功能：TIM2中断服务函数
入口参数：无
返回 值：无
**************************************************************************/
void TIM2_IRQHandler(void)
{
if(TIM2->SR&0X0001)//溢出中断
{
}
TIM2->SR&=~(1<<0);//清除中断标志位
}
void TIM4_IRQHandler(void)//中断标志位清除
{
if(TIM4->SR&0X0001)//溢出中断
TIM4->SR&=~(1<<0);//清除中断标志位
}
评论前必须登录！
注册