CN103824298A - 一种基于双摄像机的智能体视觉三维定位装置与方法 - Google Patents

Abstract

本发明提供一种基于双摄像机的的智能体视觉三维定位方法及其装置，其方法包括：步骤1，在智能体上设置双摄像机，使第一摄像机的主轴与第二摄像机的主轴互相垂直；步骤2，两个摄像机对同一对象分别进行单目视觉的三维定位，分别获得第一定位结果、第二定位结果；步骤3，对所述第一定位结果、第二定位结果进行融合处理，获得最优三维定位结果，融合处理为：用第二摄像机的X分量代替第一摄像机的Z分量；用第一摄像机的X负分量代替第二摄像机的Z分量。本发明通过巧妙的融合两个摄像机的单目视觉定位数据，进而融合两者三维定位深度方向的误差，从与深度平方成正比例降低为与深度成正比例，保证了视觉系统的正常定位。

Description

一种基于双摄像机的智能体视觉三维定位装置与方法

技术领域

本发明属于计算机视觉三维领域，尤其涉及一种基于双摄像机的智能体视觉三维定位装置与方法。

背景技术

机器人、无人车辆等智能体在环境中作业时，需要知道自身的位姿。视觉系统是面扫描传感器，信息量大，常用于机器人的环境感知与定位。机器人视觉定位通常有两种方式，一种是单摄像机的定位方法，一种是双摄像机的立体定位方法。然而，这两种方式针对智能体视觉定位深度方向均存在误差较大的问题，无法保证视觉系统的正常使用。

因为，这两种方法有一个共同的特点，即在三维定位时，与X,Y方向定位误差只是X,Y方向偏差的正比，深度方向的误差与深度的平方成正比。因此，视觉系统在深度方向的误差会比X,Y方向的误差大，当距离较远时误差无法容忍，造成无法获知自身位姿的后果。

发明内容

为解决上述问题，本发明提供一种基于双摄像机的的智能体视觉三维定位装置与方法，该方法有效地降低视觉系统的深度定位误差，使三维定位深度方向的误差从与深度平方成正比例降低为与深度成正比例，保证了视觉系统的正常定位。

本发明的基于双摄像机的的智能体视觉三维定位方法，其包括：

步骤1，在智能体上设置双摄像机，使第一摄像机的主轴与第二摄像机的主轴互相垂直，则满足条件：第一摄像机坐标系的Z方向与第二摄像机的坐标系下的X方向相同，所述第一摄像机坐标系的X方向与第二摄像机坐标系的Z方向相差180度；

步骤2，两个摄像机对同一对象分别进行单目视觉的三维定位，分别获得第一定位结果、第二定位结果；

步骤3，对所述第一定位结果、第二定位结果进行融合处理，获得最优三维定位结果，融合处理为：

用第二摄像机的X分量代替第一摄像机的Z分量；

用第一摄像机的X负分量代替第二摄像机的Z分量。

进一步的，若p为智能体上任意一点，其在第一摄像机的坐标系下的坐标为^Ap，在第二摄像机的坐标系下的坐标为^Bp，在世界坐标系下的坐标为^Wp，A、B分别表示第一、第二摄像机的坐标系，则满足：

$\left\{\begin{array}{c}p_A=R_W^{A}p_W+t_W^A\\ p_B=R_W^{B}p_W+t_W^B\end{array}\right.---\left(1\right)$

通过步骤2的PnP法得出由世界坐标系转化为第一摄像机坐标系的旋转矩阵^AR_w、的平移矩阵^At_W，由世界坐标系转化为第二摄像机坐标系的旋转矩阵^BR_w、平移矩阵^Bt_W，且 $t_W^A=\left[\begin{array}{c}t_{\mathrm{Wx}}^A\\ t_{\mathrm{Wy}}^A\\ t_{\mathrm{Wz}}^A\end{array}\right],t_W^B=\left[\begin{array}{c}t_{\mathrm{Wx}}^B\\ t_{\mathrm{Wy}}^B\\ t_{\mathrm{Wz}}^B\end{array}\right];$

根据步骤3采用^Bt_W的X分量代替^At_W的Z分量，用^At_W的X分量的负值代替^Bt_W的Z分量，获得

$\left\{\begin{array}{c}p_A=R_W^{A}p_W+t_W^{\′}{}_A\\ p_B=R_W^{B}p_W+t_W^{\′}{}_B\end{array}\right.$

其中， $t_W^{\′}{}_A=\left[\begin{array}{c}t_{\mathrm{Wx}}^A\\ t_{\mathrm{Wy}}^A\\ t_{\mathrm{Wx}}^B\end{array}\right],t_W^{\′}{}_B=\left[\begin{array}{c}t_{\mathrm{Wx}}^B\\ t_{\mathrm{Wy}}^B\\ -t_{\mathrm{Wx}}^A\end{array}\right]$

对上述方程进行求解得到^Wp，得到最优的世界坐标。

进一步的，若p为智能体上任意一点，其在第一摄像机的坐标系下的坐标为^Ap，在第二摄像机的坐标系下的坐标为^Bp，在世界坐标系下的坐标为^Wp，A、B分别表示第一、第二摄像机的坐标系，则满足：

$\left\{\begin{array}{c}p_A=R_W^{A}p_W+t_W^A\\ p_B=R_W^{B}p_W+t_W^B\end{array}\right.---\left(1\right)$

通过步骤2的PnP法得出由世界坐标系转化为第一摄像机坐标系的旋转矩阵^AR_w、的平移矩阵^At_W，由世界坐标系转化为第二摄像机坐标系的旋转矩阵^BR_w、平移矩阵^Bt_W，且 $t_W^A=\left[\begin{array}{c}t_{\mathrm{Wx}}^A\\ t_{\mathrm{Wy}}^A\\ t_{\mathrm{Wz}}^A\end{array}\right],t_W^B=\left[\begin{array}{c}t_{\mathrm{Wx}}^B\\ t_{\mathrm{Wy}}^B\\ t_{\mathrm{Wz}}^B\end{array}\right];$

利用第二摄像机坐标系到第一摄像机坐标系的旋转矩阵^AR_B、平移矩阵^At_B，将^Bt_W转化为代替^At_W的^At_W″；

利用第一摄像机坐标系到第二摄像机坐标系的旋转矩阵^BR_A、平移矩阵^Bt_A，将^At_W转化为代替式（1）中^Bt_W的^Bt_W″；

$\left\{\begin{array}{c}t_W^{\′\′}{}_A|_Z=(R_B^{A}t_W^B+t_B^A)|_Z\\ t_W^{\′\′}{}_B|_Z=(R_A^{B}t_W^A+t_A^B)|_Z\end{array}\right.$

其中|_Z表示Z分量。

本发明还提供一种基于双摄像机的的智能体视觉三维定位装置，其包括：

机械本体、两个相同型号的摄像机、人工路标、处理装置；

所述机械本体固定在智能体上，该机械本体为长方体，且该长方体的底面和顶面是正方形，四个侧面以相互平行的平面为一组，分为两组，每组开有圆柱孔，且两组的圆柱孔相互垂直，上下相切；

每个圆柱孔的正前方开槽，放置滤光片，所述圆柱孔的后方放置一个摄像机；该摄像机的镜头探入所述圆柱孔内；

所述人工路标是在一个平面上均匀布置有源红外LED灯的装置，不同位置的人工路标通过点亮不同的有源红外LED灯来区别，获得不同的路标ID；该人工路标的有源红外LED灯中心点的世界坐标通过人工测量后保存于处理装置的数据库中；

所述处理装置包括第一摄像机子单元，第二摄像机子单元，及双摄像机三维定位融合子单元；第一摄像机子单元和第二摄像机子单元均包括：采集图像模块，与采集图像模块连接的检测模块，与检测模块连接的ID识别模块，数据模块，与ID识别模块、数据模块、检测模块分别连接的单目视觉三维定位模块；

其中采集图像模块负责从所属摄像机中周期性地读出所感知的数字图像；检测模块用于根据该数字图像进行人工路标中心点检测，得出人工路标关键点的图像坐标；ID识别模块根据检测出的中心点阵列进行人工路标ID的识别，并根据识别出的人工路标ID从所接数据模块中的人工路标世界坐标数据库中查找出对应ID的人工路标关键点的世界坐标；单目视觉三维定位模块，结合人工路标关键点的图像坐标和世界坐标以及自身的摄像机参数，进行单目视觉的三维定位，并将定位结果输出至双摄像机三维定位融合子单元；双摄像机三维定位融合子单元根据两个摄像机子单元的单目视觉定位结果进行融合运算，融合运算具体为：用第二摄像机的X分量代替第一摄像机的Z分量；用第一摄像机的X负分量代替第二摄像机的Z分量，得到减小深度误差的三维定位结果。

进一步的，所述两个圆柱孔上放置的滤光片为相同带宽的带通滤波器，该带通滤波器以940nm为中心频率，带宽为10nm。

进一步的，所述两个摄像机为普通CCD摄像机。

本发明的有益效果在于：

与普通三维定位方法相比，本发明通过巧妙的融合两个摄像机的单目视觉定位数据，进而融合两者三维定位深度方向的误差，从与深度平方成正比例降低为与深度成正比例，保证了视觉系统的正常定位。

附图说明

图1是本发明的基于双摄像机的的智能体视觉三维定位装置的机械本体及摄像机示意图；

图2是本发明的基于双摄像机的的智能体视觉三维定位装置的人工路标示意图；

图3是本发明的基于双摄像机的的智能体视觉三维定位装置的处理装置示意图；

图4是本发明的基于双摄像机的的智能体视觉三维定位装置的处理装置中的摄像机PnP单目定位示意图；

图5是本发明的基于双摄像机的的智能体视觉三维定位装置的处理装置中的双摄像机三维定位融合示意图。

附图说明：

圆柱孔A1、滤光片A2、摄像机A3、相机罩A4；圆柱孔B1、滤光片B2、摄像机B3、相机罩B4。

具体实施方式

本发明的基于双摄像机的的智能体视觉三维定位装置，包括机械本体、两个同型号的摄像机、处理装置。

1、机械本体及摄像机

如图1所示，机械本体为立柱。立柱通过下部的安装孔通过螺栓或其他方式固定在机器人或无人车辆等智能体上。该立柱为长方体，底面和顶面是正方形，侧面是长宽一致的长方形。四个侧面分为平行的两组，其中一组为A组，一组为B组。A组开有圆柱孔A1，B组开有圆柱孔B1，圆柱孔A1与圆柱孔B1相互垂直，A1圆柱孔在B1圆柱孔的上方。

圆柱孔A1的正前方开槽，放置滤光片A2，圆柱孔A1的后方放置摄像机A3。摄像机A3的机体露在外面，摄像机A3的镜头探入圆柱孔A1内。摄像机A3露出立柱部分被相机罩A4裹住。同样，B1圆柱孔的正前方开槽，放置滤光片B2，B1圆柱孔的后方放置摄像机B3。摄像机B3的机体露在外面，摄像机B3的镜头探入圆柱孔B1内。摄像机B3露出立柱部分被相机罩B4裹住。

其中，滤光片A2与滤光片B2为相同带宽的带通滤波器。只允许特定频段的光谱通过，其他频段的光谱则被滤除。优选地，本发明中两滤光片以940nm为中心频率，带宽为10nm.

优选的，摄像机A3、摄像机B3均为普通CCD摄像机。

安装于机械本体上的两个摄像机，摄像机A3的深度方向也即光轴方向，与摄像机B3的Y方向相同。摄像机B3的深度方向也即光轴方向，与摄像机A3的Y方向相同。环境中的图像通过安装在圆柱孔一侧的滤光片A或滤光片B后，被摄像机A3或摄像机B3的镜头成像于各自的摄像机敏感单元平面，形成图像。

2、人工路标

如图2所示，人工路标在一个平面内，均匀布置3*3的有源红外LED灯。当人工路标放置好之后，有源红外LED灯中心点即人工路标关键点的世界坐标可以通过人工测量记录下来并保存于人工路标世界坐标数据库中。不同位置的人工路标可以点亮不同的有源红外LED灯来区别，这样不同位置的人工路标就有不同的路标ID。

3、处理装置

该处理装置通过软件编程实现，其软件程序存储在可读取的存储介质中，存储介质例如：计算机中的硬盘、光盘或软盘，通过计算机运算获得定位。

如图3所示，处理装置包括第一摄像机子单元，第二摄像机子单元，及双摄像机三维定位融合子单元。第一摄像机子单元与第二摄像机子单元功能相同，只是针对不同摄像机的输入数据。第一摄像机子单元的输入是摄像机A3的图像数据。第二摄像机子单元的输入是摄像机B3的图像数据。

第一摄像机子单元包括：第一采集图像模块、第一检测模块、第一ID识别模块、第一单目视觉三维定位模块、第一数据模块。

第一采集图像模块负责从摄像机中周期性地读出感知的数字图像；第一检测模块根据该数字图像进行人工路标中心点检测，得出人工路标关键点的图像坐标。由于定位装置的滤光片，使得只有人工路标中的红外灯关键点成像成白色光斑，其他背景为单一的黑色。通过图像算法可以检测出人工路标关键点中心的图像坐标。

第一ID识别模块根据检测出的中心点阵列进行人工路标ID的识别，根据识别出的人工路标ID，从第一数据模块的人工路标世界坐标数据库中，查找出对应ID的人工路标关键点的世界坐标。

第一单目视觉三维定位模块，结合人工路标关键点的图像坐标和世界坐标，以及自身的摄像机参数，进行单目视觉的三维定位。单目视觉的三维定位采用PnP法，并将单目视觉定位的结果输出至双摄像机三维定位融合子单元。

第二摄像机子单元包括：第二采集图像模块、第二检测模块、第二ID识别模块、第二单目视觉三维定位模块。

第二采集图像模块负责从摄像机中周期性地读出感知的数字图像；第二检测模块根据该数字图像进行人工路标中心点检测，得出人工路标关键点的图像坐标。由于定位装置的滤光片，使得只有人工路标中的红外灯关键点成像成白色光斑，其他背景为单一的黑色。通过图像算法可以检测出人工路标关键点中心的图像坐标。

第二ID识别模块根据检测出的中心点阵列进行人工路标ID的识别，根据识别出的人工路标ID，从人工路标世界坐标数据库模块中，查找出对应ID的人工路标关键点的世界坐标。

第二单目视觉三维定位模块，结合人工路标关键点的图像坐标和世界坐标，以及自身的摄像机参数，进行单目视觉的三维定位。单目视觉的三维定位采用PnP法，并将单目视觉定位的结果输出至双摄像机三维定位融合子单元。

双摄像机三维定位融合子单元根据两个摄像机子单元的单目视觉定位结果进行优化运算，得到可减小深度误差的三维定位结果。

PnP法单目视觉定位

如图4所示，世界坐标空间中的三维点，经摄像机摄影后在图像平面成像。一个图像点与世界坐标点有如下关系。

$s\tilde{m}=A\left[\begin{array}{cc}R& t\end{array}\right]\tilde{M}---\left(1\right)$

其中

是图像坐标，

是世界坐标系下的坐标。A是摄像机的内参数矩阵，主要包括焦距和光心在图像中的坐标点等参数。公式中实际包含了图像坐标系中的u坐标成像方程和v坐标成像方程。因此在摄像机内参数A已知的情况下只需要最少三个空间点及其成像相对应即可求出外参数[R t]的解，其中R是旋转矩阵，t是平移矩阵，s是比例因子。在本系统中摄像机的内参数A仅与摄像机的硬件有关，可以事先标定成已知参数。

具体地，人工路标经过投影得

$s\tilde{m}=A\left[\begin{array}{cc}I& 0\end{array}\right]T_W^A\tilde{M}---\left(2\right)$

公式中 $T_W^A=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right],$ 其中I是单位矩阵，s是比例因子，A是摄像机的内参数矩阵，是人工路标在世界坐标系中的坐标，通过外部仪器测出。公式中

是人工路标红外灯中心点在图像坐标系中的图像，通过图像处理方法得出。

通过多个点的投影矩阵

$\left\{\begin{array}{c}s{\tilde{m}}_1=A\left[\begin{array}{cc}I& 0\end{array}\right]T_W^A{\tilde{M}}_1\\ s{\tilde{m}}_2=A\left[\begin{array}{cc}I& 0\end{array}\right]T_W^A{\tilde{M}}_2\\ ...\\ s{\tilde{m}}_n=A\left[\begin{array}{cc}I& 0\end{array}\right]T_W^A{\tilde{M}}_n\end{array}\right.---\left(3\right)$

上述公式每一行可求解出两个未知数，^AT_W一共有六个未知数，只需要最少三个点即可求解。点数越多越精确。同理，可以对B相机得出^BT_W。

这种方法在摄像机坐标系的X(或Y)方向的定位误差

$\mathrm{\ΔX}=\frac{\mathrm{Z\Δu}}{f}---\left(4\right)$

其中Δu是像素识别误差，Z是深度，f是焦距。

在深度方向上的误差为

$\mathrm{\ΔZ}=\frac{Z^2\mathrm{\Δu}}{\mathrm{Xf}}---\left(5\right)$

其中，X是靶标在摄像机坐标系x轴方向的最大值

由于深度方向的误差与深度的平方成正比，因此在深度的方向误差比较大。

双目视觉定位理论证明，双目视觉定位在深度方向上的误差为

$\mathrm{\ΔZ}=\frac{Z^2\mathrm{\Δu}}{\mathrm{fb}}---\left(6\right)$

公式中b是两个摄像机光心之间的距离。由于深度方向的误差与深度的平方成正比，因此在深度的方向误差比较大。

双摄像机三维定位融合

系统可通过A相机和B摄像机来分别对智能体上的点进行全局三维定位。由于PnP算法在深度方向误差较大，因此迫切需要通过两个摄像头融合的办法减小误差。如图5所示，摄像机A的深度Z方向与摄像机B的X方向相同，摄像机B的深度Z方向与摄像机A的X方向相差180度。因此可以分别利用两个相机的X,Y来代替Z方向的值。从而实现更为精确的定位。

设智能体中有一点p，它在摄像机A坐标系下的坐标为^Ap，在世界坐标系下的坐标为^Wp，在摄像机B坐标系下的坐标为^Bp。由于点p相对于摄像机三维定位装置是不动的，因此可以在初始状态下获得。因此我们假定^Ap、^Bp为相机的内参数，为已知。求^Wp。可通过如下的公式求解。

$\left\{\begin{array}{c}p_A=T_W^{A}p_W=R_W^{A}p_W+t_W^A\\ p_B=T_W^{B}p=R_W^{B}p_W_W+t_W^B\end{array}\right.---\left(7\right)$

显然，上述公式中平移矩阵^At_W、^Bt_W通过单目PnP三维定位的方法得出。^AT_W为世界坐标系和A相机坐标系之间的转换矩阵、^BT_W为世界坐标系和和B相机坐标系之间的转换矩阵,^AR_W为世界坐标系和A相机坐标系之间的旋转矩阵、^BR_W为世界坐标系和和B相机坐标系之间的旋转矩阵，其中^At_W的Z分量值不准确，本发明采用^Bt_W的X分量代替。而^Bt_W的Z分量不准确。本发明用^At_W的X分量的负值代替。即^At_W转化为^At_W'，^Bt_W转化为^Bt_W'；

$t_W^{\text{'}}{}_A=\left[\begin{array}{c}t_{\mathrm{Wx}}^A\\ t_{\mathrm{Wy}}^A\\ t_{\mathrm{Wx}}^A\end{array}\right]---\left(8\right)$

$t_W^{\text{'}}{}_B=\left[\begin{array}{c}t_{\mathrm{Wx}}^B\\ t_{\mathrm{Wy}}^B\\ -t_{\mathrm{Wx}}^B\end{array}\right]---\left(9\right)$

于是求解公式变为

$\left\{\begin{array}{c}p_A=R_W^{A}p_W+t_W^{\text{'}}{}_A\\ p_B=R_W^{B}p_W+t_W^{\text{'}}{}_B\end{array}\right.---\left(10\right)$

对上述方程进行求解得到^Wp，可得到最优的世界坐标求解。

更为精确地，由于考虑到安装误差，摄像机A和摄像机B的坐标系不一定刚好完全垂直，因此简单地用X代替Z的方法误差仍然存在。为进一步消除误差。我们有

$\left\{\begin{array}{c}t_W^{}{}_A|_Z=(R_B^{A}t_W^B+t_B^A)|_Z\\ t_W^{}{}_B|_Z=(R_A^{B}t_W^A+t_A^B)|_Z\end{array}\right.$

对式（10）进行误差消除处理，使消除误差的最优世界坐标的深度方向的误差与深度成正比，消除摄像机A和摄像机B的坐标系不完全垂直带来的误差：

由于在上述求解过程中，通过PnP求解的深度误差为与深度平方成正比的Z分量，被另一个摄像机的非深度方向所代替，其误差降低为与深度成正比例。从而实现了减小深度误差的目的。

当然，本发明还可有其他多种实施例，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (6)

1.一种基于双摄像机的的智能体视觉三维定位方法，其特征在于，包括：

步骤1，在智能体上设置双摄像机，使第一摄像机的主轴与第二摄像机的主轴互相垂直，则满足条件：第一摄像机坐标系的Z方向与第二摄像机的坐标系下的X方向相同，所述第一摄像机坐标系的X方向与第二摄像机坐标系的Z方向相差180度；

步骤2，两个摄像机对同一对象分别进行单目视觉的三维定位，分别获得第一定位结果、第二定位结果；

步骤3，对所述第一定位结果、第二定位结果进行融合处理，获得最优三维定位结果，融合处理为：

用第二摄像机的X分量代替第一摄像机的Z分量；

用第一摄像机的X负分量代替第二摄像机的Z分量。

2.如权利要求1所述的基于双摄像机的的智能体视觉三维定位方法，其特征在于：

若p为智能体上任意一点，其在第一摄像机的坐标系下的坐标为^Ap，在第二摄像机的坐标系下的坐标为^Bp，在世界坐标系下的坐标为^Wp，A、B分别表示第一、第二摄像机的坐标系，则满足：

$\left\{\begin{array}{c}p_A=R_W^{A}p_W+t_W^A\\ p_B=R_W^{B}p_W+t_W^B\end{array}\right.---\left(1\right)$

通过步骤2的PnP法得出由世界坐标系转化为第一摄像机坐标系的旋转矩阵^AR_w、的平移矩阵^At_W，由世界坐标系转化为第二摄像机坐标系的旋转矩阵^BR_w、平移矩阵^Bt_W，且 $t_W^A=\left[\begin{array}{c}t_{\mathrm{Wx}}^A\\ t_{\mathrm{Wy}}^A\\ t_{\mathrm{Wz}}^A\end{array}\right],t_W^B=\left[\begin{array}{c}t_{\mathrm{Wx}}^B\\ t_{\mathrm{Wy}}^B\\ t_{\mathrm{Wz}}^B\end{array}\right];$

根据步骤3采用^Bt_W的X分量代替^At_W的Z分量，用^At_W的X分量的负值代替^Bt_W的Z分量，获得

$\left\{\begin{array}{c}p_A=R_W^{A}p_W+t_W^{\′}{}_A\\ p_B=R_W^{B}p_W+t_W^{\′}{}_B\end{array}\right.$

其中， $t_W^{\′}{}_A=\left[\begin{array}{c}t_{\mathrm{Wx}}^A\\ t_{\mathrm{Wy}}^A\\ t_{\mathrm{Wx}}^B\end{array}\right],t_W^{\′}{}_B=\left[\begin{array}{c}t_{\mathrm{Wx}}^B\\ t_{\mathrm{Wy}}^B\\ -t_{\mathrm{Wx}}^A\end{array}\right]$

对上述方程进行求解得到^Wp，得到最优的世界坐标。

3.如权利要求1所述的基于双摄像机的的智能体视觉三维定位方法，其特征在于，

若p为智能体上任意一点，其在第一摄像机的坐标系下的坐标为^Ap，在第二摄像机的坐标系下的坐标为^Bp，在世界坐标系下的坐标为^Wp，A、B分别表示第一、第二摄像机的坐标系，则满足：

$\left\{\begin{array}{c}p_A=R_W^{A}p_W+t_W^A\\ p_B=R_W^{B}p_W+t_W^B\end{array}\right.---\left(1\right)$

通过步骤2的PnP法得出由世界坐标系转化为第一摄像机坐标系的旋转矩阵^AR_w、的平移矩阵^At_W，由世界坐标系转化为第二摄像机坐标系的旋转矩阵^BR_w、平移矩阵^Bt_W，且 $t_W^A=\left[\begin{array}{c}t_{\mathrm{Wx}}^A\\ t_{\mathrm{Wy}}^A\\ t_{\mathrm{Wz}}^A\end{array}\right],t_W^B=\left[\begin{array}{c}t_{\mathrm{Wx}}^B\\ t_{\mathrm{Wy}}^B\\ t_{\mathrm{Wz}}^B\end{array}\right];$

利用第二摄像机坐标系到第一摄像机坐标系的旋转矩阵^AR_B、平移矩阵^At_B，将^Bt_W转化为代替^At_W的^At_W″；

利用第一摄像机坐标系到第二摄像机坐标系的旋转矩阵^BR_A、平移矩阵^Bt_A，将^At_W转化为代替式（1）中^Bt_W的^Bt_W″；

$\left\{\begin{array}{c}t_W^{\′\′}{}_A|_Z=(R_B^{A}t_W^B+t_B^A)|_Z\\ t_W^{\′\′}{}_B|_Z=(R_A^{B}t_W^A+t_A^B)|_Z\end{array}\right.$

其中|_Z表示Z分量。

4.一种基于双摄像机的的智能体视觉三维定位装置，其特征在于，包括：

机械本体、两个相同型号的摄像机、人工路标、处理装置；

所述机械本体固定在智能体上，该机械本体为长方体，且该长方体的底面和顶面是正方形，四个侧面以相互平行的平面为一组，分为两组，每组开有圆柱孔，且两组的圆柱孔相互垂直，上下相切；

每个圆柱孔的正前方开槽，放置滤光片，所述圆柱孔的后方放置一个摄像机；该摄像机的镜头探入所述圆柱孔内；

所述人工路标是在一个平面上均匀布置有源红外LED灯的装置，不同位置的人工路标通过点亮不同的有源红外LED灯来区别，获得不同的路标ID；该人工路标的有源红外LED灯中心点的世界坐标通过人工测量后保存于处理装置的数据库中；

所述处理装置包括第一摄像机子单元，第二摄像机子单元，及双摄像机三维定位融合子单元；第一摄像机子单元和第二摄像机子单元均包括：采集图像模块，与采集图像模块连接的检测模块，与检测模块连接的ID识别模块，数据模块，与ID识别模块、数据模块、检测模块分别连接的单目视觉三维定位模块；

其中采集图像模块负责从所属摄像机中周期性地读出所感知的数字图像；检测模块用于根据该数字图像进行人工路标中心点检测，得出人工路标关键点的图像坐标；ID识别模块根据检测出的中心点阵列进行人工路标ID的识别，并根据识别出的人工路标ID从所接数据模块中的人工路标世界坐标数据库中查找出对应ID的人工路标关键点的世界坐标；单目视觉三维定位模块，结合人工路标关键点的图像坐标和世界坐标以及自身的摄像机参数，进行单目视觉的三维定位，并将定位结果输出至双摄像机三维定位融合子单元；双摄像机三维定位融合子单元根据两个摄像机子单元的单目视觉定位结果进行融合运算，融合运算具体为：用第二摄像机的X分量代替第一摄像机的Z分量；用第一摄像机的X负分量代替第二摄像机的Z分量，得到减小深度误差的三维定位结果。

5.如权利要求4所述的基于双摄像机的的智能体视觉三维定位装置，其特征在于，所述两个圆柱孔上放置的滤光片为相同带宽的带通滤波器，该带通滤波器以940nm为中心频率，带宽为10nm。

6.如权利要求4所述的基于双摄像机的的智能体视觉三维定位装置，其特征在于，所述两个摄像机为普通CCD摄像机。

Images