一种变电站巡检机器人导航控制系统及方法与流程

本发明涉及一种变电站巡检机器人导航控制系统及方法。

背景技术：

变电站巡检机器人是一种全自主运行的地面移动机器人，其可携带可见光摄像机、红外热像仪、拾音器等传感器对电力设备进行自动巡检，有效降低人工巡检的劳动强度，保障变电站设备安全运行。

在变电站巡检机器人的应用中，可靠稳定的导航运行是保证其完成巡检任务的基础。目前，巡检机器人变电站内导航运行一般采用磁轨迹引导方式实现，相应的导航控制系统主要由磁传感器阵列、导航控制模块和运动控制模块三部分组成。磁传感器阵列是由数个单一磁传感器沿直线排列制成，每当磁轨迹产生磁场落入阵列中传感器检测范围内时，对应传感器将以数字开关量方式输出以表明检测到磁轨迹；导航控制模块则采样传感器阵列中各传感器输出开关量状态序列，即当前传感器阵列状态，以得到传感器阵列中心相对于磁轨迹的横向偏移大小，并据此生成运动控制量发送至运动控制模块执行，从而保证机器人沿磁轨迹运行。

基于磁轨迹引导的导航控制系统，目前已在巡检机器人产品中得到了普遍应用，但在实际应用过程中，却发现机器人高速运行时运行稳定性不理想，制约了巡检任务执行效率的进一步提升。造成该问题的主要原因，经分析如下：

1.由于磁传感器阵列由数量有限的单个磁传感器组成，其中心相对于磁轨迹的横向偏移所产生的传感器阵列状态为有限个，现有巡检机器人导航控制策略一般是根据每个传感器阵列状态通过查表输出预先设定的运动控制量，从而使机器人运行在某一速度下保持稳定，若超出该速度限制机器人运行稳定性将会显著降低。

2.由于磁传感器阵列中每个传感器都有一定的检测范围，且只有当磁轨迹磁场进入该检测范围时传感器开关量输出状态才会发生变化，这将造成磁传感器阵列输出不能实时跟踪偏移量变化，基于传感器阵列状态的导航控制策略，必然会存在导航控制输出滞后，这也会对机器人高速运行时的稳定性带来不利影响。

针对上述问题，虽然可以通过增加传感器阵列中磁传感器数量或者针对不同运行速度设定不同运动控制量的办法，以提高机器人导航控制的效果，但这必然会增加机器人生产制造成本，同时相关控制量参数的人工调整也比较困难。

技术实现要素：

本发明为了解决上述问题，提出了一种变电站巡检机器人导航控制系统及方法，本发明在不对现有巡检机器人导航控制系统做大幅改变的前提下，有效提升巡检机器人导航控制效果。

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

一种变电站巡检机器人导航控制系统，包括磁传感器阵列、阵列状态处理模块、导航控制模块和运动控制模块，其中：

所述磁传感器模块，采集实际横向偏移量，将传感器阵列状态信息传输给阵列状态处理模块；

所述阵列状态处理模块，连接磁传感器阵列和导航控制模块，用于依据磁传感器阵列标定信息，得到磁传感器阵列状态所对应的横向偏移量以及状态变化时间间隔，之后发送至导航控制模块；

所述导航控制模块，用于利用当前横向偏移量和状态改变时间间隔计算导航控制量，之后发送至运动控制模块；

所述运动控制模块执行导航控制模块的导航指令，控制巡检机器人的运动。

所述磁传感器模块以数字开关量输出方式与阵列状态处理模块连接，然后阵列状态处理模块则利用数字通讯方式与导航控制模块连接，最后导航控制模块也通过数字通讯方式连接运动控制模块。

所述运动控制模块内还包括电机驱动器与车轮驱动电机，电机驱动器驱动车轮驱动电机工作。

一种变电站巡检机器人导航控制方法，包括以下步骤：

(1)机器人导航运行开始时，进行初始化，同时载入磁传感器阵列标定信息；

(2)机器人导航运行过程中，阵列状态处理模块采集传感器阵列状态输出，与传感器阵列标定信息进行比较，得到当前磁传感器阵列中心相对于磁轨迹的横向偏移量，在此过程中，阵列状态处理模块还负责在传感器阵列状态改变时测量状态改变之间的时间间隔；

(3)导航控制模块综合利用阵列状态处理模块得到的当前横向偏移量和状态改变时间间隔，计算得到导航控制量并发送至运动控制模块执行。

所述步骤(1)中，磁传感器阵列标定信息是在机器人导航运行前，通过执行传感器标定步骤得到，进行标定的主要目的则是为了将各传感器阵列状态所代表的横向偏移量进行定量描述，以便后续导航控制使用。

所述步骤(1)中，具体方法为：首先将机器人静止放置于水平面上，使标定用磁轨迹至于磁传感器一侧水平面；由于磁传感器阵列中传感器一般沿垂直于机器人运行方向排列，标定时需面向机器人运行的反方向，从传感器阵列的左外侧向右外侧沿水平面滑动，此过程中，将磁传感器阵列中首次检测到磁轨迹的位置设为零点，之后每当磁传感器阵列状态变化就记录当时磁轨迹位置与零点之间的距离，直至磁轨迹从右端滑出为止；最后，求标定过程得到距离数值的均值，再将以上距离值与该均值相减，从而得到传感器阵列每个状态所代表的横向偏移量。

所述步骤(2)中，测量传感器阵列状态改变时间间隔步骤为：

(2-1)磁传感器阵列检测到磁轨迹时，阵列状态处理模块计时器清零，并开始计时；

(2-2)当磁传感器阵列输出状态变化，则读取当前计时器值Tc，如果在计时器允许最大计时时间Tmax内，当前读出值Tc即为相邻状态变化时间间隔ΔT，否则将相邻状态变化时间间隔ΔT修正为计时器允许最大计时值Tmax；

(2-3)清零计时器，并开始下一次状态变化时间间隔测量计时。

所述步骤(3)中，导航控制量的具体计算方法包括：

(3-1)将从阵列状态处理模块得到的当前横向偏移量Dc与系数Kp相乘，得到横向偏移控制量Up；

(3-2)将当前横向偏移量Dc和上一次获取的横向偏移量Dp相减，得到横向偏移变化量，之后该变化量再乘以采样控制周期Ts与阵列状态变化时间间隔ΔT之商，从而得到横向偏移变化趋势值，最后该趋势值乘以系数Kd，得到横向偏移变化趋势控制量Ud；

(3-3)横向偏移控制量Up与横向偏移变化趋势控制量Ud相加，即可得到最终的导航控制量U。

所述步骤(3-3)中，最终的导航控制量U的计算方法为：

U＝Up+Ud＝Kp*Dc+Kd*(Dc-Dp)*(Ts/ΔT)

其中：Up为横向偏移控制量，Ud为横向偏移变化趋势控制量；Kp为横向偏移控制系数，Kd为横向偏移变化趋势控制系数；Dc为当前获取的横向偏移量，Dp为上一次获取的横向偏移量；Ts为系统采样控制周期，ΔT为磁传感器阵列相邻状态变化时间间隔。

本发明的有益效果为：

(1)通过在导航控制策略内引入横向偏移误差变化趋势信息，相当于在导航控制系统内引入了“微分”环节，因此可以实现导航控制系统对机器人运动导致的横向偏移变化趋势进行预测并提前进行调整，改善原有导航控制系统中存在的导航控制输出滞后对系统稳定性造成的影响，提升机器人运行的稳定性。

(2)通过磁传感器阵列标定，对不同磁传感器阵列状态代表的横向偏差进行量化处理，在前述获得横向偏差和横向偏差变化趋势信息基础上，巡检机器人导航控制也可很方便的采用其它控制策略(如模糊控制等)实现，从而在不对现有巡检机器人导航控制系统做大幅改变的前提下，为后续机器人导航控制性能的进一步提升奠定了必要的基础。

(3)导航控制系统性能调试时，只需调整横向偏移控制系数和横向偏移变化趋势控制系数，与原有导航系统需要对每个传感器阵列状态调整并设定控制量输出值相比，降低了系统性能调试的难度。

附图说明

图1为本发明的导航控制系统结构示意图；

图2为本发明的导航系统运行流程图；

图3为本发明的磁传感器阵列标定示意图；

图4为本发明的导航控制原理框图。

具体实施方式：

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

如图1所示，一种变电站巡检机器人导航控制系统，包括：磁传感器阵列、阵列状态处理模块、导航控制模块和运动控制模块。各模块的硬件连接关系为：首先磁传感器模块以数字开关量输出方式与阵列状态处理模块连接，之后阵列状态处理模块则利用数字通讯方式与导航控制模块连接，最后导航控制模块也通过数字通讯方式连接运动控制模块。另外，运动控制模块内又包含了电机驱动器与车轮驱动电机。

磁传感器阵列、导航控制模块和运动控制模块均为现有巡检机器人中已经存在的模块，主要区别是导航控制模块内包含的导航控策略与现有机器人导航控制策略不同；阵列状态处理模块位于磁传感器阵列和导航控制模块之间，将得到磁传感器阵列状态所对应的横向偏移量以及状态变化时间间隔发送至导航控制模块，供机器人导航控制使用。

如图2所示，在上述导航控制系统的基础上，提出了巡检机器人导航控制方法，其控制流程，包括以下步骤：

步骤1机器人导航运行开始时，初始化系统内各模块，同时载入磁传感器阵列标定信息；

步骤2机器人导航运行过程中，阵列状态处理模块采集传感器阵列状态输出，并与传感器标定信息进行比较，得到当前磁传感器阵列中心相对于磁轨迹的横向偏移量Dc。在此过程中，阵列状态处理模块还负责在传感器阵列状态改变时测量状态改变之间的时间间隔ΔT；

步骤3导航控制模块综合利用阵列状态处理模块得到的当前横向偏移量Dc和状态改变时间间隔ΔT，计算得到导航控制量u，并发送至运动控制模块执行。

所述步骤1中，磁传感器阵列标定信息是在机器人导航运行前，通过执行传感器标定步骤得到，进行标定的主要目的则是为了将各传感器阵列状态所代表的横向偏移量进行定量描述，以便后续导航控制使用。

如图3所示，具体的磁传感器阵列标定的方法为：首先将机器人静止放置于水平面上，使标定用磁轨迹至于磁传感器一侧水平面；由于磁传感器阵列中传感器一般沿垂直于机器人运行方向排列，标定时需面向机器人运行的反方向，从传感器阵列的左外侧向右外侧沿水平面滑动，此过程中，将磁传感器阵列中首次检测到磁轨迹的位置设为零点，之后每当磁传感器阵列状态变化就记录当时磁轨迹位置与零点之间的距离，直至磁轨迹从右端滑出为止；最后，求标定过程得到距离数值的均值，再将以上距离值与该均值相减，从而得到传感器阵列每个状态所代表的横向偏移量。

所述步骤2中，测量传感器阵列状态改变时间间隔步骤为：

步骤2-1磁传感器阵列检测到磁轨迹时，阵列状态处理模块计时器清零，并开始计时；

步骤2-2当磁传感器阵列输出状态变化，则读取当前计时器值Tc，如果在计时器允许最大计时时间Tmax内，当前读出值Tc即为相邻状态变化时间间隔ΔT，否则将相邻状态变化时间间隔ΔT修正为计时器允许最大计时值Tmax；

步骤2-3清零计时器，并开始下一次状态变化时间间隔测量计时。

如图4所示，步骤3中导航控制量的具体计算步骤如下：

步骤3-1将从阵列状态处理模块得到的当前横向偏移量Dc与系数Kp相乘，得到横向偏移控制量Up；

步骤3-2将当前横向偏移量Dc和上一次获取的横向偏移量Dp相减，得到横向偏移变化量，之后该变化量再乘以采样控制周期Ts与状态变化时间间隔ΔT之商，从而得到横向偏移变化趋势值，最后该趋势值乘以系数Kd，得到横向偏移变化趋势控制量Ud；

步骤3-3将上述横向偏移控制量Up与横向偏移变化趋势控制量Ud相加，即可得到最终的导航控制量U,其完整计算公式如下所示：

U＝Up+Ud＝Kp*Dc+Kd*(Dc-Dp)*(Ts/ΔT)

其中：Up为横向偏移控制量，Ud为横向偏移变化趋势控制量；Kp为横向偏移控制系数，Kd为横向偏移变化趋势控制系数；Dc为当前获取的横向偏移量，Dp为上一次获取的横向偏移量；Ts为系统采样控制周期，ΔT为磁传感器阵列相邻状态变化时间间隔。

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