[發明專利]航姿解算方法、裝置、電子設備及計算機可讀存儲介質

在審

• 申請號: 202010874420.8
• 申請日: 2020-08-26
• 公開（公告）號: CN111964676A
• 公開（公告）日: 2020-11-20
• 發明（設計）人: 孫國岐;王玥;楊旺喜
• 申請（專利權）人: 三一機器人科技有限公司
• 主分類號: G01C21/18
• 分類號: G01C21/18;G01C21/20
• 代理公司: 北京超凡宏宇專利代理事務所(特殊普通合伙) 11463
• 代理人: 安衛靜

1.一種航姿解算方法，其特征在于，包括：

通過目標對象的慣性測量單元獲取當前時刻所述目標對象多個方向上的測量參數；其中，所述測量參數包括角速度和加速度；

根據所述測量參數，采用梯度下降算法解算得到所述目標對象的航姿；其中，所述航姿包括橫滾角、俯仰角和航向角。

2.根據權利要求1所述的航姿解算方法，其特征在于，所述方法還包括：

當所述目標對象靜止時，通過所述慣性測量單元獲取初始角速度；

根據所述初始角速度確定初始姿態四元數。

3.根據權利要求2所述的航姿解算方法，其特征在于，所述根據所述測量參數，采用梯度下降算法解算得到所述目標對象的航姿的步驟，包括：

根據所述角速度確定當前時刻的姿態四元數；

根據所述當前時刻的姿態四元數和所述加速度確定所述加速度與重力加速度之間的偏離誤差；

當所述偏離誤差處于預設范圍內時，根據所述初始姿態四元數和所述偏離誤差進行迭代計算得到目標姿態四元數；

根據所述目標姿態四元數解算得到所述目標對象的航姿。

4.根據權利要求3所述的航姿解算方法，其特征在于，所述根據所述目標姿態四元數解算得到所述目標對象的航姿的步驟，包括：

根據所述目標姿態四元數和預先確定的所述目標姿態四元數與方向余弦矩陣之間的對應關系解算得到所述目標對象的航姿；其中，所述方向余弦矩陣用于表征坐標系之間的轉換關系。

5.根據權利要求3所述的航姿解算方法，其特征在于，所述根據所述當前時刻的姿態四元數和所述加速度確定所述加速度與重力加速度之間的偏離誤差的步驟，包括：

按照如下公式計算所述加速度與重力加速度之間的偏離誤差：

其中，E(q)表示偏離誤差；q表示姿態四元數，q＝[q₀，q₁，q₂，q₃]；[a_x a_y a_z]^T表示歸一化的加速度。

6.根據權利要求3所述的航姿解算方法，其特征在于，所述根據所述當前時刻的姿態四元數和所述加速度確定所述加速度與重力加速度之間的偏離誤差的步驟之后，還包括：

根據所述當前時刻的姿態四元數確定所述偏離誤差的雅克比矩陣。

7.根據權利要求6所述的航姿解算方法，其特征在于，所述根據所述初始姿態四元數和所述偏離誤差進行迭代計算得到目標姿態四元數的步驟，包括：

按照如下公式計算目標姿態四元數：

其中，q(k)表示k時刻的姿態四元數，k＝0，1，2…；μ表示下降步長，表示偏離誤差的變化率，J(q)表示偏離誤差的雅克比矩陣。

8.一種航姿解算裝置，其特征在于，包括：

參數獲取模塊，用于通過目標對象的慣性測量單元獲取當前時刻所述目標對象多個方向上的測量參數；其中，所述測量參數包括角速度和加速度；

航姿解算模塊，用于根據所述測量參數，采用梯度下降算法解算得到所述目標對象的航姿；其中，所述航姿包括橫滾角、俯仰角和航向角。

9.一種電子設備，其特征在于，包括處理器和存儲器，所述存儲器存儲有能夠被所述處理器執行的計算機可執行指令，所述處理器執行所述計算機可執行指令以實現權利要求1至7任一項所述的方法的步驟。

10.一種計算機可讀存儲介質，所述計算機可讀存儲介質上存儲有計算機程序，其特征在于，所述計算機程序被處理器運行時執行上述權利要求1至7任一項所述的方法的步驟。