CN104748751A - 姿态矩阵的计算方法及基于姿态矩阵的定位导航方法 - Google Patents

Abstract

本发明公开了一种姿态矩阵的计算方法及基于姿态矩阵的定位导航方法,其中姿态矩阵的计算方法可基于智能机加速度计、智能机磁力计或者智能机Android系统函数中的getRotationMatrix函数。通过计算得到的姿态矩阵Rg的第1行是智能机x轴在局部坐标系下的单位矢量；第2行是智能机y轴在局部坐标系下的单位矢量；第3行是智能机z轴在局部坐标系下的单位矢量；其中局部坐标系是z轴沿重力指向地球外，x轴朝北y轴朝西的坐标系。运用姿态矩阵可以进行室内外导航、运动测量姿态控制、遥控、指向控制、视频图像纠正处理、基于图像的定位等。

Description

姿态矩阵的计算方法及基于姿态矩阵的定位导航方法

技术领域

本发明涉及信息技术领域，尤其涉及一种姿态矩阵的计算方法及基于姿态矩阵的定位导航方法。

背景技术

地理信息位置服务一直只强调空间定位和空间距离，却忽视了一个非常重要的数据和概念——姿态，实际上用户更关心目的地或目标相对于自身的方向，没有姿态的位置服务是不完整的位置服务。比如一款的士打车软件，司机在抢单的时候如果只考虑距离，就可能选择位于自身后方的乘客，由于车道不能逆行，导致司机无法接到乘客，乘客也无法乘坐的士。作战的时候如果只知道敌人在接近而不知道敌人从哪个方向接近，这必然会导致混乱和恐慌。因此智能机姿态的测定能实现一种真正意义上的位置服务，使得基于智能机姿态的位置服务成为可能，具有非常重要的意义，并且必将形成一种新的数据新标准而被广泛应用。

姿态传感器的应用涉及方方面面，在很多领域都需要用到智能机自身姿态的信息,如影像、视频纠正、单目定位，3D遥控、室内导航、导弹制导。传统的姿态测量非常笨重和昂贵，一般安装在大型飞机，车船上，主要用于航天军事领域。近年来随着传感器技术的提高，各种廉价、轻小型号的用于姿态测定的传感器得以安装在小型智能机上，这为智能机姿态传感数据新标准的建立和广泛应用创造了硬件条件。

一旦这些传感器数据通过数据融合处理得到智能机姿态数据之后就能实现各种重要的应用。表示姿态的传感数据用得最多、最传统的是航位角、俯仰角和翻滚角，主要用于无人机姿态表述和地面移动测量系统MMS，但是这样表示缺点很多：

(1)首先需要用大量三角函数重构姿态矩阵，导致计算速度慢；

(2)三个角度相对于坐标系的转角不统一；

(3)三个角度的转换顺序没有一个统一的标准，不同导航设备上的三个角的顺序定义是不一样的，没有统一的标准，导致针对不同导航设备的姿态数据软件不能统一接口和通用共享，因此缺少统一的姿态数据标准极大地阻碍了导航技术的发展。

有些软件内部采用的定位算法不是基于图像的，在无GPS和wifi环境下软件是不能正常定位的，软件中的摄像头扫描界面效果给人一种图像识别定位的感觉。传统的图像定位方法是不知道姿态数据的，这样就要求至少需要三对以上的物像同名点对应关系才能进行定位计算，事实上三对同名点除了进行定位还要计算姿态，因此得到的结果很不稳定，很不准确，特别是在三点位一条直线或者三点之间距离很小都会出现很不稳定，不精确的问题，原因就是这三点除了进行定位计算以外还要负担姿态运算。

本发明中提到的智能机姿态数据的各个方面潜在应用，包括姿态指向、视频(图像)纠正、3D遥控，3D导航、目标叠加，智能机视频与地图影像相互之间的信息叠加，目前还没有在轻便廉价的智能机领域得到应用。

发明内容

本发明要解决的技术问题在于针对现有技术中的上述缺陷，提供一种姿态矩阵的计算方法及基于姿态矩阵的定位导航方法。

本发明解决其技术问题所采用的技术方案是：

提供一种智能机姿态矩阵计算方法，包括以下步骤：

通过智能机加速度计检测三个值{v_ax’,v_ay’,v_az’}计算a_x、a_y和a_z：

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=R_a^T\left[\begin{array}{c}{v_{\mathrm{ax}}}^{\′}\\ {v_{\mathrm{ay}}}^{\′}\\ {v_{\mathrm{az}}}^{\′}\end{array}\right],$

或对{v_ax’,v_ay’,v_az’}进行滤波后得到{v_ax,v_ay,v_az}，计算a_x、a_y和a_z：

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=R_a^T\left[\begin{array}{c}{v}_{\mathrm{ax}}\\ v_{\mathrm{ay}}\\ v_{\mathrm{az}}\end{array}\right],$ 其中R_a是重力加速度传感器相对于智能机的姿态转换矩阵；

通过智能机磁力计获取方位角的三角函数；或者方向传感器获取方位角

计算姿态矩阵

姿态矩阵R_g的第1行是智能机x轴在局部坐标系下的单位矢量；第2行是智能机y轴在局部坐标系下的单位矢量；第3行是智能机z轴在局部坐标系下的单位矢量；其中局部坐标系是z轴沿重力指向地球外，x轴朝北y轴朝西的坐标系；

其中

$R_{\mathrm{\θ}}=\left[\begin{array}{ccc}\frac{{\mathrm{sa}}_z}{L\sqrt{1-{(a_y/L)}^2}}& -{\mathrm{sa}}_x{a}_y/\left[L^2\sqrt{1-{(a_y/L)}^2}\right]& a_x/L\\ 0& s\sqrt{1-{(a_y/L)}^2}& a_y/L\\ \frac{-s{a}_x}{L\sqrt{1-{(a_y/L)}^2}}& -{\mathrm{sa}}_z{a}_y/\left[L^2\sqrt{1-{(a_y/L)}^2}\right]& a_z/L\end{array}\right],L=\sqrt{a_x^2+a_y^2+a_z^2}$

当a_z<0时，s＝-1，a_z≥0时，s＝1；

$\left[\begin{array}{c}{m}_{0x}\\ m_{0y}\\ m_{0z}\end{array}\right]=R_{\mathrm{\&theta;}}^T\left[\begin{array}{c}{m}_x\\ m_y\\ m_z\end{array}\right].$

其中m_x,m_y,m_z是智能机磁力计检测出来的三个值{v_mx,v_my,v_mz}，或{v_mx,v_my,v_mz}经过滤波得到的三个值。

本发明还提供一种智能机姿态矩阵计算方法，该方法基于智能机支持Android系统，具体包括以下步骤：

获取智能机Android系统函数中的getRotationMatrix函数获取矩阵R_g纵东’,然后将R_g纵东’经过矩阵转置操作得到R_g纵东＝(R_g纵东’)^T，R_g纵东是X轴指向东Y轴指向北坐标系下的姿态矩阵；

计算局部坐标系下的姿态矩阵姿态矩阵R_g的第1行是智能机x轴在局部坐标系下的单位矢量；第2行是智能机y轴在局部坐标系下的单位矢量；第3行是智能机z轴在局部坐标系下的单位矢量；其中局部坐标系是z轴沿重力指向地球外，x轴朝北y轴朝西的坐标系。

本发明还提供一种基于智能机姿态矩阵的指向或导航方法，包括以下步骤：

1)定义智能机主方向或载体主方向相对于智能机自身的矢量单位p；

2)根据权利要求1-4中任一项所述的方法计算智能机当前姿态矩阵R_g；

3)计算p_┴＝R_g ^Tp，为智能机主方向或载体主方向相对于指北坐标系的矢量；

4)计算局部坐标系中的目标矢量v_o＝O-P，其中局部坐标系中智能机自身坐标为P，目标的坐标为O，则目标矢量v_o＝O-P；

5)计算矢量p_┴与目标矢量v_o之间夹角的余弦，或计算两个矢量之间的夹角；

6)若夹角的余弦或者两个矢量之间的夹角在预设范围内，则触发智能机执行预设关联事件。

本发明还提供一种基于智能机姿态矩阵的2D导航方法，包括以下步骤：

将智能机水平放置，利用方向传感器的第一个值value[0]得出主方向角矢量与智能机到目标矢量之间的夹角，或两者之间的点积；

实时计算智能机2维主矢量{cosφ,sinφ}与2维目标矢量v_o＝O-P示意图，O是局部坐标系中X，Y平面上目标的2维坐标，P是局部坐标系中X，Y平面上的智能机2维坐标；其中φ＝value[0]+△φ，value[0]是方向传感器的第一个值，△φ是在地图平面上用户定义的主方向相对于智能机主方向之间的夹角；

若{cosφ,sinφ}与目标矢量v_o之间的夹角小于预设值说明方向吻合，或者{cosφ,sinφ}与2目标矢量v_o点积除以|v_o|的结果大于预设值，则说明方向吻合。

本发明还提供一种基于智能机姿态数据的视频影像实时纠正方法，其特征在于，包括以下步骤：

采用上文所述的方法，获取智能机拍摄视频的每一帧对应的姿态矩阵R_g，根据R＝△R·R_g得到智能机上的摄像头的局部坐标姿态矩阵R，其中△R是摄像头相对于智能机的转换矩阵；

将R矩阵分解为R＝RτR_，计算得到矩阵Rτ；

其中，R_是预设期待输出图像的姿态矩阵，Rτ是变换矩阵；

将原始图像I经过H＝K₂R_τ ^TK^-1的Homograph变换得到I’，

其中 $K=\left[\begin{array}{ccc}f& & c_x\\ & f& c_y\\ & & 1\end{array}\right]$ 是拍摄相机的内部参数矩阵，f是焦距的像素长度，(c_x,c_y)是相机主点坐标，c_x＝w/2，c_y＝h/2，w、h是原始图像I的宽和高；

$K_2=\left[\begin{array}{ccc}{f}_2& & c_{x2}\\ & f_2& c_{y2}\\ & & 1\end{array}\right]$ 是纠正以后虚拟相机的内部参数矩阵，f2是纠正以后虚拟相机的焦距的像素长度，(c_x2,c_y2)是纠正以后虚拟相机主点坐标。

本发明还提供一种基于姿态数据的智能机单像定位方法，包括以下步骤：

搜集采集物方点，转化成局部坐标系坐标；

采用权利要求上述述的方法获取智能机的姿态矩阵R_g，根据R＝△R·R_g得到智能机上摄像头的姿态矩阵R，△R是摄像头相对于智能机的转换矩阵；

拍摄一张图像，自动提取其中的多个特征点，通过匹配算法得到与物点对应的n对物像同名点；n个物方点的坐标是X_i＝[X_ix,X_iy,X_iz]^T i＝1,…,n；对应n个像点坐标是[u_i v_i]^Ti＝1,…,n；

计算相机在局部坐标系下的坐标T＝[Tx，Ty，Tz]^T：

(1)特征点无错情况下

若确认多个特征点无错误，则采用数学模型解方程得到自身在局部坐标系下坐标Tx，Ty，Tz，

$\left[\begin{array}{ccc}{z}_1& & -x_1\\ & z_1& -y_1\\ ...& ...& ...\\ z_n& & -x_n\\ & z_n& -y_n\end{array}\right]\left[\begin{array}{c}{T}_x\\ T_y\\ T_z\end{array}\right]=\left[\begin{array}{c}{z}_1{X}_{1x}-x_1{X}_{1z}\\ z_1{X}_{1y}-y_1{X}_{1z}\\ ...\\ z_n{X}_{\mathrm{nx}}-x_n{X}_{\mathrm{nz}}\\ z_n{X}_{\mathrm{ny}}-y_n{X}_{\mathrm{nz}}\end{array}\right]---\left(\mathrm{HH}2\right)$

其中 $\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right]=R^T\left[\begin{array}{c}\frac{u_i-c_x}{f_x}\\ \frac{v_i-c_y}{f_y}\\ 1\end{array}\right]$

进而得到相机位置T＝[Tx，Ty，Tz]^T；

(2)容错情况下

若不确认多个特征点是否全部正确，采用Ransac容错算法，通过共线方程模型随机选取两对点n＝2,根据模型式(HH2)计算相机位置T，并通过共线方程z_i[u’_i v’_i 1]^T＝KR(X_i-T)验证T，通过KR(X_i-T)算出的u’_i,v’_I与u_i,v_i之间的距离误差小于error像素认为是符合共线方程；选取符合共线方程点数最多的正确特征点集ζ_max并计算ζ_max对应的相机位置T*。

本发明还提供一种基于智能机姿态数据的单目单点定位方法，包括步骤：

假设智能机上的相机内部参数矩阵为:

$K=\left[\begin{array}{ccc}{f}_x& & c_x\\ & f_y& c_y\\ & & 1\end{array}\right]$

fx，fy分别指相机内部参数中的水平和垂直焦距像素长，

被观察物体是一个直径＝D大小的物体,

在智能机上成像的宽度是Δu,高度是Δv,

成像中心点像坐标(u,v),(c_x,c_y)是相机主点坐标；

采用权利要求1-4方法获取智能机的姿态矩阵R_g，R＝△RR_g得到智能机上摄像头的姿态矩阵R，△R是摄像头相对于智能机的转换矩阵；

计算 $z=D\&CenterDot;\mathrm{MIN}(\frac{f_x}{\mathrm{\&Delta;u}},\frac{f_y}{\mathrm{\&Delta;v}})$

(1)若已知智能机摄像头光心局部坐标系坐标是T,

单目估算出物体的局部坐标系下三维坐标X:

$X=T+z{R}^T{K}^{-1}\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=T+z{R}^T\left[\begin{array}{c}\frac{u-c_x}{f_x}\\ \frac{v-c_y}{f_y}\\ 1\end{array}\right]$

(2)若已知物体的局部坐标系下三维坐标X，计算相机坐标T

$T=X-z{R}^T\left[\begin{array}{c}\frac{u-c_x}{f_x}\\ \frac{v-c_y}{f_y}\\ 1\end{array}\right].$

本发明还提供一种基于智能机姿态数据的相对定位方法，假设相机1，2之间有i＝1..n≥2对同名点u_1i,v_1i～u_2i,v_2i，

相机2相对于相机1的姿态矩阵R＝R₂R₁ ^T；其中R₁＝△RR_g1 R₂＝△RR_g2,R_g1，R_g2是智能机相机在拍摄的1,2两个地点测出来的姿态矩阵数据,△R是摄像头相对于智能机的转换矩阵；已知相机1的内参矩阵 $K_1=\left[\begin{array}{ccc}{f}_1& & c_{x1}\\ & f_1& c_{y1}\\ & & 1\end{array}\right],$ 相机2的内参矩阵 $K_2=\left[\begin{array}{ccc}{f}_2& & c_{x2}\\ & f_2& c_{y2}\\ & & 1\end{array}\right],$ 其中(c_xi,c_yi)是相机i的像素主点坐标，f_i是相机i的焦距像素长，已知两相机之间的距离是L；计算相机2相对于相机1的位移矢量T；

方法具体包括如下步骤：

1)用权利要求1所述的方法计算智能机的姿态R_g1,R_g2，进一步计算2个智能机上各自相机姿态R₁＝△RR_g1 R₂＝△RR_g2，得到相对姿态R＝R₂R₁ ^T

2)首先构建矩阵A

$A=\left[\begin{array}{ccc}{u}_{12*z}{u}_{11R*y}-u_{12*y}{u}_{11R*z}& u_{12*x}{u}_{11R*z}-u_{12*z}{u}_{11R*x}& u_{12*y}{u}_{11R*x}-u_{12*x}{u}_{11R*y}\\ ......& ...& ...\\ u_{n2*z}{u}_{n1R*y}-u_{n2*y}{u}_{n1R*z}& u_{n2*x}{u}_{n1R*z}-u_{n2*z}{u}_{n1R*x}& u_{n2*y}{u}_{n1R*x}-u_{n2*x}{u}_{n1R*y}\end{array}\right]$

其中u_ji*＝K_i ^-1u_i＝[u_ji*x u_ji*y u_ji*z]^T，i＝1,2；j＝1..n；u_jiR*＝Ru_ji*＝[u_jiR*x u_jiR*y u_jiR*z]^T；

3)如果确认n对点都是正确的，则求约束||t_3x1||＝1下A_nx3t_3x1＝0的解或采用SVD分解得到A_nx3＝U_nxnW_nx3V^T _3x3,分解出来的V的最后1列的矢量，即对应于最小奇异值的特征矢量，即为t，T＝-LR^Tt得到结果，结束；否则继续进行步骤4)；

4)设置迭代初值n_max＝0，k＝1；

5)随机挑选A矩阵中的2行，构建A_2x3；

就是求约束||t₂||＝1下A_2x3t₂＝0的解，或采用SVD分解得到A_2x3＝U_2x2W_2x3V^T _3x3,

分解出来的V的最后1列的矢量即为t₂；

6)将t₂＝[t_tx t_ty t_tz]^T代入第j＝1...n个点，采用式(AB)验证点j是否是内点,

(u_j2*zu_j1R*y-u_j2*yu_j1R*z)t_tx+(u_j2*xu_j1R*z-u_j2*zu_j1R*x)tt_y+(u_j2*yu_j1R*x-u_j2*xu_j1R*y)t_tz＜ε

                                                          (AB)

其中ε表示最大容忍误差物方长度，相对于两相机之间的距离＝1而言，

满足式(AB)，则点j是内点，如果满足内点的数量n_t超过最佳值n_max则ζ_t-->ζ，n_t-->n_max；

7)如果(1-(n_max/n)²)^k<0.00001，退出迭代进入步骤9)；

8)迭代次数k增加1，回到步骤5)反复迭代；

9)最后采用最小二乘法针对最大内点集ζ重新计算一次最优t；

求约束||t_3x1||＝1下A_n’x3t_3x1＝0的解；其中n’＝n_max；

或采用SVD分解得到A_n’x3＝U_n’xn’W_n’x3V^T _3x3；分解出来的V的最后1列矢量,即对应于最小奇异值的特征矢量，即为t；最终得到T＝-LR^Tt。

本发明还提供一种基于姿态矩阵的智能机3D遥控方法，包括以下步骤：

首先上述方法获取遥控智能机的姿态矩阵R_g，假设r_ij是R_g第i行j列的元素值；

然后按以下方法实现x方向，y方向，旋转三个自由度控制：

1)控制y值：y轴r_y的z分量控制上下移动

控制对象的屏幕y坐标Y_m＝(h/2)(1+k_yr₂₃)或者

其中h为屏幕高度，k_y为某一灵敏系数，k_y＝0.01～1之间的某值；

2)控制x值：y轴r_y的x,y分量控制水平移动

初始时X_m＝W/2，W是显示屏幕宽度；根据实时不停采集到的姿态矩阵R，不停修正控制对象的屏幕x坐标X_m，X_m不断减小Tk_x(φ-φ’)，其中φ＝atan2(r₂₂,r₂₁)；φ’是上次得到的φ值；k_x为某一灵敏系数，k_x＝0.01～1之间的某值，T是两次采样时间间隔；在不停的修改过程中若X_m小于0则X_m等于0，若X_m大于W则X_m等于W；

3)控制旋转值：绕y轴r_y旋转控制旋转

根据采集得到的R矩阵得到绕y轴r_y旋转的角度弧度值τ＝atan2(r₁₃,r₃₃)。

构造通过n个关于R_g的函数实现智能机控制多自由度控制，智能机与被控制设备通过有线或无线机制进行数据通信，实现如鼠标或投影仪指针的移动和旋转控制，智能灯的红绿蓝三分量控制，无人机的空中飞行的3轴姿态控制。

本发明还提供一种基于姿态数据的运动方向测定及导航方法，其特征在于，包括以下步骤：

步骤1.根据上述方法获取智能机的姿态矩阵R_g；

步骤2.测量局部坐标系下物体的运动加速度a_┴，a_┴的x分量代表往北移动的分量，a_┴的y分量代表往西移动的分量；

步骤3.对运动加速度a_┴积分一次，得到目标在指北坐标系下的移动速度v_┴，积分两次，得到目标在指北坐标系下的位移s_┴；

步骤4.为避免累积误差，若获取到其它精确的定位方法得到的定位值，则立即对目标位置进行纠正；

步骤5.回到步骤1进行下一轮的定位位置修正。

本发明还提供一种基于姿态数据的虚拟现实方向漫游控制方法，包括以下步骤：

1)根据漫游者的输入或者漫游者的自身位置设置局部坐标系中的初始坐标位置(e_x,e_y,e_z)；

2)采用上述方法得到智能机的姿态矩阵R_g，R＝△RR_g得到智能机显示屏的姿态矩阵R，△R是智能机显示屏相对于智能机的转换矩阵；

3)获取智能机漫游者手指在屏幕本体上的滑动数据；

4)在滑动数据是漫游者单指相对于智能机屏幕本体滑动矢量△X,△Y的情况下，如果|r₃₃|>|r₁₃|且|r₃₃|>|r₂₃|，其中r_ij是R矩阵的i行j列元素值，则单指滑动后，在局部坐标系中的定位坐标改正数(v_x，v_y)，v_x＝ΔXr₁₁+ΔYr₂₁ v_y＝ΔXr₁₂+ΔYr₂₂；否则：

$\left[\begin{array}{c}{v}_x\\ v_y\end{array}\right]=\frac{1}{\sqrt{r_{31}^2+r_{32}^2}\sqrt{r_{13}^2+r_{23}^2}}\left[\begin{array}{cc}{r}_{13}{r}_{31}-r_{23}{r}_{32}& r_{13}{r}_{32}+r_{23}{r}_{31}\\ r_{13}{r}_{32}+r_{23}{r}_{31}& -(r_{13}{r}_{31}-r_{23}{r}_{32})\end{array}\right]\left[\begin{array}{c}\mathrm{\&Delta;X}\\ \mathrm{\&Delta;Y}\end{array}\right]$

5)在滑动数据是漫游者双指做拉开或合拢操作所获得的情况下，则得到拉开和合拢之前双指之间的屏幕距离，以及拉开和合拢之后双指之间的屏幕距离；

6)根据步骤4)和5)中得到的数据修正漫游者在局部坐标系中的坐标位置(e_x,e_y,e_z)，并根据修正后的坐标位置显示局部坐标系中的三维场景。

本发明还提供了一种基于姿态数据的视频图像投影到地图的方法，包括以下步骤：

采用权利要求1-4的方法得到智能机的姿态矩阵R＝△RR_g，△R是智能机摄像头相对于智能机的转换矩阵；

根据GPS测得拍摄相机在局部坐标系中的坐标为T，此时相机或摄像头拍摄下图像帧I；

将图像帧I经过Homograph变换H_map＝[K[r₁ r₂ -RT]]^-1＝[r₁ r₂ -RT]^-1K^-1得到I_map，I_map是视频图像映射到地图上的正射影像，其中r₁ r₂分别是R矩阵的第1列和第2列，K是智能机上的相机内部参数矩阵， $K=\left[\begin{array}{ccc}{f}_x& & c_x\\ & f_y& c_y\\ & & 1\end{array}\right],$ fx，fy分别指相机内部参数中的水平和垂直焦距像素长，c_x,c_y)是相机主点坐标。

本发明产生的有益效果是：实时快速测量便携智能机姿态，帮助位置服务用户更好的了解自身与其它移动用户之间的相互位置关系，触发智能机与周边目标之间的某一关联事件，如指向该目标，选中该目标，击中该目标、通知该目标、联络该目标等，实现基于智能机姿态的导航导向以及各种虚拟现实，增强现实应用；视频图像的每一幅画面有了对应的姿态就可以作水平、垂直纠正处理，在颠簸的飞机、船只、用户倾斜手持状态下拍摄的视频经处理能得到正放的稳定图像；可以进行摄影测量双点相对定位、单像定位、单点定位等处理(以前由于缺乏姿态数据需要双目3点以上才能实现，而且精度不高)。本发明使得以上技术在各种廉价、轻型的智能机上得以广泛应用。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1是本发明实施例位置服务中同时符合位置和方向的目标位示意图；

图2是本发明实施例的单像双点定位示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

由于传统的三角度表示法存在各种缺陷，本发明提出采用以下2种姿态描述方法解决与姿态有关的各种问题。

1.姿态矩阵表述法

姿态矩阵表述法的优点是不会像三角度法那样由于角度顺序不统一而引起歧义，具有很好的几何描述特性和实际物理意义，比如姿态矩阵的三行表示在北坐标系下x，y，z三个轴的三个单位矢量，而且也非常便于各种坐标转换计算，很多情况下避免了繁琐耗时的三角函数运算，大大提高了运算效率。

2.4元数表示法

姿态矩阵需要3x3＝9个浮点数表示一个姿态，由于姿态矩阵中的9个元素构建的是一个单位正交矩阵，内部存在很大耦合，所以数据量冗余大，不便于高速传输和存储在有限的空间，因此可进一步采用4元数表示法。

采用4元数压缩则克服了各方面困难，不仅传输效率高，且可直接通过导航算法获取，4元数域的矩阵乘法也很方便。4元数还有一个优点就是在定姿算法中具有很好的可导性，通过导函数计算方便地实现了姿态的动态优化，这样定姿算法中得到的4元数可直接用来保存为数据协议的必要内容。

本发明中所有的方法应用都是基于描述智能机姿态的姿态矩阵或是4元数的，而姿态矩阵可以用四元数紧凑的描述出来。因此智能机姿态传感数据需要建立统一的数据，以便实现与其相关的各种应用。

本发明所说的姿态数据是指智能机传感器通过数据融合得到的智能机相对于局部坐标系下的姿态矩阵，或者姿态矩阵对应的四元数。建立智能机姿态传感数据标准使得各领域与姿态相关的应用都可以通过该数据标准进行抽象，应用主要分为以下几大类：

1.室内外导航，运动测量

2.姿态控制，遥控，指向控制

3.视频图像纠正处理

4.基于图像的定位

5.视频目标叠加及地图叠加。

这里所说的智能机的概念包括一切含显示屏和姿态传感的智能设备，如手机，平板电脑，笔记本电脑，带姿态传感和显示屏的摄像头，相机，智能显示器，智能电视机显示屏、以及各种可穿戴设备。

本发明基于智能机姿态数据的目标方向选择

获取智能机姿态矩阵R_g

本发明所说的姿态矩阵R_g描述了智能机的姿态，姿态矩阵R_g的第1行是智能机x轴在局部坐标系下的单位矢量；第2行是智能机y轴在局部坐标系下的单位矢量；第3行是智能机z轴在局部坐标系下的单位矢量；其中局部坐标系是z轴沿重力指向地球外，x轴朝北y轴朝西的坐标系。Rg的获取方法如下：

方法1

计算姿态矩阵

其中

$R_{\mathrm{\&theta;}}=\left[\begin{array}{ccc}\frac{{\mathrm{sa}}_z}{L\sqrt{1-{(a_y/L)}^2}}& -{\mathrm{sa}}_x{a}_y/\left[L^2\sqrt{1-{(a_y/L)}^2}\right]& a_x/L\\ 0& s\sqrt{1-{(a_y/L)}^2}& a_y/L\\ \frac{-s{a}_x}{L\sqrt{1-{(a_y/L)}^2}}& -{\mathrm{sa}}_z{a}_y/\left[L^2\sqrt{1-{(a_y/L)}^2}\right]& a_z/L\end{array}\right],L=\sqrt{a_x^2+a_y^2+a_z^2}$

a_x,a_y,a_z是智能机加速度计检测出来的三个值{v_ax,v_ay,v_az}，或{v_ax,v_ay,v_az}经过滤波得到的三个值a_x,a_y,a_z。

当a_z<0时，s＝-1，a_z≥0时，s＝1；

$\left[\begin{array}{c}{m}_{0x}\\ m_{0y}\\ m_{0z}\end{array}\right]=R_{\mathrm{\&theta;}}^T\left[\begin{array}{c}{m}_x\\ m_y\\ m_z\end{array}\right]$

m_x,m_y,m_z是智能机磁力计检测出来的三个值{v_mx,v_my,v_mz}，或{v_mx,v_my,v_mz}经过滤波得到的三个值m_x,m_y,m_z。

或者采用带入公式(1)计算R_g

如果定义

带入公式(1)得到的正好是R_g纵东，即X轴指向东Y轴指向北坐标系下的姿态矩阵。

方法2

若智能机使用了Android系统，并支持Android系统函数中的getRotationMatrix功能，则可采用：getRotationMatrix函数获取矩阵R_g纵东’,然后将R_g纵东’经过矩阵转置操作(R_{g 纵东}’)^T得到R_g纵东＝(R_g纵东’)^T。

R_g纵东是X轴指向东Y轴指向北坐标系下的姿态矩阵，可将R_g纵东转换成X轴指北，Y轴指西的局部坐标系R_g。进一步得到局部坐标系下的姿态矩阵

方法3，通过智能机旋转矢量传感器输出的3个数据q1＝values[0],q2＝values[1],q3＝values[2]，计算若旋转矢量传感器有第4个输出则可直接计算q₀＝values[3]，再按以下公式计算R_g纵东矩阵

再采用公式(2)进一步得到局部坐标系下的姿态矩阵R_g。

计算姿态矢量并用于指向判断

步骤简述：

1.定义智能机主方向或载体主方向相对于智能机自身的矢量单位p

2.采用上一节所述的方法获取智能机当前姿态矩阵R_g

3.计算p_┴＝R_g ^Tp为智能机主方向或载体主方向相对于指北坐标系的矢量。

4.计算局部坐标系中的目标矢量v_o＝O-P

假设局部坐标系中智能机自身坐标为P,目标的坐标为O，则目标矢量v_o＝O-P；

5.计算矢量p_┴与目标矢量v_o之间夹角的余弦，或计算两个矢量之间的夹角。

如果夹角的余弦很大，接近1，或者夹角偏差的绝对值|dφ|很小则触发智能机与该目标O之间的某一关联事件。如指向该目标，选中该目标，击中该目标、通知该目标、联络该目标等。

可通过两矢量的点积除以|v_o|得到夹角dφ的余弦，

cos(dφ)＝(p_┴点积v_o)/|v_o|

如果该余弦cos(dφ)>cos(φa)则该目标方向选择事件可触发。

6.在需要与用户的可视化交互情况下，在指北坐标系中实时绘制目标，智能机，和矢量p_┴。

计算局部坐标系中智能机主方向矢量与目标矢量之间夹角的方向余弦。根据余弦值大小判定夹角。并且在三维空间或2维空间中绘制出智能机主方向矢量与目标矢量之间方向关系便于人机交互。

具体应用方式有以下几种类型：

方法1姿态指向

用户通过操作智能机所指主方向所形成的矢量与目标矢量之间的夹角保持较小角度，或夹角的余弦>cos(φa)，则将触发某一事件。

用户用智能机主方向指向某一目标，如果目标矢量与主方向矢量之间的夹角的余弦>cos(φa)可执行与该目标相关联的事件，如选中该目标，击中该目标、通知该目标、联络该目标等。Φa为最大容忍偏差角。

在三维空间中实时绘制智能机姿态矢量r_z与目标矢量v_o之间的关系示意图，以便于实现用户的交互控制。

实施方式：

假设智能机头部定义为主方向，头部相对于智能机自身坐标系的矢量是p＝{0,1,0}^T

那么p_┴＝R_g ^Tp＝{r₂₁ r₂₂ r₂₃}^T其中r_ij是R矩阵第i行j列的元素。

假设智能机右侧定义为主方向，右侧相对于智能机自身坐标系的矢量是p＝{1,0,0}^T

那么p_┴＝R_g ^Tp＝{r₁₁ r₁₂ r₁₃}^T

方法2导航服务

将智能机与某一载体，如车船，固定起来，定义载体主方向相对于智能机自身的矢量p。

在地图中实时绘制载体主方向矢量p_┴＝R^Tp与目标矢量v_o之间的关系示意图。

计算夹角的余弦cos(dφ)>cos(φa)说明方向吻合，再结合距离约束可触发某一事件。

本方法也可以与运动方向形成的轨迹矢量结合智能机主方向进行融合，进而计算与目标矢量之间的夹角。

实施方式：

摄像头拍摄方向朝前固定安装在车船上，摄像头拍摄方向相对于智能机自身坐标系的矢量是p＝{0,0,-1}^T

那么p_┴＝R_g ^Tp＝{-r₃₁-r₃₂-r₃₃}^T其中r_ij是R_g矩阵第i行j列的元素。

p_┴与v_o之间的夹角的余弦＝(p_┴点积v_o)/|v_o|

如图1所示，位置服务中同时符合位置和方向的目标位于图中的阴影区。

方法3 2D导航服务

这种方法要求智能机水平放置，利用方向传感器的第一个值value[0]得出主方向角矢量与智能机到目标矢量之间的夹角。或两者之间的点积。这里可以用局部坐标系中忽略Z轴的2维坐标，也可以直接用经纬度建立的2维坐标系。

实时计算智能机2维主矢量{cosφ,sinφ}与2维目标矢量v_o＝O‐P示意图。

其中φ＝value[0]+△φ value[0]是方向传感器的第一个值，△φ是在地图平面上，用户定义的主方向相对于智能机主方向之间夹角。

O是局部坐标系中X，Y平面上目标的2维坐标，P是局部坐标系中X，Y平面上的智能机2维坐标。

{cosφ,sinφ}与目标矢量v_o之间的夹角<φa说明方向吻合。

或者{cosφ,sinφ}与2目标矢量v_o点积除以|v_o|的结果>cosφa则说明方向吻合。

实施方式：

应用：导航，顺路方向的的士叫车抢单，司机将智能机固定在车上，智能机软件只可选择cos(dφ)>cos(φa)的乘客，选出来的都是顺路乘客。

2D导航的具体实施方式：

要求智能机水平放置，利用方向传感器的第一个值value[0]得出主方向角矢量与智能机到目标矢量之间的夹角，或两者之间的点积。

计算2维地图上的目标矢量v_o＝{v_ox，v_oy}的方位角φ_vo可调用反三角函数atan2(v_oy,v_ox)实现，计算φ角与φ_vo角形成的矢量在平面上的夹角

实施方法，采用但不局限于以下方法：

φ角与φ_vo之间的夹角可以按以下方式计算：

一般采用2维点积的方式计算φ角与φ_vo之间的夹角余弦＝({cosφ,sinφ}点积v_o)/|v_o|，若该余弦值>cos(φa)认定为方向一致。

为便于用户交互控制，在地图上将目标矢量v_o与智能机矢量{cosφ,sinφ}同时画出。特别是当目标不在地图视野范围内时，地图上智能机到目标连线与地图边框交点处绘制方向为v_o的箭头指向目标。

基于智能机姿态数据的视频影像实时纠正

一般方法步骤如下：

1.采用权利要求1方法获取智能机拍摄视频的每一帧对应的姿态矩阵Rg。

2.R＝△RR_g得到智能机上的摄像头的局部坐标姿态矩阵R，其中△R是摄像头相对于智能机的转换矩阵。

3.根据实际应用需求R矩阵分解为R＝R_τR_从而得到矩阵R_τ，计算矩阵R_τ的方法有三类

(a)根据实际应用中R_的特性直接得到，再通过R_τ＝R R_^T得出R_τ

详见下文中的标准对地坐标系下的影像正射纠正

(b)根据实际应用中R_τ和R_的特性列方程R_τ＝R R_^T或R＝R_τR_求解得到R_τ中的几个重要元素的值，从而构建R_τ。

详见下文中的任意R矩阵的Z轴旋转分解出水平矩阵

(c)根据R_τ和R_的性质列方程R_τ＝R R_^T或R＝R_τR_求解得到R_中的几个重要元素的值，从而构建R_，最后通过R_τ＝R R_^T得出R_τ

详见下文中的对地坐标系下的影像正射纠正

4.对图像I经过H＝K₂R_τ ^TK^-1的Homograph变换得到I’

其中 $K=\left[\begin{array}{ccc}f& & c_x\\ & f& c_y\\ & & 1\end{array}\right]$ 是拍摄相机的内部参数矩阵，f是焦距的像素长度，c_x,c_y是相机主点坐标。对于理想相机c_x,c_y是图像的中心点，c_x＝w/2,c_y＝h/2,w,h是原始图像I的宽和高。

$K_2=\left[\begin{array}{ccc}{f}_2& & c_{x2}\\ & f_2& c_{y2}\\ & & 1\end{array}\right]$ 是纠正以后虚拟相机的内部参数矩阵，f2是纠正以后虚拟相机的焦距的像素长度，(c_x2,c_y2)是纠正以后虚拟相机主点坐标。

注：

-对于理想相机(c_x2,c_y2)是纠正以后虚拟相机图像的中心点，c_x2＝H/2,c_y2＝H/2,W,H是图像纠正以后虚拟相机的宽高。

-通常f₂选取与f相等，而W，H的大小根据实际应用需求选取，有时为避免纠正后图像出界而选取W>w H>h

-如果H＝K₂R_τ ^TK^-1中的第三行元素是[0 0 1]，那么Homograph透视变换可简化成仿射变换从而提高图像变换速度。

下面具体给出前述三类计算矩阵R_τ的应用案例和方法

(a)标准对地坐标系下的影像正射纠正

视频摄像机往下拍地面上的场景，但由于各种原因，视线不可能完全垂直于地面。为得到垂直于地面的图像，先通过以下方法计算R_τ

R＝R_τR_中的 $R\_=\left[\begin{array}{ccc}1& & \\ & -1& \\ & & -1\end{array}\right],$ $R_{\mathrm{\&tau;}}=R{R\_}^T=R\left[\begin{array}{ccc}1& & \\ & -1& \\ & & -1\end{array}\right]$

最后根据公式H＝K₂R_τ ^TK^-1实时将原始视频帧影像纠正成垂直于地面拍摄的效果，这个变换效果是图像I_┴始终是垂直于地面往下拍的效果，若姿态矩阵R是相对于局部指北坐标系的，图像I_┴的右侧始终指向北，下侧始终指东。垂直于地面的图像实际上可直接映射到地理信息系统地图平面上，因此本技术实现了从视频图像到地理信息系统地图平面上的实时映射。(b)任意R矩阵的Z轴旋转分解出水平矩阵

对于红外末制导导引头图像获取，或者在颠簸的车、船、飞机上安装有摄像头的视频捕获到视频帧图像的同时，将该帧对应的姿态数据保留并加以处理，保证载体获取的图像始终是保持水平正方的，这样目标识别程序就不用考虑目标的旋转了，这会极大提高目标识别的效率和准确性，也能使得用户获取的视频更稳定更准确。本方法涉及第(b)类计算R_τ的方法如下：

首先设 $R_{\mathrm{\&tau;}}=\left[\begin{array}{ccc}\cos \mathrm{\&tau;}& -\sin \mathrm{\&tau;}& \\ \sin \mathrm{\&tau;}& \cos \mathrm{\&tau;}& \\ & & 1\end{array}\right]$

由R_＝R_τ ^TR列方程得到

$R\_=\left[\begin{array}{ccc}& & 0\\ & & \\ & & \end{array}\right]={\left[\begin{array}{ccc}\cos \mathrm{\&tau;}& -\sin \mathrm{\&tau;}& \\ \sin \mathrm{\&tau;}& \cos \mathrm{\&tau;}& \\ & & 1\end{array}\right]}^T\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]\begin{array}{}\end{array}=\left[\begin{array}{ccc}\cos \mathrm{\&tau;}& \sin \mathrm{\&tau;}& \\ -\sin \mathrm{\&tau;}& \cos \mathrm{\&tau;}& \\ & & 1\end{array}\right]\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$

针对矩阵第1行第3列元素等于0列方程

r₁₃cosτ+r₂₃sinτ＝0

再考虑到{cosτ,sinτ}是2维单位向量，解得

有两个解是正常的，因为旋转两个角度(还有一个相机底朝天，两者刚好相差180度)都能保证本体x轴保持水平于地面。因此必须保证本体y轴朝下，也就是R_的第2行第3列元素值<0,即列约束不等式：

-r₁₃sinτ+r₂₃cosτ＜0

解得 $\left\{\begin{array}{c}\cos \mathrm{\&tau;}=\frac{-r_{23}}{\sqrt{r_{13}^2+r_{23}^2}}\\ \sin \mathrm{\&tau;}=\frac{r_{13}}{\sqrt{r_{13}^2+r_{23}^2}}\end{array}\right.$ 代入上式得到 $-r_{13}\sin \mathrm{\&tau;}+r_{23}\cos \mathrm{\&tau;}=-\sqrt{r_{13}^2+r_{23}^2}<0$

另一个解不满足条件舍去。

最后根据公式H＝K₂R_τ ^TK^-1实时将原始视频帧影像纠正成水平于地平面拍摄的效果。

(c)对地坐标系下的影像正射纠正

为得到的垂直纠正图像且跟随摄像头绕垂直于地面方向轴的旋转而旋转，采用如下方法：

假设摄像头旋转了τ角，

R＝R_τR_中的 $R=\left[\begin{array}{ccc}-\cos \mathrm{\&tau;}& \sin \mathrm{\&tau;}& \\ \sin \mathrm{\&tau;}& \cos \mathrm{\&tau;}& \\ & & -1\end{array}\right]$

列方程

$R_{\mathrm{\&tau;}}=\left[\begin{array}{ccc}& & \\ 0& & \\ & & \end{array}\right]=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]{\left[\begin{array}{ccc}-\cos \mathrm{\&tau;}& \sin \mathrm{\&tau;}& \\ \sin \mathrm{\&tau;}& \cos \mathrm{\&tau;}& \\ & & -1\end{array}\right]}^T\begin{array}{}\end{array}=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]\left[\begin{array}{ccc}-\cos \mathrm{\&tau;}& \sin \mathrm{\&tau;}& \\ \sin \mathrm{\&tau;}& \cos \mathrm{\&tau;}& \\ & & -1\end{array}\right]$

解得: $\left\{\begin{array}{c}\cos \mathrm{\&tau;}=\frac{r_{22}}{\sqrt{r_{21}^2+r_{22}^2}}\\ \sin \mathrm{\&tau;}=\frac{r_{21}}{\sqrt{r_{21}^2+r_{22}^2}}\end{array}\right.$ 或 $\left\{\begin{array}{c}\cos \mathrm{\&tau;}=\frac{-r_{22}}{\sqrt{r_{21}^2+r_{22}^2}}\\ \sin \mathrm{\&tau;}=\frac{-r_{21}}{\sqrt{r_{21}^2+r_{22}^2}}\end{array}\right.$

(d)最终得到：

(e) $R_{\mathrm{\&tau;}}=R{R_{\&perp;}}^T=R\left[\begin{array}{ccc}-\cos \mathrm{\&tau;}& \sin \mathrm{\&tau;}& \\ \sin \mathrm{\&tau;}& \cos \mathrm{\&tau;}& \\ & & -1\end{array}\right]$

最后根据公式H＝K₂R_τ ^TK^‐1实时将原始视频帧影像纠正成垂直于地面拍摄的效果，这个变换效果是图像I_┴会跟随摄像头绕垂直于地面方向轴的旋转而旋转。

基于姿态数据的智能机单像定位方法

假设相机或摄像机内参数据已知，f是焦距的像素长度，(c_x,c_y)是相机主点坐标。

内参矩阵 $K=\left[\begin{array}{ccc}f& & c_x\\ & f& c_y\\ & & 1\end{array}\right]$

单像目标定位整体实施步骤如下：

1.搜集采集物方点，转化成局部坐标系坐标，如指北为x轴，指西为y轴

2.采用权利要求1方法获取智能机的姿态矩阵R_g，R＝△RR_g得到智能机上摄像头的姿态矩阵R。△R是摄像头相对于智能机的转换矩阵

3.拍摄一张图像，自动提取其中的多个特征点，通过匹配算法得到与物点对应的n对物像同名点。

n个物方点的坐标是X_i＝[X_ix,X_iy,X_iz]^T i＝1,…,n

对应n个像点坐标是[u_i v_i]^T i＝1,…,n

4.分无错和容错2种情况计算相机在局部坐标系下的坐标T：

(1)若确认多个特征点无错误，则

采用数学模型解方程得到自身在局部坐标系下坐标Tx，Ty，Tz

$\left[\begin{array}{ccc}{z}_1& & -x_1\\ & z_1& -y_1\\ ...& ...& ...\\ z_n& & -x_n\\ & z_n& -y_n\end{array}\right]\left[\begin{array}{c}{T}_x\\ T_y\\ T_z\end{array}\right]=\left[\begin{array}{c}{z}_1{X}_{1x}-x_1{X}_{1z}\\ z_1{X}_{1y}-y_1{X}_{1z}\\ ...\\ z_n{X}_{\mathrm{nx}}-x_n{X}_{\mathrm{nz}}\\ z_n{X}_{\mathrm{ny}}-y_n{X}_{\mathrm{nz}}\end{array}\right]---\left(\mathrm{HH}2\right)$

其中 $\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right]=R^T\left[\begin{array}{c}\frac{u_i-c_x}{f_x}\\ \frac{v_i-c_y}{f_y}\\ 1\end{array}\right]$

进而得到相机位置T。

(2)若不确认多个特征点是否全部正确，采用Ransac容错算法，通过共线方程模型随机选取两对点n＝2,根据模型式(HH2)计算相机位置T，并通过共线方程z_i[u’_i v’_i 1]^T＝KR(X_i-T)验证T，

通过KR(X_i-T)算出的u’_i,v’_I与u_i,v_i之间的误差距离小于error像素认为是符合共线方程。选取符合共线方程点数最多的正确特征点集ζ_max并计算ζ_max对应的相机位置T*。单目多点容错RANSAC算法流程如下：

1)从n个像点中随机挑选出2个距离足够大的像点，一般至少要大于容忍误差error的2倍；

2)挑选出来的2个像点与物点对应形成2对物象同名点，按式(HH2)计算智能机位置T；

3)用T验证所有n对物象同名点的共线方程对应关系，共线方程KR(X_i‐T)算出的u’_i,v’_I与u_i,v_i之间的误差距离小于error像素认为是符合共线方程，统计符合共线方程的个数i_in,如果i_in数量超过前期的数量n_max则记录本组满足共线方程容忍误差数量最多的正确特征点集ζ_max，并刷新n_max＝i_in；

4)更新迭代次数k+1→k

5)回到第1步，直到满足ransac概率收敛条件(1‐(n_max/n)²)^k<p_e p_e是RANSAC容忍错误概率，一般设为10^‐2～10^‐20之间的一个数值。

6)将前面搜索得到的正确特征点集ζ_max搜集起来进行最后一次多点最小2乘计算，按公式(HH2)计算得到T_best得到最终结果。

5.如需进一步提高精度，可基于数学模型式(HH2)对正确特征点集ζ_max中多点进行LM优化。以期望得到更精确的解。

LM优化中一般是固定所有物点坐标Xix,Xiy,Xiz,i＝1,…,n优化相机的位置T和姿态R

6.如需要得出目标的经纬度，可根据局部坐标系原点的经纬度将相机在局部坐标系下的坐标(Tx，Ty，Tz)转换成大地经纬度。

说明：

真正实现了基于场景图像的室内定位，用于商场室内定位，导航，基于位姿的电子商务商铺广告发布。

如图2所示为单像双点定位示意图

基于智能机姿态数据的单目单点定位方法

假设智能机上的相机内部参数矩阵为:

$K=\left[\begin{array}{ccc}{f}_x& & c_x\\ & f_y& c_y\\ & & 1\end{array}\right]$

fx，fy分别指相机内部参数中的水平和垂直焦距像素长，理想情况下应满足fx＝fy

被观察物体是一个直径＝D大小的物体,

在智能机上成像的宽度是Δu,高度是Δv,

成像中心点像坐标u,v,

采用上述方法获取智能机的姿态矩阵R_g，R＝△RR_g得到智能机上摄像头的姿态矩阵R。△R是摄像头相对于智能机的转换矩阵；

以下分两种情况讨论：

(3)若已知智能机摄像头光心局部坐标系坐标是T,

可单目估算出物体的局部坐标系下三维坐标:

$X=T+z{R}^T{K}^{-1}\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=T+z{R}^T\left[\begin{array}{c}\frac{u-c_x}{f_x}\\ \frac{v-c_y}{f_y}\\ 1\end{array}\right]---\left(\mathrm{HH}7\right)$

其中 $z=D\&CenterDot;\mathrm{MIN}(\frac{f_x}{\mathrm{\&Delta;u}},\frac{f_y}{\mathrm{\&Delta;v}})$

(4)反过来若已知物体的局部坐标系下三维坐标X，可以求相机坐标T

$T=X-z{R}^T\left[\begin{array}{c}\frac{u-c_x}{f_x}\\ \frac{v-c_y}{f_y}\\ 1\end{array}\right]---\left(\mathrm{HH}8\right)$

这种情况实现了相机的单目定位。

这种单像单点定位方法同样也可以采用前述单像双点定位中的Ransac容错模式，只不过随机挑选2个像点改为随机挑选1个像点；数学模型由(HH2)改为由(HH8)得到相机坐标。

基于智能机姿态数据的相对定位算法

假设相机1，2之间有i＝1..n≥2对同名点u_1i,v_1i～u_2i,v_2i，前面所述方法计算智能机的姿态R_g1,R_g2，进一步计算2个智能机上各自相机姿态R₁＝△RR_g1 R₂＝△RR_g2△R是摄像头相对于智能机的转换矩阵。

相机2相对于相机1的姿态矩阵R＝R₂R₁ ^T。

已知相机1的内参矩阵 $K_1=\left[\begin{array}{ccc}{f}_1& & c_{x1}\\ & f_1& c_{y1}\\ & & 1\end{array}\right],$ 相机2的内参矩阵 $K_2=\left[\begin{array}{ccc}{f}_2& & c_{x2}\\ & f_2& c_{y2}\\ & & 1\end{array}\right].$ 其中c_xi,c_yi是相机i的像素主点坐标，f_i是相机i的焦距像素长。

已知两相机之间的距离是L；

可以计算相机2相对于相机1的位移矢量T。

方法步骤如下：

1.首先构建矩阵A

$A=\left[\begin{array}{ccc}{u}_{12*z}{u}_{11R*y}-u_{12*y}{u}_{11R*z}& u_{12*x}{u}_{11R*z}-u_{12*z}{u}_{11R*x}& u_{12*y}{u}_{11R*x}-u_{12*x}{u}_{11R*y}\\ ......& ...& ...\\ u_{n2*z}{u}_{n1R*y}-u_{n2*y}{u}_{n1R*z}& u_{n2*x}{u}_{n1R*z}-u_{n2*z}{u}_{n1R*x}& u_{n2*y}{u}_{n1R*x}-u_{n2*x}{u}_{n1R*y}\end{array}\right]$

其中

u_ji*＝K_i ^-1u_i＝[u_ji*x u_ji*y u_ji*z]^T i＝1,2 j＝1..n

u_jiR*＝R_uji*＝[u_jiR*x u_jiR*y u_jiR*z]^T

2.如果确认n对点都是正确的，则

求约束||t_3x1||＝1下A_nx3t_3x1＝0的解

或采用SVD分解得到A_nx3＝U_nxnW_nx3V^T _3x3,

分解出来的V的最后1列的矢量，即对应于最小奇异值的特征矢量，即为t

T＝-LR^T _t得到结果，结束

否则继续进行以下步骤

3.设置迭代初值n_max＝0 k＝1

4.随机挑选A矩阵中的2行,构建A_2x3

就是求约束||t₂||＝1下A_2x3t₂＝0的解

或采用SVD分解得到A_2x3＝U_2x2W_2x3V^T _3x3,

分解出来的V的最后1列的矢量即为t₂

5.将t₂＝[t_tx t_ty t_tz]^T代入第j＝1..n个点，采用式(AB)验证点j是否是内点,

(u_j2*zu_j1R*y-u_j2*yu_j1R*z)t_tx+(u_j2*xu_j1R*z-u_j2*zu_j1R*x)t_ty+(u_j2*yu_j1R*x-u_j2*xu_j1R*y)t_tz＜ε

                                                           (AB)

其中ε表示最大容忍误差物方长度，暂且相对于两相机之间的距离＝1而言。

满足式(AB)则点j是内点，如果满足内点的数量n_t超过最佳值n_max则ζ_t-->ζn_t-->n_max

6.如果(1-(n_max/n)²)^k<0.00001错误率足够小，退出迭代进入步骤8.

7迭代次数k增加1，回到步骤4反复迭代；

8.最后采用最小二乘法针对最大内点集ζ重新计算一次最优t,

求约束||t_3x1||＝1下A_n’x3t_3x1＝0的解其中n’＝n_max

或采用SVD分解得到A_n’x3＝U_n’xn’W_n’x3V^T _3x3,

分解出来的V的最后1列矢量,即对应于最小奇异值的特征矢量，即为t最终得到T＝-LR^Tt。

以前在未知相机姿态数据的情况下，两相机之间相对定向计算是需要至少5对点的，而本发明中在已知相机姿态的前提下，只需要至少2对即可计算相对位置了，可应用于序列图像三维重建，基于图像的室内外定位等领域。

本算法在得到相对姿态T以后可进一步虚拟出物方点，然后通过多相机LM算法优化R，T数据，使得精度进一步提高。

基于姿态数据的智能机3D遥控方法

基于智能机姿态的多自由度控制

构造通过n个关于R_g的函数实现智能机控制多自由度控制，智能机与被控制设备通过有线或无线机制进行数据通信，实现如鼠标或投影仪指针的移动和旋转控制，智能灯的红绿蓝三分量控制，无人机的空中飞行的3轴姿态控制等。

以控制鼠标为例，传统的鼠标只可能控制x，y两个自由度移动，而本发明的3自由度鼠标控制还包含鼠标在显示屏上的旋转，以便实现各种新应用。

方法的核心思想就是直接根据姿态矩阵R_g的三个矢量r_x,r_y,r_z来控制鼠标，实际上就是期待r_y矢量所指的方位来控制鼠标移动。

首先用权利要求1方法获取遥控智能机的姿态矩阵R_g，假设r_ij是R_g第i行j列的元素值；然后按以下方法实现x方向，y方向，旋转三个自由度控制鼠标：

y方向mouse2[1]：y轴r_y的z分量控制鼠标上下移动；

鼠标的屏幕y坐标Y_m＝(h/2)(1+k_yr₂₃)或者

其中h为屏幕高度，k_y为某一灵敏系数k_y＝0.01～1之间的某值。

分析：当kr₂₃＝0时，智能机r_y保持水平，鼠标的屏幕y坐标正式h/2，位于屏幕中间高度。代码实现如下：

mouse2[1]＝gl.win_height/2+3*gl.win_height/2*imu.r[5]；//y轴的z分量

x方向mouse2[0]：y轴r_y的x,y分量控制鼠标水平移动；

初始时X_m＝W/2 W是显示屏幕宽度。

根据实时不停采集到的姿态矩阵R，不停修正鼠标的屏幕x坐标X_m，

X_m不断减小Tk_x(φ-φ’)

其中φ＝atan2(r₂₂,r₂₁) φ’是上次得到的φ值

k_x为某一灵敏系数k_x＝0.01～1之间的某值。

T是两次采样时间间隔ms

在不停的修改过程中若X_m小于0则X_m等于0，若X_m大于W则X_m等于W。

代码实现如下：

float angleh＝atan2(imu.r[4],imu.r[3])；//φ

static float old_angle_h＝angleh；//φ’

mouse2[0]-＝T*gl.win_width/2*(angleh-old_angle_h)/8；

old_angle_h＝angleh；

//鼠标的水平位置不能超界

if(mouse2[0]<0)mouse2[0]＝0；else if(mouse2[0]>gl.win_width)mouse2[0]＝gl.win_width；

旋转方向mouse2[2]：绕y轴r_y旋转控制鼠标旋转；

根据采集得到的R矩阵得到绕y轴r_y旋转的角度弧度值

τ＝atan2(r₁₃,r₃₃)

这个弧度值可以用作特殊用途的控制，比如游戏中飞机旋转角，机械设计中的零件旋转，房屋建筑CAD设计中的墙面旋转等。

绕y轴旋转来控制鼠标旋转,mouse2[2]得到的就是绕y轴旋转角的弧度值τ，

代码实现如下：

mouse2[2]＝atan2(imu.r[2],imu.r[8])；

如果智能机没有安装磁力计，而是安装了陀螺仪，采用以下方法连续获取姿态矩阵Rg

第1步计算Rg的初值

$R_g=\left[\begin{array}{ccc}\frac{{\mathrm{sa}}_z}{L\sqrt{1-{(a_y/L)}^2}}& -{\mathrm{sa}}_x{a}_y/\left[L^2\sqrt{1-{(a_y/L)}^2}\right]& a_x/L\\ 0& s\sqrt{1-{(a_y/L)}^2}& a_y/L\\ \frac{-s{a}_x}{L\sqrt{1-{(a_y/L)}^2}}& -{\mathrm{sa}}_z{a}_y/\left[L^2\sqrt{1-{(a_y/L)}^2}\right]& a_z/L\end{array}\right]$

a_z>0，±和μ取上半部分，其中a_x,a_y,a_z是当前加速度计的采样值，

当a_z≤0时，s＝-1，a_z>0时,s＝1；

将Rg转换成4元数q₀,q₁,q₂,q₃作为以下第2-7步迭代的初值；

第2步设定e_xInt，e_yInt，e_zInt原始值为0，e_xInt＝0，e_yInt＝0，e_zInt＝0；

第3步根据接收到的重力加速度计数据a_x,a_y,a_z,

得到误差矢量{e_x,e_y,e_z}并计算其累计值e_xInt，e_yInt，e_zInt；

先将矢量{a_x,a_y,a_z}替换成将其单位化以后的矢量

v_x＝2*(q₁*q₃-q₀*q₂)；

v_y＝2*(q₀*q₁+q₂*q₃)；

v_z＝q₀*q₀-q₁*q₁-q₂*q₂+q₃*q₃；

e_x＝(a_y×v_z-a_z×v_y)；

e_y＝(a_z×v_x-a_x×v_z)；

e_z＝(a_x×v_y-a_y×v_x)；

计算误差累计值

e_xInt替换为e_xInt+e_x×Ki；e_yInt替换为e_yInt+e_y×Ki；e_zInt替换为e_zInt+e_z×Ki；

其中Ki为一可调节的正系数，Ki在0.00001至0.5中任意选取；

第4步根据误差矢量{e_x,e_y,e_z}及其累计值纠正陀螺仪数据{g_x0,g_y0,g_z0}

假设智能机读出当前的一组陀螺仪数据为{g_x0,g_y0,g_z0} g_x＝g_x0+Kp×e_x+e_xInt；g_y＝g_y0+Kp×e_y+e_yInt；g_z＝g_z0+Kp×e_z+e_zInt；

其中Kp为一可调节的正系数，Kp在0.000001至20.0中任意选取；

第5步根据陀螺仪数据g_x,g_y,g_z修正四元数

随着不断接收到陀螺仪数据g_x,g_y,g_z，对4元数按如下方式修正，halfT为修正周期，halfT＝0.00001～-10.0*与上次循环的时间间隔

q₀替换为q₀+(-q₁×g_x-q₂×g_y–q₃×g_z)×halfT；

q₁替换为q₁+(q₀×g_x-q₃×g_y+q₂×g_z)×halfT；

q₂替换为q₂+(q₃×g_x+q₀×g_y-q₁×g_z)×halfT；

q3替换为q₃+(-q₂×g_x+q₁×g_y+q₀×g_z)×halfT；

第6步输出Rg矩阵和四元数

将四元数{q₀,q₁,q₂,q₃}单位化成 $\{q_0,q_1,q_2{,q}_3\}/\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}$ 输出

4元数转Rg矩阵公式如下

$\left[\begin{array}{ccc}{q_0}^2+{q_1}^2-{q_2}^2-{q_3}^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& {q_0}^2-{q_1}^2+{q_2}^2-{q_3}^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& {q_0}^2-{q_1}^2-{q_2}^2+{q_3}^2\end{array}\right];$

第7步回到第3步继续接收陀螺仪运动数据更新姿态4元数q₀～q₃，在循环的过程中每次到第7步都能输出当前的R_g矩阵和四元数。

在本技术可以用于智能机姿态遥控设备。

具体实施方式，智能机绕z轴旋转角速度积分控制鼠标在屏幕水平方向移动，智能机绕x轴旋转角速度积分控制鼠标在屏幕垂直方向移动，智能机绕y轴旋转角速度积分控制鼠标自身的旋转(普通的鼠标没有旋转控制的功能，因此这是一个可选功能，可增加或放弃此项功能)。

通过智能机无线控制计算机鼠标、平板电脑、笔记本电脑，电视机、幻灯片指针等各种设备中的指针移动操作。

基于姿态数据的运动方向测定及导航方法

步骤1.按权利要求1中所述方法获取智能机的姿态矩阵R_g

步骤2.测量局部值坐标系下物体的运动加速度a_┴

有两种获取方式，

(1)利用线性加速度传感器得到的a，再通过公式a_┴＝R_g ^Ta计算出a_┴为指北坐标系下智能机的加速度；其中R_g是指北坐标系下的姿态矩阵。

说明

线性加速度a得到的是相对智能机自身的运动加速度，然而用户往往关心的是智能机的绝对运动加速度而不是自身相对运动加速度，所以需要通过姿态矩阵R将直接检测出来的自身相对运动加速度a还原成绝对运动加速度a_┴＝R^Ta。

(2)如果没有线性加速度传感器，测量智能机重力加速度矢量a_g＝[a_gx a_gy a_gz]^T，通过公式a_┴＝R_g ^Ta_g-[0 0 g₀]^T计算得到，其中a_g是重力加速度传感器的测量值，g₀是重力加速度的传感器平均幅值,即重力加速度计在静止条件下测得的加速度矢量模的均值。

这种方式中通常a_┴是相对于局部指北坐标系的，也就是a_┴的x分量代表往北移动的加速度分量，a_┴的y分量代表往西移动的加速度分量。

步骤3.对a_┴积分一次，也就是累加a_┴，即可得到目标在指北坐标系下的移动速度v_┴，积分两次，也就是累加v_┴即可得到目标在指北坐标系下的位移s_┴。

具体实施方式：

v_┴+＝a_┴

s_┴+＝v_┴

步骤4.为避免累积误差，若这时获取到其它精确的定位方法得到的定位值，如GPS，WIFI或权利要求3图像定位得到的定位值s*，立即对目标位置进行纠正。

具体实施方式：

s_┴＝s*

v_┴＝0

a_┴＝0

步骤5.回到步骤1进行下一轮的定位位置修正。

本方法适用于任意智能机姿态的室内定位，运动方向检测和室内辅助定位。这种方式通常a_┴是相对于局部指北坐标系的，也就是a_┴的x分量代表往北移动的分量，a_┴的y分量代表往西移动的分量。

基于姿态数据的虚拟现实方向漫游控制方法

1)根据漫游者的输入或者漫游者自身位置确定局部坐标系中的初始坐标位置(e_x,e_y,e_z)。

2)采用上述方法得到智能机的姿态矩阵R_g，R＝△RR_g得到智能机显示屏的姿态矩阵R。△R是智能机显示屏相对于智能机的转换矩阵；

3)如果是智能机操作者单指相对于智能机屏幕本体滑动矢量△X,△Y

如果|r₃₃|>|r₁₃|且|r₃₃|>|r₂₃|则

v_x＝ΔXr₁₁+ΔYr₂₁ v_y＝ΔXr₁₂₊ΔYr₂₂

否则：

$\left[\begin{array}{c}{v}_x\\ v_y\end{array}\right]=\frac{1}{\sqrt{r_{31}^2+r_{32}^2}\sqrt{r_{13}^2+r_{23}^2}}\left[\begin{array}{cc}{r}_{13}{r}_{31}-r_{23}{r}_{32}& r_{13}{r}_{32}+r_{23}{r}_{31}\\ r_{13}{r}_{32}+r_{23}{r}_{31}& -(r_{13}{r}_{31}-r_{23}{r}_{32})\end{array}\right]\left[\begin{array}{c}\mathrm{\&Delta;X}\\ \mathrm{\&Delta;Y}\end{array}\right]$

如果是智能机操作者双指做拉开或合拢操作，得到拉开和合拢之前双指之间的屏幕距离d₀，以及拉开和合拢之后双指之间的屏幕距离d₁。

4)修正漫游者的移动位置e_x,e_y,e_z

e_x-v_x-->e_x e_y-v_y-->e_y e_z*(d₀/d₁)->e_z

5)根据漫游者e_x,e_y,e_z，注视方向e_x+r₃₁,e_y+r₃₂,e_z+r₃₃，相机自身朝上方向-r₂₁,-r₂₂,-r₂₃

其中r_ij是R矩阵的i行j列元素值,

设置漫游者在局部坐标系中的位置和观察姿态

在Android OpenES中可采用

GLU.gluLookAt(gl,Xd,Yd,Zd,

Xd+r₃₁,Yd+r₃₂,Zd+r₃₃,

‐r₂₁,‐r₂₂,‐r₂₃)；

编程实现设置漫游观察者位置和姿态；

6)智能机屏幕渲染局部坐标系中的三维场景；

7)回到步骤2)动态连续修正漫游者的姿态和位置。

本发明实施例基于姿态数据的视频图像投影到地图的方法，主要包括以下步骤：

采用上述方法得到智能机的姿态矩阵R＝△RR_g，△R是智能机摄像头相对于智能机的转换矩阵；

根据GPS测得拍摄相机在局部坐标系中的坐标为T＝R_v(X-T_s)，其中

$T_s=\left[\begin{array}{c}(N_s+h_s)\cos {\mathrm{\&theta;}}_s\cos {\mathrm{\&phi;}}_s\\ (N_s+h_s)\cos {\mathrm{\&theta;}}_s\sin {\mathrm{\&phi;}}_s\\ (N_s(1-e^2)+h_s)\sin {\mathrm{\&theta;}}_s\end{array}\right]$ $N=\left[\begin{array}{c}(N_s+h)\cos \mathrm{\&theta;}\cos \mathrm{\&phi;}\\ (N_s+h)\cos \mathrm{\&theta;}\sin \mathrm{\&phi;}\\ (N_s(1-e^2)+h)\sin \mathrm{\&theta;}\end{array}\right]$

是局部坐标系原点的经度，θ_s是局部坐标系原点的纬度，h_s局部坐标系原点的高度。

θ_s，h_s确定了地图投影面原点的位置。

N_s是随纬度变化的卯酉圈曲率半径；

a,b是地球椭球的长短轴，是大地测量学中的已知常量；e是偏心率，是定值；

h是智能机在局部坐标系中的Z坐标，即高度

此时相机或摄像头拍摄下图像帧I。

将图像帧I经过Homograph变换H_map＝[K[r₁ r₂ -R_gT]]^-1＝[r₁ r₂ -RT]^-1K^-1得到I_map是视频图像映射到地图上的正射影像。其中

$K=\left[\begin{array}{ccc}{f}_x& & c_x\\ & f_y& c_y\\ & & 1\end{array}\right]$ 是智能机上的相机内部参数矩阵。fx，fy分别指相机内部参数中的水平和垂直焦距像素长，c_x,c_y)是相机主点坐标。

本发明中明确说明了根据姿态矩阵R_g在X轴指北局部坐标系中的几何意义，若定义成其他局部坐标系，相关结论可用已有技术进行替换。

如果定义X轴朝东，Y轴朝北则权利要求1方法1公式(1)中的替换成算出R_g的三个行矢量正是X轴朝东Y朝北坐标系下的3个分量。

方法2,3中的R_g纵东，即直接得到X轴指向东Y轴指向北坐标系下的姿态矩阵。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (17)

1.一种智能机姿态矩阵计算方法，其特征在于，包括以下步骤：

通过智能机加速度计检测三个值{v_ax’,v_ay’,v_az’}计算a_x、a_y和a_z：

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=R_a^T\left[\begin{array}{c}{v_{\mathrm{ax}}}^{\&prime;}\\ {v_{\mathrm{ay}}}^{\&prime;}\\ {v_{\mathrm{az}}}^{\&prime;}\end{array}\right],$

或对{v_ax’,v_ay’,v_az’}进行滤波后得到{v_ax,v_ay,v_az}，计算a_x、a_y和a_z：

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=R_a^T\left[\begin{array}{c}{v}_{\mathrm{ax}}\\ v_{\mathrm{ay}}\\ v_{\mathrm{az}}\end{array}\right],$ 其中R_a是重力加速度传感器相对于智能机的姿态转换矩阵；

通过智能机磁力计获取方位角的三角函数；或者方向传感器获取方位角

计算姿态矩阵

姿态矩阵R_g的第1行是智能机x轴在局部坐标系下的单位矢量；第2行是智能机y轴在局部坐标系下的单位矢量；第3行是智能机z轴在局部坐标系下的单位矢量；其中局部坐标系是z轴沿重力指向地球外，x轴朝北y轴朝西的坐标系；

其中

$R_{\mathrm{\&theta;}}=\left[\begin{array}{ccc}\frac{{\mathrm{sa}}_z}{L\sqrt{1-{(a_y/L)}^2}}& -{\mathrm{sa}}_x{a}_y/\left[L^2\sqrt{1-{(a_y/L)}^2}\right]& a_x/L\\ 0& s\sqrt{1-{(a_y/L)}^2}& a_y/L\\ \frac{-s{a}_x}{L\sqrt{1-{(a_y/L)}^2}}& -{\mathrm{sa}}_z{a}_y/\left[L^2\sqrt{1-{(a_y/L)}^2}\right]& a_z/L\end{array}\right],L=\sqrt{a_x^2+a_y^2+a_z^2}$

当a_z<0时，s＝-1，a_z≥0时，s＝1；

$\left[\begin{array}{c}{m}_{0x}\\ m_{0y}\\ m_{0z}\end{array}\right]=R_{\mathrm{\&theta;}}^T\left[\begin{array}{c}{m}_x\\ m_y\\ m_z\end{array}\right];$

其中m_x,m_y,m_z是智能机磁力计检测出来的三个值{v_mx,v_my,v_mz}，或{v_mx,v_my,v_mz}经过滤波得到的三个值；

若将替换成

则算出的R_g是X轴朝东Y朝北坐标系下的姿态矩阵。

2.根据权利要求1所述的方法，其特征在于，若智能机中安装有方向传感器：则通过该智能机获取方位角带入公式(1)计算姿态矩阵R_g。

3.一种智能机姿态矩阵计算方法，其特征在于，该方法基于智能机支持Android系统，具体包括以下步骤：

获取智能机Android系统函数中的getRotationMatrix函数获取矩阵R_g纵东’,然后将R_g纵东’经过矩阵转置操作得到R_g纵东＝(R_g纵东’)^T，R_g纵东是X轴指向东Y轴指向北坐标系下的姿态矩阵；

计算局部坐标系下的姿态矩阵姿态矩阵R_g的第1行是智能机x轴在局部坐标系下的单位矢量；第2行是智能机y轴在局部坐标系下的单位矢量；第3行是智能机z轴在局部坐标系下的单位矢量；其中局部坐标系是z轴沿重力指向地球外，x轴朝北y轴朝西的坐标系。

4.根据权利要求2所述的方法，其特征在于，若智能机支持旋转矢量传感器，则通过智能机旋转矢量传感器输出的3个数据q₁＝values[0],q₂＝values[1],q₃＝values[2]，计算如果旋转矢量传感器能输出4个数据，则q₀＝values[3]；

再按以下公式计算R_g纵东矩阵

其中，R_g纵东是X轴指向东Y轴指向北坐标系下的姿态矩阵；

再采用公式进一步得到局部坐标系下的姿态矩阵R_g。

5.一种基于智能机姿态矩阵的指向或导航方法，其特征在于，包括以下步骤：

1)定义智能机主方向或载体主方向相对于智能机自身的矢量单位p；

2)根据权利要求1-4中任一项所述的方法计算智能机当前姿态矩阵R_g；

3)计算p_┴＝R_g ^Tp，为智能机主方向或载体主方向相对于指北坐标系的矢量；

4)计算局部坐标系中的目标矢量v_o＝O-P，其中局部坐标系中智能机自身坐标为P，目标的坐标为O，则目标矢量v_o＝O-P；

5)计算矢量p_┴与目标矢量v_o之间夹角的余弦，或计算两个矢量之间的夹角；

6)若夹角的余弦或者两个矢量之间的夹角在预设范围内，则触发智能机执行预设关联事件。

6.一种基于智能机姿态矩阵的2D导航方法，其特征在于，包括以下步骤：

将智能机水平放置，利用方向传感器的第一个值value[0]得出主方向角矢量与智能机到目标矢量之间的夹角，或两者之间的点积；

实时计算智能机2维主矢量{cosφ,sinφ}与2维目标矢量v_o＝O-P示意图，O是局部坐标系中X，Y平面上目标的2维坐标，P是局部坐标系中X，Y平面上的智能机2维坐标；其中φ＝value[0]+△φ，value[0]是方向传感器的第一个值，△φ是在地图平面上用户定义的主方向相对于智能机主方向之间的夹角；

若{cosφ,sinφ}与目标矢量v_o之间的夹角小于预设值说明方向吻合，或者{cosφ,sinφ}与2目标矢量v_o点积除以|v_o|的结果大于预设值，则说明方向吻合；

为便于用户交互，可在地图上将目标矢量v_o箭头与智能机矢量{cosφ,sinφ}同时画出。

7.一种基于智能机姿态数据的视频影像实时纠正方法，其特征在于，包括以下步骤：

采用权利要求1-4中任一项所述的方法，获取智能机拍摄视频的每一帧对应的姿态矩阵R_g，根据R＝△R·R_g得到智能机上的摄像头的局部坐标姿态矩阵R，其中△R是摄像头相对于智能机的转换矩阵；

将R矩阵分解为R＝R_τR_，计算得到矩阵R_τ；

其中，R_是预设期待输出图像的姿态矩阵，R_τ是变换矩阵；

将原始图像I经过H＝K₂R_τ ^TK^-1的Homograph变换得到I’，

其中 $K=\left[\begin{array}{ccc}f& & c_x\\ & f& c_y\\ & & 1\end{array}\right]$ 是拍摄相机的内部参数矩阵，f是焦距的像素长度，(c_x,c_y)是相机主点坐标，c_x＝w/2，c_y＝h/2，w、h是原始图像I的宽和高；

$K_2=\left[\begin{array}{ccc}{f}_2& & c_{x2}\\ & f_2& c_{y2}\\ & & 1\end{array}\right]$ 是纠正以后虚拟相机的内部参数矩阵，f₂是纠正以后虚拟相机的焦距的像素长度，(c_x2,c_y2)是纠正以后虚拟相机主点坐标。

8.一种基于姿态数据的智能机单像定位方法，其特征在于，包括以下步骤：

搜集采集物方点，转化成局部坐标系坐标；

采用权利要求1-4中任一项所述的方法获取智能机的姿态矩阵R_g，根据R＝△R·R_g得到智能机上摄像头的姿态矩阵R，△R是摄像头相对于智能机的转换矩阵；

拍摄一张图像，自动提取其中的多个特征点，通过匹配算法得到与物点对应的n对物像同名点；n个物方点的坐标是X_i＝[X_ix,X_iy,X_iz]^T i＝1,…,n；对应n个像点坐标是[u_i v_i]^Ti＝1,…,n；

计算相机在局部坐标系下的坐标T＝[Tx，Ty，Tz]^T：

(1)特征点无错情况下

若确认多个特征点无错误，则采用数学模型解方程得到自身在局部坐标系下坐标Tx，Ty，Tz，

$\left[\begin{array}{ccc}{z}_1& & -x_1\\ & z_1& -y_1\\ ...& ...& ...\\ z_n& & -x_n\\ & z_n& -y_n\end{array}\right]\left[\begin{array}{c}{T}_x\\ T_y\\ T_z\end{array}\right]=\left[\begin{array}{c}{z}_1{X}_{1x}-x_1{X}_{1z}\\ z_1{X}_{1y}-y_1{X}_{1z}\\ ...\\ z_n{X}_{\mathrm{nx}}-x_n{X}_{\mathrm{nz}}\\ z_n{X}_{\mathrm{ny}}-y_n{X}_{\mathrm{nz}}\end{array}\right]---\left(\mathrm{HH}2\right)$

其中 $\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right]=R^T\left[\begin{array}{c}\frac{u_i-c_x}{f_x}\\ \frac{v_i-c_y}{f_y}\\ 1\end{array}\right]$

进而得到相机位置T＝[Tx，Ty，Tz]^T；

(2)容错情况下

若不确认多个特征点是否全部正确，采用Ransac容错算法，通过共线方程模型随机选取两对点n＝2,根据模型式(HH2)计算相机位置T，并通过共线方程z_i[u’_i v’_i 1]^T＝KR(X_i-T)验证T，通过KR(Xi-T)算出的u’_i,v’_I与u_i,v_i之间的距离误差小于error像素认为是符合共线方程；选取符合共线方程点数最多的正确特征点集ζ_max并计算ζ_max对应的相机位置T*。

9.根据权利要求8所述的方法，其特征在于，其中单目多点容错Ransac算法流程如下：

1)从n个像点中随机挑选出2个距离足够大的像点，至少要大于容忍误差error的2倍；

2)挑选出来的2个像点与物点对应形成2对物象同名点，按式(HH2)计算智能机位置T；

3)用T验证所有n对物象同名点的共线方程对应关系，共线方程KR(X_i-T)算出的u’_i,v’_I与u_i,v_i之间的误差距离小于error像素认为是符合共线方程，统计符合共线方程的个数i_in,如果i_in数量超过前期的数量n_max则记录本组满足共线方程容忍误差数量最大的点集ζ_max，并刷新n_max＝i_in；

4)更新迭代次数k+1→k；

5)回到第1)步，直到满足ransac概率收敛条件(1-(n_max/n)²)^k<p_e，p_e是RANSAC容忍错误概率，设为10^-2～10^-20之间的一个数值；

6)将前面搜索得到的最优内点集合ζ_max搜集起来进行最后一次多点最小2乘计算，按公式(HH2)计算得到最终结果T_best。

10.基于智能机姿态数据的单目单点定位方法，其特征在于，包括步骤：

假设智能机上的相机内部参数矩阵为:

$K=\left[\begin{array}{ccc}{f}_x& & c_x\\ & f_y& c_y\\ & & 1\end{array}\right]$

fx，fy分别指相机内部参数中的水平和垂直焦距像素长，

被观察物体是一个直径＝D大小的物体,

在智能机上成像的宽度是Δu,高度是Δv,

成像中心点像坐标(u,v),(c_x,c_y)是相机主点坐标；

采用权利要求1-4方法获取智能机的姿态矩阵R_g，R＝△RR_g得到智能机上摄像头的姿态矩阵R，△R是摄像头相对于智能机的转换矩阵；

计算 $z=D\&CenterDot;\mathrm{MIN}(\frac{f_x}{\mathrm{\&Delta;u}},\frac{f_y}{\mathrm{\&Delta;v}})$

(1)若已知智能机摄像头光心局部坐标系坐标是T,

单目估算出物体的局部坐标系下三维坐标X:

$X=T+z{R}^T{K}^{-1}\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=T+z{R}^T\left[\begin{array}{c}\frac{u-c_x}{f_x}\\ \frac{v-c_y}{f_y}\\ 1\end{array}\right]$

(2)若已知物体的局部坐标系下三维坐标X，计算相机坐标T

$T=X-z{R}^T\left[\begin{array}{c}\frac{u-c_x}{f_x}\\ \frac{v-c_y}{f_y}\\ 1\end{array}\right].$

11.一种基于智能机姿态数据的相对定位方法，其特征在于，假设相机1，2之间有i＝1..n≥2对同名点u_1i,v_1i～u_2i,v_2i，

相机2相对于相机1的姿态矩阵R＝R₂R₁ ^T；其中R₁＝△RR_g1 R₂＝△RR_g2,R_g1，R_g2是智能机相机在拍摄的1,2两个地点测出来的姿态矩阵数据,△R是摄像头相对于智能机的转换矩阵；已知相机1的内参矩阵 $K_1=\left[\begin{array}{ccc}{f}_1& & c_{x1}\\ & f_1& c_{y1}\\ & & 1\end{array}\right],$ 相机2的内参矩阵 $K_2=\left[\begin{array}{ccc}{f}_2& & c_{x2}\\ & f_2& c_{y2}\\ & & 1\end{array}\right],$ 其中(c_xi,c_yi)是相机i的像素主点坐标，f_i是相机i的焦距像素长，已知两相机之间的距离是L；计算相机2相对于相机1的位移矢量T；

方法具体包括如下步骤：

1)用权利要求1所述的方法计算智能机的姿态R_g1,R_g2，进一步计算2个智能机上各自相机姿态R₁＝△RR_g1 R₂＝△RR_g2，得到相对姿态R＝R₂R₁ ^T

2)首先构建矩阵A

$A=\left[\begin{array}{ccc}{u}_{12*z}{u}_{11R*y}-u_{12*y}{u}_{11R*z}& u_{12*x}{u}_{11R*z}-u_{12*z}{u}_{11R*x}& u_{12*y}{u}_{11R*x}-u_{12*x}{u}_{11R*y}\\ ......& ...& ...\\ u_{n2*z}{u}_{n1R*y}-u_{n2*y}{u}_{n1R*z}& u_{n2*x}{u}_{n1R*z}-u_{n2*z}{u}_{n1R*x}& u_{n2*y}{u}_{n1R*x}-u_{n2*x}{u}_{n1R*y}\end{array}\right]$

其中u_ji*＝K_i ^-1u_i＝[u_ji*x u_ji*y u_ji*z]^T，i＝1,2；j＝1..n；u_jiR*＝Ru_ji*＝[u_jiR*x u_jiR*y u_jiR*z]^T；

3)如果确认n对点都是正确的，则求约束||t_3x1||＝1下A_nx3t_3x1＝0的解或采用SVD分解得到A_nx3＝U_nxnW_nx3V^T _3x3,分解出来的V的最后1列的矢量，即对应于最小奇异值的特征矢量，即为t，T＝-LR^Tt得到结果，结束；否则继续进行步骤4)；

4)设置迭代初值n_max＝0，k＝1；

5)随机挑选A矩阵中的2行，构建A_2x3；

就是求约束||t₂||＝1下A_2x3t₂＝0的解，或采用SVD分解得到A_2x3＝U_2x2W_2x3V^T _3x3,

分解出来的V的最后1列的矢量即为t₂；

6)将t₂＝[t_tx t_ty t_tz]^T代入第j＝1...n个点，采用式(AB)验证点j是否是内点,

(u_j2*zu_j1R*y-u_j2*yu_j1R*z)t_tx+(u_j2*xu_j1R*z-u_j2*zu_j1R*x)t_ty+(u_j2*yu_j1R*x-u_j2*xu_j1R*y)t_tz＜ε

                                                           (AB)

其中ε表示最大容忍误差物方长度，相对于两相机之间的距离＝1而言，

满足式(AB)，则点j是内点，如果满足内点的数量n_t超过最佳值n_max则ζ_t-->ζ，n_t-->n_max；

7)如果(1-(n_max/n)²)^k<0.00001，退出迭代进入步骤9)；

8)迭代次数k增加1，回到步骤5)反复迭代；

9)最后采用最小二乘法针对最大内点集ζ重新计算一次最优t；

求约束||t_3x1||＝1下A_n’x3t_3x1＝0的解；其中n’＝n_max；

或采用SVD分解得到A_n’x3＝U_n’xn’W_n’x3V^T _3x3；分解出来的V的最后1列矢量,即对应于最小奇异值的特征矢量，即为t；最终得到T＝-LR^Tt。

12.一种基于姿态矩阵的智能机3D遥控方法，其特征在于，包括以下步骤：

设计关于姿态矩阵R_g的n个函数f₁(R_g),…,f_n(R_g)，实现智能机姿态的n自由度遥控；

首先用权利要求1-4中任一项所述的方法获取遥控智能机的姿态矩阵R_g，假设r_ij是R_g第i行j列的元素值；

然后按以下方法实现x方向，y方向，旋转三个自由度控制：

1)控制y值：y轴r_y的z分量控制y方向上下移动

控制对象的屏幕y坐标Y_m＝(h/2)(1+k_yr₂₃)或者

其中h为屏幕高度，k_y为某一灵敏系数，k_y＝0.01～1之间的某值；

2)控制x值：y轴r_y的x,y分量控制x方向水平移动

初始时X_m＝W/2，W是显示屏幕宽度；根据实时不停采集到的姿态矩阵R，不停修正控制对象的屏幕x坐标X_m，X_m不断减小Tk_x(φ-φ’)，其中φ＝atan2(r₂₂,r₂₁)；φ’是上次得到的φ值；k_x为某一灵敏系数，k_x＝0.01～1之间的某值，T是两次采样时间间隔；在不停的修改过程中若X_m小于0则X_m等于0，若X_m大于W则X_m等于W；

3)控制旋转值：绕y轴r_y旋转控制旋转

根据采集得到的R矩阵得到绕y轴r_y旋转的角度弧度值τ＝atan2(r₁₃,r₃₃)。

13.根据权利要求12所述的方法，其特征在于，如果智能机安装了陀螺仪，则采用以下方法连续获取姿态矩阵R_g：

第1步计算R_g的初值

$R_g=\left[\begin{array}{ccc}\frac{{\mathrm{sa}}_z}{L\sqrt{1-{(a_y/L)}^2}}& -{\mathrm{sa}}_x{a}_y/\left[L^2\sqrt{1-{(a_y/L)}^2}\right]& a_x/L\\ 0& s\sqrt{1-{(a_y/L)}^2}& a_y/L\\ \frac{-s{a}_x}{L\sqrt{1-{(a_y/L)}^2}}& -{\mathrm{sa}}_z{a}_y/\left[L^2\sqrt{1-{(a_y/L)}^2}\right]& a_z/L\end{array}\right]$

a_z>0，±和μ取上半部分，其中a_x,a_y,a_z是当前加速度计的采样值，

当a_z≤0时，s＝-1，a_z>0时,s＝1；

将Rg转换成4元数q₀,q₁,q₂,q₃作为以下第2-7步迭代的初值；

第2步设定e_xInt，e_yInt，e_zInt原始值为0，e_xInt＝0，e_yInt＝0，e_zInt＝0；

第3步根据接收到的重力加速度计数据a_x,a_y,a_z,

得到误差矢量{e_x,e_y,e_z}并计算其累计值e_xInt，e_yInt，e_zInt；

先将矢量{a_x,a_y,a_z}替换成将其单位化以后的矢量

v_x＝2*(q₁*q₃-q₀*q₂)；

v_y＝2*(q₀*q₁+q₂*q₃)；

v_z＝q₀*q₀-q₁*q₁-q₂*q₂+q₃*q₃；

e_x＝(a_y×v_z-a_z×v_y)；

e_y＝(a_z×v_x-a_x×v_z)；

e_z＝(a_x×v_y-a_y×v_x)；

计算误差累计值

e_xInt替换为e_xInt+e_x×Ki；e_yInt替换为e_yInt+e_y×Ki；e_zInt替换为e_zInt+e_z×Ki；

其中Ki为一可调节的正系数，Ki在0.00001至0.5中任意选取；

第4步根据误差矢量{e_x,e_y,e_z}及其累计值纠正陀螺仪数据{g_x0,g_y0,g_z0}

假设智能机读出当前的一组陀螺仪数据为{g_x0,g_y0,g_z0}g_x＝g_x0+Kp×e_x+e_xInt；g_y＝g_y0+Kp×e_y+e_yInt；g_z＝g_z0+Kp×e_z+e_zInt；

其中Kp为一可调节的正系数，Kp在0.000001至20.0中任意选取；

第5步根据陀螺仪数据g_x,g_y,g_z修正四元数

随着不断接收到陀螺仪数据g_x,g_y,g_z，对4元数按如下方式修正，halfT为修正周期，halfT＝0.00001～-10.0*与上次循环的时间间隔

q₀替换为q₀+(-q₁×g_x-q₂×g_y–q₃×g_z)×halfT；

q₁替换为q₁+(q₀×g_x-q₃×g_y+q₂×g_z)×halfT；

q₂替换为q₂+(q₃×g_x+q₀×g_y-q₁×g_z)×halfT；

q3替换为q₃+(-q₂×g_x+q₁×g_y+q₀×g_z)×halfT；

第6步输出Rg矩阵和四元数

将四元数{q₀,q₁,q₂,q₃}单位化成 $\{q_0,q_1,q_2{,q}_3\}/\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}$ 输出4元数转Rg矩阵公式如下

$\left[\begin{array}{ccc}{q_0}^2+{q_1}^2-{q_2}^2-{q_3}^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& {q_0}^2-{q_1}^2+{q_2}^2-{q_3}^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& {q_0}^2-{q_1}^2-{q_2}^2+{q_3}^2\end{array}\right];$

第7步回到第3步继续接收陀螺仪运动数据更新姿态4元数q₀～q₃，在循环的过程中每次到第7步都能输出当前的R_g矩阵和四元数。

14.一种基于姿态数据的运动方向测定及导航方法，其特征在于，包括以下步骤：

步骤1.根据权利要求1-4中任一项所述的方法获取智能机的姿态矩阵R_g；

步骤2.测量局部坐标系下物体的运动加速度a_┴，a_┴的x分量代表往北移动的分量，a_┴的y分量代表往西移动的分量；

步骤3.对运动加速度a_┴积分一次，得到目标在指北坐标系下的移动速度v_┴，积分两次，得到目标在指北坐标系下的位移s_┴；

步骤4.为避免累积误差，若获取到其它精确的定位方法得到的定位值，则立即对目标位置进行纠正；

步骤5.回到步骤1进行下一轮的定位位置修正。

15.根据权利要求14所述的方法，其特征在于，步骤2中测量局部坐标系下物体的运动加速度a_┴通过以下方法得到：

(1)若智能机中有线性加速度传感器，则利用线性加速度传感器得到加速度a，再通过公式a_┴＝R_g ^Ta计算出为指北坐标系下智能机的加速度a_┴；其中R_g是指北坐标系下的姿态矩阵；或者

(2)如果没有线性加速度传感器，测量智能机重力加速度矢量a_g＝[a_gx a_gy a_gz]^T，通过公式a_┴＝R_g ^Ta_g-[0 0 g₀]^T计算得到，其中a_g是重力加速度传感器的测量值，g₀是重力加速度的传感器平均幅值,即重力加速度计在静止条件下测得的加速度矢量模的均值。

16.一种基于姿态数据的虚拟现实方向漫游控制方法，其特征在于，包括以下步骤：

1)根据漫游者的输入或者漫游者的自身位置设置局部坐标系中的初始坐标位置(e_x,e_y,e_z)；

2)采用权利要求1-4的方法得到智能机的姿态矩阵R_g，R＝△RR_g得到智能机显示屏的姿态矩阵R，△R是智能机显示屏相对于智能机的转换矩阵；

3)获取智能机漫游者手指在屏幕本体上的滑动数据；

4)在滑动数据是漫游者单指相对于智能机屏幕本体滑动矢量△X,△Y的情况下，如果|r₃₃|>|r₁₃|且|r₃₃|>|r₂₃|，其中r_ij是R矩阵的i行j列元素值，则单指滑动后，在局部坐标系中的定位坐标改正数(v_x，v_y)，v_x＝ΔXr₁₁+ΔYr₂₁ v_y＝ΔXr₁₂+ΔYr₂₂；否则：

$\left[\begin{array}{c}{v}_x\\ v_y\end{array}\right]=\frac{1}{\sqrt{r_{31}^2+r_{32}^2}\sqrt{r_{13}^2+r_{23}^2}}\left[\begin{array}{cc}{r}_{13}{r}_{31}-r_{23}{r}_{32}& r_{13}{r}_{32}+r_{23}{r}_{31}\\ r_{13}{r}_{32}+r_{23}{r}_{31}& -(r_{13}{r}_{31}-r_{23}{r}_{32})\end{array}\right]\left[\begin{array}{c}\mathrm{\&Delta;X}\\ \mathrm{\&Delta;Y}\end{array}\right]$

5)在滑动数据是漫游者双指做拉开或合拢操作所获得的情况下，则得到拉开和合拢之前双指之间的屏幕距离，以及拉开和合拢之后双指之间的屏幕距离；

6)根据步骤4)和5)中得到的数据修正漫游者在局部坐标系中的坐标位置(e_x,e_y,e_z)，并根据修正后的坐标位置显示局部坐标系中的三维场景。

17.一种基于姿态数据的视频图像投影到地图的方法，其特征在于，包括以下步骤：

采用权利要求1-4所述的方法得到智能机的姿态矩阵R＝△RR_g，△R是智能机摄像头相对于智能机的转换矩阵；

根据GPS测得拍摄相机在局部坐标系中的坐标为T，此时相机或摄像头拍摄下图像帧I；

将图像帧I经过Homograph变换H_map＝[K[r₁ r₂ -RT]]^-1＝[r₁ r₂ -RT]^-1K^-1得到I_map，I_map是视频图像映射到地图上的正射影像，其中r₁ r₂分别是R矩阵的第1列和第2列，K是智能机上的相机内部参数矩阵， $K=\left[\begin{array}{ccc}{f}_x& & c_x\\ & f_y& c_y\\ & & 1\end{array}\right],$ fx，fy分别指相机内部参数中的水平和垂直焦距像素长，c_x,c_y)是相机主点坐标。