[發(fā)明專利]基于誤差狀態(tài)卡爾曼濾波的多旋翼飛行器組合導(dǎo)航方法

有效

• 申請?zhí)枺?/td>: 202010193890.8
• 申請日: 2020-03-19
• 公開（公告）號: CN111351482B
• 公開（公告）日: 2023-08-18
• 發(fā)明（設(shè)計）人: 張永;徐賢鵬;秦偉;劉自力;黃成;馬國梁
• 申請（專利權(quán)）人: 南京理工大學(xué)
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20;G01S19/45;G01S19/47
• 代理公司: 南京理工大學(xué)專利中心 32203
• 代理人: 薛云燕

本發(fā)明公開了一種基于誤差狀態(tài)卡爾曼濾波的多旋翼飛行器組合導(dǎo)航方法。該方法包括：首先，建立傳感器的測量模型，推導(dǎo)四元數(shù)形式的導(dǎo)航方程，并對該方程進(jìn)行線性化處理得到誤差狀態(tài)方程；然后，基于GPS、磁力計和加速度計的測量信息，建立用于卡爾曼濾波估計的測量方程；最后，結(jié)合慣性?GPS松組合模式，采用誤差狀態(tài)卡爾曼濾波器ESKF對誤差狀態(tài)進(jìn)行最優(yōu)估計，并實現(xiàn)對導(dǎo)航狀態(tài)的校正。本發(fā)明使用四元數(shù)時計算量較小，而且能夠消除慣性導(dǎo)航系統(tǒng)存在的累計誤差，實現(xiàn)高精度導(dǎo)航。

技術(shù)領(lǐng)域

本發(fā)明屬于導(dǎo)航技術(shù)領(lǐng)域，特別是一種基于誤差狀態(tài)卡爾曼濾波的多旋翼飛行器組合導(dǎo)航方法。

背景技術(shù)

導(dǎo)航系統(tǒng)及其配套子系統(tǒng)是自主無人機(jī)研究的重點。自主導(dǎo)航系統(tǒng)是利用各個子系統(tǒng)提供的信息來實現(xiàn)三個基本任務(wù)的系統(tǒng)，三個基本任務(wù)包括：1)定位：估計無人機(jī)的位置；2)障礙檢測和躲避：識別周圍障礙物并因此采取行動以躲避它們；3)發(fā)出控制指令：發(fā)送指令以使無人機(jī)穩(wěn)定姿態(tài)并遵循導(dǎo)航指令。

隨著SUAVs(小型無人機(jī))和MAVs(微型無人機(jī))的出現(xiàn)，比如Parrot公司的Ar.Drone?Parrot、大疆無人機(jī)公司的DJI?Phantom?series、Walkera公司的Voyager3、TALIH500和3D?Robotics公司的3DR?SOLO等，無人機(jī)的尺寸越來越小、重量越來越小，導(dǎo)致有效載荷變小，需要找到合適的板載傳感器來實現(xiàn)相應(yīng)的導(dǎo)航目的。

在大多數(shù)用于IMU姿態(tài)估計的卡爾曼濾波方法中，首先通過積分來自陀螺儀的角速率傳播姿態(tài)，以用作輸入信號，然后使用來自加速度計和磁力計的信息更新姿態(tài)。在靜態(tài)或低動力條件下，加速度計可測量地球的重力并提供橫滾和俯仰的信息，而磁力計主要是通過測量地球的磁場來提供偏航信息。由于除了地球赤道以外，磁場的傾斜角都不為零，因此磁力計還提供了一些關(guān)于俯仰和橫滾的信息。所以，可以將同時使用加速度計和磁力計測量的信息融合過程用作卡爾曼濾波方案中的測量更新過程。然而，由于測量噪聲的存在，IMU對位置的估計存在長期不穩(wěn)定性，借助GPS進(jìn)行輔助導(dǎo)航是一種較為實際的方案。

GPS/IMU組合導(dǎo)航系統(tǒng)是將來自IMU和GPS的信息融合在一起以補(bǔ)償兩個傳感器產(chǎn)生的誤差，提高定位過程的精度。因為GPS和IMU測量噪聲近似為高斯噪聲，所以線性卡爾曼濾波器能夠?qū)碜远鄠€天線GPS的數(shù)據(jù)與來自機(jī)載IMU的信息融合。為了減小位置誤差，文獻(xiàn)1(Barczyk?M,Lynch?AF.Integration?of?a?Triaxial?Magnetometer?into?aHelicopter?UAV?GPS-Aided?INS[J].IEEE?Transactions?on?Aerospace?and?ElectronicSystems,2012,48(4):2947-2960.)采用擴(kuò)展卡爾曼濾波器，文獻(xiàn)2(Yang?Yuhong,Zhou?J,Loffeld?O.Quaternion-based?Kalman?filtering?on?INS/GPS[C]Information?Fusion(FUSION),2012?15th?International?Conference.IEEE,2012:511-518.)分析了使用基于四元數(shù)的EKF和UKF算法的緊密耦合的IMU/GPS系統(tǒng)的系統(tǒng)性能。然而，這些方法都是直接法，即直接以導(dǎo)航參數(shù)作為估計對象，由于各子系統(tǒng)的誤差源和量測誤差都是隨機(jī)的，導(dǎo)致慣性導(dǎo)航系統(tǒng)存在大量累計誤差，降低了導(dǎo)航精度。

發(fā)明內(nèi)容

本發(fā)明的目的在于提供一種精度高、實時性好、計算量小的基于誤差狀態(tài)卡爾曼濾波的多旋翼飛行器組合導(dǎo)航方法。

實現(xiàn)本發(fā)明目的的技術(shù)解決方案為：一種基于誤差狀態(tài)卡爾曼濾波的多旋翼飛行器組合導(dǎo)航方法，包括以下步驟：

步驟1，建立傳感器的測量模型，推導(dǎo)四元數(shù)形式的導(dǎo)航方程，并對該方程進(jìn)行線性化處理得到誤差狀態(tài)方程；

步驟2，基于GPS、磁力計和加速度計的測量信息，建立用于卡爾曼濾波估計的測量方程；