2015年3月10日 星期二

兩輪自平衡車DIY

0. 前言 

本文將介紹如何利用Raspberry Pi自行製作一台兩輪自平衡車(segway).
先聲明一下, 這方面我只是初學者, 自動控制與工程數學理論都還給老師很久了,
寫這篇純粹是為了拋磚引玉, 希望有經驗的高手們也能分享經驗.

1. 硬體需求

  • Raspberry Pi + 外殼 + WiFi網卡
    我用的是Pi 2, 請自行先安裝raspbian
    WiFi網卡是edimax的, 安裝方法不另贅述
  • DC motors + 輪胎 + 車架 + 壓克力板 + 銅柱
    我從這裡買的,
    我當時買到的馬達規格是減速比1:34. 我想1:21應該更好.
    其它的馬達如果轉速夠快扭力夠大應該也可以用, 有經驗的朋友們請幫忙分享心得.
  • L293D IC 馬達驅動IC
    我從這裡買的.
  • MPU6050六軸感測器
    我從這裡買的,
    順便也買到焊接針腳, 這樣就可以將感測器固定到麵包板上了.

2. 機構組裝



  • 上面是我買的那一包車體零件開箱照
  • 依照說明安裝車體, 包括L架, 馬達, 輪胎, 最後把壓克力板還有銅柱都裝上去.
    參考這裡的組裝圖
  • 裝完以後的樣子


(其實拍完上面那一張照片, 下一分鐘我的Pi就摔到地面, 透明壓克力盒就摔破了...)


這裡要說明一下, 車體零件內附的銅柱是 3cm的, 如果您的Pi外殼超過3cm 會塞不進去, 可以考慮換銅柱. 或是更換Pi的外殼.

我是考量原廠外殼太重, 卡榫又容易摔斷, 所以後來換了一款薄型類似名片盒的外殼. 又輕又耐摔. 最上面那一片壓克力板是準備要來放麵包板的.




3. 安裝線路

開始插麵包版吧



這裡簡單說明一下電路,

MPU6050模組是使用I2C介面, 有四根線需要接上Raspberry Pi, 分別是:
Vcc --> RPi GPIO header pin#1 (3.3V),
SDA --> RPi GPIO header pin#3 (SDA)
SCL --> RPI GPIO header pin#5 (SCL)
GND --> RPi GPIO header pin#6 (GND)

關於馬達的部分, 我使用常見的L293D 驅動IC.
GPIO header pin#11, pin#13, pin#15 控制左輪, 接到L293D的pin#2, pin#7, pin#1
GPIO header pin#16, pin#18, pin#22 控制右輪, 接到L293D的pin#15, pin#10, pin#9
L293D的pin#3, pin#6 接左輪馬達
L293D的pin#14, pin#11 接右輪馬達
Vs 接 12V DC
Vss 接 5V DC
(Vs & Vss 這兩隻腳不要搞混)


還有就是, L293D 需要有夠高的電壓來源才能驅動馬達,
我的做法是買一個 12V DC 1A 變壓器, 再配一個轉接頭再接上麵包板, 這樣才能供應 L293D 的 Vs (pin#8).


還要注意一點就是, 請把車體前後的接線整理固定好, 因為後面做平衡調校測試的時候一定會摔車好多次, 前後的線路就是首當其衝, 最好都用束線帶綁好.

全部都固定完畢的樣子








後面我們會講到用軟體測試上面的接線是否正確.

4. 軟體部份

這裡我們要搞定I2C, 並且使用wiringPi函式庫.
wiringPi函式庫, 有基本GPIO功能, 又支援I2C 讀取MPU6050資料, 還有SoftPWM 功能可以控制DC motors轉速.
真是一舉數得呀!!


安裝步驟如下

先安裝i2c driver.

$ sudo apt-get install libi2c-dev

設定下次開機要啟動 i2c driver,

$ sudo vi /etc/modules 

增加以下兩行到裡面, 儲存.
i2c-bcm2708
i2c-dev

有一個blacklist檔案要檢查
$ sudo vi /etc/modprobe.d/raspi-blacklist.conf

確認裡面沒有以下兩行, 如果有出現, 請在前面都加上 #符號 (也就是把他comment掉), 儲存.

#blacklist spi-bcm2708
#blacklist i2c-bcm2708

再確定raspi-config

$ sudo raspi-config
進入Advanced Options -> I2C, 把它 enabled


然後Pi要重新開機
$ sudo bash; sync;sync;reboot

開機回來後, 要確認i2c driver kernel module 有正常啟動

$ lsmod |grep i2c
i2c_dev                 6027  0
i2c_bcm2708             4990  0


安裝 git
$ sudo apt-get install git-core

下載並安裝 wiringPi

$ cd
$ git clone git://git.drogon.net/wiringPi           
$ cd wiringPi
$ sudo ./build


5. 測試MPU6050

先安裝測試工具
$ sudo apt-get install i2c-tools

執行這個工具測試 i2c bus上是否有看到MPU6050.

$ sudo i2cdetect -y 0 (for a Revision 1 board)
   or
$ sudo i2cdetect -y 1 (for a Revision 2 board)


如果看到以下輸出... 注意那個"68", 就表示已經正常抓到MPU6050了.

     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f
00:          -- -- -- -- -- -- -- -- -- -- -- -- --
10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
60: -- -- -- -- -- -- -- -- 68 -- -- -- -- -- -- --
70: -- -- -- -- -- -- -- --

再來我們要下載測試程式嘗試抓取加速度計(accelerometer) & 陀螺儀(gyroscope meter)數據.

下載測試程式, 並編譯

$ cd
$ git clone https://github.com/wennycooper/mpu6050_in_c.git
$ cd mpu6050_in_c
$ cat README.md

$ gcc -o mpu6050_accl ./mpu6050_accl.c  -lwiringPi -lpthread -lm
$ gcc -o mpu6050_gyro ./mpu6050_gyro.c  -lwiringPi -lpthread -lm


現在來看看加速度的數據, 請執行以下程式:

$ sudo ./mpu6050_accl

應該要看到X, Y, Z 加速度數據, 如下:
My acclX_scaled: 0.140625
My acclY_scaled: -0.031006
My acclZ_scaled: 0.994141
My X rotation: -1.768799
My Y rotation: -8.047429

其中 acclX, acclY, acclZ 是三軸加速度數據
(單位是g, 沒錯! 就是物理課本上面的重力加速度 g=9.8m/s^2)
My X/Y rotation 是換算出來的角度 (單位是 degree).

請試著將車體前傾或後仰, 看看數據的變化.
如果您安裝MPU6050 的方向正確的話, 車體前傾或後仰應該會看到 Y rotation 介於 -90度 ~ 90度.


再來看看陀螺儀的數據, 請執行以下程式:

$ sudo ./mpu6050_gyro

應該要看到X, Y, Z軸的角速度, 單位是 degree/s
My gyroZ_scaled: -1.954198
My gyroX_scaled: -4.312977
My gyroY_scaled: 0.458015
My gyroZ_scaled: 0.366412
My gyroX_scaled: -4.053435
My gyroY_scaled: 0.427481
My gyroZ_scaled: -0.160305

請嘗試著觀察轉動瞬間與數據的變化, 就會理解陀螺儀數據了.

這裡說明一下, 單純用上面那個加速度計不就可以推算出傾斜角度不是嗎? 為何需要用到陀螺儀數據呢??

主要是因為加速度數據很容易浮動, 後來有人發明了使用陀螺儀數據再補上加速度數據的方法, 這樣就能算出比較穩定可靠的傾斜角度.
這套方法叫做 "complementary filter". 詳細說明請看參考文件.

6. 測試DC motors


現在我們用 wiringPi 寫一個程式測試 DC motors.


$ cd
$ git clone https://github.com/wennycooper/dcMotor.git
$ cd dcMotor
$ cat README.md

$ gcc -o dcMotor0 dcMotor0.c -lwiringPi -lpthread

準備要讓馬達轉囉!! 請確認12V電源有正常插上, 並把車體用手拿起來, 執行程式!!

$ sudo ./dcMotor0

如何?? 正常的話兩個馬達應該正轉5秒鐘, 再反轉5秒鐘, 最後停止.
馬達如果沒有轉, 表示前面步驟執行有錯, 請回頭去檢查.
如果有轉, 請確認兩個馬達都有轉同樣方向, 如果反向, 請把一個馬達接腳兩根交換, 應該可以改善.

還要注意一點, 您的兩個馬達都有轉得很 "夠快" 嗎??
如果轉得很慢, 比方說每秒鐘大約才2轉, 很有可能是L293D 的部分接線有問題, 請回頭再確認!!

轉速太慢可能會造成車體傾斜時馬達來不及將車體導正, 這樣就無法平衡了.


7. 最後整合


令人興奮的時刻要來了!! 我們要把上面所有的努力全部整合到一起!!
請下載並編譯最後這套兩輪自平衡車控制程式.

$ cd
$ git clone https://github.com/wennycooper/mySegway.git
$ cd mySegway
$ gcc -o mySegway ./mySegway.c ./motors.c  -lwiringPi -lpthread -lm

這程式會根據測到的數據, 努力保持車體平衡.
如果傾斜角度過大(>60度), 會自動讓馬達停止.

8. 執行

由於每一台車體安裝後重心位置都不一樣, 所以平衡角度也不一樣.
我在程式碼 mySegway.c 裡面有一個變數 angle_offset 就是這個用途.
這個變數如果沒有調整適當, 車子會偏向某一個方向最後傾倒.
請試幾次, 找到適合您的車子的angle_offset.

還有就是, 由於馬達扭力與轉速的限制, 這台車無法在傾斜角度過大的狀況平衡回來.
所以在 "程式啟動瞬間請盡量保持車體平衡". 這有一點點難度, 我的做法是左手放車子到地面, 扶著它盡量保持車體水平, 然後瞬間右手敲鍵盤執行程式.

以下是結果錄影.

https://www.youtube.com/watch?v=QyjIQknHPFw


https://www.youtube.com/watch?v=GkUGnLWh41k


https://www.youtube.com/watch?v=KOAsOaqeauU


如果不成功, 可能原因如下:

1. 馬達驅動方向和感測器方向相反, 可能需要改一下motors.c speed 的方向
2. 如果馬達有往正確的方向轉, 但還是傾倒, 可能是角度太大, 馬達轉速不夠或扭力不夠.
3. 執行啟動的時候沒有保持平衡, 請再執行一次
4. 不一樣的車體, 高度, 重心都不一樣, 可能需要改 PID參數
5. 其他我也不知道的原因


最後祝福各位也能夠成功!!


9. 參考文件

  • http://blog.bitify.co.uk/2013/11/interfacing-raspberry-pi-and-mpu-6050.html
  • http://blog.bitify.co.uk/2013/11/reading-data-from-mpu-6050-on-raspberry.html
  • https://projects.drogon.net/raspberry-pi/wiringpi/software-pwm-library/
  • http://robotrabbit.blogspot.tw/2012/07/pid.html
  • http://www.bajdi.com/building-a-self-balancing-bot/

4 則留言:

  1. 你用的材料跟我2年多前用Arduino做了同樣的平衡車。。。
    分享:
    http://regishsu.blogspot.tw/2014/06/arduino-2wd-robot-sonar-feature.html

    回覆刪除
  2. 請問一下,為甚麼 選用減速比1:21會比較好。

    是在實做過程中,遇見什麼問題嗎?

    回覆刪除
  3. 減速比低,轉速會較高, 應該平衡得比較好

    回覆刪除