王雪竹，杜 佳，張美玲，陳少南

（中廣核研究院有限公司，廣東 深圳 518031）

1 引言

隨著機(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è)。

2 機(jī)器人自主開門規(guī)劃

2.1 系統(tǒng)構(gòu)成

研制的開門機(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

2.2 機(jī)器人運(yùn)動學(xué)

定義坐標(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，則有：

2.3 力位混合控制

為實(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則代表選擇力控制模式。

2.4 開門作業(yè)規(guī)劃和控制

由于履帶式移動車體的精度遠(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)動，打開門。

3 自主定位

自主定位用于在上述自主開門流程中完成對x0、y0和茲0的辨識。

3.1 門所在直線的辨識

對于二維激光掃描儀而言，門是一條線段，通過識別門所在直線（也即世界坐標(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)。

3.2 門把手定位

為抓握門把手打開門鎖，需要辨識門把手的三維坐標(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；

3.3 門軸識別和機(jī)器人定位

在機(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。

4 實(shí)驗(yàn)

為驗(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

5 結(jié)論

（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，滿足開門需求。