[發明專利]一種無人機飛控多傳感器姿態置信解算方法

在審

• 申請號: 202010172639.3
• 申請日: 2020-03-12
• 公開（公告）號: CN111141286A
• 公開（公告）日: 2020-05-12
• 發明（設計）人: 劉哲寧
• 申請（專利權）人: 江蘇藍鯨智慧空間研究院有限公司
• 主分類號: G01C21/18
• 分類號: G01C21/18;G01C21/20;G05D1/08
• 代理公司: 南京君陶專利商標代理有限公司 32215
• 代理人: 沈根水

1.一種無人機飛控多傳感器姿態置信解算方法，其特征在于，該方法具體包括如下步驟：

1）對無人機飛控系統中內置的3組傳感器進行數據預測，建立數學觀測模型：

2）各傳感器通過測量科里奧利力來間接得到角速度，通過角速度數學模型階導求得當前狀態角速度與數學模型預測量；

3）將3組傳感器最終測得的角速度數據中的其中一組直接輸出至無人機飛控系統，其他兩組數據分別輸出至2個冗余觀測器，通過計算IMU中加速度計和陀螺儀的誤差來獲得角速度預測量；

4）2個冗余觀測器分別輸出角速度預測量與實際角速度，共同解算與測量差值，在兩組傳感器數據中進對比；將以上數據進行整合，選擇數據正常的傳感器。

2.根據權利要求1所述的一種無人機飛控多傳感器姿態置信解算方法，其特征在于，所述的步驟1）中建立的數學觀測模型具體如下：

將無人機IMU中的加速度計簡化為質量塊+彈簧+指示計的結構，其具體公式如下

式中a_m為加速度計測量值，f為彈簧拉力，m為質量塊質量，a為物體在世界坐標系下的實際加速度。

3.根據權利要求1所述的一種無人機飛控多傳感器姿態置信解算方法，其特征在于，所述的步驟2）中的角速度數學模型為：

傳感器采用東北天坐標系，考慮一個從原點出發的向量 r 繞單位軸 u 旋轉，角速度大小為，角速度矢量表示為，易得向量r末端點P的速度矢量，即r的時間一階導為：

坐標系{B}繞單位軸u旋轉，如上所述，其三個軸的時間一階導同樣為：

[i_B jB k_B]為是坐標系{B}相對于參考坐標系的旋轉矩陣R，R的時間一階導為：

。

4.根據權利要求1所述的一種無人機飛控多傳感器姿態置信解算方法，其特征在于，所述的步驟3）中IMU中加速度計和陀螺儀的誤差可以分為確定性誤差和隨機誤差。

5.根據權利要求4所述的一種無人機飛控多傳感器姿態置信解算方法，其特征在于，所述的確定性誤差事先標定確定，包括偏置誤差和尺度誤差；其中加速度計的偏置誤差是假設理論上當沒有外部作用時，IMU傳感器的輸出為 0，但實際輸出數據存在的偏置數值；尺度誤差是實際數值和傳感器輸出值之間的比值。

6.根據權利要求4所述的一種無人機飛控多傳感器姿態置信解算方法，其特征在于，所述的隨機誤差假設噪聲服從高斯分布，包括高斯白噪聲和bias隨機游走誤差。

7.根據權利要求1所述的一種無人機飛控多傳感器姿態置信解算方法，其特征在于，所述的步驟4）中數據對比遵循如下原則：

①若其中2組傳感器出現異常，則認為觀測誤差較大的該2組傳感器，而剩下1組傳感器與該2組傳感器差值較大；

②若單個傳感器出現異常，該傳感器與另外兩組傳感器誤差較大，而另外兩組傳感器誤差較小。