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

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

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

硬件篇

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

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

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

PCB的layout（圖6）：

軟件篇

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

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

仔細(xì)想想上面這種輸入的方式，我們發(fā)現(xiàn)現(xiàn)實(shí)世界中的情況是沒有這么離散的，通過上面的方式我們可以得到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了，這個資料數(shù)不勝數(shù)，就不再啰嗦啦，大家可以直接看提供的源碼。

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

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

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

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

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

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

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

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

我們有了第一個公式：

P_term = kP * error

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

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

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

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

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

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

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

我們有了第二個公式：

D_term = kD* （error- last_error）

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

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

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

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

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

第三個公式：I_term = kI*（I_term + error）

以上就是PID的全部計算了，最后三者加起來就得到了：

PID_output = P_term + I_term + D_term

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

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