[发明专利]一种姿态矫正方法及装置

权利要求书

1.一种姿态矫正方法，其特征在于，包括：

步骤a、检测头部摆动造成的姿态参数的数值，所述姿态参数包括航向角、横滚角和俯仰角的角度数值；

步骤b、对测得的所述姿态参数进行参数处理；

步骤c、对处理后的所述姿态参数进行解读，获得矫正者的各个所述角度数值；

步骤d、利用卡尔曼滤波器对各个所述角度数值进行消除噪声处理；

步骤e、对所述角度数值进行实时融合处理，得到实时动态姿态数据；

步骤f、将所述实时动态姿态数据与预设姿态数据进行比较，若当前姿态不在预设姿态范围内，计时指定时间，若不符合预设姿态数据持续时间超过指定时间时，进行报警提示。

2.根据权利要求1所述的姿态矫正方法，其特征在于，所述步骤a中所述航向角、所述横滚角和所述俯仰角的确定方法为：以矫正者的颈部为原点，以右侧为x轴正方向，以前方为y轴的正方向，以上方为z轴的正方向，头部绕所述x轴旋转的角度为所述俯仰角，头部绕所述y轴旋转的角度为所述横滚角，头部绕所述z轴旋转的角度为所述航向角。

3.根据权利要求2所述的姿态矫正方法，其特征在于，所述航向角、所述横滚角、所述俯仰角的测量范围均为±180°。

4.根据权利要求1-3中任一所述的姿态矫正方法，其特征在于，所述预设姿态数据中，所述俯仰角的预设范围为15°～20°。

5.根据权利要求1-3中任一所述的姿态矫正方法，其特征在于，所述预设姿态数据中，所述横滚角的预设范围为-5°～5°。

6.根据权利要求1-3中任一所述的姿态矫正方法，其特征在于，所述预设姿态数据中，所述航向角的预设范围为-5°～5°。

7.根据权利要求1-3中任一所述的姿态矫正方法，其特征在于，所述步骤f中，所述指定时间为30s。

8.根据权利要求1-3中任一所述的姿态矫正方法，其特征在于，所述步骤b中，所述参数处理为对所述姿态参数进行处理得到实际参数，所述实际参数的计算公式为：

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

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

Pi=Σj=1nNij]]>

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

上式中，A表示所求的实际参数，i、j、k均表示测量参数的序号，所述测量参数为所述姿态参数，其中k表示最佳中心参数对应的序号，n表示所述测量参数的数量，m表示百分比，A_i、A_j分别表示第i、j个测量参数，M_ij表示第j个测量参数对第i个测量参数的判断中间值，N_ij表示第j个测量参数对第i个测量参数的判断值，N_kj表示第j个测量参数对最佳中心参数的判断值，P_i表示所有测量参数(第i个测量参数除外)对第i个测量参数的判断值之和，P_k表示所有测量参数(第k个测量参数除外)对最佳中心参数的判断值之和，α表示修正因子，β表示修正值。