《循迹算法》由会员分享，可在线阅读，更多相关《循迹算法（9页珍藏版）》请在金锄头文库上搜索。
1、1 路径识别算法11 抛物插值法拉格朗日插值多项式：Ln xk0nyk j0,jkn x xjxk xj当 n=2 时，此公式称为抛物插值。设 x0 x1 x2，且x1 x0 h,x2 x0 2h，代入拉格朗日插值多项式可得抛物线方程，并对抛物线方程求导数，令导数等于零，可得：x x0 h h y0 y22y0 2y1 y2x 就是所要得到的车模偏离跑道中心的距离。12 最小二乘法m xi . xinxi xi2 .xin1: : . :xin xin1 . xi2na0a1:anxixiyi:yi xin为保持精度，令 m=4，选择函数类型 f x c bx ax2，上式可写成：4 xi x
2、i2xi xi2 xi3xi2 xi3 xi4cbayiyixiyi xi2令 x0 x1 x2 x3， x1 x0 h,x2 x0 2h,x3 x0 3h，已知 x b2a是偏移量，把上述式子的解代入化简得：x h x0 h 11y0 3 y1 y4 7 y310y0 y1 y4 y32 控制算法21 数字 PID 控制211 PID 控制规律：比例环节。跟随性好，能即时成比例地反映控制系统的偏差信号 e(t)，Kp 增大，系统动作灵敏。在系统稳定的情况下，增大 Kp，有利于减小稳态误差，提高系统控制精度，但随着Kp 过大时，系统趋于不稳定。积分环节。由上式知，它是对误差 e(t)进行积分，
3、用于消除静差，提高系统的无差度。 Ti过小，积分作用强，系统将不稳定，振荡次数增多；Ti 过大，积分作用小，影响控制精度。微分环节。反映偏差信号的变化速率，并能在偏差信号值变得太大之前，在系统中引入一个有效的早期修正信号，从而加快系统的动作速度。对于滞后时间长的系统应增大微分的作用。212 流程图增量式 PID 算法流程图设置 A、B、C、f(k)f(k)=Ae(k)-Be(k-1)+Ce(k-2)设置 e(k-1)=e(k-2)=0输出 f(k)=f(k-1)+f(k)e(k-1)-e(k-2), e(k)-e(k-1)出口入口#include#includemain()float a,b,
4、c,u=1,u1;int i;float e0,e1,e2,v;printf(input v: ); /*程序中所用的输入输出函数都只是用于调试*/scanf(%f,a=1.01;b=0;c=0.01;e1=e2=0;e0=v;for(i=0;ie(k-2), e(k)-e(k-1)设置 A1、B1e(k-1)=0u(k)=A1e(k)+B1e(k-1)e(k)-e(k-1)u(k)int R35=60,60,60,60,20,40,40,40,20,20,20,20;struct ttfloat t;float u;main()int i,j,p=0;float a=0.0,b=0.0;fl
5、oat r35;float xt5;float xm3;struct tt T4;float G=0.0,x1,x2;for(i=0;i=0.0&x1=35.0&x1=40.0if(x1=55.0&x1=60.0&x1=72.0&x1=75.0&x1=87.0&x1=90.0&x1=102.0)xt4=1.0;for(i=0;i0.0&x2=9.0&x2=10.0&x2=18.0&x2=26.0&x2=26.0&x2=40.0)xm2=1.0;for(i=0;i=xmi)rij=xmi;elserij=xtj;for(i=0;iTi.u)Ti.u=0;for(i=0;i#includemain
6、()int i=0;float k=0.8,ki=0.001,kd=0.00002,kp=3;float u=0,u0=0,e1=0,e0=0,e2;float p;float x1,x2,x3,A;float w1=0.01,w2=0.02,w3=0.01;printf(input P : ); /*程序中所用的输入输出函数都只是用于调试*/scanf(%f,e2=p-u;while(fabs(e2)=0.01)x1=e2;x2=e2-e1;x3=e2-2*e1+e0;A=fabs(w1+w2+w3);u=u0+k/A*(w1*x1+w2*x2+w3*x3);w1=w1+ki*x1*u*x1;w2=w2+kp*x1*u*x2;w3=w3+kd*x1*u*x3;u0=u;e0=e1;e1=e2;e2=p-u;i+; if(i=20) printf(u=%-18f,u);printf(e2=%-18f,e2);printf(i=%dn,i);运行此程序，发现这种算法收敛速度很慢（当 K,Kp,Kd,Ki,w1,w2,w3 的取值不十分理想时，其中 w1,w2,w3 常常是随机数） ，与传统 PID 相似，这些参数是很难确定的。由于学习时间过长，不适合在时变的干扰较大的实时系统中直接使用。24 BP 神经网络
