CN105654466A - 地球仪的位姿检测方法及其装置 - Google Patents

Abstract

本发明涉及地球仪的姿态检测方法及其装置，其属于科普、教育、智能加工领域。本发明通过硬件陀螺仪传感器及一系列算法，克服了技术本身常用计算方式的缺点，把此部分繁杂的位姿标定的计算过程交给硬件来完成，最大限度节省移动端的硬件资源给其他后续处理部分，比如后续用于3D渲染部分，以便渲染出更绚丽的图像。

Description

地球仪的位姿检测方法及其装置

技术领域

本发明涉及地球仪的姿态检测方法及其装置,其属于科普、教育、智能加工领域。

背景技术

目前没有一种技术对于真实的三维物体位姿标定，三维物体位姿标定是目前识别技术的难点，普通的图像识别技术运算量巨大，效果不是很好，达不到实时的要求，并且计算过程复杂繁琐，与其他技术结合时占用硬件资源。

发明内容

鉴于已有生产方法存在的缺陷，本发明提供地球仪的姿态检测方法及其装置克服了原有技术的缺点，通过硬件陀螺仪传感器简化计算过程，为与其他技术结合时节省硬件资源。

为实现上述目的，本发明所采用的技术方案是地球仪的姿态检测方法，包括如下步骤：

步骤1：初始化地球仪中的陀螺仪传感器得到初始角度，根据初始角度计算得到初始四元数；

步骤2：初始化刻度标定系数；

步骤3：用陀螺仪传感器检测地球仪状态，更新四元数，通过四元数及三角函数求出欧拉角作为地球仪的姿态角度；

步骤4：手机通过步骤1到步骤3得到手机的姿态角度；具体为初始化手机中的陀螺仪传感器得到初始角度，根据初始角度计算得到初始四元数；初始化刻度标定系数；用陀螺仪传感器检测手机状态，更新四元数，通过四元数及三角函数求出欧拉角作为手机的姿态角度；

步骤5：手机对地球仪球体进行识别，通过步骤1所述的地球仪的姿态角度和步骤4所述的手机的姿态角度结合霍夫变换确定球心在摄像头中的位置坐标。

步骤6：通过所述步骤5中地球仪的球心在摄像头的位置以及地球仪在摄像头中的半径得到球的偏移角度。

步骤7：将步骤3中地球仪的姿态角度、步骤4中手机的姿态角度和步骤6中的偏移角度做差得到地球仪在手机端的姿态角度。

步骤8：手机客户端接收地球仪的姿态角度、深度距离和地球仪的球心在摄像头的位置信息，通过相机中心点、球心在摄像头的位置以及深度距离建立两点式得出虚拟地球仪在相机坐标系的三维坐标，通过接收到的地球仪的角度信息得出移动设备的摄像头注视点经纬度及地球仪在手机虚拟空间的相对位置与姿态角度。所述步骤1中根据初始角度计算得到初始四元数的公式为：

$C_B^N=\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\θ}\\ -\cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}\sin \mathrm{\γ}\sin \mathrm{\θ}+\cos \mathrm{\γ}\cos \mathrm{\ψ}& \sin \mathrm{\γ}\cos \mathrm{\θ}\\ \cos \mathrm{\ψ}\cos \mathrm{\γ}\sin \mathrm{\θ}+\sin \mathrm{\γ}\sin \mathrm{\ψ}& \sin \mathrm{\ψ}\cos \mathrm{\γ}\sin \mathrm{\θ}-\sin \mathrm{\γ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]$

$\left\{\begin{array}{c}\left|q_1\right|=0.5\×\sqrt{1+T11-T22-T33}\\ \left|q_2\right|=0.5\×\sqrt{1-T11+T22-T33}\\ \left|q_3\right|=0.5\×\sqrt{1-T11-T22+T33}\\ \left|q_0\right|=0.5\×\sqrt{1+T11+T22+T33}\end{array}\right.$

其中，q0、q1、q2、q3代表四元数；

ψ₀表示方位角，θ₀表示俯仰角，γ₀表示翻滚角。

通过延标尺移动手机将摄像头中呈现的图像和手机中呈现的图像重合，并输入重合后手机在标尺上的刻度。

所述步骤3中所述更新四元数的具体方法为：

1)在时间中断时获取陀螺仪传感器的数值；

2)用获取的数值进行更新的四元数；

3)将更新后的四元数进行优化进而再一次更新四元数。

所述步骤6中球的偏移角度具体的求解方法为：

1)将球心在摄像头中的位置通过相似变换得到实际摄像头的偏移量；

2)根据地球仪在摄像头中的半径和地球仪的实际半径建立相似比例标定得到摄像头到球体的距离，即深度距离；

3)根据实际摄像头的偏移量和深度距离求得偏移角度。

所述位姿检测方法还包括如下步骤：

客户端程序3D渲染模块接受到步骤5中得到的地球仪的姿态数据和切平面方向后，再根据后台数据库与用户选择的结果将指定的3D模型通过手机注视点坐标及切平面方向放置在对应的位置上进行渲染与实时视频背景拟合，根据步骤7中的球体在手机端的姿态，将虚拟地球仪旋转至该角度与真实地球仪吻合，显示AR图像。

将步骤5中得到的球心坐标结合球面方程确定切平面方向。

所述的优化四元数的方法具体是：利用卡尔曼滤波算法进行滤波，包括卡尔曼滤波算法需要状态方程以及观测方程进行滤波。

用于地球仪位姿检测的装置，包括地球仪与带摄像头的移动设备，地球仪由陀螺仪传感器和控制芯片组成，陀螺仪传感器用于记录地球仪的姿态数据，通过移动设备上的蓝牙进行搜索配对将地球仪的姿态数据发到移动设备。

用于地球仪位姿检测的装置，所述的移动设备还包括3D渲染模块，用于接受到地球仪的姿态数据及切平面方向后，根据后台数据库与用户选择的结果将指定的3D模型通过手机注视点坐标及切平面方向放置在对应的位置上进行渲染与实时视频背景拟合；所述移动设备为带摄像头的手机或PAD；所述地球仪位姿检测的装置还包括加速度传感器；所述地球仪位姿检测的装置还包括地磁传感器。

采用上述方法后，与现有技术方法相比具有以下有益效果：本发明克服了技术本身常用计算方式的缺点，把此部分繁杂的位姿标定的计算过程交给硬件来完成，最大限度节省移动端的硬件资源给其他后续处理部分，比如后续用于3D渲染部分，以便渲染出更绚丽的图像。

附图说明

图1为硬件流程图。

具体实施方式

第一步、打开地球仪电源，地球仪硬件参数初始化：

对地球仪上电，对地球仪内部电路传感器进行硬件电路初始化，包括MPU5060陀螺仪传感器初始化，加速度传感器初始化，地磁传感器初始化；并读取加速度传感器的原始加速度，在静止的条件下求取了1600个陀螺仪的平均值，初始化时间中断寄存器，使硬件电路在之后以每20ms的频率采集各个传感器的信息。并初始化四元数算法，得到初始的方位角ψ₀，俯仰角θ₀，翻滚角γ₀，MPU5060陀螺仪传感器的标准输出参数命名方位角ψ₀，俯仰角θ₀，翻滚角γ₀，即MPU5060XYZ三个轴上的角度值并通过

$C_B^N=\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\θ}\\ -\cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}\sin \mathrm{\γ}\sin \mathrm{\θ}+\cos \mathrm{\γ}\cos \mathrm{\ψ}& \sin \mathrm{\γ}\cos \mathrm{\θ}\\ \cos \mathrm{\ψ}\cos \mathrm{\γ}\sin \mathrm{\θ}+\sin \mathrm{\γ}\sin \mathrm{\ψ}& \sin \mathrm{\ψ}\cos \mathrm{\γ}\sin \mathrm{\θ}-\sin \mathrm{\γ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]$

以及 $\left\{\begin{array}{c}\left|q_1\right|=0.5\×\sqrt{1+T11-T22-T33}\\ \left|q_2\right|=0.5\×\sqrt{1-T11+T22-T33}\\ \left|q_3\right|=0.5\×\sqrt{1-T11-T22+T33}\\ \left|q_0\right|=0.5\×\sqrt{1+T11+T22+T33}\end{array}\right.$ 公式得到四元数q0，q1，q2，q3。

第二步、打开手机应用程序，初始化刻度标定系数：

初始化刻度标定系数是为了校正不同摄像头测量距离的差别，使用者在手机或者平板电脑第一次使用需要进行摄像头测量的标定，此时手机摄像头会出现一个和实物一样图案的图样，使用者通过延标尺移动手机将摄像头中呈现的图像和手机中呈现的图像重合，并输入重合后手机在标尺上的刻度d0。

第三步、地球仪检测球的角度，将自己的方向以及球的角度通过蓝牙传输到手机：硬件在时间中断中获取加速度、地磁、陀螺仪传感器的数值，在XYZ三个轴的数值分别为：AX，AY，AZ；MX，MY，MZ；GX，GY，GZ。将这些数值进行更新新的四元数：

$\left[\begin{array}{c}{q}_0\left(k+1\right)\\ q_1\left(k+1\right)\\ q_2\left(k+1\right)\\ q_3\left(k+1\right)\end{array}\right]=\left[\begin{array}{cccc}\cos \left(\frac{\mathrm{\Δ}\mathrm{\θ}}{2}\right)& -\frac{{\mathrm{\Δ\θ}}_x}{\mathrm{\Δ}\mathrm{\θ}}\sin \left(\frac{\mathrm{\Δ}\mathrm{\θ}}{2}\right)& -\frac{{\mathrm{\Δ\θ}}_y}{\mathrm{\Δ}\mathrm{\θ}}\sin \left(\frac{\mathrm{\Δ}\mathrm{\θ}}{2}\right)& -\frac{{\mathrm{\Δ\θ}}_z}{\mathrm{\Δ}\mathrm{\θ}}\sin \left(\frac{\mathrm{\Δ}\mathrm{\θ}}{2}\right)\\ \frac{{\mathrm{\Δ\θ}}_x}{\mathrm{\Δ}\mathrm{\θ}}\sin \left(\frac{\mathrm{\Δ}\mathrm{\θ}}{2}\right)& \cos \left(\frac{\mathrm{\Δ}\mathrm{\θ}}{2}\right)& \frac{{\mathrm{\Δ\θ}}_z}{\mathrm{\Δ}\mathrm{\θ}}\sin \left(\frac{\mathrm{\Δ}\mathrm{\θ}}{2}\right)& -\frac{{\mathrm{\Δ\θ}}_y}{\mathrm{\Δ}\mathrm{\θ}}\sin \left(\frac{\mathrm{\Δ}\mathrm{\θ}}{2}\right)\\ \frac{{\mathrm{\Δ\θ}}_y}{\mathrm{\Δ}\mathrm{\θ}}\sin \left(\frac{\mathrm{\Δ}\mathrm{\θ}}{2}\right)& -\frac{{\mathrm{\Δ\θ}}_z}{\mathrm{\Δ}\mathrm{\θ}}\sin \left(\frac{\mathrm{\Δ}\mathrm{\θ}}{2}\right)& \cos \left(\frac{\mathrm{\Δ}\mathrm{\θ}}{2}\right)& \frac{{\mathrm{\Δ\θ}}_x}{\mathrm{\Δ}\mathrm{\θ}}\sin \left(\frac{\mathrm{\Δ}\mathrm{\θ}}{2}\right)\\ \frac{{\mathrm{\Δ\θ}}_z}{\mathrm{\Δ}\mathrm{\θ}}\sin \left(\frac{\mathrm{\Δ}\mathrm{\θ}}{2}\right)& \frac{{\mathrm{\Δ\θ}}_y}{\mathrm{\Δ}\mathrm{\θ}}\sin \left(\frac{\mathrm{\Δ}\mathrm{\θ}}{2}\right)& -\frac{{\mathrm{\Δ\θ}}_x}{\mathrm{\Δ}\mathrm{\θ}}\sin \left(\frac{\mathrm{\Δ}\mathrm{\θ}}{2}\right)& \cos \left(\frac{\mathrm{\Δ}\mathrm{\θ}}{2}\right)\end{array}\right]\×\left[\begin{array}{c}{q}_0\left(k\right)\\ q_1\left(k\right)\\ q_2\left(k\right)\\ q_3\left(k\right)\end{array}\right]$

△θxyz是△θ在xyz三轴上的分量，△θ是角度变化的大小

更新后的四元数，使用卡尔曼滤波算法进行优化，卡尔曼滤波算法需要状态方程(滤波和估计的参数)以及观测方程(传感器的精确数值)设动态系统的状态方程和测量方程分别为：

X_K＝Φ_K,K-1X_K-1+Γ_K,K-1W_K-1

Z_K＝H_KX_K+V_K

上两式子中，X_K是k时刻的系统状态，Φ_K,K-1和Γ_K,K-1是k-1时刻到k时刻的状态转移矩阵，Z_K是k时刻的测量值，H_K是测量系统的参数，W_K和V_K分别表示过程和测量的噪声，他们被假设成高斯白噪声。如果被估计状态和观测量是满足上述第一式，系统过程噪声和观测噪声满足第二式的假设，k时刻的观测X_K的估计可按下述方程求解。

进一步预测：X_K,K-1＝Φ_K,K-1X_K-1(1)

状态估计： ${\hat{X}}_k={\hat{X}}_{K,K-1}+K_K\[Z_K{H}_K{\hat{X}}_{K,K-1}\]---\left(2\right)$

滤波增益矩阵： $K_K=P_{K,K-1}{H}_K^T{R}_K^{-1}---\left(3\right)$

一步预测误差方差阵： $P_{K,K-1}={\mathrm{\Φ}}_{K,K-1}{P}_{K,K-1}{\mathrm{\Φ}}_{K,K-1}^T+{\mathrm{\Γ}}_{K,K-1}{Q}_{K,K-1}{\mathrm{\Γ}}_{K,K-1}^T---\left(4\right)$

估计误差方差阵：P_K＝[I-K_KH_K]P_K,K-1(5)

给定初值X₀和P₀，根据k时刻的观测值Z_K，就可以递推计算得k时刻的状态估计(K＝1，2，N)。

进而再一次更新四元数。然后通过反变换以及反三角函数求解出更新后的欧拉角，从而得到地球仪的姿态以及偏角，得到θ，γ，ψ。

第四步、手机获取自身姿态：

手机通过和硬件同样的方法和算法检测自身的姿态，得到θ1，γ1，ψ1。

第五步、手机对地球仪球体进行识别，并确定球心位置以及摄像头中球的半径：手机对地球仪球体进行识别，因为地球仪有着明显的图案来与背景区分，采用霍夫变换(参见OpenCV标准函数库)可以精确的求得球心及轮廓与半径，确定球心在摄像头的位置X1，Y1，通过相似变换得到实际摄像头的偏移量X0，Y0，根据球体在摄像头的半径r，以及实际半径R，建立一个相似比例的标定，从而计算出摄像头到球体的距离D：r/d0＝R/D。

第六步、通过球心位置以及球在摄像头中的大小判断球的偏移角度：

根据公式 ${\mathrm{\θ}}_2=arcsin\frac{Y_0}{\sqrt{D^2-X_0^2}}$ 以及 ${\mathrm{\ψ}}_2=arcsin\frac{X_0}{\sqrt{D^2-Y_0^2}}$ 计算偏移角度

第七步、通过以上数据重新建立球体坐标系：

将以上欧拉角做差，重新建立球体的坐标系：θ_final＝θ-θ₁-θ₂，ψ_final＝ψ-ψ₁-ψ₂，γ_final＝γ-γ₁，即为球体在手机端的姿态。

第八步、手机客户端实现增强现实，并计算标记点的经纬度：

客户端程序启动后通过设备蓝牙进行搜索与配对，连接成功后实时接收地球仪发送的地球仪姿态等信息，确认连接建立成功后打开摄像头对图像进行识别找出轮廓圆的半径和圆心，由圆心和相机中心确定射线方程(已知相机中心点和地球仪球心，根据两点式可得出直线方程(x-x1)/(x2-x1)＝(y-y1)/(y2-y1)＝(z-z1)/(z2-z1))，再由半径确定深度距离D(D：r/d0＝R/D)，带入射线方程便可得出虚拟地球仪在相机坐标系的三维坐标，在通过蓝牙接收到地球仪的角度信息便可得出移动设备的摄像头注视点经纬度(已知)及地球仪在手机虚拟空间的相对位置与姿态。进而根据球面方程和球心确定切平面方向将数据传送给3D渲染模块。

第九步、手机计算需要显示的增强现实模型：

客户端程序3D渲染模块接受到地球仪的姿态数据及切平面方向后，在根据后台数据库与用户选择的结果将指定的3D模型通过手机注视点坐标及切平面方向放置在对应的位置上进行渲染与实时视频背景拟合已达到增强现实的目的。

Claims (10)

1.地球仪的位姿检测方法，其特征在于，包括如下步骤：

步骤1：初始化地球仪中的陀螺仪传感器得到初始角度，根据初始角度计算得到初始四元数；

步骤2：初始化刻度标定系数；

步骤3：用陀螺仪传感器检测地球仪状态，更新四元数，通过四元数及三角函数求出欧拉角作为地球仪的姿态角度；

步骤4：手机通过步骤1到步骤3得到手机的姿态角度；

步骤5：手机对地球仪球体进行识别，通过步骤1所述的地球仪的姿态角度和步骤4所述的手机的姿态角度结合霍夫变换确定球心在摄像头中的位置坐标；

步骤6：通过所述步骤5中地球仪的球心在摄像头的位置以及地球仪在摄像头中的半径得到球的偏移角度；

步骤7：将步骤3中地球仪的姿态角度、步骤4中手机的姿态角度和步骤6中的偏移角度做差得到地球仪在手机端的姿态角度；

步骤8：手机客户端接收地球仪的姿态角度、深度距离和地球仪的球心在摄像头的位置信息，通过相机中心点、球心在摄像头的位置以及深度距离建立两点式得出虚拟地球仪在相机坐标系的三维坐标，通过接收到的地球仪的角度信息得出移动设备的摄像头注视点经纬度及地球仪在手机虚拟空间的相对位置与姿态角度。

2.根据权利要求1所述的地球仪的位姿检测方法，其特征在于，所述步骤1中根据初始角度计算得到初始四元数的公式为：

$C_B^N=\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\θ}\\ -\cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}\sin \mathrm{\γ}\sin \mathrm{\θ}+\cos \mathrm{\γ}\cos \mathrm{\ψ}& \sin \mathrm{\γ}\cos \mathrm{\θ}\\ \cos \mathrm{\ψ}\cos \mathrm{\γ}\sin \mathrm{\θ}+\sin \mathrm{\γ}\sin \mathrm{\ψ}& \sin \mathrm{\ψ}\cos \mathrm{\γ}\sin \mathrm{\θ}-\sin \mathrm{\γ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]$

$\left\{\begin{array}{c}\left|q_1\right|=0.5\×\sqrt{1+T11-T22-T33}\\ \left|q_2\right|=0.5\×\sqrt{1-T11+T22-T33}\\ \left|q_3\right|=0.5\×\sqrt{1-T11-T22+T33}\\ \left|q_0\right|=0.5\×\sqrt{1+T11+T22+T33}\end{array}\right.$

其中，q₀、q₁、q₂、q₃代表四元数；

ψ₀表示方位角，θ₀表示俯仰角，γ₀表示翻滚角。

3.根据权利要求1所述的地球仪的位姿检测方法，其特征在于，所述步骤2的具体步骤为：所述刻度标定系数是通过延标尺移动手机将摄像头中呈现的图像和手机中呈现的图像重合，并输入重合后手机在标尺上的刻度来初始化。

4.根据权利要求1所述的地球仪的位姿检测方法，其特征在于，所述步骤3中所述更新四元数的具体方法为：

1)在时间中断时获取陀螺仪传感器的数值；

2)用获取的数值进行更新的四元数；

3)将更新后的四元数进行优化进而再一次更新四元数。

5.根据权利要求1所述的地球仪的位姿检测方法，其特征在于，所述步骤6中球的偏移角度具体的求解方法为：

1)将球心在摄像头中的位置通过相似变换得到实际摄像头的偏移量；

2)根据地球仪在摄像头中的半径和地球仪的实际半径建立相似比例标定得到摄像头到球体的距离，即深度距离；

3)根据实际摄像头的偏移量和深度距离求得偏移角度。

6.根据权利要求1所述的地球仪的位姿检测方法，其特征在于，所述位姿检测方法还包括如下步骤：

客户端程序3D渲染模块接受到步骤5中得到的地球仪的姿态数据和切平面方向后，再根据后台数据库与用户选择的结果将指定的3D模型通过手机注视点坐标及切平面方向放置在对应的位置上进行渲染与实时视频背景拟合，根据步骤7中的球体在手机端的姿态，将虚拟地球仪旋转至该角度与真实地球仪吻合，显示AR图像。

7.根据权利要求6所述的地球仪的位姿检测方法，其特征在于，所述切平面方向的确定方法为：将步骤5中得到的球心坐标结合球面方程确定切平面方向。

8.根据权利要求4所述的地球仪的位姿检测方法，其特征在于，所述的优化四元数的方法具体是：利用卡尔曼滤波算法进行滤波，包括卡尔曼滤波算法需要状态方程以及观测方程进行滤波。

9.用于地球仪位姿检测的装置，其特征在于：包括地球仪与带摄像头的移动设备，地球仪由陀螺仪传感器和控制芯片组成，陀螺仪传感器用于记录地球仪的姿态数据，通过移动设备上的蓝牙进行搜索配对将地球仪的姿态数据发到移动设备。

10.根据权利要求9所述的用于地球仪位姿检测的装置，其特征在于：所述的移动设备还包括3D渲染模块，用于接受到地球仪的姿态数据及切平面方向后，根据后台数据库与用户选择的结果将指定的3D模型通过手机注视点坐标及切平面方向放置在对应的位置上进行渲染与实时视频背景拟合；所述移动设备为带摄像头的手机或PAD；所述地球仪位姿检测的装置还包括加速度传感器；所述地球仪位姿检测的装置还包括地磁传感器。