matlab模拟的PID算法:
kp=-50;
%比例常数td=0.2;
%微分时间常数ti=-1/300;
%积分时间常数propertion=( gyro(i) - 0 ) * kp;
%角速度就是PID中的偏移量e。gyro(i)指的是当前角速度。speed=( gyro(i) - gyro(i-1) )/0.001;
%这里的speed指的是角速度变化的速度即角加速度，控制速度是每一毫秒控制一次所以要除以0.001求得角加速度。diff=td*speed;
%微分项。微分项主要是为了消除比例控制中由于延迟造成的自激振荡，就像弹簧挂着重物上下晃动，弹力就是比例项，空气阻力就是微分项，晃动是自激振荡，而空气阻力会将晃动消减掉。integra=0;
int_num=0;
for j=1:integra_num
%循环integra_num次。因为我是要稳定频率高于2Hz的抖动，所以只用积分最近一段时间内的数据。 if ( gyro(i-j) / gyro(i-j) == gyro(i) / gyro(i) )
%判断抖动方向是否跟当前一样。实验发现这句要不要影响都不大。 integra=integra+gyro(i-j);
end
end
integra=integra/ti;
%求得积分项angle_acc(i) = propertion + diff + integra;
%装置要产生的角角速度等于三项的和。 if angle_acc(i)>40
%电机输出力矩也是有限制的，所以限制电机产生的角加速度值 angle_acc(i)=40;
end
if angle_acc(i)<-40
angle_acc(i)=-40;
end
就是这些。感觉PID控制的鲁棒性不太好可能不适合干这样的工作。
参考：
PID控制器：
XXXXXXXXXXXXXXXXXXXXXX/view/XXXXXXXXXm鲁棒：
XXXXXXXXXXXXXXXXXXXXXX/view/XXXXXXXXXm曾想过用皮带，但我认为皮带的弹性会造成力矩滞后和抖动所以没有采用；另外小皮带轮不好做。
另外推荐xv3500和xv3700这两个陀螺，温漂真的很低。
