5.6.3 舵機(jī)控制算法
在小車行駛途中，由于在直道和過彎時(shí)，攝像頭所采集到的圖像的有效行數(shù)是會(huì)變化的，在過彎時(shí)，采集到的圖像行數(shù)會(huì)減少，同時(shí)黑線的偏差值也會(huì)相應(yīng)的變化，這樣便可利用有效行的大小來分段給定舵機(jī)的Kp值，再結(jié)合偏差細(xì)化Kp值。經(jīng)過測(cè)試和調(diào)試得出以下分段控制:
按照有效行進(jìn)行分段確定比例系數(shù)：
Effect_row≤12； Kp=7；
12<Effect_row≤18; Kp=6;
18<Effect_row≤25; Kp=6;
Effect_row>25&&左邊丟線數(shù)B_LZ>20||右邊丟線數(shù)B_LR>20; Kp=6;
Effect_row>25&&B_LZ≤20&&B_LR≤20; KP=5;
經(jīng)過實(shí)際調(diào)試測(cè)量控制后，小車的轉(zhuǎn)向控制是非常準(zhǔn)確。
5.6.2 電機(jī)控制算法
小車行駛途中，在不同的路況時(shí)，攝像頭所采集到的圖像的有效行數(shù)和偏差值是會(huì)變化的。在過彎時(shí)，采集到的圖像行數(shù)會(huì)減少，偏差會(huì)增大，這樣便可利用有效行和偏差的大小來控制速度。
控制的主要思想：將一幅圖像劃分成三段，計(jì)算每一段圖像中兩條黑線的累積偏差。對(duì)于直道來說累積偏差較小，有效行較多，對(duì)于彎道來說累積偏差較大，有效行較少。若前面兩段的累積偏差大，后一段的累積偏差小，即可判定是直道入彎，此時(shí)電機(jī)減速；若前面兩段的累積偏差較小，后一段的累積偏差較大，即可判定是開始由彎道進(jìn)入直道，此時(shí)電機(jī)加速。在直道和彎道中主要通過彎道計(jì)數(shù)器和直道計(jì)數(shù)器來完成對(duì)電機(jī)的控制。若正在處理的一行如上一行的偏差大于一個(gè)設(shè)定的值（通過測(cè)試得到），彎道計(jì)數(shù)器加一，否則直道計(jì)數(shù)器加一，通過一幅圖像的中彎道計(jì)數(shù)器與直道計(jì)數(shù)器的值的相應(yīng)大小即可判定是直道還是彎道，并設(shè)定相應(yīng)的速度。
這種算法的優(yōu)點(diǎn)是對(duì)于小S彎，計(jì)算的累積偏差不大，即可判定為直道，此時(shí)可以以最優(yōu)的路徑?jīng)_過小S彎。
5.5.4 轉(zhuǎn)角控制 根據(jù)賽道的偏差來控制舵機(jī)的轉(zhuǎn)角，采用如下： - voidduoji(void){
- if(duandianflag==0){
-
-
- if(error>0)
- duty=DUOJICENTER+error*kp;
- else
-
- duty=DUOJICENTER+error*kp;
- error1=error;
-
- }else if(duandianflag==1){
- duty= DUOJICENTER+ZHUANMAX;
- }else if(duandianflag==2)
- duty= DUOJICENTER-ZHUANMAX;
- if(duty>DUOJICENTER+ZHUANMAX)
-
- duty=DUOJICENTER+ZHUANMAX;
- if(duty<DUOJICENTER-ZHUANMAX)
- duty=DUOJICENTER-ZHUANMAX;
-
-
- PWMDTY01=duty;
- }
- ////////////////////////////////////////////////////////////////////
斜率控制
判斷賽道類型，給出速度基準(zhǔn)
5.3 舵機(jī)控制從采集回來的圖像中提取控制量來控制舵機(jī)的轉(zhuǎn)向，實(shí)現(xiàn)智能車的自動(dòng)循跡。本系統(tǒng)采用虛擬黑線的偏移量e_steer和虛擬黑線的某段斜率K_buf對(duì)舵機(jī)進(jìn)行控制，可稱為PD控制器。
其控制形式如下式所表示：
e_steer=(POINT/2-(int)mid_image[1])*KD2_2;
K_buf=((int)(mid_image[1])-(int)(mid_image[hang_en]))*(KS2-2)/hang_en;
式中的POINT為攝像頭采集的列數(shù)，mid_imag數(shù)組時(shí)根據(jù)兩條黑線虛擬出來的中間線，KD2_2和KS2_2是結(jié)合小車行駛的情況整定的參數(shù)。
由于車模是個(gè)隨動(dòng)系統(tǒng)，我們的小車的前瞻是70cm，而真正用到的只用我們指定的17行。在攝像頭70cm前瞻內(nèi)覆蓋的黑線超過17行，特別是前方的彎特別急的時(shí)候，在攝像頭前瞻視角范圍內(nèi)覆蓋的黑線會(huì)特別少。根據(jù)這個(gè)特點(diǎn)，可設(shè)置一個(gè)有效前瞻量hang_en作為對(duì)前方的彎的平緩程度的反應(yīng)。
這樣，從一場(chǎng)的黑線位置數(shù)據(jù)中，系統(tǒng)提取了有效行hang_en、e_steer、K_buf 3個(gè)量來對(duì)舵機(jī)進(jìn)行控制。在實(shí)際情況中，K_buf可以很靈活，因?yàn)榍胺胶诰�的斜率可以取不同段得到，可以根據(jù)實(shí)際要求得到不同段的斜率值．如速度快時(shí)可以適當(dāng)?shù)娜【嘬囕^遠(yuǎn)處的黑線斜率，以實(shí)現(xiàn)超前控制。在智能車調(diào)試參數(shù)的時(shí)候．對(duì)這3個(gè)量的理解很是重要，具體來說，hang_en表征車模的有效前瞻， 即看得有多遠(yuǎn)，對(duì)于智能車在道路上行駛，看得遠(yuǎn)說明黑線都在前方，看不遠(yuǎn)說明智能車前方的黑線已經(jīng)偏左或者偏右，而這個(gè)量的大小正好可以表征彎的平緩與急切。另外，看得遠(yuǎn)則攝像頭采集的黑線多，系統(tǒng)信息量大，那么怎么處理這些大量的信息為我們所用就變得很關(guān)鍵，如看得70cm都能看見．說明小車必然在長(zhǎng)直道上，不然也是小S彎，稍作處理就可以過濾掉小S彎了，讓小車像都是在直道上跑；看得很近說明彎已經(jīng)很急，這時(shí)候，只要能夠判斷出彎往那邊拐就可以給舵機(jī)一個(gè)極值急拐。e_steer表征在某一個(gè)特定視野下．小車與黑線偏離的程度，這個(gè)量可以讓智能車在某個(gè)特定視野下決定給舵機(jī)多大的轉(zhuǎn)角。K_buf則在有效前瞻遠(yuǎn)的時(shí)候尤為關(guān)鍵，因?yàn)樗梢灶A(yù)判前方的彎，從而超前的轉(zhuǎn)彎。 5.4 速度控制 電機(jī)控制方面我們才用的是增量式PID。行駛過程中給定的速度主要要六種，這六個(gè)速度值對(duì)應(yīng)舵機(jī)賽道的不同半徑的彎道，也就是說在半徑小的圓彎，速度不能太大，因此賦予小的速度值，而半徑大的圓彎則要賦予較高的速度值。這樣在小車行駛過程中既保證了小車的穩(wěn)定性，也提高了速度。具體實(shí)現(xiàn)程序如下所示： - if(Steer_PWM>1125) //右彎
- {
- if(Steer_PWM>1215)
- {speed_goal=speed_strategy.very_slow;}
- else if(Steer_PWM>1185)
- {speed_goal=speed_strategy.slow;}
- else if(Steer_PWM>1155)
- {speed_goal=speed_strategy.medium;}
- else
- {speed_goal=speed_strategy.high;}
- }
- else if(Steer_PWM<1095) //左彎
- {
- if(Steer_PWM<1005)
- { speed_goal=speed_strategy.very_slow;}
- else if(Steer_PWM<1035)
- { speed_goal=speed_strategy.slow;}
- else if(Steer_PWM<1065)
- { speed_goal=speed_strategy.medium;}
- else
- {speed_goal=speed_strategy.high;}
- }
- else{if(Judgment_straight(hang_en))speed_goal=speed_strategy.very_high;}
為了加快系統(tǒng)的響應(yīng)，使用PID控制器來提高響應(yīng)速度.具體的實(shí)現(xiàn)如下代碼： ek=(int)speed_goal-(int)speed; out=(int)(ek*Kp)-(ek_1*Ki)+(ek_2*Kd); 其中ek為當(dāng)前速度與期望值的偏差，ek_1為上一次的偏差，而ek_2為更早一次的偏差，Kp為比例系數(shù)，Ki為積分環(huán)節(jié)系數(shù)，Kd為微分環(huán)節(jié)系數(shù)，這三個(gè)參數(shù)經(jīng)過大量的調(diào)試得來
////////////////////////////////////////
5.3.2 舵機(jī)轉(zhuǎn)向控制算法
經(jīng)過上面的圖像處理方法，得到本場(chǎng)第一個(gè)有效行為istart，最后一個(gè)有效行iend。為了使智能車的路徑更優(yōu)化，需要把賽道分類。針對(duì)不同賽道選用不同的走線策略。由(istart,cent[istart])和(iend,cent[iend])兩點(diǎn)確定一直線y=a*i+b記為L。當(dāng)有效行大于3行時(shí)，從istart+1開始到iend-1結(jié)束進(jìn)行逐行處理。每行計(jì)算cent[ i]-a*i+b[ i]。如果掃描中發(fā)現(xiàn)有的行導(dǎo)航線在L左側(cè)，有的在L右側(cè)，因?yàn)橹挥?/font>S彎才會(huì)看到此現(xiàn)象，所以此時(shí)賽道為S彎，再根據(jù)bulb（bulb為一場(chǎng)中最大的cent[ i]-a*i+b[ i]）的大小可確定是大S彎還是小S彎。如不是S彎，再根據(jù)iend的大小來判斷。iend大說明看到的圖像長(zhǎng)，那轉(zhuǎn)彎處必定較遠(yuǎn)，記為遠(yuǎn)彎。相應(yīng)的iend小則為近彎。如果有效行小于3行，再看istart是否為第0行。是則說明比較信息比較可靠，且彎很急記為急彎，否則信息不可靠記為不可靠信號(hào)。因?yàn)橹钡郎喜还芤阅囊恍锌刂撇顒e都不大，所以直到也可歸為遠(yuǎn)彎。
得到了路徑識(shí)別的結(jié)果，再確定不同路徑的控制行。先按在直線上以每行作為控制行分別得到的轉(zhuǎn)角相等為標(biāo)準(zhǔn)，確定每行的轉(zhuǎn)角系數(shù)。這樣同樣大小的cent[ i]所對(duì)應(yīng)的轉(zhuǎn)角隨著i增大而減小。如果路徑為小S彎，就以iend為控制行，以達(dá)到直走的效果。如果是大S彎，根據(jù)S玩的大小，分別以iend和ibulb控制行算得的轉(zhuǎn)角按一定權(quán)值求平均值作為最終轉(zhuǎn)角。如果是遠(yuǎn)彎，如果以iend行的黑線中值來控制舵機(jī)會(huì)使智能車過彎太提前，容易從彎內(nèi)側(cè)沖出賽道，為此要沿線走較合理，而ibulb正好是相當(dāng)于直線與拐彎交界處，所以以ibulb為控制行。如果是近彎，以iend控制，會(huì)走內(nèi)線，既不易沖出賽道又縮短路徑。
如果是急彎，以iend控制轉(zhuǎn)的最大。如果是不可靠信號(hào)，就以近處某一固定行控制，以達(dá)到循線走的效果。
為了更好的解決轉(zhuǎn)向的問題，我們采用分段轉(zhuǎn)角系數(shù)控制舵機(jī)。根據(jù)不同的路況信息來設(shè)定不同的轉(zhuǎn)角系數(shù)。因?yàn)樵谥钡阑蛐?/font>S上轉(zhuǎn)角系數(shù)太大，往往會(huì)導(dǎo)致舵機(jī)的抖動(dòng)使得小車在直線上左右搖擺，所以如果小車行駛在直線上那么舵機(jī)的轉(zhuǎn)角系數(shù)應(yīng)該比較��；如果小車舵機(jī)行駛灣道上，那么轉(zhuǎn)角系數(shù)應(yīng)該同時(shí)變大否則很可能不能及時(shí)地過彎道。
5.3.3 直流電機(jī)的控制算法
根據(jù)路徑識(shí)別的情況，如果當(dāng)前路徑為直道，則需要加速；若是彎道，則需要降速，而且根據(jù)不同的彎道速度也是有所區(qū)別，這個(gè)區(qū)別體現(xiàn)在速度設(shè)定上。我們根據(jù)iend、舵機(jī)轉(zhuǎn)角、bulb等多個(gè)變量的大小來確定速度設(shè)定的策略。
當(dāng)iend等于最遠(yuǎn)行時(shí)，說明彎道離得很遠(yuǎn)，速度可以設(shè)為很高的值。這時(shí)需要根據(jù)bulb來確定遠(yuǎn)處是彎道還是直道，bulb越大速度應(yīng)該越小，bulb為0時(shí)速度為最高檔。當(dāng)iend不等于最遠(yuǎn)行時(shí)，速度需要降一個(gè)檔，再根據(jù)iend越大速度越高，舵機(jī)轉(zhuǎn)角越大，速度越低的原則，對(duì)速度做進(jìn)一步微調(diào)。當(dāng)iend小于4時(shí)，說明有沖出賽道的危險(xiǎn)，速度再降一檔，再根據(jù)iend越大速度越高，舵機(jī)轉(zhuǎn)角越大，速度越低的原則，對(duì)速度做進(jìn)一步微調(diào)。由于速度太低時(shí)，電機(jī)動(dòng)力不足難以推動(dòng)智能車前進(jìn)。因此速度設(shè)定應(yīng)該有個(gè)最低限幅值。當(dāng)運(yùn)算得到的速度設(shè)定小于該限幅值時(shí)，速度直接賦值為最低值。速度設(shè)定中用到的各參數(shù)都可用撥碼開關(guān)來改變，這在實(shí)際比賽中有較大的實(shí)際意義。
以上只是速度設(shè)定的算法，實(shí)際的車速還需要閉環(huán)控制，用PID算法來實(shí)現(xiàn)穩(wěn)定、準(zhǔn)確、快速地跟隨速度設(shè)定。下面介紹速度的控制算法。
系統(tǒng)利用測(cè)速模塊反饋的當(dāng)前速度值，和設(shè)定速度值來選擇不同的P參數(shù)來進(jìn)行調(diào)節(jié)，從而控制直流電機(jī)對(duì)當(dāng)前路徑進(jìn)行快速反應(yīng),利用I的作用使得消除靜差，加入D使得速度更加穩(wěn)定。
本系統(tǒng)采用的是數(shù)字PID 控制，通過每一控制周期讀入脈沖數(shù)來間接測(cè)得小車當(dāng)前轉(zhuǎn)速vi_FeedBack，將vi_ FeedBack與模糊推理得到的小車期望速度vi_Ref比較，既而算得速度偏差error，再通過調(diào)用PID函數(shù)來獲得速度的控制。 考慮到CMOS方案的控制周期較長(zhǎng)，假設(shè)按2.5m/s的平均速度計(jì)算，則一個(gè)控制周期小車大概可以跑過5cm，如果按這種周期用上述PID調(diào)節(jié)速度，則會(huì)導(dǎo)致加速減速均過長(zhǎng)的后果，嚴(yán)重的影響小車的快速性和穩(wěn)定性。為了解決這個(gè)問題，可以在PID調(diào)速控制中適當(dāng)加入Bang-Bang控制：根據(jù)error的大小，如果正大，則正轉(zhuǎn)給全額占空比；如果負(fù)大，則自由停車或給一個(gè)反轉(zhuǎn)的PWM，否則就采用PID來計(jì)算所需的占空比。
| 
