一种头戴式设备的抗磁干扰相对偏航角自检测方法

1.本发明涉及一种头戴式设备的抗磁干扰相对偏航角自检测方法，属于传感器融合技术领域。


背景技术：

2.工程机械遥操作技术中，随动云台的远程姿态控制需要作业人员穿戴vr头戴式设备采集头部姿态。当作业人员处于某种狭域环境下，头戴式设备的imu模块易受到环境因素影响，特别地，外界设备的磁场干扰会导致z轴的偏航角漂移严重，进而导致传输到现场的数据存在误差。专利201810003966.9提出了一种确定姿态的方法，通过imu与最近一次设备的姿态确定第一姿态,接下来imu磁力计的第一磁力向量从存储的姿态映射表中查找第二姿态,基于第一姿态和第二姿态,确定设备当前的姿态，该方法对周围环境磁场的要求比较宽松,但是只是通过imu获取姿态角，对设备可靠性要求很高。专利202010276652.3提出了一种采用光流测量三维角运动的方法，利用指向不共面的三个及以上的光流传感器，得到对应的x轴、y轴和z轴上对应的角增量比例式，最终得到对应的三维角增量，该方法没有考虑到设备的平移和复杂旋转带来的光流数据变化影响，适用场景较为单一。本专利提出一种头戴式设备的抗磁干扰相对偏航角自检测方法，以抑制磁干扰导致的z轴偏航角偏差，进而提高头部随动控制精度。


技术实现要素：

3.本发明的目的是提供一种头戴式设备的抗磁干扰相对偏航角自检测方法，以克服磁场干扰导致的偏航角数据漂移，提高数据的精确性。
4.为了实现上述目的，本发明采用的技术方案是：
5.一种头戴式设备的抗磁干扰相对偏航角自检测方法，包括以下步骤:
6.s1、作业人员穿戴头戴式设备，所述头戴式设备上安装有光流测距模块1、光流测距模块2和光流测距模块3，所述光流测距模块1、光流测距模块2和光流测距模块3均包括光流模块和激光测距模块；
7.s2、将所述光流测距模块1和光流测距模块2所测的数据滤波后得到偏航角角度；
8.s3、对头戴式设备的平移以及复杂运动导致的光流数据变化建立约束方程进行抑制；
9.s4、将步骤s2得到的偏航角角度经步骤s3抑制后与imu测得的偏航角角度进行融合输出；
10.s5、光流绝对静止时数据不改变，对imu静止时的磁场变化导致的偏航角数据漂移加以抑制。
11.本发明技术方案的进一步改进在于：所述头戴式设备包括外部头盔、嵌入外部头盔内部的主控芯片、外部头盔顶部的9轴imu模块、光照强度传感器和三个光流测距模块，所述三个光流测距模块分别安装在外部头盔的右、前、上侧，两两正交且中心共面，分别为光
流测距模块1、光流测距模块2和光流测距模块3。
12.本发明技术方案的进一步改进在于：所述步骤s2的具体步骤为：
13.根据δts时间内光流测距模块1和光流测距模块2的x轴方向镜头内像素变化量得到x轴方向角速度变化量δω1和δω2，取二者均值，并对角速度积分得到角度变化量δθi，对角度变化量累积再进行坐标转换，把光流模块得到自身坐标系下的角度转换到地理坐标系下，得到yaw
of
：
[0014][0015]
本发明技术方案的进一步改进在于：所述步骤s3的具体操作步骤为：
[0016]
假设光流模块的坐标系与设备机体坐标系对齐，光流模块的视场角为θ，视场平面对应的像素总数为n
p
，那么光流模块经过采样时间δts得到的x轴和y轴方向上像素个数变化量(dx,dy)与角速度(w
x
,wy)的关系为：
[0017][0018]
得到
[0019][0020]
其中，n
p
会随着光流模块到物体距离l的改变而发生变化，由三角形相似原理得到二者的关系为：
[0021][0022]
其中，n
x
为摄像头固定视野内像素个数，f为镜头焦距，由具体传感器的参数而定；代入得到：
[0023][0024]
而光流模块像素变化与线速度的关系与采样时间δts的关系为：
[0025]kv
＝δts[0026]
从而得到光流模块的特定系数k
wkv
，建立头戴式设备全对称坐标系，把设备旋转某个角度方向的速度分解为v
x
和vy，根据右手定则得到整体的约束方程：
[0027][0028]
式中dx1、dy1、dx2、dy2、dx3、dy3分别为光流测距模块1、光流测距模块2和光流测距模块3经过采样时间δts得到的x轴和y轴方向上各自像素个数变化量，x轴和y轴角速度可由imu数据获得，且误差较小可信度高，故求解方程得到三轴线速度与z轴角速度，判断出光流数据变化是否由平移导致，进而加以约束。
[0029]
本发明技术方案的进一步改进在于：所述步骤s4的具体步骤为：
[0030]
①
当imu中的磁力计测得外界磁场强度不超过设定的阈值k
mag
且磁场强度变化小时，采用光流模块输出的偏航角；当光照强度传感器测得光照强度超过设定的下限值k
of
且光照强度变化小，采用imu输出的偏航角；
[0031]
②
当imu与光流输出的偏航角均可信，此时把数据融合以降低误差，利用卡尔曼滤波原理融合，建立方程：
[0032]
zk＝z1+kk(z
2-z1)
[0033]
令z1为imu测得的角度数据，z2为光流模块测得的角度数据，目的是求kk使得估计值的标准最少，具体为求出估计值方差的最小值，对kk求导，然后令它为0，求得极值，得到卡尔曼增益，然后计算估计值，最后更新—下估计值的方差，循环迭代。
[0034]
本发明技术方案的进一步改进在于：所述步骤s5的具体步骤为：
[0035]
yaw＝k
of
*z1+z2[0036]
当设备运动时，光流数据变化，k
of
为1，输出的偏航角为imu与光流模块融合输出的结果，此时姿态角角度会随着设备的动作不断变化；
[0037]
当设备停止运动时,此时光流数据不变化，k
of
为0，从而可用光流输出的数据判断设备是否动作，进而消去因磁场干扰导致imu输出的偏航角角度的漂移。
[0038]
由于采用了上述技术方案，本发明取得的技术效果有：
[0039]
本发明利用两个光流模块采集数据的均值输出z轴偏航角角速度，对角速度积分并且滤波后获得偏航角角度，与imu输出的偏航角根据不同的现场环境进行分时复用或者融合输出，同时针对设备的平移以及复杂运动导致的光流数据误差，建立了约束方程进行抑制，最后提出利用光流绝对静止时数据不改变的原理，对imu静止时的磁场变化导致的偏航角数据漂移加以抑制，提高了数据的精确性。
附图说明
[0040]
图1是本发明的流程示意图；
[0041]
图2是头戴式设备的硬件结构示意图；
[0042]
图3是头戴式设备旋转α角度下的坐标系图；
[0043]
图4是光流视野几何关系图；
[0044]
图5是头戴式设备全对称坐标系图。
具体实施方式
[0045]
下面结合附图及具体实施例对本发明做进一步详细说明，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。
[0046]
一种头戴式设备的抗磁干扰相对偏航角自检测方法，如图1所示，包括以下步骤：
[0047]
s1、作业人员穿戴头戴式设备，如图2所示，所述头戴式设备包括：外部头盔，嵌入外部头盔内部的主控芯片stm32f103c8t6，外部头盔顶部的9轴imu模块jy901s、光照强度传感器gy-302和三个光流测距模块，所述光流测距模块为光流模块pmw3901和激光测距模块vl53l0x集成而得。其中，三个光流测距模块分别安装在外部头盔的右、前、上侧，两两正交且中心共面，分别为光流测距模块1、光流测距模块2和光流测距模块3。
[0048]
s2、将光流测距模块1和光流测距模块2所测的数据滤波后得到偏航角角度，具体步骤如下：
[0049]
由于光流模块原始数据为摄像头镜头内δts(数据采集周期，与具体光流模块有关)时间内流过的像素点个数，易得到δts内光流测距模块1和光流测距模块2的x轴方向镜头内流过的像素点个数of
1x_sum
和of
2x_sum
与角速度的变化量的关系：
[0050][0051]
其中，δω1和δω2分别为δts内光流测距模块1和光流测距模块2的x轴方向角速度变化量。
[0052]
取二者均值，并对角速度积分得到角度变化量δθi[0053][0054]
对角度变化量累积，得到头戴式设备坐标系下的角度z
of
：
[0055][0056]
由于imu给出的欧拉角是地理坐标系旋转的值，而光流模块得到的角速度是围绕自身坐标系的坐标轴旋转的，因此角度需要坐标转换，最终得到地理坐标系下光流模块输出的偏航角：
[0057][0058]
由于光流模块在求偏航角角度时是对角速度进行积分，所以存在积分误差，无法
长时间信任，因此需要利用卡尔曼滤波对光流输出数据进行滤波，给出具体方法：
[0059]
只关注z轴偏航角角度,系统状态矩阵为：
[0060][0061]
其中，θ表示偏航角度，δωb(b代表bias)代表角速度偏差，这里的角速度偏差具体指光流模块数据的漂移量。
[0062]
①
建立偏航角角度测量模型方程:
[0063]
yaw
of
＝yaw
of
+(δω-δωb)*δts[0064]
即角度估计值＝上一次角度估计值+(当前角速度
–
角速度漂移)*dt
[0065]
每一次的漂移δωb是—样的，用θ代替yaw
of
[0066]
θ＝θ+(δω-δωb)*δts[0067]
得到方程：
[0068][0069]
②
状态预测方程：
[0070][0071]
对比可得到：
[0072][0073]
③
协方差矩阵预测方程：
[0074][0075]
其中，pk是协方差矩阵
[0076][0077]
初始时可认为状态确定，误差协方差矩阵为：
[0078][0079]
接下来就是过程噪声协方差矩阵qk：
[0080][0081]
假设角度和角速度偏差相互独立，所以对角线上的协方差为0，剩下的就是角度方差q
θ
和角速度偏差的方差这两个值是极其关键的参数，需要人为确定。
[0082]
④
计算残差：
[0083][0084]
这里计算实际观测值zk和预测值之间的差，zk就是光流模块测量出的角度，h为测量矩阵
[0085]
h＝[1 0]
[0086]
⑤
计算卡尔曼增益：
[0087][0088]
把协方差矩阵映射到观测空间来计算,r为测量噪声，注意r不是一个矩阵，测量噪声和测量的方差相等，这个值也是第三个关键参数，通过传感器厂家提供,这里用r
mea
来表示。
[0089]
⑥
状态更新方程：
[0090][0091]
代表此刻的最佳估计，等于上一时刻对此刻的预测加上卡尔曼增益kk乘以残差乘以残差可以循环到下-次的状态预测过程使用，从而实现迭代。
[0092]
⑦
协方差矩阵更新方程：
[0093]
p
k,k
＝(i-kkh)p
k,k-1
[0094]
卡尔曼增益是一个n*1的矩阵，要转为2*2矩阵来计算，乘测量矩阵。
[0095]
s3、对头戴式设备的平移以及复杂运动导致的光流数据变化，建立约束方程进行抑制。
[0096]
建立图3所示头戴式设备旋转α角度下的坐标系，仅考虑z轴旋转可得如下方程：
[0097][0098]
其中分别为在x-o-y平面的线速度和角速度，α是速度运动方向，kv和kw是光流模块像素变化与线速度和角速度的关系系数,下面给出kv和kw的确定方法：
[0099]
如图4所示的光流视野几何关系图，假设光流模块的坐标系与设备机体坐标系对齐，光流模块的视场角为θ(弧度)，视场平面对应的像素总数为n
p
，那么光流模块经过采样时间δts得到的x轴和y轴方向上像素个数变化量(dx,dy)与角速度(w
x
,wy)的关系为：
[0100][0101]
得到
[0102][0103]
而n
p
会随着光流模块到物体距离l的改变而发生变化，由图3三角形相似原理可得到二者的关系为：
[0104][0105]
其中，n
x
为摄像头固定视野内像素个数，f为镜头焦距，都由具体传感器的参数而定。代入得到：
[0106][0107]
而光流模块像素变化与线速度的关系与采样时间δts的关系为：
[0108]kv
＝δts[0109]
再建立如图5所示的头戴式设备全对称坐标系，把α方向的速度分解为v
x
和vy，根据右手定则得到整体的约束方程：
[0110][0111]
拆解得到
[0112][0113]
即：
[0114][0115]
式中dx1、dy1、dx2、dy2、dx3、dy3分别为光流测距模块1、光流测距模块2和光流测距模块3经过采样时间δts得到的x轴和y轴方向上各自像素个数变化量，imu可以提供x和y两个轴的准确角度和角速度，因此代入w
x
wy，使用三个光流模块建立四组线性方程组可求解出v
x
vyvzwz，对平移以及复杂运动导致的wz变化进行约束。
[0116]
s4、将步骤s2得到的偏航角角度经步骤s3抑制后与imu测得的偏航角角度进行融合输出，具体方法包括：
[0117]
①
当imu中的磁力计测得外界磁场强度不超过设定的阈值k
mag
(以imu数据是否发生严重漂移为准，一般设定为0.6gauss)且磁场强度变化小时，imu测得的偏航角是可信的，反之不可信，采用光流模块输出的偏航角。
[0118]
②
在头戴式vr设备外围安装光照强度传感器，当光照强度超过设定的下限值k
of
(一般设定为60lux)且光照强度变化小，此时光流输出偏航角可信，反之不可信，继续采用imu输出的偏航角。
[0119]
③
当imu与光流输出的偏航角均可信，此时把数据融合以降低误差，利用卡尔曼滤波原理融合，建立方程：
[0120]
zk＝z1+kk(z
2-z1)
[0121]
可令z1为imu测得的角度数据，z2为光流模块测得的角度数据，目的是求kk使得估计值的标准最少，也就是使得估计值的方差最小，即估计值的方差：
[0122][0123]
要求出估计值方差的最小值，需要对kk求导，然后令它为0，就可以求得极值，得到卡尔曼增益：
[0124][0125]
先求值kk卡尔曼增益，然后计算估计值，最后更新—下估计值的方差，循环迭代。
[0126]
s5、光流绝对静止时数据不改变，可对imu静止时的磁场变化导致的偏航角数据漂移加以抑制，参考图1。
[0127]
yaw＝k
of
*z1+z2[0128]
当设备运动时，光流数据变化，k
of
为1，输出的偏航角为imu与光流模块融合输出的结果，此时姿态角角度会随着设备的动作不断变化。
[0129]
当设备停止运动时,此时光流数据不变化，k
of
为0，从而可用光流输出的数据判断设备是否动作，进而消去因磁场干扰导致imu输出的偏航角角度的漂移。
[0130]
本发明利用两个光流模块采集数据的均值输出z轴偏航角角速度，对角速度积分并且滤波后获得偏航角角度，与imu输出的偏航角根据不同的现场环境进行分时复用或者融合输出，同时针对设备的平移以及复杂运动导致的光流数据误差，建立了约束方程进行抑制，最后提出利用光流绝对静止时数据不改变的原理，对imu静止时的磁场变化导致的偏航角数据漂移加以抑制，提高了数据的精确性。
[0131]
以上所揭露的仅为本发明一种较佳实例而已，当然不能以此来限定本发明之权利范围，本领域普通技术人员可以理解实现上述实施例的全部或部分流程，并依本发明权利要求所作的等同变化，仍属于发明所涵盖的范围。