基于視覺和慣導(dǎo)融合的多臂連續(xù)體形狀檢測

王 寧,陳 柏,鞠 鋒,郭 昊

(南京航空航天大學(xué)機電學(xué)院,江蘇 南京 210016)

連續(xù)體機器人在人體膀胱腫瘤微創(chuàng)手術(shù)、飛機發(fā)動機檢測、地震被困人員搜救、核電站管道內(nèi)部維修等狹小、受限、障礙物較多的工作環(huán)境中,可利用其自身柔順、靈活度高的特點,通過改變自身形狀來適應(yīng)工作空間。為了更好地讓連續(xù)體機器人準(zhǔn)確、安全地運動,對連續(xù)體機器人形狀信息的獲取十分重要。連續(xù)體機器人是一種仿章魚觸手、象鼻和蛇等的柔性機構(gòu),沒有離散關(guān)節(jié)和剛性連桿,無法像傳統(tǒng)機器人那樣使用電位計或編碼器來獲得位姿信息,故而大多數(shù)連續(xù)體機器人沒有形狀感知能力,因此研究一種可以在狹小或者障礙物多的工作空間內(nèi)檢測連續(xù)體機器人形狀的方法尤為重要。

目前,連續(xù)體機器人形狀檢測方法主要有基于視覺的形狀檢測、基于智能材料的形狀感知和基于傳感器的形狀檢測等方法,其中基于視覺的形狀檢測方法應(yīng)用最為廣泛。基于視覺的形狀檢測方法是一種模擬人的視覺功能從客觀圖像中提取信息的方法,視覺檢測的方法根據(jù)相機個數(shù)不同,分為單目、雙目、三目或者多目視覺檢測幾類[1]。美國學(xué)者Hannan等[2]利用單目高速相機獲取連續(xù)體機器人圖像,通過捕捉均勻布置的標(biāo)記點,將灰度圖二值化,再通過算法得到連續(xù)體機器人二維形狀,該方法圖片處理時間長、精度低,不能滿足控制的實時性要求;Chitrakaran等[3]在文獻(xiàn)[2]的基礎(chǔ)上進(jìn)行了改進(jìn),結(jié)合運動學(xué)分解獲得了連續(xù)體機器人三維空間形狀,并且通過視覺信息完成了對連續(xù)體機器人的控制;Croom等[4]提出了一種基于雙目視覺無標(biāo)記的形狀檢測方法,利用自組織映射算法SOM對圖像進(jìn)行處理得到二維曲線,進(jìn)而重構(gòu)三維形狀;美國學(xué)者Camarillo等[5]研究出一種在三維空間中對連續(xù)體機器人形狀進(jìn)行檢測的方法,通過立體視覺來檢測標(biāo)記點,估計其形狀?；谥悄懿牧系男螤罡兄椒ㄆ渲悄懿牧现饕芯燮蚁?PVDF)和離子交換聚合金屬材料(IPMC)。以色列學(xué)者Shapiro等[6]提出一種采用PVDF材料制作的嵌入式壓電薄膜實時跟蹤柔性機器人,通過PVDF和運動捕捉系統(tǒng)最終實現(xiàn)了二維形狀重構(gòu);Bahramzadeh等[7]針對IPMC材料的懸臂梁利用上位機計算梁的彎曲角度變化,以此來進(jìn)行形狀重構(gòu)。基于傳感器的形狀檢測方法主要有基于光纖光柵(FBG)傳感器的檢測方法和基于電磁傳感器的檢測方法。荷蘭學(xué)者Roesthuis等[8]在三維空間擴展了基于FBG傳感器的形狀檢測方法,設(shè)計了一種直徑為1 mm、長為172 mm、集成了12個FBG點的傳感器,通過檢測應(yīng)變來計算曲率,從而重構(gòu)出三維形狀;Song等[9]采用永磁體作為連續(xù)體機器人末端跟蹤的目標(biāo),通過磁傳感器陣列平臺測量磁通密度,得到機器人末端的位置和姿態(tài),然后根據(jù)貝塞爾曲線擬合連續(xù)體形狀,但是這種方法還需要在機器人外部增加額外的檢測設(shè)備。

針對在狹小或者障礙物較多的工作空間中存在視覺遮擋和缺失的問題,本文提出一種視覺和慣導(dǎo)融合的多臂連續(xù)體機器人形狀檢測方法,通過獲取連續(xù)體機器人頭部標(biāo)識物位姿信息,利用位姿信息進(jìn)行曲線擬合,從而重構(gòu)出連續(xù)體機器人三維形狀,完成對多臂連續(xù)體機器人的形狀檢測。

1 多臂連續(xù)體機器人形狀檢測系統(tǒng)

1.1 連續(xù)體機器人結(jié)構(gòu)設(shè)計

針對膀胱介入手術(shù),課題組提出由多個結(jié)構(gòu)和功能相同的彎曲單元串聯(lián)而成的繩驅(qū)動連續(xù)體機器人來實現(xiàn)手術(shù)操作,其骨架結(jié)構(gòu)如圖1所示。繩索驅(qū)動控制簡單,密封性要求低,便于微型化,是一種很好的連續(xù)體機器人驅(qū)動方式。

圖1 連續(xù)體機器人導(dǎo)管骨架

圖2所示為多臂連續(xù)體機器人機構(gòu),機械臂長度為220 mm,直徑為30 mm,由4根繩索驅(qū)動,導(dǎo)管頭部內(nèi)側(cè)安裝了慣導(dǎo)傳感器,外側(cè)安裝了球形標(biāo)識物。

圖2 多臂連續(xù)體機器人機構(gòu)

1.2 視覺與慣導(dǎo)融合形狀檢測系統(tǒng)

視覺檢測在連續(xù)體機器人形狀檢測中應(yīng)用較為廣泛,但是將其直接應(yīng)用于多臂連續(xù)體機器人時有一定的局限性。在狹小空間或障礙物較多的工作空間中會存在一定的視覺遮擋和缺失,僅通過視覺無法獲得完整的形狀信息。若將慣性傳感器布滿整個連續(xù)體機器人,可以利用機器人運動學(xué)解算方位得到形狀,但是過程過于復(fù)雜,無法實時檢測連續(xù)體形狀。為了解決上述問題,本文將視覺與慣導(dǎo)進(jìn)行多傳感融合,通過視覺與慣導(dǎo)獲取連續(xù)體機器人頭部標(biāo)識物位姿,然后采用形狀擬合重構(gòu)的方法,獲得多臂連續(xù)體機器人在工作空間中的形狀。

1.2.1視覺檢測系統(tǒng)

本文的連續(xù)體機器人視覺檢測系統(tǒng)主要由圖像采集模塊、圖像處理器和相機萬向支架組成。其中圖像采集模塊包括兩個相機,圖像處理器由圖像處理PC(personal computer)構(gòu)成。首先由相機采集連續(xù)體機器人圖像,然后通過串口將圖像信息傳輸?shù)綀D像處理PC,最終由LabVIEW進(jìn)行運算處理。

對相機進(jìn)行標(biāo)定,即建立實際場景點與圖像坐標(biāo)系之間的關(guān)系,根據(jù)已知特征點的世界坐標(biāo)和圖像坐標(biāo)求解相機內(nèi)外參數(shù)。在針孔成像模型[10]基礎(chǔ)上,研究4種坐標(biāo)系的關(guān)系,如圖3所示。

圖3 針孔成像模型

由于視覺檢測系統(tǒng)設(shè)備的局限性、安裝位置的準(zhǔn)確性、環(huán)境燈光以及系統(tǒng)各部分連接過程中存在的問題,圖像會產(chǎn)生畸變,因此綜合考慮形狀檢測視覺系統(tǒng)成像的多種畸變因素,對系統(tǒng)進(jìn)行校準(zhǔn),找出圖像與真實世界坐標(biāo)系之間的映射關(guān)系。本文采用圓點標(biāo)定盤(7×7圓點標(biāo)定盤,點陣中縱向和橫向相鄰點之間的距離為20 mm,圓點的直徑為10 mm),為視覺系統(tǒng)提供這些點的已知信息,包括在真實世界坐標(biāo)系中點與點的距離,結(jié)合視覺系統(tǒng)拍攝的圖像中各像素點與真實世界之間的映射關(guān)系,將得到的標(biāo)定關(guān)系應(yīng)用到之后的圖像采集處理中,可以降低各種畸變對系統(tǒng)檢測結(jié)果的影響,完成對畸變圖像的矯正。

為了獲得連續(xù)體機器人頭部標(biāo)識物位姿信息,要進(jìn)行標(biāo)識物的圖像模板匹配工作,通過分析標(biāo)識物圖像與實時圖像中邊緣、灰度、外形結(jié)構(gòu)及對應(yīng)關(guān)系等特征的相似性,從實時圖像中找到與標(biāo)識物相似的區(qū)域。這一過程包括學(xué)習(xí)和匹配兩個階段,在學(xué)習(xí)階段提取標(biāo)識物的特征信息,在匹配階段找到并提取相似的標(biāo)識物特征信息,從而實時抓取標(biāo)識物,得到連續(xù)體機器人頭部標(biāo)識物的位姿信息。

1.2.2慣性傳感器

本文采用慣性傳感器來解決視覺遮擋和視覺缺失的問題,將慣性傳感器安裝在連續(xù)體機器人頭部內(nèi)側(cè),來獲取連續(xù)體機器人頭部標(biāo)識物姿態(tài),如圖4所示。

圖4 慣導(dǎo)與標(biāo)識物

采用東北天坐標(biāo)系將慣性傳感器的測量值與地球的主方向聯(lián)系起來,地理坐標(biāo)系到慣性傳感器自身坐標(biāo)系的轉(zhuǎn)化矩陣為:

(1)

式中:C為姿態(tài)矩陣;n代表地理坐標(biāo)系;b代表機器人本體坐標(biāo)系;T為姿態(tài)矩陣相乘得到矩陣中的值;γ為滾轉(zhuǎn)角;θ為俯仰角;Ψ為航向角。

由姿態(tài)矩陣計算出滾轉(zhuǎn)角γ、俯仰角θ、航向角Ψ,公式如下:

(2)

根據(jù)得到的連續(xù)體機器人頭部位標(biāo)識物位姿信息,通過貝塞爾曲線擬合重構(gòu)連續(xù)體機器人在三維空間中的形狀。三次貝塞爾曲線的顯示形式為[11]:

B(t)=(1-t)3P1+3t(1-t)2P2+3t2(1-t)P3+t3P4

(3)

式中:B(t)為貝塞爾曲線函數(shù);t為時間;P1為起點位姿矩陣;P2為終點位姿矩陣;P3和P4為控制點位姿矩陣。

通過本文提出的視覺和慣導(dǎo)融合的多臂連續(xù)體機器人形狀檢測方法,獲得多臂連續(xù)體機器人頭部標(biāo)識物位姿,從而在三維空間中擬合重構(gòu)出連續(xù)體機器人的三維形狀。

2 實驗平臺搭建與形狀重構(gòu)

2.1 實驗平臺

測試平臺由多臂連續(xù)體機器人、視覺系統(tǒng)和慣性傳感器組成。兩個相機分別安裝在框架結(jié)構(gòu)的左側(cè)和前部,用于采集連續(xù)體機器人頭部位置信息。連續(xù)體機器人由尼龍材料3D打印而成,連續(xù)體頭部凹槽用于安裝慣性傳感器(JY901),頭部外側(cè)凸出的球體作為采集位置信息的標(biāo)識物。多臂連續(xù)體機器人形狀檢測實驗平臺如圖5所示。

圖5 多臂連續(xù)體機器人形狀檢測實驗平臺

本文采用Vision Assistant標(biāo)定工具進(jìn)行視覺系統(tǒng)的標(biāo)定,用Calibration函數(shù)進(jìn)行畸變矯正,如圖6所示。完成視覺系統(tǒng)校準(zhǔn)工作后,再進(jìn)行標(biāo)識物的模板匹配,通過相機拍攝識別標(biāo)識物,從而獲得多臂連續(xù)體機器人頭部標(biāo)識物位置信息,如圖7所示。

圖6 標(biāo)定

圖7 模板匹配

2.2 連續(xù)體形狀檢測實驗及擬合重構(gòu)

首先,對單臂連續(xù)體機器人在平面內(nèi)的彎曲運動進(jìn)行實驗,得到平面內(nèi)一組不同位置的單臂連續(xù)體彎曲形狀,然后根據(jù)本文形狀檢測方法,通過實驗平臺得到機器人頭部位姿,再進(jìn)行貝塞爾曲線擬合,最終重構(gòu)出單臂連續(xù)體機器人形狀,如圖8所示。

圖8 不同位置下的單臂連續(xù)體及擬合結(jié)果

將不同位置下的單臂連續(xù)體形狀用毫米方格紙記錄下來,與擬合結(jié)果進(jìn)行對比,結(jié)果表明,其最大誤差為1.832 mm,如圖9所示。

圖9 不同位置下的單臂連續(xù)體實際形狀

接下來進(jìn)行三維空間中的多臂連續(xù)體機器人形狀檢測實驗,在繩索驅(qū)動下,多臂連續(xù)體機器人形狀不斷發(fā)生改變,如圖10所示。在存在視覺遮擋和視覺缺失的情況下,通過得到的連續(xù)體機器人頭部位姿信息,重構(gòu)多臂連續(xù)體機器人的三維形狀,結(jié)果如圖11所示。

圖10 存在視覺遮擋的多臂連續(xù)體形狀檢測實驗

圖11 多臂連續(xù)體形狀重構(gòu)結(jié)果

通過激光跟蹤儀對三維空間內(nèi)的多臂連續(xù)體機器人實際形狀進(jìn)行測量,然后與擬合結(jié)果進(jìn)行對比,結(jié)果表明,最大誤差為2.386 mm。實驗結(jié)果表明,本文提出的形狀檢測方法有著較高的擬合度,在工作空間有限、存在視覺遮擋和缺失的情況下,能夠有效重構(gòu)出連續(xù)體機器人形狀。

3 結(jié)束語

本文提出了一種基于視覺和慣導(dǎo)融合的連續(xù)體機器人形狀檢測方法,在工作空間有限、存在視覺遮擋的情況下,通過兩個相機與三個慣性傳感器獲取多臂連續(xù)體機器人頭部標(biāo)識物的位姿信息,利用位姿信息進(jìn)行三次貝塞爾曲線擬合,最終重構(gòu)出多臂連續(xù)體機器人形狀。