基于三維點云的雙機器人協作打磨軌跡規劃研究

周 震，姜笑言，韓家哺

（上海工程技術大學 機械與汽車工程學院，上海 201620）

0 引言

隨著現代智能化產業的日益發展，對生產領域的自動化程度提出了更高要求，機器人在工業應用中占據重要角色。智能化是工業機器人發展的方向，協作是未來工業機器人發展的必然選擇［1］。由于多機器人協作在路徑空間和靈活適應性等方面優于單機器人，且在同一系統中具有互相協作的優勢，使其工程應用十分廣泛。多機器人協作主要涉及協作機器人的基坐標系標定，軌跡規劃及位置／力協同控制，控制與避碰等方向。而雙機器人協作也有廣泛關注和研究，Liu 等［2］以雙臂搬運方塊為研究對象，在雙臂末端與物體之間分別設置位置約束、速度約束和加速度約束進行優化，實時補償了運動誤差，但由于較強的約束導致雙臂運動范圍極其有限；Cohen 等［3］在高維度空間利用雙臂構造搜索空間，采用啟發式搜索算法，解決雙臂末端軌跡光滑問題，但會產生抖動現象；于廣東等［4］對雙機器人同步協調運動和相對協調運動做了詳細的分析，并對這兩類協調運動進行簡單的實驗；Zhang 等［5］使用三維激光掃描儀掃描葉片形成了密集點云，并通過點云預處理算法和切片算法，生成機器人的磨拋軌跡，但點云數據處理的算法復雜；侯仰強等［6］建立雙機器人協調焊接系統，分析影響焊縫質量及機器人平穩性的各參數，建立了評價無干涉焊接路徑的目標函數，但該方法只局限于焊接類似“馬鞍式”焊縫的焊接任務。

本文在吸取借鑒前人研究的同時，打破了輪轂打磨設備靈活性差、復雜空間軌跡難以獲取、方法適用領域單一等局限性，以期加深對多機器人協作系統及軌跡規劃的研究。通過構建雙機器人協作打磨坐標系和運動學約束關系，對采集的三維點云數據預處理，規范標準化姿態，進行輪轂表面特征線的提取，獲得理想的空間打磨軌跡。對模型及方法進行仿真實驗驗證，分析雙機器人相對運動協調打磨的可行性。

1 雙機器人協作打磨運動學模型

1.1 協作打磨各坐標系的建立

在主從式雙機器人協作輪轂打磨過程中，夾持輪轂的機器人作為主機器人，從機器人夾持磨具，以相對協調的運動方式對預定輪轂軌跡點跟蹤打磨。根據主機器人與從機器人末端位姿的相對變換矩陣是否時變，可將協調運動分為緊耦合和松耦合形式［7］。緊耦合協調運動時末端位姿矩陣相對不變，應用于協同貨物搬運等任務；松耦合協調運動時相對末端位姿矩陣時刻變化，運動軌跡較為復雜，應用于協作焊接、包裝碼垛、裝配等任務。

雙機器人系統坐標系如圖1 所示，定義n ＝1 為夾持機器人，n ＝2 為打磨機器人，選取主機器人基座標系 [B1] 為世界坐標系 [S]，則 [B2] 為從機器人基坐標系。 [En] 為機器人的末端坐標系，[Tn] 為工具坐標系，[ w] 為打磨離散點坐標系。

圖1 雙機器人系統坐標系Fig.1 Coordinate system of dual robot systems

1.2 協作打磨任務運動學關系

雙機器人相對運動協作打磨是兩臺機器人相互協調完成既定打磨任務，不僅存在末端位姿約束，還存在相對運動約束。在雙機器人協作打磨系統中，兩臺機器人形成的一條閉合運動鏈，可解耦成兩條開鏈形式［7］。一條運動鏈由夾持機器人的“夾持機器人基底-末端法蘭-工具-輪轂-打磨軌跡”組成；另一條由打磨機器人的“夾持機器人基底-打磨機器人基底-末端法蘭-工具-工件-打磨軌跡”組成。研究雙機器人運動學關系就是研究運動鏈的耦合和解耦。雙機器人運動的表達形式就是用各個坐標系的轉換找到坐標系之間矩陣變換關系實現的，根據相對協調運動時的打磨任務要求，整體的閉合運動鏈模型為

將式（1）所示運動鏈解耦，分別得到兩組機器人的開鏈模型表達式為式（2）和式（3）：

2 雙機器人協作打磨軌跡規劃

2.1 空間復雜輪轂點云特征線的提取

在自動化打磨生產過程中，需要對輪轂表面不同區域進行路徑分析。打磨路徑一般可分解為直線或規則圓弧，通過插補算法將軌跡離散成一系列空間打磨軌跡點，機器人末端依次跟蹤序列點完成打磨任務。而復雜不規則的空間曲線，由于輪轂制造廠商對于非標工件知識產權的維護，不提供相應的CAD／CAM 三維模型，則不能獲得輪轂結構和空間曲面信息。本文通過激光掃描臂對輪轂精細掃描，逆向成型獲得輪廓三維點云數據，并利用基于姿態標準化的點云特征線方法提取空間曲線特征信息。

2.1.1 構建輪轂的三維點云模型

要獲得打磨作業時末端執行器運動軌跡和精準的位姿信息，前期點云模型構建的準確度非常關鍵。利用FARO 激光掃描臂，通過逆向工程的方式對無CAD 三維模型的輪轂結構進行數字化掃描。先用掃描儀器的掃描探頭在輪轂中心軸的最低點采集3個點，構造一個平面，并以底部圓平面中心點的位置建立基準坐標系，隨后手持掃描儀器對輪轂各個區域來回往復掃描。采集的輪轂點云模型總坐標點數為886 921 個，激光掃描臂實驗過程和初始輪轂三維點云模型如圖2、圖3 所示。

圖2 激光掃描臂實驗Fig.2 Laser scanning arm experiment

圖3 初始輪轂三維點云模型Fig.3 Initial Hub three-dimensional point cloud model

2.1.2 輪轂點云的姿態標準化

激光掃描臂采集的原始輪轂點云模型會存在點云數據過于密集或冗余、噪點等問題。利用掃描臂適配的PolyWorks 軟件中的點云處理命令對模型進行簡單預處理，過濾噪點，去除多余的掃描數據使點云數據均勻化。處理后的點云數據呈現無序狀態，確定點云的姿態首先要快速搜索臨近點云，提高后期計算效率。點云鄰域搜索主要用KD-TREE 算法實現，通過在基準參考坐標系上構建的決策樹搜索臨近點云［8］。為了消除點云姿態對輪轂曲面擬合誤差的影響，應先調整點云姿態，避免不同姿態導致的擬合誤差問題。

設得到的臨近點云數據為：A ＝｛（xi，yi，zi）｜i ＝1，2，3，…，n｝，將數據擬合成形如z ＝f（x，y）的目標曲面方程，則曲面擬合誤差為

由式（6）可看出，當模型中各點云的坐標系方向不同時，目標方程會有所差別，且擬合誤差也受影響，所以要規范姿態標準化。點云特征信息的主方向可以表達點云分布情況，當點云數據的主方向與基準坐標系Z軸重合，則在XOY平面區域分布更平均，姿態標準化后曲面擬合效果更好。臨近點云A的主方向Dmain可用PCA（Principal Component Analysis）算法得到，先計算點云A的協方差矩陣，并對矩陣特征值分解，得出特征值λ1、λ2、λ3和特征向量α1、α2、α3，則點云主方向Dmain可表示為

利用Householder 矩陣求得轉換矩陣R，使點云主方向Dmain與坐標系Z軸對準，則姿態標準化后的點云A為

2.1.3 點云特征線的提取

工件型面一般都有明顯的幾何特征，這些幾何特征能反映工件表面輪廓的一些空間曲線、曲面等。本文點云特征線提取是基于輪轂表面結構的最大主曲率絕對值作為擬合曲面曲率的估計值，識別提取出表面輪廓邊界的曲線、棱邊、尖角、連接過渡等形狀特征。工件的點云特征曲線同樣也是輪轂分區域分塊打磨的依據。

將姿態標準化后的坐標點云數據用二次曲面方程進行曲面擬合為

曲面擬合的系數為

令X ＝［a，b，c，d，e，k］T，則式（10）中的系數可表示為：

由于計算曲率的坐標點Ai（xi，yi，zi）不是擬合曲面上的點，則需將Ai沿著點云主方向向曲面位置投影，可得投影點At i的矩陣，式（11）：

令：

若存在式（12）：

當等式（12）成立時，主曲率值滿足方程（13）［9］：

將式（12）、（13），解方程得曲面投影點的主方向曲率為k1，k2。結合輪廓點云特征，利用最大主曲率絕對值作為擬合曲面曲率的估計值，提高可區分性，使點云特征線與非特征線的曲率的估計值差別更大，更易識別提取點云，則曲率可表示為

將激光掃描臂實驗采集的輪轂三維點云與處理后特征線提取的數據文件，導入到Matlab 中編寫程序，進行可視化分析。初始輪轂點云模型與數據處理后模型如圖4、圖5 所示，對比結果表明所用算法能較精準清晰的表達輪轂特征線，為后續待打磨點的選擇提供依據。

圖4 初始輪轂點云模型Fig.4 Initial hub point cloud model

圖5 特征線提取后點云模型Fig.5 Point cloud model after feature line extraction

2.2 輪轂空間打磨軌跡規劃

在雙機器人協作打磨過程中，選取待打磨點能在一定程度上表達空間軌跡的所有特征。完成高質量連續打磨要求不僅與打磨方案和工藝參數有關，更要建立協調運動中待打磨軌跡點與打磨工具位姿模型及相對位姿的關系。

2.2.1 打磨點位姿數學模型

從精簡后的點云數據中，選取期望的20 個待打磨軌跡坐標點。利用多項式擬合獲得關于時間t（0 ≤t≤10）的空間打磨點軌跡方程組，式（15）：

令打磨點位置矩陣Pi＝xi yi zi]T，其中t的單位為s，坐標系的單位為mm。

式（15）擬合的打磨軌跡是由離散點（待打磨點）連接而成。基于打磨點的空間位姿求取，在每個打磨點上建立坐標系，并通過世界坐標系與工件打磨點坐標系的相對位姿矩陣關系來確定系列打磨點的位姿。位置是該打磨點在輪轂工件坐標系下的相對位置，姿態是空間打磨軌跡在該打磨點上的切向和法向。以擬合的輪轂空間曲線為例，建立的待打磨點坐標系如圖6 所示，［T1］為輪轂工件坐標系，［wi］為第i個打磨點坐標系。由機器人工件坐標系、待打磨點坐標系相對姿態關系作以下規定：空間離散打磨點的法向量u令作打磨點坐標系的X軸，打磨點的切向量v令作坐標系的Y軸并指向下一點離散點方向，法向量u和切向量v的矢量叉乘得到的矢量w是打磨點坐標系［wi］的Z軸。各個點不同的位姿信息共同構成協調打磨軌跡運動。

圖6 待打磨點位姿坐標系Fig.6 Position of point to be polished system

空間姿態變化表明，對打磨點姿態調整就是通過機器人末端夾持對工件姿態的調整。利用ZYX序列的歐拉角表示從上一個打磨點到下一個打磨點坐標系調整后的旋轉矩陣Ri為

由式（15）、（16）知，最終得到任意打磨點i在輪轂工件坐標系［T1］下的位姿矩陣可表示為

2.2.2 打磨工具位姿數學模型

在雙機器人相對運動協調打磨任務中要獲得好的打磨效果，打磨工具的姿態須與打磨點的矢量方向一致，打磨頭也要和打磨點位置重合。在打磨時機器人夾持輪轂，經過輪轂位姿的調整達到每個待打磨點理想的位姿，打磨工具從上一個點到下個打磨點也要求調整至理想位姿。令打磨工具坐標系［T2］的Y軸方向與打磨軌跡上點的切向相同并指向下一點是工具的運動方向；［T2］的X軸方向與軌跡上點的法向重合，同時是打磨力控的方向；法向量u與切向量v的矢量叉乘值w是坐標系［T2］的Z軸向，控制工具運動方向。打磨工具中心軸線是Z軸，與打磨點坐標系的Z軸同向，打磨精確度高時打磨點與工件末端打磨頭位置一致。所以打磨點坐標系［wi］與打磨工具坐標系［T2］相對姿態的關系就是以Z軸向不變的前提下，X、Y軸自轉角α的調整，如圖7 所示。

圖7 待磨點和打磨工具坐標系相對姿態調整Fig.7 Adjust the relative posture of the coordinate system of the grinding point and the grinding tool

當打磨工具在某待磨點打磨瞬間，輪轂與打磨工具相對靜止，待磨點與打磨頭位置重合，設置合適的自轉角α旋轉即可調整相對位姿關系，調整后的打磨工具在世界坐標系下位姿矩陣為

3 雙機器人協作打磨運動學仿真

本文雙工業機器人協作打磨方式能去除復雜輪轂表面任意位置毛刺或凸起等不規則雜質，提高打磨精細化處理的能力。為驗證協調運動學模型和點云特征線提取算法的有效性，基于SolidWorks／Simscape 聯合仿真搭建雙機器人物理模型。為實現機器人的控制，需要將生成的原始模型添加合適的組件模塊，并設置屬性參數；編程實現雙機器人協作打磨運動學模型和相關算法；通過信號線連接Simulink 工具箱中模塊，封裝程序間的邏輯關系。

3.1 雙機器人仿真系統原理

由式（4）、（5）可知，為得到夾持機器人和打磨機器人末端位姿，還需要知道打磨點坐標系在世界坐標系下的位姿矩陣變化，式（19）和式（20）：

其中，Rw是打磨點在世界坐標系下的旋轉矩陣，由定義的坐標系方向可知；Pw、Pi是打磨點在世界坐標系、輪轂工件坐標系下的位置矩陣；Ri是磨點在輪轂工件坐標系下姿態矩陣。Pi、Ri由式（20）得到；PT1是輪轂工件在世界坐標系下的位置，可由上位機讀取進行轉換。

依據式（4）、（5）、（19）和（20）可得雙機器人的末端位姿矩陣并利用逆運動學算法得到機器人各軸關節角序列變化，控制協調打磨運動。雙機器人運動學仿真系統的原理框架如圖8 所示。

圖8 仿真系統原理框架Fig.8 Principle framework of simulation system

3.2 仿真實驗分析及結果

采用Matlab／Simulink 仿真環境平臺，雙機器人物理模型、運動學約束關系及正逆運動學求解算法都被封裝成獨立模塊，并通過信號線和Simulink 庫中的模塊集連接。雙機器人協作打磨運動學仿真模型如圖9 所示。

圖9 雙機器人運動學仿真模型Fig.9 Kinematics simulation model of two robots

仿真實驗所使用的打磨軌跡是輪轂點云數據經特征處理后，擬合而成的空間曲線。若有優秀的跟蹤打磨表現，其研究方法也必能沿用到空間直線或平面曲線等其他打磨軌跡上。實驗時夾持輪轂機器人進行空間位置和姿態的不斷調整，打磨機器人跟蹤20 個期望待打磨點進行打磨運動。雙機器人協調打磨過程如圖10 所示，仿真時間是10 s，圖10（a）是打磨機器人磨頭行駛到初始待磨點且磨頭與待磨點位置重合，打磨工具軸線Z軸與待磨點法線矢量平行；圖10（b）、（c）、（d）是第3 s、7 s 和第9 s時的雙機器人位姿調整示意圖。雙機器人位姿變化良好，能有效地進行協作打磨運動。

圖10 雙機器人協調打磨Fig.10 Coordinated grinding by dual robot

雙機器人在協調運動中，機器人各關節角序列變化如圖11 所示，曲線變化平滑無突變；協調打磨工況下各個機器人末端的三維空間軌跡如圖12 所示，打磨機器人的運動軌跡與期望打磨軌跡一致。

圖11 機器人各關節角序列變化Fig.11 The sequence changes of robot joint angles

圖12 各機器人末端軌跡Fig.12 End trajectories of each robot

仿真實驗結果表明：雙機器人在協作打磨運動過程中，磨具末端與待打磨點有良好的接觸吻合，3D 動畫顯示打磨運動平穩未出現抖動現象，各關節角度曲線變化平滑未出現尖點，運動狀態有較好的連續性，證明了協作打磨運動學模型和算法的可行性。

4 結束語

本文提出雙機器人相對運動協調打磨方法及建立雙機器人協作打磨坐標系、雙機器人運動學模型和約束，能有效完成工件的復雜空間曲線打磨問題，增大工作空間并提高了操作的靈活性；基于三維點云的姿態標準化，并用最大主曲率絕對值作為擬合曲面曲率的估計值，能提高特征點云可區分性，便于空間打磨點的提取，三維掃描技術結合機器人打磨為工程實際應用提供了新思路。針對復雜多變的工業環境，雖然本文提出的方法簡單且可復制性強，但雙機器人協作的力控制及提高變工件打磨效率下一步需解決。