多傳感器融合的無人車自主定位實驗研究

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

關鍵詞： 多傳感器融合； 無人車定位； 慣性測量單元（IMU）； 激光雷達； 擴展卡爾曼濾波； 坐標轉換； 退化環境； 同步定位與建圖（SLAM）

中圖分類號： TN958.98?34； TP273" " " " " " " " " "文獻標識碼： A" " " " " " " " " 文章編號： 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" 引" 言

機器人專業是面向國家重大需求和國際發展趨勢而設立的新工科專業。機器人專業具有學科交叉性強、實踐性強、創新性強、團隊協作性強、國際化視野強等特點。傳統實踐課程受教學模式約束，實踐內容過于程序化，僅規定了實驗步驟和實驗選項，缺少學生自主探索環節，不利于學生思維訓練和綜合能力提高，同時也無法與發展接軌。習近平總書記在2018年9月10日在全國教育大會上的講話提到“要在增強綜合素質上下功夫，教育引導學生培養綜合能力，培養創新思維。”在本科生的機器人專業課程中，實驗是研究性教學環節的重要組成部分。因此，研究一個適當的實驗系統對人才培養有很大的幫助。

一種多傳感器融合的無人車自主定位實驗系統，主要采用IMU獲得位姿信息，采用3D激光雷達獲得點云數據。然后，通過擴展卡爾曼濾波處理位姿信息，通過體素濾波處理點云數據。通過車載SLAM定位系統的測試方案，構建相關車載SLAM定位系統的上位機軟件和程序，通過試驗對得出的結論進行了驗證。所得結論能夠為車載激光SLAM以及慣性導航的定位實現與定位實驗提供依據。這種組合方式能夠有效地提高SLAM系統在復雜現實環境中的測量精度和魯棒性，適用于無人車的自主定位。該系統有利于提高學生的學習主動性，以及提高學生實踐動手能力、創新能力以及相關課程知識綜合應用能力。

1" 多傳感器融合的無人車自主定位實驗系統設計

1.1nbsp; 設計要求

本文選擇多傳感器融合的機器人，并通過算法分析來研究環境退化實驗，分析IMU與激光雷達數據，實現激光慣性融合的定位與建圖。

1.2" 設計方案

對多傳感器融合的無人車自主定位進行實驗研究。首先選擇北京博創尚和科技有限公司的產品“智行Mini2”智能機器人開放平臺作為載體；然后，通過IMU獲得位姿信息，通過激光雷達獲得點云數據，采用擴展卡爾曼濾波處理位姿信息，并利用體素濾波處理點云數據，實現數據預處理；其次，通過坐標轉換實現激光慣性組合定位；最后，在硬件平臺上研究退化環境實驗，實現3D激光慣性融合的定位與建圖。

2" 實驗數據預處理

2.1" 慣性測量模型

設置nt表示高斯白噪聲，該噪聲符合高斯分布。設置[RIW]表示全局坐標系轉換到IMU坐標系的轉換矩陣。采用IMU測量自身在環境中的姿態變化，因此測量值均在IMU的坐標系中表示，其中某一時刻的測量值都由[at]和[wt]兩部分組成。

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

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

對式（1）、式（2）進行積分，可以進一步得到位置、速度以及旋轉的信息。但因為IMU測量的數據不連續，因此需要使用離散形式來表示IMU的位置、速度及旋轉量。

[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）

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

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

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

[ΔRij=RTiRj] （8）

在劇烈運動下，利用IMU的組合定位方式能夠有效減少較大的誤差和點云畸變，并為機器人提供更準確的位姿信息。

2.2" IMU非線性優化

在無人車定位過程中，IMU存在許多不確定因素，如環境噪聲、電機的控制偏差等。因此，需要利用概率論來處理IMU數據的不確定性，計算和描述無人車在各個地方的概率。卡爾曼濾波（Kalman Filter， KF）可以將無人車實時獲取到的信息分散到貝葉斯網絡的概率分布中，以此估算其當前時刻的位姿信息。卡爾曼濾波算法是基于時域描述的線性動態高斯系統，用矩陣參數表示高斯分布，將不同傳感器數據融合，解算出更加精確的測量值的算法。卡爾曼濾波算法可在有噪聲的狀態下對系統狀態進行估計。卡爾曼濾波在線性動態系統問題中起重要的作用，但在非線性問題下失效。因此，提出了擴展卡爾曼濾波（Extended Kalman Filter， EKF）算法，其本質只是將非線性問題做線性處理。

對于一般的非線性系統，在t時刻的狀態方程和觀測方程分別為：

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

式中：[Xt]為t時刻的系統狀態；[Xt-1]為t-1時刻的系統狀態；Ut為t時刻對系統的控制量；Zt為t時刻的觀測值；Wt、Vt分別為觀測噪聲和過程噪聲；h為觀測系統方程；f為系統狀態轉移方程。擴展卡爾曼濾波算法的預測過程和更新過程與卡爾曼濾波算法一致。

預測過程：

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

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

更新過程：

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

2.3" 激光雷達點云數據預處理

根據飛行時間法的原理，計算出不同方向上激光雷達與障礙物反射界面的距離，顯示每條掃描線上的反射距離點，構成雷達的點云圖像。在雷達點云圖像數據處理過程中，會因為環境、設備、人為等因素，造成點云圖像中產生部分畸變及噪聲。因此，需要先處理雷達點云數據，才能更高效地研究點云數據。

激光雷達點云數據在激光雷達運行過程中會出現運動畸變。激光雷達在旋轉掃描作業過程中，若在一個掃描周期內靜止不動，則坐標系一致；但若在運動過程中進行掃描作業，采集到的點云中每個點的測量坐標系都不一致，即坐標系在時變，就會產生較大的運動畸變。本文利用多種方法來消除這種運動畸變，例如：利用激光雷達電機旋轉算出直接預測運動軌跡來消除這種畸變[1]；利用里程計實時提供數據，算出激光雷達的位姿來抵消誤差[2]。但無論是何種方式，都是利用激光雷達的運動軌跡來消除運動畸變。

2.4" 體素濾波

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

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

體素濾波可以過濾大部分的離群噪聲，且減少點云密度和計算量。

3" 激光慣性組合定位

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

3.1" 坐標系定義與轉換（TF坐標變換）

采用機器人判斷自身的位姿，按照不同的參照系與參照物就會有不同的運動描述。因此將各個傳感器與載體本身的坐標系進行統一是必須的，而坐標轉換是將某一傳感器定義的參照物的坐標系轉換到另一參照物的坐標系的運動狀態的過程。在定位實驗當中，需要根據激光雷達和IMU傳感器的安裝特點與位置，進行傳感器的坐標變換，這是數據融合定位的準備。在實驗中坐標系分為世界坐標系與傳感器坐標系，其中傳感器坐標系又可以分為激光雷達坐標系和IMU慣導坐標系。

3.2" 激光雷達坐標系

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

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

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

設激光雷達掃描到的待測物體自身的笛卡爾坐標系為：[XL=xL，yL，zL]，則此點在世界坐標系中如式（14）所示。

[XW=RXL+qWL] （14）

式中：[XW]為待測物體在世界坐標系下的狀態；R為旋轉矩陣。R由歐拉角求得：

以上用歐拉角表示的矩陣R便是坐標系的變換狀態，表示激光雷達坐標系轉化為世界坐標系下的運動狀態。然而利用計算機處理矩陣R時，計算量相當大，甚至可能會產生畸變，因此使用四元數表示，如下：

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

式中i、j、k為復數。其中的關系為：

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

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

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

通過坐標變換將激光雷達數據轉換到全局坐標系下。

3.3" 慣導坐標系

IMU得到的數據經過變換矩陣A后映射至世界坐標系。若IMU某一時刻繞x、y、z軸分別旋轉?、θ、[φ]后，兩個坐標系重合，則A可求解得到：

式中A為正交矩陣，即矩陣A的轉置便為兩個坐標系的轉換。

[C=AT] （20）

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

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

[?θ=arctanc23c22arctan c21] （22）

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

3.4" 激光慣導融合

通過TF坐標系變換，將激光雷達得到的點云數據與IMU得到的位姿信息進行參考系變換，集中到一個質點上，通過激光SLAM算法將其融合。在激光SLAM算法中對點云數據和位姿信息進行幀間配對，使得在某一時間的點云數據擁有同一時刻的位姿信息，以此提高無人車的定位精度，增強算法中檢測的魯棒性。

4" 試驗研究

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

4.1" 環境退化實驗

在走廊場景下進行激光雷達與IMU融合的環境退化實驗，實驗地點在學校A樓的2層。在環境退化實驗當中，使用擴展卡爾曼濾波來對IMU得到的數據進行處理。圖2為一段時間內節點/imu/data_raw和節點/imu/data發布的數據。可看出IMU的原始位姿數據（初始值為0）經過EKF濾波后，使得數據正弦化，實現IMU非線性優化。圖3為由算法得出的用ROS的rviz顯示的定位與地圖。

在實驗時，操作員站在無人車的后方，由于阻擋了掃描，激光產生了小幅度角的空白。無人車不僅較好地顯示了自身定位，還描繪了場景中的細節。地圖中展現出了場景中出現的門以及樓梯。

通過上述可以看出內部數據流與其原理一致。以激光雷達為主、IMU為輔助進行位姿調整、濾波處理、松耦合融合。

在融合實驗當中分別進行了只使用激光雷達的定位建圖和融合激光雷達與IMU的定位建圖，并進行比較分析。在實驗中行進一定的距離，分別記錄行進的距離和構建的地圖。不同情況下無人車行走距離如表1所示。激光雷達定位效果如圖4所示。

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

4.2" 回環校正實驗

本次實驗基于激光雷達與IMU的融合定位來實現算法的閉環檢測功能。實驗地點同樣是在學校A樓的2層。圖5為3D激光慣性融合定位仿真實驗過程中部分節點的數據變化。

圖6的一系列地圖則是無人車順時針繞學校A樓2層運動過程中逐步生成的地圖。

IMU時刻提供精準的里程計數據，為無人車定位提供了校準指標。在實驗中，分別使用scan?to?map方式、map?to?map方式進行建圖，并時刻進行回環檢測。這樣更易于回環檢測來消除地圖重影，并能利用構建局域地圖來構建更準確的全局地圖。

從圖6a）～圖6c）可以看出，無人車隨著建圖規模的增大，所累積的誤差也不斷增大，特別是在行進轉彎時，位姿信息產生了巨大的誤差，在地圖上引起了畸變，出現了重影的現象。

當無人車逐步接近出發點，軌跡形成一個回環時，采用閉環檢測對其進行矯正，如圖6c）、圖6d）所示對構建的地圖分段進行直接修正，最后產生一個正確的地圖。實驗結果表明，IMU測量數據有較高的準確性，而激光點云則會約束IMU的測量偏差。這種組合方式能夠有效地提高SLAM系統在復雜現實環境中的測量精度和魯棒性，適用于無人車的自主定位。

5" 結" 論

學生學習了本系統實驗的設計，對激光雷達與IMU的傳感器工作原理有深刻的認識，并提升了專業理論知識。通過仿真數據分析，學生的編程動手能力得到鍛煉，綜合素養得到提升。經過實物退化實驗，使得學生直觀地明白了算法的可移植應用，提升了學生的自我創新能力。

參考文獻

[1] 陳海明，石海龍，李勐，等.物聯網服務中間件：挑戰與研究進展[J].計算機學報，2017，40（8）：1725?1749.

[2] 殷孝雎，周莉，孫志強，等.基于視覺識別的隨動機械臂實驗裝置設計[J].實驗技術與管理，2023，40（11）：168?174.

[3] 馬晨，蘇易衡，張紫鈺，等.全向移動式智能安防機器人[J].實驗室研究與探索，2023，42（7）：94?98.

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

[5] 張禮廉，屈豪，毛軍，等.視覺/慣性組合導航技術發展綜述[J].導航定位與授時，2020，7（4）：50?63.

[6] 馬學條.改進AOD?Net的端到端圖像去霧實驗研究[J].實驗室研究與探索，2023，42（7）：38?43.

[7] 王雨蒙，王欣，朱冰，等.ROS智能車創新性實驗設計及教學應用[J].實驗室研究與探索，2023，42（6）：182?186.

[8] 關桂霞，任魯魯，王晶.基于計算機系統能力培養的課程改革探索與實踐[J].實驗技術與管理，2019，36（5）：239?241.

[9] 魏彤，龍琛.基于改進遺傳算法的移動機器人路徑規劃[J].北京航空航天大學學報，2020，46（4）：703?711.

[10] 程瑞嘉，吳玉秀，張捍東.基于深度強化學習的機器人未知環境路徑規劃[J].皖西學院學報，2023，39（2）：55?61.

[11] 蔡澤利，譚振江.物聯網智能無線節點自動監控數據采集系統設計[J].現代電子技術，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] 楊志勇，黃文鋒，劉燦.基于樹莓派的遠程控制智能拍照小車[J].現代電子技術，2019，42（8）：168?170.

[14] 惠澤宇.基于激光IMU緊耦合的移動機器人SLAM定位算法研究[D].沈陽：沈陽工業大學，2022.

[15] 韓宇，張磊，吳澤民，等.基于嵌入式樹莓派和OpenCV的運動檢測與跟蹤系統[J].電視技術，2017，41（2）：6?10.