基于Arduino的迷你巡線機器人Qbot

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

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

硬件篇

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

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

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

PCB的layout（圖6）：

軟件篇

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

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

仔細想想上面這種輸入的方式，我們發現現實世界中的情況是沒有這么離散的，通過上面的方式我們可以得到5種不同的狀態，但是如果車子位于其中兩種狀態之間呢？這樣的話我們就可以取消閾值化的步驟，直接讀取所有傳感器的值進行加權求和，公式如下：

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了，這個資料數不勝數，就不再啰嗦啦，大家可以直接看提供的源碼。

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

首先需要明確一個事實就是，要實現PID算法，必須在硬件上具有閉環控制，就是得有反饋。比如控制一個電機的轉速，就得有一個測量轉速的傳感器，并將結果反饋到控制器中，而在自平衡系統中，常用的有三個控制環 — 角度環、速度環、轉向環。

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

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

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

2.車通過攝像頭發現自己處在軌跡的右邊，位置誤差值為負，那么就需要向左轉向，轉向值為負；

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

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

我們有了第一個公式：

P_term = kP * error

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

在P參數的作用下：

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

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

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

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

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

我們有了第二個公式：

D_term = kD* （error- last_error）

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

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

I（積分）：有的時候我們會發現，系統中存在一些固定的阻力，例如，我們用PID控制一個電機的轉速，當給定的目標速度很小的時候，就會出現這樣的情況：

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

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

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

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

PID_output = P_term + I_term + D_term

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

然后是APP，有對應的庫開源了，大家可以試著開發一下。