汪智祥， 劉益劍， 曹　楠

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

一種輪式倒立擺機(jī)器人設(shè)計(jì)*

汪智祥， 劉益劍， 曹楠

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

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

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

0引言

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

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

本文基于高性能的K60微控制器，設(shè)計(jì)了一種輪式倒立擺機(jī)器人。

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

1硬件設(shè)計(jì)

本文設(shè)計(jì)的輪式倒立擺機(jī)器人如圖1所示。

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

圖1　輪式倒立擺機(jī)器人Fig 1　Wheeled inverted pendulum robot

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

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

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

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

1.2電機(jī)驅(qū)動(dòng)電路

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

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

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

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

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

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

運(yùn)動(dòng)物體的姿態(tài)通常采用慣性坐標(biāo)系中的橫滾角(φ)、俯仰角(θ)和航向角(ψ)來描述[7]。當(dāng)機(jī)器人車身豎直時(shí)，以車軸中點(diǎn)為原點(diǎn)建立慣性坐標(biāo)系，使車身的姿態(tài)通過歐拉角來描述。其中，車身方向?yàn)楦┭鲚S(z)，車軸方向?yàn)楹较蜉S(x)，垂直于這兩個(gè)方向的為橫滾軸(y)。

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

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

(1)

式中θacc為加速度計(jì)輸出得到的俯仰角，ax,ay,az為加速度計(jì)在體坐標(biāo)系中測(cè)得加速度的分量。

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

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

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

圖5　互補(bǔ)濾波原理Fig 5　Principle of complementary filtering

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

(2)

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

(3)

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

2.2姿態(tài)控制器

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

(4)

2.3速度控制器

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

(5)

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

2.4轉(zhuǎn)向控制器

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

(6)

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

2.5電機(jī)控制

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

(7)

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

3測(cè)試

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

圖6　機(jī)器人運(yùn)行軌跡圖Fig 6　Trajectory figure of robot running

4結(jié)論

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

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

參考文獻(xiàn):

[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]王曉宇.兩輪自平衡機(jī)器人的研究[D].哈爾濱：哈爾濱工業(yè)大學(xué),2007.

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

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

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

[8]楊天雨,賈文峰,賴際舟,等.慣性/光流/磁組合導(dǎo)航技術(shù)在四旋翼飛行器中的應(yīng)用[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

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

中圖分類號(hào):TP 242.6

文獻(xiàn)標(biāo)識(shí)碼:A

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

作者簡(jiǎn)介:

汪智祥(1994-)，男，江蘇泰興人，主要研究方向?yàn)闄C(jī)器人開發(fā)與設(shè)計(jì)。

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