摘 要：針對缺乏明顯位置標識的室內環(huán)境，為實現無人機的精確定位，提出了一種多傳感器融合算法。該算法綜合運用了中值濾波、閾值分割、投影變換、最小二乘法等手段，以實現無人機位置和位姿的精確估計。通過一系列實驗，對所提出的算法與模擬算法進行了對比分析，驗證了所提算法的準確性，并完成了四旋翼無人機在室內環(huán)境下的初步定位任務。

關鍵詞：四旋翼無人機；多傳感器；室內環(huán)境；位置定位；融合算法；激光雷達

中圖分類號：TP273 文獻標識碼：A 文章編號：2095-1302（2025）01-00-05

0 引 言

近年來，隨著信息技術的不斷更新和發(fā)展，四旋翼無人機已經成為了研究熱點；同時，具有垂直起降功能的多旋翼無人機也逐漸廣泛地應用于軍事和民用生活的諸多領域。然而，在室內環(huán)境中，由于信號遮擋、墻壁效應等因素的影響，GPS等傳統定位技術往往難以提供精確的定位信息。為此，本文采用多傳感器融合算法，通過對多種傳感器的信息進行互補、冗余處理和融合，可以顯著提高室內定位的精度和穩(wěn)定性。這一方法為實現四旋翼無人機在室內環(huán)境下的精確定位提供了有效的解決方案和先進的控制策略[1]。

1 四旋翼無人機系統硬件平臺及建模

1.1 四旋翼無人機系統硬件平臺

四旋翼無人機硬件平臺由檢測模塊、控制模塊、驅動模塊以及電源模塊4個部分組成。其中檢測模塊主要集成了陀螺儀、加速度計、數字羅盤、氣壓計、紅外傳感器、超聲波傳感器、位置傳感器等感官器件；控制模塊則包含了微處理器、通用處理器、數字信號處理器、視頻處理器等關鍵器件[2]。

1.2 四旋翼無人機運動學建模

建立四旋翼無人機運動學模型的具體步驟如下：

（1） 確定四旋翼無人機的坐標系。該模型涉及2個坐標系，即全局坐標系和機體坐標系。

首先，全局坐標系與大地固定相連，其原點被設在無人機的起飛點附近，x軸的正方向為北（N），y軸的正方向為東（E），z軸的正方向為下（D）。

其次，機體坐標系與無人機固定相連，其原點被設在無人機的重心位置，x軸的正方向為前（F），y軸的正方向為右（R），z軸的正方向為下（D）。

（2）確定四旋翼無人機的運動狀態(tài)。在機體坐標系下，無人機的運動狀態(tài)由其線速度和角速度決定。因此無人機的位置信息可用下式表示：

（1）

（3）構建一個四旋翼無人機的動力學模型。該模型的輸入信息包括螺旋槳產生的拉力和力矩，而模型的輸出信息則反映四旋翼的速度和角速度，即四旋翼無人機的位置和姿態(tài)。無人機的角度信息用下式表示：

Ф=[ ?" θ" Ψ]T=W·ωb （2）

式中：Ф為無人機的姿態(tài)角；ωb為無人機的角速度；W為兩者之間的旋轉矩陣，可用下式表示：

（3）

四旋翼無人機模型中的旋轉矩陣可以用于描述無人機的姿態(tài)變化，即從機體坐標系到全局坐標系的旋轉矩陣是將無人機在機體坐標系下的運動狀態(tài)轉換到全局坐標系下的運動狀態(tài)的關鍵[3]。

2 傳感器之間的融合技術

傳感器信息融合技術中，傳感器之間的數據融合被分為數據級融合、特征級融合和決策級融合這3種數據融合方式[4-5]，具體見表1。

數據級融合主要適用于同類型傳感器的數據融合，通過充分挖掘和利用數據間的互補性，能夠更全面、準確地反映觀測對象的特征和狀態(tài)。這種融合方式的優(yōu)勢在于能夠最大限度地保留原始數據的信息量，同時避免數據在融合過程中丟失。然而，隨著傳感器種類的增多和數據復雜性的提高，數據級融合的難度和計算量也會相應增加。

特征級融合會自動丟失一部分原始數據，但在處理異構、異質傳感器數據時具有較好的適應性。這種融合方式通過提取不同傳感器數據的特征信息，將它們有機地結合起來，從而實現對觀測對象的多角度、多層次描述。然而，特征級融合可能會損失一些細節(jié)信息，導致結果不夠全面和準確。

決策級融合技術對分散的傳感器收集到的各項數據進行匯總融合處理。這種融合方式具有較好的魯棒性和靈活性，能夠根據不同的任務需求和場景特點，自適應地選擇合適的融合算法和策略。然而，使用決策級融合技術需要具備一定的學習和推理能力，才能實現更高層次的數據分析和處理。

四旋翼無人機的位置姿態(tài)測量是實現精確定位的關鍵。特別是在室內沒有GPS的情況下，要獲得位置姿態(tài)，必須采用多種傳感器技術，從實際應用的角度出發(fā)，采用數據級融合方法。

3 改進算法的室內定位技術

為進一步提升室內定位的精度，提出一種改進算法的室內無人機定位技術。首先采用算法對無人機飛行空間中二維平面上的相關數據進行分析，然后對三維立體空間數據進行分析，進一步得到準確的位置數據以及與參考點的距離數據。通過姿態(tài)估計，得到與參考數據的最優(yōu)匹配，并利用激光雷達根據掃描序列中的角度掃描信息與當前掃描點的關系，計算出二者間的位姿信息。

3.1 預處理激光雷達數據

激光雷達傳感器使用中值濾波器處理數據，以去除噪聲并提高精度。同時，通過延長測量時間，可以避免在2個不同目標掃描點之間進行不必要的插值計算。此外，激光雷達傳感器還采用閾值法對數據進行標記[6]。假設所有數據點在同一區(qū)域內，起點（r1， ξ1）作為第一個點，終點（rn， ξn）為最后一個點。計算出第i個點和第i-1個點的插值D，并通過比較D與閾值Max diff的關系，來判斷該點是否與前一數據點同屬一個區(qū)域。通過此方法，最終可獲得多個不相交的區(qū)域。

3.2 掃描投影

雷達的數據預處理完成后，對數據點進行投影變換。假定當前掃描數據為{（rc1， ξc1）， （rc2， ξc2）， ...， （rcn， ξcn）， ...}，而參考掃描數據為{（rr1， ξr1）， （rr2， ξr2）， ...， （rrn， ξrn）， ...}。當前掃描的位置點在參考掃描坐標系下的坐標表示為（xc， yc， ψc），則第i點的極坐標為（r'ci， ξ'ci），表達式為：

（4）

（5）

3.3 位移估計

在獲得新的插值點和當前掃描和參考掃描中的數據點之間的關系后，即可進行位移的估計。

為減小配準誤差對位移估計的影響，本文提出一種基于最小二乘法的方法來估算其位移。采用了加權最小二乘法，并對其進行了線性化。

（6）

×

（7）

×

（8）

（9）

至此，得到噪聲向量的矩陣H，表達式為：

（10）

加權誤差平方和函數表達式為：

（11）

對其求導可得：

（12）

令=0，可求得：

（13）

由此可得權值表達式為：

（14）

此時，可知當d的數值越小時，越能有效減小錯誤點對位移估計的影響[7]。

3.4 偏航角估計

在參考掃描坐標系下，當前掃描位置點的偏航角表示為ψc，其改變意味著激光雷達的角度測量范圍向左或向右偏移。假設當前掃描位置點相對于參考掃描位置點的位移量為（xc， yc），在2次掃描過程中檢測到同一目標[8]。為了尋找使當前掃描數據點與參考掃描數據點完全重合的角度估計值，可以假設當前掃描位置點的偏航角度在±20°范圍內，將所有當前掃描投影點的角度（r\" ci， ξ\" ci）分別偏移-20°、-19°、

-18°、…、18°、19°、20°，并計算每個偏移量對應的絕對平均誤差值[9]。這樣，掃描位置點的偏航角估計就轉換成了拋物線最小值的計算問題，得到拋物線方程為：

（15）

求導得：

（16）

令=0，可得：

（17）

為了得到a、b的值，把et-1、et、et+1代入拋物線方程可得：

（18）

解得a和b分別為：

（19）

（20）

計算出最低點橫坐標的值為：

" （21）

3.5 定位算法實現

定位算法流程如圖1所示。

在四旋翼無人機姿態(tài)估算中，系統建模的首要步驟是建立四旋翼無人機的動力學模型，該模型涵蓋了無人機的運動學方程和傳感器觀測模型。在室內環(huán)境下，由于缺乏GPS等外部定位信號，無人機依賴IMU傳感器來獲取其姿態(tài)和加速度信息。具體而言，通過IMU傳感器檢測無人機的加速度和角速度，并通過積分運算得出無人機的姿態(tài)信息。獲取初始姿態(tài)估計值后，系統開始讀取參考掃描和當前掃描的數據點。為了提升數據質量，需要對這些數據進行中值濾波，以濾除噪聲點，并標記遠距離點和區(qū)域，隨后對當前掃描數據進行變換投影處理。當迭代次數達到最大值時，系統即可輸出無人機當前掃描點相對位姿信息。若迭代次數未達到最大值，則需繼續(xù)進行位移估計，系統會判斷是否滿足條件：|dx|+|dy|lt;1。若滿足，則進行偏移估計，從而得到無人機當前的相對位姿信息；若不滿足條件，則須重新判斷迭代次數是否已達到最大值；并據此決定是否繼續(xù)循環(huán)。需要注意的是，由于室內環(huán)境的復雜性和變化性，需要定期對算法進行優(yōu)化和調整，以保證姿態(tài)估算的準確性和實時性。

4 四旋翼無人機定位

4.1 位姿估算

四旋翼無人機在進行三維姿態(tài)估計時，會對慣性導航模塊測量的姿態(tài)角度信息進行旋轉轉換，并將轉換后的結果投射到二維x-y平面上，以便進行位置估計[10]。在此過程中值得注意的是，四旋翼無人機的激光雷達原點和機身的原點是重合的，同時激光雷達的x軸和y軸的定義與無人機的飛行方向保持一致[11]。為了更好地研究無人機在世界坐標系下的位姿情況，利用虛擬坐標系中機體坐標系在x軸、y軸上的變換獲得投影坐標系，同時用標準歐拉角η=[， θ， ψ]表示無人機的姿態(tài)角，為滾轉角（繞x軸旋轉），θ為俯仰角（繞y軸旋轉），ψ為偏航角（繞z軸旋轉）。在姿態(tài)估計過程中，轉換矩陣通常用一個3×3的旋轉矩陣R來表示，該矩陣是一個由上述姿態(tài)角決定的3D旋轉矩陣。歐拉角和旋轉矩陣的關系表達式為：

（22）

式中：Rz、Ry、Rx分別是z軸、y軸、x軸的旋轉矩陣。最后將機體坐標系B到坐標系I沿各個方向軸的轉換矩陣分別表示為：

（23）

（24）

（25）

通過機體坐標系B繞x軸和y軸兩個方向軸到虛擬坐標系V下的旋轉變換，可以得到：

（26）

設在機體坐標系B下激光雷達掃描數據點的極坐標為（ri， ξi），則笛卡爾坐標為[Pi]B=[ri cos（ξi）， ri sin（ξi）， 0]T。經過轉換矩陣變換后可得：

（27）

極坐標系是用于描述位置和方向的坐標系，其中位置由距離和角度確定，結合所提出的基于角度信息計算的位姿估算方法，將[Pi']V轉換到極坐標，并使用角度信息進行位姿估算。

（28）

（29）

4.2 實驗結果

為進一步獲得更準確的位置掃描信息，在實際測試中，將四旋翼無人機的位置激光雷達分別部署于實驗室的對角線位置A點和B點。利用A點和B點的不同位置來構建基準掃描圖與當前掃描圖。獲得數據后，采用PSM方法對這2組數據進行離線處理，并將處理結果與采用常規(guī)的ICP方法得到的處理結果進行比較。

場景一為實驗室環(huán)境，如圖2所示，相對于空曠的室內環(huán)境而言較為復雜，設定當前掃描位姿為xc=45 cm，yc=60 cm，ψc=45°。

場景二是室內障礙物較多的環(huán)境，如圖3所示，掃描位姿為xc=-60 cm，yc=-25 cm，ψc=-90°。

場景三是無人機實驗室的室外走廊，如圖4所示，環(huán)境較為空曠，掃描位姿為xc=-65 cm，yc=60 cm，ψc=0°。

圖5是無人機在特定場景飛行后呈現出的不同區(qū)域的基準數據，這種數據通過變換后改變了原有的數據形態(tài)，如圖6所示。雖然無法在三維環(huán)境中精確計算出位置誤差，但圖7表明，經過算法處理后的數據匹配效果明顯優(yōu)于未處理的數據。

圖8中的軌跡為一個閉合三角形。選取起飛的參考點后，當無人機回到參考點時，其x、y飛行方向以及偏航角誤差分別為：-12.63 cm，8.84 cm，7.09°。

由此可知，基于角度匹配的位姿估計方法能夠應用在四旋翼無人飛行器的室內位置測量中。在該試驗中，還可以先采用PSM方法求出連續(xù)2個激光雷達的相對位姿，再對其進行簡單的累積，得出四旋翼無人機的初始位置。

表2給出了不同場景下的位姿估計結果。

實驗結果表明，本文提出的基于卡爾曼濾波的多傳感器融合算法可以有效提高四旋翼無人機室內定位技術的精度和穩(wěn)定性。與傳統的單一傳感器定位技術相比，該方法的定位精度提高了30%以上，穩(wěn)定性提高了20%以上。

5 結 語

本文以四旋翼無人機為研究對象，結合四旋翼無人機控制技術和多傳感器融合技術，研究利用二維激光雷達、慣性傳感器、聲納等設備，使無人機在缺乏GPS信號的復雜室內環(huán)境具有導航定位能力；同時，通過融合更為先進的三維激光雷達和里程計，無人機能夠在室內環(huán)境中獲取更高精度的傳感器數據和飛行參數，提高無人機的運算精度，以成功完成同步定位和構圖、自主避障、自主導航等復雜任務。

參考文獻

[1]趙敏. 四旋翼無人機平穩(wěn)飛行的控制研究[D].天津：天津科技大學，2019.

[2]馬向進，占禮彬，匡仁，等. 基于四旋翼無人機下的自動巡檢系統[J]. 物聯網技術，2019，9（7）：103-104.

[3]李平伯. 基于雙目和結構光的弱特征SLAM研究[D].南京：東南大學，2019.

[4]向奉卓. 多傳感器組合的室內自主定位技術研究[D].鄭州：戰(zhàn)略支援部隊信息工程大學，2018.

[5]意法半導體.基于MEMS傳感器的行人航位推算解決方案[J].單片機與嵌入式系統應用，2016，16（9）：80-81.

[6]吳丹. 基于深度視覺的室內機器人定位研究[D].北京：北京交通大學，2017.

[7]甘培源，梅聰聰，謝逸菲.四旋翼自主精準降落平臺設計[J].物聯網技術，2019，9（8）：50-52.

[8]鐘歡.關于無人機巡查照明設施的研究和管理機制的探索[J].市政技術，2021，39（z1）：173-176.

[9]喬文艷. 基于UWB技術的高速公路車輛定位系統研究與設計[D].呼和浩特：內蒙古大學，2019.

[10]朱偉康. 基于改進ORB的視覺SLAM算法研究[D].天津：天津工業(yè)大學，2020.

[11]朱明紅. 基于低成本IMU的通用多傳感器集成動態(tài)定位關鍵技術研究[D].哈爾濱：哈爾濱工程大學，2021.