基于骨架點的矮化密植棗樹三維點云自動配準(zhǔn)

馬保建 鄢金山 王 樂 蔣煥煜

(1.浙江大學(xué)生物系統(tǒng)工程與食品科學(xué)學(xué)院， 杭州 310058; 2.新疆理工學(xué)院機械電氣工程學(xué)院， 阿克蘇 843100)

0 引言

相比于過去的棗樹種植模式，矮化密植樹形更利于棗園管理[1]，目前有自由紡錘形、小冠疏層形和開心形3種主要的矮化密植樹形結(jié)構(gòu)。棗樹冬剪是棗園管理過程中的重要環(huán)節(jié)，通過去除多余或長勢過長的樹枝來控制樹形結(jié)構(gòu)，可達(dá)到增加產(chǎn)量的目的[2]。果樹修剪目前主要有2種方式：人工修剪和整株幾何修剪。其中人工單枝選擇性修剪質(zhì)量高，但修剪效率低，并且人工成本高、勞動強度大，容易對操作者手指造成損傷；整株幾何修剪效率高，令修剪機械與果樹保持固定的修剪距離，但錯剪、漏剪比較嚴(yán)重[3-4]。自動化選擇性修剪對果樹進(jìn)行有選擇的精剪，能夠減少用工成本，避免大量有用枝條被誤剪，從而提高果樹的產(chǎn)量并延長其生長周期[5]。

合適的信息獲取技術(shù)對于果樹自動化選擇性修剪十分重要[6]，相關(guān)研究表明，利用2D視覺技術(shù)識別果樹樹枝可以達(dá)到修剪的目的[7-10]，然而利用2D圖像提取果樹枝干三維幾何結(jié)構(gòu)信息十分困難。3D視覺技術(shù)克服了2D視覺技術(shù)的這些缺點，并能很好地重構(gòu)果樹的幾何結(jié)構(gòu)信息[11-12]，文獻(xiàn)[13]利用雙目視覺對果樹進(jìn)行重建,文獻(xiàn)[14]利用3個固定的RGB相機采用SFM算法對葡萄樹枝干進(jìn)行三維重建并完成了田間的修剪作業(yè)實驗，但是其計算復(fù)雜度比較高；文獻(xiàn)[15-18]在自然環(huán)境下利用激光雷達(dá)獲取果樹的三維點云信息，但是獲取果樹信息的過程比較繁瑣、傳感器比較昂貴且受限于獲取的稀疏點云信息，對于重建較高精度的棗樹枝干比較困難；文獻(xiàn)[19-20]搭建結(jié)合RGB相機和ToF原理的深度相機的系統(tǒng)對蘋果樹進(jìn)行了部分三維重建；文獻(xiàn)[21]采用2臺固定的Kinect v1相機(基于結(jié)構(gòu)光原理)對櫻桃樹在室內(nèi)進(jìn)行了重建實驗，并說明了該系統(tǒng)無法在室外使用；文獻(xiàn)[22]利用Kinect v2相機(基于TOF原理)獲取樹的多角度點云信息，用標(biāo)記球的方式人工配準(zhǔn)，然后利用空間殖民算法結(jié)合管道模型生成樹的結(jié)構(gòu)；同樣的，文獻(xiàn)[23-24]利用Kinect v2相機重建部分蘋果樹的三維模型，主要枝干重建的誤差在5 mm，但沒有提及重建時間。

本文以矮化密植開心形棗樹為研究對象，利用2臺消費級Azure Kinect DK深度相機搭建三維重建系統(tǒng)平臺，在自然環(huán)境下對無葉棗樹進(jìn)行點云信息采集，并提出一種基于骨架點的棗樹點云配準(zhǔn)方法，首先利用骨架點對2個視角下的棗樹點云進(jìn)行快速初配準(zhǔn)獲得較優(yōu)的初始位姿，然后利用經(jīng)典的ICP算法進(jìn)行精配準(zhǔn)，只需2個視角就能快速重建出完整的棗樹枝干三維模型，并對3種典型自然環(huán)境下的棗樹點云進(jìn)行配準(zhǔn)實驗以及誤差分析。

1 材料與方法

1.1 實驗場地

實驗場地位于浙江省義烏市農(nóng)場室外的無主動光源的大棚內(nèi)，其有2個主要作用：首先，保持棚內(nèi)一定的溫度，促進(jìn)棗樹早開花結(jié)果；其次，在南方雨季來臨之時可以防止雨水對于棗樹花、果的影響。大棚內(nèi)種植的是矮化密植5～6年生棗樹，開心形的樹形結(jié)構(gòu)，生長過程是在自然光照環(huán)境下進(jìn)行的，其種植模式為株距1.5 m，行間距1.8 m，高度1.8～2.2 m，如圖1所示(拍攝于2019年12月)。

1.2 棗樹三維重建系統(tǒng)平臺

本文搭建的棗樹三維重建系統(tǒng)平臺如圖2所示，該系統(tǒng)平臺主要包括：支撐架、2臺Azure Kinect DK深度相機(兼顧相機獲取數(shù)據(jù)的速度，選擇如表1所示的相機部分參數(shù))、聯(lián)想計算機(i5-6300HQ主頻2.3 GHz處理器、4 GB內(nèi)存、NVIDIV GeForce GTX 960M顯卡)、2個LED燈(30 W、色溫3 000～5 700 K、尺寸為17.6 mm×14.4 mm×3 mm、發(fā)光角度120°)、音頻線、移動電源(220 V、65 000 mA·h、300 W)等。采用2臺Azure Kinect DK深度相機平行固定串聯(lián)的方式搭建，2臺深度相機平行安裝距離35 cm且距離地面1.1 m的位置，固定在支撐架上。用音頻線把2臺相機串聯(lián)起來實現(xiàn)2臺相機的同步，音頻線一端插入副相機的IN端，另一端插入主相機的OUT端，同時2臺相機通過數(shù)據(jù)線連接在同一臺計算機上。本文算法在Windows 10系統(tǒng)上的VS 2019結(jié)合點云PCL 1.9庫、OpenCV 4.2庫編寫。

表1 Azure Kinect DK深度相機部分參數(shù)Tab.1 Partial parameters of Azure Kinect DK depth camera

1.3 2臺固定相機的位姿標(biāo)定

由于Azure Kinect DK深度相機的內(nèi)部參數(shù)在出廠時已設(shè)定，因此只需采用Matlab軟件立體視覺標(biāo)定工具箱，利用張正友標(biāo)定法[25-26]，計算2臺固定主、副相機之間的相對位姿矩陣Tsub→master，標(biāo)定過程是將尺寸為300 mm×300 mm棋盤格標(biāo)定板放在距離2臺相機1.2 m的位置進(jìn)行標(biāo)定，如圖3所示。30組不同角度的棋盤格圖像同時被2臺相機獲取，每組2幅圖像類似于雙目相機的左右攝像頭，經(jīng)標(biāo)定可得2臺固定相機的位姿矩陣為

圖3 2臺固定相機標(biāo)定過程示意圖Fig.3 Process of camera calibration

且標(biāo)定的相機內(nèi)部參數(shù)和出廠時的內(nèi)部參數(shù)基本一致。Azure Kinect DK相機能獲取彩色圖像、深度圖像和紅外圖像3種數(shù)據(jù)，且相機出廠時已經(jīng)實現(xiàn)了彩色相機與深度相機的精確標(biāo)定，利用Azure Kinect DK for Windows 10 SDK v1.2和編程接口API，并結(jié)合相機的內(nèi)部參數(shù)可實時獲取物體在相機坐標(biāo)系下的彩色三維點云數(shù)據(jù)，然后利用位姿矩陣Tsub→master把副相機獲取的彩色三維點云轉(zhuǎn)換到主相機的坐標(biāo)系下，但是通過標(biāo)定獲取的位姿矩陣Tsub→master并不能使2臺相機的彩色點云數(shù)據(jù)很好地融合，如圖4a所示，文獻(xiàn)[27]采用非線性優(yōu)化的方法對4臺固定的Kinect v2相機位姿進(jìn)行優(yōu)化，得到較好的室內(nèi)重建結(jié)果，文獻(xiàn)[28]利用標(biāo)定球的方式求取2臺固定相機的轉(zhuǎn)換矩陣并重建出豬的三維模型。而本文采用更加方便的點云重配準(zhǔn)算法[29]優(yōu)化Tsub→master矩陣，運用公式

(1)

式中Topt——優(yōu)化后的矩陣

其中令ε=(α,β,γ,a,b,c)，參數(shù)ε運用高斯牛頓法求解得到，其公式為

(2)

式中r——殘差矢量

Jr——殘差矢量的雅可比矩陣

不斷迭代得到最優(yōu)位姿矩陣

最后實現(xiàn)2臺Azure Kinect DK深度相機的實時點云直接疊加。假設(shè)主相機的坐標(biāo)系為Cmaster，副相機的坐標(biāo)系為Csub，獲取的點云數(shù)據(jù)分別是Pmaster、Psub，由優(yōu)化后得到的2臺固定深度相機的變換矩陣Topt，并最終在主相機坐標(biāo)系Cmaster下輸出1幀點云數(shù)據(jù)為Pout，完成2幀點云的直接融合，見圖4b，計算公式為

Pout=Pmaster+ToptPsub

(3)

式中Pout——主相機輸出的點云

Pmaster——主相機獲取的點云

Psub——副相機獲取的點云

圖4 點云配準(zhǔn)結(jié)果Fig.4 Registration results of point clouds

1.4 棗樹三維點云信息采集

在3種典型的光照環(huán)境下(晴天、陰天、夜間)距離棗樹主桿中心1.1～1.3 m的位置對開心形棗樹進(jìn)行拍攝(圖5a)，其中晴天的平均光照強度為77 300 lx，陰天的平均光照強度為12 600 lx，夜間通過2個LED燈補光后的平均光照強度為4 000 lx(本文下面所有的3種典型光照環(huán)境都是采用平均光照強度)。對于參差不齊的矮化密植棗樹，盡管樹形結(jié)構(gòu)是開心形，但由于棗園管理不規(guī)范，開心形棗樹的側(cè)枝沒有統(tǒng)一的模式，側(cè)枝分布有2～5個，考慮到后續(xù)修剪樣機的設(shè)計，并經(jīng)現(xiàn)場實驗，最終確定逆時針旋轉(zhuǎn)系統(tǒng)平臺55°，如圖5b所示。對有5個側(cè)枝的開心形棗樹的采集過程，如圖5c～5h所示。獲取的一個視角下的點云命名為Tree-A，逆時針旋轉(zhuǎn)系統(tǒng)平臺55°獲取另一個視角下的點云，命名為Tree-B。

圖5 3種典型光照環(huán)境下棗樹信息Fig.5 Jujube tree information under three typical lighting environments

1.5 棗樹點云預(yù)處理

三維重建系統(tǒng)平臺獲取的棗樹場景空間信息，包含了大量的背景噪聲，如附近的棗樹、大棚和地面等，因此需要對獲取的原始棗樹點云進(jìn)行預(yù)處理，以獲得只有單棵棗樹的點云信息。同時對單棵棗樹點云數(shù)據(jù)進(jìn)行下采樣和濾波處理，以提高棗樹點云后續(xù)的配準(zhǔn)速度和精度，點云預(yù)處理流程如圖6所示。根據(jù)棗樹的三維點云坐標(biāo)信息，通過設(shè)置棗樹三維點云的距離閾值范圍，將小于閾值的點云視為內(nèi)點保留下來，大于閾值的視為無用點去除。依據(jù)棗樹樹冠幅寬在0.8～1 m以及高度在1.8～2.2 m，設(shè)置空間x、y方向閾值范圍分別為(-0.5 m,0.6 m)、(-1.5 m,1.2 m)，可很好地去除附近棗樹和頂部大棚的噪聲信息，并且系統(tǒng)平臺放置在距離棗樹主桿中心1.1～1.3 m的位置，設(shè)置空間方向的閾值為(0 m,1.4 m)可很好地去除棗樹背景噪聲，這樣就可獲得只包含單棵棗樹的三維點云信息。

圖6 點云預(yù)處理流程圖Fig.6 Flow chart of point clouds pre-processing

為了加速點云數(shù)據(jù)處理速度，需要降低棗樹點云的密度，利用體素濾波器[30]對棗樹點云進(jìn)行下采樣，其中柵格尺寸設(shè)置為5 mm，這樣能夠最大程度地保留棗樹的幾何結(jié)構(gòu)信息，對于下采樣后的棗樹點云周圍還存在著一些飛行點，這是由棗樹枝干之間相互遮擋以及消費級Azure Kinect DK深度相機本身測量誤差引起的。棗樹周圍的飛行點距離棗樹枝干較遠(yuǎn)且稀疏，因此本文采用基于半徑的濾波器[31]去除，該算法需要搜索半徑為R的圓內(nèi)的點數(shù)，將閾值小于k的點云去除，而保留滿足閾值的點，依據(jù)對棗樹點云處理的實驗結(jié)果選擇R=11 mm，k=10，能有效去除離群點。

1.6 棗樹點云配準(zhǔn)過程

由于棗樹本身結(jié)構(gòu)復(fù)雜，樹枝之間相互遮擋嚴(yán)重，對于本系統(tǒng)平臺采集的單視角點云不能得到完整枝干三維模型，棗樹枝干存在多處不連續(xù)的斷點，如圖7b所示(陰天時采集)。因此需要另外一個視角下的棗樹點云，然后對2個視角下的點云進(jìn)行配準(zhǔn)，對棗樹枝干的斷點進(jìn)行補全。對于結(jié)構(gòu)復(fù)雜的棗樹來說，直接采用ICP或者NDT算法進(jìn)行配準(zhǔn)，誤匹配率高，計算復(fù)雜度高，并且容易陷入局部最優(yōu)而不能完成2個視角點云的配準(zhǔn)[32]。因此提出一種基于骨架點對棗樹點云進(jìn)行初配準(zhǔn)，獲得2幀棗樹點云較近的位置，然后再利用經(jīng)典ICP算法進(jìn)行精確配準(zhǔn)，配準(zhǔn)流程如圖8所示。

圖7 非完整棗樹枝干模型Fig.7 Incomplete model of jujube tree branches

圖8 棗樹點云配準(zhǔn)流程圖Fig.8 Pipline of point cloud registration

首先利用拉普拉斯算法[33]提取出預(yù)處理后棗樹點云的骨架線，該算法可以實現(xiàn)在有噪聲的棗樹點云中對其幾何結(jié)構(gòu)進(jìn)行骨架線提取，公式為

(4)

式中P——預(yù)處理后的棗樹點云

P′——提取的棗樹點云

L——m×m的拉普拉斯矩陣

WL、WH——對角權(quán)重矩陣

拉普拉斯矩陣中m設(shè)置為16，并且在提取點云P′時最遠(yuǎn)下采樣的半徑設(shè)置為4 mm，能較好地快速獲得棗樹的骨架線，并對骨架線下采樣獲取其骨架點，如圖9所示。

圖9 棗樹骨架點提取Fig.9 Extraction of jujube tree skeleton point

其次對獲取的棗樹點云骨架點進(jìn)行法向量估計，利用特征描述子FPFH(快速點特征直方圖)[34]計算骨架點的特征矢量，并經(jīng)過SAC-IA(采樣一致性)算法[35]完成2個視角下棗樹點云骨架點的映射關(guān)系，該算法的基本步驟：①從點云Tree-A的骨架點中選n個采樣點，且采樣點之間的距離應(yīng)滿足大于設(shè)定的最小閾值dmin。②在點云Tree-B的骨架點中查詢與點云Tree-A具有相似特征的1個或多個點，并隨機從這些點中選擇1個點作為Tree-A和Tree-B之間的對應(yīng)點。③計算對應(yīng)點的變換矩陣，并通過對應(yīng)點變換后的“距離誤差和”函數(shù)來度量2幀棗樹點云之間轉(zhuǎn)換的質(zhì)量。通過不斷重復(fù)這3個步驟得到最小的誤差，完成2幀點云的初始匹配，得到初始轉(zhuǎn)換矩陣T0。

最后采用經(jīng)典ICP算法對初始轉(zhuǎn)換矩陣T0進(jìn)一步優(yōu)化完成棗樹點云精配準(zhǔn)過程并得到轉(zhuǎn)換矩陣T，其中設(shè)置ICP最大的迭代次數(shù)為100以及前后2次的迭代誤差為0.02 mm。最后利用轉(zhuǎn)換矩陣T來重建完整的棗樹枝干三維模型，棗樹總體三維重建過程如圖10所示。

圖10 棗樹重建總體流程圖Fig.10 Overall process of jujube tree reconstruction

2 結(jié)果與討論

本文搭建系統(tǒng)平臺有2個主要原因：①矮化密植開心形棗樹的枝干較細(xì)，樹形結(jié)構(gòu)復(fù)雜，枝干之間遮擋比較嚴(yán)重。②棗樹的枝干表面非常粗糙，傳感器到達(dá)棗樹表面的紅外光線不能被完全反射回來，造成枝干信息不能被傳感器接收。即使利用單個深度相機分別獲取2個視角下的棗樹點云(陰天時采集)將其配準(zhǔn)在一起，也不能準(zhǔn)確反映棗樹枝干的三維模型，如圖11d所示。而利用2個平行固定實時同步的深度相機在單視角下就能夠得到質(zhì)量較好的棗樹枝干模型，但存在不連續(xù)的枝干斷點(陰天時采集)，如圖12b所示。

圖11 單相機重建結(jié)果Fig.11 Reconstruction of single camera

圖12 系統(tǒng)平臺獲取的單視角棗樹點云Fig.12 Point clouds of jujube tree obtained by system platform in single view

2.1 標(biāo)定方法的配準(zhǔn)精度評估

通過張正友標(biāo)定法確定2臺固定深度相機的初始位姿，然后對初始位姿進(jìn)一步優(yōu)化，最終確定2臺相機的轉(zhuǎn)換矩陣Topt。為了定量分析2臺固定相機之間點云的配準(zhǔn)精度，采用點到點的方法(C2C)[36]測量原始點云與轉(zhuǎn)換后的目標(biāo)點云的平均距離來評估2幀點云的融合程度，2幀點云之間的平均距離誤差越小說明配準(zhǔn)越好，反之配準(zhǔn)越差。在3種典型自然環(huán)境下(晴天、陰天、夜間)，利用系統(tǒng)平臺獲取300 mm×300 mm棋盤格標(biāo)定板的點云信息，然后采用CloudCompare軟件計算標(biāo)定板的原始點云和轉(zhuǎn)換后的目標(biāo)點云之間的平均距離誤差。晴天平均距離誤差為0.311 mm，陰天平均距離誤差為0.249 mm，夜間平均距離誤差為0.167 mm，在3種典型環(huán)境下，配準(zhǔn)的平均距離誤差變化較小，說明標(biāo)定方法具有很好的魯棒性。

2.2 2個視角下點云配準(zhǔn)效果

對于預(yù)處理后的2幀點云Tree-A與Tree-B，利用提取的骨架點在3種典型自然環(huán)境下(晴天、陰天、夜間)進(jìn)行配準(zhǔn)實驗，配準(zhǔn)實驗結(jié)果如圖13、14所示。在自然條件下利用系統(tǒng)平臺采集棗樹點云時，光照的變化對其有一定的影響，在晴天時每個視角下采集的棗樹點云數(shù)量小于陰天采集，陰天采集的棗樹點云數(shù)量小于夜間采集。晴天時對采集系統(tǒng)有一定影響使得重建的棗樹枝干還有部分不完整，如圖14b所示，而陰天和夜間對采集系統(tǒng)影響小，能夠重建完整的棗樹枝干，如圖13d、14d所示。

圖13 陰天配準(zhǔn)過程Fig.13 Process of registrationin in cloudy day

圖14 晴天、夜間精配準(zhǔn)結(jié)果Fig.14 Results of registration in sunny day and at night

2.3 棗樹點云配準(zhǔn)結(jié)果分析

通過單臺相機獲取2個視角下棗樹點云，然后進(jìn)行匹配得到的棗樹枝干三維模型質(zhì)量不高，故不對其配準(zhǔn)誤差做進(jìn)一步分析。利用匹配點總數(shù)(需要配準(zhǔn)的最少點數(shù))、配準(zhǔn)時間(骨架點或關(guān)鍵點提取，初配準(zhǔn)和精配準(zhǔn)整個配準(zhǔn)流程時間)和擬合分?jǐn)?shù)3個指標(biāo)進(jìn)行配準(zhǔn)結(jié)果分析，其中的擬合分?jǐn)?shù)是源點云與對應(yīng)旋轉(zhuǎn)后目標(biāo)點云之間的平均距離，且其值越小，說明配準(zhǔn)效果越好[37]。表2～4對經(jīng)典ICP算法、SIFT-ICP算法、ISS-ICP算法、HARRIS-ICP算法[38-40]以及本文提出的算法在3種典型的光照環(huán)境下(晴天、陰天、夜間)進(jìn)行了對比。配準(zhǔn)結(jié)果表明：①經(jīng)典ICP算法相對于SIFT-ICP算法、ISS-ICP算法、HARRIS-ICP算法配準(zhǔn)時間短，但其擬合分?jǐn)?shù)最大即配準(zhǔn)誤差最大。②3種利用關(guān)鍵點的算法在相同光照環(huán)境下的擬合分?jǐn)?shù)基本一樣；在不同光照環(huán)境下，3種算法在陰天時擬合分?jǐn)?shù)最小，配準(zhǔn)時間介于晴天和夜間之間，而在晴天時配準(zhǔn)時間最少，但配準(zhǔn)誤差最大。③本文提出的算法相對于以上4種算法其配準(zhǔn)時間和配準(zhǔn)誤差都進(jìn)一步降低；相對于陰天和夜間，在晴天時，棗樹點云配準(zhǔn)所需時間最少，為0.09 s，而配準(zhǔn)誤差最大，其擬合分?jǐn)?shù)為0.000 29；在陰天時，棗樹點云配準(zhǔn)的時間介于晴天和夜間，而此時配準(zhǔn)誤差最小，其擬合分?jǐn)?shù)為0.000 11。

表2 晴天時點云配準(zhǔn)結(jié)果Tab.2 Analysis of point cloud registration in sunny day

表3 陰天時點云配準(zhǔn)結(jié)果Tab.3 Analysis of point cloud registration in cloudy day

表4 夜間時點云配準(zhǔn)結(jié)果Tab.4 Analysis of point cloud registration at night

2.4 棗樹枝干重建的完整性和配準(zhǔn)穩(wěn)定性分析

在3種典型環(huán)境下，任意選擇5棵矮化密植開心形棗樹進(jìn)行重建，并對重建的枝干完整性和配準(zhǔn)穩(wěn)定性加以分析。重建后的棗樹枝干數(shù)目以及在晴天時重建后枝干的斷點數(shù)量(在陰天和夜間時，沒有斷點)，如表5所示。在3種典型環(huán)境下的配準(zhǔn)結(jié)果如表6所示，在相同的光照條件下，擬合分?jǐn)?shù)差別較小，表明本文算法具有較好的穩(wěn)定性。

表5 棗樹枝干數(shù)量與枝干斷點數(shù)Tab.5 Numbers of jujube branch and breakpoint

表6 棗樹點云配準(zhǔn)結(jié)果Tab.6 Results of jujube point clouds registration

3 結(jié)論

(1)提出了采用2臺消費級固定的深度相機實時重建矮化密植棗樹的系統(tǒng)平臺，利用點云重配準(zhǔn)算法提高了2臺深度相機之間配準(zhǔn)精度，簡化了配準(zhǔn)流程，使得該系統(tǒng)平臺能夠獲取高質(zhì)量棗樹枝干的三維點云模型。

(2)提出了基于骨架點的重建棗樹枝干算法的整體流程，縮短了棗樹重建時間以及進(jìn)一步降低了配準(zhǔn)誤差，并在3種典型光照環(huán)境下(晴天、陰天、夜間)進(jìn)行了對比實驗。結(jié)果表明，光照對于本文棗樹三維重建系統(tǒng)平臺有一定的影響，進(jìn)而影響到棗樹點云的配準(zhǔn)時間和精度。本文算法在晴天所需配準(zhǔn)時間最少，為0.09 s，但是配準(zhǔn)誤差卻最大，擬合分?jǐn)?shù)為0.000 29；陰天配準(zhǔn)誤差最小，其擬合分?jǐn)?shù)為0.000 11；夜間配準(zhǔn)所需時間最長，為0.16 s，配準(zhǔn)誤差介于晴天和陰天。因此后續(xù)設(shè)計棗樹修剪機器人樣機時要考慮光照的影響。