王雪竹,杜 佳,張美玲,陳少南
(中廣核研究院有限公司,廣東 深圳 518031)
隨著機(jī)器人技術(shù)的發(fā)展,機(jī)器人越來越多地進(jìn)入人的工作和生活環(huán)境中執(zhí)行任務(wù)。對于需要在房間內(nèi)外穿行的機(jī)器人,如核電站環(huán)境監(jiān)測機(jī)器人、應(yīng)急救援機(jī)器人、家庭服務(wù)機(jī)器人等,開門是基本功能需求。然而這些環(huán)境在建造時(shí)并未考慮機(jī)器人的需求,機(jī)器人自主開門作業(yè)面臨諸多難點(diǎn),包括對門的辨識、定位,以及開鎖、推門、拉門過程中的軌跡規(guī)劃和柔順控制等。
研究人員提出了多種機(jī)器人自主開門方法。2009年,文獻(xiàn)[2]針對KIST機(jī)器人,采用視覺和觸覺結(jié)合的方式識別門把手位置,在抓住門把手后試探晃動,根據(jù)機(jī)器人末端位置變化識別門軸位置,并在整個開門過程中使用力控制保障機(jī)器人的安全。2010年,文獻(xiàn)[3-4]針對PR2機(jī)器人,使用踝關(guān)節(jié)處的二維激光掃描儀進(jìn)行導(dǎo)航,使用頭部帶云臺的二維激光掃描儀辨識門軸和門把手的位置等信息,然后規(guī)劃出一條無碰撞的運(yùn)動軌跡完成開門。文獻(xiàn)[5]針對非結(jié)構(gòu)化環(huán)境中未知的門,采用基于視覺的學(xué)習(xí)方法識別圖像中的門把手,然后使用立體相機(jī)獲取其三維坐標(biāo)。2012年,文獻(xiàn)[6]對KUKA機(jī)器人設(shè)計(jì)笛卡爾空間下的阻抗控制,使用立體相機(jī)結(jié)合觸覺獲取精確的鎖孔位置,插入鑰匙打開門鎖,然后根據(jù)觸覺找到門把手,下壓門把手開門。2015年,DARPA機(jī)器人挑戰(zhàn)賽中,參賽的DRC-HUBO、CHIMP、Atlas等機(jī)器人紛紛在頭部安裝三維激光掃描儀和多個攝像機(jī),用來掃描環(huán)境建立三維模型,進(jìn)行開門作業(yè)[7]。
然而,更多的傳感器往往意味著更大更重的機(jī)器人,影響其可達(dá)區(qū)域。尤其是對于核電作業(yè)機(jī)器人,耐輻照要求使得相機(jī)等傳感器重達(dá)數(shù)公斤,受限于機(jī)器人負(fù)載能力,需要考慮使用較少的傳感器來完成開門作業(yè)。
對帶六自由度機(jī)械臂的履帶式移動機(jī)器人進(jìn)行研究,規(guī)劃開門作業(yè)流程,使用安裝在機(jī)械臂末端的單目相機(jī)和安裝在車體上的二維激光掃描儀完成對門把手和門軸的識別定位,并設(shè)計(jì)力位混合控制方法,實(shí)現(xiàn)自主開門作業(yè)。
研制的開門機(jī)器人主要由履帶式移動車體和六自由度機(jī)械臂構(gòu)成,如圖1所示。該結(jié)構(gòu)設(shè)計(jì)使得機(jī)器人具有較強(qiáng)的爬坡、越障、負(fù)載和多功能作業(yè)能力。在車體上安裝二維激光掃描儀、在機(jī)械臂末端安裝單目手眼相機(jī),用于開門過程的導(dǎo)航和定位。二維激光掃描儀的功能,如圖2所示。能夠測量所在平面內(nèi)的障礙物,用于避障行進(jìn);同時(shí),結(jié)合相機(jī)對圖像的辨識,可以得到環(huán)境的部分三維信息。
圖1 機(jī)器人系統(tǒng)及坐標(biāo)系定義Fig.1 Robot System and Its Coordinate Systems
圖2 二維激光掃描儀示意圖Fig.2 Illustration of the 2D Laser Scanner
定義坐標(biāo)系,如圖1所示。世界坐標(biāo)系O-xyz固定在門軸處,與車體同高。
機(jī)器人坐標(biāo)系O0-x0y0z0定義在車體中心,跟隨車體運(yùn)動,與世界坐標(biāo)系之間存在夾角θ0,其原點(diǎn)在O-xyz下的坐標(biāo)為p0=[x0,y0,0]T,則二者之間的變換矩陣為:
末端坐標(biāo)系O6-x6y6z6定義在末端工具處,機(jī)械臂各連桿之間的變換矩陣為T(i=1,…,6),則運(yùn)動學(xué)方程為
二維激光掃描儀坐標(biāo)系OL-xLyLzL與機(jī)器人坐標(biāo)系同方向,其原點(diǎn)在 O0-x0y0z0下的坐標(biāo)為0pL=[0xL,0yL,0zL]T,則有:
相機(jī)坐標(biāo)系Oc-xcyczc定義在相機(jī)光心處,與末端坐標(biāo)系存在夾角 θc,其原點(diǎn)在 O6-x6y6z6下的坐標(biāo)為6pc=[6xc,6yc,6zc]T,則有:
為實(shí)現(xiàn)機(jī)器人與環(huán)境之間的柔順接觸,避免硬碰撞,同時(shí)保證非接觸方向的位置運(yùn)動精度,對機(jī)械臂設(shè)計(jì)力位混合控制算法如下:
式中:τ6×1—關(guān)節(jié)力矩控制量;x6×1=[x,y,z,rx,ry,rz]—機(jī)械臂末端的位置和姿態(tài)角;xd6×1—其期望值;J6×6—雅可比矩陣,用于將末端廣義力變換到關(guān)節(jié)力矩;對角陣 Kp6×6、Kd6×6—位置控制的比例系數(shù)矩陣和微分系數(shù)矩陣;g6×1—重力補(bǔ)償項(xiàng);Fd6×1—末端廣義力的期望值;I6×6—單位陣;對角陣 Λ6×6為控制模式選擇矩陣,某行為1則代表對應(yīng)的位置或姿態(tài)角選擇位置控制,某行為0則代表選擇力控制模式。
由于履帶式移動車體的精度遠(yuǎn)低于機(jī)械臂,尤其是在轉(zhuǎn)彎運(yùn)動中,車體的運(yùn)動誤差容易導(dǎo)致機(jī)械臂受力過大損壞。因此在開門時(shí)盡量令車體靜止,必須運(yùn)動時(shí)則令其勻速直線運(yùn)動以減少誤差,同時(shí)機(jī)械臂配合車體運(yùn)動。
自主開門流程設(shè)計(jì)如下:
(1)行進(jìn)到門前
車體行進(jìn)到門前停下,且門把手在相機(jī)視野內(nèi)。
(2)門把手定位
使用手眼相機(jī)和二維激光掃描儀,獲取機(jī)器人和門把手之間的相對位置姿態(tài)。
(3)接近門把手
機(jī)械臂在位置控制模式下,移動到門把手正上方,并令末端工具垂直于門。
(4)開鎖
對機(jī)械臂使用力位混合控制,使末端工具下壓門把手,并跟隨門把手轉(zhuǎn)動,從而打開門鎖。
力位混合控制設(shè)計(jì)如下:令平行于門板的z自由度處于力控制模式,該方向的期望力豎直向下,用于下壓門把手;令垂直于門板旋轉(zhuǎn)的rx自由度處于力控制模式,該方向的期望旋轉(zhuǎn)力矩為零,即處在自由轉(zhuǎn)動狀態(tài),以跟隨門把手的旋轉(zhuǎn);其余4個自由度處于位置控制模式,期望位置(姿態(tài))保持不變。
(5)輕微打開門并完成機(jī)器人定位
對機(jī)械臂使用力位混合控制,使機(jī)械臂輕微向前(推門)或向后(拉門)打開門。同時(shí),使用二維激光掃描儀辨識門軸位置,完成機(jī)器人和門把手在世界坐標(biāo)系下的定位。
力位混合控制設(shè)計(jì)如下:令繞豎直方向旋轉(zhuǎn)的rz自由度處于力控制模式,該方向的期望旋轉(zhuǎn)力矩為零,即處在自由轉(zhuǎn)動狀態(tài),以跟隨門的旋轉(zhuǎn);其余5個自由度處于位置控制模式,向前或向后運(yùn)動。
(6)開門
根據(jù)門把手在世界坐標(biāo)系下的坐標(biāo),計(jì)算門的運(yùn)動軌跡。推門情況下,機(jī)械臂放開門把手,改用位置控制模式,根據(jù)機(jī)械臂自身運(yùn)動范圍規(guī)劃出一條軌跡,推開門板;拉門情況下,令車體勻速后退,對機(jī)械臂使用和步驟(5)相同的力位混合控制,令機(jī)械臂沿門把手軌跡運(yùn)動,打開門。
自主定位用于在上述自主開門流程中完成對x0、y0和茲0的辨識。
對于二維激光掃描儀而言,門是一條線段,通過識別門所在直線(也即世界坐標(biāo)系y軸),可得出機(jī)器人與門之間的距離x0和夾角 茲0。
二維激光掃描儀的數(shù)據(jù)是極坐標(biāo)(r,琢)形式,如圖2所示。將其變換到直角坐標(biāo)系OL-xLyL下,有:
對各個點(diǎn)進(jìn)行如下Hough變換:
式中:茲—斜率;籽—截距。從而將數(shù)據(jù)從xLyL平面中的一個點(diǎn)投影成參數(shù)坐標(biāo)系籽茲平面中的一條曲線。
在 籽茲平面內(nèi),找到各條曲線的聚集點(diǎn)(籽*,茲*),則Lxcos茲*+Lysin茲*=籽*即為門所在直線在OL-xLyL坐標(biāo)系下的表示。
根據(jù)式(1)和式(2),可得:
值得注意的是,為獲取更準(zhǔn)確的直線方程,在投影計(jì)算時(shí)采用較小的斜率間隔以提高分辨率。然而由于測量數(shù)據(jù)自身存在抖動誤差,較小的斜率間隔會導(dǎo)致籽茲平面內(nèi)數(shù)據(jù)的分散,易受周圍環(huán)境干擾。為解決這一問題,令籽茲平面內(nèi)各個點(diǎn)的值等于其周圍一定范圍內(nèi)的點(diǎn)的值之和,使數(shù)據(jù)變得更凝聚,然后再計(jì)算最大值點(diǎn)作為聚集點(diǎn)。
為抓握門把手打開門鎖,需要辨識門把手的三維坐標(biāo)。由于相機(jī)在深度方向的辨識精度不夠高,因此僅使用相機(jī)對門把手進(jìn)行圖像平面內(nèi)的辨識,然后結(jié)合二維激光掃描儀獲取的深度信息,得到更精確的門把手的三維坐標(biāo)。
具體方法是:令機(jī)械臂帶著相機(jī)在世界坐標(biāo)系yz平面移動,辨識得到相機(jī)在不同位置時(shí)門把手在圖像坐標(biāo)系下的坐標(biāo),然后進(jìn)行擬合,并聯(lián)合對門所在直線的辨識結(jié)果,得到門把手與機(jī)器人的相對位姿。為提高擬合精度,令機(jī)械臂移動過程中門把手逐漸靠近圖像中心。流程如下:
(1)不妨令y0=0,也即將世界坐標(biāo)系沿y軸進(jìn)行平移;
(2)按照3.1節(jié)方法,通過激光掃描儀數(shù)據(jù)計(jì)算x0和茲0,得到
(3)為提高圖像識別精度,在門和車體的夾角茲0已測得的情況下,根據(jù)茲0和茲c調(diào)整機(jī)械臂末端姿態(tài),使相機(jī)正對門,即zc∥x且 xc∥y;
(5)使用尺度不變的模板匹配方法,從相機(jī)圖像中辨識門把手,得到門把手中心在圖像坐標(biāo)系(原點(diǎn)在圖像中心)下的坐標(biāo)(wh,hh);
(6)在世界坐標(biāo)系下平移機(jī)械臂末端,平移量為駐p6=(k1-k2i)[0,-sgn(wh),-sgn(hh)]T,其中 i表示第 i次執(zhí)行本步,k1和 k2為系數(shù);
(7)重復(fù)步驟(4)~步驟(6)若干次;
(8)根據(jù)得到的一系列 yc、wh、zc和 hh,進(jìn)行最小二乘擬合,得到 yc=kywh+by,zc=kzhh+bz。(by,bz)即為門把手在圖像中心時(shí)的相機(jī)坐標(biāo),則門把手中心在世界坐標(biāo)系下的坐標(biāo)為ph=[0,by,bz]T;
在機(jī)械臂輕微打開門過程中,門所在直線發(fā)生旋轉(zhuǎn),通過對直線旋轉(zhuǎn)過程的擬合,可以得出轉(zhuǎn)軸(即門軸)位置,計(jì)算出y0,從而完成機(jī)器人和門把手在世界坐標(biāo)系下的定位,用于門打開過程中對機(jī)器人進(jìn)行軌跡規(guī)劃。
在門運(yùn)動時(shí),按照3.1節(jié)方法,通過二維激光掃描儀數(shù)據(jù)多次辨識門所在直線,得到OL-xLyL坐標(biāo)系下的一系列直線…,n 表示第 i次辨識結(jié)果。
據(jù)此修正 y0:y0=-yh,修正門把手的坐標(biāo):ph=ph+[0,yh,0]T。
為驗(yàn)證提出的機(jī)器人自主開門規(guī)劃控制和定位方法的有效性,進(jìn)行自主開門實(shí)驗(yàn),如圖3所示。
在此實(shí)驗(yàn)過程中的自主定位結(jié)果如下:首先使用3.1節(jié)方法得到:茲0=-3.5°,x0=-1006mm;然后使用 3.2 節(jié)方法得到:ph=[0,-8.5mm,803mm];最后使用3.3節(jié)方法得到:yh=-y0=-474mm,據(jù)此修正:ph=[0,465.5mm,803mm]。對機(jī)器人和門進(jìn)行實(shí)際測量可得,門把手坐標(biāo)應(yīng)為:ph=[0,459mm,800mm],初始時(shí)機(jī)器人與門之間距離為1002mm。鑒于門把手坐標(biāo)和機(jī)器人與門的距離分別反映了定位算法在門平面和法線方向的精度,對比實(shí)測值和辨識結(jié)果,可以看出所提方法能夠?qū)崿F(xiàn)自主定位,且辨識誤差小于1cm。
圖3 開門實(shí)驗(yàn)照片F(xiàn)ig.3 Pictures of the Door-Opening Experiment
(1)為實(shí)現(xiàn)機(jī)器人在房間內(nèi)外穿行所需的開門功能,提出一種基于力位混合控制的機(jī)器人自主開門規(guī)劃方法,該方法能夠降低接觸力,保障開門過程機(jī)器人和環(huán)境的安全。實(shí)驗(yàn)表明該方法能夠完成自主開門。(2)為完成上述開門規(guī)劃所需的自主定位,設(shè)計(jì)一種使用單目手眼相機(jī)和二維激光掃描儀的門把手和門軸辨識方法,能夠在開門作業(yè)中完成對門的識別和機(jī)器人自主定位。實(shí)驗(yàn)驗(yàn)證該方法辨識精度優(yōu)于1cm,滿足開門需求。