41
陀螺原理：
陀螺是一個(gè)古老的學(xué)科， 自 1852 年， 傅科將高速旋轉(zhuǎn)剛體命名為陀螺， 至 今已有 160 年左右的歷史。陀螺連同其支撐框架總體被稱(chēng)作陀螺儀。 陀螺具有獨(dú)特的力學(xué)特性， 如定軸性、進(jìn)動(dòng)性和陀螺動(dòng)力效應(yīng)等， 因而常 被作為陀螺穩(wěn)定裝置的敏感元件或者執(zhí)行元件。 陀螺穩(wěn)定裝置是一種以陀螺為 敏感元件或執(zhí)行元件，使被穩(wěn)定對(duì)象在干擾因素作用下能相對(duì)大地坐標(biāo)系保持 方位不變或者在指令力矩的作用下使其按照給定規(guī)律相對(duì)慣性空間轉(zhuǎn)動(dòng)的陀螺 裝置。 陀螺穩(wěn)定裝置按照陀螺力矩在穩(wěn)定裝置中的作用， 可分為直接式陀螺穩(wěn)定裝置、間接式陀螺穩(wěn)定裝置、 動(dòng)力式陀螺穩(wěn)定裝置、 指示式陀螺穩(wěn)定裝置和指 示-動(dòng)力式陀螺穩(wěn)定裝置。 直接式陀螺穩(wěn)定裝置是一種用陀螺力矩抵抗作用于被 穩(wěn)定對(duì)象上的干擾力矩，而使被穩(wěn)定對(duì)象相對(duì)慣性空間保持方位的穩(wěn)定的陀螺 穩(wěn)定裝置。 在這類(lèi)穩(wěn)定裝置中， 陀螺是直接抵抗干擾力矩裝置的執(zhí)行元件。
下圖為獨(dú)輪車(chē)結(jié)構(gòu)：
本設(shè)計(jì)所研究的是基于慣性飛輪的自行車(chē)側(cè)向平衡控制。
下圖為本實(shí)驗(yàn)平臺(tái)：
飛輪平衡效果測(cè)試視頻：
自行車(chē)初步行走測(cè)試視頻：
主控采用STM32f103RCT6,傳感器采用的是MPU6050，姿態(tài)解算采用的是卡爾曼濾波，側(cè)向飛輪控制采用的是角度-角速度串級(jí)PID控制。
工程編譯環(huán)境為IAR7.3~7.6,軟件百度云下載地址為：https://pan.baidu.com/s/1skT57at
代碼提供了必要的注釋?zhuān)?a class="article-link" target="_blank" href="/baike/518799.html">PID控制器示例代碼如下：
float PID_Control(PID_Controler *Controler)
{
/*******偏差計(jì)算*********************/
Controler->Last_Err=Controler->Err;//保存上次偏差
Controler->Err=Controler->Expect-Controler->FeedBack;//期望減去反饋得到偏差
if(Controler->Err_Limit_Flag==1)//偏差限幅度標(biāo)志位
{
if(Controler->Err>=Controler->Err_Max) Controler->Err= Controler->Err_Max;
if(Controler->Err<=-Controler->Err_Max) Controler->Err=-Controler->Err_Max;
}
/*******積分計(jì)算*********************/
if(Controler->Integrate_Separation_Flag==1)//積分分離標(biāo)志位
{
if(ABS(Controler->Err)<=Controler->Integrate_Separation_Err)
Controler->Integrate+=Controler->Ki*Controler->Err;
}
else
{
Controler->Integrate+=Controler->Ki*Controler->Err;
}
/*******積分限幅*********************/
if(Controler->Integrate_Limit_Flag==1)//積分限制幅度標(biāo)志
{
if(Controler->Integrate>=Controler->Integrate_Max)
Controler->Integrate=Controler->Integrate_Max;
if(Controler->Integrate<=-Controler->Integrate_Max)
Controler->Integrate=-Controler->Integrate_Max ;
}
/*******總輸出計(jì)算*********************/
Controler->Last_Control_OutPut=Controler->Control_OutPut;//輸出值遞推
Controler->Control_OutPut=Controler->Kp*Controler->Err//比例
+Controler->Integrate//積分
+Controler->Kd*(Controler->Err-Controler->Last_Err);//微分
/*******總輸出限幅*********************/
if(Controler->Control_OutPut>=Controler->Control_OutPut_Limit)
Controler->Control_OutPut=Controler->Control_OutPut_Limit;
if(Controler->Control_OutPut<=-Controler->Control_OutPut_Limit)
Controler->Control_OutPut=-Controler->Control_OutPut_Limit;
/*******返回總輸出*********************/
return Controler->Control_OutPut;
}
1偏差限幅標(biāo)志； 2積分限幅標(biāo)志；3積分分離標(biāo)志； 4期望；
5反饋 6偏差； 7上次偏差； 8偏差限幅值；
9積分分離偏差值；10積分值 11積分限幅值； 12控制參數(shù)Kp；
13控制參數(shù)Ki； 14控制參數(shù)Kd； 15控制器總輸出； 16上次控制器總輸出
17總輸出限幅度
*/
const float Control_Unit[12][17]=
{
/* Kp Ki Kd */
/*1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17*/
{1 ,1 ,0 ,0 ,0 ,0 , 0 ,45 ,0 ,0 , 40, 2 ,0 ,0.00 ,0 ,0 , 200},//Pitch_Angle;偏航角度
{0 ,1 ,0 ,0 ,0 ,0 , 0 ,500 ,0 ,0 ,300, 15 ,1.2 ,5 ,0 ,0 ,500},//Pitch_Gyro;偏航角速度
{1 ,1 ,0 ,0 ,0 ,0 , 0 ,45 ,0 ,0 , 40, 1.8 ,0 ,0.00 ,0 ,0 , 200},//Roll_Angle;橫滾角
{0 ,1 ,0 ,0 ,0 ,0 , 0 ,500 ,0 ,0 ,150, 1.86 ,0.026 ,0 ,0 ,0 ,500},//Roll_Gyro;橫滾角速度
{1 ,1 ,0 ,0 ,0 ,0 , 0 ,45 ,0 ,0 , 25, 1.5 ,0 ,0.00 ,0 ,0 , 150},//Yaw_Angle;偏航角
{1 ,1 ,0 ,0 ,0 ,0 , 0 ,500 ,0 ,0 ,250, 2.5 ,0.01 ,0 ,0 ,0 ,2000},//Yaw_Gyro;偏航角速度
{1 ,1 ,0 ,0 ,0 ,0 , 0 ,0 ,0 ,0 ,300, 2.5 ,0.0 ,0 ,0 ,0 ,1000},//High_Position;海拔高度位置
{1 ,1 ,0 ,0 ,0 ,0 , 0 ,0 ,0 ,0 ,300, 2.5 ,0.0 ,0 ,0 ,0 ,1000},//High_Speed;海拔攀升速度
{1 ,1 ,0 ,0 ,0 ,0 , 0 ,0 ,0 ,0 ,300, 2.5 ,0.0 ,0 ,0 ,0 ,1000},//Longitude_Position;水平經(jīng)度位置
{1 ,1 ,0 ,0 ,0 ,0 , 0 ,0 ,0 ,0 ,300, 2.5 ,0.0 ,0 ,0 ,0 ,1000},//Longitude_Speed;水平經(jīng)度速度
{1 ,1 ,0 ,0 ,0 ,0 , 0 ,0 ,0 ,0 ,300, 2.5 ,0.0 ,0 ,0 ,0 ,1000},//Latitude_Position;水平緯度位置
{1 ,1 ,0 ,0 ,0 ,0 , 0 ,0 ,0 ,0 ,300, 2.5 ,0.0 ,0 ,0 ,0 ,1000},//Latitude_Speed;水平緯度速度
};
本PID控制器設(shè)計(jì)包含偏差限幅、積分分離、積分限幅等部分，
實(shí)際運(yùn)用中只需要將初始化結(jié)構(gòu)體中的對(duì)應(yīng)標(biāo)志位設(shè)置即可，應(yīng)用起來(lái)很方便。
