履帶輪椅機(jī)器人爬樓姿態(tài)控制的研究

曹鵬彬，趙建宇，湯旭青

武漢工程大學(xué)機(jī)電工程學(xué)院，湖北　武漢　430205

履帶輪椅機(jī)器人爬樓姿態(tài)控制的研究

曹鵬彬，趙建宇，湯旭青

武漢工程大學(xué)機(jī)電工程學(xué)院，湖北武漢430205

提出一種采用履帶結(jié)構(gòu)的輪椅機(jī)器人方案，并對(duì)其爬樓姿態(tài)的控制方法進(jìn)行研究.利用6軸運(yùn)動(dòng)傳感器獲取機(jī)體爬樓時(shí)的姿態(tài)數(shù)據(jù)，超聲波傳感器采集機(jī)體兩側(cè)的距離數(shù)據(jù)，根據(jù)輪椅機(jī)器人自身特點(diǎn)與實(shí)際經(jīng)驗(yàn)，使用模糊PID（Proportion Integration Differentiation）算法設(shè)計(jì)出模糊自整定PID控制器.通過在MATLAB/SIMULINK下仿真，該控制器能將方向角穩(wěn)定在預(yù)定角度，建立穩(wěn)態(tài)時(shí)間為0.17 s，最大超調(diào)量不超過7.2%.實(shí)際試驗(yàn)中，方向角穩(wěn)定在預(yù)設(shè)值±0.2°內(nèi)，具有很好的控制效果，實(shí)現(xiàn)了爬樓過程中的自動(dòng)控制.

履帶輪椅機(jī)器人；爬樓姿態(tài)控制；模糊PID

1　引言

對(duì)有行動(dòng)障礙需要使用輪椅的人群來說，生活中的樓梯成為一大障礙，傳統(tǒng)輪椅已經(jīng)無法滿足要求，為了使其生活更加便利，設(shè)計(jì)出了能夠爬樓梯的輪椅機(jī)器人.

在現(xiàn)有的研究成果中，有多種類型的爬樓機(jī)器，例如軌道式、行星輪式［1］、踏步式［2］、履帶式等等，其中，行星輪式和踏步式均需要專人輔助才能完成正常爬樓動(dòng)作，而軌道式和履帶式能夠自主運(yùn)行.軌道式爬樓裝置，需要預(yù)先安裝固定軌道，靈活性差，適用范圍窄.履帶式爬樓輪椅［3］是目前應(yīng)用較多的方案，其重心波動(dòng)小，傳動(dòng)效率高，能沿著與樓梯臺(tái)階直角頂端連線相平行的直線運(yùn)動(dòng)，雖然其在爬樓時(shí)無需人為操控，但不加反饋的開環(huán)動(dòng)力輸出存在著很大的安全隱患，因?yàn)闃翘荽嬖趦A角，如果輪椅運(yùn)動(dòng)方向與樓梯延伸方向之間的夾角過大，會(huì)有傾覆的危險(xiǎn)，也會(huì)因?yàn)槠x軌跡方向而撞到護(hù)欄，因此，迫切需要對(duì)其爬樓時(shí)的姿態(tài)進(jìn)行控制.

針對(duì)這些問題，采用傳感器獲取輪椅機(jī)器人的運(yùn)行姿態(tài)和位置，結(jié)合模糊控制與PID控制的優(yōu)點(diǎn)，利用模糊PID自適應(yīng)算法對(duì)其進(jìn)行實(shí)時(shí)控制，使其運(yùn)行方向與樓梯延伸方向保持相同，且維持其左右兩側(cè)的空隙處于一個(gè)安全的范圍內(nèi)，最終使輪椅機(jī)器人在樓梯上自動(dòng)、安全、穩(wěn)定的運(yùn)行.

2　輪椅機(jī)器人的結(jié)構(gòu)

結(jié)合現(xiàn)有各種輪椅的優(yōu)缺點(diǎn)，提出一種履帶結(jié)構(gòu)的輪椅機(jī)器人方案，結(jié)構(gòu)如圖1所示，圖中1為車架，履帶機(jī)構(gòu)由主履帶5、副擺臂履帶4組成，副履帶作為擺臂，在爬樓時(shí)伸出，增加與樓梯面的接觸面積.機(jī)器人在平地行走時(shí)只依靠驅(qū)動(dòng)輪2、萬向輪3，輪子由連桿機(jī)構(gòu)帶動(dòng)與地面接觸.爬樓時(shí)依靠輪、履的協(xié)同工作，履帶接觸樓梯的同時(shí)保持后驅(qū)動(dòng)輪也接觸臺(tái)階面，給底盤一個(gè)支撐力，可以防止由于重心不均導(dǎo)致底盤傾覆，此結(jié)構(gòu)可以減少履帶的長(zhǎng)度，使得機(jī)器人的底盤更加小巧.

由于機(jī)器人底盤工作在非常態(tài)的環(huán)境，會(huì)產(chǎn)生一些固有的不穩(wěn)定性因素［4］：履帶速度差；重力不均；臺(tái)階面形狀缺陷；外界干擾等，上述因素都會(huì)導(dǎo)致機(jī)器人底盤在行駛方向上產(chǎn)生較大的隨機(jī)偏差以及累積誤差，導(dǎo)致其可靠性、安全性能下降，發(fā)生危險(xiǎn).

圖1　輪椅底盤結(jié)構(gòu)圖Fig.1Structure diagram of wheelchair chassis

當(dāng)機(jī)器人底盤完全處于樓梯面上時(shí)，其姿態(tài)如圖2所示，主、副履帶與后驅(qū)動(dòng)輪保持平直，均與臺(tái)階接觸，在行進(jìn)方向上增大了接觸面積.在樓梯上移動(dòng)過程中，系統(tǒng)使用MPU6050來解算其姿態(tài)，MPU6050是整合性6軸運(yùn)動(dòng)處理組件，可以實(shí)時(shí)檢測(cè)底盤的角速度和加速度，由于MPU6050數(shù)據(jù)具有漂移的特征，所以加速度數(shù)據(jù)用來作為矯正，保證角速度數(shù)據(jù)穩(wěn)定可靠.

圖2　輪椅底盤爬樓狀態(tài)圖（a）二維示意圖；（b）三維示意圖Fig.2Climbing state diagram of wheelchair chassis（a）2D sketch；（b）3D sketch

三維狀態(tài)下，機(jī)器人底盤的運(yùn)動(dòng)狀態(tài)如圖2（b）所示，φ、θ、ψ表示X、Y、Z三軸的轉(zhuǎn)角，在三維空間中可以由四元數(shù)［5］來描述，通過角速度的轉(zhuǎn)換得到四元數(shù)，微分方程如式（1）所示：

其中q0、q1、q2、q3為四元數(shù)的參數(shù)，ωx、ωy、ωz為角速度，將上式轉(zhuǎn)化為角度關(guān)系式：

由式（2）可以確定輪椅底盤在樓梯面爬行時(shí)的姿態(tài)角度數(shù)據(jù)，φ和θ不參與控制，而對(duì)ψ進(jìn)行模糊PID控制，一方面觀察角度控制與否對(duì)最終效果的影響，另一方面從φ和θ可以看出機(jī)體運(yùn)行的平穩(wěn)性，觀察干擾因素的存在情況.

3　PID控制器硬件設(shè)計(jì)

本文搭建了一套基于STM32的嵌入式控制系統(tǒng)［6］，為了調(diào)試方便，采用無線的方式將PC機(jī)與嵌入式系統(tǒng)相連，形成一套完整的試驗(yàn)平臺(tái)，該平臺(tái)能實(shí)時(shí)獲取輪椅底盤的運(yùn)行數(shù)據(jù)，并在線修改參數(shù)，使得控制效果能同步反映，其結(jié)構(gòu)如圖3所示.

圖3　試驗(yàn)平臺(tái)結(jié)構(gòu)框圖Fig.3Structure diagram of test platform

控制器的硬件部分如表1所示，其中包括以STM32F4為核心的控制板，檢測(cè)姿態(tài)、速度等信息的各類型傳感器，2.4 GHz無線網(wǎng)絡(luò)通訊模塊.

設(shè)計(jì)的試驗(yàn)樣機(jī)包括輪椅機(jī)器人底盤的機(jī)械結(jié)構(gòu)與電氣控制裝置，該試驗(yàn)樣機(jī)模型整備質(zhì)量為4 kg，額定負(fù)載為20 kg.

圖4為輪椅機(jī)器人底盤控制硬件結(jié)構(gòu)圖，主控芯片通過I2C總線、SPI總線、I/O口與外部傳感器和模塊相連.系統(tǒng)采集姿態(tài)檢測(cè)傳感器MPU6050與電子羅盤、編碼器、超聲測(cè)距儀3部分?jǐn)?shù)據(jù)，分別形成3路閉環(huán)控制，如圖5所示，與傳統(tǒng)的單環(huán)控制相比較，多路閉環(huán)控制能保證更加精確的控制效果，抗干擾能力強(qiáng)，避免某個(gè)傳感器出現(xiàn)問題后失去控制能力，提升了系統(tǒng)可靠性.

表1　控制器的硬件組成Tab.1Hardware components of controller

圖4　硬件結(jié)構(gòu)框圖Fig.4Structure diagram of hardware framework

圖5　閉環(huán)控制原理圖Fig.5Principle diagram of closed-loop control

4　PID控制器算法設(shè)計(jì)

4.1模糊PID控制器設(shè)計(jì)

在方向?qū)Ш降膽?yīng)用中，移動(dòng)物體需要對(duì)實(shí)時(shí)的航向數(shù)據(jù)與目標(biāo)之間的偏差E進(jìn)行糾正.對(duì)于處在樓梯面上，依靠履帶面接觸的輪椅機(jī)器人底盤來說，建立精確的數(shù)學(xué)模型較為困難，無法抑制由于不穩(wěn)定因素所帶來的系統(tǒng)誤差，抗干擾性差，而結(jié)合現(xiàn)代控制理論中的模糊自適應(yīng)控制，能夠在線識(shí)別姿態(tài)的特征參數(shù)，實(shí)時(shí)改變控制策略，使參數(shù)保持在合理的范圍內(nèi).

控制器采用兩輸入，三輸出的方式，規(guī)定機(jī)體的行走方向后，將當(dāng)前方向與目標(biāo)方向的誤差E與誤差變化率Ec為輸入，經(jīng)過控制器一系列的模糊化，去模糊化［7］之后，實(shí)時(shí)輸出PID的3個(gè)參數(shù)，根據(jù)機(jī)體當(dāng)前的姿態(tài)數(shù)據(jù)做出最合適的控制.

系統(tǒng)在不同的誤差E和誤差率Ec的值時(shí)，通過一系列模糊規(guī)則的處理，最終得出相應(yīng)的修正系數(shù)kp，ki，kd，加入到當(dāng)前的PID控制中，由于系統(tǒng)在運(yùn)行過程中始終參與檢查與運(yùn)算，可以對(duì)參數(shù)進(jìn)行在線修改［8］，以滿足不同的誤差E和誤差率Ec對(duì)控制參數(shù)的不同要求.本文對(duì)誤差E的計(jì)算樣本是來自多個(gè)傳感器的值，根據(jù)不同值的穩(wěn)定性情況設(shè)計(jì)不同的動(dòng)態(tài)權(quán)重值，若是傳感器發(fā)生故障，跳動(dòng)較大，則相應(yīng)減小其權(quán)重，維持計(jì)算值處于正常范圍.

設(shè)系統(tǒng)中輸入輸出參數(shù)E、Ec、kp，ki，kd的模糊子集均為｛NB，NM，NS，ZO，PS，PM，PB｝，子集中的7個(gè)元素分別表示負(fù)大、負(fù)中、負(fù)小、零、正小、正中、正大.5個(gè)輸入輸出參數(shù)論域均為［-3，3］，其中｛-3、-2、-1、0、1、2、3｝7個(gè)點(diǎn)與模糊子集中的元素一一對(duì)應(yīng).取輸入輸出參數(shù)的隸屬度函數(shù)均為三角形曲線.

模糊PID控制［9］的核心是根據(jù)相關(guān)的技術(shù)要求和實(shí)際操作經(jīng)驗(yàn)，建立相應(yīng)的模糊規(guī)則表，按照MATLAB格式轉(zhuǎn)換成模糊規(guī)則，形式如下：

If（E is NB）and（Ecis NB）then（kp is PB）（ki is NB）（kd is PS）（1）

If（E is NB）and（Ecis NM）then（kp is PB）（ki is NB）（kd is NS）（1）

……

If（E is PB）and（Ecis PB）then（kp is NB）（ki is PB）（kd is PB）（1）

末尾的“1”為每條規(guī)則的權(quán)重值，根據(jù)模糊規(guī)則建立E，Ec兩項(xiàng)輸入，kp，ki，kd三輸出的模糊控制器.

4.2模糊PID控制器仿真

在MATLAB中，SIMULINK工具箱［10］和模糊邏輯工具箱具有組件式的功能塊，根據(jù)上述的控制原理，構(gòu)建仿真模型系統(tǒng).系統(tǒng)包含兩大部分，分別是模糊處理部分和PID處理部分，結(jié)合傳統(tǒng)PID和模糊控制，對(duì)系統(tǒng)進(jìn)行動(dòng)態(tài)仿真，如圖6所示.

圖6　SIMULINK仿真系統(tǒng)Fig.6SIMULINK simulation system

如圖7、圖8所示，采樣時(shí)間定為1 ms，在控制系統(tǒng)開始后，能夠在0.17 s內(nèi)完成調(diào)節(jié)，最大超調(diào)量為7.2%，速度較快，反應(yīng)迅速.

圖7　控制系統(tǒng)階躍響應(yīng)曲線Fig.7Step response curve of control system

圖8　控制器輸出狀態(tài)曲線Fig.8Output state curve of controller

為驗(yàn)證系統(tǒng)的抗干擾性能，在0.3 s時(shí)刻對(duì)系統(tǒng)施加1.0的干擾，如圖7、圖8所示，系統(tǒng)能迅速對(duì)干擾進(jìn)行調(diào)節(jié)，將時(shí)間控制在0.8 s內(nèi)，超調(diào)量小于3.3%，滿足預(yù)期目標(biāo).

5　試驗(yàn)與結(jié)果分析

爬樓試驗(yàn)環(huán)境為一段兩側(cè)均有墻體的樓梯，具體相關(guān)的參數(shù)如表2所示.

表2　樓梯參數(shù)Tab.2Stairs parameters

試驗(yàn)的基本過程為：將輪椅機(jī)器人輪子收起，放在樓梯底端，只靠履帶接觸臺(tái)階凸起部分，通過2.4 GHz無線模塊與PC上位機(jī)連接，發(fā)送運(yùn)行指令后，輪椅機(jī)器人在樓梯上爬行，左右放置的超聲波傳感器實(shí)時(shí)檢測(cè)兩側(cè)的距離，此數(shù)據(jù)作為MPU6050數(shù)據(jù)的參考值帶入控制系統(tǒng)進(jìn)行計(jì)算，機(jī)體運(yùn)行到樓梯頂端時(shí)停止，記錄并保存相關(guān)的數(shù)據(jù)，便于后期的分析.

圖9所示為輪椅機(jī)器人在X，Y，Z軸方向上的姿態(tài)角數(shù)據(jù)，其中翻滾角φ和俯仰角θ不由系統(tǒng)所直接控制，反映的是輪椅機(jī)器人在運(yùn)行狀態(tài)時(shí)的平穩(wěn)性，由圖9可以看出圖9（a）、（b）的跳動(dòng)較小，不超過±0.35°.方向角ψ為機(jī)體的行進(jìn)方向，由模糊PID控制算法控制，圖9（c）所示，最大偏差不超過±0.2°，與圖9（a）、（b）中不加控制的隨機(jī)跳動(dòng)相比，在行進(jìn)方向上具有較好的控制效果.

圖9　機(jī)體姿態(tài)曲線（a）翻滾角；（b）俯仰角；（c）方向角Fig.9Posture curves of chassis（a）Rolling angle；（b）Pitching angle；（c）Direction angle

如圖10所示，行進(jìn)方向的垂直方向用X軸表示，圖10中顯示了輪椅機(jī)器人在運(yùn)行時(shí)X軸上的位置誤差以及控制結(jié)果，其中誤差大部分落在（-0.02，0.02）范圍內(nèi)，最大不超過±0.3 m，控制效果較好.

圖10　x方向位移誤差Fig.10Error of x direction displacement

為了驗(yàn)證該輪椅機(jī)器人能適應(yīng)非直線型樓梯，設(shè)計(jì)了圓形樓梯模型，讓其運(yùn)行在帶弧度的臺(tái)階上.

設(shè)計(jì)弧度值從0°到80°均勻增加，如圖11所示，直線代表預(yù)期的方向變化，折線代表底盤實(shí)際運(yùn)行時(shí)的方向變化，從圖11（a）中可以看出，整個(gè)60 s的運(yùn)行過程中，折線跳動(dòng)較小，基本與直線重合，圖11（b）為20 s到34 s內(nèi)方向變化的局部放大圖，圖11中顯示折線在基準(zhǔn)直線上下振幅較小，實(shí)際運(yùn)行效果與數(shù)據(jù)顯示相符.

圖11　弧形臺(tái)階運(yùn)行曲線（a）完整曲線；（b）局部放大圖Fig.11Performance curves of arc stair（a）Complete curve；（b）Magnification curve

6　結(jié)語

以上通過對(duì)輪椅底盤在爬樓狀態(tài)情況下的運(yùn)動(dòng)分析，總結(jié)了對(duì)姿態(tài)影響的因素，設(shè)計(jì)了嵌入式控制平臺(tái)，根據(jù)運(yùn)動(dòng)處理組件MPU6050與其他輔助傳感器的實(shí)時(shí)數(shù)據(jù)，運(yùn)用模糊PID算法對(duì)輸出進(jìn)行控制.搭建實(shí)驗(yàn)平臺(tái)，通過MATLAB進(jìn)行模糊PID算法的仿真實(shí)驗(yàn)，并在自行研制的輪椅底盤模型上進(jìn)行了運(yùn)動(dòng)控制實(shí)驗(yàn)，能將方向偏差控制在±0.2°內(nèi)，位移偏差穩(wěn)定在±0.02 m范圍內(nèi)，控制效果優(yōu)于常規(guī)的PID系統(tǒng)［11］，驗(yàn)證了MATLAB下的仿真結(jié)論，具有較小的穩(wěn)態(tài)誤差，且能適應(yīng)不同類型的樓梯面，通用性好.

［1］CHARLES J M.Robotic lifesaver-vecna's humanoid robot can walk，run，lift heavy objects，and climb stairs on its track-powered，jointed legs［J］.Design news，2006，61（16）：46-48，50.

［2］DONGKYUC，JONGKYUNO，JONGWONK. Analysis method of climbing stairs with the rocker-bogie mechanism［J］.Journal of mechanical science and technology，2013，27（9）：2783-2788.

［3］孟曉東.輪履組合式電動(dòng)爬樓輪椅的研究［D］.長(zhǎng)春：長(zhǎng)春理工大學(xué)，2013.

［4］莫海軍，朱文堅(jiān).履帶式移動(dòng)機(jī)器人越障穩(wěn)定性分析［J］.機(jī)械科學(xué)與技術(shù)，2007，26（1）：65-67. MO H J，ZHU W J.Analysis of the stability of a crawler-typemobilerobotcrossingoverobstacles［J］.Mechanical science and technology for aerospace engineering，2007，26（1）：65-67.

［5］王茂永.下肢康復(fù)機(jī)器人感知系統(tǒng)設(shè)計(jì)與研究［D］.成都：電子科技大學(xué)，2014.

［6］廖煜雷，萬磊，莊佳園.噴水推進(jìn)型無人水面艇的嵌入式運(yùn)動(dòng)控制系統(tǒng)研究［J］.高技術(shù)通訊，2012，22（4）：416-422. LIAO Y L，WAN L，ZHUANG J Y.An embedded motion control system for water-jet-propelled unmanned surface vehicles［J］.Chinese high technology letters，2012，22（4）：416-422.

［7］劉淵，周旺平.基于DSP的斜軸式望遠(yuǎn)鏡機(jī)架控制系統(tǒng)設(shè)計(jì)［J］.計(jì)算機(jī)測(cè)量與控制，2014，22（11）：3572-3575. LIU Y，ZHOU W P.Design of oblique axis telescope mount system controller based on DSP［J］.Computer measurement&control，2014，22（11）：3572-3575，3580.

［8］尤洋，文小玲，鄒艷華.一種無線溫度監(jiān)控系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)［J］.武漢工程大學(xué)學(xué)報(bào)，2015，37（1）：30-34. YOU Y，WEN X L，ZHOU Y H.Design of wireless temperature monitoring system based on zigbee［J］. Journal of Wuhan institute of technology，2015，37（1）：30-34.

［9］儲(chǔ)岳中，陶永華.基于MATLAB的自適應(yīng)模糊PID控制系統(tǒng)計(jì)算機(jī)仿真［J］.安徽工業(yè)大學(xué)學(xué)報(bào)（自然科學(xué)版），2004，21（1）：49-52. CHU Y Z，TAO Y H.Computer simulation of the self-adaptive fuzzy PID control system based on matlab［J］.Journal of Anhui university of technology（Natural Science），2004，21（1）：49-52.

［10］侯喜茹，柳貢慧，梁景偉.基于Matlab/Simulink模糊工具箱的井眼軌跡模糊控制仿真［J］.石油學(xué)報(bào)，2006，27（3）：108-111. HOU X R，LIU G H，LIANG J W.Fuzzy control simulation of wellbore trajectory based on matlab/ simulink fuzzy tool box［J］.Acta petrolei sinica，2006，27（3）：108-111.

［11］楊世勇，徐國(guó)林.模糊控制與PID控制的對(duì)比及其復(fù)合控制［J］.自動(dòng)化技術(shù)與應(yīng)用，2011，30（11）：21-25. YANG S Y，XU G L.Comparison and composite of fuzzy control and PID control［J］.Techniques of automation and applications，2011，30（11）：21-25.

本文編輯：陳小平

Stair-Climbing Attitude Control of Tracked Wheelchair Robot

CAO Pengbin,ZHAO Jianyu,TANG Xuqing
School of Mechanical and Electrical Engineering,Wuhan Institute of Technology,Wuhan 430205,China

A structural scheme of tracked robot was proposed and the control method of the stair-climbing attitude was studied.A 6-axis motion sensor was employed to acquire the attitude data of the robot in the process of climbing stairs.In addition,supersonic sensors were installed to acquire the distance data on both sides of the robot body.According to the robot characteristics and practical experience,a kind of fuzzy self-tuning proportion integration differentiation（PID）controller was devised using fuzzy PID algorithm.A simulation by the tool of MATLAB/SIMULINK was performed,in which the controller could be stabilized at a predetermined angle in 0.17 s and the maximum overshoot was less than 7.2%.The direction angle was keeping with in±0.2° of the preset value and the automatic control of the tracked wheel chair robot climbing stairs was realized with good effect in the practical experiments

tracked wheelchair robot；stair-climbing attitude control；fuzzy-PID

TP242.6

A

10.3969/j.issn.1674?2869.2016.05.014

1674-2869（2016）05-0484-06

2016-03-06

湖北省教育廳科學(xué)技術(shù)研究項(xiàng)目（D20141502）；武漢工程大學(xué)研究生教育創(chuàng)新基金（CX2014035）

曹鵬彬，博士，副教授.E-mail：caopengbin@wit.edu.cn