CN100582653C - 一种采用多束光确定位置姿态的系统和方法 - Google Patents

Abstract

本发明涉及一种采用多束光确定位置姿态的系统和方法，属于人机交互技术领域。本发明的系统包括基准平面、需要确定位姿的物体、固定在物体上的光束发射装置、摄像机、计算装置，其中基准平面为位置和姿态已知的光滑平面，并具有反射光束发射装置发出的光的功能；光束发射装置固定在需要确定位姿的物体上，并向基准平面发射三束或以上光束；摄像机对包括投射图案在内的基准平面的原始图像进行采集，并传输给计算装置；计算装置获取原始图像进行图像处理，并进行光束投射图案的位置计算，进而完成需要确定位姿的物体的位置和姿态的计算。本发明还提出了一种使用所述系统确定物体位置姿态的方法。该方法具有非接触测量、造价低廉、实用性强等优点。

Description

一种采用多束光确定位置姿态的系统和方法

技术领域

本发明涉及一种物体相对于已知基准平面的位置姿态确定系统和方法，具体涉及一种采用多束光确定位置姿态的系统和方法，属于人机交互技术领域。

背景技术

三维空间中的定位的方法从几何方面基本可以分为三类：三边法、三角法、导线法，这些方法需要对两点间的距离或两边的夹角进行测量。而测量的方法目前主要有机械定位法、超声波定位法、电磁定位法和光学定位法等。机械定位法一般采用可变长度的线性装置连接定位与被定位物体，例如，一种飞行模拟器的定位方法是通过测量支撑模拟舱的各条腿的长度，然后通过公式计算舱的6自由度的位置和姿态。

使用无线电波、红外线和超音波等介质的传输信号也可以用来测量距离或夹角。常用的测量方法有以下几种：抵达时间(Time-of-Arrival：TOA)、抵达时间差(Time-Difference-of-Arrival：TDOA)、信号强度测距(Signal StrengthRanging：SSR)、近场区电磁测距(Near-Field EM Ranging：NFER)、抵达角度(Angle-of-Arrival：AOA)。

光学定位法是其中定位精度最高、使用最方便的一种定位方法，一种机器人六自由度柔性腕力传感器就是采用红外发光管与线性PSD(Position SensitiveDetector位置敏感元件)对进行位移检测并进行位置姿态解算。美国专利局公开过一个姿态和高度系统(专利号：3352223)，该系统用三束激光确定带有装置的太空船相对于没有装置的平面或月球表面的姿态和高度。但这个装置只能获取横滚和俯仰的角度，不能确定太空船方位及经度、纬度方面的数据。

在很多情况下，需要确定一个物体相对于另一个物体的空间关系，即包括位置和姿态的六个自由度的参数信息。

发明内容

本发明的主要目的是针对现有技术的局限性，提供一种确定三维空间中一个物体相对于一个位置姿态已知的平面的空间六个自由度，即位置和姿态的方法。

为了解决上述技术问题，本发明是通过以下技术方案实现：

系统由基准平面、需要确定位姿的物体、固定在物体上的光束发射装置、摄像机、计算装置等部分组成。

基准平面为位置和姿态已知的光滑平面，需要确定位置的物体以其为参考。同时，基准平面具有反射光束发射装置发出的光的功能，光束投射在上面形成投射图案，供摄像机采集。

需要确定位置和姿态的物体是要依照基准平面来确定位置和姿态的对象。

光束发射装置是固定在需要确定位姿的物体上的，并随着物体的运动而运动，他们之间没有相对运动，光束发射装置可以发射至少三束光，并且三束光与物体的位置和方向的关系为已知。

摄像机包括镜头、滤光片、传感装置以及图像传输装置等，主要完成对包括投射图案在内的基准平面的原始图像采集，并传输给计算装置以进行进一步的处理和计算。

计算装置可以是通用计算机，也可以是专用的处理器，它从摄像机获取原始图像进行图像处理，并进行光束投射图案的位置计算，进而完成需要确定位姿的物体的位置和姿态的计算。

本发明的方法分为如下步骤：

(1)加装光束发射装置

该装置的作用是发出多束光并投射在基准平面上形成投射图案。在需要确定位置姿态的物体上附加光束发射装置，该装置可以发射三个以上的光束，且光束的夹角为已知，这些光束投射到一个位置姿态已知的基准平面上。

(2)摄像机标定

摄像机标定就是在摄像机获得的投射图案的位置与基准平面上的点的位置建立映射关系。在基准平面上建立坐标系，将摄像机与基准平面的位置固定并使得基准平面在摄像机的摄像范围内，然后对摄像机进行标定。如果摄像机与基准平面的位置或姿态有变化，需要重新标定。

(3)获取图像

该步骤的主要作用是通过已经标定的摄像机来获取带有投射图案的图像并传输给计算机。获取图像可以单幅获取，也可以连续获取，获取图像后传给计算机，也可以进行简单的图像处理后传送。

(4)图像处理

该步骤的作用是完成各投射图案在基准平面里的位置确定。处理可以采用颜色过滤、高斯过滤、二值化、形态滤波等图像处理方法识别各个投射图案，并根据标定的结果确定各投射图案的坐标。

(5)位置和姿态计算

该步骤是根据已经获取的投射图案的位置确定物体相对于基准平面的位置和姿态。首先根据各投射图案的位置及光束的夹角计算出发射装置(即物体)到各个投射图案的连线的长度，进而计算出物体相对于基准平面的位置和姿态。

本发明还可以通过与发射光颜色相同或相近滤光片来过滤其他光对投射图案的干扰，提高识别效果。本发明还可以通过不同颜色或不同形状的光束来分辨光束与平面不同的投射图案。

与现有的技术相比，本发明有如下特色和优点：

(1)采用三束或以上光束确定物体的位置和姿态。

(2)采用单摄像机进行图像获取。

(3)可以实现空间六自由度的位置姿态确定。

(4)采用非接触测量距离，对需要确定位置姿态关系的双方的机械等特性方面无影响或影响较小。

(5)可以采用通用设备，造价比较低廉，实用性强。

附图说明

图1是本系统组成图，其中1-计算机，2-需要定位物体，3-光束，4-光束在平面投射图案，5-基准平面，6-摄像机。

图2.对称式三束光线位姿确定系统示意图，其中A、B、C为三束光SA、SB、SC与基准平面的投射图案，S为需要定位物体的发射光线的汇聚点，S’为物体轴心与基准平面的投射图案。

具体实施方式

下面以三束光为例，结合附图对本发明进行详细说明。

图1是本系统的组成图，本系统由基准平面5、装有能够发射光束装置的物体2、采集视频信号的摄像机6、进行图像处理与计算的计算机1、物体发射的光束3以及光束与平面的投射图案4组成，这里投射图案为圆点。按右手定则建立如图1所示的参考坐标系OXYZ，原点为基准平面的左上角，OX轴水平向右，OY垂直向下，OZ垂直平面向物体相反的方向。在图2上按照右手定则建立物体坐标系O’X’Y’Z’，其中，O’X’||OX，O’Y’||OY，O’Z’||OZ。

光束发射装置要保证三束光SA、SB、SC共点S，令光束之间的夹角相等并等于α，即：

∠ASB＝∠BSC＝∠CSA＝α

(1)

同时，令三束光线呈中心对称分布，即∠ASS’＝∠BSS’＝∠CSS’。基准平面为有限大小，摄像机的安装位置要保证基准平面在摄像机的取景范围内，光束在基准平面的投射图案处亮度应比基准平面其他位置亮度高。

确定物体空间六个自由度参数(即位置姿态)需要：摄像机标定、图像预处理、投射图案的识别、计算物体的位置和姿态角四个步骤。

摄像机标定

摄像机标定是为了建立摄像机拍摄的图像的点坐标和基准平面内点坐标之间的映射关系。这里采用的是基于标定物的方法，标定物为二维基准平面，为了降低复杂性，采用线性模型摄像机的标定方法，也就是针孔模型来标定。设向量[x，y，z]^T和[u，v]^T分别表示三维空间中的一点在世界坐标系和以像素为单位的摄像机所获取的图像坐标系下的坐标，则有：

$\text{\ρ}\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{cccc}\frac{f}{\mathrm{dx}}& 0& u_0& 0\\ 0& \frac{f}{\mathrm{dy}}& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]=\left[\begin{array}{cccc}{m}_{11}& m_{12}& m_{13}& m_{14}\\ m_{21}& m_{22}& m_{23}& m_{24}\\ m_{31}& m_{32}& m_{33}& m_{34}\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$

(2)

其中，ρ是一个常数，(R，t)是世界坐标系相对于摄像机坐标系的旋转矩阵和平移向量，即摄像机外参数。f是摄像机的焦距，(u₀，v₀)是摄像机光心在图像坐标系下的坐标，(dx，dy)是一个像素在x轴和y轴方向上的物理尺寸，即摄像机的内参数。

因为坐标建立在基准平面上，并且，光束的投射图案也在平面上，所以，这里只需要求取摄像机图像平面和基准平面间的摄影变换关系，而基准平面为X-Y平面(Z＝0)，式(2)可变换为：

$\mathrm{\ρ}\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{cccc}{m}_{11}& m_{12}& m_{13}& m_{14}\\ m_{21}& m_{22}& m_{23}& m_{24}\\ m_{31}& m_{32}& m_{33}& m_{34}\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]=\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{14}\\ m_{21}& m_{22}& m_{24}\\ m_{31}& m_{32}& m_{34}\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=M\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$

(3)

上式包括三个方程，化简后得到：

$\left\{\begin{array}{c}{m}_{11}.x+m_{12}.y+m_{14}-u.m_{31}.x-u.m_{32}.y=u.m_{34}\\ m_{21}.x+m_{22}.y+m_{24}-v.m_{31}.x-v.m_{32}.y=v.m_{34}\end{array}\right.$

(4)

如果已知基准平面上n个点的坐标(x_i，y_i)(i＝1，...，n)以及相应图像坐标(u_i，v_i)(i＝1，...，n)，则可以构成关于m_ij的2n个线性方程，如下：

$\left[\begin{array}{cccccccc}{x}_l& u_l& 1& 0& 0& 0& -u_l\·x_l& -u_l\·y_l\\ 0& 0& 0& x_l& y_l& 1& -v_l\·x_l& -v_l\·y_l\\ ...& ...& ...& ...& ...& ...& ...& ...\\ x_n& y_n& 1& 0& 0& 0& -u_n\·x_n& -u_n\·y_n\\ 0& 0& 0& x_n& y_n& 1& -v_n\·x_n& -v_n\·y_n\end{array}\right]\left[\begin{array}{c}{m}_{11}\\ m_{12}\\ m_{14}\\ m_{21}\\ m_{22}\\ m_{24}\\ m_{31}\\ m_{32}\end{array}\right]=\left[\begin{array}{c}{u}_1\·m_{34}\\ v_1\·m_{34}\\ ...\\ u_n\·m_{34}\\ v_n\·m_{34}\end{array}\right]$

(5)

其中M＝[m₁₁，m₁₂，m₁₄，m₂₁，m₂₂，m₂₄，m₃₁，m₃₂]^T

因为M矩阵乘以任意不为零的常数并不影响(x，y)与(u，v)的关系，因此指定m₃₄＝1。标定就是求M矩阵里的8个参数，未知数有8个，只要取n为4，即识别基准平面上的四个已知坐标的点，就可以求取这8个未知数。这里选取矩形基准平面的四个顶点，其坐标分别为(0，0)，(0，y₀)，(x₀，0)，(x₀，y₀)，连同识别出来的图像坐标，分别代入(5)，求解上述方程组，即可求出基准平面和图像平面的映射关系。即完成了摄像机标定过程。

图像预处理

对于摄像机拍摄的图像中的每一个像素，使用一个数值来表示其光强。这里使用标准的8位灰度级来表示0到255的光强级别。一般情况下，由摄像机采集到的图像会有不理想的情况，如光线过强，或者偏弱，这些都会对后续的图像处理产生影响。为了获得较好处理的灰度图像，在对采集的原始图像进行灰度化后要对其进行灰度转换。对图像灰度拉伸，使灰度级占据0～255整个区域，这样做的目的是为了减少光线过强，或者偏弱时造成的灰度级过少。

为了方便图像的处理，需要对拍摄的图像进行二元化处理，即图像中的每一个像素只使用0和1来表示(非黑即白)。有光束投射的像素用1表示，其它像素用0来表示。

二元化处理采用阈值的方法：当像素的原始光强大于事先设定的阈值时，将该像素标为1；否则标为0。二元化方法基于假设：光束在平面的投射图案比平面的其他部分更亮。使用的阈值是在系统配置时设定的，即在没有光束投射到平面上的情况下，平面上可能出现的最大光强值。当拍摄到的图像中有像素的光强大于该阈值时，说明这些像素有光束投射从而导致其光强超出阈值。在实际使用的时候，可以把这个阈值设成最大可能光强再加上一个富余量，用以消除反射或者其他噪音的影响。

投射图案的识别

经过处理的图像已经是二值的图像，一般的边缘检测的方法检测到的边缘信息含有大量的无用信息，排除一些噪声并增强边缘，去掉多余的噪声，很容易找到光束投射图案区域。通常情况下，光束的投射图案面积要比像素的尺寸大。并且由于摄像机的曝光速度限制，当一束光在基准平面上快速运动的时候，可能会产生拖影，即一束光在图片上留下的感光面积要比实际的投射图案面积大。因此需要对二元化的图像进行模式识别，检测出连续的像素区域(相邻的值为1的像素)作为光束投射图案。

连续像素区域的检测使用经典的基于区域标记(component labeling)的单通道算法。

系统对检测出来的投射图案存储其所有像素坐标，然后对每个投射图案的所有像素计算坐标平均值作为投射图案的中心点坐标。

在投射图案的识别中，主要的问题包括图像变形和抖动。如前所述，在计算投射图案坐标时使用识别出来的投射图案像素的平均坐标，理论上可以有效地过滤部分抖动，但抖动对识别的实际影响仍需要在实际试验中验证。

计算物体的位置和姿态

当部分光束投射到基准平面外导致从基准平面上识别出来的投射图案的数量少于从物体发射的光束个数时，系统无法计算出物体的坐标和角度信息，可以提示出界。

如果可识别出的投射图案数量刚好为三个，就可以根据识别的投射图案坐标计算锥顶的坐标。在计算之前，需要先完成投射图案和光束之间的关联。可以采用不同的形状来区分不同的光束，也可以采用不同的颜色来区分不同的光束。把投射到基准平面上的投射图案相连会构成一个三角形。这个三角形加上三条光束构成一个三棱锥。将该棱锥体的底上的三个顶点分别记为A(x_a，y_a，z_a)、B(x_b，y_b，z_b)、C(x_c，y_c，z_c)，这三个点可以根据标定好的摄像机获取的图像进行处理，按式(4)计算出它们的坐标。因为这三个点在基准平面内，所以Z坐标为0，三个点可记为：A(x_a，y_a，0)、B(x_b，y_b，0)、C(x_c，y_c，0)将锥顶的坐标记为S(x，y，z)。

(1)锥顶坐标计算

找到投射图案和光束的对应关系后，就可以根据余弦定理，任意一条底边(即两个投射图案之间的连线)的长度及其所对应的两条光束的夹角获得一组约束方程。

$\left\{\begin{array}{c}{\left|\mathrm{AB}\right|}^2={\left|\mathrm{AS}\right|}^2+{\left|\mathrm{BS}\right|}^2-2\·\left|\mathrm{AS}\right|\·\left|\mathrm{BS}\right|\·\cos \left(\mathrm{\α}\right)\\ {\left|\mathrm{BC}\right|}^2={\left|\mathrm{BS}\right|}^2+{\left|\mathrm{CS}\right|}^2-2\·\left|\mathrm{BS}\right|\·\left|\mathrm{CS}\right|\·\cos \left(\mathrm{\α}\right)\\ {\left|\mathrm{AC}\right|}^2={\left|\mathrm{AS}\right|}^2+{\left|\mathrm{CS}\right|}^2-2\·\left|\mathrm{AS}\right|\·\left|\mathrm{CS}\right|\·\cos \left(\mathrm{\α}\right)\end{array}\right.---\left(6\right)$

其中三个边的长度，|AB|²＝(x_a-x_b)²+(y_a-y_b)²，

|BC|²＝(x_b-x_c)²+(y_b-y_c)²，

|AC|²＝(x_a-x_c)²+(y_a-y_c)²

这里的AB、BC、AC边的长度可以通过坐标计算出来。

将以上三个方程及A、B、C三点坐标代入等式(6)中，可获得一个关于(x，y，z)的三元二次方程组。

解这个三元二次方程组，并综合考虑其他约束条件，舍弃不合适的值：

x_a、y_a、x_b、x_b、x_c、x_c＞0

(7)

z＜0

(8)

即可求出锥顶S的坐标值(x，y，x)。

(2)物体姿态计算

首先，设初始姿态为SS’||OZ，S’A||OX轴。物体的姿态可以用固定在其上的坐标系的三个单位向量来表示：

$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$

(9)

在初始状态时，式(9)简化为单位矩阵：

$R=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$

(10)

当物体坐标系与初始状态的姿态不一样时，就认为物体发生了旋转。旋转后SS’为旋转后Z轴的方向，X轴的方向在平面SAS’内并与SS’垂直，Y轴与新的XY平面垂直。

计算物体旋转后的坐标系向量只需要计算S’点的坐标即可，不难证明，S’点为三角形△ABC的内角平分线的交点，运用平面几何的算法可以求取这一点的平面坐标。

求出物体坐标系在参考系中新的X、Y、Z轴的向量，代入式(9)，即可求出旋转矩阵，物体相对于基准平面的姿态也就确定了。

Claims (7)

1、一种采用多束光确定位置姿态的系统，其特征在于：包括基准平面、需要确定位姿的物体、固定在物体上的光束发射装置、摄像机、计算装置；其中基准平面为位置和姿态已知的光滑平面，并具有反射光束发射装置发出的光的功能，光束投射在上面形成投射图案，供摄像机采集；光束发射装置固定在需要确定位姿的物体上，随着物体的运动而运动，并向基准平面发射三束或以上光束且光束之间的夹角已知；摄像机与基准平面的位置固定并且基准平面在摄像机的摄像范围内，摄像机需要标定，即建立摄像机获得的投射图案的位置与基准平面上的点的位置之间的映射关系，如果摄像机与基准平面的位置或姿态有变化，需要重新标定；摄像机对包括投射图案在内的基准平面的原始图像进行采集，并传输给计算装置；计算装置获取原始图像进行图像处理，并进行光束投射图案的位置计算，进而完成需要确定位姿的物体的空间六自由度的位置和姿态的计算。

2、一种采用多束光确定位置姿态的方法，该方法使用权利要求1所述系统确定物体的位置和姿态，包括以下步骤：

(1)在需要确定位置姿态的物体上加装光束发射装置，该装置发射的光束投射到基准平面上形成投射图案；

(2)在基准平面上建立坐标系，将摄像机与基准平面的位置固定并使得基准平面在摄像机的摄像范围内，然后对摄像机进行标定，即建立摄像机获得的投射图案的位置与基准平面上的点的位置之间的映射关系，如果摄像机与基准平面的位置或姿态有变化，需要重新标定；

(3)通过已经标定的摄像机以单幅或连续方式获取带有投射图案的图像并传输给计算机，或进行简单的图像处理后传送；

(4)通过图像处理方法识别各个投射图案，并根据摄像机标定的结果确定各投射图案的坐标；

(5)根据各投射图案的位置及光束的夹角计算出发射装置到各个投射图案的连线的长度，进而计算出物体相对于基准平面的位置和姿态。

3、根据权利要求2所述的确定物体位置姿态的方法，其特征在于通过光束发射装置发射不同颜色的光束形成不同颜色来区分投射图案。

4、根据权利要求2所述的确定物体位置姿态的方法，其特征在于：通过发射装置发射不同形状的光束形成不同图案来区分投射图案。

5、根据权利要求2或3或4所述的确定物体位置姿态的方法，其特征在于：通过与光束相同或相近颜色的滤光片来提高对投射图案位置的识别能力。

6、根据权利要求2或3或4所述的确定物体位置姿态的方法，其特征在于：采用二维空间方位已知基准平面进行摄像机标定，采用三束均布的光线投射到基准平面形成投射图案，通过二值化处理和边缘检测算法识别投射图案，并通过几何计算确定物体相对于已知平面的六自由度位置和姿态。

7、根据权利要求5所述的确定物体位置姿态的方法，其特征在于：采用二维空间方位已知基准平面进行摄像机标定，采用三束均布的光线投射到基准平面形成投射图案，通过二值化处理和边缘检测算法识别投射图案，并通过几何计算确定物体相对于已知平面的六自由度位置和姿态。

Images