高性能循跡跟蹤小車，怎么在比賽中擊敗對(duì)手？

編者注：本項(xiàng)目來自Instructables，作者為newtonis。這是一個(gè)用于比賽的循跡跟蹤小車項(xiàng)目，他是怎么在兼顧尺寸和速度性的同時(shí)滿足小車高性能的要求的呢？

循跡小車在創(chuàng)客圈里面并不是什么新鮮事，但普通的循跡小車要么比較大，要么則比較重，并不適合用來比賽。在我們當(dāng)?shù)氐膭?chuàng)客圈子里面有一個(gè)循跡小車的比賽項(xiàng)目，要求上場(chǎng)的小車的尺寸不能超過12x20cm，那我們?cè)撛鯓觼頋M足這樣的需求呢？

本項(xiàng)目的主要開發(fā)過程分為三個(gè)部分：電子構(gòu)建，結(jié)構(gòu)設(shè)計(jì)和3D打印，程序配置和調(diào)試。下面我們就來完成它吧。

第一步：電機(jī)

在開發(fā)此項(xiàng)目的過程中，要切記的一點(diǎn)是我們是在開發(fā)一個(gè)比賽用的小車。此處我選用了Pololu提供的電機(jī)，他家的電機(jī)分為四種：低功率、中等功率、高功率和長(zhǎng)壽命高功率，這四種電機(jī)的工作電壓都為6V，而且物理尺寸也基本相同。低功率的電機(jī)功耗較低，但是要知道我們這是為了比賽，所以應(yīng)該選擇那兩種高功率的電機(jī)。其中長(zhǎng)壽命高功率的電機(jī)的使用壽命更長(zhǎng)，但是并不能承受高于6V的電壓。綜合考慮之后，我選擇了高功率的電機(jī)，它們能夠不中斷地連續(xù)使用好幾個(gè)小時(shí)，這已經(jīng)足夠了。

接下來我們應(yīng)該選擇多少的傳動(dòng)比呢？通常電機(jī)的傳動(dòng)比越高，電機(jī)的速度就越低，但是電機(jī)提供的扭矩卻更高，就能提供更高的加速度。對(duì)比較重的機(jī)器人來說這確實(shí)非常重要。我的建議是在30:1到10:1的傳動(dòng)比之間做出選擇，最后我選擇的四個(gè)傳動(dòng)比為10:1的電機(jī)，畢竟是為了比賽，我會(huì)盡量降低小車的質(zhì)量，從而提供更高的速度。當(dāng)然，你也可以選擇使用2個(gè)30:1的傳動(dòng)比的電機(jī)，這樣為了獲得更高的速度，你需要為電機(jī)提供高于6V的電壓。

第二步：車輪

因?yàn)殡姍C(jī)的速度是以每分鐘的轉(zhuǎn)速衡量的，所以在轉(zhuǎn)速固定的情況下，輪子直徑越大，理論上就跑得越快，但與此同時(shí)輪子也會(huì)越重。綜合考慮之后我選擇了直徑32毫米的窄邊車輪，其單個(gè)重量為3.2克。

第三步：電池

電池方面我推薦使用儲(chǔ)能密度大、可充電的LiPo電池。我使用的是850mAh 7.4V雙芯LiPo電池。

第四步：球腳輪

為了能讓小車的電機(jī)獲得足夠的反應(yīng)時(shí)間，需要將小車的循跡探測(cè)器盡量前置，那么這就需要用到2個(gè)前置的球腳輪.當(dāng)然這兩個(gè)球腳輪并不是必需的，如果你的小車的設(shè)計(jì)能夠在行進(jìn)過程中維持平衡，或者只是采用后輪驅(qū)動(dòng)的方式運(yùn)動(dòng)（將探測(cè)器安裝在車體前部），那么就不需要這兩個(gè)球腳輪。

注意兩只球腳輪的相對(duì)位置，不要讓其對(duì)小車的行進(jìn)方向產(chǎn)生影響。

第五步：小車結(jié)構(gòu)

為了在保證小車基本結(jié)構(gòu)上盡量降低小車的質(zhì)量，我選擇自己設(shè)計(jì)小車的結(jié)構(gòu)件。點(diǎn)擊這里下載我用Google SketchUp設(shè)計(jì)的3D結(jié)構(gòu)模型，然后用3D打印機(jī)打印出來即可。注意，打印之前需要導(dǎo)出適合3D打印機(jī)的STL文件。

第六步：探測(cè)器組件

這一步我需要搭建能夠用來探測(cè)地面標(biāo)線的探測(cè)器設(shè)備。這里我用到了9個(gè)CNY70光電傳感器（5個(gè)或7個(gè)應(yīng)該也可以），9個(gè)220Ω電阻和9個(gè)56kΩ下拉電阻。另外我還設(shè)計(jì)了一個(gè)簡(jiǎn)易的PCB板用來承載我的電路結(jié)構(gòu)。

這套探測(cè)器的原理很簡(jiǎn)單，每個(gè)CNY70傳感器都會(huì)產(chǎn)生0-5V的電壓，其中0V左右表示探測(cè)到黑色，5V左右則是白色。CNY70提供了0-1024的精度范圍。

電路板上一共引出了11個(gè)接口，其中兩個(gè)分別是接地和5V電源，其它9個(gè)是傳感器的模擬輸出。電路本身并不復(fù)雜，復(fù)制以下電路9次即可。

第七步：主電路設(shè)計(jì)

探測(cè)器的電路不復(fù)雜，主電路卻相對(duì)較為復(fù)雜一點(diǎn)。這里我們沒有使用常見的Arduino，而是使用了PIC18F4550單片機(jī)，因?yàn)槠潴w積更小，而且在某些比賽中是禁止使用Arduino的。

電路設(shè)計(jì)如下：

相關(guān)文件：

• VPOWEREXP.pdf

• TOPLAYER.pdf

• BOTTONLAYER.pdf

• VPOWER.pdsprj

主電路的組件清單如下：

• PIC18F4550單片機(jī)

• 20MHz晶振

• 3到6個(gè)LED（PCB上只使用了3個(gè)）

• 3個(gè)220歐姆電阻（用于LED）

• 2個(gè)10nF陶瓷電容

• 2個(gè)18nF陶瓷電容

• L293D或SN7544（電機(jī)驅(qū)動(dòng)模塊）

• 開關(guān)

• 三個(gè)按鈕

• 7805三端穩(wěn)壓器（為單片機(jī)提供5V電壓）

• 線材等

接下來就是制作對(duì)應(yīng)的PCB板。

然后將各個(gè)組件焊接到PCB上。

第八步：編程

接下來就要對(duì)我們機(jī)器人的大腦做文章了。這里我使用的是MPLAB XC8編譯器和Sublime Text作為開發(fā)環(huán)境。具體的安裝過程這里不提，這里提供了Sublime Text用來構(gòu)建和運(yùn)行XC8的命令行代碼，你需要用它來配置XC8的路徑和安裝PK2CMD。

rayito.c是我編寫的代碼，下一個(gè)步驟我會(huì)對(duì)其中一些代碼進(jìn)行一些說明。

第九步：代碼解析

代碼的工作原理示意圖：

各個(gè)傳感器的工作模式示意圖：

傳感器的精度為0-1024，但事實(shí)上我們的測(cè)量基本上不可能達(dá)到兩邊的頂點(diǎn)（純黑和純?nèi)瓷洌Ｄ敲次覀兙涂梢詫?duì)傳感器得到的數(shù)值進(jìn)行定義，低于A值都定義為黑色，高于B值都定義為白色，則有0<=A<=B<=1024。

對(duì)傳感器的讀數(shù)進(jìn)行調(diào)試和校準(zhǔn)：

void initLED(){ ///初始化讀數(shù)變量，在程序啟動(dòng)時(shí)調(diào)用一次

    int x;

    for (x = 0;x < 8;x++){

        amax[x] = 0; ///Amax是上面提到的B值

        amin[x] = 1024; ///Amin是上面提到的A值

    }

}

void CalRead(){ ///讀取校準(zhǔn)數(shù)值

L_ROJO = T1000 < 500*6; ///紅色LED閃爍

    int x;

    for (x = 0;x < 7;x++){  //跟蹤傳感器

        /***注意： 使用T(x)是因?yàn)槠湓诔绦虻牡谝徊糠志瓦M(jìn)行了定義，讓排成列的傳感器能夠以正確的順序排列。***/

        amax[x] = max(amax[x],T(x)); ///如果得到的值高于上一次值則替代

        amin[x] = min(amin[x],T(x)); ///如果得到的值低于上一個(gè)值則替代

    }

    ///不能使用T(7)加入第7個(gè)傳感器的數(shù)值，該傳感器不用于讀路線。. 

    ///我們用其來讀取某些賽道特定的曲線路徑標(biāo)記。

    

    amax[7] = max(amax[7],J(7));  

    amin[7] = min(amin[7],J(7));

}

經(jīng)過了此段之后，我們會(huì)得到7個(gè)由傳感器產(chǎn)生的數(shù)值，并將其存儲(chǔ)在C數(shù)列中，每一個(gè)傳感器都有amin (A)和amax (B)值。那么在程序進(jìn)行標(biāo)準(zhǔn)取值時(shí)，我們將小于A的值都當(dāng)成A，將大于B的值都當(dāng)成B。中間值忽略。

///這是偽代碼，不能直接工作，只用于了解原理

///在實(shí)際代碼中，amax[x]是B而amin[x]是A

int w = W[x]; ///W[x] 取值0~1024

w = min(w,B); //如果w大于B，則w=B

w = max(w,A); //如果w小于A，則w= A

w -= A; ///讓w邏輯上取值在0到B-A

w *= 1024; ///w取值為（0到B-A）*1024

w /= (B-A); ///現(xiàn)在w取值為0 到1024

第十步：加權(quán)平均

這一部分算法是我們實(shí)現(xiàn)高性能的關(guān)鍵部分之一。在本算法中，用于取平均數(shù)的數(shù)字的價(jià)值越高，其對(duì)最終平均數(shù)的影響就越大。這樣我們的算法就不只是以下的算法那么簡(jiǎn)單了：

if (S1 see black){

Left();

}else if (S2 see black){

Right();

}else{

Front();

}

為了真正造出一個(gè)高性能的機(jī)器小車，我們需要盡可能準(zhǔn)確地確定路線的中心線。使用傳感器進(jìn)行測(cè)量時(shí)，每一個(gè)程序循環(huán)中我們都可以得到5組不同的中心線位置，而我們需要通過加權(quán)平均的方式將這5個(gè)數(shù)值轉(zhuǎn)變成一個(gè)數(shù)值。

void Ponderado(){    sum = 0;

    division = 0;

    nove = 0;

    char center;

    for (x = 0;x <= 6;x++){

 

if(T(x)>amax[x]){

w=amax[x];

}else if(T(x) < amin[x]){

w=amin[x];

}else{

w=T(x);

}***/

 

w = ran(T(x),amin[x],amax[x]);

        w -= amin[x];

        w *= (ll)1000;

        w /= (amax[x]-amin[x]);

        if (w > TH){

            nove = 1;

        }

        if (x == 3){

            if (w > TH){ center = 1; }else{ center = 0; } ///We store here if the center sensor reads 0 or 1000

        }

        ///A is the weight

        ///B is the position

        if (x == 0 or x == 6){ continue; } ///We ignore the sensors 0 and 6

v = (1000) * (x-3);///This variable will be -2000,-1000,0,1000,2000 in each cycle

        sum += (w*v); 

        division += (w);

    }

   if (nove == 0){ ///if we don't see the line        POSICION = POSICION > 0 ? 200 : - 200; ///Now we set the value accordingly the last value set. if posicion is > to 0 then posicion equals 200, if not it equals -200

        

    }else{

        POSICION = (ll)(sum) / (ll)(division); ///We get the weighted average result

        POSICION /= 10; ///We change the scale from -2000,2000 to -200,200

    }

}

第十一步：路徑跟蹤

確定了中心線之后就需要控制電機(jī)對(duì)線進(jìn)行跟蹤了。由于我們采用的電機(jī)并不具有直接轉(zhuǎn)向的功能，我們需要用到另一種轉(zhuǎn)向方式：速度差轉(zhuǎn)向——通過為小車的輪子設(shè)定不同的速度從而讓小車整體產(chǎn)生轉(zhuǎn)向效果?？刂扑俣葎t用到了常用的脈沖寬度調(diào)制，這一部分整合到了小車的代碼中。此處我們需要用到一個(gè)函數(shù)MotorsSpeed(a,b)，其將定義電機(jī)的速度，并將其設(shè)置到區(qū)間（-1000,1000）之間，包括前后速度。

void MotorsSpeed(int A,int B){

///If mode equals alfa then we invert the motors voltage

    MotorASpeed(MODE == ALFA ? A : B); 

    MotorBSpeed(MODE == BETA ? A : B);

}

void MotorASpeed(int S){

S = min(S,1000); 

S = max(S,-1000);

    

    ADIR = S > 0 ? 0 : 1;

    S = S > 0 ? S : 1000 + S; 

    

    CCP1CONbits.DC1B1 = S % 4;

    CCPR1L = S / 4;

}

void MotorBSpeed(int S){

    S = min(S,1000);

    S = max(S,-1000);

    

    BDIR = S > 0 ? 0 : 1;

    S = S > 0 ? S : 1000 + S;

    

    CCP2CONbits.DC2B = S % 4;

    CCPR2L = S / 4;

}

實(shí)現(xiàn)了對(duì)電機(jī)的速度控制，就可以實(shí)現(xiàn)轉(zhuǎn)向算法了。

void LineFollow(){

    double kp,kd,kr,speed;

///POSICION has the value of the center of the line previously calculated

    kp = KP[speedMode];

    kd = KD[speedMode];

    kr = KR[speedMode]; 

    DER = POSICION - LP; ///We calculate how much the line has moved from the last iteration

    PIDf = (POSICION* kp + DER * kd);

 

        if (PIDf > 0){

            MotorsSpeed(Mr(speed-PIDf,kr) , speed); ///Mr makes the value to multiply by KR if it is negative. You can delete it        

}else{

            MotorsSpeed(speed , Mr(speed+PIDf,kr) ); ///Mr makes the value to multiply by KR if it is negative. You can delete it

        }

    LP = POSICION;  ///We store the last line position

}

第十二步：完成

部署好了算法也就大功告成了，接下來就該拿到賽場(chǎng)上比試比試。哦，看起來高手還是挺多的嘛……

2015-2016賽季全球創(chuàng)客馬拉松深圳大學(xué)站持續(xù)報(bào)名中！關(guān)注“硬創(chuàng)邦”（微信號(hào)：leiphone_bang），回復(fù)“深大”即可參與報(bào)名！而且，雷鋒網(wǎng)在未來三個(gè)月內(nèi)選一個(gè)合適的時(shí)間，在北上深選擇一個(gè)地點(diǎn)，舉辦創(chuàng)馬“火星救援”專場(chǎng)！詳情可點(diǎn)擊此處了解。

此外還可加入全球創(chuàng)客馬拉松主群（群號(hào)：259592983），參與我們的互動(dòng)討論~

雷峰網(wǎng)原創(chuàng)文章，未經(jīng)授權(quán)禁止轉(zhuǎn)載。詳情見轉(zhuǎn)載須知。