結合多層次碰撞檢測與IKFast運動學的磨削平臺

方 健，尹 曠，王紅斌，張 鐵

（1.南方電網中低壓電氣設備質量檢驗測試重點實驗室（廣東電網有限責任公司廣州供電局電力試驗研究院），廣東廣州 510410；2.華南理工大學機械與汽車工程學院，廣東 廣州 510640）

1 引言

磨削[1]作為機械加工的尤為重要的工序，是對工件進行表面光整加工的可靠方法。磨削是在一定壓力下，通過磨削工具的磨料與工件相對運動，從工件表面切去極薄的材料，使其達到足夠的表面粗糙度和尺寸精度。可是，磨削環境由于金屬粉塵等的存在，對作業人員的健康不利。同時，低下的作業效率，參考不齊的加工質量，正進一步加快推動自動化模型平臺的發展，而機器人具有通用性和靈活性，成為當前自動化磨削系統的研究主流。

盡管機器人能成功替代工人進行磨削作業，但目前基于機器人磨削平臺多以拖動示教的工作模式[2]，自動化程度較低。并且編寫示教程序費事費力，并且靈活性較差，難以應對變化的加工環境，造成刻板的作業和低下的效率。為解決上述問題，從機器人磨削行程的運動規劃方法以及機器人磨削加工系統的軌跡生成問題進行研究，并提出了一種結合多層次碰撞檢測與IKFast運動學的高效磨削軌跡規劃算法。

機器人運動規劃[3]作為一種在障礙環境下，確定從機器人起始位姿到目標位姿的無碰撞的最優運動軌跡的方法，是當前機器人智能化技術的研究熱點，其中，最優性和完備性是機器人規劃中兩大準則Bug算法[4]作為經典規劃算法，由于其簡易性而被廣泛使用于二維游戲行業，但在障礙復雜的磨削環境，其單一繞行規則往往無法得到理想的結果。文獻[5]通過代價函數來通過節點計算最短路徑，但受節點數量的約束，因此只適用于簡單的規劃問題。由于A*算法[6]利用啟發式函數進行引導搜索，所以可以得到最優解，但隨著算法的不斷運行，其空間呈指數級增長。人工勢場法[7]引入物理學勢場，在目標區域產生吸引勢場，在障礙物區域產生排斥勢場，通過人工勢場函數得到無障礙的運動路徑。文獻[8]將蟻群算法應用于建立好的規劃場景柵格法模型，完成復雜環境下的實時路徑規劃。文獻[9]提出了一種面向路徑規劃的連續碰撞檢測算法，提高了路徑規劃中碰撞檢測的質量與效率。

盡管上述的經典方法在二維空間的規劃有效，但對于高維的磨削機器人平臺，復雜的障礙物的碰撞檢測問題要求將障礙物映射到高維構型空間，使得經典的規劃算法不能適用。近年來，基于采樣的規劃算法逐漸成為科研人員的研究重心。

基于采樣的規劃算法[10]采用碰撞檢測算法來對障礙物進行構造，而無需考慮障礙物在構型空間的分布情況，能較好地應對高維度的機器人運動規劃問題，成為這里的首選算法。概率路圖法PRM[11]作為多查詢算法，利用采樣無碰撞點進行建圖，接著由A*等搜索算法在路線圖得到最優路徑完成查詢過程。類似的多查詢算法有：lazyPRM[12]。

而單查詢算法具有求解速度快的優點。快速擴展隨機樹算法RRT[13]通過初始點擴張到目標點來獲得無障礙路徑，但搜索方式隨機，搜索空間龐大。文獻[14]引入了路徑代價的概念，提出了漸進最優的RRT*算法，但采樣空間大而收斂速度慢。為加快了收斂速度，文獻[15]引入了一種三角形優化存在的路徑，提出了一種RRT*-Smart算法。

基于采樣的運動規劃算法需對采樣點進行碰撞檢測以獲取障礙物信息，無需描述高維機器人規劃空間的障礙物。為得到一條無碰撞的路徑，碰撞檢測模塊需要在規劃中被反復調用，故其在算法求解的時間占比很大。需對碰撞檢測進行改進，以提高機器人磨削平臺的整體效率。碰撞檢測包含空間分解法和層次包圍盒法。層次包圍盒法通過逐漸細化的幾何體來進行層次遞歸的包圍判斷，具有良好的精度和效率，因此適用于復雜的障礙物。

需對機器人進行運動學分析，以生成機器人最終的磨削軌跡。常用方法為通過D-H參數對機器人運動學建模，并求解其正逆解。在逆解求解過程中，解析解精度比數值解高，但會遇到很多奇異點。基于文獻[16]只需建立基坐標系和末端坐標系，不需建立復雜繁瑣的D-H坐標模型，便能有效地分析運動學正逆解，規避部分奇異位置的同時具有高于數值解的精度，使機器人磨削更加穩定。

為克服傳統磨削系統中存在的低精度和低效率的缺陷，這里基于UR5機器人搭建自動磨削平臺，并提出一種結合多層次碰撞檢測與IKFast運動學的高效磨削軌跡規劃算法。研究內容如下：首先，為提高規劃算法的整體效率，基于混合包圍盒AABB和OBB建立碰撞檢測模型，通過多層次遍歷的檢測方法來進行層次遞歸的包圍判斷；

其次，為規避機器人磨削軌跡中奇異點并提高最終磨削軌跡的穩定性，建立IKFast運動學模型，以逆解仿真驗證的方式求解出機器人奇異位置的正確逆解。章節組織如下：第二節主要介紹我們層次遍歷的高效碰撞檢測算法，通過層次遞歸的判斷策略提高磨削效率；第三節分析生成軌跡中奇異點，并通過IKFast運動學模型減少奇異位置，提高磨削穩定性；第四節介紹磨削平臺，并通過實驗證實提出方法的優勢。

2 多層次碰撞檢測

考慮到基于人工示教的磨削方式自動化程度低，且差靈活性難以應對變化的加工環境，這里采用運動規劃的智能方式來獲得免示教的優化路徑以提高磨削系統的精度、效率以及靈活性。但經典的規劃算法往往只適用于二維空間，不適用于磨削環境中的高維空間。通過對隨機采樣點進行碰撞檢測，并利用基于采樣的規劃算法來獲取障礙物信息，解決高維構型問題。同時為提高規劃算法的整體精度和效率，基于層次遍歷的方法進行碰撞檢測。

2.1 基于采樣的運動規劃

對于磨削機器人的m維構型空間C，其可劃分為自由空間Cfree和障礙物空間Cobs，即C=Cfree∪Cobs。而磨削運動規劃問題可定義為，獲得一條連接初始構型qinit和目標構型qgoal的無碰撞路徑Pfree，即要求路徑中的所有構型必須位于Cfree，且不能與Cobs有任何交集。而基于采樣的運動規劃算法通過采樣、度量、最近領域、碰撞檢測、局部規劃器和父節點選擇一系列步驟找到一條可行的、無碰撞的路徑。

對于UR5機器人磨削運動規劃，首先采用均勻隨機采樣方法，如此獲得的構型可能位于自由空間Cfree和障礙物空間Cobs；接著，根據機器人連桿長度的不同，為每個關節加入權系數，通過歐幾里得度量來計算兩個構型間需要的勢能；然后根據隨機采樣點以及度量的勢能值，搜索最近鄰的節點；首先判斷兩個構型是否會發生碰撞，若不會，則進行連接；兩個構型以及它們的離散化中間構型均位于Cfree時，表示連接成功。最后采用直線連接的方式建立兩構型的連接，并對機器人運動路徑進行平滑處理以及父節點的選擇。

2.2 碰撞檢測技術

碰撞檢測是機器人規劃中保證其不發生嚴重碰撞事故的關鍵，但由于磨削障礙物的多樣性以及磨削效率的重要性，使得如何進行高效準確的碰撞檢測成為一大難題。為應對磨削過程的復雜障礙物，基于混合包圍盒AABB和OBB建立碰撞檢測模型，以多層次的遞歸判斷來兼顧效率與精度。

2.2.1 軸向包圍盒算法

軸向包圍盒采用四邊形對二維障礙物進行描述，和采用六面體對三維空間障礙物進行描述。因此磨削平臺采用最小六面體對三維空間障礙物進行描述，并滿足六面體每條邊平行于空間坐標軸，其表示形式如下：

式中：lx，hx，ly，hy，lz，hz—障礙物包圍盒在各坐標X，Y，Z的最小值和最大值。通過軸向包圍盒算法，僅需六個點便能完成對三維空間障礙物的進行描述，通過判斷兩包圍盒在各坐標軸的投影是否重疊，來進行碰撞檢測，當各個坐標軸的投影均發生相交，則認為存在碰撞。

雖然軸向包圍盒算法實現簡單，效率快，但其準確性受到物體旋轉的影響。例如：圖1平面空間兩物體A和B，虛線為其軸向包圍盒，由于投影結果x1x2與x3x4沒有相交，兩物體僅在Y軸投影有相交，因此可以判斷無碰撞；但當物體B旋轉為右圖后，投影結果和以及和均有交叉，則認為存在碰撞，但與事實相反，物體旋轉的結果影響了軸向包圍盒算法的準確性。

圖1 軸向包圍盒算法Fig.1 Axial Bounding Box Algorithm

2.2.2 有向包圍盒算法

有向包圍盒算法雖然同樣使用四邊形或立方體進行障礙物描述，但方向具有任意性，不受物體旋轉影響。對三條描述，但方向具有任意性，不受物體旋轉影響。對三條邊以上的多邊形進行三角剖分，用協方差和平均值來描述，設第i個三角形的頂點為（pi，qi，ri)，則均值與協方差矩陣如下：

式中：n—三角形個數；

Cjk—3×3協方差矩陣的各元素。

通過將協方差矩陣的特征規范化得到基向量，再由基向量的軸方向確定包圍盒。

要判斷包圍盒的碰撞情況，采用分離軸的方法，將包圍盒投影到分離軸上，得到A和B包圍盒的碰撞檢測分析圖，如圖2 所示。圖中分離軸的方向向量為l，中心點的方向向量為v，包圍盒A和B在分離軸的投影半徑為rA，rB，ai為包圍盒A在方向i的單位向量，ai則為包圍盒A在方向i的寬度的一半，bi和bi同理，則我們的判斷依據為：

圖2 有向包圍盒碰撞檢測Fig.2 Collision Detection of Oriented Bounding Box

若兩個包圍盒在分離軸的投影結果不重合，則兩物體一定不相交，而在15次判斷檢測都相交則認為兩包包圍盒存在碰撞。顯然，有向包圍盒得到的檢測結果更加準確，但計算量較大。

2.2.3 基于FCL的碰撞檢測

軸向包圍盒算法簡單而準確性差，有向包圍盒算法準確但計算量大，不同的場景需根據需要選擇不同的包圍盒。為充分利用包圍盒的優勢，基于FCL［17］為UR5 機器人建立層次包圍盒模型，如圖3（a）所示。其中，采用1個軸向包圍盒和6個有向包圍盒對機器人基座和關節連桿進行描述。對于基本的幾何體用一個單層包圍盒描述，而隨機幾何體用多層次包圍盒描述。而在包圍盒的碰撞檢測中，首先對包圍盒之間的碰撞進行粗判斷，若不相交則沒有發生碰撞；反之遞歸進行下一層次的判斷，通過逐次的判斷得出精確的碰撞情況，實現效率與精度的兼顧。

圖3 基于UR5機器人的碰撞檢測模型Fig.3 Collision Detection Model of UR5 Robot

此外，根據建立的層次包圍模型，對機器人碰撞檢測進行仿真，通過層次遞歸的判斷，快速地排除簡單無碰撞的情況，而對復雜的情況進行逐層細化的判斷，實現高效率、高精度的判斷，并得到仿真結果，如圖3（b）所示。紅色部分說明機器人末端和肘部發生碰撞。

3 基于IKFast的運動學建模

為得到生成機器人最終的磨削軌跡，常用的方法為通過DH參數對機器人運動學建模，并對其正逆解進行求解。通常采用數值解和解析解進行求解，可避免由于逆運動存在解不存在或不唯一、非線性等特征而導致逆解求解困難的問題。

解析解求解效率和精度更高，而數值解具有普適性，但這兩種方法均都無法避開奇異點。當機器人第二、三、四關節軸線共面且平行時，其肘部關節發生奇異；而當第四關節和第六關節平行時，機器人腕部位置發生奇異。對于這兩種常見的奇異情況，數值解由于雅可比矩陣存在 |J|=0 出現不可求逆情況，造成運算量大大增加，而解析解則由于代數關系無法獲得正確的解，因此降低磨削加工的穩定性。

IKFast運動學算法作為求解機器人運動學逆解中解析解的普適算法，適用于絕大部分串聯機器人的逆解求解過程，同時能有效規避奇異點的影響，提高磨削的穩定性，為此采用IKFast運動學算法對UR5機器人運動學分析。首先定義一個n關節機器人末端坐標系相對基座的坐標變換齊次矩陣，以實現平移變換和旋轉變換的分離：

式中：T1—常量變換矩陣；J1—一個帶參數關節值θi的變量矩陣，旋轉軸為：

或平移軸表示為：

式中：I4×4—4維單位矩陣；θi—繞軸vi的旋轉角度；ωi—與基坐標系同方向的第i個關節軸線的單位向量，且ωi=[ωix，ωiy，ωiz]T。

接著建立UR5機器人IKFast運動學模型，如圖4所示。基坐標系為紅色坐標系{B}，機器人末端坐標系為紅色坐標系{T}。由此我們可得到UR5機器人模型參數，如表1所示。其中，ωi—旋轉軸；pi—ωi-1到ωi相對基坐標系得平移變換向量。由此得到UR5機器人的IKFast正運動學公式：

表1 機器人的結構參數Tab.1 Structural Parameters of the Robot

圖4 IKFast的結構參數和坐標系建立Fig.4 Structure Parameters and Coordinate System Establishment of IKFAST

通過對式（4）進行移項操作可得到相應的約束公式：

式中：k1，k2—矩陣元素序號，通過對方程中cosθ=c，a*cosθ+b*sinθ=c，cos2θ=c等形式進行搜索，完成對逆解退化以及除零等判斷，以得到UR5機器人穩定的解析解，規避奇異點的影響，生成穩定的磨削軌跡。

4 實驗平臺及方法驗證

提出一種結合多層次碰撞檢測與IKFast運動學的高效磨削軌跡規劃算法，以提高磨削的整體效率、精度和穩定性。為驗證提出的方法的有效性，這里搭建了基于浮動平臺的機器人磨削系統，其包含浮動研磨裝置部分和機器人部分。浮動研磨裝置起到恒力控制器的作用，它通過上下浮動使磨削過程中保持恒力。機器人則作為運動規劃的執行器，夾持工件執行固定的磨削加工軌跡。機器人部分由UR5機器人、力傳感器、工件夾具和工件組成，如圖5所示。而浮動研磨裝置包括研磨盤、研磨電機、直線模組和伺服電機組成。為節省成本，我們選用一維力傳感器以感知工件表面法向的受力，并由工件夾具通過螺旋緊定夾緊工件。而浮動研磨臺由支持托架和滑塊連接伺服電機，通過電機控制直線模組實現位置控制，其精度理論上可達μm級。浮動平臺的重量由鋼絲繩和固定滑輪的重物進行補償。

圖5 浮動平臺的機器人磨削系統Fig.5 Robot Grinding System for Floating Platform

搭建了一個機器人磨削運動規劃實驗場景，如圖6所示。用于驗證算法的效率和精度。場景中，障礙物箱子長寬高為60×40×40，機器人夾持工件從箱子內無碰撞地運動到研磨臺的加工位置，磨削運動的起始位置的構型為[80.2，-115，-124，-30，87.6，172]，目標位置的構型為[188，-86.5，-124，-29.5，87.5，173.4]，單位為“°”。

圖6 機器人磨削運動規劃實驗平臺Fig.6 Experimental Platform for Robot Grinding Motion Planning

先在Rviz 中搭建與實驗場景對應的仿真平臺而后仿真結果，如圖7所示。綠色為障礙物，規劃的起始位置為灰色UR5機器人，規劃的目標位置為黃色的UR5 機器人，綠色的圓點線為規劃算法得到的末端運動路徑。同時，通過實物實驗，我們得規劃算法關節角變化曲線，如圖8 所示。由仿真實驗結果和實物實驗結果可知，本文提出的算法有效地限制了無效樹節點的采樣，減少了碰撞檢測的次數，同時機器人各關節角度變化平穩，規劃路徑平滑且避免無效路徑，使機器人能高效且準確地得到磨削軌跡。

圖7 機器人末端仿真軌跡Fig.7 Simulation Trajectory of the Robot End

圖8 機器人關節角變化曲線Fig.8 Change Curve of Robot Joint Angle

同時，采用IKFast運動學求解器來自動生成機器人逆解算法的C++文件，用以驗證文中提出的基于IKFast運動學逆解求解的有效性。在這里我們取UR5機器人的普通磨削位姿、腕部關節奇異位姿以及肘部關節奇異位姿來對驗證文中算法。普通磨削位姿的ZYX歐拉角表示為：x=-452.4mm，y=53.2mm，z=304.6mm，Rx=151.35°，Ry=5.52°，Rz=-79.31°，所得到的IKFast 逆解結果，如表2所示。

表2 普通磨削位姿逆解Tab.2 Inverse Solution of Common Grinding Position and Pose

腕部關節奇異位姿的歐拉角表示為：x=-433mm，y=135.6mm，z=338.7mm，Rx=90.0°，Ry=88.5°，Rz=172.5°，所得到的IKFast逆解結果，如表3所示。肘部關節奇異位姿的歐拉角表示為：x=-94.6mm，y=-162.1mm，z=969.6mm，Rx=0.00°，Ry=40°，Rz=-90.0°，所得到的IKFast逆解結果，如表4所示。

表3 腕部關節奇異位姿逆解Tab.3 Inverse Solution of The Singular Pose of Wrist Joint

表4 肘部關節奇異位姿逆解Tab.4 Inverse Solution of the Singular Pose of Elbow Joint

由表2～表4可以發現，基于IKFast的運動學方法無需建立復雜繁瑣的D-H坐標模型便能進行運動學正逆解分析，同時具有比數值解有更精確的解，且避免部分奇異位置，保證了機器人磨削加工軌跡的穩定性。

5 結論

為提高傳統磨削系統的精度、效率和穩定性，提出一種結合多層次碰撞檢測與IKFast運動學的高效磨削軌跡規劃算法，并成功運用于機器人磨削平臺。

首先，通過混合軸向包圍盒算法與有向包圍盒算法搭建了多層次的碰撞檢測模型，通過逐層遞歸的檢測判斷來達到整體效率與精度的兼顧。

接著，構建了IKFast運動學模型，該模型規避了部分奇異位置的影響，無需建立復雜繁瑣的D-H坐標模型便能通過運動學正逆解分析得到精度高于數值解的解，解決了磨削軌跡生成過程中存在的奇異問題，有利于磨削系統穩定性的提高。

最后，基于UR5機器人搭建基于浮動平臺的機器人磨削系統，并進行規劃實驗與軌跡生成實驗，證實了本文提出方法的高效性和優異性。