基于Arduino的迷你巡線機(jī)器人Qbot

Qbot是之前朋友一起構(gòu)思的一個(gè)項(xiàng)目，主要起因是看到這個(gè)會(huì)解迷宮的巡線小機(jī)器人（圖1）。覺(jué)得很搞笑，想弄一個(gè)耍耍，但是這玩意兒國(guó)內(nèi)并不好買，這個(gè)小車在我看來(lái)還是有很大的改進(jìn)空間的（而且不是基于Arduino的而是原生AVR），于是打算自己動(dòng)手重新造輪子。

仔細(xì)想想這東西的可DIY性還是挺高的，除了巡線外，解迷宮、遙控、對(duì)戰(zhàn)等玩法還有很多，要是基于Arduino開(kāi)源當(dāng)然就更合適啦。最后選定的主控芯片方案是Mega32U4，主要是因?yàn)樽詭Т贐ootLoader，可以直接通過(guò)USB下載程序。對(duì)比于3pi增加了：128×64分辨率的OLED顯示屏、兩個(gè)可編程配置的炫彩RGB尾燈、可以直接USB充電的鋰電池管理電路、陀螺儀模塊用來(lái)識(shí)別一些手勢(shì)以及當(dāng)然更小巧的機(jī)身。

硬件篇

巡線傳感器使用的是貼片的對(duì)射式光電開(kāi)關(guān)，其實(shí)就是跟普通的巡線模塊一樣的傳感器只不過(guò)體積會(huì)小巧很多。一共使用了5個(gè)紅外管，實(shí)踐測(cè)試巡線效果非常好。

充電管理IC使用的是TP5400，選它的一個(gè)重要原因是，這顆IC不僅能夠管理鋰電池的充電，還帶升壓功能，因?yàn)殇囯姵氐碾妷菏?.7V，而Arduino需要工作在5V上，所以就免去了電源升壓電路的設(shè)計(jì)，進(jìn)一步縮小了體積。也加入了藍(lán)牙模塊（圖2），可以使用手機(jī)連接進(jìn)行一些調(diào)試和遙控。

電機(jī)采用的是N20減速電機(jī)，非常常用的型號(hào)，不需要編碼器，電機(jī)驅(qū)動(dòng)使用TB6612兩路直流電機(jī)驅(qū)動(dòng)（圖3）。輪子兩邊的主動(dòng)輪網(wǎng)上DIY店鋪都可以找得到，后面的萬(wàn)向輪可能就沒(méi)那么好找啦（圖4），市面上沒(méi)有這么小的萬(wàn)向輪，所以想了個(gè)辦法：使用一根鐵絲和一個(gè)小的軸承做成了萬(wàn)向輪，最終系統(tǒng)原理圖如圖5。

PCB的layout（圖6）：

軟件篇

軟件部分主要就是讀取傳感器的數(shù)據(jù)，以及老生常談的PID了。讀取光電管的數(shù)據(jù)有兩種方式：數(shù)字式和模擬式，分別介紹一下，假設(shè)我們的5個(gè)光電管是排成一排的，而每個(gè)光電管下面如果是黑色（軌跡），則輸出的值就很小（因?yàn)楹谏樟斯饩€），反之白色的話就值比較大，這樣我們?cè)O(shè)定一個(gè)閾值，ADC大于這個(gè)值的話就保存為1，反之為0，于是當(dāng)我們的車處在不同位置的時(shí)候，就得到一個(gè)不同的編碼（圖7）。

可以看到，這個(gè)編碼，其實(shí)就可以量化為一個(gè)數(shù)字，用這個(gè)數(shù)字減去正中的編碼，就可以得到我們需要的誤差了，然后將這個(gè)誤差輸入到PID中進(jìn)行運(yùn)算，就可以得到控制電機(jī)的輸出。原理非常簡(jiǎn)單，這種數(shù)據(jù)方式我就稱為數(shù)字式的輸入，與之對(duì)應(yīng)的，我在Qbot中使用的是模擬式的輸入。

仔細(xì)想想上面這種輸入的方式，我們發(fā)現(xiàn)現(xiàn)實(shí)世界中的情況是沒(méi)有這么離散的，通過(guò)上面的方式我們可以得到5種不同的狀態(tài)，但是如果車子位于其中兩種狀態(tài)之間呢？這樣的話我們就可以取消閾值化的步驟，直接讀取所有傳感器的值進(jìn)行加權(quán)求和，公式如下：

adc[0] = analogRead（A1）；

adc[1] = analogRead（A2）；

adc[2] = analogRead（A3）；

adc[3] = analogRead（A4）；

adc[4] = analogRead（A5）；

error = error + 2*adc[0]；

error = error + 1*adc[1]；

error = error - 1*adc[3]；

error = error - 2*adc[4；

這樣可以更好地表征誤差，大家可以思考一下為什么。

有了誤差，其次就是PID了，這個(gè)資料數(shù)不勝數(shù)，就不再啰嗦啦，大家可以直接看提供的源碼。

這里貼上我在Nano項(xiàng)目里對(duì)PID的介紹：所謂PID就是比例-積分-微分的英文縮寫，但并不是必須同時(shí)具備這三種算法，也可以是 PD、PI，甚至只有 P算法控制，下面分別介紹每個(gè)參數(shù)的含義。

首先需要明確一個(gè)事實(shí)就是，要實(shí)現(xiàn)PID算法，必須在硬件上具有閉環(huán)控制，就是得有反饋。比如控制一個(gè)電機(jī)的轉(zhuǎn)速，就得有一個(gè)測(cè)量轉(zhuǎn)速的傳感器，并將結(jié)果反饋到控制器中，而在自平衡系統(tǒng)中，常用的有三個(gè)控制環(huán) — 角度環(huán)、速度環(huán)、轉(zhuǎn)向環(huán)。

大家可以想象出每個(gè)閉環(huán)的反饋元件分別是什么嗎？對(duì)就是上面元件清單里面包含的 IMU（陀螺儀+加速度計(jì)）、編碼器、攝像頭（或者其他可以確定方位的元件比如陀螺儀、磁場(chǎng)計(jì)等）。

P（比例）：以小車巡線為例，現(xiàn)在需要讓小車跟隨一條軌跡前進(jìn)，用PID算法控制方向環(huán)，反饋傳感器就假設(shè)為攝像頭。那么小車行進(jìn)中有這么幾種情況：

1.車通過(guò)攝像頭發(fā)現(xiàn)自己處在軌跡的左邊，位置誤差值為正，那么就需要向右轉(zhuǎn)向，轉(zhuǎn)向值為正；

2.車通過(guò)攝像頭發(fā)現(xiàn)自己處在軌跡的右邊，位置誤差值為負(fù)，那么就需要向左轉(zhuǎn)向，轉(zhuǎn)向值為負(fù)；

3.車通過(guò)攝像頭發(fā)現(xiàn)自己處在軌跡的正中間，位置誤差值為0，很歡快地筆直前行，轉(zhuǎn)向值為0。

于是我們發(fā)現(xiàn)，小車轉(zhuǎn)向值的輸出可以簡(jiǎn)單地通過(guò)把位置誤差乘以一個(gè)系數(shù)就得到了，而且顯然，誤差越大，得到的轉(zhuǎn)向值也越大，符合需求。這里面這個(gè)系數(shù)，就是P了，而系數(shù)具體的大小，需要根據(jù)實(shí)際情況調(diào)試確定。

我們有了第一個(gè)公式：

P_term = kP * error

D（微分）：還是以小車巡線為例，依然是那車那線那比例。那么小車行進(jìn)中有這么幾種情況：

在P參數(shù)的作用下：

1.小車從左邊向中間逐漸靠攏，終于它到達(dá)了中點(diǎn)……然而，由于慣性，它根本停不下來(lái)，于是小車又到了線的右邊；

2.小車從右邊向中間逐漸靠攏，終于它到達(dá)了中點(diǎn)……然而，由于慣性，它根本停不下來(lái)！于是小車又到了線的左邊。

這跟說(shuō)好的不一樣！于是這個(gè)時(shí)候D出場(chǎng)了，想想我們期待的效果是啥，我們希望小車到達(dá)中點(diǎn)，此時(shí)不光位置誤差為0，還要轉(zhuǎn)向速度誤差也為0。

那么我們?cè)O(shè)定期望的轉(zhuǎn)向速度為0，此時(shí)如果小車轉(zhuǎn)向速度向右的話誤差為+，向左為-，再看前面的情況1，小車的轉(zhuǎn)向速度誤差為+，我們應(yīng)該在P之外再給它一個(gè)向左的轉(zhuǎn)向力，才能保證它在到達(dá)中點(diǎn)時(shí)速度不會(huì)那么快；情況2類似，此時(shí)需要向右的轉(zhuǎn)向力。

也就是說(shuō)，D相當(dāng)于給了小車一個(gè)轉(zhuǎn)向的阻力，而這個(gè)力，又恰好可以通過(guò)簡(jiǎn)單地把轉(zhuǎn)向速度誤差乘以一個(gè)系數(shù)得到，顯然，轉(zhuǎn)向速度誤差越大，得到的阻力越大，符合需求（值得注意的是這里的轉(zhuǎn)向速度是相對(duì)中點(diǎn)的，并不是指小車輸出的轉(zhuǎn)向速度，可以理解為“位置變化的速度”）。

我們有了第二個(gè)公式：

D_term = kD* （error- last_error）

如果上面的例子還是不好理解的話，考慮前面的單擺模型：

P相當(dāng)于重力的作用，讓擺左右往復(fù)運(yùn)動(dòng)，而D則相當(dāng)于空氣阻力，讓擺慢慢停在中點(diǎn)。D的大小很理想的情況下，應(yīng)該是大概擺動(dòng)左右各一下之后就停在中點(diǎn)，想象把擺放在水中擺動(dòng)的情況。

I（積分）：有的時(shí)候我們會(huì)發(fā)現(xiàn)，系統(tǒng)中存在一些固定的阻力，例如，我們用PID控制一個(gè)電機(jī)的轉(zhuǎn)速，當(dāng)給定的目標(biāo)速度很小的時(shí)候，就會(huì)出現(xiàn)這樣的情況：

根據(jù)P_term = kP * error，由于error很小，P的輸出也很小，而由于摩擦力的存在，此時(shí)并不能讓電機(jī)轉(zhuǎn)動(dòng)起來(lái)；又由D_term = kD* （error- last_error），由于電機(jī)沒(méi)有轉(zhuǎn)動(dòng)，顯然（error- last_error）始終為0于是D輸出也為0，那么問(wèn)題來(lái)了，除非改變目標(biāo)值，否則電機(jī)就永遠(yuǎn)轉(zhuǎn)不起來(lái)了…

I的作用就是消除這樣的靜態(tài)誤差，它會(huì)將每次的誤差都積累起來(lái)，然后同樣也是乘以一個(gè)系數(shù)之后作為輸出。比如上面的情況，雖然誤差很小，但卻不是0，于是在每一輪的計(jì)算中，I項(xiàng)把error逐漸累積，直到超過(guò)臨界值讓電機(jī)轉(zhuǎn)起來(lái)；而在誤差為0的情況下，I項(xiàng)卻又不會(huì)幫倒忙。

第三個(gè)公式：I_term = kI*（I_term + error）

以上就是PID的全部計(jì)算了，最后三者加起來(lái)就得到了：

PID_output = P_term + I_term + D_term

每隔一段固定時(shí)間把它運(yùn)行一遍，就是PID算法了。

然后是APP，有對(duì)應(yīng)的庫(kù)開(kāi)源了，大家可以試著開(kāi)發(fā)一下。