Nano的創意始于2013年暑假，那時候它還叫“蛋黃”，當時的想法是制作一個入門級的自平衡小車（因為趕上學校飛思卡爾比賽，當時報的平衡組，當預習功課了），初步的設想是：基于Arduino制作，可以用PS2手柄遙控，能平衡能走，最好還會賣萌。

其實那也是我第一次接觸和使用Arduino，當時少年窮…買了一塊國產的mini pro裸板，感覺有點開心，然后沒多久就被我瞎接電源給霍霍了…

最初版的蛋黃就是用mini pro做的，由于當時還沒有3D打印機可供差遣，所以所有部件連接基本都是靠萬能的萬能膠，整體結構十分的粗獷，怎么說呢這也許就是蒸汽朋克風吧（并不是）。

蛋黃的資料發在論壇之后引起了不少關注，很多同學被順利地帶進了自平衡小車的坑…一年多時間還不斷有人問我相關的問題，也正因如此，羞愧于靠著這么點干貨應付大家許久，加之個人對自平衡系統有了更深刻的理解…于是決定改進初代蛋黃的諸多不足，著手設計制作蛋黃的2.0版本。

對于初代的不滿情緒主要集中體現在外表（畢竟顏值就是戰斗力），速度控制（當時幾乎沒有用上速度環，只能靠不斷手動調節平衡點進行移動），擴展性（初代的外設太簡陋，沒有完全發揮處理器的性能），以及外表（可以看出我真的很在意…）。

最后就有了Nano啦~

——————————————————————————————————————–

好啦接下來會介紹制作一只Nano的詳細教程，其中包括一些有關自動控制的原理和個人遇到的一些問題和經驗總結。另外值得說明的是，實現自平衡機器人的完整控制需要大量的參數調試過程，因此本教程會盡量以通俗的方式介紹原理和調試方法，但還是需要您擁有一定的電子制作基礎、熟悉 Arduino的使用、較強的動手能力，以及堅定的極客精神，祝成功：-）

#原理篇

自平衡小車是一種典型的倒立擺控制模型，什么是倒立擺呢，普通的鐘擺相信大家都見過

當物體離開垂直的平衡位置之后，便會受到重力與懸線的作用合力，驅動重物回復平衡位置，這個力稱之為回復力，其大小為

F = ?mgsinθ

在偏移角度 θ很小的情況下， sinθ ≈ θ ，所以回復力與偏移的角度之間大小成正比關系，方向相反，在此恢復力作用下，單擺便進行周期運動。

而考慮在空氣中運動的單擺，由于受到空氣的阻尼力， 單擺最終會停止在垂直平衡位置?？諝獾淖枘崃εc單擺運動的角速度成正比，方向相反。阻尼力越大，單擺越會盡快在垂直位置穩定下來。

現在來看這樣一個等效模型

我們的車模其實就相當于一個倒立的鐘擺，可以看到，這時候重力對于物體的作用是向下的，也就是當物體偏移了一點角度之后，重力作用會和偏移角度方向相同，如果車輪不運動的話，擺就會很快倒下。

為了解決這個問題， 使得倒立擺能夠像單擺一樣，穩定在垂直位置，我們有兩個辦法：（1）改變重力的方向（2）增加額外的受力，使得恢復力與位移方向相反。

顯然能夠做到的只有第二種方法，為此我們根據擺的偏移角度控制車輪加減速運動，這樣在小車的坐標系（非慣性系）里，擺就會受到額外的慣性力作用，最終讓擺平衡起來。

再通俗一點說，就是當我們發現小車向前倒的時候就趕緊讓它輪子加速向前，發現小車向后倒的時候就趕緊讓它輪子加速向后，只要這個過程做得足夠精確和快速，就能實現小車的自平衡。

#硬件篇

原理里面說了我們要根據小車偏移的角度來控制輪子的加減速，那么根據需求這里面需要用到的模塊就有：

Arduino主控板 — 選任何一塊你熟悉的就行，推薦nano，小巧，下載方便

陀螺儀加速度計模塊 — 用來測量傾角，推薦MPU6050，便宜，使用方便

減速電機 — 尺寸自定，但最終輸出轉速為300rpm左右會比較合適，值得注意的是電機必須帶編碼器或者碼盤來測速，單相或者兩相的都可以

電機驅動 — 普通尺寸的電機推薦TB6612驅動芯片，比L298的效率高不易發熱（平均電流1.2A左右，功率更大請選L298或其他驅動）；迷你電機用L9110s模塊即可，便宜也很小巧

藍牙模塊 — 用于和手機通信，從模塊或者主從一體的都可以

按鍵 — 任何兩個腳的按鍵都可以，用來進行一些設置

電池 — 如果是用高于5V的鋰電供電的話就可以直接使用，但如果做迷你車用3.7v的小電池供電，就需要注意得額外加一個DC升壓模塊，否則Arduino可能無法正常工作在16MHz

額外擴展的模塊可以有：

超聲波模塊 — 可用于測距和避障，SR04比較常用，更小巧一點的有RCW-0001，當然更小的還可以買收發一體的自己DIY

距離傳感器 — 夏普的一系列傳感器，比超聲模塊貴一些，但效果也更好

OLED顯示屏 — 用來顯示狀態數據當然少不了屏幕，0.96寸的分辨率128×64效果非常好，注意最好買SPI接口的，因為I2C可能跟MPU6050有沖突（可能是個例，按理說地址是不沖突的，具體原因沒有深究）

蜂鳴器 — 讓小車發聲，往往比盯著一個LED看效果更好，推薦使用有源蜂鳴器

攝像頭 — 準確的說是紅外光傳感器，由于Arduino的性能不足以進行圖像處理所以無法使用一般的攝像頭

此外除一般焊接工具和手工工具外，一把熱熔膠槍會成為你DIY的得力助手。至于結構件，有3D打印機的同學可以直接下載后面的 STL文件自行打印，沒有打印機的同學也可以到萬能的橙色網站找到3D打印服務店。還有一點需要說明的是我當時使用的小電機，是自己改裝過的，原本是一個數碼相機的變焦馬達所以非常小巧而且可以裝碼盤，改了減速比和增加出軸之后就拿來做小車了，只不過現在好像沒有這款賣了…

不過后來無意找到一款更合適的N20減速小電機，性能比自己改裝的那款好很多，虛位和摩擦損耗都會更好，出軸也能直接接輪子，而且配備了霍爾測速，長這樣：

有意做超迷你平衡車的同學可以選用這款。TB鏈接就不發了…橙網搜 “N20減速電機 迷你”就能找到的

#軟件篇

軟件篇主要介紹PID算法，可以說PID是整個項目程序的核心，其使用的好壞決定了你的小車能不能自平衡，以及平衡得穩定不穩定。 PID的算法和理論分析網絡上有很多介紹，這里就不詳細講解了大家可以自行搜索?；跀祵W模型的介紹有點不好理解，本文從控制學的角度簡單講解一下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算法了

可以看出，PID的算法實現其實非常簡單，不過只有幾行代碼而已，所以非常建議自己實現一遍PID代碼。Arduino平臺上也是有PID庫的，但庫的名字叫什么我不告訴你，自己去找哦。

#制造篇

如果上面的都可以理解的話就可以開始動手制作啦，這里會以Nano的制作過程為例，但是大家可以根據實際情況自行調整。

網盤老是鏈接失效，所有文件（源代碼+STL模型文件+APP）都發在與非電路城上（收1元當請我吃辣條了…）

價格可能還在審核，需要下載的可以晚一點再去

對里面文件說明一下：

STL文件是Nano頭部和身體的結構件，底座由于大家使用電機不同需要自己確定，按照自己買到的電機的情況制作一個帶兩個電機的底座就行，有熱熔膠槍的幫助應該挺簡單的

源碼建議用1.6.5版本的IDE編譯，舊版本的庫文件有些區別

先看看原理圖（點擊可以打開大圖）

需要說明的是圖里面的電源線是沒有連接的（一正一負大家都知道），另外IO大家可自己調換，只要在程序里改一下宏定義就行；另外編碼器分為單相和AB相的，我這里用的是單相，只能測轉速不能測方向，所以其實方向的檢測是通過給電機的PWM方向近似模擬的，有條件的話還是用雙向的更好。單相編碼器的話直接把信號線接到D2，D3腳，分別屬于中斷0和中斷1；AB相的話則是一根線接到D2或D3另一根線接到一個普通IO用來判斷方向（這時候需要修改程序最后的ENCODER_L（）和ENCODER_R（）函數內容）。

然后就開始按圖連接模塊到外殼里

文件打印好之后按原理圖組合，頭部里面裝了超聲波，Arduino nano板（不帶排針），藍牙模塊，兩個LED，攝像頭以及蜂鳴器，注意把所有的IO都用導線引出到脖子部位

正面的樣子

頭部模塊都塞完之后先別急著粘合，之后還有身體的導線要連接到主控板，里面的部件可以先用熱熔膠固定一下。對了導線選擇的話如果技術好可以用漆包線最省空間，沒有把握的話就用這種FC排線，比較軟去皮容易，比較細而且是整排一起的會美觀一些，杜邦線真的很不好用…

身體部分主要放舵機（如果要用的話），OLED，電機驅動板及升壓板，OLED塞的時候注意不要用蠻力不然屏幕玻璃易碎（不要問我怎么知道的…）

如果上面裝頭部的時候有把導線引出這時就可以焊在屏幕上了

MPU6050在底部，也用502或者熱熔膠固定

然后是舵機，舵機用的是最常見的9g舵機，雖然也有更小的但是怕力矩不夠，可以看到由舵機擺臂推動脖子的連接桿就可以讓頭部運動了。舵機不是很建議采用，一個是小機器人的電池功率比較小，舵機在堵轉的時候容易造成死機，另一個頭部重量還是相對比較大，所以運動的時候重心變化容易干擾平衡，參數設置會更復雜

然后是裝身體部分側面，先把升壓板用熱熔膠固定在一邊，然后蓋上蓋板用502粘合（紅色圈住的是一塊DC-DC升壓板）

這時候主體就已經快完成啦

這時可以開始制作電機底座了，一個參考方案是，用一張廢棄的電話卡或者銀行卡剪裁一下作為基板，然后把兩個電機用熱熔膠固定在基板上，這樣基板就可以用膠和身體粘合起來了

現在可以裝電機驅動了，我用的是L9110S，直接蓋在身體背面

裝上背蓋整個身體部分就完成啦

最后是電池包，找塊輕薄一點的電池直接粘在背部就行，也可以加上電池蓋

側面的樣子

小Nano完成了！

當然現在它還不會動，這時候還是先別急著把頭部粘合，先寫幾個小程序逐個測試一下每個模塊的工作狀態，比如超聲波是否工作，6050有沒有數據，這些程序直接去各個庫里面找到例程改一下定義的引腳運行看效果就行。

當確定沒有問題之后，用熱熔膠仔細地固定好每個部位，準備開始程序調試工作了。

打開源程序文件夾里任意一個文件，就可以看到整個工程代碼

首先需要修改的是引腳定義，把引腳改成你實際連接的情況

/*********************引腳定義*********************/

#define LFT 0

#define RHT 1

#define BUZZER 4 //蜂鳴器

#define BUTTON 5 //按鈕

#define LED 11 //臉頰LED

//#define SERVO 13 //舵機，小機器人不推薦使用，電流易過載

#define TRIG_PIN 8 //超聲波模塊觸發腳

#define ECHO_PIN 7 //超聲波模塊接收腳

然后再最上面的調試選項中，取消IMU_OUTPUT的注釋，像這樣

下載程序到主控里，打開串口監視器，按一下按鍵，就可以看到輸出的角度數據了。

如果數據不正常的話需要檢查前面哪里出問題了，這一步是為了獲取機器人平衡的自然角度：手動把機器人放正，就是大概在自然重心的平衡角度，讀取串口的角度數據，記錄下這個值就是angle_setpoint的值了，把66行的 angle_setpoint = 0改成你得到的值。

下一步把IMU_OUTPUT重新注釋掉，開啟MOTOR_ENABLE的注釋，然后用 Motor（char LR， int SPEED）這個函數放在setup（）的最后面來檢測電機的正負極是否正確，也就是當給Motor（LFT，100）時左輪正轉，Motor（LFT，-100）時左輪反轉，Motor（RHT，100）時右輪正轉，Motor（RHT，-100）時右輪反轉。

上一步也檢測成功之后，就可以開始調節角度環的PID參數了

說明一下這里用宏定義了一遍參數的原因是，如果用的mega328p的芯片的話由于整個程序代碼量還是挺大的所以會提示動態內存緊張，所以在調試完參數之后就把它們用宏固化了節省SRAM

總體的參數整定原則是：

先P后D，如果電機響應慢（比如減速級很多的電機），再調I，如果PD效果足夠好的話則不需要I

單一變量法，即調整一個參數的時候其他參數都固定不變

先定量級再定數值，比如調整P的時候，先從0.0001開始，查看小車反應，沒有效果的話改為0.001，以此類推，直到確定一個合適的數量級，然后才開始在這個數量級里微調，這樣其實就已經把調整的范圍縮小到很小了

先超調再減小，即所有參數都先盡量加大，直到系統震蕩，然后再取這個值的小一點的值作為最合適參數

調試過程中盡量讓車處于自然狀態沒有額外的力作用，即盡量用無線調試，有線的話找根軟一些的線

好那么調節過程中小車什么表現才算是“好”呢？

在角度環中，當P逐漸增大，小車會開始有恢復力作用，也就是當手扶著往前倒的時候小車也能大概跟著往前走，但是還是走的很“軟”，再逐漸增大參數，恢復力越來越大，直到大到一定程度，小車開始前后自主劇烈抖動，電機性能好一點的小車即使手不扶著也能大概站立了。

稍微減小剛剛震蕩的參數，作為P值固定在程序里，開始調節D值，依然是確定量級之后逐漸增大，在增大的過程中會發現小車的震蕩頻率逐漸降低了，增大到一定程度小車基本不再震蕩，這個值就是需要的D值了

也會有一部分情況下由于電機性能原因上面PD的調整過程中始終無法達到很好的效果，那么此時需要加入I，在調完P值之后，D值置0，增加 I值，直到小車的恢復力變得比較“硬”，然后稍微減小P值，直到出現比較理想的直立效果；最后再加D，視效果增加到震蕩為止，再減小到70%左右，這樣角度環PID所有參數就整定完成了。

角度環調好了，小車可以穩定平衡了，可是為啥它一直往一邊跑呢？

因為角度環的任務就是維持小車的角度，除此之外就是它能力范圍之外了，角度環是不關心小車是靜止著平衡的，還是邊跑邊平衡的 — 如果恰好目標平衡點和小車重心平衡點重合，那么小車可以大概靜止，而如果不是，那小車就會在平衡中不斷加速，直到輪子的速度超出了電機所能提供的轉速，于是小車還是會倒下。

所以我們需要添加一個速度環，用編碼器測量速度來作為反饋

在速度環中，首先確定編碼器獲取的數值是正確的，在程序中分別是count_L和count_R儲存計數，打印輸出一下轉動輪子看數據對不對，正確的話，在調節好角度環的基礎上，就可以添加速度環了。

在調試選項中取消SPEED_LOOP的注釋，然后這次我們不需要D，速度環單純靠PI調節，而且先調 I再調P，對應的表現如下：

先給一個比較小的P值（因為I是P的累加，如果P為0的話I也就沒有意義了），隨著I值逐漸增大，用手輕推小車，小車會前進之后慢慢后退，就說明參數起作用了。此時你可以決定到底給一個更大的 I值還是小的 I值，越大的 I 值對應了更快的恢復速度，在偏離之后會更劇烈地后退，在更短的距離內回到原點，但是當然這樣也會降低小車的穩定性，而小的參數則于此相反，需要在前進更長的距離之后才會慢慢回到原點，但是也使得抗干擾能力增強，也就是不會被輕易推倒。

只用 I 值的話小車的回復會是往復的，逐漸逼近原點，加上P值則可以消除這種來回震蕩，與角度環調節D值的過程一樣，逐漸增加直到推了之后可以在前后各擺動一次就回復到原點靜止，此時這個P值便是對應于那個 I值的最合適參數。

這里有個我之前做的一個大車的抗干擾視頻，該車的電機性能非常暴力，因此可以看到其平衡性能非常好

PID參數的調整過程大概如此，總之這是一項既需要細心調整，但自由度也很大的工作，所謂“大膽假設，小心求證”，在多嘗試幾種參數組合之后，你會找到適合你的小車的magic point的~

關于參數的調整本來還有很多可以說，比如調整的形式上，大家可以在小車上加上幾個電位器用analogRead讀取后作為參數值，這樣就可以方便而直觀地觀察到參數連續變化帶來的影響；又比如無線調試的話使用SSH終端的串口協議方式控制會比使用串口助手方便很多等等。但我想對于參數理解最有效的方式還是親手去操作，多嘗試多對比，這里也有一個更詳細的參數整定視頻教程，非常推薦大家繼續觀看，方向環的調試這里面也有詳細介紹：