在上一次谈到基于MPU6050的基于一阶互补滤波算法实现后，本来想接着就自适应一阶互补滤波和卡尔曼滤波再写一篇的，但是卡尔曼滤波算法我自己写出来并进行姿态解算后发现效果不很好，才疏学浅，等我调好了再写吧，昨天花了半下午做了一个基于MPU6050的舵机云台自稳定系统，采用PD控制，下面就从控制理论原理到代码实现来谈一谈。 1.P项对系统性能影响的分析 假设现在我们有如下一个系统：

这个系统采用简单的比例控制，当kp=1时，我们来分析一下他的稳定性，从奈奎斯特曲线的角度来分析：

由P-Z=N这个奈奎斯特判据来看，P作为开环传递函数在右半平面的极点数等于1，N作为奈奎斯特曲线逆时针绕（-1,0j）这个点的圈数等于0，所以Z等于1，意味着闭环传递函数在右半平面的极点数为1，系统不稳定。我们可以通过时域响应来验证一下： 我们扩大kp值，将kp变为10：

由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是两个分界点。 当kp250) PDOUT=250; else if(PDOUT