CN106092090B - 一种用于室内移动机器人定位的红外路标及其使用方法 - Google Patents

Abstract

本发明涉及一种用于室内移动机器人定位的红外路标，该红外路标由位置识别标记点和身份识别标记点组成，所述位置识别标记点是由所有标记点的最小外包四边形的四个顶点位置上的标记点组成，且在组成位置识别标记点的四个标记点中，其中一个标记点的尺寸大于其他三个标记点的尺寸；所述身份识别标记点是由除位置标记点之外的其他标记点组成。本发明还公开了一种用于室内移动机器人定位的红外路标的使用方法。本发明采用红外路标作为视觉定位的依据，有效降低环境中可见光照明的影响，提高图像处理算法的效率，增加视觉定位的稳定性。本发明采用四个标记点作为位置识别标记点，增加算法的鲁棒性，灵活性更强。

Description

一种用于室内移动机器人定位的红外路标及其使用方法

技术领域

本发明涉及计算机视觉技术领域，尤其是一种用于室内移动机器人定位的红外路标及其使用方法。

背景技术

室内移动机器人对定位精度要求比较高，通常为厘米级。无线定位对设备的布置具有严格的要求，并且因为室内建筑和家具等造成多路径干涉，导致精度难以满足要求。根据室内环境的特殊性和室内定位要求的多样性，目前研究者已经开发了多种不同的用于室内移动机器人导航的定位方法，主要包括无线定位、激光SLAM、视觉SLAM和路标定位。激光SLAM技术采用激光设备，价格不菲，并且激光SLAM技术抗干扰能力弱，在环境变动幅度较大或环境频繁变动时，无法胜任定位任务。视觉SLAM方法的缺点是可靠性较低，场景频繁改变或者环境的视觉特征过于简单都会造成视觉SLAM方法定位任务失败。

路标定位方法一般分为自然路标和人工路标两种模式。自然路标定位计算复杂，鲁棒性不强，实用性差。人工路标定位包括RFID标签、磁条、带有编码或文字信息的可视化标签等，其中RFID标签和磁条类的路标要求工作距离较近，并且机器人只能沿着固定铺设好的路线移动，灵活性较差。可视化标签通常利用摄像机采集图像，并检测图像中存在的可视化标签，利用标签特征对机器人进行定位，这种方法的特点是成本低、精度高、灵活性强。可视化标签的使用又分为自然光环境下和红外光环境下两种情况，自然光环境下容易受到环境光亮度的影响，使得室内定位的可靠性较低。而红外光下的可视化标签通常采用红外发光或红外反光材料路标，通过在红外摄像机的镜头前增加红外带通滤光片，使得红外摄像机仅捕捉环境中特定波长的路标。这种方式的应用可以有效降低可见光对定位结果的影响，无论室内是否有照明均可以工作。

近几年出现了几种类似的红外路标定位方法，如Stargazer方法，该方法提取路标中构成直角坐标系的三个标记点作为确定机器人位置的依据，并在假设摄像机光轴与路标平面垂直的前提下，实现机器人的定位。而实际定位过程中由于加减速惯性力作用、安装夹角等问题，难免会出现摄像机光轴与路标平面不垂直情况，这种情况下采用两轴倾斜仪来辅助提高定位精度，但是当出现路标所在平面与重力方向不垂直时，这种辅助方法将会失效，影响定位效果。另外，采用三个标记点作为位置识别标记点，未能完整包围整个标记点区域，导致身份识别标记点经常出现误识别现象。目前也出现了采用四个标记点作为定位依据，但是这四个点也未能完整包围整个标记点区域，也存在标记点误识别现象。并且由于两个大的标记点相距较近，在红外摄像机下容易产生干涉现象，导致两个标记点成像连成一片，增加了后续图像处理算法的难度。为解决这个问题只能增加路标的尺寸，这样又会导致路标尺寸过大，对环境的人为影响也更大。

发明内容

本发明的首要目的在于提供一种在室内实现对移动机器人的快速、稳定、精确的定位的用于室内移动机器人定位的红外路标。

为实现上述目的，本发明采用了以下技术方案：一种用于室内移动机器人定位的红外路标，该红外路标由位置识别标记点和身份识别标记点组成，所述位置识别标记点是由所有标记点的最小外包四边形的四个顶点位置上的标记点组成，且在组成位置识别标记点的四个标记点中，其中一个标记点的尺寸大于其他三个标记点的尺寸；所述身份识别标记点是由除位置标记点之外的其他标记点组成；所述最小外包四边形是指包含所有标记点的四边形中面积最小的四边形。

所述位置识别标记点和身份识别标记点均采用红外LED发光灯，或者均采用在表面涂有逆反射材料的标签片。

所述红外路标为正方形，位置识别标记点和身份识别标记点的每个标记点的中心点位于3×3或4×4的等距网格点位置。

所述红外路标贴在天花板上，配有红外光源的摄像机竖直向上固定于在地面上移动的移动机器人的顶部，摄像机和红外路标相对垂直布置，所述摄像机的拍摄视野中存在一个或多个红外路标。

所述红外路标贴于在地面上移动机器人的顶部，配有红外光源的摄像机竖直向下固定在天花板上，摄像机和红外路标相对垂直布置，所述摄像机的拍摄视野中存在一个或多个红外路标。

本发明的另一目的在于提供一种用于室内移动机器人定位的红外路标的使用方法，该方法包括下列顺序的步骤：

(1)将红外路标贴在与摄像机垂直布置的位置处，摄像机采集含有红外路标的红外图像；

(2)对摄像机采集的红外图像进行自适应二值化预处理后得到二值化图像；

(3)根据标记点的尺寸、数量和分布特征，对二值化图像中的所有标记点进行聚类，提取所有候选红外路标；

(4)对每个候选红外路标，提取其四个位置识别标记点；

(5)对每个候选红外路标，接着提取其身份识别标记点，识别红外路标的身份ID；

(6)计算摄像机坐标系与红外路标坐标系之间的相对位置，得到摄像机相对于每个路标坐标系的位置和方位角；

在步骤(3)中，所述对二值化图像中的所有标记点进行聚类是指，首先，提取每个高亮像素块的中心，依据红外路标的标记点数量和分布特征，提取出候选红外路标的标记点集合；其次，对每个标记点集合进一步检测，判断是否存在四个位置识别标记点，若判断结果为是，则进入步骤(4)，提取四个位置识别标记点，否则，不做检测；

在步骤(4)中，所述对每个候选红外路标提取其四个位置识别标记点是指，对候选红外路标的标记点集合，以固定角度逐次按逆时针或顺时针旋转，直至旋转超过180°，对每次旋转的标记点集合提取最小外接矩形，对该外接矩形经过的标记点进行统计，最后将出现次数最多的前四位标记点作为可能的四个位置识别标记点；这四个位置识别标记点依次连接构成整个路标的最小外包四边形，而最小外包四边形分布特征为近似平行四边形，其两组对边长度的比值接近于1；最后对四个位置识别标记点按照其分布特征进行约束,提高检测准确性；

在步骤(5)中，所述提取其身份识别标记点是指，根据四个位置识别标记点的双线性插值，生成内部多个网格点，以每个网格点为中心，一定阈值范围为半径，生成一个可能的检测范围；当每个范围内存在标记点时，则将该红外路标的二进制身份编码相应位置置为1，否则置为0；

步骤(6)的计算方法如下：首先，建立摄像机坐标系和红外路标坐标系，假设空间中一点B在图像上成像为b，b在图像上的像素坐标为(u，v),摄像机坐标系的原点为摄像机光轴与感光芯片的交点，X坐标为平行图像水平方向U，Y坐标为平行图像垂直方向V，Z坐标为沿着光轴向外的方向，则OcXcYcZc为摄像机坐标系；

红外路标坐标系的原点设为四个位置标记点中尺寸最大的标记点中心位置，按右手螺旋法则依次定义X、Y和Z方向，OC为X方向，OA为Y方向，垂直向外的方向为Z方向，红外路标坐标系表示为OmXmYmZm；在红外路标坐标系下确定红外路标中四个位置识别标记点，OABC四个位置的坐标分别为(0,0,0)，(0，L,0)，(L，L,0)和(L，0,0)，其中L为OA的实际距离；

假设图像坐标系下一点p，其像素坐标为(u,v)，该点在摄像机坐标系下的坐标为(x_c,y_c,z_c)，两者之间满足下面的关系：

其中，矩阵M₁为预先设定的摄像机内参矩阵,其元素α_x,α_y,u₀,v₀为相机的内参数；

摄像机坐标系到红外路标坐标系的刚性变换关系表示为旋转矩阵R和平移向量t，满足下面的关系：

其中，[X_c,Y_c,Z_c,1]^T和[X_m,Y_m,Z_m,1]^T分别为同一空间点在摄像机坐标系和红外路标坐标系下的齐次坐标，矩阵M₂为摄像机坐标系到红外路标坐标系之间的变换矩阵，其中旋转矩阵R和平移向量t即为待求解的未知量；

将(2)式带入(1)式中，得到：

设位置识别标记点OABC在路标坐标系下坐标为(X_mi,Y_mi,Z_mi)，i＝1,2,3,4，其图像坐标为(u_i,v_i)，i＝1,2,3,4，在摄像机坐标系下坐标为(X_ci,Y_ci,Z_ci)，i＝1,2,3,4，带入(3)式得到四个方程：

方程组(4)共有12个方程，10独立变量，利用优化算法求解旋转矩阵R和平移向量t，通过求解得到的R和t值，得到红外路标坐标系的原点在摄像机坐标系下的坐标为t,而摄像机坐标系下的原点在红外路标坐标系下的位置坐标为-R^Tt，摄像机坐标系相对于路标坐标系下的方位角即为-R^T的欧拉角。

由上述技术方案可知，本发明采用红外路标作为视觉定位的依据，有效降低环境中可见光照明的影响，提高图像处理算法的效率，增加视觉定位的稳定性。此外，本发明采用四个标记点作为位置识别标记点，增加算法的鲁棒性，并且无需要求摄像机光轴必须垂直于红外路标所在平面的条件，不仅可以计算摄像机相对于红外路标坐标系之间的位置和方位角，还可以计算出摄像机相对于红外路标坐标系的倾角，灵活性更强。本发明采用四个标记点作为位置识别标记点，这四个点构成红外路标所有标记点的最小外包四边形，即所有标记点均处于由这四个标记点组成的四边形的内部，根据这四个标记点的构成条件，再检测内部的身份识别标记点，有效提高识别的稳定性，降低误识别率。根据这四个标记点的构成条件，再检测内部的身份识别标记点，有效提高路标识别的稳定性，降低误识别率。

附图说明

图1为本发明中红外路标的示意图；

图2、3均为本发明中两种典型的红外路标示意图；

图4为本发明中红外路标贴在天花板上，摄像机安装在机器人上的工作示意图；

图5为本发明中红外路标贴在机器人上，摄像机安装在天花板上的工作示意图；

图6为本发明的方法流程图；

图7(a)红外图像中的路标标记点集合P，最小外接矩形检测结果示意图；

图7(b)为红外图像中的路标标记点集合P逆时针旋转15°，最小外接矩形检测结果示意图；

图7(c)为红外图像中的路标标记点集合P逆时针旋转30°，最小外接矩形检测结果示意图；

图7(d)为红外图像中的路标标记点集合最终的最小外包四边形检测结果示意图；

图8为本发明中从红外路标中查找身份识别标记点示意图；

图9为本发明中的摄像机坐标系示意图；

图10为本发明中的红外路标坐标系示意图；

图11为本发明中摄像机坐标系相对于视野中红外路标坐标系的位置关系示意图。

具体实施方式

如图1所示，一种用于室内移动机器人定位的红外路标，该红外路标由位置识别标记点1和身份识别标记点2组成，所述位置识别标记点1是由所有标记点的最小外包四边形的四个顶点位置上的标记点组成，且在组成位置识别标记点1的四个标记点中，其中一个标记点O的尺寸大于其他三个标记点A、B、C的尺寸；所述身份识别标记点2是由除位置标记点之外的其他标记点组成。所述位置识别标记点1和身份识别标记点2均采用红外LED发光灯，或者均采用在表面涂有逆反射材料的标签片；所述最小外包四边形是指包含所有标记点的四边形中面积最小的四边形。

如图2所示，所述红外路标为正方形，位置识别标记点1和身份识别标记点2的每个标记点的中心点位于3×3的等距网格点位置。以3×3模式为例，该路标由九个标记点组成，每个标记点的中心点位于3×3等距网格点位置，其中四个矩形顶点位置的标记点构成了位置识别标签，这四个标记点中的三个标记点尺寸相同，另一个标记点尺寸要略大。每个红外路标都必须包括四个位置识别标记点1，而其它网格位置的标记点作为身份识别标签，根据是否存在作为相互区别的依据。3×3模式路标共有9个标记点，去除4个位置固定的位置识别标记点1，还剩下5个身份识别标签，因此共有2^5＝32种标签组合，同理，4×4模式标签则可以有2^(16-4)＝4096种标签组合。如图3所示，所述红外路标为正方形，位置识别标记点1和身份识别标记点2的每个标记点的中心点位于4×4的等距网格点位置。

如图4所示，所述红外路标贴在天花板上，配有红外光源的摄像机4竖直向上固定于在地面上移动的移动机器人3的顶部，摄像机4和红外路标相对垂直布置，所述摄像机4的拍摄视野中存在一个或多个红外路标M1,M2,M3。如图5所示，所述红外路标贴于在地面上移动机器人3的顶部，配有红外光源的摄像机4竖直向下固定在天花板上，摄像机4和红外路标相对垂直布置，所述摄像机4的拍摄视野中存在一个或多个红外路标M4,M5,M6。通过红外摄像机4采集场景中的图像，可以有效去除可见光的干扰，突出红外路标的成像效果，增加定位的稳定性。

红外路标可以是一个或多个，红外路标的数量和布局需要根据机器人3的移动范围以及摄像机4的视野大小和摄像机4离天花板的高度综合考虑，以机器人3在可移动范围内不存在视野盲区为原则。红外光源一般采用近红外光，如850nm波长发光二极管，为降低环境可见光的干扰，通常在摄像机4镜头前安装相应波段的窄带滤光片，从而使得摄像机4基本上仅对红外路标成像，排除其他可见光反射的干扰像素，降低图像处理算法的复杂度。为增加摄像机4的观察视野，通常采用广角镜头，如2.2mm或2.5mm的广角镜头。另外，如果采用红外反光材料构成的路标，还需要增加红外二极管发射角度，使得在红外二极管发射角度基本与广角镜头的视角相符合，才能确保摄像机4可以有效地采集视野内任何地方的红外路标。

如图6所示，用于室内移动机器人3定位的红外路标的使用方法包括下列顺序的步骤：

(1)将红外路标贴在与摄像机4垂直布置的位置处，摄像机4采集含有红外路标的红外图像；

(2)对摄像机4采集的红外图像进行自适应二值化预处理后得到二值化图像；

(3)根据标记点的尺寸、数量和分布特征，对二值化图像中的所有标记点进行聚类，提取所有候选红外路标；

(4)对每个候选红外路标，提取其四个位置识别标记点1；

(5)对每个候选红外路标，接着提取其身份识别标记点2，识别红外路标的身份ID；

(6)计算摄像机4坐标系与红外路标坐标系之间的相对位置，得到摄像机4相对于每个路标坐标系的位置和方位角。

在步骤(2)中，二值化的目的是提取图像中高亮的反射标签。通常，图像中除了反射标签外，其余部分均为黑色的背景，因此一个简单的阈值方法即可将高亮像素从黑色背景中分割出来。但是场景中的一些容易反光的物品，如日光灯的金属外壳、风扇、出风口的金属部件等也会对红外光源产生反射，并在图像中形成高亮的像素，高亮像素也不全是反射标签。

在步骤(3)中，所述对二值化图像中的所有标记点进行聚类是指，首先，提取每个高亮像素块的中心，依据红外路标的标记点数量和分布特征，提取出候选红外路标的标记点集合；其次，对每个标记点集合进一步检测，判断是否存在四个位置识别标记点1，若判断结果为是，则进入步骤(4)，提取四个位置识别标记点1，否则，不做检测。

在步骤(4)中，所述对每个候选红外路标提取其四个位置识别标记点1是指，对候选红外路标的标记点集合，以固定角度逐次按逆时针或顺时针旋转，直至旋转超过180°，对每次旋转的标记点集合提取最小外接矩形，对该外接矩形经过的标记点进行统计，最后将出现次数最多的前四位标记点作为可能的四个位置识别标记点1。图7(b)和图7(c)是由图7(a)依次逆时针旋转15°所得到的标记点集合，两次得到最小外接矩形都恰好经过位置识别标记点1。图7(d)为图7(a)标记点集合最终检测结果，得到了四个位置识别标记点1，这四个位置识别标记点1依次连接构成整个路标的最小外包四边形，而最小外包四边形分布特征为近似平行四边形，其两组对边长度的比值接近于1；最后对四个位置识别标记点1按照其分布特征进行约束,提高检测准确性。在图7(a)、。图7(b)、图7(c)和图7(d)中，黑色虚线方框均是指最小外接矩形。

在步骤(5)中，所述提取其身份识别标记点2是指，根据四个位置识别标记点1的双线性插值，生成内部多个网格点，以每个网格点为中心，一定阈值范围为半径，生成一个可能的检测范围；当每个范围内存在标记点时，则将该红外路标的二进制身份编码相应位置置为1，否则置为0。以3×3模式为例，根据这四个位置识别标记点1的双线性插值点E，可以生成内部5个网格点，以每个网格点为中心，一定阈值范围为半径，生成一个可能检测范围F，根据图7(d)的检测结果，生成身份识别标记点2检测示意图，如图8所示。当每个范围内存在标记点时，则将该路标的二进制身份编码相应位置置为1，否则置为0。以3×3模式路标为例，路标身份信息可以设置为5位的二进制编码，可以生成32种不同的路标身份信息，4×4模式路标的身份信息可以设置为12位的二进制编码，可以生成4096中不同的路标身份信息。

当红外路标的位置识别标记点1检测失败时，则无需进行身份识别标记点2检测，当红外路标的位置识别标记点1检测成功时，通常最小外包四边形内部的标记点不会出现漏检和多检的情况，因此按照本发明提供的方法，很少会出现身份识别错误的情况。

步骤(6)的计算方法如下：首先，建立摄像机4坐标系和红外路标坐标系，如图9所示，假设空间中一点B在图像上成像为b，b在图像上的像素坐标为(u，v),摄像机4坐标系的原点为摄像机4光轴与感光芯片的交点，X坐标为平行图像水平方向U，Y坐标为平行图像垂直方向V，Z坐标为沿着光轴向外的方向，如图9中的OcXcYcZc为摄像机4坐标系；

红外路标坐标系的原点设为四个位置标记点中尺寸最大的标记点中心位置，按右手螺旋法则依次定义X、Y和Z方向，如图10所示，OC为X方向，OA为Y方向，垂直向外的方向为Z方向，红外路标坐标系表示为OmXmYmZm；在红外路标坐标系下确定红外路标中四个位置识别标记点1，OABC四个位置的坐标分别为(0,0,0)，(0，L,0)，(L，L,0)和(L，0,0)，其中L为OA的实际距离；

假设图像坐标系下一点p，其像素坐标为(u,v)，该点在摄像机4坐标系下的坐标为(x_c,y_c,z_c)，两者之间满足下面的关系：

其中，矩阵M₁为预先设定的摄像机内参矩阵,其元素α_x,α_y,u₀,v₀为相机的内参数；

摄像机4坐标系到红外路标坐标系的刚性变换关系表示为旋转矩阵R和平移向量t，满足下面的关系：

其中，[X_c,Y_c,Z_c,1]^T和[X_m,Y_m,Z_m,1]^T分别为同一空间点在摄像机坐标系和红外路标坐标系下的齐次坐标，矩阵M₂为摄像机坐标系到红外路标坐标系之间的变换矩阵，其中旋转矩阵R和平移向量t即为待求解的未知量；

将(2)式带入(1)式中，得到：

如图9所示，设位置识别标记点OABC在路标坐标系下坐标为(X_mi,Y_mi,Z_mi)，i＝1,2,3,4，其图像坐标为(u_i,v_i)，i＝1,2,3,4，在摄像机坐标系下坐标为(X_ci,Y_ci,Z_ci)，i＝1,2,3,4，带入(3)式得到四个方程：

方程组(4)共有12个方程，10独立变量，利用优化算法求解旋转矩阵R和平移向量t，如图11所示。通过求解得到的R和t值，得到红外路标坐标系的原点在摄像机4坐标系下的坐标为t,而摄像机4坐标系下的原点在红外路标坐标系下的位置坐标为-R^Tt，摄像机4坐标系相对于路标坐标系下的方位角即为-R^T的欧拉角。

综上所述，本发明提供用于确定机器人3位置信息的红外路标，通过红外摄像机4可以拍摄路标图像，并能利用图像处理算法计算机器人3的位置信息，红外路标在室内环境有无照明的条件下均可以稳定使用。本发明还提供用于检测路标并提取路标中四个用于定位标记点的提取方法，通过红外照相机获得路标的图像，对图像进行简单预处理后可以准确获得每个路标的四个定位标记点和其它身份识别标记点2。本发明提供利用位置识别标记点1实现机器人3定位的方法，通过标记点提取方法，利用摄像机4事先标定好的参数，可以快速计算机器人3的位置信息，并根据身份识别标记点2计算得到该路标的唯一身份信息ID，最终可以获得摄像机4视觉内每个不同ID路标的相对位置信息。

Claims (2)

1.一种用于室内移动机器人定位的红外路标，其特征在于：该红外路标由位置识别标记点和身份识别标记点组成，所述位置识别标记点是由所有标记点的最小外包四边形的四个顶点位置上的标记点组成，且在组成位置识别标记点的四个标记点中，其中一个标记点的尺寸大于其他三个标记点的尺寸；所述身份识别标记点是由除位置标记点之外的其他标记点组成；所述最小外包四边形是指包含所有标记点的四边形中面积最小的四边形；

所述位置识别标记点和身份识别标记点均采用红外LED发光灯，或者均采用在表面涂有逆反射材料的标签片；

所述红外路标为正方形，位置识别标记点和身份识别标记点的每个标记点的中心点位于3×3或4×4的等距网格点位置；

所述红外路标贴在天花板上，配有红外光源的摄像机竖直向上固定于在地面上移动的移动机器人的顶部，摄像机和红外路标相对垂直布置，所述摄像机的拍摄视野中存在一个或多个红外路标；

所述红外路标贴于在地面上移动机器人的顶部，配有红外光源的摄像机竖直向下固定在天花板上，摄像机和红外路标相对垂直布置，所述摄像机的拍摄视野中存在一个或多个红外路标。

2.根据权利要求1所述的用于室内移动机器人定位的红外路标的使用方法，该方法包括下列顺序的步骤：

(1)将红外路标贴在与摄像机垂直布置的位置处，摄像机采集含有红外路标的红外图像；

(2)对摄像机采集的红外图像进行自适应二值化预处理后得到二值化图像；

(3)根据标记点的尺寸、数量和分布特征，对二值化图像中的所有标记点进行聚类，提取所有候选红外路标；

(4)对每个候选红外路标，提取其四个位置识别标记点；

(5)对每个候选红外路标，接着提取其身份识别标记点，识别红外路标的身份ID；

(6)计算摄像机坐标系与红外路标坐标系之间的相对位置，得到摄像机相对于每个路标坐标系的位置和方位角；

在步骤(3)中，所述对二值化图像中的所有标记点进行聚类是指，首先，提取每个高亮像素块的中心，依据红外路标的标记点数量和分布特征，提取出候选红外路标的标记点集合；其次，对每个标记点集合进一步检测，判断是否存在四个位置识别标记点，若判断结果为是，则进入步骤(4)，提取四个位置识别标记点，否则，不做检测；

在步骤(4)中，所述对每个候选红外路标提取其四个位置识别标记点是指，对候选红外路标的标记点集合，以固定角度逐次按逆时针或顺时针旋转，直至旋转超过180°，对每次旋转的标记点集合提取最小外接矩形，对该外接矩形经过的标记点进行统计，最后将出现次数最多的前四位标记点作为可能的四个位置识别标记点；这四个位置识别标记点依次连接构成整个路标的最小外包四边形，而最小外包四边形分布特征为近似平行四边形，其两组对边长度的比值接近于1；最后对四个位置识别标记点按照其分布特征进行约束,提高检测准确性；

在步骤(5)中，所述提取其身份识别标记点是指，根据四个位置识别标记点的双线性插值，生成内部多个网格点，以每个网格点为中心，一定阈值范围为半径，生成一个可能的检测范围；当每个范围内存在标记点时，则将该红外路标的二进制身份编码相应位置置为1，否则置为0；

步骤(6)的计算方法如下：首先，建立摄像机坐标系和红外路标坐标系，假设空间中一点B在图像上成像为b，b在图像上的像素坐标为(u，v),摄像机坐标系的原点为摄像机光轴与感光芯片的交点，X坐标为平行图像水平方向U，Y坐标为平行图像垂直方向V，Z坐标为沿着光轴向外的方向，则OcXcYcZc为摄像机坐标系；

红外路标坐标系的原点设为四个位置标记点中尺寸最大的标记点中心位置，按右手螺旋法则依次定义X、Y和Z方向，OC为X方向，OA为Y方向，垂直向外的方向为Z方向，红外路标坐标系表示为OmXmYmZm；在红外路标坐标系下确定红外路标中四个位置识别标记点，OABC四个位置的坐标分别为(0,0,0)，(0，L,0)，(L，L,0)和(L，0,0)，其中L为OA的实际距离；

假设图像坐标系下一点p，其像素坐标为(u,v)，该点在摄像机坐标系下的坐标为(x_c,y_c,z_c)，两者之间满足下面的关系：

其中，矩阵M₁为预先设定的摄像机内参矩阵,其元素α_x,α_y,u₀,v₀为相机的内参数；

摄像机坐标系到红外路标坐标系的刚性变换关系表示为旋转矩阵R和平移向量t，满足下面的关系：

其中，[X_c,Y_c,Z_c,1]^T和[X_m,Y_m,Z_m,1]^T分别为同一空间点在摄像机坐标系和红外路标坐标系下的齐次坐标，矩阵M₂为摄像机坐标系到红外路标坐标系之间的变换矩阵，其中旋转矩阵R和平移向量t即为待求解的未知量；

将(2)式带入(1)式中，得到：

设位置识别标记点OABC在路标坐标系下坐标为(X_mi,Y_mi,Z_mi)，i＝1,2,3,4，其图像坐标为(u_i,v_i)，i＝1,2,3,4，在摄像机坐标系下坐标为(X_ci,Y_ci,Z_ci)，i＝1,2,3,4，带入(3)式得到四个方程：

方程组(4)共有12个方程，10独立变量，利用优化算法求解旋转矩阵R和平移向量t，通过求解得到的R和t值，得到红外路标坐标系的原点在摄像机坐标系下的坐标为t,而摄像机坐标系下的原点在红外路标坐标系下的位置坐标为-R^Tt，摄像机坐标系相对于路标坐标系下的方位角即为-R^T的欧拉角。

Images