郝世宇,王 昆

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

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

0 引 言

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

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

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

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

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

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

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

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

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

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

(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í)際場景數(shù)據(jù),并將數(shù)據(jù)映射至X軸、Y軸和Z軸,使每個(gè)數(shù)據(jù)都具備幾何效應(yīng)。滿足幾何效應(yīng)的實(shí)際場景數(shù)據(jù)不僅描繪了機(jī)器人所處環(huán)境的周圍情況,還記錄了形狀大小各異的障礙物位置信息,為后續(xù)機(jī)器人避障路徑的規(guī)劃提供有力的保證。Hectot SLAM算法的表達(dá)式為

(2)

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

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

(3)

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

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

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

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

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

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

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

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

(4)

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

(2)線段分割

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

(5)

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

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

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

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

(6)

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

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

(7)

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

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

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

(8)

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

(9)

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

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

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

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

(10)

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

(11)

3 實(shí)驗(yàn)設(shè)計(jì)與結(jié)果分析

3.1 實(shí)驗(yàn)測試參數(shù)

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

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

圖4 四種型號的機(jī)器人

激光雷達(dá)機(jī)器人避障實(shí)驗(yàn)具體的測試參數(shù)如下:

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

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

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

3.2 普通障礙環(huán)境實(shí)驗(yàn)

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

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

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

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

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

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

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

4 結(jié)束語

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