900字范文 > STM32入门笔记(02)：麦克纳姆轮 TB6612电机驱动 PID电机控制和编码器学习记录

STM32入门笔记(02)：麦克纳姆轮 TB6612电机驱动 PID电机控制和编码器学习记录

时间：2019-10-01 02:58:43

麦克纳姆轮麦克纳姆轮平移原理麦克纳姆轮运动学分析TB6612直流电机驱动直流电机的原理减速器TB6612电机驱动电机接线说明编码器编码器原理四倍频程序实现数据离散化PID 控制PID 控制原理3个组成单元调整PID参数：位置式PID==增量式==PID参考资料

小车整体造型和选用的器材如下图所示：

点击这里：3輹Zhi6¥0tJZ2her1o9¥://,打開/

麦克纳姆轮

麦克纳姆轮(Mecanum Wheel)，又称为艾隆轮(Ilon Wheel)。其由轮毂和固定在外周的许多小辊子构成，轮轴和辊轴之间的夹角通常为45°

每个轮子具有三个自由度，分别是绕轮轴转动，沿垂直于与地面接触的辊子的辊轴方向移动，绕轮子和地面的接触点转动。

根据机械原理，机构的原动件数应该等于机构的自由度，因此，若要实现三个自由度的控制，则应该有三个独立的输入。

而每个麦克纳姆轮可以看作一个原动件，因此，若要实现平面3 个自由度的控制，就应该至少有 3 个麦克纳姆轮。

也就是说，理论上只要有3个这样的轮子组成的移动平台便可实现全向移动。

但是，在实际应用中，麦克纳姆轮都是成对使用的，两个左旋轮，两个右旋轮，总共4 个，其中左旋轮和右旋轮呈手性对称，这样既可以增加机构的稳定性，又方便控制，同时还提升了载重能力。

麦克纳姆轮平移原理

红色表示每个轮子速度分解，蓝色表示轮子转动方向：

麦克纳姆轮平移原理

【B站】可以前后左右平移的神器轮子来看看是怎么做到的-麦克纳姆轮平移原理

麦克纳姆轮运动学分析

void Set_Pwm(int motor_a,int motor_b,int motor_c,int motor_d,int servo){//Forward and reverse control of motor//电机正反转控制if(motor_a<0)AIN2=0,AIN1=1;else AIN2=1,AIN1=0;//Motor speed control //电机转速控制PWMA=abs(motor_a);//Forward and reverse control of motor//电机正反转控制if(motor_b<0)BIN2=1,BIN1=0;else BIN2=0,BIN1=1;//Motor speed control //电机转速控制PWMB=abs(motor_b);//Forward and reverse control of motor//电机正反转控制if(motor_c>0)CIN2=0,CIN1=1;else CIN2=1,CIN1=0;//Motor speed control //电机转速控制PWMC=abs(motor_c);//Forward and reverse control of motor//电机正反转控制if(motor_d>0)DIN2=0,DIN1=1;else DIN2=1,DIN1=0;//Motor speed control //电机转速控制PWMD=abs(motor_d);//Servo control//舵机控制Servo_PWM =servo;}

下面我们以四轮麦轮机器人为例进行运动学分析

运动学分析求的就是Vx、Vy、Vz 与 VA轮、VB轮轮、VC轮、VD轮的关系。

参数讲解完毕，我们现在讲解这些参数之间的关系，以及如何求出四轮麦轮机器人的运动学正逆解公式。

通过速度分解可得：

TB6612直流电机驱动

直流电机的原理

直流电机的工作原理

【B站】直流电机的工作原理

减速器

一般直流电机的转速都是一分钟几千上万转的，所以一般需要安

装减速器。减速器是一种相对精密的机械零件，使用它的目的是降低转速，增加转矩。减速后的直流电机力矩增大、可控性更强。

按照传动级数不同可分为单级和多级减速器；按照传动类型可分为齿轮减速器、蜗杆减速器和行星齿轮减速器。

齿轮减速箱体积较小，传递扭矩大，但是有一定的回程间隙。

蜗轮蜗杆减速机的主要特点是具有反向自锁功能，可以有较大的减速比，但是一般体积较大，传动效率不高，精度不高。

行星减速机其优点是结构比较紧凑，回程间隙小、精度较高，使用寿命很长，额定输出扭矩可以做的很大，但价格略贵。

TB6612电机驱动

TB6612FNG 是东芝半导体公司生产的一款直流电机驱动器件，

它具有大电流 MOSFET-H 桥结构，双通道电路输出，可同时驱动 2个电机。

相比 L298N 的热耗性和外围二极管续流电路，它无需外加散

热片，外围电路简单，只需外接电源滤波电容就可以直接驱动电机，利于减小系统尺寸。对于 PWM 信号输入频率范围，高达 100 kHz 的频率。

TB6612FNG 的主要参数：

最大输入电压：VM = 15V最大输出电流：Iout = 1.2A（平均）/3.2A（峰值）正反转/短路刹车/停机功能模式内置过热保护和低压检测电路

要实现的调试和换向功能，可以使用单片机实现的，但是单片机~~ IO 的带负载能力较弱~~，而直流电机是大电流感性负载，所以需要功率放大器件TB6612FNG。

VM 直接接电池即可VCC 是内部的逻辑供电，一般给 3.3 或者5VGND 建议一个接电源地，一个接单片机地STBY置高模块才能正常工作

上图中红色部分的 5 个引脚控制一路电机，蓝色部分的控制另外一路电机。

这里只讲其中的A路，B 路的使用是一样的。AO1和AO2分别接到电机的+和-。然后通过PWMA、AIN2、AIN1控制电机。其中 PWMA 接到单片机的 PWM 引脚，一般 10Khz 的 PWM 即可，并通过改变占空比来调节电机的速度。下面是真值表：

电机接线说明

编码器

编码器是一种将角位移或者直线位移转换成一连串电数字脉冲的一种传感器。

我们可以通过编码器测量电机转动的位移或者速度信息。

编码器按照工作原理，可以分为增量式编码器和绝对式编码器，绝对式编码器的每一个位置对应一个确定的数字码，因此它的示值只与测量的起始和终止位置有关，而与测量的中间过程无关。我们常用的编码器为增量式编码器。

从编码器检测原理上来分，还可以分为光学式、磁式、感应式、电容式。常见的是光电编码器（光学式）和霍尔编码器（磁式）。一般来说光电编码器是霍尔编码器精度的几十倍。

编码器原理

光电编码器是由光码盘和光电检测装置组成。

光码盘是在一定直径的圆板上等分地开通若干个长方形孔。由于光电码盘与电动机同轴，电动机旋转时，检测装置检测输出若干脉冲信号，为判断转向，一般输出两组存在一定相位差的方波信号。

霍尔编码器是由霍尔码盘和霍尔元件组成。

霍尔码盘是在一定直径的圆板上等分地布置有不同的磁极。霍尔码盘与电动机同轴，电动机旋转时，霍尔元件检测输出若干脉冲信号，为判断转向，一般输出两组存在一定相位差的方波信号。然后我们就可以通过单片机采集 AB 相的脉冲数据，根据脉冲换算即可得到电机位置数据。

四倍频

四倍频是通过程序提升我们的编码器的精度的一种数据处理方法，可以有效的最大化我们的编码器的精度和测量精度。

上图是一个我们编码器输出的波形图，正常我们一般的处理方式是通过A相去计数，B 相去判断目前的转动方向。

具体实现比如：A 相的上升沿计数或者下降沿计数，同时在 A 相的上升沿或者下降沿来根据 B 相此时的电平状态来判断转向。

四倍频则是同时计算AB 两相的每个跳边沿，这样原本在A 相计数的一个脉冲周期内就实现了4次计数，从而实现了精度的提升。

基本框图

程序实现

示例程序源码：STM32F103_Encoder_demo资料下载

参考资料：【灰信网】 STM32控制编码器

STM32有对硬件编码器处理，只需要对编码器模式初始化即可。对定时器的通道1和通道二处理。微控制器、单片机都有正交脉冲处理接口。

ENCODER.c

#include "ENCODER.h"/*TIM4初始化为编码器接口*/void Encoder_Init_TIM4(u16 arr,u16 psc){GPIO_InitTypeDef GPIO_InitStructure; //定义一个引脚初始化的结构体 TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;//定义一个定时器初始化的结构体TIM_ICInitTypeDef TIM_ICInitStructure; //定义一个定时器编码器模式初始化的结构体RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); //使能TIM4时钟RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); //使能CPIOB时钟GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;//PB6、PB7GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入GPIO_Init(GPIOB, &GPIO_InitStructure);//根据GPIO_InitStructure的参数初始化GPIOB0TIM_TimeBaseStructure.TIM_Period = arr; //设定计数器自动重装值TIM_TimeBaseStructure.TIM_Prescaler = psc; // 预分频器 TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; //选择时钟分频：不分频TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct的参数初始化定时器TIM4TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);//使用编码器模式3：CH1、CH2同时计数，四分频TIM_ICStructInit(&TIM_ICInitStructure); //把TIM_ICInitStruct 中的每一个参数按缺省值填入TIM_ICInitStructure.TIM_ICFilter = 10; //设置滤波器长度TIM_ICInit(TIM4, &TIM_ICInitStructure); //根TIM_ICInitStructure参数初始化定时器TIM4编码器模式TIM_Cmd(TIM4, ENABLE); //使能定时器4}//读取编码器计数int Read_Encoder_TIM4(void){int Encoder_TIM;Encoder_TIM=TIM4->CNT; //读取计数if(Encoder_TIM>0xefff)Encoder_TIM=Encoder_TIM-0xffff; //转化计数值为有方向的值，大于0正转，小于0反转。//TIM4->CNT范围为0-0xffff，初值为0。TIM4->CNT=0; //读取完后计数清零return Encoder_TIM; //返回值}

main.c

#include "ENCODER.h"#include "usart.h" #include "delay.h"int main(void){delay_init();uart_init(9600);Encoder_Init_TIM4(0xffff,0);while(1){delay_ms(200); //每隔200ms读取一次编码器计数，即速度。//可以使用定时中断实现更精准的速度计算，用户可自定义printf("Encoder=%d\r\n", Read_Encoder_TIM4());}}

数据离散化

PID 控制

PID简单介绍：所谓 PID 控制，就是对系统偏差进行比例、积分以及微分的控制。

PID是闭环控制，因此需要有传感器测量我们需要控制的参数，并且反馈到我们的控制计算当中，并且参与控制。

PID 由 3 个单元组成，分别是比例（Proportion单元、积分（Integral单元、微分（Differential单元。

通过对这三个单元的处理计算输出给执行器，达到减小偏差最终实现收敛的过程。

PID 控制原理

【B站@421施工队】PID从理论到实践，从不懂到会用！

【B站@泽阳一米九】基于DJI RoboMaster S1机器人课程视觉标签跟随项目《PID控制器》原理深入浅出

【B站@程欢欢的智能控制集】学会PID-基于板球平衡系统-初中基础就能听懂

闭环控制：

例如，一个人沿着一条白线直线行走。

目标量 - 白线

传感器 - 眼睛

执行器 - 腿

偏差量 - 由传感器(眼睛)获取

执行量 - 通过偏差量进行计算得出

计算出执行量之后，双腿根据执行量微调方向。

飞机、火箭、家电……自动控制领域都会运用到PID控制算法。

核心：如何通过偏差量计算出执行量？

3个组成单元

比例P-比例值

开关只有 0 和 1 （开和关）

旋钮的值可以是1、2、3……100 （比例值），传感器、执行器有数值范围。

通过比例值让偏差量和执行量都有数值范围，这样就可以进行计算了。

通过传感器获取小球位置，与目标量之间的关系计算出偏差量，偏差量再转换成执行量，但是这二者单位不一样，需要通过比例系数P进行转换。

只有比例P是不能完成任务的，因为它的数值固定，不能分辨情形，只能执行单一的时间点，如果通过时间的维度……增幅器 - 积分I，抑制器 - 微分D。如果比例P值小了，增幅器来补充；比例P值大了，抑制器来削减。

增幅器 - 积分I

积分I 是通过判断过去积分I是否需要增值，不断累加过去经验，判断比例P是否需要帮助，是否需要增值，让天平趋近于0。

注意：增幅器需要限制，如果系统出现意外或错误，增幅器可能会被累加到无限大。

两个方法限制：

1、在任何时刻限制积分I的幅度，控制积分I的最大值和最小值；

2、当系统判断到目标丢失，或者系统没有运行不用的时候，主动将积分I清零，等待下一次正常运行。

抑制器 - 微分D

微分D 预测未来，通过当前的偏差量 - 上一次的偏差量 = 预测下一次偏差量。用下一次的偏差量提前参与计算中，防止比例P超出目标的问题。

每次把偏差量装填如上一次偏差量备用：

上一次偏差量 = 偏差量

注意: 考虑到积分I 和微分D都是跟时间息息相关，所以系统有延时的情况下，积分I要乘以时间系数；微分D要除以时间系数。

调整PID参数：

比例P - 基础运行能力。大小适中。P太小，不能到目标；P太大，超出目标。

积分I - 补足P过小的为题。小偏差量起效。I太小，不能到目标，小偏差量不能回正；I太大，超出目标，系统表现迟钝。

微分D：阻止超出目标。D太小，回超出目标；D太大，系统会在目标范围内高频抖动，另外对误差会很敏感。

位置式PID

闭环控制就是根据编码器的脉冲累加测量电机的位置信息，并与目标值进行比较，得到控制偏差，然后通过对偏差的比例、积分、微分进行控制，使偏差趋向于0的过程。

目标位置一般我们可以通过按键或者开关等方式编程实现改变目标值；测量位置前面在编码器就已经有说到就是通过单片机去采集编码器的数据。

目标位置和测量位置之间做差这个就是目前系统的偏差。送入PID 控制器进行计算输出，然后再经过电机驱动的功率放大控制电机的转动去减小偏差，最终达到目标位置。

总结控制原理为以下5步：

1、获取目标值，一般可以通过输入信号（比如按键控制）获取；

2、获取测量值，单片机通过电机编码器根据脉冲数测得位置；

3、比较目标值、测量值得到偏差量，送入PID控制器计算得，得到执行量；

4、PID控制器计算输出，根据输出值控制输出给驱动的信号；

5、驱动放大控制信号然后输出给电机，实现电机控制。

增量式PID

速度闭环控制就是根据单位时间获取的脉冲数（这里使用了M法测速）测量电机的速度信息，并与目标值进行比较，得到控制偏差，然后通过对偏差的比例、积分、微分进行控制，使偏差趋向于0的过程。

目标速度一般我们可以通过按键或者开关等方式编程实现改变目标值；

测量速度前面讲编码器时候已经有说到就是通过单片机定时去采集编码器的数据并清零；

目标速度和测量速度之间做差这个就是目前系统的偏差。送入PID控制器进行计算输出执行量；

然后再经过电机驱动的功率放大控制电机的转动去减小偏差，最终达到目标速度的过程。

基于STM32F103C8T6 PID （库函数版）部分程序：

timer.h 文件

#ifndef __TIMER_H#define __TIMER_H#include <sys.h> void Timer1_Init(u16 arr,u16 psc); #endif

timer.c文件

#include "timer.h"/**************************************************************************函数功能：定时中断初始化入口参数：arr：自动重装值 psc：时钟预分频数返回值：无**************************************************************************/void Timer1_Init(u16 arr,u16 psc) {TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;NVIC_InitTypeDef NVIC_InitStruct;RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1,ENABLE);TIM_TimeBaseInitStruct.TIM_Period = arr;//重装载值TIM_TimeBaseInitStruct.TIM_Prescaler = psc; //预分频系数TIM_TimeBaseInitStruct.TIM_ClockDivision =0; //时钟分割TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;//TIM向上计数模式TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStruct);TIM_ITConfig(TIM1,TIM_IT_Update,ENABLE);//使能定时器中断NVIC_InitStruct.NVIC_IRQChannel = TIM1_UP_IRQn; //使能按键所在的外部中断通道NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE; //使能外部中断通道NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 1; //抢占优先级1NVIC_InitStruct.NVIC_IRQChannelSubPriority = 3; //响应优先级3NVIC_Init(&NVIC_InitStruct);TIM_Cmd(TIM1,ENABLE);}

control.h文件

#ifndef __CONTROL_H#define __CONTROL_H#include "sys.h"#define PI 3.14159265#define ZHONGZHI 3085 //终止externint Balance_Pwm,Velocity_Pwm;int TIM1_UP_IRQHandler(void);void Set_Pwm(int moto); //赋值给PWM寄存器void Key(void); //按键修改运行状态 void Xianfu_Pwm(void); //限制PWM幅值int myabs(int a);//取绝对值int Position_PID (int position,int target);int Incremental_PI (int Encoder,int Target); #endif

control.c文件

#include "control.h"/**************************************************************************函数功能： TIM1控制的定时中断 **************************************************************************/int TIM1_UP_IRQHandler(void) {if(TIM_GetFlagStatus(TIM1,TIM_FLAG_Update)==SET)//5ms定时中断{TIM_ClearITPendingBit(TIM1,TIM_IT_Update); //===清除定时器1中断标志位Encoder=Read_Velocity(4); //===输入捕获读取实际值Key(); //===按键控制目标值Moto=Incremental_PI(Encoder,Target_Velocity); //===位置PID控制器Xianfu_Pwm();//===PWM限幅Set_Pwm(Moto);} return 0; } /**************************************************************************函数功能：赋值给PWM寄存器入口参数：PWM返回值：无**************************************************************************/void Set_Pwm(int moto){if(moto<0)AIN2=0,AIN1=1;else AIN2=1,AIN1=0;PWMA=myabs(moto);}/**************************************************************************函数功能：限制PWM赋值入口参数：无返回值：无**************************************************************************/void Xianfu_Pwm(void){int Amplitude=7100; //===PWM满幅是7200 限制在7100if(Moto<-Amplitude) Moto=-Amplitude;if(Moto>Amplitude) Moto=Amplitude;}/**************************************************************************函数功能：按键修改运行状态入口参数：无返回值：无**************************************************************************/void Key(void) {int tmp,Velocity_Amplitude=10; tmp=click_N_Double(10);//检测按键 if(tmp==1)Target_Velocity+=Velocity_Amplitude; //单击增加位置else if(tmp==2)Target_Velocity-=Velocity_Amplitude; //单击增加位置if(Target_Velocity>40)Target_Velocity=40;if(Target_Velocity<-40)Target_Velocity=-40;}/**************************************************************************函数功能：取绝对值入口参数：int返回值：unsigned int**************************************************************************/int myabs(int a){int temp;if(a<0) temp=-a; else temp=a;return temp;}/**************************************************************************函数功能：增量PI控制器入口参数：编码器测量值，目标速度返回值：电机PWM根据增量式离散PID公式 pwm+=Kp[e（k）-e(k-1)] + Ki*e(k) + Kd[e(k)-2e(k-1)+e(k-2)]Kp：p系数Ki：i系数Kd: d系数e(k)：本次偏差 e(k-1)：上一次的偏差以此类推 pwm代表增量输出在我们的速度控制闭环系统里面，只使用PI控制pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)**************************************************************************/int Incremental_PI (int Encoder,int Target){static float Bias,Pwm,Last_bias;Bias=Target-Encoder; //计算偏差Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; //增量式PI控制器Last_bias=Bias; //保存上一次偏差 return Pwm; //增量输出}

资料下载：

【CSDN】麦克纳姆轮运动学分析、TB6612直流电机驱动原理图、PID控制与编码器教学视频例程相关资料【CSDN】STM32F407VET6-麦克纳姆轮小车项目源码-MDK5-库函数版本