三維點云驅(qū)動關(guān)節(jié)目標定位和運動逆向分解算法研究

吳亞緯 郝泳濤

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

0 引言

點云數(shù)據(jù)[1]是指在一個三維坐標系統(tǒng)里的一組向量的集合。三維點云是一種很重要的安慰數(shù)據(jù)結(jié)構(gòu)，隨著計算機和機器人學科的發(fā)展，三維點云也在機械臂領(lǐng)域獲得了深入應用。

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

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

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

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

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

1 三維點云數(shù)據(jù)處理

1.1 PCL 庫

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

1.2 點云關(guān)鍵點提取

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

1.3 點云標準化協(xié)方差矩陣

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

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

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

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

1.4 點云處理流程

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

2.1 機械臂建立

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

機械臂運動需要一個基本的坐標系，當建立好一個DH坐標系以后，就可以根據(jù)DH參數(shù)表來計算機械臂各個關(guān)節(jié)之間的關(guān)系。

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

2.2 目標定位算法的研究

2.2.1 質(zhì)心定位算法的研究

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

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

通過對質(zhì)心算法進行加權(quán)，計算節(jié)點和未知的節(jié)點之間的距離，然后以每個點為圓心做一個圓，多個圓形相交所組成的區(qū)域構(gòu)成一個新的多邊形，然后計算這個新的多邊形的中心就可以求得未知節(jié)點。

2.2.2 最小二乘法定位算法

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

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

3 實驗結(jié)果與分析

3.1 機械臂軌跡規(guī)劃

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

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

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

3.2 機械臂運動逆向分解

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

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

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

4 結(jié)束語

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

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

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

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