機器人自主開門作業(yè)研究

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

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

1 引言

隨著機器人技術的發(fā)展，機器人越來越多地進入人的工作和生活環(huán)境中執(zhí)行任務。對于需要在房間內外穿行的機器人，如核電站環(huán)境監(jiān)測機器人、應急救援機器人、家庭服務機器人等，開門是基本功能需求。然而這些環(huán)境在建造時并未考慮機器人的需求，機器人自主開門作業(yè)面臨諸多難點，包括對門的辨識、定位，以及開鎖、推門、拉門過程中的軌跡規(guī)劃和柔順控制等。

研究人員提出了多種機器人自主開門方法。2009年，文獻[2]針對KIST機器人，采用視覺和觸覺結合的方式識別門把手位置，在抓住門把手后試探晃動，根據機器人末端位置變化識別門軸位置，并在整個開門過程中使用力控制保障機器人的安全。2010年，文獻[3-4]針對PR2機器人，使用踝關節(jié)處的二維激光掃描儀進行導航，使用頭部帶云臺的二維激光掃描儀辨識門軸和門把手的位置等信息，然后規(guī)劃出一條無碰撞的運動軌跡完成開門。文獻[5]針對非結構化環(huán)境中未知的門，采用基于視覺的學習方法識別圖像中的門把手，然后使用立體相機獲取其三維坐標。2012年，文獻[6]對KUKA機器人設計笛卡爾空間下的阻抗控制，使用立體相機結合觸覺獲取精確的鎖孔位置，插入鑰匙打開門鎖，然后根據觸覺找到門把手，下壓門把手開門。2015年，DARPA機器人挑戰(zhàn)賽中，參賽的DRC-HUBO、CHIMP、Atlas等機器人紛紛在頭部安裝三維激光掃描儀和多個攝像機，用來掃描環(huán)境建立三維模型，進行開門作業(yè)[7]。

然而，更多的傳感器往往意味著更大更重的機器人，影響其可達區(qū)域。尤其是對于核電作業(yè)機器人，耐輻照要求使得相機等傳感器重達數公斤，受限于機器人負載能力，需要考慮使用較少的傳感器來完成開門作業(yè)。

對帶六自由度機械臂的履帶式移動機器人進行研究，規(guī)劃開門作業(yè)流程，使用安裝在機械臂末端的單目相機和安裝在車體上的二維激光掃描儀完成對門把手和門軸的識別定位，并設計力位混合控制方法，實現自主開門作業(yè)。

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

2.1 系統(tǒng)構成

研制的開門機器人主要由履帶式移動車體和六自由度機械臂構成，如圖1所示。該結構設計使得機器人具有較強的爬坡、越障、負載和多功能作業(yè)能力。在車體上安裝二維激光掃描儀、在機械臂末端安裝單目手眼相機，用于開門過程的導航和定位。二維激光掃描儀的功能，如圖2所示。能夠測量所在平面內的障礙物，用于避障行進；同時，結合相機對圖像的辨識，可以得到環(huán)境的部分三維信息。

圖1 機器人系統(tǒng)及坐標系定義Fig.1 Robot System and Its Coordinate Systems

圖2 二維激光掃描儀示意圖Fig.2 Illustration of the 2D Laser Scanner

2.2 機器人運動學

定義坐標系，如圖1所示。世界坐標系O-xyz固定在門軸處，與車體同高。

機器人坐標系O0-x0y0z0定義在車體中心，跟隨車體運動，與世界坐標系之間存在夾角θ0，其原點在O-xyz下的坐標為p0=［x0，y0，0］T，則二者之間的變換矩陣為：

末端坐標系O6-x6y6z6定義在末端工具處，機械臂各連桿之間的變換矩陣為T（i=1，…，6），則運動學方程為

二維激光掃描儀坐標系OL-xLyLzL與機器人坐標系同方向，其原點在 O0-x0y0z0下的坐標為0pL=［0xL，0yL，0zL］T，則有：

相機坐標系Oc-xcyczc定義在相機光心處，與末端坐標系存在夾角 θc，其原點在 O6-x6y6z6下的坐標為6pc=［6xc，6yc，6zc］T，則有：

2.3 力位混合控制

為實現機器人與環(huán)境之間的柔順接觸，避免硬碰撞，同時保證非接觸方向的位置運動精度，對機械臂設計力位混合控制算法如下：

式中：τ6×1—關節(jié)力矩控制量；x6×1=［x，y，z，rx，ry，rz］—機械臂末端的位置和姿態(tài)角；xd6×1—其期望值；J6×6—雅可比矩陣，用于將末端廣義力變換到關節(jié)力矩；對角陣 Kp6×6、Kd6×6—位置控制的比例系數矩陣和微分系數矩陣；g6×1—重力補償項；Fd6×1—末端廣義力的期望值；I6×6—單位陣；對角陣 Λ6×6為控制模式選擇矩陣，某行為1則代表對應的位置或姿態(tài)角選擇位置控制，某行為0則代表選擇力控制模式。

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

由于履帶式移動車體的精度遠低于機械臂，尤其是在轉彎運動中，車體的運動誤差容易導致機械臂受力過大損壞。因此在開門時盡量令車體靜止，必須運動時則令其勻速直線運動以減少誤差，同時機械臂配合車體運動。

自主開門流程設計如下：

（1）行進到門前

車體行進到門前停下，且門把手在相機視野內。

（2）門把手定位

使用手眼相機和二維激光掃描儀，獲取機器人和門把手之間的相對位置姿態(tài)。

（3）接近門把手

機械臂在位置控制模式下，移動到門把手正上方，并令末端工具垂直于門。

（4）開鎖

對機械臂使用力位混合控制，使末端工具下壓門把手，并跟隨門把手轉動，從而打開門鎖。

力位混合控制設計如下：令平行于門板的z自由度處于力控制模式，該方向的期望力豎直向下，用于下壓門把手；令垂直于門板旋轉的rx自由度處于力控制模式，該方向的期望旋轉力矩為零，即處在自由轉動狀態(tài)，以跟隨門把手的旋轉；其余4個自由度處于位置控制模式，期望位置（姿態(tài)）保持不變。

（5）輕微打開門并完成機器人定位

對機械臂使用力位混合控制，使機械臂輕微向前（推門）或向后（拉門）打開門。同時，使用二維激光掃描儀辨識門軸位置，完成機器人和門把手在世界坐標系下的定位。

力位混合控制設計如下：令繞豎直方向旋轉的rz自由度處于力控制模式，該方向的期望旋轉力矩為零，即處在自由轉動狀態(tài)，以跟隨門的旋轉；其余5個自由度處于位置控制模式，向前或向后運動。

（6）開門

根據門把手在世界坐標系下的坐標，計算門的運動軌跡。推門情況下，機械臂放開門把手，改用位置控制模式，根據機械臂自身運動范圍規(guī)劃出一條軌跡，推開門板；拉門情況下，令車體勻速后退，對機械臂使用和步驟（5）相同的力位混合控制，令機械臂沿門把手軌跡運動，打開門。

3 自主定位

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

3.1 門所在直線的辨識

對于二維激光掃描儀而言，門是一條線段，通過識別門所在直線（也即世界坐標系y軸），可得出機器人與門之間的距離x0和夾角 茲0。

二維激光掃描儀的數據是極坐標（r，琢）形式，如圖2所示。將其變換到直角坐標系OL-xLyL下，有：

對各個點進行如下Hough變換：

式中：茲—斜率；籽—截距。從而將數據從xLyL平面中的一個點投影成參數坐標系籽茲平面中的一條曲線。

在 籽茲平面內，找到各條曲線的聚集點（籽*，茲*），則Lxcos茲*+Lysin茲*=籽*即為門所在直線在OL-xLyL坐標系下的表示。

根據式（1）和式（2），可得：

值得注意的是，為獲取更準確的直線方程，在投影計算時采用較小的斜率間隔以提高分辨率。然而由于測量數據自身存在抖動誤差，較小的斜率間隔會導致籽茲平面內數據的分散，易受周圍環(huán)境干擾。為解決這一問題，令籽茲平面內各個點的值等于其周圍一定范圍內的點的值之和，使數據變得更凝聚，然后再計算最大值點作為聚集點。

3.2 門把手定位

為抓握門把手打開門鎖，需要辨識門把手的三維坐標。由于相機在深度方向的辨識精度不夠高，因此僅使用相機對門把手進行圖像平面內的辨識，然后結合二維激光掃描儀獲取的深度信息，得到更精確的門把手的三維坐標。

具體方法是：令機械臂帶著相機在世界坐標系yz平面移動，辨識得到相機在不同位置時門把手在圖像坐標系下的坐標，然后進行擬合，并聯合對門所在直線的辨識結果，得到門把手與機器人的相對位姿。為提高擬合精度，令機械臂移動過程中門把手逐漸靠近圖像中心。流程如下：

（1）不妨令y0=0，也即將世界坐標系沿y軸進行平移；

（2）按照3.1節(jié)方法，通過激光掃描儀數據計算x0和茲0，得到

（3）為提高圖像識別精度，在門和車體的夾角茲0已測得的情況下，根據茲0和茲c調整機械臂末端姿態(tài)，使相機正對門，即zc∥x且 xc∥y；

（5）使用尺度不變的模板匹配方法，從相機圖像中辨識門把手，得到門把手中心在圖像坐標系（原點在圖像中心）下的坐標（wh，hh）；

（6）在世界坐標系下平移機械臂末端，平移量為駐p6=（k1-k2i）［0，-sgn（wh），-sgn（hh）］T，其中 i表示第 i次執(zhí)行本步，k1和 k2為系數；

（7）重復步驟（4）～步驟（6）若干次；

（8）根據得到的一系列 yc、wh、zc和 hh，進行最小二乘擬合，得到 yc=kywh+by，zc=kzhh+bz。（by，bz）即為門把手在圖像中心時的相機坐標，則門把手中心在世界坐標系下的坐標為ph=［0，by，bz］T；

3.3 門軸識別和機器人定位

在機械臂輕微打開門過程中，門所在直線發(fā)生旋轉，通過對直線旋轉過程的擬合，可以得出轉軸（即門軸）位置，計算出y0，從而完成機器人和門把手在世界坐標系下的定位，用于門打開過程中對機器人進行軌跡規(guī)劃。

在門運動時，按照3.1節(jié)方法，通過二維激光掃描儀數據多次辨識門所在直線，得到OL-xLyL坐標系下的一系列直線…，n 表示第 i次辨識結果。

據此修正 y0：y0=-yh，修正門把手的坐標：ph=ph+［0，yh，0］T。

4 實驗

為驗證提出的機器人自主開門規(guī)劃控制和定位方法的有效性，進行自主開門實驗，如圖3所示。

在此實驗過程中的自主定位結果如下：首先使用3.1節(jié)方法得到：茲0=-3.5°，x0=-1006mm；然后使用 3.2 節(jié)方法得到：ph=［0，-8.5mm，803mm］；最后使用3.3節(jié)方法得到：yh=-y0=-474mm，據此修正：ph=［0，465.5mm，803mm］。對機器人和門進行實際測量可得，門把手坐標應為：ph=［0，459mm，800mm］，初始時機器人與門之間距離為1002mm。鑒于門把手坐標和機器人與門的距離分別反映了定位算法在門平面和法線方向的精度，對比實測值和辨識結果，可以看出所提方法能夠實現自主定位，且辨識誤差小于1cm。

圖3 開門實驗照片Fig.3 Pictures of the Door-Opening Experiment

5 結論

（1）為實現機器人在房間內外穿行所需的開門功能，提出一種基于力位混合控制的機器人自主開門規(guī)劃方法，該方法能夠降低接觸力，保障開門過程機器人和環(huán)境的安全。實驗表明該方法能夠完成自主開門。（2）為完成上述開門規(guī)劃所需的自主定位，設計一種使用單目手眼相機和二維激光掃描儀的門把手和門軸辨識方法，能夠在開門作業(yè)中完成對門的識別和機器人自主定位。實驗驗證該方法辨識精度優(yōu)于1cm，滿足開門需求。