[發明專利]一種無人機的航向角解算算法

有效

• 申請號: 202010645235.1
• 申請日: 2020-07-07
• 公開（公告）號: CN111982100B
• 公開（公告）日: 2022-05-06
• 發明（設計）人: 不公告發明人
• 申請（專利權）人: 廣東工業大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C25/00;G01P13/02
• 代理公司: 廣東廣信君達律師事務所 44329
• 代理人: 張生梅

1.一種無人機的航向角解算算法，其特征在于，包括以下步驟：

步驟1，采用無人機自身攜帶的加速度計和陀螺儀傳感器，通過數據融合解算出無人機的姿態角度-俯仰角和滾轉角；

步驟2，建立無人機動力學模型，通過自適應算法估算出動力學模型中的參數，具體步驟包括：

步驟2.1，建立無人機動力學模型，表示如下：

上式中，φ為無人機姿態的滾轉角，θ為俯仰角，ψ為偏航角，表示無人機的位置信息，x,y,z為不同方向的位置信息；k_x,k_y,k_z為待估計的阻力參數，m為無人機質量，g為重力加速度，u₁為無人機電機的拉力，參數上標一個圓點、兩個圓點表示該參數的一階導數、二階導數；

步驟2.2，令為速度，為加速度，則有：

令速度誤差：

其中v_x,d為期望的速度值，對速度誤差求導得：

步驟2.3，設計控制輸入：

其中為自適應估計值，為估計誤差值；K_x為待調整參數；

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

根據步驟2.1中的無人機動力模型，按照步驟2.2～2.4同樣的方法，令為速度，表示對變量*的一階求導，為速度誤差，其中*分別表示為位置y，z的信息，從而得到對應的自適應估計律最后得到待估計的阻力參數k_y,k_z的估計值和

步驟3，結合動力學模型方程所解算出來的航向角和慣性導航傳感器融合解算出來的航向角，采用添加權重的形式估計當前的偏航角，由此實現不受磁場干擾的航向角解算，具體步驟包括：

步驟3.1，將步驟2中建立的無人機動力學模型改寫為：

其中u_1x,u_1yu_1z分別對應無人機坐標系x，y，z軸的虛擬控制輸入，導航坐標系下的速度和加速度可以從無人機的定位系統或者是光流傳感器中獲取，因此參數已知，由此可以根據上式計算出u_1x,u_1yu_1z虛擬控制輸入量；

步驟3.2，由步驟3.1中的模型可得：

即：

步驟3.3，根據步驟3.1中的模型第三行公式可得：

步驟3.4，結合步驟3.2和步驟3.3可得航向角度：

步驟3.5，根據解算出的陀螺儀測量數據的漂移量b_g,k，可以獲得一個經過矯正后的陀螺儀測量數據，即：ω_k＝ω_m,k-b_g,k，其中ω_m,k為k時刻陀螺儀的測量數據值；

步驟3.6：根據求得其中所以能獲得當前的航向角速度則航向角其中ψ_k，ψ_k-1分別為當前和前一時刻的航向角度；

步驟3.7，修正航向角度：

ψ_corr＝ψ*w₁+ψ_k*w₂

其中w₁，w₂分別為步驟3.4解算出的航向角的權重和步驟3.5的矯正陀螺儀測量數據后的航向角度的權重。