??车模平衡控制也是通过负反馈实现的，与保持木棒直立相比相对简单。因为车模有两个车轮着地，车身只会在车轮滚动的方向倾斜。控制车轮旋转，抵消一个维度的倾斜趋势，保持车身平衡

    平衡车辆直立环使用PD(比例微分)控制器实际上是一般控制系统的简单P控制或PI控制就可以了，但是那些对干扰要做出迅速响应的控制过程需要D(微分)控制。 //形参:(float Angle):汽车的俯仰角 /(float Gyro):x轴的角速度 Int    Vertical_Ring_PD(float Angle, float Gyro) { 
           float Bias;   int balance;      Bias=Angle-Mechanical_balance;      /////在平衡角度找到中值 和机械相关    balance=PID.Balance_Kp*Bias    Gyro*PID.Balance_Kd; //计算直立PWM   return balance;           //回到直立PWm }  

分析：
Mechanical_balance一般取0，因为想平衡车，期望值的角度是0°，以汽车的俯仰角作为反馈，让汽车保持在0°周围。
角速度的积分等于角度，所以角度的微分是角速度
使用PD调节

E(k) = Bias
(E(k) - E(k-1)) = Gyro
PID.Balance_Kp 比例系数
PID.Balance_Kd 微分系数

??平衡车辆直立环的调试过程包括确定平衡车辆的机械中值和机械中值kp极性(即正负号)和大小，kd极性、大小等步骤。
1.确定平衡车的机械中值:
??将平衡车放在地面上，绕电机轴旋转平衡车，记录能使车接近平衡的角度，一般为0°附近。我们调试的车刚好是0度，所以是Bias=Angle-0
2. 确定kp极性值(令)kd=0）
??首先，我们估计kp取值范围。我们的PWM设置8000代表100%的空间，然后考虑避免电机死区，我们直立环返回PWM电机在6000左右就会满载。
??假如我们设定kp如果值800，平衡车在±10°当它满转时。根据我们的解，这显然太大了，所以我们可以估计kp值在0~800之间。
??首先，我们给一个值kp=-200，我们可以观察到汽车倒在哪里，电机会加速汽车下降，这是我们不想看到的积极反馈效果。kp值的极性反转了，接下来我们设置kp=在这个时候，我们可以看到平衡车有直立的趋势。虽然响应太慢，但我们可以肯定kp值是正的。下面仔细调试具体数据。
??设定kp=100时候，我们可以看到，虽然汽车有平衡的趋势，但显然响应有点慢。
??设定kp=在这个时候，我们可以看到，虽然汽车有平衡的趋势，反应加快，但总体感觉很好。
??设定kp=400,这个时候我们可以看到，小车的响应明显加快，而且来回推动小车的时候，会有一定幅度的低频抖动。说明这个时候kp值足够大，需要增加微分控制来削弱p控制，抑制低频抖动。
??整体比较： 我们选择参数为kp = 200；
??我们得到的MPU通过观察6050输出陀螺仪的原始数据，我们发现最大值不会超过4位数，然后根据8000代表占100%，所以我们估计kd值应该在0~2之间
??我们先设定kd=-0.5.当我们拿起汽车旋转时，车轮会反转，无法达到跟随效果。这说明了kd极性反转。
??接下来，我们设置kd=0.5.此时，我们可以看到，当我们旋转汽车时，车轮以相同的速度旋转，这表明我们实现了角速度闭环。到目前为止，我们可以肯定kd极性是正的。下一步仔细调试具体数据。
最后，我们选择kd = 1;

确定kp的范围：
  积分项由偏差的积分得到,所以积分控制和比例控制的极性相同的,而根据工程经验,在不同的系统中,PID 参数相互之间会有一定的比例关系。在我们的平衡小车速度控制系统里面,一般我们可以把ki 值设置为
  ki = kp/200
  这样,只要我们可以得到kp 值的大小和极性,就可以完成速度控制部分的参数整定了。显然,这样大大缩短了PID 参数整定的时间。
  我们通过STM32定时器的编码器接口模式对编码器进行四倍频,并使用M 法测速(每10ms 的脉冲数)得到小车的速度信息,通过观察数据,我们发现两路编码器相加最大值在160左右,而由经验可知,一般平衡小车行驶的最快速度不会超过电机最大速度的40%,再根据PWM = 6000时，在加上电机死区、占空比接近100%,我们可以大概估算
  kp 最大值=6000/(160*40%)=93.75
确定速度环调节为正负馈的调节：
  当小车以一定的速度运行的时候,我们要让小车停下来,小车需要行驶更快的速度去“追”,小车运行的速度越快,去“追”的速度也就越快,所以这是一个正反馈的效果。如果使用常规的速度负反馈,当小车以一定的速度运行的时候,我们通过减速让小车慢下来,小车会因为惯性向前倒下。
判断速度控制是正反馈还是负反馈：
  根据之前的估计,先设定kp=50,ki=kp/200,当我们拿起小车,旋转其中一个小车轮胎的时候,根据我们设定的速度偏差
  Encoder_Least =(Encoder_Left+Encoder_Right)-0;
  另外一个车轮会反向转动,让偏差趋向于零。这就是常规的速度控制里面的负反馈,不是我们需要的效果。接下来设定kp=-50,ki=kp/200,此时,当我们旋转其中一个小车轮胎的时候,两个轮胎会往相同的方向加速,直至电机的最大速度,这是典型的正反馈效果,也是我们期望看到的。至此,我们可以确定kp,ki
  首先,设定kp=-30,ki=kp/200这个时候我们可以看到,小车的速度控制比较弱,很难让速度恒定。
  设定kp=-50,ki=kp/200这个时候我们可以看到,小车的速度控制的响应有所加快, 静止抖动可接受。
  设定kp=-70,ki=kp/200这个时候我们可以看到,小车虽然回正力度增大了,而且响应更加快了,但是稍微加入一点的干扰都会让小车大幅度摆动,抗干扰能力明显不足,所以这组参数不可取。

  转向环是最不重要的一环，所以网上资料都是同样的，摸棱两可没说清楚，自己整理了一下
  如果需要转向，其实需要左轮右轮有一个速度差就行了
  转向是对六轴传感器的z轴的旋转，就是偏航角作为反馈信息
  PD环节可能有些同学看不懂，我们一个一个来看

E(k)　　　　　：是期望值和反馈值的差
E(k) - E(k-1)　:　是E(k)的微分
带进转向环
E(K) = (期望的角度 -反馈的角度 )
E(k) - E(k-1) 就是角度的微分就是角速度
注：角速度的积分等于角度、那么角度的微分就是角速度

int Vertical_turn_PD(short yaw)
{ 
        
		float Turn;     
	    float Bias;	  
	  Bias=90-Yaw;
	  Turn=-Bias*PID.Turn_Kp-yaw*PID.Turn_Kd;
	  return Turn;
}

Bias=90-Yaw;
90是我期望的角度
Yaw是当前的偏航角
这样就能完成原地转向90°的算法了
但是一般人脸识别，CCD模块中我们是不会这样去算的，
Bias直接取差值就行

/************************************************************************************************************** *函数名:Vertical_turn_PD() *功能:转向环PD *形参:无 CCD小于64左转、CCD大于64右转。 yaw = z轴陀螺仪数值 *返回值:无 ***************************************************************************************************************/
int Vertical_turn_PD(u8 CCD,short yaw)
{ 
        
		float Turn;     
	    float Bias;	  
	  Bias=CCD-64;
	  Turn=-Bias*PID.Turn_Kp-yaw*PID.Turn_Kd;
	  return Turn;
}

Bias=CCD-64
64是ccd模块的中值