本人是参加过三年（也可以说是两年）NX智能车竞赛的老菜鸡了，做的都有直立车。感觉大学就要过去了，是该给自己的大学做一些留念与总结了，回顾自己这几年也没有什么拿的，就拿直立说说事。第一次写博客，写的可能不是特别好，欢迎各位大佬指导交流。

直立车控制原理分析对比

直立车的控制分为两种控制方式。一种为传统的并行控制理论，直立环(5ms)+速度环(10ms)，更短的控制周期其控制效果会相应的变好；另一种是从四轴飞行器上衍生移植而成的串级控制理论，速度环(8ms)——>角度环(4ms)——>角速度环(2ms),各个PID控制的时间周期因人而异，时间遵循最内环控制周期最短、最外环控制周期最长的原则。

感觉传统的控制方案里的速度环是最让人头痛的。众所周知，闭环反馈调节可分为两种反馈调节，一种是平常大家熟悉的负反馈调节，还有一种就是正反馈调节了。传统方案的速度环的反馈是正反馈调节，感觉有点违背正常人思考的理念，刚接触的可能就会一脸懵逼。以温度调节为例子，要维持恒温的话，温度高了就要降温，温度低了就要升温，这就是正常的负反馈调节。正反馈就是，温度高了不仅不降温，反而要升温。因为这个原因，传统的速度环就没有那么好调试。而反观串级控制的速度环，它只是普通的负反馈的而已，让人能够更加容易理解。

个人感觉对比两种控制方案都是殊途同归，让我非要分一个优劣的话我会选择后一种。

传统控制方式

传统的控制方式比较简单，只有两个环，直立环只有单独一个而已，所以直立环的调试会更加的简单，但是速度环的调试是比较烦人的。因为他是并行控制，两个环无关的。但是实际上的话两个环都会作用在同一对电机上，相互之间的影响特别大。如果只是想要单纯的立起来的话传统的控制方式不失为一个不错的选择，但是一旦需要较好的速度控制的话则串级控制会更加好。

//直立环----位置式PID
float balance_pid(float Angle, float GYRO)
{
    float pwm；
    pwm=Kp*(Angle-balance_angle) + Kd*GYRO；
    return pwm;
}

//速度环----位置式PID
float speed_pid(float Speeed_Left,float Speed_Right)
{
        static float Speed_Error_last,Speed_Integral;
        float Speed_Error,Velocity；
        
       //计算速度偏差
        // speed_target为自己设定的速度目标值
        Speed_Error=(Speeed_Left+Speed_Right) - speed_target;      
        
        //速度低通滤波
        Speed_Error *= 0.8;		                                             
        Speed_Error += Speed_Error_last*0.2;	        
        
        //更新上次速度的值  
        Speed_Error_last =  Speed_Error ;           
        
        //速度积分    
        Speed_Integral+=Speed_Error ;                   
        
        //积分限幅               
        if(Speed_Integral>4000)  	Speed_Integral=4000;          
        if(Speed_Integral<-4000)	Speed_Integral=-4000;            
         
        Velocity=Speed_Error*speed_Kp+Speed_Integral*speed_Ki;                       
        return Velocity;
}

参数调试的话可以参照平衡小车之家的平衡车调试指南来进行相应的调试，里边写的还是比较好的哦https://wenku.baidu.com/view/3e5fc765bb4cf7ec4bfed00b.html

串级控制方式

串级控制方式能让小车的控制会更加稳定，但是相应的过多的参数可能会使不熟悉串级参数调试的人感觉更加复杂（参数调试的下文再说），但是前一种的话直立环只有PD两个参数，直立环的调试会更加简单。也有人说串级参数的调试更加简单，其实我感觉两种控制方式的差不多而已。串级的话单单一个角速度环就能让小车能挺不错的立住了，调试也是从最内环（角速度环）开始调的，最后调的才是最外环。角速度环已经能让车立住了那还需要其他的环为什么呢。个人理解，角速度环参照代码可知是让车保持角速度为0的，但是小车在机械平衡位置角速度为0才是正常的直立，如果是在其他位置保持角速度为0的话就不能保持平衡。所以角度环的作用就是控制角速度环在什么角度去保持角速度为0，一般我们要他保持的角度就是机械平衡的角度值了，所以这两个环能让小车稳稳的直立住了。最外环——速度环的作用毫无疑问就是保持速度了。前两个环调试完毕之后小车就能保持直立了，但是如果想让他跑的话就需要打破当前的平衡状态。不难理解，直立车要跑起来就会低头与抬头，反映到角度上就是角度的变化，所以速度环的输出一个微小的角度作为角度环的一个输入。简单的说就是让小车以什么速度来保持直立。三个环的共同作用就能实现小车的直立控制。

角速度环的话本人用的是位置PID,第一年我用到了PID三个参数,但是个人调试过程中发现D对整车的影响并不大，所以在第二年的话只使用到了PI而已。有些人的话角速度环只用到了PD而已，放弃使用积分量，个人感觉积分量是比较重要的，积分对小车立起来起到非常大的作用，但是过大的积分量又会破坏系统的平衡，所以其中的取舍要看个人而已。

//角速度环----位置式PID
//Gyro_Data——陀螺仪获取的自身实际角速度、
//Gyro_m——角度环输出的值，作为角速度环的输入
float Gyro_PID(float Gyro_Data,float Gyro_m)
{
      static float Gyro_Error_Count;
      float Realize,Gyro_Error;
      
      //偏差计算
      Gyro_Error = Gyro_m - Gyro_Data;
      
      //积分
      Gyro_Error_Count += Gyro_Error;
      
      //积分饱和限幅
      if(Gyro_Error_Count>10000) Gyro_Error_Count=10000;
      else if(Gyro_Error_Count<-10000) Gyro_Error_Count=-10000;
      
      //位置PID计算
      Realize = Gyro.P * Gyro_Error +  Gyro.I * Gyro_Error_Count;
     
      return Realize;
}

角度环的控制基本都是采用的PD控制，我看过很多人也都是如此，这没有什么争议。

//角度环——位置式PID
//Angle_Data——互补滤波计算出的实际角度
//Angle_m——速度环的输出，作为角度环的一个输入
//balance_angle——小车自身的机械平衡角度
float Angle_PID(float Angle_Data,float Angle_m，float balance_angle)
{
      static float Angle_Error_Least;
      float Angle_Error，Realize;
      
      //偏差计算
      Angle_Error = Angle_m - Angle_Data - balance_angle;
      
      Realize = Angle.P * Angle_Error + Angle.D * (Angle_Error - Angle_Error_Least);
      
      //更新上一次的偏差
      Angle_Error_Least = Angle_Error;
      
      return Realize;
}

速度环的话很多人只用到了P，而第一年本人用到了PI，第二年只用了P。对比单P控制与PI控制效果感觉差别不大，单P就能够很好的控制速度了，I的加入只是更稳定而已，同样的积分的过饱和反而会破坏平衡起到反效果，建议慎用！！！一般都会对速度环的做种输出值做一个限幅保护，防止给角度环的值过大。

//速度环——位置式PID
//Speed_L——左轮的速度
//Speed_R——右轮的速度
//Speed_m——设定的目标速度
float Speed_PID(float Speed_L,float Speed_R,float Speed_m)
{
      static float Speed_Error_Integral;
      float Speed_Error;Realize,Speed_New;

	  //计算左右电机的平均速度
      Speed_New = (Speed_L + Speed_R)/2;
      
      //偏差计算
      Speed_Error = Speed_m - Speed_New;
      
      //积分的计算
      Speed_Error_Integral+= Speed_Error;
      if(Speed_Error_Integral>=4000) Speed_Error_Integral=4000;
      if(Speed_Error_Integral<=-4000) Speed_Error_Integral=-4000;
      
      Realize = Speed.P * Speed_Error + Speed_Error_Integral* Speed.I ;
      
      if(Realize>30) Realize=30;
      if(Realize<-30) Realize=-30;
      
      return Realize;
}

参数调试，先调试角速度环，再调试角度环，最后调试速度环，调试的时候无关的环可以设置参数为0。角速度的PI均是让小车立起来的，单P无法立住，单I的话可以勉强立一下，但是现象就会是前后拼命摇晃。P项反映灵敏快速，I项由于是积分，所以I项反映一般会滞后，但是I项的稳态效果特别好，可按效果对参数修改。要注意的是因为没有角度环的参与，角速度环只会在I项的积分

角度环的调试就比较简单了，单单角速度环已经能在一个位置立住了，角度环调试完毕的效果只是不需要人为的操作车模自己就能在原地立住，单单给一个参数P就能立的不错了。

速度环的调试则比较费劲了，它需要跑起来才能更好的看出参数的效果，如果只是设定速度为0，很难获得比较好的参数。

角度的获取

角度的获取主要则是三种方式而已，一是四元数，二是卡尔曼滤波，三是互补滤波。之前看过网上有人对比了三种方式获取的波形，几乎是没有差别的，本人用的是简单粗暴的互补滤波（线代困难户专属）来融合角度的。

首先说四元数，四元数的话就把陀螺仪和加速度计的值进行一系列的矩阵运算，最后获取去角度。线代不好的我果断放弃。大疆RM机器人上的角度用的就是四元数进行运算的，可能是比较好吧。个人对四元数了解不多。

其次说说卡尔曼滤波，他也是用的矩阵运算，但是网上都能随便找到他的例子以及源码，附录一下我自己用过的一段卡尔曼滤波的代码。

void Kalman_Filter_x(float Accel,float Gyro)

{

Angle_x+=(Gyro - Q_bias_x) * dt_x; //先验估计

Pdot_x[0]=Q_angle_x - PP_x[0][1] - PP_x[1][0]; // Pk-先验估计误差协方差的微分

Pdot_x[1]=- PP_x[1][1];

Pdot_x[2]=- PP_x[1][1];

Pdot_x[3]=Q_gyro_x;

PP_x[0][0] += Pdot_x[0] * dt_x; // Pk-先验估计误差协方差微分的积分

PP_x[0][1] += Pdot_x[1] * dt_x; // =先验估计误差协方差

PP_x[1][0] += Pdot_x[2] * dt_x;

PP_x[1][1] += Pdot_x[3] * dt_x;

Angle_err_x = Accel - Angle_x; //zk-先验估计

PCt_0_x = C_0_x * PP_x[0][0];

PCt_1_x = C_0_x * PP_x[1][0];

E_x = R_angle_x + C_0_x * PCt_0_x;

K_0_x = PCt_0_x / E_x;

K_1_x = PCt_1_x / E_x;

t_0_x = PCt_0_x;

t_1_x = C_0_x * PP_x[0][1];

PP_x[0][0] -= K_0_x * t_0_x; //后验估计误差协方差

PP_x[0][1] -= K_0_x * t_1_x;

PP_x[1][0] -= K_1_x * t_0_x;

PP_x[1][1] -= K_1_x * t_1_x;

Angle_x += K_0_x * Angle_err_x; //后验估计

Q_bias_x += K_1_x * Angle_err_x; //后验估计

Gyro_y_x = Gyro - Q_bias_x; //输出值(后验估计)的微分=角速度

}

最后再来说一下互补滤波，用的就是两个陀螺仪积分出来的角度与加速度计反三角函数算出的角度进行互补。有些人可能会说都算出有两个角度了，还那么麻烦的融合他干啥啊。陀螺仪跟加速度计都能计算出角度，但是都各自有着各自的问题。陀螺仪出来的角度因为是积分出来的，因为传感器误差存在的温漂、零飘等，长时间的积分就会导致角度偏移，与实际值完全对应不上。加速度计出来的角度因为自身原因算出来的角度会抖动非常的剧烈，所以也不能直接用。两者互补之后的波形则能满足我们的要求了。以下就是互补滤波的代码。

void get_data_ang_z(void)

{

MPU_Get_Gyroscope(&gx,&gy,&gz); // 读取陀螺仪数据

MPU_Get_Accelerometer(&ax,&ay,&az);

gx -= GYRO_Zero_X;

gy -= GYRO_Zero_Y;

gz -= GYRO_Zero_Z;

ang_accel=atan2(ax,az)*180/3.14; //加速度计计算角度

gyro_x=-gx/16.4;

gyro_y=-gy/16.4;

gyro_z=-gz/16.4;

angle = K1 * ang_accel+ (1-K1) * (angle_last + gyro_y * ang_dt);

angle_last = angle;

}

机械结构的影响

这里说一下机械结构对控制的影响。机械结构对直立控制的影响还是比较巨大的，特别是车速越快对结构的要求就越高，如果只是单纯的想立起来的话对结构的要求则没那么高。

主要遵循的原则就是机械重心越集中、越靠近两车轮轴线越好。车子除去底盘的话基本就电池和电路板了。电池是比较重的，所以可以优先考虑电池的放置，其次就是考虑电路板的放置了。重心越低越靠近车轮轴线相应的PID参数的调试就会更加的简单。反之的话则会更难。

说一下个人经历，（打一波广告）刚接触直立车的我用的是平衡小车之家的那种底盘调试的，因为经费的原因也只有底盘给我了，我给他配了一个特别特别特别沉的电池，用的传统控制方案，最后弄了特别久还是不能立起来（可能是我太菜了）。之后换了一个底盘，重心变低也变得靠近车轮轴线字后，两天就能勉强站起来立住了。

题外话

高速行驶的直立车转向环跟串级直立的三个环是并行的，所以也会存在干扰。特别是路面不平（颠婆）的状态下两者的干扰尤为严重。直立车的转向依靠的是两轮的差速，故也可以把转向环的输出串到速度环上边，作为速度环的一个传入参数。但是这会带来一个新的问题，转向环作为最外环的话其控制周期就会变得特别长。过长的转向周期也使得车模的转向变得迟钝。高速状态下的直立车如何获得最优的控制呢？？？

直立车控制方案原理附录源码直立车控制原理分析对比

直立车控制原理分析对比

传统控制方式

串级控制方式

角度的获取

机械结构的影响

题外话

继续阅读

GPS 射频调试总结

解决STM32调试,无法进入main函数的问题

你所不知道的Android Studio调试技巧单步调试区断点管理区变量观察区断点的分类调试的两种方式

Android Studio你不知道的高级调试技巧

解决bug的一些总结

java.lang.outofmemoryerror unable to create new native thread

Ubuntu下用dmesg查看printk的输出

android studio 调试手机模拟器找不到

通过wifi连接手机进行调试

ADT实现手机调试Android应用程序

无线调试android应用——ADB OVER NETWORK

maven插件编写与调试

netbeanside学习php入门篇

AndroidStudio Debug调试使用单步调试区断点管理区修改变量值变量观察区断点的分类调试的两种方式

欢迎使用CSDN-markdown编辑器Nginx配置文件nginx.conf中文详解（总结）

LoadRunner脚本调试-关联