一種位置傳感器和姿態(tài)傳感器的導(dǎo)航系統(tǒng)及其融合方法
【專利摘要】本發(fā)明公開了一種位置傳感器和姿態(tài)傳感器的導(dǎo)航系統(tǒng)及其融合方法，將互補(bǔ)濾波與卡爾曼濾波相結(jié)合，簡稱互補(bǔ)卡爾曼濾波，將經(jīng)過低通濾波后GPS/氣壓計(jì)測量的位置和速度信息，高通濾波后加速度估算的位置和速度信息作為卡爾曼濾波器的輸入，通過卡爾曼濾波自動(dòng)調(diào)節(jié)互補(bǔ)濾波的權(quán)值，有效的解決手動(dòng)調(diào)節(jié)互補(bǔ)濾波的權(quán)值的難題，實(shí)現(xiàn)將GPS位置信息與姿態(tài)傳感器的加速度信息融合輸出橫向位置與速度信息，將氣壓計(jì)高度信息與姿態(tài)傳感器輸出的z軸加速度進(jìn)行融合并輸出z軸位置和速度信息。融合后的輸出的位置和速度信息和融合前普通GPS和氣壓計(jì)輸出的位置信息相比更加精確，并且能提供較精確的速度信息。
【專利說明】
一種位置傳感器和姿態(tài)傳感器的導(dǎo)航系統(tǒng)及其融合方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及一種位置傳感器和姿態(tài)傳感器的導(dǎo)航系統(tǒng)及其融合方法，屬于導(dǎo)航定 位與控制技術(shù)領(lǐng)域。
【背景技術(shù)】
[0002] 利用GPS定位衛(wèi)星，在全球范圍內(nèi)實(shí)時(shí)進(jìn)行定位、導(dǎo)航的系統(tǒng)，稱為全球衛(wèi)星定位 系統(tǒng)，簡稱GPS。姿態(tài)傳感器是以陀螺和加速度計(jì)為敏感器件的導(dǎo)航參數(shù)解算系統(tǒng)，該系統(tǒng) 根據(jù)陀螺的輸出建立導(dǎo)航坐標(biāo)系，輸出運(yùn)載體在導(dǎo)航坐標(biāo)系中的加速度和歐拉角。氣壓計(jì) 根據(jù)托里拆利的實(shí)驗(yàn)原理而制成，用以測量大氣壓強(qiáng)的儀器，再通過對大氣壓強(qiáng)與溫度的 計(jì)算得到高度，輸出z軸高度信息。
[0003] 目前GPS可提供導(dǎo)航信息，但是由于較高精度的GPS價(jià)格偏貴，成本太高。并且氣壓 計(jì)高度輸出不穩(wěn)，讀數(shù)在20cm內(nèi)浮動(dòng)，不利于定位。本發(fā)明使用成本較低的GPS、航姿傳感器 及氣壓計(jì)，將三者信息融合后輸出運(yùn)載體(比如飛行器)在地球坐標(biāo)系的位置、速度、高度信 息。

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

[0004] 本發(fā)明所要解決的技術(shù)問題是提供一種位置傳感器和姿態(tài)傳感器的導(dǎo)航系統(tǒng)及 其融合方法，該導(dǎo)航系統(tǒng)將GPS位置信息與姿態(tài)傳感器的加速度信息融合后輸出運(yùn)載體的 橫向位置與速度信息。同理，將氣壓計(jì)高度信息與姿態(tài)傳感器輸出的z軸加速度進(jìn)行融合后 輸出運(yùn)載體的z軸位置和速度信息。融合后的輸出的位置、速度信息和融合前普通GPS和氣 壓計(jì)輸出的位置信息相比更加精確，并且能提供較精確的速度信息。
[0005] 本發(fā)明為解決上述技術(shù)問題采用以下技術(shù)方案：
[0006] -方面，本發(fā)明提供一種位置傳感器和姿態(tài)傳感器信息融合方法，將互補(bǔ)濾波與 卡爾曼濾波相結(jié)合，將經(jīng)過低通濾波后位置傳感器測量的位置和速度信息、高通濾波后加 速度估算的位置和速度信息作為卡爾曼濾波器的輸入，位置傳感器測量的位置信息和姿態(tài) 傳感器測量的加速度信息經(jīng)過互補(bǔ)濾波與卡爾曼濾波后進(jìn)行融合，得到融合后的位置信息 和速度信息：
[0007] 1)融合后的速度信息通過以下方程計(jì)算得到：
[0012]式中，v(k| k-Ι)為根據(jù)加速度信息計(jì)算得到的k時(shí)刻速度信息，v(k-l |k-l)為k-1 時(shí)刻融合后的速度信息，ag(k)為k時(shí)刻姿態(tài)傳感器測量的加速度信息，dt為k-1時(shí)刻和k時(shí) 亥IJ之間的時(shí)間間隔，PKklk-1)為k時(shí)刻根據(jù)加速度信息計(jì)算得到的速度信息的協(xié)方差，Pi (k_l | k_l)為k_l時(shí)刻根據(jù)加速度fg息計(jì)算得到的速度fg息的協(xié)方差，Q為加速度fg息的協(xié) 方差，κκιο為第一卡爾曼濾波增益，為k時(shí)刻位置傳感器測量的速度信息，心為 Vbrfore (k)的協(xié)方差;V (k I k)為k時(shí)刻融合后的速度信息；
[0013] 2)融合后的位置信息通過以下方程計(jì)算得到：
[0018] 式中，L(k|k-1)為根據(jù)加速度信息計(jì)算得到的k時(shí)刻位置信息，L(k-l|k-l)為k-1 時(shí)刻融合后的位置信息，v af ter (k-1)為k-1時(shí)刻融合后的速度信息，P2 (k I k-1)為k時(shí)刻根據(jù) 加速度息計(jì)算得到的位置息的協(xié)方差，P2(k_l | k_l)為k_l時(shí)刻根據(jù)加速度fg息計(jì)算得 到的位置信息的協(xié)方差，K2(k)為第二卡爾曼濾波增益，L befcire(k)為k時(shí)刻位置傳感器測量 的位置信息，R2為Lbef ore (k)的協(xié)方差;L (k I k)為k時(shí)刻融合后的位置信息。
[0019] 另一方面，本發(fā)明提供一種位置傳感器和姿態(tài)傳感器的導(dǎo)航系統(tǒng)，包括CPU以及分 別與其相連的姿態(tài)傳感器、氣壓計(jì)、GPS，其中，姿態(tài)傳感器用于測量運(yùn)載體的加速度信息， 氣壓計(jì)用于測量運(yùn)載體的高度信息，GPS用于測量運(yùn)載體的位置信息；CPU用于采用上述位 置傳感器和姿態(tài)傳感器信息融合方法，分別對氣壓計(jì)測量的高度信息和姿態(tài)傳感器測量的 z軸加速度信息、GPS測量的X軸位置信息和姿態(tài)傳感器測量的X軸加速度信息進(jìn)行融合，GPS 測量的y軸位置信息和姿態(tài)傳感器測量的y軸加速度信息進(jìn)行融合，輸出運(yùn)載體的x、y、z軸 方向的位置信息與速度信息。
[0020] 作為本發(fā)明的進(jìn)一步優(yōu)化方案，若GPS測量的運(yùn)載體位置信息為經(jīng)煒度信息，則根 據(jù)以下公式轉(zhuǎn)化為地面坐標(biāo)系下水平位置信息：
[0023]式中，pn、pe分別為運(yùn)載體當(dāng)前X軸位移量、y軸位移量，（lati，longi)為運(yùn)載體當(dāng)前 經(jīng)煒度，（lato，long。)為運(yùn)載體起飛點(diǎn)的經(jīng)煒度，R為地球半徑。
[0024]作為本發(fā)明的進(jìn)一步優(yōu)化方案，若GPS的測量頻率小于姿態(tài)傳感器的測量頻率，則 通過前一時(shí)刻融合得到的速度信息預(yù)估當(dāng)前位置信息，對GPS測量的運(yùn)載體位置信息進(jìn)行 插值處理，增加其頻率。
[0025]作為本發(fā)明的進(jìn)一步優(yōu)化方案，CPU的型號為STM32F103。
[0026] 本發(fā)明采用以上技術(shù)方案與現(xiàn)有技術(shù)相比，具有以下技術(shù)效果：
[0027] 1、將氣壓計(jì)高度與航姿傳感器的z軸加速度信息融合，提高氣壓計(jì)高度信息精度， 輸出z軸高度信息和速度信息；
[0028] 2、將GPS位置信息與航姿傳感器的x、y軸加速度信息融合，得到大地坐標(biāo)系中相對 于home點(diǎn)(起飛點(diǎn)）的位置信息與速度信息；
[0029] 3、采用STM32F103處理器，編程簡單，易于入門；且傳感器成本低，體積小，系統(tǒng)方 便攜帶。
【附圖說明】
[0030] 圖1為本發(fā)明硬件結(jié)構(gòu)框圖。
[0031] 圖2為本發(fā)明速度融合框圖。
[0032]圖3為本發(fā)明位置融合框圖。
[0033] 圖4為本發(fā)明地面坐標(biāo)系X軸位置融合前后對比圖。
[0034] 圖5為本發(fā)明地面坐標(biāo)系y軸位置融合前后對比圖。
[0035] 圖6為本發(fā)明高度融合前后對比以及融合后的速度對比圖。
【具體實(shí)施方式】
[0036]下面結(jié)合附圖對本發(fā)明的技術(shù)方案做進(jìn)一步的詳細(xì)說明：
[0037]本發(fā)明提供一種位置傳感器和姿態(tài)傳感器的導(dǎo)航系統(tǒng)，如圖1所示，包括CPU以及 分別與其相連的姿態(tài)傳感器、氣壓計(jì)、GPS，其中，姿態(tài)傳感器用于測量運(yùn)載體的加速度信 息，氣壓計(jì)用于測量運(yùn)載體的高度信息，GPS用于測量運(yùn)載體的位置信息;CPU用于采用一種 位置傳感器和姿態(tài)傳感器信息融合方法，將姿態(tài)傳感器測量的加速度信息分別與氣壓計(jì)測 量的高度信息、GPS測量的位置信息進(jìn)行融合，得到融合后的地面坐標(biāo)系下x、y、z軸方向的 速度信息和位置信息。
[0038] 本發(fā)明中，CPU采用STM32F103模塊，STM32F103屬于中低端的32位ARM微控制器，是 基于1.25DMIPS的Cortex-M3架構(gòu)的32位單片機(jī)，時(shí)鐘頻率高達(dá)72MHZ，其豐富的硬件接口資 源(3個(gè)USART，2個(gè)I2C，2個(gè)SPI，1個(gè)CAN等等)及功能強(qiáng)大的DMA控制方式。氣壓計(jì)通過I2C與 CHJ相連，姿態(tài)傳感器和GPS通過串口與CPU相連。，
[0039] 姿態(tài)傳感器由三軸加速度計(jì)三軸陀螺、三軸加速度計(jì)、三軸磁場傳感器組成，用于 測量載體三個(gè)方向的的絕對角速率、加速度以及磁場強(qiáng)度，并采用特定姿態(tài)解算方法和卡 爾曼濾波信息融合得到載體的四元數(shù)、姿態(tài)數(shù)據(jù)等。解算方法在本文中不做贅述。本發(fā)明 中，選用美國Invensense公司生產(chǎn)的六軸姿態(tài)傳感器MPU-6000,其整合了3軸陀螺儀、3軸加 速器，為全球首例整合性6軸運(yùn)動(dòng)處理組件。相較于多組件方案，MPU-6000免除了組合陀螺 儀與加速器時(shí)之軸間差的問題，減少了大量的包裝空間，具有低功耗、低成本、高性能的特 點(diǎn)。傳感器的測量數(shù)據(jù)最終可通過最高400kHz的I2C總線或最高20MHz的SPI總線輸出。三軸 磁場傳感器采用的是Honeywell公司的HMC5883L。該傳感器能在±8高斯的磁場中實(shí)現(xiàn)5毫 高斯分辨率，內(nèi)置自檢功能，能讓羅盤航向精度精確到1°-2°，采用霍尼韋爾各向異性磁阻 (AMR)技術(shù)，具有在軸向高靈敏度和線性高精度的特點(diǎn)。
[0040] 氣壓計(jì)采用由MEAS推出的新一代高分辨率氣壓傳感器MS5611，該傳感器用來測量 多旋翼飛行器絕對飛行高度。該模塊包含了一個(gè)高線性度的壓力傳感器和一個(gè)超低功耗的 24位模數(shù)轉(zhuǎn)換器，提供了一個(gè)精確的24位數(shù)字壓力值和溫度值以及不同的操作模式，可以 提高轉(zhuǎn)換速度并優(yōu)化電流消耗。高分辨率的溫度輸出無須額外傳感器可實(shí)現(xiàn)高度計(jì)/溫度 計(jì)功能。工作溫度范圍：-40_85°C，精確度:在飛行高度750m時(shí)，偏差-1.5m~+1.5m。
[0041 ] GPS模塊采用ATK-NE0-6M，模塊采用υ-BLOX NE0-6M模組，體積小巧，性能優(yōu)異，模 塊自帶高性能無源陶瓷天線(無需再購買昂貴的有源天線了），并自帶可充電后備電池（以 支持溫啟動(dòng)或熱啟動(dòng)，后備電池在主電源斷電后，可以維持半小時(shí)左右的GPS接收數(shù)據(jù)保 存）。模塊通過串口與外部系統(tǒng)連接，串口波特率支持4800、9600、38400(默認(rèn)）、57600等不 同速率。
[0042] 本發(fā)明中還提供一種位置傳感器和姿態(tài)傳感器信息融合方法，下面具體論述其方 法及原理：
[0043] 位置傳感器測量的位置信息，其測量誤差不隨時(shí)間累加，但在短期內(nèi)精度較差;而 加速度計(jì)積分產(chǎn)生的位置，由于漂移等問題的存在，一段時(shí)間后精度下降甚至發(fā)散，但在短 時(shí)間內(nèi)可提供精度較高的位置數(shù)據(jù)。利用兩者互補(bǔ)的特性，采用互補(bǔ)濾波器對二者進(jìn)行數(shù) 據(jù)融合，其互補(bǔ)濾波原理如圖2和3所示。
[0044]其中，互補(bǔ)濾波融合后的速度信息1^^(1〇具體為：
[0048]從而得到互補(bǔ)濾波融合后的位置信息為：
[0050] 式中，Lbefcire(k)為k時(shí)刻位置傳感器測量的位置信息，Lafter(k)為k時(shí)刻融合后CPU 輸出的位置信息，Lafter(k-1)為k-1時(shí)亥ijCPU融合輸出的位置信息，vafter(k-l)為k-1時(shí)刻CPU 融合后輸出的速度信息，ag(k)為k時(shí)刻姿態(tài)傳感器測量的加速度信息，dt為k-1時(shí)刻和k時(shí) 刻之間的時(shí)間間隔（即數(shù)據(jù)融合算法兩次運(yùn)行之間的時(shí)間間隔），比如本文中數(shù)據(jù)融合算法
特性，本發(fā)明中系統(tǒng)為離散系統(tǒng)，故采用z變換進(jìn)行濾波。
[0051] 互補(bǔ)濾波對姿態(tài)傳感器的加速度信息的高通作用，加速度計(jì)的低頻漂移可以得到 很好地抑制；低通作用可以有效抑制位置傳感器測量的位置信息的噪音。選取合適的(： 2值 可以使互補(bǔ)濾波器獲得合適的截止頻率
P在大于fr的高頻段，姿態(tài)傳感 器的加速度信息對結(jié)算結(jié)果起主要作用；在小于的低頻段，位置傳感器測量的位置信息 對結(jié)算結(jié)果起主要作用。
[0052]采用同樣的互補(bǔ)濾波方式對運(yùn)載體的速度信息進(jìn)行融合，具體為：
[0054]其中，Vg(k) =Vafter(k_l )+ag(k)*dt
[0055]從而得到互補(bǔ)濾波融合后的速度信息為：
[0057] 式中，VbeforeU)為k時(shí)刻位置傳感器測量的速度信息，Vafter(k)為k時(shí)刻融合后CPU 輸出的速度信息，vafto(k-l)為k-1時(shí)刻融合后CPU輸出的速度信息，ag(k)為k時(shí)刻姿態(tài)傳感 器測里的加速度?曰息。Vafter (0 ) = Vbefore (0 ) 〇
[0058]而在z軸方向，由于氣壓計(jì)不能直接輸出速度信息，先將氣壓計(jì)高度信息進(jìn)行微分 處理得到，本發(fā)明使用下面公式計(jì)算速度
[0060] 其中，Vbefore(0)、Vbefor e(l)、Vbefore(2)為0,由于程序運(yùn)行頻率高，且程序運(yùn)行時(shí)有 初始化時(shí)間，前三幀數(shù)據(jù)為〇，不影響融合結(jié)果。
[0061] 加速度計(jì)易受振動(dòng)影響，選取合適的(^、(：2值，獲得較好的靜態(tài)、動(dòng)態(tài)位置信息，較 為困難，給調(diào)試工作帶來很大的難題，因此本發(fā)明將互補(bǔ)濾波與卡爾曼濾波相結(jié)合，簡稱互 補(bǔ)卡爾曼濾波，將經(jīng)過低通濾波后gps/氣壓計(jì)測量的位置和速度信息，高通濾波后加速度 估算的位置和速度信息作為卡爾曼濾波器的輸入，通過卡爾曼濾波自動(dòng)調(diào)節(jié)Ci、C 2的值，有 效的解決手動(dòng)調(diào)節(jié)值的難題。
[0062]本發(fā)明中，將卡爾曼濾波應(yīng)用于離散線性時(shí)變系統(tǒng)，線性隨機(jī)微分方程可描述為
[0063] x(k) =Ax(k~l )+Bu(k)+w(k)
[0064] y(k)=Cx(k)+v(k)
[0065] 式中，x(k)是k時(shí)刻的系統(tǒng)狀態(tài)，x(k-l)是上一狀態(tài)最優(yōu)的結(jié)果，u(k)是k時(shí)刻對系 統(tǒng)的控制量，y(k)是k時(shí)刻的測量值，w(k)和v(k)分別表示系統(tǒng)過程中和測量的噪音，它們 的coveriance(協(xié)方差)分別用Q與R表示(假設(shè)不隨系統(tǒng)狀態(tài)而改變)A、B、C分別表示系統(tǒng)狀 態(tài)參數(shù)、系統(tǒng)輸入控制參數(shù)和測量系統(tǒng)的參數(shù)。
[0066] 用系統(tǒng)的過程模型預(yù)測下一狀態(tài)。當(dāng)前k時(shí)刻，可以根據(jù)上一狀態(tài)預(yù)測出現(xiàn)在的狀 態(tài)：
[0067] x(k | k~l) =Ax(k~l | k~l )+Bu(k)+w(k)
[0068] 其中，x(k I k-1)是利用上一狀態(tài)預(yù)測的結(jié)果，x(k_l I k-1)是上一狀態(tài)最優(yōu)的結(jié)果， x(k | k_l)的coveriance用P表示：
[0069] P(k|k-l)=AP(k-l |k-l)AT+Q
[0070] 式中P(k I k-1)是x(k_l I k-1)時(shí)刻的coveriance。結(jié)合預(yù)測值和測量，可以得到現(xiàn) 在狀態(tài)k時(shí)刻的最優(yōu)化估計(jì)值x(k | k):
[0071 ] x(k|k)=x(k| k_l )+Kg(k) (y(k)-Cx(k | k_l))
[0073] Kg(k)是k_l時(shí)刻的卡爾曼增益，k時(shí)刻x(k | k)的coveriance:
[0074] P(k|k) = (I-Kg(k)C)P(k|k-l)
[0075] 式中I為單位矩陣。本發(fā)明中，采用一階系統(tǒng)，u(k)為0，1 = 1，故上式可簡化為
[0076] P(k|k-l)=P(k-l |k-l)+Q
[0078] x(k|k)=x(k| k_l )+Kg(k) (y(k)-x(k|k_l))
[0079] P(k|k) = (l-Kg(k))P(k|k-l)
[0080] 本發(fā)明將GPS/氣壓計(jì)測量的數(shù)據(jù)低通濾波后的位置信息作為系統(tǒng)的測量值，高通 濾波后加速度估算的信息作為系統(tǒng)的預(yù)估值，在控制系統(tǒng)中采用融合后的最優(yōu)結(jié)果。這種 組合的主要優(yōu)點(diǎn)是實(shí)現(xiàn)簡單、可以根據(jù)實(shí)際情況估算出可獲得較好的位置和速度信 息。
[0081] 本發(fā)明的系統(tǒng)是一階離散線性時(shí)變系統(tǒng)，結(jié)合互補(bǔ)濾波與卡爾曼濾波原理，因此 得到融合后的位置信息和速度信息，具體為：
[0082] 1)融合后的速度信息通過以下方程計(jì)算得到：
[0083] v(k | k~l) =v(k-l | k~l )+ag(k)*dt
[0084] Pi(k|k-l)=Pi(k-l|k-l)+Q
[0087] 式中，v(k|k-l)為根據(jù)加速度信息計(jì)算得到的k時(shí)刻速度信息，v(k-l|k-l)為k-1 時(shí)刻融合后的速度信息，a g(k)為k時(shí)刻姿態(tài)傳感器測量的加速度信息，dt為k-1時(shí)刻和k時(shí) 亥IJ之間的時(shí)間間隔，PKk| k-Ι)為k時(shí)刻根據(jù)加速度信息計(jì)算得到的速度信息的協(xié)方差，Pi (k_l | k_l)為k_l時(shí)刻根據(jù)加速度fg息計(jì)算得到的速度fg息的協(xié)方差，Q為加速度fg息的協(xié) 方差，κκιο為第一卡爾曼濾波增益，為k時(shí)刻位置傳感器測量的速度信息，心為 Vbrfore (k)的協(xié)方差;V (k I k)為k時(shí)刻融合后的速度信息；
[0088] 2)融合后的位置信息通過以下方程計(jì)算得到：
[0093]式中，L(k|k-1)為根據(jù)加速度信息計(jì)算得到的k時(shí)刻位置信息，L(k-l|k-l)為k-1 時(shí)刻融合后的位置信息，vafter (k-1)為k-1時(shí)刻融合后的速度信息，P2 (k I k-1)為k時(shí)刻根據(jù) 加速度息計(jì)算得到的位置息的協(xié)方差，P2(k_l | k_l)為k_l時(shí)刻根據(jù)加速度fg息計(jì)算得 到的位置信息的協(xié)方差，K2(k)為第二卡爾曼濾波增益，L befcire(k)為k時(shí)刻位置傳感器測量 的位置信息，R2為Lbef ore (k)的協(xié)方差;L (k I k)為k時(shí)刻融合后的位置信息。
[0094]由于GPS模塊采集一般是運(yùn)載體當(dāng)前的經(jīng)煒度信息，那么一般需要轉(zhuǎn)化為地面坐 標(biāo)系下水平位置信息。假設(shè)地球是一個(gè)球體，不同的煒度對應(yīng)不同的曲率半徑，當(dāng)運(yùn)載體處 在p點(diǎn)時(shí)，曲率半徑(R '）近似計(jì)算公式為：
，其中，角度a為當(dāng)前的煒度，R 為地球半徑。若p點(diǎn)經(jīng)煒度為（lati，longi)，運(yùn)載體的起飛點(diǎn)設(shè)為home點(diǎn)，home點(diǎn)的經(jīng)煒度為 (latojongo)，將運(yùn)載體當(dāng)前位置近似轉(zhuǎn)化到以home點(diǎn)為原點(diǎn)的二維平面坐標(biāo)系中，轉(zhuǎn)換公 式如下：
[0097] 式中，pn、pe分別為運(yùn)載體當(dāng)前X軸位移量、y軸位移量。
[0098] 由于GPS的頻率一般較低，則通過前一時(shí)刻融合得到的速度信息預(yù)估當(dāng)前位置信 息，將預(yù)估位置信息插入到GPS測量的位置信息中，以增加 GPS測量的位置信息的頻率。本發(fā) 明中，對GPS信息進(jìn)行插值處理，插值處理每10hz運(yùn)行一次，GPS讀取程序4hz-次，輸出相對 于起點(diǎn)的相對位置，在插值處理時(shí)，會比較k時(shí)刻GPS測量得到的x、y軸位置信息L x(k)、Ly(k) 是否與k-IGPS測量得到的x、y軸位置信息Lx(k-l)、L y(k-l)-樣，如果不一樣，則表示GPS數(shù) 據(jù)有更新，則不進(jìn)行插值，若不一樣，則表示GPS未更新，通過前一時(shí)刻融合得到的速度信息 預(yù)估當(dāng)前位置信息。
[0099] 圖4為本發(fā)明地面坐標(biāo)系X軸位置融合前后對比圖，圖5為本發(fā)明地面坐標(biāo)系y軸位 置融合前后對比圖，圖6為本發(fā)明高度融合前后對比以及融合后的速度對比圖，其中，超聲 波高度是用來做比較的，因?yàn)槌暡ǖ母叨仍诟叨缺容^小的時(shí)候比較準(zhǔn)確。從圖中可以看 出本發(fā)明可以有效抑制波動(dòng)。
[0100] 以上所述，僅為本發(fā)明中的【具體實(shí)施方式】，但本發(fā)明的保護(hù)范圍并不局限于此，任 何熟悉該技術(shù)的人在本發(fā)明所揭露的技術(shù)范圍內(nèi)，可理解想到的變換或替換，都應(yīng)涵蓋在 本發(fā)明的包含范圍之內(nèi)，因此，本發(fā)明的保護(hù)范圍應(yīng)該以權(quán)利要求書的保護(hù)范圍為準(zhǔn)。
【主權(quán)項(xiàng)】
1. 一種位置傳感器和姿態(tài)傳感器信息融合方法，其特征在于，將互補(bǔ)濾波與卡爾曼濾 波相結(jié)合，將經(jīng)過低通濾波后位置傳感器測量的位置和速度信息、高通濾波后加速度估算 的位置和速度信息作為卡爾曼濾波器的輸入，位置傳感器測量的位置信息和姿態(tài)傳感器測 量的加速度信息經(jīng)過互補(bǔ)濾波與卡爾曼濾波后進(jìn)行融合，得到融合后的位置信息和速度信 息： 1) 融合后的速度信息通過以下方程計(jì)算得到： v(k|k-l)=v(k-l| k-1 )+ag(k)*dt Pi(k I k_l) = Pi(k_l I k_l )+Q式中，v(k|k-l)為根據(jù)加速度信息計(jì)算得到的k時(shí)刻速度信息，v(k-l |k-l)為k-1時(shí)刻 融合后的速度信息，ag(k)為k時(shí)刻姿態(tài)傳感器測量的加速度信息，dt為k-1時(shí)刻和k時(shí)刻之 間的時(shí)間間隔JKklk-l)為k時(shí)刻根據(jù)加速度信息計(jì)算得到的速度信息的協(xié)方差，PKk-ll k_l)為k_l時(shí)刻根據(jù)加速度fg息計(jì)算得到的速度fg息的協(xié)方差，Q為加速度fg息的協(xié)方差， Ki(k)為第一卡爾曼濾波增益，Vbefcire(k)為k時(shí)刻位置傳感器測量的速度信息，foSvbrforJk) 的協(xié)方差;v(k|k)為k時(shí)刻融合后的速度信息； 2) 融合后的位置信息通過以下方程計(jì)算得到：式中，L(k|k-1)為根據(jù)加速度信息計(jì)算得到的k時(shí)刻位置信息，L(k-llk-l)為k-Ι時(shí)刻 融合后的位置信息，vafter(k-l)為k-Ι時(shí)刻融合后的速度信息，P2(k|k-1)為k時(shí)刻根據(jù)加速 度ig息計(jì)算得到的位置息的協(xié)方差，P2(k_l | k_l)為k_l時(shí)刻根據(jù)加速度fg息計(jì)算得到的 位置信息的協(xié)方差，K2(k)為第二卡爾曼濾波增益，L befcire(k)為k時(shí)刻位置傳感器測量的位 置信息，R2為LbrfOre (k)的協(xié)方差;L( k | k)為k時(shí)刻融合后的位置信息。2. -種位置傳感器和姿態(tài)傳感器的導(dǎo)航系統(tǒng)，其特征在于，包括CPU以及分別與其相連 的姿態(tài)傳感器、氣壓計(jì)、GPS，其中，姿態(tài)傳感器用于測量運(yùn)載體的加速度信息，氣壓計(jì)用于 測量運(yùn)載體的高度信息，GPS用于測量運(yùn)載體的位置信息;CPU用于采用如權(quán)利要求1所述的 融合算法，分別對氣壓計(jì)測量的高度信息和姿態(tài)傳感器測量的z軸加速度信息、GPS測量的X 軸位置信息和姿態(tài)傳感器測量的X軸加速度信息進(jìn)行融合，GPS測量的y軸位置信息和姿態(tài) 傳感器測量的y軸加速度信息進(jìn)行融合，輸出運(yùn)載體的x、y、z軸方向的位置信息與速度信 息。3. 根據(jù)權(quán)利要求2所述的一種位置傳感器和姿態(tài)傳感器的導(dǎo)航系統(tǒng)，其特征在于，若 GPS測量的運(yùn)載體位置信息為經(jīng)煒度信息，則根據(jù)以下公式轉(zhuǎn)化為地面坐標(biāo)系下水平位置 信息：式中，PN、pE分別為運(yùn)載體當(dāng)前X軸位移量、y軸位移量，（latijongi)為運(yùn)載體當(dāng)前經(jīng)煒 度，（lato，longo)為運(yùn)載體起飛點(diǎn)的經(jīng)煒度，R為地球半徑。4. 根據(jù)權(quán)利要求2所述的一種位置傳感器和姿態(tài)傳感器的導(dǎo)航系統(tǒng)，其特征在于，若 GPS的測量頻率小于姿態(tài)傳感器的測量頻率，則通過前一時(shí)刻融合得到的速度信息預(yù)估當(dāng) 前位置信息，對GPS測量的運(yùn)載體位置信息進(jìn)行插值處理，增加其頻率。5. 根據(jù)權(quán)利要求2所述的一種位置傳感器和姿態(tài)傳感器的導(dǎo)航系統(tǒng)，其特征在于，CPU 的型號為STM32F103。
【文檔編號】G01C21/20GK105865453SQ201610340135
【公開日】2016年8月17日
【申請日】2016年5月20日
【發(fā)明人】劉翎予, 王新華, 李騰
【申請人】南京航空航天大學(xué)