[發(fā)明專利]一種拒止環(huán)境下的無人機(jī)自主導(dǎo)航定位方法

在審

• 申請(qǐng)?zhí)枺?/td>: 202111445139.3
• 申請(qǐng)日: 2021-11-30
• 公開（公告）號(hào): CN114184194A
• 公開（公告）日: 2022-03-15
• 發(fā)明（設(shè)計(jì)）人: 何中翔;張禮;李鵬程;劉博;李津;袁雙;王淑君;唐蔚
• 申請(qǐng)（專利權(quán)）人: 中國電子科技集團(tuán)公司第二十九研究所
• 主分類號(hào): G01C21/20
• 分類號(hào): G01C21/20;G01C21/16
• 代理公司: 成都九鼎天元知識(shí)產(chǎn)權(quán)代理有限公司 51214
• 代理人: 羅強(qiáng)

1.一種拒止環(huán)境下的無人機(jī)自主導(dǎo)航定位方法，其特征在于，包括以下過程：

步驟1、無人機(jī)進(jìn)入拒止環(huán)境時(shí)，通過慣性導(dǎo)航系統(tǒng)獲取無人機(jī)初始位置、速度以及姿態(tài)信息；并實(shí)時(shí)獲取無人機(jī)的加速度計(jì)、磁強(qiáng)計(jì)與陀螺儀輸出的數(shù)據(jù)，用于后續(xù)對(duì)初始位置、速度以及姿態(tài)信息的更新；

步驟2、判斷無人機(jī)當(dāng)前是否處于靜止或勻速運(yùn)動(dòng)狀態(tài)，若是，則進(jìn)入步驟3；反之，則進(jìn)入步驟4；

步驟3、根據(jù)加速度計(jì)和磁強(qiáng)計(jì)輸出的數(shù)據(jù)利用擴(kuò)展卡爾曼濾波定時(shí)對(duì)初始姿態(tài)信息進(jìn)行更新，進(jìn)入步驟5；

步驟4、在上一次更新后的姿態(tài)基礎(chǔ)上使用陀螺儀數(shù)據(jù)進(jìn)行慣導(dǎo)捷聯(lián)解算獲取姿態(tài)信息，直到無人機(jī)恢復(fù)靜止或勻速狀態(tài)，進(jìn)入步驟5；

步驟5、輸出無人機(jī)的更新后的姿態(tài)信息；

步驟6、根據(jù)無人機(jī)的光流傳感器、氣壓計(jì)、加速度計(jì)與陀螺儀輸出數(shù)據(jù)通過擴(kuò)展卡爾曼濾波融合解算出最新的位置、高度和速度信息。

2.根據(jù)權(quán)利要求1所述的拒止環(huán)境下的無人機(jī)自主導(dǎo)航定位方法，其特征在于，所述步驟3的具體子步驟為：

步驟3.1、通過加速度輸出數(shù)據(jù)獲取無人機(jī)當(dāng)前的橫滾角與俯仰角；

步驟3.2、根據(jù)磁強(qiáng)計(jì)輸出數(shù)據(jù)與無人機(jī)當(dāng)前的橫滾角與俯仰角獲取無人機(jī)當(dāng)前的航向角；

步驟3.3、通過擴(kuò)展卡爾曼濾波利用無人機(jī)當(dāng)前的橫滾角與俯仰角以及航向角進(jìn)行解算，并根據(jù)解算結(jié)果對(duì)慣性導(dǎo)航系統(tǒng)獲取的姿態(tài)進(jìn)行更新。

3.根據(jù)權(quán)利要求2所述的拒止環(huán)境下的無人機(jī)自主導(dǎo)航定位方法，其特征在于，所述步驟3.1中橫滾角與俯仰角獲取方法為：

其中，為無人機(jī)處于靜止?fàn)顟B(tài)或勻速運(yùn)動(dòng)狀態(tài)時(shí)加速度計(jì)測量值；γ和θ分別為無人機(jī)的橫滾角和俯仰角。

4.根據(jù)權(quán)利要求3所述的拒止環(huán)境下的無人機(jī)自主導(dǎo)航定位方法，其特征在于，所述步驟3.2的航向角獲取方法為：

其中，為地磁強(qiáng)度，根據(jù)已知的當(dāng)前無人機(jī)所處大體地理位置通過查表獲取，為無人機(jī)的磁強(qiáng)計(jì)給出，φ為無人機(jī)航向角。

5.根據(jù)權(quán)利要求4所述的拒止環(huán)境下的無人機(jī)自主導(dǎo)航定位方法，其特征在于，所述步驟6具體為：采用擴(kuò)展卡爾曼濾波將氣壓計(jì)、加速度計(jì)以及陀螺儀數(shù)據(jù)，以及光流傳感器以及超聲波傳感器的數(shù)據(jù)進(jìn)行融合,選取無人機(jī)在導(dǎo)航坐標(biāo)系下的速度和位置信息作為狀態(tài)量，利用無人機(jī)光流傳感器和超聲波傳感器以及氣壓計(jì)的輸出作為觀測量，估計(jì)無人機(jī)的位置、高度和速度信息。

6.根據(jù)權(quán)利要求1所述的拒止環(huán)境下的無人機(jī)自主導(dǎo)航定位方法，其特征在于，所述步驟6包括以下子步驟：

步驟61、采用光流估計(jì)法中的針孔模型將三維運(yùn)動(dòng)投影到攝像頭的二維像平面上，獲取攝像頭在成像平面的坐標(biāo)以及光流矢量在x，y，z三個(gè)方向的分量表達(dá)式，

步驟62、基于氣壓計(jì)或超聲波傳感器、陀螺儀輸出的數(shù)據(jù)，對(duì)光流矢量的分量表達(dá)式進(jìn)行解算，得到無人機(jī)在攝像頭坐標(biāo)系下的平均速度；

步驟63、通過坐標(biāo)系轉(zhuǎn)換矩陣將攝像頭坐標(biāo)系下的平均速度轉(zhuǎn)換為無人機(jī)在地理坐標(biāo)系下的速度；

步驟65、地理坐標(biāo)系下的速度進(jìn)行積分即可得到無人機(jī)在地理坐標(biāo)系下的位置信息。