應用于油罐內機械臂作業的點云法矢計算方法

程健勇，吳功平，樊 飛，解建忠

(1.武漢大學動力與機械學院，湖北 武漢 430072；2.湖北三江航天紅林探控有限公司，湖北 孝感 432000)

1 前言

六軸機械臂撬裝復合系統由工控系統和機械臂系統組成，用于實現對大型儲油罐內壁的改造工作。工控系統負責處理油罐內壁的激光點云數據，設計機械臂行走路徑，提供機械臂末端的位置與姿態；機械臂系統，如圖1所示，則根據上述信息實現對油罐內壁的自動改造作業。改造作業包括噴砂除銹、毛刺碾壓、樹脂噴涂，改造作業工具為固定在機械臂末端的相應裝置，改造的可靠性取決于機械臂末端與油罐內壁的垂直程度，因此對點云法矢的計算是最為重要的一部分。另一方面，由于油罐內壁部分特征會對改造工作造成影響，因此需要先行剔除掉這些特征，即點云分割。

點云分割的方法包括霍夫變換法、RANSAC算法、聚類或區域增長法等[1-2]。文獻[3]利用直方圖統計法與區域生長算法，對建筑物的特征區域進行了分離。法矢計算多采用協方差分析法，該方法由文獻[4]于1992年首次提出，但是對于分布不均勻的點云數據效果較差。文獻[5]提出基于代數球的移動最小二乘法(MLS)擬合，優化了在低采樣率和高曲率的情況下對點云表面特征的處理。文獻[6]提出了靜態MLS的擴展，通過動態選擇使只有K鄰域內的點對最小二乘優化有顯著貢獻。然而，上述方法在一些局部表面上，會造成特征的過度平滑。但在油罐內壁上局部變形很多，因而這些方法并不適用于油罐點云處理。

因此，采用一種簡化的直方圖統計法對油罐點云數據分割，同時為保證機械臂的平穩運行、提升改造效果，提出一種應用于油罐內壁作業的點云法矢計算方法。該方法利用混合拓撲構建方法建立點云之間的拓撲結構，根據改造作業的不同，實現自適應的K鄰域搜索；利用改進的加權協方差分析法，控制路徑點的鄰域點對其產生的影響，實現法矢的準確計算。

圖1 六軸機械臂撬裝復合系統Fig.1 Six-axis Manipulator Armored Composite System

2 油罐內壁點云法矢計算基本方法

機械臂進行改造作業時，需要保證機械臂末端時時垂直于油罐內壁，因而需要計算路徑點處的法矢。對某一路徑點使用協方差分析法求取點云法矢時，首先要確定的是該點的k個鄰域點以便進行局部平面擬合。由于油罐點云數據量較大，為提高K鄰域搜索的速度，應先建立點云之間的拓撲關系，常用的方法有柵格法和八叉樹法。

2.1 油罐內壁點云拓撲關系建立

2.1.1 基于柵格法的點云拓撲關系建立

柵格法將油罐內壁點云數據的最小包圍長方體，按照設定的區域間隔L劃分為均等的多個立方體單元格。進行柵格劃分時，遍歷點云數據，得到X、Y、Z軸上的最大坐標值與最小坐標值，即：[Xmin，Xmax]、[Ymin，Ymax]、[Zmin，Zmax]。油罐點云模型最大柵格的起點設為P(Xmin，Ymin，Zmin)，柵格的劃分示意圖，如圖2所示。

圖2 柵格編號示意圖Fig.2 Grid Numbering Diagram

對于空間中的任意一點Pi(xi,yi,zi)，其所在柵格的編號id可通過式(1)確定：

2.1.2 基于八叉樹法的點云拓撲關系建立

八叉樹結構通過循環遞歸的劃分方法對大小為2n*2n*2n的三維空間中的幾何對象進行剖分，從而構成一個具有根節點的方向圖，剖分次數最多為n次[7]。若子立方體中含有點云數據，則稱其為“黑節點”，否則稱為“白節點”。當黑節點的邊長大于或等于最小劃分邊長Lmin，或黑節點中含有的點的數目大于或等于最少劃分點數μmin時，對黑節點進行子節點劃分，直到被劃分的子節點的邊長小于Lmin，或點數少于μmin。

根據八叉樹的線性結構屬性，使用八進制數系統對其節點編碼，實現八進制數到節點的映射[8]。編碼可以描述如下：

式中：n—八叉樹的深度，用于衡量八叉樹劃分的層次，n越大，則子立方體的劃分越精細。qi是八進制數數據，qi∈[0，7]，i∈[0，n-1]。

根據八叉樹的編碼方式，對任一點pi，其所在子立方體的左下角坐標為(x，y，z)，取a，b，c為八進制編碼，有：

根據式(3)，可以獲得任一子立方體在X，Y，Z軸上距離原點的間隔的子立方體個數，同時，也可以計算空間中任一點pi所在的子立方體編碼及其周圍劃分的子立方體編碼，實現點云拓撲結構的建立。

2.2 油罐內壁點云法矢與曲率計算

對于油罐內壁點云數據中的任一點pi，搜索與之最近的k個鄰域點，獲得K鄰域集合后，使用最小二乘法將k個鄰域點擬合成一個局部平面Pl，平面Pl的數學表示形式如下：

式中：—擬合平面Pl的法向量；pj—pi的鄰域點；d—坐標原點到平面Pl的距離。

由于法向量滿足=1，式(4)可以簡化為對協方差矩陣C的特征值分解，C的最小特征值對應的特征向量即為pi對應的法向量。構造協方差矩陣C：

求取任一點pi處的曲率時，在該點建立局部坐標系，采用二次曲面逼近的方法估計該點處的曲率。曲面的形式如下：

式中：u，v—曲面的描述參數，取為pi的切平面上的兩個正交向量；a，b，c，d，e—二次曲面系數。

點pi處的切平面的單位法向量為：

二次曲面的第一基本量為：

第二基本量為：

其中，Su=(1，0，2au+bv+d)，Sv=(0，1，bu+2cv+e)，Suu=(0，0，2a)，Svv=(0，0，2c)，Suv=(0，0，b)。

可得油罐該點處的高斯曲率K、平均曲率Kavg分別為：

3 改進的油罐內壁法矢計算方法

基于柵格法或八叉樹法實現的K鄰域搜索方法，搜索時間長、占用內存大，且不能適應油罐改造作業時不同工序的需求。另一方面，油罐內壁上局部變形較多，使用傳統的加權協方差分析法在這些變形區域上會產生過度平滑的現象，機械臂作業時會發生應力碰撞。綜合考慮，提出一種應用于油罐內壁的法矢計算方法，該方法使用基于混合拓撲關系實現的自適應的K鄰域搜索；獲得k個鄰域點后，利用改進的加權協方差分析法確定最終的法矢方向。

3.1 基于自適應的K鄰域搜索的鄰域點確定

使用八叉樹法建立點云拓撲關系時，點云空間繼續分割的條件是該空間中存在有足夠多的點或者劃分的子立方體的邊長足夠大。但是油罐內腔為空，點云的分布密度較為不均，會造成對油罐無點空間的無意義劃分和點云密集區域的過度劃分，造成時間和空間上的浪費。而柵格法則是對整個空間進行均等劃分，忽視點云密度的影響，在相同柵格大小的油罐空間中，點云的密度可能會有較大的差異。

針對單純地使用柵格法或是八叉樹法建立拓撲關系導致的缺點，并考慮到油罐模型的特征，受文獻[11]啟發，結合兩種劃分方法，提出一種混合拓撲構建方法，以實現自適應的K鄰域搜索。

首先，對油罐進行點云分割后，按照工作要求對點云數據進行多項式擬合，設計機械臂末端行走路徑方案。路徑簡圖如下：

圖3 路徑規劃簡圖Fig.3 Path Planning Sketch

其中，紅色點為機械臂實際運動的路徑規劃點，深色線條為模擬的機械臂行走路徑。油罐內壁改造工作包括噴砂除銹、毛刺碾壓、樹脂噴涂，取兩條路徑之間的間隔為L，該值取決于工序的種類。構建拓撲關系時，首先利用柵格法對點云空間進行一次剖分，最小立方體包圍盒的邊長取為L。一次剖分完成后，根據每個柵格中點的數目，標記非空的柵格為1，即主柵格，空柵格為0。對主柵格進行二次剖分。二次剖分時采用非固定深度的八叉樹剖分方法，由點的密度決定八叉樹剖分的深度。當主柵格中點的數目小于μ時，停止剖分。μ代表剖分的精度，μ越小，則剖分的精度越高。二次剖分結束后，根據柵格的編號和剖分關系，可以得到每個柵格的頂點坐標，確定每個點所處的柵格。

為了提高機械臂運行的平穩性和經濟性，在求解路徑點對應的法向量時，應當充分考慮到路徑點的k鄰域點分布的區域性，使得機械臂末端工具在路徑點時既能保證良好的垂直度以滿足工作要求，又能較好的適應該路徑點周圍的油罐壁面條件，使機械臂運動平穩、無沖擊。因此，在獲取路徑點的k近鄰點時，終止K鄰域搜索的首要條件是搜索區域的范圍達到一定的要求。下圖為K鄰域搜索程序框圖，如圖4所示。

圖4 K鄰域搜索程序框圖Fig.4 Block Diagram of K-Neighbor Search

圖中：k—路徑點要求的鄰域點個數；n—該路徑點所處主柵格中點的個數；k′—該路徑點已經搜索到的鄰域點的個數；路徑點通過鏈表結構串聯起來。基本流程，如圖4所示。若k′≥k，取k=k′，轉向下一路徑點；否則，將搜索范圍擴大到與本主柵格相鄰的26個柵格，尋找距離該路徑點最近的k-n個鄰域點。循環查找所有路徑點的K鄰域點。

本方法根據不同的改造需求建立點云之間的拓撲關系，以滿足搜索區域大小為首要條件實現了自適應的K鄰域搜索，充分考慮到了路徑點處的基本特征及其鄰域特征，提高了機械臂末端工具在該點的通過性。

3.2 基于改進的加權協方差分析的法矢計算

文獻[6]在使用協方差分析法時，對K鄰域點施以不同的高斯權重系數，使得距離當前點越近的點，對擬合平面的貢獻度越大，距離越遠，則貢獻度越小。文獻[12]求得初始法矢后，使用法矢夾角對法矢方向進行修正。文獻[6、9]，可將式(4)中的擬合平面Pl改寫為：

式中：ωd(pi)—當前點pi到鄰域點pj的高斯權重系數；σd—距離帶寬。這種方法在精度上有了一定的提升，但式(11)僅僅考慮了鄰域點到當前點pi的距離對其造成的影響，對于pi處法矢的修正較小。

針對上式的缺點，文獻[10]提出了一種改進的加權協方差矩陣形式：

取pi到平面Pl的距離為dmax，鄰域點pj到pi的距離為dij，εj的取值如下：

以上兩種方法，均只考慮了dij對法矢估計產生的影響：越近的鄰域點對pi產生的影響越大。導致pi處的法矢有向里收縮、集中于路徑點法矢方向的趨勢。但是，考慮到機械臂特殊的工作環境，文中將路徑點的鄰域點曲率考慮進來，加強鄰域點的表面特征施加到協方差矩陣上的影響。

對于油罐任一路徑點pi，其K鄰域點集為qj∈Q(j=1，2，…，k)。曲率均方差計算形式如下：

式中：Hm—點qj處的平均曲率；—點qj的鄰域點的平均曲率的平均值，即：

對曲率均方差歸一化處理，得到曲率權值為：

式中：σjmax—點pi的鄰域點集的曲率均方差最大值；σjmin—曲率均方差最小值。式(16)反映點pi的鄰域點集局部特征及其離散程度：在ωj較大的點qj處，其平均曲率的離散程度較大，能夠反映更多的點云特征，對協方差矩陣貢獻更大。同時，式(14)采用了領域點集均方差的形式，對于較小量級的噪聲點不敏感，抗噪性較好[13]。改進的加權協方差分析法將距離權值和曲率權值結合起來，使用加權權值的形式，可以更好的反映油罐路徑點pi的鄰域點集對pi處產生的影響。改寫式(5)如下：

式中：κj=(εj+ωj)/2，εj—距離權值；ωj—曲率權值。

4 算法實現及結果分析

針對特定的油罐點云模型，根據不同改造工藝的需求，對油罐點云數據進行相匹配的處理。算法流程簡介如下：

(1)點云讀入，根據油罐的特征使用直方圖統計法對點云數據進行分割，排除機械臂末端不能夠工作的區域。分割完成后，使用統計濾波實現點云去噪；

(2)對分割后得到的油罐可工作區域進行路徑規劃，得到路徑規劃點；

(3)使用混合拓撲構建方法，建立路徑點在點云數據中的拓撲關系；

(4)根據改造需求自適應地搜索路徑點的K鄰域點；

(5)使用改進的加權協方差分析法估算法矢；

(6)根據上一步得到的法矢，在路徑點處建立局部坐標系，利用坐標變換求得機械臂末端的姿態；

4.1 點云分割

直方圖統計法聚類的原理是，通過統計落在某個區域中點的數量，劃分同一維度上處于不同高度的點云。實際操作過程中，油罐模型取其中一半，點的總數為597557。半罐在軟件Geomagic Studio中的示意圖，如圖5所示。

圖5 油罐示意圖Fig.5 Oil Tank Schematic

使用簡化的直方圖統計法，通過統計油罐XOZ平面上Z坐標的最小值對點云進行分割。根據統計折線圖，將機械臂不能夠加工的區域提取出來并舍棄，只保留可工作區域。分割后，油罐模型被分為3個部分，藍色區域和綠色區域為舍棄區。考慮到機械臂末端工具的尺寸，將這兩塊區域適當放大，以適應實際的加工需求。對分割后的點云進行統計濾波，除去點云中的稀疏離群點。統計折線圖及點云分割圖，如圖6所示。

圖6 點云分割示意圖Fig.6 Point Cloud Segmentation Diagram

4.2 法矢計算

建立點云拓撲關系時，改造工藝取為噴砂除銹，L=30mm；二次剖分時最小柵格中點的數目μ=4；K鄰域搜索的K值選取依據文獻[4]，取k=20；取距離帶寬σd=0.2。采用改進的加權協方差分析法求取油罐點云數據的法矢。油罐頭部橢球處的法矢示意圖，如圖7所示。圖中，綠色部分為油罐點云數據，白色線條為求取的點云法矢，可知：使用改進的加權協方差分析法求得的法矢較為精確，并實現了法矢方向的統一。

圖7 法矢示意圖Fig.7 Normal Vector Diagram

4.3 實驗驗證

在路徑點處建立局部坐標系p-UVW，坐標原點p為路徑點，坐標軸W指向為法矢的方向，UV滿足右手正交法則且U與末端加工工具的方向一致。坐標系示意圖，如圖8所示。圖中的局部坐標系原點即為路徑規劃點。通過坐標變換將法矢轉換到機械坐標系下，求得機械坐標系下的機械臂姿態，即Roll、Pitch、Yaw旋轉角。將坐標信息導入機械臂進行實體驗證，驗證效果，如圖9所示。

圖8 坐標系示意圖Fig.8 Coordinate System Diagram

圖9 實驗驗證效果Fig.9 Experimental Verification Effect

機械臂末端的起始工作位置，末端工具垂直于油罐表面，貼合良好，如圖9(a)所示。機械臂正常工作時的狀態，可以觀察到機械臂末端在改造過程中始終垂直于油罐內壁，末端工具處理過的區域有比較明顯的特征，處理效果良好、均勻，如圖9(b)、圖9(c)所示。在進行法矢計算誤差分析時，由于油罐表面局部變形較多，無法利用油罐的理論模型獲取油罐的標準法矢，因而此處采用人工自測的方法獲取油罐內壁法矢，并將其作為法矢的標準值。將采用改進加權協方差分析法求得的法矢與標準值進行對比，分析該方法的可行性。機械臂控制系統每300ms反饋一次機械臂末端旋轉角及其三維坐標值，選取油罐YOZ面上的一條封閉路徑曲線，提取其末端旋轉角與坐標值。機械臂結束當前作業后，均勻選取該路徑曲線中的360個點，人工調整機械臂姿態使機械臂末端垂直于油罐表面，獲取這360個點對應的機械臂末端旋轉角。在圖10中，橫坐標為機械臂在路徑曲線上的位置，以角度為單位，起點為油罐坐標中Z值最大的點。縱坐標分別為末端的三個旋轉角：Roll角、Pitch角和Yaw角。黑色點為人工記錄的360個采樣點對應的旋轉角，紅色曲線為機械臂反饋的旋轉角。通過兩者對比可知，使用該方法得到的油罐壁面計算法矢和油罐壁面實際法矢方向基本重合，求得的末端旋轉角在油罐最上方和最下方偏差稍大，最大誤差在(5～8)°左右。考慮到機械臂自重對油罐造成的變形以及人工調整機械臂姿態導致的累計誤差，該誤差是可以接受的。另一方面，根據機械臂反饋的旋轉角來看，機械臂末端的角度變化非常均勻，運動平穩無突變，有利于提高壁面改造的質量。結合圖9中的表面改造狀況，證明該方法能夠較好的處理油罐的點云數據，并從中計算得到了準確的法矢值，適用于油罐內壁的改造工作。

圖10 機械臂末端旋轉角Fig.10 Robotic Arm End Rotation Angle

5 結論

針對油罐內壁改造工作，對油罐內壁點云法矢精確計算進行了研究。為保證改造效果、提高機械臂運動穩定性，提出基于二次剖分的混合拓撲構建方法，實現了基于改造工序的自適應的K鄰域搜索；改進傳統的加權協方差分析法，利用距離權值和曲率權值對協方差矩陣進行加權，使得距離路徑點越近的點、曲率變化越大的點，對路徑點處的法矢貢獻度更大。實驗結果表明，使用該方法得到了準確的油罐點云法矢，機械臂工作時末端工具能夠很好的貼合油罐表面并保持垂直，運動穩定，無應力沖擊。然而該方法的部分參數是根據經驗設定的，需要做進一步的研究以確定更合適的參數。