[發明專利]一種基于四元數和卡爾曼濾波的人體姿態解算方法

在審

• 申請號: 201610405814.2
• 申請日: 2016-06-08
• 公開（公告）號: CN107478223A
• 公開（公告）日: 2017-12-15
• 發明（設計）人: 李炳炳;黃成;王歆洵;陳嘉;譚正永;徐志良;李曉曉;金威
• 申請（專利權）人: 南京理工大學
• 主分類號: G01C21/18
• 分類號: G01C21/18;G01C21/08
• 代理公司: 南京理工大學專利中心32203
• 代理人: 薛云燕

1.一種基于四元數和卡爾曼濾波的人體姿態解算方法，其特征在于，包括以下步驟：

步驟1，確定載體坐標系和導航坐標系，根據已確定的坐標系確定姿態矩陣；

步驟2，加速度計、陀螺儀和磁強計分別采集加速度、角速度和磁感應強度信號，根據采集到的姿態數據對人體姿態檢測系統進行初始對準，求得初始的姿態角即俯仰角、翻滾角和航向角，并將初始姿態角轉化為初始化四元數；

步驟3，根據四元數微分方程進行系統建模，以初始化四元數為測量值輸入，采用卡爾曼濾波算法對姿態數據進行數據融合輸出估計值，即更新后的四元數；

步驟4，將更新后的四元數進行歸一化處理，作為最終的姿態信息，并以歸一化處理后的四元數更新姿態矩陣，得到更新后的姿態角。

2.根據權利要求1所述的基于四元數和卡爾曼濾波的人體姿態解算方法，其特征在于，步驟1所述的載體坐標系即b系Y軸指向前進方向，X軸水平向右，Z軸垂直向下；導航坐標系即n系坐標軸沿北、東、地方向；姿態角由載體坐標系到導航坐標系之間的相對角位置關系來表示，用歐拉角來對人體姿態進行描述，歐拉角包括俯仰角θ、橫滾角γ和偏航角ψ，俯仰角是坐標系沿Y軸旋轉的角度，旋轉范圍為-90°～+90°；橫滾角是坐標系繞X軸旋轉的角度，旋轉范圍為-180°～+180°，偏航角是坐標系繞Z軸旋轉的角度，旋轉范圍為0°～360°；

兩個坐標系之間的相互轉換由姿態矩陣來表示，姿態矩陣也稱作捷聯矩陣T，由載體坐標系到導航坐標系的姿態矩陣表示為：

Cbn=T=T11T12T11T21T22T23T31T32T33]]>

由載體坐標系與導航坐標系之間的轉換關系得：

Cbn=cosθcosψ-cosγsinψ+sinγsinθcosψsinγsinψ+cosγsinθcosψcosθsinψcosγcosψ+sinγsinθsinψ-sinγcosψ+cosγsinθsinψ-sinθsinγcosθcosγcosθ]]>

因此，由捷聯矩陣T與姿態矩陣的矩陣關系求得的三個姿態角γ_T、θ_T、ψ_T計算如下：

θ_T＝arcsin(-T₃₁)