改進(jìn)模糊算法的六足機(jī)器人自主導(dǎo)航研究

趙章焰，張瑞東

（武漢理工大學(xué)物流工程學(xué)院，湖北 武漢 430063）

1 引言

六足機(jī)器人相較于輪式、履帶式等機(jī)器人具有更強(qiáng)的移動(dòng)靈活性和地形適應(yīng)能力，在科研、探險(xiǎn)和勘測(cè)等領(lǐng)域具有較強(qiáng)的發(fā)展?jié)摿Γ灾鲗?dǎo)航是六足機(jī)器人的關(guān)鍵研究方向之一。六足機(jī)器人的自主導(dǎo)航指在未知地形環(huán)境探索一條從初始位置到終點(diǎn)位置的無(wú)碰撞安全路徑。其中避障是六足機(jī)器人自主導(dǎo)航的功能之一，決定了六足機(jī)器人是否可以安全到達(dá)目標(biāo)位置。

六足機(jī)器人較強(qiáng)的地形適應(yīng)能力使得其可以在崎嶇地形移動(dòng)，較強(qiáng)的靈活性使得其能夠針對(duì)不同地形選擇恰當(dāng)?shù)奈蛔撕吐渥泓c(diǎn)[1]。當(dāng)六足機(jī)器人航向?yàn)榭煽缭秸系K物時(shí)可以越障，當(dāng)遇到不可跨越障礙物時(shí)，若不及時(shí)進(jìn)行避障則無(wú)法安全移動(dòng)。因此，對(duì)六足機(jī)器人跨越障礙物、獲取障礙物高度信息能力的研究顯得尤為關(guān)鍵。六足機(jī)器人的運(yùn)動(dòng)本質(zhì)上是六條腿交替組合支撐和擺動(dòng)的往復(fù)過(guò)程[2]。

因此對(duì)單腿足端運(yùn)動(dòng)學(xué)分析尤為重要。基于單腿足端運(yùn)動(dòng)學(xué)分析，求解出足端工作空間，采用復(fù)合擺線作為足端運(yùn)動(dòng)軌跡，由此計(jì)算得出的越障最大高度將作為六足機(jī)器人移動(dòng)方式選擇的重要參考。采用雙目視覺圖像尺寸測(cè)量的方法來(lái)獲取障礙物高度信息，該方法使用雙目相機(jī)拍攝航向障礙物，獲取左右相機(jī)對(duì)應(yīng)的兩張圖片，獲取障礙物頂部和底部匹配點(diǎn)并根據(jù)視差計(jì)算測(cè)量點(diǎn)的空間位置，準(zhǔn)確地測(cè)定被測(cè)點(diǎn)之間的距離，即障礙物的高度信息[3?4]。

針對(duì)于六足機(jī)器人面對(duì)不可逾越障礙物的避障問(wèn)題，模糊控制算法、遺傳算法和神經(jīng)網(wǎng)絡(luò)等諸多智能算法用于解決機(jī)器人避障問(wèn)題。遺傳算法在應(yīng)對(duì)大量數(shù)據(jù)時(shí)，代碼實(shí)現(xiàn)困難，硬件要求高；神經(jīng)網(wǎng)絡(luò)擁有較強(qiáng)的自組織能力但需要精確信息作為輸入。而模糊控制算法使用模糊語(yǔ)言，結(jié)合專家經(jīng)驗(yàn)實(shí)現(xiàn)自主導(dǎo)航，對(duì)運(yùn)算能力要求不高，在機(jī)器人避障領(lǐng)域應(yīng)用較多[5?6]。

因此提出一種基于改進(jìn)模糊算法的控制系統(tǒng)，其相較于傳統(tǒng)機(jī)器人模糊算法控制系統(tǒng)，增設(shè)障礙物高度信息作為算法輸入量，提高了算法對(duì)于六足機(jī)器人的適用性。

V?REP是一種可實(shí)現(xiàn)復(fù)雜場(chǎng)景、多種傳感器和多執(zhí)行器運(yùn)行的3D機(jī)器人仿真軟件[7]，最后利用V?REP仿真軟件建立仿真環(huán)境并對(duì)該改進(jìn)算法進(jìn)行檢驗(yàn)。經(jīng)過(guò)仿真得出，六足機(jī)器人能夠完成無(wú)碰避障，并有效提高了移動(dòng)速度。

2 機(jī)器人的越障能力分析

六足機(jī)器人若要完成從起始點(diǎn)至終點(diǎn)的路徑最優(yōu)解，需要對(duì)障礙物的大小進(jìn)行獲取，進(jìn)而決定越過(guò)障礙物或避開障礙物。為了保障機(jī)器人能夠順利地越過(guò)障礙物，首先需要對(duì)其越障能力進(jìn)行分析，包括其單腿運(yùn)動(dòng)學(xué)分析及越障最大高度的求解，其中越障高度的求解為后續(xù)機(jī)器人路徑選取提供了參考。

2.1 機(jī)器人單腿運(yùn)動(dòng)學(xué)分析

與其他類型機(jī)器人不同，六足機(jī)器人依靠所有支撐腿的關(guān)節(jié)變換完成指定運(yùn)動(dòng)。六足機(jī)器人的越障過(guò)程是六條機(jī)械腿交替完成越障，所以對(duì)單條腿的越障能力進(jìn)行分析即可完成對(duì)整個(gè)機(jī)器人越障能力的評(píng)估。設(shè)定單腿D?H坐標(biāo)系，并求出單腿各部分的空間坐標(biāo)，是完成越障能力評(píng)估的條件。建立六足機(jī)器人單腿D?H 坐標(biāo)系，Ob-XbYbZb為軀體中心坐標(biāo)系，O0-X0Y0Z0、O1-X1Y1Z1、O2-X2Y2Z2、O3-X3Y3Z3分別為各關(guān)節(jié)和足端運(yùn)用D?H方法設(shè)定的空間坐標(biāo)系[2]，其中Zb和Zo同向，如圖1所示。

圖1 單腿坐標(biāo)系Fig.1 Single Leg Coordinate System

六足機(jī)器人的結(jié)構(gòu)參數(shù)，如表1所示。結(jié)合式（1）足端軌跡方程，以足端坐標(biāo)為坐標(biāo)系參數(shù)，求得足端工作空間（單腿足端相對(duì)于原點(diǎn)O0可到達(dá)的位置），坐標(biāo)O-XYZ與O0-X0Y0Z0為同一坐標(biāo)系，如圖2所示。六足機(jī)器人的足端移動(dòng)范圍決定了其能否跨越指定障礙物。對(duì)六足機(jī)器人單腿位置運(yùn)動(dòng)學(xué)分析為了解機(jī)器人越障能力和進(jìn)行足端越障軌跡規(guī)劃提供了理論支撐[2?9]。

表1 單腿結(jié)構(gòu)參數(shù)Tab.1 Single Leg Structural Parameters

圖2 足端坐標(biāo)空間Fig.2 Foot Coordinate Space

2.2 單腿足端軌跡規(guī)劃

六足機(jī)器人變換三種腿部運(yùn)動(dòng)參數(shù)仰起高度、跨越距離及跨越時(shí)間來(lái)調(diào)換移動(dòng)形式，而足端軌跡決定了腿部運(yùn)動(dòng)參數(shù)，進(jìn)而影響越障能力。在越障的情況下，足端按照一定的運(yùn)動(dòng)軌跡逾越縱向距離h的障礙物完成擺動(dòng)相，接著足端穩(wěn)定支撐軀體，完成一個(gè)周期運(yùn)動(dòng)后又回到軀體的相對(duì)原始位置。

足末端運(yùn)動(dòng)曲線決定了能否順利越過(guò)越障礙物，因此首先規(guī)劃運(yùn)動(dòng)曲線。選取預(yù)設(shè)曲線作為足端運(yùn)動(dòng)軌跡是常用的方法，如擺線、拋物線、正弦線等，需要完成：①各關(guān)節(jié)及足末端軌跡平滑；②處于前后極限處，各關(guān)節(jié)及足末端速度和加速度是零；③足末端的加速度及速度在移動(dòng)中連續(xù)無(wú)突變。由單腿跨越障礙物及整機(jī)穩(wěn)定性的要求，選取基于擺線的運(yùn)動(dòng)方式規(guī)劃足末端越障軌跡。以足端開始位置原點(diǎn)O建立運(yùn)動(dòng)軌跡坐標(biāo)系O-XYZ，使用X（t），Y（t），Z（t），分別代表在行進(jìn)方向、橫向和縱向，足端坐標(biāo)關(guān)于時(shí)間的函數(shù)，則足端軌跡方程為：

經(jīng)求得，最大步高Hmax的值約為181.86mm，設(shè)定支撐寬度為180mm，擺動(dòng)相周期為4s，步長(zhǎng)為150mm。求出在擺動(dòng)相內(nèi)的足端軌跡，如圖3所示。

圖3 足端軌跡曲線Fig.3 Foot Trajectory Curve

六足機(jī)器人需要逾越障礙物的尺寸大小只需在軌跡曲線與X軸所圍成的范圍內(nèi)即可完成跨越。在后續(xù)仿真實(shí)驗(yàn)中，選定在X向障礙物寬度為100mm，由足端軌跡曲線可得出，六足機(jī)器人能夠逾越100mm寬度、縱向高度156.78mm障礙物。

3 六足機(jī)器人避障模型

3.1 環(huán)境和位置信息的獲取

為實(shí)現(xiàn)六足機(jī)器人避障功能，機(jī)器人需要具備感知環(huán)境信息和位置信息的能力。距離信息采集選擇超聲波扇形掃描測(cè)距法，在轉(zhuǎn)動(dòng)平臺(tái)放置單個(gè)超聲波傳感器，控制舵機(jī)完成大角度轉(zhuǎn)動(dòng)測(cè)距［10］。雙目攝像機(jī)可得到環(huán)境三維數(shù)據(jù)并完成地形構(gòu)建，完成對(duì)障礙物尺寸的識(shí)別。

超聲波傳感器在六足機(jī)器人前端，每次旋轉(zhuǎn)30°，測(cè)量7個(gè)方位的距離值，對(duì)所有測(cè)量方位編號(hào)為1～7，如圖4所示。對(duì)六足機(jī)器人使用GPS獲得實(shí)時(shí)位置及目標(biāo)位置信息，使用電子羅盤實(shí)時(shí)測(cè)量航向角信息。

圖4 距離測(cè)量方位Fig.4 Distance Detect Locations

3.2 六足機(jī)器人避障分析

六足機(jī)器人在坐標(biāo)系中避開障礙物，并保持向最終位置移動(dòng)的方位，如圖5所示。六足機(jī)器人在t?1時(shí)刻的位置為（X（t?1），Y（t?1）），目標(biāo)點(diǎn)坐標(biāo)為（Xg，Yg），t時(shí)刻所處的坐標(biāo)定為（X（t），Y（t）），t時(shí)刻坐標(biāo)同目標(biāo)坐標(biāo)的夾角由θg（t）表示［10］：

圖5 機(jī)器人避障模型Fig.5 Robot Obstacle Avoidance Model

（t?1）時(shí)刻六足機(jī)器人，航向角設(shè)定θ（t?1），則t時(shí)航向角是θ（t）。t時(shí)刻機(jī)器人目標(biāo)夾角T（t）為：

3.3 避障安全性分析

采用虛擬包絡(luò)機(jī)體的安全分析方法，利用傳感器獲取的環(huán)境距離信息，計(jì)算得出最大許可避障航向角，完成安全性能的評(píng)定。六足機(jī)器人傳感器探測(cè)的方向?yàn)槠邆€(gè)均等分布方向，為了使得機(jī)器人獲得前進(jìn)方向更準(zhǔn)確的環(huán)境信息，機(jī)器人在D4上探測(cè)方向與前進(jìn)方向一致。

由于六足機(jī)器人的對(duì)稱結(jié)構(gòu)決定了可使用范圍包絡(luò)的方法限制機(jī)器人的對(duì)大邊緣范圍，防止與障礙物發(fā)生碰撞。安全性能評(píng)定可分為如下過(guò)程：

（1）基于機(jī)器人運(yùn)動(dòng)學(xué)分析，以軀體重心為圓心，最大足端距離為半徑r，并設(shè)置安全范圍ε，即圓形包絡(luò)邊界的半徑為r+ε，建立虛擬包絡(luò)機(jī)體模型，如圖6所示。

（2）比較七個(gè)位置的距離數(shù)據(jù)，D5、D6是測(cè)量得出的較小數(shù)值。建立如圖所示的障礙物邊緣E點(diǎn)與虛擬機(jī)體模型的切線，并做圓心O與障礙物的垂線，在最短路徑下，機(jī)器人的航向角為θt，如圖6所示。可求得

圖6 虛擬包絡(luò)機(jī)體模型Fig.6 Virtual Envelope Body Model

采用此方法可以避免六足機(jī)器人與障礙物發(fā)生干涉，在避障過(guò)程中可以根據(jù)測(cè)得的距離信息實(shí)時(shí)調(diào)節(jié)避障的航向角θ，完成機(jī)器人的安全避障。

4 模糊控制系統(tǒng)的設(shè)計(jì)

六足機(jī)器人具備靈活的單腿構(gòu)造和多樣的運(yùn)動(dòng)形式，因而相較于諸如輪式和履帶式機(jī)器人擁有更加優(yōu)越的在不平坦地面的運(yùn)動(dòng)效能。不同于其余移動(dòng)機(jī)器人的避障策略，本文采用的模糊控制策略充分考慮了六足機(jī)器人優(yōu)越的越障性能。模糊邏輯控制系統(tǒng)由控制量、控制規(guī)則、模糊化和清晰化、模糊推理構(gòu)成，屬于智能控制策略，能有效解決復(fù)雜且難以精確表達(dá)的系統(tǒng)[5]。因此，結(jié)合越障性能的模糊控制理論更適用于六足機(jī)器人的移動(dòng)控制。

4.1 變量及模糊化的描述

超聲波傳感器可以探測(cè)環(huán)境的距離信息，位置和航向角信息可由GPS和電子羅盤測(cè)得，障礙物的高度尺寸可由雙目攝像機(jī)測(cè)得。所以，可取六足機(jī)器人七個(gè)傳感器探測(cè)的距離數(shù)據(jù)、航向角夾角信息及障礙物高度作為輸入變量，將行進(jìn)速度及轉(zhuǎn)向角作為輸出變量。

如上所述，測(cè)量七個(gè)方向的距離數(shù)據(jù)。為簡(jiǎn)化輸入變量數(shù)量，對(duì)七個(gè)方向的測(cè)定數(shù)值進(jìn)行分組，分別確定D1、D2是右向環(huán)境距離數(shù)值，D3、D4、D5為前向環(huán)境距離數(shù)值，D6和D7為左向環(huán)境距離數(shù)值，將三組距離信息進(jìn)行比較并將數(shù)值最小的一組數(shù)據(jù)當(dāng)做機(jī)器人當(dāng)前位姿的環(huán)境距離數(shù)值，由此可知，該改進(jìn)模糊控制系統(tǒng)為5 輸入∕2 輸出。得出改進(jìn)的模糊控制系統(tǒng)框圖，如圖7所示。

圖7 模糊控制系統(tǒng)Fig.7 Fuzzy Control System

對(duì)于輸入變量，設(shè)定左向、前向、右向距離分別為L(zhǎng)D、FD、RD，取值范圍［0，3mm］，模糊語(yǔ)言集合｛近，中，遠(yuǎn)｝，相應(yīng)記做｛N，M，F(xiàn)｝；機(jī)器人輸入角度T論域取［?180o，180o］，詞集取｛偏左大，偏左小，不偏，偏右小，偏右大｝，對(duì)應(yīng)記為｛LB，LM，ZO，RM，RB｝；前方障礙物高度H模糊分為兩個(gè)等級(jí)，分別代｛BH，LH｝表障礙物高度信息｛偏高，偏低｝。對(duì)于輸出變量，令移動(dòng)速度分別設(shè)為FV快速、MV中速、SV慢速，論域取［0cm∕s，20cm∕s］；令行走轉(zhuǎn)向角記為M，分為5個(gè)等級(jí)：LLT（向左大幅度轉(zhuǎn)彎）、LT（向左微轉(zhuǎn)）、FT（直行）、RT（向右微轉(zhuǎn)）和RRT（向右大幅度轉(zhuǎn)彎），論域取為［?180o，180o］。

4.2 輸入輸出隸屬度函數(shù)

由于高斯型隸屬函數(shù)為平滑曲線，使用它可使系統(tǒng)有更高的精度，將該函數(shù)應(yīng)用于輸入變量中的左向、前向、右向距離LD、FD、RD，輸入角度T及輸出變量中的移動(dòng)速度和轉(zhuǎn)向角。對(duì)于輸入變量中的障礙物高度H，由于前述分析超過(guò)156.78mm即可視為不可越過(guò)障礙物，取155mm為臨界值，低于臨界值時(shí)和高于臨界值時(shí)分別采用高斯型和梯形隸屬度函數(shù)。得出輸入輸出隸屬函數(shù)，如圖8所示。

圖8 輸入、輸出的隸屬度函數(shù)Fig.8 Membership Function of Input and Output

4.3 模糊控制規(guī)則

模糊控制規(guī)則是一種知識(shí)庫(kù)集合，其正確與否直接影響移動(dòng)過(guò)程的安全性，在設(shè)計(jì)時(shí)，使用專家經(jīng)驗(yàn)編寫并融入控制規(guī)則中。

六足機(jī)器人的路徑規(guī)劃如下：六足機(jī)器人從起點(diǎn)坐標(biāo)向目標(biāo)坐標(biāo)移動(dòng)，當(dāng)距離傳感器探測(cè)到障礙物，對(duì)三組距離信息進(jìn)行比較。當(dāng)行進(jìn)方位距離信息較大時(shí)，繼續(xù)保持當(dāng)前移動(dòng)方位；當(dāng)前進(jìn)方向的障礙物高度低于臨界值時(shí)，機(jī)器人利用其越障能力進(jìn)行越障；當(dāng)障礙物高度高于臨界值時(shí)，選擇避障方向，當(dāng)左側(cè)距離信息數(shù)值較小，向右轉(zhuǎn)向，當(dāng)右側(cè)距離信息數(shù)值較小，向左轉(zhuǎn)向。

控制規(guī)則使用更為常用的“IF條件，THEN結(jié)果”的樣式，總的控制規(guī)則數(shù)為N=n1n2…nm，其中，m為輸入變量數(shù)量，n1，n2，…，nm是各輸入變量的模糊詞集數(shù)。因此，可得出N=3×3×3×5×2=270條模糊控制規(guī)則，其表述形式是

IF（FDisFDiandRDisRDjandLDisLDkandTisTlandHisHh）THEN（VisVmandMisMn）

在表達(dá)式中，F(xiàn)Di、RDj、LDk、Tl、Hh、Vm、Mn分別為定義在FD、RD、LD、T、H、V、M上的模糊集。其中部分模糊控制規(guī)則，如表2所示。

表2 模糊控制規(guī)則庫(kù)Tab.2 Fuzzy Control Rule Base

由預(yù)設(shè)的規(guī)則庫(kù)為依據(jù)進(jìn)行模糊推理來(lái)求出變量的關(guān)聯(lián)方程。Mamdani型、Larsen型、Zadeh型為常用的方法，三種方法中使用Mamdani取小運(yùn)算。

4.4 清晰化

六足機(jī)器人的執(zhí)行機(jī)構(gòu)要求的輸入是精確值，但得出的輸出變量是一個(gè)模糊量[11]。為了將其進(jìn)行清晰化計(jì)算，采用加權(quán)平均法進(jìn)行清晰化處理，式中xiuU1（xi）（i=1，2，…，n）為集合U1的各元素xi（i=1，2，…，n）與相應(yīng)隸屬度uU1（xi）相乘，即：

5 仿真試驗(yàn)及分析

V?REP能夠搭建出空間三維仿真界面，并且擁有多種環(huán)境搭建模塊，可以運(yùn)用自帶的模型，搭建出機(jī)器人導(dǎo)航運(yùn)動(dòng)中的外部障礙物環(huán)境。將傳感器模塊融合到機(jī)器人中，來(lái)監(jiān)測(cè)其運(yùn)動(dòng)情況，并根據(jù)輸出的狀態(tài)信息及時(shí)調(diào)整控制程序，完成機(jī)器人導(dǎo)航算法的開發(fā)[7]。

運(yùn)用V?REP 進(jìn)行機(jī)器人仿真，在操作環(huán)境中設(shè)置了一個(gè)10×10的柵格地圖并放置諸多尺寸不同的障礙物作為模擬地面環(huán)境信息。設(shè)機(jī)器人的起點(diǎn)和終點(diǎn)在柵格地圖中的坐標(biāo)分別為（?2.5m，2.5m）和（2.5m，2.5m），并將機(jī)器人的初始和目標(biāo)坐標(biāo)信息預(yù)存于控制程序中。在V?REP中的仿真過(guò)程，如圖9所示。

圖9 仿真結(jié)果Fig.9 Simulation Result

機(jī)器人判斷障礙物可跨越，如圖9（a）所示；機(jī)器人跨越障礙物過(guò)程，如圖9（b）所示；在六足機(jī)器人后續(xù)移動(dòng)過(guò)程中，障礙物高度較高，采用避障的方式移動(dòng)，如圖9（c）所示；機(jī)器人最終到達(dá)目的地，如圖9（d）所示。截取部分六足機(jī)器人足端在行進(jìn)方向和縱向的速度及加速度變化過(guò)程，如圖10所示。可知速度加速度曲線平滑無(wú)突變，六足機(jī)器人在移動(dòng)進(jìn)程下未與障礙物產(chǎn)生干涉。

圖10 速度及加速度結(jié)果Fig.10 Velocity and Acceleration Results

在V?REP 仿真環(huán)境中，如圖11 所示。其中傳統(tǒng)模糊避障算法對(duì)應(yīng)一條曲線路徑，改進(jìn)模糊避障算法對(duì)應(yīng)另一條曲線路徑。傳統(tǒng)模糊避障算法對(duì)于任何障礙物都采用避障的方式，增大了行進(jìn)距離。

圖11 機(jī)器人路徑Fig.11 Robot Path

相較于傳統(tǒng)避障算法，改進(jìn)后的模糊避障算法充分考慮了由六足機(jī)器人自身特點(diǎn)決定的越障能力，在仿真環(huán)境中仿真時(shí)間相對(duì)于傳統(tǒng)算法縮短11.457s，且由于前述對(duì)于機(jī)器人越障高度的分析，移動(dòng)進(jìn)程中并未與障礙物產(chǎn)生接觸。

6 結(jié)論

針對(duì)六足機(jī)器人在復(fù)雜情況下的自主導(dǎo)航問(wèn)題，設(shè)計(jì)出基于改進(jìn)模糊算法的控制系統(tǒng)。硬件設(shè)計(jì)上采用雙目相機(jī)獲取障礙物高度信息、單個(gè)超聲波傳感器轉(zhuǎn)動(dòng)測(cè)距、GPS和電子羅盤獲取位置信息的方式來(lái)獲取環(huán)境地形信息。改進(jìn)后的模糊控制算法創(chuàng)新性地融合了六足機(jī)器人優(yōu)越的越障能力，依據(jù)獲取的環(huán)境地形信息，選擇預(yù)設(shè)的避障方法，實(shí)現(xiàn)自主導(dǎo)航。

通過(guò)V?REP機(jī)器人仿真軟件對(duì)導(dǎo)航運(yùn)動(dòng)實(shí)行仿真，證實(shí)了該模糊控制算法能夠使得機(jī)器人實(shí)時(shí)、準(zhǔn)確地完成自主導(dǎo)航，相對(duì)于未結(jié)合越障能力的傳統(tǒng)避障算法，運(yùn)行時(shí)間顯著減少，驗(yàn)證了該算法的可行性和實(shí)用性。