CN103400392A - 基于变电站巡检机器人的双目视觉导航系统及方法 - Google Patents

Abstract

本发明公开了一种基于变电站巡检机器人的双目视觉导航系统，它包括机器人本体，在机器人本体的前部设有用于采集前行道路上的环境信息图的图像采集系统将采集的图像，基于网络传输系统，上传给视觉分析系统：根据图像采集系统采集的双目图像信息及摄像机内外参数信息，检测变电站道路区域内的障碍物；路径规划系统：根据视觉系统检测的环境信息进行路径规划，对机器人行走路线及时做出调整，避免机器人与障碍物发生碰撞；机器人控制系统：根据路径规划系统规划的路径控制机器人本体移动。同时公开了一种视觉导航方法，提高了对环境的自适应能力，真正解决了室外复杂环境下，电力机器人的自主导航功能，提高了机器人的灵活性和安全性。

Description

基于变电站巡检机器人的双目视觉导航系统及方法

技术领域

本发明涉及一种双目视觉导航系统及方法，尤其涉及一种基于变电站巡检机器人的双目视觉导航系统及方法。

背景技术

随着传感器技术及处理器技术的发展，机器人技术朝着越来越智能化的方向发展。计算机视觉理论的形成和发展，让机器人具备了类似人类双眼的视觉系统，让机器人可以根据视觉系统得更多的环境信息。视觉导航技术主要包括基于单目的、基于双目立体视觉的、三目及多目摄像机结构。单目视觉主要利用摄像机采集到的图像信息，对结构场景中的标识进行识别，直接利用图像平面信息对场景进行判断，也有利用基于运动的三维信息恢复的方法，基于单目相机结构进行导航。立体视觉技术主要利用由两幅或两幅以上的图像重构得到的三维信息，在此基础上进行障碍物检测和路况检测，最终实现避障和导航等功能。如早期的火星探测车，使用了双目立体视觉技术进行视觉避障和导航。

避障功能是智能移动机器人必备的一个功能，基于该功能机器人可以对变换的环境实时做出反应，避免发生碰撞危险，并且可以自动避开障碍物，继续原始路径行驶，提高了机器人的自主性和系统安全性。避障功能主要包括障碍物检测和路径规划两部分内容。其中，障碍物检测就是通过对传感器采集的信息，进行处理、分析，得到障碍物的位置、尺寸等信息；路径规划就是利用当前检测到的障碍物信息以及其他道路信息构成地图，并对可行驶的路径进行规划，给出机器人可以行驶的路径。当前，基于视觉系统的障碍物检测方法可以分为基于三维信息恢复的障碍物检测方法、基于逆向投影的障碍物检测、基于视差直方图的检测方法。

现有技术中，通过分析现有技术中电力机器人在行驶过程中采集到的双目图像信息发现：

（1）变电站巡检机器人在行驶过程中，行驶路径两侧设备较多，造成图像背景复杂，从而产生障碍物与设备区域重叠的情况，给目标提取、分割等操作带来了较大困难；

（2）道路区域信息会受室外光照变化、特殊天气等影响，使得路面存在干扰信息，容易引入障碍物检测误差；

（3）现有技术中，往往不能对变电站巡检机器人的行走路径进行具体规划，不能有效的避开障碍物。

发明内容

本发明的目的就是为了解决上述问题，提供一种基于变电站巡检机器人的双目视觉导航系统及方法，它具有对机器人进行路径具体规划，有效避开障碍物的优点。

为了实现上述目的，本发明采用如下技术方案：

一种基于变电站巡检机器人的双目视觉导航系统，它包括机器人本体，在机器人本体的前臂端部设有用于采集前行道路上的环境信息图的图像采集系统；

图像采集系统：图像采集系统采集机器人前行道路上的环境图像，然后将采集的图像通过网络传输系统上传给视觉分析系统，图像采集系统包括双目摄像机，双目摄像机通过传输线连接到图像采集卡，然后通过有线连接到网络传输系统上；

网络传输系统包括无线网桥，无线路由器、接收器，将图像采集系统采集的图像信息通过无线网络传输至后台处理平台；所述后台处理平台包括视觉分析系统和路径规划系统；

视觉分析系统：根据图像采集系统采集的双目图像信息及摄像机内外参数信息，通过逆向投影理论及三维重构技术实现对变电站道路区域内的障碍物的检测，并将信息传输至机器人控制系统，视觉分析系统基于后台计算机，通过无线网络获取前方采集的图像数据，将分析结果发送给路径规划系统；

路径规划系统：根据视觉系统检测的环境信息，构建二维占据地图，进行路径规划，出现障碍物时，对机器人行走路线及时做出调整，避免机器人与障碍物发生碰撞，然后将路径信息通过网络传输系统下发到机器人控制系统；

机器人控制系统：通过无线网络，接受当前机器人运动信息，下发至机器人运动平台，机器人控制系统根据路径规划系统规划的路径控制机器人本体移动，机器人控制系统运行的平台为工控机。

所述视觉分析系统、路径规划系统运行的硬件平台为计算机。

一种基于变电站巡检机器人的双目视觉导航系统的视觉导航方法，具体步骤为：

步骤一：采集双目环境图像，通过图像解析，畸变校正，立体校正获得去除了畸变效果、且像素匹配关系约束在相同的Y轴的双目图像；

步骤二：将校正后的图像逆向投影变换，将左目图像和右目图像投影到地平面上，并将重新投影后的左目图像和右目图像的像素做差，并对差值图像做Canny边缘检测，然后，使用Hough直线检测出道路区域和道路边缘；

步骤三：在确定道路区域与道路边缘后，在图像中道路区域内根据区域的灰度区域相关性确定左目图像和右目图像的匹配关系；然后，根据左目图像和右目图像的匹配关系生成视差图，统计视差图的直方图，对视差图进行直方图分割，并判断左目图像和右目图像中有无障碍物；如果存在障碍物，则进行步骤四，否则，返回步骤一；

步骤四：根据分割得到的障碍物区域、摄像机标定信息确定得到障碍物的三维信息，根据障碍物的三维信息确定障碍物区域的尺寸和平均距离；

步骤五：将检测到的障碍物信息传输至机器人控制系统，根据新的障碍物信息更新地图，根据已存在的路径信息，路径规划系统对机器人下一步移动方向进行规划，机器人控制系统根据当前的机器人行驶方向及速度输入到移动平台驱动器，实现机器人移动，当无法执行下一步操作时，机器人停止，并上报信号给上位机，如果可以执行，重复步骤一。

所述步骤一的具体步骤为：

（1-1）双目摄像机采集环境图像信息；

（1-2）根据双目摄像机的内部参数K_l,K_r，双目摄像机相对位置关系R,T，以及标定得到的畸变参数（k₁,k₂,k₃,p₁,p₂）对变电站巡检机器人变电站巡检行走过程中采集到的左目图像和右目图像进行畸变校正和立体校正；

（1-3）将校正后的左目图像和右目图像逆向投影变换，将双目图像重新投影到地平面上，其中，通过双目摄像机参数K_l,K_r,R,T,以及参考摄像机坐标系相对于地平面世界坐标系下的旋转矩阵和平移矩阵R_w,T_w，确定逆向投影变换；

设当前左目或右目摄像机内部参数为： $K=\left[\begin{array}{ccc}{f}_u& s& u\\ 0& f_v& v\\ 0& 0& 1\end{array}\right],$ f_u和f_v是横向、纵向焦距，u₀,v₀为图像平面主点位置，s为像素纵横比，当前摄像机相对于世界坐标系的旋转矩阵R_w和平移矩阵T_w，图像平面空间像素坐标为(u,v)，地平面上世界坐标系下的目标坐标为(X,Y,Z)，设双目摄像机光心相对于地平面高度为H，双目摄像机的光心相对于地平面的俯仰夹角为θ，设电力机器人的坐标系为O₂，P_ground为机器人坐标系下地平面的方程，根据变电站实际环境，例如，假设地平面方程为Z=0，所以有 $P_{\mathrm{ground}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 0\\ 0& 0& 1\end{array}\right];$

根据双目摄像机投影关系得到图像平面与地平面间的投影关系为：

$w\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=P_{\mathrm{inv}}\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]$ 即 $w\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=K\left[R\right|T]P_{\mathrm{ground}}\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]$

所述步骤二的具体步骤为：

（2-1）将左目图像和右目图像逆向投影到世界坐标系中，根据平面间映射关系，仍然得到保持平行关系的道路边缘信息；

（2-2）在得到双目摄像机的逆向投影矩阵后，将左目图像和右目图像逆向投影到世界坐标系中得到图像ImageL_remap和ImageR_remap；然后，将图像ImageL_remap和ImageR_remap进行差值运算得到Image_difference，将世界坐标系中图像ImageL_remap和ImageR_remap的重合信息进行过滤，提取道路边缘及障碍物区域不重合的区域信息；使用Hough变换检测直线，根据直线的象限方向、长度、位置进行约束，提取道路两侧边缘在摄像机坐标系下的直线方程；根据摄像机相对机器人坐标系的坐标变换关系，计算得到机器人坐标系下的道路信息，将参考道路信息提供给路径规划系统进行路径规划。

所述步骤三的具体步骤为：

（3-1）根据双目摄像机坐标间的内外参数，基于像素间的SAD灰度相关性，计算双目图像间的像素匹配关系，得到视差图I_disparity；

（3-2）设图像I(x,y)与图像J(x,y)间的窗口尺寸为(w,w)的SAD灰度相关度，则有I(x,y)图像中(x,y)点与J(x,y)图像中(x′,y′)点间的SAD相关性为：

$\mathrm{SAD}(x,y)={\mathrm{\Σ}}_{i=-\frac{w-i}{2}}^{i=\frac{w-1}{2}}{\mathrm{\Σ}}_{j=-\frac{w-1}{2}}^{j=\frac{w-1}{2}}|I(x+i,y+j)-J(x+i,y+j)|,i,j\∈l;$

根据左目图像空间中一个像素(xl,y)，在右目图像中搜索相同纵坐标上的每个像素点(xr,y)，通过确定每个像素点间的SAD相似性，将相似度较高的点作为候选匹配点，在根据顺序性，唯一性约束得到最终的匹配关系；

(3-4)然后，对视差图统计直方图，然后对直方图进行高斯平滑操作，视差直方图计算方法就是将图像中所有像素相同的像素的个数进行累加，生成一个一维的数组来记录二维图像中每个灰度值出现的概率；

平滑过程为：

$P\left(x\right)=\frac{1}{\sqrt{2\mathrm{\π}{\mathrm{\σ}}^2}}{e}^{\frac{-(x-x_0)}{{2\mathrm{\σ}}^2}};$

其中P(x)指滤波后的值，x为直方图每个单元的值，(x₀,σ)为高斯函数的均值和方差；

（3-5）根据平滑过的直方图，给定一定斜率的直线检测高于直线的直方图区间，从而得到障碍物的初始检测结果。

所述步骤四的具体步骤为：

（4-1）在得到了障碍物区域的匹配关系后，根据已知的双目摄像机的内部参数和外部参数，根据三角形定位原理，得到障碍物区域的三维点坐标；

（4-2）根据左右目图像间的匹配关系和双目摄像机的内部参数和外部参数，构造三维重构方程Q，并得到障碍物区域的三维点坐标：

$Q=\left[\begin{array}{ccccc}1& 0& & 0& -u_0\\ 0& 1& & 0& -v_0\\ 0& 0& 0& & f\\ 0& 0& -\frac{1}{T_x}& & (u_0-{u_0}^{\′})T_x\end{array}\right],\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]=Q\left[\begin{array}{c}u\\ v\\ d\\ 1\end{array}\right]$

其中，u₀，v₀为校正后参考摄像机的主点的横、纵坐标，u₀′为相机的主点横坐标，Ｔ_x为两摄像机间基线距离,(X,Y,Z)是以摄像机为原点的三维坐标系下的三维点坐标，(u,v,d)为图像坐标值与对应的视差值；

（4-3）获取图像中障碍物区域的三维点后，根据障碍物的三维点分布，拟合各个连通区域三维点所在的平面，建立最最小外接矩形，并求取质心，最终确定障碍物的实际尺寸和具体位置。

所述步骤五的具体步骤为：

（5-1）将障碍物的三维点坐标映射到世界坐标系中，以双目摄像机光心连线上的中点为原点，构建一定范围内的栅格地图，根据三维点在地平面上的投影个数，对栅格进行填充；

（5-2）根据二维占据局部地图，以及已知的全局目标点，及电力机器人在全局地图中的位置，使用马尔可夫路径规划算法，对电力机器人进行路径规划，得到避障路径；

（5-3）路径规划系统对机器人下一步移动方向进行规划，机器人控制系统根据当前的机器人行驶方向机速度输入到移动平台驱动器，实现机器人移动。

本发明的有益效果：

（1）本发明基于双目视觉系统，使用逆向投影算法，提取了道路边缘信息，在不需要恢复三维信息的前提下，就可以给出机器人航向角偏差，对机器人行驶路径进行调整。

（2）本发明还提供了基于视差直方图分割的障碍物检测，实现了道路区域内出现的高于地面一定距离以上的障碍物的自动检测，为机器人自主避障与导航提供了充分的信息。

（3）基于本发明，机器人可以自动检测运行方向的障碍物，并根据障碍物的位置的尺寸，自动做出停止、避让、报警等反应，从而避免与障碍物发生碰撞，提高了对环境的自适应能力，真正解决了室外复杂环境下，电力机器人的自主导航功能，提高了机器人的灵活性和安全性；

（4）本发明采用非接触环境信息感知技术，不影响变电站内设备正常运行；

（5）基于图像信息分析，信息量丰富；相对于激光、磁轨迹等导航方式，成本低，易推广。

附图说明

图1为本系统的系统框图，

图2为本发明的方法流程图。

其中，1、图像采集系统，2、网络传输系统，3、视觉分析系统，4、路径规划系统，5、机器人控制系统，6、机器人运动平台；

具体实施方式

下面结合附图与实施例对本发明作进一步说明。

如图1所示，一种基于变电站巡检机器人的双目视觉导航系统，它包括机器人本体，在机器人本体的前臂端部设有用于采集前行道路上的环境信息图的图像采集系统1；

图像采集系统1：图像采集系统1采集机器人前行道路上的环境图像，然后将采集的图像通过网络传输系统2上传给视觉分析系统3，图像采集系统1包括双目摄像机，双目摄像机通过传输线连接到图像采集卡，然后通过有线连接到网络传输系统2上；

网络传输系统2包括无线网桥，无线路由器、接收器，将图像采集系统1采集的图像信息通过无线网络传输至后台处理平台；所述后台处理平台包括视觉分析系统3和路径规划系统4；

视觉分析系统3：根据图像采集系统1采集的双目图像信息及摄像机内外参数信息，通过逆向投影理论及三维重构技术实现对变电站道路区域内的障碍物的检测，并将信息传输至机器人控制系统5，视觉分析系统3基于后台计算机，通过无线网络获取前方采集的图像数据，将分析结果发送给路径规划系统4；

路径规划系统4：根据视觉系统检测的环境信息，构建二维占据地图，进行路径规划，出现障碍物时，对机器人行走路线及时做出调整，避免机器人与障碍物发生碰撞，然后将路径信息通过网络传输系统下发到机器人控制系统；

机器人控制系统5：通过无线网络，接受当前机器人运动信息，下发至机器人运动平台6，机器人控制系统5根据路径规划系统4规划的路径控制机器人本体移动，机器人控制系统5的运行平台为工控机。

所述视觉分析系统、路径规划系统运行的硬件平台为计算机。

如图2所示，通过路径规划确定变电站巡检机器人在设备间隔道路上的行走轨迹，机器人开始行驶。行驶过程中，通过机器人车载处理器下发指令开启双目摄像机。

（1）双目摄像机开始采集环境图像信息。

（2）准备图像

根据已知的双目摄像机内部参数，对变电站巡检机器人行走过程中采集到的双目图像进行畸变矫正和立体矫正，计算方法可参考《学习OpenCV》第410页透镜畸变与第467页立体校正。

（3）基于逆向投影的道路检测

对校正后的左目图像和右目图像，进行逆向投影变换，将右目图像和左目图像投影到世界坐标系中；其中，通过相机内部参数K以及相机外部参数R和T计算得到逆向投影变换：

定当前双目摄像机内部参数为： $K=\left[\begin{array}{ccc}{f}_u& s& u\\ 0& f_v& v\\ 0& 0& 1\end{array}\right],$ 双目摄像机相对于世界坐标系的旋转矩阵与平移矩阵R_w,T_w，图像平面空间像素坐标为(u,v)，地平面上的世界坐标系下的目标坐标为(X,Y,Z)，已知双目摄像机光心相对于地平面高度为H，双目摄像机的光心相对于地平面的俯仰夹角为θ，定义电力机器人的坐标系为O₂，P_ground为电力机器人坐标系下地平面的方程，根据变电站实际环境，一般情况下定义地平面方程为Z=0，所以有

$P_{\mathrm{ground}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 0\\ 0& 0& 1\end{array}\right].$

根据双目摄像机投影关系得到图像平面与地平面间的投影关系在相差一个尺度w下的齐次坐标变换为：

$w\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=P_{\mathrm{inv}}\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]$ 即 $w\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=K\left[R\right|T]P_{\mathrm{ground}}\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right].$

将左目图像逆向投影到地平面的世界坐标系中，根据仿射不变性，得到仍然保持平行关系的道路边缘信息；在得到双目摄像机的逆向投影矩阵后，将左目图像和右目图像逆向投影到地平面上的世界坐标系中得到图像ImageL_remap，ImageL_remap；然后，将图像ImageL_remap，ImageL_remap进行差值运算得到Image_difference，将地平面的世界坐标系上的图像重合信息进行过滤，提取道路边缘及障碍物区域不重合的区域信息；使用Hough变换检测直线，根据直线的象限方向、长度、位置进行约束，提取道路两侧边缘在摄像机坐标系下的直线方程；根据摄像机相对机器人坐标系的坐标变换关系，计算得到机器人坐标系下的道路信息，将参考道路信息提供给路径规划系统进行路径规划。

所述Hough变换检测直线是将每一个边缘点为通过它的所有的可能的直线进行投票，然后检测直线方程坐标系空间内最高累加值对应的点，作为最可能匹配的直线，设直线方程为：

y＝kx+d，在计算得到多条可能的直线后，根据直线斜率方向K的取值方位及直线长度等信息，得到最符合的两条直线方程。

（4）障碍物检测假设

根据双目摄像机坐标间的内外参数，基于像素间的SAD灰度相关性，计算双目图像间的像素匹配关系，得到视差图I_disparity；

设图像I(x,y)与图像J(x,y)间的窗口尺寸为(w,w)的SAD灰度相关度，则有I(x,y)图像中(x,y)点与J(x,y)图像中(x′,y′)点间的SAD相关性为：

$\mathrm{SAD}(x,y)={\mathrm{\Σ}}_{i=-\frac{w-i}{2}}^{i=\frac{w-1}{2}}{\mathrm{\Σ}}_{j=-\frac{w-1}{2}}^{j=\frac{w-1}{2}}|I(x+i,y+j)-J(x+i,y+j)|,i,j\∈l;$

根据左目图像空间中一个像素(xl,y)，在右目图像中搜索相同纵坐标上的每个像素点(xr,y)，通过确定每个像素点间的SAD相似性，将相似度较高的点作为候选匹配点，在根据顺序性，唯一性约束得到最终的匹配关系；

然后，对视差图统计直方图，然后对直方图进行高斯平滑操作，视差直方图计算方法就是将图像中所有像素相同的像素的个数进行累加，生成一个一维的数组来记录二维图像中每个灰度值出现的概率；

平滑过程为：

$P\left(x\right)=\frac{1}{\sqrt{2\mathrm{\π}{\mathrm{\σ}}^2}}{e}^{\frac{-(x-x_0)}{{2\mathrm{\σ}}^2}};$

其中P(x)指滤波后的值，x为直方图每个单元的值，(x₀,σ)为高斯函数的均值和方差；

根据平滑过的直方图，给定一定斜率的直线检测高于直线的直方图区间，从而得到障碍物的初始检测结果。

（5）三维信息恢复

根据左目图像和右目图像的匹配关系和双目摄像机的内部参数和外部参数，构造三维重构方程Q直接计算得到三维点坐标：

$Q=\left[\begin{array}{cccc}1& 0& 0& -u_0\\ 0& 1& 0& {-v}_0\\ 0& 0& 0& f\\ 0& 0& -\frac{1}{T_x}& (u_0-{u_0}^{\′})T_x\end{array}\right],\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]=Q\left[\begin{array}{c}u\\ v\\ d\\ 1\end{array}\right]$

其中，u₀，v₀为校正后参考摄像机的主点的横、纵坐标，u₀′为相机的主点横坐标，T_x为两摄像机间基线距离,(X,Y,Z)是以摄像机为原点的三维坐标系下的三维点坐标，(u,v,d)为图像坐标值与对应的视差值；

获取了图像中障碍物区域的三维点后，根据三维点分布拟合各个连通区域三维点所在的平面，建立最小外接矩形，并求取质心，最终确定障碍物的实际尺寸和具体位置。

（6）栅格地图生成

将障碍物的三维点坐标映射到世界坐标系中，以双目摄像机光心连线上的中点为原点，构建一定范围内的栅格地图，根据三维点在地平面上的投影个数，对栅格进行填充；

（7）路径规划

基于步骤（6）中得到的二维占据局部地图，以及已知的全局目标点，及定位系统得到的机器人在全局地图中的位置，使用马尔可夫路径规划算法，对机器人进行路径规划，得到新的路径。

（8）控制机器人

根据新得到的路径，按照机器人当前的速度及路径更新时间，计算机器人行走结构应执行的速度及偏移的角度，控制系统将该信息传输至行走机构驱动器。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (8)

1.一种基于变电站巡检机器人的双目视觉导航系统，其特征是，它包括机器人本体，在机器人本体的前部设有用于采集前行道路上的环境信息图的图像采集系统；

图像采集系统：图像采集系统采集机器人前行道路上的环境图像，然后将采集的图像通过网络传输系统上传给视觉分析系统，图像采集系统包括双目摄像机，双目摄像机通过传输线连接到图像采集卡，然后通过有线连接到网络传输系统上；

网络传输系统包括无线网桥，无线路由器、接收器，将图像采集系统采集的图像信息通过无线网络传输至后台处理平台；所述后台处理平台包括视觉分析系统和路径规划系统；

视觉分析系统：根据图像采集系统采集的双目图像信息及摄像机内外参数信息，通过逆向投影理论及三维重构技术实现对变电站道路区域内的障碍物的检测，并将信息传输至机器人控制系统，视觉分析系统基于后台计算机，通过无线网络获取前方采集的图像数据，将分析结果发送给路径规划系统；

路径规划系统：根据视觉系统检测的环境信息，构建二维占据地图，进行路径规划，出现障碍物时，对机器人行走路线及时做出调整，避免机器人与障碍物发生碰撞，然后将路径信息通过网络传输系统下发到机器人控制系统；

机器人控制系统：通过无线网络，接受当前机器人运动信息，下发至机器人运动平台，机器人控制系统根据路径规划系统规划的路径控制机器人本体移动，机器人控制系统运行平台为工控机。

2.如权利要求1所述的双目视觉导航系统，其特征是，所述视觉分析系统、路径规划系统运行的硬件平台为计算机。

3.利用权力要求1所述的一种基于变电站巡检机器人的双目视觉导航系统的视觉导航方法，其特征是，具体步骤为：

步骤一：采集双目环境图像，通过图像解析，畸变校正，立体校正获得去除了畸变效果、且像素匹配关系约束在相同的Y轴的双目图像；

步骤二：将校正后的图像逆向投影变换，将左目图像和右目图像投影到地平面上，并将重新投影后的左目图像和右目图像的像素做差，并对差值图像做Canny边缘检测，然后，使用Hough直线检测出道路区域和道路边缘；

步骤三：在确定道路区域与道路边缘后，在图像中道路区域内根据区域的灰度区域相关性确定左目图像和右目图像的匹配关系；然后，根据左目图像和右目图像的匹配关系生成视差图，统计视差图的直方图，对视差图进行直方图分割，并判断左目图像和右目图像中有无障碍物；如果存在障碍物，则进行步骤四，否则，返回步骤一；

步骤四：根据分割得到的障碍物区域、摄像机标定信息确定得到障碍物的三维信息，根据障碍物的三维信息确定障碍物区域的尺寸和平均距离；

步骤五：将检测到的障碍物信息传输至机器人控制系统，根据新的障碍物信息更新地图，根据已存在的路径信息，路径规划系统对机器人下一步移动方向进行规划，机器人控制系统根据当前的机器人行驶方向及速度输入到移动平台驱动器，实现机器人移动，当无法执行下一步操作时，机器人停止，并上报信号给上位机，如果可以执行，重复步骤一。

4.如权利要求3所述的视觉导航方法，其特征是，所述步骤一的具体步骤为：

（1-1）双目摄像机采集环境图像信息；

（1-2）根据双目摄像机的内部参数K_l,K_r，双目摄像机相对位置关系R,T，以及标定得到的畸变参数（k₁,k₂,k₃,p₁,p₂）对变电站巡检机器人变电站巡检行走过程中采集到的左目图像和右目图像进行畸变校正和立体校正；

（1-3）将校正后的左目图像和右目图像逆向投影变换，将双目图像重新投影到地平面上，其中，通过双目摄像机参数K_l,K_r,R,T,以及参考摄像机坐标系相对于地平面世界坐标系下的旋转矩阵和平移矩阵R_w,T_w，确定逆向投影变换；

设当前左目或右目摄像机内部参数为： $K=\left[\begin{array}{ccc}{f}_u& s& u\\ 0& f_v& v\\ 0& 0& 1\end{array}\right],$ f_u和f_v是横向、纵向焦距，u₀,v₀为图像平面主点位置，s为像素纵横比，当前摄像机相对于世界坐标系的旋转矩阵R_w和平移矩阵T_w，图像平面空间像素坐标为（u,v)，地平面上世界坐标系下的目标坐标为(X,Y,Z)，设双目摄像机光心相对于地平面高度为H，双目摄像机的光心相对于地平面的俯仰夹角为θ，设电力机器人的坐标系为O₂，P_ground为机器人坐标系下地平面的方程，根据变电站实际环境，设地平面方程为Z=0，所以有 $P_{\mathrm{ground}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 0\\ 0& 0& 1\end{array}\right];$

根据双目摄像机投影关系得到图像平面与地平面间的投影关系在相差一个尺度w下的齐次坐标变换为：

$w\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]{=P}_{\mathrm{inv}}\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]$ 即 $w\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=K\left[R\right|T]P_{\mathrm{ground}}\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right].$

5.如权利要求3所述的视觉导航方法，其特征是，所述步骤二的具体步骤为：

（2-1）将左目图像和右目图像逆向投影到世界坐标系中，根据平面间映射关系，仍然得到保持平行关系的道路边缘信息；

（2-2）在得到双目摄像机的逆向投影矩阵后，将左目图像和右目图像逆向投影到世界坐标系中得到图像ImageL_remap和ImageR_remap；然后，将图像ImageL_remap和ImageR_remap进行差值运算得到Image_difference，将世界坐标系中图像ImageL_remap和ImageR_remap的重合信息进行过滤，提取道路边缘及障碍物区域不重合的区域信息；使用Hough变换检测直线，根据直线的象限方向、长度、位置进行约束，提取道路两侧边缘在摄像机坐标系下的直线方程；根据摄像机相对机器人坐标系的坐标变换关系，计算得到机器人坐标系下的道路信息，将参考道路信息提供给路径规划系统进行路径规划。

6.如权利要求3所述的视觉导航方法，其特征是，所述步骤三的具体步骤为：

（3-1）根据双目摄像机坐标间的内外参数，基于像素间的SAD灰度相关性，计算双目图像间的像素匹配关系，得到视差图I_disparity；

（3-2）设图像I(x,y)与图像J(x,y)间的窗口尺寸为(w,w）的SAD灰度相关度，则有I（x,y）图像中(x,y)点与J(x,y)图像中(x′,y′)点间的SAD相关性为：

$\mathrm{SAD}(x,y)={\mathrm{\Σ}}_{i=-\frac{w-i}{2}}^{i=\frac{w-1}{2}}{\mathrm{\Σ}}_{j=-\frac{w-1}{2}}^{j=\frac{w-1}{2}}|I(x+i,y+j)-J(x+i,y+j)|,i,j\∈l;$

根据左目图像空间中一个像素(xl,y)，在右目图像中搜索相同纵坐标上的每个像素点(xr,y)，通过确定每个像素点间的SAD相似性，将相似度较高的点作为候选匹配点，在根据顺序性，唯一性约束得到最终的匹配关系；

(3-4)然后，对视差图统计直方图，然后对直方图进行高斯平滑操作，视差直方图计算方法就是将图像中所有像素相同的像素的个数进行累加，生成一个一维的数组来记录二维图像中每个灰度值出现的概率；

平滑过程为：

$P\left(x\right)=\frac{1}{\sqrt{2\mathrm{\π}{\mathrm{\σ}}^2}}{e}^{\frac{-(x-x_0)}{{2\mathrm{\σ}}^2}};$

其中P(x)指滤波后的值，x为直方图每个单元的值，(x₀,σ)为高斯函数的均值和方差；

（3-5）根据平滑过的直方图，给定一定斜率的直线检测高于直线的直方图区间，从而得到障碍物的初始检测结果。

7.如权利要求3所述的视觉导航方法，其特征是，所述步骤四的具体步骤为：

（4-1）在得到了障碍物区域的匹配关系后，根据已知的双目摄像机的内部参数和外部参数，根据三角形定位原理，得到障碍物区域的三维点坐标；

（4-2）根据左右目图像间的匹配关系和双目摄像机的内部参数和外部参数，构造三维重构方程Q，并得到障碍物区域的三维点坐标：

$Q=\left[\begin{array}{ccccc}1& 0& & 0& -u_0\\ 0& 1& & 0& -v_0\\ 0& 0& 0& & f\\ 0& 0& -\frac{1}{T_x}& & (u_0-{u_0}^{\′})T_x\end{array}\right],\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]=Q\left[\begin{array}{c}u\\ v\\ d\\ 1\end{array}\right]$

其中，u₀，v₀为校正后参考摄像机的主点的横、纵坐标，u₀′为相机的主点横坐标，T_x为两摄像机间基线距离,(X,Y,Z)是以摄像机为原点的三维坐标系下的三维点坐标，(u,v,d)为图像坐标值与对应的视差值；

（4-3）获取图像中障碍物区域的三维点后，根据障碍物的三维点分布，拟合各个连通区域三维点所在的平面，建立最最小外接矩形，并求取质心，最终确定障碍物的实际尺寸和具体位置。

8.如权利要求3所述的视觉导航方法，其特征是，所述步骤五的具体步骤为：

（5-1）将障碍物的三维点坐标映射到世界坐标系中，以双目摄像机光心连线上的中点为原点，构建一定范围内的栅格地图，根据三维点在地平面上的投影个数，对栅格进行填充；

（5-2）根据二维占据局部地图，以及已知的全局目标点，及电力机器人在全局地图中的位置，使用马尔可夫路径规划算法，对电力机器人进行路径规划，得到避障路径；

（5-3）路径规划系统对机器人下一步移动方向进行规划，机器人控制系统根据当前的机器人行驶方向机速度输入到移动平台驱动器，实现机器人移动。

Images