[發明專利]一種小型無人機姿態與位置解算方法

有效

• 申請號: 202011382997.3
• 申請日: 2020-12-01
• 公開（公告）號: CN112649001B
• 公開（公告）日: 2023-08-22
• 發明（設計）人: 趙晉偉;王志剛;楊大鵬
• 申請（專利權）人: 中國航空工業集團公司沈陽飛機設計研究所
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/00;G01C21/16;G01S19/48;G01S19/49
• 代理公司: 北京航信高科知識產權代理事務所(普通合伙) 11526
• 代理人: 高原

1.一種小型無人機姿態與位置解算方法，其特征在于，

姿態解算包括：

根據三軸加速度計和三軸磁強計在無人機靜止條件下的輸出推算出無人機的初始姿態角，接著根據三軸陀螺儀輸出角速度進行積分推算后續每一時刻的第一姿態角；

在無人機運動時，根據三軸加速度計和三軸磁強計輸出同步計算第二姿態角；

將根據陀螺儀輸出推算的第一姿態角與根據加速度計和磁強計輸出推算的第二姿態角分別進行高通濾波和低通濾波，輸入互補濾波器進行融合，最終得到無人機的姿態；

所述根據三軸加速度計和三軸磁強計在無人機靜止條件下的輸出推算出無人機的初始姿態角的過程包括：

從地面坐標系到機體坐標系進行坐標矩陣轉換：

式中，為轉換矩陣，θ、ψ、φ分別為俯仰角、偏航角和滾轉角；

三軸加速度計的敏感軸沿機體坐標系各軸安裝，在靜基座初始對準時，其輸出與地面坐標系中的比力關系如下：

式中，f_b為加速度計三軸輸出，n_x、n_y和n_z為三軸加速度計測量到的無人機加速度在機體坐標系三軸方向上的分量與當地重力加速度之比，g為當地重力加速度；

三軸磁強計的各敏感軸沿無人機坐標系各軸安裝，其測得三軸磁場強度和導航坐標系與無人機坐標系的關系如下：

其中，

式中，M_b為無人機坐標系下測得三軸磁場強度，M_n為導航坐標系下測得三軸磁場強度，H₀為初始高度；

當無人機靜止時，三軸加速度計測量到重力加速度在無人機三軸上的投影，三軸磁力計的輸出為地磁場在無人機三軸上的投影，得到計算輔助姿態角：

式中，H_x、H_y和H_z為磁力計的三軸輸出；

根據三軸陀螺儀輸出角速度進行積分推算后續每一時刻的第一姿態角為：

其中，k為時刻，p_k、q_k、r_k為三軸陀螺儀測量到的當前時刻系統的角速度信息，Δt為三軸陀螺儀輸出時間間隔；

位置解算包括：

根據經緯度接收模塊獲得無人機的經度、緯度和高度信息，并將上述信息轉換到以某已知固定點為坐標原點的地面坐標系，輸出無人機在地面坐標系中的位置坐標信息；

根據最終得到的無人機姿態角進行二次積分得出相對于無人機運動起始點的三軸坐標；

最后將無人機地面坐標系轉的位置信息和無人機姿態角積分得到的位置信息分別進行高通和低通濾波輸入互補濾波器進行融合，最終得到無人機的位置；

根據最終得到的無人機姿態角進行二次積分得出相對于無人機運動起始點的三軸坐標的過程包括：

建立地面坐標系S_g(o_gx_gy_gz_g)，原點選在一已知經緯高的地點，將經緯度坐標轉換為地面坐標系中的坐標：

式中，λ、分別為經緯度模塊所在位置的經度和緯度，λ₀、分別為坐標原點的經度和緯度，R_m為當地子午圈曲率半徑，R_n為當地卯酉圈曲率半徑；

進一步可得：

在接收經緯度模塊輸出數據并進行坐標轉換的同時，使用慣性傳感器進行二次積分估算無人機位置，再將二者融合輸出高精度高更新速率的位置信息：

式中，u、v和w為無人機速度投影到機體坐標系三軸方向上的分量，g為當地重力加速度，n_x、n_y和n_z為三軸加速度計測量到的無人機加速度在機體坐標系三軸方向上的分量與當地重力加速度之比，p、q和r為三軸陀螺儀測量到的無人機角速度在機體坐標系三軸方向上的分量，φ和θ為無人機的滾轉角和俯仰角；

最終得到無人機的位置為：

式中，x，y，h為無人機的位置坐標及高度坐標。