[发明专利]一种基于数据融合的头部姿态识别方法

权利要求书

1.一种基于数据融合的头部姿态识别方法，其特征在于：包括以下步骤：

A、采集头部转动过程中精确头部X轴转动角度和精确头部Y轴转动角度；具体为：

a1：头部转动，倾角传感器随动；

a2：采用倾角传感器采集精确头部X轴转动角度和精确头部Y轴转动角度；

B、采集头部转动过程中Z轴转动角度，Z轴转动角度包括陀螺仪Z轴转动角度和红外特征点Z轴转动角度；陀螺仪Z轴转动角度由陀螺仪采集获取，红外特征点Z轴转动角度由红外工业相机采集获取；

C、将陀螺仪Z轴转动角度和红外特征点Z轴转动角度作为数据融合算法的输入量，经过数据融合获取精确头部Z轴转动角度；

所述融合算法的具体过程为：

(a)参数初始化：设置补偿步长θ_STEP取值范围为0.0001～0.1，设置补偿角度θ_CPS(K)＝0；设置循环计数K＝0；

(b)读入陀螺仪输出的Z轴旋转角速度γ_GYRO(K)，对γ_GYRO(K)进行积分运算得到Z轴转角θ_GYRO(K)；

(c)判断红头部姿态是否在红外特征识别范围内，是则读取红外特征识别法计算的Z轴转角θ_IR(K)，否则进入步骤(h)；

(d)判断是否K＝0，是则计算初始角度差θ_INI＝θ_IR(K)-θ_GYRO(K)并返回步骤(b)，否则进入下一步；

(e)计算漂移偏差θ_ERR(K)；漂移偏差θ_ERR(K)计算公式为θ_ERR(K)＝θ_GYRO(K)-θ_IR(K)+θ_INI+θ_CPS(K)；

(f)判断|θ_ERR(K)|是否大于是则进入步骤(g)，否则进入步骤(h)；

(g)判断θ_ERR(K)是否大于0，是则令θ_CPS(K)＝θ_CPS(K-1)-θ_STEP并进入步骤(h)；否则令θ_CPS(K)＝θ_CPS(K+1)-θ_STEP并进入步骤(h)；

(h)计算输出Z轴转角θ_OUT(K)，θ_OUT(K)计算公式为θ_OUT(K)＝θ_GYRO(K)+ θ_INI+θ_CPS(K)；

(i)输出Z轴转角θ_OUT，即精确头部Z轴转动角度，同时，令K加1并返回步骤(b)；

D、输出由精确头部X轴转动角度、精确头部Y轴转动角度和精确头部Z轴转动角度构成的头部姿态参数。

2.根据权利要求1所述的基于数据融合的头部姿态识别方法，其特征在于：所述步骤B中陀螺仪Z轴转动角度的获取过程具体为：

b1.1：头部转动，陀螺仪随动；

b1.2：陀螺仪测量头部Z轴转动角速度；

b1.3：头部Z轴转动角速度经积分获取陀螺仪Z轴转动角度。

3.根据权利要求1所述的基于数据融合的头部姿态识别方法，其特征在于：所述步骤B中红外特征点Z轴转动角度的获取过程具体为：

b2.1：头部转动，红外特征点随动；

b2.2：红外工业相机采集红外特征点图像；

b2.3：采用图像处理算法解算红外特征点图像，获取包括红外特征点Z轴转动角度在内的头部自由度姿态参数。

4.根据权利要求3所述的基于数据融合的头部姿态识别方法，其特征在于：所述步骤b2.3中，解算红外特征点图像采用的图像处理算法包括EPNP方法、POSIT方法或牛顿迭代法中的一种。