一種輪式倒立擺機器人設計*

汪智祥， 劉益劍， 曹　楠

(南京師范大學 電氣與自動化工程學院，江蘇 南京 210042)

一種輪式倒立擺機器人設計*

汪智祥， 劉益劍， 曹楠

(南京師范大學 電氣與自動化工程學院，江蘇 南京 210042)

摘要:倒立擺是檢驗各種控制理論的理想模型，通過它可以判斷各類控制算法的有效性和控制性能。為此，結(jié)合K60微控制器強大的運算能力和豐富的接口功能，設計了該輪式倒立擺機器人，可以通過它驗證各種新型控制算法。硬件上介紹了與姿態(tài)控制相關(guān)的姿態(tài)檢測系統(tǒng)和電機驅(qū)動單元。軟件上介紹了根據(jù)系統(tǒng)數(shù)學模型設計的互補濾波器、姿態(tài)控制器、方向控制器和速度控制器。實驗結(jié)果表明：該系統(tǒng)具有良好的魯棒性和穩(wěn)定性，可以準確地對控制算法的有效性和控制性能進行判斷。

關(guān)鍵詞:輪式倒立擺； 互補濾波； 姿態(tài)估計； 控制器

0引言

2002年,瑞士聯(lián)邦工業(yè)大學的GrasserF等人研制出基于DSP的輪式倒立擺機器人Joe，通過解耦運動分解并簡化控制實現(xiàn)了較好的機動性，一時達到了前所未有的水平[1]，并引發(fā)了熱潮，帶動了諸多學者對輪式倒立擺機器人研究的關(guān)注，例如可以自主規(guī)劃避障行走的Nbot[2];新型載人兩輪交通工具Segway[3];采用模塊化結(jié)構(gòu)并應用差分電機驅(qū)動方式的Legway，可以通過調(diào)節(jié)自身平衡點實現(xiàn)爬坡等。

我國關(guān)于輪式倒立擺機器人的研究起步較晚，其主要局限在數(shù)學模型的建立、系統(tǒng)穩(wěn)定性的論證等。近年來,哈爾濱工業(yè)大學[4]、北京工業(yè)大學等大學也在這方面進行了相關(guān)理論研究和機器人平臺設計，并取得了一定的成果。因此，基于輪式倒立擺機器人展開的研究，無論是在理論研究還是工程應用都是具有很大價值的。

本文基于高性能的K60微控制器，設計了一種輪式倒立擺機器人。

輪式倒立擺機器人是本征不穩(wěn)定的控制系統(tǒng)，具有多變量、高階次、非線性和參數(shù)不確定的特點，它是驗證各種控制算法效果的理想實驗平臺。系統(tǒng)穩(wěn)定特點在一定程度與空間飛行器有很大的相似性，在控制問題上具有一定的普遍性和典型性。通過對該輪式倒立擺機器人的控制，能夠有效檢測控制方法是否具有處理非線性和不穩(wěn)定等問題的能力。與此同時，該系統(tǒng)可以對姿態(tài)、速度、方向進行控制，并可以作為一種姿態(tài)估計和機器人控制算法的實驗平臺，降低算法驗證的成本與復雜度。

1硬件設計

本文設計的輪式倒立擺機器人如圖1所示。

硬件主要由微控制器、電機驅(qū)動、圖像采集、速度檢測、姿態(tài)檢測等模塊組成，圖2為系統(tǒng)框圖。

圖1　輪式倒立擺機器人Fig 1　Wheeled inverted pendulum robot

圖2　硬件系統(tǒng)框圖Fig 2　Block diagram of hardware system

本文選用了Freescale公司Kinetis系列的32位微控制器K60。它基于ARMCortex—M4內(nèi)核，具有高達32位的DMA，并能夠提供多種CPU頻率；它的運算能力強、處理速度快，并擁有豐富的片內(nèi)資源，包括多個高精度16位ADC、12位DAC,6位模擬比較器、I/O口等；同時還擁有豐富的外部接口，包括SPI，IIS，IIC，CAN，SDHC，UART等，可以很好地實現(xiàn)機器人的控制任務。

1.1姿態(tài)檢測模塊

姿態(tài)檢測模塊由陀螺儀、加速度計及其放大電路組成，采用慣性測量方法進行姿態(tài)估計。設計選用IDG—650式陀螺儀，可以檢測運動時x軸和y軸角速度；選用MMA7361式三軸MEMS加速度計，可以同時輸出x，y，z三個方向上加速度信號； 姿態(tài)檢測模塊固定在機器人車身的垂直方向上，并假設它們之間是剛性連接。

1.2電機驅(qū)動電路

機器人采用了RS380型直流電機驅(qū)動同軸的兩個車輪。為了使電機充分發(fā)揮性能，提高靈敏性，設計了雙H橋電機驅(qū)動電路(如圖3所示)，為電機提供穩(wěn)定的工作電壓。使用NTMFS4833N型N溝道MOS管設計的驅(qū)動電路具有導通電阻小，導通電流可達191A，電機噪聲小，發(fā)熱少等特點[5]，并且極大地簡化了硬件電路。

圖3　單個H橋驅(qū)動電路Fig 3　Single H-bridge driver circuit

2控制結(jié)構(gòu)

控制部分由三個閉環(huán)控制回路：姿態(tài)環(huán)、速度環(huán)和方向環(huán)構(gòu)成(如圖4所示)，其中，姿態(tài)環(huán)的優(yōu)先級最高。

圖4　系統(tǒng)控制框圖Fig 4　Block diagram of system control

2.1姿態(tài)檢測單元

運動物體的姿態(tài)通常采用慣性坐標系中的橫滾角(φ)、俯仰角(θ)和航向角(ψ)來描述[7]。當機器人車身豎直時，以車軸中點為原點建立慣性坐標系，使車身的姿態(tài)通過歐拉角來描述。其中，車身方向為俯仰軸(z)，車軸方向為航向軸(x)，垂直于這兩個方向的為橫滾軸(y)。

姿態(tài)環(huán)由姿態(tài)檢測系統(tǒng)和姿態(tài)控制器構(gòu)成，影響俯仰軸方向的穩(wěn)定性。精確獲取當前的姿態(tài)，才能進行有效的姿態(tài)控制，實現(xiàn)機器人的直立平衡。機器人俯仰軸的運動是通過俯仰角θ描述，對應的角速度為ω，因此，只需要檢測俯仰角θ。

忽略其余加速度的影響，假設重力是唯一的加速度來源，可直接利用加速度計的輸出來估計俯仰角θ，計算公式為[7]

(1)

式中θacc為加速度計輸出得到的俯仰角，ax,ay,az為加速度計在體坐標系中測得加速度的分量。

由于運動過程中擺桿擺動造成很強的干擾，使加速度計輸出信號產(chǎn)生高頻誤差，無法獲得準確的姿態(tài)。而陀螺儀存在累計誤差和溫漂現(xiàn)象，靜態(tài)特性差，也不能單獨使用陀螺儀的輸出[8]。

因此，將陀螺儀和加速度計的輸出進行數(shù)據(jù)融合，得到估計的俯仰角θ。

通常數(shù)據(jù)融合采用卡爾曼濾波或互補濾波。本文結(jié)合陀螺儀和加速度計的頻響特性采用互補濾波進行數(shù)據(jù)融合。由于陀螺儀的高頻特性較好，對陀螺儀的輸出進行高通濾波；而加速度計的低頻特性較好，對加速度計的輸出進行低通濾波，將濾波后的數(shù)據(jù)進行融合，獲得精確的俯仰角θ。互補濾波的原理如圖5。

圖5　互補濾波原理Fig 5　Principle of complementary filtering

預估得到的姿態(tài)表達式為[7]

(2)

將式(2)離散化后得到預估的俯仰角為

(3)

式中θn為第n次濾波后角度，θn-1為第n-1次濾波后角度，a為高通濾波系數(shù)，T為采樣時間，b為低通濾波系數(shù)，az為加速度計z軸上的分量。

2.2姿態(tài)控制器

以估計的俯仰角θ作為姿態(tài)控制器的反饋輸入，進行姿態(tài)控制，可以使機器人達到直立平衡的狀態(tài)。本文采用比例微分(PD)控制器進行姿態(tài)控制。姿態(tài)控制器的輸出為

(4)

2.3速度控制器

在保證了穩(wěn)定平衡的情況下，可以對機器人進行速度控制和轉(zhuǎn)向控制等。采用數(shù)字比例積分微分(PID)控制器實現(xiàn)速度的閉環(huán)控制。以旋轉(zhuǎn)編碼器反饋的速度作為輸入，則速度控制器的輸出為

(5)

式中ev為輸出占空比與旋轉(zhuǎn)編碼器測得的占空比之差，亦即速度偏差，Tv為速度控制周期，Kvp為比例系數(shù)，Tvd為微分系數(shù)，Tvi為積分系數(shù)。

2.4轉(zhuǎn)向控制器

為了使機器人實現(xiàn)可以沿著測試道路的中線自動循跡，設計了由攝像頭和轉(zhuǎn)向控制器構(gòu)成的方向環(huán)。通過攝像頭采集圖像，微控制器對采集的圖像處理、提取道路信息并計算機器人中心偏離道路中線偏差。根據(jù)偏差控制車輪的速度差，實現(xiàn)機器人的轉(zhuǎn)向。PD控制器能很好地實現(xiàn)轉(zhuǎn)向，它的輸出為

(6)

式中ep為方向偏差，Tp為速度控制周期，Kpp為比例系數(shù)，Tpd為微分系數(shù)。

2.5電機控制

驅(qū)動電路提供兩路10kHz的脈寬調(diào)制(PWM)信號來控制電機，使電機轉(zhuǎn)動。兩路PWM信號的占空比是姿態(tài)、速度和轉(zhuǎn)向控制器輸出的線性疊加。左右電機驅(qū)動獲得的占空比分別為

(7)

式中Uleft，Uright分別為左、右驅(qū)動獲得的占空比，Ua為姿態(tài)控制的輸出，Uv為速度控制的輸出，Up為轉(zhuǎn)向控制的輸出。

3測試

最終對一些控制算法進行了測試，得出機器人運行軌跡如圖6所示，運行時俯仰角如圖7所示。從圖中可以看出：機器人大致是沿著道路的中線行駛的，行駛軌跡比較好，俯仰角的變化也比較小，說明控制算法是有效的，并且性能很好。

圖6　機器人運行軌跡圖Fig 6　Trajectory figure of robot running

4結(jié)論

實驗結(jié)果表明:該控制系統(tǒng)具有良好的魯棒性和穩(wěn)定性，可以準確地對控制算法的有效性和控制性能進行判斷。因此，可以將輪式倒立擺機器人作為實驗平臺，以其姿態(tài)、速度、位置作為被控對象，來驗證各類控制算法，并判定優(yōu)劣。

圖7　機器人姿態(tài)俯仰角曲線Fig 7　Curve of pitch angle of robot attitude

參考文獻:

[1]GrasserF,ArrigoAD,ColombiS,etal.JOE:Amobile,inverted,pendulum[J].IEEETransonIndustrialElectronic,2002,49(1):107-114.

[2]SalernoA,AngelesJ.Anewfamilyoftwo-wheeledmobilerobots:Modelingandcontrollability[J].IEEETransactionsonRobotics,2007,23(1):169-173.

[3]ArlingAW,ChangST,FieldJD,etal.Personaltransporter：US,D528468S[P].2006—09—19.

[4]王曉宇.兩輪自平衡機器人的研究[D].哈爾濱：哈爾濱工業(yè)大學,2007.

[5]吳苗苗,沈世斌,王亮,等.基于CMOS攝像頭的直立循跡智能車系統(tǒng)設計[J].自動化技術(shù)與應用,2014,33(7):21-25.

[6]蘭建軍,李佳璐，劉穎，等.基于自適應互補濾波算法的輪式倒立擺控制[J].組合機床與自動化加工技術(shù)，2014(6):66-68.

[7]LaiYC,JanSS,HsiaoFB.Developmentofalow-costattitudeandheadingreferencesystemusingathree-axisrotatingplat-form[J].Sensors,2010(10):2472-2491.

[8]楊天雨,賈文峰,賴際舟,等.慣性/光流/磁組合導航技術(shù)在四旋翼飛行器中的應用[J].傳感器與微系統(tǒng),2016,35(1):156-160.

Designofawheeledinvertedpendulumrobot*

WANGZhi-xiang，LIUYi-jian，CAONan

(SchoolofElectricalEngineeringandAutomation,NanjingNormalUniversity,Nanjing210042,China)

Abstract:Inverted pendulum is an ideal model for testing different control strategies,through which effectiveness and control performance of various control algorithms can be judged.A control system for wheeled inverted pendulum is designed by using powerful operation function and interface of K60 micro control unit(MCU) based on ARM Cortex-M4,and it is used to verify various new type control algorithm.For hardware,attitude detection system and motor driver unit which related to attitude control are introduced.For software,complementary filter,attitude controller,direction controller and velocity controller are introduced according to math model for system.Experimental results show that the control system has good robustness and stability,and it can exactly judge validity and control performance of control algorithm.

Key words:wheeled inverted pendulum; complementary filtering; attitude estimation; controller

DOI:10.13873/J.1000—9787(2016)04—0108—03

收稿日期：2016—03—10

*基金項目：江蘇省普通高校研究生科研創(chuàng)新計劃項目(1812000002A573)

中圖分類號:TP 242.6

文獻標識碼:A

文章編號:1000—9787(2016)04—0108—03

作者簡介:

汪智祥(1994-)，男，江蘇泰興人，主要研究方向為機器人開發(fā)與設計。

劉益劍，通訊作者，E—mail:63055@njnu.edu.cn。