飞思卡尔智能车之舵机算法

这篇文章，我将会从舵机的控制策略来向介绍我的调试经历，供新手进行参考，尽量详细的记录我们调试一步一步变化的过程，坦白的说我们的车子最终跑的不是很好，原因是各个方面的。但是我认为我们遇到的问题对新手应该还是有些用处的，时间原因，我将会不定期更新，望谅解。（已完结）

前言：

想要控制舵机，就需要先了解舵机的工作原理，这一点前辈们的资料相当的丰富，大家可以先行学习一下。

然后因为我们组做的是光电组，所以就引入一些摄像头的一些名词，其他的组别也可以根据这个思路进行舵机调节，不影响学习（仅限赛道组，无赛道组例外）

阶段一：

刚刚入手舵机首要目标当然是理解它是如何工作的，控制原理很简单，这个了解了以后我们就开始想办法让他工作了。那刚开始该怎么办呢？先想一下我现在有什么呢？摄像头传来的中线信息和有效行信息。那第一步就是利用这些数据，怎么用呢？

自然而然的我们就该想到，如果图像上面的中线是偏左的，说明我的车子是在中线的右边，那我就该让舵机往左边打角；如果中线是偏右的，说明我的车子是在中线的左边，那我们就该让舵机往右边打角。

那么问题来了，我怎么让单片机根据现在的中线数组知道我的车子是往左还是往右的呢？我们人看到图像的时候一眼就能够看到这个中线到底是往左还是往右、、、哎，等等，你怎么能够一眼看出来？你是不是经过了某种魔术的计算啊？啊啊啊，我没有啊，你看这图不是很明显中线在图像的左边吗？Emmm有道理，我们让这个中线数组和图片最中间的那个数组做差不就得到了吗？那该选那些行进行计算呢？管他那么多呢，先选上中间行再说吧。于是我们得到：

Deltax=Center_line[Image_H/2]-Image_L/2;

如果deltax>0说明中线在图像的右侧，否则就在左侧，那么打多少呢？当然是直接打到Limit了，肯定不会不够用。好了知道车子相对于赛道的位置，我们就可以控制舵机向对应的方向打角就好了。那我们不如给这个方法起个名字吧，那不如就叫棒棒算法吧（嗯，这个名字好像有那么点难听，但前辈们就是这样起的啊，我也很绝望啊）。

阶段二：

拿着棒棒算法弄出来的程序到赛道上面跑一跑，怎么回事，车子走个直线都走不稳，怎么一会这边一会那边，虽然整体在中间跑的，但是这也太难看了吧。只有一点点偏差舵机就会打角非常的大，怎么解决呢？当然是让车子距离中线近的时候让舵机打角小一点；当车子距离中线远的时候让舵机的打角大一点。那么这个距离中线的远近是？对，就是上一个阶段计算出来的deltax。

if(abs(Deltax)    Control=Limit/3;

else if(abs(Deltax)>=Image_L/6&&abs(Deltax)<=Image_L/3)

        Control=Limit*2/3;

     else

        Control=Limit;

当然啦大家想分多少段多可以了，我在这里只是拿三段举个例子。

我们就有了第二个阶段的东西，分段式棒棒算法，快整理一下代码烧进去试试效果。

阶段三：

棒棒算法对这个打角分了阶段，但是好像又发现了新的问题，当计算得到的deltax处在临界点附近的时候，舵机的 打角会在一个较大范围内进行摆动，这样看到车子的效果就是猛地一下在这边，然后就又变到了另一边，好像和第一阶段的棒棒算法差别不是很大，那该如何将它变得顺滑一点点呢？对，让对应范围内的打角变成线性的打角，随着这个线性系数的增加而逐渐变化的舵机打角值，这个东西其实就是一个纯p的pid算法，用代码表示就是：

 Control=S3010_kp*deltax+Contorl_median;

接下来的问题就是该如何调参数了，终于算是进入到舵机调试的核心部分了，那这个参数该如何进行调试呢？首先是进行计算，我需要算出来这个S3010_kp到底处于一个什么样的数量级范围内怎么算呢？左右极值的差除以我所要计算的delatx的范围好像就行；

S3010_kp=(Control_left_limit-Control_right_limit)/deltax_scale;

通过这个公式，我们就可以得到一个最为基础的打角kp值然后代入进去，发现效果好像还行，多跑几圈，感觉还不是很理想，那该怎么办呢？我的kp调节是不是还有点问题这样粗暴的算出一个kp好像不是太合适，那该怎么调节呢？这个时候就真的没有什么办法了，只能一点一点尝试，一个参数一个的实验，最开始的时候每次增加十，直到看到的效果不好，然后一点一点往下面减，直到车子跑起来还算顺畅的时候。在这个过程之前，一定要记得加上舵机打角的限幅代码，要不然我得到的舵机打角过大或者过小而机械结构达不到然后就大概率可以考虑换舵机了/无奈。限幅的方法呢，也很简单，这样就可以了

if(Control>Control_left_limit)Control=Control_left_limit;

if(ControlControl_right_limit

到这，纯p调节的舵机打角就算完成了，下一步就是继续深化了，要求不高的话这样子得到的kp值也是可以使用的，整体而言看不到太大的问题，如果想要继续优化，那就要再往下进行调节了。

阶段四：

纯p调节得到的舵机打角虽然能够工作，但是如果想要提速，就会参数的问题就会大范围的暴露出来，速度一高，跑出来的效果还是很差，总能感觉到我们的舵机打角不够或者太多的问题，我们还是应该对p进行精细化调节，那到底该怎么精细呢？查表法，怎么查表呢?我们的每一个deltax都会对应着一个Control值，我们把这个关系列出来，当得到这个deltax的时候查询一下此时的kp然后算出对应的Control，我们不就得到了相对应的关系。那么问题来了，我们该如何得到这个表呢？

这个时候，就该进行一些数据测量了，我们需要从赛道上得到几组数据，怎么测量呢，让舵机固定打角，然后推着他走弯道，直到它能够完美的过来这个弯道，然后记录此时的舵机打角值和得到的deltax值，再测试不同的弯道得到至少三组数据，正走反走都需要那样就是至少六组数据得到数据以后该怎么办呢？这么点数据肯定不够查表，那我们就需要对数据进行一下处理了，最有用的办法就是对所需要的数据进行一下拟合，一般上拟合成二次函数曲线就可以了，太高次数会更加接近我们所需要的结果，但是因为数据有限，我们也只能得到这个值。那么数据拟合该怎么做呢？当然是强大的MATLAB了，参考代码如下：

name='6.2.2';

jpg=strcat(name,'.jpg');

txt=strcat(name,'.txt');

x=[59 43 19 51];y=[530 390 255 400];%参考数据，这是一边的数据

p=polyfit(x,y,3);

x1=1:80;

y1=polyval(p,x1);

h=plot(x,y,'r',x1,y1,'b');

k1=zeros(80,1);

fid=fopen(txt,'w');%将生成的数据写到txt文件里面方便复制

for i=1:length(y1)

    k1(i)=y1(i);

    fprintf(fid,'%3.2f,',k1(i));

end

fclose(fid);

这样我们就得到了一个表，有了这个表，我们再进行查表，舵机的打角就变得更加圆滑了，这个阶段的任务也就完成了。

阶段五：

做过上面的这些步骤以后，车子已经能够满足大多数情况的转弯打角等，似乎是没有什么再改变的空间了，但是在不断地加速以后，我们还是发现了问题，车子转弯的时候有点缓慢，容易冲出去，那该怎样进行调整呢？当车子快到转弯的地方的时候，我们希望他就提前打角，舵机自身的响应延迟加上此时的高车速，提前打角显得非常有必要，那该如何提前呢？加大我算出来的值，让deltax开始变大的时候我们就加上一个微分d进行校正，让得到的舵机打角更加偏向我所期待的值；当要出弯的时候，我期待车子响应不要那么快，解决方式也是对这个偏差的差值乘以某个系数加到我们的舵机打角上面。这些，其实就是我们pid调节中的微分调节过程。

Control=Control_median+S3010_kp*deltax+S3010_kd*(deltax_last-deltax_now);

那么下一个问题就是，这个kd的参数该如何确定呢？首先我们要确定都有哪些因素影响了kd，首先是偏差的差值，这个是我们主要要进行校正的部分，其次是速度，我们的速度不一样得到的kd值也应该是有那么一点点差别的，所以主要根据这两个之间的关系，确定出一个比较好的值。其实如果kp的值能够调好，kd调节作用不会太明显，除非你能到达一种超高速。不过车速那么快，必定老司机。老司机就不用在乎这些了。

至此，我们组在调试舵机的时候所用到的调试方法大致讲述完了，希望你看完这篇文章以后能加快调车进程，然后开始为其他的问题头疼吧（哈哈哈哈哈哈哈）。