王 歡，門 濤，茍維東，張子平，姜 勇

（1.鞍鋼集團(tuán)礦業(yè)有限公司能源管控中心，鞍山110168；2.中國科學(xué)院沈陽自動(dòng)化研究所 機(jī)器人國家重點(diǎn)實(shí)驗(yàn)室，沈陽110016；3.中國科學(xué)院機(jī)器人與智能制造創(chuàng)新研究院，沈陽110016）

隨著柔性制造及人機(jī)協(xié)作概念的提出，機(jī)器人的工作環(huán)境由傳統(tǒng)的結(jié)構(gòu)化封閉環(huán)境更多地轉(zhuǎn)變?yōu)榕c人共享工作空間的非結(jié)構(gòu)化環(huán)境。在這樣的環(huán)境中，協(xié)作型機(jī)器人因其輕質(zhì)、靈活易用，能與人協(xié)同作業(yè)等特性而具有了更加廣闊的應(yīng)用前景[1-2]。人機(jī)協(xié)作過程中，首要考慮的是安全問題。目前，為了保證人機(jī)安全，在協(xié)作型機(jī)器人的設(shè)計(jì)中常采用輕量化和柔順性設(shè)計(jì)，并且在控制策略中加入功率和力限制，以保證機(jī)器人在與人發(fā)生碰撞時(shí)能及時(shí)停止，避免對(duì)人身造成更大的傷害[3]。當(dāng)觸碰到人體時(shí)，機(jī)器人將暫停工作進(jìn)程，如果人體障礙一直存在，那么機(jī)器人將一直保持準(zhǔn)靜止?fàn)顟B(tài)，其工作效率會(huì)受到極大的影響。因此，實(shí)時(shí)檢測(cè)機(jī)器人與人之間的距離，當(dāng)距離過近時(shí)，機(jī)器人將人視為障礙進(jìn)行躲避，避免人機(jī)碰撞的發(fā)生，這樣既可保證安全性，又不會(huì)對(duì)機(jī)器人的工作進(jìn)程產(chǎn)出太大影響。但這個(gè)動(dòng)態(tài)的過程中，外界環(huán)境是不可預(yù)知的，需要依賴可靠的檢測(cè)和避障控制方法[4-5]。

避障控制一直是機(jī)器人領(lǐng)域的研究熱點(diǎn)，常見的避障控制方法主要分為兩類：全局避障和局部避障。其中，全局避障方法主要包括構(gòu)型空間法、C 空間法、A* 算法、單元分解法等[6-8]。全局避障方法需要全局建?；蛩阉髯顑?yōu)解，需要全局信息，計(jì)算量較大，并且不能應(yīng)對(duì)突然出現(xiàn)的動(dòng)態(tài)障礙物，因此不適合人機(jī)協(xié)作場(chǎng)景下的避障控制。局部避障方法最具代表性的是人工勢(shì)場(chǎng)法，即將機(jī)器人視為在虛擬的人工受力場(chǎng)中運(yùn)動(dòng)，障礙物對(duì)機(jī)器人產(chǎn)生斥力，目標(biāo)點(diǎn)對(duì)機(jī)器人產(chǎn)生引力，引力與斥力的合力控制機(jī)器人的運(yùn)動(dòng)。人工勢(shì)場(chǎng)法具有計(jì)算量小、實(shí)時(shí)性高等優(yōu)點(diǎn)，但傳統(tǒng)的人工勢(shì)場(chǎng)法依然無法解決動(dòng)態(tài)避障問題。為此，學(xué)者們提出了排斥向量法等改進(jìn)算法[9-12]，但算法復(fù)雜度較高，實(shí)時(shí)性差。另外，此類局部避障方法由于缺乏全局信息，因此控制過程中容易陷入局部最小值點(diǎn)，造成機(jī)器人震蕩或停滯不前。文獻(xiàn)[7]提出了一種基于障礙物與機(jī)器人位置圖像尋找最小距離的方法，該方法通過遍歷圖像中與機(jī)器人有關(guān)的凸包，直至找到障礙物，獲得機(jī)器人與障礙物的最小距離。該方法雖然可以避免陷入局部最小值點(diǎn)，但使用圖像獲得的是兩點(diǎn)之間的距離信息，該距離信息僅能使機(jī)器人以停止運(yùn)動(dòng)的方式避障，避障之后機(jī)器人不能繼續(xù)運(yùn)動(dòng)到目標(biāo)點(diǎn)。文獻(xiàn)[8]使用Kinect 深度傳感器獲取障礙物與機(jī)器人的深度信息來估計(jì)機(jī)器人與障礙物的距離，然后將該距離引入人工勢(shì)場(chǎng)法中，作為機(jī)器人的排斥指令進(jìn)行避障。該方法始終是在笛卡爾空間控制機(jī)器人避障，在避障過程中機(jī)器人有可能因到達(dá)奇異點(diǎn)無法運(yùn)動(dòng)，而導(dǎo)致避障失敗。

針對(duì)上述避障控制問題，本文提出一種基于速度修正的協(xié)作機(jī)械臂動(dòng)態(tài)避碰控制方法。該方法將Kinect 檢測(cè)的機(jī)器人與人體障礙的距離轉(zhuǎn)化為虛擬力，作為機(jī)器人導(dǎo)納控制的輸入，控制機(jī)器人下一時(shí)刻的速度進(jìn)行避障。人體障礙運(yùn)動(dòng)速度過大時(shí)，通過速度修正控制機(jī)器人的運(yùn)動(dòng)方向來進(jìn)行緊急避障。最后，利用人機(jī)協(xié)作測(cè)試平臺(tái)對(duì)機(jī)械臂的動(dòng)態(tài)避碰控制方法進(jìn)行了實(shí)驗(yàn)驗(yàn)證。

1 Kinect 標(biāo)定與人體障礙檢測(cè)

Kinect 傳感器由IR 紅外元件、RGB 圖像元件及Mic 麥克風(fēng)矩陣構(gòu)成，可采集深度（紅外）、色彩和音頻信息。其中色彩幀分辨率為1920×1080，深度幀分辨率為512×424，同時(shí)提供了C++語言環(huán)境的音頻數(shù)據(jù)處理、身體數(shù)據(jù)索引、坐標(biāo)系轉(zhuǎn)換（Camera/Color/Depth）、色彩及深度信息獲取等基本例程。

Kinect 標(biāo)定是求解相機(jī)模型參數(shù)的過程，也是人體障礙檢測(cè)中的重要環(huán)節(jié)。其中，相機(jī)模型可近似為典型的針孔成像模型，如圖1 所示。

圖1 針孔相機(jī)模型Fig.1 Pinhole camera model

設(shè)笛卡爾空間中任一點(diǎn)P 在世界坐標(biāo)系中的坐標(biāo)為Pw=（xwywzw）T，經(jīng)過旋轉(zhuǎn)和平移變換后，可得到相機(jī)坐標(biāo)系下的對(duì)應(yīng)點(diǎn)Pc=（xcyczc）T，變換關(guān)系如下：

式中：R3×3和t3×1分別是旋轉(zhuǎn)變換矩陣和平移變換矩陣。

P 點(diǎn)映射在相機(jī)坐標(biāo)系和像素坐標(biāo)系中的點(diǎn)Pc和Pu具有如下變換關(guān)系：

式中：fx=f/dx，fy=f/dy。dx和dy分別表示每個(gè)像素在x′和y′軸上的長(zhǎng)度。

據(jù)式（1）和式（2），可得笛卡爾空間中的點(diǎn)P 在像素坐標(biāo)系中的投影Pu=（pupv）T為

僅用像素坐標(biāo)Pu無法獲知點(diǎn)P 的深度信息，因此引入三維深度矩陣Dep=（pupvpd）T，其中pu和pv是P 點(diǎn)的像素坐標(biāo)，pd是P 點(diǎn)到相機(jī)平面的距離，即：

利用深度矩陣不僅可以得到笛卡爾空間點(diǎn)的像素信息，也可以獲得其深度信息，實(shí)現(xiàn)笛卡爾空間點(diǎn)數(shù)據(jù)與Kinect 數(shù)據(jù)之間的轉(zhuǎn)換。

為了獲取機(jī)械臂與人體障礙之間的距離，需要獲知在同一坐標(biāo)系下兩者的位置關(guān)系，為此將相機(jī)世界坐標(biāo)系上的人體障礙位置轉(zhuǎn)換到機(jī)械臂基坐標(biāo)系中。選取距離機(jī)械臂最近的人體部位作為障礙物，并將其位置通過轉(zhuǎn)換矩陣（R｜t）轉(zhuǎn)換到機(jī)械臂基坐標(biāo)系中得到障礙點(diǎn)位置O=（xo，yo，zo）。

機(jī)械臂末端執(zhí)行器的位置設(shè)為P=（xp，yp，zp）。通過空間向量法，計(jì)算出機(jī)械臂末端執(zhí)行器與人體障礙之間的距離如下：

如圖2 所示，基于Kinect 的圖像和深度信息可以識(shí)別出人體框架和機(jī)械臂位置，計(jì)算出機(jī)械臂末端執(zhí)行器與人體障礙的距離。

圖2 基于Kinect 的人機(jī)距離獲取Fig.2 Kinect-based human-machine distance acquisition

2 基于速度修正的避障控制

2.1 距離估計(jì)

控制點(diǎn)P 與障礙點(diǎn)O 之間的相對(duì)運(yùn)動(dòng)信息是成功避障的一個(gè)充分條件。將Kinect 捕獲到的障礙點(diǎn)O 用深度矩陣表示為：OD=（ouovod）T，控制點(diǎn)P表示為P=（pupvpd）T，使用如下方法估計(jì)障礙點(diǎn)與控制點(diǎn)在笛卡爾空間中的距離：

式中：Δx 為控制點(diǎn)P 與障礙物點(diǎn)O 之間的距離。在避障過程中，過遠(yuǎn)的障礙點(diǎn)對(duì)控制點(diǎn)P 當(dāng)前運(yùn)動(dòng)狀態(tài)的影響非常小，可忽略不計(jì)。因此劃定一個(gè)以控制點(diǎn)P 為中心，半徑為ρ 的球形區(qū)域，去掉過遠(yuǎn)的障礙點(diǎn)。在該區(qū)域內(nèi)，基于最短距離進(jìn)行避障，以像素點(diǎn)（pu，pv）為中心，向外進(jìn)行遍歷，來獲取新的局部最小值。

2.2 導(dǎo)納控制

采用基于位置的阻抗控制，即導(dǎo)納控制來控制機(jī)械臂避障。在導(dǎo)納控制中，末端執(zhí)行器位置與環(huán)境作用力之間的動(dòng)態(tài)導(dǎo)納關(guān)系如下所示：

式中：Fe為外力項(xiàng)；xd，為機(jī)械臂期望的運(yùn)動(dòng)信息；x0，為規(guī)劃的運(yùn)動(dòng)信息；Mn×n，Bn×n，Kn×n分別為常數(shù)慣性矩陣、阻尼矩陣與剛度矩陣，這些矩陣決定了外力Fe與機(jī)械臂運(yùn)動(dòng)信息之間的動(dòng)態(tài)特性。

在拉氏域中，式（7）的導(dǎo)納控制關(guān)系可描述如下：

式中：

常規(guī)導(dǎo)納控制器中輸入的是傳感器測(cè)量的外力，但是當(dāng)傳感器精度較低時(shí)，無法在碰撞瞬間測(cè)量出準(zhǔn)確的實(shí)際受力值，加之導(dǎo)納控制器存在的時(shí)延，導(dǎo)致人機(jī)碰撞時(shí)機(jī)器人不能及時(shí)停止而對(duì)人身造成傷害。因此本質(zhì)上，基于實(shí)際外力的導(dǎo)納控制無法滿足人機(jī)協(xié)作安全性要求。而通過Kinect 實(shí)時(shí)檢測(cè)控制點(diǎn)與障礙點(diǎn)的相對(duì)運(yùn)動(dòng)信息，并將其轉(zhuǎn)化為虛擬力代替實(shí)際的外力，輸入到導(dǎo)納控制器中，控制機(jī)器人通過避障來避免實(shí)際碰撞的發(fā)生，可以從根本上保障人員的安全[10]。

虛擬力的定義如下：

其方向與Δx 相同，幅值定義為

式中：當(dāng)‖Δx‖＝0 時(shí)，虛擬力的最大幅值為Vmax。需要注意的是必須滿足‖Δx‖＜ρ 的條件，因?yàn)楫?dāng)‖Δx‖＝ρ 時(shí)，幅值為0。式（10）中虛擬力的幅值與距離的關(guān)系如圖3 所示，當(dāng)Vmax=2 m/s，ρ＝0.4 時(shí)，虛擬力的幅值隨著距離的增大而減小。

圖3 距離與虛擬力幅值關(guān)系圖Fig.3 Admittance control block diagram

導(dǎo)納控制中，虛擬力代替實(shí)際力作為控制系統(tǒng)的輸入，經(jīng)過導(dǎo)納控制得到的是控制點(diǎn)與障礙點(diǎn)之間的相對(duì)速度，即：

另設(shè)，

由于障礙點(diǎn)的位置信息與Δx 有關(guān)，因此使用Δx 求導(dǎo)所得的速度估計(jì)障礙點(diǎn)的運(yùn)動(dòng)速度，即：

則避障過程如圖4 所示。

圖4 導(dǎo)納控制的在線避障Fig.4 Admittance control online obstacle avoidance

2.3 速度修正

由虛擬導(dǎo)納控制所得的控制量是控制點(diǎn)P 避開障礙點(diǎn)O 的速度向量V（P），當(dāng)障礙點(diǎn)O 向控制點(diǎn)P 的移動(dòng)速度大于控制點(diǎn)的避障速度時(shí)，會(huì)存在避障不及時(shí)的問題。因此，當(dāng)無法及時(shí)調(diào)整控制點(diǎn)的速度大小時(shí)，通過調(diào)整速度的方向來解決該問題。文獻(xiàn)[8]中通過在控制點(diǎn)運(yùn)動(dòng)的方向上施加與當(dāng)前方向近似垂直的方向向量來克服此類情況，施加的方向向量與控制點(diǎn)和障礙點(diǎn)之間的角度有關(guān)。

向量n 與Γ（P）和V（P）所屬的平面Π 垂直，向量v 與n 和V（P）正交。向量（α，n，v）（α，n 指定的正交基）用來修正平面Π 上V（P）的方向。速度向量V（P）的幅值變化函數(shù)（由正標(biāo)量c 所形成）由平面Π 上的速度向量的角度γ 決定，當(dāng)速度向量的幅值變化量為0 時(shí)，函數(shù)值為β；當(dāng)速度向量的變化量趨于最大允許變量值Vmax（O）時(shí)，該函數(shù)值收斂于π/2。

圖5 避障方向修正過程Fig.5 Obstacle avoidance direction correction process

由此可知，使用方向修正量Vpivot（P）對(duì)控制點(diǎn)P的避障方向進(jìn)行在線修正，使其在遇到高速運(yùn)動(dòng)的障礙物時(shí)，通過向Vpivot（P）的方向運(yùn)動(dòng)進(jìn)行避障，可以明顯提高機(jī)械臂避障的成功率。

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

3.1 實(shí)驗(yàn)設(shè)計(jì)

機(jī)械臂的控制和軌跡規(guī)劃均在笛卡爾空間進(jìn)行，虛擬導(dǎo)納控制系統(tǒng)的控制對(duì)象為機(jī)械臂的末端執(zhí)行器。障礙物為實(shí)驗(yàn)者的左手。障礙點(diǎn)與控制點(diǎn)之間的距離由一臺(tái)Kinect V2 傳感器實(shí)時(shí)測(cè)量，其捕獲像素為640×480，深度圖像的頻率為30 Hz。實(shí)驗(yàn)中球形避障區(qū)域的半徑為0.35 m。障礙物與目標(biāo)點(diǎn)距離小于0.35 m 時(shí)，虛擬導(dǎo)納控制系統(tǒng)有虛擬力輸入，輸出給末端執(zhí)行器的為下一時(shí)刻的避障速度指令，直至目標(biāo)點(diǎn)遠(yuǎn)離障礙點(diǎn)的距離超過0.35 m。以下實(shí)驗(yàn)中，虛擬力幅值參數(shù)為Vmax=4 m/s，ρ＝0.35 m。

3.2 實(shí)驗(yàn)分析

實(shí)驗(yàn)分為2 組，第一組為靜態(tài)障礙物避障實(shí)驗(yàn)，第二組為動(dòng)態(tài)障礙物避障實(shí)驗(yàn)。

第一組實(shí)驗(yàn)過程如圖6 所示，整個(gè)過程中障礙物處于靜止?fàn)顟B(tài)。機(jī)械臂運(yùn)動(dòng)初始時(shí)刻，障礙物與目標(biāo)點(diǎn)之間的距離為0.435 m，機(jī)械臂無需避障；而當(dāng)機(jī)械臂沿規(guī)劃軌跡運(yùn)動(dòng)遇到處于靜止的障礙物時(shí)，障礙物進(jìn)入球形避障區(qū)域，機(jī)械臂開始避障。

圖6 靜態(tài)障礙物避障Fig.6 Static obstacle avoidance

機(jī)械臂的初始軌跡和遇到靜態(tài)障礙物時(shí)的軌跡如圖7 所示。

圖7 機(jī)械臂的靜態(tài)障礙物避障軌跡Fig.7 Static obstacle avoidance trajectory of robotic arm

第二組實(shí)驗(yàn)過程如圖8 所示，機(jī)器人沿規(guī)劃軌跡運(yùn)動(dòng)遇到側(cè)邊快速運(yùn)動(dòng)的障礙物時(shí)（障礙點(diǎn)的移動(dòng)速度明顯大于控制點(diǎn)的避障速度），控制點(diǎn)沿速度修正量修正后的方向運(yùn)動(dòng)，避開障礙點(diǎn)后重新回到規(guī)劃軌跡上。

圖8 動(dòng)態(tài)障礙物避障Fig.8 Dynamic obstacle avoidance

圖9 為機(jī)械臂在遇到動(dòng)態(tài)障礙物時(shí)的運(yùn)行軌跡。

圖9 機(jī)械臂的動(dòng)態(tài)障礙物避障軌跡Fig.9 Dynamic obstacle avoidance trajectory of robotic arm

由以上實(shí)驗(yàn)可以看出，使用基于速度修正的虛擬導(dǎo)納控制避障方法，機(jī)械臂遇到靜止和高速運(yùn)動(dòng)的障礙物均可以順利完成避障過程。同時(shí)，與文獻(xiàn)[8]相比，控制點(diǎn)和障礙點(diǎn)的距離超過球形避障區(qū)域的半徑時(shí)，機(jī)械臂可以重新運(yùn)動(dòng)到規(guī)劃軌跡上繼續(xù)工作進(jìn)程。

4 結(jié)語

針對(duì)人機(jī)協(xié)作環(huán)境下機(jī)械臂的動(dòng)態(tài)軌跡控制問題，提出了一種基于速度修正的協(xié)作機(jī)械臂動(dòng)態(tài)避碰控制方法。其核心思想是，通過捕捉人體障礙與機(jī)械臂之間的距離信息，利用虛擬導(dǎo)納控制器，分別對(duì)靜態(tài)和動(dòng)態(tài)障礙物進(jìn)行避障控制。當(dāng)障礙物為靜止?fàn)顟B(tài)時(shí)，直接采用等效虛擬力的導(dǎo)納控制完成避障；當(dāng)障礙物為運(yùn)動(dòng)狀態(tài)時(shí)，在導(dǎo)納控制的基礎(chǔ)上引入機(jī)械臂速度空間的修正算子，利用速度修正控制機(jī)械臂緊急避障。最后通過在Kinect V2 和UR5 機(jī)械臂平臺(tái)上開展的驗(yàn)證實(shí)驗(yàn)，證明了該方法的實(shí)現(xiàn)可行性與有效性。