[发明专利]智能机姿态测定、全景影像生成及目标识别方法

摘要

本发明公开了一种智能机姿态测定、全景影像生成及目标识别方法，其中智能机姿态测定方法包括以下步骤定义局部坐标系；测定智能机姿态矩阵Rg，Rg是一个相对于局部坐标系的3x3单位正交矩阵，其根据智能机内部的不同传感器可以通过多种方法得到。根据智能机的姿态测定可实现多种应用，包括智能机的虚拟现实漫游方法、智能机的多目视觉定位、智能机的单目单点定位、智能机的全景影像生成、智能机中的目标方向选择、视频影像实时纠正、智能机单像定位以及多个相机间的相对定位等。

主权项

一种智能机姿态测定方法，其特征在于，所述方法包括：定义局部坐标系：局部坐标系是指原点在智能机，Z轴指向地球表面方向,Y轴或X轴与纬线相平行的坐标系；或局部坐标系原点位于地球上任意一点，Z轴与重力方向一致，北代表局部坐标系X轴，西代表局部坐标系Y轴；测定智能机姿态矩阵Rg，Rg是一个相对于局部坐标系的3x3单位正交矩阵；所述测定智能机姿态矩阵Rg具体是采用智能机的加速度传感器，以及磁力计传感器或方向传感器，或者，采用智能机的旋转矢量传感器，或者，采用智能机的陀螺仪、加速度传感器与磁力传感器来测定；当测定智能机姿态矩阵Rg具体是采用智能机的加速度传感器，以及磁力计传感器或方向传感器来测定时，所述测定智能机姿态矩阵Rg具体包括：用以下公式(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；若采用方向传感器：通过方向传感器获取方位角将其带入公式(1)计算Rg；若采用磁力计传感器：计算mxmymz=RmTvmxvmyvmz]]>其中Rm是磁力传感器相对于智能机的姿态转换矩阵；{vmx,vmy,vmz}是磁力传感器检测到的三个值或这三个值经滤波的结果；再计算m0xm0ym0z=RθTmxmymz]]>利用磁力传感器计算方位角的三角函数为：带入公式(1)计算Rg；当测定智能机姿态矩阵Rg具体是采用智能机的旋转矢量传感器来测定时，所述测定智能机姿态矩阵Rg具体包括：若检测到旋转矢量传感器只有3个数据values[0],values[1],values[2]，则采用以下公式得到q1＝values[0],q2＝values[1],q3＝values[2],q0=1-q12-q22-q32]]>若检测到旋转矢量传感器有4个数据values[0],values[1],values[2],values[3]则采用以下公式得到q1＝values[0],q2＝values[1],q3＝values[2],q0＝values[3]则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;]]>根据智能机旋转矢量传感器坐标系定义的不同，得到Rg＝Rg0RLT，其中RLT是局部坐标系与旋转矢量传感器坐标系之间的变换矩阵；当测定智能机姿态矩阵Rg具体是采用智能机的陀螺仪、加速度传感器与磁力传感器来测定时，所述测定智能机姿态矩阵Rg具体包括：第1步采用智能机的加速度传感器，以及磁力计传感器或方向传感器来测定Rg的初值或采用智能机的旋转矢量传感器来测定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}，bz＝hz；其中：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)；再转换到正确磁场矢量{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中任意选取；若没有磁力计，则第3步不执行，同时将该第4步中的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)；改为ex＝ay×vz‑az×vy；ey＝az×vx‑ax×vz；ez＝ax×vy‑ay×vx；第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修正4元数随着不断接收到陀螺仪数据gx,gy,gz，对4元数按如下方式修正：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；其中halfT为修正周期，halfT＝0.00001～‑10.0；第7步输出Rg矩阵和4元数将4元数{q0,q1,q2,q3}单位化成输出；4元数转Rg矩阵公式如下第8步回到第3步继续接收陀螺仪运动数据更新姿态4元数q0～q3，在循环的过程中每次到第7步都能输出当前的Rg矩阵和4元数。