三維點云驅動關節目標定位和運動逆向分解算法研究

吳亞緯 郝泳濤

關鍵詞：三維點云；機械臂；逆運動學；目標定位

0 引言

點云數據[1]是指在一個三維坐標系統里的一組向量的集合。三維點云是一種很重要的安慰數據結構，隨著計算機和機器人學科的發展，三維點云也在機械臂領域獲得了深入應用。

由于三維點云包含的多種信息，比如顏色、其強度等信息使得它對三維空間內的物體描述更加精準。

機械臂通常有多個自由度，這就使得機械臂擁有很高的靈活度，可以勝任很多復雜的工作，但是機械臂對環境的適應性是遠不如人類的，為了提高機械臂對環境的適應能力，就需要結合計算機視覺的能力，通過三維點云提供的目標位置，機械臂通過逆向運動分解，使得機械臂運動到固定位置。

點云數據由于數據龐大，因此可以描繪出物體的三維輪廓[2-3]。在工業界，利用激光雷達獲取點云數據，很早就有應用了，比如進行測高、遙感等，近幾年的大規模發展得益于自動駕駛和機器人領域的火熱，激光雷達成為重要的感知手段而得到人們關注，點云處理也成為熱門。

點云的發展[4-8]主要是幾個方面，首先從數據采集設備來說，目前包括了有人/無人機、地面、車載、便攜式等多個平臺共存。其次點云的場景特點有數據量大、冗余度高的特點。因此，必須在點云的數據模型、處理模型方面有一些突破，實現在點云場景方面的智能化。

與此同時，對機械臂的逆運動學求解通常有兩種方法：解析法和數值法。解析法的特點包括運算速度快，但是通用性差，可以分為代數法和幾何法求解，對解析法來說，串聯機械臂有逆運動學解析解的充分條件是滿足Pieper準則。即如果機器人滿足兩個充分條件中的一個，就可以得到封閉解：1） 三個相鄰關節軸相交于一個點；2） 三個相鄰關節軸相互平行。數值法的通用性較高，除了一些特殊的機械臂構型外，機械臂逆運動學問題很難用解析解求解，所以在很多情況下會使用數值解求解。通常設定一個目標函數，是把逆解求解問題轉化為一個優化問題求數值解。

1 三維點云數據處理

1.1 PCL 庫

PCL是在吸收了大量前人對點云研究的基礎上，建立的大型跨平臺開源庫，實現了大量點云相關的通用算法和高效的數據結構，涉及點云獲取、濾波、分割、配準、檢索、特征提取、識別、追蹤、曲面重建、可視化等。支持多種操作系統平臺，可在Windows、Linux、Android、MacOSX、部分嵌入式實時系統上運行。如果說OpenCV是2D信息獲取與處理的結晶，那么PCL就在3D信息獲取與處理上具有同等地位，PCL是BSD 授權方式，可以免費進行商業和學術應用。

1.2 點云關鍵點提取

點云的關鍵點是一個點云數據中心有特殊特征的結合，并且其數量會遠遠小于初始的點云數量，可以很好地描述點云的特征。比較通用的關鍵點提取的算法有Harris算法、SIFT算法和ISS算法等。本文使用的是SIFT算法。

1.3 點云標準化協方差矩陣

協方差可以衡量兩個隨機變量在一個總體中共同變化的程度。在面臨大量的數據時，或者維度很多的數據時，可以使用協方差矩陣描述不同維度的關系并且使用每兩個隨機變量之間的關系來描述整體維度中的關系。

計算協方差矩陣：在PCL中，首先加載需要讀取的點云文件，使用Maxtrix3f創建協方差矩陣存儲對象，并且創造點云質心對象用于計算協方差矩陣，最后計算點云質心，使用PCL庫中現有的函數計算點云協方差矩陣。

標準化協方差矩陣：將協方差矩陣的每一個元素除以點的個數就可以得到標準化以后的協方差矩陣。

計算獲得的協方差矩陣和標準化協方差矩陣用于為機械臂的定位以及運動提供相應的信息。

1.4 點云處理流程

2 機械臂建立及其運動分析

2.1 機械臂建立

建立機械臂時通常會使用到DH參數法，這是一種1955年提出的對機械臂建模行之有效的建模方法。

機械臂運動需要一個基本的坐標系，當建立好一個DH坐標系以后，就可以根據DH參數表來計算機械臂各個關節之間的關系。

可以通過表1來建立一個標準型DH參數表，并驗證機械臂的建模是否成功。

2.2 目標定位算法的研究

2.2.1 質心定位算法的研究

質心定位算法是一種比較簡單的、理想化的定位算法[9]，首先需要找到未知節點的信息，這些所有的節點構成一個多邊形，假設未知節點的分布相對整個節點來說是均勻的，然后通過數學方法，去計算這個多邊形的質心節點。將多邊形的所有頂點建立一個平面坐標系，將所有的橫坐標相加后求得平均值，將縱坐標也進行相同的處理，由此求得多邊形的中心作為最終結果。

可知，如果通過求多邊形的中心位置來定位位置節點的位置，對于節點的連通性要求很高，當不同的節點所構成圖形的中心并不與未知節點接近，這種方法的效果會很差，所以我們需要考慮一種改進的算法來應對這些弊端。

通過對質心算法進行加權，計算節點和未知的節點之間的距離，然后以每個點為圓心做一個圓，多個圓形相交所組成的區域構成一個新的多邊形，然后計算這個新的多邊形的中心就可以求得未知節點。

2.2.2 最小二乘法定位算法

對于最小二乘法來說，它也被叫作最小平方法。最小二乘法定位算法的目標之一是最小化誤差的平方，對于多個數據建立多個方程，把多個方程的誤差最小化，從而獲得最優的估算坐標。

對于n個二維數據來說，我們使用最小二乘法，首先將所有的橫縱坐標分別相加并求得平均值，然后計算滅個數據點的橫坐標的平方并進行求和，算法橫縱坐標相乘的結果并且進行求和，然后計算所要求的直線的斜率得到相應的直線方程，任取兩個原來有數據中相隔較遠的點的橫坐標，帶入求得的方程得到相應的縱坐標，得到兩個點，然后用它們做回歸直線，也可以拓展到三維空間中。

3 實驗結果與分析

3.1 機械臂軌跡規劃

機械臂要在空間中實現自由運動[10-12]，就需要在空間的坐標系的運動路徑進行規劃，計算出路徑點，從而保證機械臂運動過程中經過這些路徑點，常見的軌跡規劃就包括直線和圓形軌跡。

直線規劃的基本原理：當知道機器人手臂末端的起始和結束坐標時，使用三維空間的勾股定理計算兩點之間的距離，即直線規劃中所需直線的移動長度。我們設定運動速度，計算移動這個距離所需的時間，設定插值周期，運動時間除以插值周期得到插值步數n，從直線除以n個均勻分布的坐標。然后通過逆分解的原理，獲取機械臂的所有關節信息，控制機械臂實現軌跡運動。

曲線規劃的基本原則：當知道機器人末端的開始和結束坐標時，應該有所不同。坐標系下的空間轉換、狀態轉移矩陣設置、將空間圓弧映射到平面空間進行處理、轉換完成。然后以圓弧角度為研究對象，計算機器人運動的插值步數。線束段固定圓弧角度位移。計算與每個步驟相對應的圓弧上的點位置。這些運算求得了在空間轉換以后插補點的位置坐標。最后，通過狀態變換矩陣獲取并發送原始空間下插值點的位置坐標。完成機械臂的圓弧軌跡規劃。

3.2 機械臂運動逆向分解

使用UR的機械臂，已知一個機械臂的變換矩陣，參照給出的DH的參數表來逆向求解出六個關節角度，并且進行驗證。

使用數學解析法求得上述的關節角度，為了驗證是否正確，還需要編寫正向運動代碼進行驗證。

使用正向運動代碼驗證上述信息是否正確，將上述信息一個個代入編寫的正向運動函數進行驗證，結果均等于所求的變換矩陣，可以證明結果正確。

4 結束語

激光雷達等掃描設備帶來了大量的三維點云數據，點云數據有很好的優勢，對于三維點云的研究也越來越多，三維點云的出現拓展了機械臂的研究可能性。

本文介紹了處理點云的常用庫PCL，并且通過PCL庫實現了三維點云的讀取、顯示，將點云進行處理以后，計算相應的協方差矩陣和標準協方差矩陣，用于后續的機械臂研究。

本文也介紹了常用的機械臂建模方法，DH方法，并應用此方法進行了機械臂建模。我們對機械臂的直線和圓弧曲線軌跡規劃的實驗，并且通過PCL讀取點云信息處理后得到相關矩陣變量，使得機械臂確認姿態信息，然后進行逆向分解運算。

關于點云與機械臂的結合研究還有許多東西值得學習，是十分值得探究的方向。