[發(fā)明專利]一種姿態(tài)角的融合解算方法和裝置

有效

• 申請(qǐng)?zhí)枺?/td>: 201810060960.5
• 申請(qǐng)日: 2018-01-22
• 公開(kāi)（公告）號(hào): CN108398128B
• 公開(kāi)（公告）日: 2021-08-24
• 發(fā)明（設(shè)計(jì)）人: 時(shí)廣軼;謝偰偉;國(guó)林旗
• 申請(qǐng)（專利權(quán)）人: 北京大學(xué)深圳研究生院
• 主分類號(hào): G01C21/20
• 分類號(hào): G01C21/20
• 代理公司: 深圳鼎合誠(chéng)知識(shí)產(chǎn)權(quán)代理有限公司 44281
• 代理人: 郭燕

一種姿態(tài)角的融合解算方法和裝置，姿態(tài)角的融合解算方法包括：根據(jù)陀螺儀的測(cè)量數(shù)據(jù)解算出第一姿態(tài)角；根據(jù)加速度計(jì)的測(cè)量數(shù)據(jù)和磁力計(jì)的測(cè)量數(shù)據(jù)解算出第二姿態(tài)角；根據(jù)互補(bǔ)濾波原理對(duì)第一姿態(tài)角和第二姿態(tài)角進(jìn)行融合計(jì)算，得到第三姿態(tài)角；以第三姿態(tài)角為第一狀態(tài)變量且互補(bǔ)濾波后獲得的第二姿態(tài)角為觀測(cè)量，利用擴(kuò)展卡爾曼濾波計(jì)算得到融合姿態(tài)角。由于將互補(bǔ)濾波和卡爾曼濾波相結(jié)合來(lái)解算姿態(tài)角，彌補(bǔ)了互補(bǔ)濾波和卡爾曼濾波各自的缺陷，既能夠提高動(dòng)態(tài)條件下姿態(tài)角的跟隨性，又能夠達(dá)到較快的收斂速度。

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

本發(fā)明涉及姿態(tài)角測(cè)量技術(shù)領(lǐng)域，具體涉及一種姿態(tài)角的融合解算方法和裝置。

背景技術(shù)

姿態(tài)角主要是指物體的俯仰角、偏航角和翻滾角，可用來(lái)表征物體的姿態(tài)，姿態(tài)角度的測(cè)量在控制、機(jī)械、通信、航空航天等技術(shù)領(lǐng)域得到了廣泛的應(yīng)用，其主要利用慣性測(cè)量單元(Inertialmeasurementunit，IMU)進(jìn)行測(cè)量。一般而言，IMU包含加速度計(jì)和陀螺儀，加速度計(jì)用于檢測(cè)物體在載體坐標(biāo)系中獨(dú)立三軸的加速度信號(hào)，而陀螺儀用于檢測(cè)物體相對(duì)于地理坐標(biāo)系的角速度信號(hào)，通過(guò)測(cè)量物體在三維空間中的角速度和加速度，并以此解算出物體的姿態(tài)。

在具體利用IMU解算姿態(tài)角信息時(shí)，陀螺儀自身的工作原理決定了其長(zhǎng)時(shí)間的積分會(huì)導(dǎo)致誤差的累積，使測(cè)量值隨時(shí)間而漂移，導(dǎo)致姿態(tài)角解算的精度降低，因而需要利用外部信息對(duì)陀螺儀的漂移進(jìn)行修正。磁力計(jì)是一種直接感測(cè)地磁場(chǎng)信息的傳感器，其沒(méi)有累積誤差，工作較為穩(wěn)定，因而可用來(lái)修正陀螺儀的漂移。目前，在進(jìn)行姿態(tài)角解算時(shí)，主要利用的是互補(bǔ)濾波或者卡爾曼濾波，但利用互補(bǔ)濾波解算出的姿態(tài)角的跟隨性較差，而通過(guò)卡爾曼濾波器解算姿態(tài)角時(shí)的收斂性又較慢，在動(dòng)態(tài)條件下采用這兩種方法進(jìn)行姿態(tài)角的解算時(shí)都不利于姿態(tài)角的更新。因此，針對(duì)IMU和磁力計(jì)構(gòu)成的姿態(tài)角測(cè)量單元，現(xiàn)有的姿態(tài)角解算方法還無(wú)法實(shí)現(xiàn)在提高動(dòng)態(tài)條件下姿態(tài)的跟隨性的同時(shí)，又能使姿態(tài)角達(dá)到較快的收斂速度。

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

本申請(qǐng)?zhí)峁┮环N姿態(tài)角的融合解算方法和裝置，以實(shí)現(xiàn)在姿態(tài)角的解算過(guò)程中，既提高動(dòng)態(tài)條件下姿態(tài)的跟隨性，又使姿態(tài)角達(dá)到較快的收斂速度。

根據(jù)第一方面，一種實(shí)施例中提供一種姿態(tài)角的融合解算方法，包括：

根據(jù)陀螺儀的測(cè)量數(shù)據(jù)解算出第一姿態(tài)角；

根據(jù)加速度計(jì)的測(cè)量數(shù)據(jù)和磁力計(jì)的測(cè)量數(shù)據(jù)解算出第二姿態(tài)角；

根據(jù)互補(bǔ)濾波原理對(duì)所述第一姿態(tài)角和所述第二姿態(tài)角進(jìn)行融合計(jì)算，得到第三姿態(tài)角；

以所述第三姿態(tài)角為第一狀態(tài)變量且互補(bǔ)濾波后獲得的所述第二姿態(tài)角為觀測(cè)量，利用擴(kuò)展卡爾曼濾波計(jì)算得到融合姿態(tài)角。

根據(jù)第二方面，一種實(shí)施例中提供一種姿態(tài)角融合解算的裝置，包括：

陀螺儀，用于檢測(cè)物體相對(duì)于地理坐標(biāo)系的角速度信號(hào)并輸出；

加速度計(jì)，用于檢測(cè)物體在載體坐標(biāo)系中三個(gè)軸的加速度信號(hào)并輸出；

磁力計(jì)，用于檢測(cè)地磁場(chǎng)的強(qiáng)度和方向信息并輸出；

數(shù)據(jù)處理器，用于執(zhí)行如上述方案所述的姿態(tài)角的融合解算方法。

依據(jù)上述實(shí)施例的姿態(tài)角的融合解算方法和裝置，首先根據(jù)陀螺儀的測(cè)量數(shù)據(jù)解算出第一姿態(tài)角，且根據(jù)加速度計(jì)的測(cè)量數(shù)據(jù)和磁力計(jì)的測(cè)量數(shù)據(jù)解算出第二姿態(tài)角；接著根據(jù)互補(bǔ)濾波原理對(duì)第一姿態(tài)角和第二姿態(tài)角進(jìn)行融合計(jì)算，得到第三姿態(tài)角；然后以第三姿態(tài)角為狀態(tài)變量且互補(bǔ)濾波后獲得的第二姿態(tài)角為觀測(cè)量，利用擴(kuò)展卡爾曼濾波計(jì)算得到融合姿態(tài)角，從而將互補(bǔ)濾波和卡爾曼濾波相結(jié)合，融合解算出姿態(tài)角，彌補(bǔ)了互補(bǔ)濾波和卡爾曼濾波各自的缺陷，既能夠提高動(dòng)態(tài)條件下姿態(tài)角的跟隨性，又能夠達(dá)到較快的收斂速度。

附圖說(shuō)明

圖1為一種實(shí)施例中姿態(tài)角的融合解算裝置的結(jié)構(gòu)示意圖；