飞思卡尔所有的算法Word格式.docx
《飞思卡尔所有的算法Word格式.docx》由会员分享，可在线阅读，更多相关《飞思卡尔所有的算法Word格式.docx（76页珍藏版）》请在冰豆网上搜索。
1.1.1.弯道速度控制
在模型车入弯时刻出于稳定性考虑做减速控制。
减速原则是在原来直道速度设定值的基础上，减小速度设定值到低速挡，保证模型车可以安全入弯。
另一方面，入弯之后，为了过弯时模型车能够不明显的左右摆动并采用较好的姿态通过弯路，令车速与偏差成线性关系，偏差越大速度设定值越小。
速度设定方法如………………………………………………………………………….公式12。
………………………………………………………………………….公式12
其中：
为速度闭环设定值；
模型车全程运动平均速度设定值；
车体偏离理想轨迹的当前偏差值；
为减速控制比例系数。
同时，通过实验发现模型车入弯之后，令模型车以某一线性规律加速运行可以使车在不冲出轨道的前提下以更短的时间通过弯道。
控制规律如……………………………………………………………………….公式13。
……………………………………………………………………….公式13
为弯道加速系数；
弯道加速变量；
为常数，初始化时设定。
入弯时刻初始化为0，每个控制周期累加1。
1.1.2.直道速度控制
直道采用匀速控制方案，速度设定在此控制系统下可以控制的最大入弯速度。
在从弯道驶出进入直道后须进行加速控制，增大速度设定值。
因速度控制回路有一定的惯性，所以在提速时为了使速度迅速改变，加入了“棒棒控制”。
控制规律如…………………………………………………………………公式14。
…………………………………………………………………公式14
补偿效果控制系数；
调速补偿变量。
出弯时刻初始化为
，每个控制周期累加
。
1.1.3.PID闭环控制算法
根据路径识别的情况，如果当前路径为直道，则需要加速；
若是弯道，则需要降速，而且根据不同的弯道速度也是有所区别。
系统利用测速模块反馈的当前速度值，通过增量式PID算法进行调节，从而控制直流电机对当前路径进行快速反应。
增量式PID的算式为：
在增量式PID处理的过程中，有一个步骤需要注意，即在算完△u（k）后，需要把它赋值给电机控制对应的PWM通道信号，这时要判断该△u（k）的值，如果它小于0，则把PWM信号赋值为0，如果它大于PWM信号的最大值，即大于PWM整个周期所对应的数值时，要把PWM信号赋值为该最大值。
然后，再存储本次△u（k），和上次△u（k）。
error=speed_v-infrared_value7;
pwmtemp=PWMDTY23+PID_P*（error-last_error）+PID_I*（error）+PID_D*（error+pre_error-2*last_error）;
由上述代码中，speed_v表示标准速度，infrared_value7表示ATD1转换的经公式4.5计算出的即时速度值，通过计算它们的差值error，再利用增量PID控制算法计算出pwmtemp，再对pwmtemp进行处理，最后PID程序处理的结果是给出一个PWM信号，而这个信号就是驱动电机的，当error大，即标准速度和即时速度相差的比较多时，pwmtemp的绝对值相对比较大，则给电机的PWM信号相对较大，这样电机转速相对较快；
反之，电机转速则相对较慢。
由此可以看出，PID算法主要的功能是，在闭环系统中，利用即时速度的反馈，使得即时速度逼近标准速度的时间尽量变短，这样小车就可以根据路径识别得出的速度标准值，及时调整自己的速度，以适应各种路况。
例如，小车正在直道上行进，而且直道的时速应该是各种赛道中最快的，当系统的路径识别算法察觉到前方出现弧度大的弯道后，系统会根据事先调试的结果，给出大弧度弯道的速度标准值，然后PID算法根据速度标准值speed_v和即时速度反馈值infrared_value7对电机的输入信号PWM进行及时调整，反映到实际中，就是小车及时减速并顺利通过弯道。
1.2.反向制动算法
要使赛车在较短的时间完成比赛，速度自然越高越好，显然速度太高弯道是过不去的，如果以弯道的极限速度匀速跑，又浪费了直道的时间。
所以最佳的策略是直道以较高的速度跑，到弯道时再尽快将速度降下来。
在入弯减速时如果只靠赛道的摩擦阻力效果显然是不够理想的。
为此我们引入反向制动算法。
由于MC33886芯片集成的H桥驱动电路本身就具有反向制动功能，所以不需再外加硬件电路。
反向制动流程图如图：
在制动时，电机反向电动势对整个系统电路造成的冲击，从而引起单片机工作不稳定，电流过大导致电机过热、电机频繁换向导致电刷打火剧烈导致电机寿命缩短等一系列问题都是需要考虑的。
1.3.速度控制策略
1、为了达到好的速度控制效果，对速度进行闭环控制是必须的。
这里所说的速度控制策略是指设定速度的确定方法——设定速度主要由道路与直道的偏差来决定，道路越接近直道，设定速度越高，反之越低。
车模行驶中的最低速度是这样确定的：
令车模以较低的速度匀速行使，在保证无犯规行为出现的前提下，逐渐提高匀速行使的速度，直到车模出现犯规行为，此速度再减去一个安全量，即为所需的最低速度。
简单的说，变速行使的最低速度等于匀速行使的最高速度。
车模行驶中的最高速度是这样确定的：
在确定最低速度以后，加入变速策略，不断提高最高速度的设定值，直到模型车出现犯规行为，此速度再减去一个安全量，即为所需的最高速度。
车模行驶过程中难免出现“失去道路”的情况，对此需要采取一定的安全策略，防止赛车“盲跑”而导致犯规。
2、针对本文所研究的智能车来说，车体速度是大惯性的被控对象。
对于这样一个大惯性系统，我们选用PID和棒棒相结合的办法进行速度控制。
公式43PID的公式
同时设定误差门限，在误差比较大的时候采用大输出控制电机，将误差在最短时间内减小到所要求的范围，这就是棒棒控制的思想。
1.3.1.弯道速度控制
速度设定方法如………………………………………………………………………….公式44。
………………………………………………………………………….公式44
为减速控制比例系数
同时，通过实验发现模型车入弯之后，令模型车以某一线性规律加速运行可以使车在不冲出轨道的前提下以更短的时间通过弯道。
控制规律如……………………………………………………………………….公式45。
……………………………………………………………………….公式45
1.3.2.直道速度控制
控制规律如…………………………………………………………………公式46。
…………………………………………………………………公式46
出弯时刻初始化为，每个控制周期累加。
1.3.3.速度及加速度信号采集子程序
当图像数据采集处理完后，我们读取速度传感器和加速度传感器的值，采集这两个传感器信号的频率也为60Hz。
流程图如图4.3所示。
图4.3速度及加速度信号采集子程序流程图
1.4.速度PID算法
根据偏差的比例（Proportional）、积分（Integral）、微分（Derivative）的线性组合进行反馈控制（简称PID控制），数字PID控制算法是电机微机控制中常用的一种基本控制算法[9]。
在连续系统中，模拟PID调节器是一种线性调节器，控制系统原理框图如图5.10。
图5.10PID控制系统原理框图
控制规律为：
（5.1）
式中：
：
比例增益，
的倒数称为比例带；
积分时间常数；
微分时间常数；
控制量；
偏差，等于给定量与反馈量的差。
在计算机控制系统中，数字PID控制算法通常又分为位置式PID和增量式PID。
本次设计中，我们采用增量式PID。
增量型算法与位置型算法相比，具有以下优点：
增量型算法不需要做累加，增量的确定仅与最近几次偏差采样值有关，计算精度对控制量的计算影响较小，而位置型算法要用到过去偏差的累加值，容易产生大的累加误差；
增量型算法得出的是控制量的增量，而位置型算法的输出是控制量的全量输出，误动作影响大；
采用增量型算法，易于实现手动到自动的无冲击切换。
在实际应用中，采样的反馈值
即为脉冲累加器中的PACN32中的脉冲数，预设门限值
在参数整定时根据实际情况调节，输出
并不能直接用来控制电机，需要将其转换为控制PWM占空比，然后用增大或减小PWM占空比的方法来实现对电机的加减速的控制。
换句话说，在求偏差量时，实际上用的是每20ms电机转过的齿轮数和实际期望电机转过的齿轮数，通过二者的差值，再乘以相应的系数，即KP、KI、KD的协调控制，计算出相应的PWM占空比，实际上用的是PWMDTY的值[10]。
本设计中综合考虑各种因素，最后选用的采样周期为20ms，即每20ms对电机进行一次PID调节。
由于在程序中，对图像的采集使用的是PH口的中断程序，因此，PID采样周期的选择实际上是受限制与图像采集，因为每行的扫描周期为64μs，有效扫描时间为52μs，采用的是隔行扫描的方式，即每隔6行采集一行图像的信息，如果在每行之间加入PID调节的话，那么处理PID子程序的时间必须控制在64*5＝320μs之内，另外图像采集只是采集了奇场中的行数，在偶场中没有采集，因此PID子程序的执行是不均匀的，并没有达到预期的效果，同时还可能会影响到视频采集，因此，经过分析，最终决定将PID的采样周期定为20ms，即当进行一次场采集进行一次PID调节。
而且经过最终的检验，这样能够满足对速度控制的需要。
电机调速
本作品采用增量式的PI控制。
图4.12
增量式PI算法:
pwm=Kp*（errork0-errork1）/100+Ki*errork0/100+pwm;
1.5.直流驱动电机控制子程序
本设计通过调整方波的占空比来实现电机的调速。
经过初始化子程序的设置，PWM0和PWM5都能输出频率为5KHz的方波。
由硬件部分可知，PWM0产生的方波经MC33886功率放大后接到电机的负极，PWM5产生的方波经MC33886功率放大后接到电机的正极。
车模向前行驶时，PWM0输出方波的占空比为零，通过调整PWM5输出方波的占空比实现加速或缓慢减速。
当需要快速减速时，PWM5输出方波的占空比为零，PWM0输出方波的占空比为合适的值，电机反转实现车模制动。
该部分程序工作流程图如图4.7所示。
1.6.舵机控制
1.6.1.舵机转角的决策
由于CCD图像传感器的应用，大大提高了车的前瞻性。
经测量，本系统预测距离可达1米以上。
但是，考虑到各部件延时严重且具有很大不确定性，所以预测距离越远，控制算法就会越复杂。
综合各种因素考虑，我们最终将预测距离定在80~100cm。
黑线中心位置轨迹中包含着两方面的信息，一方面是反映前方道路趋势的信息，另一方面是反映车体与轨道偏离程度的信息。
经过反复试验，我们发现，前者可以通过计算黑线的斜率来很好的描述。
而后者，恰恰就是屏幕最下端黑线的位置。
最后用这二者的线性组合得到舵机转角。
有了这两方面信息，可以使智能车既能充分利用前瞻性，又能不过分地偏离轨道。
从而得到了较理想的转角控制策略解决方案。
1.6.2.偏航距离的计算
由于已经获得了赛道中心线的位置，所以计算偏航距离的问题是选取何处的中心线的距离为当前的偏航距离。
控制算法的执行周期为40ms，如果赛车的速度为2m/s，则在两次控制算法的执行中间，赛车要前进8cm，赛车所处的环境将发生比较大的改变，所以赛车的控制只能算是半实时控制，这是所有使用摄像头作为主要
寻线传感器的参赛队都避免不了的问题。
因为算法的滞后性，赛车需要将“当前位置”进行适当前移。
前移量应该跟赛车当前速度成正比，但实际中我们发现，适当增加一些前移距离是有好处的，因为可以在入弯处提前转弯，使得赛车沿弯道内侧行驶，缩短了过弯距离。
1.6.3.舵机参数测定
通过实验测出已安装在模型车上的舵机的中位PWM值（Servo_center），模型车转向轮左右极限时舵机PWM（Servo_left、Servo_right）值是舵机控制的基础。
测试舵机中位时，先在舵机控制模块中给舵机PWM设计一个全局变量（Pwm_Servo），将赛车放在长直道上，保证车身放正并处于赛道正中间，利用BDM查看此时Pwm_Servo的值，将此值直接赋给舵机PWM，并将赛车放到直道上跑，看是否跑偏，微调Pwm_Servo的值，直到赛车在直道上不再跑偏为止。
测定舵机左右极限时，可根据转向轮的左右极限转角，利用BDM查看舵机Servo_left和Servo_right的值。
注意转向轮在左右极限转角时，转向轮不可被赛车底盘卡住，这样不但会增加转向阻力，严重时还可能使转向轮抱死，失去转向能力。
舵机的PWM计算公式为：
Pwm_Servo=Servo_center+（Servo_left-Servo_right）/Simage_number*offline_one;
……公式四
式中Simage_number为每行采集的点数，
offline_one为黑线在整幅图像中的位置。
1.6.4.路径规划
要想以最短的时间完成比赛，并不需要严格按照黑色引导线行驶，我们需要找出相对较短的行驶路径，即路径规划。
例如过普通弯道的时候，以同样的速度走内弯显然比走中间或外弯所花时间少，过小S形弯时。
直接过去和严格循迹相比速度快，行驶距离短等等。
（图）
如果能提前预知前方赛道信息，那么我们就有可能对路径进行规划。
由于我们采用的是摄像头，扫描距离较大，这就为我们进行路径规划提供了条件，这也是我们选择摄像头的原因之一。
由于摄像头看得较远，每幅图像反映的赛道信息量较大，我们需要针对赛车在不同位置，对其有侧重的采用。
具体就是将采到的15行划分为几个区域，针对不同路况对每个区域赋以不同的权值。
其原理图如图：
图5.10路径规划示意图
偏航距离的计算
控制算法的执行周期为40ms，如果赛车的速度为2m/s，则在两次控制算法的执行中间，赛车要前进8cm，赛车所处的环境将发生比较大的改变，所以赛车的控制只能算是半实时控制，这是所有使用摄像头作为主要寻线传感器的参赛队都避免不了的问题。
前移量应该跟赛车当前速度成正比，但实际中我们发现，适当
增加一些前移距离是有好处的，因为可以在入弯处提前转弯，使得赛车沿弯道内侧行驶，缩短了过弯距离。
1.6.5.偏航角度的计算
计算偏航角度的实质是直线拟合问题，因为赛道中心线所在的直线确定了，而直线的斜率与偏航角度一一对应。
直线拟合最有效的方法是最小二乘法[7]，但是直接应用存在一个问题，即如何确定进行直线拟合的区间？
在整个成功识别出赛道的区间内进行直线拟合显然是欠缺考虑的，因为在弯道的情况下，这种方法拟合出的是一条弦线，而不是当前该弯道处的切线。
摄像头视野越大，弯道曲率越大，弦线偏离切线的程度也就越大。
为了能够在直道和弯道上都能正确的拟合出正确的直线，我们采用了直线检测的方法，即首先根据残差的大小确定直线的范围，然后在这一范围内进行直线拟合。
1.6.6.曲率的计算
如果说斜率的计算需要某种技巧的话，计算曲率则更是一种技巧的应用。
首届时很多参赛队针对各自的实际需要，提出了自己的方法。
其中最普遍的是根据斜率的导数来计算曲率[8]。
但是斜率的计算本身就很不准确，特别是某个点的斜率，对斜率求导就更不准确，所以使用这种方法只能得出一个大致的结果。
本文作者提出了另外一种方法，首先对获得的路径进行滤波，使得路径尽可能平滑，然后取其两个端点和中间点，计算这3个点组成的三角形的外接圆的半径，半径的倒数就是这段路径的曲率。
经过多次实验，这种方法的误差一般不大于20%，对智能车的控制来说已经足够了。
图5.8是实验赛道的照片，让赛车从起跑线开始，在赛道上行驶一圈，记录下每个时刻的曲率，如图5.9。
从图5.9可以看出，计算出的曲率能较为正确的反映实际赛道的弯曲情况。
但是这种方法得出的曲率不是摄像头所看到的当前位置的曲率，而是摄像头所看到的
路径的整体曲率，因为这种算法仅仅与路径中三个点的位置有关。
然而实验中却发现，这种特点反而给赛车带来了一个好处：
即摄像头在小S型弯道时舵机几乎不跟随路径的摆动而摆动，而是直线冲过。
这是因为较小的S型路径其弯曲部分能完整的显现在摄像头的视野中，而算法中对路径进行了滤波，滤除了中间部分的弯曲，使得路径变直了，赛车因此就直线走过。
1.7.车辆横向控制算法
本设计采用的横向控制算法是模拟人驾驶汽车的方式控制车模转向。
首先，根据图像数据处理后的数据判断车模偏离黑线的距离。
其次，综合考虑车速传感器获得的当前速度值，计算目标转角。
然后，在下一次采样时，与上次采样值比较，判断偏离距离的变化趋势，再次计算目标转角，当车模回到中线位置时，前轮回正。
该部分程序流程图如图4.4所示。
图4.4横向控制算法流程图
1.8.车辆纵向控制算法
本设计中模型车的速度控制采用PID控制算法。
首先，图像数据处理后的数据判断车模当前行驶的路况，得到目标速度值。
其次，根据车速度传感器的值，判断车速是否达到目标速度，进行加减速。
第三，结合加速度传感器的值判断模型车是否发生滑移，如果滑移进行防滑处理。
该部分程序流程图如图4.5所示。
图4.5纵向控制算法流程图
1.9.转向控制子程序
由于舵机反应速度的局限，本设计每10ms对舵机控制一次。
可知PWM23已经在初始化子程序中设置完毕，其能产生周期为20ms的方波，在此我们只需修改PWMDTY23寄存器即可改变方波的占空比，进而控制舵机转向。
本车舵机的安装位置和PWM23通道的设置使得前轮在中间位置时，PWMDTY23的值为4960，左极限位置时值为5300，右极限位置时为4640，即将左极限位置到右极限位置的转向级别分为660级。
为了防止舵机转向超过极限位置，发生卡死现象，损坏舵机，本设计通过软件限定极限转角。
该子程序工作过程如图4.6所示。
图4.6舵机控制子程序流程图
1.10.车体控制策略：
对于硬件结果已经确定的智能车来说，算法控制的实际上就是车体的方向和速度，即前轮舵机的转角和后轮的转速。
如果前面图像识别和虑噪环节做的好的话，对于控制量的给出的算法就相对容易，但是需要花很大精力进行参数的调整，使得车的行驶轨迹更加优化，并且让速度尽可能的提高。
对于前轮的控制来说，由于执行机构（舵机）和被控对象的惯性相对较小，换句话说算法给出的控制量在误差允许的范围内可以认为由执行机构准确执行，可以进行开环的控制[2]。
我们的控制算法是在图像中分别取上、中、下三个点，即在23行的数组中取3、12、21三个点，计算出图像的位置，然后用最上面一点和最下面一点的差做为图像的斜率。
然后用两者的线性组合算出舵机的转角。
用下面公式给出：
Warp=P1*（Average[2]+Average[11]+Average[20]）+P2*（Average[20]–Average[2]）
公式15舵机的转角公式
其中，warp是车体对于黑线的误差，也是舵机应该转动的角度。
同时，我们发现在车运行过程当中经常会出现图像中只有半幅图像有线，或者整幅图像都没有黑线的情况。
在半幅图像有线的情况下，我们就用图像的在视野中的终点、图像最下面的点及两者的中点进行控制；
如果整
升级会员 