CN102252653B - 基于tof无扫描三维成像的位姿测量方法 - Google Patents

Abstract

本发明公开了一种基于TOF无扫描三维成像的位姿测量方法，其特征是首先对目标物体建立一个三维的坐标信息数据库，利用TOF相机作为成像和距离数据获取装置，在拍摄的图片中选取三个可识别的物体作为标记点，在数据库中获得各标记点在目标本体坐标系下的坐标信息，建立一个向量组；通过TOF相机获得的数据信息，由非迭代三维空间两点间距离计算方法得出光心到各标记点的距离，经过计算得到标记点在相机坐标系下的坐标，再建立另一个向量组，由两个向量组之间的关系可以计算得出旋转矩阵和平移矩阵，从而获得姿态角和平移量，即目标物体的相对姿态。本发明避免了大量的迭代算法，并且可以快速求解满足位姿参数确定精度的要求。

Description

基于TOF无扫描三维成像的位姿测量方法

技术领域

本发明是基于TOF无扫描三维成像的位姿测量方法，属于三维视觉的目标位姿测量领域。

背景技术

物体位置与姿态的测量在航空航天、汽车车轮定位等领域都有着重要的应用价值，在很多应用领域必须对位姿进行测量后才能进行后续工作。

现有的一些技术有基于单目视觉的位姿测量方法和基于双目视觉的位姿测量方法，以及一些在此基础上加以改进和优化的方法。

基于单目视觉方法是利用几个者共面或非共面的特征光标点和单个CCD相机，通过获取特征光标物理坐标和对应图像点图像坐标之间的简化数学模型，以解析的形式给出了观察物体和目标物体之间的相对位姿参数，并对解析解进行修正；基于双目视觉是利用视差原理并利用成像设备从不同的位置获取被测物体的两幅图像，通过计算图像对应点间的位置偏差，来获取物体三维几何信息，计算目标物体的相对位姿；在此基础上加以改进和优化的方法，原理上没有大的改变，只是改变标记点的设置位置和一些参数，以达到提高计算精度和鲁棒性等效果。

以上方法在具体实现上存在以下不足：

1、单目算法中的基本方程为二次非线性方程，无直接解析解，需迭代求数值解，因此迭代求解的收敛性和速度将极大影响测量的实时性和准确性。

2、对于双目立体视觉来说，由于各种测量误差的存在，使得各个特征点空间坐标值的计算值与实际值间存在一定的误差，这样特征点间的位置关系不再满足模型的约束，位姿计算的刚性条件得不到保证，计算结果的误差会很大。

3、多数方法都是迭代求解，出现多值结果，带来识别的困难，迭代求解的收敛性和收敛速度将极大影响测量精度，而迭代过程的收敛性和收敛速度主要取决于迭代算法和初值选取。

发明内容

本发明针对上述现有技术所存在的不足，提出一种基于TOF的无扫描三维成像的位姿测量方法，避免了大量的迭代算法，并且可以快速的求解满足位姿参数确定精度的要求。

本发明解决技术问题采用如下技术方案：

本发明基于TOF无扫描三维成像的位姿测量方法的特点是包括以下步骤：

1)通过TOF相机拍摄目标物体，选取目标物体的成像图像中三个可识别物体为标记点P_i，i＝1，2，3，并从事先建立的目标物体三维坐标信息数据库中获得标记点P_i在目标本体坐标系下的坐标P_i＝(X_i，Y_i，Z_i)，i＝1，2，3，通过任意两个标记点的连线求出两个线性无关的单位列向量

和

令

为

所构成的单位列向量，构造出三个标记点P_i在目标本体坐标系下的向量组： $n_o=\left[\begin{array}{ccc}\stackrel{\→}{n_{o1}}& \stackrel{\→}{n_{o2}}& \stackrel{\→}{n_{o3}}\end{array}\right];$

2)通过TOF相机拍摄目标物体获得的标记点P_i的像点在像坐标系下的像素坐标计算出标记点P_i在相机坐标系下的坐标S_i，所述标记点P_i在相机坐标系下的坐标S_i计算方法为：

a.将标记点P_i的像点在像坐标系下的像素坐标(u_i，v_i)转化为像点在相机坐标系下的坐标；

b.由标记点P_i的像点在相机坐标系下的坐标确定出TOF相机光心到标记点P_i的方向向量，获得单位方向向量

c.由TOF相机测得标记点P_i到像平面的垂直距离L_i，并通过非迭代三维空间两点间距离方法得出TOF相机光心到各标记点P_i的距离D_i；

d.由式(1)求出各标记点P_i在相机坐标系下的坐标S_i，i＝1，2，3；

$S_i{=D}_i\stackrel{\→}{a_i}---\left(1\right)$

通过任意两个标记点P_i的连线求出两个线性无关的单位列向量

和

令

为所构成的单位列向量；构造出三个标记点P_i在相机坐标系下的向量组： $n_s=\left[\begin{array}{cc}\stackrel{\→}{n_{s1}}& \stackrel{\→}{n_{s2}}\end{array}\right.$ $\stackrel{\→}{n_{s3}}];$

3)由向量组 $n_o=\left[\begin{array}{ccc}\stackrel{\→}{n_{o1}}& \stackrel{\→}{n_{o2}}& \stackrel{\→}{n_{o3}}\end{array}\right]$ 和 $n_s=\left[\begin{array}{ccc}\stackrel{\→}{n_{s1}}& \stackrel{\→}{n_{s2}}& \stackrel{\→}{n_{s3}}\end{array}\right]$ 通过式(2)式(3)的运算可得出旋转矩阵R和平移矩阵T；

R＝n_sn_o ^-1(2)

T＝S_i-RP_i(3)

4)将旋转矩阵 $R=\left(\begin{array}{ccc}{r}_{00}& r_{01}& r_{02}\\ r_{10}& r_{11}& r_{12}\\ r_{20}& r_{21}& r_{22}\end{array}\right)$ 通过式(4)计算获得绕相机坐标系的x轴、y轴和z轴沿逆时针方向的转角α、β和γ；旋转顺序是从x轴到y轴再到z轴；所述转角α、β和γ为目标物体相对于相机坐标系的三个姿态角；

$\left\{\begin{array}{c}\mathrm{\γ}=-\arctan \frac{r_{01}}{r_{00}}\\ \mathrm{\β}=\arctan \frac{r_{20}}{r_{00}\cos \mathrm{\γ}-r_{10}\sin \mathrm{\γ}}\\ \mathrm{\α}=\arctan \frac{r_{02}\sin \mathrm{\γ}+r_{12}\cos \mathrm{\γ}}{r_{01}\sin \mathrm{\γ}+r_{11}\cos \mathrm{\γ}}\end{array}\right.---\left(4\right)$

5)将平移矩阵 $T=\left(\begin{array}{c}{t}_0\\ t_1\\ t_2\end{array}\right)$ 通过式(5)计算获得目标本体坐标系和相机坐标系之间的距离d，其中t₀、t₁和t₂分别为目标物体相对于相机坐标在x轴、y轴和z轴上的平移量；

$d=\sqrt{t_0^2+t_1^2+t_2^2}---\left(5\right).$

本发明基于TOF无扫描三维成像的位姿测量方法的特点也在于步骤2)中所述非迭代三维空间两点间距离方法是TOF相机通过记录光线在空间中的往返时间，获取场景的三维信息，直接获得标记点P_i到像平面的垂直距离L_i，即P_iQ_i＝L_i，而且OO′＝f和O′P′_i＝m_i为已知，由相似三角形得：

$P_i^{\′}{Q}_i=\frac{L_i\·m_i}{f}---\left(6\right)$

再由勾股定理求得TOF相机光心到标记点P_i的距离D_i：

$D_i=\sqrt{{\left(\frac{L_i\·m_i}{f}\right)}^2+{L_i}^2}+\sqrt{f^2+{m_i}^2}---\left(7\right)$

其中O为TOF相机光心，O′为像平面中心点，P′_i为标记点P_i在像平面的像点，Q_i为标记点P_i到像平面的垂足，f为相机的焦距，L_i为TOF相机实时测得的标记点P_i到像平面的垂直距离，m_i为O′点与P′_i之间的距离。

与现有技术相比，本发明的有益效果体现在：

1、本发明采用的TOF相机是通过记录光线在空间中的往返时间，获取场景的三维信息。在获取目标物体上标记点距相机光心的距离时不需要进行复杂的计算，具有速度快，计算量小，精度高等特点。

2、本发明采用预先对目标物体进行三维信息的采集，并建立目标物体上可识别物体在目标本体坐标系下的三维坐标信息数据库，不需要预先设定标记点，在拍摄过程中可以随即选取可识别物作为标记点，而不需要寻找预设标记点，这样具有更强的实际应用效果。

3、本发明采用直接将标记点定义在目标本体坐标系上，所以在位姿的测量中可以不需要建立标记点坐标系，减少了一个标记点坐标系和目标坐标系之间的转换矩阵和平移矩阵，使计算更加简化。

4、TOF相机中的测距成像仪是一种主动式视觉传感器，包含有一个主动调制光源，所以测量时不受外界光照条件限制。

附图说明

图1本发明方法的测量系统图：

图1中标号：1目标物体；2为TOF相机；3数据处理系统；

图2本发明方法的系统流程图；

图3本发明方法中目标本体坐标系、相机坐标系和像坐标系之间的关系；

图4本发明方法中的TOF相机光心到标记点的距离计算几何图示。

具体实施方式

参见图1，本实施例系统构成包括目标物体1、TOF相机2和数据处理系统3。

参见图2，本实施例中基于TOF无扫描三维成像的位姿测量方法的工作流程为：

1)通过TOF相机拍摄目标物体，选取目标物体的成像图像中三个可识别物体为标记点P_i，i＝1，2，3，并从事先建立的目标物体三维坐标信息数据库中获得标记点P_i在目标本体坐标系下的坐标P_i＝(X_i，Y_i，Z_i)，i＝1，2，3，通过任意两个标记点的连线求出两个线性无关的单位列向量

和

令

为

所构成的单位列向量，构造出三个标记点P_i在目标本体坐标系下的向量组： $n_o=\left[\begin{array}{ccc}\stackrel{\→}{n_{o1}}& \stackrel{\→}{n_{o2}}& \stackrel{\→}{n_{o3}}\end{array}\right],$ 目标本体坐标系是以目标物体的质心为坐标原点，如图3建立目标本体坐标系O_o-X_oY_oZ_o；

2)通过TOF相机拍摄目标物体获得的标记点P_i的像点在像坐标系下的像素坐标计算出标记点P_i在相机坐标系下的坐标S_i，相机坐标系和像坐标系如图3所示，建立相机坐标系O_s-X_sY_sZ_s。其原点O_s为摄像机光心，O_s到像平面的距离称为摄像机的焦距f，Y_s轴为主光轴方向，X_s轴与像坐标系横方向u平行，Z_s轴与像坐标系竖直方向v平行，并与其它两轴构成右手系；O_i-X_iZ_i表示像坐标系，其原点O_i定义为相机光轴与像平面的交点，X_i轴和Z_i轴分别与图像像素的列数u与行数v平行，标记点P_i在相机坐标系下的坐标S_i计算方法为：

a.将标记点P_i的像点在像坐标系下的像素坐标(u_i，v_i)转化为像点在相机坐标系下的坐标；

b.由标记点P_i的像点在相机坐标系下的坐标确定出TOF相机光心到标记点P_i的方向向量，获得单位方向向量

c.由TOF相机测得标记点P_i到像平面的垂直距离L_i，并通过非迭代三维空间两点间距离方法得出TOF相机光心到各标记点P_i的距离D_i，如图4所示：取i＝1，以标记点P₁为例，TOF相机可以直接测得标记点P₁到像平面的垂直距离P₁Q₁＝L₁，OO′＝f，O′P′₁＝m₁已知，有相似三角形可知

由勾股定理可得 $D_1=\sqrt{{\left(\frac{L_1\·m_1}{f}\right)}^2+{L_1}^2}+\sqrt{f^2+{m_1}^2}.$

d.由式(1)求出各标记点P_i在相机坐标系下的坐标S_i，i＝1，2，3；

$S_i{=D}_i\stackrel{\→}{a_i}---\left(1\right)$

通过任意两个标记点P_i的连线求出两个线性无关的单位列向量和

令

为

所构成的单位列向量；构造出三个标记点P_i在相机坐标系下的向量组： $n_s=\left[\begin{array}{cc}\stackrel{\→}{n_{s1}}& \stackrel{\→}{n_{s2}}\end{array}\right.$ $\stackrel{\→}{n_{s3}}];$

3)由向量组 $n_o=\left[\begin{array}{ccc}\stackrel{\→}{n_{o1}}& \stackrel{\→}{n_{o2}}& \stackrel{\→}{n_{o3}}\end{array}\right]$ 和 $n_s=\left[\begin{array}{ccc}\stackrel{\→}{n_{s1}}& \stackrel{\→}{n_{s2}}& \stackrel{\→}{n_{s3}}\end{array}\right]$ 通过式(2)式(3)的运算可得出旋转矩阵R和平移矩阵T；

R＝n_sn_o ^-1(2)

T＝S_i-RP_i(3)

4)将旋转矩阵 $R=\left(\begin{array}{ccc}{r}_{00}& r_{01}& r_{02}\\ r_{10}& r_{11}& r_{12}\\ r_{20}& r_{21}& r_{22}\end{array}\right)$ 通过式(4)计算获得绕相机坐标系的x轴、y轴和z轴沿逆时针方向的转角α、β和γ；旋转顺序是从x轴到y轴再到z轴；转角α、β和γ为目标物体相对于相机坐标系的三个姿态角；

$\left\{\begin{array}{c}\mathrm{\γ}=-\arctan \frac{r_{01}}{r_{00}}\\ \mathrm{\β}=\arctan \frac{r_{20}}{r_{00}\cos \mathrm{\γ}-r_{10}\sin \mathrm{\γ}}\\ \mathrm{\α}=\arctan \frac{r_{02}\sin \mathrm{\γ}+r_{12}\cos \mathrm{\γ}}{r_{01}\sin \mathrm{\γ}+r_{11}\cos \mathrm{\γ}}\end{array}\right.---\left(4\right)$

5)将平移矩阵 $T=\left(\begin{array}{c}{t}_0\\ t_1\\ t_2\end{array}\right)$ 通过式(5)计算获得目标本体坐标系和相机坐标系之间的距离d，其中t₀、t₁和t₂分别为目标物体相对于相机坐标在x轴、y轴和z轴上的平移量。

$d=\sqrt{t_0^2+t_1^2+t_2^2}---\left(5\right).$

Claims (2)

1.一种基于TOF无扫描三维成像的位姿测量方法，其特征在于包括以下步骤：

1）通过TOF相机拍摄目标物体，选取目标物体的成像图像中三个可识别物体为标记点P_i，i=1,2,3，并从事先建立的目标物体三维坐标信息数据库中获得标记点P_i在目标本体坐标系下的坐标P_i=(X_i,Y_i,Z_i)，i=1,2,3，通过任意两个标记点的连线求出两个线性无关的单位列向量

和

令

为

所构成的单位列向量，构造出三个标记点P_i在目标本体坐标系下的向量组： $n_o=\left[\begin{array}{ccc}{\stackrel{\→}{n}}_{o1}& {\stackrel{\→}{n}}_{o2}& {\stackrel{\→}{n}}_{o3}\end{array}\right];$

2）通过TOF相机拍摄目标物体获得的标记点P_i的像点在像坐标系下的像素坐标计算出标记点P_i在相机坐标系下的坐标S_i，所述标记点P_i在相机坐标系下的坐标S_i计算方法为：

a．将标记点P_i的像点在像坐标系下的像素坐标(u_i,v_i)转化为像点在相机坐标系下的坐标；

b．由标记点P_i的像点在相机坐标系下的坐标确定出TOF相机光心到标记点P_i的方向向量，获得单位方向向量

c．由TOF相机测得标记点P_i到像平面的垂直距离L_i，并通过非迭代三维空间两点间距离方法得出TOF相机光心到各标记点P_i的距离D_i；

d．由式（1）求出各标记点P_i在相机坐标系下的坐标S_i，i=1,2,3；

$S_i=D_i{\stackrel{\→}{a}}_i---\left(1\right)$

通过任意两个标记点P_i的连线求出两个线性无关的单位列向量

和

令

为

所构成的单位列向量；构造出三个标记点P_i在相机坐标系下的向量组： $n_s=\left[\begin{array}{cc}{\stackrel{\→}{n}}_{s1}& {\stackrel{\→}{n}}_{s2}\end{array}\right.$ $\left.\begin{array}{c}{\stackrel{\→}{n}}_{s3}\end{array}\right];$

3）由向量组 $n_o=\left[\begin{array}{ccc}{\stackrel{\→}{n}}_{o1}& {\stackrel{\→}{n}}_{o2}& {\stackrel{\→}{n}}_{o3}\end{array}\right]$ 和 $n_s=\left[\begin{array}{ccc}{\stackrel{\→}{n}}_{s1}& {\stackrel{\→}{n}}_{s2}& {\stackrel{\→}{n}}_{s3}\end{array}\right]$ 通过式（2）式（3）的运算可得出旋转矩阵R和平移矩阵T；

R=n_sn_o ^-1         （2）

T=S_i-RP_i         （3）

4）将旋转矩阵 $R=\left(\begin{array}{ccc}{r}_{00}& r_{01}& r_{02}\\ r_{10}& r_{11}& r_{12}\\ r_{20}& r_{21}& r_{22}\end{array}\right)$ 通过式（4）计算获得绕相机坐标系的x轴、y轴和z轴沿逆时针方向的转角α、β和γ；旋转顺序是从x轴到y轴再到z轴；所述转角α、β和γ为目标物体相对于相机坐标系的三个姿态角；

$\left\{\begin{array}{c}\mathrm{\γ}=-\arctan \frac{r_{01}}{r_{00}}\\ \mathrm{\β}=\arctan \frac{r_{20}}{r_{00}\cos \mathrm{\γ}-r_{10}\sin \mathrm{\γ}}\\ \mathrm{\α}=\arctan \frac{r_{02}\sin \mathrm{\γ}+r_{12}\cos \mathrm{\γ}}{r_{01}\sin \mathrm{\γ}+r_{11}\cos \mathrm{\γ}}\end{array}\right.---\left(4\right)$

5）将平移矩阵 $T=\left(\begin{array}{c}{t}_0\\ t_1\\ t_2\end{array}\right)$ 通过式（5）计算获得目标本体坐标系和相机坐标系之间的距离d，其中t₀、t₁和t₂分别为目标物体相对于相机坐标在x轴、y轴和z轴上的平移量；

$d=\sqrt{t_0^2+t_1^2+t_2^2}---\left(5\right);$

所述目标本体坐标系是以目标物体的质心为坐标原点；

所述像坐标系是以相机光轴与像平面的交点为坐标原点；

所述相机坐标系是以摄像机光心为坐标原点。

2.根据权利要求1所述的基于TOF无扫描三维成像的位姿测量方法，其特征的是步骤2)中所述非迭代三维空间两点间距离方法是TOF相机通过记录光线在空间中的往返时间，获取场景的三维信息，直接获得标记点P_i到像平面的垂直距离L_i，即P_iQ_i=L_i，而且OO′=f和O′P_i′=m_i为已知，由相似三角形得：

$P_i^{\′}{Q}_i=\frac{L_i\·m_i}{f}---\left(6\right)$

再由勾股定理求得TOF相机光心到标记点P_i的距离D_i：

$D_i=\sqrt{{\left(\frac{L_i\·m_i}{f}\right)}^2+L_i^2}+\sqrt{f^2+m_i^2}---\left(7\right)$

其中O为TOF相机光心，O′为像平面中心点，P_i′为标记点P_i在像平面的像点，Q_i为标记点P_i到像平面的垂足，f为相机的焦距，L_i为TOF相机实时测得的标记点P_i到像平面的垂直距离，m_i为O′点与P_i′之间的距离。

Images