PID实践笔记-两轮直立车的速度控制

写在前面：   

      不知不觉已经调了一个学期的车了，从让大公主（原型为恩智浦智能车竞赛E车模）学会站，到会走，最后能跑起来，简直就像是一个自己一把屎一把尿带大的孩子一样（可能带孩子还不如这个那么累吧），自己也成为了学弟学长眼中的老司机。感觉作车就和旅游爬山一样，山上的风景很美，但你永远也不知道翻过了这座山，后面还有多少座山在等着你，其中的艰辛与芳华，唯你自知。作车调车的过程中学到的知识很多，领悟的经验也挺多，一直想找个时间总结总结，但因为各种事一拖就拖了好久...嗯，我是拖延症患者......本文就是博主在爬其中一座山过程中的感悟，希望之后的各位新手司机能够少走些弯路，看到更好的风景。

好了扯犊子结束，下面进入正题：

       当小车开始能直立起来后，接下来的首要问题自然就是怎样能让他跑起来，先不说跑，但也至少能走吧，而如果在加入速度环之后，小车还是傲娇的立着不动，或者一个劲的哈腰点头（可能是快过年了想要红包了），或者就像脱缰的野狗一样，完全不按你设定的速度来，或者走到一半突然停下来（也许是想思考一下人生吧）...那肯定就是三个问题：1.小车机械结构不行，重心不够集中；2.PID参数没调好；3.速度环加入的姿势不对。若是第1种情况，长痛不如短痛啊少年赶紧改结构吧，下面主要就2.3种情况来谈谈如何以正确的姿势插入速度环。

       直立车的速度控制本质上就是一句话：用角度换取速度！换句话也可以说成是：速度环本身就是直立环的一部分。

       先从直立环开始说起吧，首先在直立环的控制部分，就实际情况而言，由于直立车机械结构的不同，直立车的机械零点也不同（甚至可能没有机械零点，敢设计出这种结构的一般都是大神，后面再讨论），而且机械零点大多都不是某个固定的角度，而是一个范围。比如我家的大公主，由陀螺仪反馈回来的数据显示，最后的结构搭建出来她的机械零点大概在-5°到5°左右，因此也可以看出在直立控制中我们应该求稳求快，但并不求精确，因而最好采用PD控制。

       下面附上直立环代码：

     

       其中Angle和Attitude.Gyro_Speed_Y是由陀螺仪返回的大公主当前俯仰角角度和俯仰角角加速度。而且在实际的PID应用中，根据不同情况，往往需要加入滤波、限幅和定时清零等操作，顺便提一句，这里的PD参数不是像书本上理论说的调的越大越好，而且应该适量的偏小，具体原因暂且按下不表，大家实际动手试试便知。而其中的PID控制器中KP所乘的东西，采用了串级PID的思想，也就是速度控制的关键啦，客官请接着往下看。

       说完直立环，接下来就讨论如何将速度控制添加进去了，当你的小车可以直立起来，并且直立环的PID参数调的比较“硬”的时候，可以像类似捉弄不倒翁一样去捉弄平衡车，可以很容易的发现，人为的使小车往前或往后倾斜，小车便会往前或往后跑了，也正是因为这一机制，小车才能实现直立。也就是说，当PID控制器检测出角度偏差（人为的）的时候，就会通过控制电机使得小车回到设置的目标角度，体现在实际中就是小车向前或者向后运动了。

      因此速度控制实际上就是通过对直立环进行干扰而实现的，也就是文章开头说的以牺牲角度为代价换取速度。而且在实际情况中速度控制应该求准，也就是能尽量达到预期的速度，因而推荐采用PI控制。

     下面就来说说如何“干扰直立环”，也就是传说中的串级PID。先来看看下面两个直立环的PD控制器和速度环的PI控制器：

                

      而在之前，是我们用手去人为改变小车的平衡角度，但怎样能让小车在平衡时自己“以为”自己的角度被改变了呢？我们可以将直立环的PD控制器改写成下面这样：

                

       而这串进去的a，其实就是速度环PI控制器的输出SpeedOut，这样，便构成了传说中的串级PID啦，注意其中的加减号需要根据你定义的小车前进方向按实际确定。从上面这个式子我们可以看出，即使小车当前处于平衡状态，也就是说角度偏差θ_Bias为0，但因为串入了速度控制量SpeedOut,会让小车傻傻的以为自己不在平衡状态，从而通过直立环的控制向前或向后运动，直到小车趋近设定速度后，SppedOut才会趋近于0。其实后面不难理解，能够构成串级PID，在于速度控制本身就是对直立控制的干扰，或者说速度环本身就可以看成是直立环的一部分。

       而前面提到的那些没有机械零点的车，意味这他们的小车永远无法通过加减速达到平衡状态，也就是说不能稳定的控制在某一速度下，甚至无法正常直立，他们的一生从打开驱动开始就是在不断的加速...从起点一直加速到终点，可想而知这需要多么超级无敌强大的转向环算法与之配合啊，传说中某届的国赛第二就是走的这条路，但个人建议新手还是绕道吧。

      下面附上速度环代码：

     

       同样在这里考虑到实际情况，加入了滤波和限幅等操作。

       最后再多啰嗦两句，这个方案和清华直立方案（电磁组直立行车设计方案2.0，竞赛秘书处）中的速度控制虽然有些不同，但依然要避免两个环之间的耦合过大，也就是要将速度控制的输出进行平滑处理，将其分配到多个直立控制周期中去，具体方法参考清华直立方案即可。

      如果对文章有任何的疑问或指教批评，欢迎在下方评论留言，希望各位同学，车友不吝赐教，共同学习。