人工路标导航方式 篇1：
单目视觉人工路标的辅助INS的导航定位方法
第一、技术领域
本发明属于定位导航领域，尤其涉及一种单目视觉人工路标的辅助INS的导航定位方法。
第二、背景技术
传统的定位方法主要包括卫星、惯性导航定位等，卫星导航定位应用广泛、精度高，但非常容易受到环境的影响，在室内无卫星信号和卫星信号有遮挡区域都将无法使用。惯性导航系统是一种自主式的导航系统，其优点是可提供连续的导航信息，短期的定位精度和稳定性好，但是由于导航信息是由积分产生，定位误差随时间而增大，不能长期独立工作。
近年来，视觉导航作为新的定位方法得到越来越多的关注。视觉里程计(visualodometer，VO)的概念是由Nistér提出，它是通过相邻图像匹配特征点坐标的变化来估计载体的位姿变化情况。因为VO是通过一系列前后帧的对比来估计相机的运动情况，所以不可避免的会造成误差的累计。基于视觉的同时定位与地图构建技术(visual simultaneouslocalization and mapping，VSLAM)，由VO前端、后端优化、回环检测以及建图几个部分组成，在机器人自主定位和路径规划中起到重要的作用。VSLAM相比于VO计算量大大提高，对数据处理终端计算能力有很高的要求，很难实现实时定位与建图。
第三、发明内容
本发明主要解决了传统室内定位方法定位误差会随着时间增长而增大的问题，提出成本低、操作简单、准确性高的单目视觉人工路标的辅助INS的导航定位方法。
本发明解决其技术问题所采用的技术方案是，一种单目视觉人工路标的辅助INS的导航定位方法，包括以下步骤：
S1：设计人工路标;
S2：标定尺度因子;
S3：建立视觉路标库;
S4：基于路标库对相机位置和姿态进行单目视觉估计;
S5：单目视觉人工路标辅助INS定位。
本发明在已知位置设置人工路标图像，建立视觉路标库，根据视觉路标库中图像与当前采集图像的关系，结合已知的路标库图像获取时的相机位置和姿态从而获得当前相机的位置和姿态，利用相机的定位信息对INS定位数据进行误差修正，从而减小INS的误差累积，使定位更加精确。
作为上述一种优选方案，所述步骤S1人工路标外围尺寸为R，正中设有用于路标标号的二维码，二维码到人工路标边缘的区域设有标识图案。标识图案能够使人工路标具有足够多的ORB特征点。
作为上述一种优选方案，所述步骤S2中标定尺度因子，包括以下步骤：
S21：利用相机在已知距离D正对人工路标进行拍摄;
S22：获取人工路标的像素尺寸r;
S23：根据小孔成像原理，获取尺度因子f
f=D*r/R。
作为上述一种优选方案，所述步骤S3中建立视觉路标库，包括以下步骤：
S31：将多个人工路标设置在带有相机的载体的移动线路上;
S32：使相机在已知位置正对人工路标进行拍摄，记录相机位置、姿态;
S33：检测拍摄图像中的路标区域以及像素尺寸，根据尺度因子得到路标到相机的距离，并对拍摄图像中的非人工路标区域进行填充;
S34：将相机位置、姿态、相机到路标的距离及填充后的图像对应存入到视觉路标库中。
作为上述一种优选方案，所述步骤S4中基于路标库对相机位置和姿态进行帧间位姿估计，包括以下步骤：
S41：根据相机拍摄图像中人工路标的像素尺寸获得相机到人工路标的距离;
S42：识别拍摄图像中人工路标中的二维码，在视觉路标库中寻找对应的人工路标数据;
S43：对拍摄图像中非人工路标区域进行填充，与路标库中对应图像进行特征提取和匹配;
S44：根据匹配的特征点像素坐标和相机到路标的距离得到路标图像中匹配特征点在相机坐标系下的三维点集合P={p1，...，pn}，以及当前图像中匹配特征点在相机坐标系下的三维点集合P′={p′1，...，p′n};
S45：匹配的三维点之间满足欧式变化，使得
pi=Rp′i+t
解上式，获得旋转矩阵R和平移向量t;
S46：结合旋转矩阵R和平移向量t及已知的库图像获取时相机的位置、姿态信息，获得相机拍摄当前图像时的位置和姿态信息。
作为上述一种优选方案，所述步骤S44中通过以下方法获取匹配点的三维点集合：在相机成像的像平面上建立像素坐标系，以相机光心为原点建立三维相机坐标系，已知图像上特征点p′在像素坐标系下坐标(u，v)，得实际特征点p在三维相机坐标系上的坐标(X，Y，Z)
(cx，cy)为光心于像平面上的投影点在像素坐标系上的坐标，以光心于像平面上的投影点为原点建立像坐标系，图像上特征点p′在像坐标系的坐标为(x′，y′)，根据像坐标系和像素坐标系的关系可得
其中f1为相机焦距，αf1=fx，βf1=fy，α为图像上特征点p′的像素坐标系横坐标在像坐标系横坐标轴上的缩放倍数，β为图像上特征点p′的像素坐标系纵坐标在像坐标系纵坐标轴上的缩放倍数。
作为上述一种优选方案，所述步骤S45中通过奇异值分解的方法获得旋转矩阵R和平移向量t：计算当前图像和库图像中匹配点的质心位置p，p′，再计算每个点的去质心坐标
qi=pi-p，q′i=p′i-p′
计算旋转矩阵R和平移向量t
根据旋转矩阵R求平移向量t
t*=p-Rp′。
作为上述一种优选方案，所述S5中单目视觉人工路标辅助INS定位：以INS航位推算公式为预测模型建立状态方程
以单目视觉人工路标定位方式为观测模型建立观测方程
Yk=HXk+v(k)
其中，Xk、Xk-1为小车相邻的时刻对应的状态向量，Xk=[xk，yk，zk]，(xk，yk，zk)为小车当前时刻在三维导航坐标系上的坐标，三维导航坐标系为载体所处活动空间的坐标系;A为状态转移矩阵;w(k)为系统噪声误差;Yk为观测值，即人工视觉路标定位结果;H为观测矩阵;v(k)为观测过程中的噪声误差;得到状态更新方程
Kk为Kalman增益;是由状态方程得到的该时刻状态估计量;为由观测方程得到的该时刻的观测量;即为融合后的位置信息。
本发明的优点是：利用已知视觉路标库信息来推算采集当前人工路标图像时相机的位置和姿态信息，将相机当前位置和INS数据利用卡尔曼滤波算法进行融合，能够减小传统INS定位方法中累积误差，使定位更准确、成本低、操作简单。
第四、附图说明
图1为本发明的一种流程示意图。
图2为本发明中人工路标的一种结构示意图。
图3为本发明中标定尺寸因子的一种流程示意图。
图4为本发明中建立视觉路标库的一种流程示意图。
图5为本发明中基于路标库对相机位置和姿态进行帧间位姿估计的一种流程示意图。
图6为本发明中相机坐标系模型。
图7为本发明中帧间运动估计图。
第五、具体实施方式
下面通过实施例，并结合附图，对本发明的技术方案作进一步的说明。
实施例：
本实施例一种单目视觉人工路标的辅助INS的导航定位方法，如图1所示，包括以下步骤：
S1：设计人工路标;如图2所示，人工路标为已知外围半径R的圆形，便于使用霍夫圆变换对圆形路标进行检测;在圆内设置路标标号的二维码，便于路标的识别。在圆形路标区域内设计各种标识图案，使得路标区域内有足够多的ORB特征点，用于之后的特征提取和匹配，不同路标只需替换中间区域的标号二维码。
S2：标定尺度因子，如图3所示，包括以下步骤：
S21：利用相机在已知距离D正对人工路标进行拍摄;
S22：对拍摄的图像采用霍夫圆变换检测圆形路标的像素半径，获取人工路标的像素半径r，已知人工路标实际半径R;
S23：根据小孔成像原理
R/r=D/f
，获取尺度因子f
f=D*r/R
根据尺度因子，在检测到人路标的像素半径为r′时，可知相机到路标的距离为
d=f*R/r′。
S3：建立视觉路标库，如图4所示，包括以下步骤：
S31：将所有人工路标分别设置在带有相机的小车移动线路的墙壁上，高度和相机距离地面的高度相同;
S32：使相机在已知位置正对依次人工路标进行拍摄，记录相机位置、姿态;
S33：利用霍夫圆变换检测检测拍摄图像中的路标区域以及像素半径，根据S23中的尺度因子得到路标到相机的距离，并对拍摄图像中的非人工路标区域用黑色进行填充;
S34：将相机位置、姿态、相机到路标的距离及填充后的图像对应存入到视觉路标库中;
S4：基于路标库对相机位置和姿态进行帧间位姿估计，如图5所示，包括以下步骤：
S41：根据相机拍摄图像中人工路标的像素尺寸获得相机到人工路标的距离;
S42：识别拍摄图像中人工路标中的二维码，在视觉路标库中寻找对应的人工路标数据;
S43：对拍摄图像中非人工路标区域进行黑色填充，与路标库中对应图像进行ORB特征提取和匹配;
S44：根据匹配的特征点像素坐标和相机到路标的距离得到路标图像中匹配特征点在相机坐标系下的三维点集合P={p1，...，pn}，以及当前图像中匹配特征点在相机坐标系下的三维点集合P′={p′1，...，p′n}。三维点集合获取方法如下：如图6所示在相机成像的像平面上建立像素坐标系o″-uv，以相机光心为原点建立三维相机坐标系o-xyz，已知图像上特征点p′在像素坐标系下坐标(u，v)，得实际特征点p在三维相机坐标系上的坐标(X，Y，Z)
(cx，cy)为光心于像平面上的投影点在像素坐标系上的坐标，以光心于像平面上的投影点为原点建立像坐标系o′-x′y′，图像上特征点p′在像坐标系的坐标为(x′，y′)，根据像坐标系和像素坐标系的关系可得
其中f1为相机焦距，αf1=fx，βf1=fy，α为图像上特征点p′的像素坐标系横坐标在像坐标系横坐标轴上的缩放倍数，β为图像上特征点p′的像素坐标系纵坐标在像坐标系纵坐标轴上的缩放倍数。
S45：匹配的三维点之间满足欧式变化，使得
pi=Rp′i+t
通过奇异值分解的方法解上式，获得旋转矩阵R和平移向量t，求解过程如下：
计算当前图像和库图像中匹配点的质心位置p，p′，再计算每个点的去质心坐标
qi=pi-p，q′i=p′i-p′
计算旋转矩阵R和平移向量t
根据旋转矩阵R求平移向量t
t*=p-Rp′。
S46：如图7所示，结合由奇异值分解(SVD)方法获取旋转矩阵R和平移向量t及已知的库图像获取时相机的位置、姿态信息获得相机拍摄当前图像时的位置和姿态信息;
S5：单目视觉人工路标辅助INS定位，以INS/里程计作为小车的主要导航方式，而将基于视觉人工路标的定位方式作为辅助，对INS/里程计定位数据进行误差修正。因为由视觉求解的姿态变化较为准确，这里直接用视觉得到的航向角对惯性导航向的累积误差进行修正，而对位置信息则采用Kalman滤波的方式进行融合：以INS航位推算公式为预测模型建立状态方程
以单目视觉人工路标定位方式为观测模型建立观测方程
Yk=HXk+v(k)
其中，Xk、Xk-1为小车相邻的时刻对应的状态向量，Xk=[xk，yk，zk]，(xk，yk，zk)为小车当前时刻在三维导航坐标系上的坐标，导航三维坐标系为小车所处活动空间的坐标系;A为状态转移矩阵;w(k)为系统噪声误差;Yk为观测值，即人工视觉路标定位结果;H为观测矩阵;v(k)为观测过程中的噪声误差;
得到状态更新方程
其中，Kk为Kalman增益;是由状态方程得到的该时刻状态估计量;为由观测方程得到的该时刻的观测量;即为融合后的位置信息。
本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。
人工路标导航方式 篇2：
基于人工路标的室内机器人导航方法及装置
第一、技术领域
本发明涉及机器人导航技术领域，尤其涉及一种基于人工路标的室内机器人导航方法 及装置。
第二、背景技术
近年来，由于人工智能的不断发展，在多学科、多领域互相渗透的的基础上，越来 越多的人开始关注基于导航技术领域的机器人研究。智能机器人是在复杂的环境下，具 有环境感知能力、动态决策能力、运动控制能力等多种能力的综合系统。它运用了传感 器技术、自动控制技术、信息处理技术、计算机技术以及人工智能技术等多学科的技术 成果，代表人工智能的最高技术水准，是目前科学技术发展较快的领域之一。
随着机器人研究水平的不断提高，智能机器人的应用范围也逐渐变大。在机器人的 相关技术中，导航技术是其核心技术，也是实现自主运行、自主组织、自主规划的关键 技术。现在最常见的导航技术有电磁导航、惯性导航、视觉导航、无线导航、传感器导 航、卫星导航等等。这些导航方式存在这样那样的短处。电磁导航定位不准、灵活性不 高且要高密度铺设磁条维护起来成本很高。惯性导航也存在定位准确度低，而且运动过 程中极易产生偏差，不利于长时间长距离的移动。对于视觉导航，又因为运动时实时运 算量较大，而且受光照影响较大，不尽人意。其他传统的导航方式也存在着稳定性差或 者维护成本高等缺点。
第三、发明内容
本发明的目的是提供一种基于人工路标的室内机器人导航方法及装置，实现了在室 内复杂环境下的精确导航。
本发明的目的是通过以下技术方案实现的：
一种基于人工路标的室内机器人导航装置，包括：设置在室内天花板上的人工路 标，以及设置在机器人内部的人机交互管理模块、运动控制模块、地图管理与路径规划 模块、图像采集与处理模块;其中：
人机交互管理模块，用于实现人机交互的通讯、配置和管理;在初始阶段，由用户 通过人机交互管理模块输入目标位置，所述人机交互管理模块将输入的目标位置传输给 地图管理与路径规划模块;
地图管理与路径规划模块，用于室内地图的构建以及机器人走行至目标位置的路径 规划;所构建的室内地图中各个人工路标的位置信息与设置在室内天花板上的人工路标 位置信息一致;
图像采集与处理模块，用于采集设置在室内天花板上的人工路标的图像，并根据所 采集到的人工路标的图像获取机器人的当前位置;
所述运动控制模块，用于根据地图管理与路径规划模块规划的路径，以及图像采集 与处理模块获取的机器人的当前位置来调整机器人的姿态和控制其运动状态，使机器人 朝着目标位置运动。
进一步的，该装置还包括：安全报警模块，用于定时检测机器人自身的安全状态， 当发生异常时执行相应的报警处理。
进一步的，所述设置在室内天花板上的人工路标为黑白相间的多圆环编码图标或者 带有黑色圆环形轮廓的字母图标;
其中，黑白相间的多圆环编码图标是一组宽度相等黑白相间，最外层是黑色的多圆 环编码图标，在圆环宽度不变的情况下，增加圆环数即可增加编码的位数使得每个多圆 环编码图标的人工路标都可以识别为一组唯一的编码，从而标识不同的位置信息;
带有黑色圆环形轮廓的字母图标由外部黑色环形轮廓及黑色环形轮廓内的英文字母 组成;英文字母共26个，用于标识不同的位置信息。
进一步的，所述图像采集与处理模块，用于采集设置在室内天花板上的人工路标的 图像，并根据所采集到的人工路标的图像获取机器人的当前位置包括：
所述图像采集与处理模块采集设置在室内天花板上的人工路标的图像后，若所述人 工路标为黑白相间的多圆环编码图标，则进行灰度与二值化处理再利用霍夫找圆算法计 算多圆环编码图标的圆心和半径，依次将黑色圆环区域被识别为1，白色圆环区域被识别 为0，从而确定多圆环编码图标的编码，再利用该编码在所构建的室内地图中进行查询， 从而获得取机器人的当前位置;
若所述人工路标为黑色圆环形轮廓的字母图标，则识别出相应的字母图标后，在所 构建的室内地图中进行查询，从而获得取机器人的当前位置。
一种基于人工路标的室内机器人导航方法，包括：
构建室内地图，该室内地图中各个人工路标的位置信息与预先设置在室内天花板上 的人工路标位置信息一致;
当确定目标位置后，进行路径规划，并按照规划后的路径运动;
在运动过程中实时扫描室内天花板，当扫描区域包含人工路标时，根据所采集到的 人工路标的图像获取机器人的当前位置;
根据所规划的路径以及机器人的当前位置来调整机器人的姿态和控制其运动状态， 使机器人朝着目标位置运动。
进一步的，该方法还包括：定时检测机器人自身的安全状态，当发生异常时执行相 应的报警处理。
进一步的，预先设置在室内天花板上的人工路标为黑白相间的多圆环编码图标或者 带有黑色圆环形轮廓的字母图标;
其中，黑白相间的多圆环编码图标是一组宽度相等黑白相间，最外层是黑色的多圆 环编码图标，在圆环宽度不变的情况下，增加圆环数即可增加编码的位数使得每个多圆 环编码图标的人工路标都可以识别为一组唯一的编码，从而标识不同的位置信息;
带有黑色圆环形轮廓的字母图标由外部黑色环形轮廓及黑色环形轮廓内的英文字母 组成;英文字母共26个，用于标识不同的位置信息。
进一步的，所述根据所采集到的人工路标的图像获取机器人的当前位置包括：
当采集到室内天花板上的人工路标的图像后，若所述人工路标为黑白相间的多圆环 编码图标，则进行灰度与二值化处理再利用霍夫找圆算法计算多圆环编码图标的圆心和 半径，依次将黑色圆环区域被识别为1，白色圆环区域被识别为0，从而确定多圆环编码 图标的编码，再利用该编码在所构建的室内地图中进行查询，从而获得取机器人的当前 位置;
若所述人工路标为黑色圆环形轮廓的字母图标，则识别出相应的字母图标后，在所 构建的室内地图中进行查询，从而获得取机器人的当前位置。
由上述本发明提供的技术方案可以看出，基于人工路标实现室内导航，具有着自主 导航、抗干扰能力强、无轨迹移动等优点，也通过对人工路标识别进行定位校准来克服 了目前大多数导航方式定位精度差的问题，提高了室内导航的精确度，改善了室内导航 的合理性。
第四、附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的 附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于 本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得 其他附图。
图1为本发明实施例提供的一种基于人工路标的室内机器人导航装置的示意图;
图2为本发明实施例提供的黑白相间的多圆环编码图标示意图;
图3为本发明实施例提供的带有黑色圆环形轮廓的字母图标示意图;
图4为本发明实施例提供的一种基于人工路标的室内机器人导航方法的流程图。
第五、具体实施方式
下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地 描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于 本发明的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他 实施例，都属于本发明的保护范围。
图1为本发明实施例提供的一种基于人工路标的室内机器人导航装置的示意图。如图 1所示，该装置主要包括：设置在室内天花板上的人工路标，以及设置在机器人内部的人 机交互管理模块、运动控制模块、地图管理与路径规划模块、图像采集与处理模块;其 中：
人机交互管理模块，用于实现人机交互的通讯、配置和管理;在初始阶段，由用户 通过人机交互管理模块输入目标位置，所述人机交互管理模块将输入的目标位置传输给 地图管理与路径规划模块;
地图管理与路径规划模块，用于室内地图的构建以及机器人走行至目标位置的路径 规划;所构建的室内地图中各个人工路标的位置信息与设置在室内天花板上的人工路标 位置信息一致;
图像采集与处理模块，用于采集设置在室内天花板上的人工路标的图像，并根据所 采集到的人工路标的图像获取机器人的当前位置;
所述运动控制模块，用于根据地图管理与路径规划模块规划的路径，以及图像采集 与处理模块获取的机器人的当前位置来调整机器人的姿态和控制其运动状态，使机器人 朝着目标位置运动。
本发明实施例中，所述人工路标均固定于室内天花板上，选择在这样的位置固定， 机器人距离人工路标高度固定，易识别、抗干扰、不易遭到脏污或人为破坏的特点。
优选的，该装置还包括：安全报警模块，用于定时检测机器人自身的安全状态，当 发生异常时执行相应的报警处理。
本发明实施例中，所述设置在室内天花板上的人工路标为黑白相间的多圆环编码图 标或者带有黑色圆环形轮廓的字母图标;
如图2所示，为黑白相间的多圆环编码图标示意图;黑白相间的多圆环编码图标是一 组宽度相等黑白相间，最外层是黑色的多圆环编码图标，在圆环宽度不变的情况下，增 加圆环数即可增加编码的位数使得每个多圆环编码图标的人工路标都可以识别为一组唯 一的编码，从而标识不同的位置信息。
如图3所示，为带有黑色圆环形轮廓的字母图标示意图;带有黑色圆环形轮廓的字母 图标由外部黑色环形轮廓及黑色环形轮廓内的英文字母组成;英文字母共26个，用于标 识不同的位置信息。
本发明实施例中，室内各个位置的位置信息与对应的人工路标的编码或者字母信息 均预先存储在机器人数据库内。
本发明实施例中，所述图像采集与处理模块采集设置在室内天花板上的人工路标的 图像后，若所述人工路标为黑白相间的多圆环编码图标，则进行灰度与二值化处理再利 用霍夫找圆算法计算多圆环编码图标的圆心和半径，依次将黑色圆环区域被识别为1，白 色圆环区域被识别为0，从而确定多圆环编码图标的编码，再利用该编码在所构建的室内 地图中进行查询，从而获得取机器人的当前位置;示例性的，图2所示的多圆环编码图标 的编码为0101。
若所述人工路标为黑色圆环形轮廓的字母图标，则识别出相应的字母图标后，在所 构建的室内地图中进行查询，从而获得取机器人的当前位置。
本发明实施例中，不仅可以通过人工路标可以实现室内导航，还可以通过对人工路 标识别进行定位校准，来对对机器人运动轨迹的整体控制和细节调整。所谓整体控制是 指：电子地图形成后，机器人一旦接收到了目的地的位置信息，便知道了自己目的地的 行进方向，其自身携带的方向传感器便会调节机器人自身操着目的地方向运动，这就是 机器人对自己运动轨迹的整体控制。所谓的细节调整是指，机器人在经过每一个目标位 置时，都会对自身的运动方向进行细微的调整。机器人根据人工路标中心点坐标与图像 中心点坐标偏转角度和距离的关系，调整机器人运动，使图像中心点与人工路标中心尽 可能重合，使机器人尽可能运动到人工路标的正下方，这样确保朝着当前方向前进可以 达到下一目标位置，一步步朝着目标位置运动。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，仅以上述各功能模 块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能模 块完成，即将装置的内部结构划分成不同的功能模块，以完成以上描述的全部或者部分 功能。
本发明另一实施例还提供一种基于人工路标的室内机器人导航方法，如图4所示，其 主要包括如下：
步骤41、构建室内地图，该室内地图中各个人工路标的位置信息与预先设置在室内 天花板上的人工路标位置信息一致。
步骤42、当确定目标位置后，进行路径规划，并按照规划后的路径运动。
步骤43、在运动过程中实时扫描室内天花板，当扫描区域包含人工路标时，根据所 采集到的人工路标的图像获取机器人的当前位置。
步骤44、根据所规划的路径以及机器人的当前位置来调整机器人的姿态和控制其运 动状态，使机器人朝着目标位置运动。
优选的，该方法还包括：定时检测机器人自身的安全状态，当发生异常时执行相应 的报警处理。
本发明实施例中，预先设置在室内天花板上的人工路标为黑白相间的多圆环编码图 标或者带有黑色圆环形轮廓的字母图标;
其中，黑白相间的多圆环编码图标是一组宽度相等黑白相间，最外层是黑色的多圆 环编码图标，在圆环宽度不变的情况下，增加圆环数即可增加编码的位数使得每个多圆 环编码图标的人工路标都可以识别为一组唯一的编码，从而标识不同的位置信息;
带有黑色圆环形轮廓的字母图标由外部黑色环形轮廓及黑色环形轮廓内的英文字母 组成;英文字母共26个，用于标识不同的位置信息。
本发明实施例中，所述根据所采集到的人工路标的图像获取机器人的当前位置包 括：
当采集到室内天花板上的人工路标的图像后，若所述人工路标为黑白相间的多圆环 编码图标，则进行灰度与二值化处理再利用霍夫找圆算法计算多圆环编码图标的圆心和 半径，依次将黑色圆环区域被识别为1，白色圆环区域被识别为0，从而确定多圆环编码 图标的编码，再利用该编码在所构建的室内地图中进行查询，从而获得取机器人的当前 位置;
若所述人工路标为黑色圆环形轮廓的字母图标，则识别出相应的字母图标后，在所 构建的室内地图中进行查询，从而获得取机器人的当前位置。
需要说明的是，上述方法是基于前述装置所实现的，装置的具体功能及实现方式在 前面的实施例中已经有详细描述，故在这里不再赘述。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例可以 通过软件实现，也可以借助软件加必要的通用硬件平台的方式来实现。基于这样的理 解，上述实施例的技术方案可以以软件产品的形式体现出来，该软件产品可以存储在一 个非易失性存储介质(可以是CD-ROM，U盘，移动硬盘等)中，包括若干指令用以使得 一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施 例所述的方法。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此， 任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可轻易想到的变化或替 换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求书的 保护范围为准。
人工路标导航方式 篇3：
综合应用人工路标和栅格地图的移动机器人自主导航方法
第一、技术领域
本发明涉及移动机器人领域，尤其是一种综合应用人工路标和栅格地图的移动机器人自主导航方法。该方法主要应用于环境信息已知，已建立环境现场栅格地图的场合。
第二、背景技术
自主导航技术是移动机器人实现自主移动的核心技术，主要包括自主定位、路径规划、跟踪控制等。目前，针对自主移动机器人导航问题的实现方案已有不少，但均不够完善。自主定位是移动机器人实现自主导航的前提，就需要实时地确定自身相对于外界环境的位姿信息，即自主定位，常用的自主定位技术可分为：相对定位技术、绝对定位技术和组合定位技术。
相对定位主要包括测距法和惯性导航法，其优点是不依赖于外部环境信息，可以提供完全的自主性和完备的导航信息，基于里程计、陀螺仪等传感器的航迹推算会产生较大的累积误差。绝对定位是利用外部参照系统，通过测量移动机器人的绝对位置来实现定位，主要包括网络定位、路标定位、地图匹配定位，其优点是定位精度较高，不存在累计误差，但是实现方法复杂，往往需要借助激光雷达、摄像头等昂贵的传感器，成本较高。组合定位是将相对定位和绝对技术结合起来进行定位，通常的做法是利用相对定位进行位姿估计，利用绝对定位适时清除相对定位的累积误差。
路径规划是移动机器人实现路径跟踪的前提，是移动机器人导航中最重要的任务之一。移动机器人路径规划主要可以分为模板匹配路径规划和基于环境模型的路径规划两种类型。模板匹配路径规划是将机器人当前状态与过去模板库中的事例相比较，寻找出一个最优匹配事例，修正该事例中的路径，从而得到一条新的路径。基于环境模型的路径规划是目前最为成熟的方法，根据掌握信息的完整程度可分为环境信息已知的全局路径规划和环境信息完全未知或部分未知的局部路径规划。
根据控制目标的不同，移动机器人的路径跟踪问题包括点镇定问题、轨迹跟踪问题、路径跟踪问题。点镇定问题是指设计控制器，使移动机器人从任意的初始状态到达并稳定在任意的终止状态，其目的是获得一个反馈控制律，使移动机器人闭环系统的一个平衡点是渐进稳定的。轨迹跟踪问题是指通过反馈控制，使得机器人从任意初始位置出发，都能够跟随预先设定好的期望轨迹。路径跟踪问题是指移动机器人以给定的速度或加速度，跟随预先设定好的期望路径。
以上是移动机器人在自主导航过程中必须解决的三个问题，本领域内的发明人员可根据应用场合和设计要求的不同，设计具体的技术方案。
第三、发明内容
发明目的：为解决上述技术问题，本发明提出一种综合应用人工路标和栅格地图的移动机器人自主导航方法，该方法综合利用人工路标和栅格地图，将机器人相对定位、绝对定位和路径规划技术进行组合，实现移动机器人自主导航。
技术方案：为实现上述技术目的，本发明提出的技术方案为：
一种综合应用人工路标和栅格地图的移动机器人自主导航方法，包括步骤：
(1)栅格地图预处理：获取栅格地图，对栅格地图中的障碍物进行膨化处理;
(2)在预处理后的栅格地图的栅格点上设置人工路标，所述人工路标内存储有自身坐标、方位以及自身与相邻栅格之间的间距，且所述人工路标存储的数据能被移动机器人读取;
(3)移动机器人下载栅格地图，获取栅格地图中的障碍位置信息和人工路标位置信息;移动机器人获取系统任务指定的目标工位，并将离当前位置最近的栅格点作为指定栅格点;设置最大距离阈值L;
(4)移动机器人采用相对定位法导航以速度v向指定栅格点移动，在移动过程中，当移动机器人与指定栅格点之间的距离缩小到最大距离阈值L时，移动机器人将行驶速度降低至预设速度v0，并调整自身姿态为预设的读取姿态;移动机器人以速度v0和读取姿态运行至指定栅格点，读取指定栅格点处人工路标内存储的路标信息;所述预设速度v0为预先设置的能够获取人工路标信息的速度;所述读取姿态为预先设置的能够获取人工路标信息的姿态;
(5)移动机器人根据路标信息实现绝对定位，消除相对定位的累积误差，得到当前绝对定位的位姿信息;
(6)移动机器人根据位姿信息规划路径，将规划的路径中的下一栅格点作为指定栅格点，返回步骤(4);
(7)重复执行步骤(4)至(6)，直至移动机器人到达目标工位。
进一步的，所述相对定位法包括基于惯性传感器的航迹推算定位法和基于测距传感器的航迹推算定位法。
进一步的，所述绝对定位法是通过读取人工路标的路标信息，并与预先下载的栅格地图进行匹配后实现定位。
进一步的，所述人工路标为RFID标签或二维码标签。
进一步的，机器人进行路径规划所采用的方法为Dijkstra算法、A*算法或Floyd算法，规划的目标为最小化行驶路径或最小化行驶时间。
有益效果：与现有技术相比，本发明具有以下优势：
本发明中，在栅格地图处理中通过为栅格点增加人工路标信息，便于采用相对定位与绝对定位相结合的组合定位方式，路标可采用成本较低的采用RFID标签或二维码，能够以较低的成本获得较高的定位精度，而通过相对定位能够便于机器人感知自身在路标间的距离信息，便于在接近栅格点时提前减速以提高传感器对人工路标获取的准确度，提高了定位系统的稳定性和可靠性。
第四、附图说明
图1为本发明的流程图;
图2为栅格地图的示意图;
图3为预处理后的栅格地图的示意图;
图4为绝对定位与相对定位自由组合定位的流程图。
第五、具体实施方式
下面结合附图对本发明作更进一步的说明。
本发明提出的一种综合应用人工路标和栅格地图的移动机器人自主导航方法，其流程如图1所示，包括步骤：
(1)栅格地图预处理：获取栅格地图，对栅格地图中的障碍物进行膨化处理;
(2)在预处理后的栅格地图的栅格点上设置人工路标，所述人工路标内存储有自身坐标、方位以及自身与相邻栅格之间的间距，且所述人工路标存储的数据能被移动机器人读取;
(3)移动机器人下载栅格地图，获取栅格地图中的障碍位置信息和人工路标位置信息;移动机器人获取系统任务指定的目标工位，并将离当前位置最近的栅格点作为指定栅格点;设置最大距离阈值L;
(4)移动机器人采用相对定位法导航以速度v向指定栅格点移动，在移动过程中，当移动机器人与指定栅格点之间的距离缩小到最大距离阈值L时，移动机器人将行驶速度降低至预设速度v0，并调整自身姿态为预设的读取姿态;移动机器人以速度v0和读取姿态运行至指定栅格点，读取指定栅格点处人工路标内存储的路标信息;所述预设速度v0为预先设置的能够获取人工路标信息的速度;所述读取姿态为预先设置的能够获取人工路标信息的姿态;
(5)移动机器人根据路标信息实现绝对定位，消除相对定位的累积误差，得到当前绝对定位的位姿信息;
(6)移动机器人根据位姿信息规划路径，将规划的路径中的下一栅格点作为指定栅格点，返回步骤(4);
(7)重复执行步骤(4)至(6)，直至移动机器人到达目标工位。
上述步骤主要分为4个模块，分别为栅格地图预处理、自主组合定位、路径规划和跟踪控制，下面对每个模块分别进行阐述。
(一)栅格地图预处理
以图2所示栅格地图为例，首先对栅格地图中的障碍物进行膨化处理，然后在每个栅点增加人工路标信息，预处理后的栅格地图如图3所示。人工路标根据设计要求可采用成本较低的RFID标签或二维码，人工路标内存储的路标信息包括栅格点方位、相邻栅点间距等数据，融入人工路标信息的栅格地图可下载到各移动机器人存储单元，供自主组合定位、路径规划和跟踪控制时使用。
(二)自主组合定位
在加入人工路标的栅格地图中，移动机器人通过获取人工路标确定自身位置(自主定位)，现有的人工路标(Rfid标签或二维码等)获取方式均对移动机器人的位姿和移动速度提出了很高的要求，位姿偏移或移动速度过快均会出现漏标现象，不仅影响机器人自主定位的准确性，甚至因此造成机器人间发生碰撞。为了提高机器人获取路标的准确性，要求机器人必须以较低速度运行，然而，机器人的一直低速运行必然降低系统效率。因此本发明提出了绝对定位与相对定位的组合定位方式。通过相对定位预测机器人自身相对于下一人工路标的距离，在靠近人工路标前，提前减速并调整姿态以提高获取人工路的准确性。在获取到路标信息后，再清除相对定位的累积误差以提高相对定位的准确性。具体实现流程如图4所示，步骤如下：
1)移动机器人下载栅格地图，获取栅格地图中的障碍位置信息和人工路标位置信息;移动机器人获取系统任务指定的目标工位，并将离当前位置最近的栅格点作为指定栅格点;设置最大距离阈值L;
2)移动机器人采用相对定位法导航以速度v向指定栅格点移动，在移动过程中，当移动机器人与指定栅格点之间的距离缩小到最大距离阈值L时，移动机器人将行驶速度降低至预设速度v0，并调整自身姿态为预设的读取姿态;移动机器人以速度v0和读取姿态运行至指定栅格点，读取指定栅格点处人工路标内存储的路标信息;所述预设速度v0为预先设置的能够获取人工路标信息的速度;所述读取姿态为预先设置的能够获取人工路标信息的姿态;
3)移动机器人根据路标信息实现绝对定位，消除相对定位的累积误差，得到当前绝对定位的位姿信息;
4)移动机器人根据位姿信息规划路径，将规划的路径中的下一栅格点作为指定栅格点，返回步骤2);
5)重复执行步骤2)至4)，直至移动机器人到达目标工位。
(三)路径规划
当移动机器人收到任务后，从任务中提取出目标位置信息，在行驶途中，根据自主组合定位提供的定位信息，规划一条从移动机器人当前位置指向目标工位的最优路径，优化目标根据应用场合可以最小化路径总路程、最小化路径行程时间等，路径规划算法可采用经典的Dijkstra算法、A*算法或Floyd算法。规划出的最优路径由跟踪控制模块实现。
(4)跟踪控制
根据移动机器人导引方式的不同，分别采用不同的跟踪控制方案。惯性导引、激光导引等无固定导引路径的移动机器人可采用轨迹跟踪控制方案。
磁导引(主要包括：磁感应线导引、磁条导引、磁钉导引等)、视觉导引等有固定导引路径的移动机器人可采用路径跟踪方案。
以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。
人工路标导航方式 篇4：
人工路标视觉导航方法
第一、技术领域
信息技术自动控制领域，适合在各种物流机器人导航中控制使用。
第二、背景技术
目前，自主移动机器人的定位与导航问题是智能机器人领域的一个重要研究方向，同时也是智能移动机器人的一项关键技术，为此，各种定位导航技术和车载传感器系统相继涌现，目前所采用的主要方法有：依靠罗盘、里程计等给出移动机器人大致位置和方向的相对定位系统;利用声纳通过测量景物与机器人之间的距离来建立局部景物模型;电磁导引方法是利用铺在地下或地面上的磁条构成机器人的运动路径，来约束机器人沿磁条行走。该方法多用于工业，存在的不足是路径不能轻易改动，而且造价也很昂贵;激光扫描法是激光发射器与特定的合作目标相结合，可以实时算出激光发射器的精确位置;沿线导航是机器人在运动中利用感光元件跟踪一个事先被划在地上的可见或不可见荧光颜料;最为广泛使用的视觉导航是通过摄像机摄取周围景物的图像，然后利用数字图像处理中的各种方法来分析、识别出景物中的一些自然特征(自然路标)或者是人造特征(人工路标)，从而来确定机器人的位置，在科研和军事上广为应用。
视觉方法是近年来随着计算机速度和光学仪器精度的提高而迅速发展起来的结果，也是一种先进定位导航方法，基于视觉导航的移动机器人具有更好的柔性，具有更高的智能，才能真正称为智能移动机器人。基于自然路标的视觉导航与人工路标导航相比，目标设定复杂、控制难度大，难于实现机器人的精确控制，给物流机器人的控制应用带来不便。
第三、发明内容
为解决以上导航方法之不足，本发明提供一种基于人工路标的视觉导航方法，此方法中，人工路标在世界坐标系中的位置是预先已知的，当从捕获的景物图像中提取出路标的图像坐标后，通过路标在图像中的位置和它们在世界坐标系中的几何关系，就可以计算出摄像机在世界坐标系中的绝对位置，从而达到费用低廉、无噪音、无有害影响、信息量大的机器人导航目的。
本发明的设计方案是这样实现的：
本发明人工路标视觉导航方法由下列方法和设备组成：
1、路标的设计方法
2、路标存放位置
3、路标识别导航装置
4、路标识别控制流程
其中，路标设计方法为一种灰度模式路标，如图4所示，它由两个竖着的同宽度的长黑条、四个横着的同宽度的长黑条及中间的数字三部分构成，对于不同路标的六个黑条是相同的，用于路标检测，而数字的变化用来区别路标，由于每个数字可从0到9，因此可以组成100种(2位数字，0～99)或1000种(3位数字，0～999)不同的模式。
路标存放位置：
将打印好的数字路标粘在与地面垂直的平面上，如墙壁或电脑桌的侧面;高度与机器人的摄像机高度大致相同。
路标识别导航装置仅由带有单个摄像机的机器人构成，摄像机分辨率可任意设置，本装置为320×240。
路标识别控制流程由下列步骤组成，其流程图由图10所示：
步骤1：首先将所摄取的256色灰度图像二值化，方法很多，这里仅介绍一种简单方法。机器人摄像机照射路标后，在整个图像中搜索出最大灰度值Graymax和最小灰度值Graymin，从而求出图像二值化时所要用到的阈值 T = Gray max + Gray min 2 , 利用这个阈值把图像二值化，将灰度值大于T的变为255，将灰度值小于T的变为0，主要是增加路标与周围环境的对比度，突出人工路标识别信号;
步骤2：先从图像底部向上部逐行扫描先检测出水平方向①是否有想要的复比存在，如果存在，则检测其左边②和右边⑧是否有纵向复比存在。一旦两个方向都检测到了复比，那么就找到了一个准路标，进而可确定出由直线③、④、⑤、⑥所围成的矩形区域。在这个矩形区域内再次利用第一步中取阈值的方法对区域内的原图像进行精确二值化，这样便于变光照条件下的路标识别，针对由直线③、④、⑤、⑥、⑦所围成的两个矩形区域，其中⑦是③和④的中线，使各个边都逐渐向数字的中心平行移动，从而找到包含数字的最小矩形位置;
(如图6所示)
步骤3：计算机分别在由上一步中得到的两个最小矩形内识别数字，仔细观察图7中的10个数字，在图中根据三条直线a，b，c与10个数字的相交位置的不同，计算机就可以识别出不同数字来，例如仅只用直线c就能区分出0，1，4，7;而剩下的数字2，3，5，6，8，9与c相交的情况相同，此时再由直线a，b与数字的相交情况就可以进一步确定具体是哪个数字了。
人工路标视觉导航方法基于几何不变性和数字结构特点为依据：
其中线性摄像机成像过程：(如图1所示)
其中OW-XWYWZW为世界坐标系，Oc-XcYcZc为摄像机坐标系，OI-YIZI为理想图像坐标系。摄像机可以以任何位姿位于世界坐标系中，Oc为镜头光心，Xc轴的正向为光轴方向，OI位于Xc轴的正向f处，f为镜头的焦距，因此，根据小孔成像原理，空间中的任一物体P在镜头的后方也就是Xc轴的负向-f处的像平面上成一缩小了的倒影Q，为了与人类视觉联系起来同时也出于说明上的方便，我们在Xc轴的正向f处建立一个理想图像平面OI-YIZI，此时物体P将在该平面上成一个正立的缩小图像I，其投影大小与Q相同;
最后，再把理想图像平面上的图像转变为像素图像，即是物体P与像素图像之间的关系，对于基于人工路标的移动机器人自定位问题实质上就是由几个已知空间坐标的人工路标在像素图像上的相对位置来确定摄像机的空间位姿。
成像原理中的几何变换及性质：
设摄像机在世界坐标系中的位置即光心的坐标为(x0，y0，z0)T，姿态矩阵为R。空间中一点在世界坐标系中的坐标为(xw，yw，zw)T，在摄像机坐标系中的坐标为(xc，yc，zc)T，则它们之间的变换为
x w y w z w = R · x c y c z c + x 0 y 0 z 0 若令P=(x0，y0，z0)T，则有 x w y w z w R P 3 × 4 x c y c z c 1 , - - - - ( 1 ) 即世界坐标系到摄像机坐标系的空间正交变换(刚体变换)，三维摄像机坐标系(xc，yc，zc)T到理想图像坐标系(yu，zv)T之间的转换中心投影变换： y u = f · y c x c , z v = f · z c x c 另一种形式为 y u z v 1 = f 0 0 0 f 0 0 0 f · y c ‾ x c z c ‾ x c 1 - - - ( 2 )
理想图像坐标系(yu，zv)T到像素坐标系(u，v)T之间的变换关系为平面仿射变换： u v = k y 0 u 0 0 k z v 0 y u z v 1 - - - ( 3 )
其中ky，kz是像素尺度系数，即Y与Z方向上的分辨率，(u0，v0)T是图像中心坐标。
我们称摄像机的空间位置参数(x0，y0，z0)T及方向参数α，β，γ即旋转矩阵R为摄像机外部参数;而称焦距f，像素尺度系数ky，kz为摄像机内部参数。
总之，从空间物体到像素图像共经历了三种几何变换：空间正交变换、中心投影变换、平面仿射变换，在一般情况下，这三种几何变换有两个共同的不变性，即变直线为直线同时点在直线上的位置顺序不变和复比不变性。所谓复比是指设A，B，C，D是共线的四个相异点，若它们在直线上按A，C，D，B的顺序排列，则下面这个关系就叫做该四点的复比，记为 ( ABCD ) = AC CB AD DB . 所谓复比不变性，如图2所示，通过任意中心点Ω把直线l上的四点ACDB投
影到任意直线Δ上，便得到点A′C′D′B′，此时有 ( ACDB ) = AC CB AD DB = A ′ C ′ C ′ B ′ A ′ D ′ D ′ B ′ = ( A ′ C ′ D ′ B ′ ) . 由于空间正交变换不改变线段长度，所以显然满足这个性质;中心投影变换正是图2所示的例子;对于平面仿射变换简单比是其变换的不变量，即 AC CB = A ′ C ′ C ′ B ′ , AD DB = A ′ D ′ D ′ B ′ , 所以也满足复比不变性。由于中心点Ω和直线l的任意性，可推导出如下结论，在正视、斜视、近视、远视等不同条件下，像素图像中的复比都等于空间中的复比，如图3所示。
本发明的优点：结合“先找门后找号”的导航经验，根据摄像机成像原理中三个几何变换(空间正交变换、中心投影变换、平面仿射变换)下的复比不变性和数字结构特点，我们提出了一种可应用于实际视觉导航的人工路标。该路标的特点是：在复杂背景下也很容易被检测到，模式种类多达1000种;在近视、远视、大角度斜视以及变光照条件下具有很高的识别稳定性;模式简单，很容易制作，而且费用低廉;路标所含信息很直观，安装使用方便。
在路标的检测与识别过程中，路标的检测充分利用了复比不变性的特点，而不同路标的识别则利用了数字间结构上的不同。基于上述两点，我们也可以把路标模式中的数字换成任何易识别的符号，效果是一样的。
第四、附图说明
图1为本发明线性摄像机成像原理图;
图2为本发明复比不变性示意图;
图3为本发明各种观察环境下的复比示意图;
图4为本发明路标模式图;
图5为本发明黑条在复比不变性应用原理图;
图6为本发明检测路标示意图;
图7为本发明路标数字识别原理图;
图8为本发明路标优化设计示意图;
图9为本发明Xc几何意义示意图;
图10为本发明路标识别流程图;
图11为本发明路标定位示意图。
第五、具体实施方式
本发明人工路标视觉导航方法结合附图加以详细说明。
路标模式设计说明：
利用黑条来构造复比的另一个优点，可以证明在倾斜状态下，复比值仍不变，即 AC CB AD DB = MP PN MQ QN , 从而增强了路标检测的稳定性。
虽然这种路标不能解决部分遮挡问题，但在路标模式很多的情况下，遮挡问题就显得不重要了，这个被遮挡了，还可以从图像中的其它路标来自定位。
在实际应用中，图像分辨率和路标都可以大些，这样识别距离更远，稳定性更高。
路标各部分尺寸的优化设计也对识别的稳定性有影响，如图8所示，a部分要足够长，以保证数字的大小;b部分要有一定距离，便于在斜视情况下确定包含数字的矩形区域;数字大小确定要保证c、d、e足够长，便于在远视和斜视情况下确定包含数字的最小矩形，f要有一定高度，这样就可以每隔5行或10行进行路标检测了，大大的节省运行时间，g要有一定长度，这样可以保证在大角度斜视时仍能检测出竖直方向上的复比。
基于一个路标的机器人定位方法：
在前面的摄像机成像原理一节中，已经知道，从空间物体到由摄像机所成的像素图像共经历了三种几何变换：空间正交变换、中心投影变换、平面仿射变换。如图1所示，假设Ow-XwYwZw为任意取定的世界坐标系;Oc-XcYcZc为摄像机坐标系，镜头的光心为原点，其在世界坐标系中的位置为(x0，y0，z0)T，Xc轴为光轴方向，如果已知摄像机坐标系在世界坐标系中的欧拉角α，β，γ，我们很容易得到摄像机的姿态矩阵R，于是就可写出世界坐标系与摄像机坐标系之间的刚体变换公式 x w y w z w = R · x c y c z c + x 0 y 0 z 0 - - - - ( 4 )
其中(xw，yw，zw)T代表空间中一点在世界坐标系中的坐标，(xc，yc，zc)T代表该点在摄像机坐标系中的坐标，显然，由几何知识可得到如下关系 1 y I z I = 1 0 0 0 f 0 0 0 f · 1 y c / x c z c / x c , 其中(yI，zI)T为理想图像坐标系。这就是从三维摄像机坐标系到理想图像坐标系(yI，zI)T之间的中心投影变换。
由理想图像坐标系根据 y p z p = k y 0 0 k z · y I z I 就可得到像素坐标系，其中ky，kz是像素尺度系数，即Y与Z方向上的分辨率。
综合上述分析，我们可得到由世界坐标系到图像数据坐标系之间的变换关系。公式中摄像机的空间位置参数(x0，y0，z0)T及方向参数α，β，γ即旋转矩阵R为摄像机外部参数;而称焦距f，像素尺度系数ky，kz为摄像机内部参数。
因为我们把人工路标粘在了与地面相垂直的平面上，而且高度与摄像机的高度大致相同，那么如果自主移动机器人的工作路面是平坦的，则机器人相对于世界坐标系的姿态就只有绕Zw轴的旋转γ，即是摄像机的外参仅为x0，y0，γ，这也是机器人在世界坐标系中的位姿。于是，公式(4)可简化为 x w y w z w = cos γ - sin γ 0 sin γ cos γ 0 0 0 1 · x c y c z c + x 0 y 0 0 . 从而由世界坐标系到像素坐标系之间的变换公式可具体写为
x c · 1 y p z p = 1 0 0 0 k y 0 0 0 k z · 1 0 0 0 f 0 0 0 f · cos γ sin γ 0 - sin γ cos γ 0 0 0 1 · [ x w y w z w - x 0 y 0 0 ] , 进一步展开得
在推导自定位算法之前，先解释一下xc的几何意义，如图9所示，可以证明xc就是点F在Xc轴上的投影与光心的距离，即AE。PROOF：在ΔABE中，由于BA⊥AE，故AE=DE·sin∠ADE，又由于BE⊥GH，所以∠ADE=π-∠AGH，而∠AGH=γ，因此AE=DE·sinγ.
BE=yw-y0，FB=xw-x0，故BD=FB·ctg(π-γ).所以AE=(BE-BD)·sinγ=(yw-y0)·sinγ+(xw-x0)·cosγ.
假设摄像机为理想的针孔模型，那么Q点为图像的中心点。根据物点F在像素图像中的位置P很容易求出PQ在理想图像中的长度PQ=yI=yp/ky，由于EQ=f，所以∠PEQ=arctan(PQ/EQ)=arctan(yp/(ky·f))，根据公式EF=xc/cos(∠PEQ)=xc/cos(arctan(yp/(ky·f)))，就很容易求得物点F与摄像机光心之间的距离，即是，如果知道了物点在主光轴上的投影与镜头光心的距离，就可求出物点与镜头光心的距离。
下面举例说明基于一个人工路标如何进行自定位，如图11所示，一旦识别出图像中含有路标，那么利用某条从左到右的扫描线很容易就能得到竖着的两个黑条的四个边界在像素坐标系中Yp轴向的坐标ypA、ypB、ypC、ypD，沿着ypA和ypD分别从下向上找到横着的两个黑条的最下边和最上边在像素坐标系中Zp轴向的坐标zpE，zpF，zpG，zpH。由于路标被贴在与地面垂直的墙上，而且摄像机采用平视方式，所以EF与GH都垂直于摄像机的主光轴，即XC轴。于是不难证明，点E与点F在XC轴上的投影是同一点，不妨设为xc，那么根据公式(5)中的第三个方程，能得到如下所示的两个方程，相减可求出 x c = f · k z · ( z w E - z w F ) / ( z p E - z p F ) , 式中zwE-zwF是在路标设计时就能确定下来的固定值，zpE，zpF是点E与点F在像素坐标系中Zp轴向的坐标，于是根据前面的分析，可求出摄像机光心到直线EF的距离，设为Lleft，同理，也可求出摄像机光心到直线GH的距离，设为Lright。由于竖着的两个黑条的最左侧A边与最右侧D边在世界坐标系中的坐标是已知的，分别设为(xwA，ywA)T和(xwD，ywD)T。于是根据摄像机的光心到这两个边的距离就可构造出两个圆的方程，其中(x0，y0)T为摄像机光心在世界坐标系中的坐标，也可看作是机器人在世界坐标系中的位置，上面的方程是两圆相交，一般情况下会得到两组解，通过检验很容易将不合理的一组解删掉，利用已求出的机器人位置(x0，y0)T，再结合(5)中前两个方程，就可求出机器人在世界坐标系中的姿态角γ，具体过程如下：将(5)中第一个方程代入第二个方程整理可得 tan γ = f · k y · ( y w - y 0 ) - y p · ( x w - x 0 ) y p · ( y w - y 0 ) + f · k y · ( x w - x 0 ) , 然后根据实际情况得到γ的值。
至此，已经解决了利用一个人工路标进行机器人自定位的问题。当然，如果图像中存在多个路标时，可以采用多路标定位方法。
本发明人工路标视觉导航方法，经过现场实施运行稳定可靠，是一种较为科学的物流机器人导航方法。
