兩輪機(jī)器人自平衡行走控制系統(tǒng)設(shè)計(jì)

，，，，，，

（南昌大學(xué)信息工程學(xué)院，江西南昌 330031）

1 引言

兩輪機(jī)器人是一種兩輪左右并行結(jié)構(gòu)的新型移動機(jī)器人，由于其占地空間小，運(yùn)動靈活，近年來發(fā)展迅速，Segway公司生產(chǎn)的兩輪警用車［1］已經(jīng)被廣泛運(yùn)用于安保巡邏。德國Transporter公司研發(fā)的兩輪攝影車，可以供攝像工作者在體育運(yùn)動場或新聞現(xiàn)場使用，還可用于電視或電影的拍攝。同時兩輪機(jī)器人系統(tǒng)具有多變量、高階次、強(qiáng)耦合、非線性等特性［2］，它也成為了研究人員檢驗(yàn)新型控制策略和方法的重要模型［3］。

國內(nèi)外已有不少自平衡算法的研究，有學(xué)者采用狀態(tài)反饋［4］、極點(diǎn)配置［5］、LQR［6］等基于現(xiàn)代控制理論的控制方案，它們雖然能得到良好的控制效果，但這些算法依賴精確的數(shù)學(xué)模型、實(shí)現(xiàn)和參數(shù)整定復(fù)雜、魯棒性不強(qiáng)。中國臺灣Tsai等采用人工神經(jīng)網(wǎng)絡(luò)的方法［7］為兩輪機(jī)器人設(shè)計(jì)控制器，雖然有適應(yīng)能力強(qiáng)、不需要建立數(shù)學(xué)模型等優(yōu)點(diǎn)，但同時存在計(jì)算量大、實(shí)時性較差、學(xué)習(xí)效果不確定性等缺點(diǎn)。

本文提出了一種雙回路PID控制方法，能實(shí)現(xiàn)機(jī)器人傾角與速度兩個相互耦合變量的控制。在傾角的測量過程中，應(yīng)用卡爾曼濾波算法及多傳感器數(shù)據(jù)融合技術(shù)，將加速度計(jì)和陀螺儀的數(shù)據(jù)進(jìn)行融合，實(shí)時估算出實(shí)際機(jī)器人的傾角。實(shí)驗(yàn)結(jié)果表明，本文設(shè)計(jì)的控制系統(tǒng)能夠?qū)崿F(xiàn)雙輪機(jī)器人的自平衡直立行走及速度控制。該控制系統(tǒng)同時繼承了傳統(tǒng)PID技術(shù)不依賴精確模型、實(shí)現(xiàn)簡單、參數(shù)整定方便、魯棒性強(qiáng)等優(yōu)點(diǎn)。

2 雙回路PID控制原理

兩輪機(jī)器人由輪子和身體2部分組成，這2部分通過輪子的軸相連。圖1為機(jī)器人身體的受力原理圖。當(dāng)身體受到外界微弱的干擾后，產(chǎn)生傾斜（即φ≠0）時，如果不對其進(jìn)行控制，由于重力Mbg的作用，機(jī)器人將朝傾斜方向倒下。

圖1 機(jī)器人身體的受力原理圖Fig.1 Force analysis of the two-wheeled robot

如果需保持機(jī)器人直立，并使其以期望的速度平穩(wěn)運(yùn)行，系統(tǒng)既要實(shí)現(xiàn)機(jī)器人的平衡控制（即傾角控制），又要實(shí)現(xiàn)車輪的速度控制，這2個被控量之間互相耦合，不能看成2個完全不相干的變量，對其采用獨(dú)立的PID控制。因此，本文研究了一種雙回路PID的控制方法。它能同時控制好機(jī)器人的傾角和速度，使PID控制能夠應(yīng)用多變量、強(qiáng)耦合的被控制對象。同時該控制方法繼承了傳統(tǒng)PID控制不依賴精確模型、實(shí)現(xiàn)簡單、參數(shù)整定方便、魯棒性強(qiáng)等優(yōu)點(diǎn)。

系統(tǒng)的雙回路PID控制框圖如圖2所示，包括機(jī)器人平衡PD控制器與機(jī)器人速度PI控制器2個回路。平衡PD控制器用來實(shí)現(xiàn)機(jī)器人傾角控制，微分環(huán)節(jié)能很好地改善平衡控制系統(tǒng)的動態(tài)性能，提高其抗干擾性；速度PI控制器用來實(shí)現(xiàn)機(jī)器人速度調(diào)節(jié)，積分環(huán)節(jié)實(shí)現(xiàn)實(shí)際速度無靜差跟隨期望速度。

圖2 機(jī)器人平衡和速度控制器Fig.2 The balance and speed controller for the robot

圖2中，GB（s）是機(jī)器人傾角φ的傳遞函數(shù)，輸入是施加在機(jī)器人車輪上的電壓U（s），輸出是機(jī)器人的傾角φ(s)；Gs(s)是機(jī)器人速度傳遞函數(shù)，輸入是施加在機(jī)器人車輪上的電壓U（s），輸出是機(jī)器人的速度v（s）。DB(s)和Ds(s)分別是傾角 PD控制器和速度PI控制器：

2個控制器的輸出疊加在一起得到控制量，作用于電機(jī)上，給輪子動力，從而控制機(jī)器人傾角和速度這2個被控變量。

本控制方法的最大特點(diǎn)是采用了正負(fù)反饋結(jié)合的控制方法。平衡控制回路為常規(guī)的負(fù)反饋（即Kpb＞0，Kdb＞0），而速度控制回路為正反饋（即Kps＜0，Kis＜0），正負(fù)反饋結(jié)合實(shí)現(xiàn)機(jī)器人以期望的速度平衡行走。

3 系統(tǒng)硬件設(shè)計(jì)

兩輪機(jī)器人由機(jī)器人身體、2個輪子和相應(yīng)的電機(jī)及驅(qū)動電路等組成。具體硬件設(shè)計(jì)框圖如圖3所示。由于沒有直接的傾角傳感器，系統(tǒng)無法直接測量機(jī)器人的傾角。本設(shè)計(jì)中傾角信號的采集是由運(yùn)動傳感器MPU6050來實(shí)現(xiàn)，它內(nèi)部整合了3軸陀螺儀和3軸加速器。單片機(jī)接收到陀螺儀和加速度器的實(shí)時信號之后，通過數(shù)據(jù)融合得到機(jī)器人的傾角。速度信號的采集是通過編碼器來實(shí)現(xiàn)的?？刂铺幚砥鞑捎肁rm內(nèi)核單片機(jī)STM32F103芯片。利用傳感器對被控變量（傾角和速度）進(jìn)行測量，將測量數(shù)據(jù)送入Arm處理器中進(jìn)行處理，輸出相應(yīng)的PWM信號，再通過功率放大加載到電機(jī)上，控制電機(jī)轉(zhuǎn)動。為方便調(diào)試和觀測數(shù)據(jù)，系統(tǒng)增加了液晶顯示和藍(lán)牙通信接口。

圖3 系統(tǒng)硬件設(shè)計(jì)框圖Fig.3 The hardware design of the system

4 系統(tǒng)軟件設(shè)計(jì)

4.1 傾角計(jì)算方案

機(jī)器人傾角測量的準(zhǔn)確性直接關(guān)系到機(jī)器人的平衡控制。如果用傳感器將垂直于Z軸的一面貼在機(jī)器人上（見圖4），那么Z軸方向的輸出信號與機(jī)器人受到的重力分量成正比。由于MPU6050為16位數(shù)字傳感器，可以通過下式得到機(jī)器人傾角θ：

圖4 加速度計(jì)測量傾角原理圖Fig.4 The principle of inclination measurement with accelerometer

加速度計(jì)在實(shí)際應(yīng)用過程中存在靜態(tài)輸出噪聲較大，加速度計(jì)輸出易受其他方向的加速度影響等問題，因此，僅采用加速度計(jì)得到的傾角值不準(zhǔn)確。陀螺儀是一種角速度計(jì)，對其積分可以得到機(jī)器人傾角。但陀螺儀的輸出零點(diǎn)受溫度影響，再經(jīng)過積分后，誤差會隨著時間增長而累積［8］。

為了提高傾角測量的精度，本文采用卡爾曼濾波來實(shí)現(xiàn)加速度計(jì)和陀螺儀信號的數(shù)據(jù)融合，得到機(jī)器人的傾角?？柭鼮V波器能將過去的測量估計(jì)誤差合并到新的測量誤差中，來估計(jì)被測量值將來的誤差。

卡爾曼濾波器是建立在帶白噪聲的狀態(tài)觀測器模型上的，狀態(tài)觀測器模型方程如下。

離散狀態(tài)方程為

式中：xk和xk-1分別為第k個周期和k-1周期的狀態(tài)向量；A為作用在xk-1上的狀態(tài)變換模型矩陣；uk為輸入矩陣；B為輸入變換模型矩陣；wk為過程激勵噪聲向量。

觀測方程為

卡爾曼濾波算法（見圖5）可分為兩大塊：一塊為狀態(tài)更新，作用是假設(shè)系統(tǒng)沒有過程噪聲，然后根據(jù)狀態(tài)空間表達(dá)式對系統(tǒng)的狀態(tài)變量進(jìn)行估計(jì)并推算出誤差協(xié)方差（可信度）；第2塊為測量更新，其作用是根據(jù)推算出的誤差協(xié)方差計(jì)算卡爾曼增益，利用卡爾曼增益把觀測值和估計(jì)值融合，得出最優(yōu)估計(jì)。其中x為狀態(tài)變量，P為誤差協(xié)方差，Q為系統(tǒng)過程誤差協(xié)方差，R為觀測誤差協(xié)方差。

圖5 卡爾曼濾波算法框圖Fig.5 Kalman filter algorithm

該算法在計(jì)算機(jī)中實(shí)現(xiàn)的思想是：先選取合適的狀態(tài)變量建立系統(tǒng)狀態(tài)方程和觀測方程，得到上述公式中x，A，B，u，H矩陣，然后將上述公式編成程序，并以固定的時間周期重復(fù)地執(zhí)行上述程序。

具體實(shí)現(xiàn)過程如下：

1）選取合適的狀態(tài)變量（待估計(jì)的變量必須放入其中），建立系統(tǒng)方程和觀測方程。得到x，A，B，u，H陣；

2）根據(jù)系統(tǒng)激勵噪聲和觀測噪聲的特性，選取適當(dāng)?shù)腝和R；

3）初始化過程：初始化x（0），P（0），K（0）；

4）根據(jù)系統(tǒng)方程先驗(yàn)估計(jì)狀態(tài)變量，即計(jì)算x（k|k-1）=Ax（k-1）+Bu（k）；

5）推導(dǎo)出當(dāng)前的誤差協(xié)方差，即計(jì)算P（k|k-1）=AP（k-1）AT+Q；

6）計(jì)算卡爾曼增益，即計(jì)算K（k）=P（k|k-1）HT[HP（k|k-1）HT+R]-1；

7）根據(jù)觀測值修正誤差，即計(jì)算x（k|k）=x（k|k-1）+K（k）[z（k）-Hx（k|k-1）]；

8）更新當(dāng)前的誤差協(xié)方差，即計(jì)算P（k|k）=[1-K（k）H]P（k|k-1）；

9）在下一個測量周期，從第4）部重新執(zhí)行程序。經(jīng)過卡爾曼濾波算法后，就可以得到機(jī)器人的實(shí)際傾角。

4.2 控制系統(tǒng)的軟件設(shè)計(jì)

系統(tǒng)的控制流程圖如圖6所示，控制周期為10 ms。

圖6 控制程序流程圖Fig.6 The diagram for control system

首先進(jìn)行信號采樣處理，包括傾角及速度的采樣。在每個控制周期，分別對陀螺儀和加速度計(jì)進(jìn)行連續(xù)20次的采樣，然后去掉最大值和最小值后取它們的平均值，以減小傳感器測量過程中的隨機(jī)誤差。經(jīng)過平滑濾波后，還需要進(jìn)行標(biāo)度變換，將傳感器信號的A/D數(shù)值換算為以國際單位度量的物理量。

根據(jù)傳感器實(shí)際檢測到的速度和傾角值，分別根據(jù)傾角和速度的PID控制算法，計(jì)算出相應(yīng)的PWM輸出值，然后將2個PID控制器的輸出疊加，控制施加在機(jī)器人輪子上的電壓，使機(jī)器人的傾角和速度同時得到控制，使雙輪機(jī)器人能夠以一定的速度平衡直立行走。雙回路PID程序流程如圖7所示，分別對傾角誤差采用PD負(fù)反饋控制算法和速度誤差采用PI正反饋控制算法進(jìn)行計(jì)算，然后將2個控制器的輸出疊加，控制電機(jī)。

圖7 PID控制子程序流程圖Fig.7 Program flow diagram for PID controller

5 實(shí)驗(yàn)

本文采用自制的兩輪自平衡小車進(jìn)行實(shí)驗(yàn)，通過理論計(jì)算和實(shí)驗(yàn)整定，確定雙回路PID控制器參數(shù)如下：Kpb=45，Kdb=3，Kps=-12，Kis=-10。

5.1 系統(tǒng)定點(diǎn)平衡控制實(shí)驗(yàn)

設(shè)定機(jī)器人的期望速度為0 m/s，期望角度為0 rad，系統(tǒng)實(shí)際控制結(jié)果如圖8所示。系統(tǒng)的傾角和速度兩個狀態(tài)都是穩(wěn)定的。其中傾角在-0.005～0.005 rad附近波動。速度在-0.05～0.05 m/s范圍內(nèi)波動。

圖8 系統(tǒng)定點(diǎn)平衡實(shí)驗(yàn)中傾角和速度變化曲線Fig.8 The inclination and speed in the balance control experiment

5.2 速度階躍響應(yīng)實(shí)驗(yàn)

在這個實(shí)驗(yàn)過程中，先讓系統(tǒng)處于定點(diǎn)平衡狀態(tài)，在2.5 s時刻，改變期望的速度為0.5 m/s，測試其雙輪機(jī)器人的速度跟隨的動態(tài)性能。實(shí)際實(shí)驗(yàn)結(jié)果如圖9所示，在2.5 s之前，系統(tǒng)處于定點(diǎn)平衡狀態(tài)，機(jī)器人輪子首先反向加速，然后正向加速，機(jī)器人也相應(yīng)地向前傾斜，然后又恢復(fù)過來，最終在4.8 s時刻速度基本穩(wěn)定在期望值。相應(yīng)的傾角在6 s時刻也恢復(fù)到初始狀態(tài)。

圖9 速度階躍響應(yīng)實(shí)驗(yàn)中傾角和速度變化曲線Fig.9 The inclination and speed in the experiment of speed step response

6 結(jié)論

本文采用卡爾曼濾波算法將實(shí)際檢測的陀螺儀和加速度計(jì)信號進(jìn)行數(shù)據(jù)融合，得到機(jī)器人實(shí)際傾角信號，并提出了一種雙回路PID控制方法，實(shí)現(xiàn)兩輪機(jī)器人的穩(wěn)定行走控制。該控制方法具有以下特點(diǎn)：不需要精確的數(shù)學(xué)模型，程序?qū)崿F(xiàn)簡單，控制參數(shù)整定方便，抗干擾性較強(qiáng)。但是采用雙回路PID控制時，系統(tǒng)超調(diào)量和穩(wěn)定時間略長。如果采用專家PID或分段PID控制算法可實(shí)現(xiàn)更精細(xì)的控制，明顯改善系統(tǒng)的動態(tài)時間。

［1］Nguyen H G，Morrell J，Mullens K，et al.Segway Robotic Mobility Platform［OL］.［2008-07-11］.http：//www.spawar.navy.mil/robots/pubs/spie5609-27.pdf.

［2］Oryschuk P，Salerno A，Al-Husseini A M，et al.Experimental Validation of an Underactuated Two-wheeled Mobile Robot［J］.IEEE/ASME Transactions on Mechatronics，2009，14（3）：252-257.

［3］Salerno A，Angeles J.A New Family of Two-wheeled Mobile Robots：Modeling and Controllability［J］.IEEE Transaction on Robotics，2007，23（1）：169-173.

［4］Deng M，Inoue A，Sekiguchi Ket al.Two-wheeled Mobile Robot Motion Control in Dynamic Environments［J］.Robotics and Computer-integrated Manufacturing，2010，26（3）：268-272.

［5］李潮全，高學(xué)山，李科杰.基于完整約束的兩輪機(jī)器人建模及平衡控制［J］.控制工程，2012，19（3）：478-481.

［6］Xu Changkai，Li Ming.The System Design and LQR Control of a Two-wheels Self-balancing Mobile Robot［C］//International Conference on Electrical and Control Engineering，2011：2786-2789.

［7］Tsai C C，Huang H C，Lin S C.Adaptive Neural Network Control of a Self-balancing Two-wheeled Scooter［J］.IEEE Transactions on Industrial Electronics，2010，57（4）：1420-1428.

［8］王陸軍，許勇.兩輪自平衡電動車及其電機(jī)控制器設(shè)計(jì)［J］.電氣傳動，2011，41（11）：41-45.