基于IMU 和融合濾波的LiDAR 點(diǎn)云數(shù)據(jù)矯正

魏寶源，姚壯潤(rùn)，張喜杰，梁俊軒，王堅(jiān)輝，薛秀云

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

0 引言

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

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

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

1 試驗(yàn)平臺(tái)與方法

1.1 試驗(yàn)平臺(tái)

試驗(yàn)平臺(tái)整體結(jié)構(gòu)由勻速滑臺(tái)運(yùn)動(dòng)導(dǎo)軌、檢測(cè)采集數(shù)據(jù)傳感器單元兩部分組成。該平臺(tái)如圖1 所示。

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

圖1 試驗(yàn)平臺(tái)裝置圖

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

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

1.2 試驗(yàn)方案

實(shí)驗(yàn)檢測(cè)對(duì)象是正方體，該正方體邊長(zhǎng)為11.4cm，實(shí)驗(yàn)時(shí)雷達(dá)到物體中心的垂直距離為2m，這與噴霧的實(shí)際距離貼近，正方體將放在高度50cm 的方凳上，導(dǎo)軌平臺(tái)速度可選擇控制在0.33m/s，雷達(dá)高度為1.1m。正方體如圖3 所示。

圖3 檢測(cè)對(duì)象實(shí)物圖

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

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

2 理論基礎(chǔ)

2.1 激光雷達(dá)檢測(cè)方法

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

圖4 激光雷達(dá)架設(shè)示意圖

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

圖5 點(diǎn)云數(shù)據(jù)極坐標(biāo)映射到笛卡爾坐標(biāo)示意圖

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

式中f 為L(zhǎng)iDAR 的掃描頻率，單位為Hz；v 為L(zhǎng)i?DAR 在水平導(dǎo)軌運(yùn)動(dòng)的速度，單位為m/s；n 為掃描點(diǎn)云數(shù)據(jù)組的幀數(shù)序號(hào)。

利用式（2）計(jì)算y、z 坐標(biāo)值。

式中ρ為L(zhǎng)iDAR 掃描原始點(diǎn)云數(shù)據(jù)的距離長(zhǎng)度，單位為mm；θ為L(zhǎng)iDAR 掃描原始點(diǎn)云數(shù)據(jù)的各數(shù)據(jù)點(diǎn)對(duì)應(yīng)角度，單位為°。

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

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

圖6 位姿輔助測(cè)量單元框圖

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

圖7 互補(bǔ)濾波與卡爾曼濾波混合算法設(shè)計(jì)框圖

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

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

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

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

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

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

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

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

圖9 反映出當(dāng)偏航角存在時(shí)，檢測(cè)目標(biāo)物在橫向上會(huì)被傾斜壓縮，當(dāng)橫滾角存在時(shí)，檢測(cè)目標(biāo)物在縱向上會(huì)被傾斜壓縮，當(dāng)俯仰角存在時(shí)，檢測(cè)目標(biāo)物在橫縱向均會(huì)被傾斜壓縮；圖10 反映出當(dāng)姿態(tài)偏移復(fù)合存在時(shí)，檢測(cè)目標(biāo)物在橫縱向深度均受到影響；通過(guò)矯正算法，目標(biāo)檢測(cè)物在重構(gòu)上得到較大改善。

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

圖10 復(fù)合角度（10°，10°，10°）姿態(tài)偏移目標(biāo)物矯正前后點(diǎn)云圖

4 結(jié)語(yǔ)

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