基于輪球自平衡機構的機器人設計

毛麗民，盧振利，朱培逸，劉叔軍

（常熟理工學院 電氣與自動化工程學院，常熟 215500）

0 引言

自平衡機器人的概念源于二十世紀八十年代，基于倒立擺的模型，其系統是一個非線性、強耦合、多變量和自然不穩定的系統[1]。

輪球自平衡機構組成的機器人通過自身的平衡調節器，使其在球上近似平穩的站立，實現動態平衡。球與地面有一個微小的接觸點，和一般的多輪車相比，單足平衡機器人靈活性更好。可以應用到大型購物中心、際會議或展覽場所等領域，根據不同的應用場合加入各類不同功能，例如加入避障、定位和網絡訪問控制，實現物體運送的功能

1 機器人機構及自平衡原理

1.1 機器人機構

機器人結構主要由主控制器、主框架、藍牙模塊、陀螺儀模塊、各類模塊等組成。機器人總高度為28cm；身長25cm，寬25cm；全向輪直徑5cm；球直徑18cm。機器人模型如圖1所示。

圖1中數字代表：1為構架，2為電機驅動，3為主控制器獨立電源，4為OLED，5為主控制器，6為藍牙模塊，7為24V電源8為穩壓模塊，9為總開關，10為直流電機，11為全向輪。

圖1 輪球自平衡機器人模型（無球部分機構）

根據上述模型，設計完成第一代輪球自平衡機器人（無球部分機構）樣機如圖2所示。

圖2 輪球自平衡機器人樣機

1.2 輪球移動機器人自平衡原理

該機器人驅動部分是通過三個電機驅動的輪子，組成一個平面，彼此相差120度。該部分與球組成一個移動機構，該球直徑應該滿足R球＞α*R萬向輪組, α為實驗安全系數，這里取1.414。

在機器人維持平衡過程中，由于變化的角度相對較小，把機器人的運動系統近似的看成平面，建立的運動模型如圖3所示。

圖3 運動模型

模型中三個電機兩兩相隔120度，Vr是萬向輪運動正方向的運動速度，V是任意的某一方向的運動速度，a是V和0度線間的夾角，通過調節三個電機的轉速和正反轉合成V，由于本模型的建立是在剛性物體的基礎上，得到以下公式：

通過上述的數學模型，實現機器人在保持自平衡基礎上的二維運動。Vr1，Vr2，Vr3分別代表萬向輪系統中3個輪的轉速。

2 基于多傳感器設計機器人控制系統

本文設計的單足自平衡機器人，以STM32為控制核心，以MPU-9150實現姿態檢測，得到角速度和角加速度，經過卡爾曼數據融合得到真實的姿態值。通過PID算法對實時姿態值處理得到PWM波，對以F2807S為開關管的H橋電機驅動進行控制，以三號足球為機器人的單足，實現機器人在足球上保持平衡。利用OLED顯示屏實現人機交互，通過藍牙模塊進行通訊。機器人多傳感器控制系統框圖如圖4所示。

圖4 機器人多傳感器控制系統框圖

3 機器人多傳感器控制系統硬件設計

3.1 系統的供電電路設計

單足自平衡機器人的電源是3.7V的鋰電池，選用AMS1117三端穩壓器將3.7V電壓轉成3.3V給單片機供電，如圖5所示。

圖5 AMS1117-3.3V穩壓電路圖

3.2 姿態檢測電路設計

輪球自平衡移動機器人為保持平衡，需實時采集機器人的姿態值，在本設計選用MPU-9150姿態傳感器，通過對加速度值和速度值的處理可得到機器人的姿態值。姿態檢測電路如圖6所示。

圖6 姿態檢測電路圖

3.3 電機驅動電路設計

本文設計的電機驅動電路是N溝道MOS管全橋電路，主要由控制電路和主電路組成，實現直流電機的加、減速和正反轉的功能。

圖7為電機驅動信號邏輯電路圖，圖中控制電路使用與非門邏輯器，控制端口使用0和1來控制電機正反轉，PWM波占空比來控制電機轉速，達到最優的控制效果。

圖7 信號邏輯電路圖

圖8為電機驅動主電路圖，圖中IRF2807S為MOS管，漏極電流為82A，最大電壓為75V。IR2104S是半橋驅動器，使得電路可以無需脈沖變壓器驅動，得到所須的工作頻率，提高了電路的頻率穩定性。

圖8 電機驅動主電路圖

4 機器人多傳感器控制系統軟件設計

4.1 姿態檢測軟件設計

控制器通過I2C傳輸協議讀取姿態傳感器中的速度和加速度值，進行處理，得出姿態角（傾斜角和航向角），流程如圖12所示。

圖9 陀螺儀軟件設計流程圖

圖12 單足自平衡機器人調試

姿態傳感器程序中包括讀取相應寄存器中的值和整個處理得出初始值兩個部分，其中BUF[12]是一個用于陀螺儀數據緩存的數組，ACCEL_XOUT_L、ACCEL_YOUT_L、ACCEL_ZOUT_L分別表示X、Y、Z軸的角加速度值得低八位數據；ACCEL_XOUT_H、ACCEL_YOUT_H、ACCEL_ZOUT_H分別表示X、Y、Z軸的角加速度值得高八位數據；GYRO_XOUT_L、GYRO_YOUT_L、GYRO_ZOUT_L分別表示X、Y、Z軸的角速度值得低八位數據；GYRO_XOUT_H、GYRO_YOUT_H、GYRO_ZOUT_H分別表示X、Y、Z軸的角速度值得高八位數據；A_X、A_Y、A_Z為整合處理后的角加速度初始值；G_X、G_Y、G_Z則為整合處理后的角速度初始值。

4.2 基于卡爾曼濾波器設計機器人姿態估計器

卡爾曼濾波器的控制主要有兩個方面：預測與更新。在預測時，通過上一時刻狀態的判斷，計算出實時狀態；在更新時，實時觀測值使預測階段取得的測量值最優化，得出最新估計值。卡爾曼濾波步驟如下：

1）先驗狀態估計：

2）先驗估計誤差協方差：

3）卡爾曼增益：

4）后驗狀態估計：

5）后驗誤差協方差：

程序中使用公式Angle+=(Gyro_y-Q_bias)*dt計算出機器人某一個軸的角度，Q_bias表示陀螺儀輸出值與期望值之間的誤差，Angle就是系統預先估計值，然后得到觀測方程；但加速度傳感器采回的角度值Accel就是類似于系統中的測量值，從而得出系統的狀態方程式。

Q_angle和Q_gyro各代表了系統對加速度傳感器及陀螺儀傳感器的信任程度。根據Pdot[0]=Q_angle-PP[0][1]-PP[1][0]計算出先驗估計協方差的微分值，再把實時估計值進行線性化計算。之后計算系統估計角度的協方差矩陣PP。計算卡爾曼增益K_0和K_1，K_0用于最優預算值，K_1用于處理最優預算值的偏差和刷新協方差矩陣PP。通過進一步的計算得較真實的機器人姿態值。

4.3 PID控制器設計

4.3.1 PID控制器原理

PID因其簡單的結構、較高的穩定性和便于調整的特性，得到了廣泛的應用。

圖10 PID控制示意圖

上圖為PID控制示意圖，r(t)為輸入量；e(t)為穩態誤差量；u(t)為PID輸出量；c(t)為實際輸出量。PID的輸入e(t)與輸出u(t)的等式為：

在式(13)中，Kp為比例系數；T1為積分系數；TD為微分系數，PID調節器原理簡單、適應性強，實現自平衡機器人的控制方案是完全可行的。

4.3.2 PID控制器實現

在本系統中，Uk代表卡爾曼數據融合后機器人的實時傾斜角；U代表機器人直立時的傾斜角；ek為實時傾斜角度值與期望值的誤差；Kp、Ki和Kd分別為比例、積分和微分環節的系數；Pp、Pi和Pd分別為比例環節、積分環節和微分環節增益量；P為PID輸出量，用來控制PWM波的占空比，實現電機的轉速控制。

5 基于多傳感器的機器人自平衡實驗

5.1 卡爾曼濾波器參數設計

機器人在調節過程中會出現輕微的抖動，通過卡爾曼濾波使抖動部分更為平緩。在對濾波器進行調試的過程中，要對卡爾曼濾波器的陀螺儀、加速度傳感器的權值Q_gyro、Q_angle和卡爾曼增益R_angle設定參數，設定完成后可發送數據到上位機顯示，如圖11所示。

圖11 卡爾曼濾波調試圖

上圖為卡爾曼濾波調試圖，圖中1表示姿態傳感器原始曲線，2表示經過卡爾曼濾波后的曲線。從圖可以看出，姿態值的初始曲線有很多的尖峰，噪聲比較大，經過卡爾曼濾波后的波形比較的平滑，毛刺較少，可以比較精確的體現出機器人姿態的真實情況。

5.2 PID控制參數整定

通過實驗，比例調節能使自平衡機器人完成動態平衡，但是響應時間較長，同時在穩定的位置不停地抖動，加入積分和微分調節使機器人的動態偏差減小，縮短調節反應時間。

圖13 單足自平衡機器人PID參數整定

PID參數的整定過程：第一步調整比例系數值的大小，使機器人能直站立起來，機器人在實現站立的過程中可能會伴隨著抖動現象，第二步，將比例系數值降低到設定值的70%左右，小幅度的增大微分的系數值讓機器人可以穩定的平衡。

6 結束語

面向服務型行業中對移動機器人的需求，設計輪球自平衡機器人，應用多傳感器實時檢測機器人位姿信息，應用卡爾曼濾波算法對數據進行優化，通過整定PID控制器參數實現了該機器人的自平衡。實驗結果表明該設計能使機器人在無干擾情況下實現自平衡，并通過有干擾自平衡實驗表明了該控制器的魯棒性。該設計為機器人實用化提供了新的技術和方法。

[1] 張友民,戴冠中,等.卡爾曼濾波計算方法研究進展[J].控制理論與應用.1995(05).

[2] 陸如華,徐傳玉.卡爾曼濾波的初值計算方法及其應用[J].應用氣象學報.1997(01)：1-3.

[3] 陸芳,劉俊.卡爾曼濾波在陀螺儀隨機漂移中的應用[J].微計算機信息.2007(23)：12-16

[4] 張鵬飛,齊曉慧.基于N溝道MOS管H橋驅動電路設計與制作[J].科技信息.2012(20)：2-4.

[5] 張程.基于STM32的永磁同步電機驅動器設計[J].福建工程學院學報.2011(01)：5-10.

[6] 張志強.基于STM32的雙輪平衡車[J].電子設計工程.2011(13)：19-25.

[7] 楊智,朱海鋒,等.PID控制器設計與參數整定方法綜述[J].化工自動化及儀表.2005(05)：25-27.

[8] 鄒凌,孫玉強.基于卡爾曼濾波器的PID控制仿真研究[J].微計算機信息.2007(16)：1-20.