一、介绍
1、控制输入：加速度传感器和陀螺仪测得的小车倾角。当加速度传感器水平放置时，Z轴会测量到重力加速度，大小为g。当加速度传感器倾斜时，Z轴上测量的g就会变小，根据变小之前的值和之后的值，就可以算得倾角值。
这样一看似乎只需要加速度就可以获得小车的倾角，再对此信号进行微分便可以获得倾角速度。但在实际小车运行过程中，由于小车本身的摆动所产生的加速度会在Z轴上有分量，它叠加在上述测量信号上使得输出信号无法准确反映车模的倾角。
为了减少运动与摆动的影响，加速度传感器安装的高度应该越低越好，但是也无法彻底消除运动与摆动的影响。因此就需要引入陀螺仪，用于测量小车倾角变化的速度，即角速度。将角速度进行积分便可以得到小车的倾角。
由于陀螺仪输出的是小车的角速度，不会受到车体运动的影响，因此该信号中噪声很小。但是由于从陀螺仪测量的角速度值获得角度值，需要经过积分运算。如果角速度值存在微小偏差或漂移，比如当小车静止时，陀螺仪的输出不为零。经过积分运算后，微小的变化也会变得很大。因此要测量小车倾角也不能全部依赖陀螺仪。
比较理想的做法是把加速度传感器和陀螺仪组合起来使用。思路如下：每5ms读一次加速度传感器和陀螺仪的值，根据加速度传感器的值计算出一个角度值，angle_acc。再把陀螺仪的值乘以5ms，算得在5ms间隔中，小车倾角的变化值，再加上前一次计算出的小车倾角，得到angle_gyro。这样就得到了加速度传感器测量到的小车倾角和陀螺仪测量到的小车倾角。把这两个角度结合起来使用，就可以保证小车的倾角比较可靠了。结合的方法一般有两种：
一种是互补滤波，其实就是加权计算：
float K1 =0.02;
angle = K * angle_acc + (1-K) * (angle + angle_gyro * 0.005);K是权值，代表更相信加速度传感器还是陀螺仪。
还有一种是卡尔曼滤波，卡尔曼滤波是什么，自己去搜吧，资料太多了。简易型的代码如下：
float angle, angle_dot;
float Q_angle=0.001;// 过程噪声的协方差
float Q_gyro=0.003;//0.003 过程噪声的协方差 过程噪声的协方差为一个一行两列矩阵
float R_angle=0.5;// 测量噪声的协方差 既测量偏差
float dt=0.005;//
char C_0 = 1;
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
/**************************************************************************
函数功能：简易卡尔曼滤波
入口参数：
Accel：加速度传感器测量角度值
Gyro： 陀螺仪测量角度值
**************************************************************************/
void Kalman_Filter(float Accel,float Gyro)
{angle+=(Gyro - Q_bias) * dt; //先验估计Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分Pdot[1]=-PP[1][1];Pdot[2]=-PP[1][1];Pdot[3]=Q_gyro;PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差PP[1][0] += Pdot[2] * dt;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_0;K_0 = PCt_0 / E;K_1 = PCt_1 / E;t_0 = PCt_0;t_1 = C_0 * PP[0][1];PP[0][0] -= K_0 * t_0; //后验估计误差协方差PP[0][1] -= K_0 * t_1;PP[1][0] -= K_1 * t_0;PP[1][1] -= K_1 * t_1;angle += K_0 * Angle_err; //后验估计Q_bias += K_1 * Angle_err; //后验估计angle_dot = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
}通过上面这两种方法，就可以得到比较可靠的小车倾角了，变量名为angle。
2、控制对象：小车两边电机的转速和方向。
一般使用直流电机来制作平衡小车，直流电机驱动芯片推荐使用死区小一点的驱动芯片，比如TB6612FNG。
给PWMA、PWMB引脚通入10KHz的PWM，通过控制占空比可以控制电机的转速。
控制AIN1、AIN2、BIN1、BIN2的电平可以控制电机的转动方向。
#define myabs(m) (m>0?m:-m) //绝对值
/**************************************************************************
函数功能：直流电机方向和转速控制
入口参数：两个电机的pwm值，有正负
**************************************************************************/
void MotorControl(int moto1,int moto2)
{int siqu=400; //电机PWM死区，当PWM太小，电机转不起来。本值根据电机和驱动芯片做调节if(moto1<0) AIN2=1,AIN1=0; //反转else AIN2=0,AIN1=1; //正转PWMA=myabs(moto1)+siqu; //转速if(moto2<0) BIN1=0,BIN2=1;else BIN1=1,BIN2=0;PWMB=myabs(moto2)+siqu;
}3、控制任务：
1）、控制小车平衡：通过控制两个电机正反向运动保持小车直立平衡状态，这就要使用PID算法了。
#define Angle_AIM 0 //倾角的目标值，如果传感器装得有点偏，可能本值不为0
/**************************************************************************
函数功能：小车平衡PD控制
入口参数：
Angle：小车的倾角值
Gyro：陀螺仪的测量值，即5ms间隔内倾角的变化值
返回 值：对电机转速和方向的控制值，输出给MotorControl(int moto1,int moto2)函数
**************************************************************************/
int PID_Balance(float Angle,float Gyro)
{float Bias;int pwm;Bias=Angle-Angle_AIM; //计算角度偏差值pwm=Kp*Bias + Gyro*Kd; //PD控制，kp是P系数，kd是D系数return pwm; //返回电机的pwm控制值，有正负
}2）、控制小车运动速度：要在小车平衡PID控制的基础上加上速度PID控制，即所谓的串级PID，把速度PID的结果作为平衡PID的输入。要让小车前进，那么就不让小车的目标倾角为0，而人为给定一个大于0的倾角值，当目标倾角值大于0了，小车为了平衡，就会前进。目标倾角值越大，前进速度越快。我们可以得到下面关系式：
式子(1)是直立PD控制，式子(2)是速度PI控制。e(k)是速度控制偏差，a1是速度控制的结果，作为目标倾角输入式子(1)。把式子(2)代入式子(1)：
从上式可以知道，加入速度环后，最终的电机PWM等于直立PD输出值减速度PI输出值乘以Kp。
/**************************************************************************
函数功能：速度PI控制
入口参数：
SpeedL：左电机测量速度值
SpeedL：右电机测量速度值
Speed_Aim：小车目标速度
返回 值：速度控制PWM
**************************************************************************/
int PID_Velocity(int SpeedL,int SpeedR,int Speed_Aim)
{static float pwm,bias,Integral;bias =(SpeedL+SpeedR)/2-Speed_Aim; //计算速度偏差Integral +=bias; //误差累积 if(Integral>15000) Integral=15000; //积分限幅if(Integral<-15000) Integral=-15000; //积分限幅pwm=Integral*Kp+Integral*Ki; //速度控制return pwm;
}3）、控制小车运动方向：通过控制两个电机之间的转动差速实现小车转向控制。转向环仍然使用PD控制，根据上面式子的推导，我们知道只要把转向控制的结果加到电机的pwm上就可以了。其中一个电机加上转向控制结果，另一个电机减去转向控制结果，这样就造成了两个电机转速不同，进而转向。
/**************************************************************************
函数功能：转向控制
入口参数：
TurnSpeed：转向速度
gyro：陀螺仪测量到的小车转向速度
返回 值：转向控制PWM
**************************************************************************/
int PID_Turn(int TurnSpeed,float gyro)//转向控制
{float Turn;Turn=TurnSpeed*Kp+gyro*Kd; //为了消除小车方向控制中的过冲，需要增加微分控制return Turn;
}其实小车的转向只需要简单的比例控制即可，可是小车上有电池等比较重的物体，具有很大的转动惯量，如果在转动过程中不加抑制，会使小车转向过冲。因此在小车转向控制中，加入了使用小车方向的变化率对电机差动控制量进行修正的控制方式。
二、代码
1、小车倾角的测量（使用MPU6050）
控制板上，小车的前进方向是MPU6050的X轴正方向。
于是倾角测量代码为：（5ms测量一次，因为要对陀螺仪进行积分，故尽量保证5ms调用间隔准确）
float K1 =0.02;
float angle,Gyro_Balance,Gyro_Turn; //平衡倾角 平衡陀螺仪 转向陀螺仪
void Get_Angel()
{float angle_acc,Accel_X,Accel_Z,Gyro_Y,Gyro_Z;Gyro_Y=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_L); //读取Y轴陀螺仪Gyro_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_L); //读取Z轴陀螺仪Accel_X=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_L); //读取X轴加速度计Accel_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_L); //读取Z轴加速度计if(Gyro_Y>32768) Gyro_Y-=65536; //数据类型转换 也可通过short强制类型转换if(Gyro_Z>32768) Gyro_Z-=65536; //数据类型转换if(Accel_X>32768) Accel_X-=65536; //数据类型转换if(Accel_Z>32768) Accel_Z-=65536; //数据类型转换Gyro_Balance=-Gyro_Y; //更新平衡角速度angle_acc=atan2(Accel_X,Accel_Z)*180/PI; //计算加速度传感器测量得到倾角值Gyro_Y=Gyro_Y/16.4; //陀螺仪量程转换angle = K1 * angle_acc+ (1-K1) * (angle + Gyro_Balance * 0.005); //互补滤波，加权计算 Gyro_Turn=Gyro_Z; //更新转向角速度
}获取小车倾角（变量名为angle）之后，进行直立PD控制：
Balance_Pwm =PID_Balance(angle,Gyro_Balance); //平衡PID控制速度PI控制：
Velocity_Pwm=PID_Velocity(SpeedL,SpeedR,Speed_Aim); //速度环PID转向PD控制：
Turn_Pwm=PID_Turn(TurnSpeed,Gyro_Turn); //转向PID控制最后控制电机：
MotorControl(Balance_Pwm+Velocity_Pwm-Turn_Pwm,Balance_Pwm+Velocity_Pwm+Turn_Pwm);
