摘" 要： 為了滿足機(jī)器人專業(yè)課程實(shí)驗(yàn)研究性教學(xué)需求，設(shè)計(jì)一個(gè)多傳感器融合的無(wú)人車自主定位實(shí)驗(yàn)。選擇智能機(jī)器人開(kāi)放平臺(tái)作為載體，在硬件平臺(tái)上研究退化環(huán)境實(shí)驗(yàn)，實(shí)現(xiàn)3D激光慣性融合的定位與建圖。在實(shí)驗(yàn)環(huán)節(jié)中，首先，通過(guò)慣性測(cè)量單元（IMU）獲得位姿信息，通過(guò)激光雷達(dá)獲得點(diǎn)云數(shù)據(jù)，利用擴(kuò)展卡爾曼濾波處理位姿信息，并利用體素濾波處理點(diǎn)云數(shù)據(jù)，從而完成數(shù)據(jù)預(yù)處理；然后通過(guò)坐標(biāo)轉(zhuǎn)換實(shí)現(xiàn)激光慣性組合定位；最后在硬件平臺(tái)上研究退化環(huán)境實(shí)驗(yàn)，實(shí)現(xiàn)3D激光慣性融合的定位與建圖。實(shí)驗(yàn)結(jié)果表明，IMU測(cè)量數(shù)據(jù)有較高的準(zhǔn)確性，而激光點(diǎn)云則會(huì)約束IMU的測(cè)量偏差。這種組合方式能夠有效地提高同步定位與建圖（SLAM）系統(tǒng)在復(fù)雜現(xiàn)實(shí)環(huán)境中的測(cè)量精度和魯棒性，適用于無(wú)人車的自主定位。應(yīng)用表明，該實(shí)驗(yàn)系統(tǒng)使學(xué)生獲得了綜合訓(xùn)練，提升了學(xué)生的綜合實(shí)踐創(chuàng)新能力和科研創(chuàng)新能力，并取得了良好的教學(xué)效果。

關(guān)鍵詞： 多傳感器融合； 無(wú)人車定位； 慣性測(cè)量單元（IMU）； 激光雷達(dá)； 擴(kuò)展卡爾曼濾波； 坐標(biāo)轉(zhuǎn)換； 退化環(huán)境； 同步定位與建圖（SLAM）

中圖分類號(hào)： TN958.98?34； TP273" " " " " " " " " "文獻(xiàn)標(biāo)識(shí)碼： A" " " " " " " " " 文章編號(hào)： 1004?373X（2024）16?0090?07

Research on unmanned vehicle autonomous positioning experiment of

based on multi?sensor fusion

CAO Yuehua， LI Hui

（School of Information Engineering， Hangzhou Dianzi University， Hangzhou 310018， China）

Abstract： In order to meet the requirements of research?based teaching of professional robot courses， an unmanned vehicle autonomous positioning experiment based on multi?sensor fusion is designed. An open platform for intelligent robots is selected as the carrier， and the degradation environment experiments is conducted on the hardware platform， so as to realize the 3D laser inertial fusion positioning and mapping. The pose information can be obtained by means of IMU （inertial measurement unit）， and the point cloud data can be obtained by lidar. The extended Kalman filtering is used to process the pose information， and the point cloud data is pre?processed by means of voxel filtering， so as to realize the data preprocessing. The laser inertial combination positioning is realized by means of the coordinate transformation. The degraded environment experiment is researched on hardware platforms to achieve localization and mapping of 3D laser inertial fusion. The experimental results show that IMU measurement data has high accuracy， while laser point clouds constrain the measurement deviation of IMU. This combination method can effectively improve the measurement accuracy and robustness of simultaneous localization and mapping （SLAM） systems in complex real?world environments， and is suitable for autonomous positioning of unmanned vehicles. The application shows that the experimental system can provide students with comprehensive training， improve their comprehensive practical and scientific research innovation abilities， and achieve good teaching results.

Keywords： multisensor fusion; unmanned vehicle positioning; inertial measurement unit; lidar; extended Kalman filtering; coordinate conversion; degradation environment; synchronous localization and mapping

0" 引" 言

機(jī)器人專業(yè)是面向國(guó)家重大需求和國(guó)際發(fā)展趨勢(shì)而設(shè)立的新工科專業(yè)。機(jī)器人專業(yè)具有學(xué)科交叉性強(qiáng)、實(shí)踐性強(qiáng)、創(chuàng)新性強(qiáng)、團(tuán)隊(duì)協(xié)作性強(qiáng)、國(guó)際化視野強(qiáng)等特點(diǎn)。傳統(tǒng)實(shí)踐課程受教學(xué)模式約束，實(shí)踐內(nèi)容過(guò)于程序化，僅規(guī)定了實(shí)驗(yàn)步驟和實(shí)驗(yàn)選項(xiàng)，缺少學(xué)生自主探索環(huán)節(jié)，不利于學(xué)生思維訓(xùn)練和綜合能力提高，同時(shí)也無(wú)法與發(fā)展接軌。習(xí)近平總書記在2018年9月10日在全國(guó)教育大會(huì)上的講話提到“要在增強(qiáng)綜合素質(zhì)上下功夫，教育引導(dǎo)學(xué)生培養(yǎng)綜合能力，培養(yǎng)創(chuàng)新思維。”在本科生的機(jī)器人專業(yè)課程中，實(shí)驗(yàn)是研究性教學(xué)環(huán)節(jié)的重要組成部分。因此，研究一個(gè)適當(dāng)?shù)膶?shí)驗(yàn)系統(tǒng)對(duì)人才培養(yǎng)有很大的幫助。

一種多傳感器融合的無(wú)人車自主定位實(shí)驗(yàn)系統(tǒng)，主要采用IMU獲得位姿信息，采用3D激光雷達(dá)獲得點(diǎn)云數(shù)據(jù)。然后，通過(guò)擴(kuò)展卡爾曼濾波處理位姿信息，通過(guò)體素濾波處理點(diǎn)云數(shù)據(jù)。通過(guò)車載SLAM定位系統(tǒng)的測(cè)試方案，構(gòu)建相關(guān)車載SLAM定位系統(tǒng)的上位機(jī)軟件和程序，通過(guò)試驗(yàn)對(duì)得出的結(jié)論進(jìn)行了驗(yàn)證。所得結(jié)論能夠?yàn)檐囕d激光SLAM以及慣性導(dǎo)航的定位實(shí)現(xiàn)與定位實(shí)驗(yàn)提供依據(jù)。這種組合方式能夠有效地提高SLAM系統(tǒng)在復(fù)雜現(xiàn)實(shí)環(huán)境中的測(cè)量精度和魯棒性，適用于無(wú)人車的自主定位。該系統(tǒng)有利于提高學(xué)生的學(xué)習(xí)主動(dòng)性，以及提高學(xué)生實(shí)踐動(dòng)手能力、創(chuàng)新能力以及相關(guān)課程知識(shí)綜合應(yīng)用能力。

1" 多傳感器融合的無(wú)人車自主定位實(shí)驗(yàn)系統(tǒng)設(shè)計(jì)

1.1nbsp; 設(shè)計(jì)要求

本文選擇多傳感器融合的機(jī)器人，并通過(guò)算法分析來(lái)研究環(huán)境退化實(shí)驗(yàn)，分析IMU與激光雷達(dá)數(shù)據(jù)，實(shí)現(xiàn)激光慣性融合的定位與建圖。

1.2" 設(shè)計(jì)方案

對(duì)多傳感器融合的無(wú)人車自主定位進(jìn)行實(shí)驗(yàn)研究。首先選擇北京博創(chuàng)尚和科技有限公司的產(chǎn)品“智行Mini2”智能機(jī)器人開(kāi)放平臺(tái)作為載體；然后，通過(guò)IMU獲得位姿信息，通過(guò)激光雷達(dá)獲得點(diǎn)云數(shù)據(jù)，采用擴(kuò)展卡爾曼濾波處理位姿信息，并利用體素濾波處理點(diǎn)云數(shù)據(jù)，實(shí)現(xiàn)數(shù)據(jù)預(yù)處理；其次，通過(guò)坐標(biāo)轉(zhuǎn)換實(shí)現(xiàn)激光慣性組合定位；最后，在硬件平臺(tái)上研究退化環(huán)境實(shí)驗(yàn)，實(shí)現(xiàn)3D激光慣性融合的定位與建圖。

2" 實(shí)驗(yàn)數(shù)據(jù)預(yù)處理

2.1" 慣性測(cè)量模型

設(shè)置nt表示高斯白噪聲，該噪聲符合高斯分布。設(shè)置[RIW]表示全局坐標(biāo)系轉(zhuǎn)換到IMU坐標(biāo)系的轉(zhuǎn)換矩陣。采用IMU測(cè)量自身在環(huán)境中的姿態(tài)變化，因此測(cè)量值均在IMU的坐標(biāo)系中表示，其中某一時(shí)刻的測(cè)量值都由[at]和[wt]兩部分組成。

[at=RIW（at-g）+bat+nat] （1）

[wt=wt+bwt+nwt] （2）

對(duì)式（1）、式（2）進(jìn)行積分，可以進(jìn)一步得到位置、速度以及旋轉(zhuǎn)的信息。但因?yàn)镮MU測(cè)量的數(shù)據(jù)不連續(xù)，因此需要使用離散形式來(lái)表示IMU的位置、速度及旋轉(zhuǎn)量。

[vt+Δt=vt+gΔt+Rt（at-bat-nat）Δt] （3）

[pt+Δt=pt+vtΔt+12gΔt2+12Rt（at-bat-nat）Δt2] （4）

[Rt+Δt=Rte（wt-bwt-nwt）Δt]" "（5）

由于有噪聲的存在且噪聲隨著時(shí)間變化，IMU的積分值在幾秒內(nèi)會(huì)發(fā)散，導(dǎo)致機(jī)器人位姿產(chǎn)生較大偏差。因此，IMU只能短時(shí)估計(jì)。對(duì)式（3）～式（5）進(jìn)一步預(yù)積分，便能預(yù)先估計(jì)下一時(shí)刻的位姿，從而有效避免長(zhǎng)時(shí)間的誤差累計(jì)。利用IMU在測(cè)量的2幀內(nèi)進(jìn)行短時(shí)間的預(yù)估，i～j時(shí)刻，機(jī)器人位置、速度及旋轉(zhuǎn)變換量分別為：

[Δvij=RTi（vj-vi-gΔtij）] （6）

[Δpij=RTipj-pi-viΔtij-12gΔt2ij]" （7）

[ΔRij=RTiRj] （8）

在劇烈運(yùn)動(dòng)下，利用IMU的組合定位方式能夠有效減少較大的誤差和點(diǎn)云畸變，并為機(jī)器人提供更準(zhǔn)確的位姿信息。

2.2" IMU非線性優(yōu)化

在無(wú)人車定位過(guò)程中，IMU存在許多不確定因素，如環(huán)境噪聲、電機(jī)的控制偏差等。因此，需要利用概率論來(lái)處理IMU數(shù)據(jù)的不確定性，計(jì)算和描述無(wú)人車在各個(gè)地方的概率?？柭鼮V波（Kalman Filter， KF）可以將無(wú)人車實(shí)時(shí)獲取到的信息分散到貝葉斯網(wǎng)絡(luò)的概率分布中，以此估算其當(dāng)前時(shí)刻的位姿信息?？柭鼮V波算法是基于時(shí)域描述的線性動(dòng)態(tài)高斯系統(tǒng)，用矩陣參數(shù)表示高斯分布，將不同傳感器數(shù)據(jù)融合，解算出更加精確的測(cè)量值的算法。卡爾曼濾波算法可在有噪聲的狀態(tài)下對(duì)系統(tǒng)狀態(tài)進(jìn)行估計(jì)。卡爾曼濾波在線性動(dòng)態(tài)系統(tǒng)問(wèn)題中起重要的作用，但在非線性問(wèn)題下失效。因此，提出了擴(kuò)展卡爾曼濾波（Extended Kalman Filter， EKF）算法，其本質(zhì)只是將非線性問(wèn)題做線性處理。

對(duì)于一般的非線性系統(tǒng)，在t時(shí)刻的狀態(tài)方程和觀測(cè)方程分別為：

[Xt=f（Xt-1，Ut）+WtZt=hXt+Vt] （9）

式中：[Xt]為t時(shí)刻的系統(tǒng)狀態(tài)；[Xt-1]為t-1時(shí)刻的系統(tǒng)狀態(tài)；Ut為t時(shí)刻對(duì)系統(tǒng)的控制量；Zt為t時(shí)刻的觀測(cè)值；Wt、Vt分別為觀測(cè)噪聲和過(guò)程噪聲；h為觀測(cè)系統(tǒng)方程；f為系統(tǒng)狀態(tài)轉(zhuǎn)移方程。擴(kuò)展卡爾曼濾波算法的預(yù)測(cè)過(guò)程和更新過(guò)程與卡爾曼濾波算法一致。

預(yù)測(cè)過(guò)程：

[X（tt-1）=f（X（t-1t-1），Ut）" " " " " " " "P（tt-1）=F?P（t-1t-1）?FT+Qt]" " "（10）

式中F為方程f在t時(shí)刻的雅可比矩陣。

更新過(guò)程：

[X（tt）=X（tt-1）+Gt?（Zt-hX（tt-1））P（tt）=（1-GtH）?P（tt-1）" " " " " " " " " ]" （11）

2.3" 激光雷達(dá)點(diǎn)云數(shù)據(jù)預(yù)處理

根據(jù)飛行時(shí)間法的原理，計(jì)算出不同方向上激光雷達(dá)與障礙物反射界面的距離，顯示每條掃描線上的反射距離點(diǎn)，構(gòu)成雷達(dá)的點(diǎn)云圖像。在雷達(dá)點(diǎn)云圖像數(shù)據(jù)處理過(guò)程中，會(huì)因?yàn)榄h(huán)境、設(shè)備、人為等因素，造成點(diǎn)云圖像中產(chǎn)生部分畸變及噪聲。因此，需要先處理雷達(dá)點(diǎn)云數(shù)據(jù)，才能更高效地研究點(diǎn)云數(shù)據(jù)。

激光雷達(dá)點(diǎn)云數(shù)據(jù)在激光雷達(dá)運(yùn)行過(guò)程中會(huì)出現(xiàn)運(yùn)動(dòng)畸變。激光雷達(dá)在旋轉(zhuǎn)掃描作業(yè)過(guò)程中，若在一個(gè)掃描周期內(nèi)靜止不動(dòng)，則坐標(biāo)系一致；但若在運(yùn)動(dòng)過(guò)程中進(jìn)行掃描作業(yè)，采集到的點(diǎn)云中每個(gè)點(diǎn)的測(cè)量坐標(biāo)系都不一致，即坐標(biāo)系在時(shí)變，就會(huì)產(chǎn)生較大的運(yùn)動(dòng)畸變。本文利用多種方法來(lái)消除這種運(yùn)動(dòng)畸變，例如：利用激光雷達(dá)電機(jī)旋轉(zhuǎn)算出直接預(yù)測(cè)運(yùn)動(dòng)軌跡來(lái)消除這種畸變[1]；利用里程計(jì)實(shí)時(shí)提供數(shù)據(jù)，算出激光雷達(dá)的位姿來(lái)抵消誤差[2]。但無(wú)論是何種方式，都是利用激光雷達(dá)的運(yùn)動(dòng)軌跡來(lái)消除運(yùn)動(dòng)畸變。

2.4" 體素濾波

得到雷達(dá)點(diǎn)云圖像數(shù)據(jù)后，點(diǎn)云圖像中會(huì)存在一些離群噪聲。對(duì)于這種離群噪聲，需要結(jié)合點(diǎn)云的特性加以去除。體素濾波（Voxel Filter）在點(diǎn)云處理中被廣泛運(yùn)用，其主要作用是對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行下采樣，在保證雷達(dá)點(diǎn)云數(shù)據(jù)原有幾何結(jié)構(gòu)不變的情況下，減少雷達(dá)數(shù)據(jù)數(shù)量和離群點(diǎn)。體素濾波器采用體素格中接近中心點(diǎn)的點(diǎn)來(lái)替代體素內(nèi)的所有點(diǎn)云。以體素為單位將點(diǎn)云數(shù)據(jù)劃分到三維網(wǎng)格中，對(duì)于網(wǎng)格中的每個(gè)體素，計(jì)算其包含點(diǎn)云點(diǎn)的重心，根據(jù)所需降采樣的分辨率確定保留哪些重心點(diǎn)，將保留的重心點(diǎn)重新構(gòu)建為點(diǎn)云。因此，體素越大，點(diǎn)云越稀疏；體素越小，點(diǎn)云越稠密。含有n個(gè)點(diǎn)云數(shù)據(jù)的三維體素柵格采樣后的點(diǎn)云坐標(biāo)為：

[x=1ni=1nxi，" y=1ni=1nyi，" z=1ni=1nzi]" " （12）

體素濾波可以過(guò)濾大部分的離群噪聲，且減少點(diǎn)云密度和計(jì)算量。

3" 激光慣性組合定位

同步定位與建圖（SLAM）技術(shù)是無(wú)人車定位的基礎(chǔ)。由于使用單一傳感器構(gòu)建地圖與定位存在定位精度不高、穩(wěn)定性不高等問(wèn)題，因此提出使用慣性傳感器IMU和激光Li?DAR的原理、模型和數(shù)據(jù)預(yù)處理以及組合定位所使用的數(shù)據(jù)融合的方法。

3.1" 坐標(biāo)系定義與轉(zhuǎn)換（TF坐標(biāo)變換）

采用機(jī)器人判斷自身的位姿，按照不同的參照系與參照物就會(huì)有不同的運(yùn)動(dòng)描述。因此將各個(gè)傳感器與載體本身的坐標(biāo)系進(jìn)行統(tǒng)一是必須的，而坐標(biāo)轉(zhuǎn)換是將某一傳感器定義的參照物的坐標(biāo)系轉(zhuǎn)換到另一參照物的坐標(biāo)系的運(yùn)動(dòng)狀態(tài)的過(guò)程。在定位實(shí)驗(yàn)當(dāng)中，需要根據(jù)激光雷達(dá)和IMU傳感器的安裝特點(diǎn)與位置，進(jìn)行傳感器的坐標(biāo)變換，這是數(shù)據(jù)融合定位的準(zhǔn)備。在實(shí)驗(yàn)中坐標(biāo)系分為世界坐標(biāo)系與傳感器坐標(biāo)系，其中傳感器坐標(biāo)系又可以分為激光雷達(dá)坐標(biāo)系和IMU慣導(dǎo)坐標(biāo)系。

3.2" 激光雷達(dá)坐標(biāo)系

激光雷達(dá)坐標(biāo)系用{L}表示，它的原點(diǎn)是激光雷達(dá)接收器的正中心，坐標(biāo)軸方向符合右手定則，左方為x軸，前方為y軸，上方為z軸。IMU坐標(biāo)系用{B}表示，原點(diǎn)為慣導(dǎo)中心，坐標(biāo)軸方向與{L}一致。世界坐標(biāo)系用{W}表示，正北為x軸，正東為z軸，且處于同一水平面，符合右手定則。利用激光雷達(dá)得到的點(diǎn)云數(shù)據(jù)需經(jīng)過(guò)坐標(biāo)軸變換至世界坐標(biāo)系后，才能估計(jì)無(wú)人車狀態(tài)。坐標(biāo)變換用[TWL]表示，如式（13）所示。

[TWL=（txWL，tyWL，tzWL，?xWL，θyWL，φzWL）=（PWL，qWL）] （13）

式中：[txWL]、[tyWL]、[tzWL]分別代表三軸上的平移；[?xWL]、[θyWL]、[φzWL]分別代表歐拉角（橫滾角、俯仰角、偏航角）。

設(shè)激光雷達(dá)掃描到的待測(cè)物體自身的笛卡爾坐標(biāo)系為：[XL=xL，yL，zL]，則此點(diǎn)在世界坐標(biāo)系中如式（14）所示。

[XW=RXL+qWL] （14）

式中：[XW]為待測(cè)物體在世界坐標(biāo)系下的狀態(tài)；R為旋轉(zhuǎn)矩陣。R由歐拉角求得：

以上用歐拉角表示的矩陣R便是坐標(biāo)系的變換狀態(tài)，表示激光雷達(dá)坐標(biāo)系轉(zhuǎn)化為世界坐標(biāo)系下的運(yùn)動(dòng)狀態(tài)。然而利用計(jì)算機(jī)處理矩陣R時(shí)，計(jì)算量相當(dāng)大，甚至可能會(huì)產(chǎn)生畸變，因此使用四元數(shù)表示，如下：

[q=q0+q1i+q2j+q3k] （16）

式中i、j、k為復(fù)數(shù)。其中的關(guān)系為：

[i2=j2=k2=-1ij=k，ij=-k，jk=ikj=-1，ki=j，ik=-j] （17）

若存在一單位矢量ε=[a，b，c]，待測(cè)物體繞該矢量旋轉(zhuǎn)角度θ，其四元數(shù)表示為：

[q=cosθ2，" asinθ2，" bsinθ2，" csinθ2]" （18）

通過(guò)坐標(biāo)變換將激光雷達(dá)數(shù)據(jù)轉(zhuǎn)換到全局坐標(biāo)系下。

3.3" 慣導(dǎo)坐標(biāo)系

IMU得到的數(shù)據(jù)經(jīng)過(guò)變換矩陣A后映射至世界坐標(biāo)系。若IMU某一時(shí)刻繞x、y、z軸分別旋轉(zhuǎn)?、θ、[φ]后，兩個(gè)坐標(biāo)系重合，則A可求解得到：

式中A為正交矩陣，即矩陣A的轉(zhuǎn)置便為兩個(gè)坐標(biāo)系的轉(zhuǎn)換。

[C=AT] （20）

式中C為方向矩陣。最后求得3個(gè)歐拉角：

[φ=arctan-c31c11，" c11gt;0arctan-c31c11-π，" c11lt;0，c31gt;0arctan-c31c11+π，" c11lt;0] （21）

[?θ=arctanc23c22arctan c21] （22）

式中cij代表第i行第j列元素。

3.4" 激光慣導(dǎo)融合

通過(guò)TF坐標(biāo)系變換，將激光雷達(dá)得到的點(diǎn)云數(shù)據(jù)與IMU得到的位姿信息進(jìn)行參考系變換，集中到一個(gè)質(zhì)點(diǎn)上，通過(guò)激光SLAM算法將其融合。在激光SLAM算法中對(duì)點(diǎn)云數(shù)據(jù)和位姿信息進(jìn)行幀間配對(duì)，使得在某一時(shí)間的點(diǎn)云數(shù)據(jù)擁有同一時(shí)刻的位姿信息，以此提高無(wú)人車的定位精度，增強(qiáng)算法中檢測(cè)的魯棒性。

4" 試驗(yàn)研究

實(shí)驗(yàn)所使用的無(wú)人車平臺(tái)是北京博創(chuàng)尚和科技有限公司的產(chǎn)品“智行Mini2” 智能機(jī)器人開(kāi)放平臺(tái)。激光雷達(dá)是SLAMTEC思嵐科技研發(fā)的思嵐RPLIDAR?A1激光雷達(dá)，使用三角測(cè)距法。思嵐RPLIDAR?A1激光雷達(dá)測(cè)量范圍半徑為12 m，掃描測(cè)距為360°，掃描頻率為5.5 Hz；寧美CR160 MINI魔方主機(jī)的運(yùn)行參數(shù)為8 GB內(nèi)存，256 GB固態(tài)硬盤，用于定位與建圖；IMU是由TDK公司生產(chǎn)的ICM?20948九軸姿態(tài)傳感器。用戶可控制傳感器的測(cè)量范圍。陀螺儀可測(cè)量范圍為±250 （°）/s、±500 （°）/s、±1 000 （°）/s、±2 000 （°）/s；加速度計(jì)測(cè)量范圍為±2g、±4g、±8g、±16g；磁力計(jì)可測(cè)量范圍為±4 900 μT。實(shí)驗(yàn)包含環(huán)境退化實(shí)驗(yàn)和回環(huán)校正實(shí)驗(yàn)。圖1為無(wú)人車實(shí)驗(yàn)場(chǎng)地。

4.1" 環(huán)境退化實(shí)驗(yàn)

在走廊場(chǎng)景下進(jìn)行激光雷達(dá)與IMU融合的環(huán)境退化實(shí)驗(yàn)，實(shí)驗(yàn)地點(diǎn)在學(xué)校A樓的2層。在環(huán)境退化實(shí)驗(yàn)當(dāng)中，使用擴(kuò)展卡爾曼濾波來(lái)對(duì)IMU得到的數(shù)據(jù)進(jìn)行處理。圖2為一段時(shí)間內(nèi)節(jié)點(diǎn)/imu/data_raw和節(jié)點(diǎn)/imu/data發(fā)布的數(shù)據(jù)。可看出IMU的原始位姿數(shù)據(jù)（初始值為0）經(jīng)過(guò)EKF濾波后，使得數(shù)據(jù)正弦化，實(shí)現(xiàn)IMU非線性優(yōu)化。圖3為由算法得出的用ROS的rviz顯示的定位與地圖。

在實(shí)驗(yàn)時(shí)，操作員站在無(wú)人車的后方，由于阻擋了掃描，激光產(chǎn)生了小幅度角的空白。無(wú)人車不僅較好地顯示了自身定位，還描繪了場(chǎng)景中的細(xì)節(jié)。地圖中展現(xiàn)出了場(chǎng)景中出現(xiàn)的門以及樓梯。

通過(guò)上述可以看出內(nèi)部數(shù)據(jù)流與其原理一致。以激光雷達(dá)為主、IMU為輔助進(jìn)行位姿調(diào)整、濾波處理、松耦合融合。

在融合實(shí)驗(yàn)當(dāng)中分別進(jìn)行了只使用激光雷達(dá)的定位建圖和融合激光雷達(dá)與IMU的定位建圖，并進(jìn)行比較分析。在實(shí)驗(yàn)中行進(jìn)一定的距離，分別記錄行進(jìn)的距離和構(gòu)建的地圖。不同情況下無(wú)人車行走距離如表1所示。激光雷達(dá)定位效果如圖4所示。

從表1、圖4a）與圖4b）對(duì)比可以看出，融合定位與僅使用激光雷達(dá)定位存在差別。實(shí)驗(yàn)過(guò)程中，讓無(wú)人車實(shí)際行進(jìn)了7.8 m，融合定位誤差為0.4 m，而僅采用激光雷達(dá)定位則誤差為1.2 m，由此可以看出融合定位的優(yōu)越性。另外，與融合定位相比，沒(méi)有IMU的位姿數(shù)據(jù)輔助，地圖很快產(chǎn)生了畸變，走廊地圖變得傾斜且模糊。相比之下，融合定位依靠IMU的位姿數(shù)據(jù)校正了無(wú)人車的定位及所處位置的激光雷達(dá)數(shù)據(jù)，構(gòu)建了更加清楚、正確的地圖。

4.2" 回環(huán)校正實(shí)驗(yàn)

本次實(shí)驗(yàn)基于激光雷達(dá)與IMU的融合定位來(lái)實(shí)現(xiàn)算法的閉環(huán)檢測(cè)功能。實(shí)驗(yàn)地點(diǎn)同樣是在學(xué)校A樓的2層。圖5為3D激光慣性融合定位仿真實(shí)驗(yàn)過(guò)程中部分節(jié)點(diǎn)的數(shù)據(jù)變化。

圖6的一系列地圖則是無(wú)人車順時(shí)針繞學(xué)校A樓2層運(yùn)動(dòng)過(guò)程中逐步生成的地圖。

IMU時(shí)刻提供精準(zhǔn)的里程計(jì)數(shù)據(jù)，為無(wú)人車定位提供了校準(zhǔn)指標(biāo)。在實(shí)驗(yàn)中，分別使用scan?to?map方式、map?to?map方式進(jìn)行建圖，并時(shí)刻進(jìn)行回環(huán)檢測(cè)。這樣更易于回環(huán)檢測(cè)來(lái)消除地圖重影，并能利用構(gòu)建局域地圖來(lái)構(gòu)建更準(zhǔn)確的全局地圖。

從圖6a）～圖6c）可以看出，無(wú)人車隨著建圖規(guī)模的增大，所累積的誤差也不斷增大，特別是在行進(jìn)轉(zhuǎn)彎時(shí)，位姿信息產(chǎn)生了巨大的誤差，在地圖上引起了畸變，出現(xiàn)了重影的現(xiàn)象。

當(dāng)無(wú)人車逐步接近出發(fā)點(diǎn)，軌跡形成一個(gè)回環(huán)時(shí)，采用閉環(huán)檢測(cè)對(duì)其進(jìn)行矯正，如圖6c）、圖6d）所示對(duì)構(gòu)建的地圖分段進(jìn)行直接修正，最后產(chǎn)生一個(gè)正確的地圖。實(shí)驗(yàn)結(jié)果表明，IMU測(cè)量數(shù)據(jù)有較高的準(zhǔn)確性，而激光點(diǎn)云則會(huì)約束IMU的測(cè)量偏差。這種組合方式能夠有效地提高SLAM系統(tǒng)在復(fù)雜現(xiàn)實(shí)環(huán)境中的測(cè)量精度和魯棒性，適用于無(wú)人車的自主定位。

5" 結(jié)" 論

學(xué)生學(xué)習(xí)了本系統(tǒng)實(shí)驗(yàn)的設(shè)計(jì)，對(duì)激光雷達(dá)與IMU的傳感器工作原理有深刻的認(rèn)識(shí)，并提升了專業(yè)理論知識(shí)。通過(guò)仿真數(shù)據(jù)分析，學(xué)生的編程動(dòng)手能力得到鍛煉，綜合素養(yǎng)得到提升。經(jīng)過(guò)實(shí)物退化實(shí)驗(yàn)，使得學(xué)生直觀地明白了算法的可移植應(yīng)用，提升了學(xué)生的自我創(chuàng)新能力。

參考文獻(xiàn)

[1] 陳海明，石海龍，李勐，等.物聯(lián)網(wǎng)服務(wù)中間件：挑戰(zhàn)與研究進(jìn)展[J].計(jì)算機(jī)學(xué)報(bào)，2017，40（8）：1725?1749.

[2] 殷孝雎，周莉，孫志強(qiáng)，等.基于視覺(jué)識(shí)別的隨動(dòng)機(jī)械臂實(shí)驗(yàn)裝置設(shè)計(jì)[J].實(shí)驗(yàn)技術(shù)與管理，2023，40（11）：168?174.

[3] 馬晨，蘇易衡，張紫鈺，等.全向移動(dòng)式智能安防機(jī)器人[J].實(shí)驗(yàn)室研究與探索，2023，42（7）：94?98.

[4] 林昀軒，劉超，包建榮，等.通信實(shí)驗(yàn)數(shù)據(jù)的分布式智能無(wú)線傳輸及共享方案[J].實(shí)驗(yàn)室研究與探索，2021，40（7）：116?120.

[5] 張禮廉，屈豪，毛軍，等.視覺(jué)/慣性組合導(dǎo)航技術(shù)發(fā)展綜述[J].導(dǎo)航定位與授時(shí)，2020，7（4）：50?63.

[6] 馬學(xué)條.改進(jìn)AOD?Net的端到端圖像去霧實(shí)驗(yàn)研究[J].實(shí)驗(yàn)室研究與探索，2023，42（7）：38?43.

[7] 王雨蒙，王欣，朱冰，等.ROS智能車創(chuàng)新性實(shí)驗(yàn)設(shè)計(jì)及教學(xué)應(yīng)用[J].實(shí)驗(yàn)室研究與探索，2023，42（6）：182?186.

[8] 關(guān)桂霞，任魯魯，王晶.基于計(jì)算機(jī)系統(tǒng)能力培養(yǎng)的課程改革探索與實(shí)踐[J].實(shí)驗(yàn)技術(shù)與管理，2019，36（5）：239?241.

[9] 魏彤，龍琛.基于改進(jìn)遺傳算法的移動(dòng)機(jī)器人路徑規(guī)劃[J].北京航空航天大學(xué)學(xué)報(bào)，2020，46（4）：703?711.

[10] 程瑞嘉，吳玉秀，張捍東.基于深度強(qiáng)化學(xué)習(xí)的機(jī)器人未知環(huán)境路徑規(guī)劃[J].皖西學(xué)院學(xué)報(bào)，2023，39（2）：55?61.

[11] 蔡澤利，譚振江.物聯(lián)網(wǎng)智能無(wú)線節(jié)點(diǎn)自動(dòng)監(jiān)控?cái)?shù)據(jù)采集系統(tǒng)設(shè)計(jì)[J].現(xiàn)代電子技術(shù)，2018，41（4）：183?186.

[12] LIU Penghou， CHEN Haichao， DU Yanzhe. Design of multifunctional intelligent security robot based on single chip microcomputer [J]. Journal of physics： conference series， 2019， 1187（3）：032017.

[13] 楊志勇，黃文鋒，劉燦.基于樹莓派的遠(yuǎn)程控制智能拍照小車[J].現(xiàn)代電子技術(shù)，2019，42（8）：168?170.

[14] 惠澤宇.基于激光IMU緊耦合的移動(dòng)機(jī)器人SLAM定位算法研究[D].沈陽(yáng)：沈陽(yáng)工業(yè)大學(xué)，2022.

[15] 韓宇，張磊，吳澤民，等.基于嵌入式樹莓派和OpenCV的運(yùn)動(dòng)檢測(cè)與跟蹤系統(tǒng)[J].電視技術(shù)，2017，41（2）：6?10.