基于IMU 和融合濾波的LiDAR 點云數據矯正

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

（華南農業大學電子工程學院、人工智能學院，廣州510642）

0 引言

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

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

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

1 試驗平臺與方法

1.1 試驗平臺

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

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

圖1 試驗平臺裝置圖

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

圖2 姿態角平臺實物示意圖

1.2 試驗方案

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

圖3 檢測對象實物圖

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

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

2 理論基礎

2.1 激光雷達檢測方法

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

圖4 激光雷達架設示意圖

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

圖5 點云數據極坐標映射到笛卡爾坐標示意圖

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

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

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

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

2.2 姿態角檢測及解算方法

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

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

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

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

2.3 點云數據與姿態角數據的矯正匹配

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

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

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

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

3 試驗結果分析與討論

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

具有復合角度姿態偏移的點云數據和矯正后的點云數據重構圖如圖10 所示。

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

圖9 單一姿態偏移10°目標物矯正前后點云圖

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

4 結語

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