多傳感器融合算法的四旋翼無(wú)人機(jī)室內(nèi)定位技術(shù)研究

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

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

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

0 引 言

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

1 四旋翼無(wú)人機(jī)系統(tǒng)硬件平臺(tái)及建模

1.1 四旋翼無(wú)人機(jī)系統(tǒng)硬件平臺(tái)

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

1.2 四旋翼無(wú)人機(jī)運(yùn)動(dòng)學(xué)建模

建立四旋翼無(wú)人機(jī)運(yùn)動(dòng)學(xué)模型的具體步驟如下：

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

首先，全局坐標(biāo)系與大地固定相連，其原點(diǎn)被設(shè)在無(wú)人機(jī)的起飛點(diǎn)附近，x軸的正方向?yàn)楸保∟），y軸的正方向?yàn)闁|（E），z軸的正方向?yàn)橄拢―）。

其次，機(jī)體坐標(biāo)系與無(wú)人機(jī)固定相連，其原點(diǎn)被設(shè)在無(wú)人機(jī)的重心位置，x軸的正方向?yàn)榍埃‵），y軸的正方向?yàn)橛遥≧），z軸的正方向?yàn)橄拢―）。

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

（1）

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

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

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

（3）

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

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

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

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

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

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

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

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

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

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

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

3.2 掃描投影

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

（4）

（5）

3.3 位移估計(jì)

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

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

（6）

×

（7）

×

（8）

（9）

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

（10）

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

（11）

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

（12）

令=0，可求得：

（13）

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

（14）

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

3.4 偏航角估計(jì)

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

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

（15）

求導(dǎo)得：

（16）

令=0，可得：

（17）

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

（18）

解得a和b分別為：

（19）

（20）

計(jì)算出最低點(diǎn)橫坐標(biāo)的值為：

" （21）

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

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

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

4 四旋翼無(wú)人機(jī)定位

4.1 位姿估算

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

（22）

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

（23）

（24）

（25）

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

（26）

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

（27）

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

（28）

（29）

4.2 實(shí)驗(yàn)結(jié)果

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

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

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

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

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

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

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

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

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

5 結(jié) 語(yǔ)

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

參考文獻(xiàn)

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

[2]馬向進(jìn)，占禮彬，匡仁，等. 基于四旋翼無(wú)人機(jī)下的自動(dòng)巡檢系統(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)略支援部隊(duì)信息工程大學(xué)，2018.

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

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

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

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

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

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

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