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

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

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

0 引 言

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

1 四旋翼無人機系統(tǒng)硬件平臺及建模

1.1 四旋翼無人機系統(tǒng)硬件平臺

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

1.2 四旋翼無人機運動學(xué)建模

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

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

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

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

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

（1）

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

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

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

（3）

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

2 傳感器之間的融合技術(shù)

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

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

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

決策級融合技術(shù)對分散的傳感器收集到的各項數(shù)據(jù)進行匯總?cè)诤咸幚?。這種融合方式具有較好的魯棒性和靈活性，能夠根據(jù)不同的任務(wù)需求和場景特點，自適應(yīng)地選擇合適的融合算法和策略。然而，使用決策級融合技術(shù)需要具備一定的學(xué)習(xí)和推理能力，才能實現(xiàn)更高層次的數(shù)據(jù)分析和處理。

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

3 改進算法的室內(nèi)定位技術(shù)

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

3.1 預(yù)處理激光雷達數(shù)據(jù)

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

3.2 掃描投影

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

（4）

（5）

3.3 位移估計

在獲得新的插值點和當(dāng)前掃描和參考掃描中的數(shù)據(jù)點之間的關(guān)系后，即可進行位移的估計。

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

（6）

×

（7）

×

（8）

（9）

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

（10）

加權(quán)誤差平方和函數(shù)表達式為：

（11）

對其求導(dǎo)可得：

（12）

令=0，可求得：

（13）

由此可得權(quán)值表達式為：

（14）

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

3.4 偏航角估計

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

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

（15）

求導(dǎo)得：

（16）

令=0，可得：

（17）

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

（18）

解得a和b分別為：

（19）

（20）

計算出最低點橫坐標(biāo)的值為：

" （21）

3.5 定位算法實現(xiàn)

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

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

4 四旋翼無人機定位

4.1 位姿估算

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

（22）

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

（23）

（24）

（25）

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

（26）

設(shè)在機體坐標(biāo)系B下激光雷達掃描數(shù)據(jù)點的極坐標(biāo)為（ri， ξi），則笛卡爾坐標(biāo)為[Pi]B=[ri cos（ξi）， ri sin（ξi）， 0]T。經(jīng)過轉(zhuǎn)換矩陣變換后可得：

（27）

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

（28）

（29）

4.2 實驗結(jié)果

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

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

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

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

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

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

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

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

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

5 結(jié) 語

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

參考文獻

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

[2]馬向進，占禮彬，匡仁，等. 基于四旋翼無人機下的自動巡檢系統(tǒng)[J]. 物聯(lián)網(wǎng)技術(shù)，2019，9（7）：103-104.

[3]李平伯. 基于雙目和結(jié)構(gòu)光的弱特征SLAM研究[D].南京：東南大學(xué)，2019.

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

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

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

[7]甘培源，梅聰聰，謝逸菲.四旋翼自主精準(zhǔn)降落平臺設(shè)計[J].物聯(lián)網(wǎng)技術(shù)，2019，9（8）：50-52.

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

[9]喬文艷. 基于UWB技術(shù)的高速公路車輛定位系統(tǒng)研究與設(shè)計[D].呼和浩特：內(nèi)蒙古大學(xué)，2019.

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

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