【《基于PID算法的两轮自平衡小车控制系统软件设计案例》4200字】

基于PID算法的两轮自平衡小车控制系统软件设计案例目录TOC\o"1-3"\h\u31647基于PID算法的两轮自平衡小车控制系统软件设计案例 136461相关算法理论 1220171.1PID算法 1220241.1.1位置型PID算法 234401.1.2增量式PID算法 2185431.2Kalman滤波简介 373612软件设计部分 4217722.1两轮自平衡小车主程序设计 4319602.2介绍所使用的软件 55432.3PID控制算法 6287252.4滤波算法 641932.5数据融合算法 710282.5.1卡尔曼滤波 7128902.5.2一阶互补滤波 9149862.5.2DMP（数字运动处理器） 970562.5两轮自平衡车的实物展示与测试 101相关算法理论1.1PID算法PID算法是平衡小车保持平衡的核心算法整个小车的控制思路都是为他展开十分重要，PID中P是比例环节、I是积分环节、D是微分环节线性组合的总体。其控制流程原理图如图3-1所示.输入量通过PID控制器作用于被控对象，然后检测被控对象的输出值并把输出值与设定的目标值进行比较从而形成闭环负反馈系统。图3-1PID控制系统原理图在PID控制系统中e（t）是偏差值最后实际值与实际输出值y（t）的差值，其数学表达式为ut=Kpe公式中Kp是比例系数，T1是时间积分常数，TD是时间微分常数。由此可见PID控制是有三方面分别进行矫正的。比例部分就是通过检测偏差值，并进行相关的控制来减少偏差值，积分环节主要作用是去除静态误差，而微分部分就对超前校正即提前加入一个矫正信号，从而加快系统调节速度。上述公式所描述为连续型的函数，但是在实际使用中都是离散型的数据。下面我介绍两种离散型的PID控制方法。1.1.1位置型PID算法在这个方法中把时间常数用采样时刻序列来进行代替，把微分用求和来进行代替，把积分用向后差分来代替，在采样频率足够高时（即采样时间很短），即可得到离散化后的数学公式：uK=Kp1.1.2增量式PID算法增量式PID是指数字控制器的输出只是控制量的增量Δu(k)。采用增量式算法时，计算机输出的控制量Δu(k)对应的是本次执行机构位置的增量，而不是对应执行机构的实际位置，因此要求执行机构必须具有对控制量增量的累积功能，才能完成对被控对象的控制操作。执行机构的累积功能可以采用硬件的方法实现；也可以采用软件来实现，如利用算式

u(k)=u(k-1)+Δu(k)程序化来完成。Δu(k)=KP[e(k)-e(k-1)]+k1e(k)+KD[e(k)-2e(k-1)+e(k-2)](3-3)两者的区别：（1）位置式PID控制的输出与整个过去的状态有关，用到了误差的累加值;而增量式PID的输出只与当前状态和前两次状态的误差有关，因此位置式PID控制的累积误差相对来说更大;（2）增量式PID控制输出的是控制量增量，没有积分作用，因此该方法适用于执行机构带积分部件的对象，如步进电机等，但位置式PID适用于执行机构不带积分部件的对象，如电液伺服阀。（3）由于增量式PID输出的是控制量增量，如果计算机出现故障，其误动作影响较小且执行机构本身有记忆功能恢复后可保持原位不变，不会严重影响系统的工作，但位置式的输出与对应对象的输出有着直接关系，因此对系统影响比较大。基于以上三点情况我们在使用PID算法的一般偏向于使用增量型PID算法。1.2Kalman滤波简介滤波的含义是传感器除了产生我所需要的信号外还有含有别的杂波信号但是这些杂波信号不是我所需要的所以我们需要去除那些杂波信号这个过程就叫做滤波。通常来说滤波指的是将不需要的信号通过一定的硬件或者软件算法进行过滤去除，留下或放大我自己所需要的目标信号。杂波信号也可以称作为称作噪声，它对于整个系统有着一定干扰作用使得系统偏离了自己的预期。滤波理论建立的基础是测量得到系统的可以观测的信号，然后种用某种方法对状态进行估计计算。这种滤波算法的应用十分广泛，在理论研究上卡尔曼滤波是建立在系统是线性的以及系统中的状态变量和噪声干扰都是呈高斯分布的45。但是在实际生活中要满足这两个条件非常的困难，这也因此限制了它在现实生活中的应用。为了解决它的这两现在，

研究人员提出了扩展卡尔曼滤波（EKF）这一思想方法。EKF的思想方法是这样的，假设卡尔曼滤波对系统当前的值估计的误差非常小和真实值比较接近。那么我们可以对非线性系统的当前的估计值处进行泰勒展开实现局部线性化。但是泰勒展开线性化本身就有一定的误差从而导致EKF存在一定误差。卡尔曼滤波在本质上利用不断递推运算，处理数据的原则是使误差的均方值为最小，根据前一组数据的估计值和观察数据的估计值来估计当前采集信息。卡尔曼滤波算法是优点非常突出:1）在使用卡尔曼滤波时，我们对观测数据个数的要求不高，有限的观测数据便可以进行相关递推运算。2)卡尔曼滤波算法中的时间域语言是非常直观的，在算法编写方面它的物理意义表述的十分清楚便于理解。3)卡尔曼滤波运算中的中间变量数目相对于其他滤波算法来说要少，这也便于计算机进行处理，加快了计算机处理的速度。2软件设计部分2.1两轮自平衡小车主程序设计两轮自平衡小车的主程序设计就是根据相关的模块，编写系统的总体程序框架。以主程序为主体各个子程序按照其功能依次被调用，其程序流程图如下图4-1所示，首先初始化各个模块声明，自定义声明，系统时钟的声明等，在所有的初始化完成之后，就可以通过编码采集小车的速度数据和角度的数据并对数据进行相关的滤波处理从而减小数据的误差或者错误。然后进行PID控制通过PWM来控制电机的转速从而调节小车的速度使小车达到保持平衡。在这个整个控制流程中我使用的主控芯片是STM32f103C8T6，位姿检测模块为MPU-6050，两者通过I2C进行通信传输数据，主控芯片从而获得小车的车身倾角和小车的加速度，而小车的实时车速则是通过编码器采集到的信息转化为小车的车速来进行功能实现的，左右电机分别都有一个编码器可以独立读取单个电机的速度从而实现小车的平衡。图4-1自平衡小车主控制程序流2.2介绍所使用的软件本次毕业设计中软件部分程序编写和编译用到的是keil5软件，下载所用的软件名为Fly。Keil软件平台是非常强大的微型控制器软件编译平台，从我们以前学的51单片机和现在STM32单片机都是在这上面进行的编程，它有着很强大的功能，它把程序编写、编译、调试、下载集成为一体。相比于其他软件操作起来更加简单，伸手广大师生及业界相关工作者的喜爱和欢迎。并且它还有一下几大特征。1.源代码编辑器功能很多十分的齐全且还有代码语法实时检测功能十分的方便和省心。2.该软件使用人数较多在遇到很多不懂的软件错误时，可以快速的找到相关的解决方法。3.该软件有官方提供的入门教程指导，相对其他软件来说上手更加的方便。2.该软件平台可以创建工程使得文件可以更好的阅读和维护我们已经写好的工程文件。2.3PID控制算法在这次实验中实验了速度环和控制环即PD算法和PI算法speed=Mean_Filter(velocity_left,velocity_right); Encoder*=0.7; Encoder+=speed*0.3; Encoder_Integral+=Encoder; if(Encoder_Integral>320000) Encoder_Integral=320000; if(Encoder_Integral<-320000) Encoder_Integral=-320000; Velocity=Encoder*Velocity_Kp/100+Encoder_Integral*Velocity_Ki/100;这个时PI控制器的核心算法，我先读取我们滤波完后的速度，然后对取的速度再进行一次低通滤波保证速度的准确性，然后进行在进行积分得到积分值，并给积分值设置一个幅值。我再根据使用位置式PI算法，其公式为u(t)=Kpe(k)+KiTe(k)dt（4-1）可得Velocity=Encoder*Velocity_Kp/100+Encoder_Integral*Velocity_Ki/100;其中KP和KI为比例系数而小车原来使用的是增量型的PID算法，他相对来说考虑的更加的周全它的偏差相对来说更小，但是我也在上面经过两次滤波也减小了其误差发生的概率。因为我用的是位置型算法，所以其滤波是我才采用了一阶滞后滤波的方法来进行调节。不会使得系统过于滞后。PD算法因为有些参数与结构有关就没有进行多加修改使用的是它的源代码。2.4滤波算法对速度的滤波我采用的是中位值平均滤波法，它的核心是兼顾了“中位值滤波法”+“算术平均滤波法”两种滤波法的优点，而缺点仅仅是计算量会大一点。相对于小车自带的平均值法来说准确性更好也显得更加平滑，核心代码如下所示。s32Sum_Speed=0;s16Filter_Speed;statics16Speed_Buff[FILTERING_TIMES]={0};//存储前面十个速度值for(i=1;i<FILTERING_TIMES;i++){Speed_Buff[i-1]=Speed_Buff[i];}Speed_Buff[FILTERING_TIMES-1]=moto1+moto2;for(i=0;i<FILTERING_TIMES;i++){Sum_Speed+=Speed_Buf[i];}Filter_Speed=(s16)(Sum_Speed/FILTERING_TIMES);2.5数据融合算法2.5.1卡尔曼滤波MPU-6050的噪声非常的大，所以我需要进行滤波处理才能去读取数据，我参考了小车的源代码发现，它使用的是DMP算法他的方法相对独特，我取网上查找了相关资料发现网上对于MPU-6050都是使用的Kalman滤波算法，于是我决定使用卡尔曼滤波算法，在经过一系列的调试后小车能够读取到传感器的数据但是相比于它的算法我的小车在直立运行的稳定性和小车从不平衡到平衡所需的时间更长。不过这种算法还是可以使用的，后面还要进行一定能的优化我会在后在接着进行相关的调试。Kalman代码如图16所示。voidKalman_Filter(floatAccel,floatGyro) {angle+=(Gyro-q_bias)*dt;//先验估计Pdot[0]=Q_angle-PP[0][1]-PP[1][0];//Pdot[1]=-PP[1][1];//PK-先验估计误差协方差的微分Pdot[2]=-PP[1][1];Pdot[3]=Q_gyro;PP[0][0]=PP[0][0]+Pdot[0]*dt;//PK-先验估计误差协方差的微分的积分PP[0][1]=PP[0][1]+Pdot[1]*dt;//先验估计误差协方差PP[1][0]=PP[1][0]+Pdot[2]*dt;PP[1][1]=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_0K_0=PCt_0/E;K_1=PCt_1/Et_0=PCt_0;t_1=C_0*PP[0][1];PP[0][0]=PP[0][0]-K_0*t_0; //后验估计误差协方差PP[0][1]=PP[0][1]-K_0*t_1;PP[1][0]=PP[1][0]-K_1*t_0;PP[1][1]=PP[1][1]-K_1*t_1; angle+=K_0*Angle_err; //后验估计q_bias+=K_1*Angle_err; angle=Gyro-q_bias; //输出值（后验估计）的微分等于角速度}2.5.2一阶互补滤波一阶互补滤波说的是如果我们在测量两个量时，一个量是高频杂波信号另一个量是低频杂波信号那么我们就可以设计一个高通滤波器和一个低通滤波器然后对这两个滤波器进行加权然后进行求和处理;Y=αx1+(1-α)x2(1)(4-1)MPU6050中的陀螺仪和加速度计刚好一个是含有低频杂波信号一个是含有高频杂波信号所以刚好可以使用一阶互补滤波算法来读取角速度和倾角。一阶互补滤波算法的代码为：angle=K1*angle_m+(1-K1)*(angle+gyro_m*0.005);其中K1为加权系数。这种算法相比于卡尔曼滤波来说小车的不稳定性更大，保持平衡相对来说更加的困难，但是也可以保持平衡。2.5.2DMP（数字运动处理器）MPU6050本身就一个数字运动处理器DMP，它可以直接输出小车的姿态解数据而不需要我们自己再去求取小车的姿态解，它的算法是用的四元法，误差相对更小、计算量也减小许多而且官方提供相应的库文件以及教程从而大大降低了开发的难度。我们只需要在进行调用即可。其核心代码如下所示longquat[4];dmp_read_fifo(gyro,accel,quat,&sensor_t