CN108613673A

CN108613673A - 一种单目视觉位姿估计装置及方法

Info

Publication number: CN108613673A
Application number: CN201810605775.XA
Authority: CN
Inventors: 刘小明; 陈万春; 陈中原
Original assignee: Beihang University
Current assignee: Beihang University
Priority date: 2018-06-13
Filing date: 2018-06-13
Publication date: 2018-10-02
Anticipated expiration: 2038-06-13
Also published as: CN108613673B

Abstract

本发明一种单目视觉位姿估计装置及方法，包括：圆形平面反射镜、锥形反射镜、CCD相机以及目标平面；所述的圆形平面反射镜、锥形反射镜以及CCD相机同轴。所述估计方法为：确定锥形反射镜的锥角大小约束；选取β和θ，使得该单目视觉位姿估计装置产生的光程差大；确定圆形平面反射镜所在平面与CCD相机镜头所在平面的距离d和锥形反射镜母线最小长度M_min；计算机首先将图像灰度化，然后采用平滑滤波及梯度算法识别出图像的清晰区域在何处；根据已知的相机焦距f、锥形成像靶面上的检测出的清晰的像素点距离镜头中心的距离pix_distance(x*,y*)及其横坐标x*、纵坐标y*；根据透镜成像原理及几何关系得出传感器的位姿信息。

Description

一种单目视觉位姿估计装置及方法

技术领域

本发明涉及一种单目视觉位姿估计装置及方法，属于航空航天导航领域。

背景技术

微型无人机的导航能力，是其实现各种任务和应用的基础和关键。目前的无人机装备了各种导航设备，如全球卫星定位***(GPS)、气压高度计、惯性测量单元(IMU)、激光测距仪、雷达扫描仪等，但对于微型无人机而言，这些设备将不再适用，原因在于：

1)微型无人机多工作于超低空环境或室内环境，由于地形的散射、遮蔽等因素，GPS的测高功能将大打折扣，有的地方根本就没有GPS信号；

2)气压高度计无法获得对地高度，而且易受气象条件影响；

3)单独的微机电IMU漂移快，常用于微小型无人机的MPU6050的零漂高达0.15度/秒，必须辅以其它信息予以修正；

4)激光测距仪和雷达扫描仪质量、体积、功耗都比较大，无法在微型无人机上使用。

在这种情况下，被动式的机载视觉将是解决微型无人机的导航问题的一种可行方法。

根据摄像机数目的不同，机载视觉定位方法可以分为多目视觉定位、双目视觉定位、单目视觉定位。对于微型无人机来说，其质量/体积/功率(MVP) 都比较小，考虑到摄像机的基线长度、自身尺寸以及安装位置等因素的制约，飞行平台不宜安装多台视觉成像设备，一般只安装一台，即形成单目视觉。

将单目视觉测量技术应用于微型无人机导航，其优势体现在如下几个方面：

第一，单目视觉传感器是被动式的，不会产生电磁辐射，故隐蔽性好，适合军事应用；

第二，单目视觉传感器的计算量小，这就降低了算法的复杂度，机载处理器可实时在线运算，提高了微型无人机的实际应用价值；

第三，单目视觉传感器的重量轻，体积小。

尽管单目视觉无法像GPS一样提供绝对位置(经纬度)，但它可以提供载体相对于环境的飞行高度和飞行姿态，这对于在复杂环境中执行任务的微小型无人机而言更具有现实意义。因此，利用单目视觉技术进行微小型无人机导航具有重要研究价值，设计出具有优良性能的单目视觉传感器和导航算法是一项具有科学意义和应用前景的工作。

现有的单目视觉位姿估计方法的本质，是将三维实景投影成二维平面图像来进行位姿估计，投影过程中丢失了深度信息，因而需要多帧图像或者先验知识来重建深度信息。计算负担大，实时性不易保障。因此，本发明试图通过改变单目视觉传感器的结构来寻求突破。

发明内容

(一)发明的目的

本发明的目的是提供一种单目视觉位姿估计装置及方法，以简便、高效、实时地估计飞行器的高度、滚转角、俯仰角。

(二)技术方案

一种单目视觉位姿估计装置，包括：

圆形平面反射镜、锥形反射镜、CCD相机以及目标平面。

所述的圆形平面反射镜、锥形反射镜以及CCD相机同轴，目标平面与CCD 相机的光轴是垂直的(即目标平面与相机焦平面是平行的)。

利用上述的一种单目视觉位姿估计装置，本发明实现了一种单目视觉位姿估计方法，包括以下步骤：

步骤1，确定锥形反射镜的锥角大小约束，锥形反射镜的镜面母线与CCD 相机镜头所在平面的夹角为β，CCD相机视场角的1/2为θ：

定义：

C点为圆形平面反射镜上任意一条直径的延长线与入射光线内界的交点；

A点为圆形平面反射镜上距C点最远的点；

AC为A点与C点的距离；

AE为圆形平面反射镜的直径；

O点为CCD相机镜头的中心；

AO为A点与O点的距离；

B点为锥形反射镜外边缘距A点最近的点；

AB为A点与B点的距离；

d为圆形平面反射镜所在平面与CCD相机镜头所在平面的距离。

1.1为了保证目标平面的光线可入射到锥形反射镜上，则需AC＞AE。

由于，

则，

另外，

因此，

而，

因此：

1.2锥形反射镜与圆形平面反射镜间不会产生连续反射现象(即锥形反射镜的反射光线照射到圆形平面反射镜上后将直接反射到CCD相机镜头，而不会再次反射到锥形反射镜上)，则需∠CBC′＞β。

定义：

C′点为锥形反射镜外边缘上与B点相对的点；

∠CBC′为线段BC与线段BC′的夹角；

∠ACB为线段CA与线段CB的夹角；

G₁点为直线OB与目标平面的交点；

∠ABG₁为线段BG₁和线段BA的交点；

∠ABC为线段BA和线段BC的交点；

∠BAO为线段AB和线段AO的交点；

∠AOB为线段OA和线段OB的交点。

∠ACB＝180°-∠ABG₁-∠ABC-β (8)

其中，

则，

因此，

综上所述，β和θ的约束关系为式(7)和(12)。

另外，由于CCD相机自身的视场角不会超过90度，即

2θ＜90° (13)

则，

因此，

对于式(7)来说：

若分子和分母均大于零，即

则有：

sin(2β-2θ)＞sin(2θ) (17)

因此，

β＞2θ (18)

若分子和分母均小于零，则有

显然不成立。

综上，角度约束方程为：

由公式(19)还可以得出：

步骤2，为了使得产生的锥形的像的母线较长，从而使得CCD相机所采集的像模糊与清晰的区域差异较明显，显然，根据透镜成像公式，物距变化越明显越好，也就是单目视觉位姿估计装置对目标平面产生的光程差越大越好。因此需要在公式(19)和(20)的限制下，选取较优的β和θ，使得该单目视觉位姿估计装置产生的光程差较大。

该单目视觉位姿估计装置产生的光程差Δ为

式中，h为CCD相机镜头所在平面与目标平面间的距离。

显然，从式(21)可以看出，h和d越大越好。当确定了h和d后，β越小且θ 越大，光程差越大。

根据公式(19)和(20)，以及CCD相机视场与焦距的约束，单目视觉位姿估计装置的关键参数取值如下表：

步骤3，确定θ和β后，若给定圆形平面反射镜的直径L，则可以确定圆形平面反射镜所在平面与CCD相机镜头所在平面的距离d和锥形反射镜母线最小长度M_min，即

步骤4，来自目标平面的光线入射到锥形反射镜的镜面上，锥形反射镜将光线反射到圆形平面反射镜上，圆形平面反射镜将光线反射到CCD相机的镜头，计算机首先将图像灰度化，然后采用平滑滤波及梯度算法识别出图像的清晰区域在何处，平滑滤波算法如下：

对于图像上的一个3×3的像素区域，其灰度值矩阵为：

其中，p_i(i＝1,2,…,9)为每个像素点的灰度值。

给定一个权重矩阵K，

其中，k_i(i＝1,2,…,9)为每个像素点的灰度值的权重。

那么，像素区域中心点的灰度值为，

对全局梯度进行阈值，即：

其中，x≤image_width,y≤image_height。I、J、K分别为原图、平滑滤波图以及阈值图的灰度矩阵，x，y表示某个点的像素坐标(图像的左上角为原点)， threshold为灰度门限值，image_width及image_height为图像的横纵方向的像素数。

步骤5，根据已知的相机焦距f、锥形成像靶面上的检测出的清晰的像素点距离镜头中心的距离pix_distance(x*,y*)及其横坐标x*、纵坐标y*(也就是清晰的像的三维先验信息)，利用多元线性最小方差理论：

其中，为包含飞行器位姿信息(高度、俯仰角和滚转角)的矩阵， X＝[pix_distance(x*,y*)x* y*]，Y＝f。

步骤6，根据透镜成像原理及几何关系可得传感器的位姿信息，包括高度、俯仰角和滚转角，即：

式中，和为矩阵的三个元素。

(三)优点效果

本发明基于光学畸变的单目视觉导航机理，通过改变传统相机结构，获得同时包含距离信息和姿态信息的单幅图像，通过简单的清晰度辨识方法便可识别出清晰区域，利用单目视觉位姿估计装置的先验结构及镜头参数信息即可获得飞行器的位姿信息，该方法成本低，算法简单。

附图说明

图1为本发明单目视觉位姿估计装置原理图。

图2为光程差随θ和β的变化关系图。

图3为本发明单目视觉位姿估计装置设计流程图。

图4为本发明图像清晰域及位姿信息辨识算法框图。

图5为高度估计相对误差。

图6为俯仰角估计相对误差。

图7为滚转角估计相对误差。

图8为位姿估计误差。

具体实施方式

下面结合图1-7所示，对本发明做进一步说明。

一种单目视觉位姿估计装置，原理图如附图1所示，包括：

圆形平面反射镜、锥形反射镜、CCD相机以及目标平面。

定义：

A点为圆形平面反射镜上距C点最远的点；

AC为A点与C点的距离；

AE为圆形平面反射镜的直径；

O点为CCD相机镜头的中心；

AO为A点与O点的距离；

B点为锥形反射镜外边缘距A点最近的点；

AB为A点与B点的距离；

d为圆形平面反射镜所在平面与CCD相机镜头所在平面的距离。

由于，

则，

另外，

因此，

而，

因此：

定义：

C′点为锥形反射镜外边缘上与B点相对的点；

∠CBC′为线段BC与线段BC′的夹角；

∠ACB为线段CA与线段CB的夹角；

G₁点为直线OB与目标平面的交点；

∠ABG₁为线段BG₁和线段BA的交点；

∠ABC为线段BA和线段BC的交点；

∠BAO为线段AB和线段AO的交点；

∠AOB为线段OA和线段OB的交点。

∠ACB＝180°-∠ABG₁-∠ABC-β (34)

其中，

则，

因此，

综上所述，β和θ的约束关系为式(7)和(12)。

另外，由于CCD相机自身的视场角不会超过90度，即

2θ＜90° (39)

则，

因此，

对于式(7)来说：

若分子和分母均大于零，即

则有：

sin(2β-2θ)＞sin(2θ) (43)

因此，

β＞2θ (44)

若分子和分母均小于零，则有

显然不成立。

综上，角度约束方程为：

由公式(19)还可以得出：

该单目视觉位姿估计装置产生的光程差Δ为

式中,h为CCD相机镜头所在平面与目标平面间的距离。

显然，从式(21)可以看出，h和d越大越好。

当确定了h和d后，由附图2可知，β越小且θ越大，光程差越大。

根据公式(19)和(20)，以及附图3所示CCD相机视场与焦距的约束，单目视觉位姿估计装置的关键参数取值如下表：

步骤3，确定θ和β后，若给定圆形平面反射镜的直径L，则可以确定圆形平面反射镜与CCD相机镜头的距离d和锥形反射镜母线最小长度M_min，即

步骤1、步骤2和步骤3的求解流程可参见附图4。

对于图像上的一个3×3的像素区域，其灰度值矩阵为：

其中，p_i(i＝1,2,…,9)为每个像素点的灰度值。

给定一个权重矩阵K，

其中，k_i(i＝1,2,…,9)为每个像素点的灰度值的权重。

那么，像素区域中心点的灰度值为，

对全局梯度进行阈值，即：

式中，和为矩阵的三个元素。

步骤4、步骤5和步骤6的求解流程可参见附图5。

实施例

给定一个1/3″、f＝25mm的CCD相机镜头，CCD相机焦平面与CCD相机镜头的距离为26mm，CCD相机距地面高度为650mm，俯仰角为30度，滚转角为45度，现准备应用此CCD相机进行单目视觉位姿估计。

步骤1，根据附图3所示的CCD相机镜头焦距和视场角的约束关系，可以看出1/3″、f＝25mm的CCD相机镜头的视场角为2θ＝10.6°，即θ＝5.3°。

根据约束(46)，可知，

θ＝5.3°＜18°

即，CCD相机镜头视场角满足约束。

另外，根据约束(45)，锥形反射镜母线与CCD相机镜头所在平面夹角β需满足

即

10.6°＜β＜31.7667°

步骤2，由附图2可知，当CCD相机视场角2θ确定后，锥形反射镜母线与CCD相机镜头所在平面夹角β越小越好。考虑到机械加工的难易程度，这里选取β＝15°。

步骤3，确定θ和β后，若给定圆形平面反射镜的直径L＝50mm，则可以确定圆形平面反射镜与CCD相机镜头的距离d和锥形反射镜母线最小长度 M_min，即

对于图像上的一个3×3的像素区域，其灰度值矩阵为：

其中，p_i(i＝1,2,…,9)为每个像素点的灰度值。

给定一个权重矩阵K，

其中，k_i(i＝1,2,…,9)为每个像素点的灰度值的权重。

这里，

那么，像素区域中心点的灰度值为，

对全局梯度进行阈值，即：

其中，x≤image_width,y≤image_height。A、B、C分别为原图、平滑滤波图以及阈值图的灰度矩阵，x，y表示某个点的像素坐标(图像的左上角为原点)，threshold为灰度门限值，image_width及image_height为图像的横纵方向的像素数。

这里，灰度门限值threshold取为20。

步骤5，根据已知的相机焦距f、锥形成像靶面上的检测出的清晰的像素点 (即灰度值超过灰度阈值的点)距离镜头中心的距离pix_distance(x*,y*)及其横坐标x*、纵坐标y*，利用多元线性最小方差理论：

其中，为包含飞行器位姿信息(高度、俯仰角和滚转角)的估计矩阵，式中X＝[pix_distance(x*,y*)x* y*]，Y＝f。

这里pix_distance(x*,y*)为一常值10mm。得出，

步骤6，根据透镜成像原理可得传感器的位姿信息，包括高度、俯仰角和滚转角，即：

式中，和为矩阵的三个元素。

将数值代入后得出，

则高度估计误差为

俯仰角估计误差为

滚转角估计误差为

重复上述实施例240次，位姿估计效果即位姿估计误差如附图6、附图7 和附图8所示，可见位姿估计误差可以保证小于5％。

Claims

1.一种单目视觉位姿估计装置，其特征在于，包括：

圆形平面反射镜、锥形反射镜、CCD相机以及目标平面；

所述的圆形平面反射镜、锥形反射镜以及CCD相机同轴，目标平面与CCD相机的光轴是垂直的，目标平面与相机焦平面是平行的。

2.一种根据权利要求1所述的单目视觉位姿估计装置的估计方法，其特征在于，包括以下步骤：

步骤1，确定锥形反射镜的锥角大小约束，锥形反射镜的镜面母线与CCD相机镜头所在平面的夹角为β，CCD相机视场角的1/2为θ：

参数定义如下：

A点为圆形平面反射镜上距C点最远的点；

AC为A点与C点的距离；

AE为圆形平面反射镜的直径；

O点为CCD相机镜头的中心；

AO为A点与O点的距离；

B点为锥形反射镜外边缘距A点最近的点；

AB为A点与B点的距离；

d为圆形平面反射镜所在平面与CCD相机镜头所在平面的距离；

1.1为了保证目标平面的光线可入射到锥形反射镜上，则需AC＞AE；

由于，

则，

另外，

因此，

而，

因此：

1.2锥形反射镜与圆形平面反射镜间不会产生连续反射现象，也就是说锥形反射镜的反射光线照射到圆形平面反射镜上后将直接反射到CCD相机镜头，而不会再次反射到锥形反射镜上，则需∠CBC′＞β；

参数定义如下：

C′点为锥形反射镜外边缘上与B点相对的点；

∠CBC′为线段BC与线段BC′的夹角；

∠ACB为线段CA与线段CB的夹角；

G₁点为直线OB与目标平面的交点；

∠ABG₁为线段BG₁和线段BA的交点；

∠ABC为线段BA和线段BC的交点；

∠BAO为线段AB和线段AO的交点；

∠AOB为线段OA和线段OB的交点；

∠ACB＝180°-∠ABG₁-∠ABC-β (8)

其中，

则，

因此，

综上所述，β和θ的约束关系为式(7)和(12)；

另外，由于CCD相机自身的视场角不会超过90度，即

2θ＜90° (13)

则，

因此，

对于式(7)来说：

若分子和分母均大于零，即

则有：

sin(2β-2θ)＞sin(2θ) (17)

因此，

β＞2θ (18)

若分子和分母均小于零，则有

显然不成立；

综上，角度约束方程为：

由公式(19)得出：

步骤2，为了使得产生的锥形的像的母线较长，从而使得CCD相机所采集的像模糊与清晰的区域差异较明显，根据透镜成像公式，物距变化越明显越好，也就是单目视觉位姿估计装置对目标平面产生的光程差越大越好；因此，需要在公式(19)和(20)的限制下，选取β和θ，使得该单目视觉位姿估计装置产生的光程差大；

该单目视觉位姿估计装置产生的光程差Δ为

式中，h为CCD相机镜头所在平面与目标平面间的距离；

从式(21)看出，h和d越大越好；当确定了h和d后，β越小且θ越大，光程差越大；

步骤3，确定θ和β后，若给定圆形平面反射镜的直径L，则确定圆形平面反射镜所在平面与CCD相机镜头所在平面的距离d和锥形反射镜母线最小长度M_min，即

对于图像上的一个3×3的像素区域，其灰度值矩阵为：

其中，p_i为每个像素点的灰度值；i＝1,2,…,9；

给定一个权重矩阵K，

其中，k_i为每个像素点的灰度值的权重；i＝1,2,…,9；

那么，像素区域中心点的灰度值为，

对全局梯度进行阈值，即：

其中，x≤image_width,y≤image_height；I、J、K分别为原图、平滑滤波图以及阈值图的灰度矩阵，x，y表示某个点的像素坐标，threshold为灰度门限值，image_width及image_height为图像的横纵方向的像素数；

步骤5，根据已知的相机焦距f、锥形成像靶面上的检测出的清晰的像素点距离镜头中心的距离pix_distance(x*,y*)及其横坐标x*、纵坐标y*，利用多元线性最小方差理论：

其中，为包含飞行器位姿信息的矩阵，X＝[pix_distance(x*,y*)x* y*]，Y＝f；

步骤6，根据透镜成像原理及几何关系得传感器的位姿信息，包括高度、俯仰角和滚转角，即：

式中，和为矩阵的三个元素。