基于激光雷達局部地圖構(gòu)建的機器人避障路徑規(guī)劃

郝世宇,王 昆

(1. 山西工程職業(yè)學(xué)院 機械工程系, 山西 太原 030009)

(2. 山西大學(xué) 物理電子工程學(xué)院, 山西 太原 030006)

0 引 言

激光雷達是一種感知環(huán)境的傳感器,它發(fā)射的激光波長比可見光和紅外線更短,具有高能量和抗干擾能力強的特點,因此在環(huán)境目標(biāo)的感知和識別中應(yīng)用廣泛。激光雷達還具有高分辨率、體積小等優(yōu)勢,在機器人行走過程避障路徑規(guī)劃的新領(lǐng)域受到了廣泛關(guān)注。

為了提高機器人的性能,優(yōu)化機器人導(dǎo)航、控制和避障能力,研究人員致力于機器人避障路徑規(guī)劃的研究。文獻[1]通過分析機器人運動學(xué)規(guī)律獲取機器人運動相關(guān)的節(jié)點信息,并建立基于全局路徑節(jié)點信息的多種柵格全局地圖。該地圖內(nèi)的路徑軌跡平滑度較高,能夠及時確定局部目標(biāo)區(qū)域,有利于提升機器人避障檢測效率。在該地圖中,引入避障控制策略可實現(xiàn)機器人避障路徑規(guī)劃。然而,在復(fù)雜的避障環(huán)境下,全局地圖的規(guī)劃效果較差。文獻[2]通過增加自我定位模式改善了機器人的運動系統(tǒng)結(jié)構(gòu)。機器人自我定位模式可基于所處環(huán)境建立尋徑運動模型,并在虛擬算法的協(xié)助下求解該模型,以實現(xiàn)機器人避障路徑規(guī)劃。然而,該方法的全局地圖對機器人避障路徑規(guī)劃效果較差。文獻[3]通過虛擬機器人收集了大量不確定任務(wù)情況下的隨機運動軌跡,并將軌跡輸入以懲罰避障函數(shù)為基礎(chǔ)構(gòu)建的快速軌跡規(guī)劃模型中。根據(jù)模型輸出的運動軌跡觀測值,構(gòu)建全局地圖,實現(xiàn)機器人避障路徑的規(guī)劃。但是,該方法在普通避障環(huán)境和復(fù)雜避障環(huán)境下,都存在機器人避障路徑規(guī)劃效果較差的問題。

為了解決上述方法中存在的問題,提出基于激光雷達局部地圖的機器人避障路徑規(guī)劃的方法。

1 構(gòu)建全局與局部地圖

地圖構(gòu)建是機器人避障方法設(shè)計的基礎(chǔ)。為了進一步增強機器人避障問題的完整性,本文將地圖構(gòu)建分為全局和局部兩個層次。全局地圖利用深度信息采集設(shè)備獲取數(shù)據(jù),為機器人提供整體視角的環(huán)境信息。然而,在機器人避障過程中需要更為詳細(xì)的數(shù)據(jù),因此引入激光雷達技術(shù),結(jié)合激光聚合和分割方法,構(gòu)建任意線段與端點之間的區(qū)域,并基于全局地圖構(gòu)建具有更多細(xì)節(jié)信息的局部地圖。通過這種方式,可以為機器人提供更加準(zhǔn)確和完整的環(huán)境地圖,從而更好地解決機器人避障問題。

1.1 全局地圖的構(gòu)建

(1)Kinect傳感器采集機器人環(huán)境深度信息

Kinect是微軟公司開發(fā)的一款外設(shè)傳感器,其核心設(shè)備由彩色攝像機、紅外線發(fā)射器和紅外線互補金屬氧化物半導(dǎo)體(CMOS)攝像機組成。其能夠快速地同時獲取場景彩色圖像和深度信息,且不受環(huán)境可見光譜干擾,成本低。利用機器人內(nèi)設(shè)Kinect傳感器[4]采集所處環(huán)境的深度信息,并建立基于所處環(huán)境的全局地圖,是實現(xiàn)機器人避障路徑規(guī)劃的第一步。Kinect傳感器是由可見光傳感器和深度傳感器共同組成的深度信息采集設(shè)備,其針對所處環(huán)境深度信息的采集主要分為兩部分,一是利用可見光傳感器采集所處環(huán)境的彩色圖像數(shù)據(jù),二是利用深度傳感器采集所處環(huán)境的紅外光斑數(shù)據(jù)。

(2)深度信息轉(zhuǎn)化為3D信息

Kinect傳感器不僅負(fù)責(zé)采集機器人所處環(huán)境的深度信息,還能將采集到的深度信息傳送至機器人操作系統(tǒng)(ROS)[5]。ROS具備視圖化技術(shù),能夠?qū)⒉杉降纳疃刃畔⑥D(zhuǎn)化成3D視圖,構(gòu)建屬于機器人當(dāng)下場景的全局地圖。Kinect傳感器傳送深度信息的過程借助了有效信息融合原理,因為ROS僅接收3D信息,所以在深度信息傳送前需要根據(jù)有效信息融合原理將深度信息有效融合成3D信息。有效信息融合原理的表達式為

(1)

式中:L表示有效融合常數(shù);I表示參與傳送的深度信息量;ω表示深度信息的權(quán)重;z表示3D信息轉(zhuǎn)化系數(shù)。

ROS接收來自Kinect傳感器的3D信息,視圖化技術(shù)利用結(jié)合了2D 同步定位與地圖構(gòu)建方法和慣性傳感系統(tǒng)導(dǎo)航(Hectot SLAM)算法讀取3D信息基于三維空間的實際場景數(shù)據(jù),并將數(shù)據(jù)映射至X軸、Y軸和Z軸,使每個數(shù)據(jù)都具備幾何效應(yīng)。滿足幾何效應(yīng)的實際場景數(shù)據(jù)不僅描繪了機器人所處環(huán)境的周圍情況,還記錄了形狀大小各異的障礙物位置信息,為后續(xù)機器人避障路徑的規(guī)劃提供有力的保證。Hectot SLAM算法的表達式為

(2)

式中:N表示Hectot SLAM常數(shù);δ表示實際場景數(shù)據(jù)的讀取率;d表示實際場景數(shù)據(jù)的讀取誤差。

視圖化技術(shù)在獲取滿足幾何效應(yīng)的實際場景數(shù)據(jù)后,會主動向ROS傳遞指令。指令通信是ROS特有的任務(wù)呈遞流程,ROS會在接收到指令的第一時間開啟三維模擬器,將實際場景數(shù)據(jù)引流過來,用以搭建機器人所處環(huán)境的相關(guān)地圖。三維模擬器首先利用Gmapping算法獲取基于實際場景數(shù)據(jù)的建模匹配點,再利用激光掃描儀在建模匹配點的基礎(chǔ)上重塑環(huán)境信息,最終實現(xiàn)機器人所處環(huán)境全局地圖的構(gòu)建。全局地圖定位點的計算方法為

(3)

式中:λ表示Gmapping常數(shù);xl表示建模匹配點獲取系數(shù);xm表示建模匹配點獲取誤差。

三維模擬器搭建的全局地圖如圖1所示。

圖1 三維模擬器搭建的全局地圖

1.2 基于激光雷達的局部地圖構(gòu)建

激光雷達作為機器人導(dǎo)航定位和路徑規(guī)劃中的常見技術(shù)[6],往往能通過激光聚合和分割的方法,在短時間內(nèi)獲取任意線段與端點之間的區(qū)域,并在全局地圖的基礎(chǔ)上,構(gòu)造出信息儲存量充足且逼真度較高的局部地圖。構(gòu)建局部地圖的目的是為了細(xì)化機器人所處環(huán)境,提升避障路徑規(guī)劃的可行性。激光雷達構(gòu)建局部地圖主要分為三個步驟,即坐標(biāo)轉(zhuǎn)換、線段分割、構(gòu)建局部地圖。

(1)坐標(biāo)轉(zhuǎn)換

激光雷達同時掃描機器人所處環(huán)境和全局地圖的數(shù)據(jù)點,通過對比兩處數(shù)據(jù)點分布情況,識別全局地圖的缺損數(shù)據(jù)。全部數(shù)據(jù)點均以激光雷達中心發(fā)射點為坐標(biāo)原點,這樣獲取的每個數(shù)據(jù)點坐標(biāo)清晰,減少了地圖創(chuàng)建誤差的可能性。為了將數(shù)據(jù)點連接成能夠分割的線段結(jié)構(gòu),需要利用笛卡爾公式將全部數(shù)據(jù)點坐標(biāo)轉(zhuǎn)化為笛卡爾系下的全局坐標(biāo)。笛卡爾公式的表達式為

G=cos(t·2π·xi/360)cosβ

(4)

式中:t表示笛卡爾常數(shù);xi表示原始數(shù)據(jù)點坐標(biāo)權(quán)重;β表示數(shù)據(jù)點坐標(biāo)轉(zhuǎn)化誤差。

(2)線段分割

經(jīng)過笛卡爾公式轉(zhuǎn)化的數(shù)據(jù)點坐標(biāo)能夠在線性函數(shù)的作用下,將鄰近坐標(biāo)聚攏在一起,共同連接成一條描述機器人所處環(huán)境線性區(qū)域的矢量線。線性函數(shù)的表達式為

(5)

式中:N表示線性常數(shù);yn表示鄰近坐標(biāo)聚攏系數(shù);m表示矢量線連接誤差;θ表示單位數(shù)據(jù)點的坐標(biāo)權(quán)重。多條矢量線將機器人所處環(huán)境分割成了多個線性區(qū)域,從二維視角觀察機器人所處環(huán)境的線性區(qū)域,其示意圖如圖2所示。

圖2 機器人所處環(huán)境的線性區(qū)域

(3)局部地圖的構(gòu)建

如圖2所示,經(jīng)過線段分割的機器人所處環(huán)境從原始的大面積區(qū)域轉(zhuǎn)化成了由矢量線分割的小面積線性區(qū)域。以線性區(qū)域為單位,利用動態(tài)閾值方法構(gòu)建機器人所處環(huán)境的局部地圖?？v軸坐標(biāo)計算點表達式為

(6)

式中:εi表示動態(tài)閾值常數(shù);y0表示局部地圖構(gòu)建參數(shù);yi+1表示局部地圖構(gòu)建誤差。

利用協(xié)方差函數(shù)將Kinect傳感器建立的全局地圖和激光雷達建立的局部地圖重疊在一起,使根據(jù)機器人所處環(huán)境所建立的虛擬地圖得到全面細(xì)化。局部地圖的橫坐標(biāo)為

(7)

胃腸道間質(zhì)瘤形態(tài)大多為圓形或橢圓形，邊緣清晰，為分葉狀，非常容易發(fā)生出血、壞死和囊變，CT診斷密度不均勻，MRI診斷為軟組織，能夠清楚看到囊變和壞死。本次研究，40例患者中，22例發(fā)生分葉，壞死和囊變比較常見，直接超過5cm都屬于壞死和囊變。對27例患者使用CT和MRI增強掃描，腫塊沒有明顯強化。有研究指出，根據(jù)腫瘤血管可以判斷腫瘤惡性程度，本次結(jié)果，14例發(fā)現(xiàn)腫瘤血管，7例屬于高度危險性，4例屬于中度危險性，3例屬于低度危險性。本組患者沒有全部進行增強掃描，34例患者獲得準(zhǔn)確定位，準(zhǔn)確率為85%。

2 路徑控制中的模糊PID控制器設(shè)計

模糊比例積分微分(PID)控制器又稱模糊PID控制系統(tǒng),其設(shè)計理念參考了移動機器人運動學(xué)模型,該模型利用Cartographer算法[7]優(yōu)化機器人控制參數(shù),并在拉格朗日定律的協(xié)助下規(guī)劃機器人運動軌跡。模糊PID控制器將移動機器人運動學(xué)模型與3D視覺傳感器[8]相結(jié)合,在保留模型跟蹤控制性能的同時,還增加了信息反饋平臺,使機器人在途中遇臨時障礙物時,能夠第一時間反饋障礙物信息[9],并重新規(guī)劃避障路線。Cartographer算法的表達式為

(8)

式中:Ye表示Cartographer常數(shù);φm表示機器人控制參數(shù)的優(yōu)化系數(shù);H表示機器人控制參數(shù)優(yōu)化誤差。約束條件表達式為

(9)

模糊PID控制器[10]以路徑規(guī)劃和約束條件為基礎(chǔ),由控制層和導(dǎo)航層兩大模塊組成。模糊PID控制器結(jié)構(gòu)[11]如圖3所示。

圖3 模糊PID控制器結(jié)構(gòu)

模糊PID控制器在Kinect傳感器和激光雷達所建立的虛擬地圖上規(guī)劃機器人避障路徑,其具體步驟如下:模糊PID控制器首先啟動控制層的比例模塊[12],利用模糊隸屬度函數(shù)計算虛擬地圖與實際場景的比例值,然后將比例值結(jié)果傳送至導(dǎo)航層。導(dǎo)航層作為機器人避障路徑規(guī)劃的核心結(jié)構(gòu),在接收到來自控制層的積分信號時,會第一時間讀取虛擬地圖的障礙物位置信息[13],同時規(guī)劃機器人避障路徑。

模糊隸屬度函數(shù)的表達式為

(10)

式中:e(r)表示模糊隸屬度常數(shù);y(t)表示虛擬地圖與實際場景的比例值計算誤差;k表示虛擬地圖的參數(shù)。將以上函數(shù)計算出的結(jié)果傳送至導(dǎo)航層[14]后,得到的機器人避障路徑規(guī)劃的表達式為

(11)

3 實驗設(shè)計與結(jié)果分析

3.1 實驗測試參數(shù)

為了驗證基于激光雷達及模糊PID的機器人避障路徑規(guī)劃的整體有效性,需要對其進行測試。在測試平臺上構(gòu)建普通、復(fù)雜兩種機器人避障環(huán)境,以此測試實驗方法的有效性。實驗環(huán)境均為50×50的二維直角坐標(biāo)系,黑色區(qū)域為障礙物位置,機器人起始點與終止點位置固定,起始點坐標(biāo)為(0,100),終止點坐標(biāo)為(100,0)。對比不同方法在兩種實驗環(huán)境下的機器人避障性能,判斷不同方法的機器人避障路徑規(guī)劃性能。選擇四種型號不同的機器人參與實驗,如圖4所示。四種型號的機器人參數(shù)如表1所示。

表1 四種型號的機器人參數(shù)

圖4 四種型號的機器人

激光雷達機器人避障實驗具體的測試參數(shù)如下:

(1)激光雷達參數(shù):雷達型號為Hokuyo UBG-04LX-F01,旋轉(zhuǎn)角度范圍為± 135°,分辨率為0.25°,最大掃描速度為10 000 rpm,最大測距距離為4 m。

(2)機器人運動控制節(jié)點參數(shù):控制算法為模糊PID,路徑規(guī)劃算法為全局地圖與局部地圖算法。

(3)障礙物模型參數(shù):障礙物形狀為正方形、圓形和多邊形,其寬度和高度為0.5 m～1.5 m,長度為1 m～3 m。 包含多個不同形狀、大小的障礙物。

3.2 普通障礙環(huán)境實驗

普通障礙環(huán)境僅采用獨立障礙物作為測試樣本。分別采用文獻[1]提出的多重A*算法的機器人避障路徑規(guī)劃方法和文獻[2]提出的自我定位模式的機器人避障路徑規(guī)劃方法作為實驗的對比方法,普通障礙環(huán)境下不同方法的避障路徑如圖5所示。

圖5 普通障礙環(huán)境下不同方法對于四種型號的機器人避障路徑規(guī)劃結(jié)果

如圖5所示,在普通避障環(huán)境下文獻[1]方法和文獻[2]方法對于四種型號的機器人避障路徑規(guī)劃結(jié)果與障礙物接觸明顯,且路徑選擇并不是最優(yōu)距離,說明采用文獻[1]方法和文獻[2]方法規(guī)劃機器人避障路徑的效果較差。所提方法在普通避障環(huán)境下的機器人避障路徑均未與障礙物接觸,且路徑選擇呈現(xiàn)最優(yōu)距離,說明采用所提方法規(guī)劃機器人避障路徑的效果較好。這是因為所提方法采用Kinect傳感器和激光雷達建立機器人所處環(huán)境的虛擬地圖,并結(jié)合模糊PID控制器,在虛擬地圖的基礎(chǔ)上規(guī)劃機器人避障路徑,使最終獲取的機器人避障路徑規(guī)劃結(jié)果更加簡潔,避障率更高。經(jīng)上述對比可知,所提方法針對機器人避障路徑的規(guī)劃效果明顯優(yōu)于傳統(tǒng)方法。

3.3 復(fù)雜障礙環(huán)境實驗

復(fù)雜障礙環(huán)境采用曲折的U型障礙物作為測試樣本。分別采用所提方法、文獻[1]方法和文獻[2]方法規(guī)劃上述四種型號的機器人避障路徑,結(jié)果如圖6所示。

圖6 復(fù)雜障礙環(huán)境下不同方法機器人避障路徑規(guī)劃結(jié)果

如圖6所示,所提方法在復(fù)雜避障環(huán)境下對于四種型號的機器人避障路徑規(guī)劃結(jié)果均未觸及障礙物,且路徑選擇呈現(xiàn)最優(yōu)距離。本次實驗結(jié)果說明,所提方法適用于較為復(fù)雜的環(huán)境,進一步驗證了所提方法對機器人避障路徑規(guī)劃的良好效果。

4 結(jié)束語

激光雷達可以通過發(fā)射高能量的激光并根據(jù)返回的數(shù)據(jù)確定周邊環(huán)境的信息,受外界干擾小,可將其與模糊PID控制器結(jié)合,應(yīng)用于機器人行走控制中,提升機器人對不同環(huán)境的避障能力。通過在多種環(huán)境下進行避障路徑跟蹤,驗證了該方法可以使機器人采用最優(yōu)距離進行避障路徑規(guī)劃。但該方法還需要繼續(xù)優(yōu)化,如何在保證機器人避障路徑規(guī)劃性能的同時,縮短機器人避障路徑規(guī)劃的時間,是下一步工作的重點。