[發(fā)明專利]一種無人機(jī)的航向角解算算法

有效

• 申請?zhí)枺?/td>: 202010645235.1
• 申請日: 2020-07-07
• 公開（公告）號: CN111982100B
• 公開（公告）日: 2022-05-06
• 發(fā)明（設(shè)計）人: 不公告發(fā)明人
• 申請（專利權(quán)）人: 廣東工業(yè)大學(xué)
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C25/00;G01P13/02
• 代理公司: 廣東廣信君達(dá)律師事務(wù)所 44329
• 代理人: 張生梅

本發(fā)明公開了一種無人機(jī)的航向角解算算法，包括：采用無人機(jī)自身攜帶的加速度計和陀螺儀傳感器，通過數(shù)據(jù)融合解算出無人機(jī)的姿態(tài)角度?俯仰角和滾轉(zhuǎn)角；建立無人機(jī)動力學(xué)模型，通過自適應(yīng)算法估算出動力學(xué)模型中的參數(shù)；結(jié)合動力學(xué)模型方程所解算出來的航向角和慣性導(dǎo)航傳感器融合解算出來的航向角，采用添加權(quán)重的形式估計當(dāng)前的航向角，由此實(shí)現(xiàn)不受磁場干擾的航向角解算。本方法能解算出較為精確的無人機(jī)航向角，解決現(xiàn)有磁力計傳感器測量容易受到強(qiáng)磁場干擾而出現(xiàn)航向角解算出現(xiàn)較大偏差的問題，為無人機(jī)飛行控制器提供更為準(zhǔn)確的航向角信息；不需要額外添加其他傳感器，僅用無人機(jī)自帶現(xiàn)有傳感器即可解決解算航向角受強(qiáng)磁場干擾問題。

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

本發(fā)明涉及無人機(jī)技術(shù)領(lǐng)域，具體涉及一種無人機(jī)的航向角解算算法。

背景技術(shù)

四旋翼飛行器是一種能實(shí)現(xiàn)垂直起降的小型飛行器，因其體型小巧，靈活性高，成本低，易維護(hù)等特點(diǎn)受到關(guān)注，它能在各種受限環(huán)境中工作，無論是叢林，地底或者是室內(nèi)環(huán)境等場景。在農(nóng)業(yè)方面，廣泛地應(yīng)用于農(nóng)藥的噴灑，農(nóng)作物生長情況的監(jiān)管。快遞的派送、消防安防、電網(wǎng)的安全巡檢、國土資源的勘測以及無人機(jī)的探測與無線電反制等領(lǐng)域中。但是由于無人機(jī)系統(tǒng)大多是運(yùn)用于多干擾，復(fù)雜環(huán)境下完成一些復(fù)雜任務(wù)的智能控制系統(tǒng)，在遇到一些強(qiáng)磁干擾的情況之前，傳統(tǒng)的基于磁力傳感器解算出來的無人機(jī)偏航角將不再是可信的，然而在無人機(jī)缺失航向角的引導(dǎo)將缺失方向感；不準(zhǔn)確的航向角將導(dǎo)致的無人機(jī)可能出現(xiàn)馬桶式螺旋發(fā)散。

目前已公開文獻(xiàn)中并沒有針對無人機(jī)中磁力計傳感器容易受干擾的缺陷給出缺失磁力計傳感器后如何解算出航向角。

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

本發(fā)明的目的是提供一種無人機(jī)的航向角結(jié)算算法，用以克服現(xiàn)有技術(shù)中無人機(jī)磁力計傳感器容易受干擾而導(dǎo)致解算出的航向角不準(zhǔn)確的問題。

為了實(shí)現(xiàn)上述任務(wù)，本發(fā)明采用以下技術(shù)方案：

一種無人機(jī)的航向角解算算法，包括以下步驟：

步驟1，采用無人機(jī)自身攜帶的加速度計和陀螺儀傳感器，通過數(shù)據(jù)融合解算出無人機(jī)的姿態(tài)角度-俯仰角和滾轉(zhuǎn)角；

步驟2，建立無人機(jī)動力學(xué)模型，通過自適應(yīng)算法估算出動力學(xué)模型中的參數(shù)，具體步驟包括：

步驟2.1，建立無人機(jī)動力學(xué)模型，表示如下：

上式中，表示無人機(jī)的位置信息，x,y,z為不同方向的位置信息；k_x，k_y，k_z為待估計的阻力參數(shù)，m為無人機(jī)質(zhì)量，g為重力加速度，u₁為無人機(jī)電機(jī)的總拉力，參數(shù)上標(biāo)一個圓點(diǎn)、兩個圓點(diǎn)表示該參數(shù)的一階導(dǎo)數(shù)、二階導(dǎo)數(shù)；

步驟2.2，令為速度，為加速度，

則有：

令速度誤差：

其中v_x，d為期望的速度值，對速度誤差求導(dǎo)得：

步驟2.3，設(shè)計控制輸入：

其中為自適應(yīng)估計值，為估計誤差值；K_x為控制器輸入的待調(diào)整參數(shù)；

步驟2.4，根據(jù)所設(shè)計的控制輸入，設(shè)計待估計的阻力參數(shù)k_x的自適應(yīng)參數(shù)估計律為然后根據(jù)自適應(yīng)律迭代或積分的方式獲得自適應(yīng)估計值