CN106444774A - 基于室内照明灯的移动机器人视觉导航方法 - Google Patents

Abstract

本发明公开了一种基于室内照明灯的移动机器人视觉导航方法，步骤包括：步骤1、给厂房环境建模；步骤2、标定双目摄像头；步骤3、检测移动机器人双目视觉位姿；步骤4、导航PID控制算法，利用PID控制规律实现控制移动机器人的移动速度以及移动方向，在整个导航过程中，不断改变移动机器人跟踪目标点，同时循环检测当前位姿，根据目标位置，按PID控制规律控制移动机器人沿规划路径运行。本发明的方法解决了标志物容易被遮挡的问题，同时，图像处理算法简单，提高了导航的实时性。

Description

基于室内照明灯的移动机器人视觉导航方法

技术领域

本发明属于智能视觉导航技术领域，涉及一种基于室内照明灯的移动机器人视觉导航方法。

背景技术

位姿估计是移动机器人(或移动小车，以下附图中均标示为小车)研究的一个核心的问题，精确的位姿估计对于移动机器人的定位，地图生成，路径规划，目标检测和跟踪等具有重要意义。目前位姿估计方法主要分为两大类：相对位姿估计和绝对位姿估计。绝对位姿估计前提是要有预先设置的环境信息，精度相对较高。路标定位是绝对位姿估计方法，多用于结构化环境，该方法主要依靠计算机视觉图像处理，将环境中的特征从数据中提取处理，进行位姿估计，这样对环境就没有具体要求。工业机器人一般工作在室内，一般采用信标进行绝对位姿估计，目前主要有基于室内踢脚线的导航，跟踪黑色导引线实现导航控制，基于场景的移动机器人定位、导航，以及基于视觉和人工地标的机器人自定位、导航方法；但在生产实践过程中，道路标示线或地标可能磨损而变得模糊，人工地标被道路上的其他物体所遮挡，无法导航；完全的场景导航，图像处理工作量大，导航存在滞后。本文提出了一种基于屋顶悬挂的灯管为标志物，实现移动机器人的视觉导航方法。室内照明灯作为标志物，相对位置固定而且高，不会被别的物体遮挡，同时，屋顶背景空旷，而且地面相对照明灯距离比较固定，在视觉图像处理时算法相对简单，明显提高了处理效率。

发明内容

本发明的目的是提供一种基于室内照明灯的移动机器人视觉导航方法，解决了现有技术中，标志物容易被遮挡，以及图像处理效率低，系统存在滞后的问题。

本发明所采用的技术方案是，一种基于室内照明灯的移动机器人视觉导航方法，按照以下步骤实施：

步骤1、给厂房环境建模；

步骤2、标定双目摄像头；

步骤3、检测移动机器人双目视觉位姿；

步骤4、导航PID控制算法

设当前移动机器人绝对坐标系下的位姿为(x_k,y_k,θ_k)，跟踪路径为ABC，计算当前移动机器人控制方向为θ，

移动机器人与目标B点之间的距离为：

利用PID控制规律实现控制移动机器人的移动速度以及移动方向，保证移动机器人从A点向B点运行；当移动机器人达到B点后，将移动机器人的下一个目标位置调整为C点，循环上述过程，在整个导航过程中，不断改变移动机器人跟踪目标点，同时循环检测当前位姿，根据目标位置，按PID控制规律控制移动机器人沿规划路径运行。

本发明的有益效果是，实现了对室内环境下移动机器人的视觉定位与导航，图像处理算法简单，提高了导航的实时性。

附图说明

图1是本发明方法中对环境建模示意图；

图2是本发明方法中对双目摄像头测距原理示意图；

图3是本发明方法中对照明灯管特征提取示意图；

图4是本发明方法中对移动机器人坐标设置示意图；

图5是本发明方法中对移动机器人导航算法示意图。

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

本发明的基于室内照明灯的移动机器人视觉导航方法，按照以下步骤具体实施：

步骤1、给厂房环境建模

参照图1，给厂房环境中的移动机器人行走道路以及室内照明灯建模，具体步骤是：

1.1)在厂房内建立绝对坐标系，如图1中的OXY坐标系；

1.2)添加标志物灯光(室内照明灯)的位置L1(x₁,y₁,θ₁),L2(x₂,y₂,θ₂),…Ln(x_n,y_n,θ_n)；图1中建立了6个灯管标志物，其中，x,y是照明灯在OXY坐标系中的横向及纵向位置；θ为照明灯的纵向方向；

1.3)构建移动机器人行走道路模型

一般采用线段对道路建模，构建多边形模拟曲线；

如图1中ABCDE为实际道路，该道路分为直线段AB、BC、DE及圆弧段CD，根据道路情况，一般采用多条直线段对圆弧段进行模拟，图1实施例中采用了一条直接段CD(虚线)进行圆弧段的模拟。

步骤2、标定双目摄像头

利用黑白等距离相间的方块作为标定板，以及MATLAB工具对双目摄像头进行标定，

首先利用标定板同时采集双目摄像头获得的图像，然后通过MATLAB命令calib，获得左右摄像头的焦距长度f、光轴坐标以及畸变系数；

步骤3、检测移动机器人双目视觉位姿

双目摄像头垂直向上安装在移动机器人上，实时获取屋顶照明灯的图像，照明灯在图像中的位姿会随小车位姿的变化而变化；由于工厂屋顶的照明灯位置以及方向是相对固定的，于是，利用照明灯在图像位姿就能够反推移动机器人的实际位置。

3.1)图像预处理

考虑到照明灯相对屋顶背景的亮度高，对图像进行二值化处理，然后，进行二值图像的膨胀和腐蚀，得到边界圆滑的标志物轮廓，并删除图像面积不近似于标志物的区域；

3.2)通过双目摄像机视觉差测距

考虑不同厂房照明灯距离地面的高度不同，在进行移动机器人视觉导航时，需要首先判断高度，根据双目摄像机视觉差原理，参照图2，得到摄像头距屋顶的距离为：

其中，f为摄像头的焦距，B为两摄像头的镜头间距，x_l,x_r分别为任意标志点Q在左右摄像头中成像点的横坐标，见图2；

3.3)提取照明灯特征

目前室内照明灯常见的为日光灯以及白炽灯，日光灯一般为矩形，白炽灯一般为球形，根据实际情况，分别提取日光灯的四个角点，或提取白炽灯的圆心，

以日光灯为例，图3为经过预处理后的日光灯图像，提取日光灯管的四个角点的图像坐标为(a1,b1)、(a2,b2)、(a3,b3)、(a4,b4)，进而计算日光灯中心点(图3中黑点)的图像坐标为：

[c1,c2]＝[(a1+a2+a3+a4)/4,(b1+b2+b3+b4)/4]；

3.4)检测移动机器人位姿

照明灯在双目摄像机图像中的姿态是由移动机器人的姿态所决定的，在绝对坐标系下，移动机器人旋转时，图像中照明灯的方向角θ会向相反方向变化相同的角度；移动机器人平移时，图像中照明灯的位置(x,y)会向相反方向变化相同的位移；由于照明灯的位姿相对是固定的，因此根据图像先确定移动机器人在图像中的位姿，计算出移动机器人在绝对坐标系中的位姿。

如图4，以移动机器人中心以及移动方向建立移动机器人坐标系o₀x₀y₀，根据成像原理，设照明灯中心在右摄像头中的图像坐标为(c1_r，c2_r,1)，计算照明灯中心点在移动机器人坐标系下的坐标为：

其中，f_x,f_y为摄像头焦距长度，u₀、v_o分别为光轴坐标，均通过摄像头标定得到；平移矩阵t＝[x_r，y_r，1]^T；(x_r,y_r)为右摄像头在移动机器人坐标系下的坐标；z_c为被测点到摄像机坐标系原点的垂直距离，见图4；

同理，根据照明灯中心在右摄像头中的图像坐标为(c1_l，c2_l，1)，则照明灯中心点在移动机器人坐标系中的坐标为[x_cl，y_cl，1]^T；

取照明灯中心点在左右摄像头图像中的坐标平均值，得到照明灯中心点在移动机器人坐标系中的坐标为：

根据灯管角点在图像中的坐标计算出灯管方向角度为：

其中，

设灯管中心在绝对坐标系中的位姿为(x₀,y₀,θ₀)，则得到移动机器人绝对坐标系下的位姿为：

步骤4、导航PID控制算法

移动机器人的导航控制主要控制移动速度以及移动方向，设当前移动机器人绝对坐标系下的位姿为(x_k,y_k,θ_k)，跟踪路径为ABC，如图5，计算当前移动机器人控制方向为θ，

移动机器人与目标B点之间的距离为：

见图5，利用PID控制规律实现控制移动机器人的移动速度以及移动方向，保证移动机器人从A点向B点运行；当移动机器人达到B点后，将移动机器人的下一个目标位置调整为C点，循环上述过程，在整个导航过程中，不断改变移动机器人跟踪目标点，同时循环检测当前位姿，根据目标位置，按PID控制规律控制移动机器人沿规划路径运行。

实施例

双目摄像机采用双目黑白摄像机，主要参数为：镜头间距B为5cm，每秒25帧；镜头参数包括：焦距f为4mm，规格为1/3inch，光圈F1.6，视角为70.6度，百万像素。

通过标定板对左右摄像头进行标定，得到双目摄像机内部参数如下表1。

表1、双目摄像机的内部参数

利用本发明上述的方法，控制移动机器人的移动位置，得到移动机器人位置误差小于3％；角度误差小于5％；利用上述方法进行移动机器人导航，移动机器人(移动小车)完全能够按规划路径运动。

Claims (4)

1.一种基于室内照明灯的移动机器人视觉导航方法，其特征在于，按照以下步骤实施：

步骤1、给厂房环境建模；

步骤2、标定双目摄像头；

步骤3、检测移动机器人双目视觉位姿；

步骤4、导航PID控制算法

设当前移动机器人绝对坐标系下的位姿为(x_k,y_k,θ_k)，跟踪路径为ABC，计算当前移动机器人控制方向为θ，

移动机器人与目标B点之间的距离为：

利用PID控制规律实现控制移动机器人的移动速度以及移动方向，保证移动机器人从A点向B点运行；当移动机器人达到B点后，将移动机器人的下一个目标位置调整为C点，循环上述过程，在整个导航过程中，不断改变移动机器人跟踪目标点，同时循环检测当前位姿，根据目标位置，按PID控制规律控制移动机器人沿规划路径运行。

2.根据权利要求1所述的基于室内照明灯的移动机器人视觉导航方法，其特征在于，所述的步骤1中，给厂房环境中的移动机器人行走道路以及室内照明灯建模，具体步骤是：

1.1)在厂房内建立绝对坐标系，即OXY坐标系；

1.2)添加室内照明灯的位置L1(x₁,y₁,θ₁),L2(x₂,y₂,θ₂),…Ln(x_n,y_n,θ_n)；其中，x,y是照明灯在OXY坐标系中的横向及纵向位置；θ为照明灯的纵向方向；

1.3)构建移动机器人行走道路模型

采用线段对道路建模，构建多边形模拟曲线。

3.根据权利要求1所述的基于室内照明灯的移动机器人视觉导航方法，其特征在于，所述的步骤2中，利用黑白等距离相间的方块作为标定板，以及MATLAB工具对双目摄像头进行标定，

首先利用标定板同时采集双目摄像头获得的图像，然后通过MATLAB命令calib，获得左右摄像头的焦距长度f、光轴坐标以及畸变系数。

4.根据权利要求1所述的基于室内照明灯的移动机器人视觉导航方法，其特征在于，所述的步骤3中，具体包括：

3.1)图像预处理

对图像进行二值化处理，然后进行二值图像的膨胀和腐蚀，得到边界圆滑的标志物轮廓，并删除图像面积不近似于标志物的区域；

3.2)通过双目摄像机视觉差测距

根据双目摄像机视觉差原理，得到摄像头距屋顶的距离为：

$z_c=\frac{fB}{x_l-x_r}$

其中，f为摄像头的焦距，B为两摄像头的镜头间距，x_l,x_r分别为任意标志点Q在左右摄像头中成像点的横坐标；

3.3)提取照明灯特征

分别提取日光灯的四个角点，或提取白炽灯的圆心；

3.4)检测移动机器人位姿

以移动机器人中心以及移动方向建立移动机器人坐标系o₀x₀y₀，根据成像原理，设照明灯中心在右摄像头中的图像坐标为(c1_r，c2_r,1)，计算照明灯中心点在移动机器人坐标系下的坐标为：

$\left[\begin{array}{c}{x}_{cr}\\ y_{cr}\\ 1\end{array}\right]={\left[\begin{array}{cccc}{f}_x& 0& u_0& 0\\ 0& f_y& v_0& 0\\ 0& 0& 1& 0\end{array}\right]}^T{\left[\begin{array}{cc}{0}^T& t\\ 0^T& 1\end{array}\right]}^T\left[\begin{array}{c}c{1}_r/z_c\\ c{2}_r/z_c\\ 1\end{array}\right]$

其中，f_x,f_y为摄像头焦距长度，u₀、v_o分别为光轴坐标，均通过摄像头标定得到；平移矩阵t＝[x_r，y_r，1]^T；(x_r,y_r)为右摄像头在移动机器人坐标系下的坐标；z_c为被测点到摄像机坐标系原点的垂直距离；

同理，根据照明灯中心在右摄像头中的图像坐标为(c1_l，c2_l，1)，则照明灯中心点在移动机器人坐标系中的坐标为[x_cl，y_cl，1]^T；

取照明灯中心点在左右摄像头图像中的坐标平均值，得到照明灯中心点在移动机器人坐标系中的坐标为：

根据灯管角点在图像中的坐标计算出灯管方向角度为：

${\mathrm{\θ}}_c=arccos\left(\frac{\stackrel{\→}{X}\·(1,0)}{\left|\stackrel{\→}{X}\right|}\right)$

其中，

设灯管中心在绝对坐标系中的位姿为(x₀,y₀,θ₀)，则得到移动机器人绝对坐标系下的位姿为：