劉 恒, 李 哲, 楊明輝, 鄧曉剛, 曹玉蘋(píng)

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

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

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

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

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

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

1.1 底盤(pán)設(shè)計(jì)

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

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

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

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

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

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

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

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

1.2 云臺(tái)設(shè)計(jì)

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

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

1.3 仿真測(cè)試

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

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

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

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

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

2.1 電源模塊設(shè)計(jì)

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

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

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

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

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

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

圖7 ICM20948電路原理圖

圖8 CAN通信差分電路

圖9 STM32F405主控設(shè)計(jì)

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

3.1 主程序邏輯框架

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

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

圖11 主體程序流程圖

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

3.2 云臺(tái)算法設(shè)計(jì)

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

圖12 云臺(tái)控制邏輯框圖

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

圖13 云臺(tái)Pitch軸串級(jí)控制框圖

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

3.3 底盤(pán)算法設(shè)計(jì)

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

3.3.1 底盤(pán)整體控制邏輯設(shè)計(jì)

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

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

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

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

(1)

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

③ 建立模糊規(guī)則表。將偏差E以及偏差增量EC劃分為7個(gè)模糊子集:負(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值過(guò)大會(huì)產(chǎn)生較大的超調(diào),甚至使系統(tǒng)不穩(wěn)定,減小KP可以減小超調(diào),提高穩(wěn)定性,但KP過(guò)小會(huì)減慢響應(yīng)速度,延長(zhǎng)調(diào)節(jié)時(shí)間。因此,調(diào)節(jié)初期應(yīng)適當(dāng)取較大的KP值以提高響應(yīng)速度,而在調(diào)節(jié)中期,KP則取較小值,以使系統(tǒng)具有較小的超調(diào)并保證一定的響應(yīng)速度;而在調(diào)節(jié)過(guò)程后期再將KP值調(diào)到較大值來(lái)減小靜差,提高控制精度?；谏鲜雒枋龆xKP的模糊規(guī)則,如表2所示。

表2 KP規(guī)則表

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

表3 KI規(guī)則表

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

表4 KD規(guī)則表

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

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

(2)

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

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

3.3.3 模糊控制算法仿真

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

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

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

(3)

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

(4)

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

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

(5)

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

(6)

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

(7)

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

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

圖15 ΔKP輸出曲面

圖16 ΔKI輸出曲面

圖17 ΔKD輸出曲面

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

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

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

圖18 受到外界擾動(dòng)時(shí)仿真結(jié)果

3.3.4 模糊控制算法驗(yàn)證

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

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

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

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

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

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

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

4 結(jié)束語(yǔ)

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