CN108375382B - 基于单目视觉的位置姿态测量系统精度检校方法和装置 - Google Patents

基于单目视觉的位置姿态测量系统精度检校方法和装置 Download PDF

Info

Publication number
CN108375382B
CN108375382B CN201810153912.0A CN201810153912A CN108375382B CN 108375382 B CN108375382 B CN 108375382B CN 201810153912 A CN201810153912 A CN 201810153912A CN 108375382 B CN108375382 B CN 108375382B
Authority
CN
China
Prior art keywords
coordinate system
imu
target
matrix
camera
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.)
Active
Application number
CN201810153912.0A
Other languages
English (en)
Other versions
CN108375382A (zh
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.)
Beihang University
Original Assignee
Beihang University
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 Beihang University filed Critical Beihang University
Priority to CN201810153912.0A priority Critical patent/CN108375382B/zh
Publication of CN108375382A publication Critical patent/CN108375382A/zh
Application granted granted Critical
Publication of CN108375382B publication Critical patent/CN108375382B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明涉及基于单目视觉的位置姿态测量系统精度检校方法和装置，首先使用基于动态三维重构的单目视觉测量方法获取靶标坐标系相对相机坐标系的位姿变化量；然后使用基于正交矢量和动态滤波的联合标定法获取惯性测量单元(IMU)坐标系和靶标坐标系间的相对位姿关系，将第一位姿数据转到IMU坐标系下，与POS自身输出的位置姿态数据进行比对，检校所述POS的测量精度。本发明的方法和装置实现了对POS的位姿精度的有效检校，具有精度高、实现简单的特点，可用于在室内对目前低精度、高精度惯性产品进行位姿精度检校。

Description

基于单目视觉的位置姿态测量系统精度检校方法和装置
技术领域
本发明涉及惯性系统精度检校技术领域，具体涉及一种基于单目视觉的位置姿态测量系统精度检校方法和装置。
背景技术
高分辨率对地观测的高精度运动成像中，载体由于受气流、高空风及载体自身性能等诸多因素的影响，造成载体在轨迹、速度和姿态上的扰动，使载体偏离理想的运动状态。由此产生的运动误差，将导致遥感图像几何失真、分辨率降低、对比度损失。因此必须对载体的运动信息进行高精度的估计和运动补偿，从而才能实现遥感图像像质的退化抑制。高精度POS由惯性测量单元(Inertial measurement Unit,IMU)、导航计算机系统(POSComputer System，PCS)和GPS(Global Positioning System)组成。高精度POS可以为高分辨率航空遥感系统提供高频、高精度的时间、空间及精度信息，通过运动误差补偿提高成像精度和效率，是实现高分辨率成像的关键。
POS可以实现高精度姿态测量和位置测量，而POS测量精度的有效检校则是保证POS高精度的一个重要前提。目前，POS的精度检校主要是空中三角测量检校方法和塔吊台检校方法。空中三角测量是将相机和POS安装在稳定平台，飞机飞行过程，相机拍摄地面预设标致点解算位置、姿态。塔吊台检校方法是在地面模拟飞机飞行，进行检校。这些方法实现成本均较高。
发明内容
本发明的技术解决问题是：克服现有技术的不足，提出了一种基于单目视觉的位置姿态测量系统精度检校方法和装置，克服传统检校方法成本高等缺点。
本发明技术解决方案是：一种基于单目视觉的位置姿态测量系统精度检校方法，实现步骤如下：
基于单目视觉测量，将位置固定的相机的坐标系作为测量坐标系，获取与所述IMU固连的靶标的第一位姿数据；
标定IMU坐标系和靶标坐标系之间的固定相对位姿关系，将所述第一位姿数据转到IMU坐标系下，得出第二位姿数据；
将所述第二位姿数据与POS自身输出的位置姿态数据进行比对，检校所述POS的测量精度。
优选地，步骤基于单目视觉测量、将位置固定的相机的坐标系作为测量坐标系、获取与所述IMU固连的靶标的第一位姿数据，具体包括步骤：
通过所述相机预先从不同角度拍摄三幅靶标的参考图像；
将IMU与所述靶标固连，通过所述相机获取一张所述靶标的实时图像；
基于多视图三维重构原理，根据三幅参考图像和一张实时图像进行动态三维重构，获取靶标的第一位姿数据。
优选地，步骤基于多视图三维重构原理、根据三幅参考图像和一张实时图像进行动态三维重构，具体包括：
由所述相机的投影模型得到每个标志点和对应的像点之间有公式：
Figure GDA0002651384660000021
式中：
Figure GDA0002651384660000031
为第i幅图上第j个标志点像坐标，Xj为第j个标志点物坐标，pi为第i幅图对应的投影矩阵；
Figure GDA0002651384660000032
为第i幅图中第j个像点和其对应的物点的距离在相机光轴方向的投影，称为射影深度；
构建测量矩阵W：
Figure GDA0002651384660000033
其中，
Figure GDA0002651384660000034
为测量坐标系下所测数据，使用共轭梯度法迭代每个点的射影深度
Figure GDA0002651384660000035
通过测量坐标系的实测坐标每次更新测量矩阵。
优选地，步骤基于多视图三维重构原理、根据三幅参考图像和一张实时图像进行动态三维重构中，在构建测量矩阵W之后还包括步骤：
对W阵通过SVD分解：
W＝V·diag(σ1,…σN)·UT
由P、X阵，可知，W的秩小于等于4，即σ5＝σ6＝…＝σ9＝0因此有：
Figure GDA0002651384660000036
此时W阵可以分解为一个表示相机运动的P阵和一个表示靶标形状的X阵：
Figure GDA0002651384660000037
Figure GDA0002651384660000038
式中V1,U1分别为V,U的前4列；分解不唯一，上式在相差任意非奇异4x4矩阵H的情况下都满足：
W＝PHH-1X
当存在非奇异矩阵H满足欧式变换条件：
PpH＝αK[RT]
式中α是满足矩阵方程的任意非零实数；将H改写为H＝[Ha,Hb],Ha是H阵的前3列，Hb是H阵第四列；从欧式变换矩阵中分割出旋转部分得：
PpHa＝αKR
对上式进行转置并令
Figure GDA0002651384660000041
ω＝KKT，可得：
Figure GDA0002651384660000042
将
Figure GDA0002651384660000043
展开，可得3x3对称阵，用Dab表示该阵第a行b列的元素：
Figure GDA0002651384660000044
Dab是对称阵，将其拆为方程组，列出上三角6个元素对应方程：
Figure GDA0002651384660000045
对M进行奇异值分解：
Figure GDA0002651384660000046
式中Ua为U的前3列；因H满足下式PpHb＝[0,0,0]T，带入任意Pp解出Hb；最终解出H，完成欧式重构：
Figure GDA0002651384660000047
C是Pe中前3行，前3列数据，即为所求的第一位姿数据。
优选地，步骤标定IMU坐标系和靶标坐标系之间的固定相对位姿关系，具体包括步骤：
基于正交矢量和动态滤波的联合标定法，先使用正交矢量法标定两坐标系姿态关系，再使用动态滤波法估计两坐标系位置关系，滤波状态量为：
Figure GDA0002651384660000051
其中，
Figure GDA0002651384660000052
为IMU输出的在地理坐标系位置；
Figure GDA0002651384660000053
为IMU输出的在地理坐标系速度；ba为IMU中加速度计漂移误差；
Figure GDA0002651384660000054
为IMU坐标系和靶标坐标系之间的相对位置关系；
滤波量测量为：
Figure GDA0002651384660000055
其中，z为相机测得的靶标的位置信息；
Figure GDA0002651384660000056
其中，
Figure GDA0002651384660000057
由IMU直接输出，
Figure GDA0002651384660000058
可由已标定姿态和IMU、相机输出姿态计算得到：
Figure GDA0002651384660000059
其中，
Figure GDA00026513846600000510
由正交矢量法标定得到。
本发明还提供一种基于单目视觉的位置姿态测量系统精度检校装置，用于检测POS输出的位姿数据的精度，包括靶标、相机和检校模块；
靶标与POS的IMU固连；相机固定设置；检校模块，用于：
基于单目视觉测量，将位置固定的相机的坐标系作为测量坐标系，获取靶标的第一位姿数据；标定IMU坐标系和靶标坐标系之间的固定相对位姿关系，将第一位姿数据转到IMU坐标系下，得出第二位姿数据；将第二位姿数据与POS自身输出的位置姿态数据进行比对，检校POS的测量精度。
优选地，所述相机，用于预先从不同角度拍摄三幅靶标的参考图像，并在固定后拍摄一张所述靶标的实时图像；
所述检校模块，用于：基于多视图三维重构原理，根据三幅参考图像和一张实时图像进行动态三维重构，获取靶标的第一位姿数据。
优选地，所述检校模块，用于首先构建测量矩阵W：
由所述相机的投影模型得到每个标志点和对应的像点之间有公式：
Figure GDA0002651384660000061
式中：
Figure GDA0002651384660000062
为第i幅图上第j个标志点像坐标，Xj为第j个标志点物坐标，pi为第i幅图对应的投影矩阵；
Figure GDA0002651384660000063
为第i幅图中第j个像点和其对应的物点的距离在相机光轴方向的投影，称为射影深度；
构建测量矩阵W：
Figure GDA0002651384660000064
其中，
Figure GDA0002651384660000065
为测量坐标系下所测数据，使用共轭梯度法迭代每个点的射影深度
Figure GDA0002651384660000066
通过测量坐标系的实测坐标每次更新测量矩阵。
优选地，所述检校模块，还用于：
基于正交矢量和动态滤波的联合标定法来标定IMU坐标系和靶标坐标系之间的固定相对位姿关系，先使用正交矢量法标定两坐标系姿态关系，再使用动态滤波法估计两坐标系位置关系，滤波状态量为：
Figure GDA0002651384660000067
其中，
Figure GDA0002651384660000068
为IMU输出的在地理坐标系位置；
Figure GDA0002651384660000069
为IMU输出的在地理坐标系速度；ba为IMU中加速度计漂移误差；
Figure GDA00026513846600000610
为IMU坐标系和靶标坐标系之间的相对位置关系；
滤波量测量为：
Figure GDA00026513846600000611
其中，z为相机测得的靶标的位置信息；
Figure GDA0002651384660000071
其中，
Figure GDA0002651384660000072
由IMU直接输出，
Figure GDA0002651384660000073
可由已标定姿态和IMU、相机输出姿态计算得到：
Figure GDA0002651384660000074
其中，
Figure GDA0002651384660000075
由正交矢量法标定得到。
本发明与现有技术相比，具有如下技术效果：
本发明针对POS的检校成本较高问题，使用单目视觉检校的方法，对POS的位置、姿态输出进行精度检校；针对单目视觉测量光轴向精度低的问题，使用动态三维重构方法，提高测量精度；针对辅助靶标和IMU坐标系统一问题，使用基于正交矢量和动态滤波的联合标定法标定IMU坐标系和靶标坐标系之间的相对位姿关系，克服现有坐标系统方法精度低的问题，提高了坐标转换的精度，可用于在地面对目前低精度、高精度惯性产品进行位姿精度检校。同时基于本发明的检校方案，实施便捷无需特殊设备，检校成本较低。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明基于单目视觉进行位置姿态测量系统精度检校的硬件系统结构示意图；
图2为本发明基于单目视觉的位置姿态测量系统精度检校方法的一个实施例的主要流程示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明公开了一种基于单目视觉的位置姿态测量系统精度检校方法，该方法基于单目视觉测量，将位置固定的相机的坐标系作为测量坐标系，获取与IMU固连的靶标的第一位姿数据；标定IMU坐标系和靶标坐标系之间的固定相对位姿关系，将第一位姿数据转到IMU坐标系下，得出第二位姿数据；将第二位姿数据与POS自身输出的位置姿态数据进行比对，检校POS的测量精度。
本发明还公开了一种基于单目视觉的位置姿态测量系统精度检校装置，用于检测POS输出的位姿数据的精度，包括靶标、相机和检校模块。
靶标与POS的IMU固连；相机固定设置。检校模块，用于基于单目视觉测量，将位置固定的相机的坐标系作为测量坐标系，获取靶标的第一位姿数据；标定IMU坐标系和靶标坐标系之间的固定相对位姿关系，将第一位姿数据转到IMU坐标系下，得出第二位姿数据；将第二位姿数据与POS自身输出的位置姿态数据进行比对，检校POS的测量精度。
参见图1所示，优选地，作为一种可实施方式，本发明检校装置的硬件基本组成包括相机110、靶标111，被检校的POS系统则包括IMU112、PCS113和GPS天线114，即IMU、PCS和GPS组成POS系统。
其中，IMU112和靶标111一面刚性固连，并一起安装在转台上，相机110单独固定在靶标前方，拍摄靶标。运动过程中，靶标平面始终在相机视场内，由GPS秒脉冲触发IMU和相机工作，实现时间同步。
参见图2所示，作为一种可实施方式，本发明检校方法包括步骤：
首先搭建POS的精度检校试验环境，将高精度IMU和经由高精度加工的视觉辅助靶标刚性固连，及将靶标平面和IMU一面贴合，将IMU安装在运动转台上，相机单独固定，运动过程中，靶标平面始终在相机视场内。IMU和相机由GPS秒脉冲同步触发工作。
然后通过单相机实现动态三维重构测量，基于多视图三维重构原理，单相机固定作为测量坐标系，相机实施获取的一幅靶标图像，和三幅靶标初始时刻的不同角度的图像，建立测量矩阵，由矩阵分解、三维重构，获取靶标的高精度的位置、姿态。
标定出IMU坐标系和靶标坐标系之间相对位姿关系，使用基于正交矢量和动态滤波的联合标定法，先使用正交矢量法标定两坐标系姿态关系，再使用动态滤波估计两坐标系位置关系。
最后将视觉测量得到靶标的位姿变化量，由标定得到的IMU和靶标坐标系关系，转到IMU坐标系，实现对POS自身输出的位置、姿态数据进行检校。
下面列举本发明检校方法的一个优选实施例进行具体说明，该较优的实施方式具体实施步骤如下：
步骤S11：首先对靶标平面从不同角度拍摄3幅图像，将相机固定后每拍摄的一幅图像和初始3幅图像构建测量矩阵W。
单相机实现动态三维重构测量是基于多视图三维重构原理，靶标平面有9个特征点，由相机投影模型得到每个标志点和对应的像点之间有公式：
Figure GDA0002651384660000101
式中：
Figure GDA0002651384660000102
为第i幅图上第j个标志点像坐标，Xj为第j个标志点物坐标，pi为第i幅图对应的投影矩阵。
Figure GDA0002651384660000103
为第i幅图中第j个像点和其对应的物点的距离在相机光轴方向的投影，称为射影深度。
构建测量矩阵W：
Figure GDA0002651384660000104
其中，
Figure GDA0002651384660000105
为第i幅参考图像上第j个标志点像坐标，为3行1列的向量，i＝1,,,4表示共有4幅图像，其中，上式中第一行为相机实测的实时图像的图片信息，作为测量基准坐标系，另外三行为静态情况下测量的三幅图片信息；
Figure GDA0002651384660000106
为第i幅图中第j个像点和其对应的物点的距离在所述相机光轴方向的投影，即射影深度，j＝1,,,9表示靶标上的9个特征点。
当对靶标上9个标志点拍摄4幅图像，可得测量矩阵W；
Figure GDA0002651384660000107
其中，W为12x9阵，P为12x4阵，X为4x9阵。
Figure GDA0002651384660000111
为测量坐标系下所测数据，使用共轭梯度法迭代每个点的射影深度
Figure GDA0002651384660000115
通过测量坐标系的实测坐标每次更新测量矩阵。
步骤S12：此时，对W阵通过SVD分解：
W＝V·diag(σ1,…σN)·UT
由P、X阵，可知，W的秩小于等于4，即σ5＝σ6＝…＝σ9＝0因此有：
Figure GDA0002651384660000112
此时阵可以分解为一个表示相机运动的P阵和一个表示靶标形状的X阵：
Figure GDA0002651384660000113
Figure GDA0002651384660000114
式中V1,U1分别为V,U的前4列。本发明所采用的SVD分解即数学上通用矩阵求解方法，参数V、U为酉矩阵，σ为奇异值。
但分解不唯一，因为上式在相差任意非奇异4x4矩阵H的情况下都满足：
W＝PHH-1X
当存在非奇异矩阵H满足欧式变换条件：
PpH＝αK[RT]
式中α是满足矩阵方程的任意非零实数。Pp为P在射影变换下的表示，α为比例系数，K为相机内参，R、T为相机外参。H为变换矩阵。
将H改写为H＝[Ha,Hb],Ha是H阵的前3列，Hb是H阵第四列。从欧式变换矩阵中分割出旋转部分得：
PpHa＝αKR
对上式进行转置得：
Figure GDA0002651384660000121
令
Figure GDA0002651384660000122
ω＝KKT，所以可得：
Figure GDA0002651384660000123
将
Figure GDA0002651384660000124
展开，可得3x3对称阵，用Dab表示该阵第a行b列的元素：
Figure GDA0002651384660000125
由于Dab是对称阵，将其拆为方程组时，只需列出上三角6个元素对应方程：
Figure GDA0002651384660000126
上式共有20个未知量和24个方程，可解出所有未知数。对M进行奇异值分解：
Figure GDA0002651384660000127
式中Ua为U的前3列。因H满足下式PpHb＝[0,0,0]T，带入任意Pp解出Hb。最终解出H，完成欧式重构：
Figure GDA0002651384660000128
C是Pe中前3行，前3列数据，既为所求的物体姿态信息(第一位姿数据)。
M、ω是为简化公式书写而用的代替字母，c、d表示矩阵的行列序号，Pe、Xe分别为P、X在欧式变换下的表示。
步骤S13：IMU和辅助靶标的相对姿态标定是使用基于正交矢量和动态滤波的联合标定法。
①首先由正交矢量法标定相对姿态，以转台坐标系作为过渡坐标系，转台首先分别绕x轴、y轴匀速转动，旋转角速度在转台坐标系分别表示
Figure GDA0002651384660000131
同时IMU输出角速度
Figure GDA0002651384660000132
正交矢量差乘可以得到第三个矢量。
Figure GDA0002651384660000133
为转台坐标系和IMU坐标系之间的相对姿态，则有：
Figure GDA0002651384660000134
IMU绕转台x轴转动时，相机使用步骤(2)中方法在运动的初始时刻和终止时刻分别采集一幅图像，并求解两个时刻靶标坐标系到相机坐标系的旋转矩阵，记为Cw0、Cw1,两者之间的关系为：
Figure GDA0002651384660000135
转换为四元数
Figure GDA0002651384660000136
则有：
Figure GDA0002651384660000137
可以得到转台坐标系x轴方向的单位向量u1。
Figure GDA0002651384660000138
同理，绕y轴可得u2。由u1、u2和两者差乘矢量，可解得靶标坐标系和转台坐标系之间的相对姿态关系
Figure GDA0002651384660000139
Figure GDA00026513846600001310
q1,q2,q3,q4分别为四元数q的标量常数和三轴失量系数；
由转台和IMU位姿关系、靶标和转台位姿关系，最终可得到靶标和IMU之间的相对姿态关系：
Figure GDA0002651384660000141
②然后使用动态滤波法(EKF)(扩展卡尔曼滤波器)标定相对位置：
滤波状态量如下：
Figure GDA0002651384660000142
式中：
Figure GDA0002651384660000143
为IMU在地理坐标系位置；
Figure GDA0002651384660000144
为IMU在地理坐标系速度；ba为IMU加速度计漂移误差；
Figure GDA0002651384660000145
为IMU坐标系和靶标坐标系之间的相对位置关系；
状态方程由控制矢量uk驱动，其中
Figure GDA0002651384660000146
分别为IMU在地理坐标系输出的线性加速度和影响IMU加速度漂移误差的高斯白噪声。
Figure GDA0002651384660000147
因靶标与IMU保持固定，所以相对位置
Figure GDA0002651384660000148
始终不变，时间更新：
xk+1/k＝Fkxk+Cuk
Pk+1/k＝FkPkFk+Qk
上式中，xk+1/k为k+1时刻状态xk的一步预测值，Pk+1/k为k+1时刻一步预测均方误差阵。Pk为第k时刻估计均方误差阵，Qk为第k时刻系统噪声方差阵。Fk为k到k+1时刻的一步转移阵，C为系统噪声驱动阵。
滤波量测模型：
相机通过拍摄靶标，计算出相机在世界坐标系w中的位置
Figure GDA0002651384660000149
及其相对世界坐标系的相对转向
Figure GDA00026513846600001410
将
Figure GDA00026513846600001411
作为EKF的3维量测矢量z。
Figure GDA00026513846600001412
式中
Figure GDA0002651384660000151
可由已标定姿态
Figure GDA0002651384660000152
和IMU、相机输出姿态
Figure GDA0002651384660000153
计算得到：
Figure GDA0002651384660000154
测量矩阵H为对xk+1/k求导得到的雅可比矩阵：
Figure GDA0002651384660000155
EKF的量测更新：
Figure GDA00026513846600001510
xk+1＝xk+1/k+Kk+1(zk+1-Hk+1xk+1/k)
Pk+1＝(I-Kk+1Hk+1)Pk+1/k
上式中，xk+1为状态量估计值，Kk+1为滤波增益阵，Rk+1为量测噪声方差阵，Hk+1为量测量对状态量的雅可比矩阵，下标k,k+1为时间序列，表示k和k+1时刻。
步骤S13：检校IMU输出数据精度
相机测量的靶标的位姿数据变化量为
Figure GDA0002651384660000156
标定的IMU与辅助靶标的的相对位姿为
Figure GDA0002651384660000157
由此，可得IMU坐标系的姿态变化量为：
Figure GDA0002651384660000158
POS系统自身输出姿态变化量记为Cb，则POS输出的姿态精度为ΔCb：
Figure GDA0002651384660000159
现有技术中的空中三角测量检校方法和塔吊台检校方法不仅存在检校成本高的缺点，而且操作复杂、误差因素多，误差累积降低精度；现有技术中的地面上位置、姿态检校方法有伪卫星检校和星敏感器检20校，伪卫星姿态高精度检校实现困难，星敏感器在地面应用受环境影响大，导致精度不高。
本发明提供的上述基于单目视觉的位置姿态测量系统精度检校方法和装置，首先使用基于动态三维重构的单目视觉测量方法获取靶标坐标系相对相机坐标系的位姿变化量；然后使用基于正交矢量和动态滤波的联合标定法获取惯性测量单元(IMU)坐标系和靶标坐标系间的相对位姿关系。最后将视觉测得的靶标的位姿变化转换到IMU坐标系下，并对POS自身输出的位姿数据精度进行检校，克服了现有技术中检校方案实施成本高、实现复杂、测量精度低的缺点，本发明提供的检校方案具有精度高、实现便捷、抗干扰能力强的特点，可用于在地面对目前低精度、高精度惯性产品进行位姿精度检校。
本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (2)

1.基于单目视觉的位置姿态测量系统精度检校方法，其特征在于，包括以下步骤：
通过相机预先从不同角度拍摄三幅靶标的参考图像；
将IMU与所述靶标固连，通过所述相机获取一张所述靶标的实时图像；
由所述相机的投影模型得到每个标志点和对应的像点之间的关系为：
Figure FDA0002666089360000011
式中：
Figure FDA0002666089360000012
为第i幅图上第j个标志点像坐标，Xj为第j个标志点物坐标，Pi为第i幅图对应的投影矩阵；
Figure FDA0002666089360000013
为第i幅图中第j个像点和其对应的物点的距离在相机光轴方向的投影，称为射影深度；
构建测量矩阵W：
Figure FDA0002666089360000014
其中，
Figure FDA0002666089360000015
为测量坐标系下所测数据，使用共轭梯度法迭代每个点的射影深度
Figure FDA0002666089360000016
通过测量坐标系的实测坐标更新测量矩阵，对W阵通过SVD分解：
W＝V·diag(σ1,…σN)·UT
由P、X阵，可知，W的秩小于等于4，即σ5＝σ6＝…＝σ9＝0因此有：
Figure FDA0002666089360000017
此时W阵可以分解为一个表示相机运动的P阵和一个表示靶标形状的X阵：
Figure FDA0002666089360000018
Figure FDA0002666089360000019
式中V1,U1分别为V,U的前4列；分解不唯一，上式在相差任意非奇异4x4矩阵H的情况下都满足：
W＝PHH-1X
当存在非奇异矩阵H满足欧式变换条件：
PpH＝αK[RT]
式中α是满足矩阵方程的任意非零实数；将H改写为H＝[Ha,Hb],Ha是H阵的前3列，Hb是H阵第四列；从欧式变换矩阵中分割出旋转部分得：
PpHa＝αKR
对上式进行转置并令
Figure FDA0002666089360000021
ω＝KKT，可得：
Figure FDA0002666089360000022
将
Figure FDA0002666089360000023
展开，可得3x3对称阵，用Dab表示该阵第a行b列的元素：
Figure FDA0002666089360000024
Dab是对称阵，将其拆为方程组，列出上三角6个元素对应方程：
Figure FDA0002666089360000025
对M进行奇异值分解：
M＝Udiag(σ1,σ2,σ3,0)VT
Figure FDA0002666089360000026
式中Ua为U的前3列；因H满足下式PpHb＝[0,0,0]T，带入任意Pp解出Hb；最终解出H，完成欧式重构：
Figure FDA0002666089360000027
C是Pe中前3行，前3列数据，即为所求的第一位姿数据，获取靶标的第一位姿数据；
标定IMU坐标系和靶标坐标系之间的固定相对位姿关系，具体包括步骤：
基于正交矢量和动态滤波的联合标定法，先使用正交矢量法标定两坐标系姿态关系，再使用动态滤波法估计两坐标系位置关系，滤波状态量为：
Figure FDA0002666089360000031
其中，
Figure FDA0002666089360000032
为IMU在地理坐标系的位置；
Figure FDA0002666089360000033
为IMU在地理坐标系的速度；ba为IMU中加速度计漂移误差；
Figure FDA0002666089360000034
为IMU坐标系和靶标坐标系之间的相对位置关系；
滤波量测量为：
Figure FDA0002666089360000035
其中，z为相机测得的靶标的位置信息；
Figure FDA0002666089360000036
其中，
Figure FDA0002666089360000037
由IMU直接输出，
Figure FDA0002666089360000038
由已标定姿态和IMU、相机输出姿态计算得到：
Figure FDA0002666089360000039
其中，
Figure FDA00026660893600000310
由正交矢量法标定得到；
将所述第一位姿数据转到IMU坐标系下，得出第二位姿数据；
将所述第二位姿数据与POS自身输出的位置姿态数据进行比对，检校所述POS的测量精度。
2.基于单目视觉的位置姿态测量系统精度检校装置，用于检测POS输出的位姿数据的精度，其特征在于，包括靶标、相机和检校模块；
所述靶标与所述POS的IMU固连；
所述相机，固定设置，用于预先从不同角度拍摄三幅靶标的参考图像，并在固定后拍摄一张所述靶标的实时图像；
所述检校模块，用于：
由所述相机的投影模型得到每个标志点和对应的像点之间的关系为：
Figure FDA0002666089360000041
式中：
Figure FDA0002666089360000042
为第i幅图上第j个标志点像坐标，Xj为第j个标志点物坐标，Pi为第i幅图对应的投影矩阵；
Figure FDA0002666089360000043
为第i幅图中第j个像点和其对应的物点的距离在相机光轴方向的投影，称为射影深度；
构建测量矩阵W：
Figure FDA0002666089360000044
其中，
Figure FDA0002666089360000045
为测量坐标系下所测数据，使用共轭梯度法迭代每个点的射影深度
Figure FDA0002666089360000046
通过测量坐标系的实测坐标更新测量矩阵，对W阵通过SVD分解：
W＝V·diag(σ1,…σN)·UT
由P、X阵，可知，W的秩小于等于4，即σ5＝σ6＝…＝σ9＝0因此有：
Figure FDA0002666089360000047
此时W阵可以分解为一个表示相机运动的P阵和一个表示靶标形状的X阵：
Figure FDA0002666089360000048
Figure FDA0002666089360000049
式中V1,U1分别为V,U的前4列；分解不唯一，上式在相差任意非奇异4x4矩阵H的情况下都满足：
W＝PHH-1X
当存在非奇异矩阵H满足欧式变换条件：
PpH＝αK[RT]
式中α是满足矩阵方程的任意非零实数；将H改写为H＝[Ha,Hb],Ha是H阵的前3列，Hb是H阵第四列；从欧式变换矩阵中分割出旋转部分得：
PpHa＝αKR
对上式进行转置并令
Figure FDA0002666089360000051
ω＝KKT，可得：
Figure FDA0002666089360000052
将
Figure FDA0002666089360000053
展开，可得3x3对称阵，用Dab表示该阵第a行b列的元素：
Figure FDA0002666089360000054
Dab是对称阵，将其拆为方程组，列出上三角6个元素对应方程：
Figure FDA0002666089360000055
对M进行奇异值分解：
Figure FDA0002666089360000056
式中Ua为U的前3列；因H满足下式PpHb＝[0,0,0]T，带入任意Pp解出Hb；最终解出H，完成欧式重构：
Figure FDA0002666089360000057
C是Pe中前3行，前3列数据，即为所求的第一位姿数据，获取靶标的第一位姿数据；
标定IMU坐标系和靶标坐标系之间的固定相对位姿关系，将所述第一位姿数据转到IMU坐标系下，得出第二位姿数据；
将所述第二位姿数据与POS自身输出的位置姿态数据进行比对，检校所述POS的测量精度；
所述检校模块，还用于：
基于正交矢量和动态滤波的联合标定法来标定IMU坐标系和靶标坐标系之间的固定相对位姿关系，先使用正交矢量法标定两坐标系姿态关系，再使用动态滤波法估计两坐标系位置关系，滤波状态量为：
Figure FDA0002666089360000061
其中，
Figure FDA0002666089360000062
为IMU在地理坐标系的位置；
Figure FDA0002666089360000063
为IMU在地理坐标系的速度；ba为IMU中加速度计漂移误差；
Figure FDA0002666089360000064
为IMU坐标系和靶标坐标系之间的相对位置关系；
滤波量测量为：
Figure FDA0002666089360000065
其中，z为相机测得的靶标的位置信息；
Figure FDA0002666089360000066
其中，
Figure FDA0002666089360000067
由IMU直接输出，
Figure FDA0002666089360000068
由已标定姿态和IMU、相机输出姿态计算得到：
Figure FDA0002666089360000069
其中，
Figure FDA00026660893600000610
由正交矢量法标定得到。
CN201810153912.0A 2018-02-22 2018-02-22 基于单目视觉的位置姿态测量系统精度检校方法和装置 Active CN108375382B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810153912.0A CN108375382B (zh) 2018-02-22 2018-02-22 基于单目视觉的位置姿态测量系统精度检校方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810153912.0A CN108375382B (zh) 2018-02-22 2018-02-22 基于单目视觉的位置姿态测量系统精度检校方法和装置

Publications (2)

Publication Number Publication Date
CN108375382A CN108375382A (zh) 2018-08-07
CN108375382B true CN108375382B (zh) 2021-04-02

Family

ID=63017813

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810153912.0A Active CN108375382B (zh) 2018-02-22 2018-02-22 基于单目视觉的位置姿态测量系统精度检校方法和装置

Country Status (1)

Country Link
CN (1) CN108375382B (zh)

Families Citing this family (23)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109186596B (zh) * 2018-08-14 2020-11-10 深圳清华大学研究院 Imu测量数据生成方法、系统、计算机装置及可读存储介质
CN109341724B (zh) * 2018-12-04 2023-05-05 中国航空工业集团公司西安航空计算技术研究所 一种机载相机-惯性测量单元相对位姿在线标定方法
CN110660108B (zh) * 2019-09-11 2022-12-27 北京控制工程研究所 一种交会对接测量相机和对接抓捕机构的联合标定方法
CN112789655A (zh) * 2019-09-23 2021-05-11 北京航迹科技有限公司 用于标定惯性测试单元和相机的系统和方法
CN111024117B (zh) * 2019-11-21 2023-03-14 中国航空工业集团公司西安飞行自动控制研究所 一种基于视觉的惯导系统快速对准系统及对准方法
CN110954067B (zh) * 2019-12-28 2023-02-07 长安大学 一种基于靶标的单目视觉挖掘机位姿测量系统及测量方法
CN111323048B (zh) * 2020-02-28 2022-03-04 上海航天控制技术研究所 一种相对位姿测量单机性能测试方法及系统
CN111486867B (zh) * 2020-03-19 2023-08-11 天津大学 一种视觉和惯性混合跟踪组件安装参数的标定装置及方法
CN111489399B (zh) * 2020-03-19 2023-04-14 天津大学 一种视觉跟踪组件安装参数标定装置和方法
CN111504255B (zh) * 2020-04-28 2022-06-21 湖北三江航天万峰科技发展有限公司 一种基于机器视觉的三维对准精度自动测量装置及方法
CN111739102B (zh) * 2020-05-27 2023-07-21 杭州易现先进科技有限公司 电子设备的内外参标定方法、装置和计算机设备
CN112270691B (zh) * 2020-10-15 2023-04-21 电子科技大学 一种基于动态滤波器网络的单目视频结构和运动预测方法
CN112284253B (zh) * 2020-10-19 2022-08-26 北京航天众信科技有限公司 基于单目视觉的掘进机位姿测量方法、系统以及存储介质
CN112325905B (zh) * 2020-10-30 2023-02-24 歌尔科技有限公司 一种用于识别imu的测量误差的方法、装置及介质
CN112268548B (zh) * 2020-12-14 2021-03-09 成都飞机工业（集团）有限责任公司 一种基于双目视觉的飞机局部外形测量方法
CN113386130B (zh) * 2021-05-21 2023-02-03 北部湾大学 一种仿生蛇形机器人控制系统及其控制方法
CN113405568B (zh) * 2021-06-03 2022-10-28 中国农业大学 视觉测量系统结构参数测量的方法、系统及电子设备
CN113865506B (zh) * 2021-09-09 2023-11-24 武汉惟景三维科技有限公司 一种无标志点拼接的自动化三维测量方法及系统
CN114577448B (zh) * 2022-03-24 2023-12-26 山西支点科技有限公司 一种新型便携式光轴校靶适配装置的双光轴标定方法
CN115082557B (zh) * 2022-06-29 2024-03-15 中交第二航务工程局有限公司 一种基于双目视觉的塔柱吊装相对姿态测量方法
CN116026276B (zh) * 2023-03-28 2023-06-13 江苏集萃清联智控科技有限公司 港用门座起重机外置式转台转角测量方法及装置
CN116152357B (zh) * 2023-04-04 2023-07-28 国科天成科技股份有限公司 一种无限远对焦相机的参数标定系统和方法
CN117893610B (zh) * 2024-03-14 2024-05-28 四川大学 基于变焦单目视觉的航空装配机器人姿态测量系统

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102168989B (zh) * 2010-12-20 2013-01-02 北京航空航天大学 一种pos方位精度和姿态精度的地面测试方法
CN105698765B (zh) * 2016-02-22 2018-09-18 天津大学 双imu单目视觉组合测量非惯性系下目标物位姿方法
CN106595640A (zh) * 2016-12-27 2017-04-26 天津大学 基于双imu和视觉融合的动基座上物体相对姿态测量方法及系统

Also Published As

Publication number Publication date
CN108375382A (zh) 2018-08-07

Similar Documents

Publication Publication Date Title
CN108375382B (zh) 基于单目视觉的位置姿态测量系统精度检校方法和装置
CN108106637B (zh) 一种分布式pos的精度检校方法和装置
CN108592950B (zh) 一种单目相机和惯性测量单元相对安装角标定方法
CN106289246B (zh) 一种基于位置和姿态测量系统的柔性杆臂测量方法
CN102829785B (zh) 基于序列图像和基准图匹配的飞行器全参数导航方法
CN111044074B (zh) 基于外场观星的星敏感器标定装置与观星标定方法
KR100762891B1 (ko) Ｌｏｓ벡터 조정모델을 이용한 영상의 기하보정 방법 및 그장치
CN104698486B (zh) 一种分布式pos用数据处理计算机系统实时导航方法
CN107728182A (zh) 基于相机辅助的柔性多基线测量方法和装置
CN111947652A (zh) 一种适用于月球着陆器的惯性/视觉/天文/激光测距组合导航方法
CN108375383B (zh) 多相机辅助的机载分布式pos柔性基线测量方法和装置
CN113267794B (zh) 一种基线长度约束的天线相位中心校正方法及装置
CN110954134B (zh) 陀螺仪偏差校正方法、校正系统、电子设备及存储介质
GB2576569A (en) Inertial navigation system
CN106643670B (zh) 一种无人机航摄站点坐标求解装置及方法
CN110133692B (zh) 惯导技术辅助的高精度gnss动态倾斜测量系统及方法
CN109087355A (zh) 基于迭代更新的单目相机位姿测量装置与方法
JP2019120587A (ja) 測位システム及び測位方法
CN110646016A (zh) 基于经纬仪与视觉辅助柔性基线分布式pos标校方法和装置
CN110793542A (zh) 一种基于广义探元指向角的面阵光学遥感卫星在轨几何定标方法
CN110220533A (zh) 一种基于传递对准的机载光电吊舱设备失准角标定方法
CN114972078A (zh) 应用sar影像提升国产光学卫星影像无控几何质量方法及系统
CN113029132A (zh) 一种结合地面影像与恒星光行差测量的航天器导航方法
CN108594255A (zh) 一种激光测距辅助光学影像联合平差方法及系统
CN111486867A (zh) 一种视觉和惯性混合跟踪组件安装参数的标定装置及方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant