[發(fā)明專利]一種姿態(tài)矯正方法及裝置

在審

• 申請?zhí)枺?/td>: 201510405200.X
• 申請日: 2015-07-10
• 公開（公告）號: CN105043383A
• 公開（公告）日: 2015-11-11
• 發(fā)明（設(shè)計）人: 劉平;陳偉華;王勃然;孫程
• 申請（專利權(quán)）人: 哈爾濱醫(yī)科大學(xué);陳偉華;王勃然;黑龍江驛貸同科技發(fā)展有限公司
• 主分類號: G01C21/00
• 分類號: G01C21/00;G06F19/00;G08B21/24
• 代理公司: 北京隆源天恒知識產(chǎn)權(quán)代理事務(wù)所(普通合伙) 11473
• 代理人: 閆冬

1.一種姿態(tài)矯正方法，其特征在于，包括：

步驟a、檢測頭部擺動造成的姿態(tài)參數(shù)的數(shù)值，所述姿態(tài)參數(shù)包括航向角、橫滾角和俯仰角的角度數(shù)值；

步驟b、對測得的所述姿態(tài)參數(shù)進(jìn)行參數(shù)處理；

步驟c、對處理后的所述姿態(tài)參數(shù)進(jìn)行解讀，獲得矯正者的各個所述角度數(shù)值；

步驟d、利用卡爾曼濾波器對各個所述角度數(shù)值進(jìn)行消除噪聲處理；

步驟e、對所述角度數(shù)值進(jìn)行實時融合處理，得到實時動態(tài)姿態(tài)數(shù)據(jù)；

步驟f、將所述實時動態(tài)姿態(tài)數(shù)據(jù)與預(yù)設(shè)姿態(tài)數(shù)據(jù)進(jìn)行比較，若當(dāng)前姿態(tài)不在預(yù)設(shè)姿態(tài)范圍內(nèi)，計時指定時間，若不符合預(yù)設(shè)姿態(tài)數(shù)據(jù)持續(xù)時間超過指定時間時，進(jìn)行報警提示。

2.根據(jù)權(quán)利要求1所述的姿態(tài)矯正方法，其特征在于，所述步驟a中所述航向角、所述橫滾角和所述俯仰角的確定方法為：以矯正者的頸部為原點，以右側(cè)為x軸正方向，以前方為y軸的正方向，以上方為z軸的正方向，頭部繞所述x軸旋轉(zhuǎn)的角度為所述俯仰角，頭部繞所述y軸旋轉(zhuǎn)的角度為所述橫滾角，頭部繞所述z軸旋轉(zhuǎn)的角度為所述航向角。

3.根據(jù)權(quán)利要求2所述的姿態(tài)矯正方法，其特征在于，所述航向角、所述橫滾角、所述俯仰角的測量范圍均為±180°。

4.根據(jù)權(quán)利要求1-3中任一所述的姿態(tài)矯正方法，其特征在于，所述預(yù)設(shè)姿態(tài)數(shù)據(jù)中，所述俯仰角的預(yù)設(shè)范圍為15°～20°。

5.根據(jù)權(quán)利要求1-3中任一所述的姿態(tài)矯正方法，其特征在于，所述預(yù)設(shè)姿態(tài)數(shù)據(jù)中，所述橫滾角的預(yù)設(shè)范圍為-5°～5°。

6.根據(jù)權(quán)利要求1-3中任一所述的姿態(tài)矯正方法，其特征在于，所述預(yù)設(shè)姿態(tài)數(shù)據(jù)中，所述航向角的預(yù)設(shè)范圍為-5°～5°。

7.根據(jù)權(quán)利要求1-3中任一所述的姿態(tài)矯正方法，其特征在于，所述步驟f中，所述指定時間為30s。

8.根據(jù)權(quán)利要求1-3中任一所述的姿態(tài)矯正方法，其特征在于，所述步驟b中，所述參數(shù)處理為對所述姿態(tài)參數(shù)進(jìn)行處理得到實際參數(shù)，所述實際參數(shù)的計算公式為：

A=Σj=1nAjNkjΣj=1nNkjα+β]]>

其中，N_kj由下述公式確定：

Pi=Σj=1nNij]]>

P_k＝max{P_i}i∈{1,2,…,n}

上式中，A表示所求的實際參數(shù)，i、j、k均表示測量參數(shù)的序號，所述測量參數(shù)為所述姿態(tài)參數(shù)，其中k表示最佳中心參數(shù)對應(yīng)的序號，n表示所述測量參數(shù)的數(shù)量，m表示百分比，A_i、A_j分別表示第i、j個測量參數(shù)，M_ij表示第j個測量參數(shù)對第i個測量參數(shù)的判斷中間值，N_ij表示第j個測量參數(shù)對第i個測量參數(shù)的判斷值，N_kj表示第j個測量參數(shù)對最佳中心參數(shù)的判斷值，P_i表示所有測量參數(shù)(第i個測量參數(shù)除外)對第i個測量參數(shù)的判斷值之和，P_k表示所有測量參數(shù)(第k個測量參數(shù)除外)對最佳中心參數(shù)的判斷值之和，α表示修正因子，β表示修正值。