倒立摆 角度环和位置环 分析以及参数整定

倒立摆为什么有了角度环还必须有位置环呢？

答案是：和平衡小车类比可得，当只有角度环的时候，倒立摆是可以对摆锤偏向做出pwm的反应的，但是具体要反应合适程度到哪里，还是应该由位置环来告诉系统，应该尽量稳定在什么位置范围。

角度环5ms采集一下 位置环25ms采集一次，电机控制是5ms控制一次。

这里位置环25ms是这样的出来的：首先尝试5ms，但是不论参数怎么调试都是反应剧烈，所以逐渐加大延长编码器采集时间，到了30ms的时候根据相同参数比较反应滞后，座椅最后确定25ms。

角度环是根据角度的差值来给定pwm 需要的参数是角位移传感器的采集的adc的值

int balance(float Angle)

{

float Bias; //倾角偏差

static float Last_Bias,D_Bias; //PID相关变量

int balance; //PWM返回值

Bias=Angle-ZHONGZHI; //求出平衡的角度中值 和机械相关

D_Bias=Bias-Last_Bias; //求出偏差的微分 进行微分控制（这次偏差和上次偏差）

balance=-Balance_KP*Bias-D_Bias*Balance_KD; //===计算倾角控制的电机PWM PD控制

Last_Bias=Bias; //保持上一次的偏差

return balance;

}

位置环是用编码器测得的位置的差值作为比例参数，用上一次位置的差值和这一次位置的差值作为微分参数

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;

}

自我总结的调试指南：

注：红色是目标位置

黑色是编码器测的实际位置

角度环：p ：加参数加到放手可以直立一会 但是一碰就抖动很厉害

加上d后：加参数加到可以直立不倒 向两边或一边加速，碰一下不抖

位置环：p：因为电机总得脉冲是7200，然后倒立摆转一圈编码器根据正交解码测得是1024个脉冲，在25ms中断中大部分时候达到的最大脉冲差值是100左右，所以我们P参数确定的范围暂且是1~50. 初始位置放在编码器值是10000的时候（这里编码器CNT初始值给的10000），目标位置也是10000，这时候给p=1，我们会观察到这样的波形：

红色是目标位置，黑色是编码器测得实际位置，因为有位置环p的存在，所以倒立摆在偏了一小段的时候会有恢复力，但这个恢复力超调太大了，导致倒立摆剧烈来回摆动，所以此时加微分参数D，来减小超调。

微分参数大小的判断是因为它乘的是这次位置差-上次位置差，这个差值在25ms中断中最大位十左右，所以我们暂定微分参数是0~300。先增加到100，这时候波形如下：

黑色是实际编码器测定位置变化，红色是目标位置，这时候我们看可以看到超调量减小了并且处于稳态，但是却迟迟达不到实际值，这说明有静差，我们可以先不增加积分，再增大比例参数，直到出现这种波形：

黑色是实际波形，红色是目标波形，这时候可以看出，已经达到目标位置值，并且超调量不是很大，这时候就在增大点微分参数D减小点超调量，使实际波形更接近于目标值就好，最好波形如下：