這個項目做了很長的時間，核心算法就是PID，目標就是讓一個三輪車（輪子爲全向輪）站到足球上並可以進行平移旋轉等平面運動。
機械部分
使用三個橡膠全向輪，通過聯軸器連接減速電機，斜向下45°連接於車身，車身上承載多層臺面，下層放置電路板及電池，上層作爲承物平臺，選用摩擦力大的5號橡膠足球作爲摩擦球，如圖1所示：
板與電機的連接部分採用3D打印方案，3D打印精度可達0.1mm，完全滿足需要，3D模型如圖2：
電路部分
如圖3所示，通過433MHz的無線串口透傳模塊，將控制指令傳送至單片機，單片機通過IIC接口獲取當前姿態信息，通過自定義的並口獲取當前速度信息。在控制器中完成角度閉環和速度閉環。通過PWM將給電機的指令信號傳給FPGA，FPGA對每個電機分別做速度閉環，同時將速度信息回傳至MCU。
控制系統模型建立
兩個方向的獨立控制方程可以看作兩個一維倒立擺，倒立擺模型如圖4：
採用動力學分析建模，最終經過線性化和拉普拉斯變換後的兩個傳遞函數分別如下所示：
式中各符號的含義如下：
Ib、Ip | 慣性力矩 |
mp | 機器人主體質量 |
mb | 球體質量 |
r | 球體直徑 |
h | 擺杆質心到球心的長度 |
θ | 擺杆傾角 |
ψ | 擺杆與球的相對角度 |
M | 作用在球上的力矩 |
g | 重力加速度 |
單球輪平衡機器人的控制問題本質上是對三個全向輪的控制問題，將機器人模型分爲三個座標系x、y、z，如圖5所示。
通過在x軸、y軸上安裝的加速度計和陀螺儀來檢測兩個軸上的角度和加速度變化，進而計算出等效的兩個虛擬輪上的速度vx 、vy ， vx、vy 由相應的加速度ax 、ay 積分求得，ax 、ay 分別如下式所示：
由虛擬輪的速度 vx、 vy最終推導出三個全向輪的轉速vs2 、vs2 、vs3 分別如下式所示：
控制方案
對於控制器來說，需要同時進行角度閉環和速度閉環，來增加小車穩定時的狀態同時增加阻尼。爲了使後端的功放環節更像一個線性的放大環節，減少非線性，我們加入FPGA使後端電機驅動部分自成一個速度閉環，使單片機可以獲得更爲準確的速度反饋。
單片機從傳感器同時獲得角度和角速度，通過增量式PID進行角度反饋，輸出PWM，同時讀入FPGA返回的速度值，控制小車不發生較爲嚴重的振盪。
如圖7爲控制框圖：
單片機工作流程
如圖8，單片機上電之後先進行系統初始化，並讀取參數，然後從測量元件獲得測量數據，通過其中加速度計和陀螺儀的數據，輸入融合算法可確定小車的姿態，結合由編碼器得到的FPGA速度值以及期望數值計算得到小車的速度輸出。值得再次說明的是，此係統中的速度閉環是由控制器（單片機）實現的，而FPGA中速度反饋的功能是提高電機的線性度。
FPGA工作流程：
三路PWM速度信號由單片機輸入FPGA，FPGA由編碼器的速度反饋信號及輸入的期望速度進行PID控制算法後將控制信號以PWM的形式輸入電機驅動，並將得到的速度反饋值傳給單片機。
控制芯片的選擇
控制器的硬件電路的設計，必須首先決定主控制器採用的芯片，然後將控制算法編製成相應的控制程序，實現對被控對象的控制。究竟選擇哪一種單片機，取決於系統對速度的要求和該應用系統的複雜性，在本設計中選用通用數字單片機，其它器件的選擇遵循控制系統的精度要求，同時滿足體積小、耗電少、可靠性高的要求，因此儘可能選用功耗小，性能穩定的集成電路芯片。
在衆多單片機產品中，我們選擇了Atmel公司的AVR單片機，該單片機具有編程簡單，算法庫和驅動庫衆多，方便快速開發，同時該單片機爲8位單片機，主時鐘16MHz，運算速度不算太快，但可以滿足50Hz-100Hz的系統控制頻率，該單片機具有片上8通道10位ADC，6個硬件PWM，一路I2C，一路串口，一路SPI接口，接口齊全，能滿足各類傳感器的需求。是一款通用性極佳，開發極容易的單片機。如圖10爲單片機最小系統電路。
爲了實現3路電機的各自速度閉環，單片機沒有足夠的資源可以使用，而且速度不夠，我們選用了Xlinx公司的Spartan6系列的FPGA XC6SLX9，用來做並行的3路閉環輔助處理器，單片機通過6路PWM將信號傳給FPGA,每個電機用兩路PWM來區別轉向，同時FPGA每個電機另外產生兩路PWM來驅動電機，同時FPGA捕獲每個電機編碼器的的兩路正交信號進行速度反饋。
陀螺儀與加速度計
我們需要測得小車精確的角度變化量，及時修正來保證小車的相對穩定性。常見的測量角度的儀器有陀螺儀和加速度計。
陀螺儀可以測量角速度,具有高動態特性,但是它是一個間接測量器件,它測量的是角度的導數，即角速度，顯然我們要將角速度對時間積分才能得到角度。積分會存在積分誤差，積分誤差的來源主要有兩個：一個是積分時間。積分時間Dt越小，輸出角度越準；一個是器件本身的誤差，假設陀螺儀固定不動，理想角速度值是0dps(degreeper second)，但是如果有一個偏置0.1dps加在上面,測量出來則是0.1dps,積分一秒之後,得到的角度是0.1度,1分鐘之後是6度,一小時之後是360度,轉了一圈。所以說陀螺儀在短時間內有很大的參考價值。
加速度計可以測量加速度,包括重力加速度。但是加速度傳感器不會區分重力加速度與外力加速度，所以當系統在三維空間做變速運動時，它的輸出就不正確了或者說它的輸出不能表明物體的姿態和運動狀態。在靜止或勻速運動（勻速直線運動）的時候,加速度計僅僅測量的是重力加速度,而重力加速度與絕對座標系是固連的,通過這種關係,可以得到加速度計所在平面與地面的角度關係也就是橫滾角和俯仰角，計算公示如下：
俯仰角ρ=-arctan(AY/AZ)
橫滾角ψ=arctan(AX/AZ)
綜合考慮，加速度計是極易受外部干擾的傳感器，但是測量值隨時間的變化相對較小。陀螺儀可以積分得到角度關係，動態性能好，受外部干擾小，但測量值隨時間變化比較大。可以看出，它們優缺點互補，結合起來纔能有好的效果。我們可以設計算法在短時間尺度內增加陀螺儀的權值，在更長時間尺度內增加加速度權值，這樣系統輸出角度就更真實了。
選擇MPU6050傳感器，該芯片集成3軸陀螺儀和3軸加速度計，陀螺儀量程可選擇±250,±500,±1000,±2000°/s，加速度計量程可選擇±2g, ±4g,±8g, ±16g。且輸出增加低通濾波，解決溫度漂移問題，更有利於單片機信號採集。該傳感器的ADC分辨率均爲16位，對單片機的數據接口爲I2C，工作頻率最高爲400kHz。
我們將傳感器配置在±250°/s，±2g，即可滿足我們的要求，同時可以達到較高精度。因爲系統在穩定的時候晃動不超過5°，晃動頻率不超過1Hz，所以選用最低量程完全滿足我們的要求，且量程越低精度越高。控制頻率在50Hz-100Hz，傳感器靈敏度可以滿足我們的需求。在控制器內，我們以400kHz的頻率獲取原始數據，進行卡爾曼濾波數據融合獲取3軸角度，該數據和3軸角速度以控制頻率頻率被控制器使用。另外爲了減輕處理器負擔，我們可以使用MPU6050內部的DMP運算內核，該內核基於DSP，內部固化官方的濾波算法，可以在芯片內FIFO中直接讀取四元數，角速度等值，同時DMP內核具有一定的抑制零漂算法。
在使用時爲了抑制零漂，我們採用在設備上加裝水平尺的方式，每次上電前十秒採用人工校正來保證在本次工作中系統傳感器的數據真實度。
如圖爲MPU6050部分原理圖：
磁強計
採用磁強計讓小車工作時可以得知自己的絕對朝向，同時方便陀螺儀進行Z軸校準，以此獲得系統相對於地球的絕對方位角，傳感器精度要求不高，採用HMC5883傳感器。該傳感器的特點和優點如下：
特點 | 優點 |
三軸磁阻傳感器和ASIC都被封裝在3.0×3.0×0.9mmLCC表面裝配中 | 體積小高集成產品。只需添加一個微處理器接口，外加兩個外部SMT電容。專爲大批量、成本敏感的OEM生產而設計，易於裝配並與高速SMT裝備件兼容。 |
12-bit ADC與低干擾AMR傳感器，能在±8高斯的磁場中實現5毫高斯分辨率。 | 能讓羅盤航向精度精確到1°~2°。 |
內置自檢功能。 | 產品組裝後能進行低成本功能性測試。 |
低電壓工作（2.16V—3.6V）和超低功耗（100uA）。 | 適用於電池供電的應用場合。 |
內置驅動電路。 | 帶置位/復位和偏置驅動器用於消磁、自測和偏移補償。 |
I2C數字接口 | 適用於消費類電子設備應用中通用雙線串行數據接口 |
無引線封裝結構 | 符合RoHS標準 |
磁場範圍廣（+/-80e） | 傳感器能在強磁場環境中羅盤航向精度達到1°~2°。 |
有相應軟件及算法支持 | 可獲得羅盤航向、硬磁、軟磁以及自動校準庫。 |
最大輸出頻率可達160Hz | 能應用於個人導航系統和LBS |
該傳感器使用簡單的I2C數字接口，內置12位ADC。最大輸出頻率爲160Hz，高於控制頻率，比較合適。在PCB佈線安裝的時候要遠離電機，防止被電機磁場干擾。迴流焊要求溫度不能高於260℃，否則會損傷芯片內部結構，這一點要特別注意。如圖爲HMC5883部分原理圖：
光電編碼器
採用光電編碼器，該編碼器與電機一體，做在了一個軸上，電機本身減速比64:1，碼盤爲12CPR，則測角精度爲360/(12*64)=0.468°，同時該光電編碼器爲AB相輸出。編碼器要注意供電問題，小心燒掉光電管,如圖13，電源加入保護電阻，輸出增加上拉電阻。
電機選型方案
電機 | 類型 | 參數 | 特點 |
1 | Faulhaber2342L012空心杯減速電機帶編碼器 | 輸出功率：17W 扭矩：大扭矩(1.72Nm) 空載電流：75mA 堵轉電流：1400mA 減速箱型號：日本定製（全金屬精密行星減速箱） 減速比：64：1 編碼器：光電式 輸出路數：ＡＢ雙路輸出 每圈脈衝：12CPR（脈衝每圈）工作電壓: 12V 空載轉速：8100RPM（轉每分鐘） 減速後速：120RPM（轉每分鐘）
| 減速組 自帶編碼器，有虛位 力矩大 |
2 | 42步進電機 | 重量：0.4kg 步距角：1.8±5％度/步 電壓：12V 電流：1.2A/phase的 阻力位：10.0±10％Ω/相 電感值：20±20％MH/相位 鉛風格：AWG26 UL1007 配備4引線 保持轉矩：48N.cm敏 制動扭矩：2.6N.cm最大 轉子的力矩：68g.cm2 | 便宜，力矩大，無需速度閉環，沒有虛位 |
3 | 無刷電機+電調 | 1400kv 30A | 力矩大，無需做閉環 |
最終我們選擇了Faulhaber2342L012空心杯減速電機帶編碼器，看中其功率大，驅動簡單，自帶較高精度編碼器減少機械設計難度，如圖14：
電機驅動的選擇
電機最大功率17W，最大工作電壓15V，可以估算出每個電機的工作電流不超過2A，選擇L298N芯片作爲電機驅動。該芯片可以驅動兩路電機，最高工作電壓35V，穩態工作電流2A，峯值工作電流3A，可以滿足功率需要。輸入信號爲兩路PWM，可控制電機的正反轉，PWM信號頻率約爲40kHz。該芯片在使用時要特別注意散熱，芯片發熱量很大，並且板上佈線線寬至少40mil，遠離數字芯片。如圖15爲驅動電路的原理圖：
輪胎的選擇
圖16輪胎採用雙層橡膠全向輪，3個輪呈120度排布，同時向下傾斜45度，以便與球面全面貼合。我們最終選擇瞭如圖全向輪，直徑7cm，六方形聯軸器。
球的選擇
採用5號橡膠足球，與橡膠輪胎的摩擦較大，球與輪胎摩擦係數約爲0.5。原則上儘量增加摩擦力。同時保證輪子可以全部與球接觸
電源的選擇
每個電機耗電峯值達到2A，整個系統考慮峯值要留出冗餘，確定放電能力6.5A，電機工作電壓在14V左右，電壓越高，電機工作特性越好，故採用3000mah，14.8V，4s，35C放電能力的鋰電池，估算最大放電電流可達9A，用來保證電機的供電。
控制器，傳感器的供電電壓爲5V和3.3V，FPGA部分供電爲3.3V和1.2V，控制部分功率較小，採用7805三端穩壓芯片將電池電壓降至5V，後接1117-3.3將其降壓至3.3V，後接1117-1.2，將其降壓至1.2V。由於7805壓降較大，發熱量較大，需要增加散熱片以確保穩壓芯片的正常工作。電源部分原理圖如圖17所示：
主控部分代碼可參照:https://github.com/BG2CRW/ballBalacne
