CN103528569B - 标志点正交分光成像位姿测试方法及传感器 - Google Patents

Abstract

本发明涉及位姿测量，为实现大范围、高精度、快速的单目视觉位姿测量，满足航空航天中瞄准和定位、空间坐标测量、空间物体合作移动等任务中对于空间位姿测量定位的需求，为此，本发明采用的技术方案是，标志点正交分光成像位姿测试方法及传感器，由一个成像镜头、分光镜、两个柱面镜和两个正交放置的线阵CCD、以及DSP组成，标志点经过物镜后成为圆形光斑，圆形光斑经过分光镜分束，成为两个相互正交的光束，在光束的传播方向上分别放置柱面镜，该线状像的垂直平分面上放置线阵CCD，该线状像与线阵CCD相交，并成像于该线阵CCD上，使用DSP根据标志点的二维坐标，解算其投射直线，并将信息输出。本发明主要应用于位姿测量。

Description

标志点正交分光成像位姿测试方法及传感器

技术领域

本发明涉及位姿传感器，尤其适用于高速大范围高精度位姿测量中。具体讲涉及标志点正交分光成像位姿测试方法及传感器。

技术背景

单目视觉位姿传感器可以实现大测量视场角测量，但是面阵CCD在数据采集的过程中数据量比较大，大大限制了面阵CCD进行位姿测量过程中图像数据的采集速度。而通常位姿测量过程中都只使用4-6个空间特征点，在摄像机像面上占的像素很少，这种结构对面阵CCD上数据的利用率比较低。同时，面阵CCD像素的分辨率也限制了空间位姿测量的精度。

线阵CCD可以实现高速高分辨率的数据采集。然而，线阵CCD的特殊结构只能完成一维测量。为实现三维位姿测量，现有技术是将几个线阵CCD结合，但是在这种基于多线阵CCD的位姿测量过程中，特征点必须在多个线阵CCD的公共视场之内，大大限制了多线阵CCD位姿测量系统的测量范围。

发明内容

为克服现有技术的不足，克服多线阵CCD位姿测量系统在测量范围上的限制，解决现有单目位姿测量和多线阵CCD位姿测量传感器中存在的测量速度、测量精度和测量范围的矛盾，实现大范围、高精度、快速的单目视觉位姿测量，满足航空航天中瞄准和定位、空间坐标测量、空间物体合作移动等任务中对于空间位姿测量定位的需求，为此，本发明采用的技术方案是，标志点正交分光成像位姿传感器，由一个成像镜头、分光镜、两个柱面镜和两个正交放置的线阵CCD、以及DSP组成，标志点经过物镜后成为圆形光斑，圆形光斑经过分光镜分束，成为两个相互正交的光束，在光束的传播方向上分别放置柱面镜，则将标志点的像拉长为与柱面镜水平子午面平行的线状像，该线状像的垂直平分面上放置线阵CCD，该线状像与线阵CCD相交，并成像于该线阵CCD上，得到空间发光点在两个线阵CCD上的成像位置，即空间特征点在虚拟像面上的二维坐标，使用DSP根据标志点的二维坐标，解算其投射直线，并将信息输出。

柱面镜相互垂直，与分光镜的水平和垂直面平行，两个线阵CCD的中心线与柱面镜的母线垂直，并同时与镜头光轴相交。

标志点正交分光成像位姿测试方法，借助于前述位姿传感器实现，并包括如下步骤：综合考虑垂直线阵CCD和水平线阵CCD得到的发光点所在两个光平面的位置，采用得到的空间发光特征点通过镜头成像过程中的透视投影直线，综合物镜和柱面镜畸变，建立正交分光成像位姿传感器的数学模型：

$s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{f}{S_x}& \mathrm{\τ}& u_0\\ 0& \frac{f}{S_y}& v_0\\ 0& 0& 1\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]=M_1{M}_2\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$

其中，s为任意比例常数，(u,v)为CCD测得的标志点横纵坐标，f为物镜焦距，S_x为横像素距离，S_y为纵像素距离，τ为成像平面倾斜因子，(u₀,v₀)为系统光学中心在CCD上的坐标，(x,y,z)为对应的标志点的空间坐标，RT为旋转平移矩阵，M₁为内参，M₂为外参。

通过标定，即可得到系统的内参和外参矩阵M₁M₂，因此，通过CCD上标志点的坐标，即可得到该标志点在空间中所处的透视投影直线。

由于径向、偏心和薄透镜畸变，由该模型推算得到的空间标志点的位置需要进一步地校正：

$\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\end{array}\right]=\left[\begin{array}{c}x\\ y\end{array}\right]+A{\left(A^{T}A\right)}^{-1}{A}^T\left[\begin{array}{c}{x}_i-x\\ y_i-y\end{array}\right]$

其中，(x',y',1)^T为校正畸变后的标志点空间投射直线，(x,y,z)^T为根据模型计算得到的标志点空间坐标，

其中， $A=\left[\begin{array}{cccccc}x(x^2+y^2)& 0& 3x^2+y^2& 2\mathrm{xy}& x^2+y^2& 0\\ 0& y(x^2+y^2)& 2\mathrm{xy}& x^2+3y^2& 0& x^2+y^2\end{array}\right]$

使用3个以上的标志点进行标定即可得到畸变系数阵P，进而可以通过上式得到校正后的标志点空间透视投影直线。

先确定位姿传感器其视场角，即所测物体大小与距离之比，根据视场角和CCD有效测量尺寸确定成像物镜和柱面镜的组合焦距，再根据所需分辨率确定通光孔径的尺寸，进而使用CodeV验证其精度并根据光路确定分光镜的尺寸和位置。

本发明具备下列技术效果：

在大范围空间位姿测量中，本发明的位姿传感器利用正交分光原理将面阵CCD象元平方数量级的运算量降低为象元和的运算量，大大提高了测量速度，同时，线阵CCD和DSP相结合的方式提高了系统的实时响应速度和测量速度，线阵CCD的高分辨率也同时保证了测量结果的精度，单物镜的使用不损失其成像范围，可实现大视场角测量。因此，该正交分光原理高速大范围高精度位姿测量系统，在保测量范围的同时，达到很高的测量精度和很高的测量速度，且结构简单，结果稳定，从而解决了现有单目位姿测量和多线阵CCD位姿传感器中存在的测量速度、测量精度和测量范围的矛盾。

附图说明

图1是标志点正交分光成像位姿传感器原理框图。

图2是本发明特征点的正交分光成像示意图。

图3是DSP的软件流程图。

具体实施方式

本发明的标志点正交分光成像位姿传感器采用的是单物镜，分光测量，这样就保证了两个CCD的测量范围重合度为100%，从而保证了大测量范围的要求，并且充分发挥线阵CCD的优势，提高测量精度。同时，使用DSP和线阵CCD相结合的方式可以实现该传感器快速响应和快速测量。

本发明解决其技术问题所采用的技术方案是：

标志点正交分光成像位姿传感器的设计主要包括成像镜头设计和正交分光光路两方面。传感器由一个成像镜头、分光镜、两个柱面镜和两个正交放置的线阵CCD（或PSD）组成。成像镜头的设计过程中主要考虑空间分辨率、景深、视场角等因素，在满足测量成像要求的情况下尽量减小镜头的畸变。根据空间测量范围，采用分光棱镜和柱面镜结合的方式将空间特征点的成像变成两个空间正交的两条线状像，并成像于空间正交放置的两个线阵CCD上。

一个光点通过柱面透镜，将在其像平面上成一与柱面透镜的水平子午面平行且与母线垂直的线状像，当光点沿着柱面透镜的轴心线移动时，线状像将相应作水平运动，而当光点沿着垂直于轴心线的方向移动时，线状像将相应作反向移动。由高斯公式可知，当物距为无穷远（工程上取物距大于30倍焦距）时，像平面与焦平面重合。因此，由柱面透镜和一个放在它焦平面上且与其轴心线垂直的线阵CCD构成的一维像机可以检测远处光点在垂直于其轴心线方向上的运动。

该传感器的原理框图如图1所示，标志点经过物镜后成为圆形光斑，圆形光斑经过分光镜分束，成为两个相互正交的光束，在光束的传播方向上分别放置柱面镜，则将标志点的像拉长为与柱面镜水平子午面平行的线状像，该线状像的垂直平分面上放置线阵CCD，该线状像与线阵CCD相交，并成像于该线阵CCD上，可得到空间发光点在两个线阵CCD上的成像位置，即空间特征点在虚拟像面上的二维坐标，使用DSP根据标志点的二维坐标，解算其投射直线，并将信息输出。

正交分光位姿传感器具体结构和特征点的成像示意图如图2所示，正交分光成像位姿测量传感器由物镜5、分光镜4、柱面镜3、垂直线阵CCD和水平线阵CCD六部分构成，2为线状像与线阵CCD的交点，6为红外LED标志点。物镜焦距需配合CCD有效尺寸和测量所需的视场角，柱面镜尺寸不小于分光镜的尺寸；柱面镜相互垂直，与分光镜的水平和垂直面平行，两个线阵CCD的中心线与柱面镜的母线垂直，并同时与镜头光轴相交。

如图2所示的特征点成像示意图中，偏离镜头光轴的空间某红外发光特征点发出的光经过物镜光心，经分光镜后分为两束：一束沿原路径传播，经柱面镜会聚在垂直线性CCD上，另外一束经分光棱镜反射，顺时针偏折后沿垂直于光轴的方向传播，经柱面镜会聚在水平线性CCD上。即完成了将光平面转换为两个正交的光线。发光点的投影直线经物镜光心和分光镜后以两个正交的光平面的形式成像于正交的两个线阵CCD上，根据发光点的成像位置，根据柱面镜的成像特征，即可得到发光标志点的坐标和投影直线。

综合考虑垂直线阵CCD和水平线阵CCD得到的发光点所在两个光平面的位置，采用得到的空间发光特征点通过镜头成像过程中的透视投影直线，综合物镜和柱面镜畸变，建立正交分光成像位姿传感器的数学模型。

$s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{f}{S_x}& \mathrm{\τ}& u_0\\ 0& \frac{f}{S_y}& v_0\\ 0& 0& 1\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]=M_1{M}_2\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$

其中，s为任意比例常数，(u,v)为CCD测得的标志点横纵坐标，f为物镜焦距，S_x为横像素距离，S_y为纵像素距离，τ为成像平面倾斜因子，(u₀,v₀)为系统光学中心在CCD上的坐标，(x,y,z)为对应的标志点的空间坐标，RT为旋转平移矩阵，M₁为内参，M₂为外参。

通过标定，即可得到系统的内参和外参矩阵M₁M₂，因此，通过CCD上标志点的坐标，即可得到该标志点在空间中所处的透视投影直线。

由于径向、偏心和薄透镜畸变，由该模型推算得到的空间标志点的位置需要进一步地校正。

$\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\end{array}\right]=\left[\begin{array}{c}x\\ y\end{array}\right]+A{\left(A^{T}A\right)}^{-1}{A}^T\left[\begin{array}{c}{x}_i-x\\ y_i-y\end{array}\right]$

其中，(x',y',1)^T为校正畸变后的标志点空间投射直线，(x,y,z)^T为根据模型计算得到的标志点空间坐标，

其中， $A=\left[\begin{array}{cccccc}x(x^2+y^2)& 0& 3x^2+y^2& 2\mathrm{xy}& x^2+y^2& 0\\ 0& y(x^2+y^2)& 2\mathrm{xy}& x^2+3y^2& 0& x^2+y^2\end{array}\right]$

使用3个以上的标志点进行标定即可得到畸变系数阵P，进而可以通过上式得到校正后的标志点空间透视投影直线。

如图3所示，两个线阵CCD的输出的模拟信号经ADC转换为数字信号后传入DSP进行数据处理，由于ADC的转换速率远远低于DSP的外部时钟频率，而读取总线上的数据时，DSP外部存储器读写时钟须与A/D的时钟相匹配，若DSP的数据线与A/D直接相连将导致DSP在大部分时间里都在等待A/D就绪。为了保证系统的使用效率，在两者之间入FIFO作为二者之间的桥梁。数字信号经滤波和提取边缘后可以得到标志点在两个CCD上的横纵坐标(u,v)，经标定，可以得到该系统的内外参矩阵M₁M₂，根据公式，即可得到标志点在空间中的坐标(x,y)，经畸变校正后，即可得到校正后的空间投射投影直线，即可获得标志点的位姿信息。

下面结合附图和实施例对本发明进一步说明。

位姿传感器应先确定其视场角，即所测物体大小与距离之比，根据视场角和CCD有效测量尺寸确定成像物镜和柱面镜的组合焦距，再根据所需分辨率确定通光孔径的尺寸，进而使用CodeV验证其精度并根据光路确定分光镜的尺寸和位置。此外，由于柱面透镜的非对称性，使用正负透镜焦度分离的方式校正像差，由于只使用窄波段的红外LED标志点且亮度恒定，故不存在强烈色差，也不需要对CCD感光的动态范围进行调整。由于水平线阵CCD和竖直线阵CCD的成像对称，比例相等，为面阵CCD图像的v、u坐标，因此可以根据标定的相机参数获得对应面阵CCD上的成像坐标。

CCD光采样的光积分启动控制信号由DSP产生。采用DSP的定时器中断产生CCD光积分控制信号可以在光强容许的范围内可以灵活调整CCD光积分时间T_SH，每一次ADC转换完成都产生1次中断，将采样结果保存到FIFO中，当FIFO中数据半满时，再产生中断将数据保存到DSP的RAM中进行处理。CPU工作可不被打断，处理能力得到充分发挥。

Claims (2)

1.一种标志点正交分光成像位姿测试方法，其特征是，借助于下述位姿传感器实现，由一个成像镜头、分光镜、两个柱面镜和两个正交放置的线阵CCD、以及DSP组成，标志点经过成像镜头后成为圆形光斑，圆形光斑经过分光镜分束，成为两个相互正交的光束，在光束的传播方向上分别放置柱面镜，则将标志点的像拉长为与柱面镜水平子午面平行的线状像，该线状像的垂直平分面上放置线阵CCD，该线状像与线阵CCD相交，并成像于该线阵CCD上，得到空间发光点在两个线阵CCD上的成像位置，即空间特征点在虚拟像面上的二维坐标，使用DSP根据标志点的二维坐标，解算其投射直线，并将信息输出；还包括如下步骤：由垂直线阵CCD和水平线阵CCD成像光平面的位置，以及得到的成像镜头成像的透视投影直线，结合成像镜头和柱面镜畸变，建立正交分光成像位姿传感器的数学模型：

$s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{f}{S_x}& \mathrm{\τ}& u_0\\ 0& \frac{f}{S_y}& v_0\\ 0& 0& 1\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]=M_1{M}_2\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$

其中，s为任意比例常数，(u,v)为CCD测得的标志点横纵坐标，f为成像镜头焦距，S_x为横像素距离，S_y为纵像素距离，τ为成像平面倾斜因子，(u₀,v₀)为系统光学中心在CCD上的坐标，(x,y,z)为对应的标志点的空间坐标，R和T'为旋转平移矩阵，M₁为内参，M₂为外参；

通过标定，即可得到系统的内参和外参矩阵M₁和M₂，因此，通过CCD上标志点的坐标，即可得到该标志点在空间中所处的透视投影直线；

由于径向、偏心和薄透镜畸变，由该模型推算得到的空间标志点的位置需要进一步地校正；

$\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\end{array}\right]=\left[\begin{array}{c}x\\ y\end{array}\right]+A{\left(A^{T}A\right)}^{-1}{A}^T\left[\begin{array}{c}{x}_i-x\\ y_i-y\end{array}\right]$

其中，(x',y',1)^T为校正畸变后的标志点空间投射直线，(x,y,z)^T为根据模型计算得到的标志点空间坐标；

其中，

使用3个以上的标志点进行标定即可得到畸变系数阵P，进而可以通过上式得到校正后的标志点空间透视投影直线。

2.如权利要求1所述的标志点正交分光成像位姿测试方法，其特征是，还包括如下细化步骤：先确定位姿传感器其视场角，即所测物体大小与距离之比，根据视场角和CCD有效测量尺寸确定成像镜头和柱面镜的组合焦距，再根据所需分辨率确定通光孔径的尺寸，进而使用CodeV验证其精度并根据光路确定分光镜的尺寸和位置。