劉宇飛，丁 亮，高海波，劉 振，胡艷明，何玉慶，鄧宗全

(1. 哈爾濱工業(yè)大學(xué)機器人技術(shù)與系統(tǒng)國家重點實驗室，哈爾濱 150080； 2. 中國科學(xué)院沈陽自動化研究所機器人學(xué)重點實驗室，沈陽 110016)

0 引 言

足式機器人可以自主行走在崎嶇地形下，在月球探測[1]、搶險救災(zāi)[2]等領(lǐng)域內(nèi)都體現(xiàn)了腿式機器人的優(yōu)勢。在多任務(wù)環(huán)境下，足式機器人的自主導(dǎo)航和避障是移動機器人研究中非常重要的部分[3-4]。AMBLER[5]是由美國卡內(nèi)基梅隆大學(xué)與噴氣推進(jìn)實驗室共同研制的用于星球探測的六足移動機器人，機器人可以在無人幫助的環(huán)境下通過崎嶇地形。NASA-JPL研制的ATHLETE機器人被譽為最先進(jìn)的六足機器人[6-7]，其用途是在月球表面運送人員和裝備，具有很強的地形適應(yīng)能力。

足式機器人在一個結(jié)構(gòu)地形下，可以通過本體傳感器對地形做一個初步估計并指導(dǎo)足端落足點選擇；當(dāng)機器人遇到小障礙物時，可以選擇踏過去，但當(dāng)機器人面臨一個極度崎嶇或不可逾越的障礙物時，若不借助視覺信息是無法順利通過的。在移動機器人實際工作中，自主的躲避障礙物是非常必要的任務(wù)之一，這涉及地形識別、步態(tài)、路徑規(guī)劃以及落足點選擇等任務(wù)。

在未知環(huán)境中辨別障礙物是避障運動的前提，一般大部分機器人利用相機信息[8]或激光測距傳感器數(shù)據(jù)[9]建立地形高程圖，利用柵格表示環(huán)境模型起源于美國CMU大學(xué)，Borenstein曾采用柵格表示環(huán)境，提出矢量場矩形法控制機器人行走方向[10]。激光測距儀掃描精度較高[11]，一種基于激光測距儀的自適應(yīng)調(diào)節(jié)閾值求可行方向的方法被提出[12]，此方法根據(jù)移動機器人的行駛方向與參考方向的偏差值作為對機器人前進(jìn)方向線速度的限制，并在ASR機器人上進(jìn)行了實驗驗證，使機器人可以安全平滑地通過障礙區(qū)域。針對單一視覺傳感器無法實現(xiàn)復(fù)雜空間作業(yè)問題，文獻(xiàn)[13]提出基于單目相機與激光雷達(dá)融合的位姿估計方法，實現(xiàn)了尺度模糊下的位姿快速估計。本文采用激光測距儀獲取地形信息，并基于障礙物與機體之間的距離信息得到最優(yōu)可行方向和最短運動距離。

足式機器人的運動是一個復(fù)雜的軌跡規(guī)劃過程，當(dāng)機器人直線行走的時候，可以在考慮能耗和流量的前提下去規(guī)劃足端軌跡[14]。根據(jù)機器人本體傳感器和視覺信息，Belter[15]采用A*規(guī)劃算法去實現(xiàn)基于地形高程圖的機器人運動規(guī)劃研究，基于地形引導(dǎo)的RRT算法找到了精確的可行方向，通過辨識出地形的可通過性指標(biāo)，完成了機器人的自主避障運動。文獻(xiàn)[16]針對自由漂浮空間機器人，提出一種FFSR的避障規(guī)劃，實現(xiàn)了FFSR的避障控制并保證其環(huán)境適應(yīng)性。針對月球車的避障技術(shù)，文獻(xiàn)[17]提出了虛擬主體避障技術(shù)，實現(xiàn)了月球車在障礙圖中的避障學(xué)習(xí)與控制。避障運動過程中，本文采用了環(huán)境中靜止的物體作為障礙物，且障礙物的高度高于機器人本體，即視為不可逾越的物體。

目前，在足式機器人中，針對自主避障運動的研究和實驗驗證較少。本文以電驅(qū)動六邊形對稱分布的六足機器人為研究對象，模擬月壤崎嶇地面，提出了基于虛擬機體模型的自主避障策略，得到了機器人偏航運動過程的擺動相和支撐相軌跡，通過實驗證實了六足機器人可以自主的通過障礙地形。

1 地形建模

為了實現(xiàn)機器人安全的在未知復(fù)雜環(huán)境下自主行走，需要機器人預(yù)先檢測地形并建模，從而幫助機器人避開行走軌跡上的障礙物。針對腿部對稱分布的六足機器人，采用基于機體質(zhì)心的地圖建模方法。在機器人行走過程中，根據(jù)檢測得到的地形實時距離和高度信息，生成更新的地形高程圖。通過機器人本體上的激光傳感器可以準(zhǔn)確得到實時更新的機體與障礙物之間的距離，根據(jù)障礙物的形狀和距離，決定機器人下一步的運動狀態(tài)。

激光測距傳感器可以得到地形環(huán)境下的點云信息，通過對點云信息的分析和處理，區(qū)分平坦地形和障礙地形。下一步機器人根據(jù)障礙的距離和高度，在運動過程中的安全裕度范圍內(nèi)，在大地坐標(biāo)系下沿著安全路徑自主行走。為更準(zhǔn)確的描述地形信息，建立了基于地形建模的4個坐標(biāo)系系統(tǒng)，如圖1所示。分別為大地坐標(biāo)系OW，本體坐標(biāo)系OB，傳感器坐標(biāo)系OV和腿部坐標(biāo)系OL。其中障礙物高為h，寬度為w。

相對于大地坐標(biāo)系，機體坐標(biāo)系的坐標(biāo)為BP(BPX,BPY,BPZ)，腿部坐標(biāo)系為LiP(LiPX,LiPY,LiPZ)，傳感坐標(biāo)系為VP(VPX,VPY,VPZ)。為了建立基于機體質(zhì)心的地形信息，將激光測距傳感器信息轉(zhuǎn)換到機體坐標(biāo)系下：

(1)

(2)

由于在高度測量過程中，物體在兩個坐標(biāo)系統(tǒng)下的橫縱坐標(biāo)不變，因此矩陣P=[0 0 1]。高度測量的標(biāo)準(zhǔn)差如下所示：

(3)

其中ΣV代表激光傳感器模型的協(xié)方差矩陣，ΣP,q是傳感器坐標(biāo)系旋轉(zhuǎn)的協(xié)方差矩陣。JV是傳感器測量的雅克比矩陣，Jq是傳感器坐標(biāo)旋轉(zhuǎn)的雅克比矩陣。

針對地形高程圖的云圖更新，采用擴展卡爾曼濾波的方式對更新數(shù)據(jù)進(jìn)行數(shù)據(jù)融合[19]。擴展卡爾曼濾波方程中的狀態(tài)變量等同于地圖信息中的高度信息。擴展卡爾曼濾波更新方程如方程(4)-(6)所示。

(4)

(5)

(6)

(7)

(8)

通過這種方式，可以更新實時的地形高度信息和高度的標(biāo)準(zhǔn)差，從而得到實時的地形高程圖。圖2是通過此方法得到的地形信息測試。

2 障礙檢測及避障算法

基于地形建模得到的高程圖，可以通過相鄰網(wǎng)格判斷其是否為障礙物。需要通過障礙檢測方法得到障礙物在大地坐標(biāo)系下的位置。下一步根據(jù)障礙物和機器人之間的位置關(guān)系，實現(xiàn)機器人自主避障。

2.1 障礙檢測

(9)

其中柵格的高度信息表示為h[p,q]，代表了部分地形的平均高度。

(10)

其中k屬于柵格中點云的數(shù)量，利用h[p,q]去評價地形的平坦程度，判斷是否是障礙物。

(11)

2.2 避障算法

地圖上的每個柵格存儲了地圖上每個點的高度值，首先對地圖上的整個區(qū)域進(jìn)行評價，得到平均高度最低區(qū)域，即為可通過性最好區(qū)域。本文提出一種基于虛擬機體模型的自主避障的方法，利用激光測距儀獲得的地形信息得到地形通過性最好區(qū)域，最后計算得到最優(yōu)的可行方向，并實現(xiàn)運動距離最短完成避障運動。由于六足機器人是六邊形對稱結(jié)構(gòu)，因此在任意方向都可以當(dāng)作前進(jìn)方向，并不需要做轉(zhuǎn)向運動，可以沿著給定的方向?qū)崿F(xiàn)全向運動?；诒倔w傳感器得到機器人本體及各腿的位置信息，建立虛擬機體模型，通過虛擬機體模型可以最大限度的包絡(luò)整個機器人的運動范圍，防止機器人和障礙物相碰。通過虛擬機體和障礙物的幾何模型，可以實現(xiàn)高效簡便的計算得到機器人與障礙物的距離及最優(yōu)的運動可行方向。機器人的避障運動可以分為以下幾個步驟：

第一步：基于本體傳感器信息，建立一個以機體質(zhì)心為圓心，機體質(zhì)心到足末端位置距離為半徑的虛擬機體幾何模型，如圖3所示。

第二步：以虛擬機體模型構(gòu)成的圓形區(qū)域作為每一步的采樣評價區(qū)域，其最大包絡(luò)尺寸為以機器人原點為圓心，半徑為1430 mm的圓形區(qū)域。在機器人行走避障過程中實時更新激光掃描獲得的柵格信息，對機器人前進(jìn)方向的采樣區(qū)域進(jìn)行評價。根據(jù)激光測距儀得到地圖上每個柵格點的高度值，對采樣點評價區(qū)域內(nèi)的所有柵格點做加和取平均值，得到平均高度最低區(qū)域，即為可行方向最優(yōu)。計算得到機器人質(zhì)心到障礙物的垂直距離dOV和質(zhì)心到最優(yōu)方向點T的距離dOT，根據(jù)距離dOT和機器人虛擬模型的包絡(luò)范圍可以確定機器人的偏航方向，并保證運動距離最短，實現(xiàn)最快速的避障運動。

第三步：根據(jù)建立的虛擬圓幾何模型，得到圓切點M距離障礙邊界點N的連線，并計算得到機器人的避障偏航角φ。

(12)

其中dOT可以根據(jù)本體傳感器及激光傳感器的測量值計算得到。

以此方法可以保證機器人沿著最短距離達(dá)到目標(biāo)位置O′，當(dāng)機器人在運動過程中，根據(jù)激光測距信息實時調(diào)節(jié)避障偏航角φ，從而實現(xiàn)最快速度的避障運動。

在避障過程中保證機器人的運動直徑l′

3 基于自主避障的運動規(guī)劃

機器人的運動是根據(jù)預(yù)先設(shè)定好的軌跡實現(xiàn)的，當(dāng)有激光測距傳感器反饋時，機器人按照給定的方向角和步長實現(xiàn)運動，完成自主避障運動，因此機體運動規(guī)劃是實現(xiàn)運動的重要一環(huán)。根據(jù)機器人本體的運動規(guī)劃，得到機器人支撐腿的運動軌跡，從而規(guī)劃出各腿擺動相的落足點，最終實現(xiàn)足端支撐相和擺動相的軌跡規(guī)劃。

3.1 六足機器人機身運動規(guī)劃

機器人機體運動軌跡的時間函數(shù)由六個變量組成，選取機器人質(zhì)心在大地坐標(biāo)系平面內(nèi)的行進(jìn)速度，機體支撐高度，轉(zhuǎn)彎半徑和三個姿態(tài)角的時間函數(shù)。記為：(vB(t),hB(t),rB(t),α(t),β(t),γ(t))T。機器人的行進(jìn)速度，支撐高度，俯仰角和滾轉(zhuǎn)角都可以直接線性給出，轉(zhuǎn)彎半徑和偏航角根據(jù)激光測距儀檢測得到的信息實時給定，從而達(dá)到最優(yōu)的偏航軌跡。機體的偏航角度的變化與轉(zhuǎn)彎半徑有關(guān)，根據(jù)圖5所示，當(dāng)機器人沿著轉(zhuǎn)彎半徑進(jìn)行規(guī)劃時，其轉(zhuǎn)向半徑可以表示為：

rB=S/(2sin(rwπ/4))

(13)

其中，S為機體質(zhì)心距運動速度點的距離，rw為轉(zhuǎn)向參數(shù)，在避障運動過程中給定。

當(dāng)機器人保持直線勻速運動過程中，機體質(zhì)心運動速度可以通過多項式方程擬合得到：

(14)

(15)

其中，λ是足端運動步長。根據(jù)機體質(zhì)心距運動速度點的距離和前向運動速度vB，可以得到機體的偏航速度為：

vp=vB/(Ssec(rwπ/4))

(16)

當(dāng)?shù)玫綑C器人本體的運動軌跡和速度之后，可以根據(jù)機體質(zhì)心的運動實現(xiàn)對足端軌跡的實時規(guī)劃。

3.2 六足機器人足端軌跡規(guī)劃

由于足式機器人的移動是通過各腿足的交替運動實現(xiàn)，基于規(guī)劃好的機器人本體運動，將機體規(guī)劃轉(zhuǎn)化為各腿有序的行走。采用二步態(tài)的步序，如圖6所示，將六足腿分成兩組，實現(xiàn)機器人最快速度的行走及避障運動。

足端軌跡規(guī)劃過程中需要先對足端軌跡進(jìn)行規(guī)劃，后通過逆運動學(xué)轉(zhuǎn)換為各關(guān)節(jié)的協(xié)調(diào)運動。將足端運動規(guī)劃分為支撐相和擺動相軌跡。

(17)

根據(jù)足端目標(biāo)位置和機體期望的偏航速度，基于前進(jìn)方向的目標(biāo)位置，速度和加速度，對足端擺動相軌跡進(jìn)行規(guī)劃：

(18)

其中矩陣A可以被寫為：

(19)

通過參數(shù)辨識，得到避障運動過程中多項式中的各參數(shù)：

(20)

根據(jù)機器人的偏航速度，得到足端的支撐相軌跡方程為：

(21)

根據(jù)機器人質(zhì)心的運動速度和實時改變的運動方向，通過多項式擬合的方式，實現(xiàn)了機器人的擺動相和支撐相軌跡。通過機器人逆運動學(xué)方程得到跟關(guān)節(jié)角α，髖關(guān)節(jié)角β和膝關(guān)節(jié)角γ：

(22)

(23)

(24)

表1 六足機器人參數(shù)Table 1 Parameters of hexapod robot

通過計算得到各腿關(guān)節(jié)角之后，通過控制器實現(xiàn)對規(guī)劃的足端速度和位置的跟蹤，實現(xiàn)機器人自主避障運動。

4 試驗驗證

通過提出的避障策略和運動規(guī)劃方法，可以讓機器人在未知的環(huán)境下找出一條安全的路徑。為驗證所提出方法的可行性，在ElSpider機器人上對控制策略進(jìn)行了試驗驗證，其六足機器人系統(tǒng)的實現(xiàn)如圖7所示。

六足機器人的試驗系統(tǒng)包括內(nèi)部和外部傳感器系統(tǒng)，其內(nèi)部傳感器包括運動控制器，驅(qū)動器，電機；外部傳感器包括足底六維力傳感器，安裝在本體上的姿態(tài)傳感器IMU，以及激光測距傳感器Velodyne，其采樣頻率達(dá)到10 Hz。六足機器人傳感器信息見表2所示。

表2 六足機器人傳感設(shè)備信息Table 2 Equipments of the ElSpider robot

為最大限度保證激光雷達(dá)的掃描范圍，將激光雷達(dá)傾斜10°安裝在機器人本體上，以便可以最大限度看到前面落足位置并掃描到前方物體，如圖7所示。

通過激光測距傳感器信息，設(shè)定一個長3 m×寬8 m的地形圖，地形圖上每一個柵格尺寸均為0.3 m的正方形。

在機器人避障運動過程中，通過避障策略得到最優(yōu)航向角度，通過UDP通信方式將實時角度發(fā)送給機器人運動規(guī)劃。為了驗證機器人可以正確、實時響應(yīng)最優(yōu)航向角度，通過機器人本體傳感器測量機體的偏航角度并與激光測距信息得到的偏航角度對比，如圖8所示。

試驗曲線中的紅色實線是根據(jù)3.2節(jié)中通過激光測距傳感器得到的最優(yōu)可行偏航角度信息；藍(lán)色星線代表通過本體傳感器計算得到的機體實時偏航角度，通過對比發(fā)現(xiàn)機器人可以實時跟蹤激光傳感器給定的最優(yōu)偏航角度，但是由于六足機器人的足端運動是非連續(xù)運動，在運動過程中會有一定的滯后，導(dǎo)致無法完全跟蹤偏航角度。

從曲線中可以看出在開始時機器人距離障礙物很近，因此通過計算得出機器人的偏航角度為66.8°，當(dāng)機器人向偏航方向運動時，偏航角度逐漸變小，到272 s時，機器人完全避開了障礙物，開始朝向前進(jìn)方向運動，因此偏航角度變?yōu)?°。

通過本體傳感器數(shù)據(jù)采集得到機器人各足的足端位置曲線，其足端軌跡曲線是基于機體坐標(biāo)系繪制的，可以體現(xiàn)出機器人的運行方向，通過足端軌跡驗證機器人可以準(zhǔn)確沿著偏航方向運動，由于在運動過程中采用二步態(tài)實現(xiàn)機器人的步態(tài)運動，因此分別繪制足1和足2的軌跡曲線，如圖9和圖10所示。

從圖9和圖10中可以看出在機體坐標(biāo)系下機器人的足端軌跡曲線完成了擺動相和支撐相的循環(huán)運動，并時刻沿著偏航的方向行走，兩組不同的腿可以相互協(xié)調(diào)的完成偏航運動過程。當(dāng)檢測到障礙物時，足端沿著前進(jìn)方向和水平方向的夾角運動，當(dāng)避開障礙物以后，足端沿著前進(jìn)方向運動，完成整個的避障過程。

針對六足機器人完成的避障運動，為驗證關(guān)節(jié)運動的連續(xù)性，圖11和12所示為腿1和腿2的跟關(guān)節(jié)，髖關(guān)節(jié)和膝關(guān)節(jié)曲線。

通過曲線11和12可以看出機器人的腿1和腿2關(guān)節(jié)運動連續(xù)，并根據(jù)足端軌跡曲線完成了相應(yīng)的避障運動。六足機器人的避障運動過程見圖13所示，可以根據(jù)地圖信息近似得到機器人距離障礙物的距離，地圖中標(biāo)亮的地形為障礙物，可以根據(jù)地圖尺寸信息和機器人尺寸信息得出機器人質(zhì)心距離障礙物的近似距離為2.6 m。最終，依據(jù)實時更新的地圖信息，機器人實現(xiàn)了自主避障運動。

5 結(jié) 論

基于激光測距信息的避障運動是機器人在復(fù)雜環(huán)境下作業(yè)的重要手段之一，通過模擬月壤環(huán)境并實現(xiàn)地形建模，解決了機器人在未知環(huán)境下的自主避障和導(dǎo)航問題，本文對其地形建模和自主避障策略進(jìn)行了研究，有以下結(jié)論：

(1) 基于激光測距儀信息，完成了實時的地形建模，并得到了障礙和機器人本體幾何信息的映射，提出基于虛擬機體模型的自主避障策略，得到機器人避障運動過程中的最優(yōu)可行方向和最短運動距離。

(2) 針對電驅(qū)動六邊形對稱分布的六足機器人，建立了其運動學(xué)模型，規(guī)劃了實時避障的機體和足端運動軌跡，完成了擺動相和支撐相的運動規(guī)劃，使機器人可以根據(jù)給定的可行方向?qū)崿F(xiàn)運動。

(3) 進(jìn)行了六足機器人樣機避障運動的實驗研究，實現(xiàn)了六足機器人在未知環(huán)境下的自主避障運動。實驗結(jié)果表明，六足機器人可以實時、準(zhǔn)確跟蹤避障策略中給定的偏航角度，避障過程中的擺動相和支撐相的運動軌跡驗證了避障策略的有效性和實時性。

在未來的工作中，考慮到六足機器人會在復(fù)雜凸凹不平的地形下行走，此時需要對機器人足底的地形進(jìn)行建模，并根據(jù)實時的地形信息為機器人提供安全的落足點選擇是進(jìn)一步需要研究的問題。