CN105444687B - 基于对视摄像测量和激光测距的相对位姿变化测量方法 - Google Patents

Abstract

本发明主要利用摄像测量和激光测距技术高精度、简单快速的优点，测量两个刚体之间的相对位姿变化。本发明利用固联像机与激光测距仪构成测量单元，两个测量单元分别安装在各自的刚体上，通过相互对视拍摄测量刚体之间的相对位姿变化。本方法采用测量单元相互拍摄的方式，构成相对位姿变化测量的闭合回路，有效地提高了两个刚体之间的相对位姿变化测量精度。该方法不要求事先已知固联标志的坐标，而直接通过2个及以上标志点图像坐标的增量来解算相对变形量，简单高效。本发明可用于刚体之间的相对位姿变化实时测量，具有重要的理论研究意义和广泛的应用前景。

Description

基于对视摄像测量和激光测距的相对位姿变化测量方法

技术领域

本发明主要利用摄像测量和激光测距技术高精度、简单快速的优点，测量两个刚体之间的相对位姿变化。

背景技术

随着科学技术的发展，各个领域对测量相对位姿变化的需求越来越迫切，比如由于船体变形、地面沉降、外力作用等因素导致刚体基座之间的相对变形，从而影响刚体基座上测量设备的测量精度。因此开发高精度、稳定性及自动化的相对位姿变化测量系统有着重要的理论意义和应用价值。

近年来，基于计算机视觉技术的相对位姿变化测量方法发展迅速。单目位姿估计算法需要刚体具备3个以上已知结构的特征点，并由于相机成像模型等因素限制，单目位姿估计在长焦距、远距离测量条件下精度明显降低，特别是沿摄像机光轴方向上的位移测量精度一般远低于垂直光轴方向上的位移测量精度。为了提高沿摄像机光轴方向上的位移测量精度, 通过在沿摄像机光轴方向上加装一个高精度的激光测距传感器，组成单目摄像机-激光测距传感器位姿测量系统。但是单目摄像机-激光测距传感器位姿测量系统对待测目标结构存在要求，一般需要选择合作目标坐标系中的特殊表面（如平面等）来接收激光光斑，从而实现合作目标的相对位姿变化测量。在实际测量情况中，往往不具备合作目标，导致该方法无法有效实施。

发明内容

为解决上述问题，特提出一种基于对视摄像测量和激光测距的相对位姿变化测量方法。

本方法利用固联像机与激光测距仪构成测量单元，两个测量单元分别安装在各自的刚体上，创新性地通过相互对视拍摄测量刚体之间的相对位姿变化，其中激光测距仪用于高精度地测出两个刚体之间的平移向量，像机则用于测量两个刚体之间的角度变化。采用测量单元相互拍摄的方式，构成相对位姿变化测量的闭合回路，有效地提高了两个刚体之间的相对位姿变化测量精度。该方法不要求事先已知固联标志的坐标，而直接通过2个及以上标志点图像坐标的增量来解算相对变形量，简单高效。本发明可用于刚体之间的相对位姿变化实时测量，具有重要的理论研究意义和广泛的应用前景。

本发明要解决的技术问题是利用摄像测量技术和激光测距技术高精度快速的测量两刚体之间的相对姿态变化。

本发明的技术方案是：由激光测距仪和高速摄像机固联组成测量单元，两套测量单元构成相对位姿变化测量系统，如图1所示。事先在实验室内调整使激光测距仪出光点与摄像机光心位置尽量接近，并保证激光测距仪光束与摄像机的光轴基本平行。摄像机通过互相拍摄标志点和激光测距来测量摄像单元之间的相对形变。从刚体之间相对位姿变化来说，可以假定其中一个测量单元固定，另一个测量单元相对该固定单元发生形变。假设像机2所在的测量单元固定不动，像机1所在的测量单元相对像机2运动，即所有相对变形都发生在像机1所在的测量单元上。分为以下两步：首先利用激光测距仪高精度地测出两个刚体之间的平移向量，然后通过互视像机计算两个刚体之间的相对旋转角，从而获得刚体之间六自由度的相对位姿变化。

本发明提出的测量相对位姿变化方法按以下步骤进行:

1）通过对视2号像机观测对视1号像机光心的像点移动，结合激光测距信息得到对视1号像机在对视2号像机系中的平移量，然后利用标定时刻对视1号和对视2号像机的相对关系得到对视1号像机相对于自身的平移量；

2）再利用上一步中得到的对视1号像机平移量来对其所观测的与对视2号像机相固联的对顶角标志像点坐标进行补偿，从而抵消掉由平移运动产生的像点变化；

3）最后利用补偿后的对顶角标志像点坐标偏移来确定对视1号像机相对于自身的姿态变化量，并最终确定完整的相对变形。

2相对位姿变化测量的基本原理:

以下详细介绍刚体之间相对位姿变化的测量步骤：

1）假设发生变形以后对视1号像机的光心像点坐标偏移如图2所示，标定时刻的激光测距值为r₀，此时的激光测距值为r，由于激光测距仪的朝向与像机光轴基本一致，因此激光测距值的变化量dr=(r-r₀)基本上等于对视1号像机光心深度的变化量，则在已知光心像点坐标偏移(dx, dy)以及2号像机等效焦距f_{x_2}, f_{y_2}的情况下，容易推得对视1号像机光心在对视2号像机系中的平移量为

(1)

上式得到的还只是对视1号像机光心在对视2号像机系中的平移量，结合标定时刻1号和2号像机的相对关系就可以得到对视1号像机光心相对于自身坐标系的平移量，如下式

(2)

其中为标定时刻1号和2号像机之间的相对关系。

2）上一步中已经得到了对视1号像机相对于自身坐标系的平移量，可以想象，这一平移运动也可以看作是对视1号像机不动，而与对视2号像机固联的对顶角标志在1号像机系中平移了，因此对视1号像机自身的平移运动给这些对顶角标志在其图像上的像点带来的变化为

（3）

其中f_{x_1}, f_{x_1}为对视1号像机的等效焦距。为了抵消自身平移运动的影响，先在1号像机观测到的各对顶角标志像点坐标的基础上减去式（3）所示的像点偏移量以为补偿，然后再利用补偿后的对顶角像点坐标去进行后续的姿态变化量解算。

3）假定对视1号像机自身的姿态依次发生滚转、偏航和俯仰的变化，则会造成标志像点坐标发生的偏移

其中c为对视1号像机的主点。

像机的滚转角可以直接利用标定时刻标志像点坐标()和当前时刻补偿后的标志像点坐标()解算得到，当然仅仅利用单个点，比方说()来估计像机的滚转角显然是不准确的，因为像点还发生了平移，如图 3所示。但是利用两个点的连线()之间的夹角就可以排开像点平移的影响而单纯得到像机的滚转角。已知两个标志p₁和p₂在变形前后两个时刻的像点坐标，可以通过下式来计算像机的滚转角γ。

（4）

为了提高算法的抗噪能力，在实际进行解算时是取任意两个点变形前后两时刻的像点都算一个滚转角，然后算得的所有滚转角取平均值为最终的滚转角γ。

在得到像机滚转角的基础上，将未变形时刻的标志像点坐标绕主点旋转γ，再利用旋转之后的标志像点坐标()与当前时刻补偿后的标志像点坐标()来解算得到像机的偏航角α和俯仰角β。假设已知某标志点旋转之后的坐标，以及当前时刻观测的坐标，可以通过下式算得像机的偏航角α和俯仰角β：

（5）

为提高算法的抗噪能力，在实际中是利用所有标志像点平移量的均值来解算像机偏航角α和俯仰角β。

接着可按下列式得到对视1号像机相对于自身的旋转矩阵

（6）

（7）

再结合式(4)算得的光心平移，可以得到对视1号像机相对于自身的相对变形量

（8）

其中向量为式（2）中的。

3 本发明可以达到以下的技术效果：

1)非接触测量刚体之间的相对位姿变化，可用于修正补偿刚体的相对变形；

2) 可以实现实时测量，相对位姿变化测量结果精度高；

3) 不要求事先已知刚体固联标志的坐标，而直接通过2个及以上标志点图像坐标的增量来解算刚体之间的相对变形量，简单高效；

4)设备通用性强，方便使用，对待测刚体结构没有要求；

5)所需硬件设备成熟，成本较低，具有很强的工程实用性；

4 具体实施措施：

1）设计组装用于相对位姿变化测量的测量单元，其中测量单元由1个像机和1个激光测距仪固联构成。在实验室内调整使激光测距仪出光点与摄像机光心位置基本一致，并保证激光测距仪光束与摄像机的光轴平行；

2）根据测量现场要求，在需要测量的刚体上安装测量单元并绘制2个及以上标志点，标定出像机之间的相对位姿关系。两个测量单元相互拍摄，构成相对位姿变化测量系统；

3）测量系统中激光测距仪和像机同步采集数据，根据上述原理计算刚体之间六自由度的相对位姿变化。

附图说明

图1基于对视摄像测量和激光测距的相对位姿变化测量方法示意图，

图2像机光心像点坐标偏移，

图3像机自身姿态变化造成的标志像点偏移。

具体实施方式

本方法利用固联像机与激光测距仪构成测量单元，两个测量单元分别安装在各自的刚体上，创新性地通过相互对视拍摄测量刚体之间的相对位姿变化，其中激光测距仪用于高精度地测出两个刚体之间的平移向量，像机则用于测量两个刚体之间的角度变化。采用测量单元相互拍摄的方式，构成相对位姿变化测量的闭合回路，有效地提高了两个刚体之间的相对位姿变化测量精度。该方法不要求事先已知固联标志的坐标，而直接通过2个及以上标志点图像坐标的增量来解算相对变形量，简单高效。本发明可用于刚体之间的相对位姿变化实时测量，具有重要的理论研究意义和广泛的应用前景。

基于对视摄像测量和激光测距的相对位姿变化测量系统的具体实施方法按以下步骤进行：

设计组装用于相对位姿变化测量的测量单元，其中测量单元由1个像机和1个激光测距仪固联构成。在实验室内调整使激光测距仪出光点与摄像机光心位置基本一致，并保证激光测距仪光束与摄像机的光轴平行；

根据测量现场要求，在需要测量的刚体上安装测量单元并绘制2个及以上标志点，标定出像机之间的相对位姿关系。两个测量单元相互拍摄，构成相对位姿变化测量系统；

测量系统中激光测距仪和像机同步采集数据，根据上述原理计算刚体之间六自由度的相对位姿变化。

Claims (4)

1.基于对视摄像测量和激光测距的相对位姿变化测量方法，其特征在于，利用固联像机与激光测距仪构成测量单元，两个测量单元分别安装在各自的刚体上，通过相互对视拍摄测量刚体之间的相对位姿变化，其中激光测距仪用于测出两个刚体之间的平移向量，像机则用于测量两个刚体之间的角度变化，采用测量单元相互拍摄的方式，构成相对位姿变化测量的闭合回路，不要求事先已知固联标志的坐标，而直接通过2个及以上标志点图像坐标的增量来解算相对变形量，具体步骤如下：

1)通过对视2号像机观测对视1号像机光心的像点移动，结合激光测距信息得到对视1号像机在对视2号像机系中的平移量，然后利用标定时刻对视1号和对视2号像机的相对关系得到对视1号像机相对于自身的平移量；

2)再利用上一步中得到的对视1号像机平移量来对其所观测的与对视2号像机相固联的对顶角标志像点坐标进行补偿，从而抵消掉由平移运动产生的像点变化；

3)最后利用补偿后的对顶角标志像点坐标偏移来确定对视1号像机相对于自身的姿态变化量，并最终确定完整的相对变形H_11’。

2.如权利要求1所述基于对视摄像测量和激光测距的相对位姿变化测量方法，其特征在于，所述步骤1)具体内容为：

假设发生变形以后对视1号像机的光心像点坐标偏移，标定时刻的激光测距值为r₀，此时的激光测距值为r，由于激光测距仪的朝向与像机光轴基本一致，因此激光测距值的变化量dr＝(r-r₀)基本上等于对视1号像机光心深度的变化量，则在已知光心像点坐标偏移(dx,dy)以及2号像机等效焦距f_{x_2},f_{y_2}的情况下，推得对视1号像机光心在对视2号像机系中的平移量为

${\left(\begin{array}{c}dX\\ dY\\ dZ\end{array}\right)}_{c_2}=\left(\begin{array}{c}dx*r/f_{x\_2}\\ dy*r/f_{y\_2}\\ dr\end{array}\right)---\left(1\right)$

上式得到的还只是对视1号像机光心在对视2号像机系中的平移量，结合标定时刻1号和2号像机的相对关系就可以得到对视1号像机光心相对于自身坐标系的平移量，如下式

${\left(\begin{array}{c}{\mathrm{dX}}^{,}\\ {\mathrm{dY}}^{,}\\ {\mathrm{dZ}}^{,}\\ 0\end{array}\right)}_{c_1}=H_{c_2{c}_1}{\left(\begin{array}{c}dX\\ dY\\ dZ\\ 0\end{array}\right)}_{c_2}---\left(2\right)$

其中为标定时刻1号和2号像机之间的相对关系。

3.如权利要求1所述基于对视摄像测量和激光测距的相对位姿变化测量方法，其特征在于，所述步骤2)具体内容为：

步骤1)已经得到了对视1号像机相对于自身坐标系的平移量，这一平移运动也可以看作是对视1号像机不动，而与对视2号像机固联的对顶角标志在1号像机系中平移量(-dX’，-dY’，-dZ’)^T，因此对视1号像机自身的平移运动给这些对顶角标志在其图像上的像点带来的变化为

$\left\{\begin{array}{c}dx=-{\mathrm{dx}}^{,}*f_{x\_1}/r\\ dy=-{\mathrm{dy}}^{,}*f_{y\_1}/r\end{array}\right.---\left(3\right)$

其中f_{x_1},f_{y_1}为对视1号像机的等效焦距，为抵消自身平移运动的影响，先在1号像机观测到的各对顶角标志像点坐标的基础上减去式(3)所示的像点偏移量以为补偿，然后再利用补偿后的对顶角像点坐标去进行后续的姿态变化量解算。

4.如权利要求1所述基于对视摄像测量和激光测距的相对位姿变化测量方法，其特征在于，所述步骤3)具体内容为:

假定对视1号像机自身的姿态依次发生滚转、偏航和俯仰的变化，则会造成标志像点坐标发生的偏移，

其中c为对视1号像机的主点,

像机的滚转角可以直接利用标定时刻标志像点坐标p₁(x₁，y₁)和当前时刻补偿后的标志像点坐标p”₁(x”₁，y”₁)解算得到，当然仅仅利用单个点，比方说(p₁，p”₁)来估计像机的滚转角显然是不准确的，因为像点还发生了平移，但是利用两个点的连线(p₁→p₂，p″₁→p″₂)之间的夹角就可以排开像点平移的影响而单纯得到像机的滚转角，已知两个标志p₁和p₂在变形前后两个时刻的像点坐标，通过下式来计算像机的滚转角γ，

$\left\{\begin{array}{c}dx=x_2-x_1\\ dy=y_2-y_1\\ {\mathrm{dx}}^{,,}=x_2^{,,}-x_1^{,,}\\ {\mathrm{dy}}^{\′,}=y_2^{,,}-y_1^{,,}\\ \mathrm{\γ}={\tan }^{-1}\left(\right({\mathrm{dxdy}}^{\′,}-{\mathrm{dx}}^{,,}dy)/({\mathrm{dxdx}}^{,,}+{\mathrm{dydy}}^{,,}\left)\right)\end{array}\right.---\left(4\right)$

为提高算法的抗噪能力，在实际进行解算时是取任意两个点变形前后两时刻的像点都算一个滚转角，然后算得的所有滚转角取平均值为最终的滚转角γ，

在得到像机滚转角的基础上，将未变形时刻的标志像点坐标绕主点旋转γ，再利用旋转之后的标志像点坐标p’_i(x’₁，y’₁)与当前时刻补偿后的标志像点坐标p”_i(x”₁，y”₁)来解算得到像机的偏航角α和俯仰角β，假设已知某标志点旋转之后的坐标p’_i，以及当前时刻观测的坐标p”_i，可以通过下式算得像机的偏航角α和俯仰角β：

$\left\{\begin{array}{c}\mathrm{\α}={\tan }^{-1}\left(\right(x_i^{,,}-x_i^{,})/f_{x\_1})\\ \mathrm{\β}={\tan }^{-1}(-(y_i^{,,}-y_i^{,})/f_{y\_1})\end{array}\right.---\left(5\right)$

为提高算法的抗噪能力，在实际中是利用所有标志像点平移量的均值来解算像机偏航角α和俯仰角β，

接着按下列式得到对视1号像机相对于自身的旋转矩阵

$R_x\left(\mathrm{\β}\right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\β}& -\sin \mathrm{\β}\\ 0& \sin \mathrm{\β}& \cos \mathrm{\β}\end{array}\right]$

$R_Y\left(\mathrm{\α}\right)=\left[\begin{array}{ccc}cos\mathrm{\α}& 0& sin\mathrm{\α}\\ 0& 1& 0\\ -sin\mathrm{\α}& 0& cos\mathrm{\α}\end{array}\right]$

$R_Z\left(\mathrm{\γ}\right)=\left[\begin{array}{ccc}\cos \mathrm{\γ}& -\sin \mathrm{\γ}& 0\\ \sin \mathrm{\γ}& \cos \mathrm{\γ}& 0\\ 0& 0& 1\end{array}\right]---\left(6\right)$

R_11’＝R_X(β)R_Y(a)R_Z(γ) (7)

再结合式(4)算得的光心平移，得到对视1号像机相对于自身的相对变形量H_11’

其中向量dX’为式(2)中的(dX’，dY’，dZ’)^T。