稚暉君教你制作全球最迷你的自平衡機器人
干貨福利,第一時間送達(dá)!
自平衡站立
自平衡行走轉(zhuǎn)向
手機APP遙控及交互
超聲波感應(yīng)
攝像頭目標(biāo)跟蹤
賣萌
Nano與蛋黃
Nano的創(chuàng)意始于2013年暑假,那時候它還叫“蛋黃”,當(dāng)時的想法是制作一個入門級的自平衡小車(因為趕上學(xué)校飛思卡爾比賽,當(dāng)時報的平衡組,當(dāng)預(yù)習(xí)功課了),初步的設(shè)想是:基于Arduino制作,可以用PS2手柄遙控,能平衡能走,最好還會賣萌。其實那也是我第一次接觸和使用Arduino,當(dāng)時少年窮…買了一塊國產(chǎn)的mini pro裸板,感覺有點開心,然后沒多久就被我瞎接電源給霍霍了…制作教程
好啦接下來會介紹制作一只Nano的詳細(xì)教程,其中包括一些有關(guān)自動控制的原理和個人遇到的一些問題和經(jīng)驗總結(jié)。另外值得說明的是,實現(xiàn)自平衡機器人的完整控制需要大量的參數(shù)調(diào)試過程,因此本教程會盡量以通俗的方式介紹原理和調(diào)試方法,但還是需要您擁有一定的電子制作基礎(chǔ)、熟悉Arduino的使用、較強的動手能力,以及堅定的極客精神,祝成功:-)原理篇
自平衡小車是一種典型的倒立擺控制模型,什么是倒立擺呢,普通的鐘擺相信大家都見過sinθ ≈ θ
,所以回復(fù)力與偏移的角度之間大小成正比關(guān)系, 方向相反,在此恢復(fù)力作用下,單擺便進行周期運動。而考慮在空氣中運動的單擺,由于受到空氣的阻尼力, 單擺最終會停止在垂直平衡位置。空氣的阻尼力與單擺運動的角速度成正比, 方向相反。阻尼力越大,單擺越會盡快在垂直位置穩(wěn)定下來。現(xiàn)在來看這樣一個等效模型硬件篇
原理里面說了我們要根據(jù)小車偏移的角度來控制輪子的加減速,那么根據(jù)需求這里面需要用到的模塊就有:模塊 | 說明 |
---|---|
Arduino主控板 | 選任何一塊你熟悉的就行,推薦nano,小巧,下載方便 |
陀螺儀加速度計模塊 | 用來測量傾角,推薦MPU6050,便宜,使用方便 |
減速電機 | 尺寸自定,但最終輸出轉(zhuǎn)速為300rpm左右會比較合適,值得注意的是電機必須帶編碼器或者碼盤來測速,單相或者兩相的都可以 |
電機驅(qū)動 | 普通尺寸的電機推薦TB6612驅(qū)動芯片,比L298的效率高不易發(fā)熱(平均電流1.2A左右,功率更大請選L298或其他驅(qū)動);迷你電機用L9110s模塊即可,便宜也很小巧 |
藍(lán)牙模塊 | 用于和手機通信,從模塊或者主從一體的都可以 |
按鍵 | 任何兩個腳的按鍵都可以,用來進行一些設(shè)置 |
電池 | 如果是用高于5V的鋰電供電的話就可以直接使用,但如果做迷你車用3.7v的小電池供電,就需要注意得額外加一個DC升壓模塊,否則Arduino可能無法正常工作在16MHz |
超聲波模塊 | 可用于測距和避障,SR04比較常用,更小巧一點的有RCW-0001,當(dāng)然更小的還可以買收發(fā)一體的自己DIY |
距離傳感器 | 夏普的一系列傳感器,比超聲模塊貴一些,但效果也更好 |
OLED顯示屏 | 用來顯示狀態(tài)數(shù)據(jù)當(dāng)然少不了屏幕,0.96寸的分辨率128×64效果非常好,注意最好買SPI接口的,因為I2C可能跟MPU6050有沖突(可能是個例,按理說地址是不沖突的,具體原因沒有深究) |
蜂鳴器 | 讓小車發(fā)聲,往往比盯著一個LED看效果更好,推薦使用有源蜂鳴器 |
攝像頭 Motion Sensor | 準(zhǔn)確的說是紅外光傳感器,由于Arduino的性能不足以進行圖像處理所以無法使用一般的攝像頭 |
軟件篇
軟件篇主要介紹PID算法,可以說PID是整個項目程序的核心,其使用的好壞決定了你的小車能不能自平衡,以及平衡得穩(wěn)定不穩(wěn)定。PID的算法和理論分析網(wǎng)絡(luò)上有很多介紹,這里就不詳細(xì)講解了大家可以自行搜索?;跀?shù)學(xué)模型的介紹有點不好理解,本文從控制學(xué)的角度簡單講解一下PID及其使用方法。所謂PID就是比例-積分-微分的英文縮寫,但并不是必須同時具備這三種算法,也可以是 PD, PI,甚至只有 P算法控制,下面分別介紹每個參數(shù)的含義:首先需要明確一個事實就是,要實現(xiàn)PID算法,必須在硬件上具有閉環(huán)控制,就是得有反饋。比如控制一個電機的轉(zhuǎn)速,就得有一個測量轉(zhuǎn)速的傳感器,并將結(jié)果反饋到控制器中,而在自平衡系統(tǒng)中,常用的有三個控制環(huán) — 角度環(huán)、速度環(huán)、轉(zhuǎn)向環(huán)大家可以想象出每個閉環(huán)的反饋元件分別是什么嗎,對就是上面元件清單里面包含的 IMU(陀螺儀 加速度計)、編碼器、攝像頭(或者其他可以確定方位的元件比如陀螺儀,磁場計等)P(比例):以小車巡線為例,現(xiàn)在需要讓小車跟隨一條軌跡前進,用PID算法控制方向環(huán),反饋傳感器就假設(shè)為攝像頭。那么小車行進中有這么幾種情況: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ù)實際情況調(diào)試確定。我們有了第一個公式:D_term?=?kD*?(error-?last_error)
如果上面的例子還是不好理解的話,考慮前面的單擺模型:P相當(dāng)于重力的作用,讓擺左右往復(fù)運動,而D則相當(dāng)于空氣阻力,讓擺慢慢停在中點。D的大小很理想的情況下,應(yīng)該是大概擺動左右各一下之后就停在中點,想象把擺放在水中擺動的情況。I(積分):有的時候我們會發(fā)現(xiàn),系統(tǒng)中存在一些固定的阻力,例如,我們用PID控制一個電機的轉(zhuǎn)速,當(dāng)給定的目標(biāo)速度很小的時候,就會出現(xiàn)這樣的情況:根據(jù)P_term = kP * error
,由于error
很小,P
的輸出也很小,而由于摩擦力的存在,此時并不能讓電機轉(zhuǎn)動起來;又由D_term = kD* (error- last_error)
,由于電機沒有轉(zhuǎn)動,顯然(error- last_error)
始終為0于是D輸出也為0,那么問題來了,除非改變目標(biāo)值,否則電機就永遠(yuǎn)轉(zhuǎn)不起來了…I的作用就是消除這樣的靜態(tài)誤差,它會將每次的誤差都積累起來,然后同樣也是乘以一個系數(shù)之后作為輸出。比如上面的情況,雖然誤差很小,但卻不是0,于是在每一輪的計算中,I項把error
逐漸累積,直到超過臨界值讓電機轉(zhuǎn)起來;而在誤差為0的情況下,I項卻又不會幫倒忙。第三個公式:I_term = kI*(I_term error)
以上就是PID的全部計算了,最后三者加起來就得到了:PID_output?=?P_term? ?I_term? ?D_term
每隔一段固定時間把它運行一遍,就是PID算法了。可以看出,PID的算法實現(xiàn)其實非常簡單,不過只有幾行代碼而已,所以非常建議自己實現(xiàn)一遍PID代碼。Arduino平臺上也是有PID庫的,但庫的名字叫什么我不告訴你,自己去找哦。制造篇
如果上面的都可以理解的話就可以開始動手制作啦,這里會以Nano的制作過程為例,但是大家可以根據(jù)實際情況自行調(diào)整。網(wǎng)盤老是鏈接失效,所有文件(源代碼 STL模型文件 APP)都發(fā)在與非電路城上(收1元當(dāng)請我吃辣條了…)對里面文件說明一下:1、STL文件是Nano頭部和身體的結(jié)構(gòu)件,底座由于大家使用電機不同需要自己確定,按照自己買到的電機的情況制作一個帶兩個電機的底座就行,有熱熔膠槍的幫助應(yīng)該挺簡單的2、源碼建議用1.6.5版本的IDE編譯,舊版本的庫文件有些區(qū)別原理圖
/*********************引腳定義*********************/
#define?LFT?0
#define?RHT?1
#define?BUZZER?4?//蜂鳴器
#define?BUTTON?5?//按鈕
#define?LED?11?//臉頰LED
//#define?SERVO?13?//舵機,小機器人不推薦使用,電流易過載
#define?TRIG_PIN?8?//超聲波模塊觸發(fā)腳
#define?ECHO_PIN?7?//超聲波模塊接收腳
然后再最上面的調(diào)試選項中,取消IMU_OUTPUT的注釋,像這樣/*****************************調(diào)試選項********************************/
//#define?TIMING_DEBUG???//PID周期調(diào)試,開啟后打印時間信息
//#define?PARAM_DEBUG????//PID參數(shù)調(diào)試,關(guān)閉后用宏取代變量值節(jié)省動態(tài)內(nèi)存
#define?IMU_OUTPUT??//輸出6050數(shù)據(jù)
//#define?SONIC_OUTPUT??//輸出超聲波數(shù)據(jù)
//#define?SPEED_LOOP???????//速度環(huán)開關(guān)
//#define?MOTOR_ENABLE?????//電機使能
//#define?SONIC_ENABLE?????//超聲波使能
//#define?CAMERA_ENABLE??//攝像頭使能
下載程序到主控里,打開串口監(jiān)視器,按一下按鍵,就可以看到輸出的角度數(shù)據(jù)了。如果數(shù)據(jù)不正常的話需要檢查前面哪里出問題了,這一步是為了獲取機器人平衡的自然角度:手動把機器人放正,就是大概在自然重心的平衡角度,讀取串口的角度數(shù)據(jù),記錄下這個值就是angle_setpoint
的值了,把66行的angle_setpoint = 0
改成你得到的值。下一步把IMU_OUTPUT
重新注釋掉,開啟MOTOR_ENABLE
的注釋,然后用?Motor(char LR, int SPEED)
這個函數(shù)放在setup()
的最后面來檢測電機的正負(fù)極是否正確,也就是當(dāng)給Motor(LFT,100)
時左輪正轉(zhuǎn),Motor(LFT,-100)
時左輪反轉(zhuǎn),Motor(RHT,100)
時右輪正轉(zhuǎn),Motor(RHT,-100)
時右輪反轉(zhuǎn)。上一步也檢測成功之后,就可以開始調(diào)節(jié)角度環(huán)的PID參數(shù)了/***************PID變量定義**********************/
#ifdef?PARAM_DEBUG??//角度環(huán)數(shù)據(jù)
double??P_angle?=?0,?I_angle?=?0,?D_angle?=?0;
#else
#define?P_angle??0
#define?I_angle??0
#define?D_angle??0
#endif
double??angle_setpoint?=?0,?angle_output,?angle_integral;
uint32_t?angle_PID_timer;
#ifdef?PARAM_DEBUG??//速度環(huán)數(shù)據(jù)
double??P_speed?=?0,?I_speed?=?0;
#else
#define?P_speed??0
#define?I_speed??0
#endif
double??speed_setpoint?=?0,?speed_output,?speed_integral;
uint32_t?speed_PID_timer;
說明一下這里用宏定義了一遍參數(shù)的原因是,如果用的mega328p的芯片的話由于整個程序代碼量還是挺大的所以會提示動態(tài)內(nèi)存緊張,所以在調(diào)試完參數(shù)之后就把它們用宏固化了節(jié)省SRAM。總體的參數(shù)整定原則是:1、先P
后D
,如果電機響應(yīng)慢(比如減速級很多的電機),再調(diào)I,如果PD
效果足夠好的話則不需要I2、單一變量法,即調(diào)整一個參數(shù)的時候其他參數(shù)都固定不變3、先定量級再定數(shù)值,比如調(diào)整P的時候,先從0.0001
開始,查看小車反應(yīng),沒有效果的話改為0.001
,以此類推,直到確定一個合適的數(shù)量級,然后才開始在這個數(shù)量級里微調(diào),這樣其實就已經(jīng)把調(diào)整的范圍縮小到很小了4、先超調(diào)再減小,即所有參數(shù)都先盡量加大,直到系統(tǒng)震蕩,然后再取這個值的小一點的值作為最合適參數(shù)5、調(diào)試過程中盡量讓車處于自然狀態(tài)沒有額外的力作用,即盡量用無線調(diào)試,有線的話找根軟一些的線好那么調(diào)節(jié)過程中小車什么表現(xiàn)才算是“好”呢?
在角度環(huán)中,當(dāng)P逐漸增大,小車會開始有恢復(fù)力作用,也就是當(dāng)手扶著往前倒的時候小車也能大概跟著往前走,但是還是走的很“軟”
,再逐漸增大參數(shù),恢復(fù)力越來越大,直到大到一定程度,小車開始前后自主劇烈抖動,電機性能好一點的小車即使手不扶著也能大概站立了。稍微減小剛剛震蕩的參數(shù),作為P值
固定在程序里,開始調(diào)節(jié)D值
,依然是確定量級之后逐漸增大,在增大的過程中會發(fā)現(xiàn)小車的震蕩頻率逐漸降低了,增大到一定程度小車基本不再震蕩,這個值就是需要的D值
了也會有一部分情況下由于電機性能原因上面PD
的調(diào)整過程中始終無法達(dá)到很好的效果,那么此時需要加入I
,在調(diào)完P
值之后,D
值置0,增加 I值,直到小車的恢復(fù)力變得比較“硬”,然后稍微減小P
值,直到出現(xiàn)比較理想的直立效果;最后再加D
,視效果增加到震蕩為止,再減小到70%左右,這樣角度環(huán)PID
所有參數(shù)就整定完成了。角度環(huán)調(diào)好了,小車可以穩(wěn)定平衡了,可是為啥它一直往一邊跑呢?
因為角度環(huán)的任務(wù)就是維持小車的角度,除此之外就是它能力范圍之外了,角度環(huán)是不關(guān)心小車是靜止著平衡的,還是邊跑邊平衡的 — 如果恰好目標(biāo)平衡點和小車重心平衡點重合,那么小車可以大概靜止,而如果不是,那小車就會在平衡中不斷加速,直到輪子的速度超出了電機所能提供的轉(zhuǎn)速,于是小車還是會倒下。所以我們需要添加一個速度環(huán),用編碼器測量速度來作為反饋在速度環(huán)中,首先確定編碼器獲取的數(shù)值是正確的,在程序中分別是count_L
和count_R
儲存計數(shù),打印輸出一下轉(zhuǎn)動輪子看數(shù)據(jù)對不對,正確的話,在調(diào)節(jié)好角度環(huán)的基礎(chǔ)上,就可以添加速度環(huán)了。在調(diào)試選項中取消SPEED_LOOP
的注釋,然后這次我們不需要D
,速度環(huán)單純靠PI
調(diào)節(jié),而且先調(diào)?I
再調(diào)P
,對應(yīng)的表現(xiàn)如下:先給一個比較小的P值(因為I是P的累加,如果P為0的話I也就沒有意義了),隨著I值逐漸增大,用手輕推小車,小車會前進之后慢慢后退,就說明參數(shù)起作用了。此時你可以決定到底給一個更大的 I值還是小的I
值,越大的I
值對應(yīng)了更快的恢復(fù)速度,在偏離之后會更劇烈地后退,在更短的距離內(nèi)回到原點,但是當(dāng)然這樣也會降低小車的穩(wěn)定性,而小的參數(shù)則于此相反,需要在前進更長的距離之后才會慢慢回到原點,但是也使得抗干擾能力增強,也就是不會被輕易推倒。只用I
值的話小車的回復(fù)會是往復(fù)的,逐漸逼近原點,加上P
值則可以消除這種來回震蕩,與角度環(huán)調(diào)節(jié)D
值的過程一樣,逐漸增加直到推了之后可以在前后各擺動一次就回復(fù)到原點靜止,此時這個P
值便是對應(yīng)于那個 I值的最合適參數(shù)。這里有個我之前做的一個大車的抗干擾視頻,該車的電機性能非常暴力,因此可以看到其平衡性能非常好PID參數(shù)
的調(diào)整過程大概如此,總之這是一項既需要細(xì)心調(diào)整,但自由度也很大的工作,所謂“大膽假設(shè),小心求證”,在多嘗試幾種參數(shù)組合之后,你會找到適合你的小車的magic point
的~關(guān)于參數(shù)的調(diào)整本來還有很多可以說,比如調(diào)整的形式上,大家可以在小車上加上幾個電位器用analogRead
讀取后作為參數(shù)值,這樣就可以方便而直觀地觀察到參數(shù)連續(xù)變化帶來的影響;又比如無線調(diào)試的話使用SSH終端
的串口協(xié)議方式控制會比使用串口助手方便很多等等。但我想對于參數(shù)理解最有效的方式還是親手去操作,多嘗試多對比。文章來自華為天才少年稚暉君的博客,http://www.pengzhihui.xyz/2015/12/09/nano/ 關(guān)于稚暉君的介紹請看:華為天才少年——稚暉君!
End