基于三維點云的雙機器人協(xié)作打磨軌跡規(guī)劃研究

周 震，姜笑言，韓家哺

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

0 引言

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

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

1 雙機器人協(xié)作打磨運動學模型

1.1 協(xié)作打磨各坐標系的建立

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

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

圖1 雙機器人系統(tǒng)坐標系Fig.1 Coordinate system of dual robot systems

1.2 協(xié)作打磨任務(wù)運動學關(guān)系

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

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

2 雙機器人協(xié)作打磨軌跡規(guī)劃

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

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

2.1.1 構(gòu)建輪轂的三維點云模型

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

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

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

2.1.2 輪轂點云的姿態(tài)標準化

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

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

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

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

2.1.3 點云特征線的提取

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

將姿態(tài)標準化后的坐標點云數(shù)據(jù)用二次曲面方程進行曲面擬合為

曲面擬合的系數(shù)為

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

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

令：

若存在式（12）：

當?shù)仁剑?2）成立時，主曲率值滿足方程（13）［9］：

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

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

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

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

2.2 輪轂空間打磨軌跡規(guī)劃

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

2.2.1 打磨點位姿數(shù)學模型

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

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

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

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

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

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

2.2.2 打磨工具位姿數(shù)學模型

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

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

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

3 雙機器人協(xié)作打磨運動學仿真

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

3.1 雙機器人仿真系統(tǒng)原理

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

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

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

圖8 仿真系統(tǒng)原理框架Fig.8 Principle framework of simulation system

3.2 仿真實驗分析及結(jié)果

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

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

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

圖10 雙機器人協(xié)調(diào)打磨Fig.10 Coordinated grinding by dual robot

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

圖11 機器人各關(guān)節(jié)角序列變化Fig.11 The sequence changes of robot joint angles

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

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

4 結(jié)束語

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