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

汪智祥， 劉益劍， 曹　楠

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

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

汪智祥， 劉益劍， 曹楠

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

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

關鍵詞:輪式倒立擺； 互補濾波； 姿態估計； 控制器

0引言

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

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

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

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

1硬件設計

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

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

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

圖2　硬件系統框圖Fig 2　Block diagram of hardware system

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

1.1姿態檢測模塊

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

1.2電機驅動電路

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

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

2控制結構

控制部分由三個閉環控制回路：姿態環、速度環和方向環構成(如圖4所示)，其中，姿態環的優先級最高。

圖4　系統控制框圖Fig 4　Block diagram of system control

2.1姿態檢測單元

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

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

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

(1)

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

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

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

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

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

預估得到的姿態表達式為[7]

(2)

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

(3)

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

2.2姿態控制器

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

(4)

2.3速度控制器

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

(5)

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

2.4轉向控制器

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

(6)

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

2.5電機控制

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

(7)

式中Uleft，Uright分別為左、右驅動獲得的占空比，Ua為姿態控制的輸出，Uv為速度控制的輸出，Up為轉向控制的輸出。

3測試

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

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

4結論

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

圖7　機器人姿態俯仰角曲線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].哈爾濱：哈爾濱工業大學,2007.

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

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

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

[8]楊天雨,賈文峰,賴際舟,等.慣性/光流/磁組合導航技術在四旋翼飛行器中的應用[J].傳感器與微系統,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

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

中圖分類號:TP 242.6

文獻標識碼:A

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

作者簡介:

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

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