無(wú)人機(jī)導(dǎo)航系統(tǒng)的多傳感融合和姿態(tài)解算

在無(wú)人機(jī)領(lǐng)域，大疆一直都行走在第一隊(duì)列，各款消費(fèi)級(jí)無(wú)人機(jī)層出不窮，去年的人氣王MavicAir2,今年的FJIFPV和DJIAir2S。大疆無(wú)人機(jī)在中國(guó)市場(chǎng)的占有率為90%，在全球市場(chǎng)的占有率為70%，而在美國(guó)市場(chǎng)的占有率接近80%，以至于大家可以頻頻在各類(lèi)好萊塢大片中看到大疆無(wú)人機(jī)的身影。

所謂無(wú)人機(jī)，就是利用無(wú)線(xiàn)電遙控設(shè)備和自備的程序控制裝置操縱的不載人飛機(jī)，機(jī)上不配備駕駛艙，不需要飛行員駕駛，內(nèi)部配置有駕駛儀等控制裝置。近年來(lái)，無(wú)人機(jī)在民用、軍事和科研等領(lǐng)域被廣泛應(yīng)用，例如偵察監(jiān)視、對(duì)地攻擊、交通巡邏等。其中，在無(wú)人機(jī)的飛行控制中，飛行姿態(tài)是重要參數(shù)之一，慣性導(dǎo)航系統(tǒng)是較為常用的一種導(dǎo)航方式。

一、傳感器件

對(duì)于飛行器而言，姿態(tài)角的解算進(jìn)度和導(dǎo)航精度密切相關(guān)，因此姿態(tài)解算和實(shí)時(shí)更新是導(dǎo)航系統(tǒng)的重點(diǎn)。加速度計(jì)和陀螺儀是姿態(tài)解算的主要器件。

加速度計(jì)測(cè)量載體所受到的加速度力的大小，當(dāng)加速度計(jì)保持穩(wěn)定不動(dòng)時(shí)，測(cè)量的數(shù)據(jù)即為重力在三維平面上的投影生成的反作用力的大小。僅僅利用三軸加速度計(jì)可以實(shí)現(xiàn)姿態(tài)角的估計(jì)，但是由于三軸加速度計(jì)動(dòng)態(tài)響應(yīng)較慢，若僅使用加速度計(jì)獲取姿態(tài)角，會(huì)產(chǎn)生較大的延時(shí)，并且在運(yùn)動(dòng)過(guò)程中，姿態(tài)解算精度較低。

陀螺儀是測(cè)量載體繞轉(zhuǎn)軸轉(zhuǎn)動(dòng)的角速度，又稱(chēng)角速度計(jì)。當(dāng)載體以某個(gè)速率運(yùn)動(dòng)時(shí)，可以通過(guò)測(cè)量哥氏加速度獲取三維空間轉(zhuǎn)動(dòng)的角速率。理論上，對(duì)角速率進(jìn)行積分既可獲取載體額姿態(tài)角信息，但是由于陀螺儀的測(cè)量會(huì)受到漂移誤差的影響，對(duì)于有規(guī)律的漂移，可以通過(guò)建立數(shù)學(xué)模型進(jìn)行補(bǔ)償，但是對(duì)于隨機(jī)漂移誤差的補(bǔ)償尚未出現(xiàn)特別有效的方法。因此，僅利用陀螺儀解算姿態(tài)角也是不合適的。

二、慣性導(dǎo)航系統(tǒng)

1、工作原理

慣性導(dǎo)航系統(tǒng)通過(guò)對(duì)總的增量進(jìn)行積分運(yùn)算獲取位置和姿態(tài)信息，主要可分為平臺(tái)式慣性導(dǎo)航系統(tǒng)和捷聯(lián)式慣性導(dǎo)航系統(tǒng)。

平臺(tái)慣慣性導(dǎo)航系統(tǒng)中將加速度計(jì)和陀螺儀安裝在導(dǎo)航平臺(tái)上，用導(dǎo)航平臺(tái)模擬導(dǎo)航坐標(biāo)系，沿著導(dǎo)航坐標(biāo)系軸向?qū)?dǎo)航加速度計(jì)固定測(cè)量軸方向，進(jìn)而加速度計(jì)即可獲取飛行器在導(dǎo)航坐標(biāo)系中的加速度，再利用幾何方法，從平臺(tái)上獲取飛行器的姿態(tài)和航向信息。

捷聯(lián)慣性導(dǎo)航系統(tǒng)是將加速度計(jì)和陀螺儀直接固連在飛行器上，而不是采用機(jī)械式的陀螺穩(wěn)定平臺(tái)。按照飛行器的滾轉(zhuǎn)軸、俯仰軸和偏航軸的方向確定加速度計(jì)和陀螺儀的輸入軸，

利用姿態(tài)矩陣，將三軸加速度計(jì)和三軸陀螺儀測(cè)量的飛行器沿機(jī)體坐標(biāo)系軸向的加速度和角速度信息，變換至導(dǎo)航坐標(biāo)系中，進(jìn)而獲取飛行器的速度、位置、姿態(tài)信息。

兩種慣性導(dǎo)航系統(tǒng)相比之下，捷聯(lián)慣性導(dǎo)航系統(tǒng)具有自主式的導(dǎo)航方法，系統(tǒng)的可靠性高于平臺(tái)慣性導(dǎo)航系統(tǒng)。

2、導(dǎo)航系統(tǒng)融合算法

鑒于加速度計(jì)和陀螺儀頻域特性互補(bǔ)的特點(diǎn)，可以融合兩者的原始數(shù)據(jù)進(jìn)行姿態(tài)解算。下面主要介紹互補(bǔ)濾波和卡爾曼濾波兩種常用融合算法。

1.互補(bǔ)濾波：

互補(bǔ)濾波器通常用于將不同傳感器測(cè)量的物理意義相似或者相同的數(shù)據(jù)進(jìn)行融合，對(duì)于輸入的多個(gè)輸入量需要進(jìn)行相應(yīng)的預(yù)處理，即具有高頻噪聲的輸入量，需要使用低通濾波器進(jìn)行濾波。在飛行器姿態(tài)測(cè)量中，對(duì)于加速度計(jì)輸入的變量通常需要進(jìn)行低通濾波，對(duì)于陀螺儀輸入的變量會(huì)進(jìn)行高通濾波。

下圖是一種常見(jiàn)的二階互補(bǔ)濾波算法實(shí)現(xiàn)框圖。

在二階互補(bǔ)濾波中，對(duì)輸出值x與加速度計(jì)的測(cè)量值y1作差，然后將差值通過(guò)PI控制器進(jìn)行處理后，以負(fù)反饋形式對(duì)陀螺儀測(cè)量值進(jìn)行校正，簡(jiǎn)化的姿態(tài)角計(jì)算公式為

其中，Anglek為第k次濾波后的角度，Anglek-1為第k-1次濾波處理后的角度，angle_rate為陀螺儀測(cè)量的角速度值，t 為陀螺儀采樣間隔時(shí)間。Angle_acc為加速度計(jì)的角度測(cè)量值，a為高通濾波系數(shù)，b為低通濾波系數(shù)，兩者均為定值，且在系統(tǒng)工作過(guò)程中，該權(quán)值不會(huì)改變。

2.卡爾曼濾波

卡爾曼濾波是將最小均方誤差作為標(biāo)準(zhǔn)進(jìn)行最優(yōu)估計(jì)。首先，將離散系統(tǒng)的控制過(guò)程用下面的方程進(jìn)行表達(dá)。

k代表的是離散時(shí)間，X(K)代表的是k時(shí)刻的狀態(tài)，U(k)代表的是k時(shí)刻的控制量，W(k)則代表的是干擾量，其表現(xiàn)形式為高斯白噪聲，干擾量的協(xié)方差記作Q。M、N 代表系統(tǒng)的固有參數(shù)，當(dāng)系統(tǒng)是多模型系統(tǒng)時(shí)，M、N表現(xiàn)為多維向量的形式。Z(k)代表的是k時(shí)刻的測(cè)量值，V(k)代表的是測(cè)量時(shí)的干擾信號(hào)，其協(xié)方差記為R,P是測(cè)量系統(tǒng)的固有參數(shù)。

卡爾曼濾波過(guò)程：

首先根據(jù)系統(tǒng)在k-1時(shí)刻的狀態(tài)量X(k-1)估算其在下一時(shí)刻的狀態(tài)量x(k|k-1)，根據(jù)系統(tǒng)總體的運(yùn)行機(jī)制，有：

然后對(duì)x(k|k-1)的協(xié)方差進(jìn)行估算，S(k|k-1)表示X(k|k-1)的協(xié)方差，S(k-|k-1)則表示x(k-1|k-1)的協(xié)方差。

將測(cè)量值和估算值進(jìn)行融合，

x(k|k-1)表示系統(tǒng)在k時(shí)刻計(jì)算出的最優(yōu)狀態(tài)值。G(k)是卡爾曼增益，其意義是在指導(dǎo)數(shù)據(jù)融合時(shí)，應(yīng)該更加信任測(cè)量值，還是更加信任估算值.

那么G(k)如何計(jì)算？

不同于互補(bǔ)濾波，卡爾曼濾波中的參數(shù)不是固定的，其與系統(tǒng)固有參數(shù)和干擾量都有關(guān)。進(jìn)一步的，為保持系統(tǒng)的動(dòng)態(tài)實(shí)時(shí)更新，需要對(duì)x(k|k)的協(xié)方差進(jìn)行更新，即

三、光流導(dǎo)航系統(tǒng)

除了常見(jiàn)捷聯(lián)慣導(dǎo)系統(tǒng)，光流導(dǎo)航系統(tǒng)也是近年來(lái)應(yīng)用和研究較多的導(dǎo)航系統(tǒng)。光流導(dǎo)航系統(tǒng)是模仿生物的眼睛感受光的變化設(shè)計(jì)相關(guān)算法，將二維圖像中的像素通過(guò)數(shù)學(xué)建模與三維載體進(jìn)行關(guān)聯(lián)，通過(guò)光流圖像連續(xù)幀的變化，計(jì)算得到載體的運(yùn)動(dòng)信息。美國(guó)楊百翰大學(xué)的研究團(tuán)隊(duì)首次將光流導(dǎo)航技術(shù)應(yīng)用在無(wú)人機(jī)上，使用了光電鼠標(biāo)技術(shù)和激光測(cè)距。

在光流導(dǎo)航系統(tǒng)中，利用計(jì)算機(jī)視覺(jué)進(jìn)行導(dǎo)航時(shí)通常是利用兩幀連續(xù)圖片的特征點(diǎn)之間的關(guān)系建立變化坐標(biāo)，由此來(lái)判斷無(wú)人機(jī)的運(yùn)動(dòng)信息。進(jìn)行位置判斷的關(guān)鍵是解決圖像之間的特征點(diǎn)的提取以及匹配，并且由此建立起基于特征點(diǎn)的位置估計(jì)方程，設(shè)計(jì)出相應(yīng)的光流導(dǎo)航算法。

基于特征點(diǎn)算法可以獲取光流數(shù)據(jù)，但只是坐標(biāo)信息，需要進(jìn)行相應(yīng)標(biāo)定和轉(zhuǎn)化。

在光流傳感器相機(jī)標(biāo)定中，為獲取實(shí)際物體與圖像中物體的轉(zhuǎn)化關(guān)系，需要求出相機(jī)的內(nèi)參矩陣和外參矩陣，即用一定尺寸的相機(jī)，在不同的視角下對(duì)同一個(gè)物體進(jìn)行拍照，接著通過(guò)這些圖像求得相機(jī)的內(nèi)外參矩陣。內(nèi)參矩陣由相機(jī)本身決定，外參矩陣由拍照的不同角度決定。

通過(guò)內(nèi)外參矩陣，可以求出無(wú)人機(jī)上的光流傳感器與光流場(chǎng)之間的對(duì)應(yīng)關(guān)系，將光流信息轉(zhuǎn)換成無(wú)人機(jī)的速度信息。為了獲取無(wú)人機(jī)的運(yùn)動(dòng)信息，需要利用無(wú)人機(jī)的高度信息和載體坐標(biāo)和地面坐標(biāo)的轉(zhuǎn)換矩陣。

因?yàn)楫?dāng)光線(xiàn)條件不好時(shí)，僅利用光流傳感器將導(dǎo)致較大的誤差。因此，將光流導(dǎo)航系統(tǒng)經(jīng)常與慣性導(dǎo)航系統(tǒng)組合使用，可以互相彌補(bǔ)。光流/慣性松耦合模型是利用光流與慣導(dǎo)系統(tǒng)中的速度建立聯(lián)系，利用光流系統(tǒng)對(duì)慣導(dǎo)系統(tǒng)進(jìn)行數(shù)據(jù)修正，如下圖所示。

利用光流導(dǎo)航系統(tǒng)的速度與慣導(dǎo)系統(tǒng)的速度作差，得到速度測(cè)量方程

Zp是觀(guān)察矩陣，即慣導(dǎo)系統(tǒng)和光流導(dǎo)航系統(tǒng)提供的數(shù)據(jù)，Hp是測(cè)量轉(zhuǎn)移矩陣，Vp是測(cè)量噪聲矩陣，X 是t 時(shí)刻的狀態(tài)矢量。VE-nE，VN-nN 表示慣導(dǎo)系統(tǒng)與光流導(dǎo)航系統(tǒng)之間速度的差值，h-hn表示慣導(dǎo)系統(tǒng)與光流導(dǎo)航系統(tǒng)之間位置的差值。

導(dǎo)航技術(shù)以及MEMS技術(shù)等理論的進(jìn)步降低了無(wú)人機(jī)的生產(chǎn)、使用以及維護(hù)成本，使得無(wú)人機(jī)的應(yīng)用范圍逐漸擴(kuò)大，與此同時(shí)，無(wú)人機(jī)的安全問(wèn)題不可忽視，尤其是在復(fù)雜情況下更需要提高飛行的穩(wěn)定性，多傳感器融合和姿態(tài)解算具有重要的研究和應(yīng)用意義。

作者：凌霄

浙江大學(xué)機(jī)械電子專(zhuān)業(yè)博士，從事智能傳感與人機(jī)交互，智能機(jī)器人控制等領(lǐng)域的研究

責(zé)任編輯：lq