PD算法是如何去控制平衡小车的呢

　　一、平衡小车原理
　　平衡小车是通过两个电机运动下实现小车不倒下直立行走的多功能智能小车，在外力的推拉下，小车依然保持不倒下。
　　一是放在指尖上可以移动，二是通过眼睛观察木棒的倾斜角度和倾斜趋势（角速度）。通过手指的移动去抵消木棒倾斜的角度和趋势，使得木棒能直立不倒。这样的条件是不可以缺一的，实际上加入这两个条件，控制过程中就是负反馈机制。
　　平衡小车也是这样的过程，通过负反馈实现平衡。与上面保持木棒直立比较则相对简单，因为小车有两个轮子着地，车体只会在轮子滚动的方向上发生倾斜。控制轮子转动，抵消在一个维度上倾斜的趋势便可以保持车体平衡了。
　　所以根据上述原理，通过测量小车的倾角和倾角速度控制小车车轮的加速度来消除小车的倾角。因此，小车倾角以及倾角速度的测量成为直立小车的关键。
　　测量倾角和倾角速度的集成传感器陀螺仪-MPU6050
　　二、角度（物理分析 PD算法）
　　
　　
　　控制平衡小车，使得它作加速运动。这样站在小车上（非惯性系，以车轮作为坐标原点）分析倒立摆受力，它就会受到额外的惯性力，该力与车轮的加速度方向相反，大小成正比。这样倒立摆（如图2）所受到的回复力为：公式1F = mg sin θ-ma cos θ≈mg θ-mk1θ式1中，由于θ很小，所以进行了线性化。假设负反馈控制是车轮加速度a与偏角θ成正比，比例为k1。如果比例k1》g，（g是重力加速度）那么回复力的方向便于位移方向相反了。
　　而为了让倒立摆能够尽快回到垂直位置稳定下来，还需要增加阻尼力。增加的阻尼力与偏角的速度成正比，方向相反，因此公式1可改为：
　　F = mg θ-mk1 θ -mk2 θ‘
　　按照上述倒立摆的模型，可得出控制小车车轮加速度的算法：
　　a =k1θ+k2θ’ 式中θ为小车角度，θ`为角速度。k1 k2都是比例系数
　　根据上述内容，建立速度的比例微分负反馈控制，根据基本控制理论讨论小车通过闭环控制保持稳定的条件（这里需要对控制理论有基本了解）。假设外力干扰引起车模产生角加速度x（t）。沿着垂直于车模地盘方向进行受力分析，可以得到车模倾角与车轮运动加速度以及外力干扰加速度a（t）x（t）之间的运动方程。如图3所示。
　　
　　在角度反馈控制中，与角度成正比例的控制量是称为比例控制
　　与角速度比例的控制量称为微分控制（角速度是角度的微分）。
　　因此上面的系数k1，k2分别称为比例和微分控制参数。其中微分参数相当于阻尼力，可以有效抑制车模震荡。通过微分抑制控制震荡的思想在后面的速度和方向控制中也同样适用。
　　总结控制车模直立稳定的条件如下：
　　（1）能够精确测量车模倾角θ的大小和角速度θ‘的大小
　　（2）可以控制车轮的加速度。
　　上述控制实际结果是小车与地面不是严格垂直，而是存在一个对应的倾角。在重力的作用下，小车朝着一个方向加速前进。为了保持小车的静止或者均速运动需要消除这个安装误差。在实际小车制作过程中需要进行机械调整和软件参数设置。另外需要通过软件中的速度控制来实现速度的稳定性。在小车角度控制中出现的小车倾角偏差，使得小车在倾斜的方向上产生加速。这个结果可以用来进行小车的速度控制。下面利用这个原理来调节小车的速度
　　三，测速（物理模型 建立数学模型 传递函数PD算法）
　　假设小车在上面直立控制调节下已经能够保持平衡了，但是由于安装误差，传感器实际测试的角度与车模角度有偏差，因此小车实际不是保持与地面垂直，而是存在一个倾角。在重力的作用下，小车就会朝倾斜的方向加速前进。控制速度只要通过控制小车的倾角就可以实现了。
　　具体实现需要解决三个问题
　　（1）如何测量小车的速度？·
　　（2）如何通过小车直立控制实现小车倾角的改变？
　　（3）如何根据速度误差控制小车倾角？
　　一，第一个问题，可以通过安装在**电机输出轴上的光码盘**来测量得到小车的车轮速度。利用控制单片机的计数器测量在固定时间间隔内脉冲信号的个数可以反馈电机的转速。
　　
　　二，第二个问题，可以通过角度控制给定值来解决。给定小车直立控制的设定值，在角度控制调节下，小车将会自动维持在一个角度。通过前面小车直立控制算法可以知道，小车倾角最终是跟踪重力加速度Z轴的角度。因此小车的倾角给定值与重力加速度Z轴角度相减，便可以最终得到小车的倾角。
　　三，第三个问题，析起来相对比较困难，远比直观进行速度负反馈分析复杂。首先对一个简单例子进行分析。假设小车开始保持静止，然后增加给定速度，为此需要小车往前倾斜以便获得加速度。在小车直立控制下，为了能够得到一个往前的倾斜角度，车轮需要往后运动，这样会引起车轮速度下降（因为车轮往负方向运动了）。由于负反馈，使得小车往前倾角需要更大。如此循环，小车很快就会倾倒。原本利用负反馈进行速度控制反而成了“正”反馈。
　　为什么负反馈控制在这儿失灵了呢？
　　原来在直立控制下的小车速度与小车倾角之间传递函数具有非最小相位特性，在反馈控制下容易造成系统的不稳定性。
　　。
　　。
　　为了保证系统稳定，往往取的小车倾角控制时间常数Tz很大。这样便会引起系统产生两个共轭极点，而且极点的实部变得很小，使得系统的速度控制会产生的震荡现象。这个现象在实际参数整定的时候可以观察到。那么如何消除速度控制过程中的震荡呢？这样便会引起系统产生两个共轭极点，而且极点的实部变得很小，使得系统的速度控制会产生的震荡现象。这个现象在实际参数整定的时候可以观察到。
　　那么如何消除速度控制过程中的震荡呢？
　　要解决震荡问题，在前面的小车角度控制中已经有了经验，那就是在控制反馈中增加速度微分控制。但由于车轮的速度反馈信号中往往存在着噪音，对速度进行微分运算会进一步加大噪声的影响。为此需要对上面控制方法进行改进。原系统中倾角调整过程时间常数往往很大，因此可以将该系统近似为一个积分环节。将原来的微分环节和这个积分环节合并，形成一个比例控制环节。这样可以保持系统控制传递函数不变，同时避免微分计算。
　　但在控制反馈中，只是使用反馈信号的比例和微分，没有利用误差积分，所以最终这个速度控制是有残差的控制。但是直接引入误差积分控制环节，会增加系统的负杂度，为此不再增加积分控制，而是通过与角度控制结合在进行改进。
　　要求小车在原地停止，速度为0.但是由于采用的是比例控制，如果此时陀螺仪有漂移，或者加速度传感器安装误差，最终小车倾角不会最终调整为0.小车会朝着倾斜方向恒速运动下去。注意此时车模不会像没有速度控制那样加速运行下去了，但是速度不会最终为0。为了消除这个误差，可以将小车倾角设定量直接积分补偿在角度控制输出中，这样就会彻底消除速度控制误差。第二点，由于加入了速度控制，它可以补偿陀螺仪和重力加速度的漂移和误差。所以此时重力加速度传感器实际没有必要了。
　　此时小车在控制启动的时候，需要保持小车的垂直状态。此时陀螺仪的积分角度也初始化为0。当然如果电路中已经包括了重力加速度传感器，也可以保留这部分，从而提高小车的稳定性。在后面的最终给定的控制方案中，保留了这部分的控制回路。
　　四，转向控制（PD算法）
　　通过左右电机速度差驱动小车转向消除小车距离路中心的偏差。通过调整小车的方向，再加上车前行运动，可以消除小车距离中心线的偏差。这个过程是积分的过程，因此小车差动控制一般只需要进行简单的比例控制就可以完成小车的方向控制。但是由于小车装有电池等重物体，具有很大的转到惯性，在调整过程中会出现小车转向过冲现象，如果不加以抑制，会使得小车过度转向而倒下。根据前面角度和速度控制的经验，为了消除小车方向控制中的过冲，需要增加微分控制。
　　五，全方案整合
　　
　　为了实现小车直立行走，需要采集如下信号：
　　（1）小车倾角速度陀螺仪信号，获得小车的倾角和角速度。
　　（2）重力加速度信号
　　（z轴信号）补偿陀螺仪的漂移，该信号可以省略，有速度控制替代。
　　（3）小车电机转速脉冲信号，获得小车运动速度，进行速度控制
　　（4）小车转动速度陀螺仪信号，获得小车角速度，进行方向控制
　　在小车控制中的直立，速度和方向控制三个环节中，都使用了比例微分（PD）控制，这三种控制算法的输出量最终通过叠加通过电机运动来完成
　　（1）小车直立控制：使用小车倾角的PD（比例，微分）控制；
　　g_fAngleControlOut = g_fCarAngle* g_fCarAngle_P + gyro［0］ *g_fCarAngle_D；
　　（2）小车速度控制：使用PD（比例，微分）控制：
　　g_fSpeedControlOutNew = （CAR_SPEED_SET - g_fCarSpeed） * g_fCarSpeed_P+ （CAR_POSITION_SET - g_fCarPosition） * g_fCarSpeed_I;
　　（3）小车方向控制：使用PD（比例、微分）控制。
　　speednow=-speedtarget*3.4 -gyro［2］*0.0015 ; 在上面控制过程中，车模的角度控制和方向控制都是通过直接将输出电压叠加后控制电机的转速实现的。而车模的速度控制本质上是通过调节车模的倾角实现的，由于车模是一个非最小相位系统，因此该反馈控制如果比例和速度过大，很容易形成正反馈，使得车模失控，造成系统的不稳定性。因此速度的调节过程需要非常缓慢和平滑。
　　六，PID算法
　　
　　AngleCalculate：小车倾角计算函数。根据采集到的陀螺仪和重力加速度传感器的数值计算小车的角度和角速度。如果这部分的算法由外部一个运放实现，那么采集得到的直接是小车的角度和角速度，这部分算法可以省略。该函数每5ms调用一次。
　　AngleControl：小车直立函数。根据小车的角度和角速度计算小车电机的控制量。直立是5ms调用一次。
　　SpeedControl：小车速度控制函数。根据小车采集到 的电机转速和速度设定值，计算电机的控制量。该函数每40ms调用一次
　　MotorOutput：电机输出量汇集函数。根据直立函数，速度控制和方向控制所得到的控制量进行叠加，分别得到左右两个电极的输出电压控制量。对叠加后的输出量进行饱和处理。函数每5ms调用一次。在此请大家注意速度控制量叠加的极性是负。
　　MotorSpeedOut：电机PWM输出计算函数。根据左右两个电极的输出控制量的正负极性，叠加上一个小的死区数值，克服车模机械静态摩擦力。函数调用周期5毫秒。
　　SetMotorVoltage：PWM输出函数：根据两个电机的输出量，计算出PWM控制寄存器的数值，设置四个PWM控制寄存器的数值。函数调用周期1毫秒。
　　以上9个函数都是在1毫秒中断服务中进行被相互调用的。下图显示了这些函数之间的调用与参数传递关系。在个函数附近也表明了调用的周期。
　　Chaoshengbo：加入超声波壁障模块：根据前方障碍物的距离检测，一旦检测到后，通过直接PWM值输出（g_fchaoshengbooutput），相障碍物反方上运动，无需算法实现。每30毫秒调用一次。
　　七，程序
　　（1）时序总算法
　　（2）平衡程序
　　速度环控制函数
　　（3）蓝牙以及超声波
　　/***************************************************************
　　** 函数名称： BluetoothControl
　　** 功能描述： 蓝牙控制函数
　　手机发送内容
　　前：0x01 后：0x02
　　左：0x04 右：0x03
　　停止：0x07
　　功能键：（旋转）
　　左旋转：0x05 右旋转：0x06
　　停转：0x07
　　** 输　入：
　　** 输　出：
　　***************************************************************/
　　提供电路图，按模块进行电路分析
　　
　　铝电池10V转5V，DC-DC转换
　　
　　一，从电路图看Q1为SI 2306此为N mos管，当Vgs》0时则三极管导通，G极为电池VM端为12V，S极为电池负极，开关没开时，Q1 N型mos管的S极为接GND，G极没有电压则为0V，则Vgs=Vg-Vs=0V-0v=0V，此时N管不导通；当电源打开时，Q1的G极为12V，则Vg-Vs=12v-0=12v》0，则N mos管导通接地；Q1启到保护电池作用，我猜测这个CDS-2P卧式容易短路，如果短路容易造成电池回路然后烧电池；将GND放在Q1mos管下面可以在电源打开时，CDS-2P的GND才会接地;我认为CDS-2P的GND处没有真正的接地，真正接地的是Q1 N型mos管的D极
　　二，SS34肖特基二极管，防止当N管是开状态时，电池反灌。
　　三，S1我也不知道什么来的，应该是电池保护芯片 ，SW输入，OUT输出，D1灯的作用是为了，灯亮时说明有电通过，灯灭时无电通过。
　　四，芯片78M05是DC-转DC5v的芯片，Q2为Nmos管当Vgs》0时导通，芯片78M05VOUT为5V，则Q2的S极为5V，Q2的G极为ON应该是电池端（ON不是电池端，这里是ON低压保护ON=5.78V，后面分析），则开机时Vgs=Vg-Vs=12v-5v》0，则Q2导通（注：Vgs=5.78v-5v》0不影响分析，后面给出解释），芯片通过Q2输出5V
　　五，D2灯的作用：芯片是否有5V供电
　　低压保护电路
　　
　　一，D3为肖特基二极管，VM经过后压降约0.3v-0.4v则12v-0.3v=11.7v;C4为滤波电容，作用是电源滤波，R4的作用是电源关闭时，C4与R4形成回路，C4放电。感觉R4可以去掉，C4本身形成回路状态也可以放电。
　　二，R5与R6分压，此处的电路图画错，IN端电压=VM*（R6/（R5+R6））=11.7*（3.9/（10+3.9））=3.28V。