智能小車pid算法

《智能小車pid算法》由會員上傳分享，免費在線閱讀，更多相關(guān)內(nèi)容在行業(yè)資料-天天文庫。

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

2、用本次位置減去前次位置求出差值，就大致知道偏移量的變化趨勢。將該差值和比例相加后一起作為控制量，即可實現(xiàn)提前控制。這就叫做比例微分控制（PD控制）/*PID(PD)控制算法*/intPID_Control(signedcharPosition){intTemp_P,Temp_D,Temp_PID,Temp_I,k;//聲明三個變量，用于存放P、I、D三分量的運算結(jié)果(I沒用上)if(Position==-128)return(No_black);//錯誤處理(值得改進(jìn)的地方)else{Temp_I=Position;for(k=0;k<5;

3、k++)Temp_I+=Last_Position[k];Temp_I*=I_coefficient;Temp_P=P_coefficient*Position;//計算比例分量(P)=比例系數(shù)*本次位置差Temp_D=D_coefficient*(Position-Last_Position[5]);//計算微分分量(D)=微分系數(shù)*(本次位置差-前3次的位置差)//注意由于采樣比較快，用本次位置-前3次位置才有足夠大的控制量Last_Position[6]=Last_Position[5];Last_Position[4]=Last_P

4、osition[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_PID>5000)Temp_PID=5000;//防止控制量溢出if(Temp_PID<-5000)Temp_PID=-500

5、0;//控制量-5000~5000作為左右滿舵Temp_PID=Temp_PID*1/5;//-1000~+1000是左右滿舵的輸出，因此需要除以0.5/*單片機(jī)浮點運算非常慢，所以用乘2除5兩次定點運算來替代定點數(shù)要先乘后除，才能保證精度，同時要防止溢出，用起來比較麻煩，但CPU和內(nèi)存開銷小。*/return(Temp_PID);}}3.2尋光，避障算法題目要求小車到達(dá)C點之后，在光源引導(dǎo)下避開障礙物進(jìn)入車庫，這就要求小車同時完成尋光和避障的功能。如果只進(jìn)行尋光，小車會撞上障礙物，如果只進(jìn)行避障，小車也許會離光源越來越遠(yuǎn)。理論上避障的優(yōu)先

6、級是要高于尋光的，因為一旦接觸上障礙便宣告失敗。所以一旦檢測到障礙物，小車會立刻執(zhí)行避障動作，一旦傳感器環(huán)路沒有檢測到障礙小車就向光源靠攏，這樣能夠保證小車在成功避障的條件下逐漸逼近光源，直到走出障礙區(qū)之后就能直奔光源而去。這種算法小車執(zhí)行尋光避障整體采用了狀態(tài)機(jī)的切換，voidFSM(){switch(Status){caseLight_Status:Find_Light();break;//沒有障礙物就進(jìn)入尋光模式caseBarrier_Status:Avoid_Obstacle();break;//檢測到障礙進(jìn)入避障狀態(tài)default

7、:break;}}具體的尋光采用了149內(nèi)部的ADC12模塊，通過AD讀出的光敏電阻的值進(jìn)行處理。根據(jù)傳感器環(huán)路的結(jié)構(gòu)，以小車前后方向作為Y軸，左右方向作為X軸，五個光敏電阻分別位于0度，45度，90度，135度，180度的位置。先對采回的各個光強(qiáng)進(jìn)行歸一化的處理，然后根據(jù)矢量合成的原則，計算出小車應(yīng)該行進(jìn)的方向，其中X為兩個輪的速度差，Y為共同速度。代碼如下：//矢量合成，X,Y兩個軸，x=0;y=0;//1號光敏電阻，無Y軸，X為負(fù)x=x-Intensity*Light[0];//2號光敏電阻，X為-0.707，Y為0.707,45度x

8、=x-(Intensity*Light[1]*707)/1000;y=y+(Intensity*Light[1]*707)/1000;//3號光敏電阻，X為0，Y為正y=y+In