慣性飛輪側(cè)向平衡自行車機器人V1.0

陀螺原理：

       陀螺是一個古老的學科， 自 1852 年， 傅科將高速旋轉(zhuǎn)剛體命名為陀螺， 至 今已有 160 年左右的歷史。陀螺連同其支撐框架總體被稱作陀螺儀。 陀螺具有獨特的力學特性， 如定軸性、進動性和陀螺動力效應(yīng)等， 因而常 被作為陀螺穩(wěn)定裝置的敏感元件或者執(zhí)行元件。 陀螺穩(wěn)定裝置是一種以陀螺為 敏感元件或執(zhí)行元件，使被穩(wěn)定對象在干擾因素作用下能相對大地坐標系保持 方位不變或者在指令力矩的作用下使其按照給定規(guī)律相對慣性空間轉(zhuǎn)動的陀螺 裝置。 陀螺穩(wěn)定裝置按照陀螺力矩在穩(wěn)定裝置中的作用， 可分為直接式陀螺穩(wěn)定裝置、間接式陀螺穩(wěn)定裝置、 動力式陀螺穩(wěn)定裝置、 指示式陀螺穩(wěn)定裝置和指 示-動力式陀螺穩(wěn)定裝置。 直接式陀螺穩(wěn)定裝置是一種用陀螺力矩抵抗作用于被 穩(wěn)定對象上的干擾力矩，而使被穩(wěn)定對象相對慣性空間保持方位的穩(wěn)定的陀螺 穩(wěn)定裝置。 在這類穩(wěn)定裝置中， 陀螺是直接抵抗干擾力矩裝置的執(zhí)行元件。

下圖為獨輪車結(jié)構(gòu)：

本設(shè)計所研究的是基于慣性飛輪的自行車側(cè)向平衡控制。

下圖為本實驗平臺：

飛輪平衡效果測試視頻：

自行車初步行走測試視頻：

主控采用STM32f103RCT6,傳感器采用的是MPU6050，姿態(tài)解算采用的是卡爾曼濾波，側(cè)向飛輪控制采用的是角度-角速度串級PID控制。

工程編譯環(huán)境為IAR7.3~7.6,軟件百度云下載地址為：https://pan.baidu.com/s/1skT57at

代碼提供了必要的注釋，PID控制器示例代碼如下：

float PID_Control(PID_Controler *Controler)

{

/*******偏差計算*********************/

Controler->Last_Err=Controler->Err;//保存上次偏差

Controler->Err=Controler->Expect-Controler->FeedBack;//期望減去反饋得到偏差

if(Controler->Err_Limit_Flag==1)//偏差限幅度標志位

{

if(Controler->Err>=Controler->Err_Max) Controler->Err= Controler->Err_Max;

if(Controler->Err<=-Controler->Err_Max) Controler->Err=-Controler->Err_Max;

}

/*******積分計算*********************/

if(Controler->Integrate_Separation_Flag==1)//積分分離標志位

{

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)//積分限制幅度標志

{

if(Controler->Integrate>=Controler->Integrate_Max)

Controler->Integrate=Controler->Integrate_Max;

if(Controler->Integrate<=-Controler->Integrate_Max)

Controler->Integrate=-Controler->Integrate_Max ;

}

/*******總輸出計算*********************/

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偏差限幅標志； 2積分限幅標志；3積分分離標志； 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è)計包含偏差限幅、積分分離、積分限幅等部分，

實際運用中只需要將初始化結(jié)構(gòu)體中的對應(yīng)標志位設(shè)置即可，應(yīng)用起來很方便。