飞思卡尔算法精选

第五章智能汽车软件设计第1章电机控制1.1.直流电机控制策略：针对本文所研究的智能车来说，车体速度是大惯性的被控对象。算法输出的控制量只能对电机输出力进行控制。而有一定负载时电机的输出力无论对车轮的转速还是车体的形式速度都是不成正比的，车在刚开始启动的时候速度是零，而电机的输出可能很大；车在匀速行驶的时候速度很快，而电机的输出可能并不是很大。而且电池电量、车体重量都会对车速造成影响。因此只有用闭环才能对车速进行良好的控制。在车轮对地面不打滑的情况下车体的速度和后轮的转动速度是成正比的。因此我们可以直接用光电码盘对后轮的转速进行控制。对于这样一个大惯性系统，我们选用PID和鲁棒相结合的办法进行速度控制。回路的设定值由经验值确定。考虑到速度控制通道的时间滞后比较小，因此采用PID控制方案，并在进行加减速控制时，引入了“棒棒控制”。U(k+1)=U(K)+P1*e(k)+P2*(e(k)–e(k-1))+P3*((e(k)–e(k-1)–(e(k-1)–e(k-2)));公式11PID的公式其中第一项为积分项；第二项为比例项；第三项为微分项。考虑到被控对象（车体速度）本身是一个大的积分环节，公式中可以将第一项省略，即采用PD控制。E为误差。同时设定误差门限，在误差比较大的时候采用大输出控制电机，将误差在最短时间内减小到所要求的范围，这就是鲁棒控制的思想。电机控制策略，其实就是对模型车速度的控制策略，它是继路径识别之后非常重要的策略，直接关系到整个模型车比赛的性能。速度控制得当，小车才能以最好的状态，在最短的时间完成比赛路程。1.1.1.弯道速度控制在模型车入弯时刻出于稳定性考虑做减速控制。减速原则是在原来直道速度设定值的基础上，减小速度设定值到低速挡，保证模型车可以安全入弯。另一方面，入弯之后，为了过弯时模型车能够不明显的左右摆动并采用较好的姿态通过弯路，第五章智能汽车软件设计令车速与偏差成线性关系，偏差越大速度设定值越小。速度设定方法如公式12。s1v(k)=v-k*e(k)……………………………………………………………….公式12其中：sv(k)为速度闭环设定值；v模型车全程运动平均速度设定值；e(k)车体偏离理想轨迹的当前偏差值；1k为减速控制比例系数。同时，通过实验发现模型车入弯之后，令模型车以某一线性规律加速运行可以使车在不冲出轨道的前提下以更短的时间通过弯道。控制规律如.公式13。sv(k)=v+Ck*Cp…………………………………………………………….公式13其中：Ck为弯道加速系数；Cp弯道加速变量；Ck为常数，初始化时设定。Cp入弯时刻初始化为0，每个控制周期累加1。1.1.2.直道速度控制直道采用匀速控制方案，速度设定在此控制系统下可以控制的最大入弯速度。在从弯道驶出进入直道后须进行加速控制，增大速度设定值。因速度控制回路有一定的惯性，所以在提速时为了使速度迅速改变，加入了“棒棒控制”。控制规律如公式14。sapv(k)=v+100-K*k…………………………………………………………………公式14其中：aK补偿效果控制系数；pK调速补偿变量。aK为常数，初始化时设定。pK出弯时刻初始化为0，每个控制周期累加1。1.1.3.PID闭环控制算法根据路径识别的情况，如果当前路径为直道，则需要加速；若是弯道，则需要降速，而且根据不同的弯道速度也是有所区别。系统利用测速模块反馈的当前速度值，通过增量式PID算法进行调节，从而控制直流电机对当前路径进行快速反应。增量式PID的算式为：第五章智能汽车软件设计()()()()()()()1212PIDukekekekekekekKKK∆=−−+⋅+−−+−⎡⎤⎡⎤⎣⎦⎣⎦在增量式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进行及时调整，反映到实际中，就是小车及时减速并顺利通过弯道。在增量式PID处理的过程中，有一个步骤需要注意，即在算完△u(k)后，需要把它赋值给电机控制对应的PWM通道信号，这时要判断该△u(k)的值，如果它小于0，则把PWM信号赋值为0，如果它大于PWM信号的最大值，即大于PWM整个周期所对应的数值时，要把PWM信号赋值为该最大值。1.2.反向制动算法要使赛车在较短的时间完成比赛，速度自然越高越好，显然速度太高弯道是过不去的，如果以弯道的极限速度匀速跑，又浪费了直道的时间。所以最佳的策略是直道以较高的速度跑，到弯道时再尽快将速度降下来。在入弯减速时如果只靠赛道第五章智能汽车软件设计的摩擦阻力效果显然是不够理想的。为此我们引入反向制动算法。由于MC33886芯片集成的H桥驱动电路本身就具有反向制动功能，所以不需再外加硬件电路。反向制动流程图如图：是否为弯道当前车速是否超出该曲率对应极限车速判断曲率大小反向制动加速到目标车速否是否是RS380电机在制动时，电机反向电动势对整个系统电路造成的冲击，从而引起单片机工作不稳定，电流过大导致电机过热、电机频繁换向导致电刷打火剧烈导致电机寿命缩短等一系列问题都是需要考虑的。1.3.速度控制策略1、为了达到好的速度控制效果，对速度进行闭环控制是必须的。这里所说的速度控制策略是指设定速度的确定方法——设定速度主要由道路与直道的偏差来决第五章智能汽车软件设计定，道路越接近直道，设定速度越高，反之越低。车模行驶中的最低速度是这样确定的：令车模以较低的速度匀速行使，在保证无犯规行为出现的前提下，逐渐提高匀速行使的速度，直到车模出现犯规行为，此速度再减去一个安全量，即为所需的最低速度。简单的说，变速行使的最低速度等于匀速行使的最高速度。车模行驶中的最高速度是这样确定的：在确定最低速度以后，加入变速策略，不断提高最高速度的设定值，直到模型车出现犯规行为，此速度再减去一个安全量，即为所需的最高速度。车模行驶过程中难免出现“失去道路”的情况，对此需要采取一定的安全策略，防止赛车“盲跑”而导致犯规。2、针对本文所研究的智能车来说，车体速度是大惯性的被控对象。算法输出的控制量只能对电机输出力进行控制。而有一定负载时电机的输出力无论对车轮的转速还是车体的形式速度都是不成正比的，车在刚开始启动的时候速度是零，而电机的输出可能很大；车在匀速行驶的时候速度很快，而电机的输出可能并不是很大。而且电池电量、车体重量都会对车速造成影响。因此只有用闭环才能对车速进行良好的控制。在车轮对地面不打滑的情况下车体的速度和后轮的转动速度是成正比的。因此我们可以直接用光电码盘对后轮的转速进行控制。对于这样一个大惯性系统，我们选用PID和棒棒相结合的办法进行速度控制。回路的设定值由经验值确定。考虑到速度控制通道的时间滞后比较小，因此采用PID控制方案，并在进行加减速控制时，引入了“棒棒控制”。U(k+1)=U(K)+P1*e(k)+P2*(e(k)–e(k-1))+P3*((e(k)–e(k-1)–(e(k-1)–e(k-2)));公式43PID的公式其中第一项为积分项；第二项为比例项；第三项为微分项。考虑到被控对象（车体速度）本身是一个大的积分环节，公式中可以将第一项省略，即采用PD控制。E为误差。同时设定误差门限，在误差比较大的时候采用大输出控制电机，将误差在最短时间内减小到所要求的范围，这就是棒棒控制的思想。电机控制策略，其实就是对模型车速度的控制策略，它是继路径识别之后非常重要的策略，直接关系到整个模型车比赛的性能。速度控制得当，小车才能以最好的状态，在最短的时间完成比赛路程。第五章智能汽车软件设计1.3.1.弯道速度控制在模型车入弯时刻出于稳定性考虑做减速控制。减速原则是在原来直道速度设定值的基础上，减小速度设定值到低速挡，保证模型车可以安全入弯。另一方面，入弯之后，为了过弯时模型车能够不明显的左右摆动并采用较好的姿态通过弯路，令车速与偏差成线性关系，偏差越大速度设定值越小。速度设定方法如………………………………………………………………………….公式44。………………………………………………………………………….公式44其中：为速度闭环设定值；模型车全程运动平均速度设定值；车体偏离理想轨迹的当前偏差值；为减速控制比例系数同时，通过实验发现模型车入弯之后，令模型车以某一线性规律加速运行可以使车在不冲出轨道的前提下以更短的时间通过弯道。控制规律如.公式45。……………………………………………………………………….公式45其中：为弯道加速系数；弯道加速变量；为常数，初始化时设定。入弯时刻初始化为0，每个控制周期累加1。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控制系统原理框图控制规律为：子程序入口读速度传感器的值读加速度传感器的值读两个16位脉冲计数器的值返回主程序第五章智能汽车软件设计01()()[()()]tPDIdetutKetetdtTTdt=++∫（5.1）式中：pK：比例增益，pK的倒数称为比例带；IT：积分时间常数；DT：微分时间常数；()ut：控制量；()et：偏差，等于给定量与反馈量的差。在计算机控制系统中，数字PID控制算法通常又分为位置式PID和增量式PID。本次设计中，我们采用增量式PID。增量型算法与位置型算法相比，具有以下优点：增量型算法不需要做累加，增量的确