自平衡小车控制代码

合集下载

两轮自平衡小车双闭环PID控制设计

两轮⾃平衡⼩车双闭环PID控制设计两轮⾃平衡⼩车的研究意义1.1两轮平衡车的研究意义两轮平衡车是⼀种能够感知环境,并且能够进⾏分析判断然后进⾏⾏为控制的多功能的系统,是移动机器⼈的⼀种。

在运动控制领域中,为了研究控制算法,建⽴两轮平衡车去验证控制算法也是⾮常有⽤的,这使得在研究⾃动控制领域理论时,两轮平衡车也被作为课题,被⼴泛研究。

对于两轮平衡车模型的建⽴、分析以及控制算法的研究是课题的研究重点和难点。

设计的两轮平衡车实现前进、后退、转弯等功能是系统研究的⽬的,之后要对车⼦是否能够爬坡、越野等功能进⾏测试。

⼀个⾼度不稳定,其动⼒学模型呈现多变量、系统参数耦合、时变、不确定的⾮线性是两轮平衡车两轮车研究内容的难点,其运动学中的⾮完整性约束要求其控制任务的多重性,也就是说要在平衡状态下完成指定的控制任务,如在复杂路况环境下实现移动跟踪任务,这给系统设计带来了极⼤的挑战。

因此可以说两路平衡车是⼀个相对⽐较复杂的控制系统,这给控制⽅法提出了很⾼的要求,对控制理论⽅法提出来很⼤的挑战,是控制⽅法实现的典型平台,得到该领域专家的极⼤重视,成为具有挑战性的控制领域的课题之⼀。

两轮平衡车是⼀个复杂系统的实验装置,其控制算法复杂、参数变化⼤,是理论研究、实验仿真的理想平台。

在平衡车系统中进⾏解賴控制、不确定系统控制、⾃适应控制、⾮线性系统控制等控制⽅法的研究,具有物理意义明显、⽅便观察的特点,并且平衡车从造价来说不是很贵,占地⾯积⼩,是很好的实验⼯具，另外建⽴在此基础上的平衡系统的研究,能够适应复杂环境的导航、巡视等,在⼯业⽣产和社会⽣中具有⾮常⼤的应⽤潜⼒。

两轮平衡车所使⽤的控制⽅法主要有:状态回馈控制、PID控制、最优控制、极点回馈控制等,这些控制⽅法被称为传统控制⽅法。

1.2 本⽂研究内容（1）两轮⾃平衡⼩车的简单控制系统设计。

（2）基于倒⽴摆模型的两轮⾃平衡⼩车的数学建模。

（3）利⽤MATLAB⼯具进⾏两轮⾃平衡⼩车的系统控制⽅法分析。

使用STM32CubeMx搭建平衡小车代码框架

使用STM32CubeMx快速搭建平衡小车代码框架硬件平台：STM32CubeMxHAL代码库：STM32F1xx项目平台:MDK5.17A1.项目总体框架如下:MPU6050的数据读取采用软件模拟IIC,可使用MPU的DMP库直接生成角度值，减轻MCU计算负担;(DMP库资源详见，正点原子MPU6050资料)电机驱动部分采用市面上常见的直流电机驱动，引脚分布如下：PWMA,PWMB,A0,A1,B0,B1；其中PWMA、PWMB为电机驱动信号；A0、A1、B0、B1为电机方向控制信号，其控制电平如下：A0 A1 电机高高制动高低正转低高反转低低停止其中，制动为电机锁死，而停止为电机停转；2.项目搭建：Step1.打开STM32CubeMX，单击“New Project”，选择芯片型号，STM32F103C8Tx。

Step2.配置Debug，根据实际选择Step3.配置外部时钟信号Step4.配置TIM2(PWM发生器)Step5.配置模拟IIC引脚Step6.配置电机控制引脚Step7.配置TIM3(用作微妙延时时钟)，CubeMx生成的代码中不包含微妙延时，此部分用于实现模拟IIC的微妙延时Step8.配置USART1(用于串口调试)Step9.时钟配置注：关于输入时钟一定要按实际晶振频率填写，否则会造成时序混乱；参数配置（10KHz）Step11.配置TIM3(微妙延时定时器)定时器时钟频率的计算：定时器时钟频率:72MHz72MHz/(PSC+1)/ARR=72/(71+1)/1=1Mhz=1us; Step12.配置GPIO口Step13.生成项目配置至此，关于平衡小车的软件框架配置已全部完成，点击项目生成，进入MDK 编写代码：代码片段1：微妙函数的实现#include ""#include ""void Delay_us(uint32_t us){uint16_t counter=us&0xffff;HAL_TIM_Base_Start(&htim3);__HAL_TIM_SetCounter(&htim3,counter);while(counter>1){counter=__HAL_TIM_GetCounter(&htim3);}HAL_TIM_Base_Stop(&htim3);}void Delay_ms(uint32_t ms){Delay_us(1000*ms);}代码片段2 模拟IIC：#define HIGH 1#define LOW 0#define SDA_IN() {GPIOB->CRL&=0x0FFFFFFF;GPIOB->CRL|=0x;}#define SDA_OUT() {GPIOB->CRL&=0x0FFFFFFF;GPIOB->CRL|=0x;}#define IIC_SCL(n) (nHAL_GPIO_WritePin(GPIOB,GPIO_PIN_6,GPIO_PIN_SET):HAL_GPIO_WritePin( GPIOB,GPIO_PIN_6,GPIO_PIN_RESET)) //SCL#define IIC_SDA(n) (nHAL_GPIO_WritePin(GPIOB,GPIO_PIN_7,GPIO_PIN_SET):HAL_GPIO_WritePin( GPIOB,GPIO_PIN_7,GPIO_PIN_RESET)) //SDA#define READ_SDA HAL_GPIO_ReadPin(GPIOB,GPIO_PIN_7)void IIC_Init(void){IIC_SDA(HIGH);IIC_SCL(HIGH);}void IIC_Start(void){SDA_OUT();IIC_SDA(HIGH);IIC_SCL(HIGH);Delay_us(4);IIC_SDA(LOW);Delay_us(4);IIC_SCL(LOW);}void IIC_Stop(void){SDA_OUT();IIC_SCL(LOW);IIC_SDA(LOW);Delay_us(4);IIC_SCL(HIGH);IIC_SDA(HIGH);Delay_us(4);}uint8_t IIC_Wait_Ack(void){uint8_t ucErrTime=0;SDA_IN();IIC_SDA(HIGH);Delay_us(1);IIC_SCL(HIGH);Delay_us(1);while(READ_SDA){ucErrTime++;if(ucErrTime>250){IIC_Stop();return 1;}}IIC_SCL(LOW);return 0;}void IIC_Ack(void){IIC_SCL(LOW);SDA_OUT();IIC_SDA(LOW);Delay_us(2);IIC_SCL(HIGH);Delay_us(2);IIC_SCL(LOW);}void IIC_NAck(void){IIC_SCL(LOW);SDA_OUT();IIC_SDA(HIGH);Delay_us(2);IIC_SCL(HIGH);Delay_us(2);IIC_SCL(LOW);}void IIC_Send_Byte(uint8_t txd) {uint8_t t;SDA_OUT();IIC_SCL(LOW);for(t=0;t<8;t++){IIC_SDA((txd&0x80)>>7);txd<<=1;Delay_us(2);IIC_SCL(HIGH);Delay_us(2);IIC_SCL(LOW);Delay_us(2);}}uint8_t IIC_Read_Byte(uint8_t ack) {uint8_t i,receive=0;SDA_IN();for(i=0;i<8;i++ ){IIC_SCL(LOW);Delay_us(2);IIC_SCL(HIGH);receive<<=1;if(READ_SDA)receive++;Delay_us(1);}if (!ack)IIC_NAck();else IIC_Ack();return receive;}代码片段3 PID控制器//50#define P_DATA//#define I_DATA//#define D_DATA 0//以上三值需根据实际调整参数typedef struct PID{int SetPoint;double Proportion;double Integral;double Derivative;int LastError;int PrevError;}PID;void IncPIDInit(PID* sptr){sptr->LastError=0;sptr->PrevError=0;sptr->Proportion=P_DATA;sptr->Integral=I_DATA;sptr->Derivative=D_DATA;sptr->SetPoint=0;}int IncPIDCalc(PID* sptr,int nextPoint){int iError,iIncpid;iError=sptr->SetPoint-nextPoint;iIncpid=sptr->Proportion*iError-\sptr->Integral*sptr->LastError+\sptr->Derivative*sptr->PrevError;sptr->PrevError=sptr->LastError;sptr->LastError=iError;return iIncpid;}代码片段4 PWM发生器HAL_GPIO_WritePin(Left_Dir0_GPIO_Port,Left_Dir0_Pin,GPIO_PIN_S ET);HAL_GPIO_WritePin(Left_Dir1_GPIO_Port,Left_Dir1_Pin,GPIO_PIN_R ESET);HAL_GPIO_WritePin(Right_Dir0_GPIO_Port,Right_Dir0_Pin,GPIO_PIN _SET);HAL_GPIO_WritePin(Right_Dir1_GPIO_Port,Right_Dir1_Pin,GPIO_PIN _RESET);__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,0);__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,0);HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_1);HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_2);注：更改PWM的占空比使用HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,val)函数即可，其占空比的为用户设定的值除以ARR的值，即val/99+1;即val直接等于占空比；后记：关于DMP的代码直接参考正点原子的MPU6050的代码即可；。

平衡小车调试指南(直立环速度环)

平衡小车调试指南接下来将和大家一起以工程的思想去完成一个平衡小车的调试，包括平衡小车的直立环、速度环、转向环，一般我们是先调试直立环，再调试速度环，最好调试转向环。

另外需要说明的是，因为我们使用的电机性能非常好，对PID参数不敏感，也就是说每个参数的取值范围都很广，这将对我们接下来的调试有很大的帮助。

1.1平衡小车直立控制调试平衡小车直立环使用PD（比例微分）控制器，其实一般的控制系统单纯的P控制或者PI控制就可以了，但是那些对干扰要做出迅速响应的控制过程需要D （微分）控制。

下面是直立PD控制的代码：int balance(float Angle,float Gyro){float Bias,kp=300,kd=1;int balance;Bias=Angle-0;//计算直立偏差balance=kp*Bias+Gyro*kd;//计算直立PWMreturn balance;//返回直立PWM}入口参数是平衡小车倾角和Y轴陀螺仪（这个取决MPU6050的安装），我们的小车前进方向是MPU6050的X轴的正方向，电机轴与Y轴平行。

前面两行是相关变量的定义，第三行是计算角度偏差，第四行通过PD控制器计算直立PWM，最后一行是返回。

调试过程包括确定平衡小车的机械中值、确定kp值的极性（也就是正负号）和大小、kd值的极性和大小等步骤。

在调试直立环的时候，我们要屏蔽速度环和转向环，如下图所示：1.1.1确定平衡小车的机械中值把平衡小车放在地面上，绕电机轴旋转平衡小车，记录能让小车接近平衡的角度，一般都在0°附近的。

我们调试的小车正好是0度，所以就是Bias=Angle-0;1.1.2确定kp值的极性（令kd=0）首先我们估计kp的取值范围。

我们的PWM设置的是7200代表占空比100%，假如我们设定kp值为720，那么平衡小车在±10°的时候就会满转。

根据我们的感性认识，这显然太大了，那我们就可以估计kp值在0~720之间，首先大概我们给一个值kp=-200,我们可以观察到，小车往哪边倒，电机会往那边加速让小车到下，就是一个我们不愿看到的正反馈的效果。

[应用]两轮自平衡小车测试程序

两轮自平衡小车测试程序/********************************************************** *************// 两轮自平衡车最终版控制程序（6轴MPU6050+互补滤波+PWM电机）// 单片机STC12C5A60S2// 晶振：20M// 日期：2012.11.26 - ？*********************************************************** ************/#include <REG52.H>#include <math.h>#include <stdio.h>#include <INTRINS.H>typedef unsigned char uchar;typedef unsigned short ushort;typedef unsigned int uint;//******功能模块头文件*******#include "DELAY.H" //延时头文件//#include "STC_ISP.H" //程序烧录头文件#include "SET_SERIAL.H" //串口头文件#include "SET_PWM.H" //PWM头文件#include "MOTOR.H" //电机控制头文件#include "MPU6050.H" //MPU6050头文件//******角度参数************float Gyro_y; //Y轴陀螺仪数据暂存float Angle_gy; //由角速度计算的倾斜角度float Accel_x; //X轴加速度值暂存float Angle_ax; //由加速度计算的倾斜角度float Angle; //小车最终倾斜角度uchar value; //角度正负极性标记//******PWM参数*************int speed_mr; //右电机转速int speed_ml; //左电机转速int PWM_R; //右轮PWM值计算int PWM_L; //左轮PWM值计算float PWM; //综合PWM计算float PWMI; //PWM积分值//******电机参数*************float speed_r_l; //电机转速float speed; //电机转速滤波float position; //位移//******蓝牙遥控参数*************uchar remote_char;char turn_need;char speed_need;//*********************************************************//定时器100Hz数据更新中断//********************************************************* void Init_Timer1(void) //10毫秒@20MHz,100Hz刷新频率{ AUXR &= 0xBF; //定时器时钟12T模式TMOD &= 0x0F; //设置定时器模式TMOD |= 0x10; //设置定时器模式TL1 = 0xE5; //设置定时初值TH1 = 0xBE; //设置定时初值TF1 = 0; //清除TF1标志TR1 = 1; //定时器1开始计时}//*********************************************************//中断控制初始化//*********************************************************void Init_Interr(void){ EA = 1; //开总中断EX0 = 1; //开外部中断INT0EX1 = 1; //开外部中断INT1IT0 = 1; //下降沿触发IT1 = 1; //下降沿触发ET1 = 1; //开定时器1中断}//******卡尔曼参数************float code Q_angle=0.001;float code Q_gyro=0.003;float code R_angle=0.5;float code dt=0.01; //dt为kalman滤波器采样时间;char code C_0 = 1;float xdata Q_bias, Angle_err;float xdata PCt_0, PCt_1, E;float xdata K_0, K_1, t_0, t_1;float xdata Pdot[4] ={0,0,0,0};float xdata PP[2][2] = { { 1, 0 },{ 0, 1 } };//*********************************************************// 卡尔曼滤波//*********************************************************//Kalman滤波，20MHz的处理时间约0.77ms；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_Calcu(void){ //------加速度--------------------------//范围为2g时，换算关系：16384 LSB/g//角度较小时，x=sinx得到角度（弧度）, deg = rad*180/3.14//因为x>=sinx,故乘以1.3适当放大Accel_x = GetData(ACCEL_XOUT_H); //读取X轴加速度Angle_ax = (Accel_x - 1100) /16384; //去除零点偏移,计算得到角度（弧度）Angle_ax = Angle_ax*1.2*180/3.14; //弧度转换为度,//-------角速度-------------------------//范围为2000deg/s时，换算关系：16.4 LSB/(deg/s)Gyro_y = GetData(GYRO_YOUT_H); //静止时角速度Y 轴输出为-30左右Gyro_y = -(Gyro_y + 30)/16.4; //去除零点偏移，计算角速度值,负号为方向处理//Angle_gy = Angle_gy + Gyro_y*0.01; //角速度积分得到倾斜角度.//-------卡尔曼滤波融合-----------------------Kalman_Filter(Angle_ax,Gyro_y); //卡尔曼滤波计算倾角/*//-------互补滤波-----------------------//补偿原理是取当前倾角和加速度获得倾角差值进行放大，然后与//陀螺仪角速度叠加后再积分，从而使倾角最跟踪为加速度获得的角度//0.5为放大倍数，可调节补偿度；0.01为系统周期10msAngle = Angle + (((Angle_ax-Angle)*0.5 + Gyro_y)*0.01);*/ }//*********************************************************//电机转速和位移值计算//*********************************************************void Psn_Calcu(void){ speed_r_l =(speed_mr + speed_ml)*0.5;speed *= 0.7; //车轮速度滤波speed += speed_r_l*0.3;position += speed; //积分得到位移position += speed_need;if(position<-6000) position = -6000;if(position> 6000) position = 6000;}static float code Kp = 9.0; //PID参数static float code Kd = 2.6; //PID参数static float code Kpn = 0.01; //PID参数static float code Ksp = 2.0; //PID参数//********************************************************* //电机PWM值计算//*********************************************************void PWM_Calcu(void){ if(Angle<-40||Angle>40) //角度过大，关闭电机{ CCAP0H = 0;CCAP1H = 0;return;}PWM = Kp*Angle + Kd*Gyro_y; //PID：角速度和角度PWM += Kpn*position + Ksp*speed; //PID：速度和位置PWM_R = PWM + turn_need;PWM_L = PWM - turn_need;PWM_Motor(PWM_L,PWM_R);}//*********************************************************//手机蓝牙遥控//*********************************************************void Bluetooth_Remote(void){ remote_char = receive_char(); //接收蓝牙串口数据if(remote_char ==0x02) speed_need = -80; //前进else if(remote_char ==0x01) speed_need = 80; //后退else speed_need = 0; //不动if(remote_char ==0x03) turn_need = 15; //左转else if(remote_char ==0x04) turn_need = -15; //右转else turn_need = 0; //不转}/*========================================================= =======*///*********************************************************//main//*********************************************************void main(){ delaynms(500); //上电延时Init_PWM(); //PWM初始化Init_Timer0(); //初始化定时器0，作为PWM时钟源Init_Timer1(); //初始化定时器1Init_Interr(); //中断初始化Init_Motor(); //电机控制初始化Init_BRT(); //串口初始化（独立波特率）InitMPU6050(); //初始化MPU6050delaynms(500);while(1){ Bluetooth_Remote();}}/*========================================================= ===*///********timer1中断***********************void Timer1_Update(void) interrupt 3{ TL1 = 0xE5; //设置定时初值10MSTH1 = 0xBE;//STC_ISP(); //程序下载Angle_Calcu(); //倾角计算Psn_Calcu(); //电机位移计算PWM_Calcu(); //计算PWM值speed_mr = speed_ml = 0;}//********右电机中断***********************void INT_L(void) interrupt 0{ if(SPDL == 1) { speed_ml++; } //左电机前进else { speed_ml--; } //左电机后退LED = ~LED;}//********左电机中断***********************void INT_R(void) interrupt 2{ if(SPDR == 1) { speed_mr++; } //右电机前进else { speed_mr--; } //右电机后退LED = ~LED;}。

stm32平衡小车控制部分代码解析

买了这个车子好久了，拿到代码，一头雾水。

所谓的什么用户手册真的是，扯。

没考虑到初学者的感受。

但，问题总要解决，所以用这篇文章来分析一下平衡小车之家出的这个车子吧。

尽管有很多的库函数和宏，其实有很多都是厂商提供的，怎么实现的，大多每个代码段都提供了comment，英文好的话，理解应该不存在问题。

而我们做开发，首要关心的并不是每个模块如何实现，而是应该有几个模块，这些模块是怎么工作的，一个平衡小车能够安稳地在平面上静止与运动，都是通过晶振可以提供一定频率的中断，每一段时间都触发执行了中断服务程序，在此程序中可以获得此刻的欧拉角、温度以及对于平衡小车编码器的数据信息，并且通过PID控制来对小车的步进电机进行控制。

控制部分主要由MPU6050的INT引脚触发5ms定时中断，这个中断是由EXTI控制(具体如何配置要参考MPU6050的INT引脚的特性)。

本车的INT引脚挂载到PA12上，那么他相应的中断服务函数就应该是EXTI15_10_IRQHandler。

接下来我们来分析一下它是如何来控制小车的。

Begining~基本的平衡控制。

1、读取编码器的值。

--------------------Encoder_Left=Read_Encoder(2);Encoder_Right=Read_Encoder(4); 2、获取欧拉角，即俯仰角，偏航角和横滚角。

------------------Get_Angle(Way_Angle);3、平衡、速度PID 控制。

Balance_Pwm =balance(Angle_Balance,Gyro_Balance); Velocity_Pwm=velocity(Encoder_Left,Encoder_Right);4、赋值给PWM 寄存器。

Moto1=Balance_Pwm-Velocity_Pwm-Turn_Pwm;Moto2=Balance_Pwm-Velocity_Pwm+Turn_Pwm;Set_Pwm(Moto1,Moto2);平衡+移动控制刨去超声波，led 灯，按键的读取，新增加了转向环的控制。

基于STM32的两轮自平衡小车控制系统设计

基于STM32的两轮自平衡小车控制系统设计本文主要对两轮自平衡小车的姿态检测算法、PID控制算法两方面进行展开研究。

用加速度传感器和陀螺仪传感器融合而成的姿态传感系统与互补滤波器组合得到自平衡小车准确而稳定的姿态信息，然后PID调节器则利用这些姿态信息输出电机控制信号，控制电机的转动，从而使小车得以平衡。

标签：STM32；自平衡小车；控制系统；控制算法1 研究意义应用意义：两轮平衡车是一种新型的交通工具，它与电动自行车和摩托车车轮前后排列方式不同，而是采用两轮并排固定的方式，就像一种两轮平行的机器人一样。

两轮自平衡控制系统是一种两轮左右平行布置的，像传统的倒立摆一样，本身是一个自然不稳定体，必须施加强有力的控制手段才能使之稳定。

两轮平衡车具有运动灵活、智能控制、操作简单、节省能源、绿色环保、转弯半径为0等优点。

因此它适用于在狭小空间内运行，能够在大型购物中心、国际性会议或展览场所、体育场馆、办公大楼、大型公园及广场、生态旅游风景区、城市中的生活住宅小区等各种室内或室外场合中作为人们的中、短距离代步工具。

具有很大的市场和应用前景。

理论研究意义：车体状态运算主要是将各传感器测量的数据加以融合得出车体倾斜角度值、倾斜角速度值以及行车速度等。

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

运算模块相当于两轮自平衡电动车的大脑，它主要负责的工作是：控制电机的起停，向控制模块发出加速、减速、电机正反转和制动等速度控制信号，接收电机Hall信号进行车速度计算，并通过RS 一232串口向PC发送车速数据以供存储和分析。

另外，还负责接收车体平衡姿态数据，进行自平衡运算。

现有的自平衡车结构种类繁多，但车体都归根于由三层的基本结构组成，从上到下依次是电池层、主控层、电机驱动层。

电池层用于放置给整个系统供电的6V锂电池，主控层由主控芯片系统和传感器模块组成，电机驱动层接受单片机信号，并控制电机。

stm32的自平衡小车设计

stm32的自平衡小车设计STM32自平衡小车设计是一个将许多功能组合在一起的有趣项目。

它不仅需要控制技术，还需要实时处理图像，以便识别障碍物。

自平衡小车使用STM32单片机来控制，这是一款微控制器，具有16位或32位内外存储器、高速Cortex-M4 MCU和多种集成的外设。

STM32单片机的内部集成了多种传感器，如角度传感器、编码器、温度传感器和光学传感器等，可以测量周围环境的变化并作出相应的反应，使小车保持平衡。

它还有两个电机驱动的轮子，电机可以控制小车的前进和后退，而角度传感器可以测量小车的角度，从而判断小车是否已完成平衡，从而调整小车的动作来使其保持平衡。

此外，STM32开发板还具有I2C通信接口，可让开发者使用I2C总线通信，与外部设备交换数据，如摄像头等。

摄像头的主要功能是对周围环境的跟踪，可以帮助小车避开障碍物，准确地定位和预测小车的行驶路径。

为了使小车实现自主运动，还需要一块用于实现运动控制的FPGA芯片，可以用于处理传感器发来的控制信号，根据预设的算法以及图像处理结果，向STM32发出运动控制指令，使小车实现自动行驶。

FPGA芯片可以提供更高的运算速度，以满足实时性要求，这是实现智能小车自动行驶的重要前提。

最后，将所有的控制程序和程序连接在一起，并与SOC系统进行连接，形成一个完整的系统，以实现智能小车的自动行驶。

当实现了自动行驶的功能之后，可以根据需要添加更多的功能，比如跟踪、识别物体、定位、自动充电等，这些功能可以帮助小车自主行驶时更加“聪明”，也可以使小车更好地适应环境调整，实现自主运动。

总而言之，设计一台智能自平衡小车，其基本设计思路是：首先使用STM32单片机作为主控核心，集成传感器用于控制小车保持平衡，而两个电机驱动的轮子可以控制小车前后行驶；其次，使用I2C总线通信的图像传感器可以测量小车的方向，以避开前方的障碍物；最后，使用FPGA芯片实现小车的运动控制，实现智能小车的自动行驶。

利用PID控制算法控制自平衡车

近两年来，在公共场合常常能见到一种叫做体感车（或者叫平衡电动车）的代步工具，由于其便捷灵活，使得其颇为流行，并被称为“最后一公里神器”.其运作原理主要是建立在一种被称为“动态稳定”的基本原理上，也就是车辆本身的自动平衡能力。

以内置的精密固态陀螺仪来判断车身所处的姿势状态，透过精密且高速的中央计算出适当的指令后，驱动马达来做到平衡的效果。

下文采用AVR Atmega16芯片作为主，设计制作了两轮的自平衡电动车。

文中分析了测量角度和角速度传感器的选择，通过ATMEGA16单片机多路信号AD采集陀螺仪和加速度计的信号，经过Kalman滤波算法计算动态的角度和角速度，通过LCD1602显示角速度和角度的值、转向值。

利用PID控制算法控制自平衡车的平衡状态，使车体在平衡位置稳定。

利用大功率MOS管设计，通过单片机有效地控制电机的转速、电机的转向，从而有效地控制自平衡车的前进、后退及转弯功能。

我们来看看具体的设计细节吧。

1 研究意义随着科学技术水平的不断进步，交通工具正朝着小型、节能、环保的方向发展，“电动车”正是在这个背景下孕育而生并为人们所熟识。

据不完全统计，我国的电动车保有量已超过1.2亿辆，是增长速度最快的交通工具。

随着石油储量的不断减少和人们环保意识的增强，“电动车”无疑将成为未来交通工具的主力军。

就目前而言，电动车的种类主要有电动自行车、电动摩托车和电动汽车。

由于电动机制造水平的提高，尤其是大功率直流无刷电动机制造工艺的成熟，带动了电动自行车和电动摩托车行业的飞速发展。

同时，人们也根据两轮自平衡机器人工作原理，设计出了一些新式电动车--两轮自平衡电动车。

它是一种新型的交通工具，它一改电动自行车和摩托车车轮前后排列方式，而是采用两轮并排固定的方式，这种结构将给人们带来一种全新的驾驭感受。

两轮自平衡电动车仅靠两个轮子支撑车体，采用蓄电池提供动力，由电动机驱动，采用微处理器、姿态感知系统、控制算法及车体机械装置共同协调控制车体的平衡，仅靠人体重心的改变便可以实现车辆的启动、加速、减速、停止等功能。

1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。

#include <PID_v1.h>#include "Wire.h"#include "I2Cdev.h"#include "MPU6050_6Axis_MotionApps20.h"MPU6050 mpu(0x68);#define center 0x7Fchar flag=0;char num=0;double time;signed int speeds = 0;signed int oldspeed =0;byte inByte ;// MPU control/status varsbool dmpReady = false;uint8_t mpuIntStatus;uint8_t devStatus;uint16_t packetSize;uint16_t fifoCount;uint8_t fifoBuffer[64];signed int speedcount=0;// orientation/motion varsQuaternion q; // [w, x, y, z] quaternion containerVectorFloat gravity; // [x, y, z] gravity vectorfloat ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector float angle;double Setpoint, Input, Output;double kp = 18.8,ki = 185.0,kd = 0.29;//需要你修改的参数double Setpoints, Inputs, Outputs;double sp = 0.8,si = 0,sd = 0.22;//需要你修改的参数unsigned char dl=17,count;union{signed int all;unsigned char s[2];}data;volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone highvoid dmpDataReady() {mpuInterrupt = true;}PID myPID(&Input, &Output, &Setpoint,kp,ki,kd, DIRECT);PID sPID(&Inputs, &Outputs, &Setpoints,sp,si,sd, DIRECT);void motor(int v){if(v>0){v+=dl;digitalWrite(6,0);digitalWrite(7,1);digitalWrite(8,1);digitalWrite(9,0);analogWrite(10,v);analogWrite(11,v);}else if(v<0){v=-v;v+=dl;digitalWrite(6,1);digitalWrite(7,0);digitalWrite(8,0);digitalWrite(9,1);analogWrite(10,v);analogWrite(11,v);}else{analogWrite(10,0);analogWrite(11,0);}}void motort(int v){if(v>0){v+=dl;digitalWrite(8,1);digitalWrite(9,0);analogWrite(10,v);}else if(v<0){v=-v;v+=dl;digitalWrite(8,0);digitalWrite(9,1);analogWrite(10,v);}else{analogWrite(10,0);}}void setup(){pinMode(6,OUTPUT);pinMode(7,OUTPUT);pinMode(8,OUTPUT);pinMode(9,OUTPUT);pinMode(10,OUTPUT);pinMode(11,OUTPUT);digitalWrite(6,0);digitalWrite(7,1);digitalWrite(8,1);digitalWrite(9,0);analogWrite(10,0);analogWrite(11,0);Serial.begin(115200);Wire.begin();delay(100);Serial.println("Initializing I2C devices...");mpu.initialize();Serial.println("Testing device connections...");Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");delay(2);Serial.println("Initializing DMP...");devStatus = mpu.dmpInitialize();if (devStatus == 0)Serial.println("Enabling DMP...");mpu.setDMPEnabled(true);Serial.println("Enabling interrupt detection (Arduino external interrupt 0)...");attachInterrupt(0, dmpDataReady, RISING);mpuIntStatus = mpu.getIntStatus();Serial.println("DMP ready! Waiting for first interrupt...");dmpReady = true;packetSize = mpu.dmpGetFIFOPacketSize();}else{Serial.print("DMP Initialization failed (code ");Serial.print(devStatus);Serial.println(")");}Setpoint = 22.0;myPID.SetTunings(kp,ki,kd);myPID.SetOutputLimits(-255+dl,255-dl);myPID.SetSampleTime(5);myPID.SetMode(AUTOMATIC);sPID.SetTunings(sp,si,sd);sPID.SetOutputLimits(-10,70);sPID.SetSampleTime(200);sPID.SetMode(AUTOMATIC);attachInterrupt(1,speed,RISING);}void loop(){if (!dmpReady)return;// wait for MPU interrupt or extra packet(s) availableif (!mpuInterrupt && fifoCount < packetSize)return;mpuInterrupt = false;mpuIntStatus = mpu.getIntStatus();fifoCount = mpu.getFIFOCount();if ((mpuIntStatus & 0x10) || fifoCount == 1024){mpu.resetFIFO();else if (mpuIntStatus & 0x02){while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();mpu.getFIFOBytes(fifoBuffer, packetSize);fifoCount -= packetSize;mpu.dmpGetQuaternion(&q, fifoBuffer);mpu.dmpGetGravity(&gravity, &q);mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //从DMP中取出Yaw、Pitch、Roll三个轴的角度，放入数组ypr。

单位：弧度angle=-ypr[1] * 180/M_PI;}Inputs = speedcount;pute();Setpoint = 22.0+Outputs;Input = angle;pute();if(angle>60||angle<0)Output=0;if(flag){motort(Output);flag=0;}else {motor(Output);}if (Serial.available() > 0) {inByte = Serial.read();}if(inByte == 'w'){kp+=0.5;}else if(inByte == 'q'){kp-=0.5;}else if(inByte == 'r'){ki+=10;}else if(inByte == 'e'){ki-=10;}else if(inByte == 'y'){kd+=0.01;}else if(inByte == 't'){kd-=0.01;}else if(inByte == 'i'){dl+=1;}else if(inByte == 'u'){dl-=1;}else if(inByte == 's'){sp+=0.1;}else if(inByte == 'a'){sp-=0.1;}else if(inByte == 'f'){si+=1;}else if(inByte == 'd'){si-=1;}else if(inByte == 'h'){sd+=0.01;}else if(inByte == 'g'){sd-=0.01;}else if(inByte == 'v'){ Setpoints+=2;}else if(inByte == 'b'){ Setpoints-=2;}else if(inByte == 'n'){ Setpoints+=2;flag=1;}else if(inByte == 'm'){ Setpoints-=2;flag=1;}inByte='x';sPID.SetTunings(sp,si,sd); myPID.SetTunings(kp,ki,kd);num++;if(num==20){num=0;Serial.print(kp);Serial.print(',');Serial.print(ki);Serial.print(',');Serial.print(kd);Serial.print(','); Serial.print(dl); Serial.print(" "); Serial.print(sp); Serial.print(','); Serial.print(si); Serial.print(','); Serial.print(sd);Serial.print(','); Serial.println(angle);}}void speed(){if(digitalRead(6)){speedcount+=1;}elsespeedcount-=1; }。