[go: up one dir, main page]

CN103900473A - 一种基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法 - Google Patents

一种基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法 Download PDF

Info

Publication number
CN103900473A
CN103900473A CN201410124679.5A CN201410124679A CN103900473A CN 103900473 A CN103900473 A CN 103900473A CN 201410124679 A CN201410124679 A CN 201410124679A CN 103900473 A CN103900473 A CN 103900473A
Authority
CN
China
Prior art keywords
camera
pose
gravity sensor
kalman filter
frame image
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN201410124679.5A
Other languages
English (en)
Inventor
黄鸿
刘勇
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang University ZJU
Original Assignee
Zhejiang University ZJU
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Zhejiang University ZJU filed Critical Zhejiang University ZJU
Priority to CN201410124679.5A priority Critical patent/CN103900473A/zh
Publication of CN103900473A publication Critical patent/CN103900473A/zh
Pending legal-status Critical Current

Links

Landscapes

  • Image Analysis (AREA)
  • Navigation (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明所要解决的技术问题是提供一种基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法，包括以下步骤：标定摄像头的内参数、初始化位姿估计、从摄像头中读取当前帧图像同时从重力感应器中读取信号、由单目视觉运动估计算法得到当前帧图像与前一帧图像之间摄像头的运动参数、用在中的姿态即倾斜角和和滚动角构成卡尔曼滤波器的状态矢量、从滤波估计得到的中解算出当前帧图像时刻在

Description

一种基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法
技术领域
本发明涉及智能移动设备的位姿估计方法，尤其涉及通过融合摄像头视觉位姿估计技术与惯导位姿估计技术实现智能移动设备位姿估计的方法。
背景技术
摄像头与重力感应器融合的灵感源于摄像头惯导融合位姿估计。摄像头惯导融合位姿估计是一个很前沿的科研方向，但是它的方法一般比较复杂，而且基本都是为车载平台或移动机器人设计的，所使用的惯导设备的功能和价格也远非重力感应器能比。
摄像头与重力感应器融合位姿估计方法是专为智能移动设备设计的，这里的智能移动设备主要指智能手机。此融合位姿估计方法简单，可以在资源有限的移动设备上应用。此方法仅依赖智能移动设备普遍配置的单个摄像头与重力感应器，可以基于此位姿估计方法开发出多种新型的移动设备应用，如增强现实、局部地图导航、3D游戏等。
发明内容
本发明所要解决的技术问题是提供一种基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法，主要解决智能移动设备的位姿估计问题。为此，本发明采用以下技术方案：
一种基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法，其特征在于，所述方法基于智能移动设备的摄像头与重力感应器，所述方法包括以下步骤：
步骤一、标定摄像头的内参数；标定重力感应器坐标系                                                
Figure 259559DEST_PATH_IMAGE001
相对于摄像头坐标系
Figure 428502DEST_PATH_IMAGE002
的相对旋转矩阵
Figure 909162DEST_PATH_IMAGE003
。
Figure 529499DEST_PATH_IMAGE001
是固联于重力感应器上并随重力感应器一起运动的三维坐标系；
Figure 121017DEST_PATH_IMAGE002
是固联于摄像头上并随摄像头一起运动的三维坐标系；重力感应器和摄像头都固定在智能移动设备上之后，
Figure 815304DEST_PATH_IMAGE001
与
Figure 545494DEST_PATH_IMAGE002
之间的相对旋转和相对平移是固定的，它们之间的相对旋转用相对旋转矩阵
Figure 325231DEST_PATH_IMAGE003
描述。
步骤二、初始化位姿估计，包括：确定全局坐标系
Figure 782757DEST_PATH_IMAGE004
；从初始时刻智能移动设备的摄像头中读取图像并读取重力感应器信息；得到
Figure 597129DEST_PATH_IMAGE002
在
Figure 419592DEST_PATH_IMAGE004
中的初始位姿。
步骤三、从摄像头中读取当前帧图像，同时从重力感应器中读取信号；由单目视觉运动估计算法得到当前帧图像与前一帧图像之间摄像头的运动参数，运动参数包括前一帧图像时刻的
Figure 998210DEST_PATH_IMAGE002
相对当前帧图像时刻
Figure 197110DEST_PATH_IMAGE002
的相对平移
Figure 928305DEST_PATH_IMAGE005
和相对旋转
Figure 921669DEST_PATH_IMAGE006
，然后结合前一帧图像时刻
Figure 489048DEST_PATH_IMAGE002
在
Figure 226060DEST_PATH_IMAGE004
中的位姿和当前计算的运动参数得到当前帧图像时刻在中的位姿；从获取的重力感应器信号得到重力加速度在
Figure 483232DEST_PATH_IMAGE001
中的矢量表示
Figure 2014101246795100002DEST_PATH_IMAGE007
，再用
Figure 69940DEST_PATH_IMAGE003
得到重力加速度在
Figure 713411DEST_PATH_IMAGE002
中的矢量表示
Figure 110894DEST_PATH_IMAGE008
。
步骤四、用
Figure 839815DEST_PATH_IMAGE002
在
Figure 918630DEST_PATH_IMAGE004
中的姿态即倾斜角
Figure 2014101246795100002DEST_PATH_IMAGE009
和滚动角
Figure 229657DEST_PATH_IMAGE010
构成卡尔曼滤波器的状态矢量
Figure 2014101246795100002DEST_PATH_IMAGE011
，得到关于
Figure 63620DEST_PATH_IMAGE012
的卡尔曼滤波器状态方程和观测方程，将步骤三得到的
Figure 14259DEST_PATH_IMAGE006
用于卡尔曼滤波器状态预测，将步骤三得到的
Figure 2014101246795100002DEST_PATH_IMAGE013
用于卡尔曼滤波器观测。
步骤五、结合步骤四中的卡尔曼滤波器预测和观测进行卡尔曼滤波估计，然后从滤波估计得到的
Figure 208349DEST_PATH_IMAGE012
中解算出当前帧图像时刻
Figure 623150DEST_PATH_IMAGE002
在
Figure 565698DEST_PATH_IMAGE004
中的姿态即倾斜角
Figure 3632DEST_PATH_IMAGE009
和滚动角
Figure 237299DEST_PATH_IMAGE010
。
步骤六、用步骤五解算的姿态替换步骤三解算的当前帧图像时刻
Figure 709868DEST_PATH_IMAGE002
在
Figure 885635DEST_PATH_IMAGE004
中的位姿信息中的姿态，然后得到的位姿即是融合后的六自由度当前帧图像时刻在
Figure 352617DEST_PATH_IMAGE004
中的位姿，即得到融合了摄像头与重力感应器的智能移动设备的位姿估计。循环的执行步骤三到步骤六即持续的估计智能移动设备的位姿。
该方法结合了单目视觉运动估计和重力感应器两方面的信息来做融合姿态估计，获得的姿态具有精度高且鲁棒的特点，因为位姿的六个自由度即x、y、z、yaw、pitch、roll是相互耦合的，因此pitch、roll精度和稳定性的保证也使得整个位姿估计具有更好的性能。
进一步地，在所述步骤一中，采用包括以下步骤：
(1)、用MATLAB标定工具箱和标定方格板标定摄像头的内参数；
(2)、用Jeroen Hol2008（Pose Estimation and Calibration Algorithms for Vision and Inertial Sensors）中的方法标定重力感应器坐标系
Figure 414114DEST_PATH_IMAGE001
相对于摄像头坐标系
Figure 698465DEST_PATH_IMAGE002
的相对旋转矩阵
Figure 173309DEST_PATH_IMAGE003
，大致流程是：
将标定方格板水平的放在地上，然后摄像头对着标定方格板拍摄图像且同时读取重力感应器信号，变换智能移动设备的位置获取多组数据；
用开源计算机视觉库OpenCV中的自定位算法函数计算每张图像时摄像机坐标系
Figure 201308DEST_PATH_IMAGE002
相对于标定方格板的位姿，然后得到重力加速度在
Figure 382890DEST_PATH_IMAGE002
中的矢量表示，再从同一时刻获取的重力感应器信号中得到重力加速度在中的矢量表示；
最后基于最小二乘法由所有数据计算得到
Figure 285435DEST_PATH_IMAGE003
。
进一步地，在所述步骤二中，采用包括以下步骤：
（1）、确定全局坐标系
Figure 913863DEST_PATH_IMAGE004
，
Figure 215531DEST_PATH_IMAGE004
的坐标原点取在初始化时摄像头坐标系
Figure 576105DEST_PATH_IMAGE002
的原点处，的
Figure 644610DEST_PATH_IMAGE014
轴正方向取为与重力方向相反，
Figure 597523DEST_PATH_IMAGE004
的
Figure 2014101246795100002DEST_PATH_IMAGE015
轴取为初始化时
Figure 394577DEST_PATH_IMAGE001
的
Figure 816463DEST_PATH_IMAGE015
轴在水平面上的投影，剩下
Figure 989955DEST_PATH_IMAGE004
的
Figure 62953DEST_PATH_IMAGE016
轴由右手法则确定。
Figure 30909DEST_PATH_IMAGE004
确定后就一直固定不变，智能移动设备的六自由度位姿估计都是估计摄像头坐标系
Figure 438626DEST_PATH_IMAGE002
在
Figure 150230DEST_PATH_IMAGE004
坐标系下的位姿。位姿包括位置x、y、z，姿态即倾斜角pitch和滚动角roll，以及方位即偏摆角yaw,其中yaw、pitch、roll是姿态方位的欧拉角表示法。
（2）、从初始时刻智能移动设备的摄像头中读取图像并读取重力感应器信息。
（3）、得到在
Figure 216592DEST_PATH_IMAGE004
中的初始位姿：
因为
Figure 800020DEST_PATH_IMAGE004
的坐标原点取在初始化时摄像头坐标系
Figure 862785DEST_PATH_IMAGE002
的原点处，所以
Figure 848058DEST_PATH_IMAGE002
在
Figure 157817DEST_PATH_IMAGE004
中的初始位置为
Figure 2014101246795100002DEST_PATH_IMAGE017
；
因为
Figure 290858DEST_PATH_IMAGE004
的
Figure 655849DEST_PATH_IMAGE015
轴取为初始化时
Figure 495629DEST_PATH_IMAGE001
的
Figure 976289DEST_PATH_IMAGE015
轴在水平面上的投影，所以相对于
Figure 188145DEST_PATH_IMAGE004
的初始方位偏摆角为零；因重力感应器测得的是重力加速度在
Figure 695481DEST_PATH_IMAGE001
中的矢量表示，若
Figure 392358DEST_PATH_IMAGE001
相对于
Figure 849884DEST_PATH_IMAGE004
的姿态方位对应的旋转矩阵为
Figure 398677DEST_PATH_IMAGE018
，则
Figure 2014101246795100002DEST_PATH_IMAGE019
，用
Figure 792444DEST_PATH_IMAGE020
表示的第三列，则
Figure 2014101246795100002DEST_PATH_IMAGE021
，仅与
Figure 723994DEST_PATH_IMAGE001
相对于
Figure 795986DEST_PATH_IMAGE004
的倾斜角
Figure 550316DEST_PATH_IMAGE022
和滚动角
Figure 2014101246795100002DEST_PATH_IMAGE023
有关，
Figure 349644DEST_PATH_IMAGE024
，因此
Figure 2014101246795100002DEST_PATH_IMAGE025
，从这个公式由反三角函数算法可以解算出
Figure 184614DEST_PATH_IMAGE022
和
Figure 614458DEST_PATH_IMAGE023
，结合值为零的偏摆角一起换算成旋转矩阵
Figure 652822DEST_PATH_IMAGE018
，然后结合
Figure 193524DEST_PATH_IMAGE003
得到
Figure 836995DEST_PATH_IMAGE002
相对于
Figure 985211DEST_PATH_IMAGE004
的姿态方位对应的旋转矩阵，将
Figure 855264DEST_PATH_IMAGE026
换算成欧拉角即得到
Figure 353241DEST_PATH_IMAGE002
在
Figure 859309DEST_PATH_IMAGE004
中的初始姿态方位。
进一步地，在所述步骤三中，采用包括以下步骤：
（1）、从摄像头中读取当前帧图像，同时从重力感应器中读取信号；
（2）、用Davide Scaramuzza和Friedrich Fraundorfer 2011年的文章Visual Odometry所述单目视觉运动估计算法得到当前帧图像与前一帧图像之间摄像头的运动参数，运动参数包括前一帧图像时刻的相对当前帧图像时刻
Figure 269617DEST_PATH_IMAGE002
的相对平移
Figure 684417DEST_PATH_IMAGE005
和相对旋转
Figure 626966DEST_PATH_IMAGE006
，并用此文章中的方法结合前一帧图像时刻
Figure 64900DEST_PATH_IMAGE002
在
Figure 298567DEST_PATH_IMAGE004
中的位姿和当前计算的运动参数得到当前帧图像时刻
Figure 771136DEST_PATH_IMAGE002
在
Figure 619007DEST_PATH_IMAGE004
中的位姿；
（3）、从获取的重力感应器信号得到重力加速度在
Figure 606554DEST_PATH_IMAGE001
中的矢量表示
Figure 830862DEST_PATH_IMAGE007
，再用
Figure 469523DEST_PATH_IMAGE003
得到重力加速度在
Figure 753873DEST_PATH_IMAGE002
中的矢量表示。
进一步地，在所述步骤四中，建立了解决姿态信息融合的卡尔曼滤波系统模型，卡尔曼滤波是由卡尔曼提出的著名算法，步骤四采用包括以下步骤：
（1）、设
Figure 991137DEST_PATH_IMAGE002
在
Figure 172719DEST_PATH_IMAGE004
中的姿态方位对应的旋转矩阵用
Figure 2014101246795100002DEST_PATH_IMAGE027
表示，第k-1帧图像时刻的相对于第k帧图像时刻
Figure 340844DEST_PATH_IMAGE002
的相对旋转用
Figure 286715DEST_PATH_IMAGE028
表示，则有，
Figure 385121DEST_PATH_IMAGE027
是
Figure 11274DEST_PATH_IMAGE030
的矩阵，用
Figure 2014101246795100002DEST_PATH_IMAGE031
表示
Figure 211443DEST_PATH_IMAGE027
的第三列，得到，
Figure 534157DEST_PATH_IMAGE031
只与
Figure 331211DEST_PATH_IMAGE002
在
Figure 251632DEST_PATH_IMAGE004
中的姿态即
Figure 2014101246795100002DEST_PATH_IMAGE033
和
Figure 425124DEST_PATH_IMAGE034
有关，
Figure 2014101246795100002DEST_PATH_IMAGE035
。
（2）、将
Figure 498122DEST_PATH_IMAGE036
取为卡尔曼滤波器的状态矢量，将
Figure 2014101246795100002DEST_PATH_IMAGE037
做为卡尔曼滤波器的状态方程，并用步骤三中得到的运动参数
Figure 279128DEST_PATH_IMAGE006
做卡尔曼滤波预测。
（3）、设重力加速度在
Figure 437577DEST_PATH_IMAGE004
中的矢量表示为
Figure 883601DEST_PATH_IMAGE038
，
Figure 2014101246795100002DEST_PATH_IMAGE039
为重力加速度的大小，则重力加速度在
Figure 325953DEST_PATH_IMAGE002
中的矢量表示
Figure 464810DEST_PATH_IMAGE040
，将
Figure 48238DEST_PATH_IMAGE013
取为观测矢量，将做为卡尔曼滤波器的观测方程，并将步骤三得到的重力加速度在
Figure 360271DEST_PATH_IMAGE002
中的矢量表示
Figure 158594DEST_PATH_IMAGE013
用于卡尔曼滤波器观测。
进一步地，在所述步骤五中，采用包括以下步骤：
（1）、结合步骤四中的卡尔曼滤波器预测和观测，并对卡尔曼滤波器的过程噪声和观测噪声进行估计，然后根据经典卡尔曼滤波算法进行卡尔曼滤波估计，求解得到融合了摄像机和重力感应器信息的
Figure 468352DEST_PATH_IMAGE012
。
（2）、将滤波估计得到的
Figure 539077DEST_PATH_IMAGE012
归一化，再据
Figure 654800DEST_PATH_IMAGE042
用反三角函数算法从
Figure 494580DEST_PATH_IMAGE012
中解算出
Figure 286825DEST_PATH_IMAGE033
和，即得到卡尔曼滤波估计后
Figure 436363DEST_PATH_IMAGE002
在
Figure 192967DEST_PATH_IMAGE004
中的姿态。
进一步地，在所述步骤六中，用步骤五解算的姿态替换步骤三解算的当前帧图像时刻
Figure 844528DEST_PATH_IMAGE002
在中的位姿信息中的姿态，然后得到的位姿即是融合后的六自由度当前帧图像时刻
Figure 98103DEST_PATH_IMAGE002
在
Figure 646896DEST_PATH_IMAGE004
中的位姿，即得到融合了摄像头与重力感应器的智能移动设备的位姿估计。循环的执行步骤三到步骤六即持续的估计智能移动设备的位姿。
具体实施方式
本发明所述的方法基于智能移动设备的摄像头与重力感应器，所述发明方法包括以下步骤：
1、用MATLAB标定工具箱和标定方格板标定摄像头的内参数；
2、用Jeroen Hol2008（Pose Estimation and Calibration Algorithms for Vision and Inertial Sensors）中的方法标定重力感应器坐标系相对于摄像头坐标系的相对旋转矩阵
Figure 326771DEST_PATH_IMAGE003
，大致流程是：
将标定方格板水平的放在地上，然后摄像头对着标定方格板拍摄图像且同时读取重力感应器信号，变换智能移动设备的位置获取多组数据；
用开源计算机视觉库OpenCV中的自定位算法函数计算每张图像时摄像机坐标系
Figure 57967DEST_PATH_IMAGE002
相对于标定方格板的位姿，然后得到重力加速度在
Figure 316910DEST_PATH_IMAGE002
中的矢量表示，再从同一时刻获取的重力感应器信号中得到重力加速度在
Figure 618709DEST_PATH_IMAGE001
中的矢量表示；
最后基于最小二乘法由所有数据计算得到
Figure 621300DEST_PATH_IMAGE003
。
3、确定全局坐标系
Figure 144686DEST_PATH_IMAGE004
，
Figure 371268DEST_PATH_IMAGE004
的坐标原点取在初始化时摄像头坐标系
Figure 612893DEST_PATH_IMAGE002
的原点处，
Figure 465180DEST_PATH_IMAGE004
的
Figure 843072DEST_PATH_IMAGE014
轴正方向取为与重力方向相反，
Figure 443818DEST_PATH_IMAGE004
的
Figure 969477DEST_PATH_IMAGE015
轴取为初始化时
Figure 313871DEST_PATH_IMAGE001
的
Figure 624897DEST_PATH_IMAGE015
轴在水平面上的投影，剩下的
Figure 347183DEST_PATH_IMAGE016
轴由右手法则确定。
4、从初始时刻智能移动设备的摄像头中读取图像并读取重力感应器信息。
5、得到
Figure 292005DEST_PATH_IMAGE002
在
Figure 644489DEST_PATH_IMAGE004
中的初始位姿：
因为
Figure 321458DEST_PATH_IMAGE004
的坐标原点取在初始化时摄像头坐标系
Figure 70977DEST_PATH_IMAGE002
的原点处，所以
Figure 757173DEST_PATH_IMAGE002
在
Figure 26481DEST_PATH_IMAGE004
中的初始位置为
Figure 139930DEST_PATH_IMAGE017
；
因为
Figure 65161DEST_PATH_IMAGE004
的轴取为初始化时的
Figure 776262DEST_PATH_IMAGE015
轴在水平面上的投影，所以
Figure 923209DEST_PATH_IMAGE001
相对于
Figure 951208DEST_PATH_IMAGE004
的初始方位偏摆角为零；因重力感应器测得的是重力加速度在
Figure 444375DEST_PATH_IMAGE001
中的矢量表示
Figure 899628DEST_PATH_IMAGE007
，若
Figure 861767DEST_PATH_IMAGE001
相对于
Figure 427878DEST_PATH_IMAGE004
的姿态方位对应的旋转矩阵为
Figure 463967DEST_PATH_IMAGE018
，则
Figure 903170DEST_PATH_IMAGE019
，用
Figure 24710DEST_PATH_IMAGE020
表示
Figure 394511DEST_PATH_IMAGE018
的第三列，则
Figure 613003DEST_PATH_IMAGE021
，
Figure 410057DEST_PATH_IMAGE020
仅与
Figure 336337DEST_PATH_IMAGE001
相对于
Figure 244250DEST_PATH_IMAGE004
的倾斜角
Figure 317249DEST_PATH_IMAGE022
和滚动角
Figure 285205DEST_PATH_IMAGE023
有关，
Figure 381337DEST_PATH_IMAGE024
，因此
Figure 905990DEST_PATH_IMAGE025
，从这个公式由反三角函数算法可以解算出
Figure 36757DEST_PATH_IMAGE022
和
Figure 175614DEST_PATH_IMAGE023
，结合值为零的偏摆角一起换算成旋转矩阵
Figure 555780DEST_PATH_IMAGE018
，然后结合得到相对于的姿态方位对应的旋转矩阵
Figure 482837DEST_PATH_IMAGE026
，将
Figure 598560DEST_PATH_IMAGE026
换算成欧拉角即得到
Figure 438340DEST_PATH_IMAGE002
在
Figure 732049DEST_PATH_IMAGE004
中的初始姿态方位。
6、从摄像头中读取当前帧图像，同时从重力感应器中读取信号；
7、用Davide Scaramuzza和Friedrich Fraundorfer 2011年的文章Visual Odometry所述单目视觉运动估计算法得到当前帧图像与前一帧图像之间摄像头的运动参数，运动参数包括前一帧图像时刻的相对当前帧图像时刻的相对平移
Figure 903771DEST_PATH_IMAGE005
和相对旋转
Figure 555332DEST_PATH_IMAGE006
，并用此文章中的方法结合前一帧图像时刻
Figure 646653DEST_PATH_IMAGE002
在
Figure 41863DEST_PATH_IMAGE004
中的位姿和当前计算的运动参数得到当前帧图像时刻
Figure 856235DEST_PATH_IMAGE002
在
Figure 741014DEST_PATH_IMAGE004
中的位姿；
8、从获取的重力感应器信号得到重力加速度在
Figure 8048DEST_PATH_IMAGE001
中的矢量表示
Figure 19997DEST_PATH_IMAGE007
，再用得到重力加速度在
Figure 682240DEST_PATH_IMAGE002
中的矢量表示
Figure 498886DEST_PATH_IMAGE008
。
9、建立姿态信息融合的卡尔曼滤波模型：
（1）、设
Figure 501477DEST_PATH_IMAGE002
在
Figure 24862DEST_PATH_IMAGE004
中的姿态方位对应的旋转矩阵用表示，第k-1帧图像时刻的
Figure 742337DEST_PATH_IMAGE002
相对于第k帧图像时刻
Figure 283040DEST_PATH_IMAGE002
的相对旋转用
Figure 723249DEST_PATH_IMAGE028
表示，则有
Figure 323994DEST_PATH_IMAGE029
，
Figure 600386DEST_PATH_IMAGE027
是
Figure 944780DEST_PATH_IMAGE030
的矩阵，用
Figure 177178DEST_PATH_IMAGE031
表示
Figure 11142DEST_PATH_IMAGE027
的第三列，得到
Figure 961780DEST_PATH_IMAGE032
，
Figure 150011DEST_PATH_IMAGE031
只与
Figure 502495DEST_PATH_IMAGE002
在
Figure 445043DEST_PATH_IMAGE004
中的姿态即
Figure 945294DEST_PATH_IMAGE033
和
Figure 631490DEST_PATH_IMAGE034
有关，。
（2）、将
Figure 764980DEST_PATH_IMAGE036
取为卡尔曼滤波器的状态矢量，将
Figure 690210DEST_PATH_IMAGE037
做为卡尔曼滤波器的状态方程，并用第7步中得到的运动参数做卡尔曼滤波预测。
（3）、设重力加速度在中的矢量表示为
Figure 634267DEST_PATH_IMAGE038
，
Figure 46794DEST_PATH_IMAGE039
为重力加速度的大小，则重力加速度在
Figure 137110DEST_PATH_IMAGE002
中的矢量表示
Figure 318693DEST_PATH_IMAGE040
，将
Figure 586994DEST_PATH_IMAGE013
取为观测矢量，将
Figure 221238DEST_PATH_IMAGE041
做为卡尔曼滤波器的观测方程，并将第8步得到的重力加速度在
Figure 787348DEST_PATH_IMAGE002
中的矢量表示
Figure 151334DEST_PATH_IMAGE013
用于卡尔曼滤波器观测。
10、结合第9步中的卡尔曼滤波器预测和观测，并对卡尔曼滤波器的过程噪声和观测噪声进行估计，然后根据经典卡尔曼滤波算法进行卡尔曼滤波估计，求解得到融合了摄像机和重力感应器信息的
Figure 777487DEST_PATH_IMAGE012
。
11、将滤波估计得到的
Figure 210611DEST_PATH_IMAGE012
归一化，再据
Figure 580413DEST_PATH_IMAGE042
用反三角函数算法从中解算出
Figure 595959DEST_PATH_IMAGE033
和
Figure 204795DEST_PATH_IMAGE034
，即得到卡尔曼滤波估计后在
Figure 936439DEST_PATH_IMAGE004
中的姿态。
12、用第11步解算的姿态替换第7步解算的当前帧图像时刻
Figure 904395DEST_PATH_IMAGE002
在
Figure 62844DEST_PATH_IMAGE004
中的位姿信息中的姿态，然后得到的位姿即是融合后的六自由度当前帧图像时刻
Figure 774448DEST_PATH_IMAGE002
在
Figure 951220DEST_PATH_IMAGE004
中的位姿，即得到融合了摄像头与重力感应器的智能移动设备的位姿估计。循环的执行第6步到第11步即持续的估计智能移动设备的位姿。

Claims (4)

1.一种基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法，其特征在于，所述方法基于智能移动设备的摄像头与重力感应器，包括如下步骤：
步骤一、标定摄像头的内参数；标定重力感应器坐标系                                                相对于摄像头坐标系的相对旋转矩阵
Figure 65072DEST_PATH_IMAGE003
；
Figure 650774DEST_PATH_IMAGE001
是固联于重力感应器上并随重力感应器一起运动的三维坐标系；
Figure 815039DEST_PATH_IMAGE002
是固联于摄像头上并随摄像头一起运动的三维坐标系；重力感应器和摄像头都固定在智能移动设备上之后，
Figure 56665DEST_PATH_IMAGE001
与
Figure 677262DEST_PATH_IMAGE002
之间的相对旋转和相对平移是固定的，与
Figure 655900DEST_PATH_IMAGE002
之间的相对旋转用相对旋转矩阵
Figure 181559DEST_PATH_IMAGE003
描述；
步骤二、初始化位姿估计，包括：确定全局坐标系
Figure 525953DEST_PATH_IMAGE004
；从初始时刻智能移动设备的摄像头中读取图像并读取重力感应器信息；得到在
Figure 529998DEST_PATH_IMAGE004
中的初始位姿；
步骤三、从摄像头中读取当前帧图像，同时从重力感应器中读取信号；由单目视觉运动估计算法得到当前帧图像与前一帧图像之间摄像头的运动参数，运动参数包括前一帧图像时刻的
Figure 808532DEST_PATH_IMAGE002
相对当前帧图像时刻
Figure 691038DEST_PATH_IMAGE002
的相对平移
Figure 43522DEST_PATH_IMAGE005
和相对旋转
Figure 782807DEST_PATH_IMAGE006
，然后结合前一帧图像时刻
Figure 220742DEST_PATH_IMAGE002
在
Figure 906938DEST_PATH_IMAGE004
中的位姿和当前计算的运动参数得到当前帧图像时刻
Figure 176246DEST_PATH_IMAGE002
在
Figure 289695DEST_PATH_IMAGE004
中的位姿；从获取的重力感应器信号得到重力加速度在
Figure 214926DEST_PATH_IMAGE001
中的矢量表示
Figure 501551DEST_PATH_IMAGE007
，再用
Figure 828627DEST_PATH_IMAGE003
得到重力加速度在
Figure 112978DEST_PATH_IMAGE002
中的矢量表示，其中，
Figure 350241DEST_PATH_IMAGE009
；
步骤四、用
Figure 531824DEST_PATH_IMAGE002
在
Figure 49393DEST_PATH_IMAGE004
中的姿态即倾斜角
Figure 949215DEST_PATH_IMAGE010
和和滚动角
Figure 515326DEST_PATH_IMAGE011
构成卡尔曼滤波器的状态矢量
Figure 613732DEST_PATH_IMAGE012
，得到关于
Figure 239885DEST_PATH_IMAGE013
的卡尔曼滤波器状态方程和观测方程，将步骤三得到的
Figure 361425DEST_PATH_IMAGE006
用于卡尔曼滤波器状态预测，将步骤三得到的
Figure 793544DEST_PATH_IMAGE008
用于卡尔曼滤波器观测；
步骤五、结合步骤四中的卡尔曼滤波器预测和观测进行卡尔曼滤波估计，然后从滤波估计得到的
Figure 949718DEST_PATH_IMAGE013
中解算出当前帧图像时刻
Figure 746773DEST_PATH_IMAGE002
在
Figure 417926DEST_PATH_IMAGE004
中的姿态即倾斜角
Figure 325839DEST_PATH_IMAGE010
和和滚动角
Figure 336520DEST_PATH_IMAGE011
；
步骤六、用步骤五解算的姿态替换步骤三解算的当前帧图像时刻
Figure 366793DEST_PATH_IMAGE002
在
Figure 462925DEST_PATH_IMAGE004
中的位姿信息中的姿态，然后得到的位姿即是融合后的六自由度当前帧图像时刻
Figure 174529DEST_PATH_IMAGE002
在
Figure 367613DEST_PATH_IMAGE004
中的位姿，即得到融合了摄像头与重力感应器的智能移动设备的位姿估计；循环的执行步骤三到步骤六即持续的估计智能移动设备的位姿。
2.根据权利要求1所述的基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法，其特征在于，在所述步骤二中，采用包括以下步骤：
（1）、确定全局坐标系
Figure 506471DEST_PATH_IMAGE004
，的坐标原点取在初始化时摄像头坐标系
Figure 401931DEST_PATH_IMAGE002
的原点处，的
Figure 696963DEST_PATH_IMAGE014
轴正方向取为与重力方向相反，
Figure 564425DEST_PATH_IMAGE004
的
Figure 617832DEST_PATH_IMAGE015
轴取为初始化时的
Figure 589DEST_PATH_IMAGE015
轴在水平面上的投影，剩下的
Figure 150127DEST_PATH_IMAGE016
轴由右手法则确定；
Figure 178169DEST_PATH_IMAGE004
确定后就一直固定不变，智能移动设备的六自由度位姿估计都是估计摄像头坐标系在
Figure 609468DEST_PATH_IMAGE004
坐标系下的位姿，位姿包括位置x、y、z，姿态即倾斜角pitch和滚动角roll，以及方位即偏摆角yaw,其中yaw、pitch、roll是姿态方位的欧拉角表示法；
（2）、从初始时刻智能移动设备的摄像头中读取图像并读取重力感应器信息；
（3）、得到
Figure 66994DEST_PATH_IMAGE002
在
Figure 881366DEST_PATH_IMAGE004
中的初始位姿：由
Figure 703829DEST_PATH_IMAGE004
的取法得到
Figure 33179DEST_PATH_IMAGE002
在
Figure 232079DEST_PATH_IMAGE004
中的初始位置为
Figure 900958DEST_PATH_IMAGE017
；
Figure 956638DEST_PATH_IMAGE001
相对于
Figure 710968DEST_PATH_IMAGE004
的初始方位偏摆角为零，由重力感应器信息得到
Figure 713559DEST_PATH_IMAGE001
相对于的初始姿态，再用
Figure 463526DEST_PATH_IMAGE003
转换得到
Figure 705152DEST_PATH_IMAGE002
在中的初始姿态方位。
3.根据权利要求1所述的基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法，其特征在于，在所述步骤四中，采用包括以下步骤：
（1）、设在
Figure 286809DEST_PATH_IMAGE004
中的姿态方位对应的旋转矩阵用
Figure 750151DEST_PATH_IMAGE018
表示，第k-1帧图像时刻的
Figure 156862DEST_PATH_IMAGE002
相对于第k帧图像时刻
Figure 389260DEST_PATH_IMAGE002
的相对旋转用
Figure 160907DEST_PATH_IMAGE019
表示，则有，
Figure 56367DEST_PATH_IMAGE018
是
Figure 408851DEST_PATH_IMAGE021
的矩阵，用表示
Figure 851651DEST_PATH_IMAGE018
的第三列，得到
Figure 537847DEST_PATH_IMAGE023
，
Figure 807155DEST_PATH_IMAGE022
只与
Figure 920604DEST_PATH_IMAGE002
在中的姿态即
Figure 132460DEST_PATH_IMAGE010
和
Figure 459536DEST_PATH_IMAGE011
有关，
Figure 478307DEST_PATH_IMAGE024
；
（2）、将
Figure 953151DEST_PATH_IMAGE025
取为卡尔曼滤波器的状态矢量，将
Figure 981150DEST_PATH_IMAGE026
做为卡尔曼滤波器的状态方程，并用步骤三中得到的运动参数
Figure 162733DEST_PATH_IMAGE006
做卡尔曼滤波预测；
（3）、设重力加速度在
Figure 680302DEST_PATH_IMAGE004
中的矢量表示为，
Figure 880656DEST_PATH_IMAGE028
为重力加速度的大小，则重力加速度在中的矢量表示
Figure 870794DEST_PATH_IMAGE029
，将
Figure 992334DEST_PATH_IMAGE008
取为观测矢量，将
Figure 362136DEST_PATH_IMAGE030
做为卡尔曼滤波器的观测方程，并将步骤三得到的重力加速度在
Figure 580627DEST_PATH_IMAGE002
中的矢量表示
Figure 377682DEST_PATH_IMAGE008
用于卡尔曼滤波器观测。
4.根据权利要求1所述的基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法，其特征在于，在所述步骤五中，采用包括以下步骤：
（1）、结合步骤四中的卡尔曼滤波器预测和观测，并对卡尔曼滤波器的过程噪声和观测噪声进行估计，然后根据经典卡尔曼滤波算法进行卡尔曼滤波估计；
（2）、将滤波估计得到的
Figure 986518DEST_PATH_IMAGE013
归一化，再据
Figure 956748DEST_PATH_IMAGE031
用反三角函数算法从中解算出和
Figure 87975DEST_PATH_IMAGE011
，即得到卡尔曼滤波估计后
Figure 799579DEST_PATH_IMAGE002
在
Figure 727084DEST_PATH_IMAGE004
中的姿态。
CN201410124679.5A 2014-03-31 2014-03-31 一种基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法 Pending CN103900473A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201410124679.5A CN103900473A (zh) 2014-03-31 2014-03-31 一种基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201410124679.5A CN103900473A (zh) 2014-03-31 2014-03-31 一种基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法

Publications (1)

Publication Number Publication Date
CN103900473A true CN103900473A (zh) 2014-07-02

Family

ID=50991943

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201410124679.5A Pending CN103900473A (zh) 2014-03-31 2014-03-31 一种基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法

Country Status (1)

Country Link
CN (1) CN103900473A (zh)

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105094327A (zh) * 2015-07-22 2015-11-25 深圳多新哆技术有限责任公司 调整虚拟物件在虚拟空间中姿态角的方法及装置
CN108399641A (zh) * 2018-03-12 2018-08-14 北京华捷艾米科技有限公司 重新检测地面的判定方法及装置
CN109302602A (zh) * 2018-10-11 2019-02-01 广州土圭垚信息科技有限公司 一种基于视焦点预测的自适应vr无线传输方法
CN109598764A (zh) * 2018-11-30 2019-04-09 Oppo广东移动通信有限公司 摄像头标定方法和装置、电子设备、计算机可读存储介质
CN110036162A (zh) * 2016-09-30 2019-07-19 新加坡-Eth研究中心 用于将物体放置在表面上的系统和方法
CN110285753A (zh) * 2019-06-25 2019-09-27 中国海洋大学 海洋浮式结构物水池试验模型大空间光学运动测量方法
CN110520694A (zh) * 2017-10-31 2019-11-29 深圳市大疆创新科技有限公司 一种视觉里程计及其实现方法
US11274788B2 (en) 2018-04-25 2022-03-15 SZ DJI Technology Co., Ltd. Gimbal pose correction method and device
CN114565669A (zh) * 2021-12-14 2022-05-31 华人运通(上海)自动驾驶科技有限公司 一种场端多相机融合定位方法
CN115147599A (zh) * 2022-06-06 2022-10-04 浙江大学 一种面向遮挡和截断场景的多几何特征学习的物体六自由度位姿估计方法
CN115830108A (zh) * 2021-09-17 2023-03-21 北京小米移动软件有限公司 相机位姿估计方法、装置、设备及介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102162738A (zh) * 2010-12-08 2011-08-24 中国科学院自动化研究所 摄像头与惯性传感器组合定位定姿系统的标定方法
CN102435188A (zh) * 2011-09-15 2012-05-02 南京航空航天大学 一种用于室内环境的单目视觉/惯性全自主导航方法
CN102538781A (zh) * 2011-12-14 2012-07-04 浙江大学 基于机器视觉和惯导融合的移动机器人运动姿态估计方法
WO2013162140A1 (ko) * 2012-04-26 2013-10-31 서울시립대학교 산학협력단 영상매칭 기반의 스마트폰의 위치자세 결정 방법 및 시스템
WO2013171731A1 (en) * 2012-05-16 2013-11-21 Imagine Mobile Augmented Reality Ltd A system worn by a moving user for fully augmenting reality by anchoring virtual objects

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102162738A (zh) * 2010-12-08 2011-08-24 中国科学院自动化研究所 摄像头与惯性传感器组合定位定姿系统的标定方法
CN102435188A (zh) * 2011-09-15 2012-05-02 南京航空航天大学 一种用于室内环境的单目视觉/惯性全自主导航方法
CN102538781A (zh) * 2011-12-14 2012-07-04 浙江大学 基于机器视觉和惯导融合的移动机器人运动姿态估计方法
WO2013162140A1 (ko) * 2012-04-26 2013-10-31 서울시립대학교 산학협력단 영상매칭 기반의 스마트폰의 위치자세 결정 방법 및 시스템
CN104335649A (zh) * 2012-04-26 2015-02-04 首尔市立大学校产学协力团 基于图像匹配的确定智能手机位置和姿态的方法和系统
WO2013171731A1 (en) * 2012-05-16 2013-11-21 Imagine Mobile Augmented Reality Ltd A system worn by a moving user for fully augmenting reality by anchoring virtual objects

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
HENRIK REHBINDER, XIAOMING HU: "Drift-free attitude estimation for accelerated rigid bodies", 《AUTOMATIC》, vol. 40, no. 4, 31 December 2004 (2004-12-31) *
MINGYANG LI, BYUNG HYUNG KIM: "Real-time Motion Tracking on a Cellphone using Inertial Sensing and a Rolling-Shutter Camera", 《2013 IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION (ICRA)》, 10 May 2013 (2013-05-10) *

Cited By (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105094327B (zh) * 2015-07-22 2018-11-30 深圳多新哆技术有限责任公司 调整虚拟物件在虚拟空间中姿态角的方法及装置
CN105094327A (zh) * 2015-07-22 2015-11-25 深圳多新哆技术有限责任公司 调整虚拟物件在虚拟空间中姿态角的方法及装置
CN110036162B (zh) * 2016-09-30 2021-04-02 新加坡-Eth研究中心 用于将物体放置在表面上的系统和方法
CN110036162A (zh) * 2016-09-30 2019-07-19 新加坡-Eth研究中心 用于将物体放置在表面上的系统和方法
CN110520694A (zh) * 2017-10-31 2019-11-29 深圳市大疆创新科技有限公司 一种视觉里程计及其实现方法
CN108399641A (zh) * 2018-03-12 2018-08-14 北京华捷艾米科技有限公司 重新检测地面的判定方法及装置
US10776932B2 (en) 2018-03-12 2020-09-15 BeiJing Hjimi Technology Co., Ltd Determining whether ground is to be re-detected
CN108399641B (zh) * 2018-03-12 2019-10-11 北京华捷艾米科技有限公司 重新检测地面的判定方法及装置
US11274788B2 (en) 2018-04-25 2022-03-15 SZ DJI Technology Co., Ltd. Gimbal pose correction method and device
US12345370B1 (en) * 2018-04-25 2025-07-01 SZ DJI Technology Co., Ltd. Gimbal pose correction method and device
CN109302602A (zh) * 2018-10-11 2019-02-01 广州土圭垚信息科技有限公司 一种基于视焦点预测的自适应vr无线传输方法
CN109598764A (zh) * 2018-11-30 2019-04-09 Oppo广东移动通信有限公司 摄像头标定方法和装置、电子设备、计算机可读存储介质
CN110285753B (zh) * 2019-06-25 2020-08-18 中国海洋大学 海洋浮式结构物水池试验模型大空间光学运动测量方法
CN110285753A (zh) * 2019-06-25 2019-09-27 中国海洋大学 海洋浮式结构物水池试验模型大空间光学运动测量方法
CN115830108A (zh) * 2021-09-17 2023-03-21 北京小米移动软件有限公司 相机位姿估计方法、装置、设备及介质
CN114565669A (zh) * 2021-12-14 2022-05-31 华人运通(上海)自动驾驶科技有限公司 一种场端多相机融合定位方法
CN115147599A (zh) * 2022-06-06 2022-10-04 浙江大学 一种面向遮挡和截断场景的多几何特征学习的物体六自由度位姿估计方法

Similar Documents

Publication Publication Date Title
CN103900473A (zh) 一种基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法
CN111156998B (zh) 一种基于rgb-d相机与imu信息融合的移动机器人定位方法
CN106679648B (zh) 一种基于遗传算法的视觉惯性组合的slam方法
CN106940704B (zh) 一种基于栅格地图的定位方法及装置
CN110617814A (zh) 单目视觉和惯性传感器融合的远距离测距系统及方法
JP6198230B2 (ja) 深度カメラを使用した頭部姿勢トラッキング
CN103759716B (zh) 基于机械臂末端单目视觉的动态目标位置和姿态测量方法
Orghidan et al. Camera calibration using two or three vanishing points
CN110617813B (zh) 单目视觉信息和imu信息相融合的尺度估计系统及方法
CN111127524A (zh) 一种轨迹跟踪与三维重建方法、系统及装置
CN110749308B (zh) 使用消费级gps和2.5d建筑物模型的面向slam的室外定位方法
Hansen et al. Online continuous stereo extrinsic parameter estimation
CN104075688A (zh) 一种双目立体凝视监控系统的测距方法
WO2018174954A1 (en) System and method for merging maps
CN109903330A (zh) 一种处理数据的方法和装置
CN104848861B (zh) 一种基于图像消失点识别技术的移动设备姿态测量方法
WO2020133172A1 (zh) 图像处理方法、设备及计算机可读存储介质
CN113474819A (zh) 信息处理装置、信息处理方法和程序
US12366590B2 (en) Visual inertial odometry with machine learning depth
Wang et al. LF-VIO: A visual-inertial-odometry framework for large field-of-view cameras with negative plane
Huai et al. Real-time large scale 3D reconstruction by fusing Kinect and IMU data
Xian et al. Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach
Huttunen et al. A monocular camera gyroscope
De Marco et al. Position, velocity, attitude and accelerometer-bias estimation from imu and bearing measurements
CN110598370B (zh) 基于sip和ekf融合的多旋翼无人机鲁棒姿态估计

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20140702