魏寶源，姚壯潤，張喜杰，梁俊軒，王堅輝，薛秀云

（華南農(nóng)業(yè)大學電子工程學院、人工智能學院，廣州510642）

0 引言

目前，源于精準農(nóng)業(yè)的變量噴霧技術迅速發(fā)展，該技術是基于通過測量傳感器，實時地獲取作物的病蟲害、形貌和密度等噴霧對象信息，以及噴霧機位置、速度和噴霧壓力等機器狀態(tài)信息，對噴霧對象按需施藥[1-3]。果樹的特征參數(shù)的精準檢測是實現(xiàn)變量噴霧的前提條件[4]，對果樹特征參數(shù)的檢測方法常用的有紅外線探測、超聲波探測和激光探測[1]。其中激光探測相對于紅外線探測和超聲波探測[5-7]具有速度快，精度高，量程大，抗光、電干擾能力強等優(yōu)點[8]。激光傳感器可以測量靶標外形尺寸等特征信息且可對靶標進行體積測量及三維重構[9-10]。

其中車載激光掃描傳感器的應用，在一定程度上獲取準確的果樹特征參數(shù)后，改善了變量噴霧的效果。但是激光掃描傳感器對地形的要求較高，實際噴霧場景會造成車輛的顛簸，激光傳感器檢測目標物特征信息容易出現(xiàn)偏差，進而影響噴霧效果[11-12]。所以對點云數(shù)據(jù)進行位姿矯正，有助于變量噴霧精準化[13]。其中對于激光雷達位姿的姿態(tài)角檢測和解算的精確化影響著矯正效果，一般使用MEMS（Micro-Electro-Me?chanical System，微機電系統(tǒng)）慣性測量單元檢測載體的位姿，其中姿態(tài)傳感器輸出數(shù)據(jù)的融合濾波算法時獲取精確姿態(tài)角的關鍵環(huán)節(jié)[14]。

本文主要研究的是在地面崎嶇的非理想情況下，基于激光雷達和慣性測量輔助測量單元對變量噴霧對象的特征參數(shù)進行精確測量，融合了激光檢測技術、姿態(tài)檢測和解算技術，以此達到良好的矯正效果。

1 試驗平臺與方法

1.1 試驗平臺

試驗平臺整體結(jié)構由勻速滑臺運動導軌、檢測采集數(shù)據(jù)傳感器單元兩部分組成。該平臺如圖1 所示。

勻速滑臺運動導軌裝置主要由鋁制水平、垂直滑臺同步導軌，型號為臺達ASDA-B2D 的伺服電機，伺服電機驅(qū)動器電控箱組成；伺服電機配有高精度的編碼器，可以通過設置調(diào)節(jié)控制伺服電機的參數(shù)，就可以實現(xiàn)滑臺的精準速度的控制，速度控制可以達到0.01m/s 以上，可在水平方向上最大距離為3950mm 和垂直高度940mm～2440mm 進行位移。檢測采集數(shù)據(jù)傳感器單元架設在滑臺裝置的水平導軌上，主要由SICK 公司型號 TIM561 激光雷達傳感器、STM32F103ZET6 控制芯片及ATK-MPU6050 六軸姿態(tài)傳感器組成；激光雷達傳感器與計算機通過Ethernet接口使用TCP/IP（Transmission Control Protocol/Internet Protocol，傳輸控制協(xié)議/網(wǎng)際協(xié)議）進行通信。所用的SICK TIM561 激光雷達傳感器角度分辨率0.33°，具有10m、270°掃描范圍，掃描頻率為15Hz，抗環(huán)境光強度80000lx，供電電壓為DC9V-28V。MPU6050 姿態(tài)傳感器 通過I2C（Inter-Integrated Circuit，內(nèi)部總線）與STM32 F103ZET6 進行數(shù)字通信，STM32 F103ZET6 通過UART 串口與便攜式計算機進行通信。上位機軟件使用C#在.NET 框架和C++在Qt 框架上聯(lián)合開發(fā)，實現(xiàn)計算機與激光雷達傳感器和IMU 姿態(tài)傳感器進行通信，控制其工作。

圖1 試驗平臺裝置圖

其中，滑臺上搭載著可調(diào)固定姿態(tài)角平臺，如圖2所示，該平臺分為兩部分：支架底座和支架側(cè)座，兩者通過螺絲連接，并可松動調(diào)整至特定姿態(tài)角；可實現(xiàn)俯仰角±10°、橫滾角±10°、±20°和偏航角±10°、±20°的姿態(tài)角組合變化，考慮到現(xiàn)實情況各姿態(tài)角的變化幅度不大，所以該平臺基本滿足姿態(tài)角變化需求。

圖2 姿態(tài)角平臺實物示意圖

1.2 試驗方案

實驗檢測對象是正方體，該正方體邊長為11.4cm，實驗時雷達到物體中心的垂直距離為2m，這與噴霧的實際距離貼近，正方體將放在高度50cm 的方凳上，導軌平臺速度可選擇控制在0.33m/s，雷達高度為1.1m。正方體如圖3 所示。

圖3 檢測對象實物圖

LiDAR 測量系統(tǒng)實驗一共分為三步。第一步，實驗對象為單個正方體，分別調(diào)節(jié)偏航角±10°、橫滾角±10°、俯仰角±10°進行實驗，驗證單姿態(tài)角的矯正公式；第二步，實驗對象為單個正方體，考慮到現(xiàn)實情況多姿態(tài)角是融合的且幅度不會太大，調(diào)節(jié)偏航角10°、橫滾角10°、俯仰角10°進行試驗，第三步，對象為多個正方體的組合，調(diào)節(jié)偏航角10°、橫滾角10°、俯仰角10°進行試驗；以上的所有實驗皆重復三次，提高實驗數(shù)據(jù)的可靠性。

實驗獲得的數(shù)據(jù)通過MATLAB 離線處理，獲得的姿態(tài)角數(shù)據(jù)通過互補濾波和卡爾曼濾波相結(jié)合的濾波方式得到比較準確地姿態(tài)角信息，LiDAR 獲得的點云數(shù)據(jù)需要先轉(zhuǎn)換成卡爾坐標系保存起來，再與姿態(tài)角信息進行匹配矯正，矯正后的點云數(shù)據(jù)再進行環(huán)境點云數(shù)據(jù)的去除，再進行特征參數(shù)的測量。

2 理論基礎

2.1 激光雷達檢測方法

試驗所選用檢測目標物的傳感器為SICK 公司生產(chǎn)的型號為TIM561 二維激光雷達掃描傳感器，掃描最遠距離為10m，角度分辨率為0.33°，掃描角度范圍為270°，則掃描一幀可獲得811 個數(shù)據(jù)形式為極坐標數(shù)值的掃描點數(shù)據(jù)。在無姿態(tài)角變化的情況下，激光雷達平躺架設在滑臺導軌上，其架設示意圖如圖4 所示，90°的掃描盲區(qū)朝向地面。

圖4 激光雷達架設示意圖

LiDAR 掃描所得的極坐標值點云數(shù)據(jù)，需要轉(zhuǎn)換成笛卡爾坐標系數(shù)值形式（x，y，z），以方便后續(xù)處理。以LiDAR 中心為原點，水平導軌正向運動為x 軸正向，以右手螺旋定則建立笛卡爾坐標系，如圖5 所示。

圖5 點云數(shù)據(jù)極坐標映射到笛卡爾坐標示意圖

試驗中控制水平導軌特定速度勻速運動，通過Li?DAR 的固有掃描頻率和掃描點云數(shù)據(jù)的幀數(shù)可知一次掃描目標的時間，則可利用式（1）計算笛卡爾坐標系下的點云數(shù)據(jù)x 坐標值。

式中f 為LiDAR 的掃描頻率，單位為Hz；v 為Li?DAR 在水平導軌運動的速度，單位為m/s；n 為掃描點云數(shù)據(jù)組的幀數(shù)序號。

利用式（2）計算y、z 坐標值。

式中ρ為LiDAR 掃描原始點云數(shù)據(jù)的距離長度，單位為mm；θ為LiDAR 掃描原始點云數(shù)據(jù)的各數(shù)據(jù)點對應角度，單位為°。

2.2 姿態(tài)角檢測及解算方法

LiDAR 位姿輔助測量單元框圖如圖6 所示，由STM32 主控板控制外圍設備MPU6050 姿態(tài)傳感器組成，使用USB 進行供電。慣性測量單元參照激光雷達坐標系緊密裝備在激光雷達上，這樣可避免在后續(xù)數(shù)據(jù)處理中帶來的坐標系轉(zhuǎn)換統(tǒng)一的冗余計算。

圖6 位姿輔助測量單元框圖

MPU6050 的陀螺儀在使用過程中，在穩(wěn)態(tài)狀態(tài)下，隨著誤差累積，輸出姿態(tài)角很快偏移實際姿態(tài)角，從而導致測量失效；三軸加速度計在對象運動過程中會受到附加加速度干擾；三軸磁強計在檢測地磁場的過程中易受到周邊環(huán)境干擾從而無法輸出姿態(tài)測量結(jié)果，所以為了獲取精確的姿態(tài)角數(shù)據(jù)需要結(jié)合MEMS 傳感器的各自優(yōu)勢，對輸出數(shù)據(jù)進行融合濾波，從而給出較精確的姿態(tài)角，且濾波算法在保證輸出精度的條件下，還要符合低功耗微處理器實時計算能力的要求。因此，傳感器輸出數(shù)據(jù)的濾波融合算法是獲取姿態(tài)角的關鍵環(huán)節(jié)，本文根據(jù)黃鎮(zhèn)等人[15]提出的一種二階互補濾波與卡爾曼濾波混合的方法解算姿態(tài)，提高位姿檢測的準確性和實時性，實現(xiàn)該融合算法的設計框圖如圖7所示。

圖7 互補濾波與卡爾曼濾波混合算法設計框圖

2.3 點云數(shù)據(jù)與姿態(tài)角數(shù)據(jù)的矯正匹配

由于激光雷達傳感器和慣性測量單元相互獨立，激光雷達檢測得到的點云數(shù)據(jù)和慣性測量單元檢測得到的位姿數(shù)據(jù)并不匹配，本文參考張迪[16]提出利用統(tǒng)一時鐘源實現(xiàn)激光掃描系統(tǒng)的時間同步系統(tǒng)設計。實現(xiàn)該方法的框圖如圖8 所示，使用STM32 MCU 定時器資源產(chǎn)生統(tǒng)一時鐘源，并設定激光雷達和慣性測量單元在同一頻率下進行掃描，實現(xiàn)了點云數(shù)據(jù)與姿態(tài)角的匹配。

圖8 激光雷達和慣性測量單元檢測時間同步實現(xiàn)框圖

由于位姿變化，將會影響到激光雷達所檢測的點云數(shù)據(jù)，所以必須對產(chǎn)生位姿變化的點云數(shù)據(jù)進行矯正，本文根據(jù)劉慧等人[12]提出的融合翻滾角的極坐標值與三角函數(shù)重新匹配法、分段融合俯仰角平均值的檢測幀與檢測點重新組合法和融合偏航角平均值的深度值系數(shù)矯正法，對位姿影響的點云數(shù)據(jù)進行矯正，對該矯正算法數(shù)學建模可由式（3）表征和具體實現(xiàn)。

式中ρβ(i→j)原幀第i 點距離經(jīng)俯仰角（β）矯正映射到矯正后第j 點數(shù)據(jù)，α為偏航角，γ為橫滾角。

3 試驗結(jié)果分析與討論

由1.2 小節(jié)所述實驗方法進行目標物激光掃描檢測，使用MATLAB 按照2.3 小節(jié)所示理論方法對點云數(shù)據(jù)進行姿態(tài)矯正，具有單一角度姿態(tài)偏移的點云數(shù)據(jù)和矯正后的點云數(shù)據(jù)重構圖如圖9 所示。

具有復合角度姿態(tài)偏移的點云數(shù)據(jù)和矯正后的點云數(shù)據(jù)重構圖如圖10 所示。

圖9 反映出當偏航角存在時，檢測目標物在橫向上會被傾斜壓縮，當橫滾角存在時，檢測目標物在縱向上會被傾斜壓縮，當俯仰角存在時，檢測目標物在橫縱向均會被傾斜壓縮；圖10 反映出當姿態(tài)偏移復合存在時，檢測目標物在橫縱向深度均受到影響；通過矯正算法，目標檢測物在重構上得到較大改善。

圖9 單一姿態(tài)偏移10°目標物矯正前后點云圖

圖10 復合角度（10°，10°，10°）姿態(tài)偏移目標物矯正前后點云圖

4 結(jié)語

在使用激光雷達獲取檢測目標物的過程中，由于路面崎嶇的非理想狀態(tài)導致點云數(shù)據(jù)出現(xiàn)姿態(tài)偏移，為了克服姿態(tài)角偏差帶來的影響，使用姿態(tài)角矯正點云數(shù)據(jù)的方法，融合互補濾波和卡爾曼濾波的方式解算姿態(tài)角，提高姿態(tài)角的精度，通過矯正算法獲取精確的位姿姿態(tài)角數(shù)據(jù)，并重構三維圖像。試驗證明，使用姿態(tài)角矯正后的點云數(shù)據(jù)重構的三維模型得到了較好的改善。