基于三維點(diǎn)云的運(yùn)輸車糧箱動(dòng)態(tài)均勻裝載方法研究

王梟雄 王 卓 白曉平 葛志康 趙泳嘉

(1.中國(guó)科學(xué)院沈陽(yáng)自動(dòng)化研究所，沈陽(yáng) 110016；2.中國(guó)科學(xué)院大學(xué)計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院，北京 100049；3.中國(guó)科學(xué)院機(jī)器人與智能制造創(chuàng)新研究院，沈陽(yáng) 110169)

0 引言

隨著自動(dòng)駕駛技術(shù)的發(fā)展，特別是主從式協(xié)同控制技術(shù)的發(fā)展，聯(lián)合收獲機(jī)-運(yùn)輸車協(xié)同控制的自動(dòng)收獲作業(yè)方式被廣泛研究與應(yīng)用[1-4]。其作物卸料裝車普遍采用固定裝載點(diǎn)，或預(yù)設(shè)裝載路徑的方式進(jìn)行，在無(wú)人工參與平倉(cāng)的情況下，其裝載率較低。有效的裝載方法對(duì)于提高運(yùn)輸車糧箱裝載率，進(jìn)而提高主從協(xié)同作業(yè)的效率具有重要意義。

為了在裝載過(guò)程中建立有效的數(shù)據(jù)反饋，需要對(duì)運(yùn)輸車糧箱的裝載狀態(tài)進(jìn)行檢測(cè)。目前對(duì)糧箱裝載狀態(tài)的識(shí)別主要有基于超聲波傳感器[5]、基于壓力傳感器[6]和基于機(jī)器視覺(jué)[7-9]的方法。在瀝青混凝土卸料裝車系統(tǒng)中，王芳等[5]使用超聲波傳感器來(lái)檢測(cè)料堆的高度，田明銳等[8]使用機(jī)器視覺(jué)的方法，通過(guò)測(cè)量料堆角度和高度信息，來(lái)判斷運(yùn)輸車箱的裝載情況。王錄民等[6]通過(guò)在糧堆底部安裝壓力傳感器，利用糧堆高度與底部壓力的近似線性關(guān)系判斷谷物分布狀態(tài)，完成對(duì)靜態(tài)下的糧倉(cāng)裝載狀態(tài)測(cè)量。陳瑩等[7]使用CCD相機(jī)從多個(gè)角度拍攝堆料的側(cè)視圖，通過(guò)各個(gè)橫截面的面積來(lái)計(jì)算大型堆料的體積。LIU等[9]使用基于機(jī)器視覺(jué)的方法，利用谷物與糧箱的接觸線來(lái)檢測(cè)裝載狀態(tài)，將糧倉(cāng)劃分為4個(gè)區(qū)域，通過(guò)設(shè)定閾值來(lái)判斷其裝載狀態(tài)。

目前，由于對(duì)裝載狀態(tài)檢測(cè)的精度不高，基于此進(jìn)行的裝載方法對(duì)裝載率的提高有限。近年來(lái)，基于三維點(diǎn)云的測(cè)量與目標(biāo)重建被廣泛研究，通過(guò)雙目視覺(jué)[10]、結(jié)構(gòu)光[11]或激光雷達(dá)[12-13]來(lái)完成對(duì)目標(biāo)的測(cè)量，在果樹測(cè)量[14-15]和動(dòng)物體尺測(cè)量[16-17]等農(nóng)業(yè)領(lǐng)域取得了良好的效果。

本文提出基于三維點(diǎn)云的聯(lián)合收獲機(jī)-運(yùn)輸車卸料裝載方法。該方法運(yùn)用多個(gè)相機(jī)獲取能準(zhǔn)確反映運(yùn)輸車糧箱裝載狀態(tài)的三維點(diǎn)云，以此進(jìn)行裝載均勻性評(píng)估，通過(guò)實(shí)時(shí)調(diào)整卸料裝載點(diǎn)位置，使糧箱始終保持在均勻的裝載狀態(tài)，從而提高滿載時(shí)的糧箱裝載利用率，同時(shí)提高裝載過(guò)程中運(yùn)輸車糧箱的穩(wěn)定性，降低溢料的可能性。針對(duì)裝載物相互遮擋造成的點(diǎn)云不完整問(wèn)題，建立堆體的物理模型和相機(jī)遮擋模型，來(lái)估計(jì)盲區(qū)點(diǎn)云高度，從而進(jìn)行點(diǎn)云填充，以得到完整的裝載狀態(tài)信息。

1 檢測(cè)裝置與方法

1.1 檢測(cè)裝置

通常運(yùn)輸車糧箱為上方開口的長(zhǎng)方體結(jié)構(gòu)，如圖1所示。為了避免對(duì)聯(lián)合收獲機(jī)-運(yùn)輸車協(xié)同作業(yè)過(guò)程中的卸料裝載過(guò)程造成影響，且相機(jī)安裝在超出車體寬度的區(qū)域時(shí)存在安全隱患，相機(jī)的可選安裝區(qū)域被限定在糧箱四周的上方區(qū)域，如圖1中的陰影部分?？紤]到安裝過(guò)高時(shí)，相機(jī)與糧箱難以進(jìn)行剛性連接，車輛行駛過(guò)程中的振動(dòng)會(huì)對(duì)相機(jī)的檢測(cè)造成影響，安裝高度通常受到限制。

圖1 相機(jī)安裝位置與糧倉(cāng)示意圖Fig.1 Camera installation location and granary diagram

由于單個(gè)相機(jī)的可視范圍難以覆蓋較大的糧箱，而增加相機(jī)數(shù)量會(huì)增加成本，同時(shí)提高系統(tǒng)的復(fù)雜度，本文使用2個(gè)相機(jī)，通過(guò)調(diào)整相機(jī)的安裝位置及相機(jī)視場(chǎng)的俯仰角、偏航角和滾動(dòng)角來(lái)提高相機(jī)視場(chǎng)對(duì)糧箱區(qū)域的覆蓋率[18]。

相機(jī)的三維有向感知模型如圖2所示，其由P、λ、θ、ω、2α、2β、d等7個(gè)因素確定[18]，其中P為相機(jī)的安裝位置坐標(biāo)(x,y,z)，如圖1所示，其取值范圍為

(1)

式中a、b、c——糧箱的長(zhǎng)度、寬度和高度

h——相機(jī)安裝的限制高度

圖2 相機(jī)三維有向感知模型Fig.2 3D directional sensing model for camera

(2)

ω為相機(jī)繞光軸PC的滾動(dòng)角(右手定則)，取值范圍為[0,π]，2α、2β分別為相機(jī)水平方向視場(chǎng)角和垂直方向視場(chǎng)角，d為相機(jī)的最佳成像距離，取值范圍為[d1,d2],d1、d2為相機(jī)的最近成像距離與最遠(yuǎn)成像距離。

為了準(zhǔn)確地對(duì)糧箱的裝載狀態(tài)進(jìn)行檢測(cè)，相機(jī)的視野范圍應(yīng)盡可能多地覆蓋糧箱內(nèi)裝載物堆體的表面，而作物在糧倉(cāng)內(nèi)的裝載包括從空載到滿載的多種情況，將堆體表面在糧箱內(nèi)的分布近似簡(jiǎn)化為均勻隨機(jī)分布，則相機(jī)對(duì)堆體表面的覆蓋率可由相機(jī)對(duì)糧倉(cāng)空間的覆蓋率進(jìn)行度量。將糧箱空間離散化為等距離的離散點(diǎn)，對(duì)于滿足上述條件的任意一個(gè)相機(jī)安裝位姿(x0,y0,z0,λ0,θ0,ω0)，依據(jù)空間點(diǎn)覆蓋判定準(zhǔn)則計(jì)算覆蓋率η[18]，計(jì)算式為

η=|ψa|/|ψ|

(3)

式中 |·|——求集合中元素總數(shù)的運(yùn)算符號(hào)

ψ——空間區(qū)域的空間點(diǎn)集合

ψa——相機(jī)視場(chǎng)覆蓋的空間點(diǎn)集合

對(duì)于糧倉(cāng)離散空間中的任意一點(diǎn)T(x0,y0,z0),如圖2所示，在相機(jī)安裝位置為P(x,y,z)時(shí)，其屬于相機(jī)視場(chǎng)覆蓋的空間點(diǎn)集合ψa的條件為

(4)

其中

(5)

(6)

式中ec——相機(jī)感知方向單位矢量

Xt——相機(jī)的視場(chǎng)滾動(dòng)角為0°時(shí)，相機(jī)水平視場(chǎng)方向單位矢量

Xrt——相機(jī)的視場(chǎng)滾動(dòng)角為ω時(shí)，相機(jī)水平視場(chǎng)方向單位矢量

Zt——相機(jī)的視場(chǎng)滾動(dòng)角為0°時(shí)，相機(jī)垂直視場(chǎng)方向單位矢量

lPT、lPC、lCT、lCE——兩點(diǎn)間的長(zhǎng)度向量

據(jù)此可以得到使用任一安裝參數(shù)時(shí)的相機(jī)視場(chǎng)覆蓋率，然后使用視場(chǎng)覆蓋增強(qiáng)算法進(jìn)行優(yōu)化，得到覆蓋率最大的相機(jī)安裝參數(shù)[18]。

當(dāng)其中一個(gè)相機(jī)的安裝位置和姿態(tài)確定后，另一相機(jī)采用與其幾何對(duì)稱式的安裝位姿，即若相機(jī)1的安裝位姿為(x1,y1,z1,λ1,θ1,ω1)時(shí)，相機(jī)2的安裝位姿為(x2,y2,z2,λ2,θ2,ω2),計(jì)算式為

(7)

采用這種對(duì)稱式的安裝方式，能避免堆體由于自身遮擋而使相機(jī)無(wú)法觀測(cè)到堆體全貌的情況。

1.2 點(diǎn)云數(shù)據(jù)預(yù)處理

為了將2個(gè)相機(jī)獲取的點(diǎn)云數(shù)據(jù)進(jìn)行融合，采用張正友標(biāo)定法[19]對(duì)各個(gè)相機(jī)進(jìn)行內(nèi)外參數(shù)的標(biāo)定。由于2個(gè)相機(jī)有重疊視野，制作標(biāo)定圖像時(shí)，保持標(biāo)定板位置Cb不變，由2個(gè)相機(jī)分別采集圖像p1、p2加入到標(biāo)定集P1、P2中，則可得到2個(gè)相機(jī)相對(duì)于標(biāo)定板位置Cb的外參矩陣H1、H2。

利用矩陣H1、H2對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行坐標(biāo)變換，則將2個(gè)點(diǎn)云數(shù)據(jù)統(tǒng)一到標(biāo)定板Cb的坐標(biāo)下，再使用迭代最近點(diǎn)(ICP)算法[20-21]進(jìn)行精配準(zhǔn)，使其融合為一個(gè)點(diǎn)云，經(jīng)過(guò)簡(jiǎn)單的平移或旋轉(zhuǎn)變化則可得到世界坐標(biāo)系XOY下的點(diǎn)云數(shù)據(jù)。

1.3 點(diǎn)云填充

由于裝載物之間相互遮擋，融合后的點(diǎn)云存在孔洞，不能完整地表達(dá)糧箱的裝載狀態(tài)，也不利于后續(xù)處理，所以需要進(jìn)行點(diǎn)云填充。國(guó)內(nèi)外學(xué)者針對(duì)點(diǎn)云填充進(jìn)行了許多研究，何東健等[22]使用改進(jìn)的三次B樣條曲線對(duì)奶牛點(diǎn)云的缺失區(qū)域進(jìn)行修復(fù)，NOWAK等[23]提出了一種基于貝塞爾曲面的空腔重建方法用于三維掃描的人體表面填充，GAI等[24]使用徑向基函數(shù)來(lái)恢復(fù)復(fù)雜表面孔洞的信息。這些方法在點(diǎn)云填充上取得了良好的效果，但是其僅根據(jù)孔洞臨近點(diǎn)進(jìn)行填充，未充分利用盲區(qū)的形成條件，在堆體遮擋所形成的盲區(qū)進(jìn)行填充時(shí)誤差較大。為此，本文建立盲區(qū)模型進(jìn)行點(diǎn)云填充。

單一顆粒材料在連續(xù)澆筑過(guò)程中形成的堆體可以由安息角進(jìn)行表征[25-26]，形成的堆體可以近似為圓錐。圖3為堆體與相機(jī)視野的遮擋示意圖，其中△ABC為崩塌后的圓錐形堆體的橫截面，δ0為堆體ABC的安息角，相機(jī)安裝在堆體右上方，由于堆體ABC對(duì)相機(jī)視野的遮擋，在OE段形成盲區(qū)。依據(jù)堆體的物理模型，堆體的安息角不變，堆體的高度AE與其底面半徑BE呈線性關(guān)系。為了使可觀測(cè)到的堆體表面的截面AC穩(wěn)定存在，盲區(qū)OAE內(nèi)堆體的最小分布狀態(tài)為以點(diǎn)A為頂點(diǎn)的截面為△ABE的半圓錐，即在盲區(qū)截面AOE內(nèi)，ABE部分的裝載狀態(tài)確定，僅在OAB區(qū)域內(nèi)的裝載狀態(tài)不確定。如圖3a所示，當(dāng)向盲區(qū)OE內(nèi)進(jìn)行裝載時(shí)，可形成高度不超過(guò)視線OA的各種堆體，所以盲區(qū)OAB的裝載狀態(tài)存在空載到滿載的各種可能的裝載狀態(tài)。為使估計(jì)的點(diǎn)云分布與實(shí)際的點(diǎn)云分布的期望誤差最小，取盲區(qū)內(nèi)點(diǎn)云可能分布的最大高度與最小高度的平均值作為該點(diǎn)的估計(jì)高度，如圖3b所示，OFA即為估計(jì)的盲區(qū)OE內(nèi)的點(diǎn)云分布的截面。其中δ1與δ2均小于安息角δ0，即OFA是可能形成的穩(wěn)定堆體表面。

圖3 堆體與盲區(qū)示意圖Fig.3 Schematics of heap and its blind area

當(dāng)2個(gè)堆體相互遮擋時(shí)可能形成的截面如圖4所示，分別為兩谷堆A1B1C1與A2B2C2之間相互重疊，以及相互不重疊2種狀態(tài)。O1、O2為盲區(qū)的邊界點(diǎn)，綠色陰影區(qū)域?yàn)榇_定的裝載狀態(tài)，黃色陰影區(qū)域?yàn)榭赡艿淖畲笱b載狀態(tài)。同樣取某點(diǎn)可能的最低裝載高度hmin與該點(diǎn)可能的最高裝載高度hmax的平均值為該點(diǎn)的估計(jì)裝載高度h，即

(8)

圖4 堆體相互遮擋形成的盲區(qū)示意圖Fig.4 Schematics of two heap and its blind area

將點(diǎn)云集合Ω?R3投影到XOY平面，得到二維點(diǎn)集Ω′?R2，盲區(qū)的俯視圖如圖5所示。對(duì)于盲區(qū)內(nèi)一點(diǎn)Q(x,y,z)，其在XOY平面內(nèi)的投影為Q′(x,y)。C′1、C′2分別為兩相機(jī)的安裝位置C1、C2在平面XOY上的投影。連接C′1與Q′并延長(zhǎng)，其與盲區(qū)邊界相交于O′11與O′12兩點(diǎn)；同樣連接C′2與Q′并延長(zhǎng)，其與盲區(qū)投影的邊界相交于O′21與O′22兩點(diǎn)。O′11、O′12、O′21、O′22分別為點(diǎn)云集合Ω內(nèi)的點(diǎn)O11、O12、O21、O22在XOY平面內(nèi)的投影。以O(shè)11、O12、O21、O22這4個(gè)點(diǎn)為頂點(diǎn)，形成的圓錐堆體底面的半徑為dij，計(jì)算式為

(9)

式中zij——點(diǎn)Oij的z坐標(biāo)δ——堆體安息角

當(dāng)點(diǎn)Q在以點(diǎn)Oij為頂點(diǎn)的圓錐上方時(shí)，點(diǎn)Q的最低高度hmin受到點(diǎn)Oij所在堆體的影響，否則不受其影響。定義點(diǎn)Q的最小高度hij為

(10)

式中d′ij——Q′與O′ij的距離

則點(diǎn)Q(x,y,z)的z坐標(biāo)最小值為

hmin=max{hij|i,j=1,2}

(11)

點(diǎn)Q的最大高度hmax受到所有通過(guò)點(diǎn)Q′正上方的相機(jī)視線的影響，即所有在XOY平面上的投影通過(guò)點(diǎn)Q′的相機(jī)視線。由于所有的相機(jī)視線都通過(guò)相機(jī)安裝位置點(diǎn)C1或點(diǎn)C2，即所有相機(jī)視線在XOY平面上的投影都通過(guò)點(diǎn)C′1或點(diǎn)C′2。所以通過(guò)點(diǎn)Q′的相機(jī)視線投影僅有直線C1Q′和C2Q′。

定義相機(jī)視線通過(guò)點(diǎn)Q′正上方時(shí)的視線高度gi為

(12)

式中d(C′i,Q′)——點(diǎn)C′i與點(diǎn)Q′的距離

d(C′i,O′i1)——點(diǎn)C′i與點(diǎn)O′i1的距離

Ci,z——點(diǎn)Ci的z坐標(biāo)

Oi1,z——點(diǎn)Oi1的z坐標(biāo)

則點(diǎn)Q(x,y,z)的z坐標(biāo)最大值為

hmax=min{g1,g2}

(13)

點(diǎn)Q(x,y,z)的z坐標(biāo)z=h可由式(8)～(13)得到。由此，對(duì)盲區(qū)投影內(nèi)的任意一點(diǎn)Q′(x,y)，可估計(jì)其空間坐標(biāo)Q(x,y,z),完成點(diǎn)云填充。

1.4 裝載方法

1.4.1裝載均勻性評(píng)估

(14)

進(jìn)一步方差可表示為

(15)

其中

(16)

ρ——裝載物密度

即裝載均勻性可由裝載面積s上的平均高度h的方差進(jìn)行評(píng)估。

1.4.2裝載點(diǎn)位置計(jì)算

(17)

裝載后整體的方差計(jì)算式為

(18)

其中1≤j≤n，最均勻裝載網(wǎng)格l=argminσ2(j)，此時(shí)l=argmin{hi,1≤i≤n}。

利用離散化后的網(wǎng)格平均高度的方差進(jìn)行均勻性評(píng)估時(shí)，每次選取當(dāng)前平均高度最低的網(wǎng)格進(jìn)行裝載，只考慮了各個(gè)網(wǎng)格的裝載量在數(shù)值上無(wú)序化后的均勻性，未能充分考慮不同裝載量在位置上的相互關(guān)系。當(dāng)糧箱內(nèi)存在多個(gè)相同的最低平均高度的網(wǎng)格時(shí)，裝載點(diǎn)位置的選擇具有一定的隨機(jī)性，然而此時(shí)不同裝載點(diǎn)位置的選擇所產(chǎn)生的裝載均勻性效果不同。如在糧箱空載時(shí)，固定選取離糧箱幾何中心最近的網(wǎng)格進(jìn)行裝載，而在其后進(jìn)行的裝載過(guò)程中，亦存在多個(gè)相同的最低平均高度的網(wǎng)格的情況，若裝載點(diǎn)全部連續(xù)地選取在糧箱的一側(cè)(圖6a)，其整體裝載均勻性較差，糧箱右側(cè)裝載量明顯高于左側(cè)，裝載物的重心偏向糧箱幾何中心的右側(cè)。在相同裝載量的情況下，圖6b為更加理想的裝載狀態(tài)，其整體裝載均勻性較圖6a更好，裝載物的重心與糧箱的幾何中心重合，在均勻裝載的同時(shí)，可以降低由于裝載后車輛重心偏移對(duì)車輛行駛的穩(wěn)定性的影響。為此，引入重心偏移量進(jìn)行輔助評(píng)估，使裝載物整體的重心盡可能地靠近糧倉(cāng)的幾何中心。

裝載物的重心坐標(biāo)(xc,yc)計(jì)算式為

(19)

(20)

式中mc——當(dāng)前裝載物的質(zhì)量

則重心偏移量d為裝載物重心與糧倉(cāng)幾何中心的距離，計(jì)算式為

(21)

式中 (xk,yk)——糧倉(cāng)幾何中心點(diǎn)坐標(biāo)

通常最均勻裝載點(diǎn)與最小重心偏移量的裝載點(diǎn)不同，甚至?xí)霈F(xiàn)向一個(gè)網(wǎng)格裝載后，裝載均勻性提高(方差減小)，而重心偏移量也增大的情況。為此設(shè)計(jì)一個(gè)選擇策略在二者之間進(jìn)行權(quán)衡。

裝載點(diǎn)的選擇策略如下：將糧箱離散化，選取平均高度最小的網(wǎng)格的中心作為裝載點(diǎn),則該裝載點(diǎn)為最均勻裝載點(diǎn)。當(dāng)存在多個(gè)相同的最小平均高度的網(wǎng)格時(shí)，計(jì)算向這些網(wǎng)格裝載后的裝載物重心偏移量，選取其中重心偏移量最小的網(wǎng)格作為裝載點(diǎn)。

1.4.3可行性與魯棒性分析

(22)

(23)

(24)

其中

(25)

則有

(26)

從而有

(27)

(28)

(29)

即此后每個(gè)裝載周期選取平均高度最低的網(wǎng)格進(jìn)行裝載后，該網(wǎng)格的平均高度成為當(dāng)前所有網(wǎng)格中的最大值，則在n個(gè)裝載周期后，所有網(wǎng)格都進(jìn)行且僅進(jìn)行了一個(gè)裝載周期的裝載過(guò)程，裝載狀態(tài)由GK變?yōu)?/p>

(30)

由此可得

σ(GK+t)=σ(GK+t+n) (t∈N)

(31)

即裝載呈現(xiàn)周期性變化，平均高度的方差也呈現(xiàn)周期性變化，周期為n。由于方差可取的值為有限個(gè)，所以裝載狀態(tài)的方差σ(GK+t)存在最大值σmax。

假設(shè)t0個(gè)裝載周期后方差達(dá)最大值σmax，則有

σmax=σ(Gt0)

(32)

將式(17)代入式(18)可得

(33)

由于σ(Gt0)>σ(Gt0-1)，代入式(33)得

(34)

(35)

由于σ(Gt0)>σ(Gt0+1)，代入式(33)得

(36)

(37)

(38)

將式(17)代入式(38)有

(39)

(40)

將式(29)代入式(37)有

(41)

聯(lián)立式(40)、(41)有

(42)

則

(43)

該方法對(duì)不同的初始裝載狀態(tài)具有很強(qiáng)的魯棒性，裝載過(guò)程中，由于突發(fā)的顛簸或者其他意外情況導(dǎo)致未能在指定的位置進(jìn)行裝載時(shí)，此后的裝載過(guò)程可將當(dāng)前期望之外的裝載狀態(tài)視為初始狀態(tài)繼續(xù)進(jìn)行裝載，亦能達(dá)到既定的裝載狀態(tài)，即該方法具有抗干擾性。

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

2.1 實(shí)驗(yàn)裝置

為了驗(yàn)證裝載方法的有效性，搭建如圖7所示的實(shí)驗(yàn)平臺(tái)進(jìn)行實(shí)驗(yàn)驗(yàn)證。實(shí)驗(yàn)糧箱為2.0 m×2.0 m×0.65 m的長(zhǎng)方體，由于安裝工具自身高度的影響，相機(jī)的最低安裝高度限制為0.8 m，最高安裝高度限制為1 m。獲取裝載狀態(tài)的三維點(diǎn)云所使用的相機(jī)型號(hào)為MYNT-D1000-IR-120/Color,其幀率為60 f/s,深度分辨率為1 280像素×640像素，工作距離為0.32～7 m，水平方向視角為105°，垂直方向視角為58°。

圖7 實(shí)驗(yàn)平臺(tái)Fig.7 Experiment platform

為了確定相機(jī)的安裝參數(shù)(x,y,z,λ,θ,ω)，采用Matlab的遺傳工具箱進(jìn)行6參數(shù)優(yōu)化，其取值范圍的下限為{0 m,0 m,0.8 m,-π/2,0 rad,0 rad}，取值范圍的上限為{0 m,2 m,1 m,0,π,π}。適應(yīng)度函數(shù)取相機(jī)視場(chǎng)對(duì)糧箱空間的覆蓋率，其中糧倉(cāng)空間離散為間距為5 cm的點(diǎn)集。Matlab中除參數(shù)初始值設(shè)定為與參數(shù)的取值范圍一致外，其他保持默認(rèn)設(shè)置。經(jīng)過(guò)遺傳算法優(yōu)化后得到的其中一個(gè)相機(jī)的安裝參數(shù)為(0 m,0 m,0.8 m,-0.53 rad,0.785 rad,0 rad),由式(7)可以計(jì)算得到另外一個(gè)對(duì)稱安裝的相機(jī)的安裝參數(shù)為(2 m,2 m,0.8 m,-0.53 rad,3.925 rad,0 rad)。為了對(duì)點(diǎn)云進(jìn)行對(duì)比，配置了參考相機(jī)來(lái)獲取盲區(qū)的點(diǎn)云狀態(tài)作為實(shí)際點(diǎn)云分布，與算法填充的點(diǎn)云進(jìn)行對(duì)比。3個(gè)相機(jī)均通過(guò)相機(jī)云臺(tái)進(jìn)行固定，便于進(jìn)行安裝角度的調(diào)節(jié)。

目前已有稻谷、馬鈴薯、玉米、甘蔗、甜菜等農(nóng)作物使用特定的收獲機(jī)進(jìn)行收獲作業(yè)，本文選取2020年產(chǎn)自徐州市的冬季甜菜作為裝載對(duì)象。與氧化鋁等金屬粉末安息角的測(cè)量[27]不同，農(nóng)作物安息角的測(cè)量沒(méi)有相關(guān)的標(biāo)準(zhǔn)。孫驪等[28]對(duì)小麥和玉米等種子的安息角進(jìn)行了初步研究，發(fā)現(xiàn)依照安息角的變化規(guī)律，可以預(yù)測(cè)某類種子在一定條件下的安息角。為了得到甜菜在裝載過(guò)程中的安息角，使用與裝載過(guò)程中相同大小和含水率的甜菜進(jìn)行安息角的測(cè)量，并使用相同的跌落高度與相同質(zhì)地的地面，通過(guò)多次測(cè)量，取其平均值33.80°作為甜菜的安息角。

將裝載狀態(tài)檢測(cè)與動(dòng)態(tài)均勻裝載方法搭載在車載平臺(tái)Jetson AGX Xavier上運(yùn)行，使用C/C++作為實(shí)現(xiàn)語(yǔ)言，將上述Matlab離線處理得到的坐標(biāo)轉(zhuǎn)換矩陣，以及實(shí)驗(yàn)得到的安息角等參數(shù)寫入算法中直接使用，這些參數(shù)一經(jīng)獲取，在后續(xù)的實(shí)時(shí)檢測(cè)與動(dòng)態(tài)均勻裝載算法中不再變動(dòng)，無(wú)需重復(fù)計(jì)算。在實(shí)時(shí)檢測(cè)的三維重建上，一方面通過(guò)體素下采樣、條件濾波等方法在不丟失關(guān)鍵數(shù)據(jù)的情況下大幅降低三維點(diǎn)云的數(shù)據(jù)量，另一方面充分利用Jetson的GPU資源，使用C/C++與CUDA混合編程，將包括體素下采樣、條件濾波等方法在內(nèi)的三維建模與裝載方法并行化，以滿足聯(lián)合收獲機(jī)-運(yùn)輸車協(xié)同作業(yè)的實(shí)時(shí)性要求。

2.2 實(shí)驗(yàn)結(jié)果

2.2.1點(diǎn)云填充實(shí)驗(yàn)結(jié)果

圖8為糧箱在輕載、中載和重載3種不同裝載狀態(tài)下甜菜的三維點(diǎn)云，其中左圖為2個(gè)實(shí)驗(yàn)相機(jī)獲取的甜菜裝載狀態(tài)的三維點(diǎn)云，中間為依據(jù)本文提出的算法進(jìn)行點(diǎn)云填充后甜菜的三維點(diǎn)云，右圖為2個(gè)實(shí)驗(yàn)相機(jī)和參考相機(jī)獲取融合后的甜菜的三維點(diǎn)云。

圖8 不同裝載狀態(tài)下的三維點(diǎn)云圖Fig.8 Three-dimensional point clouds under different loading states

可以看到，不同裝載狀態(tài)下，采用2個(gè)實(shí)驗(yàn)相機(jī)獲取的甜菜裝載狀態(tài)的三維點(diǎn)云均有不同程度的空缺，且裝載不均勻程度增加時(shí)，甜菜之間的相互遮擋情況加劇，導(dǎo)致盲區(qū)變大。與2個(gè)實(shí)驗(yàn)相機(jī)獲取的原始三維點(diǎn)云(左)相比，經(jīng)過(guò)點(diǎn)云填充處理后的三維點(diǎn)云(中)，能更加完整地反映甜菜的裝載狀態(tài)，沒(méi)有大片的不確定區(qū)域;經(jīng)過(guò)點(diǎn)云填充處理后的三維點(diǎn)云(中)與加入?yún)⒖枷鄼C(jī)后獲取的三維點(diǎn)云(右)相比，兩者的三維點(diǎn)云所反映的裝載狀態(tài)的一致性較好。

3種狀態(tài)下進(jìn)行點(diǎn)云填充的平均誤差分別為4.62、0.17、0.21 cm。裝載狀態(tài)1的盲區(qū)主要分布在靠近其中一個(gè)相機(jī)的位置，且附近有較高的堆體對(duì)另一相機(jī)形成了遮擋，導(dǎo)致其依據(jù)相機(jī)遮擋模型得到的最大高度較高，而其實(shí)際狀態(tài)分布為最小狀態(tài)，所以在該處盲區(qū)的點(diǎn)云高度估計(jì)的誤差相較于裝載狀態(tài)2和裝載狀態(tài)3的誤差較大。3種裝載狀態(tài)下的點(diǎn)云填充誤差均低于5 cm。

填充后的三維點(diǎn)云(中)的稠密度較實(shí)際的三維點(diǎn)云(右)稀疏，一方面由于在對(duì)左側(cè)點(diǎn)云進(jìn)行填充時(shí)，對(duì)于較小的盲區(qū)，由于其對(duì)裝載狀態(tài)的影響較小，未進(jìn)行點(diǎn)云填充；另一方面在對(duì)盲區(qū)進(jìn)行填充時(shí)，只保證能完整表達(dá)該區(qū)域的裝載狀態(tài)，未進(jìn)行高密度填充，這樣能加快點(diǎn)云的填充速度。

2.2.2裝載方法仿真結(jié)果

圖9 裝載過(guò)程中高度方差與重心偏移量變化曲線Fig.9 Variance of height and offset of center of gravity during loading

為了驗(yàn)證裝載方法的魯棒性，選取實(shí)驗(yàn)中采集的裝載狀態(tài)3為初始裝載狀態(tài)，進(jìn)行仿真，其重心偏移量和高度方差的變化如圖9b所示?？梢钥吹匠跏嫉难b載不均勻性和重心偏移量都較大,隨著裝載過(guò)程的繼續(xù)，高度方差和重心偏移量迅速減小，在裝載周期數(shù)達(dá)到1 400時(shí)裝載不均勻性達(dá)到最小值，如圖9b中局部放大圖所示，隨后進(jìn)入穩(wěn)定狀態(tài)，此后裝載均勻性(高度方差)呈周期變化，其方差最大值亦滿足式(43)。進(jìn)入穩(wěn)定狀態(tài)后，重心偏移量亦開始周期變化，但其幅值周期性的不斷減小，在裝載過(guò)程持續(xù)時(shí)，無(wú)限趨近于0。

相較于從空載狀態(tài)開始的裝載過(guò)程(圖9a)，裝載過(guò)程進(jìn)入穩(wěn)定狀態(tài)后，從裝載狀態(tài)3開始的裝載過(guò)程(圖9b)，在穩(wěn)定狀態(tài)下，其最小重心偏移量較高，且最小高度方差也相對(duì)較大。這是由于最小裝載量的存在，裝載方法無(wú)法在任意的裝載狀態(tài)下，將糧倉(cāng)的裝載狀態(tài)變?yōu)樽罹鶆驙顟B(tài)(高度方差和重心偏移量均為0)。

最小裝載量受到裝載物顆粒大小和最小裝載速度的影響，在一個(gè)具體的裝載過(guò)程中，裝載設(shè)備和裝載物已經(jīng)確定，即最小裝載量固定，可以通過(guò)增加裝載速度和提高裝載周期來(lái)增加單周期裝載量。一個(gè)連續(xù)的裝載過(guò)程可以離散為以裝載周期為間隔的一次次的裝載，為了研究單次裝載量對(duì)裝載過(guò)程的影響，分別選取單次裝載量的平均高度增量4 cm和8 cm進(jìn)行仿真，結(jié)果如圖9c、9d所示。

可以看到，由于單次裝載量的增加，裝載過(guò)程在更短的裝載周期內(nèi)達(dá)到穩(wěn)定狀態(tài)。單次裝載量越大，其在穩(wěn)定狀態(tài)下的裝載不均勻性越大，重心偏移量在一個(gè)穩(wěn)定周期內(nèi)的波動(dòng)范圍也越大。但當(dāng)單次裝載量增加時(shí)，裝載相同質(zhì)量的裝載物，所需裝載周期更短，這也意味著裝載位置的變化次數(shù)降低。實(shí)際應(yīng)用中可以根據(jù)允許的裝載均勻性要求，設(shè)定合適的單次裝載量。

為了說(shuō)明本文提出的動(dòng)態(tài)均勻裝載方法的優(yōu)勢(shì)，設(shè)計(jì)了與傳統(tǒng)的固定點(diǎn)裝載方法的對(duì)比仿真實(shí)驗(yàn)。傳統(tǒng)的固定點(diǎn)裝載方法在預(yù)先設(shè)定的位置連續(xù)進(jìn)行卸料裝載作業(yè)，待料堆達(dá)到預(yù)定的高度后，將卸料口移動(dòng)到下一個(gè)裝載點(diǎn)繼續(xù)進(jìn)行卸料裝載，直到所有裝載點(diǎn)位置的料堆達(dá)到預(yù)定高度后結(jié)束卸料裝載操作，主要應(yīng)用于聯(lián)合收獲機(jī)-運(yùn)輸車協(xié)同作業(yè)[11]和瀝青混凝土裝車系統(tǒng)[5,8]。

從圖10a可以看出，固定點(diǎn)裝載方法的高度方差變化曲線分為4段，其中每個(gè)轉(zhuǎn)折點(diǎn)表示在該時(shí)刻切換了裝載點(diǎn)位置，裝載的周期數(shù)達(dá)到823時(shí)，固定點(diǎn)裝載方法結(jié)束裝載過(guò)程，此后高度方差不再變化。與動(dòng)態(tài)均勻裝載方法相比，固定點(diǎn)裝載方法的高度方差明顯較大，且其裝載的周期數(shù)823遠(yuǎn)小于動(dòng)態(tài)均勻裝載方法的周期數(shù)1 800，即動(dòng)態(tài)均勻裝載方法的裝載量高于固定點(diǎn)裝載方法的2倍。

圖10b中，固定點(diǎn)裝載方法的重心偏移量初始值較大，雖然在初始階段糧箱的已裝載總量較小，由此產(chǎn)生的力矩較小，但其在更換裝載點(diǎn)后重心偏移量下降緩慢，在裝載中期產(chǎn)生的力矩較大。在周期數(shù)達(dá)到823，固定點(diǎn)裝載方法結(jié)束裝載過(guò)程時(shí)，其重心偏移量降為0，而此時(shí)動(dòng)態(tài)均勻裝載方法的最大重心偏移量也降至0.2 cm以下，維持在較低水平，在裝載過(guò)程結(jié)束時(shí)降為0。即動(dòng)態(tài)均勻裝載方法在以重心偏移量進(jìn)行評(píng)價(jià)的裝載效果也更優(yōu)。

由此可知，與固定點(diǎn)裝載方法相比，動(dòng)態(tài)均勻裝載方法能大幅提高糧箱的裝載均勻性，提高糧箱的裝載量。

圖10 固定點(diǎn)裝載方法與動(dòng)態(tài)均勻裝載方法的裝載均勻性變化曲線Fig.10 Evolution diagrams of loading uniformity of fixed points loading and dynamic uniform loading

3 結(jié)論

(1)探討了用于獲取糧箱裝載狀態(tài)的三維點(diǎn)云的相機(jī)安裝位姿，并利用視場(chǎng)增強(qiáng)算法提高了相機(jī)視場(chǎng)對(duì)糧箱儲(chǔ)藏空間的覆蓋率。

(2)對(duì)糧倉(cāng)裝載狀態(tài)進(jìn)行三維點(diǎn)云模型重建，并依據(jù)堆體物理模型和相機(jī)遮擋模型對(duì)盲區(qū)進(jìn)行點(diǎn)云填充，盲區(qū)點(diǎn)云估計(jì)的平均誤差低于5 cm，該方法對(duì)糧倉(cāng)裝載狀態(tài)還原度較高。與常規(guī)的糧倉(cāng)裝載狀態(tài)方法相比，能提供更加全面的裝載狀態(tài)信息。

(3)基于當(dāng)前的裝載狀態(tài)，提出了以均勻裝載為目標(biāo)的糧箱裝載方法。該方法可以用于聯(lián)合收獲機(jī)-運(yùn)輸車協(xié)同作業(yè)的自動(dòng)控制，為不間斷卸料裝載過(guò)程提供裝載位置信息，能提高糧箱裝載率與協(xié)同作業(yè)的效率，同時(shí)提高作業(yè)過(guò)程中車輛行駛的穩(wěn)定性，降低由于裝載不均勻?qū)\(yùn)輸車可能造成的機(jī)械損耗，以及由于顛簸發(fā)生溢料問(wèn)題的可能性。

(4)在最高裝載高度為0.35 m時(shí)，動(dòng)態(tài)均勻裝載方法的裝載量高于傳統(tǒng)固定點(diǎn)裝載方法的2倍。該方法不僅提高裝載結(jié)果的均勻性，還提高了裝載過(guò)程的均勻性，降低了裝載物相互遮擋的可能，從而提高裝載過(guò)程中裝載狀態(tài)檢測(cè)的精度。