智能車控制算法

1、智能車轉(zhuǎn)角與速度控制算法1 .檢測黑線中點(diǎn)Center：設(shè)黑、白點(diǎn)兩個計(jì)數(shù)數(shù)組black、white,從第一個白點(diǎn)開始，檢測到一個白點(diǎn)，白點(diǎn)計(jì)數(shù)器就加1,檢測到第一個黑點(diǎn)，黑點(diǎn)計(jì)數(shù)器就加1,并且白點(diǎn)計(jì)數(shù)器停止，以此類推掃描每一行；黑線中點(diǎn)=白點(diǎn)個數(shù)+（黑點(diǎn)的個數(shù)/2）2 .判斷彎直道：找出黑線的平均位置avg（以每10行或者20作為參照，行數(shù)待定）avg=（£CenterN）/N算出相對位移之和（每一行黑線中點(diǎn)與黑線平均位置距離的絕對值之和）Curve=Ij=D|CenterN7Vgi然后用Curve的大小來確定是否彎直道（Curve的閥值待定）。3 .控制速度：根據(jù)彎度的大小控制速

2、度大小。彎度檢測函數(shù)*/*Curvecontrol()/黑點(diǎn)計(jì)數(shù)器/白點(diǎn)計(jì)數(shù)器黑線中點(diǎn)位置黑線中點(diǎn)平均位置/N行的相對位移之和判斷黑白點(diǎn)的個數(shù)/每一行的黑線中點(diǎn)/求出黑線中點(diǎn)的平均位置intblackN;intwhiteN;intcenterN;intavg;intcurve;if(白點(diǎn))+whiteN;else+blackN;centerN=whiteN+blackN/2;avg=(center1+center2+.+centerN)/N;curve=(|avg-center1|+|avg-center2|+.+|avg-centerN|)/N/求出N行的相對位移之和returncurve;

3、/返回彎度大小/*舵艙轉(zhuǎn)角控制函數(shù)*/*黑線軌跡會指引小車的行駛方向，所以向左還是向右是由黑線決定的。*/*設(shè)0為小車正向行駛，-10為向左的最大轉(zhuǎn)角，+10為向右的最大轉(zhuǎn)角。*/*設(shè)小車實(shí)際行駛路線中心A相對于黑線中線B偏移量為gap;gap有正負(fù)之分，/gap為正時，小車在黑線中線的右側(cè)，應(yīng)該左轉(zhuǎn)；gap為負(fù)時，小車在黑線中線左側(cè)，應(yīng)/該右轉(zhuǎn)。轉(zhuǎn)角的大小由剛剛得至U的彎度Curve確定。*/Viewpointcontrol()(Intgap,A,B;Inty;gap=A-B;If(gap>0)左轉(zhuǎn)elseif(gap<0)右轉(zhuǎn)else不變丫=與Curve和gap有關(guān)的公式；re

4、turny;)/*y為舵艙的轉(zhuǎn)角角度/*y有正負(fù)之分/*返回轉(zhuǎn)角角度/*速度控制函數(shù)*/*根據(jù)彎度的大小來改變速度大小*/Speedcontrol()(Returnspeed;)5.3.1偏航距離的計(jì)算由于已經(jīng)獲得了賽道中心線的位置，所以計(jì)算偏航距離的問題是選取何處的中心線的距離為當(dāng)前的偏航距離?？刂扑惴ǖ膱?zhí)行周期為40ms,如果賽車的速度為2m/s,則在兩次控制算法的執(zhí)行中間，賽車要前進(jìn)8cm,賽車所處的環(huán)境將發(fā)生比較大的改變，所以賽車的控制只能算是半實(shí)時控制，這是所有使用攝像頭作為主要尋線傳感器的參賽隊(duì)都避免不了的問題。因?yàn)樗惴ǖ臏笮裕愜囆枰獙ⅰ爱?dāng)前位置”進(jìn)行適當(dāng)前移。前移量應(yīng)該跟賽車

5、當(dāng)前速度成正比，但實(shí)際中我們發(fā)現(xiàn)，適當(dāng)增加一些前移距離是有好處的，因?yàn)榭梢栽谌霃澨幪崆稗D(zhuǎn)彎，使得賽車沿彎道內(nèi)側(cè)行駛，縮短了過彎距離。偏航角度的計(jì)算計(jì)算偏航角度的實(shí)質(zhì)是直線擬合問題，因?yàn)橘惖乐行木€所在的直線確定了，而直線的斜率與偏航角度一一對應(yīng)。直線擬合最有效的方法是最小二乘法7,但是直接應(yīng)用存在一個問題，即如何確定進(jìn)行直線擬合的區(qū)間？在整個成功識別出賽道的區(qū)間內(nèi)進(jìn)行直線擬合顯然是欠缺考慮的，因?yàn)樵趶澋赖那闆r下，這種方法擬合出的是一條弦線，而不是當(dāng)前該彎道處的切線。攝像頭視野越大，彎道曲率越大，弦線偏離切線的程度也就越大。為了能夠在直道和彎道上都能正確的擬合出正確的直線，我們采用了直線檢測的方法

6、，即首先根據(jù)殘差的大小確定直線的范圍，然后在這一范圍內(nèi)進(jìn)行直線擬合。曲率的計(jì)算如果說斜率的計(jì)算需要某種技巧的話，計(jì)算曲率則更是一種技巧的應(yīng)用。首屆時很多參賽隊(duì)針對各自的實(shí)際需要，提出了自己的方法。其中最普遍的是根據(jù)斜率的導(dǎo)數(shù)來計(jì)算曲率8。但是斜率的計(jì)算本身就很不準(zhǔn)確，特別是某個點(diǎn)的斜率，對斜率求導(dǎo)就更不準(zhǔn)確，所以使用這種方法只能得出一個大致的結(jié)果。本文作者提出了另外一種方法，首先對獲得的路徑進(jìn)行濾波，使得路徑盡可能平滑，然后取其兩個端點(diǎn)和中間點(diǎn)，計(jì)算這3個點(diǎn)組成的三角形的外接圓的半徑，半徑的倒數(shù)就是這段路徑的曲率。經(jīng)過多次實(shí)驗(yàn)，這種方法的誤差一般不大于20%,對智能車的控制來說已經(jīng)足夠了。讓賽車從起跑線開始，在賽道上行駛一圈，記錄下每個時刻的曲率，如圖5.9。圖5.9計(jì)算出的賽道曲率從圖5.9可以看出，計(jì)算出的曲率能較為正確的反映實(shí)際賽道的彎曲情況。但是這種方法得出的曲率不是攝像頭所看到的當(dāng)前位置的曲率，而是攝像頭所看到的路徑的整體曲率，因?yàn)檫@種算法僅僅與路徑中三個點(diǎn)的位置有關(guān)。然而實(shí)驗(yàn)中卻發(fā)現(xiàn)，這種特點(diǎn)反而給賽車帶來了一個好處：即攝像頭