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

在審

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

技術領域

本發明屬于姿態檢測技術領域，特別是一種基于四元數和卡爾曼濾波的人體姿態解算方法。

背景技術

人體姿態解算的方法主要有余弦矩陣法、歐拉角法和四元數法，余弦矩陣法計算量較大、解算較為復雜，歐拉角法雖然可以更加直觀地表示姿態角，但是在姿態解算中存在奇點問題，而四元數法不但計算相對余弦矩陣法簡單，而且避免了歐拉角法存在的奇點問題。

傳統的姿態解算大都基于加速度計和陀螺儀兩種慣性傳感器，陀螺儀通過積分求得姿態角，在短時間內可以提供準確的姿態信息，但長期精度較差。而加速度計的測量誤差不隨時間積累，長期性能較好，但是短期內精度較差，動態響應較慢。現有的較多解算方法缺少對含有噪聲的數據進行濾波，部分算法采用了互補濾波算法，但是互補濾波算法過于簡單并且參數的修正較為繁瑣，因此達不到良好的濾波效果。同時，也沒有對數據進行有效的融合，由此導致姿態解算的結果精度較低，穩定性和可靠性不足。除此之外，慣性傳感器選擇上的不足同樣影響姿態解算的精度。

發明內容

本發明的目的在于提供一種基于四元數和卡爾曼濾波的人體姿態解算方法，以提高人體姿態檢測的精度和響應速度，使人體姿態檢測系統具有更好的實時性和穩定性。

本發明的技術解決方案為：一種基于四元數和卡爾曼濾波的人體姿態解算方法，包括以下步驟：

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

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

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

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

進一步地，步驟1所述的載體坐標系即b系Y軸指向前進方向，X軸水平向右，Z軸垂直向下；導航坐標系即n系坐標軸沿北、東、地方向；姿態角由載體坐標系到導航坐標系之間的相對角位置關系來表示，用歐拉角來對人體姿態進行描述，歐拉角包括俯仰角θ、橫滾角γ和偏航角ψ，俯仰角是坐標系沿Y軸旋轉的角度，旋轉范圍為-90°～+90°；橫滾角是坐標系繞X軸旋轉的角度，旋轉范圍為-180°～+180°，偏航角是坐標系繞Z軸旋轉的角度，旋轉范圍為0°～360°；

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

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

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

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

三軸加速度計測量的是重力加速度在載體坐標系上的投影，假設加速度計的測量值f_b為：

其中，為加速度計的測量值在載體坐標系X、Y、Z軸方向的投影；

重力加速度在導航坐標系上的投影f_n為：

f_n＝[0 0 g]^T

由于加速度計的輸出與航向角無關，所以姿態矩陣中的航向角為零，將此時的姿態矩陣代入得：

則載體的俯仰角θ和滾轉角γ分別為：

假設磁強計的測量值B_b沿著載體坐標系的三個坐標軸分解得到如下分量：

其中，為磁強計的測量值在載體坐標系X、Y、Z軸方向的投影；

當載體坐標系與導航坐標系重合時為：

其中，為磁強計的測量值在導航坐標系X、Y、Z軸方向的投影；

使載體先繞載體坐標系Y軸旋轉當前俯仰角值，再繞X軸旋轉當前滾轉角值，從而使當前載體與水平面平行，根據坐標變換理論有：

由上式推出地磁場水平分量的表達式為：