An unmanned system multi-source information fusion positioning system

Abstract

This invention provides a multi-source information fusion positioning system for unmanned systems, including a GNSS positioning information module, a GNSS positioning fusion input information processing module, an odometer positioning fusion input information processing module, a positioning fusion module, and a fused positioning information output module. It is used to solve the problems of positioning information errors caused by GNSS signal interference or obstruction under complex working conditions, as well as the increasing errors of odometer and inertial navigation positioning information over time, and to ensure the positioning accuracy and stability of unmanned systems under complex working conditions.

Description

Technical Field

[0001] This invention relates to the field of sensor information fusion, and more specifically to a multi-source information fusion positioning system for unmanned systems. Background Technology

[0002] Precise positioning is a crucial prerequisite for the successful operation of unmanned systems. Common positioning methods include Global Navigation Satellite System (GNSS) positioning, wheeled odometer positioning, inertial navigation systems (INS), lidar positioning, visual positioning, and V2X-based positioning. GNSS information yields high-precision position information through differential calculations, but it is highly dependent on the surrounding environment and susceptible to external interference. Wheeled odometers and INS offer advantages such as high-precision positioning over short periods, independence from external information, immunity to external environmental interference, and adaptability to various operating environments. However, the accuracy of INS decreases over time, resulting in poor long-term accuracy. Therefore, in practical applications of unmanned systems, multiple positioning methods are often used and fused to obtain more accurate positioning results. Patent application number [CN202110426739.9] proposes a "GNSS-INS visual fusion positioning method and system based on sequential Kalman filtering." This method obtains the pseudorange error and pseudorange error rate by subtracting the observed values ​​of pseudorange and pseudorange rate from the GNSS signal, and obtains the heading error by subtracting the heading increment provided by vision from the heading increment obtained by INS. Based on this information, a sequential Kalman filtering method is used to obtain error correction information, thereby improving positioning accuracy. Figure 1 As shown. Patent application number [CN202110082996.5] proposes a "pedestrian positioning method and system based on graph optimization". This method stores acquired GNSS information, inertial navigation information, and barometric altitude information in a data cache. One path of the cached data is filtered and fused using an EKF filter to obtain real-time positioning results. The other path uses historical data and a graph optimization method to further optimize and correct the EKF filter's filtered and fused positioning results, thereby further improving positioning accuracy. Figure 2 As shown.

[0003] All the above patents utilize multi-source positioning information fusion to improve positioning accuracy. However, the first method, using Kalman filtering for fusion positioning, is limited by the large linearization error of Kalman filtering, its neglect of historical constraint information, and its limitations in flexibly adding or removing nodes. These issues prevent the achievement of more accurate positioning under complex working conditions. The second method, while using EKF filtering for fusion positioning, also corrects the EKF filtering fusion positioning using least-squares position optimization, solving the problem that the EKF filtering method cannot utilize historical positioning information and greatly improving positioning accuracy under complex working conditions.

[0004] However, neither of the above methods assesses the quality of GNSS positioning information. Under complex operating conditions, unmanned systems often need to navigate through large obstructions such as buildings. When moving near such obstructions, the obstruction can weaken the GNSS signal, leading to a significant deterioration in GNSS quality and a substantial reduction in positioning accuracy. If this poor-quality GNSS positioning information is used in fusion positioning, it will cause serious positioning errors in the unmanned system.

[0005] Meanwhile, the second method does not correct the inertial navigation information. Under complex working conditions, due to interference from the external environment (such as slippery road surface causing skidding, uneven road surface, or tilted road surface), the positioning information of the odometer and inertial navigation often deviates. If these deviations are not corrected in time, the deviations will continue to increase over time, causing the residuals to fail to converge during the optimization process, resulting in the failure of least squares position optimization. Summary of the Invention

[0006] This invention provides a multi-source information fusion positioning system for unmanned systems under complex working conditions. It aims to solve the problems of positioning errors caused by interference or obstruction of GNSS signals under complex working conditions, as well as the increasing errors of odometer and inertial navigation positioning information over time, and to ensure the positioning accuracy and stability of unmanned systems under complex working conditions.

[0007] According to a first aspect of the present invention, a multi-source information fusion positioning system for unmanned systems is provided, characterized in that it comprises:

[0008] The GNSS positioning information module is used to acquire HEADINGA and GNGGA information from GNSS signals and send them to the GNSS positioning fusion input information processing module.

[0009] The GNSS positioning fusion input information processing module is used for GNSS information quality judgment. It includes: synchronizing HEADINGA information and GNGGA information through timestamps, and obtaining six state variables from HEADINGA information: number of satellites used, baseline length, solution status, position type, and heading angle. It uses the number of satellites, baseline length, solution status, and position type to judge the quality of GNGGA information at that moment. It combines GNGGA information with heading angle state variables to form satellite pose information and saves it into a cache queue.

[0010] The odometer positioning fusion input information processing module is used to correct the offset of the positioning pose data, use the corrected odometer pose data as the odometer pose information of the graph optimization model, and use the odometer pose data and the corrected odometer pose data at the next time step to update the rotation matrix of the odometer pose and the corrected odometer pose, and use this matrix as the odometer correction rotation matrix.

[0011] The positioning fusion module is used to fuse satellite pose information and odometry pose information. It includes: obtaining historical satellite pose information and odometry pose information from the cache queue and synchronizing the two types of pose information in time; optimizing and correcting the synchronized historical satellite pose information and odometry pose information using a graph optimization method to obtain optimized pose information; updating the rotation matrix of the unoptimized pose information and the optimized pose information using the optimized pose information and the unoptimized pose information; and using this matrix as the graph optimization fusion rotation matrix.

[0012] The fusion positioning information output module is used to calculate and output fusion positioning information, including: obtaining the odometer correction rotation matrix and the graph optimization fusion rotation matrix, calculating the product of the two to obtain the final fusion positioning rotation matrix, and outputting the matrix as the final fusion result.

[0013] Furthermore, the unmanned system multi-source information fusion positioning system provided by the present invention is characterized in that the GNSS positioning information module further includes:

[0014] Obtain the latitude and longitude, GPS quality information, horizontal accuracy factor, and antenna altitude contained in the GNGGA information; fill this information into the ROS sensor_msgs / NavSatFix message sat; sat.header.stamp represents the timestamp storing the current time, sat.status.status represents the GPS quality information, sat.latitude represents the latitude information, sat.longitude represents the longitude information, sat.altitude represents the antenna altitude, and sat.position_covariance[0,4] represents the horizontal accuracy factor. After filling, send the sat information to the GPS positioning fusion input information processing module.

[0015] The HEADINGA information (received at 1Hz) contains the solution status and location type, number of satellites used, heading angle, and baseline length. This information is then filled into the ROS geometry_msgs::QuaternionStamped message qua. qua.quaternion.x represents the solution status and location type; x = 1 indicates the solution status is SOL_COMPUTED and the location type is NARROW_INT, while x = 0 for other states. qua.quaternion.y represents the number of satellites used, qua.quaternion.z represents the heading angle, and qua.quaternion.w represents the baseline length. After filling in the information, the qua information is sent to the GPS positioning fusion input information processing module.

[0016] Furthermore, the unmanned system multi-source information fusion positioning system provided by the present invention is characterized in that the GNSS positioning fusion input information processing module further includes:

[0017] This module is responsible for converting pose data in the Earth coordinate system into pose data in the ENU coordinate system, and judging the quality of the pose information based on information such as the number of satellites and baseline length, and selecting high-quality pose data as the first localization fusion data for the graph optimization model.

[0018] Obtain the SAT and qua information sent by the GNSS positioning information module; first, determine whether the timestamps of the SAT and qua information are the same. If the timestamps are the same, it means that the SAT and qua information are positioning information at the same time; after obtaining the SAT and qua information at the same time, create a ROS nav_msgs / Odometry information gnssodom; convert the latitude and longitude of the SAT information into ENU coordinate system position information using the forward method of GeographicLib type and fill it into gnssodom.pose.pose.position; convert the heading angle in the qua information into attitude four-element information using the createQuaternionFromYaw method of tf library and fill it into gnssodom.pose.pose.orientation. At this time, initialize gnssodom.pose.covariance[0] to 0.01, which represents the weight of the position data of gnssodom during fusion. The smaller the value, the greater the weight. Initialize gnssodom.twist.covariance[0] to 0.05, which represents the weight of the attitude data of gnssodom during fusion. The smaller the value, the greater the weight.

[0019] After obtaining the gnssodom information, it is determined whether the quality of the positioning data is good or bad, ensuring that the gnss positioning data entering the fusion positioning is accurate. The specific process is as follows:

[0020] (1) If qua.quaternion.x < 0.8 | | qua.quaternion.y < 10 is true, it means that the positioning data quality is very poor. Discard the gnssodom directly, set stable = 0, and then exit the judgment directly without performing any other operations. Wait for the next sat and qua information. If it is false, go to step 2. The stable parameter is the waiting time when the gnssodom information recovers from poor quality to good quality and remains good until the next gnssodom information is obtained.

[0021] (2) If qua.quaternion.y < 15 is true, proceed to step 3; if it is false, proceed to step 4.

[0022] (3) If 0.6 < qua.quaternion.w < 0.65 is true, it means that the positioning accuracy of gnssodom is generally low when the number of satellites is small. At this time, reduce the weight of gnssodom in the fusion positioning, set gnssodom.pose.covariance[0] to 5 and gnssodom.twist.covariance[0] to 0.1, and then proceed to step 4; if it is false, exit directly and do not perform any other operations, and wait for the next SAT and qua information.

[0023] (4) Increase stable by 1, and then proceed to step 5.

[0024] (5) If qua.quaternion.y > 18 is true, it means that the quality of the gnssodom information is very good at this time. Increase stable by 4 to obtain the gnssodom information as soon as possible, and then proceed to step 6; if it is false, proceed directly to step 6.

[0025] (6) If stable < 60 is true, it means that the quality of the gnssodom information is good but has not been maintained for a certain period of time. Exit the judgment directly and do not perform any other operations. Wait for the next sat and qua information. If it is false, proceed to step 7.

[0026] (7) The gnssodom information at this moment can be used as the satellite pose information of the fusion positioning map optimization model.

[0027] When the quality of the gnssodom information is poor, the unmanned system uses only odometry data for map optimization to remove the interference of erroneous gnssodom positioning information on the positioning accuracy of the unmanned system.

[0028] Furthermore, the multi-source information fusion positioning system for unmanned systems provided by the present invention is characterized in that the odometer positioning fusion input information processing module further includes:

[0029] After receiving the odom information sent by the unmanned system chassis, it is marked as curOdom. The position difference is calculated by comparing it with the odom information lastOdom received at the previous moment to obtain the displacement data of the unmanned system during this time. The calculation formula is as follows:

[0030] ;

[0031] This represents the odometer pose data at time i. Due to the offset of the odometer positioning data, the calculated value is affected. The actual displacement (x, y, z) is therefore reduced. Adding the displacement value to the corrected pose data at time i, we can obtain the corrected pose data at time i+1. Therefore, the formula for calculating the optimized pose data at the current time is as follows:

[0032] ;

[0033] The odometer pose correction data at time i is given, and Scaler is the correction parameter value (Scaler < 1). By reducing the calculated displacement, the offset of the odometer and inertial navigation fusion positioning data is corrected, resulting in... Converting to pose data (odom) yields more accurate odometry positioning data. Using odom as the odometry pose information in the graph optimization model, the rotation matrix between the odometry pose and the corrected odometry pose is updated using the odometry pose data and the corrected odometry pose data at time i+1. This matrix is ​​then used as the odometry correction rotation matrix.

[0034] ;

[0035] in For odometer calibration rotation matrix, The inverse of the odometry pose at time i. Let represent the odometer pose matrix at time i.

[0036] Furthermore, the unmanned system multi-source information fusion positioning system provided by the present invention is characterized in that the positioning fusion module further includes:

[0037] When the amount of data in the cache queue reaches 20, the satellite pose information gnssodom and the odometry pose information odom are obtained. The timestamp gnssodom.header.stamp and the timestamp odom.header.stamp are compared. If they are equal, gnssodom and odom are sent into the graph optimization model for fusion positioning.

[0038] In the graph optimization model, the odometry pose information at each time step is optimized using the following formula:

[0039] ;

[0040] This represents the rotation matrix that transforms the pose at time t to the pose at time t+1. This represents the pose difference matrix between time t and time t+1. This represents the weight matrix of the pose at time t.

[0041] The odometry pose information is optimized using the GPS pose information at each moment using the following formula:

[0042] ;

[0043] ;

[0044] The GPS pose matrix at time t. The pose matrix representing the odometry at time t. This represents the weight matrix of the GPS pose at time t. This represents the loss function.

[0045] After graph optimization calculation, the fused pose information is obtained. The rotation matrix is ​​updated using the odometry pose at the last moment and the fused pose information. The calculation formula is as follows:

[0046] ;

[0047] The rotation matrix represents the transformation of the odometry pose matrix into the fused pose matrix. The inverse of the odometry pose matrix, This represents the pose matrix of the fusion.

[0048] Will The rotation matrix is ​​sent as a graph optimization fusion matrix to the fusion positioning information output module.

[0049] Furthermore, the multi-source information fusion positioning system for unmanned systems provided by the present invention is characterized in that the fused positioning information output module further includes:

[0050] This module obtains the odometer correction rotation matrix. Fusion rotation matrix with graph optimization Calculate the unified transformation relation matrix :

[0051] ;

[0052] Will The final output of the multi-source fusion localization method is the fusion rotation matrix.

[0053] Compared with existing technologies, the technical solution conceived in this invention has at least the following beneficial effects: After aligning the position information collected by different sensors of the unmanned system according to time, this invention maps all position data to a directed graph, and uses the least squares principle to perform Schur decomposition iteration to obtain the error and best estimate of each data point, thereby obtaining the best positioning estimate at each moment. Furthermore, since the non-Gaussian error of each sensor leads to slow iteration speed, increased computational overhead, and rapid entrapment in local optima, this method also verifies and corrects GNSS, odometry, inertial unit, and visual sensor data through prior knowledge, ensuring that the data entering the iteration satisfies a Gaussian distribution, allowing the residuals to converge smoothly during the iteration process, and guaranteeing the positioning stability and accuracy during the operation of the unmanned system.

[0054] It should be understood that the above general description and the following detailed description are exemplary and explanatory only, and are not intended to limit the invention. Attached Figure Description

[0055] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with the invention and, together with the description, serve to explain the principles of the invention.

[0056] Figure 1 It is a GNSSINS visual fusion positioning system based on sequential Kalman filtering in the existing technology.

[0057] Figure 2 It is a pedestrian localization system based on graph optimization in the existing technology.

[0058] Figure 3 This is a schematic diagram of a multi-source information fusion positioning system module for an unmanned system, according to an exemplary embodiment.

[0059] Figure 4 This is a GNSS positioning information module illustrated according to an exemplary embodiment.

[0060] Figure 5 This is a GNSS positioning fusion input information processing module illustrated according to an exemplary embodiment.

[0061] Figure 6 This is an odometer positioning fusion input information processing module illustrated according to an exemplary embodiment.

[0062] Figure 7 This is a positioning fusion module illustrated according to an exemplary embodiment.

[0063] Figure 8 This is a fusion positioning information output module illustrated according to an exemplary embodiment. Detailed Implementation

[0064] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0065] The detailed technical solution of this invention is as follows: Figure 3 The following are included:

[0066] The GNSS positioning information module is used to acquire HEADINGA and GNGGA information from GNSS signals and send them to the GNSS positioning fusion input information processing module.

[0067] The GNSS positioning fusion input information processing module is used for GNSS information quality judgment, including: synchronizing HEADINGA information and GNGGA information through timestamps, and obtaining six state variables from HEADINGA information: number of satellites used, baseline length, solution status, position type, and heading angle. The quality of GNGGA information at that moment is judged by using the number of satellites, baseline length, solution status, and position type. The satellite pose information is composed of GNGGA information and heading angle state variables and stored in the cache queue.

[0068] The odometer positioning fusion input information processing module is used to correct the offset of the positioning pose data, use the corrected odometer pose data as the odometer pose information of the graph optimization model, and use the odometer pose data and corrected odometer pose data at the next time step to update the rotation matrix of the odometer pose and the corrected odometer pose, and use this matrix as the odometer correction rotation matrix.

[0069] The positioning fusion module is used to fuse satellite pose information and odometry pose information. It includes: obtaining historical satellite pose information and odometry pose information from the cache queue and synchronizing the two types of pose information in time; optimizing and correcting the synchronized historical satellite pose information and odometry pose information using a graph optimization method to obtain optimized pose information; updating the rotation matrix of unoptimized pose information and optimized pose information through optimized pose information and unoptimized pose information; and using this matrix as the graph optimization fusion rotation matrix.

[0070] The fusion positioning information output module is used to calculate and output fusion positioning information, including: obtaining the odometer correction rotation matrix and the graph optimization fusion rotation matrix, calculating the product of the two to obtain the final fusion positioning rotation matrix, and outputting the matrix as the final fusion result.

[0071] This invention first uses HEADINGA information to filter high-quality GNGGA information, solving the problem that weak GNSS signals under complex operating conditions lead to the use of erroneous GNGGA positioning information in fusion positioning. This ensures that the GNGGA information entering the fusion positioning is acquired after the GNSS signal has been maintained for a certain period, eliminating interference from inaccurate GNGGA positioning information. By correcting the displacement distance through a hyperparameter scaler, an odometer data offset correction method is implemented, thus solving the problem that under complex operating conditions, the odometer information deviation increases over time, causing map optimization to fail to converge. Through these methods, accurate positioning of unmanned systems under complex operating conditions is guaranteed.

[0072] Specifically, the content of each module includes:

[0073] GNSS positioning information module such as Figure 4 As shown:

[0074] Obtain the latitude and longitude, GPS quality information, horizontal accuracy factor, and antenna altitude contained in the GNGGA information; fill this information into the ROS sensor_msgs / NavSatFix message sat; sat.header.stamp represents the timestamp storing the current time, sat.status.status represents the GPS quality information, sat.latitude represents the latitude information, sat.longitude represents the longitude information, sat.altitude represents the antenna altitude, and sat.position_covariance[0,4] represents the horizontal accuracy factor. After filling, send the sat information to the GPS positioning fusion input information processing module.

[0075] The HEADINGA information (received at 1Hz) contains the solution status and location type, number of satellites used, heading angle, and baseline length. This information is then filled into the ROS geometry_msgs::QuaternionStamped message qua. qua.quaternion.x represents the solution status and location type; x = 1 indicates the solution status is SOL_COMPUTED and the location type is NARROW_INT, while x = 0 for other states. qua.quaternion.y represents the number of satellites used, qua.quaternion.z represents the heading angle, and qua.quaternion.w represents the baseline length. After filling in the information, the qua information is sent to the GPS positioning fusion input information processing module.

[0076] GNSS positioning fusion input information processing module, such as Figure 5 As shown:

[0077] This module is responsible for converting pose data in the Earth coordinate system into pose data in the ENU coordinate system, and judging the quality of the pose information based on information such as the number of satellites and baseline length, and selecting high-quality pose data as the first localization fusion data for the graph optimization model.

[0078] Obtain the SAT and qua information sent by the GNSS positioning information module; first, determine whether the timestamps of the SAT and qua information are the same. If the timestamps are the same, it means that the SAT and qua information are positioning information at the same time; after obtaining the SAT and qua information at the same time, create a ROS nav_msgs / Odometry information gnssodom; convert the latitude and longitude of the SAT information into ENU coordinate system position information using the forward method of GeographicLib type and fill it into gnssodom.pose.pose.position; convert the heading angle in the qua information into attitude four-element information using the createQuaternionFromYaw method of tf library and fill it into gnssodom.pose.pose.orientation. At this time, initialize gnssodom.pose.covariance[0] to 0.01, which represents the weight of the position data of gnssodom during fusion. The smaller the value, the greater the weight. Initialize gnssodom.twist.covariance[0] to 0.05, which represents the weight of the attitude data of gnssodom during fusion. The smaller the value, the greater the weight.

[0079] After obtaining the gnssodom information, it is determined whether the quality of the positioning data is good or bad, ensuring that the gnss positioning data entering the fusion positioning is accurate. The specific process is as follows:

[0080] (1) If qua.quaternion.x < 0.8 | | qua.quaternion.y < 10 is true, it means that the positioning data quality is very poor. Discard the gnssodom directly, set stable = 0, and then exit the judgment directly without performing any other operations. Wait for the next sat and qua information. If it is false, go to step 2. The stable parameter is the waiting time when the gnssodom information recovers from poor quality to good quality and remains good until the next gnssodom information is obtained.

[0081] (2) If qua.quaternion.y < 15 is true, proceed to step 3; if it is false, proceed to step 4.

[0082] (3) If 0.6 < qua.quaternion.w < 0.65 is true, it means that the positioning accuracy of gnssodom is generally low when the number of satellites is small. At this time, reduce the weight of gnssodom in the fusion positioning, set gnssodom.pose.covariance[0] to 5 and gnssodom.twist.covariance[0] to 0.1, and then proceed to step 4; if it is false, exit directly and do not perform any other operations, and wait for the next SAT and qua information.

[0083] (4) Increase stable by 1, and then proceed to step 5.

[0084] (5) If qua.quaternion.y > 18 is true, it means that the quality of the gnssodom information is very good at this time. Increase stable by 4 to obtain the gnssodom information as soon as possible, and then proceed to step 6; if it is false, proceed directly to step 6.

[0085] (6) If stable < 60 is true, it means that the quality of the gnssodom information is good but has not been maintained for a certain period of time. Exit the judgment directly and do not perform any other operations. Wait for the next sat and qua information. If it is false, proceed to step 7.

[0086] (7) The gnssodom information at this moment can be used as the satellite pose information of the fusion positioning map optimization model.

[0087] When the quality of the gnssodom information is poor, the unmanned system uses only odometry data for map optimization to remove the interference of erroneous gnssodom positioning information on the positioning accuracy of the unmanned system.

[0088] Odometer positioning fusion input information processing module, such as Figure 6 As shown:

[0089] This module is responsible for correcting the positioning data obtained from the fusion of odometry and inertial navigation systems from the unmanned system chassis. Since the positioning data is offset due to interference from the external environment, this module corrects the offset of the positioning pose data, uses the corrected odometry pose data as the odometry pose information in the graph optimization model, and updates the rotation matrix of the odometry pose and the corrected odometry pose using the odometry pose data at time i+1 and the corrected odometry pose data. This matrix is ​​then used as the odometry correction rotation matrix.

[0090] After receiving the odom information sent by the unmanned system chassis, it is marked as curOdom. The position difference is calculated by comparing it with the odom information lastOdom received at the previous moment to obtain the displacement data of the unmanned system during this time. The calculation formula is as follows:

[0091] ;

[0092] This represents the odometer pose data at time i. Due to the offset of the odometer positioning data, the calculated value is affected. The actual displacement (x, y, z) is therefore reduced. Adding the displacement value to the corrected pose data at time i, we can obtain the corrected pose data at time i+1. Therefore, the formula for calculating the optimized pose data at the current time is as follows:

[0093] ;

[0094] The odometer pose correction data at time i is given, and Scaler is the correction parameter value (Scaler < 1). By reducing the calculated displacement, the offset of the odometer and inertial navigation fusion positioning data is corrected, resulting in... Converting to pose data (odom) yields more accurate odometry positioning data. Using odom as the odometry pose information in the graph optimization model, the rotation matrix between the odometry pose and the corrected odometry pose is updated using the odometry pose data and the corrected odometry pose data at time i+1. This matrix is ​​then used as the odometry correction rotation matrix.

[0095] ;

[0096] in For odometer calibration rotation matrix, The inverse of the odometry pose at time i. Let represent the odometer pose matrix at time i.

[0097] Positioning fusion module, such as Figure 7 As shown:

[0098] When the amount of data in the cache queue reaches 20, the satellite pose information gnssodom and the odometry pose information odom are obtained. The timestamp gnssodom.header.stamp and the timestamp odom.header.stamp are compared. If they are equal, gnssodom and odom are sent into the graph optimization model for fusion positioning.

[0099] In the graph optimization model, the odometry pose information at each time step is optimized using the following formula:

[0100] ;

[0101] This represents the rotation matrix that transforms the pose at time t to the pose at time t+1. This represents the pose difference matrix between time t and time t+1. This represents the weight matrix of the pose at time t.

[0102] The odometry pose information is optimized using the GPS pose information at each moment using the following formula:

[0103] ;

[0104] ;

[0105] The GPS pose matrix at time t. The pose matrix representing the odometry at time t. This represents the weight matrix of the GPS pose at time t. This represents the loss function.

[0106] After graph optimization calculation, the fused pose information is obtained. The rotation matrix is ​​updated using the odometry pose at the last moment and the fused pose information. The calculation formula is as follows:

[0107] ;

[0108] The rotation matrix represents the transformation of the odometry pose matrix into the fused pose matrix. The inverse of the odometry pose matrix, This represents the pose matrix of the fusion.

[0109] Will The rotation matrix is ​​sent as a graph optimization fusion matrix to the fusion positioning information output module.

[0110] Fusion positioning information output module, such as Figure 8 As shown:

[0111] The fusion positioning information output module acquires the odometer correction rotation matrix and the graph optimization fusion rotation matrix. It calculates the graph optimization fusion rotation matrix multiplied by the odometer correction rotation matrix to obtain the final fused positioning rotation matrix. The two rotation matrices are then integrated to obtain the final fused output matrix. This results in a fused output matrix that optimizes both the RTK quality difference and the odometer offset problem, thus optimizing the transformation relationship matrix between them.

[0112] This module obtains the odometer correction rotation matrix. Fusion rotation matrix with graph optimization Calculate the unified transformation relation matrix :

[0113] ;

[0114] Will The final output of the multi-source fusion localization method is the fusion rotation matrix.

[0115] Other embodiments of the invention will readily occur to those skilled in the art upon consideration of the specification and practice of the invention disclosed herein. This application is intended to cover any variations, uses, or adaptations of the invention that follow the general principles of the invention and include common knowledge or customary techniques in the art not disclosed herein. The specification and examples are to be considered exemplary only, and the true scope and spirit of the invention are indicated by the following claims.

[0116] It should be understood that the present invention is not limited to the precise structure described above and shown in the accompanying drawings, and various modifications and changes can be made without departing from its scope. The scope of the invention is limited only by the appended claims.

Claims

1. A multi-source information fusion positioning system for unmanned systems, characterized in that, include: The GNSS positioning information module is used to acquire HEADINGA and GNGGA information from GNSS signals and send them to the GNSS positioning fusion input information processing module. The GNSS positioning fusion input information processing module is used for GNSS information quality judgment, including: synchronizing HEADINGA information and GNGGA information through timestamps, and obtaining six state variables from HEADINGA information: number of satellites used, baseline length, solution status, position type, and heading angle. The quality of GNGGA information at that moment is judged by using the number of satellites, baseline length, solution status, and position type. The satellite pose information is composed of GNGGA information and heading angle state variables and stored in the cache queue. The odometer positioning fusion input information processing module is used to correct the offset of the positioning pose data, use the corrected odometer pose data as the odometer pose information of the graph optimization model, and use the odometer pose data and corrected odometer pose data at the next time step to update the rotation matrix of the odometer pose and the corrected odometer pose, and use this matrix as the odometer correction rotation matrix. The positioning fusion module is used to fuse satellite pose information and odometry pose information. It includes: obtaining historical satellite pose information and odometry pose information from the cache queue and synchronizing the two types of pose information in time; optimizing and correcting the synchronized historical satellite pose information and odometry pose information using a graph optimization method to obtain optimized pose information; updating the rotation matrix of unoptimized pose information and optimized pose information through optimized pose information and unoptimized pose information; and using this matrix as the graph optimization fusion rotation matrix. The fusion positioning information output module is used to calculate and output fusion positioning information, including: obtaining the odometer correction rotation matrix and the graph optimization fusion rotation matrix, calculating the product of the two to obtain the final fusion positioning rotation matrix, and outputting the matrix as the final fusion result.

2. The unmanned system multi-source information fusion positioning system according to claim 1, characterized in that, The GNSS positioning information module also includes: Obtain the latitude and longitude, GPS quality information, horizontal accuracy factor, and antenna altitude contained in the GNGGA information; fill this information into the sensor_msgs / NavSatFix message sat in ROS; sat.header.stamp represents the timestamp storing the current time, sat.status.status represents the GPS quality information, sat.latitude represents the latitude information, sat.longitude represents the longitude information, sat.altitude represents the antenna altitude, and sat.position_covariance[0,4] represents the horizontal accuracy factor. After filling, send the sat information to the GPS positioning fusion input information processing module. The solution status and location type, number of satellites used, heading angle, and baseline length are obtained from the HEADINGA information at a reception speed of 1 Hz. This information is then filled into the ROS geometry_msgs::QuaternionStamped message qua, where qua.quaternion.x represents the solution status and location type, x = 1 represents the solution status as SOL_COMPUTED and the location type as NARROW_INT, and x = 0 for other states. qua.quaternion.y represents the number of satellites used, qua.quaternion.z represents the heading angle, and qua.quaternion.w represents the baseline length. After filling in the information, the qua information is sent to the GPS positioning fusion input information processing module.

3. The unmanned system multi-source information fusion positioning system according to claim 2, characterized in that, The GNSS positioning fusion input information processing module also includes: This module is responsible for converting pose data in the Earth coordinate system into pose data in the ENU coordinate system, and judging the quality of the pose information based on the number of satellites, baseline length, solution status and position type, and selecting high-quality pose data as the first localization fusion data for the graph optimization model. Get the SAT and qua information sent by the GNSS positioning information module; first, determine whether the timestamps of the SAT and qua information are the same. If the timestamps are the same, it means that the SAT and qua information are positioning information at the same time; after getting the SAT and qua information at the same time, create a ROS nav_msgs / Odometry information gnssodom; convert the latitude and longitude of the SAT information into ENU coordinate system position information using the forward method of GeographicLib type and fill it into gnssodom.pose.pose.position; convert the heading angle in the qua information into attitude four-element information using the createQuaternionFromYaw method of tf library and fill it into gnssodom.pose.pose.orientation. At this time, initialize gnssodom.pose.covariance[0] to 0.01, which represents the weight of the position data of gnssodom during fusion. The smaller the value, the greater the weight. Initialize gnssodom.twist.covariance[0] to 0.05, which represents the weight of the attitude data of gnssodom during fusion. The smaller the value, the greater the weight. After obtaining the gnssodom information, it is determined whether the quality of the positioning data is good or bad, ensuring that the gnss positioning data entering the fusion positioning is accurate. The specific process is as follows: (1) If qua.quaternion.x < 0.8 | | qua.quaternion.y < 10 is true, it means that the location data quality is very poor. Discard the gnssodom directly, set stable = 0, and then exit the judgment without performing any other operations. Wait for the next sat and qua information. If the result is false, proceed to step 2; The stable parameter is the time elapsed before the gnssodom information recovers from poor quality to good quality and remains at good quality until the next gnssodom information is obtained. (2) If qua.quaternion.y < 15 is true, proceed to step 3; If the result is false, proceed to step 4; (3) If 0.6 < qua.quaternion.w < 0.65 is true, it means that the positioning accuracy of gnssodom is generally low when the number of satellites is small. At this time, reduce the weight of gnssodom in the fusion positioning, set gnssodom.pose.covariance[0] to 5, set gnssodom.twist.covariance[0] to 0.1, and then proceed to step 4. If the result is false, the program will exit directly without performing any further operations and will wait for the next SAT and qua information. (4) Increase stable by 1, then proceed to step 5; (5) Determine that qua.quaternion.y > 17 is true, which means that the quality of the gnssodom information is very good at this time. Increase stable by 4 to obtain the gnssodom information as soon as possible, and then proceed to step 6. If the result is false, proceed directly to step 6; (6) If stable < 60 is true, it means that the quality of the gnssodom information is good but has not been maintained for a certain period of time. Exit the judgment directly and do not perform any other operations. Wait for the next sat and qua information. If the result is false, proceed to step 7; (7) The gnssodom information at this moment can be used as the satellite pose information for the fusion positioning map optimization model; When the quality of the gnssodom information is poor, the unmanned system uses only odometry data for map optimization to remove the interference of erroneous gnssodom positioning information on the positioning accuracy of the unmanned system.

4. The unmanned system multi-source information fusion positioning system according to claim 3, characterized in that, The odometer positioning fusion input information processing module also includes: After receiving the odom information sent by the unmanned system chassis, it is marked as curOdom. The position difference is calculated by comparing it with the odom information lastOdom received at the previous moment to obtain the displacement data of the unmanned system during this time. The calculation formula is as follows: ; This represents the odometer pose data at time i. Due to the offset of the odometer positioning data, the calculated value is affected. The actual displacement (x, y, z) is therefore reduced. Adding the displacement value to the corrected pose data at time i, we can obtain the corrected pose data at time i+1. Therefore, the formula for calculating the optimized pose data at the current time is as follows: ； Let be the odometry pose data at time i, and Scaler be the correction parameter value. Scaler < 1. By reducing the calculated displacement, the offset of the positioning data after fusion of odometry and inertial navigation is corrected, resulting in... Converting to pose data odom provides more accurate odometry positioning data. Using odom as the odometry pose information in the graph optimization model, the rotation matrix of the odometry pose and the corrected odometry pose is updated using the odometry pose data and the corrected odometry pose data at time i+1. This matrix is ​​then used as the odometry correction rotation matrix. ； in For odometer calibration rotation matrix, The inverse of the odometry pose at time i, Let represent the odometer pose matrix at time i.

5. The unmanned system multi-source information fusion positioning system according to claim 4, characterized in that, The location fusion module also includes: When the amount of data in the cache queue reaches 20, obtain the satellite pose information gnssodom and the odometry pose information odom, compare the timestamp gnssodom.header.stamp with the timestamp odom.header.stamp. If they are equal, send gnssodom and odom into the graph optimization model for fusion positioning. In the graph optimization model, the odometry pose information at each time step is optimized using the following formula: ； This represents the rotation matrix that transforms the pose at time t to the pose at time t+1. This represents the pose difference matrix between time t and time t+1. This represents the weight matrix for the pose at time t. The odometry pose information is optimized using the GPS pose information at each moment using the following formula: ； ； This represents the pose matrix of the GPS at time t. The pose matrix representing the odometry at time t. This represents the weight matrix of the GPS pose at time t. Represents the loss function After graph optimization calculation, the fused pose information is obtained. The rotation matrix is ​​updated using the odometry pose at the last moment and the fused pose information. The calculation formula is as follows: ； The rotation matrix represents the transformation of the odometry pose matrix into the fused pose matrix. The inverse of the odometry pose matrix, Represents the fused pose matrix; Will The rotation matrix is ​​sent as a graph optimization fusion matrix to the fusion positioning information output module.

6. The unmanned system multi-source information fusion positioning system according to claim 5, characterized in that, The integrated location information output module also includes: This module obtains the odometer correction rotation matrix. , and graph optimization with rotation matrix Calculate the unified transformation relation matrix : ； Will The final output of the multi-source fusion localization method is the fusion rotation matrix.

Images