[發(fā)明專利]一種小型無人機姿態(tài)與位置解算方法

有效

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

本申請?zhí)峁┝艘环N小型無人機姿態(tài)與位置解算方法，其中，姿態(tài)解算包括：根據(jù)三軸加速度計和三軸磁強計推算出無人機在靜止條件下的初始姿態(tài)角，接著根據(jù)三軸陀螺儀輸出的角速度進行積分推算后續(xù)每一時刻的無人機姿態(tài)角；在無人機運動時同步計算無人機姿態(tài)角；最后，將上述過程的兩個無人機姿態(tài)角分別進行高通濾波和低通濾波及融合，得到無人機姿態(tài)；位置解算包括：首先根據(jù)經(jīng)緯度模塊獲得無人機的經(jīng)度、緯度、高度信息，將其轉換到以某一已知固定點為坐標原點的地面坐標系；之后，根據(jù)三軸加速度計結合上述姿態(tài)角進行二次積分得出相對于無人機運動起始點的三軸坐標；最后將上述過程的位置信息分別進行高通和低通濾波及融合，得到無人機的位置。

技術領域

本申請屬于飛行器控制技術領域，特別涉及一種小型無人機姿態(tài)與位置解算方法。

背景技術

導航系統(tǒng)是小型無人機自主飛行過程中進行狀態(tài)估計和環(huán)境感知必不可少的組成部分。其中，姿態(tài)和位置計算是其最主要的功能之一，同樣也是自主飛行的關鍵。通常情況下，狀態(tài)估計通過融合來自不同傳感器的多源信息，進一步估計無人機姿態(tài)、高度、速度和位置。

傳統(tǒng)導航系統(tǒng)通常依靠單一傳感器的輸出數(shù)據(jù)進行姿態(tài)和位置解算，未能根據(jù)傳感器的工作特點充分有效利用慣性傳感器及衛(wèi)星定位系統(tǒng)，導致在較為惡劣的工作條件下受外界振動干擾影響較大，輸出數(shù)據(jù)穩(wěn)定性和精度較差，甚至數(shù)據(jù)發(fā)散而造成輸出結果失效，嚴重時可造成無人機失控。

發(fā)明內容

本申請的目的是提供了一種小型無人機姿態(tài)與位置解算方法，以解決或減輕背景技術中的至少一個問題。

本申請的技術方案是：一種小型無人機姿態(tài)與位置解算方法，其中：

姿態(tài)解算包括：

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

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

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

位置解算包括：

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

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

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

在本申請中，根據(jù)三軸加速度計和三軸磁強計在無人機靜止條件下的輸出推算出無人機的初始姿態(tài)角的過程包括：

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

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

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

式中，f_b為加速度計三軸輸出，n_x、n_y和n_z為三軸加速度計測量到的無人機加速度在機體坐標系三軸方向上的分量與當?shù)刂亓铀俣戎龋琯為當?shù)刂亓铀俣龋?/p>

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