[發(fā)明專(zhuān)利]基于雙歐法和四元數(shù)混合編排的無(wú)人機(jī)導(dǎo)航方法和系統(tǒng)

有效

• 申請(qǐng)?zhí)枺?/td>: 202010567793.0
• 申請(qǐng)日: 2020-06-19
• 公開(kāi)（公告）號(hào): CN111707268B
• 公開(kāi)（公告）日: 2022-07-29
• 發(fā)明（設(shè)計(jì)）人: 王玉杰;高顯忠;侯中喜;郭正;賈高偉
• 申請(qǐng)（專(zhuān)利權(quán)）人: 中國(guó)人民解放軍國(guó)防科技大學(xué)
• 主分類(lèi)號(hào): G01C21/20
• 分類(lèi)號(hào): G01C21/20
• 代理公司: 長(zhǎng)沙國(guó)科天河知識(shí)產(chǎn)權(quán)代理有限公司 43225
• 代理人: 邱軼

本申請(qǐng)涉及一種基于雙歐法和四元數(shù)混合編排的無(wú)人機(jī)導(dǎo)航方法和系統(tǒng)。所述方法包括：分別獲取無(wú)人機(jī)水平飛行模式時(shí)對(duì)應(yīng)的水平歐拉角表示的姿態(tài)旋轉(zhuǎn)矩陣，以及垂直飛行模式時(shí)對(duì)應(yīng)的垂直歐拉角表示的姿態(tài)旋轉(zhuǎn)矩陣，當(dāng)無(wú)人機(jī)的俯仰角大于設(shè)定的閾值時(shí)，將無(wú)人機(jī)的姿態(tài)旋轉(zhuǎn)矩陣的表示形式切換為垂直歐拉角表示，當(dāng)無(wú)人機(jī)的俯仰角小于或等于設(shè)定的閾值時(shí)，將無(wú)人機(jī)的姿態(tài)旋轉(zhuǎn)矩陣的表示形式切換為水平歐拉角表示，在導(dǎo)航內(nèi)部編排時(shí)，采用四元數(shù)表示對(duì)垂直歐拉角表示和水平歐拉角表示進(jìn)行姿態(tài)更新，從而對(duì)無(wú)人機(jī)進(jìn)行導(dǎo)航。采用本方法能夠避免導(dǎo)航時(shí)姿態(tài)控制出現(xiàn)奇異值。

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

本申請(qǐng)涉及無(wú)人機(jī)技術(shù)領(lǐng)域，特別是涉及一種基于雙歐法和四元數(shù)混合編排的無(wú)人機(jī)導(dǎo)航方法和系統(tǒng)。

背景技術(shù)

隨著無(wú)人機(jī)技術(shù)的發(fā)展，無(wú)人機(jī)也被用在生產(chǎn)生活中的各個(gè)領(lǐng)域中，從而對(duì)無(wú)人機(jī)的飛行形成有了新的要求。

常用的固定翼無(wú)人機(jī)，可以采用歐拉角表示方法，對(duì)導(dǎo)航時(shí)的姿態(tài)進(jìn)行控制，然而，在無(wú)人機(jī)的俯仰角等于或者接近±90度時(shí)，導(dǎo)致計(jì)算結(jié)果出現(xiàn)奇異值，從而無(wú)法正常的執(zhí)行飛行導(dǎo)航控制。

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

基于此，有必要針對(duì)上述技術(shù)問(wèn)題，提供一種能夠解決無(wú)人機(jī)歐拉角表示法進(jìn)行姿態(tài)控制時(shí)存在奇異值問(wèn)題的基于雙歐法的無(wú)人機(jī)導(dǎo)航方法和裝置。

一種基于雙歐法和四元數(shù)混合編排的無(wú)人機(jī)導(dǎo)航方法，所述方法包括：

分別獲取無(wú)人機(jī)水平飛行模式時(shí)對(duì)應(yīng)的水平歐拉角表示的姿態(tài)旋轉(zhuǎn)矩陣，以及垂直飛行模式時(shí)對(duì)應(yīng)的垂直歐拉角表示的姿態(tài)旋轉(zhuǎn)矩陣；所述水平飛行模式為按傳統(tǒng)固定翼飛機(jī)飛行的模式，無(wú)人機(jī)機(jī)體的縱軸成水平狀態(tài)；所述垂直飛行模式為按傳統(tǒng)四旋翼飛機(jī)飛行的模式，無(wú)人機(jī)機(jī)體的縱軸成豎直狀態(tài)；

當(dāng)無(wú)人機(jī)的俯仰角大于設(shè)定的閾值時(shí)，將無(wú)人機(jī)的姿態(tài)旋轉(zhuǎn)矩陣的表示形式切換為垂直歐拉角表示；

當(dāng)無(wú)人機(jī)的俯仰角小于或等于設(shè)定的閾值時(shí)，將無(wú)人機(jī)的姿態(tài)旋轉(zhuǎn)矩陣的表示形式切換為水平歐拉角表示；

在導(dǎo)航內(nèi)部編排時(shí)，采用四元數(shù)表示對(duì)所述垂直歐拉角表示和所述水平歐拉角表示進(jìn)行姿態(tài)更新，從而對(duì)無(wú)人機(jī)進(jìn)行導(dǎo)航。

在其中一個(gè)實(shí)施例中，還包括：獲取當(dāng)?shù)氐乩碜鴺?biāo)系對(duì)應(yīng)于無(wú)人機(jī)載體坐標(biāo)系的旋轉(zhuǎn)矩陣；根據(jù)所述旋轉(zhuǎn)矩陣計(jì)算當(dāng)?shù)刂亓铀俣仍跓o(wú)人機(jī)載體坐標(biāo)系中的水平投影，得到所述六自由度運(yùn)動(dòng)模型的水平歐拉角表示，根據(jù)所述水平歐拉角表示的六自由度運(yùn)動(dòng)模型，得到水平歐拉角表示的姿態(tài)旋轉(zhuǎn)矩陣。

在其中一個(gè)實(shí)施例中，還包括：獲取當(dāng)?shù)氐乩碜鴺?biāo)系對(duì)應(yīng)于無(wú)人機(jī)載體坐標(biāo)系的旋轉(zhuǎn)矩陣；根據(jù)所述旋轉(zhuǎn)矩陣計(jì)算當(dāng)?shù)刂亓铀俣仍跓o(wú)人機(jī)載體坐標(biāo)系中的垂直投影，得到所述六自由度運(yùn)動(dòng)模型的垂直歐拉角表示，根據(jù)所述垂直歐拉角表示的六自由度運(yùn)動(dòng)模型，得到垂直歐拉角表示的姿態(tài)旋轉(zhuǎn)矩陣。

在其中一個(gè)實(shí)施例中，還包括：根據(jù)所述水平歐拉角表示的六自由度運(yùn)動(dòng)模型，得到水平歐拉角表示的姿態(tài)旋轉(zhuǎn)矩陣為：

其中，B表示姿態(tài)旋轉(zhuǎn)矩陣，水平歐拉角φ繞X_b軸的旋轉(zhuǎn)，水平歐拉角θ繞Y_b軸旋轉(zhuǎn)，水平歐拉角ψ繞Z_b軸旋轉(zhuǎn)，其中水平歐拉角的旋轉(zhuǎn)順序?yàn)閆_b-Y_b-X_b；C表示下標(biāo)余弦值，S表示下標(biāo)的正弦值。

在其中一個(gè)實(shí)施例中，還包括：根據(jù)所述垂直歐拉角表示的六自由度運(yùn)動(dòng)模型，得到垂直歐拉角表示的姿態(tài)旋轉(zhuǎn)矩陣為：

其中，B表示姿態(tài)旋轉(zhuǎn)矩陣，垂直歐拉角φ_v繞軸的旋轉(zhuǎn)，垂直歐拉角θ_v繞軸旋轉(zhuǎn)，垂直歐拉角ψ_v繞軸旋轉(zhuǎn)，其中垂直歐拉角的旋轉(zhuǎn)順序?yàn)镃表示下標(biāo)余弦值，S表示下標(biāo)的正弦值。