智能小車pid算法

1、精選優(yōu)質(zhì)文檔-傾情為你奉上3.1尋跡算法采用PID(PD)控制算法，如果某時(shí)刻檢測(cè)到黑線偏左，就要向左轉(zhuǎn)彎；如果檢測(cè)到黑線偏右，就要向右轉(zhuǎn)。偏得越多，就要向黑線方向打越大的轉(zhuǎn)角。這就是比例控制（P）。遺憾的是，因?yàn)樾≤囉袘T性。假設(shè)黑線偏左，說明小車偏右了，需要左傳舵，等到小車回到中心的時(shí)候，停止轉(zhuǎn)舵，可是小車的慣性會(huì)使車身繼續(xù)左轉(zhuǎn)，直到?jīng)_過黑線，黑線又偏右。然后控制過程反復(fù)，車身是在左右搖擺中向前行走的。這種搖擺叫做“超調(diào)”，超調(diào)越大，控制越不穩(wěn)定，容易出軌。為了克服慣性，我們除了位置信息之外，還需要知道軌跡的變化趨勢(shì)。我們可以用黑線位置的微分值來提前得到變化趨勢(shì)。用本次位置減去前次位置求出差

2、值，就大致知道偏移量的變化趨勢(shì)。將該差值和比例相加后一起作為控制量，即可實(shí)現(xiàn)提前控制。這就叫做比例微分控制（PD控制）/*PID(PD)控制算法*/int PID_Control(signed char Position) int Temp_P,Temp_D,Temp_PID,Temp_I,k; /聲明三個(gè)變量，用于存放P、I、D三分量的運(yùn)算結(jié)果(I沒用上)if(Position=-128) return (No_black); /錯(cuò)誤處理(值得改進(jìn)的地方)else Temp_I=Position; for(k=0;k<5;k+)Temp_I+=Last_Positionk; Temp_

3、I*=I_coefficient; Temp_P=P_coefficient*Position; /計(jì)算比例分量(P)=比例系數(shù)*本次位置差 Temp_D=D_coefficient*(Position-Last_Position5); /計(jì)算微分分量(D)=微分系數(shù)*(本次位置差-前3次的位置差) /注意由于采樣比較快，用本次位置-前3次位置才有足夠大的控制量 Last_Position6=Last_Position5; Last_Position4=Last_Position3; Last_Position3=Last_Position2; Last_Position2=Last_Posi

4、tion1; Last_Position1=Last_Position0; Last_Position0=Position; /*保存前5次的位置，以備用。 Temp_PID=Temp_P+Temp_D+Temp_I; /P分量和D分量相加，得到控制量。 if(Temp_PID>5000) Temp_PID=5000; /防止控制量溢出 if(Temp_PID<-5000) Temp_PID=-5000; /控制量-50005000作為左右滿舵 Temp_PID=Temp_PID*1/5; /-1000+1000是左右滿舵的輸出，因此需要除以0.5 /*單片機(jī)浮點(diǎn)運(yùn)算非常慢，所以用

5、乘2除5兩次定點(diǎn)運(yùn)算來替代定點(diǎn)數(shù)要先乘后除，才能保證精度，同時(shí)要防止溢出，用起來比較麻煩，但CPU和內(nèi)存開銷小。*/ return (Temp_PID); 3.2 尋光，避障算法題目要求小車到達(dá)C點(diǎn)之后，在光源引導(dǎo)下避開障礙物進(jìn)入車庫，這就要求小車同時(shí)完成尋光和避障的功能。如果只進(jìn)行尋光，小車會(huì)撞上障礙物，如果只進(jìn)行避障，小車也許會(huì)離光源越來越遠(yuǎn)。理論上避障的優(yōu)先級(jí)是要高于尋光的，因?yàn)橐坏┙佑|上障礙便宣告失敗。所以一旦檢測(cè)到障礙物，小車會(huì)立刻執(zhí)行避障動(dòng)作，一旦傳感器環(huán)路沒有檢測(cè)到障礙小車就向光源靠攏，這樣能夠保證小車在成功避障的條件下逐漸逼近光源，直到走出障礙區(qū)之后就能直奔光源而去。這種算法小

6、車執(zhí)行尋光避障整體采用了狀態(tài)機(jī)的切換，void FSM() switch (Status) case Light_Status : Find_Light(); break; /沒有障礙物就進(jìn)入尋光模式 case Barrier_Status : Avoid_Obstacle(); break; /檢測(cè)到障礙進(jìn)入避障狀態(tài) default: break; 具體的尋光采用了149內(nèi)部的ADC12模塊，通過AD讀出的光敏電阻的值進(jìn)行處理。根據(jù)傳感器環(huán)路的結(jié)構(gòu)，以小車前后方向作為Y軸，左右方向作為X軸，五個(gè)光敏電阻分別位于0度，45度，90度，135度，180度的位置。先對(duì)采回的各個(gè)光強(qiáng)進(jìn)行歸一化的處理

7、，然后根據(jù)矢量合成的原則，計(jì)算出小車應(yīng)該行進(jìn)的方向，其中X為兩個(gè)輪的速度差，Y為共同速度。代碼如下：/矢量合成，X,Y兩個(gè)軸， x=0; y=0; /1號(hào)光敏電阻，無Y軸，X為負(fù) x=x-Intensity*Light0; /2號(hào)光敏電阻，X為-0.707，Y為0.707,45度 x=x-(Intensity *Light1*707)/1000; y=y+(Intensity *Light1*707)/1000; /3號(hào)光敏電阻，X為0，Y為正 y=y+Intensity * Light2; /4號(hào)光敏電阻，X,Y都為+0.707，45度 x=x+(Intensity*Light3*707)/

8、1000; y=y+(Intensity*Light3*707)/1000; /5號(hào)光敏電阻，Y為0，X為正 x=x+Intensity*Light4; y=y;由于小車采用左右兩輪的差動(dòng)控制，在小車測(cè)試時(shí)，若X和Y的差距太小，會(huì)導(dǎo)致趨光性并不明顯，若Y的值太小又會(huì)使小車前進(jìn)速度很慢。所以要仔細(xì)調(diào)制X和Y參數(shù)歸一到占空比CCRx的系數(shù)。或許采用舵機(jī)控制后輪或者利用汽車結(jié)構(gòu)的前輪轉(zhuǎn)向小車系統(tǒng)能夠更高效的解決尋光靈敏度的問題。在尋光小車設(shè)計(jì)中，還要考慮實(shí)際環(huán)境對(duì)小車的影響。如何濾除環(huán)境光的影響時(shí)設(shè)計(jì)的一個(gè)難點(diǎn)，引導(dǎo)小車的光源時(shí)200W的燈泡，在實(shí)際測(cè)試時(shí)，由于白天和晚上環(huán)境光的不同，小車的實(shí)際AD

9、采樣值也有差異，會(huì)造成小車運(yùn)行的不穩(wěn)定。不過即使環(huán)境光再強(qiáng)烈，在題目要求的場(chǎng)地里也不及燈泡的亮度，該問題解決方法是先大概感知周圍環(huán)境的光強(qiáng)，再根據(jù)預(yù)設(shè)的值調(diào)整光強(qiáng)系數(shù)，自適應(yīng)調(diào)整電機(jī)轉(zhuǎn)速，這樣就能保證只要是200W的燈泡作為引導(dǎo)光源，無論周圍環(huán)境光的強(qiáng)弱，小車都能穩(wěn)定地、以大概相同的速度尋光進(jìn)入車庫。這個(gè)問題要是深究下去還有很多值得研究的地方：比如同時(shí)有若干個(gè)強(qiáng)光源，小車現(xiàn)在的算法是朝著這些光源合成的幾何中心進(jìn)行，如果要實(shí)現(xiàn)朝著光最強(qiáng)的那個(gè)光源行進(jìn)該采用什么樣的算法。避障算法采用了檢測(cè)發(fā)射紅外LED，讀一體化接收頭的數(shù)據(jù)判斷障礙物的位置。這里采用38Khz敏感的接收頭。為了使小車的檢測(cè)距離適中

10、，還需調(diào)整通過紅外LED的電流為5mA左右。采用了TA0來發(fā)生紅外線，不干擾CPU的運(yùn)行：void TimerAInit() /設(shè)置數(shù)組，對(duì)應(yīng)頻率分別為38,41,44,48,54,60,67 TACTL |= TASSEL_2 + TACLR + MC_1 ; /TIMER_A時(shí)鐘源選為SMCLK,清TAR TACCR0 = 44; /產(chǎn)生約38KHZ的PWM輸出,SMCLK=4M,105個(gè)時(shí)鐘周期 TACCTL0 |= OUTMOD_4; /翻轉(zhuǎn)模式產(chǎn)生占空比為50的PWM 避障底層程序：void Measure_Distance() unsigned char flag_1=0,flag

11、_2=0,flag_3=0,flag_4=0,flag_0=0; unsigned char i,Frequency5 = 29,36,44,47,51; /66,54,44,42,38 for (i=0;i<5;i+) TACCR0 = Frequencyi; / 1 if(flag_0=0) Delay(34);PIRE1_H; / 打開1號(hào)傳感器Delay(34); / 延遲，等待數(shù)據(jù)穩(wěn)定 if(IRE1_IN=0) / 如果接收到的信號(hào)為低電平，表明前方有障礙物 flag_0 = 1;BarrierData0=5-i; else BarrierData0=0; PIRE1_L;

12、/2 if(flag_1=0) Delay(34);PIRE2_H; / 打開2號(hào)傳感器Delay(34); / 延遲，等待數(shù)據(jù)穩(wěn)定 if(IRE1_IN=0) / 如果接收到的信號(hào)為低電平，表明前方有障礙物 flag_1 = 1;BarrierData1=5-i; else BarrierData1=0; PIRE2_L; / 3if(flag_2=0) PIRE3_H ; / 打開3號(hào)傳感器Delay(34); / 延遲，等待數(shù)據(jù)穩(wěn)定if(IRE3_IN=0) / 如果接收到的信號(hào)為低電平，表明右前方有障礙物 flag_2 = 1;BarrierData2=5-i; else Barrie

13、rData2=0; PIRE3_L; / 關(guān)閉3號(hào)傳感器 / 4 if(flag_3=0) PIRE4_H ; / 打開4號(hào)傳感器Delay(34); / 延遲，等待數(shù)據(jù)穩(wěn)定 if(IRE4_IN=0) / 如果接收到的信號(hào)為低電平，表明右前方有障礙物 flag_3 = 1;BarrierData3=5-i; else BarrierData3=0; PIRE4_L; / 關(guān)閉4號(hào)傳感器 / 5if(flag_4=0) Delay(34); PIRE5_H ; / 打開5號(hào)傳感器Delay(34); / 延遲，等待數(shù)據(jù)穩(wěn)定 if(IRE5_IN=0) / 如果接收到的信號(hào)為低電平，表明右前方有

14、障礙物 flag_4 = 1;BarrierData4=5-i; else BarrierData4=0; PIRE5_L; / 關(guān)閉5號(hào)傳感器 if(flag_0|flag_1|flag_2|flag_3|flag_4) flag=1; else flag=0; 3.3串口通信由于小車采用雙核結(jié)構(gòu)，所以就涉及到了兩個(gè)單片機(jī)的通信問題。在整個(gè)進(jìn)行的過程中，由于兩個(gè)單片機(jī)分工明確，需要通信的地方大概有如下幾個(gè)地方：1. 啟動(dòng)時(shí)，149給425發(fā)送信號(hào)，開始計(jì)時(shí)；2. 檢測(cè)到金屬時(shí)，425發(fā)給149計(jì)數(shù)；3. 檢測(cè)到C點(diǎn)停車；4. 車身入庫后149發(fā)給425停車，停止計(jì)時(shí)。由此可見，通信量并不是很

15、大，而且通信距離不過十幾個(gè)厘米，所以采用兩個(gè)單片機(jī)的串口直接相連的方式。全部收發(fā)均放在中斷中執(zhí)行，以下是串口通信的部分程序：149串口初始化UART_Init149() U0CTL |= CHAR; /異步通訊模式，8位數(shù)據(jù)，無校驗(yàn)，1位停止位 ME1 |= UTXE0 + URXE0; /開啟串口0收發(fā)模塊 U0TCTL |= SSEL0; /選擇ACLK作為串口波特率時(shí)鐘源 U0BR1 = 0; / U0BR0 = 13; /分頻系數(shù)整數(shù)部分=13 U0MCTL = 0X6B; /分頻系數(shù)小數(shù)部分調(diào)制=5/8.（2400bps） P3SEL |= BIT4 + BIT5; U0CTL &= SWRST; /啟動(dòng)串口 IE1 |= URXIE0;從串口讀一個(gè)字節(jié)數(shù)據(jù)char UART0_GetChar(unsigned char *Chr) if(UART_InpLen = 0) return (0); _DINT(); UART_InpLen-; *Chr = RX_BUFFRX_IndexR; if (+RX_IndexR >= RXBUF_SIZ