[發明專利]一種無人機姿態與位置的實時高精度測量方法

有效

• 申請號: 202110648213.5
• 申請日: 2021-06-10
• 公開（公告）號: CN113551671B
• 公開（公告）日: 2023-04-11
• 發明（設計）人: 王帆;謝梅林;郝偉;黃偉;劉凱;劉波;李治國;韓俊鋒
• 申請（專利權）人: 中國科學院西安光學精密機械研究所
• 主分類號: G01C21/18
• 分類號: G01C21/18;G01S19/47
• 代理公司: 西安智邦專利商標代理有限公司 61211
• 代理人: 史曉麗

1.一種無人機姿態與位置的實時高精度測量方法，采用一種無人機姿態與位置的實時高精度測量系統，包括第一無人機單元、第二無人機單元和地面交互管理系統；

所述第一無人機單元和第二無人機單元通過地面交互管理系統進行飛行控制；

所述第一無人機單元和第二無人機單元均包括無人機，以及位于無人機上的GNSS接收模塊、慣導模塊、光電穩定平臺；

所述GNSS接收模塊帶有RTK功能，用于獲取所在無人機的位置信息和速度信息，并與地面交互管理系統進行通訊，將獲取的所在無人機位置信息和速度信息傳遞給地面交互管理系統；

所述慣導模塊為MEMS慣導模塊；

所述光電穩定平臺用于獲取脫靶量，以及自身的方位角和俯仰角，光電穩定平臺通過地面交互管理系統進行工作模式切換；所述第一無人機單元的光電穩定平臺和第二無人機單元的光電穩定平臺均為兩軸光電穩定平臺，并且均設置有雙視場接收光學組件，雙視場接收光學組件的光路末端分別為可見光相機和紅外熱像儀，兩個光電穩定平臺通過地面交互管理系統選擇可見光相機或紅外熱像儀；

其特征在于，所述無人機姿態與位置的實時高精度測量方法包括如下步驟：

步驟1：計算無人機在脫靶量輸出時刻的姿態

(1.1)定義第一無人機單元的無人機或第二無人機單元的無人機為測量無人機，則另一個為目標無人機；通過目標無人機的GNSS接收模塊獲取到目標無人機的經度、緯度、高度分別為(λ_P，L_P，H_P)，通過測量無人機的GNSS接收模塊獲取到測量無人機的實時經度、緯度、高度(λ_T，L_T，H_T)；

(1.2)將測量無人機與目標無人機的地理坐標轉換到自己選定的導航坐標系下，測量無人機的空間坐標位置為(x_P，y_P，z_P)，目標無人機的空間坐標位置為(x_T，y_T，z_T)，根據兩者坐標得到單位指向矢量D：

其中為目標無人機與測量無人機的絕對距離；

根據當前兩者位置信息，將單位指向矢量表示到導航坐標系下得到D_N；

將導航坐標系下的指向矢量D_N轉換到測量無人機機體坐標系下：

其中，θ，P，R分別為測量無人機的航向角、俯仰角、滾動角；

(1.3)由測量無人機的光電穩定平臺獲取脫靶量，由脫靶量可計算出測量無人機相機坐標系下的視線矢量N₀，則測量無人機機體坐標系下的視線出射矢量N表示為：

其中E，A分別為測量無人機的光電穩定平臺的俯仰角，方位角；

(1.4)假定測量無人機的MEMS慣導模塊獲取了測量無人機的滾動角R，令D_plane＝N，可解算出測量無人機的航向角θ與俯仰角P；

步驟2：實現無人機姿態與位置的高精度實時獲取

(2.1)由步驟(1.4)計算得到測量無人機實時的航向角θ與俯仰角P，并構成變量(θ_C，P_C，R_C)，與測量無人機的MEMS慣導模塊輸出的對應的(θ_I，P_I，R_I)作差，得到失調角

由測量無人機的GNSS接收模塊測量得到測量無人機的位置信息(λ，L，H)和速度信息(v_x，v_y，v_z)，與測量無人機的MEMS慣導模塊輸出的對應的位置信息(λ_I，L_I，H_I)和速度信息(v_xI，v_yI，v_zI)作差，得到位置誤差(δλ，δL，δH)和速度誤差(δv_x，δv_y，δv_z)；

(2.2)將步驟(2.1)得到的失調角位置誤差(δλ，δL，δH)、速度誤差(δv_x，δv_y，δv_z)，以及測量無人機的MEMS慣導模塊自身的陀螺常值偏移(∈_xk，∈_yk，∈_zk)、加速度計零偏作為系統狀態變量x_k，

使用擴展Kalman濾波算法進行數據融合，擴展Kalman濾波狀態方程如下：

x_k+1＝f(x_k)+Γ_kw_k

其中f(x_k)為非線性狀態函數，為過程噪聲轉移矩陣，為過程噪聲；

系統的測量方程為：y_k＝Hx_k+v_k，其中H為量測轉移矩陣，v_k為測量白噪聲；

如果ω_k和v_k均為高斯白噪聲，且不相關，并具有如下統計特性：

E(w_k)＝q_k，Cov(w_k，w_j)＝Q_kδ_kj

E(v_k)＝r_k，Cov(v_k，v_j)＝R_kδ_kj

其中Q_k為非負定對稱陣，R_k是正定對稱陣，δ_kj為kronecker-δ函數；

根據卡爾曼濾波的估計準則和基本思想，可推導出擴展卡爾曼濾波的基本方程為：

狀態一步預測方程：

狀態估值計算方程：

濾波增益方程：K_k＝P_k+1|kH^T(HP_k+1|kH^T+R_k)^-1；

一步預測均方誤差方程：其中F_k為f(x_k)的一階泰勒展開；

估計均方誤差方程：P_k+1＝(I-K_kH)P_k+1|k；

(2.3)由于數據融合過程中使用的測量量中含有過程噪聲，不能直接使用經典EKF進行濾波，需對系統模型進行噪聲不相關處理；

(2.4)系統使用EKF進行濾波對測量無人機慣導模塊的姿態誤差、速度誤差、位置誤差、陀螺漂移、加速度計零偏等狀態量進行修正。