[发明专利]智能机姿态测定及虚拟现实漫游方法

摘要

本发明提供一种智能机姿态测定及虚拟现实漫游方法，用户在三维空间中以任意姿态摆放智能机都能在视窗内观察到与该位姿对应的周边真实或虚拟三维渲染场景。由于用户手持智能机的姿态与三维场景中渲染的三维效果完全一致，用户会体验到一种极其逼真的身临其境三维效果。利用智能机的姿态控制观察角度，手指滑动或移动控制智能机位置，用户可在三维场景中进行漫游，基于本发明可测定智能机的姿态，并实现三维数字城市漫游，商铺广告信息三维发布、虚拟现实电子商务、游戏控制、虚拟地图旅游、摄影测量测绘等。

主权项

智能机姿态测定方法，包括以下步骤：1)定义局部坐标系：局部坐标系是指原点在智能机，Z轴指向地球表面法方向,Y轴或X轴与纬线相平行的坐标系；或局部坐标系原点位于地球上任意一点，Z轴与重力方向一致，且，若智能机磁力计指示方向是北，则北代表局部坐标系X轴，西代表局部坐标系Y轴；若智能机磁力计指示方向是东，则东代表局部坐标系X轴，北代表局部坐标系Y轴；若智能机磁力计指示方向是南，则南代表局部坐标系X轴，东代表局部坐标系Y轴；若智能机磁力计指示方向是西，则西代表局部坐标系X轴，南代表局部坐标系Y轴；2)测定智能机姿态矩阵Rg：Rg是一个相对于局部坐标系的3x3单位正交矩阵，Rg由下法之一获得：方法(1)采用重力传感器与磁力计或方向传感器：如果智能机有重力传感器，且有磁力计或方向传感器，用以下公式(1)计算Rg：其中，Rθ=sazL1-(ay/L)2-saxay/[L21-(ay/L)2]ax/L0s1-(ay/L)2ay/L-saxL1-(ay/L)2-sazay/[L21-(ay/L)2]az/L,]]>axayaz=RaTvaxvayvaz,]]>vax,vay,vaz是智能机测出来的智能机重力加速度传感器的x,y,z三个方向的值，或是这三个值经滤波的结果；Ra是重力传感器芯片相对于智能机的姿态转换矩阵；若智能机检测到的重力加速度传感器定义为负值：当az≤0时，s＝1，az>0时,s＝‑1；若智能机检测到的重力加速度传感器定义为正值：当az≤0时，s＝‑1，az>0时,s＝1；若采用方向传感器：带入公式(1)计算Rg；若采用磁力计传感器：计算mxmymz=RmTvmxvmyvmz,]]>其中Rm是磁力传感器芯片相对于智能机的姿态转换矩阵；{vmx,vmy,vmz}是智能机上磁力传感器检测到的三个值或这三个值经滤波的结果；再计算m0xm0ym0z=RθTmxmymz,]]>利用磁力计计算方位角的三角函数为：cosφ=m0xm0x2+m0y2,sinφ=m0ym0x2+m0y2,]]>带入公式(1)即可算出Rg；若既无磁力计，也无方向传感器，方位角角初值为0，当模拟逆时针绕重力方向旋转时增加角，模拟顺时针绕重力方向旋转则减小角；方法(2)用旋转矢量传感器：如果智能机配置有旋转矢量传感器，首先采用下述(a)(b)(c)中的一种方式得到Rg0矩阵：(a)如果检测到智能机上的旋转矢量传感器只有3个数据values[0],values[1],values[2]，采用以下公式得到：q1＝values[0],q2＝values[1],q3＝values[2],q0=1-q12-q22-q32,]]>则Rg0矩阵为:Rg0=q02+q12-q22-q322(q1q2+q0q3)2(q1q3-q0q2)2(q1q2-q0q3)q02-q12+q22-q322(q2q3+q0q1)2(q1q3+q0q2)2(q2q3-q0q1)q02-q12-q22+q32;]]>(b)如果检测智能机上的旋转矢量传感器有4个数据，q0＝values[3],q1＝values[0],q2＝values[1],q3＝values[2]，则Rg0矩阵为:Rg0=q02+q12-q22-q322(q1q2+q0q3)2(q1q3-q0q2)2(q1q2-q0q3)q02-q12+q22-q322(q2q3+q0q1)2(q1q3+q0q2)2(q2q3-q0q1)q02-q12-q22+q32;]]>(c)智能机上指定传感器类型为旋转矢量类型TYPE_ROTATION_VECTOR,实时得到旋转矢量传感器的观测值；利用智能机上的旋转矢量转矩阵函数将传感器的观测值转换成绘图转换矩阵R44,R44矩阵的左上角3x3子阵是Rg0；然后根据智能机旋转矢量传感器坐标系定义的不同，采取以下几种情况之一得到Rg:若旋转矢量传感器的X轴指向东,Y轴指向北,则Rg=Rg01-11T;---(2)]]>若旋转矢量传感器的X轴指向北,Y轴指向西,则Rg＝Rg0；   (3)若旋转矢量传感器的X轴指向南,Y轴指向东,则Rg=Rg0-1-11T;---(4)]]>方法(3)采用陀螺仪、加速度计与磁力计迭代：第1步：选用方法(1)或方法(2)计算Rg的初值，将Rg转换成4元数q0,q1,q2,q3作为以下第2‑7步迭代的初值；第2步：设定exInt，eyInt，ezInt原始值为0，exInt＝0，eyInt＝0，ezInt＝0；第3步：根据接收到的磁力计矢量{mx,my,mz},得到正确磁场矢量{wx,wy,wz}；先将矢量{mx,my,mz}替换成将其单位化以后的矢量得到局部坐标系下磁场的正确的磁场方向矢量{bx,0,bz}；hx＝2×mx×(0.5‑q2×q2‑q3×q3)+2×my×(q1×q2‑q0×q3)+2×mz×(q1×q3+q0×q2)；hy＝2×mx×(q1×q2+q0×q3)+2×my×(0.5‑q1×q1‑q3×q3)+2×mz×(q2×q3‑q0×q1)；hz＝2×mx×(q1×q3‑q0×q2)+2×my×(q2×q3+q0×q1)+2×mz×(0.5‑q1×q1‑q2×q2)；bx=hx2+hy2;]]>bz＝hz；再转换到正确磁场矢量{wx,wy,wz}：wx＝2×bx×(0.5‑q2×q2‑q3×q3)+2×bz×(q1×q3‑q0×q2)；wy＝2×bx×(q1×q2‑q0×q3)+2×bz×(q0×q1+q2×q3)；wz＝2×bx×(q0×q2+q1×q3)+2×bz×(0.5‑q1×q1‑q2×q2)；第4步：根据接收到的重力加速度计数据ax,ay,az,和{wx,wy,wz}得到误差矢量{ex,ey,ez}并计算其累计值exInt，eyInt，ezInt：先将矢量{ax,ay,az}替换成将其单位化以后的矢量vx＝2*(q1*q3‑q0*q2)；vy＝2*(q0*q1+q2*q3)；vz＝q0*q0‑q1*q1‑q2*q2+q3*q3；ex＝(ay×vz‑az×vy)+(my×wz‑mz×wy)；ey＝(az×vx‑ax×vz)+(mz×wx‑mx×wz)；ez＝(ax×vy‑ay×vx)+(mx×wy‑my×wx)；计算误差累计值；exInt替换为exInt+ex×Ki；eyInt替换为eyInt+ey×Ki；ezInt替换为ezInt+ez×Ki；其中Ki为一可调节的正系数，Ki在0.00001至0.5中任意选取；第5步：根据误差矢量{ex,ey,ez}及其累计值纠正陀螺仪数据{gx0,gy0,gz0}：假设智能机读出当前的一组陀螺仪数据为{gx0,gy0,gz0}，gx＝gx0+Kp×ex+exInt；gy＝gy0+Kp×ey+eyInt；gz＝gz0+Kp×ez+ezInt；其中Kp为一可调节的正系数，Kp在0.000001至20.0中任意选取；第6步：根据陀螺仪数据gx,gy,gz修正四元数：随着不断接收到陀螺仪数据gx,gy,gz，对4元数按如下方式修正，halfT为修正周期，halfT＝0.00001～10.0，q0替换为q0+(‑q1×gx‑q2×gy–q3×gz)×halfT；q1替换为q1+(q0×gx‑q3×gy+q2×gz)×halfT；q2替换为q2+(q3×gx+q0×gy‑q1×gz)×halfT；q3替换为q3+(‑q2×gx+q1×gy+q0×gz)×halfT；第7步：输出Rg矩阵和四元数：将四元数{q0,q1,q2,q3}单位化成输出；4元数转Rg矩阵公式如下：q02+q12-q22-q322(q1q2+q0q3)2(q1q3-q0q2)2(q1q2-q0q3)q02-q12+q22-q322(q2q3+q0q1)2(q1q3+q0q2)2(q2q3-q0q1)q02-q12-q22+q32;]]>第8步：回到第3步继续接收陀螺仪运动数据更新姿态4元数q0～q3，在循环的过程中每次到第7步都能输出当前的Rg矩阵和四元数。