CN104034305A - 一种单目视觉实时定位的方法 - Google Patents

Abstract

本发明涉及一种单目视觉实时定位的方法。本发明首先对摄像头进行标定，以获取内参数矩阵。然后标定好摄像头后，根据基本的小孔成像原理，通过几何变换求解出需要测量的地面指定点到参考点的距离和向量与小车中心线的角度。本发明在一般的几何换算定位基础上，考虑到摄像头主点一般不在图像中心，进行了几何修正，提高了测量精度。

Description

一种单目视觉实时定位的方法

技术领域

本发明属于计算机几何学领域，适合于实时测量二维平面上目标点到参考点的距离以及角度。

背景技术

目标定位的方法有激光测距法，GPS定位法，还有图像处理法等。在激光测距这种主动测距方法，往往会因为被测物体的形状不规则，或者激光束偏离被测物体而造成测量精度偏低，无法在实际生活中运用；GPS定位虽然比较准确，但是成本比较高，需要在被测物体上搭建GPS系统，如果对于测量未知障碍物的距离就不适合了。在基于图像的定位上，有基于单目视觉定位和多目视觉定位方法。多目定位通常是匹配多个摄像头拍摄图像的特征点序列，用三角测量法求解视差，最后恢复被测物体在世界坐标系下的坐标，这种方法虽然可以比较准确的恢复出三维场景，但是在匹配上可能因为特征点的误配，造成测量不准。单目定位结构简单，实时性比较高，可以在短时间内快速得到比较准确的结果。

发明内容

本发明的目地是为了解决当前一些定位方法的实时性不高、成本过高的问题，提出一种单目视觉实时定位的方法，与多目视觉定位相比本发明结构简单易于实现；与其他的单目视觉定位相比，本发明精准度高，且实时性强。

本发明采用的具体技术方案如下：

将广角摄像头固定在一辆小车上，让摄像头以一定的俯角朝下，确定测量摄像头到地面的高度h，小车的长度l，摄像头俯仰角γ₀，小车正前方的参考点M到小车的距离D，以及摄像头的水平视角2α和垂直视角2β；在获取以上参数后，设摄像头拍摄的图片大小H×W，目标P点在图像中为p，参考点M在图像中为m，摄像头光轴与地面交点G在图像中为主点g，采用如下步骤标定摄像头，以获取内参数矩阵R：

第一步，制作标定板。

打印出7×8正方形黑白格标定模版，将其固定在一个平整的方形板上，测量任意一个方形格的长度l₁。

第二步，标定摄像头。

按不同的方位摆放标定板，保证摄像头把全部的方形格都拍到，得到N张图像；用Matlab标定工具箱载入全部的图像，输入l₁的大小，标定摄像头；最后得到摄像头内参数矩阵R，其中R包括主点图像平面坐标(c_x,c_y)，以及摄像头的x轴方向的焦距f_x与y轴方向的焦距f_y。

标定好摄像头后，根据基本的小孔成像原理，通过几何变换求解出需要测量的地面指定点P到参考点M的距离D₁和向量与小车中心线的角度angle，具体求解方案如下：

第一步：建立P点和M点，以及交点G的二维坐标计算式，具体是：

以G为原点，Y轴为光轴的投影线，X轴垂直Y轴以小车右侧为正，建立直角坐标系XGY；根据简单的几何变换，得出地面上任何一点i在此坐标下的坐标：

$\left\{\begin{array}{c}{Y}_i=\frac{h\·k_1\·v\·(1+k_2^2)}{1-k_1\·k_2\·v}\\ X_i=\frac{(\mathrm{UG}+Y_i)\·u}{\mathrm{UG}}\·k_3\·k_4\end{array}\right.---\left(1\right)$

其中，v＝(v_g-v_i)，u＝(u_i-u_g)，v_g为主点g的图像坐标y值，v_i为i点的图像坐标y值；u_g为主点g的图像坐标x值，u_i为i点的图像坐标x值； $k_1=\frac{2\·\tan \left({\mathrm{\α}}_0\right)}{H},$ k₂＝tan(γ₀)， $k_3=\frac{h}{\cos \left({\mathrm{\γ}}_0\right)},k_4=\frac{2\·\tan \left({\mathrm{\β}}_0\right)}{W},$ $\mathrm{UG}=\frac{h\·(\tan \left({\mathrm{\γ}}_0\right)-\tan ({\mathrm{\γ}}_0-{\mathrm{\α}}_0)\·\cos ({\mathrm{\γ}}_0-{\mathrm{\α}}_0))}{\cos ({\mathrm{\γ}}_0-{\mathrm{\α}}_0)-\cos \left({\mathrm{\γ}}_0\right)};$ 其中，α₀为点i的摄像头水平视角分量，β₀为i的摄像头垂直视角分量；这样，根据这个公式算出P点和M点以及G的坐标分别为(X_P,Y_P)，(X_M,Y_M)，(0,0)。

第二步：分别计算P点和M点的摄像头视角分量，具体是：

假定地平面是平整的，以保证测量的点是二维的；假设P点的图像坐标为p(u_p,v_p)，M点的图像坐标为m(u_m,v_m)；根据如下的式子计算任意点i的摄像头水平视角分量β₀和垂直视角分量α₀。

若u_i＞c_x且v_i＞c_y，则 ${\mathrm{\α}}_0={\tan }^{-1}\left(\frac{v_i}{f_y}\right),{\mathrm{\β}}_0={\tan }^{-1}\left(\frac{W-u_i}{f_x}\right);$

若u_i＜c_x且v_i＞c_y，则 ${\mathrm{\α}}_0={\tan }^{-1}\left(\frac{v_i}{f_y}\right),{\mathrm{\β}}_0={\tan }^{-1}\left(\frac{u_i}{f_x}\right);$

若u_i＜c_x且v_i＜c_y，则 ${\mathrm{\α}}_0={\tan }^{-1}\left(\frac{H-v_i}{f_y}\right),{\mathrm{\β}}_0={\tan }^{-1}\left(\frac{u_i}{f_x}\right);$

若u_i＞c_x且v_i＜c_y，则 ${\mathrm{\α}}_0={\tan }^{-1}\left(\frac{H-v_i}{f_y}\right),{\mathrm{\β}}_0={\tan }^{-1}\left(\frac{W-u_i}{f_x}\right);$

其中，(u_i,v_i)为i点的图像坐标，分别带入P点和M点的图像坐标即可得到各自的两个分量值；其中g'为理想的主点位置，g为真实主点位置；这样将两个点各自得到的α₀、β₀值分别带入第一步得到的坐标计算式即可得出真实的坐标(X_P,Y_P)、(X_M,Y_M)。

第三步：计算P点到M点的距离和向量与小车中心线的角度。

用勾股定理求出两点之间的距离

向量与小车中心线的角度求解具体如下：

若u_p＜c_x，则angle＝angle1+angle2；

若u_p＞u_m，则angle＝angle1-angle2；

若u_p＞＝c_x且u_p＜＝u_m，则angle＝angle2-angle1；

其中，angle1为向量在XGY坐标系下相对于Y轴的夹角，即ΔXGM的夹角∠XGM，通过余弦定理计算；angle2为补偿角，即Y轴与车子中心线的夹角，该夹角通过测量G和M点间连线与车子中心线夹角得到，且在摄像头高度，位置，以及内参数不变的情况下这个补偿角也是不变的；最后小车到P点的距离，即为d＝D+D₁。

本发明的好处在于：

1、在一般的几何换算定位基础上，考虑到摄像头主点一般不在图像中心，进行了几何修正，提高了测量精度。

2、各项测量的参数都比较好测量，可操作性好。

附图说明

图1为系统结构示意图；

图2为标定板；

图3为投影关系图；

图4为投影平面图。

具体实施方式

以下结合附图对本发明作进一步说明。

本发明采用的具体技术方案如下：

将一个普通的广角摄像头，固定在一辆小车上，让摄像头以一定的俯角朝下，如图1。测量摄像头到地面的高度h，小车的长度l，摄像头俯仰角γ₀，小车正前方的参考点M(这里取摄像头最低的垂直视角与小车中心线的交点)到小车的距离D，以及摄像头的水平视角2α和垂直视角2β。在获取以上参数后，假设摄像头拍摄的图片大小H×W，P点在图像中为p，M点在图像中为m，摄像头光轴与地面交点G在图像中为主点g(即摄像头光轴与图像平面的交点)。我们需要标定摄像头，以获取内参数矩阵R。采用如下标定步骤：

第一步，制作标定板。

打印出7×8正方形黑白格标定模版如图2，将其固定在一个平整的方形板上，测量任意一个方形格的长度l₁。

第二步，标定摄像头

按不同的方位摆放标定板，保证摄像头把全部的方形格都拍到，得到N(N＞10)张图像。用Matlab标定工具箱载入全部的图像，输入l₁的大小，标定摄像头。最后得到摄像头内参数矩阵R，其中R包括主点图像平面坐标(c_x,c_y)，以及摄像头的x轴方向的焦距f_x与y轴方向的焦距f_y。

标定好摄像头后，我们可以根据基本的小孔成像原理，通过几何变换求解出我们需要测量的地面指定点P到参考点M的距离D₁和向量与小车中心线的角度angle，具体求解方案如下：

第一步：建立P点和M点，以及G的二维坐标计算式

我们以G为原点，Y轴为光轴的投影线，X轴垂直Y轴以小车右侧为正，建立直角坐标系XGY，如图3。根据简单的几何变换，文献(郭磊，徐友春，李克强，连小珉，基于单目视觉的实时测距方法研究.中国图形图像学报.2006，11)，可以得出地面上任何一点i在此坐标下的坐标：

$\left\{\begin{array}{c}{Y}_i=\frac{h\·k_1\·v\·(1+k_2^2)}{1-k_1\·k_2\·v}\\ X_i=\frac{(\mathrm{UG}+Y_i)\·u}{\mathrm{UG}}\·k_3\·k_4\end{array}\right.---\left(1\right)$

其中，v＝(v_g-v_i)，u＝(u_i-u_g)，v_g为主点g的图像坐标y值，v_i为i点的图像坐标y值；u_g为主点g的图像坐标x值，u_i为i点的图像坐标x值。 $k_1=\frac{2\·\tan \left({\mathrm{\α}}_0\right)}{H},$ k₂＝tan(γ₀)， $k_3=\frac{h}{\cos \left({\mathrm{\γ}}_0\right)},k_4=\frac{2\·\tan \left({\mathrm{\β}}_0\right)}{W},$ $\mathrm{UG}=\frac{h\·(\tan \left({\mathrm{\γ}}_0\right)-\tan ({\mathrm{\γ}}_0-{\mathrm{\α}}_0)\·\cos ({\mathrm{\γ}}_0-{\mathrm{\α}}_0))}{\cos ({\mathrm{\γ}}_0-{\mathrm{\α}}_0)-\cos \left({\mathrm{\γ}}_0\right)}.$ 其中，α₀为点i的摄像头水平视角分量，β₀为i的摄像头垂直视角分量。这样，根据这个公式算出P点和M点以及G的坐标分别为(X_P,Y_P)，(X_M,Y_M)，(0,0)。

第二步：分别计算P点和M点的摄像头视角分量

这里，我们假定地平面是平整的，以保证测量的点是二维的。假设P点的图像坐标为p(u_p,v_p)，M点的图像坐标为m(u_m,v_m)。在理想情况下，摄像头的光心没有偏移，即主点g的图像坐标为这时摄像头的垂直视角在图像的上部分和下半部分是平分的，摄像头的水平视角在图像的左半部分和右半部分是平分的，即摄像头的光轴投影恰好在小车的中心线上(如图3)，那么P点与M点的摄像头水平视角分量都为β₀＝β，垂直视角分量都为α₀＝α。但是，由于工艺上的不足，主点的图像坐标通常并不会等于即主点并不会在图像中心，而是有一定的偏移。因此，我们需要根据如下的式子计算任意点i的摄像头水平视角分量β₀和垂直视角分量α₀。

若u_i＞c_x且v_i＞c_y，则 ${\mathrm{\α}}_0={\tan }^{-1}\left(\frac{v_i}{f_y}\right),{\mathrm{\β}}_0={\tan }^{-1}\left(\frac{W-u_i}{f_x}\right).$

若u_i＜c_x且v_i＞c_y，则 ${\mathrm{\α}}_0={\tan }^{-1}\left(\frac{v_i}{f_y}\right),{\mathrm{\β}}_0={\tan }^{-1}\left(\frac{u_i}{f_x}\right).$

若u_i＜c_x且v_i＜c_y，则 ${\mathrm{\α}}_0={\tan }^{-1}\left(\frac{H-v_i}{f_y}\right),{\mathrm{\β}}_0={\tan }^{-1}\left(\frac{u_i}{f_x}\right).$

若u_i＞c_x且v_i＜c_y，则 ${\mathrm{\α}}_0={\tan }^{-1}\left(\frac{H-v_i}{f_y}\right),{\mathrm{\β}}_0={\tan }^{-1}\left(\frac{W-u_i}{f_x}\right).$

其中，(u_i,v_i)为i点的图像坐标，分别带入P点和M点的图像坐标即可得到各自的两个分量值。两个点的图像坐标位置关系如图4，其中g'为理想的主点位置，g为真实主点位置。这样将两个点各自得到的α₀、β₀值分别带入第一步得到的坐标计算式即可得出真实的坐标(X_P,Y_P)、(X_M,Y_M)。

第三步：计算P点到M点的距离和向量与小车中心线的角度

用勾股定理求出两点之间的距离我们

需要求的角度是向量与小车中心线的角度，则具体如下：

若u_p＜c_x，则angle＝angle1+angle2。

若u_p＞u_m，则angle＝angle1-angle2。

若u_p＞＝c_x且u_p＜＝u_m，则angle＝angle2-angle1。

其中，angle1为向量在XGY坐标系下相对于Y轴的夹角，即ΔXGM的夹角∠XGM，可以简单通过余弦定理计算；angle2为补偿角，即Y轴与车子中心线的夹角，该夹角可以通过测量G和M点间连线与车子中心线夹角得到，且在摄像头高度，位置，以及内参数不变的情况下这个补偿角也是不变的。最后小车到P点的距离，即为d＝D+D₁。

本实施例中使用普通的125°的广角摄像头，采集的图像分辨率为W＝640，H＝480。本实施例中用皮尺测量出摄像头的高度h，小车长度l，摄像头俯仰角γ₀。在小车的左前方放置一个物体，标记其与地面的接触点P。标记小车正前方的参考点M。测量小车最前方与其距离D＝230mm。在标定环节，将标定板以不同的位姿放置，标定板的小方格长度为25mm，通过广角摄像头拍摄15张不同位姿的标定板，再使用Matlab标定工具箱标定摄像头获得相机的内参矩阵R，以本实施例中使用的摄像头为例，得到的R矩阵如下：

$\left[\begin{array}{ccc}310.12184& 0& 341.73572\\ 0& 310.10968& 222.21821\\ 0& 0& 1\end{array}\right]$

在得到了R矩阵后，根据待测点M,P点的图像坐标，分别确定出这两个点的摄像头水平视角分量，垂直视角分量的值，带入坐标计算公式，得出两点在XGY坐标系下的坐标，得出两点的距离D₁和在XGY坐标系下相对于Y轴的夹角angle1。根据G的图像坐标，在本实施例中是主点g(341，222)(因为图像坐标是整数值，向0取整)，找出在G在地面的位置。测量参考点M与G相对与小车中心线的夹角即补偿角angle2，本实施例中是14.7°。得出P点和M点的距离D₁和向量与小车中心线之间的夹角angle。

由于人工测量的误差，特别是摄像头俯仰角γ₀和摄像头高度h比较难精确测量，导致计算结果会有一定的人工误差。因此，先标记10个已知的点(P₁,P₂...P₁₀)和参考点M，测量P_i相对于M之间的距离，利用遗传算法，将测量得到的γ₀和h作为初始值，在一定的步长内迭代求出较优的γ₀、h的值。在遗传算法的设计中，适应度函数设计为：

$\mathrm{fitness}=\underset{i=1}{\overset{10}{\mathrm{\Σ}}}\left(D_i\right)$

其中，D_i为第i个点与M计算出的相对距离与实际距离的平方差值。采用轮盘赌的选择策略，选择概率为0.9，交叉概率为0.05，迭代100次，最后得出γ₀＝48.75°，h＝605mm。最后，将得到的比较精确的γ₀和h的值带入计算式，再重新计算待测点P的坐标与M的坐标。本实施例中，测量了17组数据，发现当P点离摄像头超过1m～5m时，误差较大，但是总体可以控制在：距离误差2cm左右，角度误差在1°左右。当P点在1m范围内时误差基本上控制在2cm以内。由此，可以得出本发明方法比较实用，精确度较高。

Claims (1)

1.一种单目视觉实时定位的方法，其特征在于：

将广角摄像头固定在一辆小车上，让摄像头以一定的俯角朝下，确定测量摄像头到地面的高度h，小车的长度l，摄像头俯仰角γ₀，小车正前方的参考点M到小车的距离D，以及摄像头的水平视角2α和垂直视角2β；在获取以上参数后，设摄像头拍摄的图片大小H×W，目标P点在图像中为p，参考点M在图像中为m，摄像头光轴与地面交点G在图像中为主点g，采用如下步骤标定摄像头，以获取内参数矩阵R：

第一步，制作标定板；

打印出7×8正方形黑白格标定模版，将其固定在一个平整的方形板上，测量任意一个方形格的长度l₁；

第二步，标定摄像头

按不同的方位摆放标定板，保证摄像头把全部的方形格都拍到，得到N张图像；用Matlab标定工具箱载入全部的图像，输入l₁的大小，标定摄像头；最后得到摄像头内参数矩阵R，其中R包括主点图像平面坐标(c_x,c_y)，以及摄像头的x轴方向的焦距f_x与y轴方向的焦距f_y；

标定好摄像头后，根据基本的小孔成像原理，通过几何变换求解出需要测量的地面指定点P到参考点M的距离D₁和向量与小车中心线的角度angle，具体求解方案如下：

第一步：建立P点和M点，以及交点G的二维坐标计算式，具体是：

以G为原点，Y轴为光轴的投影线，X轴垂直Y轴以小车右侧为正，建立直角坐标系XGY；根据简单的几何变换，得出地面上任何一点i在此坐标下的坐标：

$\left\{\begin{array}{c}{Y}_i=\frac{h\·k_1\·v\·(1+k_2^2)}{1-k_1\·k_2\·v}\\ X_i=\frac{(\mathrm{UG}+Y_i)\·u}{\mathrm{UG}}\·k_3\·k_4\end{array}\right.---\left(1\right)$

其中，v＝(v_g-v_i)，u＝(u_i-u_g)，v_g为主点g的图像坐标y值，v_i为i点的图像坐标y值；u_g为主点g的图像坐标x值，u_i为i点的图像坐标x值；k₂＝tan(γ₀)， $k_3=\frac{h}{\cos \left({\mathrm{\γ}}_0\right)},k_4=\frac{2\·\tan \left({\mathrm{\β}}_0\right)}{W},$ 其中，α₀为点i的摄像头水平视角分量，β₀为i的摄像头垂直视角分量；这样，根据这个公式算出P点和M点以及G的坐标分别为(X_P,Y_P)，(X_M,Y_M)，(0,0)；

第二步：分别计算P点和M点的摄像头视角分量，具体是：

假定地平面是平整的，以保证测量的点是二维的；假设P点的图像坐标为p(u_p,v_p)，M点的图像坐标为m(u_m,v_m)；根据如下的式子计算任意点i的摄像头水平视角分量β₀和垂直视角分量α₀；

若u_i＞c_x且v_i＞c_y，则 ${\mathrm{\α}}_0={\tan }^{-1}\left(\frac{v_i}{f_y}\right),{\mathrm{\β}}_0={\tan }^{-1}\left(\frac{W-u_i}{f_x}\right);$

若u_i＜c_x且v_i＞c_y，则 ${\mathrm{\α}}_0={\tan }^{-1}\left(\frac{v_i}{f_y}\right),{\mathrm{\β}}_0={\tan }^{-1}\left(\frac{u_i}{f_x}\right);$

若u_i＜c_x且v_i＜c_y，则 ${\mathrm{\α}}_0={\tan }^{-1}\left(\frac{H-v_i}{f_y}\right),{\mathrm{\β}}_0={\tan }^{-1}\left(\frac{u_i}{f_x}\right);$

若u_i＞c_x且v_i＜c_y，则 ${\mathrm{\α}}_0={\tan }^{-1}\left(\frac{H-v_i}{f_y}\right),{\mathrm{\β}}_0={\tan }^{-1}\left(\frac{W-u_i}{f_x}\right);$

其中，(u_i,v_i)为i点的图像坐标，分别带入P点和M点的图像坐标即可得到各自的两个分量值；其中g'为理想的主点位置，g为真实主点位置；这样将两个点各自得到的α₀、β₀值分别带入第一步得到的坐标计算式即可得出真实的坐标(X_P,Y_P)、(X_M,Y_M)；

第三步：计算P点到M点的距离和向量与小车中心线的角度

用勾股定理求出两点之间的距离

向量与小车中心线的角度求解具体如下：

若u_p＜c_x，则angle＝angle1+angle2；

若u_p＞u_m，则angle＝angle1-angle2；

若u_p＞＝c_x且u_p＜＝u_m，则angle＝angle2-angle1；

其中，angle1为向量在XGY坐标系下相对于Y轴的夹角，即ΔXGM的夹角∠XGM，通过余弦定理计算；angle2为补偿角，即Y轴与车子中心线的夹角，该夹角通过测量G和M点间连线与车子中心线夹角得到，且在摄像头高度，位置，以及内参数不变的情况下这个补偿角也是不变的；最后小车到P点的距离，即为d＝D+D₁。