CN114739404A - 高精度定位方法、装置及带电作业机器人定位*** - Google Patents
高精度定位方法、装置及带电作业机器人定位*** Download PDFInfo
- Publication number
- CN114739404A CN114739404A CN202210474676.9A CN202210474676A CN114739404A CN 114739404 A CN114739404 A CN 114739404A CN 202210474676 A CN202210474676 A CN 202210474676A CN 114739404 A CN114739404 A CN 114739404A
- Authority
- CN
- China
- Prior art keywords
- imu
- data
- observation
- filter
- prediction
- 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
- 238000000034 method Methods 0.000 title claims abstract description 25
- 238000005259 measurement Methods 0.000 claims abstract description 10
- 239000011159 matrix material Substances 0.000 claims description 21
- 238000004891 communication Methods 0.000 claims description 11
- 238000012545 processing Methods 0.000 claims description 6
- 230000001133 acceleration Effects 0.000 claims description 4
- 230000002093 peripheral effect Effects 0.000 claims description 4
- 230000004927 fusion Effects 0.000 abstract description 17
- 238000005516 engineering process Methods 0.000 abstract description 9
- 238000001914 filtration Methods 0.000 abstract description 3
- 230000000295 complement effect Effects 0.000 abstract description 2
- 238000009616 inductively coupled plasma Methods 0.000 abstract 2
- 238000009499 grossing Methods 0.000 abstract 1
- 238000010586 diagram Methods 0.000 description 9
- 230000005540 biological transmission Effects 0.000 description 2
- 238000012937 correction Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000013178 mathematical model Methods 0.000 description 2
- 230000006978 adaptation Effects 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000003245 working effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
-
- 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/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/13—Receivers
- G01S19/14—Receivers specially adapted for specific applications
-
- 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
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
-
- 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种高精度定位方法、装置及带电作业机器人定位***,其定位方法融合了多种数据,一方面采用惯性测量单元IMU和RTK技术进行融合,平滑航向噪声以及姿态滤波,另一方面引入激光雷达局部精定位技术,利用点云ICP匹配位姿,结合了激光雷达和RTK技术的优势互补特性,实现目标物的厘米级精确定位,能够满足带电机器人等目标物在空旷、半遮挡、强遮挡等多种场景下的高精定位需求。
Description
技术领域
本发明涉及机器人定位技术领域,具体涉及一种高精度定位方法、装置及带电作业机器人定位***。
背景技术
目前越来越多的场合,采用带电作业机器人替代人工作业,将体力消耗大、技能需求高、危险性大的操作转变为安全、简单的操作。如在作业之前,室外带电作业机器人需要从一个粗略的位置自动或者人为遥操到达指定精确的位置进行电线操作,该过程中需要通过高精度定位模块实时反馈当前位姿。考虑到该机器人具有空旷或存在少量遮挡的场景特点,且机器人当前位置尤其是到达指定位置精度要求较高,其比较适合实时差分定位(Realtime kinematic,RTK)技术,但是RTK技术需要额外搭建基准站为移动站发送差分电文。现有技术中仍缺乏较成熟的融合定位方案。
发明内容
技术目的:针对上述技术问题,本发明提出了一种室外带电作业机器人的高精定位方法,能够为室外带电作业机器人解决移动和操作电线等作业过程中的高精定位问题。
技术方案:为实现上述技术目的,本发明采用了如下技术方案:
一种高精度定位方法,其特征在于,包括步骤:
优选地,还包括步骤:
获取激光雷达数据,激光雷达数据包括目标物当前的三维点云及其对应协方差;
所述ESKF滤波器根据IMU数据、GNSS数据和激光雷达数据,对积分预测的各种预测参考量进行修正。
优选地,所述ESKF滤波器中包括:
位置观测模型,如式(1)所示:
速度观测模型,如式(2)所示:
vrtk=vimu+R[ω]×δl
其中,ω表示旋转角速度,[ω]×表示旋转角速度的反对称矩阵;
四元数q的航向观测模型,如式(3)所示:
优选地,根据所述观测模型获得表示观测量与滤波状态量关系观测雅可比矩阵,包括:
位置观测雅可比矩阵,如式(4)所示:
速度观测雅可比矩阵,如式(5)所示:
H3×15=[O3×3 I3×3 -[R[ω]×δl]× O3×3 R[δl]×] (5)
航向观测雅可比矩阵,如式(6)所示:
其中,δθ表示角度误差,O3×3表示3*3零矩阵,I3×3表示3*3单位矩阵。
一种高精度定位装置,其特征在于,包括:
优选地,还包括激光ICP匹配位姿解算模块,用于获取激光雷达数据并对激光雷达数据进行匹配位姿解算,激光雷达数据包括目标物当前的三维点云及其对应协方差;
所述ESKF滤波器根据预测参考量、GNSS数据和激光雷达数据,对积分预测的各种预测参考量进行修正。
一种带电作业机器人定位***,其特征在于:包括主机、通信盒子、工控机和传感器,所述主机、通信盒子和传感器均安装于室外带电作业机器人上,主机集成了所述的惯性测量单元、GNSS-RTK定位模块、主控芯片以及***硬件接口,主控芯片设有所述ESKF滤波器,通信盒子、工控机、传感器均通信连接主机连接。
优选地,所述工控机通信连接所述的激光雷达模块。
有益效果:由于采用了上述技术方案,本发明具有如下有益效果:
本发明的定位方法基于IMU/RTK/激光雷达技术,能够实现厘米级定位精度,方便应用于带电作业机器人定位,一方面,该机器人利用机械臂操作电线时需要参考本体的姿态信息,而双天线RTK***提供的航向噪声大,且无法提供姿态,故采用惯性测量单元IMU和RTK技术进行融合(组合导航***),平滑航向噪声以及姿态滤波;另一方面,该机器人会遇到遮挡、多路径场景,该场景会影响组合导航***的定位性能,或无法满足该机器人的稳定定位要求,故引入激光雷达局部精定位技术,点云ICP匹配位姿,利用雷达和RTK技术的优势互补特性,满足该机器人的全场景的高精定位需求。
附图说明
图1是嵌入式硬件架构图;
图2是IMU/GNSS-RTK/激光雷达融合滤波原理图;
图3是GNSS-RTK观测量三角模型;
图4是采用市面上成熟的组合导航产品的方法与本发明方法得到的在空旷场景下的融合位姿轨迹图的对比图;
图5是遮挡场景下的点云地图和位姿融合轨迹图;
图6是遮挡场景下的激光雷达融合前后高度图(橙色为未融合激光的高度漂移数据)。
具体实施方式
下面结合附图对本发明的实施例作详细的说明。
实施例一
本实施例提供一套完整的硬件***,即一种高精度定位装置,具体包括主机、盒子、工控机、传感器。其中,主机高度集成了IMU惯性检测单元、GNSS-RTK模块、主控芯片以及***硬件接口,主控芯片中设有ESKF滤波器。若用于室外带电作业机器人上,盒子表现为电台和4G两种版本的硬件形态,鉴于带电作业机器人的小场景特点,此处采用电台方式,电台盒子通过RS232与主机连接,工控机通过RS232与主机连接,激光雷达通过以太网与工控机连接。图1仅展示了主机和盒子硬件模块图。
本实施例中,主控芯片具体包括传感器数据解析软件模块、差分电台数据透传软件模块和融合滤波器/ESKF滤波器,此外还包括激光ICP匹配位姿解算模块,其运行于工控机端。其中,GNSS差分电文是由自行搭建的静态基站输出差分RTCM电文(RTCM:行业标准协议)机器人端并通过电台传输方式向RTK模块输入差分数据(有了差分修正信息,机器人端的定位可以获取厘米级定位精度,否则仅为米级定位精度,无法满足机器人定位需求),传感器数据解析软件模块向融合滤波器输入原始传感器数据,激光ICP模块向融合滤波器输入点云位姿数据。图2展示了融合滤波器模块图,也是本发明的核心。
实施例二
本实施例提供了一种高精度定位方法,包括步骤:
优选地,还包括步骤:
获取激光雷达数据,激光雷达数据包括目标物当前的三维点云及其对应协方差;
所述ESKF滤波器根据IMU数据、GNSS数据和激光雷达数据,对积分预测的各种预测参考量进行修正。
本实施例的方法用于实施例一的装置中,能够实现高精定位功能的原理如下:
IMU与GNSS-RTK组成的双天线组合导航定位模块已经能够解决带电作业机器人的大部分定位问题,此处提供关键的数学模型。组合导航融合滤波器采用ESKF(Error-stateKalman Filter,误差状态卡尔曼滤波)算法,其核心思想是利用高频率IMU进行积分预测,用GNSS-RTK和激光点云匹配位姿观测量对IMU积分累计误差进行校正,即融合滤波器由预测和观测修正两个模型组成。ESKF滤波状态量为[δp,δv,δθ,δab,δωb],分别表示为位置、速度、角度、加速度偏差、陀螺仪偏差的误差量。其预测模型可参考文献《Quaternionkinematics for the error-state Kalman filter》,本发明不再赘述。GNSS-RTK航向、速度和位置观测模型推导如下:
(1)位置观测模型
如图3所示,GNSS-RTK位置观测量(备注:大地经纬转换成ENU),IMU位置预测量卫星定位天线在IMU坐标系下的向量(已知量,可通过机械结构测量获得),IMU坐标系相对ENU东北天坐标系的旋转为R。此处的数学关系如下:
位置观测方程关于δθ的雅可比矩阵推导如下:
位置观测雅可比矩阵如下:
(2)速度观测模型
由位置观测方程推导速度观测方程:
vrtk=vimu+R[ω]×δl
速度观测方程关于δθ的雅可比矩阵推导如下:
速度观测雅可比矩阵如下:
H3×15=[O3×3 I3×3 -[R[ω]×δl]× O3×3 R[δl]×]
(3)航向观测模型
本发明采用了312的旋转顺序,IMU坐标系选取为“左前上”。
qt=2q0q3-2q1q2
四元数q关于δθ的雅可比矩阵:
如图5和6所示,在遮挡场景下位置观测精度下降,会导致高度估计漂移,影响机器人作业效果。针对该问题,引入激光雷达局部精定位技术,在工控机端运行点云ICP匹配算法并输出带位置协方差的位姿,同时在空旷场景下进行初始化,结合机械结构计算的IMU和激光雷达、定位天线外参数,为点云ICP匹配位姿转换到ENU(东北天)坐标系下,并提供初值。故点云ICP匹配位姿、GNSS-RTK均可输出位置协方差的ENU位置观测,位置观测模型同GNSS-RTK。当面对遮挡场景,GNSS-RTK位置协方差变大,点云ICP匹配位姿权重变大,融合滤波器仍然可以输出高精度的稳定位姿输出。
图4、5、6分别表示空旷场景下的IMU/GNSS-RTK融合位姿轨迹、遮挡场景下的IMU/GNSS-RTK/激光融合位姿轨迹、遮挡场景下的激光融合前后数据对比曲线图。图4是采用市面上成熟的组合导航产品的方法与本发明方法得到的在空旷场景下的融合位姿轨迹图的对比图,图4中混叠在一起的箭头对应了上述方法两种方法,从图4结果可知,基于IMU/GNSS-RTK组合定位***的带电作业机器人正常作业。利用市面上成熟的组合导航产品作为参考,评估本发明的定位精度如下表。
表1
由表1可以看出,本发明的方法,能够实现厘米级的高精度定位。
图5可知,基于IMU/GNSS-RTK/激光组合定位***的带电作业机器人正常作业。图6可知,遮挡场景下,GNSS-RTK高度观测量出现漂移,融合激光ICP匹配位姿后,高度状态趋于正常,说明了滤波器较强的环境适应能力。
以上所述仅是本发明的优选实施方式,应当指出:对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰也应视为本发明的保护范围。
Claims (8)
2.根据权利要求1所述的高精度定位方法,其特征在于,还包括步骤:
获取激光雷达数据,激光雷达数据包括目标物当前的三维点云及其对应协方差;
所述ESKF滤波器根据IMU数据、GNSS数据和激光雷达数据,对积分预测的各种预测参考量进行修正。
6.根据权利要求5所述的一种高精度定位装置,其特征在于,还包括激光ICP匹配位姿解算模块,用于获取激光雷达数据并对激光雷达数据进行匹配位姿解算,激光雷达数据包括目标物当前的三维点云及其对应协方差;
所述ESKF滤波器根据预测参考量、GNSS数据和激光雷达数据,对积分预测的各种预测参考量进行修正。
7.一种带电作业机器人定位***,其特征在于:包括主机、通信盒子、工控机和传感器,所述主机、通信盒子和传感器均安装于室外带电作业机器人上,主机集成了如权利要求5所述的IMU模块、GNSS模块、主控芯片以及***硬件接口,主控芯片中设有如权利要求5所述的ESKF滤波器,通信盒子、工控机、传感器均通信连接主机连接。
8.根据权利要求7所述的一种带电作业机器人定位***,其特征在于:所述工控机通信连接如权利要求6所述的激光雷达模块。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210474676.9A CN114739404A (zh) | 2022-04-29 | 2022-04-29 | 高精度定位方法、装置及带电作业机器人定位*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210474676.9A CN114739404A (zh) | 2022-04-29 | 2022-04-29 | 高精度定位方法、装置及带电作业机器人定位*** |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114739404A true CN114739404A (zh) | 2022-07-12 |
Family
ID=82286035
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210474676.9A Pending CN114739404A (zh) | 2022-04-29 | 2022-04-29 | 高精度定位方法、装置及带电作业机器人定位*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114739404A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115793009A (zh) * | 2023-02-03 | 2023-03-14 | 中国人民解放***箭军工程大学 | 基于高精度北斗组合测量的多站无源定位方法 |
-
2022
- 2022-04-29 CN CN202210474676.9A patent/CN114739404A/zh active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115793009A (zh) * | 2023-02-03 | 2023-03-14 | 中国人民解放***箭军工程大学 | 基于高精度北斗组合测量的多站无源定位方法 |
CN115793009B (zh) * | 2023-02-03 | 2023-05-09 | 中国人民解放***箭军工程大学 | 基于高精度北斗组合测量的多站无源定位方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Xiong et al. | G-VIDO: A vehicle dynamics and intermittent GNSS-aided visual-inertial state estimator for autonomous driving | |
Lupton et al. | Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions | |
Langelaan | State estimation for autonomous flight in cluttered environments | |
CN111156998A (zh) | 一种基于rgb-d相机与imu信息融合的移动机器人定位方法 | |
CN112987065B (zh) | 一种融合多传感器的手持式slam装置及其控制方法 | |
CN108362288B (zh) | 一种基于无迹卡尔曼滤波的偏振光slam方法 | |
US20210183100A1 (en) | Data processing method and apparatus | |
CN108350679A (zh) | 机动平地机的铲刀自动控制*** | |
CN111380514A (zh) | 机器人位姿估计方法、装置、终端及计算机存储介质 | |
CN108387236B (zh) | 一种基于扩展卡尔曼滤波的偏振光slam方法 | |
KR20210040877A (ko) | 포지셔닝 방법 및 장치 | |
CN110160545B (zh) | 一种激光雷达与gps的增强定位***及方法 | |
CN108759822B (zh) | 一种移动机器人3d定位*** | |
CN110751123B (zh) | 一种单目视觉惯性里程计***及方法 | |
CN112967392A (zh) | 一种基于多传感器触合的大规模园区建图定位方法 | |
CN111426320A (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
Jin et al. | Fast and accurate initialization for monocular vision/INS/GNSS integrated system on land vehicle | |
CN116047565A (zh) | 一种多传感器数据融合定位*** | |
Karam et al. | Integrating a low-cost mems imu into a laser-based slam for indoor mobile mapping | |
CN114739404A (zh) | 高精度定位方法、装置及带电作业机器人定位*** | |
Xian et al. | Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach | |
Filip et al. | LiDAR SLAM comparison in a featureless tunnel environment | |
CN113960614A (zh) | 一种基于帧-地图匹配的高程图构建方法 | |
CN113008229A (zh) | 一种基于低成本车载传感器的分布式自主组合导航方法 | |
CN117387604A (zh) | 基于4d毫米波雷达和imu融合的定位与建图方法及*** |
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 |