基于惯性测量单元的后方交会动态位姿测量系统及方法

摘要

本发明公开了基于惯性测量单元的后方交会动态位姿测量系统及方法，系统包括至少两台相机、一个惯性测量单元和一个时钟同步模块，其中相机用于增大测量装置的视场范围，时钟同步模块用于实现相机和惯性测量单元之间的时间对准和数据同步。本发明利用多个相机拍摄全局控制点，结合惯性测量单元的测量信息进行位姿补偿，并依据空间后方交会原理确定相机实时位姿的方法，以克服由于相机快速运动造成的控制点错误识别、错误匹配问题，在保证测量精度的前提下，提高后方交会动态位姿测量方法的鲁棒性和适用性。

权利要求

1.基于惯性测量单元的后方交会动态位姿测量方法，基于后方交会动态位姿测量系统，包括至少两台相机、一个惯性测量单元和一个时钟同步模块，其中相机用于增大测量系统的视场范围，时钟同步模块用于实现相机和惯性测量单元之间的时间对准和数据同步，其特征在于，包括以下步骤：(1)搭建动态位姿测量系统；

(2)标定各个相机之间的相对位置关系以及相机和惯性测量单元之间的相对位置关系；

(3)环境搭建：在测量环境中布置光学标记点作为全局控制点，测量相应标记点的三维坐标，建立三维控制场；

(4)初始状态确定：将动态位姿测量系统静止放置，同步触发各个相机采集图像，将经过图像处理后的像点与全局控制点进行一一对应，建立以控制点重投影误差最小为目标的优化函数，同时结合相机之间的相对位置关系建立约束方程，通过非线性最小二乘法求解相机位姿，进一步得到惯性测量单元在此位置下的位姿，并以当前状态作为动态位姿测量系统的初始状态；

(5)动态位姿更新：将初始状态设为0时刻，根据惯性测量单元的输出，对惯性测量单元位姿进行更新，得到t1时刻惯性测量单元的位姿，结合已经标定的相机和惯性测量单元的位置关系，得到t1时刻相机的估计位姿，并在此估计位姿的基础上将控制点投影到像平面上，得到估计像点，与相机采集到的真实像点进行比较，真实像点和距离其最近的估计像点对应同一个控制点，依据此判断方法实现真实像点和全局控制点的一一匹配，进一步根据步骤(4)中的方法计算相机在t1时刻的准确位姿，同理，可确定相机在ti(i＝1,2,3,…)时刻相对于全局坐标系的位置和姿态，实现动态位姿测量系统状态的自动更新。

说明书

基于惯性测量单元的后方交会动态位姿测量系统及方法

技术领域

[0001] 本发明涉及工业大尺度环境精密测量定位领域，具体涉及基于惯性测量单元的后方交会动态位姿测量系统及方法。

背景技术

[0002] 空间后方交会，是摄影测量领域的基础理论之一，其目标是通过拍摄空间中的全局控制点确定相机的外方位元素。空间后方交会的原理为影像的共线条件方程，利用内参数已知的相机拍摄空间中若干个坐标已知的全局控制点，然后结合像点坐标和控制点坐标建立共线条件约束方程，即可求解相机在当前位姿状态下的外部参数，实现对相机的定位。空间后方交会对布点空间要求不高，接收成本较低，可以快速确定受限空间内的姿态信息，具有结构简单、方便灵活等优点，在摄影测量领域得到了广泛的应用。

[0003] 惯性测量单元(Inertial Measurement Unit,IMU)是测量物体在三维空间中角速度及加速度的装置，IMU内部装有三轴陀螺仪和三轴加速度计，通过IMU输出的各个轴的角速度和加速度信息，积分求解载体的位姿，目前已广泛应用于增强现实、航空航天、机器人导航等领域。IMU具有较高的测量频率，并能在短时间内提供较高精度的位姿信息，但是随着运行时间的增加，IMU的漂移误差也会随之增加，影响位姿求解的精度。

[0004] 空间后方交会技术利用相机进行图像采集，容易受到光照、遮挡物、视角等的影响，特别是相机运动过程中出现的抖动会造成图像模糊，以致于无法实现特征点的识别与匹配。相较于相机，IMU具有更高的测量频率，快速运动状态下仍能在短时间内提供高精度的位姿信息。因此，IMU可以在运动过程中为相机提供准确的位姿补偿信息，而相机可以解决IMU长时间运行的误差漂移问题，两者具有较好的互补性。

发明内容

[0005] 本发明的目的是面向工业大尺度复杂环境动态位姿测量，提供一种基于惯性测量单元的后方交会动态位姿测量系统及方法。利用多个相机拍摄全局控制点，结合惯性测量单元的测量信息进行位姿补偿，并依据空间后方交会原理确定相机实时位姿的方法，以克服由于相机快速运动造成的控制点错误识别、错误匹配问题，在保证测量精度的前提下，提高后方交会动态位姿测量方法的鲁棒性和适用性。

[0006] 本发明的目的是通过以下技术方案实现的：

[0007] 基于惯性测量单元的后方交会动态位姿测量系统，包括至少两台相机、一个惯性测量单元和一个时钟同步模块，其中相机用于增大测量装置的视场范围，时钟同步模块用于实现相机和惯性测量单元之间的时间对准和数据同步。

[0008] 基于惯性测量单元的后方交会动态位姿测量方法，包括以下步骤：

[0009] (1)搭建动态位姿测量系统；

[0010] (2)标定各个相机之间的相对位置关系以及相机和惯性测量单元之间的相对位置关系；

[0011] (3)环境搭建：在测量环境中布置光学标记点作为全局控制点，测量相应标记点的三维坐标，建立三维控制场；

[0012] (4)初始状态确定：将动态位姿测量系统静止放置，同步触发各个相机采集图像，将经过图像处理后的像点与全局控制点进行一一对应，建立以控制点重投影误差最小为目标的优化函数，同时结合相机之间的相对位置关系建立约束方程，通过非线性最小二乘法求解相机位姿，进一步得到惯性测量单元在此位置下的位姿，并以当前状态作为动态位姿测量系统的初始状态；

[0013] (5)动态位姿更新：将初始状态设为0时刻，根据惯性测量单元的输出，对惯性测量单元位姿进行更新，得到t1时刻惯性测量单元的位姿，结合已经标定的相机和惯性测量单元的位置关系，得到t1时刻相机的估计位姿，并在此估计位姿的基础上将控制点投影到像平面上，得到估计像点，与相机采集到的真实像点进行比较，真实像点和距离其最近的估计像点对应同一个控制点，依据此判断方法实现真实像点和全局控制点的一一匹配，进一步根据步骤(4)中的方法计算相机在t1时刻的准确位姿，同理，可确定相机在ti(i＝1,2,3,…)时刻相对于全局坐标系的位置和姿态，实现动态位姿测量系统状态的自动更新。

[0014] 与现有技术相比，本发明的技术方案所带来的有益效果是：

[0015] 1.本发明采用多台相机采集图像，扩大了视场范围并提高了定位精度，有效地结合了相机和惯性测量单元的优势，克服了相机在快速运动过程中出现的图像匹配困难，同时避免了惯性测量单元IMU长时间运行的误差漂移问题，测量装置便携性好、适用性强，实现了在复杂环境下的动态位姿测量。

[0016] 2.受限于相机测量速率较低，现有的视觉测量方法在遮挡较为严重的复杂环境下受到极大限制，本发明一方面采用多台相机后方交会测量的方式，提升了系统的灵活性和可扩展性，另一方面加入惯性测量单元，利用其高频的测量信息对相机位姿进行补偿，消除了单纯依靠视觉测量导致的测量速度较慢、受光线遮挡限制的问题，提升了动态位姿测量能力。

附图说明

[0017] 图1是动态位姿测量系统的结构示意图。

[0018] 图2是本发明的工作环境示意图。

具体实施方式

[0019] 下面结合附图对本发明作进一步的描述。

[0020] 为了实时获取测量装置的空间位置和姿态，实现复杂环境下特别是局部受限空间内工作人员的跟踪定位，本发明提出了一种基于惯性测量单元的后方交会动态位姿测量装置和方法，采用多台相机作为图像采集单元，并将IMU短时间内的高精度位姿信息作为补偿，以解决相机快速运动状态下图像匹配困难的问题，其方法详见如下步骤：

[0021] (1)搭建动态位姿测量系统，其基本结构如图1所示，在一刚体工件6上安装至少两台相机，本实例中以三个相机为例，内参数已知的相机1、2、3，惯性测量单元(IMU)4以及时钟同步模块5，采用多个相机的结构可以显著增大系统的视场范围，时钟同步模块则用于实现相机和惯性测量单元(IMU)4之间的时间对准和数据同步；

[0022] (2)系统标定：精确标定相机1、2、3之间的外部参数，确定相机2坐标系O2-X2Y2Z2相对于相机1坐标系O1-X1Y1Z1的旋转矩阵R21和平移矩阵T21，以及相机3坐标系O3-X3Y3Z3相对于相机1坐标系O1-X1Y1Z1的旋转矩阵R31和平移矩阵T31，

[0023] X1＝R21·X2+T21＝R31·X3+T31

[0024] 并标定出IMU刚体坐标系O4-X4Y4Z4相对于相机1坐标系的旋转矩阵R41和平移矩阵T41，

[0025] X1＝R41·X4+T41

[0026] 同时，选取相机1坐标系作为整个测量装置的系统坐标系；

[0027] (3)环境搭建：在测量环境中布置光学标记点7作为全局控制点，测量各个光学标记点的三维坐标，建立三维控制场，定义控制场工作坐标系为OW-XWYWZW；

[0028] (4)初始状态确定：将测量装置静止放置，触发相机1、2、3同时采集图像，将经过图像处理后的像点与全局控制点进行一一对应，则每台相机可以根据空间后方交会原理列出共线条件约束方程，建立以控制点重投影误差最小为目标的优化函数，

[0029]

[0030] 其中，i代表相机编号，j代表控制点编号，P表示经过图像处理和畸变校正的像点坐标，O表示控制点的全局三维坐标，c代表相机镜头焦距，

[0031]

[0032] 分别表示工作坐标系相对于相机i坐标系的旋转矩阵和平移矩阵，结合相机之间的相对位置关系可以得到如下等式，

[0033]

[0034] 根据上述关系可以建立约束方程，

[0035]

[0036] 采用罚函数法即可构建最终的求解目标函数，

[0037]

[0038] M为惩罚因子，通过非线性最小二乘优化方法对目标函数进行求解即可得到未知量Ri和Ti的最优解，完成了系统的初始定位，此时可以进一步得到IMU在工作坐标系下的位姿，

[0039]

[0040] 以当前的状态作为系统的初始状态；

[0041] (5)动态位姿更新：将初始状态设定为0时刻，经过短时间Δt后定位装置位置发生改变，设此时为t1时刻，时钟同步装置对相机图像采集和IMU数据采集进行同步，根据IMU内部三轴陀螺仪和三轴加速度计Δt内的输出，对IMU状态进行更新，

[0042]

[0043] 得到t1时刻IMU在工作坐标系下的位置和姿态，

[0044]

[0045] 再结合已经标定的相机和IMU的位置关系，得到t1时刻各个相机的估计位姿，[0046]

[0047] 在此估计位姿的基础上将所有控制点投影到相机像平面上，得到对应的估计像点，并与相机拍摄到的真实像点进行比较，真实像点和距离其最近的估计像点对应同一个全局控制点，依据此判断方法即可实现真实像点和全局控制点的一一匹配，然后再次根据步骤(4)中采用的后方交会方法计算测量系统在t1时刻的准确位姿，

[0048]

[0049] 同理，可以确定测量系统在ti(i＝1,2,3,…)时刻相对于工作坐标系的位置和姿态，完成了测量系统位姿的自动更新。

[0050] 本发明并不限于上文描述的实施方式。以上对具体实施方式的描述旨在描述和说明本发明的技术方案，上述的具体实施方式仅仅是示意性的，并不是限制性的。在不脱离本发明宗旨和权利要求所保护的范围情况下，本领域的普通技术人员在本发明的启示下还可做出很多形式的具体变换，这些均属于本发明的保护范围之内。