CN114739404A - 高精度定位方法、装置及带电作业机器人定位*** - Google Patents

高精度定位方法、装置及带电作业机器人定位*** Download PDF

Info

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
Application number
CN202210474676.9A
Other languages
English (en)
Inventor
徐伟
李勇兵
罗作煌
刘文博
李奇
郭永春
朱超
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Yijiahe Technology R & D Co ltd
Original Assignee
Shenzhen Yijiahe Technology R & D Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Shenzhen Yijiahe Technology R & D Co ltd filed Critical Shenzhen Yijiahe Technology R & D Co ltd
Priority to CN202210474676.9A priority Critical patent/CN114739404A/zh
Publication of CN114739404A publication Critical patent/CN114739404A/zh
Pending legal-status Critical Current

Links

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技术需要额外搭建基准站为移动站发送差分电文。现有技术中仍缺乏较成熟的融合定位方案。
发明内容
技术目的：针对上述技术问题，本发明提出了一种室外带电作业机器人的高精定位方法，能够为室外带电作业机器人解决移动和操作电线等作业过程中的高精定位问题。
技术方案：为实现上述技术目的，本发明采用了如下技术方案：
一种高精度定位方法，其特征在于，包括步骤：
获取IMU数据，所述IMU数据包括比力a和旋转角速度w，采用捷联惯性导航算法对IMU数据进行处理，获得积分预测的位置预测参考量
Figure BDA0003624828780000011
速度预测参考量
Figure BDA0003624828780000012
和姿态预测参考量
Figure BDA0003624828780000013
获取GNSS数据，所述GNSS数据包括目标当前的位置观测量
Figure BDA0003624828780000014
速度观测量vrtk和航向观测量
Figure BDA0003624828780000015
构建ESKF滤波器，所述预测参考量和GNSS数据输入ESKF滤波器，ESKF滤波器输出位置误差δp、速度误差δv、姿态误差δq、加速度偏差
Figure BDA0003624828780000016
陀螺仪偏差
Figure BDA0003624828780000017
确定目标的真实位置
Figure BDA0003624828780000018
真实速度
Figure BDA0003624828780000019
和真实姿态
Figure BDA00036248287800000110
优选地，还包括步骤：
获取激光雷达数据，激光雷达数据包括目标物当前的三维点云及其对应协方差；
所述ESKF滤波器根据IMU数据、GNSS数据和激光雷达数据，对积分预测的各种预测参考量进行修正。
优选地，所述ESKF滤波器中包括：
位置观测模型，如式(1)所示：
Figure BDA0003624828780000021
其中，
Figure BDA0003624828780000022
表示卫星定位天线在IMU坐标系下的向量，
Figure BDA0003624828780000023
表示惯性测量单元IMU的位置预测量，R表示IMU坐标系相对ENU东北天坐标系的旋转；
速度观测模型，如式(2)所示：
vrtk＝vimu+R[ω]×δl
其中，ω表示旋转角速度，[ω]×表示旋转角速度的反对称矩阵；
四元数q的航向观测模型，如式(3)所示：
Figure BDA0003624828780000024
Figure BDA0003624828780000025
其中，q1、q2、q0、q3表示姿态四元数q实部和三个虚部，
Figure BDA0003624828780000026
表示采用了321的旋转顺序对应的航向观测量，
Figure BDA0003624828780000027
表示采用了312的旋转顺序对应的航向观测量。
优选地，根据所述观测模型获得表示观测量与滤波状态量关系观测雅可比矩阵，包括：
位置观测雅可比矩阵，如式(4)所示：
Figure BDA0003624828780000028
速度观测雅可比矩阵，如式(5)所示：
H3×15＝[O3×3 I3×3 -[R[ω]×δl]× O3×3 R[δl]×] (5)
航向观测雅可比矩阵，如式(6)所示：
Figure BDA0003624828780000031
其中，δθ表示角度误差，O3×3表示3*3零矩阵，I3×3表示3*3单位矩阵。
一种高精度定位装置，其特征在于，包括：
惯性测量单元，用于获取IMU数据，所述IMU数据包括比力a和旋转角速度w，采用捷联惯性导航算法对IMU数据进行处理，获得积分预测的位置预测参考量
Figure BDA0003624828780000032
速度预测参考量
Figure BDA0003624828780000033
和姿态预测参考量
Figure BDA0003624828780000034
GNSS-RTK定位模块，用于获取GNSS数据，所述GNSS数据包括目标当前的位置观测量
Figure BDA0003624828780000035
速度观测量vrtk和航向观测量
Figure BDA0003624828780000036
ESKF滤波器，输入预测参考量和GNSS数据，处理后输出位置误差δp、速度误差δv、角度误差δθ、加速度偏差
Figure BDA0003624828780000037
陀螺仪偏差
Figure BDA0003624828780000038
输出目标的真实位置
Figure BDA0003624828780000039
真实速度
Figure BDA00036248287800000310
和真实姿态
Figure BDA00036248287800000311
优选地，还包括激光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展示了融合滤波器模块图，也是本发明的核心。
实施例二
本实施例提供了一种高精度定位方法，包括步骤：
获取IMU数据，所述IMU数据包括比力a和旋转角速度w，采用捷联惯性导航算法对IMU数据进行处理，获得积分预测的位置预测参考量
Figure BDA0003624828780000051
速度预测参考量
Figure BDA0003624828780000052
和姿态预测参考量
Figure BDA0003624828780000053
获取GNSS数据，所述GNSS数据包括目标当前的位置观测量
Figure BDA0003624828780000054
速度观测量vrtk和航向观测量
Figure BDA0003624828780000055
构建ESKF滤波器，所述预测参考量和GNSS数据输入ESKF滤波器，ESKF滤波器输出位置误差δp、速度误差δv、姿态误差δq、加速度偏差
Figure BDA0003624828780000056
陀螺仪偏差
Figure BDA0003624828780000057
确定目标的真实位置
Figure BDA0003624828780000058
真实速度
Figure BDA0003624828780000059
和真实姿态
Figure BDA00036248287800000510
优选地，还包括步骤：
获取激光雷达数据，激光雷达数据包括目标物当前的三维点云及其对应协方差；
所述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位置观测量
Figure BDA0003624828780000061
(备注：大地经纬转换成ENU)，IMU位置预测量
Figure BDA0003624828780000062
卫星定位天线在IMU坐标系下的向量
Figure BDA0003624828780000063
(已知量，可通过机械结构测量获得)，IMU坐标系相对ENU东北天坐标系的旋转为R。此处的数学关系如下：
Figure BDA0003624828780000064
位置观测方程关于δθ的雅可比矩阵推导如下：
Figure BDA0003624828780000065
位置观测雅可比矩阵如下：
Figure BDA0003624828780000066
(2)速度观测模型
由位置观测方程推导速度观测方程：
Figure BDA0003624828780000067
Figure BDA0003624828780000068
vrtk＝vimu+R[ω]×δl
速度观测方程关于δθ的雅可比矩阵推导如下：
Figure BDA0003624828780000069
速度观测雅可比矩阵如下：
H3×15＝[O3×3 I3×3 -[R[ω]×δl]× O3×3 R[δl]×]
(3)航向观测模型
双天线GNSS-RTK可以输出航向观测信息
Figure BDA00036248287800000610
其关于四元数q的数学模型如下：
Figure BDA00036248287800000611
Figure BDA00036248287800000612
本发明采用了312的旋转顺序，IMU坐标系选取为“左前上”。
航向观测方程
Figure BDA0003624828780000071
关于四元数q的雅可比矩阵
Figure BDA0003624828780000072
(此处可以利用matlab进行符号运算)：
Figure BDA0003624828780000073
qt＝2q0q3-2q1q2
Figure BDA0003624828780000074
四元数q关于δθ的雅可比矩阵：
Figure BDA0003624828780000075
则航向观测方程
Figure BDA0003624828780000076
关于δθ的雅可比矩阵：
Figure BDA0003624828780000077
如图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
Figure BDA0003624828780000081
由表1可以看出，本发明的方法，能够实现厘米级的高精度定位。
图5可知，基于IMU/GNSS-RTK/激光组合定位***的带电作业机器人正常作业。图6可知，遮挡场景下，GNSS-RTK高度观测量出现漂移，融合激光ICP匹配位姿后，高度状态趋于正常，说明了滤波器较强的环境适应能力。
以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (8)

1.一种高精度定位方法，其特征在于，包括步骤：
获取IMU数据，所述IMU数据包括比力a和旋转角速度w，采用捷联惯性导航算法对IMU数据进行处理，获得积分预测的位置预测参考量
Figure FDA0003624828770000011
速度预测参考量
Figure FDA0003624828770000012
和姿态预测参考量
Figure FDA0003624828770000013
获取GNSS数据，所述GNSS数据包括目标当前的位置观测量
Figure FDA0003624828770000014
速度观测量vrtk和航向观测量
Figure FDA0003624828770000015
构建ESKF滤波器，所述预测参考量和GNSS数据输入ESKF滤波器，ESKF滤波器输出位置误差δp、速度误差δv、姿态误差δq、加速度偏差
Figure FDA0003624828770000016
陀螺仪偏差
Figure FDA0003624828770000017
确定目标的真实位置
Figure FDA0003624828770000018
真实速度
Figure FDA0003624828770000019
和真实姿态
Figure FDA00036248287700000110
2.根据权利要求1所述的高精度定位方法，其特征在于，还包括步骤：
获取激光雷达数据，激光雷达数据包括目标物当前的三维点云及其对应协方差；
所述ESKF滤波器根据IMU数据、GNSS数据和激光雷达数据，对积分预测的各种预测参考量进行修正。
3.根据权利要求1所述的高精度定位方法，其特征在于：所述ESKF滤波器中包括：
位置观测模型，如式(1)所示：
Figure FDA00036248287700000111
其中，
Figure FDA00036248287700000112
表示卫星定位天线在IMU坐标系下的向量，
Figure FDA00036248287700000113
表示惯性测量单元IMU的位置预测量，R表示IMU坐标系相对ENU东北天坐标系的旋转；
速度观测模型，如式(2)所示：
vrtk＝vimu+R[ω]×δl
其中，ω表示旋转角速度，[ω]×表示旋转角速度的反对称矩阵；
姿态四元数q的航向观测模型，如式(3)所示：
Figure FDA0003624828770000021
Figure FDA0003624828770000022
其中，q1、q2、q0、q3表示姿态四元数q实部和三个虚部，
Figure FDA0003624828770000023
表示采用了321的旋转顺序对应的航向观测量，
Figure FDA0003624828770000024
表示采用了312的旋转顺序对应的航向观测量。
4.根据权利要求3所述的高精度定位方法，其特征在于，根据所述观测模型获得表示观测量与滤波状态量关系观测雅可比矩阵，包括：
位置观测雅可比矩阵，如式(4)所示：
Figure FDA0003624828770000025
速度观测雅可比矩阵，如式(5)所示：
H3×15＝[O3×3 I3×3 -[R[ω]×δl]× O3×3 R[δl]×] (5)
航向观测雅可比矩阵，如式(6)所示：
Figure FDA0003624828770000026
其中，δθ表示角度误差，O3×3表示3*3零矩阵，I3×3表示3*3单位矩阵。
5.一种高精度定位装置，其特征在于，包括：
惯性测量单元，用于获取IMU数据，所述IMU数据包括比力a和旋转角速度w，采用捷联惯性导航算法对IMU数据进行处理，获得积分预测的位置预测参考量
Figure FDA0003624828770000027
速度预测参考量
Figure FDA0003624828770000028
和姿态预测参考量
Figure FDA0003624828770000029
GNSS-RTK定位模块，用于获取GNSS数据，所述GNSS数据包括目标当前的位置观测量
Figure FDA00036248287700000210
速度观测量vrtk和航向观测量
Figure FDA00036248287700000211
ESKF滤波器，输入预测参考量和GNSS数据，处理后输出位置误差δp、速度误差δv、角度误差δθ、加速度偏差
Figure FDA00036248287700000212
陀螺仪偏差
Figure FDA00036248287700000213
输出目标的真实位置
Figure FDA00036248287700000214
真实速度
Figure FDA00036248287700000215
和真实姿态
Figure FDA00036248287700000216
6.根据权利要求5所述的一种高精度定位装置，其特征在于，还包括激光ICP匹配位姿解算模块，用于获取激光雷达数据并对激光雷达数据进行匹配位姿解算，激光雷达数据包括目标物当前的三维点云及其对应协方差；
所述ESKF滤波器根据预测参考量、GNSS数据和激光雷达数据，对积分预测的各种预测参考量进行修正。
7.一种带电作业机器人定位***，其特征在于：包括主机、通信盒子、工控机和传感器，所述主机、通信盒子和传感器均安装于室外带电作业机器人上，主机集成了如权利要求5所述的IMU模块、GNSS模块、主控芯片以及***硬件接口，主控芯片中设有如权利要求5所述的ESKF滤波器，通信盒子、工控机、传感器均通信连接主机连接。
8.根据权利要求7所述的一种带电作业机器人定位***，其特征在于：所述工控机通信连接如权利要求6所述的激光雷达模块。
CN202210474676.9A 2022-04-29 2022-04-29 高精度定位方法、装置及带电作业机器人定位*** Pending CN114739404A (zh)

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)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115793009A (zh) * 2023-02-03 2023-03-14 中国人民解放***箭军工程大学 基于高精度北斗组合测量的多站无源定位方法

Cited By (2)

* Cited by examiner, † Cited by third party
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