基于STM32+PID控制两轮智能自平衡小车设计

逃离我推掉我的手 2023-10-16 11:06 37阅读 0赞

1.  绪论 1  
    1.1 研究意义及背景 1  
    1.2 本文主要研究内容及任务 1
2.  系统概述 2  
    2.1 控制系统要求分析 2  
    2.2 平衡小车控制原理分析 2  
    2.3 自平衡小车PID控制器分析 4  
    2.3.1 PID控制原理 4  
    2.3.2 PID控制器设计 4
3.  系统硬件电路设计 5  
    3.1 硬件电路整体框架 5  
    3.2 电源供电部分 5  
    3.3 STM32F103C8T6最小系统 7  
    3.3.1 复位电路 8  
    3.3.2 Boot选择部分 8  
    3.3.3晶振部分 9  
    3.3.4 指示灯部分 9  
    3.3.5 st-link下载部分 9  
    3.4 九轴姿态角度传感器部分 10  
    3.4.1 九轴姿态角度传感器基本介绍 10  
    3.4.2 九轴姿态角度传感器性能 10  
    3.4.3 实物图 10  
    3.4.4 引脚说明 11  
    3.4.5 通信方式 11  
    3.5 电机驱动编码器部分 12  
    3.5.1电机编码器基本介绍 12  
    3.5.2 电机实物图 12  
    3.5.3 电机编码器引脚说明 12  
    3.5.4 电机驱动A4950 13  
    3.6 巡线避障部分 14  
    3.6.1 传感器介绍 14  
    3.7 蓝牙通信部份 15
4.  软件设计部分 16  
    4.1 操作系统RTOS(RtThread) 16  
    4.1.1简介RtThread 16  
    4.1.2 系统任务的划分 17  
    4.2 电机驱动 18  
    4.2.1 流程图 18  
    4.2.2 主要API函数 18  
    4.3 九轴姿态角度传感器数据读取 19  
    4.3.1 数据格式 19  
    4.3.2 程序流程 20  
    4.4 编码器数据读取 22  
    4.4.1 数据格式 22  
    4.4.2 程序流程 22  
    4.5 直立PID功能实现 23  
    4.5.1 控制策略 23  
    4.5.2 流程图 23  
    4.5.3 直立环 24  
    4.5.4 速度环 25  
    4.5.5 转向环 26  
    4.6 蓝牙遥控部分 26  
    4.6.1 数据协议 26  
    4.7 循迹避障部分 27  
    4.7.1 主要API解析 27
5.  系统PID调试部分 28  
    5.1 直立环 29  
    5.2 速度环 30  
    5.3 转向环 30  
    5.4 角度锁死 31
6.  总结 31  
    参考文献 32  
    致谢 32  
    附录 33  
    附录一 主要代码 33  
    附录二 原理图 40  
    附录三 实物图 41  
    设计一款两轮自平衡小车，通过对陀螺仪的数据的读取处理，从而识别出车身姿态，根据车身姿态计算PID，通过对小车进行实时的PID控制，使得小车可以按照要求实现自主平衡，根据遥控可以实现平稳前进后退；根据灰度传感器数据，可以实现循迹和避障。具体内容分为以下几个部分：  
    1.平衡小车机械设计部分：主要是机械，重心调整，电气设计部分。  
    2.信号处理部分：九轴陀螺仪数据的读取和处理，灰度传感器的读取和处理，电机编码器的数据读取和处理。  
    3.控制算法PID：根据输入的信息，完成直立环，速度环，转向环的计算控制。  
    4.遥控传输部分：采用蓝牙无线模块，接收手机的遥控数据。  
    2.系统概述  
    2.1 控制系统要求分析  
    要使小车在干扰的情况下实现自平衡，自恢复干扰，并且能够在遥控下实现前进后退，左转右转功能。结合系统分析可知，自平衡小车需要维持原地不倒和运动，运动的动力来自自平衡车的两轮，两轮由两个直流电机驱动。因此，自平衡小车可以作为被控制对象，自平衡小车的两轮可以作为控制系统的控制量。整个系统划分为三个部分：  
    1.自平衡小车的站立控制（直立环）：以小车的倾斜角度和角速度作为输入参数，通过计算直立环的PD控制器，得到电机的输出大小，控制电机的速度来保持小车的自平衡。  
    2.自平衡小车的速度控制（速度环）：在站立环的基础上，通过改变自平衡小车电机的编码器数值来调整车体的实际运动速度。通过计算PI，通过调整电机的运行速度，改变小车的实际运行速度。当速度环的速度设置为０的时候，小车可以实现在原地保持不动，当人为干扰，推动小车，小车可以自动回到原地。设置为某一数值的时候，可以实现前进后退。  
    3.自平衡小车转向调整（转向环）：通过调整自平衡小车两轮的速度，利用差速转弯，采用P控制器。可以实现左右转弯。

#include "main.h"
    
    u16 imuCorrectCount=0,imuErrorCount=0,readEncodeCount=0,sendDataCount=0,controlMotorCount=0;		//记录hz数
    
    s32 leftEncoder=0,rightEncoder=0;
    
    /*
    线	程：readEncode
    作	用：读取左右电机编码器脉冲数
    频	率：100Hz
    */
    void readEncode(void* parameter){
        
    	while(1){
        
    		readEncodeCount++;
    		leftEncoder += Read_Encoder(2);
    		rightEncoder -= Read_Encoder(4);
    		
    		rt_thread_delay(1);
    		rt_timer_check();
    	}
    	
    }
    
    /*
    线	程：sendData
    作	用：使用printf，通过串口3打印数据，也可使用rt_kprintf,通过串口1打印数据
    频	率：10Hz
    */
    void sendData(void* parameter){
        
    
    //	printf("d = %.2f\r\n\r\n",m[2]);
    //		rt_thread_delay(1000);		//do something
    //		rt_timer_check();
    	
    }
    
    
    /*
    线	程：controlMotor
    作	用：控制电机
    频	率：1000Hz
    日	期：2019年1月26日
    作	者：meetwit
    */
    void controlMotor(void* parameter){
        
    	float temp1,temp2,temp3;
    	float get_angle,get_gyro;
    	temp1 = 32768/180.0;			//角度
    	temp2 = 32768/2000.0;			//角速度
    	temp3 = 32768/16.0;				//角加速度
    	int finalPwm = 0,finalPwm2 = 0;
    	u8 time=0;
    	while(1){
        
    		time++;
    		controlMotorCount++;
    			finalPwm = 0;
    		get_angle = stcAngle.Angle[0]/temp1;
    		get_gyro = stcGyro.w[0]/temp2;
    		if(get_angle>40||get_angle<-40){
        									//角度过大，锁住
    			motor_run(0,0);
    			stop_flag = 1;
    		}
    		if(stop_flag){
        
    			if(get_angle>-5&&get_angle<5){
        									//角度适合，解锁
    				rt_thread_delay(RT_TICK_PER_SECOND*2);				//扶正后等待x秒
    				stop_flag = 0;																//解锁
    			}
    				velocity_mw();																//执行一次清除积分
    				motor_run(0,0);																
    		}else{
        
    			//10和300 进行PID控制
    			finalPwm += balance_mw(get_angle,get_gyro);
    			finalPwm += velocity_mw();
    			finalPwm2 = finalPwm+Flag_turn;
    			if(finalPwm2>=0){
        
    					motor_run(3,finalPwm2); 
    			}else{
        
    					motor_run(4,-finalPwm2); 
    			}
    			if(finalPwm>=0){
        
    					motor_run(2,finalPwm);
    			}else{
        
    					motor_run(1,-finalPwm);
    			}
    			if(time%20==0){
        
    				Flag_v = 0;
    				Flag_turn = 0;
    			}
    		}
    		rt_thread_delay(5);
    		rt_timer_check();
    	}
    	
    }
    
    void timer1_f(void* parameter){
        
    	Rx_Tm3--;
    	if(Rx_Tm3==1)
    	Task_Pc3();
    }
    
    /*
    线	程：time_thread
    作	用：通过串口1打印系统时间
    频	率：1Hz
    日	期：2019年1月26日
    作	者：meetwit
    */
    void time_thread(void* parameter){
        
    	rt_tick_t tick_temp;
      rt_uint8_t h=0,m=0,s=0;
    	
    	while(1){
        
    		tick_temp = rt_tick_get();
    		s=tick_temp/RT_TICK_PER_SECOND%60;
    		m=tick_temp/RT_TICK_PER_SECOND/60%60;
    		h=tick_temp/RT_TICK_PER_SECOND/60/60%24;
    		rt_kprintf("\r\nThe system runtime is %d:%d:%d.%d\r\n",h,m,s,tick_temp%RT_TICK_PER_SECOND);
    		rt_kprintf("imuCorrectCount=%d,imuErrorCount=%d,readEncodeCount=%d,sendDataCount=%d,controlMotorCount=%d\r\n",imuCorrectCount/3,imuErrorCount,readEncodeCount,sendDataCount,controlMotorCount);
    
    		imuCorrectCount=0;
    		imuErrorCount=0;
    		readEncodeCount=0;
    		sendDataCount=0;
    		controlMotorCount=0;
    		rt_thread_delay(RT_TICK_PER_SECOND);
    	}
    	
    }

![在这里插入图片描述][e4ee3c278c56476daf8d215ff1c81647.png_pic_center]  
![在这里插入图片描述][b0ce213f8eef41bf96aa19ced78d1f7b.png_pic_center]  
![在这里插入图片描述][72994bd8821d4152b948ffe5330a14ae.png_pic_center]  
![在这里插入图片描述][2201ec4cb48a4f209b7cd467192fbf22.png_pic_center]  
![在这里插入图片描述][4942f64eab0c4db79e2076443a4fbcb2.png_pic_center]  
![在这里插入图片描述][295315a25d2849fbaf070e986c6311f8.png_pic_center]  
![在这里插入图片描述][ea4bf519ba804e9db03a8f650054dadb.png_pic_center]  
![在这里插入图片描述][289c4ee6753b48bd8869f7059f186409.png_pic_center]  
![在这里插入图片描述][3dd367c022e9418388b4cc8cac09e9a6.png_pic_center]  
![在这里插入图片描述][80395328c68d4404a4fe2801ee6309f9.png_pic_center]  
![在这里插入图片描述][8152935f1e254627adf2accd2bface12.png_pic_center]  
![在这里插入图片描述][662bb5be68ab47db84d630e9a526c14a.png_pic_center]  
![在这里插入图片描述][1f8c2892f87541b886089ff5a6cfbc83.png_pic_center]  
![在这里插入图片描述][0aafbcde0b574ee4906818124a3406c8.png_pic_center]  
![在这里插入图片描述][cf94c92a35ef4ac79a6e1bd89c02052e.png_pic_center]  
![在这里插入图片描述][c90eeab329704bcbbabcc3eb51b46a34.png_pic_center]  
![在这里插入图片描述][5e7c4a8d53104a348b36e5b2cc5972ca.png_pic_center]  
![在这里插入图片描述][e32d6c62a99a4e658a8ef73ce0281fb4.png_pic_center]  
![在这里插入图片描述][b384a891719d46e899693ff28c38fb93.png_pic_center]  
![在这里插入图片描述][ed6868dc8a9f4868bbafdf18d01f36b8.png_pic_center]  
![在这里插入图片描述][f89c34cd6b4f48e88ef000093ed8887e.png_pic_center]  
![在这里插入图片描述][1d93782aacf34b18b82db8b2f734148e.png_pic_center]  
![在这里插入图片描述][795695473fa343ee9d56791bc307a6d8.png_pic_center]  
![在这里插入图片描述][e6cbbe9740cf4195872eaae9df9a2c92.png_pic_center]  
![在这里插入图片描述][43a6f11362614580a1475344ec3ef7ea.png_pic_center]  
![在这里插入图片描述][6aeb47e1d8684c2c9c88f5cb6ecac79b.png_pic_center]  
![在这里插入图片描述][5d93f9a2976347d1838204fc1392207b.png_pic_center]  
![在这里插入图片描述][3e3c7cbe82764a10b9b08a75b4e64b2b.png_pic_center]  
![在这里插入图片描述][5f0b14a0410f44a1b0eba853edef765d.png_pic_center]  
![在这里插入图片描述][f5f702e890964befab9ee9f179ae6c73.png_pic_center]  
![在这里插入图片描述][044f895ea2a84f1e85ae1ec3ec588344.png_pic_center]  
![在这里插入图片描述][49dcb3515fcc4d6aa484184cf73bf95f.png_pic_center]  
![在这里插入图片描述][35f67fbb93cf4c9e97373b19d19c591d.png_pic_center]  
![在这里插入图片描述][ccee3be6a7144169aa7ef613381a46cf.png_pic_center]  
![在这里插入图片描述][6457c1f8e2b1461e88ccfc9fcda3989d.png_pic_center]  
![在这里插入图片描述][ad1e753aafb54fdf95d733f1fa9a0378.png_pic_center]  
![在这里插入图片描述][7cbcb384e0c145aab435fe6163ef49cc.png_pic_center]  
![在这里插入图片描述][f8208ac260a840d4b715a5aa60894f9e.png_pic_center]  
![在这里插入图片描述][fe062ac624b0497ea078e11f1f5cbf63.png_pic_center]  
![在这里插入图片描述][f70d0f36669146f48ba5b80a4ee77221.png_pic_center]  
![在这里插入图片描述][184687d3308b4eacaef92f1ed2943d8c.png_pic_center]  
![在这里插入图片描述][841a9c4b58f84c1480e1f901ee459058.png_pic_center]  
![在这里插入图片描述][6fe425cce0924d41912bbf2f74c596df.png_pic_center]  
![在这里插入图片描述][c770f509b14648bfb731f4055ad17078.png_pic_center]  
![在这里插入图片描述][82b0408cf0d34337a757a73399a8c753.png_pic_center]  
![在这里插入图片描述][b85a8020c7c7477bb30a04ed1aefdeb5.png_pic_center]  
![在这里插入图片描述][a36950c0d9364bf089bd3c54d62f3a27.png_pic_center]  
![在这里插入图片描述][ff3cb393a2e6422396e1b75f85462a96.png_pic_center]

[e4ee3c278c56476daf8d215ff1c81647.png_pic_center]: https://img-blog.csdnimg.cn/e4ee3c278c56476daf8d215ff1c81647.png#pic_center
[b0ce213f8eef41bf96aa19ced78d1f7b.png_pic_center]: https://img-blog.csdnimg.cn/b0ce213f8eef41bf96aa19ced78d1f7b.png#pic_center
[72994bd8821d4152b948ffe5330a14ae.png_pic_center]: https://img-blog.csdnimg.cn/72994bd8821d4152b948ffe5330a14ae.png#pic_center
[2201ec4cb48a4f209b7cd467192fbf22.png_pic_center]: https://img-blog.csdnimg.cn/2201ec4cb48a4f209b7cd467192fbf22.png#pic_center
[4942f64eab0c4db79e2076443a4fbcb2.png_pic_center]: https://img-blog.csdnimg.cn/4942f64eab0c4db79e2076443a4fbcb2.png#pic_center
[295315a25d2849fbaf070e986c6311f8.png_pic_center]: https://img-blog.csdnimg.cn/295315a25d2849fbaf070e986c6311f8.png#pic_center
[ea4bf519ba804e9db03a8f650054dadb.png_pic_center]: https://img-blog.csdnimg.cn/ea4bf519ba804e9db03a8f650054dadb.png#pic_center
[289c4ee6753b48bd8869f7059f186409.png_pic_center]: https://img-blog.csdnimg.cn/289c4ee6753b48bd8869f7059f186409.png#pic_center
[3dd367c022e9418388b4cc8cac09e9a6.png_pic_center]: https://img-blog.csdnimg.cn/3dd367c022e9418388b4cc8cac09e9a6.png#pic_center
[80395328c68d4404a4fe2801ee6309f9.png_pic_center]: https://img-blog.csdnimg.cn/80395328c68d4404a4fe2801ee6309f9.png#pic_center
[8152935f1e254627adf2accd2bface12.png_pic_center]: https://img-blog.csdnimg.cn/8152935f1e254627adf2accd2bface12.png#pic_center
[662bb5be68ab47db84d630e9a526c14a.png_pic_center]: https://img-blog.csdnimg.cn/662bb5be68ab47db84d630e9a526c14a.png#pic_center
[1f8c2892f87541b886089ff5a6cfbc83.png_pic_center]: https://img-blog.csdnimg.cn/1f8c2892f87541b886089ff5a6cfbc83.png#pic_center
[0aafbcde0b574ee4906818124a3406c8.png_pic_center]: https://img-blog.csdnimg.cn/0aafbcde0b574ee4906818124a3406c8.png#pic_center
[cf94c92a35ef4ac79a6e1bd89c02052e.png_pic_center]: https://img-blog.csdnimg.cn/cf94c92a35ef4ac79a6e1bd89c02052e.png#pic_center
[c90eeab329704bcbbabcc3eb51b46a34.png_pic_center]: https://img-blog.csdnimg.cn/c90eeab329704bcbbabcc3eb51b46a34.png#pic_center
[5e7c4a8d53104a348b36e5b2cc5972ca.png_pic_center]: https://img-blog.csdnimg.cn/5e7c4a8d53104a348b36e5b2cc5972ca.png#pic_center
[e32d6c62a99a4e658a8ef73ce0281fb4.png_pic_center]: https://img-blog.csdnimg.cn/e32d6c62a99a4e658a8ef73ce0281fb4.png#pic_center
[b384a891719d46e899693ff28c38fb93.png_pic_center]: https://img-blog.csdnimg.cn/b384a891719d46e899693ff28c38fb93.png#pic_center
[ed6868dc8a9f4868bbafdf18d01f36b8.png_pic_center]: https://img-blog.csdnimg.cn/ed6868dc8a9f4868bbafdf18d01f36b8.png#pic_center
[f89c34cd6b4f48e88ef000093ed8887e.png_pic_center]: https://img-blog.csdnimg.cn/f89c34cd6b4f48e88ef000093ed8887e.png#pic_center
[1d93782aacf34b18b82db8b2f734148e.png_pic_center]: https://img-blog.csdnimg.cn/1d93782aacf34b18b82db8b2f734148e.png#pic_center
[795695473fa343ee9d56791bc307a6d8.png_pic_center]: https://img-blog.csdnimg.cn/795695473fa343ee9d56791bc307a6d8.png#pic_center
[e6cbbe9740cf4195872eaae9df9a2c92.png_pic_center]: https://img-blog.csdnimg.cn/e6cbbe9740cf4195872eaae9df9a2c92.png#pic_center
[43a6f11362614580a1475344ec3ef7ea.png_pic_center]: https://img-blog.csdnimg.cn/43a6f11362614580a1475344ec3ef7ea.png#pic_center
[6aeb47e1d8684c2c9c88f5cb6ecac79b.png_pic_center]: https://img-blog.csdnimg.cn/6aeb47e1d8684c2c9c88f5cb6ecac79b.png#pic_center
[5d93f9a2976347d1838204fc1392207b.png_pic_center]: https://img-blog.csdnimg.cn/5d93f9a2976347d1838204fc1392207b.png#pic_center
[3e3c7cbe82764a10b9b08a75b4e64b2b.png_pic_center]: https://img-blog.csdnimg.cn/3e3c7cbe82764a10b9b08a75b4e64b2b.png#pic_center
[5f0b14a0410f44a1b0eba853edef765d.png_pic_center]: https://img-blog.csdnimg.cn/5f0b14a0410f44a1b0eba853edef765d.png#pic_center
[f5f702e890964befab9ee9f179ae6c73.png_pic_center]: https://img-blog.csdnimg.cn/f5f702e890964befab9ee9f179ae6c73.png#pic_center
[044f895ea2a84f1e85ae1ec3ec588344.png_pic_center]: https://img-blog.csdnimg.cn/044f895ea2a84f1e85ae1ec3ec588344.png#pic_center
[49dcb3515fcc4d6aa484184cf73bf95f.png_pic_center]: https://img-blog.csdnimg.cn/49dcb3515fcc4d6aa484184cf73bf95f.png#pic_center
[35f67fbb93cf4c9e97373b19d19c591d.png_pic_center]: https://img-blog.csdnimg.cn/35f67fbb93cf4c9e97373b19d19c591d.png#pic_center
[ccee3be6a7144169aa7ef613381a46cf.png_pic_center]: https://img-blog.csdnimg.cn/ccee3be6a7144169aa7ef613381a46cf.png#pic_center
[6457c1f8e2b1461e88ccfc9fcda3989d.png_pic_center]: https://img-blog.csdnimg.cn/6457c1f8e2b1461e88ccfc9fcda3989d.png#pic_center
[ad1e753aafb54fdf95d733f1fa9a0378.png_pic_center]: https://img-blog.csdnimg.cn/ad1e753aafb54fdf95d733f1fa9a0378.png#pic_center
[7cbcb384e0c145aab435fe6163ef49cc.png_pic_center]: https://img-blog.csdnimg.cn/7cbcb384e0c145aab435fe6163ef49cc.png#pic_center
[f8208ac260a840d4b715a5aa60894f9e.png_pic_center]: https://img-blog.csdnimg.cn/f8208ac260a840d4b715a5aa60894f9e.png#pic_center
[fe062ac624b0497ea078e11f1f5cbf63.png_pic_center]: https://img-blog.csdnimg.cn/fe062ac624b0497ea078e11f1f5cbf63.png#pic_center
[f70d0f36669146f48ba5b80a4ee77221.png_pic_center]: https://img-blog.csdnimg.cn/f70d0f36669146f48ba5b80a4ee77221.png#pic_center
[184687d3308b4eacaef92f1ed2943d8c.png_pic_center]: https://img-blog.csdnimg.cn/184687d3308b4eacaef92f1ed2943d8c.png#pic_center
[841a9c4b58f84c1480e1f901ee459058.png_pic_center]: https://img-blog.csdnimg.cn/841a9c4b58f84c1480e1f901ee459058.png#pic_center
[6fe425cce0924d41912bbf2f74c596df.png_pic_center]: https://img-blog.csdnimg.cn/6fe425cce0924d41912bbf2f74c596df.png#pic_center
[c770f509b14648bfb731f4055ad17078.png_pic_center]: https://img-blog.csdnimg.cn/c770f509b14648bfb731f4055ad17078.png#pic_center
[82b0408cf0d34337a757a73399a8c753.png_pic_center]: https://img-blog.csdnimg.cn/82b0408cf0d34337a757a73399a8c753.png#pic_center
[b85a8020c7c7477bb30a04ed1aefdeb5.png_pic_center]: https://img-blog.csdnimg.cn/b85a8020c7c7477bb30a04ed1aefdeb5.png#pic_center
[a36950c0d9364bf089bd3c54d62f3a27.png_pic_center]: https://img-blog.csdnimg.cn/a36950c0d9364bf089bd3c54d62f3a27.png#pic_center
[ff3cb393a2e6422396e1b75f85462a96.png_pic_center]: https://img-blog.csdnimg.cn/ff3cb393a2e6422396e1b75f85462a96.png#pic_center