一、介紹
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);