44
陀螺原理：
陀螺是一个古老的学科， 自 1852 年， 傅科将高速旋转刚体命名为陀螺， 至 今已有 160 年左右的历史。陀螺连同其支撑框架总体被称作陀螺仪。 陀螺具有独特的力学特性， 如定轴性、进动性和陀螺动力效应等， 因而常 被作为陀螺稳定装置的敏感元件或者执行元件。 陀螺稳定装置是一种以陀螺为 敏感元件或执行元件，使被稳定对象在干扰因素作用下能相对大地坐标系保持 方位不变或者在指令力矩的作用下使其按照给定规律相对惯性空间转动的陀螺 装置。 陀螺稳定装置按照陀螺力矩在稳定装置中的作用， 可分为直接式陀螺稳定装置、间接式陀螺稳定装置、 动力式陀螺稳定装置、 指示式陀螺稳定装置和指 示-动力式陀螺稳定装置。 直接式陀螺稳定装置是一种用陀螺力矩抵抗作用于被 稳定对象上的干扰力矩，而使被稳定对象相对惯性空间保持方位的稳定的陀螺 稳定装置。 在这类稳定装置中， 陀螺是直接抵抗干扰力矩装置的执行元件。
下图为独轮车结构：
本设计所研究的是基于惯性飞轮的自行车侧向平衡控制。
下图为本实验平台：
飞轮平衡效果测试视频：
自行车初步行走测试视频：
主控采用STM32f103RCT6,传感器采用的是MPU6050，姿态解算采用的是卡尔曼滤波，侧向飞轮控制采用的是角度-角速度串级PID控制。
工程编译环境为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控制参数Kp；
13控制参数Ki； 14控制参数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;水平经度位置
{1 ,1 ,0 ,0 ,0 ,0 , 0 ,0 ,0 ,0 ,300, 2.5 ,0.0 ,0 ,0 ,0 ,1000},//Longitude_Speed;水平经度速度
{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控制器设计包含偏差限幅、积分分离、积分限幅等部分，
实际运用中只需要将初始化结构体中的对应标志位设置即可，应用起来很方便。
