CN104216202A - 一种结合惯性陀螺仪的摄像机视觉实时定位系统及定位方法 - Google Patents

Abstract

本发明公开了一种结合惯性陀螺仪的摄像机视觉实时定位系统，包括：上位机、通信传输模块、广角镜头摄像机、惯性陀螺仪以及标定板。本发明还公开了一种结合惯性陀螺仪的摄像机视觉实时定位方法，包括以下步骤：对所述广角镜头摄像机进行标定,获取广角镜头摄像机的内参矩阵以及径向畸变参数；安置所述标定板，并对标定板进行定位；将标定板的位置输入到所述上位机；惯性陀螺仪校准；将广角镜头摄像机与惯性陀螺仪固连；实时计算并存储广角镜头摄像机当前的位置和方向信息。本发明的定位方法具有定位范围大、方位全、精度高以及抗干扰能力强等优点，适用于各种较大的场景之中的实时定位。

Description

一种结合惯性陀螺仪的摄像机视觉实时定位系统及定位方法

技术领域

本发明涉及计一种摄像机视觉实时定位系统及定位方法，特别涉及一种结合惯性陀螺仪的摄像机视觉实时定位系统及定位方法。

背景技术

摄像机定位就是确定摄像机相对于已确定的世界坐标系的相对位置的过程。

摄像机定位过程中涉及如图1所示的4个坐标系，分别为像素坐标系、图像坐标系、相机坐标系和世界坐标系。

1)像素坐标系Ouv：原点O位于CCD图像平面的左上角，u轴和v轴分别与CCD图像平面像素行和列平行，(u,v)表示图像阵列中像素点的位置。

2)图像坐标系Oxy：原点(即主点O)定义在相机光轴与CCD图像平面的交点，x,y轴分别与u,v轴平行。Pu(xu,yu)表示针孔模型下物点P的理想图像坐标，Pd(xd,yd)是由相机镜头畸变引起的偏离Pu(xu,yu)的实际图像坐标。

3)相机坐标系OcXcYcZc：原点Oc定义在相机镜头的光心，Xc,Yc轴分别平行于x,y轴，Zc轴与相机的主光轴重合。相机的有效焦距为f，物点P在相机坐标系下的三维坐标用(xc,yc,zc)表示。

4)世界坐标系OwXwYwZw:根据具体坏境来定。(xwywzw)表示物点P在世界坐标系下的三维坐标。

现有的摄像机定位技术主要有通过视觉的定位方法，通过红外线的定位方法，以及通过无线射频技术的定位方法等。但是存在以下的缺陷

1)通过畸变很小的摄像机进行视觉定位，通过直接求解一次方程组获得摄像机的方位，虽速度较快，但由于镜头视野较小，不能获得足够的定位信息，而且误差无法有效控制。

2)红外定位和无线射频技术等方法则容易受到外界设备的干扰，难以保证定位的精度和可靠性，而且不能应用于较大的场景之中。

发明内容

发明目的：本发明的目的是为了解决现有技术中摄像机定位方法存在的镜头视野较小，不能获得足够的定位信息，而且误差无法有效控制；并且红外定位和无线射频技术等方法则容易受到外界设备的干扰，难以保证定位的精度和可靠性，而且不能应用于较大的场景之中等问题，提供了一种结合惯性陀螺仪的摄像机视觉实时定位系统及定位方法。

技术方案：为了实现以上目的，本发明公开了一种结合惯性陀螺仪的摄像机视觉实时定位系统，包括：上位机、通信传输模块、广角镜头摄像机、惯性陀螺仪以及标定板；所述广角镜头摄像机、惯性陀螺仪分别通过通信传输模块与上位机通信；所述广角镜头摄像机一侧设有用于广角镜头摄像机标定的标定板。所述广角镜头摄像机能够增加摄像机镜头的视野，能够拍摄到更大的范围，适用于较大的场景之中。所述惯性陀螺仪能够实时获取其姿态，用以辅助摄像机的定位，从而保证定位的精度。同时所述实时定位系统不受外界设备的干扰，保证了定位的抗干扰性和可靠性。

所述标定板至少为4个。所述定位板面向广角镜头摄像机的镜头分散布置，保证广角镜头摄像机能够尽量多的定位板，保证了定位的精度。

本发明还公开了一种结合惯性陀螺仪的摄像机视觉实时定位方法，包括以下步骤：

S1：对所述广角镜头摄像机进行标定,获取广角镜头摄像机的内参矩阵A以及径向畸变参数k1、k2和切向畸变参数p1、p2；

S2:安置所述标定板，并对标定板进行定位；

S3：将标定板的位置输入到所述上位机；

S4:惯性陀螺仪校准；

S5:将广角镜头摄像机与惯性陀螺仪固连；

S6：实时计算并存储广角镜头摄像机当前的位置和方向信息；步骤S6具体包含以下步骤：

S61开启广角镜头摄像机，拍摄带有定位板信息的图像；

S62在图像中识别并确定标定板位置；

S63如果是第一次计算广角镜头摄像机当前的位置和方向信息，不考虑镜头的畸变，计算世界坐标系到摄像机坐标系的平移向量，并将结果作为世界坐标系到摄像机坐标系的平移向量T的初值；否则，将上一次的利用步骤S6的计算结果作为T的初值；

S64通过惯性陀螺仪，对广角镜头摄像机的姿态进行测量，并将结果作为世界坐标系到摄像机坐标系的旋转向量R的初值；

S65考虑镜头的畸变，对外参矩阵[R,T]进行优化，获取并保存外参矩阵[R,T]的精确值；

S66根据内参矩阵A以及外参矩阵[R,T]实时计算广角镜头摄像机当前的位置和方向信息。

本发明所述一种结合惯性陀螺仪的摄像机视觉实时定位方法，通过广角镜头摄像机拍摄更大范围的图像、至少4个标定板提供更多的视觉信息、惯性陀螺仪的实时姿态输入优化结果等，提供了一种大范围、全方位、高精度、强抗干扰的摄像机定位方法。

有益效果：与现有技术相比，本发明具有以下优点：

1.由于采用了广角镜头摄像机来完成图像采集的工作，镜头视野更广，具有更大的拍摄范围，保证了大范围、全方位的定位；

2.惯性陀螺仪能够实时获取其姿态，用以辅助摄像机的定位，保证了定位的高精度；

3.定位系统不受外界设备的干扰，保证了定位的抗干扰性和可靠性。

4.定位系统适用于较大的场景之中。

附图说明

图1是摄像机定位过程中涉及的4个坐标系；

图2是一种结合惯性陀螺仪的摄像机视觉实时定位系统的系统结构图；

图3是一种结合惯性陀螺仪的摄像机视觉实时定位方法计算流程图；

图4是圆形编码图案的示意图

具体实施方式

下面结合附图和具体实施例，进一步阐明本发明，应理解这些实施例仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。

实施例：

一种结合惯性陀螺仪的摄像机视觉实时定位系统(如图2所示)，包括：上位机、通信传输模块、广角镜头摄像机、惯性陀螺仪以及标定板；所述广角镜头摄像机、惯性陀螺仪分别通过通信传输模块与上位机通信；所述广角镜头摄像机一侧设有用于广角镜头摄像机标定的标定板。所述标定板至少为4个。

一种结合惯性陀螺仪的摄像机视觉实时定位方法，包括以下步骤：

S1：对所述广角镜头摄像机进行标定,获取广角镜头摄像机的内参矩阵A,径向畸变参数k1、k2以及切向畸变参数p1、p2；

S2:将定位板面朝中间分散布置，尽量保证镜头始终能够拍摄到尽可能多的定位板(至少4个)。然后对这些定位板使用全站仪逐一进行定位；

S3：将标定板的位置输入到所述上位机；

S4:惯性陀螺仪校准；

S5:将广角镜头摄像机与惯性陀螺仪固连；

S6：实时计算并存储广角镜头摄像机当前的位置和方向信息。

所述步骤S6具体包含以下步骤(如图3所示)：

S61开启广角镜头摄像机，拍摄带有定位板信息的图像；

S62在图像中识别并确定标定板位置：

为了识别的效率和鲁棒性，定位板采用圆形图案，每个定位板外围都是一圈圆形，但内部的图案各不相同。内部图案都有一个方向标志，用以确定该圆形图案的一个方向，其余部分根据该方向，通过极坐标进行编码。

分析镜头采集的图像时，首先通过随机霍夫(Hough)变换定位图像中所有的椭圆。然后针对每一个椭圆，首先寻找方向标志，确定图案内部的编码起点，接着将内部图案解码(图4给出了一个简单的示例)，并与预存的标定板模板进行匹配，如果能够匹配上，则获得一个点对(Pw,p)，其中Pw是该定位板中心在世界坐标系中的位置，通过查找对应模板测定的位置获得，p是该定位板中心在图像中的位置。将获取的所有(Pw,p)点对保存，供后面的计算使用。

S63如果是第一次计算广角镜头摄像机当前的位置和方向信息，不考虑镜头的畸变，计算世界坐标系到摄像机坐标系的平移向量，并将结果作为世界坐标系到摄像机坐标系的平移向量T的初值，方法如下：

设图像坐标系上的一个点(u,v)和世界坐标系上其对应的点(x_w,y_w,z_w)，两者存在如下关系式：

$s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=A\·\left[\begin{array}{cc}R& T\end{array}\right]\·\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$

其中s是一个比例因子，

$A=\left[\begin{array}{ccc}\mathrm{\α}& \mathrm{\γ}& u_0\\ 0& \mathrm{\β}& v_0\\ 0& 0& 1\end{array}\right]$ 为摄像机内参矩阵，其中α、β是图像u、v轴上的尺度因子，γ图像u、v轴上的旋转因子，[u₀、v₀]为图像的中心坐标。

令H＝[R T]，H是一个投影矩阵，可知H是一个单应性矩阵。

令 $H=\left[\begin{array}{cccc}{h}_{11}& h_{12}& h_{13}& h_{14}\\ h_{21}& h_{22}& h_{23}& h_{24}\\ h_{31}& h_{32}& h_{33}& h_{34}\end{array}\right]$

从而，

su＝h₁₁*x_w+h₁₂*y_w+h₁₃*z_w+h₁₄,

sv＝h₂₁*x_w+h₂₂*y_w+h₂₃*z_w+h₂₄,

s＝h₃₁*x_w+h₃₂*y_w+h₃₃*z_w+h₃₄.

通过步骤S62中得到的多组对应点(P_w,p)，可解出H。

然后根据H的单应性，以及已知的内参矩阵A，可求出一个外参矩阵[R T]的估计值。

否则，将上一次的利用步骤S6的计算结果作为T的初值；

S64通过惯性陀螺仪对广角镜头摄像机的姿态进行测量，并将结果作为世界坐标系到摄像机坐标系的旋转向量R的初值；

步骤S63是不考虑镜头畸变而得到的外参估计，而实际镜头是一个广角镜头，存在很大的径向畸变和切向畸变，因此下面将加入畸变，计算摄像机外参的精确值。

径向畸变模型：

x_d＝x_u(1+k₁r²+k₂r⁴)

y_d＝y_u(1+k₁r²+k₂r⁴)

切向畸变模型：

x_d＝2p₁x_uy_u+p₂(r²+2x_u ²)

y_d＝p₁(r²+2y_u ²)+2p₂x_uy_u

综合畸变模型：

x_d＝x_u(1+k₁r²+k₂r⁴)+2p₁x_uy_u+p₂(r²+2x_u ²)

y_d＝y_u(1+k₁r²+k₂r⁴)+p₁(r²+2y_u ²)+2p₂x_uy_u

其中r²＝x²+y²，(x_d,y_d)是点(x,y)畸变后的位置。

摄像机外参的精确解通过非线性优化获得：

$f=\min \underset{i=1}{\overset{m}{\mathrm{\Σ}}}{\left|\right|m_i-\hat{m}(A,k_1,k_2,p_1,p_2,R_i,T_i,M_i)\left|\right|}^2$

m_i是第i个定位板在图像中的实际坐标，是根据摄像机的内外参计算出来的理论值。

的计算方法如下：

根据 $\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=\left[\begin{array}{cc}R& T\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$ 解出(x_c,y_c,z_c)，

根据 $\begin{array}{c}{x}_u=\frac{f}{z_c}\·x_c\\ y_u=\frac{f}{z_c}\·y_c\end{array}$ 解出(x_u,y_u)，

然后根据上述畸变模型算出(x_d,y_d)，

最后根据 $s\·\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{A}{f}\·\left[\begin{array}{c}{x}_d\\ y_d\\ 1\end{array}\right]$ (其中f＝α·d_x，dx为镜头单位像素的尺寸大小)计算出u，v的理论值。即的值。

S65考虑镜头的畸变，对外参矩阵[R,T]进行优化，获取并保存外参矩阵[R,T]的精确值；

优化算法使用莱文贝格－马夸特方法(Levenberg-Marquardt)进行优化，由于是非线性优化，因此需要提供R，T的初值。

惯性陀螺仪是一种能够实时传回当前姿态的定位装置，因此R的初值可由惯性陀螺仪提供；

T的初值可由两种方式提供，一种是通过上一步中的外参估计值中的T提供；一种是通过前一次计算结果的T提供。

显然，第一次计算时的T的初值只能通过上一步中外参估计值中的T提供。

由于该系统是一个摄像机的定位追踪系统，而且假设摄像机的运动不是非常快速和剧烈的，因此，后续的T的初值可由前一次的计算结果提供。

S66根据内参矩阵A以及外参矩阵[R,T]实时计算广角镜头摄像机当前的位置和方向信息。

Claims (4)

1.一种结合惯性陀螺仪的摄像机视觉实时定位系统，其特征在于，包括：上位机、通信传输模块、广角镜头摄像机、惯性陀螺仪以及标定板；所述广角镜头摄像机、惯性陀螺仪分别通过通信传输模块与上位机通信；所述广角镜头摄像机一侧设有用于广角镜头摄像机标定的标定板。

2.根据权利要求1所述的一种结合惯性陀螺仪的摄像机视觉实时定位系统，其特征在于，所述标定板至少为4个。

3.一种结合惯性陀螺仪的摄像机视觉实时定位方法，其特征在于，包括以下步骤：

S1：对所述广角镜头摄像机进行标定,获取广角镜头摄像机的内参矩阵A以及径向畸变参数k1、k2，和切向畸变参数p1、p2；

S2: 安置所述标定板，并对标定板进行定位；

S3：将标定板的位置输入到所述上位机；

S4: 惯性陀螺仪校准；

S5: 将广角镜头摄像机与惯性陀螺仪固连；

S6：实时计算并存储广角镜头摄像机当前的位置和方向信息。

4.根据权利要求3所述的一种结合惯性陀螺仪的摄像机视觉实时定位方法，其特征在于，所述步骤S6具体包含以下步骤：

S61 开启广角镜头摄像机，拍摄带有定位板信息的图像；

S62 在图像中识别并确定标定板位置；

S63 如果是第一次计算广角镜头摄像机当前的位置和方向信息，不考虑镜头的畸变，计算世界坐标系到摄像机坐标系的平移向量，并将结果作为世界坐标系到摄像机坐标系的平移向量T的初值；否则，将上一次的利用步骤S6[jl1] 的计算结果作为T的初值；

S64 开启惯性陀螺仪，对广角镜头摄像机的姿态进行测量，并将结果作为世界坐标系到摄像机坐标系的旋转向量R的初值；

S65 考虑镜头的畸变，对外参矩阵[R,T]进行优化，获取并保存外参矩阵[R,T]的精确值；

S66 根据内参矩阵A以及外参矩阵[R,T]实时计算广角镜头摄像机当前的位置和方向信息。