人工路标视觉导航方法

1.一种人工路标视觉导航方法，该方法由路标的设计方法、路标存放位置、路标识别 导航装置、路标识别控制流程四部分组成，其特征在于路标设计方法为一种灰度模式路标， 它由两个竖着的同宽度的长黑条、四个横着的同宽度的长黑条及中间的数字三部分构成，对 于不同路标的六个黑条是相同的，用于路标检测，而数字的变化用来区别路标，每个数字从 0到9，构成由2位数字组成的100种或由3位数字组成的1000种不同的路标模式。
2.按权利要求1所述的一种人工路标视觉导航方法，其特征在于路标存放位置：将打 印好的数字路标粘在与地面垂直的平面上，高度与机器人的摄像机高度大致相同。
3.按权利要求1所述的一种人工路标视觉导航方法，其特征在于路标识别导航装置仅 由带有单个摄像机的机器人构成。
4.按权利要求1所述的一种人工路标视觉导航方法，其特征在于路标识别控制流程由 下列步骤组成：
步骤1：首先将所摄取的256色灰度图像二值化，机器人摄像机照射路标后，在整个图 像中搜索出最大灰度值Graymax和最小灰度值Graymin，从而求出图像二值化时所要用到的 阈值$T=\frac{\mathrm{Gray}\max +\mathrm{Gray}\min }{2},$利用这个阈值把图像二值化，将灰度值大于T的变为255，将灰 度值小于T的变为0，主要是增加路标与周围环境的对比度，突出人工路标识别信号；
步骤2：先从图像底部向上部逐行扫描先检测出水平方向是否有想要的复比存在，如果 存在，则检测其左边和右边是否有纵向复比存在，一旦水平方向和纵向方向都检测到了复比， 那么就找到了一个准路标，进而可确定出由数字周围黑条的边缘所围成的矩形区域，在这个 矩形区域内再次利用第一步中取阈值的方法对区域内的原图像进行精确二值化，这样便于变 光照条件下的路标识别，然后检测出两个数字间空白区的纵向中线，把上述矩形区域分成两 个矩形，最后使两个矩形的各个边都逐渐向数字的中心平行移动，从而找到包含数字的最小 矩形位置；
步骤3：计算机分别在由上一步中得到的两个最小矩形内识别数字，仔细观察10个数字， 根据三条直线，即两条分别位于数字中心点上方和下方的水平直线和一条过数字中心点的纵 向直线，与10个数字的相交位置的不同，计算机就识别出不同数字来。

技术领域\n信息技术自动控制领域，适合在各种物流机器人导航中控制使用。\n背景技术\n目前，自主移动机器人的定位与导航问题是智能机器人领域的一个重要研究方向，同时 也是智能移动机器人的一项关键技术，为此，各种定位导航技术和车载传感器系统相继涌现， 目前所采用的主要方法有：依靠罗盘、里程计等给出移动机器人大致位置和方向的相对定位 系统；利用声纳通过测量景物与机器人之间的距离来建立局部景物模型；电磁导引方法是利 用铺在地下或地面上的磁条构成机器人的运动路径，来约束机器人沿磁条行走。该方法多用 于工业，存在的不足是路径不能轻易改动，而且造价也很昂贵；激光扫描法是激光发射器与 特定的合作目标相结合，可以实时算出激光发射器的精确位置；沿线导航是机器人在运动中 利用感光元件跟踪一个事先被划在地上的可见或不可见荧光颜料；最为广泛使用的视觉导航 是通过摄像机摄取周围景物的图像，然后利用数字图像处理中的各种方法来分析、识别出景 物中的一些自然特征(自然路标)或者是人造特征(人工路标)，从而来确定机器人的位置， 在科研和军事上广为应用。\n视觉方法是近年来随着计算机速度和光学仪器精度的提高而迅速发展起来的结果，也是 一种先进定位导航方法，基于视觉导航的移动机器人具有更好的柔性，具有更高的智能，才 能真正称为智能移动机器人。基于自然路标的视觉导航与人工路标导航相比，目标设定复杂、 控制难度大，难于实现机器人的精确控制，给物流机器人的控制应用带来不便。\n发明内容\n为解决以上导航方法之不足，本发明提供一种基于人工路标的视觉导航方法，此方法中， 人工路标在世界坐标系中的位置是预先已知的，当从捕获的景物图像中提取出路标的图像坐 标后，通过路标在图像中的位置和它们在世界坐标系中的几何关系，就可以计算出摄像机在 世界坐标系中的绝对位置，从而达到费用低廉、无噪音、无有害影响、信息量大的机器人导 航目的。\n本发明的设计方案是这样实现的：\n本发明人工路标视觉导航方法由下列方法和设备组成：\n1、路标的设计方法\n2、路标存放位置\n3、路标识别导航装置\n4、路标识别控制流程\n其中，路标设计方法为一种灰度模式路标，如图4所示，它由两个竖着的同宽度的长黑 条、四个横着的同宽度的长黑条及中间的数字三部分构成，对于不同路标的六个黑条是相同 的，用于路标检测，而数字的变化用来区别路标，每个数字从0到9，构成由2位数字组成 的100种或由3位数字组成的1000种不同的路标模式。\n路标存放位置：\n将打印好的数字路标粘在与地面垂直的平面上，如墙壁或电脑桌的侧面；高度与机器人 的摄像机高度大致相同。\n路标识别导航装置仅由带有单个摄像机的机器人构成，摄像机分辨率可任意设置，本装 置为320×240。\n路标识别控制流程由下列步骤组成，其流程图由图10所示：\n步骤1：首先将所摄取的256色灰度图像二值化，方法很多，这里仅介绍一种简单方法。 机器人摄像机照射路标后，在整个图像中搜索出最大灰度值Graymax和最小灰度值Graymin， 从而求出图像二值化时所要用到的阈值$T=\frac{\mathrm{Gray}\max +\mathrm{Gray}\min }{2},$利用这个阈值把图像二值化， 将灰度值大于T的变为255，将灰度值小于T的变为0，主要是增加路标与周围环境的对比度， 突出人工路标识别信号；\n步骤2：先从图像底部向上部逐行扫描先检测出水平方向①是否有想要的复比存在，如 果存在，则检测其左边②和右边⑧是否有纵向复比存在，一旦水平方向和纵向方向都检测到 了复比，那么就找到了一个准路标，进而可确定出由直线③、④、⑤、⑥，即数字周围黑条 的边缘所围成的矩形区域，在这个矩形区域内再次利用第一步中取阈值的方法对区域内的原 图像进行精确二值化，这样便于变光照条件下的路标识别，然后检测出两个数字间空白区的 纵向中线，把上述矩形区域分成两个矩形，即由直线③、④、⑤、⑥、⑦围成的两个矩形， 最后使两个矩形的各个边都逐渐向数字的中心平行移动，从而找到包含数字的最小矩形位置； (如图6所示)\n步骤3：计算机分别在由上一步中得到的两个最小矩形内识别数字，仔细观察10个数字，\n根据三条直线a、b、c，即两条分别位于数字中心点上方和下方的水平直线和一条过数 字中心点的纵向直线，与10个数字的相交位置的不同，计算机就识别出不同数字来，例如仅 只用直线c就能区分出0，1，4，7；而剩下的数字2，3，5，6，8，9与c相交的情况相同， 此时再由直线a，b与数字的相交情况就可以进一步确定具体是哪个数字了。\n人工路标视觉导航方法基于几何不变性和数字结构特点为依据：\n其中线性摄像机成像过程：(如图1所示)\n其中Ow-XwYwZw为世界坐标系，Oc-XcYcZc为摄像机坐标系，OI-YIZI，为理想图像坐 标系。摄像机可以以任何位姿位于世界坐标系中，Oc为镜头光心，Xc轴的正向为光轴方向， OI，位于Xc轴的正向f处，f为镜头的焦距，因此，根据小孔成像原理，空间中的任一物体P 在镜头的后方也就是Xc轴的负向-f处的像平面上成一缩小了的倒影Q，为了与人类视觉联系 起来同时也出于说明上的方便，我们在Xc轴的正向f处建立一个理想图像平面OI-YIZI， 此时物体P将在该平面上成一个正立的缩小图像I，其投影大小与Q相同；\n最后，再把理想图像平面上的图像转变为像素图像，即是物体P与像素图像之间的关系， 对于基于人工路标的移动机器人自定位问题实质上就是由几个已知空间坐标的人工路标在像 素图像上的相对位置来确定摄像机的空间位姿。\n成像原理中的几何变换及性质：\n设摄像机在世界坐标系中的位置即光心的坐标为(x0，y0，z0)T，姿态矩阵为R。空间中一 点在世界坐标系中的坐标为(xw，yw，zw)T，在摄像机坐标系中的坐标为(xc，yc，zc)T，则它们 之间的变换为\n$\left(\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right)=R·\left(\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right)+\left(\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right)$若令P＝(x0，y0，z0)T，则有$\left(\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right)={\left(\begin{array}{cc}R& P\end{array}\right)}_{3\times 4}\left(\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right)---\left(1\right)$\n即世界坐标系到摄像机坐标系的(刚体变换)，三维摄像机坐标系(xc，yc，zc)T到 理想图像坐标系(yu，zv)T之间的转换 $y_u=f·\frac{y_c}{x_c},$ $z_v=f·\frac{z_c}{x_c}$另一种形式为\n$\left(\begin{array}{c}{y}_u\\ z_v\\ 1\end{array}\right)=\left(\begin{array}{ccc}f& 0& 0\\ 0& f& 0\\ 0& 0& 1\end{array}\right)·\left(\begin{array}{c}\frac{y_c}{x_c}\\ \frac{z_c}{x_c}\\ 1\end{array}\right)---\left(2\right)$\n理想图像坐标系(yu，zv)T到像素坐标系(u，v)T之间的变换关系为平面仿射变换：\n$\left(\begin{array}{c}u\\ v\end{array}\right)=\left(\begin{array}{ccc}{k}_y& 0& u_0\\ 0& k_z& v_0\end{array}\right)\left(\begin{array}{c}{y}_u\\ z_v\\ 1\end{array}\right)---\left(3\right)$\n其中ky，kz是像素尺度系数，即Y与Z方向上的分辨率，(u0，v0)T是图像中心坐标。\n我们称摄像机的空间位置参数(x0，y0，z0)T及方向参数α，β，γ即旋转矩阵R为摄像机外部 参数；而称焦距f，像素尺度系数ky，kz为摄像机内部参数。\n总之，从空间物体到像素图像共经历了三种几何变换：空间正交变换、中心投影变换、 平面仿射变换，在一般情况下，这三种几何变换有两个共同的不变性，即变直线为直线同时 点在直线上的位置顺序不变和复比不变性。所谓复比是指设A，B，C，D是共线的四个相异点， 若它们在直线上按A，C，D，B的顺序排列，则下面这个关系就叫做该四点的复比，记为 $\left(\mathrm{ACDB}\right)=\frac{\frac{\mathrm{AC}}{\mathrm{CB}}}{\frac{\mathrm{AD}}{\mathrm{DB}}}.$所谓复比不变性，如图2所示，通过任意中心点Ω把直线l上的四点ACDB投 影到任意直线Δ上，便得到点A′C′D′B′，此时有$\left(\mathrm{ACDB}\right)=\frac{\frac{\mathrm{AC}}{\mathrm{CB}}}{\frac{\mathrm{AD}}{\mathrm{DB}}}=\frac{\frac{A^{\prime }{C}^{\prime }}{C^{\prime }{B}^{\prime }}}{\frac{A^{\prime }{D}^{\prime }}{D^{\prime }{B}^{\prime }}}=\left(A^{\prime }{C}^{\prime }{D}^{\prime }{B}^{\prime }\right).$由于空间正 交变换不改变线段长度，所以显然满足这个性质；中心投影变换正是图2所示的例子；对于 平面仿射变换简单比是其变换的不变量，即$\frac{\mathrm{AC}}{\mathrm{CB}}=\frac{A^{\prime }{C}^{\prime }}{C^{\prime }{B}^{\prime }},\frac{\mathrm{AD}}{\mathrm{DB}}=\frac{A^{\prime }{D}^{\prime }}{D^{\prime }{B}^{\prime }},$所以也满足复比不变性。 由于中心点Ω和直线l的任意性，可推导出如下结论，在正视、斜视、近视、远视等不同条件 下，像素图像中的复比都等于空间中的复比，如图3所示。\n本发明的优点：结合“先找门后找号”的导航经验，根据摄像机成像原理中三个几何变 换(空间正交变换、中心投影变换、平面仿射变换)下的复比不变性和数字结构特点，我们 提出了一种可应用于实际视觉导航的人工路标。该路标的特点是：在复杂背景下也很容易被 检测到，模式种类多达1000种；在近视、远视、大角度斜视以及变光照条件下具有很高的识 别稳定性；模式简单，很容易制作，而且费用低廉；路标所含信息很直观，安装使用方便。\n在路标的检测与识别过程中，路标的检测充分利用了复比不变性的特点，而不同路标的 识别则利用了数字间结构上的不同。基于上述两点，我们也可以把路标模式中的数字换成任 何易识别的符号，效果是一样的。\n附图说明\n图1为本发明线性摄像机成像原理图；\n图2为本发明复比不变性示意图；\n图3为本发明各种观察环境下的复比示意图；\n图4为本发明路标模式图；\n图5为本发明黑条在复比不变性应用原理图；\n图6为本发明检测路标示意图；\n图7为本发明路标数字识别原理图；\n图8为本发明路标优化设计示意图；\n图9为本发明Xc几何意义示意图；\n图10为本发明路标识别流程图；\n图11为本发明路标定位示意图。\n具体实施方式\n本发明人工路标视觉导航方法结合附图加以详细说明。\n路标模式设计说明：\n利用黑条来构造复比的另一个优点，可以证明在倾斜状态下，复比值仍不变，即 $\frac{\frac{\mathrm{AC}}{\mathrm{CB}}}{\frac{\mathrm{AD}}{\mathrm{DB}}}=\frac{\frac{\mathrm{MP}}{\mathrm{PN}}}{\frac{\mathrm{MQ}}{\mathrm{QN}}},$从而增强了路标检测的稳定性。\n虽然这种路标不能解决部分遮挡问题，但在路标模式很多的情况下，遮挡问题就显得不 重要了，这个被遮挡了，还可以从图像中的其它路标来自定位。\n在实际应用中，图像分辨率和路标都可以大些，这样识别距离更远，稳定性更高。\n路标各部分尺寸的优化设计也对识别的稳定性有影响，如图8所示，a部分要足够长，以 保证数字的大小；b部分要有一定距离，便于在斜视情况下确定包含数字的矩形区域；数字 大小确定要保证c、d、e足够长，便于在远视和斜视情况下确定包含数字的最小矩形，f要有 一定高度，这样就可以每隔5行或10行进行路标检测了，大大的节省运行时间，g要有一定 长度，这样可以保证在大角度斜视时仍能检测出竖直方向上的复比。\n基于一个路标的机器人定位方法：\n在前面的摄像机成像原理一节中，已经知道，从空间物体到由摄像机所成的像素图像共 经历了三种几何变换：空间正交变换、中心投影变换、平面仿射变换。如图1所示，假设 Ow-XwYwZw为任意取定的世界坐标系；Oc-XcYcZc为摄像机坐标系，镜头的光心为原点， 其在世界坐标系中的位置为(x0，y0，z0)T，Xc轴为光轴方向，如果已知摄像机坐标系在世界 坐标系中的欧拉角α，β，γ，我们很容易得到摄像机的姿态矩阵R，于是就可写出世界坐标系 与摄像机坐标系之间的刚体变换公式$\left(\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right)=R·\left(\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right)+\left(\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right)---\left(4\right)$\n其中(xw，yw，zw)T代表空间中一点在世界坐标系中的坐标，(xc，yc，zc)T代表该点在摄像机坐 标系中的坐标，显然，由几何知识可得到如下关系$\left(\begin{array}{c}1\\ y_I\\ z_I\end{array}\right)=\left(\begin{array}{ccc}1& 0& 0\\ 0& f& 0\\ 0& 0& f\end{array}\right)·\left(\begin{array}{c}1\\ y_c/x_c\\ z_c/x_c\end{array}\right),$其中(yI，zI)T为 理想图像坐标系。这就是从三维摄像机坐标系到理想图像坐标系(yI，zI)T之间的中心投影变 换。\n由理想图像坐标系根据$\left(\begin{array}{c}{y}_p\\ z_p\end{array}\right)=\left(\begin{array}{cc}{k}_y& 0\\ 0& k_z\end{array}\right)·\left(\begin{array}{c}{y}_I\\ z_I\end{array}\right)$就可得到像素坐标系，其中ky，kz是像素尺度 系数，即Y与Z方向上的分辨率。\n综合上述分析，我们可得到由世界坐标系到图像数据坐标系之间的变换关系。公式中摄 像机的空间位置参数(x0，y0，z0)T及方向参数α，β，γ即旋转矩阵R为摄像机外部参数；而称焦 距f，像素尺度系数ky，kz为摄像机内部参数。\n因为我们把人工路标粘在了与地面相垂直的平面上，而且高度与摄像机的高度大致相同， 那么如果自主移动机器人的工作路面是平坦的，则机器人相对于世界坐标系的姿态就只有绕 Zw轴的旋转γ，即是摄像机的外参仅为x0，y0，γ，这也是机器人在世界坐标系中的位姿。\n于是，公式(4)可简化为$\left(\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right)=\left(\begin{array}{ccc}\cos \gamma & -\sin \gamma & 0\\ \sin \gamma & \cos \gamma & 0\\ 0& 0& 1\end{array}\right)·\left(\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right)+\left(\begin{array}{c}{x}_0\\ y_0\\ 0\end{array}\right).$从而由世界坐标系到像素 坐标系之间的变换公式可具体写为\n$x_c·\left(\begin{array}{c}1\\ y_p\\ z_p\end{array}\right)=\left(\begin{array}{ccc}1& 0& 0\\ 0& k_y& 0\\ 0& 0& k_z\end{array}\right)·\left(\begin{array}{ccc}1& 0& 0\\ 0& f& 0\\ 0& 0& f\end{array}\right)·\left(\begin{array}{ccc}\cos \gamma & \sin \gamma & 0\\ -\sin \gamma & \cos \gamma & 0\\ 0& 0& 1\end{array}\right)·[\left(\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right)-\left(\begin{array}{c}{x}_0\\ y_0\\ 0\end{array}\right)],$进一步展开得\n$\left\{\begin{array}{c}{x}_c=(x_w-x_0)·\cos \gamma +(y_w-y_0)·\sin \gamma \\ y_p=[(y_w-y_0)·\cos \gamma ·f·k_y-(x_w-x_0)·\sin \gamma ·f·k_y]/x_c\\ z_p=f·k_z·z_w/x_c\end{array}\right.---\left(5\right)$\n在推导自定位算法之前，先解释一下xc的几何意义，如图9所示，可以证明xc就是点F 在Xc轴上的投影与光心的距离，即AE。\nPROOF：在ΔABE中，由于BA⊥AE，故AE＝DE·sin∠ADE，又由于BE⊥GH，所以 ∠ADE＝π-∠AGH，而∠AGH＝γ，因此AE＝DE·sinγ.\nBE＝yw-y0，FB＝xw-x0，故BD＝FB·ctg(π-γ).\n所以AE＝(BE-BD)·sinγ＝(yw-y0)·sinγ+(xw-x0)·cosγ.\n假设摄像机为理想的针孔模型，那么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与摄像机光心之间的 距离，即是，如果知道了物点在主光轴上的投影与镜头光心的距离，就可求出物点与镜头光 心的距离。\n下面举例说明基于一个人工路标如何进行自定位，如图11所示，一旦识别出图像中含有 路标，那么利用某条从左到右的扫描线很容易就能得到竖着的两个黑条的四个边界在像素坐 标系中Yp轴向的坐标yp A、yp B、yp C、yp D，沿着yp A和yp D分别从下向上找到横着的两个黑条的 最下边和最上边在像素坐标系中Zp轴向的坐标zp E，zp F，zp G，zp H。由于路标被贴在与地面垂直的 墙上，而且摄像机采用平视方式，所以EF与GH都垂直于摄像机的主光轴，即XC轴。于是 不难证明，点E与点F在XC轴上的投影是同一点，不妨设为xc，那么根据公式(5)中的第 三个方程，能得到如下所示的两个方程，$\left\{\begin{array}{c}{z}_p^E=f·k_z·z_w^E/x_c\\ z_p^F=f·k_z·z_w^F/x_c\end{array}\right..$相减可求出 $x_c=f·k_z·(z_w^E-z_w^F)/(z_p^E-z_p^F),$式中zw E-zw F是在路标设计时就能确定下来的固定值，zp E，zp F是 点E与点F在像素坐标系中Zp轴向的坐标，于是根据前面的分析，可求出摄像机光心到直线 EF的距离，设为Lleft，同理，也可求出摄像机光心到直线GH的距离，设为Lright。由于竖着 的两个黑条的最左侧A边与最右侧D边在世界坐标系中的坐标是已知的，分别设为(xw A，yw A)T 和(xw D，yw D)T。于是根据摄像机的光心到这两个边的距离就可构造出两个圆的方程， $\left\{\begin{array}{c}{(x_0-x_w^A)}^2+{(y_0-y_w^A)}^2=L_{\mathrm{left}}^2\\ {(x_0-x_w^D)}^2+{(y_0-y_w^D)}^2=L_{\mathrm{right}}^2\end{array}\right.,$其中(x0，y0)T为摄像机光心在世界坐标系中的坐标，也可 看作是机器人在世界坐标系中的位置，上面的方程是两圆相交，一般情况下会得到两组解， 通过检验很容易将不合理的一组解删掉，利用已求出的机器人位置(X0，y0)T，再结合(5)中 前两个方程，就可求出机器人在世界坐标系中的姿态角γ，具体过程如下：将(5)中第一个 方程代入第二个方程整理可得$\tan \gamma =\frac{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)},$然后根据实际情况得 到γ的值。\n至此，已经解决了利用一个人工路标进行机器人自定位的问题。当然，如果图像中存在 多个路标时，可以采用多路标定位方法。\n本发明人工路标视觉导航方法，经过现场实施运行稳定可靠，是一种较为科学的物流机 器人导航方法。