吳亞緯 郝泳濤

關(guān)鍵詞：三維點(diǎn)云；機(jī)械臂；逆運(yùn)動(dòng)學(xué)；目標(biāo)定位

0 引言

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

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

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

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

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

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

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

1.1 PCL 庫(kù)

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

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

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

1.3 點(diǎn)云標(biāo)準(zhǔn)化協(xié)方差矩陣

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

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

標(biāo)準(zhǔn)化協(xié)方差矩陣：將協(xié)方差矩陣的每一個(gè)元素除以點(diǎn)的個(gè)數(shù)就可以得到標(biāo)準(zhǔn)化以后的協(xié)方差矩陣。

計(jì)算獲得的協(xié)方差矩陣和標(biāo)準(zhǔn)化協(xié)方差矩陣用于為機(jī)械臂的定位以及運(yùn)動(dòng)提供相應(yīng)的信息。

1.4 點(diǎn)云處理流程

2 機(jī)械臂建立及其運(yùn)動(dòng)分析

2.1 機(jī)械臂建立

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

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

可以通過表1來建立一個(gè)標(biāo)準(zhǔn)型DH參數(shù)表，并驗(yàn)證機(jī)械臂的建模是否成功。

2.2 目標(biāo)定位算法的研究

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

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

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

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

2.2.2 最小二乘法定位算法

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

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

3 實(shí)驗(yàn)結(jié)果與分析

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

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

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

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

3.2 機(jī)械臂運(yùn)動(dòng)逆向分解

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

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

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

4 結(jié)束語

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

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

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

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