《2023年种基于卡尔曼滤波的DRLMS组合导航定位算法.doc》由会员分享，可在线阅读，更多相关《2023年种基于卡尔曼滤波的DRLMS组合导航定位算法.doc（11页珍藏版）》请在金锄头文库上搜索。
1、一种基于卡尔曼滤波的DR/LMS组合导航定位算法变电站/机器人/卡尔曼滤波1 引言 随着国家十二五规划纲要关于建设智能坚强电网要求的提出,智能电网信息化、数字化、网络化已经成为当前电网智能化开展的一个热点研究领域。目前,承载着电网输变电环节的变电站,多采用人工巡检方式监控变电设备的运行状态,这一传统巡检方式费事费力。因此,基于智能机器人的无人值班变电站巡检技术已然成为智能化变电站开展的一个创新性热点话题,而机器人实现自主巡检任务的关键及难点在于如何构建机器人自主导航定位系统。 目前,常用的导航系统多种多样,导航原理、定位精度及本钱造价也存在较大差异。常用的导航定位系统包括黑白线识别导航、磁导航
2、、GPS/惯性组合导航、视觉图像导航、激光雷达/惯性组合导航等。而较为成熟的机器人黑白线识别导航技术,通过激光对地面黑白线进行反射接收识别,保证机器人始终沿预设白线行走,该方法简单易行,但施工较大,且易受大雪天气影响;磁导航技术那么利用磁传感与测量技术,通过在地面铺设磁条,保证机器人始终沿预设磁航道行走,该方法虽然解决了大雪天气遮挡黑白线的问题,但其本钱较大,且变电站长期强磁干扰容易导致磁条失磁,降低其灵敏度,最终可能导致导航失效。DGPS/惯性导航系统1定位精度可以到达亚米级,定位灵活方便,但GPS受天气、变电站强电磁干扰等外界环境因素影响较大,考虑系统的容错性,需要增加额外独立的导航子系统
3、配合使用,而该系统本钱造价高、经济性较差。视觉图像导航定位系统借鉴人体视觉导航原理,利用图像识别及神经网络技术,通过机器人预先对巡检环境自主学习,建立基于当前环境模型下的知识库与规那么库,即利用已有学习经验来实现自主巡检任务,但该方法技术难度较大,有待进一步深化研究。 本文提出的惯性/激光雷达组合导航定位技术,利用高精度激光雷达测距技术,在预设全局路径2的前提下,通过多点全向扫描测距、信息融合与滤波处理技术,对机器人当前运行环境进行视觉建模,同时利用多点测距技术实现实时定位,该方法本钱低廉,定位精度高,但对现场环境的依赖性相对较大,易受外界随机干扰,需要优化程序算法,减小外界的随机干扰误差等。
4、 文献3基于激光雷达测距信息进行机器人周边环境建模,提出了一种时变势场算法,并通过比照实验引入了一种改良型算法多分辨率势场法,该算法可以有效地实现机器人导航与避障,实时性非常好。文献4讨论了基于激光雷达的机器人实时位姿估计方法,通过基于Hough变换的切线角度直方图算法和迭代切线加权最近点算法,解决了传统方法中未能解决的局部最小值、类孔径及大计算量问题,该方法精度高,实时性好,并且对随机噪声、遮挡和类孔径问题具有高鲁棒性特点,适用性广。上述算法对本文中激光雷达的数据程序处理方法有一定的借鉴作用。 本文基于DR/LMS的组合导航定位技术,特别地,提出了一种基于卡尔曼滤波算法的定位精度优化估计方法
5、,通过DR子系统与LMS子系统的相互补充与修正,对原有系统定位及导航精度(定位精度可以到达cm级)加以改良与优化,进一步提高了导航定位的稳定性与精确性。文献5通过对外界随机干扰噪声的统计与建模,对Kalman滤波理论做了详细的推导与误差分析,同时提出了改良型的非线性卡尔曼滤波模型与算法,并给出了实例分析与验证。2 DR导航定位系统原理 我们这里提出的DR(Dead-Reckoning)航位推算是一种基于机器人车体运动模型的、自主式导航定位系统,可以实现二维平面内的航位推算。本系统采用里程计与激光雷达的组合式传感器定位系统。里程计主要用于采样机器人左右轮的行走路程,而机器人航位角的采样计算那么是
6、高精度激光雷达的应用之一(激光雷达测距系统同时实现外部测距定位与航向角的采样计算)。基于激光雷达的机器人偏转航向角测量系统的优点是有效解决了强电磁干扰条件下电子罗盘失效以及光纤陀螺累积误差带来的定位精度大幅降低等一系列难题,但缺乏的是其需要依赖于外部参考测距对象,独立性较差。2.1 DR导航定位系统原理图简介 DR导航定位系统航位推算原理如图1所示。初始点A的定位坐标、导航测量角及光电位移传感器测量里程可以计算出系统下一时刻在B点的坐标估计值。 其中,为车轮旋转一周的传动里程,k为旋转光电位移传感器的采样脉冲计数(车轮每旋转一周进行一次脉冲采样),那么为程序采样周期内,机器人的行驶距离。表示机
7、器人与道路方向(Y轴方向)的夹角大小,其值由激光测距系统采样间接计算得到。因此,在机器人初始位置的前提下,由式(1)可以求解得到机器人在X、Y轴方向的行驶位移。 (1)图1 DR系统导航定位原理图 通过上式,可以实时推算机器人当前所在坐标。但鉴于机器人车体本身存在机械不对称性,里程计存在一定的测量误差,并考虑左右轮电机出力的不同步性,其定位精度经现场测试,可以到达80cm100cm左右,虽然可以根本满足现场应用,但由于惯性DR导航精度具有随时间发散的特性,即长期稳定性差,必须辅助外在定位系统进行及时的校正。2.2 DR导航定位系统程序流程图 DR导航定位系统程序流程图见图2所示。图2 DR导航
8、定位系统程序流程图3 激光雷达测距导航定位原理 本系统采用了SICK公司生产的LMS激光雷达全向测距传感器,该测距系统采用双脉冲快速测距技术,以50hz的扫描频率实现360度、20m范围内障碍物的快速扫描、高精度(mm级)测距需求。测距系统与上位机LABVIEW软件系统通过以太网接口实现相互通信,数据传输及处理速度能够满足实际工业需求。 机器人通过发送采样相应请求指令,对周围360度、20m范围内障碍物进行距离采样,采样角度间隔为0.5度或0.25度可选。那么,对于同一障碍物或标记物,实际中会得到多个采样距离值,因此需要对这些采样距离值进行均值滤波等一系列数据处理与合并。而如何保证滤波后的距离
9、值与实际障碍物或标记物的一一映射关系,即实现采样数据的识别与验证那么是整个定位系统的关键。 本定位子系统误差来源:(1)道路两侧给定参考标记物的地理坐标测量误差;(2)实测目标距离值滤波合并后的处理误差。 实现的功能:(1)机器人通过测量其距离道路两侧标记物(物理坐标给定)的各距离值,通过圆或三角形方程求解,粗略计算其所在的地理坐标(x,y)。(2)机器人利用扫描道路两侧标记物得到的扫描角度间隔,并结合道路方向与机器人车头方向(即扫描中线角度值),精确计算(误差相互抵消)得到机器人的实际行走方向,即航向角。而机器人正常行走的二维控制参数就是上述计算得到的x坐标(横向偏移)与导航角。3.1 LM
10、S导航定位系统原理图简介图3 LMS系统导航定位原理图 LMS导航定位系统原理图见图3所示,其定位与导航计算式分别见式(2)(3):(2) 其中,分别为参考点1,2的测量坐标值;,分别为机器人距离参考点1,2的激光测距半径值;那么为需要求解的机器人实际坐标值。 该定位原理简单,但重点、难点在于如何对多点扫描距离值进行滤波处理,包括多点合并、滤波等效以及干扰值的验证与排错等诸多环节,其预处理流程图见图4。只有保证测距目标与实际距离的一一对应,以及多点滤波合并后的高精度性,才能进一步提高定位精度。其定位精度经现场测试,可以到达2040cm,可以满足现场工业应用需求。 航向角计算式如下:(3) 其中
11、,、表示采样预处理后,机器人距离当前两参考目标1,2的有效半径值;表示半径、之间的扫描间隔角度;表示经激光测距得到的参考点1,2之间的等效距离值(这里,考虑到1,2参考点间距的人工量测误差,我们以高精度激光测距系统的理论计算值为计算标准);表示距离机器人最近的参考目标1或2分别与机器人定位点、次近参考目标2或1连线的夹角计算值;、分别表示参考目标坐标点1,2连线的斜率、道路方向的斜率(考虑到道路方向,即Y轴方向斜率为无穷大,这里我们将X,Y坐标轴进行调换求两直线的夹角); r 表示道路方向与参考目标点1,2连线的夹角;表示、中的较小者对应的扫描角度值;表示机器人实际车头方向与之间的间隔角度(其
12、中是车头方向的扫描角度,固定不变);表示机器人理想前进方向(即Y轴道路方向)与 之间的间隔角度(三角形求解);表示机器人实际车头方向与道路方向的偏角计算值,即我们要求的机器人航向角。 本算法实现相对简单,只需要通过激光测距系统对当前环境参数进行识别、采样即可快速实现在线计算与控制。其中、作差抵消了多点扫描带来的绝对误差,有效提高了航向角精度。经现场测试,可以到达0.1度甚至更高,可以满足实际工业应用需求。此外,本算法的特别之处在于,中选取的参考点1,2关于道路左右对称,即时,由式(3)中(b)(e)(g)可以看出,航向角仅与当前采样参数有关,而与目标参考点的坐标选取无关,进一步提高了系统的独立
13、性。3.2 LMS导航定位系统的程序流程图 LMS导航定位系统程序流程图如图4所示。图4 LMS系统导航定位程序流程图4 卡尔曼滤波在组合导航定位系统中的应用 卡尔曼滤波本质上是一种建立在时域空间内的线性最小方差估计算法,具有时域状态递推性,适用于对多维随机过程(平稳、非平稳)进行状态估计,包括了连续与离散两类算法。其算法简洁高效,便于在计算机上实现,而卡尔曼滤波在实际组合导航系统中是其较为成功的一个典范。本文以随机线性离散卡尔曼滤波为理论根底,实现了变电站智能机器人的实时精确导航与定位。4.1 卡尔曼滤波在DR/LMS组合导航定位系统中的现场应用 基于Kalman滤波的DR/LMS组合导航定
14、位系统整体结构图如图5所示: 上述两导航定位子系统各有其优缺点:DR系统自主导航定位能力较强,但位移传感器测量精度较低,使得系统定位精度随时间发散,即长期稳定性差;LMS系统虽然其精度高,但受外在环境影响较大,主要表现在参考点的识别与交替更新在某些不可预测条件下的误判现象,从而导致定位错乱等。为了保证系统的定位精度、提高系统的稳定性,这里我们引入一种基于卡尔曼滤波的组合导航技术,即DR/LMS组合导航技术,通过最优线性估计算法,使两个定位子系统相互校正,取长补短,既可以及时校正DR航位推算随时间带来的航位漂移,又可以有效地修正LMS系统对参考点的错误识别,极大地改善了整个系统的动态稳定性与准确
15、性。基于卡尔曼滤波模型,并考虑到导航角本身的计算精度可以满足需求,因此,这里我们仅对DR/LMS组合导航系统的X、Y定位坐标作Kalman滤波,方程如下:状态方程:(4)观测方程: (5) 考虑到前后两次采样时间极小,故认为,因此,这里我们取k-1表示k-1状态到k状态过渡过程的等效航向角。上式中,分别表示机器人在X轴、Y轴方向的状态位置坐标;k-1表示状态控制量,即计算航向角;表示各采样阶段的转移控制系数;、表示DR推算过程噪声序列;、表示DR推算过程噪声输入系数;、分别表示激光测距计算得到的机器人定位点的x、y坐标测量等效值;、表示LMS测量系统等效噪声序列。 根据变电站现场环境,系统过程噪声序列与观测噪声序列可以看做是均值恒定的高斯白噪声随机序列,并设系统过程噪声(即估计噪声)方差为,系统观测噪声(即LMS测量噪声)方差为。且在整个滤波过程中,系统过程噪声序列与观测噪声序列不相关,系统初始状态与过程噪声序列、观测噪声序列也不相关。系统定位坐标初始状态、由激光
