在上一次谈到基于MPU6050的基于一阶互补滤波算法实现后，本来想接着就自适应一阶互补滤波和卡尔曼滤波再写一篇的，但是卡尔曼滤波算法我自己写出来并进行姿态解算后发现效果不很好，才疏学浅，等我调好了再写吧，昨天花了半下午做了一个基于MPU6050的舵机云台自稳定系统，采用PD控制，下面就从控制理论原理到代码实现来谈一谈。
1.P项对系统性能影响的分析
假设现在我们有如下一个系统：
这个系统采用简单的比例控制，当kp=1时，我们来分析一下他的稳定性，从奈奎斯特曲线的角度来分析：
num=[1];
kp=1;
den=[1 1 -2];
sys=tf(num,den);
nyquist(sys);
由P-Z=N这个奈奎斯特判据来看，P作为开环传递函数在右半平面的极点数等于1，N作为奈奎斯特曲线逆时针绕（-1,0j）这个点的圈数等于0，所以Z等于1，意味着闭环传递函数在右半平面的极点数为1，系统不稳定。我们可以通过时域响应来验证一下：
我们扩大kp值，将kp变为10：
num=[10];
kp=10;
den=[1 1 -2];
sys=tf(num,den);
nyquist(sys);
由P-Z=N可以，P=1，N=1，所以Z=0，闭环系统稳定。老规矩验证一波：
可以看出，时域响应确实是稳定的，但是存在稳态误差。
现在我们可以得到一个结论：单纯的比例控制，增大或者减小比例系数kp可以改变系统的稳定性。
我们再次增大kp值，这次我们将kp增大到50：
与上面kp等于10的时域响应比较可得，其稳态误差减小了但是瞬态响应振荡加剧了。所以我们又可以得到一个结论：在系统稳定前提下，增大比例系数kp可以减小稳态误差，但是会导致系统振荡加剧。
下面从数学角度分析一下：
首先求出系统的闭环传递函数：
总所周知，在拉普拉斯变换中有一个终值定理，假设输入为一个阶跃响应，由终值定理可得：
当t趋近于无穷时，s趋近于0，所以此时输出为：
由于输入为r(t)=1，所以y(t)-r(t)为：
当kp增大时，稳态误差减小，这是有数学依据的。
那么为什么会导致系统的振荡加剧呢，这次我们感性一点分析。我们知道了系统的闭环传递函数为:
输入为阶跃值1，那么系统的时域表达式可写作:
λ1和λ2为闭环传递函数的两个极点，如果λ1和λ2实部都小于0那么系统稳定，如果有虚部存在，那么就会产生振荡。由韦达定理可以求得两个极点为：
当kp>9/4时根号下的部分为负数，会出现虚数，在e的指数部分出现虚数i，怎么这么熟悉，咦咦咦，记得有位大神叫欧拉，提出了一个强的一批的欧拉公式，把指数和三角函数联系在了一起，下面来看看这个伟大的公式：
那么一切都好理解了，如果特征值存在虚部项，那么y(t)里就会存在由特征值实部决定的eλt衰减项以及由特征值虚部决定的三角函数振荡项，实部大小决定收敛速度，虚部（虚部也是实数啊）大小决定振荡剧烈程度。当kp>9/4时，kp越大，实部永远等于-0.5，衰减速度不变，虚部绝对值越大，三角函数部分系数越大，振荡越剧烈，所以从数学上证明了kp越大，振荡越剧烈。在这个系统里，我们可以总结一下，kp=2和kp=9/4是两个分界点。
当kp<2,系统不稳定（前面提到过）；
当2<kp<9/4,系统稳定且不振荡，当kp=2.2时；
当kp>2时，系统稳定且存在振荡，kp越大振荡越剧烈但稳态误差较小。
以上便是对单纯的比例控制对系统影响的分析，大家还可以从开环bode图的角度去分析系统的稳定性，其实在奈奎斯特曲线中也能看出相位裕度和幅值裕度从而进行稳定性分析，这里我就贴一个kp=10时这个系统的开环bode图：
num=[10];
kp=2.2;
den=[1 1 -2];
sys=tf(num,den);
bode(sys);
开环伯德图分析系统性能不外乎幅值裕度和相位裕度，当幅频特性曲线与横坐标相交时对应的频率为截止频率，当相频特性特性曲线通过-（2k+1）π时对应的频率为穿越频率，在截止频率点处求相位裕度，在穿越频率点处求幅值裕度，公式就不细说了，说下结论，相位裕度越大，幅值裕度越大，系统越稳定，截止频率越大，系统响应也越快，幅值裕度越大，系统对输入信号跟随性越好，感性上来认识就是越像输入曲线，各位同志们可以下来自己在MATLAB里按照这个规律实验一下。有机会的话下次详细讲讲bode下的系统稳定性分析。
D项对系统性能的影响分析：
老规矩，先看仿真结果，再做数学分析。既然没有做任何计算，上来就随便搞一个数字，让比例系数kp=50，这样静态误差小一点，然后让微分项系数kd=20。仿真一波：
图中黄线为加入微分控制的时域响应，蓝色线为纯比例控制，我们发现加入微分量后超调量减小了，振荡也消失了，但是系统的响应速度降低了，这只是一个感性的认识，在工程中使用纯PD控制也很少，毕竟这玩意对噪声太敏感，你要是拿这个控制器去控制电机或者舵机，如果把传感器和主控制器放在电机或舵机附件，那你就会体验到什么叫蹦蹦床的快乐了。所以工程中一般都使用一个叫做超前补偿器的玩意，然后整个控制方法也就被称为不完全微分PD控制（其实不完全微分PID控制用的比较多），好了，先说到这里，总结一下我们现在遇到的问题，
1.怎么从数学角度来证明微分项加入后对系统性能的影响；
2.为什么要使用超前补偿器，他对系统会带来什么影响，为什么超前补偿器噪声不敏感。
下面我们从根轨迹的角度来分析问题1，根轨迹法是经典控制理论中分析系统性能，设计系统模态的一个令人赏心悦目的方法，这里不介绍根轨迹法如何绘制，因为我们有matlab，记得有位大佬说过，matlab为低能的用户提供方便的程序集成，确实，没有matlab，我这种低能的大学生还真就不能进行大量的科研计算，仿真建模，只能在实操中碰壁，没法用仿真指导实操，我总不能遇到什么问题就自己用C编一个函数吧，没那能耐，泰勒展开，快速傅里叶，根轨迹，bode图，那个不得写半天，matlab一个函数的事，咱们不是搞数学的，整那么多事干嘛。扯远了，下面来看看纯比例控制下的根轨迹图：
num=[1];
%kp=50;
%kd=20;
den=[1 1 -2];
sys=tf(num,den);
%bode(sys);
rlocus(sys);%求根轨迹
axis([-2.5 2 -6 6]);
我们可以看出，当增益为2时，为临界点，随着增益kp的增大，系统都为负根，系统稳定，但是随着kp的增大，根进入两条与x轴垂直的线内，大家可以看到，根在这两条直线上时，实部为-0.5，是不是和我们之前算的特征值的实部一样，这时候再增大kp，对不起，实部不会变了，虚部绝对值一直增大而已，然后这会造成什么现象，振荡加剧啊，那作为未来的工程师，现在的大学生，是不是要想办法改善一下这种情况，我不就是想让根的实部往左移吗，让根的虚部绝对值越小吗，贪心点的话，我想让根都落在负实轴上，离原点越远越好。既然我们有这样的想法，那我们可以假设让根落在哪个位置，然后用根轨迹上的根与零点连线与实轴正方向夹角-根与极点连线与实轴正方向的夹角=-180°的定理来确定零点应该放在哪个位置，如果我们是加一个微分项，那么就会增加一个零点，只要零点安放位置合适，按照上述原理，就会导致根轨迹的左移，达到我们想要的结果，不信我们验证一波：
加入微分项后系统变为如下（上面部分为pd控制，下面部分为p控制）：
num=[1 2.5];
%kp=50;
%kd=20;
den=[1 1 -2];
sys=tf(num,den);
%bode(sys);
rlocus(sys);
axis([-2.5 2 -6 6]);
随着kp增益的增大，我们可以看到根的实部几乎是向左移动的，说明系统稳定性越好而且瞬态响应快，并且虚部没有明显增加，说明振荡是可控的，不再像之前纯比例控制一样，为了增加增益就要无限制的扩大振荡，在加入纯微分项后的系统性能得到明显改善。来看看时域响应就知道了，我们把增益kp设置为20：
可以看出，经过加入微分项后，系统的时域响应性能得到了极大的改善。这就是理论指导实践的威力。我们从根轨迹的角度分析了微分项对系统性能的改善。但是纯微分项对噪声太敏感了，那我们要引出问题2了。
超前补偿器，顾名思义，就是把系统的相位往前拉一拉，增加系统的相位裕度，减小系统的滞后性，把纯微分项改成不完全微分然后在和比例项一起控制系统的控制方法就叫做不完全微分控制，下图展示了纯微分到不完全微分的变化过程（纯微分到不完全微分项）：
与kp项合并可得控制器表达式（这个表达式整体被称为超前补偿器）：
可以看到，不完全微分PD控制和纯PD控制（下面我用超前补偿器来代替不完全微分PD控制，可以说是一个东西）相比，给系统增加了一个极点，那么如何才能保证相位超前呢，也就是说，我纯PD控制给系统增加了一个零点，但是我超前补偿器在增加一个零点的同时还增加了一个极点。这里直接给出结论，零点绝对值小于极点绝对值，就可以保证相位超前，并且由根轨迹的之前说过的-180°定理可以保证根轨迹左移，提高系统稳定性和瞬态响应性能，那么零点绝对值大于极点绝对值呢，那就叫做滞后补偿器了，如果相等怎么办，相等那不就零极点相消了嘛，对系统无影响，顺便第一下，众所周知，系统增加一个零点，相位提前90°，增加一个极点，相位滞后90°，所以纯微分PD控制也是超前补偿器，只是相位超前了90°罢了。总结一下，这里只要零点绝对值小于极点绝对值，就是一个超前补偿器。可以看出，上式零点绝对值永远小于极点绝对值，所以咱这确实是一个超前补偿器。那么下一个问题就是，为什么超前补偿器对低频噪声不敏感，看看这是啥：
把kds看做输入，进过一个1/1+Ts后输出，1/1+T*s是一个一阶惯性低通滤波器啊，输入经过这个滤波器后会把高频的微分噪声过滤掉，我们来看看这个一阶低通滤波器的bode图就知道了，设T为2：
可以看出，这个低通滤波器的截止频率为0.5rad/s，当w>0.5rad/s时，幅频特性曲线快速下降，高频噪声被滤除。这就是不完全微分对高频噪声不敏感的原因，进而得知整个超前补偿器对高频噪声不敏感。那么问题2也已经说完了，来看看仿真效果吧：
黄线为不完全微分PD控制，橙线为原始输入数据，蓝线为纯比例控制。
当采用纯PD和不完全微分PD比较时，得出下图：
图中蓝色线为不完全微分PD控制，黄色线为纯PD控制，可以看出不完全微分PD控制对噪声有很大的抑制作用，输出比较稳定且平滑。
我之前写了一篇一阶互补滤波的文章，如果对本文中滤波部分有不明白的读者可以去看我写的上一篇文章。
PD控制C代码实现：
/*舵机的控制方法二：固定参数的舵机Pd控制*/
/*函数目的：获取控制舵机的PWM设定值*/
/*输入参数：中线平均偏差*/
/*输出参数:舵机PWM的设定值*/
int servopdcn2(float cha)
{
float nr,kp=500/90.0f;
float kd=1.2;
int PDOUT;
static int pr;
nr=cha;
PDOUT=(int)(kp*nr+kd*(nr-pr));
pr=nr;
if(PDOUT>250)
PDOUT=250;
else if(PDOUT<-250)
PDOUT=-250;
return(PDOUT);
}
以上。