大家好，又见面了，我是你们的朋友全栈君。

前言

近期在做2013年电赛控制类题目–简易旋转倒立摆装置，自己并不是自动化专业的学生，没有学过自动控制原理，倒立摆其实是一个十分经典的自动控制模型，我们只能是边做边学习，逐渐去了解倒立摆。
我认为倒立摆有两个难点，一个是自动起摆一个是机械结构，其中自动起摆涉及到PID算法与运动方程的求解，而机械结构主要是尽量减小转动阻尼同时避免旋转时线的缠绕。我们买了平衡小车家的机械结构套件，他们为了避免线缠绕使用了导线环，这是一个好东西，可以完美解决导线缠绕问题。我在学习平衡小车家程序与算法的过程中也是总结了一些经验，在这里分享一下。

程序框架

平衡小车家旋转倒立摆的代码符合他们家的一贯作风，所有的控制算法在定时中断中实现，可以进行确定频率的控制，使用了一个5ms中断，在中断中完成数据读取、PWM控制量计算、对电机的控制，这样的代码结构清晰、响应快，但是也有些缺点，如果你要有功能选择，在中断中的代码是比较复杂的，要么使用switch-case完全放弃代码的共用，如果有共用部分的代码那么代码逻辑要仔细考虑。
对于那些执行频率不必太高的功能，比如50ms向上位机发送数据、100ms进行LED闪烁、100ms读取电池电压，你可以用一个每次中断累加的变量来进行控制，当它取余为某一值时执行某一功能，这样就类似与一个任务调度器了。
还有一点要注意的是，在中断中那些重要的、每次都必须执行的代码要放在靠前的位置，而那些对实时性要求不太高的代码可以往后稍一稍，因为每次进入中断就清除中断标志位，下一个中断会在5ms后准时到来，如果你当前的中断没有执行完则会被打断（一般是不会发生这样的事情，除非在中断中做了耗时很长的操作），所以为了避免打断那些控制代码要放在靠前的位置。
最后，在中断中不放耗时久的操作这也是常识了，那些耗时久的以及耗时久同时有时序要求的代码都可以放在主函数中执行，比如OLED的刷新、向上位机的数据发送，示例代码如下：

while(1)
{ 
         
	DataScope();	            //===上位机
	delay_flag=1;	            //===50ms中断精准延时标志位
	oled_show();               //===显示屏打开 
	while(delay_flag);         //===50ms中断精准延时 主要是波形显示上位机需要严格的50ms传输周期 
	if(Flash_Send==1)          //===写入PID参数到Flash,由按键控制该指令
	{ 
   
		Flash_Write();	        //===把参数写入到Flash
		Flash_Send=0;	          //===标志位清零
		Flag_OLED=1;            //===显示标志位置1 在显示屏上面显示Data Is Saved的字样
	}							
} 

这里每50ms进行一次数据发送和刷屏，使用while(delay_flag);来等待50ms标志位，时间控制的还是比较好的。

关于直立

我们的旋转倒立摆使用旋转电位器来获取摆杆的当前角度，电机使用的是带编码盘的直流电机。我们要保持的直立有两个要求，摆杆垂直与平面同时要尽量静止与原地，不会随意的转动。
所以说要有两个环来控制，首先是角度环，以旋转电位器的值与垂直点的偏差为输入，目标是把这个偏差控到零，我们使用的PD控制器，因为该系统有一定的滞后性，而且是一个不稳定的系统，我认为并没有消除静差的需求，所以没有上积分，代码如下：

int balance(float Angle)
{ 
     
	float Bias;                       //倾角偏差
	static float Last_Bias,D_Bias;    //PID相关变量
	int balance;                      //PWM返回值 
	Bias=Angle-ZHONGZHI;              //求出平衡的角度中值 和机械相关
	D_Bias=Bias-Last_Bias;            //求出偏差的微分 进行微分控制
	/*===计算倾角控制的电机PWM PD控制===*/
	balance=-Balance_KP*Bias-D_Bias*Balance_KD;   
	Last_Bias=Bias;                   //保持上一次的偏差
	return balance;
}

角度环参数的整定，首先调节KP这没什么说的，首先确定极性，然后从低往高调节到出现低频抖动（比较严重的），接下来确定KD的极性，注意KD是负反馈，偏差增大时应该控制系统往减小偏差的方向运动，这样才能帮助摆杆直立，所以确定极性后从小往大调节，KD的参数是要比较大的，因为偏差的微分每次都会是一个比较小的值，调到系统基本可以直立，并且有高频振动时就可以了，这样我们就确定了KP和KD的最大参数，将它们乘0.6就可以得到正常的PD参数。
接下来就是位置环了，在前面我们调节好直立环后，摆杆可以直立一段时间，但是它会逐渐往一个方向加速，然后直到转到全速后倒下，我们就需要位置环来让它稳在原地，同时给它更快的速度去追要倒下的杆。这里我们一开始想错了，我们错以为位置环的作用就是使摆杆停在某一个位置，即进行负反馈，但是这样调的结果是摆杆更加不能倒立了还不如只有直立环的效果好，查阅资料后发现应该使用正反馈，应该让速度更快去追倒下的摆杆，也就是如果只有位置环，你往任意方向转动悬臂，悬臂会往这个方向加速旋转直到全速。位置环的输入为编码器的值与目标点的差值，同样适用PD调节，代码如下：

int Position(int Encoder)
{ 
     
   static float Position_PWM,Last_Position,Position_Bias,Position_Differential;
	 static float Position_Least;
  	Position_Least =Encoder-Position_Zero;             			//===最近位置差值
    Position_Bias *=0.8;		   
    Position_Bias += Position_Least*0.2;	             		//===一阶低通滤波器 
	Position_Differential=Position_Bias-Last_Position;
	Last_Position=Position_Bias;
	Position_PWM=Position_Bias*Position_KP+Position_Differential*Position_KD; //===速度控制 
	return Position_PWM;
}

使用低通滤波器的作用是减缓位置差值对平衡的影响，也就是上次位置差值占80% + 这次速度差占20% =此次速度差值。
调节位置换我们没有太好的办法，只能是根据经验给一些值，KP参数不要太大，否则会对直立环有较大的影响，KD参数要大一些，使得在最短时间内达到最大速度。这里建议不要单独调位置环，最好把直立环和位置环一起调节，边看效果边看数据曲线边调。
如果要实现在保持平衡的时候
最后的效果我们基本可以实现平衡小车家视频中的效果，可以较好的实现直立。

关于自动起摆

自动起摆的程序我们研究的也不是特别的透彻，尤其是涉及运动方程的部分，还没有自己去推公式，我们的自动起摆程序大致分为两步，第一步是让摆的摆幅逐渐增大，直到接近于水平，这里增大摆幅的同时还有略微减小周期，这可以通过运动公式来计算出来，当摆幅达到要求且正在下落时悬臂会迅速的往下落的方向旋转大约半圈，然后它由于过冲会有一个回摆，通过这个回摆就会把摆杆摆起来，这时开启直立环，因为位置环会削弱直立环的作用，所以在刚刚立起摆杆不够稳定的情况下先不开启位置环，过300ms后开启位置环，这时自动起摆的过程就完成了。代码如下：

void Run(u8 Way)
{ 
 		   
static float Count_FZ,Count_Next,Target_Position=10380;
static u8 Flag_Back;
static float Count_Big_Angle=0.046542;
static int Position_Max;
if(Way==1)                                 //进入自动起摆程序 按键触发
{ 

if(Flag_qb==1)  //第1步，摆杆自由摆动，振幅越来越大
{ 

Ratio=1; //正常的PID参数
Count_qb+=Count_Big_Angle;   //自变量
Count_Big_Angle-=0.0000027;      //振幅越大，摆动周期略微减小
Count_FZ+=0.025;   //振幅越来越大
Target_Position=0.6*Count_FZ*sin(Count_qb)+10000;  //运动公式 
Encoder=Read_Encoder(2);             	       //===更新编码器位置信息
Moto_qb=Position_PID(Encoder,Target_Position);  //位置闭环控制
if(Moto_qb>7200)Moto_qb=7200;//控制位置闭环控制过程的速度
if(Moto_qb<-7200)Moto_qb=-7200;//控制位置闭环控制过程的速度
//2100
if(Angle_Balance>(Angle_Max+850)&&Angle_Balance<2320&&D_Angle_Balance<=-1)   //振幅大于阈值时，进入下一步 
{ 

Flag_qb++;
Count_qb=0;
TIM2->CNT=10000;        //复位一下计数寄存器
Count_FZ=0;
}
Set_Pwm(Moto_qb); //赋值给PWM寄存器 
}
if(Flag_qb==2)            //第3步，通过位置控制，利用惯性，自动起摆
{ 
	 
Target_Position=10600;                        //设定目标值
Encoder=Read_Encoder(2);             	       //===更新编码器位置信息
Moto_qb=Position_PID(Encoder,Target_Position);//===位置PID控制器
if(Moto_qb>7200) Moto_qb=7200;
if(Moto_qb<-7200)Moto_qb=-7200;
Set_Pwm(Moto_qb);  //赋值给PWM寄存器
if(Angle_Balance<(ZHONGZHI+200)&&Angle_Balance>(ZHONGZHI-200))  //到底接近平衡位置 即可开启平衡系统
{ 
	
State=1;   //倒立状态置1
Way_Turn=0;//自动起摆标志位清零
Flag_qb=0; //自动起摆步骤清零
Angle_Max=0;
Balance_KP += 30;
Balance_KD += 200;
}
}
if(Flag_qb==3)            //停
{ 

AIN1 = 1;AIN2 = 1;
}
}
if(Way==2) //手动起摆程序
{ 

if(Angle_Balance<(ZHONGZHI+200)&&Angle_Balance>(ZHONGZHI-200))  //到底接近平衡位置 即可开启平衡系统
{ 
	
State=1;   //倒立状态置1
Way_Turn=0;//起摆标志位清零
}
}
}
int Position_PID (int Encoder,int Target)
{ 
 	
static float Bias,Pwm,Integral_bias,Last_Bias;
Bias=Encoder-Target;                                   //计算偏差
Integral_bias+=Bias;	                                  //求出偏差的积分
// Pwm=70*Bias*Ratio+0.00*Integral_bias*Ratio+200*(Bias-Last_Bias)*Ratio; //位置式PID控制器
Pwm=90*Bias*Ratio+0.00*Integral_bias*Ratio+250*(Bias-Last_Bias)*Ratio;   //位置式PID控制器
Last_Bias=Bias;                                        //保存上一次偏差 
return Pwm;                                            //增量输出
}

有几个注意的点。这里运动方程我并不知道该如何调，这个是根据你摆杆重心、电机特性、当地重力加速度等算出来的，这个还是得试着推一下；还有一个点是刚刚立起来时直立环参数应该调大一点，我们就设置成了直立环的最大参数，300ms后在用正常的参数加上位置换来控制，因为我们在调节时发现起摆后的一瞬间经常没能直立，摆的抗干扰性不够强，所以加大了直立环参数，用这样的方法起摆的成功率大大增加。