智能小车pid算法

3.1寻迹算法采用PID(PD)控制算法，如果某时刻检测到黑线偏左，就要向左转弯；如果检测到黑线偏右，就要向右转。偏得越多，就要向黑线方向打越大的转角。这就是比例控制（P）。遗憾的是，因为小车有惯性。假设黑线偏左，说明小车偏右了，需要左传舵，等到小车回到中心的时候，停止转舵，可是小车的惯性会使车身继续左转，直到冲过黑线，黑线又偏右。然后控制过程反复，车身是在左右摇摆中向前行走的。这种摇摆叫做“超调”，超调越大，控制越不稳定，容易出轨。为了克服惯性，我们除了位置信息之外，还需要知道轨迹的变化趋势。我们可以用黑线位置的微分值来提前得到变化趋势。用本次位置减去前次位置求出差值，就大致知道偏移量的变化趋势。将该差值和比例相加后一起作为控制量，即可实现提前控制。这就叫做比例微分控制（PD控制）/*PID(PD)控制算法*/intPID_Control(signedcharPosition){intTemp_P,Temp_D,Temp_PID,Temp_I,k;//声明三个变量，用于存放P、I、D三分量的运算结果(I没用上)if(Position==-128)return(No_black);//错误处理(值得改进的地方)else{Temp_I=Position;for(k=0;k5;k++)Temp_I+=Last_Position[k];Temp_I*=I_coefficient;Temp_P=P_coefficient*Position;//计算比例分量(P)=比例系数*本次位置差Temp_D=D_coefficient*(Position-Last_Position[5]);//计算微分分量(D)=微分系数*(本次位置差-前3次的位置差)//注意由于采样比较快，用本次位置-前3次位置才有足够大的控制量Last_Position[6]=Last_Position[5];Last_Position[4]=Last_Position[3];Last_Position[3]=Last_Position[2];Last_Position[2]=Last_Position[1];Last_Position[1]=Last_Position[0];Last_Position[0]=Position;/*保存前5次的位置，以备用。Temp_PID=Temp_P+Temp_D+Temp_I;//P分量和D分量相加，得到控制量。if(Temp_PID5000)Temp_PID=5000;//防止控制量溢出if(Temp_PID-5000)Temp_PID=-5000;//控制量-5000~5000作为左右满舵Temp_PID=Temp_PID*1/5;//-1000~+1000是左右满舵的输出，因此需要除以0.5/*单片机浮点运算非常慢，所以用乘2除5两次定点运算来替代定点数要先乘后除，才能保证精度，同时要防止溢出，用起来比较麻烦，但CPU和内存开销小。*/return(Temp_PID);}}3.2寻光，避障算法题目要求小车到达C点之后，在光源引导下避开障碍物进入车库，这就要求小车同时完成寻光和避障的功能。如果只进行寻光，小车会撞上障碍物，如果只进行避障，小车也许会离光源越来越远。理论上避障的优先级是要高于寻光的，因为一旦接触上障碍便宣告失败。所以一旦检测到障碍物，小车会立刻执行避障动作，一旦传感器环路没有检测到障碍小车就向光源靠拢，这样能够保证小车在成功避障的条件下逐渐逼近光源，直到走出障碍区之后就能直奔光源而去。这种算法小车执行寻光避障整体采用了状态机的切换，voidFSM(){switch(Status){caseLight_Status:Find_Light();break;//没有障碍物就进入寻光模式caseBarrier_Status:Avoid_Obstacle();break;//检测到障碍进入避障状态default:break;}}具体的寻光采用了149内部的ADC12模块，通过AD读出的光敏电阻的值进行处理。根据传感器环路的结构，以小车前后方向作为Y轴，左右方向作为X轴，五个光敏电阻分别位于0度，45度，90度，135度，180度的位置。先对采回的各个光强进行归一化的处理，然后根据矢量合成的原则，计算出小车应该行进的方向，其中X为两个轮的速度差，Y为共同速度。代码如下：//矢量合成，X,Y两个轴，x=0;y=0;//1号光敏电阻，无Y轴，X为负x=x-Intensity*Light[0];//2号光敏电阻，X为-0.707，Y为0.707,45度x=x-(Intensity*Light[1]*707)/1000;y=y+(Intensity*Light[1]*707)/1000;//3号光敏电阻，X为0，Y为正y=y+Intensity*Light[2];//4号光敏电阻，X,Y都为+0.707，45度x=x+(Intensity*Light[3]*707)/1000;y=y+(Intensity*Light[3]*707)/1000;//5号光敏电阻，Y为0，X为正x=x+Intensity*Light[4];y=y;由于小车采用左右两轮的差动控制，在小车测试时，，若X和Y的差距太小，会导致趋光性并不明显，若Y的值太小又会使小车前进速度很慢。所以要仔细调制X和Y参数归一到占空比CCRx的系数。或许采用舵机控制后轮或者利用汽车结构的前轮转向小车系统能够更高效的解决寻光灵敏度的问题。在寻光小车设计中，还要考虑实际环境对小车的影响。如何滤除环境光的影响时设计的一个难点，引导小车的光源时200W的灯泡，在实际测试时，由于白天和晚上环境光的不同，小车的实际AD采样值也有差异，会造成小车运行的不稳定。不过即使环境光再强烈，在题目要求的场地里也不及灯泡的亮度，该问题解决方法是先大概感知周围环境的光强，再根据预设的值调整光强系数，自适应调整电机转速，这样就能保证只要是200W的灯泡作为引导光源，无论周围环境光的强弱，小车都能稳定地、以大概相同的速度寻光进入车库。这个问题要是深究下去还有很多值得研究的地方：比如同时有若干个强光源，小车现在的算法是朝着这些光源合成的几何中心进行，如果要实现朝着光最强的那个光源行进该采用什么样的算法。避障算法采用了检测发射红外LED，读一体化接收头的数据判断障碍物的位置。这里采用38Khz敏感的接收头。为了使小车的检测距离适中，还需调整通过红外LED的电流为5mA左右。采用了TA0来发生红外线，不干扰CPU的运行：voidTimerAInit(){//设置数组，对应频率分别为38,41,44,48,54,60,67TACTL|=TASSEL_2+TACLR+MC_1;//TIMER_A时钟源选为SMCLK,清TARTACCR0=44;//产生约38KHZ的PWM输出,SMCLK=4M,105个时钟周期TACCTL0|=OUTMOD_4;//翻转模式产生占空比为50％的PWM}避障底层程序：voidMeasure_Distance(){unsignedcharflag_1=0,flag_2=0,flag_3=0,flag_4=0,flag_0=0;unsignedchari,Frequency[5]={29,36,44,47,51};//66,54,44,42,38for(i=0;i5;i++){TACCR0=Frequency[i];//1if(flag_0==0){Delay(34);PIRE1_H;//打开1号传感器Delay(34);//延迟，等待数据稳定if(IRE1_IN==0)//如果接收到的信号为低电平，表明前方有障碍物{flag_0=1;BarrierData[0]=5-i;}elseBarrierData[0]=0;PIRE1_L;}//2if(flag_1==0){Delay(34);PIRE2_H;//打开2号传感器Delay(34);//延迟，等待数据稳定if(IRE1_IN==0)//如果接收到的信号为低电平，表明前方有障碍物{flag_1=1;BarrierData[1]=5-i;}elseBarrierData[1]=0;PIRE2_L;}//3if(flag_2==0){PIRE3_H;//打开3号传感器Delay(34);//延迟，等待数据稳定if(IRE3_IN==0)//如果接收到的信号为低电平，表明右前方有障碍物{flag_2=1;BarrierData[2]=5-i;}elseBarrierData[2]=0;PIRE3_L;//关闭3号传感器}//4if(flag_3==0){PIRE4_H;//打开4号传感器Delay(34);//延迟，等待数据稳定if(IRE4_IN==0)//如果接收到的信号为低电平，表明右前方有障碍物{flag_3=1;BarrierData[3]=5-i;}elseBarrierData[3]=0;PIRE4_L;//关闭4号传感器}//5if(flag_4==0){Delay(34);PIRE5_H;//打开5号传感器Delay(34);//延迟，等待数据稳定if(IRE5_IN==0)//如果接收到的信号为低电平，表明右前方有障碍物{flag_4=1;BarrierData[4]=5-i;}elseBarrierData[4]=0;PIRE5_L;//关闭5号传感器}if(flag_0||flag_1||flag_2||flag_3||flag_4){flag=1;}elseflag=0;}3.3串口通信由于小车采用双核结构，所以就涉及到了两个单片机的通信问题。在整个进行的过程中，由于两个单片机分工明确，需要通信的地方大概有如下几个地方：1.启动时，149给425发送信号，开始计时；2.检测到金属时，425发给149计数；3.检测到C点停车；4.车身入库后149发给425停车，停止计时。由此可见，通信量并不是很大，而且通信距离不过十几个厘米，所以采用两个单片机的串口直接相连的方式。全部收发均放在中断中执行，以下是串口通信的部分程序：149串口初始化UART_Init149(){U0CTL|=CHAR;//异步通讯模式，8位数据，无校验，1位停止位ME1|=UTXE0+URXE0;//开启串口0收发模块U0TCTL|=SSEL0;//选择ACLK作为串口波特率时钟源U0BR1=0;//U0BR0=13;//分频系数整数部分=13U0MCTL=0X6B;//分频系数小数部分调制=5/8.（2400bps）P3SEL|=BIT4+BIT5;U0CTL&=~SWRST;//启动串口IE1|=URXIE0;}从串口读一个字节数据charUART0_GetChar(unsignedchar*Chr){if(UART_InpLen==0)return(0);_DINT();UART_InpLen--;*Chr=RX_BUFF[RX_IndexR];if(++RX_IndexR=RXBUF_SIZE){RX_IndexR=0;}_EINT();return(1);}串口发送中断#pragmavector=UART0TX_VECTOR__interruptvoidUART0_TX(void){if(UART_OutLen0){UART_OutLen--;U0TXBUF=TX_BUFF[TX_IndexR];if(++TX_IndexR=TXBUF_SIZE){TX_IndexR=0;}}elseIE1&=~UTXIE0;}