基于模糊控制的全位移平衡機(jī)器人設(shè)計

劉 恒, 李 哲, 楊明輝, 鄧曉剛, 曹玉蘋

(中國石油大學(xué)(華東) 控制科學(xué)與工程學(xué)院,山東 青島 266580)

隨著世界經(jīng)濟(jì)和科技的高速發(fā)展以及人口數(shù)量的不斷增多,人類對于各類資源的需求與日俱增,促使各國紛紛開始對未知風(fēng)險的領(lǐng)域與地區(qū)進(jìn)行資源的勘探與開發(fā)。與此同時,近年來頻發(fā)的自然災(zāi)害與安全事故帶給人們的血淚教訓(xùn),也同樣促使著人們加大關(guān)于應(yīng)用型機(jī)器人的投入與發(fā)展。

針對目前勘探工作與災(zāi)難事故救援中所具有的復(fù)雜性、緊迫性和危險性的問題,基于模糊控制的全位移平衡機(jī)器人應(yīng)用于未知環(huán)境下的探測工作與災(zāi)難事故救援時,可以有效地緩解工作中時間緊、危險性大等問題,還能克服人類無法逾越的障礙,保障人們的生命財產(chǎn)安全,減少損失[1]。

隨著嵌入式以及智能控制的發(fā)展,雙輪自平衡機(jī)器人已經(jīng)在許多領(lǐng)域開展研究和投入使用[2]。傳統(tǒng)的兩輪機(jī)器人可以實現(xiàn)前進(jìn)、后退、轉(zhuǎn)彎等常規(guī)運動,能夠滿足人們的日常需求。但在勘探救援等工作中,會面臨眾多復(fù)雜狹小的地形。雙輪機(jī)器人的體積較小,轉(zhuǎn)彎半徑小使得其運動更加靈活[3]。這類機(jī)器人在靜態(tài)下是個典型的不穩(wěn)定結(jié)構(gòu),具有非線性、不確定性和強(qiáng)耦合性的特點[4],必須實時依靠雙輪移動來保持平衡[5]。筆者所設(shè)計的四輪平衡機(jī)器人可以有效地解決這個問題,實現(xiàn)平移功能,并且通過縮小機(jī)器人的整體體積,從而增加機(jī)器人的可應(yīng)用場景。

移動機(jī)器人運動控制系統(tǒng)通常包含兩部分內(nèi)容:一是完成所需的控制功能和算法的定義;二是這些功能在硬件和軟件上的實現(xiàn)[6-7]。運動控制系統(tǒng)精度是衡量機(jī)器人性能的重要指標(biāo)之一[8],傳統(tǒng)自平衡機(jī)器人的控制算法一般采用傳統(tǒng)的PID控制、滑模(Sliding Mode)控制、狀態(tài)反饋極點配置控制器和模糊控制等[9-10],模糊控制的基本思想最早是由Zadeh和Mamdani[11-12]提出的,本設(shè)計使用了模糊控制PID改進(jìn)算法,使得機(jī)器人在起伏等復(fù)雜環(huán)境中能夠擁有較強(qiáng)的適應(yīng)性及魯棒性。

1 機(jī)械系統(tǒng)設(shè)計

1.1 底盤設(shè)計

目前常用的輪式機(jī)器人驅(qū)動方式有:麥克納姆輪式機(jī)器人、全輪偏轉(zhuǎn)式移動機(jī)器人和球輪式移動機(jī)器人[13-14]。筆者采用四同軸心麥克納姆輪設(shè)計,在具有原地轉(zhuǎn)向和水平移動功能的基礎(chǔ)上,還具有速度快、結(jié)構(gòu)穩(wěn)定、負(fù)載能力強(qiáng)、維護(hù)簡單等優(yōu)點。為進(jìn)一步縮小機(jī)器人整體尺寸,根據(jù)麥克納姆輪運動分析,采用交叉布置、同步帶傳動方式。為確保底盤結(jié)構(gòu)具有充足的魯棒性、穩(wěn)定性,使機(jī)器人能夠適應(yīng)多種復(fù)雜路況,結(jié)合實際成本、材料強(qiáng)度以及設(shè)計水平和實現(xiàn)難度等因素,懸掛機(jī)構(gòu)采用垂直懸掛。根據(jù)平衡機(jī)器人的特點,采用大扭矩電機(jī)和同步帶驅(qū)動的連桿組設(shè)計,保證了機(jī)器人在倒地后具有自救能力,以適應(yīng)復(fù)雜、惡劣工況下的任務(wù)要求。機(jī)器人底盤機(jī)械設(shè)計如圖1所示。

圖1 機(jī)器人底盤機(jī)械設(shè)計

基于麥克納姆輪的全向移動機(jī)器人能實現(xiàn)前進(jìn)、橫移、360°零半徑旋轉(zhuǎn)等基本運動和復(fù)合運動[15]。傳統(tǒng)麥克納姆輪的安裝方向分為互為鏡像關(guān)系的A輪與B輪。當(dāng)麥輪正向旋轉(zhuǎn)時,由速度的正向分解,A輪可分解為沿軸向左與垂直于軸向的前向速度分量;B輪可分解為沿軸向右與垂直于軸向的前向速度分量。據(jù)此結(jié)構(gòu)可以實現(xiàn)平移功能。傳統(tǒng)結(jié)構(gòu)麥輪組合示意圖如圖2所示。

圖2 傳統(tǒng)結(jié)構(gòu)麥輪組合示意圖

平衡機(jī)器人的底盤根據(jù)傳統(tǒng)麥克納姆輪的結(jié)構(gòu)進(jìn)行改進(jìn),使其在減小整體體積的同時兼具靈活性和穩(wěn)定性。改進(jìn)設(shè)計的平衡機(jī)器人麥克納姆輪底盤如圖3所示。

圖3 改進(jìn)設(shè)計的平衡機(jī)器人麥克納姆輪底盤

圖4為平衡機(jī)器人通過狹小環(huán)境示意圖,相比于傳統(tǒng)四輪機(jī)器人,本設(shè)計能夠更好地通過狹小空間,適應(yīng)性更強(qiáng)。

圖4 機(jī)器人通過狹小環(huán)境示意圖

1.2 云臺設(shè)計

移動式云臺的機(jī)械結(jié)構(gòu)角度可隨控制信號作用進(jìn)行調(diào)整,為提高平臺視覺系統(tǒng)在立體空間內(nèi)的檢測范圍與精度效果,選取雙軸移動式云臺結(jié)構(gòu)作為設(shè)計方案,雙軸為俯仰(Pitch)軸和旋轉(zhuǎn)(Yaw)軸。Pitch軸采用電機(jī)直驅(qū)的方式進(jìn)行驅(qū)動,Yaw軸采用同步帶進(jìn)行傳動,保證了控制的準(zhǔn)確性。云臺機(jī)械結(jié)構(gòu)設(shè)計如圖5所示。

圖5 云臺機(jī)械結(jié)構(gòu)設(shè)計

1.3 仿真測試

使用Adams對機(jī)器人自救機(jī)構(gòu)進(jìn)行仿真,計算電機(jī)需要的扭矩。根據(jù)整機(jī)的質(zhì)量,對所需輪組電機(jī)扭矩進(jìn)行計算。根據(jù)Adams進(jìn)行懸掛分析,觀察機(jī)器人的運動形態(tài)。機(jī)器人參數(shù)說明如表1所示。平衡機(jī)器人整體機(jī)械設(shè)計示意圖見圖6。

表1 機(jī)器人參數(shù)說明

圖6 平衡機(jī)器人整體機(jī)械設(shè)計示意圖

2 機(jī)器人控制系統(tǒng)的硬件設(shè)計

圍繞STM32F405最小系統(tǒng)電路,依據(jù)機(jī)器人任務(wù)要求設(shè)計功能電路,主要包括:數(shù)字/模擬電路、外設(shè)功能電路、接口保護(hù)電路、電源轉(zhuǎn)化電路、電源防反接與短路電路等。

2.1 電源模塊設(shè)計

硬件系統(tǒng)電源輸入為24 V電池,進(jìn)行LC濾波后,進(jìn)行并聯(lián)分流處理,為底盤電機(jī)、云臺電機(jī)及視覺模塊等部分提供24 V電壓電流。24～5 V(DC-DC)電路選擇MP2482電源芯片,實現(xiàn)24～5 V的電源變換。3.3 V輸出電源為所有電源電路中對信號波紋質(zhì)量要求最高的部分。為保證硬件系統(tǒng)內(nèi)所有3.3 V芯片高效、穩(wěn)定地工作,本設(shè)計采用電路信號波紋穩(wěn)定、結(jié)構(gòu)相對簡單的低壓差線性穩(wěn)壓器(LDO)電源變換芯片MIC3975實現(xiàn)5～3.3 V(LDO)轉(zhuǎn)換電路。

2.2 保護(hù)電路設(shè)計

直流電源輸入通常會給端口帶來瞬時脈沖噪聲。為了消除噪聲對器件造成的損害。設(shè)計瞬態(tài)二極和(TVS)保護(hù)電路將瞬時電流從敏感器件引到地信號中。

系統(tǒng)存在模電電源與數(shù)字電源兩種電源分量,數(shù)字電源及信號中含有的高頻分量會對模擬信號產(chǎn)生干擾、損害等影響。本設(shè)計使用LC低通濾波器,有效地將模電電源與數(shù)字電源進(jìn)行分離,保證系統(tǒng)數(shù)字電源與模擬電源之間互不干擾。

2.3 外設(shè)電路設(shè)計

外設(shè)電路設(shè)計主要包括:ICM20948傳感器電路(如圖7所示)、CAN通信差分電路(如圖8所示)、USART接口電路、DEBUS協(xié)議接收電路、PWM控制電路、蜂鳴器報警電路、串行線調(diào)試(SWD)電路等。STM32F405主控設(shè)計如圖9所示。

圖7 ICM20948電路原理圖

圖8 CAN通信差分電路

圖9 STM32F405主控設(shè)計

3 機(jī)器人控制算法的設(shè)計

3.1 主程序邏輯框架

主程序邏輯建立在STM32系統(tǒng)中斷的基礎(chǔ)上,防止各個模塊任務(wù)之間的沖突,不同模塊的任務(wù)擁有不同的優(yōu)先級,如圖10所示。最高優(yōu)先級的任務(wù)為:遙控器數(shù)據(jù)接收;高優(yōu)先級的任務(wù)為:CAN以及USART等數(shù)據(jù)接收;次高優(yōu)先級的任務(wù)為:底盤電機(jī)、云臺電機(jī)以及各種數(shù)據(jù)處理;低優(yōu)先級任務(wù)為:各模塊初始化、看門狗、LCD顯示等。

圖10 各任務(wù)優(yōu)先級順序

圖11 主體程序流程圖

通過對不同任務(wù)的中斷優(yōu)先級的設(shè)計,使得任務(wù)能夠穩(wěn)定正常地運行,同時對遙控器的數(shù)據(jù)進(jìn)行處理,使機(jī)器人在接收到不同的數(shù)據(jù)時做出對應(yīng)的決策。在沒有接收到遙控器指令時,能對自身的狀態(tài)進(jìn)行監(jiān)控,主體程序流程圖見11。

3.2 云臺算法設(shè)計

云臺等待遙控器發(fā)出信號,當(dāng)收到遙控器信號之后,云臺參數(shù)開始初始化,隨后云臺位置歸中,當(dāng)云臺歸中任務(wù)完成后,云臺給定值由遙控器發(fā)送,隨后云臺進(jìn)入串級控制模式,云臺控制邏輯如圖12所示。

圖12 云臺控制邏輯框圖

當(dāng)云臺初始化任務(wù)完成之后,云臺Pitch軸的角度由遙控器給定,云臺Yaw軸的角度由底盤給定,即云臺Yaw軸跟隨底盤旋轉(zhuǎn)。云臺外環(huán)反饋值是電機(jī)輸出的速度值,內(nèi)環(huán)反饋值是電機(jī)當(dāng)前的角度值,構(gòu)成速度環(huán)為主回路,角度環(huán)為內(nèi)回路的串級PID控制器。云臺Pitch軸串級控制如圖13所示。

圖13 云臺Pitch軸串級控制框圖

圖14 平衡機(jī)器人受力分析

3.3 底盤算法設(shè)計

在現(xiàn)實平衡機(jī)器人的控制中,當(dāng)平衡機(jī)器人受到不同的擾動時,機(jī)器人傾斜的角度與速度不同,那么恢復(fù)所需的速度與加速度往往是不同的;同時機(jī)器人在自平衡和受干擾時所需要的條件不同,這就需要動態(tài)調(diào)整PID的參數(shù),使機(jī)器人能夠更好更快更穩(wěn)地調(diào)整和保持姿態(tài),而模糊控制剛好能夠滿足這一條件。

3.3.1 底盤整體控制邏輯設(shè)計

經(jīng)過改進(jìn)的全位移平衡機(jī)器人的底盤,通過對平衡和位移運動的解耦,底盤上裝有HI220陀螺儀傳感器,可以實時測得底盤的傾斜角度和加速度,通過導(dǎo)電滑環(huán)和CAN通信與云臺上的STM32F405進(jìn)行通信,據(jù)此反饋在速度環(huán)和角度環(huán)串聯(lián)之后由模糊控制的PID形成平衡任務(wù)的算法,使得平衡機(jī)器人可以像傳統(tǒng)麥克納姆輪底盤一樣,實現(xiàn)平移、旋轉(zhuǎn)等多種運動。

3.3.2 模糊控制算法設(shè)計

① 輸入量的量化。通過量化函數(shù)將輸入量映射到對應(yīng)的區(qū)間,根據(jù)傳感器測得平衡機(jī)器人的實際傾斜角度為[-30,30],選擇映射域為[-6,6]。

② 輸入值的模糊化。首先確定對應(yīng)各語言變量的模糊子集,然后根據(jù)量化的結(jié)果,判斷該輸入所屬的集合并用三角形隸屬度函數(shù)(見公式(1))計算出對應(yīng)的隸屬度。三角形曲線的形狀由3個參數(shù)a,b,c確定:

(1)

式中:參數(shù)a和c確定三角形的最小值,參數(shù)b確定三角形的最大值。

③ 建立模糊規(guī)則表。將偏差E以及偏差增量EC劃分為7個模糊子集:負(fù)大(NL)、負(fù)中(NM)、負(fù)小(NS)、零(ZE)、正小(PS)、正中(PM)、正大(PL)。

比例調(diào)節(jié)系數(shù)KP值的選取決定于系統(tǒng)的響應(yīng)速度。增大KP能提高響應(yīng)速度,減小穩(wěn)態(tài)偏差;但是,KP值過大會產(chǎn)生較大的超調(diào),甚至使系統(tǒng)不穩(wěn)定,減小KP可以減小超調(diào),提高穩(wěn)定性,但KP過小會減慢響應(yīng)速度,延長調(diào)節(jié)時間。因此,調(diào)節(jié)初期應(yīng)適當(dāng)取較大的KP值以提高響應(yīng)速度,而在調(diào)節(jié)中期,KP則取較小值,以使系統(tǒng)具有較小的超調(diào)并保證一定的響應(yīng)速度;而在調(diào)節(jié)過程后期再將KP值調(diào)到較大值來減小靜差,提高控制精度。基于上述描述定義KP的模糊規(guī)則,如表2所示。

表2 KP規(guī)則表

同理,在系統(tǒng)控制中,積分控制主要是用來消除系統(tǒng)的穩(wěn)態(tài)偏差,根據(jù)積分調(diào)節(jié)系數(shù)KI的作用定義KI的模糊規(guī)則,如表3所示。

表3 KI規(guī)則表

微分環(huán)節(jié)的調(diào)整主要是針對大慣性過程引入的,微分環(huán)節(jié)系數(shù)KD的作用在于改變系統(tǒng)的動態(tài)特性。根據(jù)KD的作用定義KD的模糊規(guī)則,如表4所示。

表4 KD規(guī)則表

根據(jù)偏差E和偏差增量EC模糊化的結(jié)果以及規(guī)則庫推理出比例、積分、微分調(diào)節(jié)系數(shù)變化量ΔKP、ΔKI、ΔKD對應(yīng)的模糊子集。通過三角形隸屬度函數(shù)來確定輸入輸出值在模糊子集的隸屬度,推理出的ΔKP、ΔKI、ΔKD模糊子集通常是一個由模糊變量組成的矩陣,而系統(tǒng)輸入是由E和EC模糊變量組成的向量。

④ 解模糊處理。常見的解模糊的處理方法包括:最大隸屬度法、重心法、加權(quán)平均法等,本設(shè)計采用重心法來進(jìn)行解模糊,每個輸出量表達(dá)式為

(2)

式中:M為隸屬度;F為模糊量化值;n為輸出量級數(shù)個數(shù)。引入權(quán)重系數(shù)α,根據(jù)實際對象微調(diào)KP、KI和KD變化量。

至此,即可得到模糊自適應(yīng)后的PID三個參數(shù)。

3.3.3 模糊控制算法仿真

(1) 平衡機(jī)器人數(shù)學(xué)模型的建立。

使用Simulink軟件對模糊控制算法進(jìn)行驗證,首先搭建模糊控制系統(tǒng)的仿真模型。設(shè)機(jī)器人的質(zhì)量為m=14 kg,高度為L=0.3 m,重力加速度為g=9.8 m/s2,將機(jī)器人置于水平面上,當(dāng)其受到大小為F的外力干擾時,其偏角為θ。

平衡機(jī)器人系統(tǒng)的微分模型表達(dá)式為

(3)

當(dāng)偏角很小時,可以認(rèn)為sin(θ(t))=θ,cos(θ(t))=1,則微分模型可由式(3)化簡為

(4)

在初始時刻對式(4)進(jìn)行拉氏變換,可得:

mLs2θ(s)=mgθ(s)+F(s)

(5)

平衡機(jī)器人的傳遞函數(shù)為

(6)

代入?yún)?shù)可得:

(7)

(2) 模糊控制算法的建立。

通過表2～表4的規(guī)則庫可建立模糊控制算法,其中ΔKP、ΔKI、ΔKD的輸出曲面分別如圖15～圖17所示。

圖15 ΔKP輸出曲面

圖16 ΔKI輸出曲面

圖17 ΔKD輸出曲面

(3) 平衡機(jī)器人仿真系統(tǒng)的實現(xiàn)。

通過MATLAB的Simulink工具箱調(diào)整模糊PID的3個基礎(chǔ)參數(shù)值,最終分別選取為KP=140,KI=0,KD=3。

仿真結(jié)果表明模糊PID控制算法相較于傳統(tǒng)PID算法,具有更快的響應(yīng)速度和更小的超調(diào)量,同時在系統(tǒng)受到外界擾動時,系統(tǒng)的恢復(fù)速度和魯棒性也更好。受到外界擾動時仿真結(jié)果如圖18所示。

圖18 受到外界擾動時仿真結(jié)果

3.3.4 模糊控制算法驗證

根據(jù)仿真結(jié)果設(shè)計模糊PID核心控制代碼。通過實際測試調(diào)節(jié)規(guī)則庫控制邏輯,并根據(jù)實際機(jī)器人模型選擇量化論域,從而實現(xiàn)PID參數(shù)的動態(tài)調(diào)整,提高整體的魯棒性以及適應(yīng)性。模擬通過盲道仿真結(jié)果如圖19所示。

圖19 模擬通過盲道仿真結(jié)果

通過全位移平衡機(jī)器人搭載模糊控制算法進(jìn)行實際測試,在自平衡無外部干擾的情況下,使用虛擬示波器讀取機(jī)器人底盤陀螺儀的實時角度值,如圖20所示。

圖20 機(jī)器人自平衡角度變化值

測試并讀取機(jī)器人在爬升15°斜坡時和通過復(fù)雜起伏盲道地形的數(shù)據(jù),如圖21和圖22所示。

圖22 機(jī)器人通過盲道時角度變化值

測試結(jié)果表明所設(shè)計的機(jī)器人具有較好的平衡性和穩(wěn)定性;在受到有限范圍內(nèi)的外部干擾情況下,機(jī)器人能夠較快地恢復(fù)平衡并保持穩(wěn)定,由此可見使用改進(jìn)的模糊PID控制算法使機(jī)器人在魯棒性和穩(wěn)定性等方面均有良好的表現(xiàn)。全位移平衡機(jī)器人在15°斜坡測試如圖23所示。

4 結(jié)束語

針對目前勘探及救援工作中地形復(fù)雜崎嶇等難點,設(shè)計了一款全位移平衡機(jī)器人。相較于普通機(jī)器人體積大幅縮小,使得平衡機(jī)器人攜帶運輸?shù)雀臃奖?同時相較于傳統(tǒng)的兩輪平衡機(jī)器人運動上所需的空間更小,為機(jī)器人在復(fù)雜場景下的應(yīng)用提供了更多的可能性。平衡機(jī)器人兼具全位移機(jī)器人的靈活性與傳統(tǒng)機(jī)器人的穩(wěn)定性,通過改進(jìn)的模糊控制算法,提升了機(jī)器人的魯棒性與穩(wěn)定性。平衡機(jī)器人的云臺可搭載多種傳感器以及攝像頭,能夠有效地幫助工作人員勘察情況,適應(yīng)各種工作場景,獲取更多未知的信息。