吳敏，郇曉龍，陳建能*，董春旺，邵柏愷，卞賢炳，范國帥

1.浙江理工大學機械工程學院，浙江 杭州 310018；2.浙江工業(yè)職業(yè)技術學院，浙江 紹興 312000；3.浙江省種植裝備技術重點實驗室，浙江 杭州 310018；4.中國農業(yè)科學院茶葉研究所，浙江 杭州 310008

我國是茶葉生產和消費大國，目前茶園種植面積和茶葉產量均居世界第一位。鮮葉采摘是茶葉生產的重要環(huán)節(jié)，目前商業(yè)化的大宗茶采茶機主要有單人式、雙人式和自走式[1-3]。單人式和雙人式采茶機需要操作人員觀察茶樹葉片生長狀態(tài)，人工調整采茶機切割位置，操作強度高、效率低[4]；自走式采茶機作業(yè)時，割刀離地高度固定，無法適應茶樹葉面高度和茶園壟溝起伏變化，收獲質量難以控制，成為制約大宗茶機械化收獲裝備在丘陵山區(qū)茶園推廣應用的主要瓶頸。針對商業(yè)化大宗茶采茶機存在的問題，一些學者開展了相關研究。閆晶晶[5]設計了一款手扶式仿形采茶機，采用接觸式傳感器感知茶蓬面高度變化，但是容易造成茶樹損傷，測量結果受接觸力影響，數據變異大，茶鮮葉收獲質量難以控制。湯一平等[6]基于RGB相機初步實現了采茶機自動仿形功能，由于受自然光照影響，自然環(huán)境下圖像閾值分割準確性低，影響了采茶機割刀仿形精度。趙潤茂等[7]設計了基于超聲波測距的乘坐式自動仿形采茶機，經過田間試驗驗證了其仿形采收的有效性，然而超聲波雙點測距方式受茶蓬面葉片間隙影響，超聲波信號穿過葉片間隙到達鮮葉層下方時，茶蓬面測距信息失真，容易導致收獲鮮葉雜質率偏高，鮮葉完整率偏低，仿形采收效果不佳。由此可知，目前大宗茶自動仿形采茶機存在著易受接觸力、自然光照或茶蓬面葉片間隙影響，需要進一步提升采茶機性能以改善大宗茶鮮葉采摘質量。

激光雷達（Light detection and ranging，LiDAR）傳感器被廣泛應用于農業(yè)工程中，如靶標葉面積估算[8]、玉米[9]和甜菜[10]株高測量、果樹樹冠信息提取[11]等，是一種非接觸式測距傳感器，具有提取信息范圍大、抗自然光性強的特點。本研究針對目前大宗茶自動仿形采茶機存在的易受接觸力、自然光照或茶蓬面葉片間隙影響等問題，采用室外2D-LiDAR獲取茶蓬面點云信息，并通過幾何變換關系獲取茶蓬面點云感興趣區(qū)域（Range of interest，ROI）。而室外LiDAR為了抵抗自然光照影響，采用飛行時間法測量原理（Time of Flight，ToF）[12]獲取目標點云信息，其測距精度難以滿足采茶機仿形作業(yè)要求，為此提出了融合2D-LiDAR測距信息與航向姿態(tài)參考系統(tǒng)（Attitude and heading reference system，AHRS）感知加速度信息（Fusion of 2D-LiDAR ranging and acceleration，FLRA）的采茶機割刀仿形距離估計算法。在此基礎上，設計算法精度驗證機構及方法，通過室內試驗檢驗算法的有效性；并研制了融合2D-LiDAR與AHRS感知的自動仿形采茶樣機，開展田間試驗，驗證茶鮮葉仿形采收效果，以期提升大宗茶鮮葉采收效率與質量。

1 材料與方法

1.1 試驗場地與設備

本研究田間試驗地點為浙江省紹興市中國農業(yè)科學院茶葉研究所嵊州綜合實驗基地，茶樹蓬面較平整，鮮葉較整齊，茶蓬面高度為80～90 cm，蓬面寬度約100 cm；我國茶園大多屬于密植茶園，有些地區(qū)位于丘陵地帶，茶園中各隴茶樹間地勢起伏[13]，試驗樣機采用高低間隙履帶式底盤作為載具，提升采茶機對丘陵地帶等茶園的行走能力；仿形采收末端單元采用文獻[7]中仿形采茶單元結構，如圖1所示，前中后仿形采收單元交替布置，減小茶鮮葉漏采率。采摘試驗于5—6月適采期進行。

圖1 采茶機仿形采收末端Fig.1 Profiling end of tea picker

本研究室外2D-LiDAR采用上海思嵐科技有限公司的PRLIDAR S2型，采樣角度分辨率為0.12°，采樣周期為0.1 s，測距精度為±3 cm，有效測量距離0.05～10.00 m；慣性傳感器采用荷蘭XSENS公司生產的MTi-300型航向姿態(tài)參考系統(tǒng)，其中角度測量精度為0.05°，采樣頻率設置為50 Hz；計算單元采用NVIDIA Jetson AGX套件，安裝有Linux系統(tǒng)環(huán)境及機器人ROS操作系統(tǒng)；拉繩位移傳感器采用臺州量子電子科技有限公司生產的WXY31型，測量范圍為0～500 mm，測量精度為0.1 mm。

1.2 采茶機自動仿形原理

如圖2所示，2D-LiDAR傳感器安裝于仿形采茶單元割刀機構中部位置（圖2紅色原點處），與割刀同步運動，茶蓬面位于2D-LiDAR傳感器下方，由于2D-LiDAR采集數據角度范圍為360°，因此需提取茶蓬面ROI點云，然后進一步處理點云信息。為獲取割刀兩側與茶蓬面的距離，以割刀兩側端點A、B豎直方向下方C、D點附近為茶蓬面ROI點云；在2D-LiDAR極坐標系中，假設C、D點所對應極角分別為β1、β2，則對應ROI為（β1-Δβ，β1＋Δβ）與（β2-Δβ，β2＋Δβ）極角范圍的茶蓬面點云，其中2Δβ表示點云ROI范圍大小。

圖2 茶蓬面點云ROI示意圖Fig.2 The point cloud ROI of tea canopy surface

如圖3A所示，提取茶蓬面ROI點云信息后，首先根據ROI坐標信息計算采茶機割刀仿形距離的測量值（圖3A中AC和BD的距離）；然后通過AHRS感知割刀豎直方向加速度，建立割刀運動學方程，最優(yōu)估計采茶機割刀仿形距離；最后根據仿形距離信息控制電機實現采茶機割刀仿形運動，直至如圖3B所示完成仿形動作，并在此基礎上，控制割刀向下運動切割深度 Δl開始采收茶鮮葉，切割深度Δl根據待采收茶鮮葉長勢設定。

圖3 采茶機割刀仿形原理圖Fig.3 Schematic diagram of cutter profiling of tea picker

1.3 茶蓬面2D-LiDAR點云預處理

1.3.1 ROI

2D-LiDAR的原始傳感器坐標系為極坐標系，為了便于計算，將2D-LiDAR的傳感器極坐標系轉化為以 2D-LiDAR中心點O為原點，y軸指向極角0°的直角坐標系，如圖4所示。假設 2D-LiDAR極坐標系任意點坐標為（ρ，θ），則其在直角坐標系 xoy中坐標為（ρsinθ，ρcosθ）。

圖4 2D-LiDAR坐標系Fig.4 Coordinate system of 2D-LiDAR

如圖5所示，以2D-LiDAR所在xoy坐標系為動坐標系，豎直方向坐標系x'oy'為靜坐標系。坐標系xoy與x'oy'相差割刀水平傾角α，假設割刀寬度AB=l，割刀兩側端點 A、B豎直方向交茶蓬面于點C、D；2D-LiDAR傳感器垂直交于割刀面于點E，假設OE=h。通過結構幾何關系獲取茶蓬面ROI點云坐標信息，以割刀左側端點為例，具體步驟如下：

圖5 仿形單元與茶蓬面ROI示意圖Fig.5 Schematic diagram of profiling unit and ROI of tea canopy surface

假設在極坐標系下2D-LiDAR傳感器采集的任意點云極坐標為（ρ，θ），轉化為x'oy'坐標系坐標為：

根據仿形單元結構幾何關系知，在xoy坐標系中，A點坐標為（-l/2，h），根據坐標轉換關系可知x'oy'坐標系中A點坐標為：

然后遍歷2D-LiDAR點云數據，當δ=δmin時，即認為確定點C點云坐標；同理可確認點D點云坐標，從而確定2D-LiDAR采集數據的茶蓬面ROI點云范圍。

1.3.2 基于FLRA算法的采茶機割刀仿形距離估計

提取割刀下方茶蓬面ROI點云后，首先計算采茶機割刀仿形距離測量值，將極坐標轉換到x'oy'坐標系，假設圖5中C點附近極角范圍為2Δβ的ROI點云坐標分別為（x'1，y'1）、（x'2，y'2）、（x'3，y'3）、……、（x'N，y'N），則ROI點云取平均后y'軸坐標為：

式中，N為茶蓬面ROI點云數量。A點坐標位置為：

式中，l為割刀長度，mm；α為AHRS感知的割刀水平傾角，rad；h為2D-LiDAR中心與割刀面垂直方向距離，mm。由此可知，采茶機割刀左側仿形距離測量值Δh1為：

同理可計算采茶機割刀右側仿形距離測量值Δh2，mm。

由于該2D-LiDAR測距精度與實時性難以滿足自動仿形采茶機采收要求，需進一步提升采茶機割刀仿形距離測距精度與實時性。采茶機仿形作業(yè)過程中，割刀、2D-LiDAR及AHRS同步運動，通過AHRS獲取割刀豎直方向加速度信息。本研究中2D-LiDAR傳感器采集點云數據頻率為10 Hz，仿形作業(yè)過程中采茶機行駛速度約0.5 m·s-1，在人工修剪的茶園中，相鄰采樣時間間隔內茶蓬面高度幾乎不變，且AHRS采樣頻率數倍于2D-LiDAR傳感器。根據卡爾曼濾波融合算法原理[14]，融合割刀豎直方向加速度數據與2D-LiDAR測量數據可實現割刀仿形距離最優(yōu)估計。圖 6所示為卡爾曼濾波融合算法遞推示意圖，在2D-LiDAR采樣間隔內，利用割刀豎直方向加速度信息實現多次狀態(tài)遞推最優(yōu)估計采茶機割刀仿形距離。

圖6 卡爾曼濾波融合遞推示意圖Fig.6 Schematic diagram of Kalman filter fusion recursion

根據豎直方向加速度信息建立割刀豎直方向運動學方程為：

式中，T0為AHRS采樣時間間隔，s；xk為k時刻運動學模型估計的仿形距離，mm；為模型估計的仿形距離變化率，mm·s；ak為k時刻割刀豎直方向加速度，mm·s-2。

以Xk=為系統(tǒng)狀態(tài)向量，建立如公式（8）所示割刀豎直方向運動學狀態(tài)空間方程，采用離散狀態(tài)空間表示為：

式中，系統(tǒng)狀態(tài)變量為Xk=為狀態(tài)空間模型過程噪聲；Yk為輸出狀態(tài)變量；控制量uk為k時刻割刀豎直方向加速度；系統(tǒng)矩陣，輸入矩陣B=輸出矩陣C=[1 0]，直接轉移矩陣D=0。

式中，矩陣H=[1 0]為測量矩陣；vk為測量噪聲，mm。

卡爾曼濾波融合2D-LiDAR測量數據與AHRS加速度數據的具體過程如下：

式中，矩陣A為系統(tǒng)矩陣，AT為A的轉置；Q為過程噪聲協方差矩陣。

（3）計算k時刻卡爾曼增益：

早在2008 年西門子公司因違反《反海外腐敗法》被處罰金16 億美元，成為全球企業(yè)合規(guī)發(fā)展的一個里程碑事件。從那以后，強化合規(guī)管理、防范合規(guī)風險已經成為越來越多企業(yè)管理創(chuàng)新的重要內容。統(tǒng)計數據顯示，自2014 年初以來，已公開宣布115 名國有集團公司的決策層高管因貪污而受到調查，一些央企的主要負責人被調查或處罰。

式中，H為觀測矩陣，HT是H的轉置；R是2D-LiDAR測量噪聲協方差矩陣。

（4）計算k采樣時刻系統(tǒng)狀態(tài)的后驗估計：

（5）更新誤差協方差矩陣：

1.4 FLRA算法估計仿形距離精度驗證裝置及方法

采茶機田間作業(yè)時，割刀與茶蓬面的仿形距離真值無法測量，FLRA算法精度無法評估。因此，本研究設計了精度驗證裝置并開展相關室內試驗。圖7所示為FLRA算法精度驗證裝置，裝置上方安裝仿形收獲單元，仿形收獲單元兩側通過搖臂與絲桿滑塊連接，絲桿滑塊在豎直方向運動時改變割刀水平傾角與高度。裝置下方安裝模擬茶蓬面，模擬茶蓬面兩側同樣通過搖臂與絲桿滑塊連接，絲桿滑塊的運動可改變模擬茶蓬面水平傾角與高度，從而模擬實際仿形作業(yè)時割刀與茶蓬面位姿狀態(tài)。仿形收獲單元上安裝AHRS與2D-LiDAR傳感器，模擬茶蓬面底部安裝AHRS；仿形單元與模擬茶蓬面通過拉繩傳感器連接，拉繩傳感器一端可固定于仿形單元任意位置，另一端通過滑輪安裝在模擬茶蓬面水平槽中，在拉繩傳感器回復力作用下，拉繩傳感器拉繩始終保持與模擬茶蓬面垂直。

在實際茶園中，采茶機受地形的隨機起伏影響，割刀仿形距離及豎直方向加速度表現為隨機變化過程。為了室內試驗能夠模擬割刀運動過程，提高室內試驗結果的可靠性，在調整并固定模擬茶蓬面后，通過試驗裝置兩側的兩名操作人員推拉割刀所連接的絲桿滑塊，帶動割刀上下運動，模擬仿形距離變化過程。由于兩名操作人員各自獨立推拉絲桿滑塊運動，割刀兩側運動不同步且推拉作用力隨機變化，因此割刀仿形距離及豎直方向加速度呈現隨機變化。

如圖7A所示，假設拉繩傳感器測量距離MN為lMN，由幾何關系可知M點豎直方向與模擬茶蓬面距離MF為：

圖7 算法精度驗證裝置示意圖Fig.7 Algorithm accuracy verification device

式中，β為AHRS測量的模擬茶蓬面水平傾角，rad。為了測量真實割刀仿形距離，將拉繩傳感器安裝于割刀端點A、B處，假設拉繩傳感器測量值分別為lAC、lBD，則割刀兩側仿形距離分別為AC=lAC/cosβ、BD=lBD/cosβ，由于拉繩傳感器與傾角測量誤差很小，以此作為仿形距離真值。然后將2D-LiDAR直接測量的割刀仿形距離、FLRA算法最優(yōu)估計的割刀仿形距離分別與割刀仿形距離真值比較，結合誤差特征分析FLRA算法提升割刀仿形距離測距精度與實時性的有效性。

圖8 算法精度驗證裝置實物及硬件組成Fig.8 Physical object of algorithm accuracy verification device and hardware composition

1.5 田間試驗方法

圖9為融合2D-LiDAR與AHRS的采茶樣機及其系統(tǒng)硬件連接圖。作業(yè)過程中，采用FLRA算法估計采茶機割刀仿形距離，并采用非線性微分跟蹤器（Nonlinear tracking differentiator，NTD）的PID控制律控制割刀仿形運動[7,15-16]，開展自動仿形采茶樣機田間試驗，驗證自動仿形采茶樣機采收效果。駕駛采茶機在茶園內沿茶壟方向以0.5 m·s-1左右速度行駛約50 m距離，若干組試驗后，統(tǒng)計收獲鮮葉總質量，利用對角線四分法獲取鮮葉500 g，進行鮮葉分離處理，然后統(tǒng)計分析一芽三葉及以下鮮葉比例、雜質率和完整率等參數。

圖9 多傳感器感知自動仿形采茶機及硬件組成Fig.9 Automatic profiling tea picker with multi-sensor perception and hardware composition

2 結果與討論

2.1 算法精度驗證室內試驗

圖 10為2D-LiDAR直接測量值、FLRA算法估計值及拉繩傳感器測量真值的輸出曲線，其中數據采樣頻率為50 Hz，共采樣801個點。為了獲取更高角度分辨率，2D-LiDAR采用10 Hz采樣頻率，此時測量曲線與真值曲線之間存在較大延遲，數據平滑度較差；FLRA算法處理數據頻率為50 Hz，算法估計數據頻率更高，因此在2D-LiDAR采樣間隔時間內，可通過算法實時預測割刀仿形距離，輸出估計數據延遲更小；輸出曲線各周期的峰值、斜率等參數各不相同，模擬了實際茶園仿形采收時仿形距離、豎直方向加速度的隨機變化過程。對所有采樣數據進行統(tǒng)計分析，2D-LiDAR測量值與真值之間的誤差均值為36.53 mm，誤差標準差為23.21 mm，其誤差均值超過2D-LiDAR標稱誤差精度，主要原因是2D-LiDAR采樣頻率遠低于真值采樣頻率，難以實時反應割刀運動所引起的仿形距離變化；FLRA算法估計值與真值之間誤差均值為8.56 mm，誤差標準差為6.31 mm，提升了仿形距離的測距精度。綜上可知，FLRA算法不僅有效提升了采茶機割刀仿形距離的測距精度，而且提高了獲取仿形距離的實時性。

圖10 FLRA算法精度驗證試驗輸出曲線Fig.10 Output curve of FLRA algorithm accuracy verification test

2.2 田間試驗

如圖11A所示，按照1.5章節(jié)所述方法開展采茶機田間試驗，經過5組試驗后，共收獲鮮葉約 30 kg，粗略計算采收效率為180～210 kg·h-1，仿形采收過程中，自動仿形采茶樣機工作正常，茶蓬面切割面整齊，切口平整。自動仿形采收試驗完成后，利用對角線四分法對采收的全部鮮葉縮分[17]，取如圖11B所示500 g采收鮮葉樣本進行統(tǒng)計分析。

圖11 采茶樣機作業(yè)場景及采收樣本Fig.11 Operational scene of tea picker and picking samples

表1為文獻[7]中超聲波傳感器的仿形采茶原型機與本研究融合2D-LiDAR與AHRS感知的自動仿形采茶機田間試驗的對比數據。田間試驗結果表明，在本研究茶園試驗條件下，鮮葉采收結果良好，系統(tǒng)數據傳輸準確可靠，融合2D-LiDAR與AHRS的自動仿形采茶樣機受茶蓬面葉片間隙影響小，采收鮮葉在完整率、雜質率（收獲的鮮葉中老梗、老葉等雜質的質量比）和一芽三葉及以下嫩梢比例等方面優(yōu)于超聲波傳感器感知采茶機，采收效果符合采茶機作業(yè)的質量標準和大宗綠茶的制作要求；由于采茶機仿形單元結構不變，鮮葉采收率變化較小。

表1 不同傳感器的仿形采茶機田間試驗統(tǒng)計數據Table 1 Statistical data of field test of profiling tea picker with different sensors

3 結論

為了提高大宗茶鮮葉自動仿形采收的收獲效率與質量，研制了融合室外2D-LiDAR與AHRS的自動仿形采茶樣機，提出了基于FLRA算法的采茶機割刀仿形距離估計方法，提升了仿形距離測距的精度與實時性。室內試驗結果表明，2D-LiDAR采樣頻率為10 Hz時，輸出的測量數據存在較大延遲，經過FLRA算法迭代遞推，仿形距離數據更新頻率提升至50 Hz，數據延遲很小，能夠滿足采茶機實時性要求；此外，提出的FLRA算法比直接采用2D-LiDAR的測距精度高，仿形距離估計誤差為8.56 mm。在茶園試驗條件接近的情況下，仿形距離估計精度越高，越有利于割刀準確定位切割采收，提升茶鮮葉的采收質量。田間試驗結果表明，在本研究茶園試驗條件下，研制的融合2D-LiDAR與AHRS的自動仿形采茶機工作性能良好，通過對試驗樣本數據統(tǒng)計分析，鮮葉采收效率達180～210 kg·h-1，割刀覆蓋蓬面上鮮葉的平均采收率92.38%，平均芽葉完整率為85.34%，平均雜質率為4.93%，一芽三葉及以下嫩梢占90.72%，符合采茶機作業(yè)的質量標準和大宗綠茶的制作要求，與超聲波傳感器的自動仿形采茶機相比，提升了鮮葉完整率，降低了鮮葉雜質率。未來將持續(xù)改進采茶機仿形單元結構，實現采茶機割刀對茶蓬面的全覆蓋，以期進一步提升茶鮮葉的采收率。