1 引言
随着国家十二五规划纲要关于建设智能坚强电网要求的提出,智能电网信息化、数字化、网络化已经成为当前电网智能化发展的一个热点研究领域。目前,承载着电网输变电环节的变电站,多采用人工巡检方式监控变电设备的运行状态,这一传统巡检方式费事费力。因此,基于智能机器人的无人值班变电站巡检技术已然成为智能化变电站发展的一个创新性热点话题,而机器人实现自主巡检任务的关键及难点在于如何构建机器人自主导航定位系统。
目前,常用的导航系统多种多样,导航原理、定位精度及成本造价也存在较大差别。常用的导航定位系统包括黑白线识别导航、磁导航、GPS/惯性组合导航、视觉图像导航、激光雷达/惯性组合导航等。而较为成熟的机器人黑白线识别导航技术,通过激光对地面黑白线进行反射接收识别,保证机器人始终沿预设白线行走,该方法简单易行,但施工较大,且易受大雪天气影响;磁导航技术则利用磁传感与测量技术,通过在地面铺设磁条,保证机器人始终沿预设磁航道行走,该方法虽然解决了大雪天气遮挡黑白线的问题,但其成本较大,且变电站长期强磁干扰容易导致磁条失磁,降低其灵敏度,最终可能导致导航失效。DGPS/惯性导航系统[1]定位精度可以到达亚米级,定位灵活方便,但GPS受天气、变电站强电磁干扰等外界环境因素影响较大,考虑系统的容错性,需要增加额外独立的导航子系统配合使用,而该系统成本造价高、经济性较差。视觉图像导航定位系统借鉴人体视觉导航原理,利用图像识别及神经网络技术,通过机器人预先对巡检环境自主学习,建立基于当前环境模型下的知识库与规则库,即利用已有学习经验来实现自主巡检任务,但该方法技术难度较大,有待进一步深化研究。
本文提出的惯性/激光雷达组合导航定位技术,利用高精度激光雷达测距技术,在预设全局路径
[2]的前提下,通过多点全向扫描测距、信息融合与滤波处理技术,对机器人当前运行环境进行视觉建模,同时利用多点测距技术实现实时定位,该方法成本低廉,定位精度高,但对现场环境的依赖性相对较大,易受外界随机干扰,需要优化程序算法,减小外界的随机干扰误差等。
文献
[3]基于激光雷达测距信息进行机器人周边环境建模,提出了一种时变势场算法,并通过对比实验引入了一种改进型算法—多分辨率势场法,该算法可以有效地实现机器人导航与避障,实时性非常好。文献
[4]讨论了基于激光雷达的机器人实时位姿估计方法,通过基于Hough变换的切线角度直方图算法和迭代切线加权最近点算法,解决了传统方法中未能解决的局部最小值、类孔径及大计算量问题,该方法精度高,实时性好,并且对随机噪声、遮挡和类孔径问题具有高鲁棒性特点,适用性广。上述算法对本文中激光雷达的数据程序处理方法有一定的借鉴作用。
本文基于DR/LMS的组合导航定位技术,特别地,提出了一种基于卡尔曼滤波算法的定位精度优化估计方法,通过DR子系统与LMS子系统的相互补充与修正,对原有系统定位及导航精度(定位精度可以达到cm级)加以改进与优化,进一步提高了导航定位的稳定性与精确性。文献
[5]通过对外界随机干扰噪声的统计与建模,对Kalman滤波理论做了详细的推导与误差分析,同时提出了改进型的非线性卡尔曼滤波模型与算法,并给出了实例分析与验证。
2 DR导航定位系统原理
我们这里提出的DR(Dead-Reckoning)航位推算是一种基于机器人车体运动模型的、自主式导航定位系统,可以实现二维平面内的航位推算。本系统采用里程计与激光雷达的组合式传感器定位系统。里程计主要用于采样机器人左右轮的行走路程
,而机器人航位角
的采样计算则是高精度激光雷达的应用之一(激光雷达测距系统同时实现外部测距定位与航向角的采样计算)。基于激光雷达的机器人偏转航向角
测量系统的优点是有效解决了强电磁干扰条件下电子罗盘失效以及光纤陀螺累积误差带来的定位精度大幅降低等一系列难题,但不足的是其需要依赖于外部参考测距对象,独立性较差。
2.1 DR导航定位系统原理图简介
DR导航定位系统航位推算原理如图1所示。已知初始点A的定位坐标
、导航测量角
及光电位移传感器测量里程
可以计算出系统下一时刻在B点的坐标估计值。
其中,
,
为车轮旋转一周的传动里程,k为旋转光电位移传感器的采样脉冲计数(车轮每旋转一周进行一次脉冲采样),
则为程序采样周期内,机器人的行驶距离。
表示机器人与道路方向(Y轴方向)的夹角大小,其值由激光测距系统采样间接计算得到。因此,在已知机器人初始位置
的前提下,由式(1)可以求解得到机器人在X、Y轴方向的行驶位移。
(1)
图1 DR系统导航定位原理图
通过上式,可以实时推算机器人当前所在坐标。但鉴于机器人车体本身存在机械不对称性,里程计存在一定的测量误差,并考虑左右轮电机出力的不同步性,其定位精度经现场测试,可以达到80cm~100cm左右,虽然可以基本满足现场应用,但由于惯性DR导航精度具有随时间发散的特性,即长期稳定性差,必须辅助外在定位系统进行及时的校正。
2.2 DR导航定位系统程序流程图
DR导航定位系统程序流程图见图2所示。
图2 DR导航定位系统程序流程图
3 激光雷达测距导航定位原理
本系统采用了SICK公司生产的LMS激光雷达全向测距传感器,该测距系统采用双脉冲快速测距技术,以50hz的扫描频率实现360度、20m范围内障碍物的快速扫描、高精度(mm级)测距需求。测距系统与上位机LABVIEW软件系统通过以太网接口实现相互通信,数据传输及处理速度能够满足实际工业需求。
机器人通过发送采样相应请求指令,对周围360度、20m范围内障碍物进行距离采样,采样角度间隔为0.5度或0.25度可选。那么,对于同一障碍物或标记物,实际中会得到多个采样距离值,因此需要对这些采样距离值进行均值滤波等一系列数据处理与合并。而如何保证滤波后的距离值与实际障碍物或标记物的一一映射关系,即实现采样数据的识别与验证则是整个定位系统的关键。
本定位子系统误差来源:(1)道路两侧给定参考标记物的地理坐标测量误差;(2)实测目标距离值滤波合并后的处理误差。
实现的功能:(1)机器人通过测量其距离道路两侧标记物(物理坐标给定)的各距离值,通过圆或三角形方程求解,粗略计算其所在的地理坐标(x,y)。(2)机器人利用扫描道路两侧标记物得到的扫描角度间隔,并结合道路方向与机器人车头方向(即扫描中线角度值),精确计算(误差相互抵消)得到机器人的实际行走方向,即航向角
。而机器人正常行走的二维控制参数就是上述计算得到的x坐标(横向偏移)与导航角
。
3.1 LMS导航定位系统原理图简介
图3 LMS系统导航定位原理图
LMS导航定位系统原理图见图3所示,其定位与导航计算式分别见式(2)(3):
(2)
其中,
,
分别为参考点1,2的测量坐标值;
,
分别为机器人距离参考点1,2的激光测距半径值;
则为需要求解的机器人实际坐标值。
该定位原理简单,但重点、难点在于如何对多点扫描距离值进行滤波处理,包括多点合并、滤波等效以及干扰值的验证与排错等诸多环节,其预处理流程图见图4。只有保证测距目标与实际距离的一一对应,以及多点滤波合并后的高精度性,才能进一步提高定位精度。其定位精度经现场测试,可以达到20~40cm,可以满足现场工业应用需求。
航向角计算式如下:
(3)
其中,
、
表示采样预处理后,机器人距离当前两参考目标1,2的有效半径值;
表示半径
、
之间的扫描间隔角度;
表示经激光测距得到的参考点1,2之间的等效距离值(这里,考虑到1,2参考点间距
的人工量测误差,我们以高精度激光测距系统的理论计算值为计算标准);
表示距离机器人最近的参考目标1或2分别与机器人定位点、次近参考目标2或1连线的夹角计算值;
、
分别表示参考目标坐标点1,2连线的斜率、道路方向的斜率(考虑到道路方向,即Y轴方向斜率为无穷大,这里我们将X,Y坐标轴进行调换求两直线的夹角); r 表示道路方向与参考目标点1,2连线的夹角;
表示
、
中的较小者对应的扫描角度值;
表示机器人实际车头方向与
之间的间隔角度(其中π是车头方向的扫描角度,固定不变);
表示机器人理想前进方向(即Y轴道路方向)与 α 之间的间隔角度(三角形求解);
表示机器人实际车头方向与道路方向的偏角计算值,即我们要求的机器人航向角。
本算法实现相对简单,只需要通过激光测距系统对当前环境参数进行识别、采样即可快速实现在线计算与控制。其中
、
作差抵消了多点扫描带来的绝对误差,有效提高了航向角精度。经现场测试,可以达到0.1度甚至更高,可以满足实际工业应用需求。此外,本算法的特别之处在于,当选取的参考点1,2关于道路左右对称,即
时,由式(3)中(b)(e)(g)可以看出,航向角仅与当前采样参数有关,而与目标参考点的坐标选取无关,进一步提高了系统的独立性。
3.2 LMS导航定位系统的程序流程图
LMS导航定位系统程序流程图如图4所示。
图4 LMS系统导航定位程序流程图
4 卡尔曼滤波在组合导航定位系统中的应用
卡尔曼滤波本质上是一种建立在时域空间内的线性最小方差估计算法,具有时域状态递推性,适用于对多维随机过程(平稳、非平稳)进行状态估计,包括了连续与离散两类算法。其算法简洁高效,便于在计算机上实现,而卡尔曼滤波在实际组合导航系统中是其较为成功的一个典范。本文以随机线性离散卡尔曼滤波为理论基础,实现了变电站智能机器人的实时精确导航与定位。
4.1 卡尔曼滤波在DR/LMS组合导航定位系统中的现场应用
基于Kalman滤波的DR/LMS组合导航定位系统整体结构图如图5所示:
上述两导航定位子系统各有其优缺点:DR系统自主导航定位能力较强,但位移传感器测量精度较低,使得系统定位精度随时间发散,即长期稳定性差;LMS系统虽然其精度高,但受外在环境影响较大,主要表现在参考点的识别与交替更新在某些不可预测条件下的误判现象,从而导致定位错乱等。为了保证系统的定位精度、提高系统的稳定性,这里我们引入一种基于卡尔曼滤波的组合导航技术,即DR/LMS组合导航技术,通过最优线性估计算法,使两个定位子系统相互校正,取长补短,既可以及时校正DR航位推算随时间带来的航位漂移,又可以有效地修正LMS系统对参考点的错误识别,极大地改善了整个系统的动态稳定性与准确性。
基于卡尔曼滤波模型,并考虑到导航角本身的计算精度可以满足需求,因此,这里我们仅对DR/LMS组合导航系统的X、Y定位坐标作Kalman滤波,方程如下:
状态方程:
(4)
观测方程:
(5)
考虑到前后两次采样时间极小,故认为
,因此,这里我们取
k-1表示k-1状态到k状态过渡过程的等效航向角。上式中,
分别表示机器人在X轴、Y轴方向的状态位置坐标;
k-1表示状态控制量,即计算航向角;
表示各采样阶段的转移控制系数;
、
表示DR推算过程噪声序列;、
、
表示DR推算过程噪声输入系数;
、
分别表示激光测距计算得到的机器人定位点的x、y坐标测量等效值;
、
表示LMS测量系统等效噪声序列。
根据变电站现场环境,系统过程噪声序列与观测噪声序列可以看做是均值恒定的高斯白噪声随机序列,并设系统过程噪声(即估计噪声)方差为
,系统观测噪声(即LMS测量噪声)方差为
。且在整个滤波过程中,系统过程噪声序列与观测噪声序列不相关,系统初始状态
与过程噪声序列、观测噪声序列也不相关。系统定位坐标初始状态、
由激光测距系统给定,滤波误差初始值P
0已知。具体的噪声建模及参数确定,由于篇幅限制不作详述,可以参考文献[5]。
依据Kalman滤波求解步骤,DR/LMS系统的滤波估计求解过程如下:
I. 状态一步预测:
(6)
II. 状态估计:
(7)
III. 滤波增益:
(8)
IV. 一步预测误差:
(9)
V. 滤波估计误差:
(10)
4.2 DR/LMS组合导航系统现场运行结果分析
(1) 基于Matlab的主要导航参数波形分析
机器人视觉区域范围内对参考标记点的自主识别与更新曲线如图6所示,图(1)绿色曲线表示机器人视觉范围内距离参考标记点的距离值变化曲线,即随着机器人的前进路径方向,其距离参考标记点的距离值逐渐减小,直至进入下一段参考域时,参考标记将会发生更新替换,此时参考距离值会发生瞬时突变,即上图中绿色曲线由小及大的突变,也就是说机器人将根据测量距离(绿色)这一视觉参量来及时更新参考标记点,以便进入下一参考域的定位导航阶段。图(2)蓝色曲线表示与测量距离相对应的测量角度变化趋势,其主要用于视觉识别中干扰物的查找、验证及纠错等。中图绿色曲线表示突变脉冲,即参考距离值发生指定裕度内的突变时,输出记为布尔值1,反之,记为0。相应地,对于中图每次的突变脉冲,图(3)则对应显示了该时刻机器人参考标记点的依次替换、更新过程。
在本系统中,机器人实现理想直线行走的关键在于两个控制量的计算:X坐标偏移量Vx和导航角
。由于取Y轴方向为机器人理想行驶方向,故X坐标能实时记录机器人与中心路径(X=α)的偏移量Vx。
图7显示了机器人沿Y轴方向行驶过程中纵坐标Y的变化曲线。其中蓝色曲线(一部分与红色曲线相互重合)表示未经限幅滤波的轨迹变化,参杂了外界噪声的干扰,而红色曲线则添加了限幅滤波,过滤掉了实际过程中不可能出现的坐标变化量,该试验曲线线性度非常好,完全符合现场运行轨迹。经过卡尔曼滤波后的Y坐标变化曲线线性度与此红色曲线类似,区别仅仅在于相对挺高了定位精度。
图8显示了组合导航系统X坐标定位值的精度变化,图中道路中心线方向的直线方程为X=3.3附近。其中,黄线代表机器人实际行走过程中X定位值的正常变化趋势,即X坐标的理想变化曲线;蓝线代表DR/LMS组合导航定位系统原始定位坐标X的变化曲线;红线代表组合导航系统经限幅滤波、均值滤波等一系列常用经典滤波后的滤波定位坐标X的变化曲线;绿线则代表组合导航系统引入Kalman滤波后的X值变化曲线。因此,我们可以明显地看出,与X值理想定位变化趋势相比,经过Kalman滤波后的定位波形具有更高的精度与可信度。经现场测试,其定位精度完全符合实际中的运行轨迹,精度可以达到10cm左右,引入模糊控制后的系统参数可调裕度大,运行效果非常理想。
(2) 基于Labview的导航系统现场轨迹试验波形分析
图9 机器人现场运行轨迹模拟图
图9对变电站现场地理环境进行了直观的描述,坐标系的选取如图中所示。这里,取道路方向为Y轴方向,道路宽度介于X=1.7~4.7之间。其中,方块表示参考电气附件(杆塔),各个电气附件的Y坐标如图中所示,而将道路左侧电气附件的X坐标统一定义为0,道路右侧电气附件的X坐标根据现场测量结果统一定义为6.8,这样也就建立了导航系统的参考坐标知识库。根据机器人现场运行轨迹,这里我们用图中蓝色曲线近似模拟机器人的实际行走路线。在Y=40.1的标记杆塔之前,机器人有偏离道路中心线向右行走的趋势,偏离幅度在10cm~20cm之间,而经过该杆塔之后,在控制系统的反馈调节作用下,机器人则重新回到道路中心线,保持理想前进方向。
图10基于Labview数据处理与图形显示软件,对本导航系统的主要导航参数之一:实时定位坐标(X,Y)进行了直观化地显示。定位坐标同时也是整个控制系统的参数之一。图中红色曲线表示经过卡尔曼滤波后的机器人现场定位的轨迹路线测试图。对比图10与图9,我们可以明显地看出,机器人实际运行轨迹始终保持在X=3.2附近,且轨迹变化趋势基本符合上述模拟条件,定位精度可以保持在20cm左右,完全可以满足现场运行环境的要求。
图11则显示了本导航系统的另一个主要导航参数:实时航向角。导航角也给出了控制系统的另一主要参数,对机器人行驶方向与道路中心线方向偏离程度 进行了量化。图中绿色曲线直观地显示了机器人航向角的偏离程度,根据左偏角度为负,右偏角度为正的定义,在Y=40.1前后两个阶段,导航角的总体趋势由正变到0度附近,表明了机器人先右偏后左偏最终回到道路中心线的趋势,符合现场运行条件,导航精度完全可以现场运行条件。
5 结束语
通过在500KV超高压变电站现场测试及结果分析,有效地验证了本文算法实现的有效性。实践证明,DR-LMS组合导航系统实现方便,原理相对简单,且不易受变电站强电磁环境的干扰,稳定性高,实时性好(更新速率可以达到10Hz甚至更高),经济性强,即在大幅度降低导航系统成本的同时,其定位精度完全可以达到厘米级,完全不亚于同等精度条件下的其他导航系统,完全可以满足现场工业应用需求,在智能变电站建设中具有广阔的应用前景与研究价值。
参考文献:
[1] 魏鹏,张志强,张春熹等.无人值守变电站巡检机器人导航系统研究[J].自动化与仪表,2009,(12):5-8,25.
[2] 孟偲,王田苗.一种移动机器人全局最优路径规划算法[J].机器人,2008,27(3):217-222.
[3] 杨明,王宏,何克忠,张钹.基于激光雷达的移动机器人环境建模与避障[J].清华大学学报(自然科学版),2000,40(7):112-116.
[4] 杨明,董斌,王宏等.基于激光雷达的移动机器人实时位姿估计方法研究[J].自动化学报,2004,30(5):679-687.
[5] 付梦印,邓忠红,张继伟.Kalman滤波理论及其在导航系统中的应用[M].北京:科学出版社,2003.