CN115902978A - GNSS/SLAM combined navigation system pose initialization method - Google Patents

GNSS/SLAM combined navigation system pose initialization method Download PDF

Info

Publication number
CN115902978A
CN115902978A CN202310006232.7A CN202310006232A CN115902978A CN 115902978 A CN115902978 A CN 115902978A CN 202310006232 A CN202310006232 A CN 202310006232A CN 115902978 A CN115902978 A CN 115902978A
Authority
CN
China
Prior art keywords
gnss
slam
data
coordinate system
pose
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
CN202310006232.7A
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.)
Xiamen University
Original Assignee
Xiamen University
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 Xiamen University filed Critical Xiamen University
Priority to CN202310006232.7A priority Critical patent/CN115902978A/en
Publication of CN115902978A publication Critical patent/CN115902978A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

A GNSS/SLAM combined navigation system pose initialization method belongs to mobile robot navigation and positioning. Starting a GNSS and a SLAM system in a relatively open place; the mobile robot is operated to complete non-linear motion of not less than 10m, and longitude and latitude height data of GNSS and position and attitude data of SLAM in the moving process of the robot are collected; according to the timestamp of the collected SLAM data, performing linear interpolation on the longitude and latitude height data of the GNSS to obtain synchronous GNSS longitude and latitude height data; converting the GNSS data into a northeast coordinate system to be expressed by using the first data as a reference to obtain 3-dimensional track data of the robot; solving an optimization problem based on the lie group optimization theory: and aligning the SLAM track into the system according to the pose matrix obtained by solving, starting a GNSS/SLAM combined navigation fusion algorithm, and finishing initialization. The initialization precision of the integrated navigation system is effectively improved.

Description

GNSS/SLAM combined navigation system pose initialization method
Technical Field
The invention belongs to the technical field of navigation and positioning of mobile robots, and particularly relates to a GNSS/SLAM combined navigation system pose initialization method based on lie group optimization.
Background
A Global Navigation Satellite System (GNSS) is a Navigation and positioning method based on a Navigation Satellite and serving the world, can provide absolute positioning information of all weather, all time domains and no accumulated error, and is widely applied to various outdoor mobile robot Navigation tasks. The defect is that the robot is easy to be shielded by buildings and cannot provide reliable positioning information for indoor mobile robots.
Meanwhile, the positioning and Mapping technology (SLAM) is a positioning system constructed based on external sensing sensors such as laser radar and cameras, estimates the position and attitude (combined pose) of a mobile robot and estimates a map of the surrounding environment at the same time, can be widely applied to indoor and outdoor navigation tasks, and has the defect that the positioning has accumulated errors and cannot be applied to large-range navigation tasks.
The GNSS and SLAM have obvious complementarity, and researchers provide a GNSS/SLAM combined navigation system, so that the requirements of the mobile robot on large-range and high-precision navigation and positioning are met. Spatial unification is the basis of a combined navigation system, and therefore, the world coordinate system of the SLAM needs to be aligned with the northeast coordinate system of the GNSS through an initialization process before combined positioning.
There are two common initialization methods: the initial pose is given by external signals, and the initial pose is solved by singular value decomposition, but the methods still have the following defects: (1) The former relies heavily on external devices, such as a dual antenna RTK positioning and orientation system, and thus the range of use of this approach is very limited. (2) The latter uses Singular Value Decomposition (SVD) analysis to solve for the rotation and translation vectors between the two trajectories, but this approach ignores the boom vectors between the SLAM sensor coordinate system and the GNSS receiver coordinate system, which may deteriorate the accuracy of the initial pose. (3) In addition, the latter also assumes that the positioning values of GNSS and SLAM have the same uncertainty in the three dimensions X, Y, Z, which is inconsistent with the fact that they have different measurement accuracies in different dimensions.
Disclosure of Invention
The invention aims to solve the problems that the initial accuracy of the position posture of a GNSS/SLAM integrated navigation system is low and the like, and provides a GNSS/SLAM integrated navigation system position posture initialization method which is based on lie group optimization and takes a GNSS arm lever vector into consideration.
The invention comprises the following steps:
s1: the operation robot moves to a relatively open place, and simultaneously starts a GNSS positioning system and an SLAM positioning system;
s2: the mobile robot is operated to complete non-linear motion of not less than 10m, and longitude and latitude height data of GNSS and position and attitude data of SLAM in the moving process of the robot are collected;
s3: performing linear interpolation on the GNSS longitude and latitude height data according to the acquired time stamp of the SLAM data to obtain synchronous GNSS longitude and latitude height data; initializing to obtain a northeast coordinate system of the GNSS by using first data of the synchronous GNSS, and converting the remaining longitude and latitude height data of the synchronous GNSS into the northeast coordinate system for rectangular coordinate representation to obtain 3-dimensional track data of the robot;
s4: solving an optimization problem based on the lie group optimization theory:
Figure BDA0004036920500000021
where i =1, \8230;, N denotes the number of data, E denotes the northeast coordinate system of GNSS, W denotes the world coordinate system of SLAM, B denotes the SLAM sensor coordinate system, G denotes the coordinate system of GNSS receiver,
Figure BDA0004036920500000022
represents a residual, is>
Figure BDA0004036920500000023
Covariance matrix representing the correspondence of residuals, <' > based on>
Figure BDA0004036920500000024
A pose matrix representing W relative to E (SE (3) represents a 6-dimensional matrix of lie groups corresponding to the transformation matrix), -and>
Figure BDA0004036920500000025
b representing SLAM output i Relative to the pose matrix of W->
Figure BDA0004036920500000026
A position vector (which can be specified in advance and is set to a known quantity) which represents G relative to B>
Figure BDA0004036920500000027
G measured for GNSS i Position vector relative to E. The solution process for this problem is as follows:
s4.1 will
Figure BDA0004036920500000028
Initializing the pose matrix obtained by a 4-dimensional unit matrix or SVD method;
s4.2 calculating residual terms: f. of i ，i＝1，…，N
S4.3, calculating a Jacobian matrix:
Figure BDA0004036920500000029
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA00040369205000000210
s4.4 solving the incremental equation:
Figure BDA00040369205000000211
s4.5, updating the variable to be optimized by using the lie algebra-to-lie group exponential mapping:
Figure BDA0004036920500000031
s4.6 terminating the iteration if: (1) Xi * Less than a certain thresholdWhen the value is positive; (2) when the number of iterations exceeds a certain threshold; (3) when the cost function does not decrease but increases; otherwise, repeating S4.2-S4.5 until the condition of terminating iteration is met, and outputting the current estimated value as the optimal solution of the lie group optimization problem.
S5, obtaining a pose matrix according to the solution
Figure BDA0004036920500000032
And aligning the SLAM track to the E system, starting a GNSS/SLAM combined navigation fusion algorithm, completing initialization, and starting a formal combined navigation task.
According to the invention, the covariance matrix of the arm lever vector and the measured value of the corresponding SLAM sensor of the GNSS receiver is considered at the same time, so that the initialization precision of the combined navigation system can be effectively improved, and the problem of low initial precision of the pose of the GNSS/SLAM combined navigation system is solved.
Drawings
FIG. 1 is a schematic flow diagram of the process of the present invention;
FIG. 2 is a schematic flow chart of the step of optimizing the plum population in the method of the present invention;
FIG. 3 is a diagram comparing the trajectories before and after the initialization of the GNSS/SLAM integrated navigation system.
Detailed Description
In order that the above objects, features and advantages of the present invention can be more clearly understood, a more particular description of the invention will be rendered by reference to the appended drawings. It should be noted that the embodiments of the present invention and features of the embodiments may be combined with each other without conflict. In the following description, numerous specific details are set forth in order to provide a thorough understanding of the present invention, however, the present invention may be practiced in other ways than those specifically described herein, and therefore the scope of the present invention is not limited by the specific embodiments disclosed below.
In the conventional pose initialization method based on SVD, the core method is to solve the following optimization problem analytically by using SVD:
Figure BDA0004036920500000033
obviously, in this method, the GNSS receiver to SLAM sensor ground-arm vector is ignored
Figure BDA0004036920500000034
And the residual g is not determined according to the positioning precision of GNSS and SLAM i A covariance matrix is set.
As shown in FIG. 1, the initialization of the GNSS/SLAM integrated navigation system pose by the present invention comprises the following steps:
s0: respectively configuring a GNSS positioning system, an SLAM positioning system and a combined navigation positioning algorithm thereof on the mobile robot; after being electrified, the GNSS positioning system receives satellite information and resolves the satellite information to obtain single-point positioning data, wherein the single-point positioning data usually comprises longitude, latitude, altitude, corresponding uncertainty and other data; the SLAM positioning system obtains corresponding sensing data from the SLAM sensor, and estimates the position, the posture, the corresponding uncertainty and other data of the current robot relative to a world coordinate system.
S1: in order to ensure that the GNSS positioning system works normally in the initial stage and reduce the interference of building shielding and multipath effect as much as possible, the operation robot moves to an open place and respectively starts the GNSS positioning system and the SLAM positioning system.
S2: the method comprises the steps of operating the mobile robot to complete non-linear motion of not less than 10m, and collecting positioning data of GNSS (global navigation satellite system) and positioning data of SLAM (SLAM) in the moving process of the robot, wherein the data of the GNSS generally comprises a timestamp, longitude, latitude, altitude, uncertainty of the GNSS and the like, and the SLAM data generally comprises a timestamp, a pose, uncertainty of the pose and the like.
S3: and performing linear interpolation on the GNSS longitude and latitude height data according to the acquired time stamp of the SLAM data to obtain synchronous GNSS longitude and latitude height data. And then, initializing to obtain a northeast coordinate system of the GNSS by using the first data of the synchronous GNSS, and converting the residual synchronous GNSS longitude and latitude height data into the northeast coordinate system to obtain 3-dimensional track data of the robot.
S4: based on the lie group optimization theory, solving an optimization problem:
Figure BDA0004036920500000041
where i =1, \8230;, N denotes the number of data, E denotes the northeast coordinate system of GNSS, W denotes the world coordinate system of SLAM, B denotes the SLAM sensor coordinate system, G denotes the coordinate system of GNSS receiver,
Figure BDA0004036920500000042
represents a residual, is>
Figure BDA0004036920500000043
Represents the covariance matrix corresponding to the residual, and->
Figure BDA0004036920500000044
A pose matrix representing W relative to E (SE (3) represents a 6-dimensional matrix of lie groups corresponding to the transformation matrix), -and>
Figure BDA0004036920500000045
b representing SLAM output i Relative to the pose matrix of W>
Figure BDA0004036920500000046
Represents a position vector (which can be pre-calibrated and set to a known quantity) of G relative to B, and/or>
Figure BDA0004036920500000047
G measured for GNSS i Position vector relative to E. As shown in fig. 2, the solving process of the lie group optimization problem is as follows:
s4.1 will
Figure BDA0004036920500000048
Initializing the pose matrix obtained by a 4-dimensional unit matrix or SVD method;
s4.2 calculating residual terms: f. of i ，i＝1，…，N；
Figure BDA0004036920500000049
S4.3, calculating a Jacobian matrix:
Figure BDA0004036920500000051
wherein:
Figure BDA0004036920500000052
s4.4 solving the incremental equation:
Figure BDA0004036920500000053
the calculation formula of the covariance matrix is as follows:
Figure BDA0004036920500000054
here, the first and second liquid crystal display panels are,
Figure BDA0004036920500000055
represents a positioning uncertainty of the GNSS->
Figure BDA0004036920500000056
Representing SLAM's pose estimation uncertainty, matrix A i The calculation formula of (c) is as follows:
Figure BDA0004036920500000057
s4.5, updating the variables to be optimized by using the solution of the incremental equation:
Figure BDA0004036920500000058
wherein lie algebra to lie group index maps exp (ξ) * ^) is defined as:
Figure BDA0004036920500000059
s4.6 terminating the iteration if: (1) Xi * Less than a certain threshold, e.g. 10 -6 (ii) a (2) when the number of iterations exceeds a certain threshold, e.g., 50; (3) when the cost function does not decrease but increases; otherwise, repeating S4.2, S4.3 and S4.5 until the condition of terminating iteration is met, and outputting the current estimated value as the optimal solution of the lie group optimization problem.
S5, obtaining a pose matrix according to the solution
Figure BDA00040369205000000510
And aligning the SLAM track to the E system, starting the GNSS/SLAM combined navigation and then algorithm, finishing initialization and starting a formal combined navigation task.
In order to verify the technical solutions and the advantages of the present invention, the technical solutions are described below with a specific embodiment. As shown in fig. 3, a solid line represents a motion trajectory of the robot acquired by GNSS, a dotted line represents a motion trajectory of the robot estimated by SLAM before initialization, a dotted line represents a motion trajectory given by SLAM after initialization using a conventional SVD method, and a dashed line represents a motion trajectory given by SLAM after initialization using the method of the present invention. As can be seen from fig. 3, the initialized trajectory of the present invention is closer to the GNSS trajectory, and the statistical analysis is performed on the error of each sampling point, and the result is shown in table 1:
TABLE 1 Absolute trajectory error statistics table (unit: meter)
Figure BDA0004036920500000061
In conclusion, the method effectively considers the arm error and the measurement uncertainty, and obviously improves the precision of the conventional SVD initialization method.

Claims (2)

1. A GNSS/SLAM combined navigation system pose initialization method is characterized by comprising the following steps:
s1: the operation robot moves to a relatively open place, and simultaneously starts a GNSS positioning system and an SLAM positioning system;
s2: the mobile robot is operated to complete non-linear motion of not less than 10m, and meanwhile longitude and latitude height data of GNSS and position data of SLAM in the moving process of the robot are collected;
s3: performing linear interpolation on the GNSS longitude and latitude height data according to the acquired time stamp of the SLAM data to obtain synchronous GNSS longitude and latitude height data; initializing to obtain a northeast coordinate system of the GNSS by using first data of the synchronous GNSS, and converting the remaining longitude and latitude height data of the synchronous GNSS into the northeast coordinate system for rectangular coordinate representation to obtain 3-dimensional track data of the robot;
s4: solving an optimization problem based on the lie group optimization theory:
Figure FDA0004036920490000011
where i =1, \ 8230;, N denotes the number of data, E denotes the northeast coordinate system of the GNSS, W denotes the world coordinate system of the SLAM, B denotes the SLAM sensor coordinate system, G denotes the coordinate system of the GNSS receiver,
Figure FDA0004036920490000012
represents a residual->
Figure FDA0004036920490000013
Represents the covariance matrix corresponding to the residual, and->
Figure FDA0004036920490000014
Representing the pose matrix of W relative to E, SE (3) representing a pair of transformation matricesThe corresponding 6-dimensional matrix lie group>
Figure FDA0004036920490000015
B representing SLAM output i Relative to the pose matrix of W->
Figure FDA0004036920490000016
Represents the position vector of G relative to B>
Figure FDA0004036920490000017
G measured for GNSS i A position vector relative to E;
s5: obtaining a pose matrix according to the solution
Figure FDA0004036920490000018
And aligning the SLAM track to the northeast coordinate system, starting the GNSS/SLAM combined navigation fusion algorithm, completing initialization, and starting a formal combined navigation task.
2. The method for initializing the pose of the GNSS/SLAM integrated navigation system according to claim 1, wherein in step S4, the specific process for solving the optimization problem based on the lie group optimization theory is as follows:
s4.1 will
Figure FDA0004036920490000019
Initializing the pose matrix obtained by a 4-dimensional unit matrix or SVD method;
s4.2 calculating residual terms: f. of i ，i＝1，…，N；
S4.3, calculating a Jacobian matrix:
Figure FDA00040369204900000110
wherein:
Figure FDA0004036920490000021
s4.4 solving the incremental equation:
Figure FDA0004036920490000022
s4.5, updating the variable to be optimized by using the lie algebra-to-lie group exponential mapping:
Figure FDA0004036920490000023
s4.6 terminating the iteration if: (1) Xi * Less than a threshold; (2) when the number of iterations exceeds a certain threshold; (3) when the cost function does not decrease but increases; otherwise, repeating S4.2-S4.5 until the condition of terminating iteration is satisfied, and outputting the current estimation value as the optimal solution of the lie group optimization problem.
CN202310006232.7A 2023-01-04 2023-01-04 GNSS/SLAM combined navigation system pose initialization method Pending CN115902978A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310006232.7A CN115902978A (en) 2023-01-04 2023-01-04 GNSS/SLAM combined navigation system pose initialization method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310006232.7A CN115902978A (en) 2023-01-04 2023-01-04 GNSS/SLAM combined navigation system pose initialization method

Publications (1)

Publication Number Publication Date
CN115902978A true CN115902978A (en) 2023-04-04

Family

ID=86495688

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310006232.7A Pending CN115902978A (en) 2023-01-04 2023-01-04 GNSS/SLAM combined navigation system pose initialization method

Country Status (1)

Country Link
CN (1) CN115902978A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112819744A (en) * 2021-02-26 2021-05-18 中国人民解放军93114部队 GNSS and visual SLAM fused track measuring method and device

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112819744A (en) * 2021-02-26 2021-05-18 中国人民解放军93114部队 GNSS and visual SLAM fused track measuring method and device

Similar Documents

Publication Publication Date Title
Xiong et al. A robust single GPS navigation and positioning algorithm based on strong tracking filtering
CN107255795B (en) Indoor mobile robot positioning method and device based on EKF/EFIR hybrid filtering
CN103471595B (en) A kind of iteration expansion RTS mean filter method towards the navigation of INS/WSN indoor mobile robot tight integration
CN110823217A (en) Integrated navigation fault-tolerant method based on self-adaptive federal strong tracking filtering
CN105509739A (en) Tightly coupled INS/UWB integrated navigation system and method adopting fixed-interval CRTS smoothing
CN108592945A (en) Online calibration method for errors of inertia/astronomical combination system
CN111337020A (en) Factor graph fusion positioning method introducing robust estimation
CN108387236B (en) Polarized light SLAM method based on extended Kalman filtering
CN109141413B (en) EFIR filtering algorithm and system with data missing UWB pedestrian positioning
CN110702091A (en) High-precision positioning method for moving robot along subway rail
CN108362288A (en) Polarized light S L AM method based on unscented Kalman filtering
CN109931955A (en) Strapdown inertial navigation system Initial Alignment Method based on the filtering of state correlation Lie group
CN109507706B (en) GPS signal loss prediction positioning method
CN110657806B (en) Position resolving method based on CKF, chan resolving and Savitzky-Golay smooth filtering
CN115902978A (en) GNSS/SLAM combined navigation system pose initialization method
CN109141412B (en) UFIR filtering algorithm and system for data-missing INS/UWB combined pedestrian navigation
CN112797985A (en) Indoor positioning method and indoor positioning system based on weighted extended Kalman filtering
CN112904396A (en) High-precision positioning method and system based on multi-sensor fusion
CN113900061A (en) Navigation positioning system and method based on UWB wireless positioning and IMU fusion
CN111964667A (en) geomagnetic-INS (inertial navigation System) integrated navigation method based on particle filter algorithm
CN109655060B (en) INS/UWB integrated navigation algorithm and system based on KF/FIR and LS-SVM fusion
EP4143507B1 (en) Navigation apparatus and method in which measurement quantization errors are modeled as states
CN114690229A (en) GPS-fused mobile robot visual inertial navigation method
CN114608568A (en) Multi-sensor-based information instant fusion positioning method
CN111444467B (en) Method for local linear interpolation and prediction based on real-time positioning track data

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