一种基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法
【专利摘要】本发明所要解决的技术问题是提供一种基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法，包括以下步骤：标定摄像头的内参数、初始化位姿估计、从摄像头中读取当前帧图像同时从重力感应器中读取信号、由单目视觉运动估计算法得到当前帧图像与前一帧图像之间摄像头的运动参数、用在中的姿态即倾斜角和和滚动角构成卡尔曼滤波器的状态矢量、从滤波估计得到的中解算出当前帧图像时刻在中的姿态、然后得到的位姿。此融合位姿估计方法简单，可以在资源有限的移动设备上应用。此方法仅依赖智能移动设备普遍配置的单个摄像头与重力感应器，可以基于此位姿估计方法开发出多种新型的移动设备应用，如增强现实、局部地图导航、3D游戏等。
【专利说明】一种基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法
【技术领域】
[0001]本发明涉及智能移动设备的位姿估计方法，尤其涉及通过融合摄像头视觉位姿估计技术与惯导位姿估计技术实现智能移动设备位姿估计的方法。
【背景技术】
[0002]摄像头与重力感应器融合的灵感源于摄像头惯导融合位姿估计。摄像头惯导融合位姿估计是一个很前沿的科研方向，但是它的方法一般比较复杂，而且基本都是为车载平台或移动机器人设计的，所使用的惯导设备的功能和价格也远非重力感应器能比。
[0003]摄像头与重力感应器融合位姿估计方法是专为智能移动设备设计的，这里的智能移动设备主要指智能手机。此融合位姿估计方法简单，可以在资源有限的移动设备上应用。此方法仅依赖智能移动设备普遍配置的单个摄像头与重力感应器，可以基于此位姿估计方法开发出多种新型的移动设备应用，如增强现实、局部地图导航、3D游戏等。
【发明内容】
[0004]本发明所要解决的技术问题是提供一种基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法，主要解决智能移动设备的位姿估计问题。为此，本发明采用以下技术方案:
一种基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法，其特征在于，所述方法基于智能移动设备的摄像头与重力感应器，所述方法包括以下步骤:
步骤一、标定摄像头的内参数；标定重力感应器坐标系丨相对于摄像头坐标系(C]的相对旋转矩阵<。
[0005]{/丨是固联于重力感应器上并随重力感应器一起运动的三维坐标系；{C丨是固联于摄像头上并随摄像头一起运动的二维坐标系；重力感应器和摄像头都固定在智能移动设备上之后，(/)与(C)之间的相对旋转和相对平移是固定的，它们之间的相对旋转用相对旋转
矩阵< 描述。
[0006]步骤二、初始化位姿估计，包括:确定全局坐标系(G};从初始时刻智能移动设备的摄像头中读取图像并读取重力感应器信息；得到〖C]在(6)中的初始位姿。[0007]步骤三、从摄像头中读取当前帧图像，同时从重力感应器中读取信号；由单目视觉运动估计算法得到当前帧图像与前一帧图像之间摄像头的运动参数，运动参数包括前一帧
图像时刻的〖C；}相对当前帧图像时刻{C丨的相对平移t和相对旋转Afl，然后结合前一帧图像时刻【Cl在〖G)中的位姿和当前计算的运动参数得到当前帧图像时刻(C}在丨中的位姿；从获取的重力感应器信号得到重力加速度在{/]中的矢量表示Λ ,再用&得到重力加速度在CC}中的矢量表示= Ri..§r ο
[0008]步骤四、用(Cl在(6)中的姿态即倾斜角?和滚动角'3^+^:构成卡尔曼滤波器
【权利要求】
1.一种基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法，其特征在于，所述方法基于智能移动设备的摄像头与重力感应器，包括如下步骤: 步骤一、标定摄像头的内参数；标定重力感应器坐标系相对于摄像头坐标系(C]的相对旋转矩阵<; 【/}是固联于重力感应器上并随重力感应器一起运动的三维坐标系；[C]是固联于摄像头上并随摄像头一起运动的二维坐标系；重力感应器和摄像头都固定在智能移动设备上之后,{/}与{c}之间的相对旋转和相对平移是固定的，CO与{c}之间的相对旋转用相对旋转矩Wr1c描述； 步骤二、初始化位姿估计，包括:确定全局坐标系iG];从初始时刻智能移动设备的摄像头中读取图像并读取重力感应器信息；得到{C}在(G)中的初始位姿； 步骤三、从摄像头中读取当前帧图像，同时从重力感应器中读取信号；由单目视觉运动估计算法得到当前帧图像与前一帧图像之间摄像头的运动参数，运动参数包括前一帧图像时刻的O:丨相对当前帧图像时刻{?]的相对平移《和相对旋转M ,然后结合前一帧图像时刻{C]在(G)中的位姿和当前计算的运动参数得到当前帧图像时刻(C】在{G]中的位姿；从获取的重力感应器信号得到重力加速度在(I丨中的矢量表示&，再用得到重力加速度在(Cl中的矢量表示I,其中，βe =祀，氣.； 步骤四、用【C}在{G}中的姿态即倾斜角和和滚动角构成卡尔曼滤波器的状态矢量
2.根据权利要求1所述的基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法，其特征在于，在所述步骤二中，采用包括以下步骤: (I )、确定全局坐标系(G丨，(G丨的坐标原点取在初始化时摄像头坐标系{C丨的原点处，Ce}的z轴正方向取为与重力方向相反,佴丨的r轴取为初始化时0丨的r轴在水平面上的投影，剩下怀丨的y轴由右手法则确定确定后就一直固定不变，智能移动设备的六自由度位姿估计都是估计摄像头坐标系{C】在(G]坐标系下的位姿，位姿包括位置姿态即倾斜角pitch和滚动角roll,以及方位即偏摆角和妒，其中yaw、fdtch'roll是姿态方位的欧拉角表示法； (2)、从初始时刻智能移动设备的摄像头中读取图像并读取重力感应器信息；
(3)、得到O:)在(G丨中的初始位姿:由(G}的取法得到忙}在中的初始位置为
3.根据权利要求1所述的基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法，其特征在于，在所述步骤四中，采用包括以下步骤:(I )、设{C】在(G)中的姿态方位对应的旋转矩阵用表示，第帧图像时刻的{C】相对于第左帧图像时刻(Cj的相对旋转用表示，则有
4.根据权利要求1所述的基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法，其特征在于，在所述步骤五中，采用包括以下步骤: (I)、结合步骤四中的卡尔曼滤波器预测和观测，并对卡尔曼滤波器的过程噪声和观测噪声进行估计，然后根据经典卡尔曼滤波算法进行卡尔曼滤波估计；
【文档编号】G01B11/26GK103900473SQ201410124679
【公开日】2014年7月2日 申请日期:2014年3月31日 优先权日:2014年3月31日
【发明者】黄鸿, 刘勇 申请人:浙江大学