CN109975817A - 一种变电站巡检机器人定位导航方法及系统 - Google Patents
一种变电站巡检机器人定位导航方法及系统 Download PDFInfo
- Publication number
- CN109975817A CN109975817A CN201910292915.7A CN201910292915A CN109975817A CN 109975817 A CN109975817 A CN 109975817A CN 201910292915 A CN201910292915 A CN 201910292915A CN 109975817 A CN109975817 A CN 109975817A
- Authority
- CN
- China
- Prior art keywords
- current time
- mobile robot
- intelligent mobile
- data
- last moment
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
Abstract
本发明公开了一种变电站巡检机器人定位导航方法及系统。该方法包括:获取变电站巡检机器人当前时刻的位置数据和周围环境点云数据;计算第一位姿信息和第二位姿信息;获取上一时刻的校正数据;依据上一时刻的校正数据,采用卡尔曼滤波算法对第一位姿信息或第二位姿信息进行滤波处理,得到当前时刻的校正数据;依据当前时刻的校正数据,构建当前时刻变电站巡检机器人所处环境的地图模型,以实现变电站巡检机器人的定位导航。本发明能够提高定位精度,减小对周围环境进行地图构建的误差。
Description
技术领域
本发明涉及变电站巡检技术领域,特别是涉及一种变电站巡检机器人定位导航方法及系统。
背景技术
变电站是改变电压等级、电力传输的枢纽,是电力系统中的重要组成部分,对其安全性有很高的要求。因此,变电站的巡检工作必不可少,高质量、高效率的巡检能很大程度地提高变电站运行的可靠性和安全性,有利于维护电力系统的正常运行和稳定。
人工巡检是变电站巡检的传统作业方式,巡检人员有时由于工作劳累、厌烦等因素,很容易造成漏检、错检。根据中国电科院的统计显示,每年由于漏检和错检造成的直接经济损失都要超过26亿元。可见,人工巡检难以保证电力系统的安全可靠。随着科技的发展,使用巡检机器人完成巡检任务,以提高变电站运行的可靠性,成为一种发展趋势。
变电站巡检机器人中定位导航模块尤为重要,定位导航是实现巡检机器人自主运行的关键,其通过传感器根据周围环境建模,并规划行车路径,沿着路径自主行驶,实现变电站巡检任务。目前,变电站巡检机器人的定位导航技术主要有:磁轨迹导航、惯性导航、激光定位导航和GPS导航。磁轨迹导航应用较早,需预先在道路上铺设磁条,机器车通过感应磁条信息来寻找路径,编程简单,定位精度高,但是其初期造价高,且后期重新规划路径较为麻烦;惯性导航不适合应用在变电站这类电磁强度高的环境中,因为其主要利用电子罗盘测量地球磁场强度来规划方向路径,会产生较大偏差;激光定位导航虽然定位导航精度相对较高,但是需要设置路标辅助导航,使用环境受到约束;GPS导航灵活性高,但需要接受卫星信号,会有信号失真及丢失的情况,影响定位精度。
综上,现有的变电站巡检机器人利用单一传感器实现定位导航,定位精度差,对周围环境进行地图构建的误差也较大。
发明内容
基于此,有必要提供一种变电站巡检机器人定位导航方法及系统,以提高定位精度,减小对周围环境进行地图构建的误差。
为实现上述目的,本发明提供了如下方案:
一种变电站巡检机器人定位导航方法,包括:
获取变电站巡检机器人当前时刻的位置数据和周围环境点云数据;所述位置数据是采用GPS获取得到的,所述周围环境点云数据是采用激光雷达获取得到的;
计算第一位姿信息和第二位姿信息;所述第一位姿信息是依据当前时刻的位置数据计算得到的,所述第二位姿信息是依据当前时刻的周围环境点云数据计算得到的;
获取上一时刻的校正数据;
当所述上一时刻的校正数据包括上一时刻的位置状态滤波估计值和上一时刻的位置估计误差协方差时,则依据所述上一时刻的位置状态滤波估计值和所述上一时刻的位置估计误差协方差,采用卡尔曼滤波算法对所述第二位姿信息进行滤波处理,得到当前时刻的校正数据;此时,所述当前时刻的校正数据包括当前时刻的环境状态滤波估计值和当前时刻的环境估计误差协方差;
当所述上一时刻的校正数据包括上一时刻的环境状态滤波估计值和上一时刻的环境估计误差协方差时,则依据所述上一时刻的环境状态滤波估计值和所述上一时刻的环境估计误差协方差,采用卡尔曼滤波算法对所述第一位姿信息进行滤波处理,得到当前时刻的校正数据;此时,所述当前时刻的校正数据包括当前时刻的位置状态滤波估计值和当前时刻的位置估计误差协方差;
依据所述当前时刻的校正数据,构建当前时刻变电站巡检机器人所处环境的地图模型,以实现变电站巡检机器人的定位导航。
可选的,所述依据所述当前时刻的校正数据,构建当前时刻变电站巡检机器人所处环境的地图模型,以实现变电站巡检机器人的定位导航,具体包括:
当所述当前时刻的校正数据包括当前时刻的环境状态滤波估计值和当前时刻的环境估计误差协方差时,则依据所述当前时刻的环境状态滤波估计值构建当前时刻变电站巡检机器人所处环境的地图模型,以实现变电站巡检机器人的定位导航;
当所述当前时刻的校正数据包括当前时刻的位置状态滤波估计值和当前时刻的位置估计误差协方差时,则依据所述当前时刻的位置状态滤波估计值构建当前时刻变电站巡检机器人所处环境的地图模型,以实现变电站巡检机器人的定位导航。
可选的,在所述获取变电站巡检机器人当前时刻的位置数据和周围环境点云数据之后,所述计算第一位姿信息和第二位姿信息之前,还包括:
对所述位置数据和所述周围环境点云数据进行预处理。
可选的,所述位置数据z1=(Px,Py),其中,Px为变电站巡检机器人所处位置在直角坐标系下的横坐标,Py为变电站巡检机器人所处位置在直角坐标系下的纵坐标;所述周围环境点云数据其中,ρ表示变电站巡检机器人所处位置距离极坐标系原点的位置,表示变电站巡检机器人在极坐标系下的角度,表示变电站巡检机器人在极坐标系下的速度。
本发明还提供了一种变电站巡检机器人定位导航系统,所述系统包括:
第一数据获取模块,用于获取变电站巡检机器人当前时刻的位置数据和周围环境点云数据;所述位置数据是采用GPS获取得到的,所述周围环境点云数据是采用激光雷达获取得到的;
位姿计算模块,用于计算第一位姿信息和第二位姿信息;所述第一位姿信息是依据当前时刻的位置数据计算得到的,所述第二位姿信息是依据当前时刻的周围环境点云数据计算得到的;
第二数据获取模块,用于获取上一时刻的校正数据;
第一校正模块,用于当所述上一时刻的校正数据包括上一时刻的位置状态滤波估计值和上一时刻的位置估计误差协方差时,则依据所述上一时刻的位置状态滤波估计值和所述上一时刻的位置估计误差协方差,采用卡尔曼滤波算法对所述第二位姿信息进行滤波处理,得到当前时刻的校正数据;此时,所述当前时刻的校正数据包括当前时刻的环境状态滤波估计值和当前时刻的环境估计误差协方差;
第二校正模块,用于当所述上一时刻的校正数据包括上一时刻的环境状态滤波估计值和上一时刻的环境估计误差协方差时,则依据所述上一时刻的环境状态滤波估计值和所述上一时刻的环境估计误差协方差,采用卡尔曼滤波算法对所述第一位姿信息进行滤波处理,得到当前时刻的校正数据;此时,所述当前时刻的校正数据包括当前时刻的位置状态滤波估计值和当前时刻的位置估计误差协方差;
模型构建模块,用于依据所述当前时刻的校正数据,构建当前时刻变电站巡检机器人所处环境的地图模型,以实现变电站巡检机器人的定位导航。
可选的,所述模型构建模块,具体包括:
第一构建单元,用于当所述当前时刻的校正数据包括当前时刻的环境状态滤波估计值和当前时刻的环境估计误差协方差时,则依据所述当前时刻的环境状态滤波估计值构建当前时刻变电站巡检机器人所处环境的地图模型,以实现变电站巡检机器人的定位导航;
第二构建单元,用于当所述当前时刻的校正数据包括当前时刻的位置状态滤波估计值和当前时刻的位置估计误差协方差时,则依据所述当前时刻的位置状态滤波估计值构建当前时刻变电站巡检机器人所处环境的地图模型,以实现变电站巡检机器人的定位导航。
可选的,所述系统还包括:
预处理模块,用于对所述位置数据和所述周围环境点云数据进行预处理。
可选的,所述位置数据z1=(Px,Py),其中,Px为变电站巡检机器人所处位置在直角坐标系下的横坐标,Py为变电站巡检机器人所处位置在直角坐标系下的纵坐标;所述周围环境点云数据其中,ρ表示变电站巡检机器人所处位置距离极坐标系原点的位置,表示变电站巡检机器人在极坐标系下的角度,表示变电站巡检机器人在极坐标系下的速度。
与现有技术相比,本发明的有益效果是:
本发明提出了一种变电站巡检机器人定位导航方法及系统。该方法包括:获取变电站巡检机器人当前时刻的位置数据和周围环境点云数据;计算第一位姿信息和第二位姿信息;获取上一时刻的校正数据;依据上一时刻的校正数据,采用卡尔曼滤波算法对当前时刻的第一位姿信息或第二位姿信息进行滤波处理,得到当前时刻的校正数据;依据当前时刻的校正数据,构建当前时刻变电站巡检机器人所处环境的地图模型,以实现变电站巡检机器人的定位导航。本发明在现有的采用激光雷达获取的点云数据进行定位的基础上,加入了GPS数据,通过两组数组之间相互交替更新位姿信息的方式,使二者融合,提高了定位精度,减小了对周围环境进行地图构建的误差。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动性的前提下,还可以根据这些附图获得其他的附图。
图1为本发明实施例1一种变电站巡检机器人定位导航方法的流程图;
图2为本发明实施例3一种变电站巡检机器人定位导航系统的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
为使本发明的上述目的、特征和优点能够更加明显易懂,下面结合附图和具体实施方式对本发明作进一步详细的说明。
术语解释:
变电站巡检机器人:巡检机器人系统根据预先设定的参数,自主启动完成例行巡检任务或运维人员远程遥控机器人完成巡检任务。变电站巡检机器人的整个平台是以基于激光雷达的同步定位和地图创建技术(Lidar-based Simultancous Localization AndMapping,SLAM)、导航和ROS框架为基础,其中主要的硬件模块有:RPLIDAR A1激光雷达、树莓派3B+、STM32、双路电机驱动模块、减速电机和降压/稳压直流电源模块。
卡尔曼滤波:一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响,所以最优估计也可看作是滤波过程。
实施例1:
图1为本发明实施例一种变电站巡检机器人定位导航方法的流程图。
参见图1,实施例的变电站巡检机器人定位导航方法,包括:
步骤S1:获取变电站巡检机器人当前时刻的位置数据和周围环境点云数据。
所述位置数据是采用GPS获取得到的,所述周围环境点云数据是采用激光雷达获取得到的。所述位置数据z1=(Px,Py),其中,Px为变电站巡检机器人所处位置在直角坐标系下的横坐标,Py为变电站巡检机器人所处位置在直角坐标系下的纵坐标;所述周围环境点云数据其中,ρ表示变电站巡检机器人所处位置距离极坐标系原点的位置,表示变电站巡检机器人在极坐标系下的角度,表示变电站巡检机器人在极坐标系下的速度。
GPS定位的原理是把卫星某一时刻的瞬时位置作为初始值,卫星将有本身信息的无线电波发送给用户终端,终端通过接收机精确测量信号从卫星到终端的时间,计算出它们的距离,卫星到接收机的交点即为接收机所在位置。本实施例中变电站巡检机器人中的GPS采用AT6558的ATGM332D,其具有出色的定位导航功能,冷启动捕获灵敏度为-148dBm,跟踪灵敏度为-162dBm,定位精度为2.5米(CEP50),首次定位时间为32秒,能够满足系统需要。
激光雷达基于TOF原理设计,根据激光发射和遇到障碍物返回经过的时间按照激光在空气中的传播速度折算得到当前位置到障碍物的距离。该原理结构简单、扫描频率高、测量距离精确度高、数据信息丰富以及受光线影响小。本实施例中变电站巡检机器人中的激光雷达采用的是由SLAMTEC公司开发的RPLIDARA1激光雷达,该雷达成本低,利用三角测距原理对二维平面6m半径范围内进行激光测距扫描,生成平面点云地图信息,在5.5Hz的扫描频率下,测距分辨率可达当前距离值的0.2%。
步骤S2:计算第一位姿信息和第二位姿信息。
所述第一位姿信息是依据当前时刻的位置数据计算得到的,所述第二位姿信息是依据当前时刻的周围环境点云数据计算得到的。
步骤S3:获取上一时刻的校正数据。
步骤S4:当所述上一时刻的校正数据包括上一时刻的位置状态滤波估计值和上一时刻的位置估计误差协方差时,则依据所述上一时刻的位置状态滤波估计值和所述上一时刻的位置估计误差协方差,采用卡尔曼滤波算法对所述第二位姿信息进行滤波处理,得到当前时刻的校正数据。
此时,所述当前时刻的校正数据包括当前时刻的环境状态滤波估计值和当前时刻的环境估计误差协方差。
步骤S5:当所述上一时刻的校正数据包括上一时刻的环境状态滤波估计值和上一时刻的环境估计误差协方差时,则依据所述上一时刻的环境状态滤波估计值和所述上一时刻的环境估计误差协方差,采用卡尔曼滤波算法对所述第一位姿信息进行滤波处理,得到当前时刻的校正数据。
此时,所述当前时刻的校正数据包括当前时刻的位置状态滤波估计值和当前时刻的位置估计误差协方差。
步骤S6:依据所述当前时刻的校正数据,构建当前时刻变电站巡检机器人所处环境的地图模型,以实现变电站巡检机器人的定位导航。
所述步骤S6,具体包括:
当所述当前时刻的校正数据包括当前时刻的环境状态滤波估计值和当前时刻的环境估计误差协方差时,则依据所述当前时刻的环境状态滤波估计值构建当前时刻变电站巡检机器人所处环境的地图模型,以实现变电站巡检机器人的定位导航;
当所述当前时刻的校正数据包括当前时刻的位置状态滤波估计值和当前时刻的位置估计误差协方差时,则依据所述当前时刻的位置状态滤波估计值构建当前时刻变电站巡检机器人所处环境的地图模型,以实现变电站巡检机器人的定位导航。
作为一种可选的实施方式,在所述步骤S1之后,所述步骤S2之前,还包括:对所述位置数据和所述周围环境点云数据进行预处理。
本实施例的变电站巡检机器人定位导航方法,采用GPS和激光雷达混合导航的思路,利用卡尔曼滤波,进行融合数据,使其优势互补,提高了定位导航精度。
实施例2:
变电站巡检机器人在工作过程中,通过在运动过程中反复观测到的环境特征,重新定位自己的位姿,继而根据自身位置增量式地构建地图,便达到了同时定位和构建地图的作用。但由于此过程中有随机误差,传感器对机器车运动的判断也有不确定性,同时存在变电站的电磁干扰等外界因素的影响,使得在每次构建地图时,加到前一次地图上的增量有误差,随着迭代次数的增多,误差越来越大,从而降低了地图构建以及定位导航的精度。本实施利用卡尔曼滤波算法对迭代增加的误差进行补偿,以提高定位精度。
卡尔曼滤波是基于最小方差经过递推计算进行最优估计的滤波算法,在组合导航系统中,用卡尔曼滤波估计各种系统状态误差,再根据获得的状态误差估计值来进行反馈校正,得到导航参数的估计值,实现组合导航的目的。卡尔曼滤波算法采用递推式,只需要储存当前时刻的信息和上一时刻的状态滤波估值,就可以计算出当前误差状态的最优估计,而不需要存储之前的量测信息和状态滤波估值。
标准卡尔曼滤波只适用于线性系统,对于非线性系统的滤波方法,可采用扩展卡尔曼滤波方法。扩展卡尔曼滤波方法是将卡尔曼滤波的式子进行适当的变化,将非线性系统进行线性化处理,泰勒展开的方式只取一阶项。
下面对本实施例变电站巡检机器人定位方法的具体过程进行描述。
变电站巡检机器人采用激光雷达和GPS两种传感器进行定位导航,能够充分发挥激光雷达以及GPS各自的优点,即激光雷达精度高,GPS较为灵活不受环境约束;又能够去除各自的缺点,即去除激光雷达需预先标识反射板,使用时对环境要求高,GPS信号易受到干扰甚至丢失,从而导致误差增大,精度降低的缺点。在本实施例中,使用卡尔曼滤波进行激光雷达与GPS的数据融合。卡尔曼滤波以导航系统输出参数作为状态变量,经过计算,得到导航参数的最优估计值。具体为:
1)获取变电站巡检机器人当前时刻的位置数据和周围环境点云数据。所述位置数据是采用GPS获取得到的,所述周围环境点云数据是采用激光雷达获取得到的。
所述位置数据z1=(Px,Py),其中,Px为变电站巡检机器人所处位置在直角坐标系下的横坐标,Py为变电站巡检机器人所处位置在直角坐标系下的纵坐标;所述周围环境点云数据其中,ρ表示变电站巡检机器人所处位置距离极坐标系原点的位置,表示变电站巡检机器人在极坐标系下的角度,表示变电站巡检机器人在极坐标系下的速度。
2)计算第一位姿信息和第二位姿信息。所述第一位姿信息是依据当前时刻的位置数据计算得到的,所述第二位姿信息是依据当前时刻的周围环境点云数据计算得到的。具体的,第一位姿信息
第二位姿信息为
3)获取上一时刻的校正数据。
4)当所述上一时刻的校正数据包括上一时刻的位置状态滤波估计值和上一时刻的位置估计误差协方差时,则依据所述上一时刻的位置状态滤波估计值和所述上一时刻的位置估计误差协方差,采用卡尔曼滤波算法对所述第二位姿信息进行滤波处理,得到当前时刻的校正数据。
此时,所述当前时刻的校正数据包括当前时刻的环境状态滤波估计值和当前时刻的环境估计误差协方差。具体为:
x1'=x1+K1y1
P1'=(1-K1H1)P1
其中,x1′表示当前时刻的环境状态滤波估计值,P1′表示当前时刻的环境估计误差协方差,其中
y1=z1-H1x1
S1=H1PH1 T+R1
K1=P1H1 TS1 -1,
H1表示第二位姿信息对应的状态空间的到测量空间之间的映射,R1表示第二位姿信息对应的测量值的不确定度,K1表示第二位姿信息对应的卡尔曼增益,P1表示上一时刻的位置估计误差协方差。其中,
5)当所述上一时刻的校正数据包括上一时刻的环境状态滤波估计值和上一时刻的环境估计误差协方差时,则依据所述上一时刻的环境状态滤波估计值和所述上一时刻的环境估计误差协方差,采用卡尔曼滤波算法对所述第一位姿信息进行滤波处理,得到当前时刻的校正数据。
此时,所述当前时刻的校正数据包括当前时刻的位置状态滤波估计值和当前时刻的位置估计误差协方差。具体为:
x2'=x2+K2y2
P2'=(1-K2H2j)P2,
其中,x2′表示当前时刻的位置状态滤波估计值,P2′表示当前时刻的位置估计误差协方差
y2=z2-f(x)
其中,H2j表示第一位姿信息对应的状态空间的到测量空间之间的映射,R2表示第一位姿信息对应的测量值的不确定度,K2表示第一位姿信息对应的卡尔曼增益,P2表示上一时刻的环境估计误差协方差。
其中,状态空间到测量空间的非线性映射f(x)如下
非线性映射线性化后的Jacobian矩阵Hj,
测量值的不确定度
6)依据所述当前时刻的校正数据,构建当前时刻变电站巡检机器人所处环境的地图模型,以实现变电站巡检机器人的定位导航。具体的,
当所述当前时刻的校正数据包括当前时刻的环境状态滤波估计值和当前时刻的环境估计误差协方差时,则依据所述当前时刻的环境状态滤波估计值构建当前时刻变电站巡检机器人所处环境的地图模型,以实现变电站巡检机器人的定位导航。
当所述当前时刻的校正数据包括当前时刻的位置状态滤波估计值和当前时刻的位置估计误差协方差时,则依据所述当前时刻的位置状态滤波估计值构建当前时刻变电站巡检机器人所处环境的地图模型,以实现变电站巡检机器人的定位导航。
本实施例中,在收到第一个测量值后,对机器人状态x进行初始化。计算得到位姿信息后,预测下一位置
位姿预测状态方程:
x'=Fx
P'=FPF+Q
其中,x表示状态滤波估计值,P表示估计误差协方差,F是系统的控制函数,表示系统的改变,这里将问题简单化,只考虑线性系统。其中,
P表明了系统状态的不确定性程度,用x的协方差表示,
Q表明了x'=Fx未能刻画的其他外界干扰。本实施例使用线性模型,因此,加速度变成了干扰项。x'=Fx中未衡量的额外项目为v,
其中,ax,ay分别为x轴方向的加速度,y轴方向的加速度,σ为标准差,v服从高斯分布N(0,Q)。
其中,σax表示x轴方向的加速度的标准差,σay表示y轴方向的加速度的标准差。
实施例3:
本发明还提供了一种变电站巡检机器人定位导航系统,图2为本发明实施例一种变电站巡检机器人定位导航系统的结构示意图。
参见图2,实施例的变电站巡检机器人定位导航系统包括:
第一数据获取模块201,用于获取变电站巡检机器人当前时刻的位置数据和周围环境点云数据;所述位置数据是采用GPS获取得到的,所述周围环境点云数据是采用激光雷达获取得到的。
所述位置数据z1=(Px,Py),其中,Px为变电站巡检机器人所处位置在直角坐标系下的横坐标,Py为变电站巡检机器人所处位置在直角坐标系下的纵坐标;所述周围环境点云数据其中,ρ表示变电站巡检机器人所处位置距离极坐标系原点的位置,表示变电站巡检机器人在极坐标系下的角度,表示变电站巡检机器人在极坐标系下的速度。
位姿计算模块202,用于计算第一位姿信息和第二位姿信息;所述第一位姿信息是依据当前时刻的位置数据计算得到的,所述第二位姿信息是依据当前时刻的周围环境点云数据计算得到的。
第二数据获取模块203,用于获取上一时刻的校正数据。
第一校正模块204,用于当所述上一时刻的校正数据包括上一时刻的位置状态滤波估计值和上一时刻的位置估计误差协方差时,则依据所述上一时刻的位置状态滤波估计值和所述上一时刻的位置估计误差协方差,采用卡尔曼滤波算法对所述第二位姿信息进行滤波处理,得到当前时刻的校正数据;此时,所述当前时刻的校正数据包括当前时刻的环境状态滤波估计值和当前时刻的环境估计误差协方差。
第二校正模块205,用于当所述上一时刻的校正数据包括上一时刻的环境状态滤波估计值和上一时刻的环境估计误差协方差时,则依据所述上一时刻的环境状态滤波估计值和所述上一时刻的环境估计误差协方差,采用卡尔曼滤波算法对所述第一位姿信息进行滤波处理,得到当前时刻的校正数据;此时,所述当前时刻的校正数据包括当前时刻的位置状态滤波估计值和当前时刻的位置估计误差协方差。
模型构建模块206,用于依据所述当前时刻的校正数据,构建当前时刻变电站巡检机器人所处环境的地图模型,以实现变电站巡检机器人的定位导航。
作为一种可选的实施方式,所述变电站巡检机器人定位导航系统还包括:预处理模块,用于对所述位置数据和所述周围环境点云数据进行预处理。
本实施例的变电站巡检机器人定位导航系统,采用GPS和激光雷达混合导航的思路,利用卡尔曼滤波,进行融合数据,使其优势互补,提高了定位导航精度。
对于实施例公开的系统而言,由于其与实施例公开的方法相对应,所以描述的比较简单,相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述,以上实施例的说明只是用于帮助理解本发明的方法及其核心思想;同时,对于本领域的一般技术人员,依据本发明的思想,在具体实施方式及应用范围上均会有改变之处。综上所述,本说明书内容不应理解为对本发明的限制。
Claims (8)
1.一种变电站巡检机器人定位导航方法,其特征在于,包括:
获取变电站巡检机器人当前时刻的位置数据和周围环境点云数据;所述位置数据是采用GPS获取得到的,所述周围环境点云数据是采用激光雷达获取得到的;
计算第一位姿信息和第二位姿信息;所述第一位姿信息是依据当前时刻的位置数据计算得到的,所述第二位姿信息是依据当前时刻的周围环境点云数据计算得到的;
获取上一时刻的校正数据;
当所述上一时刻的校正数据包括上一时刻的位置状态滤波估计值和上一时刻的位置估计误差协方差时,则依据所述上一时刻的位置状态滤波估计值和所述上一时刻的位置估计误差协方差,采用卡尔曼滤波算法对所述第二位姿信息进行滤波处理,得到当前时刻的校正数据;此时,所述当前时刻的校正数据包括当前时刻的环境状态滤波估计值和当前时刻的环境估计误差协方差;
当所述上一时刻的校正数据包括上一时刻的环境状态滤波估计值和上一时刻的环境估计误差协方差时,则依据所述上一时刻的环境状态滤波估计值和所述上一时刻的环境估计误差协方差,采用卡尔曼滤波算法对所述第一位姿信息进行滤波处理,得到当前时刻的校正数据;此时,所述当前时刻的校正数据包括当前时刻的位置状态滤波估计值和当前时刻的位置估计误差协方差;
依据所述当前时刻的校正数据,构建当前时刻变电站巡检机器人所处环境的地图模型,以实现变电站巡检机器人的定位导航。
2.根据权利要求1所述的变电站巡检机器人定位导航方法,其特征在于,所述依据所述当前时刻的校正数据,构建当前时刻变电站巡检机器人所处环境的地图模型,以实现变电站巡检机器人的定位导航,具体包括:
当所述当前时刻的校正数据包括当前时刻的环境状态滤波估计值和当前时刻的环境估计误差协方差时,则依据所述当前时刻的环境状态滤波估计值构建当前时刻变电站巡检机器人所处环境的地图模型,以实现变电站巡检机器人的定位导航;
当所述当前时刻的校正数据包括当前时刻的位置状态滤波估计值和当前时刻的位置估计误差协方差时,则依据所述当前时刻的位置状态滤波估计值构建当前时刻变电站巡检机器人所处环境的地图模型,以实现变电站巡检机器人的定位导航。
3.根据权利要求1所述的变电站巡检机器人定位导航方法,其特征在于,在所述获取变电站巡检机器人当前时刻的位置数据和周围环境点云数据之后,所述计算第一位姿信息和第二位姿信息之前,还包括:
对所述位置数据和所述周围环境点云数据进行预处理。
4.根据权利要求1所述的变电站巡检机器人定位导航方法,其特征在于,所述位置数据z1=(Px,Py),其中,Px为变电站巡检机器人所处位置在直角坐标系下的横坐标,Py为变电站巡检机器人所处位置在直角坐标系下的纵坐标;所述周围环境点云数据其中,ρ表示变电站巡检机器人所处位置距离极坐标系原点的位置,表示变电站巡检机器人在极坐标系下的角度,表示变电站巡检机器人在极坐标系下的速度。
5.一种变电站巡检机器人定位导航系统,其特征在于,包括:
第一数据获取模块,用于获取变电站巡检机器人当前时刻的位置数据和周围环境点云数据;所述位置数据是采用GPS获取得到的,所述周围环境点云数据是采用激光雷达获取得到的;
位姿计算模块,用于计算第一位姿信息和第二位姿信息;所述第一位姿信息是依据当前时刻的位置数据计算得到的,所述第二位姿信息是依据当前时刻的周围环境点云数据计算得到的;
第二数据获取模块,用于获取上一时刻的校正数据;
第一校正模块,用于当所述上一时刻的校正数据包括上一时刻的位置状态滤波估计值和上一时刻的位置估计误差协方差时,则依据所述上一时刻的位置状态滤波估计值和所述上一时刻的位置估计误差协方差,采用卡尔曼滤波算法对所述第二位姿信息进行滤波处理,得到当前时刻的校正数据;此时,所述当前时刻的校正数据包括当前时刻的环境状态滤波估计值和当前时刻的环境估计误差协方差;
第二校正模块,用于当所述上一时刻的校正数据包括上一时刻的环境状态滤波估计值和上一时刻的环境估计误差协方差时,则依据所述上一时刻的环境状态滤波估计值和所述上一时刻的环境估计误差协方差,采用卡尔曼滤波算法对所述第一位姿信息进行滤波处理,得到当前时刻的校正数据;此时,所述当前时刻的校正数据包括当前时刻的位置状态滤波估计值和当前时刻的位置估计误差协方差;
模型构建模块,用于依据所述当前时刻的校正数据,构建当前时刻变电站巡检机器人所处环境的地图模型,以实现变电站巡检机器人的定位导航。
6.根据权利要求5所述的变电站巡检机器人定位导航系统,其特征在于,所述模型构建模块,具体包括:
第一构建单元,用于当所述当前时刻的校正数据包括当前时刻的环境状态滤波估计值和当前时刻的环境估计误差协方差时,则依据所述当前时刻的环境状态滤波估计值构建当前时刻变电站巡检机器人所处环境的地图模型,以实现变电站巡检机器人的定位导航;
第二构建单元,用于当所述当前时刻的校正数据包括当前时刻的位置状态滤波估计值和当前时刻的位置估计误差协方差时,则依据所述当前时刻的位置状态滤波估计值构建当前时刻变电站巡检机器人所处环境的地图模型,以实现变电站巡检机器人的定位导航。
7.根据权利要求5所述的变电站巡检机器人定位导航系统,其特征在于,还包括:
预处理模块,用于对所述位置数据和所述周围环境点云数据进行预处理。
8.根据权利要求5所述的变电站巡检机器人定位导航系统,其特征在于,所述位置数据z1=(Px,Py),其中,Px为变电站巡检机器人所处位置在直角坐标系下的横坐标,Py为变电站巡检机器人所处位置在直角坐标系下的纵坐标;所述周围环境点云数据其中,ρ表示变电站巡检机器人所处位置距离极坐标系原点的位置,表示变电站巡检机器人在极坐标系下的角度,表示变电站巡检机器人在极坐标系下的速度。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910292915.7A CN109975817A (zh) | 2019-04-12 | 2019-04-12 | 一种变电站巡检机器人定位导航方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910292915.7A CN109975817A (zh) | 2019-04-12 | 2019-04-12 | 一种变电站巡检机器人定位导航方法及系统 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN109975817A true CN109975817A (zh) | 2019-07-05 |
Family
ID=67084272
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910292915.7A Pending CN109975817A (zh) | 2019-04-12 | 2019-04-12 | 一种变电站巡检机器人定位导航方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109975817A (zh) |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110727278A (zh) * | 2019-09-04 | 2020-01-24 | 云南电网有限责任公司曲靖供电局 | 巡检机器人路线控制方法、装置、存储介质及巡检机器人 |
CN111812668A (zh) * | 2020-07-16 | 2020-10-23 | 南京航空航天大学 | 绕机检查装置及其定位方法、存储介质 |
CN112365622A (zh) * | 2020-10-28 | 2021-02-12 | 深圳市朗驰欣创科技股份有限公司 | 一种巡检系统、方法、终端和存储介质 |
CN112511972A (zh) * | 2020-11-27 | 2021-03-16 | 国网天津市电力公司 | 基于5g的变电站巡检机器人定位方法及装置 |
CN112747746A (zh) * | 2020-12-25 | 2021-05-04 | 珠海市一微半导体有限公司 | 基于单点tof的点云数据获取方法、芯片和移动机器人 |
CN113124869A (zh) * | 2019-12-30 | 2021-07-16 | 国网陕西省电力公司榆林供电公司 | 一种基于粒子滤波的变电站巡检机器人导航定位方法 |
CN115586511A (zh) * | 2022-11-25 | 2023-01-10 | 唐山百川工业服务有限公司 | 一种基于阵列立柱的激光雷达二维定位方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20140100713A1 (en) * | 2010-07-15 | 2014-04-10 | George C. Dedes | Gnss/imu positioning, communication, and computation platforms for automotive safety applications |
CN107091648A (zh) * | 2017-05-11 | 2017-08-25 | 江苏保千里视像科技集团股份有限公司 | 一种激光雷达与差分gps的数据融合方法及系统 |
CN107167148A (zh) * | 2017-05-24 | 2017-09-15 | 安科机器人有限公司 | 同步定位与地图构建方法和设备 |
CN108181636A (zh) * | 2018-01-12 | 2018-06-19 | 中国矿业大学 | 石化工厂巡检机器人环境建模与地图构建装置和方法 |
CN108254758A (zh) * | 2017-12-25 | 2018-07-06 | 清华大学苏州汽车研究院(吴江) | 基于多线激光雷达和gps的三维道路构建方法 |
CN108680156A (zh) * | 2018-02-26 | 2018-10-19 | 北京克路德人工智能科技有限公司 | 一种多传感器数据融合的机器人定位方法 |
-
2019
- 2019-04-12 CN CN201910292915.7A patent/CN109975817A/zh active Pending
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20140100713A1 (en) * | 2010-07-15 | 2014-04-10 | George C. Dedes | Gnss/imu positioning, communication, and computation platforms for automotive safety applications |
CN107091648A (zh) * | 2017-05-11 | 2017-08-25 | 江苏保千里视像科技集团股份有限公司 | 一种激光雷达与差分gps的数据融合方法及系统 |
CN107167148A (zh) * | 2017-05-24 | 2017-09-15 | 安科机器人有限公司 | 同步定位与地图构建方法和设备 |
CN108254758A (zh) * | 2017-12-25 | 2018-07-06 | 清华大学苏州汽车研究院(吴江) | 基于多线激光雷达和gps的三维道路构建方法 |
CN108181636A (zh) * | 2018-01-12 | 2018-06-19 | 中国矿业大学 | 石化工厂巡检机器人环境建模与地图构建装置和方法 |
CN108680156A (zh) * | 2018-02-26 | 2018-10-19 | 北京克路德人工智能科技有限公司 | 一种多传感器数据融合的机器人定位方法 |
Cited By (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110727278A (zh) * | 2019-09-04 | 2020-01-24 | 云南电网有限责任公司曲靖供电局 | 巡检机器人路线控制方法、装置、存储介质及巡检机器人 |
CN113124869A (zh) * | 2019-12-30 | 2021-07-16 | 国网陕西省电力公司榆林供电公司 | 一种基于粒子滤波的变电站巡检机器人导航定位方法 |
CN111812668A (zh) * | 2020-07-16 | 2020-10-23 | 南京航空航天大学 | 绕机检查装置及其定位方法、存储介质 |
CN112365622A (zh) * | 2020-10-28 | 2021-02-12 | 深圳市朗驰欣创科技股份有限公司 | 一种巡检系统、方法、终端和存储介质 |
CN112365622B (zh) * | 2020-10-28 | 2022-06-28 | 深圳市朗驰欣创科技股份有限公司 | 一种巡检系统、方法、终端和存储介质 |
CN112511972A (zh) * | 2020-11-27 | 2021-03-16 | 国网天津市电力公司 | 基于5g的变电站巡检机器人定位方法及装置 |
CN112747746A (zh) * | 2020-12-25 | 2021-05-04 | 珠海市一微半导体有限公司 | 基于单点tof的点云数据获取方法、芯片和移动机器人 |
CN115586511A (zh) * | 2022-11-25 | 2023-01-10 | 唐山百川工业服务有限公司 | 一种基于阵列立柱的激光雷达二维定位方法 |
CN115586511B (zh) * | 2022-11-25 | 2023-03-03 | 唐山百川工业服务有限公司 | 一种基于阵列立柱的激光雷达二维定位方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109975817A (zh) | 一种变电站巡检机器人定位导航方法及系统 | |
Gao et al. | Review of wheeled mobile robots’ navigation problems and application prospects in agriculture | |
CN109059942A (zh) | 一种井下高精度导航地图构建系统及构建方法 | |
CN103472823B (zh) | 一种智能机器人用的栅格地图创建方法 | |
Alonso et al. | Accurate global localization using visual odometry and digital maps on urban environments | |
CN104062687B (zh) | 一种空地一体的地磁场联合观测方法及系统 | |
CN102353379B (zh) | 一种适用于自动驾驶车导航的环境建模方法 | |
CN104236548A (zh) | 一种微型无人机室内自主导航方法 | |
CN107702722A (zh) | 一种激光导引agv自然导航定位方法 | |
CN101598556A (zh) | 一种未知环境下无人机视觉/惯性组合导航方法 | |
CN104914865A (zh) | 变电站巡检机器人定位导航系统及方法 | |
CN104897161A (zh) | 基于激光测距的室内平面地图制图方法 | |
CN110308729A (zh) | 基于视觉与imu或里程计的agv组合导航定位方法 | |
CN110146910A (zh) | 一种基于gps与激光雷达数据融合的定位方法及装置 | |
CN110211228A (zh) | 用于建图的数据处理方法及装置 | |
Pfaff et al. | Towards mapping of cities | |
CN107943042A (zh) | 一种地磁指纹数据库自动化构建方法与装置 | |
CN103400416B (zh) | 一种基于概率多层地形的城市环境机器人导航方法 | |
CN102288176A (zh) | 基于信息融合的煤矿救灾机器人导航系统及方法 | |
CN109839118A (zh) | 路径规划方法、系统、机器人和计算机可读存储介质 | |
CN105333869A (zh) | 一种基于自适应ekf的无人侦察机同步定位与构图方法 | |
Li et al. | Autonomous navigation and environment modeling for MAVs in 3-D enclosed industrial environments | |
Trigubovich et al. | Complex technology of navigation and geodetic support of airborne electromagnetic surveys | |
CN115183762A (zh) | 一种机场仓库内外建图方法、系统、电子设备及介质 | |
Hu et al. | A small and lightweight autonomous laser mapping system without GPS |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20190705 |
|
RJ01 | Rejection of invention patent application after publication |