第8卷第3期 智能系统学报 Vol.8 No.3 2013年6月 CAAI Transactions on Intelligent Systems Jum.2013 D0I:10.3969/i.issn.1673-4785.201203012 网络出版地址：http://www.cnki.net/kcms/detail/23.1538.TP.20130310.2131.001.html 一 种基于卡尔曼滤波与模糊算法的 变电站机器人组合导航及控制系统设计 李大伟，贾鹏飞1，李卫国'，张学龙1，渠展展 (1.华北电力大学电气与电子工程学院，北京102206：2.中国电力科学研究院电工与新材料技术研究所，北京100192) 摘要：针对不确定因素较多、外在干扰强的变电站工业现场环境，提出了一种新颖的基于卡尔曼能波与模糊控制 的变电站机器人组合导航与控制系统设计方法，实现机器人自主导航定位、自主行为控制，完成智能变电站无人巡 检任务.特别地，设计了一种基于高精度激光雷达的导航子系统，定位的同时兼有电子罗盘功能（即航角测量），有效 克服了变电站强电磁干扰环境下电子罗盘失效等问题现场试验证明，机器人行走轨迹与预设路径十分接近，可以实 现强电磁环境下的巡检要求，且具有设计成本低、精度高（亚米级）、实现方便等一系列优点，便于工业应用. 关键词：变电站：机器人；组合导航：控制系统：模糊控制：激光香达；卡尔曼滤波 中图分类号：TP242.2文献标志码：A文章编号：1673-4785(2013)03-0226-07 中文引用格式：李大伟，贾鹏飞，李卫国，等.一种基于卡尔曼滤波与模糊算法的变电站机器人组合导航及控制系统设计[J]智能系 统学报，2013,8(3)：226-232. 英文引用格式：LI Dawei,JIA Pengfei,LI Weiguo,etal.A kind of integrated navigation and control system design for substation robot based on the Kalman filtering and fuzzy algorithm[J].CAAI Transactions on Intelligent Systems,2013,8(3):226-232. A kind of integrated navigation and control system design for substation robot based on the Kalman filtering and fuzzy algorithm LI Dawei',JIA Pengfei',LI Weiguo',ZHANG Xuelong',QU Zhanzhan2 (1.School of Electrical Electronic Engineering,North China Electric Power University,Beijing 102206,China;2.Institute of Elec- trical New Materials,China Electric Power Research Institute,Beijing 100192,China) Abstract:This paper aims at examining the substation's industrial environment which has a number of uncertainty factors and strong external disturbance.This paper proposes a novel design method for the integrated navigation and control system which is used in a substation intelligent robot based on the Kalman filtering and fuzzy control theory, for the purpose of fulfilling an autonomous navigation,positioning and behavior control,and finishing the unattend- ed inspection task.In particular,a navigation subsystem based on high-precision laser radar has the functions of po- sitioning,and that of electronic compass (for navigation angle measurement),which effectively overcomes the fail- ure of compass in substation with strong electromagnetic interference.The field test shows that the robot's actual track is close to the assigned track,achieving the independent inspection mission in a strong electromagnetic inter- ference environment.In addition,the design also has the advantages of lower cost,higher precision (sub-meter), and more easy implementation for industrial use. Keywords:transformer substation;robot;integrated navigation;control system;fuzzy control;laser radar;Kalman filte- ring 目前常见的机器人导航系统主要包括黑白线循 及激光雷达导航等.不同的导航系统各自有其优缺 迹导航、磁导航、GPS/DR组合导航、视觉图像导航 点，黑白线导航原理简单易行、稳定可靠但易受雨雪 收稿日期：2012-03-15.网络出版日期：2013-03-10. 天气影响：磁导航成本相对较高且易受强电磁环境 通信作者：李大伟.E-mail:myzone2009_vip@sina.com 干扰：GPS/DR组合导航)定位精度高、灵活方便
第 ８ 卷第 ３ 期 智 能 系 统 学 报 Ｖｏｌ．８ №．３ ２０１３ 年 ６ 月 ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ Ｊｕｎ． ２０１３ ＤＯＩ：１０．３９６９ ／ ｊ．ｉｓｓｎ．１６７３⁃４７８５．２０１２０３０１２ 网络出版地址：ｈｔｔｐ： ／ ／ ｗｗｗ．ｃｎｋｉ．ｎｅｔ ／ ｋｃｍｓ／ ｄｅｔａｉｌ ／ ２３．１５３８．ＴＰ．２０１３０３１０．２１３１．００１．ｈｔｍｌ 一种基于卡尔曼滤波与模糊算法的 变电站机器人组合导航及控制系统设计 李大伟１ ，贾鹏飞１ ，李卫国１ ，张学龙１ ，渠展展２ （１．华北电力大学 电气与电子工程学院，北京 １０２２０６； ２．中国电力科学研究院 电工与新材料技术研究所，北京 １００１９２） 摘 要：针对不确定因素较多、外在干扰强的变电站工业现场环境，提出了一种新颖的基于卡尔曼滤波与模糊控制 的变电站机器人组合导航与控制系统设计方法，实现机器人自主导航定位、自主行为控制，完成智能变电站无人巡 检任务．特别地，设计了一种基于高精度激光雷达的导航子系统，定位的同时兼有电子罗盘功能（即航角测量），有效 克服了变电站强电磁干扰环境下电子罗盘失效等问题．现场试验证明，机器人行走轨迹与预设路径十分接近，可以实 现强电磁环境下的巡检要求，且具有设计成本低、精度高（亚米级）、实现方便等一系列优点，便于工业应用． 关键词：变电站；机器人；组合导航；控制系统；模糊控制；激光雷达；卡尔曼滤波 中图分类号：ＴＰ２４２．２ 文献标志码：Ａ 文章编号：１６７３⁃４７８５（２０１３）０３⁃０２２６⁃０７ 中文引用格式：李大伟，贾鹏飞，李卫国，等．一种基于卡尔曼滤波与模糊算法的变电站机器人组合导航及控制系统设计［ Ｊ］．智能系 统学报， ２０１３， ８（３）： ２２６⁃２３２． 英文引用格式：ＬＩ Ｄａｗｅｉ， ＪＩＡ Ｐｅｎｇｆｅｉ， ＬＩ Ｗｅｉｇｕｏ， ｅｔ ａｌ． Ａ ｋｉｎｄ ｏｆ ｉｎｔｅｇｒａｔｅｄ ｎａｖｉｇａｔｉｏｎ ａｎｄ ｃｏｎｔｒｏｌ ｓｙｓｔｅｍ ｄｅｓｉｇｎ ｆｏｒ ｓｕｂｓｔａｔｉｏｎ ｒｏｂｏｔ ｂａｓｅｄ ｏｎ ｔｈｅ Ｋａｌｍａｎ ｆｉｌｔｅｒｉｎｇ ａｎｄ ｆｕｚｚｙ ａｌｇｏｒｉｔｈｍ［Ｊ］． ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ， ２０１３， ８（３）： ２２６⁃２３２． Ａ ｋｉｎｄ ｏｆ ｉｎｔｅｇｒａｔｅｄ ｎａｖｉｇａｔｉｏｎ ａｎｄ ｃｏｎｔｒｏｌ ｓｙｓｔｅｍ ｄｅｓｉｇｎ ｆｏｒ ｓｕｂｓｔａｔｉｏｎ ｒｏｂｏｔ ｂａｓｅｄ ｏｎ ｔｈｅ Ｋａｌｍａｎ ｆｉｌｔｅｒｉｎｇ ａｎｄ ｆｕｚｚｙ ａｌｇｏｒｉｔｈｍ ＬＩ Ｄａｗｅｉ １ ， ＪＩＡ Ｐｅｎｇｆｅｉ １ ， ＬＩ Ｗｅｉｇｕｏ １ ， ＺＨＡＮＧ Ｘｕｅｌｏｎｇ １ ， ＱＵ Ｚｈａｎｚｈａｎ ２ （ １． Ｓｃｈｏｏｌ ｏｆ Ｅｌｅｃｔｒｉｃａｌ ＆ Ｅｌｅｃｔｒｏｎｉｃ Ｅｎｇｉｎｅｅｒｉｎｇ， Ｎｏｒｔｈ Ｃｈｉｎａ Ｅｌｅｃｔｒｉｃ Ｐｏｗｅｒ Ｕｎｉｖｅｒｓｉｔｙ， Ｂｅｉｊｉｎｇ １０２２０６， Ｃｈｉｎａ； ２． Ｉｎｓｔｉｔｕｔｅ ｏｆ Ｅｌｅｃ⁃ ｔｒｉｃａｌ ＆ Ｎｅｗ Ｍａｔｅｒｉａｌｓ， Ｃｈｉｎａ Ｅｌｅｃｔｒｉｃ Ｐｏｗｅｒ Ｒｅｓｅａｒｃｈ Ｉｎｓｔｉｔｕｔｅ， Ｂｅｉｊｉｎｇ １００１９２， Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ：Ｔｈｉｓ ｐａｐｅｒ ａｉｍｓ ａｔ ｅｘａｍｉｎｉｎｇ ｔｈｅ ｓｕｂｓｔａｔｉｏｎ’ｓ ｉｎｄｕｓｔｒｉａｌ ｅｎｖｉｒｏｎｍｅｎｔ ｗｈｉｃｈ ｈａｓ ａ ｎｕｍｂｅｒ ｏｆ ｕｎｃｅｒｔａｉｎｔｙ ｆａｃｔｏｒｓ ａｎｄ ｓｔｒｏｎｇ ｅｘｔｅｒｎａｌ ｄｉｓｔｕｒｂａｎｃｅ． Ｔｈｉｓ ｐａｐｅｒ ｐｒｏｐｏｓｅｓ ａ ｎｏｖｅｌ ｄｅｓｉｇｎ ｍｅｔｈｏｄ ｆｏｒ ｔｈｅ ｉｎｔｅｇｒａｔｅｄ ｎａｖｉｇａｔｉｏｎ ａｎｄ ｃｏｎｔｒｏｌ ｓｙｓｔｅｍ ｗｈｉｃｈ ｉｓ ｕｓｅｄ ｉｎ ａ ｓｕｂｓｔａｔｉｏｎ ｉｎｔｅｌｌｉｇｅｎｔ ｒｏｂｏｔ ｂａｓｅｄ ｏｎ ｔｈｅ Ｋａｌｍａｎ ｆｉｌｔｅｒｉｎｇ ａｎｄ ｆｕｚｚｙ ｃｏｎｔｒｏｌ ｔｈｅｏｒｙ， ｆｏｒ ｔｈｅ ｐｕｒｐｏｓｅ ｏｆ ｆｕｌｆｉｌｌｉｎｇ ａｎ ａｕｔｏｎｏｍｏｕｓ ｎａｖｉｇａｔｉｏｎ， ｐｏｓｉｔｉｏｎｉｎｇ ａｎｄ ｂｅｈａｖｉｏｒ ｃｏｎｔｒｏｌ， ａｎｄ ｆｉｎｉｓｈｉｎｇ ｔｈｅ ｕｎａｔｔｅｎｄ⁃ ｅｄ ｉｎｓｐｅｃｔｉｏｎ ｔａｓｋ． Ｉｎ ｐａｒｔｉｃｕｌａｒ， ａ ｎａｖｉｇａｔｉｏｎ ｓｕｂｓｙｓｔｅｍ ｂａｓｅｄ ｏｎ ｈｉｇｈ⁃ｐｒｅｃｉｓｉｏｎ ｌａｓｅｒ ｒａｄａｒ ｈａｓ ｔｈｅ ｆｕｎｃｔｉｏｎｓ ｏｆ ｐｏ⁃ ｓｉｔｉｏｎｉｎｇ， ａｎｄ ｔｈａｔ ｏｆ ｅｌｅｃｔｒｏｎｉｃ ｃｏｍｐａｓｓ （ｆｏｒ ｎａｖｉｇａｔｉｏｎ ａｎｇｌｅ ｍｅａｓｕｒｅｍｅｎｔ）， ｗｈｉｃｈ ｅｆｆｅｃｔｉｖｅｌｙ ｏｖｅｒｃｏｍｅｓ ｔｈｅ ｆａｉｌ⁃ ｕｒｅ ｏｆ ｃｏｍｐａｓｓ ｉｎ ｓｕｂｓｔａｔｉｏｎ ｗｉｔｈ ｓｔｒｏｎｇ ｅｌｅｃｔｒｏｍａｇｎｅｔｉｃ ｉｎｔｅｒｆｅｒｅｎｃｅ． Ｔｈｅ ｆｉｅｌｄ ｔｅｓｔ ｓｈｏｗｓ ｔｈａｔ ｔｈｅ ｒｏｂｏｔ’ ｓ ａｃｔｕａｌ ｔｒａｃｋ ｉｓ ｃｌｏｓｅ ｔｏ ｔｈｅ ａｓｓｉｇｎｅｄ ｔｒａｃｋ， ａｃｈｉｅｖｉｎｇ ｔｈｅ ｉｎｄｅｐｅｎｄｅｎｔ ｉｎｓｐｅｃｔｉｏｎ ｍｉｓｓｉｏｎ ｉｎ ａ ｓｔｒｏｎｇ ｅｌｅｃｔｒｏｍａｇｎｅｔｉｃ ｉｎｔｅｒ⁃ ｆｅｒｅｎｃｅ ｅｎｖｉｒｏｎｍｅｎｔ． Ｉｎ ａｄｄｉｔｉｏｎ， ｔｈｅ ｄｅｓｉｇｎ ａｌｓｏ ｈａｓ ｔｈｅ ａｄｖａｎｔａｇｅｓ ｏｆ ｌｏｗｅｒ ｃｏｓｔ， ｈｉｇｈｅｒ ｐｒｅｃｉｓｉｏｎ （ｓｕｂ⁃ｍｅｔｅｒ）， ａｎｄ ｍｏｒｅ ｅａｓｙ ｉｍｐｌｅｍｅｎｔａｔｉｏｎ ｆｏｒ ｉｎｄｕｓｔｒｉａｌ ｕｓｅ． Ｋｅｙｗｏｒｄｓ：ｔｒａｎｓｆｏｒｍｅｒ ｓｕｂｓｔａｔｉｏｎ； ｒｏｂｏｔ； ｉｎｔｅｇｒａｔｅｄ ｎａｖｉｇａｔｉｏｎ； ｃｏｎｔｒｏｌ ｓｙｓｔｅｍ； ｆｕｚｚｙ ｃｏｎｔｒｏｌ； ｌａｓｅｒ ｒａｄａｒ； Ｋａｌｍａｎ ｆｉｌｔｅ⁃ ｒｉｎｇ 收稿日期：２０１２⁃０３⁃１５． 网络出版日期：２０１３⁃０３⁃１０． 通信作者：李大伟． Ｅ⁃ｍａｉｌ： ｍｙｚｏｎｅ２００９＿ｖｉｐ＠ ｓｉｎａ．ｃｏｍ． 目前常见的机器人导航系统主要包括黑白线循 迹导航、磁导航、ＧＰＳ ／ ＤＲ 组合导航、视觉图像导航 及激光雷达导航等．不同的导航系统各自有其优缺 点，黑白线导航原理简单易行、稳定可靠但易受雨雪 天气影响；磁导航成本相对较高且易受强电磁环境 干扰；ＧＰＳ ／ ＤＲ 组合导航［１］ 定位精度高、灵活方便
第3期 李大伟，等：一种基于卡尔曼滤波与模糊算法的变电站机器人组合导航及控制系统设计 ·227· 但成本非常高：视觉图像导航基于图像识别与神经 需要指出的是，由于机器人本身存在机械误差 网络技术，但其原理复杂，工业实现难度较大 以及里程计带来的测量误差，DR系统导航精度随 本文涉及的导航系统采用DR/LMS组合导航 时间具有发散性，定位精度有限，故必须辅助其他导 技术，DR导航采用常用的惯性导航方法，原理简单. 航子系统加以修正： 针对LMS激光导航系统的研究，文献[2]从最优路 1.2基于高精度激光雷达LMS的自主导航系统 径角度，给出了一种全局最优路径算法：文献[3]基 本系统采用了SICK公司生产的LMS激光雷达 于激光雷达进行环境建模、避障检测，提出了一种时 全向测距传感器，该测距系统采用双脉冲快速测距 变势场导航算法，但只定性而未定量描述了机器人 技术，以50Hz的扫描频率实现360°、20m范围内障 在视觉场内的坐标和位姿分布).文献[5]基于激 碍物的快速扫描，满足高精度(mm级)测距需求.测 光系统，提出了一种三边与三角测量法，虽然原理简 距系统与上位机Labview软件系统通过以太网接口 单、定位精度高，但仅适用于理想的实验室环境.文 实现相互通信，数据传输及处理速度能够满足实际 献[6]同样在实验室环境下给出了一种机器人速度 工业需求。 跟踪算法，但未能定量地描述位姿坐标问题。 1.2.1全局路径已知条件下的机器人直行段导航 综合目前导航系统的优缺点，本文提出的一种 定位 基于卡尔曼滤波[]的DR/LMS组合导航系统设计 直行段导航定位系统原理如图2所示，其定位 方法，是一种基于多传感器的定位精度优化估计方 与导航计算公式分别见式(2)和(3)，式中各变量含 法，通过数据融合、滤波处理，实现了定位坐标定量 义及说明可以参见文献[11] 化管理，具备导航精度高的优点.特别地，本系统采 用的基于多级模糊算法[81的控制结构实现了机器 head 人运动系统的多级化闭环控制，优化机器人的行为 Left Right 轨迹的同时，增强了其对复杂工业现场环境的适应 (x1Y、 1△a 能力. 测量 1机器人导航系统原理及应用 (x,y)△ robot 本导航系统采用DR/LMS组合式设计，各传感 器相互融合，通过卡尔曼最优估计原理进行算法设 图2直行段导航定位原理 计，提高了导航的精确性与稳定性。 Fig.2 The principle diagram of navigation and positio- 1.1DR惯性导航系统 ning when going straight 图1是DR导航定位系统航位推算的原理图， 已知起始点A的定位坐标(x,y)、导航测量角 (x-x1)2+(y-y1)2=, 及光电位移传感器测量里程△S,可以推算出下一 (2) 时刻在B点的坐标估计值(x+1,y+1).式(1)给出了 (x-x)2+(y-为2)2= 机器人在X、Y轴方向的行驶位移，即 d2=√+ri-2rr2cos△a2; |xk+i=xg+△S4sin0, (1) +d品- yk+1=ye+△S4cos0. cos B= 2r2dn k1=(x1-x2)/(y1-y2): C(x,y） k2=0: (3) B(x,y） [T/2,y1=y2, Y= k1-k2 arctan ,y1≠y2; （xy) 1+kk2 X △am=T-a; 图1DR系统导航定位原理 △a,=T-B-Y; Fig.1 The principle of DR navigation and positioning sys- tem 0g=△am-△a
但成本非常高；视觉图像导航基于图像识别与神经 网络技术，但其原理复杂，工业实现难度较大． 本文涉及的导航系统采用 ＤＲ／ ＬＭＳ 组合导航 技术，ＤＲ 导航采用常用的惯性导航方法，原理简单． 针对 ＬＭＳ 激光导航系统的研究，文献［２］从最优路 径角度，给出了一种全局最优路径算法；文献［３］基 于激光雷达进行环境建模、避障检测，提出了一种时 变势场导航算法，但只定性而未定量描述了机器人 在视觉场内的坐标和位姿分布［４］ ．文献［５］ 基于激 光系统，提出了一种三边与三角测量法，虽然原理简 单、定位精度高，但仅适用于理想的实验室环境．文 献［６］同样在实验室环境下给出了一种机器人速度 跟踪算法，但未能定量地描述位姿坐标问题． 综合目前导航系统的优缺点，本文提出的一种 基于卡尔曼滤波［７］ 的 ＤＲ／ ＬＭＳ 组合导航系统设计 方法，是一种基于多传感器的定位精度优化估计方 法，通过数据融合、滤波处理，实现了定位坐标定量 化管理，具备导航精度高的优点．特别地，本系统采 用的基于多级模糊算法［８⁃１０］的控制结构实现了机器 人运动系统的多级化闭环控制，优化机器人的行为 轨迹的同时，增强了其对复杂工业现场环境的适应 能力． １ 机器人导航系统原理及应用 本导航系统采用 ＤＲ／ ＬＭＳ 组合式设计，各传感 器相互融合，通过卡尔曼最优估计原理进行算法设 计，提高了导航的精确性与稳定性． １．１ ＤＲ 惯性导航系统 图 １ 是 ＤＲ 导航定位系统航位推算的原理图， 已知起始点 Ａ 的定位坐标（ ｘｋ，ｙｋ ）、导航测量角 θｋ 及光电位移传感器测量里程 ΔＳｋ，可以推算出下一 时刻在 Ｂ 点的坐标估计值（ｘｋ＋１ ，ｙｋ＋１ ）．式（１）给出了 机器人在 Ｘ、Ｙ 轴方向的行驶位移，即 ｘｋ＋１ ＝ ｘｋ ＋ ΔＳｋ ｓｉｎ θｋ， ｙｋ＋１ ＝ ｙｋ ＋ ΔＳｋ ｃｏｓ θｋ ． { （１） 图 １ ＤＲ 系统导航定位原理 Ｆｉｇ．１ Ｔｈｅ ｐｒｉｎｃｉｐｌｅ ｏｆ ＤＲ ｎａｖｉｇａｔｉｏｎ ａｎｄ ｐｏｓｉｔｉｏｎｉｎｇ ｓｙｓ⁃ ｔｅｍ 需要指出的是，由于机器人本身存在机械误差 以及里程计带来的测量误差，ＤＲ 系统导航精度随 时间具有发散性，定位精度有限，故必须辅助其他导 航子系统加以修正． １．２ 基于高精度激光雷达 ＬＭＳ 的自主导航系统 本系统采用了 ＳＩＣＫ 公司生产的 ＬＭＳ 激光雷达 全向测距传感器，该测距系统采用双脉冲快速测距 技术，以５０ Ｈｚ的扫描频率实现 ３６０°、２０ ｍ范围内障 碍物的快速扫描，满足高精度（ｍｍ 级）测距需求．测 距系统与上位机 Ｌａｂｖｉｅｗ 软件系统通过以太网接口 实现相互通信，数据传输及处理速度能够满足实际 工业需求． １．２．１ 全局路径已知条件下的机器人直行段导航 定位 直行段导航定位系统原理如图 ２ 所示，其定位 与导航计算公式分别见式（２）和（３），式中各变量含 义及说明可以参见文献［１１］． 图 ２ 直行段导航定位原理 Ｆｉｇ．２ Ｔｈｅ ｐｒｉｎｃｉｐｌｅ ｄｉａｇｒａｍ ｏｆ ｎａｖｉｇａｔｉｏｎ ａｎｄ ｐｏｓｉｔｉｏ⁃ ｎｉｎｇ ｗｈｅｎ ｇｏｉｎｇ ｓｔｒａｉｇｈｔ （ｘ － ｘ１ ） ２ ＋ （ｙ － ｙ１ ） ２ ＝ ｒ ２ １ ， （ｘ － ｘ２ ） ２ ＋ （ｙ － ｙ２ ） ２ ＝ ｒ ２ ２ ． { （２） ｄ ～ １２ ＝ ｒ ２ １ ＋ ｒ ２ ２ － ２ｒ１ ｒ２ ｃｏｓ Δα１２ ； ｃｏｓ β ＝ ｒ ２ ２ ＋ ｄ ～ ２ １２ － ｒ ２ １ ２ｒ２ ｄ ～ １２ ； ｋ１ ＝ （ｘ１ － ｘ２ ） ／ （ｙ１ － ｙ２ ）； ｋ２ ＝ ０； γ ＝ π／ ２，ｙ１ ＝ ｙ２ ， ａｒｃｔａｎ ｋ１ － ｋ２ １ ＋ ｋ１ ｋ２ ，ｙ１ ≠ ｙ２ ； ì î í ï ï ïï Δαｍ ＝ π － α； Δαｔ ＝ π － β － γ； θｋ ＝ Δαｍ － Δαｔ ． ì î í ï ï ï ï ï ï ï ï ï ï ï ï ï ï ï ï ï ï （３） 第 ３ 期 李大伟，等：一种基于卡尔曼滤波与模糊算法的变电站机器人组合导航及控制系统设计 ·２２７·
·228· 智能系统学报 第8卷 1.2.2全局路径已知条件下的机器人转弯段导航 理坐标分别为(x1,y1)和(x2,y2),1、2分别表示机 定位 器人距离对应参考对象的距离值，由此可以方便地 机器人定位坐标的求解同式(2).与直行段不同 解算出机器人当前的定位坐标(x,y).再根据定位坐 的是，转弯段预设行走路径不再是直线方向，而是围 标(x,y)及转弯条件下计算得到的圆心坐标(xo, 绕某一圆心的圆弧运动.如图3所示，机器人以0， yo,)可以计算出机器人距离圆心O1的距离值R 为圆心，半径为R(理论值)做圆周运动，其中圆心 及机器人朝向与半径方向的夹角值A.将这2个计算 0,的地理坐标(xo,yo,)与半径R的大小已事先 量分别与转弯段的理想半径值R和理想夹角值 由系统给定.假设由式(2)求得机器人的定位坐标为 (90°)作定量比较，得到机器人自主转弯的2个控 (x,y),则机器人自主导航的控制量△R与△8的求 制量：半径偏移量△R与航角偏移量△0，并将这2 解方程如式(4)： 个控制量作为模糊控制器的2个控制输入参数，实 R=√x-o,)2+(y-yo,)尸， 现控制策略下的机器人导航定位闭环反馈调节. △R=Rnea-Ral, 本定位子系统的主要误差来源有：1)实测道路 两侧参考目标设备地理坐标带来的绝对误差：2)激 6=（T-w1)+arccos[(R2+r斤- 光雷达实测目标距离值时多点滤波等效带来的绝对 (x-xn,)2+(y1-yo,)2)/(2×R×r1)], 误差.鉴于上述两子系统的优缺点，为了提高系统定 △0=m/2-8. 位精度、保证系统运行的可靠性，引入基于卡尔曼滤 (4) 波的组合导航技术则显得尤为必要 式中：R表示机器人定位点与当前圆心O,的测量 L.3基于卡尔曼滤波技术的DR/LMS组合导航系 距离，π为机器人前进方向上激光雷达扫描点的对 统设计及应用 应角度值，w,为激光雷达在转弯内侧参考标记1 针对DR惯性导航子系统与LMS高精度激光 (x1,y:)处的扫描角度值，0表示机器人前进方向与 雷达视觉导航子系统各自的优缺点，为了进一步提 半径方向的夹角值，π/2则表示理想圆心运动条件 高系统的导航定位精度，改善系统的稳定性与可靠 下，机器人前进方向即切线方向与半径方向的垂直 性，可以以随机线性离散卡尔曼滤波为理论基础，将 夹角. DR与LMS两导航子系统优势互补，以增强系统的 (x2y) 鲁棒性与抗干扰性。 ◆6 4① ◇2 基于卡尔曼滤波模型，对DR/LMIS组合导航系 1rs 统的X、Y定位坐标进行卡尔曼滤波，可以得到如下 3④ Robot 方程 ◇5 (x.v) R 1)状态方程： O(x。y。) ①A +AStk-isin 01+I(-)W (1) y:=y1+ASL-icos -1+I(-W-Dy ⊙ 2)观测方程： 2,=g+V: 图3转弯段导航定位原理 Fig.3 The principle diagram of navigation and positio- (n =ye+Var ning when turning control 式中：xy为机器人在X轴、Y轴方向的状态位置 图3中序列1~6分别表示机器人视觉区域内 坐标，0-1为状态控制量（航向计算角），△S.-1为各 的参考对象，并且各个参考对象的地理坐标已知，机 采样阶段的转移控制系数，Wx、W-,为DR推 器人可以根据激光雷达，通过数据采样、过滤及滤波 算过程噪声序列，「k-)、Tk-w为DR推算过程 算法识别当前环境下的参考距离值，并建立参考对 噪声输入系数，之，之，为激光测距计算得到的机器人 象与参考距离值之间的一一映射关系，准确计算出 定位点的x、y坐标值，V、V为LMS测量系统等效 机器人当前定位坐标及航向角偏差.与直行段不同 噪声序列 的是，转弯段可以看作是机器人绕某一固定圆心点 系统定位坐标初始状态x。y。由系统给定，滤 的圆周运动，而不同的转弯段可以更新参考圆心坐 波误差初始值P。已知，系统过程噪声（估计噪声） 标来实现自主变轨运动.图3中参考对象1、2的地 方差Q.=E(E.W)和系统观测噪声(LMS测量噪
１．２．２ 全局路径已知条件下的机器人转弯段导航 定位 机器人定位坐标的求解同式（２）．与直行段不同 的是，转弯段预设行走路径不再是直线方向，而是围 绕某一圆心的圆弧运动．如图 ３ 所示，机器人以 Ｏ１ 为圆心，半径为 Ｒｍｅａ（理论值）做圆周运动，其中圆心 Ｏ１ 的地理坐标（ ｘＯ１ ，ｙＯ１ ）与半径 Ｒｍｅａ的大小已事先 由系统给定．假设由式（２）求得机器人的定位坐标为 （ｘ，ｙ），则机器人自主导航的控制量 ΔＲ 与 Δθ 的求 解方程如式（４）： Ｒｍｅａ ＝ （ｘ － ｘＯ１ ） ２ ＋ （ｙ － ｙＯ１ ） ２ ， ΔＲ ＝ Ｒｍｅａ － Ｒｃａｌ， θ ＝ （π － ω１ ） ＋ ａｒｃｃｏｓ［（Ｒ ２ ｍｅａ ＋ ｒ ２ １ － （（ｘ１ － ｘＯ１ ） ２ ＋ （ｙ１ － ｙＯ１ ） ２ ）） ／ （２ × Ｒｃａｌ × ｒ１ ）］， Δθ ＝ π／ ２ － θ． ì î í ï ï ï ï ï ï ï ï （４） 式中：Ｒｃａｌ表示机器人定位点与当前圆心 Ｏ１ 的测量 距离，π 为机器人前进方向上激光雷达扫描点的对 应角度值，ω１为激光雷达在转弯内侧参考标记 １ （ｘ１ ，ｙ１ ）处的扫描角度值，θ 表示机器人前进方向与 半径方向的夹角值，π／ ２ 则表示理想圆心运动条件 下，机器人前进方向即切线方向与半径方向的垂直 夹角． 图 ３ 转弯段导航定位原理 Ｆｉｇ．３ Ｔｈｅ ｐｒｉｎｃｉｐｌｅ ｄｉａｇｒａｍ ｏｆ ｎａｖｉｇａｔｉｏｎ ａｎｄ ｐｏｓｉｔｉｏ⁃ ｎｉｎｇ ｗｈｅｎ ｔｕｒｎｉｎｇ ｃｏｎｔｒｏｌ 图 ３ 中序列 １ ～ ６ 分别表示机器人视觉区域内 的参考对象，并且各个参考对象的地理坐标已知，机 器人可以根据激光雷达，通过数据采样、过滤及滤波 算法识别当前环境下的参考距离值，并建立参考对 象与参考距离值之间的一一映射关系，准确计算出 机器人当前定位坐标及航向角偏差．与直行段不同 的是，转弯段可以看作是机器人绕某一固定圆心点 的圆周运动，而不同的转弯段可以更新参考圆心坐 标来实现自主变轨运动．图 ３ 中参考对象 １、２ 的地 理坐标分别为（ｘ１ ，ｙ１ ）和（ ｘ２ ，ｙ２ ），ｒ１ 、ｒ２ 分别表示机 器人距离对应参考对象的距离值，由此可以方便地 解算出机器人当前的定位坐标（ｘ，ｙ）．再根据定位坐 标（ｘ，ｙ） 及转弯条件下计算得到的圆心坐标（ ｘＯ１ ， ｙＯ１ ）可以计算出机器人距离圆心 Ｏ１ 的距离值 Ｒｍｅａ 及机器人朝向与半径方向的夹角值 θ．将这 ２ 个计算 量分别与转弯段的理想半径值 Ｒ 和理想夹角值 （９０°）作定量比较，得到机器人自主转弯的 ２ 个控 制量：半径偏移量 ΔＲ 与航角偏移量 Δθ，并将这 ２ 个控制量作为模糊控制器的 ２ 个控制输入参数，实 现控制策略下的机器人导航定位闭环反馈调节． 本定位子系统的主要误差来源有：１）实测道路 两侧参考目标设备地理坐标带来的绝对误差；２）激 光雷达实测目标距离值时多点滤波等效带来的绝对 误差．鉴于上述两子系统的优缺点，为了提高系统定 位精度、保证系统运行的可靠性，引入基于卡尔曼滤 波的组合导航技术则显得尤为必要． １．３ 基于卡尔曼滤波技术的 ＤＲ／ ＬＭＳ 组合导航系 统设计及应用 针对 ＤＲ 惯性导航子系统与 ＬＭＳ 高精度激光 雷达视觉导航子系统各自的优缺点，为了进一步提 高系统的导航定位精度，改善系统的稳定性与可靠 性，可以以随机线性离散卡尔曼滤波为理论基础，将 ＤＲ 与 ＬＭＳ 两导航子系统优势互补，以增强系统的 鲁棒性与抗干扰性． 基于卡尔曼滤波模型，对 ＤＲ／ ＬＭＳ 组合导航系 统的 Ｘ、Ｙ 定位坐标进行卡尔曼滤波，可以得到如下 方程． １）状态方程： ｘｋ ＝ ｘｋ－１ ＋ ΔＳｋ，ｋ－１ ｓｉｎ θｋ－１ ＋ Γ（ｋ，ｋ－１）ｘＷ（ｋ－１）ｘ， ｙｋ ＝ ｙｋ－１ ＋ ΔＳｋ，ｋ－１ ｃｏｓ θｋ－１ ＋ Γ（ｋ，ｋ－１）ｙＷ（ｋ－１）ｙ ． { ２）观测方程： ｚｘ ｋ ＝ ｘｋ ＋ Ｖｋｘ， ｚｙ ｋ ＝ ｙｋ ＋ Ｖｋｙ ． { 式中：ｘｋ、ｙｋ 为机器人在 Ｘ 轴、Ｙ 轴方向的状态位置 坐标，θｋ－１为状态控制量（航向计算角），ΔＳｋ，ｋ－１为各 采样阶段的转移控制系数，Ｗ（ｋ－１）ｘ、Ｗ（ｋ－１）ｙ为 ＤＲ 推 算过程噪声序列，Γ（ｋ，ｋ－１） ｘ、Γ（ｋ，ｋ－１）ｙ为 ＤＲ 推算过程 噪声输入系数，ｚｘ ｋ 、ｚｙ ｋ 为激光测距计算得到的机器人 定位点的 ｘ、ｙ 坐标值，Ｖｋｘ、Ｖｋｙ为 ＬＭＳ 测量系统等效 噪声序列． 系统定位坐标初始状态 ｘ０ 、ｙ０ 由系统给定，滤 波误差初始值 Ｐ０ 已知，系统过程噪声（估计噪声） 方差Ｑｋ ＝ Ｅ（ＥｋＷ Ｔ ｋ ） 和系统观测噪声（ ＬＭＳ 测量噪 ·２２８· 智 能 系 统 学 报 第 ８ 卷
第3期 李大伟，等：一种基于卡尔曼滤波与模糊算法的变电站机器人组合导航及控制系统设计 ·229· 声)方差R=E(V)已确定，则得到如下滤波求解 而对于机器人自主行走控制器的另外一个控制 过程. 量一导航角，这里不对其作卡尔曼滤波处理，因为 1)状态一步预测： 其算法的优越性使得导航角的计算精度不受测距仪 ]- sin 0- 多点扫描带来的误差影响，导航精度高，仅需加以常 AS-1 用的限幅滤波、均值滤波等算法即可满足控制系统 2)状态估计： 的要求 1「(1-K. 2 基于模糊算法的机器人行走控制系 G. 0 统设计及应用 3)滤波增益： 通过提取上述机器人导航控制参数，将其作为 P(k,k-10 「K P(A-1)s +Ru 输入量，通过模糊算法设计，实现机器人的行走运动 控制，该算法极大地增强了系统对复杂工业现场环 P(k.k-1)3 境的适应能力. P(k.k-1)+Riy- 图4给出了机器人运动系统的模糊控制流程 4)步预测误差： 图新方法反映了机器人以导航反馈量，即距离偏移 P(k.t-1)x P(-D T-DQ- LP- , 量与航角偏移量为控制器输入参数，通过模糊推理、 -- 解模糊集，输出电机转速控制量，并将输出响应后的 5)滤波估计误差： 下一次导航定位数据反馈到原系统，通过给定值与 0 反馈值的比较，实现了多级模糊控制策略下的快速 响应特性与闭环控制， [P(6,k-1 「KRa P (k.k-1)y 数据库 规则库 给定值 距离偏差 左轮转速 模糊推理 右轮转速 化 糊 角度偏差 转向角度 机器人定位坐标以及朝向角 图4机器人模糊控制流程 Fig.4 The flow chart of robot fuzzy control 控制器为两输入三输出的模糊控制器，将真实 位u1和2的论域为[0,4]，划分为5个等级；机器 的确定量输入转换为一个模糊矢量.控制器实时接 人转向角度u3的论域为[220,500]，划分为9个等 收机器人的定位坐标和航向角，并将其与给定的目 级.以图5和图6为例，分别给出了基于Labview平 标值比较，计算当前距离偏差量和航角偏差量.2个 台的s和u3的隶属度函数图形. 偏差量作为控制器输入先进行模糊化处理，得到输 1.0m -NBI 入的模糊矢量后，根据预先设定的数据库和规则库 NBMI 0.8 NSI 进行模糊推理，推理后得到的结果为模糊矢量，进行 世0.6 NSMI ZI 解模糊后作为控制量输出，输出量分别为左电机转 0.4 PSMI 速、右电机转速以及转向电机调节角度.2个输入 0.2 PSI PBMI 中，机器人中心与道路中心线的距离偏差s的论域 -1.0-0.6 -0.20.20.6 1.0 为[-1.5,1.5]，划分为9个等级：机器人朝向与道路 方向的角度偏差△p的论域为[-15,15]，划分为5 图5距离偏移量s的隶属度函数 个等级.3个输出中，机器人左右2个电机的速度档 Fig.5 Membership functions of distance offset s
声）方差Ｒｋ ＝Ｅ（ＶｋＶ Ｔ ｋ ）已确定，则得到如下滤波求解 过程．． １）状态一步预测： ｘ ＾ ｋ，ｋ－１ ｙ ＾ ｋ，ｋ－１ é ë ê ê ù û ú ú ＝ ｘ ＾ ｋ－１ ｙ ＾ ｋ－１ é ë ê ê ù û ú ú ＋ ΔＳｋ，ｋ－１ ｓｉｎ θｋ－１ ｃｏｓ θｋ－１ é ë ê ê ù û ú ú ． ２）状态估计： ｘ ＾ ｋ ｙ ＾ ｋ é ë ê ê ù û ú ú ＝ （１ － Ｋｋｘ） ０ ０ （１ － Ｋｋｙ） é ë ê ê ù û ú ú ｘ ＾ ｋ，ｋ－１ ｙ ＾ ｋ，ｋ－１ é ë ê ê ù û ú ú ＋ Ｋｋｘ ｚｘ ｋ Ｋｋｙ ｚｙ ｋ é ë ê ê ù û ú ú ． ３）滤波增益： Ｋｋｘ Ｋｋｙ é ë ê ê ù û ú ú ＝ Ｐ（ｋ，ｋ－１）ｘ Ｐ（ｋ，ｋ－１）ｘ ＋ Ｒｋｘ Ｐ（ｋ，ｋ－１）ｙ Ｐ（ｋ，ｋ－１）ｙ ＋ Ｒｋｙ é ë ê ê ê ê ê ù û ú ú ú ú ú ． ４）步预测误差： Ｐ（ｋ，ｋ－１）ｘ Ｐ（ｋ，ｋ－１）ｙ é ë ê ê ù û ú ú ＝ Ｐ（ｋ－１）ｘ Ｐ（ｋ－１）ｙ é ë ê ê ù û ú ú ＋ Γ ２ （ｋ，ｋ－１）ｘＱ（ｋ－１）ｘ Γ ２ （ｋ，ｋ－１）ｙＱ（ｋ－１）ｙ é ë ê ê ù û ú ú ． ５）滤波估计误差： Ｐｋｘ Ｐｋｙ é ë ê ê ù û ú ú ＝ （１ － Ｋｋｘ） ２ ０ ０ （１ － Ｋｋｙ） ２ é ë ê ê ù û ú ú · Ｐ（ｋ，ｋ－１）ｘ Ｐ（ｋ，ｋ－１）ｙ é ë ê ê ù û ú ú ＋ Ｋ ２ ｋｘＲｋｘ Ｋ ２ ｋｙＲｋｙ é ë ê ê ù û ú ú ． 而对于机器人自主行走控制器的另外一个控制 量———导航角，这里不对其作卡尔曼滤波处理，因为 其算法的优越性使得导航角的计算精度不受测距仪 多点扫描带来的误差影响，导航精度高，仅需加以常 用的限幅滤波、均值滤波等算法即可满足控制系统 的要求． ２ 基于模糊算法的机器人行走控制系 统设计及应用 通过提取上述机器人导航控制参数，将其作为 输入量，通过模糊算法设计，实现机器人的行走运动 控制，该算法极大地增强了系统对复杂工业现场环 境的适应能力． 图 ４ 给出了机器人运动系统的模糊控制流程 图．新方法反映了机器人以导航反馈量，即距离偏移 量与航角偏移量为控制器输入参数，通过模糊推理、 解模糊集，输出电机转速控制量，并将输出响应后的 下一次导航定位数据反馈到原系统，通过给定值与 反馈值的比较，实现了多级模糊控制策略下的快速 响应特性与闭环控制． 图 ４ 机器人模糊控制流程 Ｆｉｇ．４ Ｔｈｅ ｆｌｏｗ ｃｈａｒｔ ｏｆ ｒｏｂｏｔ ｆｕｚｚｙ ｃｏｎｔｒｏｌ 控制器为两输入三输出的模糊控制器，将真实 的确定量输入转换为一个模糊矢量．控制器实时接 收机器人的定位坐标和航向角，并将其与给定的目 标值比较，计算当前距离偏差量和航角偏差量．２ 个 偏差量作为控制器输入先进行模糊化处理，得到输 入的模糊矢量后，根据预先设定的数据库和规则库 进行模糊推理，推理后得到的结果为模糊矢量，进行 解模糊后作为控制量输出，输出量分别为左电机转 速、右电机转速以及转向电机调节角度． ２ 个输入 中，机器人中心与道路中心线的距离偏差 ｓ 的论域 为［－１．５，１．５］，划分为 ９ 个等级；机器人朝向与道路 方向的角度偏差 Δφ 的论域为［ －１５，１５］，划分为 ５ 个等级．３ 个输出中，机器人左右 ２ 个电机的速度档 位 ｕ１ 和 ｕ２ 的论域为［０，４］，划分为 ５ 个等级；机器 人转向角度 ｕ３ 的论域为［２２０，５００］，划分为 ９ 个等 级．以图 ５ 和图 ６ 为例，分别给出了基于 Ｌａｂｖｉｅｗ 平 台的 ｓ 和 ｕ３ 的隶属度函数图形． 图 ５ 距离偏移量 ｓ 的隶属度函数 Ｆｉｇ．５ Ｍｅｍｂｅｒｓｈｉｐ ｆｕｎｃｔｉｏｎｓ ｏｆ ｄｉｓｔａｎｃｅ ｏｆｆｓｅｔ ｓ 第 ３ 期 李大伟，等：一种基于卡尔曼滤波与模糊算法的变电站机器人组合导航及控制系统设计 ·２２９·
·230· 智能系统学报 第8卷 为了得到合理的输出控制量，要求模糊计算方 图8定量地给出了不同滤波策略下本组合导航 法能够很好地表达输出隶属度函数的计算结果采 系统X坐标定位值的精度变化曲线，给定道路中心 用加权平均法，将论域中的每个元素x:(i=1,2,…, 线的直线方程约束条件为X=3.3m.从图8中可以 n)作为待判决输出模糊集合的隶属度u:的加权系 明显地看出，与X值理想定位变化趋势相比，经过 数，求得乘积山，再计算该乘积和三x叫，对于隶属 卡尔曼滤波后的定位波形具有更高的精度与可信 度经现场试验，引入卡尔曼滤波后，系统的定位精 度和的平均值x。,将其作为求得的判决结果最后， 度更加逼近于机器人的实际行为轨迹，其定位精度 将输出量化因子乘以xo得到所需要的控制量. NB5 可以达到10cm左右，并且经过多级参数整定后的模 NBM5 糊控制系统控制策略更加科学严谨.整个系统的导 0.8 NS5 NSM5 航控制频率在5Hz左右，其响应速度完全可以满足 0.6 Z5 现场应用需求. 0.4 PSM5 PS5 4.2m PBM5 10 4.0 限幅滤波X坐标曲线 2.2 26 3.03.43.84.24.65.0 理想X坐标曲线 43 3.8 图6转向角度输出山，的隶属度函数 E3.6 Fig.6 Membership function of steering angle output u 3.4 Kalman 3.2 滤波曲线 3.0 3 变电站机器人现场导航轨迹波形分 滤波前X坐标曲线 2.80 2040 6080100120140160 析及验证 定位个数n 本节对基于卡尔曼滤波与模糊控制的DR/LMS 图8定位X坐标滤波波形抽样分析 组合导航定位系统的机器人现场自主巡检过程进行 Fig.8 Sampling analysis on filtering wave of positio- 了轨迹跟踪与数据反馈.通过对全局路径下一个周 ning X coordinate 期路径定位数据的采集及波形分析，定量地验证了 图9定量地给出了机器人行为过程中本组合导 本导航及控制系统的动态稳定性和高精度性， 航系统定位Y坐标的精度变化曲线.由于复杂工业 图7给出了500kV变电站现场路径中的一段周 环境对采样数据带来的噪声干扰，定位Y坐标在某 期性路径，图中U形段为已知巡检路段，道路两侧 些定位点会出现幅度较大的突变；而经限幅滤波、均 的方框标识为视觉域内的参考点或自由空间点，其 值滤波等常用经典滤波方法有效地消除了外界噪声 中参考点的地理坐标已知，坐标系如图7所示理想 干扰.经过卡尔曼滤波后的Y坐标变化曲线的线性 的机器人行走轨迹应该是围绕中心路径（图7中虚 度与经典滤波波形曲线类似，区别仅仅在于一定程 线)的U形运动. 度上提高了系统定位精度与可靠性.而实际导航系 ◇ 0 统的控制量主要取决于X坐标的偏移量和导航角， 为了简化控制策略及程序的复杂性、提高响应速度， 对于Y坐标采用常用经典滤波的定位算法完全可 以满足现场要求 ◇ 30 经典滤波后的Y坐标 ◇ 中 号2 ◇ ◇ 由 18 滤波前的Y坐标测量值 (其余部分与经典滤波曲线重合) 0 2040 6080100120140160 测量个数n 图7自主巡检机器人现场理想轨迹模拟 图9定位Y坐标滤波波形抽样分析 Fig.7 Field track simulation of the independent inspec- Fig.9 Sampling analysis on filtering wave of positio- tion robot ning Y coordinate
为了得到合理的输出控制量，要求模糊计算方 法能够很好地表达输出隶属度函数的计算结果．采 用加权平均法，将论域中的每个元素 ｘｉ（ ｉ ＝ １，２，…， ｎ）作为待判决输出模糊集合的隶属度 ｕｉ 的加权系 数，求得乘积 ｘｉｕｉ，再计算该乘积和∑ ｎ ｉ ＝ １ ｘｉｕｉ 对于隶属 度和的平均值 ｘ０ ，将其作为求得的判决结果．最后， 将输出量化因子乘以 ｘ０ 得到所需要的控制量． 图 ６ 转向角度输出 ｕ３ 的隶属度函数 Ｆｉｇ．６ Ｍｅｍｂｅｒｓｈｉｐ ｆｕｎｃｔｉｏｎ ｏｆ ｓｔｅｅｒｉｎｇ ａｎｇｌｅ ｏｕｔｐｕｔ ｕ３ ３ 变电站机器人现场导航轨迹波形分 析及验证 本节对基于卡尔曼滤波与模糊控制的 ＤＲ／ ＬＭＳ 组合导航定位系统的机器人现场自主巡检过程进行 了轨迹跟踪与数据反馈．通过对全局路径下一个周 期路径定位数据的采集及波形分析，定量地验证了 本导航及控制系统的动态稳定性和高精度性． 图 ７ 给出了５００ ｋＶ变电站现场路径中的一段周 期性路径，图中 Ｕ 形段为已知巡检路段，道路两侧 的方框标识为视觉域内的参考点或自由空间点，其 中参考点的地理坐标已知，坐标系如图 ７ 所示．理想 的机器人行走轨迹应该是围绕中心路径（图 ７ 中虚 线）的 Ｕ 形运动． 图 ７ 自主巡检机器人现场理想轨迹模拟 Ｆｉｇ．７ Ｆｉｅｌｄ ｔｒａｃｋ ｓｉｍｕｌａｔｉｏｎ ｏｆ ｔｈｅ ｉｎｄｅｐｅｎｄｅｎｔ ｉｎｓｐｅｃ⁃ ｔｉｏｎ ｒｏｂｏｔ 图 ８ 定量地给出了不同滤波策略下本组合导航 系统 Ｘ 坐标定位值的精度变化曲线，给定道路中心 线的直线方程约束条件为 Ｘ ＝ ３．３ ｍ．从图 ８ 中可以 明显地看出，与 Ｘ 值理想定位变化趋势相比，经过 卡尔曼滤波后的定位波形具有更高的精度与可信 度．经现场试验，引入卡尔曼滤波后，系统的定位精 度更加逼近于机器人的实际行为轨迹，其定位精度 可以达到１０ ｃｍ左右，并且经过多级参数整定后的模 糊控制系统控制策略更加科学严谨．整个系统的导 航控制频率在５ Ｈｚ左右，其响应速度完全可以满足 现场应用需求． 图 ８ 定位 Ｘ 坐标滤波波形抽样分析 Ｆｉｇ．８ Ｓａｍｐｌｉｎｇ ａｎａｌｙｓｉｓ ｏｎ ｆｉｌｔｅｒｉｎｇ ｗａｖｅ ｏｆ ｐｏｓｉｔｉｏ⁃ ｎｉｎｇ Ｘ ｃｏｏｒｄｉｎａｔｅ 图 ９ 定量地给出了机器人行为过程中本组合导 航系统定位 Ｙ 坐标的精度变化曲线．由于复杂工业 环境对采样数据带来的噪声干扰，定位 Ｙ 坐标在某 些定位点会出现幅度较大的突变；而经限幅滤波、均 值滤波等常用经典滤波方法有效地消除了外界噪声 干扰．经过卡尔曼滤波后的 Ｙ 坐标变化曲线的线性 度与经典滤波波形曲线类似，区别仅仅在于一定程 度上提高了系统定位精度与可靠性．而实际导航系 统的控制量主要取决于 Ｘ 坐标的偏移量和导航角， 为了简化控制策略及程序的复杂性、提高响应速度， 对于 Ｙ 坐标采用常用经典滤波的定位算法完全可 以满足现场要求． 图 ９ 定位 Ｙ 坐标滤波波形抽样分析 Ｆｉｇ．９ Ｓａｍｐｌｉｎｇ ａｎａｌｙｓｉｓ ｏｎ ｆｉｌｔｅｒｉｎｇ ｗａｖｅ ｏｆ ｐｏｓｉｔｉｏ⁃ ｎｉｎｇ Ｙ ｃｏｏｒｄｉｎａｔｅ ·２３０· 智 能 系 统 学 报 第 ８ 卷
第3期 李大伟，等：一种基于卡尔曼滤波与模糊算法的变电站机器人组合导航及控制系统设计 ·231- 图l0基于上位机控制系统Labview平台，定 航向角偏差曲线转弯段 航向角偏进线 量、直观地给出了一个周期路径条件下，机器人直行 30 段与转弯段的现场定位波形及导航轨迹（图中断点 20 处是由于采集现场图像信息，未进行反馈数据的采 集).对比中心线轨迹约束条件，即右侧直行段X= 3.4m、左侧直行段X=-27m、转弯段转弯半径R= 10 19m以及图7给出的现场模拟轨迹，可以看出，经 20 卡尔曼滤波后的导航系统整体定位精度可以达到亚 米级，且机器人导航轨迹与实际行为路径匹配度较 400600 800 1023 采样点个数 高，完全可以达到自主巡检的任务要求 机人自动行走周期路线图X-Y图)曲性0 图12机器人转弯段导航偏离角在线跟踪 Fig.12 The online following of navigation angle when the robot is turning 4结束语 通过500kV超高压变电站现场试验分析，相比 其他导航系统，本系统在同样实现亚米级定位精度 的同时，很大程度上降低了经济成本，适用性强，克 服了电子罗盘等导航子系统易受变电站强电磁环境 干扰的缺点而基于模糊控制的机器人运动系统，能 够更好地适应复杂多变的工业现场环境，减少了不 同路径条件下导航误差的分散性，完全可以满足工 业应用需求因此，本系统的设计思想在实际智能变 电站建设中具有广阔的应用价值与市场前景.此外， -3025-20-15-10-50 为了进一步提高本系统的稳定性，可以考虑在此基 础上附加其他导航子系统作为反馈源，以适应更加 图10机器人完整定位数据采集及全程轨迹跟踪 复杂的工业现场条件. Fig.10 The positioning data acquisition and track fol- lowing on the whole course 参考文献： 相应地，图11和12分别定量地给出了机器人直 [1]魏鹏，张志强，张春熹，等.无人值守变电站巡检机器人导 行段、转弯段的导航角实时采样数据及变化曲线.可 航系统研究[J].自动化与仪表，2009(12)：5-8,25. 以看出经过卡尔曼滤波后的导航角，即机器人行为轨 迹与理想中心线的偏角大部分都保持在±10以内（左 WEI Peng,ZHANG Zhiqiang,ZHANG Chunxi,et al.De- sign of the navigation system for the inspecting robot in the 偏与右偏)，效果非常理想同时该动态航向角波形也 unattended operation substation[].Automation and Instru- 进一步验证了模糊控制调节策略在本系统中的高动 mentation,2009(12):5-8,25 态精度，保证了机器人有效可靠的自主行为能力 [2]孟偲，王田苗.一种移动机器人全局最优路径规划算法 航向角偏差曲线直行食 苏向角偏差曲线。 [J].机器人，2008,27(3)：217-222. 30 MENG Si,WANG Tianmiao.A global optimal path planning 20 algorithm for mobile robot[J].Robot,2008,27(3):217- 10 222. [3]杨明，王宏，何克忠，等基于激光雷达的移动机器人环境 -10 建模与避障[J].清华大学学报：自然科学版，2000,40 -20 30. (7):112-116. 200 400600 800 102 YANG Ming,WANG Hong,HE Kezhong,et al.The mo- 买柱点个数 bile robot environment modeling and obstacle avoidance 图11机器人直行段导航偏离角在线跟踪 based on laser radar[J].Tsinghua Journal of Science and Fig.11 The online following of navigation angle when Technology:Science and Technology,2000,40(7):112- the robot is going straight 116
图 １０ 基于上位机控制系统 Ｌａｂｖｉｅｗ 平台，定 量、直观地给出了一个周期路径条件下，机器人直行 段与转弯段的现场定位波形及导航轨迹（图中断点 处是由于采集现场图像信息，未进行反馈数据的采 集）．对比中心线轨迹约束条件，即右侧直行段 Ｘ ＝ ３．４ ｍ、左侧直行段 Ｘ ＝ －２７ ｍ、转弯段转弯半径Ｒ ＝ １９ ｍ以及图 ７ 给出的现场模拟轨迹，可以看出，经 卡尔曼滤波后的导航系统整体定位精度可以达到亚 米级，且机器人导航轨迹与实际行为路径匹配度较 高，完全可以达到自主巡检的任务要求． 图 １０ 机器人完整定位数据采集及全程轨迹跟踪 Ｆｉｇ．１０ Ｔｈｅ ｐｏｓｉｔｉｏｎｉｎｇ ｄａｔａ ａｃｑｕｉｓｉｔｉｏｎ ａｎｄ ｔｒａｃｋ ｆｏｌ⁃ ｌｏｗｉｎｇ ｏｎ ｔｈｅ ｗｈｏｌｅ ｃｏｕｒｓｅ 相应地，图 １１ 和 １２ 分别定量地给出了机器人直 行段、转弯段的导航角实时采样数据及变化曲线．可 以看出经过卡尔曼滤波后的导航角，即机器人行为轨 迹与理想中心线的偏角大部分都保持在±１０°以内（左 偏与右偏），效果非常理想．同时该动态航向角波形也 进一步验证了模糊控制调节策略在本系统中的高动 态精度，保证了机器人有效可靠的自主行为能力． 图 １１ 机器人直行段导航偏离角在线跟踪 Ｆｉｇ．１１ Ｔｈｅ ｏｎｌｉｎｅ ｆｏｌｌｏｗｉｎｇ ｏｆ ｎａｖｉｇａｔｉｏｎ ａｎｇｌｅ ｗｈｅｎ ｔｈｅ ｒｏｂｏｔ ｉｓ ｇｏｉｎｇ ｓｔｒａｉｇｈｔ 图 １２ 机器人转弯段导航偏离角在线跟踪 Ｆｉｇ．１２ Ｔｈｅ ｏｎｌｉｎｅ ｆｏｌｌｏｗｉｎｇ ｏｆ ｎａｖｉｇａｔｉｏｎ ａｎｇｌｅ ｗｈｅｎ ｔｈｅ ｒｏｂｏｔ ｉｓ ｔｕｒｎｉｎｇ ４ 结束语 通过５００ ｋＶ超高压变电站现场试验分析，相比 其他导航系统，本系统在同样实现亚米级定位精度 的同时，很大程度上降低了经济成本，适用性强，克 服了电子罗盘等导航子系统易受变电站强电磁环境 干扰的缺点．而基于模糊控制的机器人运动系统，能 够更好地适应复杂多变的工业现场环境，减少了不 同路径条件下导航误差的分散性，完全可以满足工 业应用需求．因此，本系统的设计思想在实际智能变 电站建设中具有广阔的应用价值与市场前景．此外， 为了进一步提高本系统的稳定性，可以考虑在此基 础上附加其他导航子系统作为反馈源，以适应更加 复杂的工业现场条件． 参考文献： ［１］魏鹏，张志强，张春熹，等．无人值守变电站巡检机器人导 航系统研究［Ｊ］．自动化与仪表， ２００９（１２）： ５⁃８， ２５． ＷＥＩ Ｐｅｎｇ， ＺＨＡＮＧ Ｚｈｉｑｉａｎｇ， ＺＨＡＮＧ Ｃｈｕｎｘｉ， ｅｔ ａｌ． Ｄｅ⁃ ｓｉｇｎ ｏｆ ｔｈｅ ｎａｖｉｇａｔｉｏｎ ｓｙｓｔｅｍ ｆｏｒ ｔｈｅ ｉｎｓｐｅｃｔｉｎｇ ｒｏｂｏｔ ｉｎ ｔｈｅ ｕｎａｔｔｅｎｄｅｄ ｏｐｅｒａｔｉｏｎ ｓｕｂｓｔａｔｉｏｎ［Ｊ］． Ａｕｔｏｍａｔｉｏｎ ａｎｄ Ｉｎｓｔｒｕ⁃ ｍｅｎｔａｔｉｏｎ， ２００９（１２）： ５⁃８， ２５． ［２］孟偲，王田苗．一种移动机器人全局最优路径规划算法 ［Ｊ］．机器人， ２００８， ２７（３）： ２１７⁃２２２． ＭＥＮＧ Ｓｉ， ＷＡＮＧ Ｔｉａｎｍｉａｏ． Ａ ｇｌｏｂａｌ ｏｐｔｉｍａｌ ｐａｔｈ ｐｌａｎｎｉｎｇ ａｌｇｏｒｉｔｈｍ ｆｏｒ ｍｏｂｉｌｅ ｒｏｂｏｔ［ Ｊ］． Ｒｏｂｏｔ， ２００８， ２７（３）： ２１７⁃ ２２２． ［３］杨明，王宏，何克忠，等．基于激光雷达的移动机器人环境 建模与避障［ Ｊ］．清华大学学报：自然科学版， ２０００， ４０ （７）： １１２⁃１１６． ＹＡＮＧ Ｍｉｎｇ， ＷＡＮＧ Ｈｏｎｇ， ＨＥ Ｋｅｚｈｏｎｇ， ｅｔ ａｌ． Ｔｈｅ ｍｏ⁃ ｂｉｌｅ ｒｏｂｏｔ ｅｎｖｉｒｏｎｍｅｎｔ ｍｏｄｅｌｉｎｇ ａｎｄ ｏｂｓｔａｃｌｅ ａｖｏｉｄａｎｃｅ ｂａｓｅｄ ｏｎ ｌａｓｅｒ ｒａｄａｒ［ Ｊ］． Ｔｓｉｎｇｈｕａ Ｊｏｕｒｎａｌ ｏｆ Ｓｃｉｅｎｃｅ ａｎｄ Ｔｅｃｈｎｏｌｏｇｙ： Ｓｃｉｅｎｃｅ ａｎｄ Ｔｅｃｈｎｏｌｏｇｙ， ２０００， ４０（７）： １１２⁃ １１６． 第 ３ 期 李大伟，等：一种基于卡尔曼滤波与模糊算法的变电站机器人组合导航及控制系统设计 ·２３１·
.232. 智能系统学报 第8卷 [4]杨明，董斌，王宏，等基于激光雷达的移动机器人实时位 DENG Jianqiu,LI Haijun,HU Song,et al.A kind of net- 姿估计方法研究[J].自动化学报，2004,30(5)：679- worked control system based on feature model and fuzzy dy- 687. namic feature model of adaptive control method[J].Jour- YANG Ming,DONG Bin,WANG Hong,et al.Research on nal of Beijing University of Science and Technology,2012. laser rader based real-time pose estimation for mobile robots 34(1):1-5. [J].Acta Automatic Sinica,2004,30(5):679-687. [11]李大伟，贾鹏飞，李卫国，等.一种基于卡尔曼滤波的 [5]肖鹏，栾贻青，郭锐，等.变电站智能巡检机器人激光导航 DR/LMS组合导航定位算法[J].自动化技术与应用， 系统研究[J].自动化与仪表，2012(5)：5-9. 2012,31(9):17-23. XIAO Peng,LUAN Yiqing,GUO Rui,et al.Research on LI Dawei,JIA Pengfei,LI Weiguo,et al.An algorithm on laser navigation system for intelligent inspection robot in DR/LMS integrated navigation and positioning system based on KALMAN filter[J].Techniques of Automation substation[J].Automation and Instrumentation,2012(5): and Applications,2012,31(9):17-23. 5-9. 作者简介： [6]刘胤伯，刘大维，高军伟.基于EPOS的激光导航移动机 李大伟，男，1987年生，硕士研究 器人运动控制研究[J].测控技术，2010,29(10)：47-50. 生，主要研究方向为智能电网与电力系 LIU Yinbo,LIU Dawei,GAO Junwei.Research on motion 统新能源技术、电气设备在线监测与故 control for mobile robot of laser navigation of based on EPOS 障诊断. [J].Measurement and Control Technology,2010,29(10): 47-50. [7]付梦印，邓忠红，张继伟.Kalman滤波理论及其在导航系 统中的应用M].北京：科学出版社，2003. 贾鹏飞，男，1988年生，硕士研究 [8]PASSINO K M,YURKOVICH S.Fuzzy control[M].Menlo 生，主要研究方向为新能源风电并网技 Park,USA:Addison Wesley Longman,2001:21-72 术、电气设备在线监测与故障诊断」 [9]汪洋，郭晨，孙富春，等基于动态神经模糊算法的船舶航 向自适应控制[J].哈尔滨工程大学学报，2009,30 (10):1159-1164 WANG Yang,GUO Chen,SUN Fuchun,et al.Adaptive control of ship course using a dynamic neural fuzzy algorithm 李卫国，男，1954年生，教授，博士 [J].Journal of Harbin Engineering University,2009,30 生导师，主要研究方向为电气设备在线 (10):1159-1164. 监测与故障诊断、移动机器人与电力设 [10]邓建球，李海军，胡松，等.基于特征模型和模糊动态特 备检测等。 征模型自适应控制方法的网络控制系统[J].北京科技 大学学报，2012,34(1)：1-5
［４］杨明，董斌，王宏，等．基于激光雷达的移动机器人实时位 姿估计方法研究［ Ｊ］．自动化学报， ２００４， ３０（ ５）： ６７９⁃ ６８７． ＹＡＮＧ Ｍｉｎｇ， ＤＯＮＧ Ｂｉｎ， ＷＡＮＧ Ｈｏｎｇ， ｅｔ ａｌ． Ｒｅｓｅａｒｃｈ ｏｎ ｌａｓｅｒ ｒａｄｅｒ ｂａｓｅｄ ｒｅａｌ⁃ｔｉｍｅ ｐｏｓｅ ｅｓｔｉｍａｔｉｏｎ ｆｏｒ ｍｏｂｉｌｅ ｒｏｂｏｔｓ ［Ｊ］． Ａｃｔａ Ａｕｔｏｍａｔｉｃ Ｓｉｎｉｃａ， ２００４， ３０（５）： ６７９⁃６８７． ［５］肖鹏，栾贻青，郭锐，等．变电站智能巡检机器人激光导航 系统研究［Ｊ］．自动化与仪表， ２０１２（５）： ５⁃９． ＸＩＡＯ Ｐｅｎｇ， ＬＵＡＮ Ｙｉｑｉｎｇ， ＧＵＯ Ｒｕｉ， ｅｔ ａｌ． Ｒｅｓｅａｒｃｈ ｏｎ ｌａｓｅｒ ｎａｖｉｇａｔｉｏｎ ｓｙｓｔｅｍ ｆｏｒ ｉｎｔｅｌｌｉｇｅｎｔ ｉｎｓｐｅｃｔｉｏｎ ｒｏｂｏｔ ｉｎ ｓｕｂｓｔａｔｉｏｎ［Ｊ］． Ａｕｔｏｍａｔｉｏｎ ａｎｄ Ｉｎｓｔｒｕｍｅｎｔａｔｉｏｎ， ２０１２（５）： ５⁃９． ［６］刘胤伯，刘大维，高军伟．基于 ＥＰＯＳ 的激光导航移动机 器人运动控制研究［Ｊ］．测控技术， ２０１０， ２９（１０）： ４７⁃５０． ＬＩＵ Ｙｉｎｂｏ， ＬＩＵ Ｄａｗｅｉ， ＧＡＯ Ｊｕｎｗｅｉ． Ｒｅｓｅａｒｃｈ ｏｎ ｍｏｔｉｏｎ ｃｏｎｔｒｏｌ ｆｏｒ ｍｏｂｉｌｅ ｒｏｂｏｔ ｏｆ ｌａｓｅｒ ｎａｖｉｇａｔｉｏｎ ｏｆ ｂａｓｅｄ ｏｎ ＥＰＯＳ ［ Ｊ］． Ｍｅａｓｕｒｅｍｅｎｔ ａｎｄ Ｃｏｎｔｒｏｌ Ｔｅｃｈｎｏｌｏｇｙ， ２０１０， ２９（１０）： ４７⁃５０． ［７］付梦印，邓忠红，张继伟．Ｋａｌｍａｎ 滤波理论及其在导航系 统中的应用［Ｍ］．北京：科学出版社， ２００３． ［８］ＰＡＳＳＩＮＯ Ｋ Ｍ， ＹＵＲＫＯＶＩＣＨ Ｓ． Ｆｕｚｚｙ ｃｏｎｔｒｏｌ［Ｍ］． Ｍｅｎｌｏ Ｐａｒｋ， ＵＳＡ： Ａｄｄｉｓｏｎ Ｗｅｓｌｅｙ Ｌｏｎｇｍａｎ， ２００１： ２１⁃７２． ［９］汪洋，郭晨，孙富春，等．基于动态神经模糊算法的船舶航 向自适应控制 ［ Ｊ］． 哈尔滨工程大学学报， ２００９， ３０ （１０）： １１５９⁃１１６４． ＷＡＮＧ Ｙａｎｇ， ＧＵＯ Ｃｈｅｎ， ＳＵＮ Ｆｕｃｈｕｎ， ｅｔ ａｌ． Ａｄａｐｔｉｖｅ ｃｏｎｔｒｏｌ ｏｆ ｓｈｉｐ ｃｏｕｒｓｅ ｕｓｉｎｇ ａ ｄｙｎａｍｉｃ ｎｅｕｒａｌ ｆｕｚｚｙ ａｌｇｏｒｉｔｈｍ ［Ｊ］． Ｊｏｕｒｎａｌ ｏｆ Ｈａｒｂｉｎ Ｅｎｇｉｎｅｅｒｉｎｇ Ｕｎｉｖｅｒｓｉｔｙ， ２００９， ３０ （１０）： １１５９⁃１１６４． ［１０］邓建球，李海军，胡松，等．基于特征模型和模糊动态特 征模型自适应控制方法的网络控制系统［ Ｊ］．北京科技 大学学报， ２０１２， ３４（１）： １⁃５． ＤＥＮＧ Ｊｉａｎｑｉｕ， ＬＩ Ｈａｉｊｕｎ， ＨＵ Ｓｏｎｇ， ｅｔ ａｌ． Ａ ｋｉｎｄ ｏｆ ｎｅｔ⁃ ｗｏｒｋｅｄ ｃｏｎｔｒｏｌ ｓｙｓｔｅｍ ｂａｓｅｄ ｏｎ ｆｅａｔｕｒｅ ｍｏｄｅｌ ａｎｄ ｆｕｚｚｙ ｄｙ⁃ ｎａｍｉｃ ｆｅａｔｕｒｅ ｍｏｄｅｌ ｏｆ ａｄａｐｔｉｖｅ ｃｏｎｔｒｏｌ ｍｅｔｈｏｄ［ Ｊ］． Ｊｏｕｒ⁃ ｎａｌ ｏｆ Ｂｅｉｊｉｎｇ Ｕｎｉｖｅｒｓｉｔｙ ｏｆ Ｓｃｉｅｎｃｅ ａｎｄ Ｔｅｃｈｎｏｌｏｇｙ， ２０１２， ３４（１）： １⁃５． ［１１］李大伟，贾鹏飞，李卫国，等． 一种基于卡尔曼滤波的 ＤＲ／ ＬＭＳ 组合导航定位算法［ Ｊ］．自动化技术与应用， ２０１２， ３１（９）： １７⁃２３． ＬＩ Ｄａｗｅｉ， ＪＩＡ Ｐｅｎｇｆｅｉ， ＬＩ Ｗｅｉｇｕｏ， ｅｔ ａｌ． Ａｎ ａｌｇｏｒｉｔｈｍ ｏｎ ＤＲ／ ＬＭＳ ｉｎｔｅｇｒａｔｅｄ ｎａｖｉｇａｔｉｏｎ ａｎｄ ｐｏｓｉｔｉｏｎｉｎｇ ｓｙｓｔｅｍ ｂａｓｅｄ ｏｎ ＫＡＬＭＡＮ ｆｉｌｔｅｒ ［ Ｊ］． Ｔｅｃｈｎｉｑｕｅｓ ｏｆ Ａｕｔｏｍａｔｉｏｎ ａｎｄ Ａｐｐｌｉｃａｔｉｏｎｓ， ２０１２， ３１（９）： １７⁃２３． 作者简介： 李大伟，男，１９８７ 年生，硕士研究 生，主要研究方向为智能电网与电力系 统新能源技术、电气设备在线监测与故 障诊断． 贾鹏飞，男，１９８８ 年生，硕士研究 生，主要研究方向为新能源风电并网技 术、电气设备在线监测与故障诊断． 李卫国，男，１９５４ 年生，教授，博士 生导师，主要研究方向为电气设备在线 监测与故障诊断、移动机器人与电力设 备检测等． ·２３２· 智 能 系 统 学 报 第 ８ 卷