CN114739404A - High-precision positioning method and device and positioning system of hot-line work robot - Google Patents

High-precision positioning method and device and positioning system of hot-line work robot 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
Chinese (zh)
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/en
Publication of CN114739404A publication Critical patent/CN114739404A/en
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

The invention discloses a high-precision positioning method, a high-precision positioning device and a live working robot positioning system, wherein the positioning method fuses various data, on one hand, an Inertial Measurement Unit (IMU) and an RTK (real-time kinematic) technology are adopted for fusion, course noise smoothing and attitude filtering are performed, on the other hand, a laser radar local precise positioning technology is introduced, and on the other hand, a point cloud ICP (inductively coupled plasma) matching pose is utilized, the complementary advantages of the laser radar and the RTK technology are combined, so that centimeter-level precise positioning of a target object is realized, and the high-precision positioning requirements of a motor robot and other target objects under various scenes such as spaciousness, half shielding, strong shielding and the like can be met.

Description

High-precision positioning method and device and positioning system of hot-line work robot
Technical Field
The invention relates to the technical field of robot positioning, in particular to a high-precision positioning method and device and a positioning system of an electric operating robot.
Background
At present, in more and more occasions, the live working robot is adopted to replace manual operation, and the operation with large physical consumption, high skill requirement and large risk is converted into safe and simple operation. For example, before operation, an outdoor live working robot needs to automatically or manually perform teleoperation from a rough position to a specified precise position for wire operation, and the current pose needs to be fed back in real time through a high-precision positioning module in the process. Considering that the robot has the characteristic of a scene with a space or a small amount of shielding, the accuracy requirement of the current position of the robot, particularly the current position reaching the designated position, is high, and the robot is suitable for a Real Time Kinematic (RTK) technology, but the RTK technology needs to additionally build a reference station to send a differential message for the mobile station. The prior art is still lacking in more mature fusion localization schemes.
Disclosure of Invention
The technical purpose is as follows: aiming at the technical problems, the invention provides a high-precision positioning method of an outdoor live working robot, which can solve the high-precision positioning problem of the outdoor live working robot in the working processes of moving, operating wires and the like.
The technical scheme is as follows: in order to achieve the technical purpose, the invention adopts the following technical scheme:
a high-precision positioning method is characterized by comprising the following steps:
obtaining IMU data, wherein the IMU data comprises specific force a and rotation angular velocity w, processing the IMU data by adopting a strapdown inertial navigation algorithm, and obtaining an integral prediction position prediction reference quantity
Figure BDA0003624828780000011
Reference quantity for speed prediction
Figure BDA0003624828780000012
And attitude prediction reference
Figure BDA0003624828780000013
Obtaining GNSS data comprising a current position observation of a target
Figure BDA0003624828780000014
Velocity observed quantity vrtkAnd course observation
Figure BDA0003624828780000015
Constructing an ESKF filter, inputting the predicted reference quantity and GNSS data into the ESKF filter, and outputting a position error delta p, a speed error delta v, an attitude error delta q and an acceleration deviation by the ESKF filter
Figure BDA0003624828780000016
Deviation of gyroscope
Figure BDA0003624828780000017
Determining the true position of an object
Figure BDA0003624828780000018
True velocity
Figure BDA0003624828780000019
And true attitude
Figure BDA00036248287800000110
Preferably, the method further comprises the steps of:
acquiring laser radar data, wherein the laser radar data comprises current three-dimensional point cloud of a target object and corresponding covariance of the point cloud;
and the ESKF filter corrects various prediction reference quantities of integral prediction according to the IMU data, the GNSS data and the laser radar data.
Preferably, the ESKF filter includes:
the position observation model is shown as formula (1):
Figure BDA0003624828780000021
wherein the content of the first and second substances,
Figure BDA0003624828780000022
representing the vector of the satellite positioning antenna in the IMU coordinate system,
Figure BDA0003624828780000023
representing the position pre-measurement of the inertial measurement unit IMU, and R representing the rotation of the IMU coordinate system relative to the ENU northeast coordinate system;
the speed observation model is shown as the formula (2):
vrtk＝vimu+R[ω]×δl
wherein ω represents a rotational angular velocity, [ ω ] is]×An antisymmetric matrix representing a rotation angular velocity;
the course observation model of the quaternion q is shown as the formula (3):
Figure BDA0003624828780000024
Figure BDA0003624828780000025
wherein q is1、q2、q0、q3Representing the real part and three imaginary parts of the attitude quaternion q,
Figure BDA0003624828780000026
indicating that the heading observed quantity corresponding to the rotation sequence of 321 is adopted,
Figure BDA0003624828780000027
indicating miningThe heading observations corresponding to the rotation order of 312 are used.
Preferably, obtaining an observation jacobian matrix representing a relation between an observation quantity and a filtering state quantity according to the observation model includes:
and (3) observing a Jacobian matrix of the position, as shown in formula (4):
Figure BDA0003624828780000028
the velocity observation jacobian matrix is shown as formula (5):
H3×15＝[O3×3 I3×3 -[R[ω]×δl]× O3×3 R[δl]×] (5)
and (3) a course observation jacobian matrix, as shown in formula (6):
Figure BDA0003624828780000031
wherein δ θ represents an angle error, O3×3Denotes a 3 x 3 zero matrix, I3×3Representing a 3 x 3 identity matrix.
A high precision positioning apparatus, comprising:
an inertial measurement unit for acquiring IMU data including specific force a and rotational angular velocity w, processing the IMU data by using strapdown inertial navigation algorithm to obtain position prediction reference quantity of integral prediction
Figure BDA0003624828780000032
Reference quantity for speed prediction
Figure BDA0003624828780000033
And attitude prediction reference
Figure BDA0003624828780000034
A GNSS-RTK positioning module to acquire GNSS data including a current position observation of a target
Figure BDA0003624828780000035
Velocity observed quantity vrtkAnd course observation
Figure BDA0003624828780000036
ESKF filter, inputting prediction reference quantity and GNSS data, and outputting position error delta after processingpVelocity error deltavAngle error deltaθDeviation of acceleration
Figure BDA0003624828780000037
Deviation of gyroscope
Figure BDA0003624828780000038
Outputting the true position of the target
Figure BDA0003624828780000039
True velocity
Figure BDA00036248287800000310
And true attitude
Figure BDA00036248287800000311
Preferably, the system further comprises a laser ICP matching pose resolving module, wherein the laser ICP matching pose resolving module is used for acquiring laser radar data and resolving the matching pose of the laser radar data, and the laser radar data comprises the current three-dimensional point cloud of the target object and the corresponding covariance thereof;
and the ESKF filter corrects various predicted reference quantities of integral prediction according to the predicted reference quantity, the GNSS data and the laser radar data.
A live working robot positioning system which characterized in that: the device comprises a host, a communication box, an industrial personal computer and a sensor, wherein the host, the communication box and the sensor are all installed on an outdoor live working robot, the host is integrated with an inertial measurement unit, a GNSS-RTK positioning module, a main control chip and a peripheral hardware interface, the main control chip is provided with an ESKF filter, and the communication box, the industrial personal computer and the sensor are all connected with the host in a communication mode.
Preferably, the industrial personal computer is in communication connection with the laser radar module.
Has the advantages that: due to the adoption of the technical scheme, the invention has the following beneficial effects:
the positioning method is based on IMU/RTK/laser radar technology, centimeter-level positioning accuracy can be realized, and the positioning method is conveniently applied to positioning of a live working robot; on the other hand, the robot can meet the sheltering and multipath scenes, the positioning performance of the combined navigation system can be influenced by the scenes, or the stable positioning requirement of the robot can not be met, so that the laser radar local precise positioning technology and the point cloud ICP matching pose are introduced, and the full-scene high-precision positioning requirement of the robot is met by utilizing the advantage complementary characteristics of the radar and the RTK technology.
Drawings
FIG. 1 is a diagram of an embedded hardware architecture;
FIG. 2 is a schematic diagram of IMU/GNSS-RTK/lidar fusion filtering;
FIG. 3 is a GNSS-RTK observation trigonometric model;
FIG. 4 is a comparison diagram of a fusion pose trajectory diagram in an open scene obtained by a method of using a commercially mature integrated navigation product and the method of the present invention;
FIG. 5 is a point cloud map and pose fusion trajectory diagram in an occlusion scene;
fig. 6 is a height map before and after laser radar fusion in an occlusion scene (orange is height drift data of unfused laser light).
Detailed Description
The following describes embodiments of the present invention in detail with reference to the accompanying drawings.
Example one
This embodiment provides a set of complete hardware system, a high accuracy positioner promptly, specifically includes host computer, box, industrial computer, sensor. The system comprises a host, an IMU inertial detection unit, a GNSS-RTK module, a main control chip and a peripheral hardware interface, wherein the host highly integrates the IMU inertial detection unit, the GNSS-RTK module, the main control chip and the peripheral hardware interface, and an ESKF filter is arranged in the main control chip. If be used for on the outdoor live working robot, the box represents the hardware form of radio station and two kinds of versions of 4G, in view of live working robot's little scene characteristics, adopts the radio station mode here, and the radio station box passes through RS232 and is connected with the host computer, and the industrial computer passes through RS232 and is connected with the host computer, and laser radar passes through the ethernet and is connected with the industrial computer. Fig. 1 shows only a host and box hardware block diagram.
In this embodiment, the main control chip specifically includes a sensor data analysis software module, a difference station data transparent transmission software module, a fusion filter/ESKF filter, and a laser ICP matching pose resolving module, which is run at the industrial control machine. The GNSS differential message is a differential RTCM message (RTCM: industry standard protocol) robot end output by a self-built static base station, differential data (with differential correction information, the positioning of the robot end can obtain centimeter-level positioning accuracy, otherwise, the positioning accuracy is only meter-level positioning accuracy and cannot meet the positioning requirement of the robot) is input to an RTK module through a radio station transmission mode, the sensor data analysis software module inputs original sensor data to the fusion filter, and the laser ICP module inputs point cloud pose data to the fusion filter. Fig. 2 shows a fusion filter block diagram, which is also the core of the present invention.
Example two
The embodiment provides a high-precision positioning method, which comprises the following steps:
obtaining IMU data, wherein the IMU data comprises specific force a and rotation angular velocity w, processing the IMU data by adopting a strapdown inertial navigation algorithm, and obtaining an integral prediction position prediction reference quantity
Figure BDA0003624828780000051
Reference quantity for speed prediction
Figure BDA0003624828780000052
Posture of harmonyReference quantity for state prediction
Figure BDA0003624828780000053
Obtaining GNSS data comprising a current position observation of a target
Figure BDA0003624828780000054
Velocity observed quantity vrtkAnd course observation
Figure BDA0003624828780000055
Constructing an ESKF filter, inputting the prediction reference quantity and the GNSS data into the ESKF filter, and outputting a position error delta p, a speed error delta v, an attitude error delta q and an acceleration deviation by the ESKF filter
Figure BDA0003624828780000056
Deviation of gyroscope
Figure BDA0003624828780000057
Determining the true position of an object
Figure BDA0003624828780000058
True velocity
Figure BDA0003624828780000059
And true attitude
Figure BDA00036248287800000510
Preferably, the method further comprises the steps of:
acquiring laser radar data, wherein the laser radar data comprises current three-dimensional point cloud of a target object and corresponding covariance of the point cloud;
and the ESKF filter corrects various predicted reference quantities of integral prediction according to the IMU data, the GNSS data and the laser radar data.
The method of the embodiment is used in the device of the first embodiment, and the principle of realizing the high-precision positioning function is as follows:
IMU and GNThe SS-RTK dual-antenna combined navigation positioning module can solve most positioning problems of the live working robot, and a key mathematical model is provided. The combined navigation fusion Filter adopts an ESKF (Error-state Kalman Filter) algorithm, and the core idea is to use a high-frequency IMU to perform integral prediction and use GNSS-RTK and laser point cloud matching pose observed quantity to correct IMU integral accumulated errors, namely the fusion Filter consists of two models of prediction and observation correction. ESKF filter state quantities [ δ p, δ v, δ θ, δ ab,δωb]The error amounts are expressed as position, velocity, angle, acceleration deviation, and gyro deviation, respectively. The prediction model can be referred to in the literature "Quaternion dynamics for the error-state Kalman filter", and the present invention is not repeated herein. The GNSS-RTK course, speed and position observation model is derived as follows:
(1) position observation model
As shown in FIG. 3, GNSS-RTK position observations
Figure BDA0003624828780000061
(remarks: geodetic longitude and latitude conversion to ENU), IMU position prediction
Figure BDA0003624828780000062
Vector of satellite positioning antenna under IMU coordinate system
Figure BDA0003624828780000063
(known quantities, obtainable by mechanical structural measurements) and the rotation of the IMU coordinate system with respect to the ENU northeast coordinate system is R. The mathematical relationship here is as follows:
Figure BDA0003624828780000064
the jacobian matrix of the position observation equation with respect to δ θ is derived as follows:
Figure BDA0003624828780000065
the position observation jacobian matrix is as follows:
Figure BDA0003624828780000066
(2) speed observation model
Deriving a velocity observation equation from the position observation equation:
Figure BDA0003624828780000067
Figure BDA0003624828780000068
vrtk＝vimu+R[ω]×δl
the jacobian matrix derivation of the velocity observation equation for δ θ is as follows:
Figure BDA0003624828780000069
the velocity observation jacobian matrix is as follows:
H3×15＝[O3×3 I3×3 -[R[ω]×δl]× O3×3 R[δl]×]
(3) course observation model
The double-antenna GNSS-RTK can output course observation information
Figure BDA00036248287800000610
Its mathematical model for quaternion q is as follows:
Figure BDA00036248287800000611
Figure BDA00036248287800000612
the present invention employs a rotation sequence of 312, with the IMU coordinate system selected as "top left front".
Heading observation equation
Figure BDA0003624828780000071
Jacobian matrix for quaternion q
Figure BDA0003624828780000072
(the sign operation can be performed here using matlab):
Figure BDA0003624828780000073
qt＝2q0q3-2q1q2
Figure BDA0003624828780000074
the Jacobian matrix of quaternions q with respect to δ θ:
Figure BDA0003624828780000075
then the course observation equation
Figure BDA0003624828780000076
Jacobian matrix for δ θ:
Figure BDA0003624828780000077
as shown in fig. 5 and 6, the reduction of the position observation accuracy in the occlusion scene may cause the drift of the height estimation, which may affect the working effect of the robot. Aiming at the problem, a laser radar local precise positioning technology is introduced, a point cloud ICP matching algorithm is operated at an industrial personal computer end, a pose with position covariance is output, meanwhile, initialization is carried out in an open scene, IMU (inertial measurement Unit), laser radar and positioning antenna external parameters calculated by a mechanical structure are combined, the point cloud ICP matching pose is converted to an ENU (northeast sky) coordinate system, and an initial value is provided. Therefore, the point cloud ICP matching pose and the GNSS-RTK can output ENU position observation of position covariance, and the position observation model is the same as the GNSS-RTK. When the shielding scene is faced, the covariance of the GNSS-RTK position is increased, the weight of the point cloud ICP matching pose is increased, and the fusion filter can still output high-precision stable pose output.
Fig. 4, 5, and 6 show graphs of comparing data before and after IMU/GNSS-RTK fusion pose trajectory in an open scene, IMU/GNSS-RTK/laser fusion pose trajectory in an occluded scene, and laser fusion in an occluded scene, respectively. Fig. 4 is a comparison diagram of a method of using a commercially mature integrated navigation product and a fusion pose trajectory diagram obtained by the method of the present invention in an open scene, arrows in fig. 4 which are mixed together correspond to the two methods, and the results in fig. 4 show that the live working robot based on the IMU/GNSS-RTK integrated positioning system works normally. The positioning accuracy of the present invention was evaluated using a commercially available integrated navigation product as a reference as shown in the following table.
TABLE 1
Figure BDA0003624828780000081
As can be seen from Table 1, the method of the present invention can realize centimeter-level high-precision positioning.
As can be seen in FIG. 5, the live working robot based on the IMU/GNSS-RTK/laser combined positioning system works normally. As can be seen in FIG. 6, in a sheltered scene, the GNSS-RTK height observed quantity drifts, and after the laser ICP matching pose is fused, the height state tends to be normal, so that the strong environment adaptive capacity of the filter is explained.
The above description is only of the preferred embodiments of the present invention, and it should be noted that: it will be apparent to those skilled in the art that various modifications and adaptations can be made without departing from the principles of the invention and these are intended to be within the scope of the invention.

Claims (8)

1. A high-precision positioning method is characterized by comprising the following steps:
obtaining IMU data, wherein the IMU data comprises a specific force a and a rotation angular velocity w, processing the IMU data by adopting a strapdown inertial navigation algorithm, and obtaining an integral prediction position prediction reference quantity
Figure FDA0003624828770000011
Reference quantity for speed prediction
Figure FDA0003624828770000012
And attitude prediction reference
Figure FDA0003624828770000013
Obtaining GNSS data comprising a current position observation of a target
Figure FDA0003624828770000014
Velocity observed quantity vrtkAnd course observation
Figure FDA0003624828770000015
Constructing an ESKF filter, inputting the predicted reference quantity and GNSS data into the ESKF filter, and outputting a position error delta p, a speed error delta v, an attitude error delta q and an acceleration deviation by the ESKF filter
Figure FDA0003624828770000016
Deviation of gyroscope
Figure FDA0003624828770000017
Determining the true position of an object
Figure FDA0003624828770000018
True velocity
Figure FDA0003624828770000019
And true attitude
Figure FDA00036248287700000110
2. The high-precision positioning method according to claim 1, further comprising the steps of:
acquiring laser radar data, wherein the laser radar data comprises a current three-dimensional point cloud of a target object and a corresponding covariance of the current three-dimensional point cloud;
and the ESKF filter corrects various prediction reference quantities of integral prediction according to the IMU data, the GNSS data and the laser radar data.
3. The high-precision positioning method according to claim 1, characterized in that: the ESKF filter comprises:
the position observation model is shown as formula (1):
Figure FDA00036248287700000111
wherein the content of the first and second substances,
Figure FDA00036248287700000112
represents the vector of the satellite positioning antenna in the IMU coordinate system,
Figure FDA00036248287700000113
representing the position pre-measurement of the inertial measurement unit IMU, and R representing the rotation of the IMU coordinate system relative to the ENU northeast coordinate system;
the speed observation model is shown as the formula (2):
vrtk＝vimu+R[ω]×δl
where ω represents a rotational angular velocity, [ ω ]]×An antisymmetric matrix representing a rotation angular velocity;
the course observation model of the attitude quaternion q is shown as the formula (3):
Figure FDA0003624828770000021
Figure FDA0003624828770000022
wherein q is1、q2、q0、q3Representing the real part and three imaginary parts of the attitude quaternion q,
Figure FDA0003624828770000023
indicating that the heading observations corresponding to the rotation order of 321 were taken,
Figure FDA0003624828770000024
indicating the heading observations corresponding to the rotation order of 312.
4. The high-precision positioning method according to claim 3, wherein obtaining an observation Jacobian matrix representing a relation between an observation quantity and a filter state quantity from the observation model includes:
the Jacobian matrix is observed at the position, as shown in the formula (4):
Figure FDA0003624828770000025
the velocity observation jacobian matrix is shown as formula (5):
H3×15＝[O3×3 I3×3 -[R[ω]×δl]× O3×3 R[δl]×] (5)
and (3) a course observation jacobian matrix, as shown in formula (6):
Figure FDA0003624828770000026
wherein δ θ represents an angle error, O3×3Denotes a 3 x 3 zero matrix, I3×33 x 3 identity matrix.
5. A high accuracy positioning apparatus, comprising:
the inertial measurement unit is used for acquiring IMU data, the IMU data comprises a specific force a and a rotation angular velocity w, and the IMU data is processed by adopting a strapdown inertial navigation algorithm to obtain an integral prediction position prediction reference quantity
Figure FDA0003624828770000027
Reference quantity for speed prediction
Figure FDA0003624828770000028
And attitude prediction reference
Figure FDA0003624828770000029
A GNSS-RTK positioning module to acquire GNSS data including a current position observation of a target
Figure FDA00036248287700000210
Velocity observed quantity vrtkAnd course observation
Figure FDA00036248287700000211
ESKF filter, inputting prediction reference quantity and GNSS data, and outputting position error delta after processingpVelocity error deltavAngle error deltaθAcceleration deviation
Figure FDA00036248287700000212
Deviation of gyroscope
Figure FDA00036248287700000213
Outputting the true position of the target
Figure FDA00036248287700000214
True velocity
Figure FDA00036248287700000215
And true attitude
Figure FDA00036248287700000216
6. The high-precision positioning device according to claim 5, further comprising a laser ICP matching pose resolving module for obtaining laser radar data and performing matching pose resolving on the laser radar data, wherein the laser radar data comprises current three-dimensional point cloud of a target object and corresponding covariance thereof;
and the ESKF filter corrects various predicted reference quantities of integral prediction according to the predicted reference quantity, the GNSS data and the laser radar data.
7. A live working robot positioning system which characterized in that: the intelligent communication system comprises a host, a communication box, an industrial personal computer and a sensor, wherein the host, the communication box and the sensor are all installed on an outdoor live working robot, the host integrates the IMU module, the GNSS module, the main control chip and the peripheral hardware interface according to claim 5, the ESKF filter according to claim 5 is arranged in the main control chip, and the communication box, the industrial personal computer and the sensor are all in communication connection with the host.
8. The live working robot positioning system according to claim 7, characterized in that: the industrial personal computer is in communication connection with the lidar module of claim 6.
CN202210474676.9A 2022-04-29 2022-04-29 High-precision positioning method and device and positioning system of hot-line work robot Pending CN114739404A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210474676.9A CN114739404A (en) 2022-04-29 2022-04-29 High-precision positioning method and device and positioning system of hot-line work robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210474676.9A CN114739404A (en) 2022-04-29 2022-04-29 High-precision positioning method and device and positioning system of hot-line work robot

Publications (1)

Publication Number Publication Date
CN114739404A true CN114739404A (en) 2022-07-12

Family

ID=82286035

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210474676.9A Pending CN114739404A (en) 2022-04-29 2022-04-29 High-precision positioning method and device and positioning system of hot-line work robot

Country Status (1)

Country Link
CN (1) CN114739404A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115793009A (en) * 2023-02-03 2023-03-14 中国人民解放***箭军工程大学 Multi-station passive positioning method based on high-precision Beidou combined measurement

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115793009A (en) * 2023-02-03 2023-03-14 中国人民解放***箭军工程大学 Multi-station passive positioning method based on high-precision Beidou combined measurement
CN115793009B (en) * 2023-02-03 2023-05-09 中国人民解放***箭军工程大学 Multi-station passive positioning method based on high-precision Beidou combined measurement

Similar Documents

Publication Publication Date Title
CN111156998B (en) Mobile robot positioning method based on RGB-D camera and IMU information fusion
CN108955688B (en) Method and system for positioning double-wheel differential mobile robot
Xiong et al. G-VIDO: A vehicle dynamics and intermittent GNSS-aided visual-inertial state estimator for autonomous driving
Langelaan State estimation for autonomous flight in cluttered environments
CN108362288B (en) Polarized light SLAM method based on unscented Kalman filtering
CN112987065B (en) Multi-sensor-integrated handheld SLAM device and control method thereof
CN108350679A (en) The perching knife automatic control system of motor-driven grader
CN111380514A (en) Robot position and posture estimation method and device, terminal and computer storage medium
CN108387236B (en) Polarized light SLAM method based on extended Kalman filtering
CN108759822B (en) Mobile robot 3D positioning system
CN110160545B (en) Enhanced positioning system and method for laser radar and GPS
KR20210040877A (en) Positioning method and device
CN110751123B (en) Monocular vision inertial odometer system and method
CN112967392A (en) Large-scale park mapping and positioning method based on multi-sensor contact
CN111426320A (en) Vehicle autonomous navigation method based on image matching/inertial navigation/milemeter
Jin et al. Fast and accurate initialization for monocular vision/INS/GNSS integrated system on land vehicle
CN116047565A (en) Multi-sensor data fusion positioning system
Karam et al. Integrating a low-cost mems imu into a laser-based slam for indoor mobile mapping
CN114739404A (en) High-precision positioning method and device and positioning system of hot-line work robot
Filip et al. LiDAR SLAM comparison in a featureless tunnel environment
CN113008229A (en) Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor
CN117387604A (en) Positioning and mapping method and system based on 4D millimeter wave radar and IMU fusion
Guanbei et al. LIDAR/IMU calibration based on ego-motion estimation
CN114397480B (en) Acoustic Doppler velocimeter error estimation method, device and system
CN109459769A (en) A kind of autonomic positioning method and system

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