CN118131781B - 储罐底板在油检测机器人不可视环境路径跟踪方法及装置 - Google Patents

Abstract

本发明公开一种储罐底板在油检测机器人不可视环境路径跟踪方法及装置，涉及自动控制技术领域。所述方法包括：确定机器人在栅格化储罐底板图纸中的当前位置；当所述当前位置位于已检测区域时，查找距离当前位置最近的未检测区域的单元格，作为目标点，并规划移动至所述目标点的移动路径和对目标点所在的未检测区域进行检测的扫查路径；进一步的，采用路径跟踪算法，控制机器人移动至所述目标点；以所述目标点为起点完成所述目标点所在的未检测区域的检测。本发明采用分区域的遍历算法实现路径规划，并采用路径跟踪算法实现对规划后路径的追踪，本发明可实现储罐在油检测机器人的自动化的路径规划和跟踪，极大地提高了检测的效率。

Description

储罐底板在油检测机器人不可视环境路径跟踪方法及装置

技术领域

本发明涉及自动控制技术领域，特别是一种储罐底板在油检测机器人不可视环境路径跟踪方法及装置。

背景技术

在石油化工领域，大型立式储罐是一种储存原油、成品油的金属储罐，其直径一般在十几米至几十米之间。这些储存介质一般具有易燃、易爆、腐蚀性等特点，为储罐的检测、维护带来了重大挑战。作为储罐中腐蚀最严重的区域，储罐底板的检测尤为重要。现有的检测方法通常是定期清罐后再进行人工底板检测，然而介质的转运、储罐底板沉积物的清理等问题都为储罐检测带来了挑战。随着技术的发展，机器人检测技术正不断应用于储罐底板检测。除了用于开罐后的储罐底板检测机器人外，储罐在油检测机器人也成为研究热点。储罐在油检测机器人是一种通过储罐顶部人孔吊装进入储罐介质内部进行底板缺陷检测的机器人，其核心难点是在不可视的环境中实现机器人的路径规划和跟踪。

发明内容

本发明的目的是提供一种储罐底板在油检测机器人不可视环境路径跟踪方法及装置，可实现储罐在油检测机器人的自动化的路径规划和跟踪，极大地提高了检测的效率。

为实现上述目的，本发明提供了如下方案。

一种储罐底板在油检测机器人不可视环境路径跟踪方法，所述方法包括如下步骤。

根据机器人的超声波定位信号，确定机器人在栅格化储罐底板图纸中的当前位置；所述栅格化储罐底板图纸上标记有不可通过区域、已检测区域和未检测区域。

当所述当前位置位于已检测区域时，基于所述栅格化储罐底板图纸查找距离当前位置最近的未检测区域的单元格，作为目标点。

利用所述栅格化储罐底板图纸，规划移动至所述目标点的移动路径和对所述目标点所在的未检测区域进行检测的扫查路径。

根据所述移动路径，采用路径跟踪算法，控制机器人移动至所述目标点。

根据所述扫查路径，以所述目标点为起点完成所述目标点所在的未检测区域的检测，并更新所述栅格化储罐底板图纸。

可选的，基于所述栅格化储罐底板图纸查找距离当前位置最近的未检测区域的单元格的方法为扩散法。

可选的，利用所述栅格化储罐底板图纸，规划移动至所述目标点的移动路径，具体包括如下步骤。

令m的数值为1。

将当前位置作为第m个起点。

确定第m个起点与目标点的方向与所有可移动方向的夹角，并确定夹角最小的可移动方向为目标移动方向；所述可移动方向为0°、45°、90°、135°、180°、225°、270°和315°八个方向上第m个起点的下一个单元格不属于不可通过区域的方向。

确定目标移动方向上以第m个起点为起始点的n个单元格内是否存在不可通过区域，n为单次规划的预设单元格数量。

当不存在不可通过区域时，将以第m个起点为起始点在目标移动方向上移动n个单元格的子路径添加至路径中，并将目标移动方向上以第m个起点为起始点的第n个单元格作为第m+1个起点。

当存在不可通过区域时，将以第m个起点为起始点在目标移动方向上移动n’个单元格的子路径添加至路径中，并将目标移动方向上以第m个起点为起始点的第n’个单元格作为第m+1个起点；n’为目标移动方向上与第m个起点最近的不可通过区域的单元格与第m个起点之间的单元格的数量。

将m的数值增加1，并返回“确定第m个起点与目标点的方向与所有可移动方向的夹角，并确定夹角最小的可移动方向为目标移动方向”的步骤，直到获得连接所述当前位置与所述目标点的移动路径。

可选的，根据所述移动路径，采用路径跟踪算法，控制机器人移动至所述目标点，具体包括如下步骤。

根据机器人的超声波定位信号，确定机器人在栅格化储罐底板图纸中的当前位置。

根据所述当前位置和所述移动路径计算偏向角误差。

当所述偏向角误差大于偏向角误差阈值时，采用原地旋转的方式纠正机器人的移动方向。

当所述偏向角误差不大于偏向角误差阈值时，采用前进中纠正偏差的方式纠正机器人的移动方向。

可选的，偏向角误差的计算公式为：；其中，为偏向角误差，()为向量，为第i个路径点的位置，A为当前位置，()为向量，为第i+1个路径点的位置，其中，位于A之前，位于A之后，和分别为A与之间的x轴距离和y轴距离，和分别为与之间的x轴距离和y轴距离。

可选的，采用原地旋转的方式纠正机器人的移动方向，具体包括如下步骤。

利用如下公式计算待旋转角度。

；其中，为待旋转角度，为向量与向量的夹角，为偏向角误差，A为当前位置，为第i个路径点的位置，为第i+1个路径点的位置，其中，位于A之前，位于A之后。

令k的数值为1。

采用机器人的惯性导航传感器，获取机器人在第k次旋转过程中，由静止状态加速至匀速状态时的旋转角度；其中，p表示第p个数据点，即检测到机器人开始进入到匀速状态的数据点。

在匀速状态下，利用机器人的惯性导航传感器持续获取机器人的旋转角度，直到满足公式，则控制机器人停止旋转；其中，为第s个数据点与第p个数据点之间的斜率，为第s-1个数据点与第p个数据点之间的斜率，为异常判断阈值。

利用如下公式计算第k次的旋转角度。

；其中，为第k次的旋转角度，为机器人在第k次旋转过程中，由静止状态加速至匀速状态时的旋转角度，为机器人惯性旋转函数，用于计算机器人由变为停止状态的旋转角度，为机器人在第k次旋转过程中，由静止状态加速至匀速状态时的旋转速度，为机器人在第k次旋转过程中，由匀速状态进入异常转向阶段时的旋转速度，为相邻两个数据点的时间间隔。

令k的数值增加1，返回“采用机器人的惯性导航传感器，获取机器人在第k次旋转过程中，由静止状态加速至匀速状态时的旋转角度”的步骤，直到旋转至所述待旋转角度。

可选的，采用前进中纠正偏差的方式纠正机器人的移动方向，具体包括如下步骤。

控制机器人的左侧电机和右侧电机的转速，使左侧电机与右侧电机的功率差小于功率失衡阈值；所述左侧电机用于为机器人的左侧驱动轮提供动力，所述右侧电机用于为机器人的右侧驱动轮提供动力。

或采用PID控制算法，控制机器人的左侧电机与右侧电机的转速，使机器人的路径垂直偏差小于垂直偏差阈值。

可选的，路径垂直偏差的计算公式为：；其中，为路径垂直偏差，为一次项系数，，为常数项系数，，和为第i个路径点的x轴坐标和y轴坐标，和为第i+1个路径点的x轴坐标和y轴坐标，和为当前位置的x轴坐标和y轴坐标。

可选的，在步骤“根据所述移动路径，采用路径跟踪算法，控制机器人移动至所述目标点”，及步骤“根据所述扫查路径，以所述目标点为起点完成所述目标点所在的未检测区域的检测，并更新所述栅格化储罐底板图纸”中还执行如下步骤。

采用机器人上设置的六个超声距离传感器，检测机器人行进过程中的新障碍物；所述新障碍物为在当前的栅格化储罐底板图纸上没有标记出的障碍物。

当存在新障碍物时，在所述栅格化储罐底板图纸上将所述新障碍物所在位置标记为不可通过区域，并采用四向种子填充算法检测栅格化储罐底板图纸上是否形成障碍物封闭区域。

当形成障碍物封闭区域时，在所述栅格化储罐底板图纸上将所述障碍物封闭区域所在位置标记为不可通过区域。

返回“根据机器人的超声波定位信号，确定机器人在栅格化储罐底板图纸中的当前位置”的步骤。

储罐底板在油检测机器人不可视环境路径跟踪装置包括：定位模块、检测传感器、避障传感器阵列和总控模块；所述定位模块包括位于机器人前方和后方的超声波发射器；所述避障传感器阵列包括位于机器人前方的三个不同安装方向的超声距离传感器和位于机器人后方的三个不同安装方向的超声距离传感器；所述定位模块、所述检测传感器和所述避障传感器阵列均与所述总控模块连接，所述总控模块与所述机器人的控制端连接；所述总控模块用于执行上述的方法，进行机器人的路径跟踪。

根据本发明提供的具体实施例，本发明公开了以下技术效果。

本发明实施例提供一种储罐底板在油检测机器人不可视环境路径跟踪方法及装置，所述方法包括：根据机器人的超声波定位信号，确定机器人在栅格化储罐底板图纸中的当前位置；所述栅格化储罐底板图纸上标记有不可通过区域、已检测区域和未检测区域；当所述当前位置位于已检测区域时，基于所述栅格化储罐底板图纸查找距离当前位置最近的未检测区域的单元格，作为目标点；利用所述栅格化储罐底板图纸，规划移动至所述目标点的移动路径和对所述目标点所在的未检测区域进行检测的扫查路径；根据所述移动路径，采用路径跟踪算法，控制机器人移动至所述目标点；根据所述扫查路径，以所述目标点为起点完成所述目标点所在的未检测区域的检测，并更新所述栅格化储罐底板图纸。本发明采用分区域的遍历算法实现路径规划，并采用路径跟踪算法实现对规划后路径的追踪，本发明可实现储罐在油检测机器人的自动化的路径规划和跟踪，极大地提高了检测的效率。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例1提供的一种储罐底板在油检测机器人不可视环境路径跟踪方法的流程示意图。

图2为本发明实施例提供的储罐底板在油检测机器人的结构示意图。

图3为本发明实施例提供的路径规划算法的流程图。

图4为本发明实施例提供的扩散法示例图。

图5为本发明实施例提供的八向寻路法示例图。

图6为本发明实施例提供的转向角异常判断的原理图。

图7为本发明实施例提供的直线前进路径跟踪结果示意图。

图8为本发明实施例提供的路径跟踪算法的流程图。

图9为本发明实施例提供的真实栅格图。

图10为本发明实施例提供的初始的虚拟路径规划图。

图11为本发明实施例提供的第一次更新后的虚拟路径规划图。

图12为本发明实施例提供的第二次更新后的虚拟路径规划图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种储罐底板在油检测机器人不可视环境路径跟踪方法及装置，可实现储罐在油检测机器人的自动化的路径规划和跟踪，极大地提高了检测的效率。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

实施例1

本发明实施例1提供一种储罐底板在油检测机器人不可视环境路径跟踪方法。

本发明实施例中的储罐底部在油检测机器人整体结构，如图2所示。机器人主体为长方体车身，前后各有一对驱动轮，依靠左右侧驱动轮的速度差实现转弯。定位模块为前方、后方的超声波发射器，其依靠支撑杆实现起落。检测传感器位于机器人主体底部的中心，多个检测传感器自左向右排列形成检测传感器阵列，可在直线行进时扫查宽度等于检测传感器阵列的宽度的底板。避障传感器阵列包括位于机器人首尾两端的两组避障传感器，安装于自机器人的主体伸出的支架上，每组避障传感器由三个不同方向安装的超声距离传感器组成，可扫查前、后方-45度、0度、45度方向偏下的一段距离内的地面障碍物。

对于机器人在路径跟踪过程中的定位方法和避障方法，本发明中只进行简单的原理介绍而不做为保护内容。

所述超声波发射器通过发送低频超声波脉冲实现定位，脉冲发射后由安装在储罐外壁同等高度的接收器环形阵列捕获，并根据到达时间计算声信号的传播距离，最终通过三点定位确定机器人位置。

所述避障传感器包含一个压电谐振器，当所述压电谐振器接受到交流脉冲信号后，将产生机械振动；该振动将引发声波，而该声波可用于近距离障碍物探测。当声波被前方障碍物反射回传感器后，原有低频脉冲信号的特征信号的幅值、周期均会产生变化，从而推断前方障碍到避障传感器的距离。所述避障传感器可扫查传感器正前方一个小角度扇形区域内的障碍物，探测距离在0.5米以内。

如图1所示，所述方法包括如下步骤。

步骤101，根据机器人的超声波定位信号，确定机器人在栅格化储罐底板图纸中的当前位置；所述栅格化储罐底板图纸上标记有不可通过区域、已检测区域和未检测区域。

本发明实施例中栅格化储罐底板图纸采用如下方式建立。

本发明实施例中将所述检测传感器阵列宽度设为，储罐直径为D，则将储罐区域划分为个格子，每个格子为一个单元格，其中，并向上取整。结果储存在地图矩阵S中，矩阵行向量和列向量的索引均为1到N。判断单元格在内或在外的方法为判断单元格中心是否在圆内。

如果距离大于零，则在圆外，标记为不可通过区域“9”；否则，标记为未检测区域“0”。表达为式（1）和式（2）。

（1）。

，（2）。

其中，和分别为第个单元格的横坐标和纵坐标，和分别为第个单元格的横坐标和纵坐标的单元格编号，和分别为栅格中心的横坐标和纵坐标。

如果储罐使用方可以提供精度较准确的图纸，则按栅格尺寸将障碍物以不可通过区域标识“9”的形式输入到已栅格化的未检测区域中。将障碍物转化为基本图形，例如，圆、椭圆、长方形、正方形后，以障碍物的代数方程判断障碍物是否在某个单元格的中心。

检测开始时不预设检测路径，当机器人由储罐人孔被吊装进入储罐内部后，通过启动所述超声波发射器可以确定自身的当前位置A。

步骤102，当所述当前位置位于已检测区域时，基于所述栅格化储罐底板图纸查找距离当前位置最近的未检测区域的单元格，作为目标点。以下步骤并非为机器人实际的运动过程，而是机器人预先在内存中的模拟过程。

根据地图矩阵S内不可通过区域和未检测区域，确定一条可以遍历未检测区域的路径，算法如图3所示。路径生成算法大致分为两步，寻路模式和扫查模式。寻路模式中，机器人以当前已检测区域“1”为起点，寻找离自己最近的未检测区域“0”，并避开障碍物的同时移动至所述未检测区域。扫查模式中，机器人从当前未检测区域开始，自上至下或自下至上在不可通过区域“9”之间沿列方向扫查，到达尽头后再返回寻路模式。

确定检测方向并移动到该列尽头。到达新的“0区”后，不是立刻检测，而是确认当前一列区域的特征。如果自当前位置向下的非“9区”的单元格数量大于向上的非“9区”的单元格数量，则自上至下检测；否则，自下至上检测。之后，按检测方向的反方向移动至这列的尽头，再沿检测方向检测。

若左侧或右侧区域为障碍物，则根据寻路模式规则，向左或向右移动进入下一列。此后，已检测区域简称“1区”，未检测区域简称“0区”，不可通过区域简称“9区”。

寻找未检测区域的具体步骤如下。

当机器人位于“1区”且右、左、下、上均不为“0区”时，采取“扩散法”寻找第一块未检测的“0区”，如图4所示。选定当前区域为中心，按照右、左、下、上的优先级，在中心点外的方框区域内以右下、右上、左下、左上、下右、下左、上右、上左的顺序查询八个点的状态；四个正方向将在当前方框内第一次扩散时重复，例如，右下、右上合并为一个单元格；如果在此后扩散中，八个方向上的点在方框角落里出现重复，则不必再寻找。当发现第一个“0区”后，完成寻找；否则，在若干区域所组成的方框内继续按照上述八个方向的顺序查找。如果当前方框内各区域已完成搜索，则开始搜索当前方框外的下一个方框区域。特殊情况，如果机器人已扫查完当前一列，则按右、左、下、上的顺序将把右侧区域定位目标区域；如果右侧为不可通过的“9区”，则按顺序将定位到左侧区域。

步骤103，利用所述栅格化储罐底板图纸，规划移动至所述目标点的移动路径和对所述目标点所在的未检测区域进行检测的扫查路径。以下步骤为机器人在内存中的模拟结果。

移动模拟过程中，按照八向寻路法移动，见图5，以当前位置为起始点，计算目标点与x轴之间夹角，与八个方向（0°、45°、90°、…、270°、315°）做比较；将夹角由小到大排序。之后选择排序第一的、夹角最小的方向，查看该方向上n个单元格以内是否存在障碍物。如果可以移动，则移动至尽头；否则，切换方向。移动后，将新的位置设为起始点，并进入迭代。

本方法的优点是计算速度快，适用于障碍环境较简单的场景，尤其是障碍物较少的场景，例如，只有牺牲阳极块、管道的储罐底板。然而，对于较复杂的场景，本方法则需要较长的寻路时间，其原因是每步移动只考虑了周围n个单元格内的环境。

步骤104，根据所述移动路径，采用路径跟踪算法，控制机器人移动至所述目标点。

机器人开始按照规划好的路径移动。路径跟踪可细分为前进和旋转。当机器人当前的运动方向与目标点的偏差较大时，则通过原地旋转纠正机器人的移动方向；否则，机器人在前进中纠正偏差。设机器人当前位置为A，上一个路径点的位置，即位于A之前的路径点的位置为，下一个路径点的位置，即位于A之后的路径点的位置为。如果向量与向量的夹角θ大于夹角阈值，则进入旋转模式。偏向角误差θ的计算公式为：，其中，()和()分别为向量与。

旋转模式的步骤如下：机器人先停止运动，之后计算向量与向量的夹角α。机器人需要旋转的角度为（θ+α）。机器人内的主控板带有惯性导航传感器，可精确测量机器人运动的三向加速度、角加速度。通过角加速度的周期性累计可以计算出机器人的转向角。然而，由于传感器的刷新率不足以完全记录机器人在转动过程中引发振动所产生的角加速度变化，转向角将出现错误，具体表现为匀速转动中的非线性转角变化。因此，必须对转向角进行补偿。

转向中的转向角变化共分为三个阶段：启动阶段，正常转向，异常转向，待旋转角度为，转向角的补偿方法如下。

启动阶段。首先，转向角归零，并开始记录转向角，之后机器人启动转向。旋转速度从零缓慢上升至匀速，这一阶段时间较短、速度较慢不会发生异常振动。

正常转向阶段。此时，转向角线性增加，开始监测转向角斜率。如果第s个数据点与第p个数据点之间的斜率的变化超过前一个所记录的斜率（即第s-1个数据点与第p个数据点之间的斜率）一定百分比，则认为出现异常，如图6所示，可记作：，其中，为异常判断阈值，；和分别为第s个数据点和第p个数据点记录的转向角，为相邻两个数据点的时间间隔，第p个数据点为检测到机器人开始进入到匀速状态的数据点，第s个数据点为机器人进入异常转向阶段的数据点。

异常转向阶段。判断转向为异常后，转向角停止记录、停止转向、并清零。用s个数据点前的转向角和角速度对本次转向角进行修正，表达式为：，其中，为第k次的旋转角度，为机器人在第k次旋转过程中，由静止状态加速至匀速状态时的旋转角度，为机器人惯性旋转函数，用于计算机器人由变为停止状态的旋转角度，为机器人在第k次旋转过程中，由静止状态加速至匀速状态时的旋转速度，为机器人在第k次旋转过程中，由匀速状态进入异常转向阶段时的旋转速度，为相邻两个数据点的时间间隔。

转向进入下一轮，直到旋转至所述待旋转角度。

前进模式的步骤如下。

对于轮式机器人，当左侧驱动轮与右侧驱动轮转速一致时，机器人前进；当左侧驱动轮速度较快时，机器人偏右转向；当右侧驱动轮速度较快时，机器人偏左转向。在实际运动过程中，机器人由于多种原因难以保持直线前进，或直线前进的路径和规划的路径会发生偏移，如图7所示。因此，不仅需要考虑纠偏的方法，还要考虑路径跟踪的方法。

通常，轮式机器人的转向通过控制驱动轮的转速实现，即左侧驱动轮转速等于右侧驱动轮转速时机器人向正前移动。然而，由于机械、硬件、环境等因素导致左右侧驱动轮出现偏差，从而使机器人的运动将发生偏斜。纠偏的目的是在命令机器人直线前进时消除上述因素引起的偏向移动。偏向是通过偏向角误差θ与路径垂直偏差d决定的。机器人的当前位置为A，上一个路径点的位置为，下一个路径点的位置。计算路径垂直偏差d的公式为：；其中，为路径垂直偏差，为一次项系数，，为常数项系数，，和为第i个路径点的x轴坐标和y轴坐标，和为第i+1个路径点的x轴坐标和y轴坐标，和为当前位置的x轴坐标和y轴坐标。

纠偏的方法包括机械纠偏、硬件纠偏、定位点纠偏。机械纠偏通过调整左右轮外径尺寸、轮轴高度等方法使对应的左右侧两轮在平地运动时达到直线前进，其主要目的是纠正机械零件的制造和安装误差。硬件纠偏通过监测左侧电机和右侧电机的功率以获取机器人左右两侧的负载，通过调整左侧电机和右侧电机的转速使机器人在直线前进过程中保持左侧电机和右侧电机的功率相等。定位点纠偏是通过声定位的方法获取机器人的坐标，从而通过电机转速修正偏向角误差；换句话说，定位点纠偏就是机器人沿直线行进时路径跟踪的方法。除机械纠偏外，其它两种纠偏方法均可在机器人行进过程中通过传感器获取机器人的偏向情况，从而减缓或纠正机器人的偏向。

硬件纠偏与定位点纠偏各有优点与缺点，因此需要根据不同传感器测量结果即时调整纠偏的方法。定位点是通过声信号定位得到的，由于存在一定误差，因此需要通过多次定位后回归的方法提高准确性，时效性和精度都较低。为提高机器人响应速度，需要硬件纠偏提高机器人的修正直线路径垂直偏差的响应速度。但是，硬件纠偏由于存在积累误差，因此只能修正地形对机器人左右驱动力的影响，不能修正机器人的偏向误差。综上所述，本发明结合了上述两种方法。在路径垂直偏差d较低的情况下采取硬件纠偏，使机器人基本沿直线前进；在d较高的情况下采取定位点纠偏，降低θ与d，直到二者降低到允许范围后再切换回硬件纠偏。

硬件纠偏的具体方法如下。

当通过定位点计算出的路径垂直偏差d小于垂直偏差阈值，且功率出现失衡时，开始硬件纠偏以遏制偏斜的倾向。通过驱动电机控制器可以监测电机的功率，左、右侧电机的功率分别为和。控制量为左、右侧电机的转速，分别为和。控制变量与输入量的关系如下所示。

，；其中，和分别为第t轮控制时左、右侧电机的转速，为第t轮控制时速度的输入量，参数K为开环控制参数，为第t轮控制时左右侧电机的功率差，。

上述表达式将使左右侧电机在每轮控制中根据当前功率差发生连续的变化。当机器人左侧负载增大时，机器人向左偏，而左侧电机的功率上升，为正值。此时，增大并减小将提升左侧驱动力并降低右侧驱动力，机器人左偏的趋势将减缓，直到最终按照左侧速度略高的实际输入量直线前进。

定位点纠偏的方法如下。

当通过定位点计算出的路径垂直偏差d大于垂直偏差阈值，且偏向角误差θ小于偏向角误差阈值时，开始通过定位点纠正机器人的前进路径。为修正路径垂直偏差d，采取PID控制，通过调整机器人的左、右侧电机的转速使d接近0。每轮控制循环内分别记录本轮路径垂直偏差的数值，上轮路径垂直偏差的数值，t轮控制循环内的积累误差。第t轮控制时的控制变量可表达为：；其中，P、I、D分别为比例、积分、微分参数。代表第t轮控制时机器人的左、右侧电机的转速之差，数值在-1与+1之间；小于0时机器人偏左，大于0时机器人偏右。

机器人的左、右侧电机的转速和可表达为：，。当机器人偏左且偏向误差较小，为较小的负数，那么增大减小，机器人将开始向偏右移动。

由于PID控制方法以路径垂直偏差为输入量，当机器人的路径垂直偏差d变为0后，运动方向可能仍然为斜向，不会立刻沿所规划路径前进。最终，运动轨迹是一个以所规划的运动路径为基准的振荡曲线。该曲线振荡的收敛速度可以通过调整PID参数提高。加快收敛速度的方法同常见PID控制算法，向提高比例参数P直至形成大幅振荡直到收敛的曲线，再提高微分参数D直至提高振荡的收敛速度，最后提高积分参数I降低平稳运动时的路径垂直偏差d。

如图8所示，路径跟踪算法的整体流程如下。

步骤801，初始化。根据储罐尺寸、已知障碍物，建立分区地图矩阵S并生成路径。此外，根据测试结果，初始化控制参数K、P、I、D。

步骤802，探测障碍物。机器人周期性读取前后六个超声距离传感器的数据。如果检测到未标记的障碍物，则根据机器人、障碍物的相对位置，将障碍物标记到地图矩阵S中。之后，重新生成路径。如果避障传感器未检测到障碍物或障碍物已被标记在地图矩阵S上，则进入步骤803。

步骤803，获取定位点与路径点。激活机器人的超声波发射器，发出定位信号，并等待接收器的信号。当三个以上接收器捕获到脉冲信号后，可根据三点定位方法计算机器人当前位置（即定位点），如果定位误差较明显，可以通过多个定位点求平均值，或回归的方法实现降噪。确认定位点后，将定位点与当前路径点做对比，如果二者之间距离远小于允许路径误差后，则认为机器人已抵达当前路径点，开始载入下一个路径点；否则，进入步骤804。

步骤804，计算路径误差。路径跟踪的模式由路径误差决定，包括偏向角误差θ和路径垂直偏差d。根据偏向角误差阈值和垂直偏差阈值，确定路径跟踪模式，包括旋转模式（步骤805）、硬件纠偏模式（步骤806）、定位点纠偏模式（步骤807）。

步骤805，旋转模式。当机器人到达的路径点为拐角，或者机器人行进出现较大偏向时，启动旋转模式原地调整机器人行进的方向。通过角速度比对、多次尝试的方法，可以克服振动对惯性导航传感器的干扰，较为准确的旋转所要求的角度。

步骤806，硬件纠偏模式。当路径垂直偏差d较低时，机器人保持硬件纠偏模式直线前进。通过平衡左右侧电机的功率，使机器人保持直线前进。

步骤807，定位点纠偏模式。当路径垂直偏差d较高时，机器人即使在以直线前进，但是已经积累了较高的误差，需要通过定位点坐标进行修正。

步骤105，根据所述扫查路径，以所述目标点为起点完成所述目标点所在的未检测区域的检测，并更新所述栅格化储罐底板图纸及地图矩阵S。

由于机器人处于不可视环境，无法探测远处障碍物，只能探测车身一周临近的六个方向是否存在障碍。因此，移动过程中通过避障传感器阵列探测周围环境。一旦发现未标记的障碍，则进入障碍处理程序。

当机器人的六个避障传感器之一检测到未标记的障碍物时，则在所对应的距离、所对应方向的单元格处，将“0区”标定为“9区”。此时，重新生成新的路径，具体流程如下。

探测器发现新障碍。按照车长、宽、探测器角度、目标距离确定对应的单元格。从当前的地图矩阵S中查找，是否该单元格为“9区”。如果未标记，则停止检测。特别地，需要避免生成封闭的障碍区域，所述区域为中心为“0区”、边缘为“9区”。封闭区域探测的方法为：标记新障碍后，采用“四向种子填充算法”检测被障碍封闭的区域。复制地图矩阵S，并以其中任一点为种子，按照右、左、下、上的顺序将“0区”和“1区”染成“9区”。如果染色结束后仍然存在“0区”，则以该点为种子在地图矩阵S中将“0区”染成“9区”。

生成新路径。连同已检测区域“1区”导出已更新的地图矩阵S’与当前的机器人所在的单元格。重新生成路径时，以当前机器人的区域为起始点，在剩余“0区”内生成路径。

在检测过程中，如果检测完当前的未检测区域，则当前的未检测区域在图纸上已标记为已检测区域，再返回步骤101进行下一个目标点的寻找，当地图矩阵S中无法通过扩散法找到新的“1区”后，认定完成检测。此时，按照八向寻路法，使机器人返回人孔位置。

需要说明的是，本发明实施例中，步骤101、102、103均为虚拟过程，机器人没有运动，步骤104、105为实际运动过程。一个完整的流程是：在步骤101中生成地图，反复执行步骤102和步骤103生成路径；开始运动，反复执行步骤104和步骤105；发现障碍后，更新地图，并返回步骤101，再反复执行步骤102和步骤103。

实施例2

本实施例通过简化后的储罐模型对本发明提到的方法进行具体说明。

储罐直径D为14米，机器人的检测传感器阵列宽度1米，避障传感器探测范围0.5米。

开始检测时，机器人从位于区域的人孔近入储罐，进入初始化阶段，首次进入步骤101。其中，区域为图9-图12中的第6行第1列的单元格。

步骤101，建立栅格化储罐底板图纸，如图9所示。每个区域为边长1米的正方形，灰色区域位于储罐外，定义为“9区”，白色区域位于储罐内，定义为“0区”，初始时的地图矩阵S为：。

图9中带有斜线的区域为实际存在但未被探明的障碍物，在初始的地图矩阵S中不进行标记。

步骤102，开始在内存中初始化一条遍历未检测区域，即“0区”的扫查路径，如图10所示。以下具体举例说明图3所示的路径规划算法步骤。

机器人初始位于“0区”，跳入扫查模式；确定第1列的扫查方向为自上至下，由于区域为“9区”，跳入图3步骤④生成子路径到，其中，和的下脚标的含义为：“，”号左边的数字表示列，“，”号右边的数字表示行，和表示的区域为对应行列的单元格，以下不再解释。

由于区域被标记为“1区”，进入寻路模式图3步骤①；根据四项扩散法，确定目标区域为最右侧区域。

步骤103，规划移动至的路径。进入图3步骤②，生成路径到。

重复步骤102，进入扫查模式，分析第2列特征并确定扫查方向为自下至上；进入图3步骤③，生成路径到；进入图3步骤④生成检测路径到。

以此类推，完成初始遍历路径，即扫查路径，表达式如下所示。

。

步骤104，采用路径跟踪算法，沿内存中规划好的遍历路径移动机器人。在区域到之间移动时，进入前进模式并采用PID控制方法等纠偏方法保持机器人沿直线行进；当机器人移动到区域后，获取到的下个区域为，规划路径与机器人当前朝向相差90度，因此进入旋转模式；旋转完成后，再进入前进模式，在区域到之间直线移动。以此类推，实现机器人运动中的路径跟踪。

步骤105，在前进模式中进行检测，并持续不断的探测障碍物。如图11所示，当机器人由向运动过程中，在区域通过避障传感器检测到前方出现障碍物后，机器人运动停止。此时，地图矩阵S如下式所示。

。

其中，被标记为不可通过区域“9区”。

重复执行步骤101以更新地图矩阵S，反复执行步骤102和步骤103生成新的遍历路径。在内存中新的遍历路径可表达为。之后，机器人重新启动，反复执行步骤104和步骤105。

如图12所示，当机器人运动到区域后，将检测出第二个障碍物，并将前方区域标记为“9区”，并生成新的遍历路径。

此后的过程以此类推。

本发明为储罐底板在油检测机器人提出了一种在不可视环境下基于避障传感器、超声定位模块的路径跟踪算法。首先，不可视环境分区域遍历算法为机器人提供了一种在不可视的原油环境下依靠传感器实现地图绘制、区域遍历、跨区域移动的方法。在机器人的实际探索过程中，机器人通过路过跟踪算法实现对已规划好路径的跟踪，利用所提出的多种算法修正自动控制中可能出现的传感器误差，例如，定位信号偏差、转向角异常、路径垂直偏差等。本发明为储罐底板在油检测机器人提供了一种可行的自动化检测方法，在储罐检测工程应用上具有极大地应用前进。

以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (8)

1.一种储罐底板在油检测机器人不可视环境路径跟踪方法，其特征在于，所述方法包括：

根据机器人的超声波定位信号，确定机器人在栅格化储罐底板图纸中的当前位置；所述栅格化储罐底板图纸上标记有不可通过区域、已检测区域和未检测区域；

当所述当前位置位于已检测区域时，基于所述栅格化储罐底板图纸查找距离当前位置最近的未检测区域的单元格，作为目标点；

利用所述栅格化储罐底板图纸，规划移动至所述目标点的移动路径和对所述目标点所在的未检测区域进行检测的扫查路径；

根据所述移动路径，采用路径跟踪算法，控制机器人移动至所述目标点；

根据所述扫查路径，以所述目标点为起点完成所述目标点所在的未检测区域的检测，并更新所述栅格化储罐底板图纸；

利用所述栅格化储罐底板图纸，规划移动至所述目标点的移动路径，具体包括：

令m的数值为1；

将当前位置作为第m个起点；

确定第m个起点与目标点的方向与所有可移动方向的夹角，并确定夹角最小的可移动方向为目标移动方向；所述可移动方向为0°、45°、90°、135°、180°、225°、270°和315°八个方向上第m个起点的下一个单元格不属于不可通过区域的方向；

确定目标移动方向上以第m个起点为起始点的n个单元格内是否存在不可通过区域；n为单次规划的预设单元格数量；

当不存在不可通过区域时，将以第m个起点为起始点在目标移动方向上移动n个单元格的子路径添加至移动路径中，并将目标移动方向上以第m个起点为起始点的第n个单元格作为第m+1个起点；

当存在不可通过区域时，将以第m个起点为起始点在目标移动方向上移动n’个单元格的子路径添加至移动路径中，并将目标移动方向上以第m个起点为起始点的第n’个单元格作为第m+1个起点；n’为目标移动方向上与第m个起点最近的不可通过区域的单元格与第m个起点之间的单元格的数量；

将m的数值增加1，并返回“确定第m个起点与目标点的方向与所有可移动方向的夹角，并确定夹角最小的可移动方向为目标移动方向”的步骤，直到获得连接所述当前位置与所述目标点的移动路径。

2.根据权利要求1所述的储罐底板在油检测机器人不可视环境路径跟踪方法，其特征在于，基于所述栅格化储罐底板图纸查找距离当前位置最近的未检测区域的单元格的方法为扩散法。

3.根据权利要求1所述的储罐底板在油检测机器人不可视环境路径跟踪方法，其特征在于，根据所述移动路径，采用路径跟踪算法，控制机器人移动至所述目标点，具体包括：

根据机器人的超声波定位信号，确定机器人在栅格化储罐底板图纸中的当前位置；

根据所述当前位置和所述移动路径计算偏向角误差；

当所述偏向角误差大于偏向角误差阈值时，采用原地旋转的方式纠正机器人的移动方向；

当所述偏向角误差不大于偏向角误差阈值时，采用前进中纠正偏差的方式纠正机器人的移动方向。

4.根据权利要求3所述的储罐底板在油检测机器人不可视环境路径跟踪方法，其特征在于，偏向角误差的计算公式为：

；

其中，为偏向角误差，和分别为A与之间的x轴距离和y轴距离，和分别为与之间的x轴距离和y轴距离，为第i个路径点的位置，A为当前位置，为第i+1个路径点的位置，其中，位于A之前，位于A之后。

5.根据权利要求3所述的储罐底板在油检测机器人不可视环境路径跟踪方法，其特征在于，采用原地旋转的方式纠正机器人的移动方向，具体包括：

利用如下公式计算待旋转角度；

；

其中，为待旋转角度，为向量与向量的夹角，为偏向角误差，A为当前位置，为第i个路径点的位置，为第i+1个路径点的位置，其中，位于A之前，位于A之后；

令k的数值为1；

采用机器人的惯性导航传感器，获取机器人在第k次旋转过程中，由静止状态加速至匀速状态时的旋转角度；其中，p表示第p个数据点，即检测到机器人开始进入到匀速状态的数据点；

在匀速状态下，利用机器人的惯性导航传感器持续获取机器人的旋转角度，直到满足公式，则控制机器人停止旋转；其中，为第s个数据点与第p个数据点之间的斜率，为第s-1个数据点与第p个数据点之间的斜率，为异常判断阈值；

利用如下公式计算第k次的旋转角度；

；

其中，为第k次的旋转角度，为机器人在第k次旋转过程中，由静止状态加速至匀速状态时的旋转角度，为机器人惯性旋转函数，用于计算机器人由变为停止状态的旋转角度，为机器人在第k次旋转过程中，由静止状态加速至匀速状态时的旋转速度，为机器人在第k次旋转过程中，由匀速状态进入异常转向阶段时的旋转速度，为相邻两个数据点的时间间隔；

令k的数值增加1，返回“采用机器人的惯性导航传感器，获取机器人在第k次旋转过程中，由静止状态加速至匀速状态时的旋转角度”的步骤，直到旋转至所述待旋转角度。

6.根据权利要求3所述的储罐底板在油检测机器人不可视环境路径跟踪方法，其特征在于，采用前进中纠正偏差的方式纠正机器人的移动方向，具体包括：

控制机器人的左侧电机和右侧电机的转速，使左侧电机与右侧电机的功率差小于功率失衡阈值；所述左侧电机用于为机器人的左侧驱动轮提供动力，所述右侧电机用于为机器人的右侧驱动轮提供动力；

或采用PID控制算法，控制机器人的左侧电机与右侧电机的转速，使机器人的路径垂直偏差小于垂直偏差阈值。

7.根据权利要求6所述的储罐底板在油检测机器人不可视环境路径跟踪方法，其特征在于，路径垂直偏差的计算公式为：

；

其中，为路径垂直偏差，为一次项系数，，为常数项系数，，和为第i个路径点的x轴坐标和y轴坐标，和为第i+1个路径点的x轴坐标和y轴坐标，和为当前位置的x轴坐标和y轴坐标。

8.一种储罐底板在油检测机器人不可视环境路径跟踪装置，其特征在于，所述装置包括：定位模块、检测传感器、避障传感器阵列和总控模块；

所述定位模块包括位于机器人前方和后方的超声波发射器；

所述避障传感器阵列包括位于机器人前方的三个不同安装方向的超声距离传感器和位于机器人后方的三个不同安装方向的超声距离传感器；

所述定位模块、所述检测传感器和所述避障传感器阵列均与所述总控模块连接，所述总控模块与所述机器人的控制端连接；

所述总控模块用于执行权利要求1-7任一项所述的方法，进行机器人的路径跟踪。