本发明公开了一种头戴式人体学输入设备空间位置及姿态测量方法，包括：采集惯性传感器和第一、第二红外面阵相机的信号；检测红外面阵相机是否检测到红外光特征结构。当检测到红外光特征结构时，系统通过图像信息反算头戴设备的空间位置和姿态参数，并更新惯性传感器的状态参数；当未检测到红外光特征结构时，系统根据最近更新的惯性传感器状态参数测量头戴设备的空间位置和姿态参数；最后，系统将参数传给上位机，由上位机实现更具体的人机交互操作。
一种头戴式人体学输入设备空间位置及姿态测量方法
技术领域
[0001] 本发明属于传感器技术领域，尤其涉及一种头戴式人体学输入设备空间位置及姿态测量方法。
背景技术
[0002] 由于当前虚拟现实技术等新兴技术的兴起，人们不再只满足于利用传统电脑鼠标实现与电脑的交互控制。目前，为了实现在虚拟现实世界中的动作，用户依然主要通过鼠标、键盘、手柄等传统人体学输入设备来完成。由于当用户沉浸于虚拟现实中时，用户无法看到鼠标、键盘的按键，由此影响了用户的体验效果。
[0003] 目前已经公开的头戴式电脑光标控制设备主要依靠多轴惯导对头戴设备的空间位置和姿态参数进行测量，从而计算此时光标在屏幕上的坐标位置。由于惯导传感器的输出信号含有一定的偏置量，且该偏置量很难彻底去除，因此对于利用惯导对头戴设备进行姿态、空间参数测量的人体输入学设备，其鼠标的定位误差会随着时间积累，从而使光标出现漂移的情况，影响用户体验。
发明内容
[0004] 针对以上问题，本发明的目的在于，提供本一种头戴式人体学输入设备空间位置及姿态测量方法，以解决在当前虚拟现实人机界面交互中存在的问题。
[0005] 一种头戴式人体学输入设备空间位置及姿态测量方法，包括以下步骤：
[0006] 步骤1，通过安装在头戴人体学输入设备上的红外面阵相机定位红外光发射装置发射的四个红外光亮点；所述红外光发射装置包括4个红外光源阵；
[0007] 步骤11，在图像坐标系下，对红外面阵相机所采集图像中的像素点(u，v)进行边缘检测，得到边缘检测后图像中像素点(u，v)对应的灰度值G(u,v)：
[0008]
[0009]
[0010]
[0011] 其中，f(u,v)为边缘检测前图像中像素点(u，v)对应的灰度值；
[0012] 所述图像坐标系以图左上顶角为原点，水平方向为u轴，垂直方向为v轴；
[0013] 步骤12，设定阈值T，若G(u,v)＞T，则像素点(u，v)为红外光亮点的边缘点；
[0014] 步骤13，通过式(4)、式(5)，计算4个红外光亮点的圆心，其中第i个红外光亮点的圆心为(uoi，voi)；
[0015]
[0016]
[0017] 其中，i＝1,2,3,4；j＝1,2，…,n，uEj为红外光亮点的边缘点u轴坐标值的集合，vEj为红外光亮点的边缘点v轴坐标值的集合；n为红外光亮点的边缘点数量，n为大于等于1的自然数；
[0018] 步骤2，确定红外光亮点在参考坐标系下的坐标(xr,yr,zr)，结合图像坐标系下红外光亮点的圆心(uoi，voi)，计算头戴设备坐标系相对世界坐标系的位姿矩阵，即可得到头戴设备的空间位置和姿态参数，包括：
[0019] 世界坐标系以红外光发射装置中红外光源阵左下方的红外光源为原点Ow，沿底座正视图长边在水平面上投影为Xw轴正方向，沿底座左视图短边在水平面上投影为Yw轴正方向，Zw轴与水平面垂直并向下；
[0020] 参考坐标系以红外光发射装置中红外光源阵左上方的红外光源为原点Or，原点Or沿红外光发射装置的长轴方向为Xr轴正方向，原点Or沿红外光发射装置的短轴方向为Yr轴正方向，依据右手法则构成参考坐标系Zr轴；
[0021] 相机坐标系以相机的光心为原点Oc，Zc轴与相机的光轴重合，并垂直于相机的成像平面，且取相机的摄影方向为正方向，Xc轴与相机的物理成像面坐标系的X轴平行，Yc轴与相机的物理成像面坐标系的Y轴平行；
[0022] 归一化成像平面坐标系所在的归一化成像平面与相机的物理成像面平行，归一化成像平面坐标系的原点O1c为相机光轴与归一化成像平面的交点，X1c轴与相机的物理成像面坐标系的X轴平行，Y1c轴与相机的物理成像面坐标系的Y轴平行；
[0023] 步骤21，通过式(6)，将步骤1得到的红外光亮点的圆心(uoi，voi)变换到归一化成像平面上；
[0024]
[0025] 式(6)中，x1ci、y1ci分别为第i个红外光亮点的圆心在归一化成像平面上的投影，i＝1，2，3，4； 为相机内参数矩阵；
[0026] 其中，ku是沿着u轴方向的放大系数，kv是沿着v轴方向的放大系数，u0、v0是相机光轴中心线与成像平面交点在图像坐标系上的坐标；
[0027] 步骤22，通过式(7)，将参考坐标系下的红外光亮点坐标变换到相机坐标系下：
[0028]
[0029] 其中，xci，yci，zci为第i个红外光亮点的圆心在相机坐标系下的坐标；xri，yri，zri为第i个红外光亮点的圆心在参考坐标系下的坐标；cMr为相机坐标系相对于参考坐标系的位姿矩阵：
[0030]
[0031] 其中，cRr为相机坐标系相对参考坐标系的姿态矩阵，cPr为相机坐标系相对参考坐标系的平移矩阵；
[0032] 进一步地，还包括：
[0033] 步骤31，从安装在红外光发射装置上的三轴加速度计中获取中重力加速度g在参考坐标系下Xr轴上的分量gxr，Yr轴上的分量gyr，Zr轴上的分量gzr；
[0034] 步骤32，利用gxr，gyr，gzr，通过式(14)、(15)得到参考坐标系相对世界坐标系的位姿矩阵
[0035]
[0036]
[0037] 式(14)中， 式(15)中，偏航角ψ为0；
[0038] 偏航角ψ为参考坐标系绕Zr轴转动，其纵轴Yr在水平面上的投影与世界坐标系Yd之间的夹角，顺时针方向为正；
[0039] 俯仰角θ为参考坐标系绕其横轴Xr转动，其纵轴Yr与世界坐标系Yd轴之间的夹角，向上为正；
[0040] 翻滚角 为参考坐标系绕其纵轴Yr转动，其垂直轴Zr与铅锤面之间的夹角，右倾为正；
[0041] 步骤33，相机坐标系相对世界坐标系的位姿矩阵cMw＝cMrrMw；
[0042] 进一步地，还包括：
[0043] 步骤41，通过式(16)得到头戴设备坐标系相对头戴设备参考坐标系的位姿矩阵dMdr_V：
[0044]
[0045] 式(16)中，dMc_V为相机坐标系现对头戴设备坐标系的位姿矩阵，
[0046] wMdr_V为世界坐标系相对头戴设备参考坐标系的位姿矩；
[0047] wMdr_V＝wMd_VdMdr_V，dMdr_V为六轴惯导传感器测得的头戴设备坐标系相对头戴设备参考坐标系的位姿矩阵，wMd_V为世界坐标系相对头戴设备的位姿态矩阵，wMd_V＝(dMc_VcMw_V)-1；
[0048] 所述头戴设备坐标系以六轴惯导传感器的安装位置的几何中心为原点Od，其垂直轴Zd与相机Xc轴平行，Xd与相机坐标系的Zc轴平行，Yd与相机坐标系的Yc轴平行；
[0049] 所述头戴设备参考坐标系以六轴惯导传感器的安装位置的几何中心为原点Odr，垂直轴Zdr正方向相对水平面垂直向上，Xdr轴为设备运行初始时刻，头戴设备的Xd轴在水平面上的投影，Ydr轴为设备运行初始时刻，头戴设备的Yd轴在水平面上的投影；
[0050] 步骤42，设通过六轴惯导传感器得到的头戴设备坐标系相对头戴设
[0051] 备参考坐标系的位姿矩阵为dMdr_I，
[0052] 设 则 则
[0053] 其中，ψ，θ，为通过相机图像得到的欧拉角；ψ′，θ′， 为通过六轴惯导传感器得到的欧拉角；
[0054] 步骤43，设 则通过式(17)得到偏差欧拉角向量AnglesE；
[0055]
[0056] 式(17)中， 为叉积运算符；
[0057] 步骤44，通过式(18)修正偏差欧拉角向量AnglesE：
[0058]
[0059] 式(15)中，α、β和γ为加权修正系数，AnglesE_积分为AnglesE的误差比例积分项，其初值为零；
[0060] 步骤45，对于平动向量dPdr_I和dPdr_V，首先得到平动偏差向量PE：
[0061] PE＝dPdr_IdPdr_V (19)
[0062] 利用PE进行修正
[0063]
[0064] 最后，通过上述方法，可以得到描述头戴式人体学输入设备的所有6个参数:ψ，θ，d d dpcx，pcy，pcz。
[0065] (1)本发明采用结合了图像和惯性传感器的信息，能够避免单纯采用惯性传感器测量带来的累积积分误差；
[0066] (2)本发明利用四个红外光亮点的位置信息来计算头戴设备的位置、姿态，能够避免对人的视觉干扰；
[0067] (3)本发明所述的装置安装有惯性传感器，从而保证装置能够在无法获取到图像信息时依然能够测量设备的空间位置、姿态参数。
附图说明
[0068] 图1为本发明的方法流程图；
[0069] 图2为头戴式人体学输入设备外观图；
[0070] 图3为本发明的红外光亮点发射装置外观示意图；
[0071] 图4为相机坐标系、归一化成像平面坐标系和物理成像平面的坐标示意图；
[0072] 图5为相机图像处理与六轴惯导传感器融合处理的方法流程图；
[0073] 图6为利用红外面阵相机拍摄的图像；
[0074] 图7为经过高亮标识的红外光亮点的轮廓和圆心；
[0075] 图8为偏航角、俯仰角和横滚角的动态变化曲线；
[0076] 下面结合附图和实施例对本发明作进一步的详细阐述。
具体实施方式
[0077] 下面结合具体图示，进一步阐述本发明。
[0078] 实施例1
[0079] 参见图1、2和3，本发明采用以下技术方案得以实现，一种头戴式人体学输入设备空间位置及姿态测量装置，包括：
[0080] 红外光发射装置包括：
[0081] 三轴加速度计(1)，MCU 1(2)，无线传输模块1(3)，红外光二极管(9)；
[0082] 头戴设备包括：
[0083] 红外面阵相机(4)，MCU 2(5)，六轴惯导传感器(6)，无线传输模块2(7)，上位机(8)。
[0084] 其中，
[0085] 所述的三轴加速度计(1)安装于红外光发射底座内部，用于探测底座与世界坐标系之间的位姿关系；
[0086] 所述的MCU 1(2)用于根据三轴加速度计(1)所测数据计算红外光发射底座与世界坐标系的位姿矩阵；
[0087] 所述的无线传输模块1(3)用于实现红外光发射底座与头戴设备之间的通信；
[0088] 所述的红外面阵相机(4)用于探测红外光亮点的位置；
[0089] 所述的MCU2(5)用于计算头戴设备的空间姿态和位置参数；
[0090] 所述的六轴惯导传感器(6)用于采集头戴式人体学输入设备在空间中的旋转角速率和加速度；
[0091] 所述的无线传输模块2(7)用于实现头戴设备与上位机的通信；
[0092] 所述的上位机(8)用于为计算系操作系统中的其他软件获取用户头部空间姿态和位置参数提供软件接口；
[0093] 所述的红外光二极管(9)用于为装置定位提供红外光亮点。
[0094] 具体的，所述的红外光发射底座上有4个红外光二极管，且这些二极管共面；
[0095] 具体的，所述的红外面阵相机(4)安装于头戴设备的前端中央；
[0096] 具体的，所述的三轴加速度计(1)和六轴惯导传感器(6)分别与红外光发射底座和头戴设备捷联；
[0097] 具体的，所述的六轴惯导传感器(6)包括三轴加速度计和三轴陀螺仪。
[0098] 实施例2
[0099] 步骤1，通过安装在头戴人体学输入设备上的红外面阵相机定位红外光发射装置发射的4个红外光亮点；所述红外光发射装置包括4个红外光源阵；
[0100] 步骤11，在图像坐标系下，对红外面阵相机所采集图像中的像素点(u，v)进行边缘检测，得到边缘检测后图像中像素点(u，v)对应的灰度值G(u,v)：
[0101]
[0102]
[0103]
[0104] 其中，f(u,v)为边缘检测前图像中像素点(u，v)对应的灰度值；
[0105] 所述图像坐标系以图左上顶角为原点，水平方向为u轴，垂直方向为v轴；
[0106] 步骤12，设定阈值T，若G(u,v)＞T，则像素点(u，v)为红外光亮点的边缘点；
[0107] 本实施例利用大津法计算动态分割阈值T；
[0108] 步骤13，通过式(4)、式(5)，计算计算4个红外光亮点的圆心，其中红外光亮点的圆心为(uoi，voi)；
[0109]
[0110]
[0111] 其中，j＝1,2，…,n，uEj为红外光亮点的边缘点u轴坐标值的集合，vEj为红外光亮点的边缘点v轴坐标值的集合；n为红外光亮点的边缘点数量，n为大于等于1的自然数；
[0112] 图6为利用红外面阵相机拍摄的图像，图7为经过高亮标识的红外光亮点的轮廓和圆心，其中，计算得到的4个红外光亮点圆心在图像坐标系中的坐标为：
[0113] 序号 1 2 3 4坐标 (500,362) (664,378) (482,582) (644,598)
[0114] 步骤2，确定红外光亮点在参考坐标系下的坐标(xr,yr,zr)，结合图像坐标系下红外光亮点的圆心(uoi，voi)，计算头戴设备坐标系相对世界坐标系的位姿矩阵，即可得到头戴设备的空间位置和姿态参数，包括：
[0115] 世界坐标系以红外光发射装置中红外光源阵左下方的红外光源为原点Ow，沿底座正视图长边在水平面上投影为Xw轴正方向，沿底座左视图短边在水平面上投影为Yw轴正方向，Zw轴与水平面垂直并向下；
[0116] 参考坐标系以红外光发射装置中红外光源阵左上方的红外光源为原点Or，原点Or沿红外光发射装置的长轴方向为Xr轴正方向，原点Or沿红外光发射装置的短轴方向为Yr轴正方向，依据右手法则构成参考坐标系Zr轴；
[0117] 相机坐标系以相机的光心为原点Oc，Zc轴与相机的光轴重合，并垂直于相机的成像平面，且取相机的摄影方向为正方向，Xc轴与相机的物理成像面坐标系的X轴平行，Yc轴与相机的物理成像面坐标系的Y轴平行；
[0118] 归一化成像平面坐标系所在的归一化成像平面与相机的物理成像面平行，归一化成像平面坐标系的原点O1c为相机光轴与归一化成像平面的交点，X1c轴与相机的物理成像面坐标系的X轴平行，Y1c轴与相机的物理成像面坐标系的Y轴平行；
[0119] 步骤21，通过式(6)，将步骤1得到的红外光亮点的圆心(uoi，voi)变换到归一化成像平面上；
[0120]
[0121] 式(6)中，x1ci、y1ci分别为红外光亮点的圆心在归一化成像平面上的投影，Min为相机内参数矩阵；本实施例采用的是4参数相机内参数模型。
[0122] 其中，ku是沿着u轴方向的放大系数，kv是沿着v轴方向的放大系数，u0、v0是相机光轴中心线与成像平面交点在图像坐标系上的坐标；
[0123] 步骤22，通过式(8)，将参考坐标系下的红外光亮点的圆心坐标变换到相机坐标系下：
[0124]
[0125] 其中，cMr为相机坐标系相对于参考坐标系的位姿矩阵：
[0126]
[0127] 其中， 为相机坐标系相对参考坐标系的姿态矩阵cRr， 为相机坐标系相对参考坐标系的平移矩阵cPr；
[0128] 将公式(6)和(7)写成方程组的形式，分别得到
[0129]
[0130]
[0131] 将公式(10)带入公式(9)，得到
[0132]
[0133] 对于每个点，都有一组如公式(10)所示的方程。将所有特征点对应的方程合并，写成矩阵的形式，得到
[0134] A1H1+A2H2＝0 (12)
[0135] 式中，
[0136]
[0137]
[0138] H1＝[nx ny nz]T
[0139] H2＝[ox oy oz Px py pz]T
[0140] 其中，A1和A2矩阵中的参数均为测量参数或已知参数，H1和H2中的参数则包含了所求未知量。只要求解出H1和H2所构成的向量H，就能解算出相机坐标系相对于参考坐标系的c旋转变换矩阵 Mr中的第1、2、4列，其第3列的值通过计算1、2列的叉乘即可得到。
[0141] 本实施例中，四个红外光亮点的圆心在参考坐标系下的坐标(xri，yri，zri)为:(从左上角的红外光亮点开始，逆时针)(0,0,0)，(124,0,0)，(0,68,0)，(124,68,0)，单位为mm。
[0142] H1和H2的解由以下公式给出：
[0143]
[0144] 式(13)中， λ为矩阵B的最小特征值。
[0145] 本实施例中，计算得到的相机内参数矩阵为：
[0146]
[0147] 则根据之前测量得到的红外光亮点在图像坐标系中的坐标，计算得到
[0148]
[0149]
[0150] 实施例3
[0151] 由于红外光发射装置可能存在不是垂直于水平面的情况，为了避免误差的产生，所以本实施例构建了世界坐标系，计算相机坐标系相对世界坐标系的位姿矩阵，本实施例在实施例3的基础上，还包括：
[0152] 步骤31，从安装在红外光发射装置上的三轴加速度计中获取中重力加速度g在参考坐标系下Xr轴上的分量gxr，Yr轴上的分量gyr，Zr轴上的分量gzr；
[0153] 步骤32，由于重力作用方向始终与世界坐标系的Zw轴重合，当参考坐标系相对世界坐标系转动时，可以利用gxr，gyr，gzr，通过式(14)、(15)得到参考坐标系相对世界坐标系的位姿矩阵
[0154] 由于重力作用方向始终与世界坐标系的Zw轴重合，此时gxr，gyr，gzr的值就是rRw矩阵中的第三列，即
[0155]
[0156]
[0157] 式(14)中， 式(15)中，在本发明中，参考坐标系相对世界坐标系的偏航角ψ为0；
[0158] 关于欧拉角的定义如下：
[0159] 偏航角ψ为参考坐标系绕Zr轴转动，其纵轴Yr在水平面上的投影与世界坐标系Yd之间的夹角，顺时针方向为正；
[0160] 俯仰角θ为参考坐标系绕其横轴Xr转动，其纵轴Yr与世界坐标系Yd轴之间的夹角，向上为正；
[0161] 翻滚角 为参考坐标系绕其纵轴Yr转动，其垂直轴Zr与铅锤面之间的夹角，右倾为正；
[0162] 步骤33，其中，由于参考坐标系与世界坐标系的原点重合，故位姿矩阵中的平移向量P＝0，从而得到 进一步地，得到相机坐标系相对世界坐标系的位姿矩阵cMw＝cMrrMw；
[0163] 实施例4
[0164] 本实施例在实施例2或实施例3的基础上，还包括：
[0165] 步骤41，通过式(16)得到头戴设备坐标系相对头戴设备参考坐标系的位姿矩阵dMdr_V：
[0166]
[0167] 式(16)中，dMc_V为相机坐标系现对头戴设备坐标系的位姿矩阵，
[0168] wMdr_V为世界坐标系相对头戴设备参考坐标系的位姿矩；
[0169] wMdr_V＝wMd_VdMdr_V，dMdr_V为六轴惯导传感器测得的头戴设备坐标系相对头戴设备参考坐标系的位姿矩阵，wMd_V为世界坐标系相对头戴设备的位姿态矩阵，wMd_V＝(dMc_VcMw_V)-1；
[0170] 所述头戴设备坐标系以六轴惯导传感器的安装位置的几何中心为原点Od，其垂直轴Zd与相机Xc轴平行，Xd与相机坐标系的Zc轴平行，Yd与相机坐标系的Yc轴平行；
[0171] 所述头戴设备参考坐标系以六轴惯导传感器的安装位置的几何中心为原点Odr，垂直轴Zdr正方向相对水平面垂直向上，Xdr轴为设备运行初始时刻，头戴设备的Xd轴在水平面上的投影，Ydr轴为设备运行初始时刻，头戴设备的Yd轴在水平面上的投影；
[0172] 步骤42，初始化红外面阵相机和六轴惯导传感器，设置与红外面阵相机通信的总线传输速率，以及捷联惯导传感器的数据更新频率，加速度、角速率测量范围等参数。
[0173] 系统采集六轴惯导传感器的数据；该数据包括头戴设备绕头戴设备坐标系三个轴的角速率，以及沿三个轴所指方向的加速度。
[0174] 系统采集完六轴惯导传感器的信号后，检测相机的位姿矩阵cMw是否更新。如果更新，结合由相机图像给出的位姿矩阵和由六轴惯导传感器给出的位姿矩阵，计算头戴设备相对世界坐标系的位姿矩阵；否则单独利用六轴惯导传感器的信号，估计头戴设备的位姿矩阵。
[0175] 结合由相机图像给出的位姿矩阵和由六轴惯导传感器给出的位姿矩阵，计算头戴设备相对世界坐标系的位姿矩阵，包括：
[0176] 设通过六轴惯导传感器得到的头戴设备坐标系相对头戴设备参考坐标系的位姿矩阵为
[0177] 设 则 则
[0178] 其中，ψ，θ，为通过相机图像得到的欧拉角；ψ′，θ′， 为通过六轴惯导传感器得到的欧拉角；
[0179] 步骤43，设 则通过式(17)得到偏差欧拉角向量AnglesE；
[0180]
[0181] 式(17)中， 为叉积运算符；
[0182] 步骤44，通过式(18)修正偏差欧拉角向量AnglesE：
[0183]
[0184] 式(18)中，α、β和γ为加权修正系数，AnglesE_积分为AnglesE的误差比例积分项，其初值为零；
[0185] 如图8所示，该图显示了采用上述方法计算得到的欧拉角曲线随时间的变化情况，黑色曲线表示采用上述方法修正后的欧拉角。
[0186] 步骤45，对于平动向量dPdr_I和dPdr_V，首先得到平动偏差向量PE：
[0187] PE＝dPdr_IdPdr_V (19)
[0188] 利用PE进行修正
[0189]
[0190] 最后，通过上述方法，可以得到描述头戴式人体学输入设备的所有6个参数:ψ，θ，dpcx，dpcy，dpcz。
