[發(fā)明專利]航姿解算方法、裝置、電子設(shè)備及計算機可讀存儲介質(zhì)

在審

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

本發(fā)明提供了一種航姿解算方法、裝置、電子設(shè)備及計算機可讀存儲介質(zhì)，包括：通過目標對象的慣性測量單元獲取當前時刻目標對象多個方向上的測量參數(shù)；其中，測量參數(shù)包括角速度和加速度；根據(jù)測量參數(shù)，采用梯度下降算法解算得到目標對象的航姿；其中，航姿包括橫滾角、俯仰角和航向角。本發(fā)明可以進行全姿態(tài)解算，提高IMU導航的適用性。

技術(shù)領(lǐng)域

本發(fā)明涉及智能制造技術(shù)領(lǐng)域，尤其是涉及一種航姿解算方法、裝置、電子設(shè)備及計算機可讀存儲介質(zhì)。

背景技術(shù)

隨著裝備制造業(yè)產(chǎn)業(yè)結(jié)構(gòu)的優(yōu)化，智能制造逐漸成為了產(chǎn)業(yè)結(jié)構(gòu)轉(zhuǎn)變過程中的新興制造模式。其中，自動導引車輛(Automated Guided Vehicle，AGV)是實現(xiàn)智能制造的重要部分。AGV的自動運行主要依賴于AGV的定位模塊，其定位方式大多采用慣性測量單元(Inertial Measurement Unit，IMU)與其它傳感器的融合來實現(xiàn)；在無人駕駛算法融合之前首先需要對AGV車體的航姿進行解算，從而才可以將IMU測得的加速度信息折算到導航坐標系下。但是現(xiàn)有的航姿解算算法采用四軸慣性傳感器，加速度計的A_x、A_y、A_z和陀螺的G_z，四軸傳感器對車體X軸和Y軸的旋轉(zhuǎn)角速度無法得知，無法進行全姿態(tài)解算，IMU導航的適用性較差。

發(fā)明內(nèi)容

有鑒于此，本發(fā)明的目的在于提供一種航姿解算方法、裝置、電子設(shè)備及計算機可讀存儲介質(zhì)，以進行全姿態(tài)解算，提高IMU導航的適用性。

為了實現(xiàn)上述目的，本發(fā)明實施例采用的技術(shù)方案如下：

第一方面，本發(fā)明實施例提供了一種航姿解算方法，包括：通過目標對象的慣性測量單元獲取當前時刻目標對象多個方向上的測量參數(shù)；其中，測量參數(shù)包括角速度和加速度；根據(jù)測量參數(shù)，采用梯度下降算法解算得到目標對象的航姿；其中，航姿包括橫滾角、俯仰角和航向角。

在一種實施方式中，方法還包括：當目標對象靜止時，通過慣性測量單元獲取初始角速度；根據(jù)初始角速度確定初始姿態(tài)四元數(shù)。

在一種實施方式中，根據(jù)測量參數(shù)，采用梯度下降算法解算得到目標對象的航姿的步驟，包括：根據(jù)角速度確定當前時刻的姿態(tài)四元數(shù)；根據(jù)當前時刻的姿態(tài)四元數(shù)和加速度確定加速度與重力加速度之間的偏離誤差；當偏離誤差處于預(yù)設(shè)范圍內(nèi)時，根據(jù)初始姿態(tài)四元數(shù)和偏離誤差進行迭代計算得到目標姿態(tài)四元數(shù)；根據(jù)目標姿態(tài)四元數(shù)解算得到目標對象的航姿。

在一種實施方式中，根據(jù)目標姿態(tài)四元數(shù)解算得到目標對象的航姿的步驟，包括：根據(jù)目標姿態(tài)四元數(shù)和預(yù)先確定的目標姿態(tài)四元數(shù)與方向余弦矩陣之間的對應(yīng)關(guān)系解算得到目標對象的航姿；其中，方向余弦矩陣用于表征坐標系之間的轉(zhuǎn)換關(guān)系。

在一種實施方式中，根據(jù)當前時刻的姿態(tài)四元數(shù)和加速度確定加速度與重力加速度之間的偏離誤差的步驟，包括：按照如下公式計算加速度與重力加速度之間的偏離誤差：

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

在一種實施方式中，根據(jù)當前時刻的姿態(tài)四元數(shù)和加速度確定加速度與重力加速度之間的偏離誤差的步驟之后，還包括：根據(jù)當前時刻的姿態(tài)四元數(shù)確定偏離誤差的雅克比矩陣。

在一種實施方式中，根據(jù)初始姿態(tài)四元數(shù)和偏離誤差進行迭代計算得到目標姿態(tài)四元數(shù)的步驟，包括：按照如下公式計算目標姿態(tài)四元數(shù)：