Adaptive factor graph optimization based agv multi-sensor close-coupled positioning method and system

Abstract

This application discloses an AGV multi-sensor tightly coupled positioning method and system based on adaptive factor graph optimization, relating to the field of AGV navigation technology. By quantitatively evaluating the real-time positioning quality of LiDAR and GNSS, and dynamically adjusting the Gaussian noise covariance matrix and sensor fusion weights, it achieves intelligent switching of the dominant positioning sensor. It can accurately identify anomalies such as satellite signal failure and decreased LiDAR matching accuracy, quickly switching to a stable sensor to avoid positioning jumps, drift, and interruptions. This enhances the anti-interference capability of the positioning system, ensures the continuity of AGV autonomous navigation and operational safety, and facilitates the penetration of AGVs into high-end industrial scenarios, improving their adaptability and operational efficiency in flexible manufacturing, intelligent warehousing, and other scenarios. It effectively solves the technical problem of existing multi-sensor fusion positioning using fixed weights and being unable to dynamically adapt to environmental changes, significantly improving the positioning performance of AGVs under complex working conditions.

Description

Technical Field

[0001] This application relates to the field of AGV navigation technology, specifically to an AGV multi-sensor tightly coupled positioning method and system based on adaptive factor graph optimization. Background Technology

[0002] Automated Guided Vehicles (AGVs) are core mobile devices in complex industrial scenarios such as flexible manufacturing, intelligent warehousing, and port logistics. The accuracy and robustness of their positioning directly determine the reliability, efficiency, and adaptability of autonomous operation. With the deepening of Industry 4.0, the application scenarios of AGVs have expanded from traditional single indoor environments to complex working conditions involving indoor-outdoor linkage and multi-scenario switching. This places stringent demands on positioning systems, requiring "centimeter-level accuracy, continuous operation at all times, and strong anti-interference capabilities." Accurate and robust positioning has become a core technological bottleneck restricting the penetration of AGVs into high-end industrial scenarios.

[0003] Currently, the mainstream AGV positioning methods in the industrial field are mainly divided into two technical paths: satellite navigation and environmental feature matching. Both methods have their advantages in different scenarios, but their individual application modes are difficult to adapt to complex and ever-changing industrial environments. Given the inherent limitations of single sensors and positioning methods, multi-sensor fusion positioning has become an inevitable technological trend to overcome positioning bottlenecks in complex scenarios and improve the robustness of AGV positioning. Its core idea is to integrate positioning data from multiple sensors such as satellite navigation, lidar, and inertial measurement units, complementing each other's strengths and weaknesses to achieve continuous high-precision positioning across all scenarios.

[0004] However, existing traditional multi-sensor fusion methods generally adopt a fixed weight allocation strategy, which can only statically fuse data from various sensors. They cannot dynamically adjust the weight ratio of each sensor according to real-time environmental changes, and even less can they achieve intelligent switching of the dominant positioning sensor. When AGVs face frequent changes in indoor and outdoor environments, the system lacks a quantitative assessment mechanism for the real-time reliability of each sensor. It cannot promptly identify anomalies such as satellite signal failure or decreased laser feature matching accuracy, and therefore cannot quickly switch the positioning control from a failed or degraded sensor to a stable one. This ultimately leads to jumps, drifts, or even interruptions in positioning results, seriously affecting the continuity of autonomous navigation and operational safety of the AGV. Summary of the Invention

[0005] In order to solve the above-mentioned technical problems, this application proposes the following technical solution: In a first aspect, embodiments of this application provide an AGV multi-sensor tightly coupled positioning method based on adaptive factor graph optimization, including: The output data from the lidar sensor, GNSS receiver, and IMU sensor are collected and preprocessed respectively. The preprocessed real-time point cloud is matched with a pre-constructed prior point cloud map to obtain a point cloud matching score, which is used to evaluate the positioning quality of the lidar sensor. The noise level of GNSS observations is dynamically determined based on the positioning status flag and positioning accuracy factor to evaluate the GNSS positioning quality. The positioning quality indicators of the LiDAR sensor and GNSS are evaluated and converted into Gaussian noise covariance matrices, and sensor fusion weights are automatically assigned. The multi-sensor observation information, after quality assessment and adaptive weighting, is transformed into the optimal pose estimate through a probabilistic graphical model.

[0006] In one possible implementation, the step of separately acquiring and preprocessing the output data from the lidar sensor, GNSS receiver, and IMU sensor includes: Load the global point cloud map that has been aligned to the ENU coordinate system; Collect point cloud data output from lidar sensors, observation data output from GNSS receivers, and IMU data output from IMU sensors; The IMU data is used to correct motion distortion in the laser point cloud and improve the positioning accuracy of GNSS observation data; Voxel filtering and range filtering were performed on the laser point cloud, and the GNSS observation data were transformed from the WGS-84 coordinate system to the ENU coordinate system.

[0007] In one possible implementation, the step of using the IMU data to correct motion distortion in the laser point cloud and improve the positioning accuracy of the GNSS observation data includes: By using high-frequency angular velocity and acceleration data collected by IMU sensors, the instantaneous pose of the carrier at each laser point scanning moment is calculated by integration or interpolation. Point cloud distortion caused by carrier motion is compensated point by point, eliminating point cloud distortion and deformation caused by platform motion. The IMU data works in conjunction with GNSS observation data through a tightly coupled algorithm to assist RTK ambiguity resolution and integrated navigation, so as to maintain positioning continuity when GNSS signal quality degrades and improve positioning accuracy in dynamic scenarios.

[0008] In one possible implementation, the step of matching the preprocessed real-time point cloud with a pre-constructed prior point cloud map to obtain a point cloud matching score includes: The spatial region occupied by the prior point cloud map data is divided into a series of regularly arranged voxels; Assuming that the spatial distribution of all point cloud data points falling within each cell is approximately described by a Gaussian distribution, the statistical properties of which are characterized by the sample mean vector representing the center position of the point cloud within the cell and the covariance matrix. Common definition: in: These are the coordinates of the points within the corresponding cells of the point cloud. The center location of the point cloud within the characterization cell. Describe the degree of dispersion and shape orientation of point clouds around the center within a unit; For each unit, its probability density function is: By solving the optimal pose transformation matrix Achieve point cloud matching: The matching score is obtained in the following way: in: For points in a real-time point cloud, For points in the prior map, The optimal transformation matrix is... The Score value represents the number of point clouds. A smaller Score value indicates a better matching quality. When the Score value is lower than the preset threshold, the matching is considered successful.

[0009] In one possible implementation, the evaluation of GNSS positioning quality based on positioning status flags and positioning accuracy factors dynamically determines the noise level of GNSS observations to assess GNSS positioning quality, including: The positioning status flag is obtained by parsing the status word output by the GNSS receiver. The positioning accuracy factor is determined by obtaining the standard deviation of position and the standard deviation of heading given by the GNSS receiver. By combining the positioning status flag and the positioning accuracy factor, the noise level of GNSS observations is dynamically determined through preset mapping rules.

[0010] In one possible implementation, the step of dynamically determining the noise level of GNSS observations by combining the positioning status flag and the positioning accuracy factor through a preset mapping rule includes: The location uncertainty of GNSS observations is determined by mapping the positioning status flags. ; The heading uncertainty of GNSS observations is determined based on the positioning accuracy factor. .

[0011] In one possible implementation, the step of converting the positioning quality metrics of the lidar sensor and GNSS into a Gaussian noise covariance matrix and automatically assigning sensor fusion weights includes: Based on the positioning quality indicators of the aforementioned photoradar sensor and GNSS, a Gaussian noise covariance matrix is ​​dynamically constructed. : Wherein: For GNSS sensors, its noise matrix The parameter settings are as follows, position components From the above Determine the attitude components. From the above Sure; For lidar sensors, their noise matrix The parameter settings adopt a unified strategy: mapping the matching score to a unified noise parameter. And assign the value to all six diagonal elements, that is: When the sensor data quality is high, the evaluation results are... The value is small, the stated The diagonal element values ​​are smaller, and its inverse matrix is ​​smaller. If the corresponding element value is large, the residual term of the sensor factor is given a high weight in the optimization objective function, and the optimizer will prioritize satisfying this constraint. When the sensor data quality is low, the evaluation results are... The value is large, the stated The diagonal element with the largest value has the largest inverse matrix. The residual terms of the sensor factor with smaller corresponding element values ​​are given lower weights, and their influence in the optimization is automatically reduced. When the sensor fails or the data quality is extremely poor, the evaluation results are... The value is extremely large, the stated When the element value approaches zero, the weight of this factor in the optimization is automatically reduced to an extremely low level, which is equivalent to being smoothed out, without the need to design additional failure detection and switching logic.

[0012] In one possible implementation, the process of transforming the multi-sensor observation information, after quality assessment and adaptive weighting, into optimal pose estimation via a probabilistic graphical model includes: A factor graph model is constructed to express the constraint relationship between the pose state of an automated guided vehicle (AGV) and observations from various sensors. The factor graph model includes variable nodes and factor nodes; each variable node... Represents the AGV at any given moment The pose with 6 degrees of freedom in the global coordinate system, where For location, For the pose, all pose nodes to be estimated constitute a state set. Factor nodes represent constraints on state variables, including prior factors, which provide an absolute baseline for the optimization problem, provided by the first reliable GNSS pose at system startup or manually specified values; GNSS factors, which impose absolute position constraints on the corresponding pose nodes, with observations from the GNSS receiver; and lidar factors, which impose absolute pose constraints on the corresponding pose nodes, with observations from the matching results of the lidar point cloud and the prior map. When constructing each GNSS factor or lidar factor, the following is adopted: and By assigning corresponding factors, the constraint strength can be adjusted adaptively in real time.

[0013] In one possible implementation, when constructing each GNSS factor or lidar factor, the method employed is... and By assigning corresponding factors, real-time adaptive adjustment of constraint strength can be achieved, including: The fusion localization problem is formalized as maximum a posteriori probability estimation: Assuming that all observations are independent, the objective function for a factor graph containing GNSS factors, lidar factors, and prior factors can be specifically expressed as: in: For observation models, This represents the initial state. Let i be the state at time i. Let j be the state at time j. For prior observations, These are LiDAR observations. These are GNSS observations. Represents the Mahalanobis distance, its weights are... Decide, and A noise model dynamically generated based on real-time quality assessment; Combining the sliding window mechanism, only the poses within the most recent N keyframes are optimized. When a new pose is added, the oldest pose is removed using edge detection, and its information is transformed into prior constraints about the remaining states. in: To preserve the state vector, The information matrix block is the state to be marginalized. For the information matrix block that retains the state, For cross-information matrix blocks, Cross-information matrix blocks, Information vector of the state to be marginalized The information vector of the preserved state; After optimization, the system outputs the optimal estimates of all pose nodes within the sliding window. ; The latest pose is encapsulated as a ROS message and published, and the coordinate system transformation is broadcast through a TF tree to provide high-precision and robust positioning information for the upper-level modules of AGV such as navigation and planning.

[0014] Secondly, embodiments of this application provide an AGV multi-sensor tightly coupled positioning system based on adaptive factor graph optimization, comprising: The data acquisition and processing module is used to acquire and preprocess the output data from the lidar sensor, GNSS receiver, and IMU sensor, respectively. The point cloud matching module is used to match the preprocessed real-time point cloud with a pre-built prior point cloud map to obtain a point cloud matching score, which is used to evaluate the positioning quality of the lidar sensor. The GNSS positioning quality assessment module is used to assess GNSS positioning quality based on positioning status flags and positioning accuracy factors. It dynamically determines the noise level of GNSS observations to evaluate GNSS positioning quality. An adaptive weight allocation module is used to convert the positioning quality indicators of the LiDAR sensor and GNSS into a Gaussian noise covariance matrix and automatically allocate sensor fusion weights. The pose optimization module is used to transform the multi-sensor observation information, which has undergone quality assessment and adaptive weighting, into the optimal pose estimate through a probabilistic graphical model.

[0015] Thirdly, embodiments of this application provide a device including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the coupling positioning method described in any possible implementation of the first aspect.

[0016] Fourthly, embodiments of this application provide a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the coupling positioning method described in any possible implementation of the first aspect.

[0017] In this embodiment, by quantitatively evaluating the real-time positioning quality of LiDAR and GNSS, and dynamically adjusting the Gaussian noise covariance matrix and sensor fusion weights, intelligent switching of the dominant positioning sensor is achieved. It can accurately identify anomalies such as satellite signal failure and decreased laser matching accuracy, quickly switching to a stable sensor to avoid positioning jumps, drift, and interruptions. It adapts to indoor / outdoor linkage and multi-scenario switching conditions, meeting the requirements for centimeter-level accuracy and continuous positioning around the clock. This enhances the anti-interference capability of the positioning system, ensures the continuity of AGV autonomous navigation and operational safety, and helps AGVs penetrate high-end industrial scenarios, improving their adaptability and operational efficiency in flexible manufacturing, intelligent warehousing, and other scenarios. It effectively solves the technical problem of existing multi-sensor fusion positioning using fixed weights and being unable to dynamically adapt to environmental changes, significantly improving the positioning performance of AGVs under complex working conditions. Attached Figure Description

[0018] Figure 1 A flowchart illustrating an AGV multi-sensor tightly coupled positioning method based on adaptive factor graph optimization provided in this application embodiment; Figure 2 This is a schematic diagram of the laser point cloud matching process provided in an embodiment of this application; Figure 3 This is a schematic diagram of the GNSS positioning quality assessment process provided in the embodiments of this application; Figure 4 This is a schematic diagram of the adaptive weight allocation strategy provided in the embodiments of this application; Figure 5 This is a schematic diagram of the factor graph optimization model provided in the embodiments of this application; Figure 6 A schematic diagram of the sliding window optimization mechanism provided in the embodiments of this application; Figure 7 A schematic diagram of an AGV multi-sensor tightly coupled positioning system based on adaptive factor graph optimization provided in this application embodiment; Figure 8 This is a schematic diagram of a device provided in an embodiment of this application. Detailed Implementation

[0019] The present solution will now be described in conjunction with the accompanying drawings and specific embodiments.

[0020] See Figure 1 The AGV multi-sensor tightly coupled positioning method based on adaptive factor graph optimization provided in this embodiment includes: S101 collects and preprocesses the output data from the lidar sensor, GNSS receiver, and IMU sensor.

[0021] First, a brief description of the lidar sensor, GNSS receiver, and IMU sensor is given. The lidar sensor is used to collect environmental point cloud data; the GNSS receiver is used to receive satellite signals and provide absolute position information; the IMU sensor is used to measure the angular velocity and acceleration information of the carrier; the IMU sensor and the GNSS receiver are hardware integrated in the same combined navigation module.

[0022] The system starts and loads a global point cloud map aligned to the ENU coordinate system. It collects point cloud data from the lidar sensor, observation data from the GNSS receiver, and angular velocity and acceleration data from the IMU sensor. Motion distortion correction is performed on the lidar point cloud: using the high-frequency angular velocity and acceleration data collected by the IMU sensor, the instantaneous pose of the carrier at each lidar point scan is calculated through integration or interpolation. Point-by-point compensation is performed to compensate for point cloud distortion caused by carrier motion, eliminating point cloud distortion and deformation caused by platform motion. The IMU data simultaneously works in conjunction with GNSS observation data through a tightly coupled algorithm to assist RTK ambiguity resolution and integrated navigation, maintaining positioning continuity even when GNSS signal quality degrades and improving positioning accuracy in dynamic scenarios. Voxel filtering and range filtering are applied to the lidar point cloud, and the GNSS observation data is transformed from the WGS-84 coordinate system to the ENU coordinate system.

[0023] S102 matches the preprocessed real-time point cloud with a pre-built prior point cloud map to obtain a point cloud matching score, which is used to evaluate the positioning quality of the lidar sensor.

[0024] See Figure 2 The Normal Distribution Transform (NDT) algorithm is used to match the preprocessed real-time point cloud with the pre-constructed prior point cloud map. The specific implementation process is as follows: First, the spatial region occupied by the reference point cloud data is divided into a series of regularly arranged voxels. Then, it is assumed that the spatial distribution of all point cloud data points falling into each cell is approximately described by a Gaussian distribution. The statistical properties of this distribution are represented by the sample mean vector, which characterizes the center position of the point cloud within the cell, and the covariance matrix. Common definition: in: These are the coordinates of the points within the corresponding cells of the point cloud. The center location of the point cloud within the characterization cell. Describe the degree of dispersion and shape orientation of point clouds around the center within a unit.

[0025] For each unit, its probability density function is: By solving the optimal pose transformation matrix Achieve point cloud matching: To facilitate numerical solutions, it is transformed into a negative log-likelihood minimization problem: A matching score is calculated to evaluate the quality of laser point cloud matching. The matching score is obtained in the following manner: in: For points in a real-time point cloud, For points in the prior map, The optimal transformation matrix is... This represents the number of point clouds. A smaller Score value indicates better matching quality. When the Score value is lower than a preset threshold (e.g., 0.5), the match is considered successful.

[0026] S103 dynamically determines the noise level of GNSS observations based on the positioning status flag and positioning accuracy factor to assess GNSS positioning quality.

[0027] See Figure 3 The evaluation of GNSS positioning quality is based on two types of real-time data: positioning status flags: by parsing the status words output by the GNSS receiver (e.g., 50 represents a fixed solution, 34 represents a floating solution), the positioning reliability is qualitatively determined. Positioning accuracy factor: by obtaining the position standard deviation and heading standard deviation given by the GNSS receiver, the positioning uncertainty is quantitatively evaluated.

[0028] Based on the above data, the noise level of GNSS observations is dynamically determined using preset mapping rules: Location uncertainty The mapping is mainly based on the positioning status flags. For example, the fixed demapping is... =0.01 (high weight), floating solution mapping is =1.0 (medium weight), single-point demapping is =10.0 (low weight).

[0029] Course uncertainty : Directly use the standard deviation of the heading angle provided by the GNSS receiver (converted to radians).

[0030] Tables 1 and 2 show the mapping relationship between laser matching scores and satellite positioning flags and noise levels, respectively: Table 1 Comparison of Laser Point Cloud Matching Score and Localization Weight Allocation Table 2 GNSS Positioning Status and Adaptive Noise Weight Mapping Table S104 will evaluate the positioning quality indicators of the lidar sensor and GNSS and convert them into a Gaussian noise covariance matrix, and automatically assign sensor fusion weights.

[0031] The core of the adaptive weight allocation mechanism in this embodiment lies in dynamically instantiating the evaluated sensor quality index into a Gaussian noise covariance matrix. The matrix is ​​then used to automatically and optimally determine the fusion weights of each sensor within a probabilistic optimization framework.

[0032] For GNSS sensors, their noise matrix The parameter settings are as follows, position division The standard deviation of the location obtained from the assessment Decision. Attitude components. Mainly derived from the standard deviation of the heading obtained through assessment Decide.

[0033] For lidar sensors, their noise matrix The parameter settings adopt a unified strategy: mapping the matching score to a unified noise parameter. And assign the value to all six diagonal elements, that is .

[0034] Automatic and optimal determination of weights: Regardless of the source of the parameters, once the noise model... Once instantiated, its weight in factor graph optimization is determined by its information matrix. The automatic and rigorous decision-making process resulted in the following rigorous adaptive logic: When the sensor data quality is high, the evaluation results are... Small value, constructed The smaller the value of the diagonal element of the matrix, the smaller its inverse matrix. If the corresponding element value is large, the residual term of the sensor factor is given a high weight in the optimization objective function, and the optimizer will prioritize satisfying this constraint.

[0035] When the sensor data quality is low, the evaluation results are... Large value, constructed The larger the diagonal element of a matrix, the larger its inverse matrix. The residual terms of the sensor factor with smaller corresponding element values ​​are assigned lower weights, and their influence on optimization is automatically reduced.

[0036] When the sensor fails or the data quality is extremely poor, the evaluation results are... The value is extremely large, its When matrix element values ​​approach zero, the weight of this factor in the optimization is automatically reduced to an extremely low level, effectively equivalent to being smoothed out, without requiring the design of additional failure detection and switching logic. For example... Figure 4 This is a schematic diagram of an adaptive weight allocation strategy.

[0037] S105 transforms the multi-sensor observation information, after quality assessment and adaptive weighting, into the optimal pose estimate through a probabilistic graphical model.

[0038] This step is the core computing engine of the fusion positioning system, used to transform multi-sensor observation information, after quality assessment and adaptive weighting, into optimal pose estimation through a probabilistic graphical model. This process achieves a complete closed-loop processing of sensor data from perception to fusion positioning.

[0039] The system constructs a factor graphical model, a probabilistic graphical model used to express the constraints between the pose state of an automated guided vehicle (AGV) and observations from various sensors. The model's architecture is as follows: Figure 5 It mainly consists of the following two parts: Variable nodes: such as Figure 5 As shown in the yellow circle, each node Represents the AGV at any given moment The pose with 6 degrees of freedom in the global coordinate system, where For location, The pose is represented by all the pose nodes to be estimated, which constitute the state set. .

[0040] Factor nodes: Represent constraints on state variables, including prior factors (green diamonds), which provide an absolute baseline for the optimization problem, provided by the first reliable GNSS pose at system startup or manually specified values; GNSS factors (red squares), which impose absolute position constraints on the corresponding pose nodes, with observations from the GNSS receiver; and lidar factors (blue squares), which impose absolute pose constraints on the corresponding pose nodes, with observations from the matching results of the lidar point cloud and the prior map.

[0041] and This is directly used as the core parameter of the corresponding factor. When constructing each GNSS factor or lidar factor, the generated parameter, reflecting the sensor quality at the current moment, is used. and By assigning corresponding factors, the constraint strength can be adjusted adaptively in real time.

[0042] The fusion localization problem is formalized as a maximum a posteriori probability estimation problem, as shown in the formula: Assuming that all observations are independent, this problem can be transformed into a nonlinear least squares problem, i.e., minimizing the sum of squares of all factor errors. For a factor graph containing GNSS factors, lidar factors, and prior factors, the objective function can be specifically expressed as: in: For observation models, This represents the initial state. Let i be the state at time i. Let j be the state at time j. For prior observations, These are LiDAR observations. These are GNSS observations. Represents the Mahalanobis distance, its weights are... Decide, and This is a noise model dynamically generated based on real-time quality assessment.

[0043] The system uses the iSAM2 algorithm for incremental optimization, and its process is as follows: Figure 6 The sliding window optimization mechanism is illustrated, which uses a Bayesian tree data structure to efficiently update the factor graph. Combined with the sliding window mechanism, the system optimizes only the poses within the most recent N keyframes. When a new pose is added, the oldest pose is removed using an edge-mapping technique, and its information is transformed into prior constraints about the remaining states, as shown in the formula: in: To preserve the state vector, The information matrix block is the state to be marginalized. For the information matrix block that retains the state, For cross-information matrix blocks, Cross-information matrix blocks, Information vector of the state to be marginalized The information vector of the preserved state is retained. This process ensures the preservation of historical information and prevents the degradation of optimization results.

[0044] After optimization, the system outputs the optimal estimates of all pose nodes within the sliding window. The latest pose is encapsulated as a ROS message and published, and the coordinate system transformation is broadcast via TF tree, providing high-precision and robust positioning information for the AGV's navigation, planning and other upper-level modules.

[0045] Corresponding to the above embodiment of the AGV multi-sensor tightly coupled positioning method based on adaptive factor graph optimization, this application also provides an embodiment of the AGV multi-sensor tightly coupled positioning system based on adaptive factor graph optimization.

[0046] See Figure 7 The AGV multi-sensor tightly coupled positioning system 20 based on adaptive factor graph optimization in this embodiment includes: The data acquisition and processing module 201 is used to acquire and preprocess the output data from the lidar sensor, GNSS receiver and IMU sensor respectively.

[0047] The point cloud matching module 202 is used to match the preprocessed real-time point cloud with a pre-built prior point cloud map to obtain a point cloud matching score, which is used to evaluate the positioning quality of the lidar sensor.

[0048] The GNSS positioning quality assessment module 203 is used to assess the GNSS positioning quality based on the positioning status flag and positioning accuracy factor, dynamically determine the noise level of GNSS observations, and evaluate the GNSS positioning quality.

[0049] The adaptive weight allocation module 204 is used to convert the positioning quality indicators of the LiDAR sensor and GNSS into a Gaussian noise covariance matrix and automatically allocate sensor fusion weights.

[0050] The pose optimization module 205 is used to transform the multi-sensor observation information after quality assessment and adaptive weighting into the optimal pose estimate through a probabilistic graphical model.

[0051] See Figure 8 The device 300 provided in this embodiment may include a processor 301, a memory 302, and a communication unit 303. These components communicate through one or more buses. Those skilled in the art will understand that the device structure shown in the figures does not constitute a limitation on the embodiments of this application. It may be a bus topology or a star topology, and may include more or fewer components than shown, or combine certain components, or have different component arrangements.

[0052] The communication unit 303 is used to establish a communication channel, enabling the device to communicate with satellites or multiple sensors.

[0053] The processor 301 serves as the control center of the device, connecting various parts of the device via various interfaces and lines. It executes software programs and / or modules stored in the memory 302, and calls data stored in the memory to perform various functions of the device and / or process data. The processor can be composed of integrated circuits (ICs), such as a single packaged IC or multiple packaged ICs with the same or different functions connected together. For example, the processor 301 may consist only of a central processing unit (CPU). In this embodiment, the CPU may have a single processing core or include multiple processing cores.

[0054] Memory 302 is used to store the execution instructions of processor 301. Memory 302 can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as static random access memory (SRAM), electrically erasable programmable read-only memory (EEPROM), erasable programmable read-only memory (EPROM), programmable read-only memory (PROM), read-only memory (ROM), magnetic storage, flash memory, magnetic disk or optical disk.

[0055] When the execution instructions in memory 302 are executed by processor 301, the device 300 is able to perform some or all of the steps in the above method embodiments. For details, please refer to the method embodiments of this application, which will not be repeated here.

[0056] Corresponding to the above embodiments, this application also provides a computer-readable storage medium, wherein the computer-readable storage medium may store a program, wherein when the program runs, it can control the device where the computer-readable storage medium is located to execute some or all of the steps in the above method embodiments. In specific implementation, the computer-readable storage medium may be a magnetic disk, an optical disk, read-only memory (ROM), or random access memory (RAM), etc.

[0057] In this application embodiment, "at least one" refers to one or more, and "more than one" refers to two or more. "And / or" describes the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent the existence of A alone, the simultaneous existence of A and B, or the existence of B alone. A and B can be singular or plural. The character " / " generally indicates that the preceding and following related objects have an "or" relationship. "At least one of the following" and similar expressions refer to any combination of these items, including any combination of single or plural items. For example, at least one of a, b, and c can represent: a, b, c, ab, ac, bc, or abc, where a, b, and c can be single or multiple.

[0058] The above description is merely a specific embodiment of this application. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the protection scope of this application. The protection scope of this application should be determined by the protection scope of the claims.

Claims

1. A tightly coupled multi-sensor positioning method for AGVs based on adaptive factor graph optimization, characterized in that, include: The output data from the lidar sensor, GNSS receiver, and IMU sensor are collected and preprocessed respectively. The preprocessed real-time point cloud is matched with a pre-constructed prior point cloud map to obtain a point cloud matching score, which is used to evaluate the positioning quality of the lidar sensor. The noise level of GNSS observations is dynamically determined based on the positioning status flag and positioning accuracy factor to evaluate the GNSS positioning quality. The positioning quality indicators of the LiDAR sensor and GNSS are evaluated and converted into Gaussian noise covariance matrices, and sensor fusion weights are automatically assigned. The multi-sensor observation information, after quality assessment and adaptive weighting, is transformed into the optimal pose estimate through a probabilistic graphical model.

2. The AGV multi-sensor tightly coupled positioning method based on adaptive factor graph optimization according to claim 1, characterized in that, The process of acquiring and preprocessing the output data from the lidar sensor, GNSS receiver, and IMU sensor includes: Load the global point cloud map that has been aligned to the ENU coordinate system; Collect point cloud data output from lidar sensors, observation data output from GNSS receivers, and IMU data output from IMU sensors; The IMU data is used to correct motion distortion in the laser point cloud and improve the positioning accuracy of GNSS observation data; Voxel filtering and range filtering were performed on the laser point cloud, and the GNSS observation data were transformed from the WGS-84 coordinate system to the ENU coordinate system.

3. The AGV multi-sensor tightly coupled positioning method based on adaptive factor graph optimization according to claim 2, characterized in that, The method of using the IMU data to correct motion distortion in the laser point cloud and improve the positioning accuracy of GNSS observation data includes: By using high-frequency angular velocity and acceleration data collected by IMU sensors, the instantaneous pose of the carrier at each laser point scanning moment is calculated by integration or interpolation. Point cloud distortion caused by carrier motion is compensated point by point, eliminating point cloud distortion and deformation caused by platform motion. The IMU data works in conjunction with GNSS observation data through a tightly coupled algorithm to assist RTK ambiguity resolution and integrated navigation, so as to maintain positioning continuity when GNSS signal quality degrades and improve positioning accuracy in dynamic scenarios.

4. The AGV multi-sensor tightly coupled positioning method based on adaptive factor graph optimization according to claim 1, characterized in that, The step of matching the preprocessed real-time point cloud with a pre-constructed prior point cloud map to obtain a point cloud matching score includes: The spatial region occupied by the prior point cloud map data is divided into a series of regularly arranged voxels; Assuming that the spatial distribution of all point cloud data points falling within each cell is approximately described by a Gaussian distribution, the statistical properties of which are represented by the sample mean vector and the covariance matrix of the point cloud within the cell. Common definition: in: These are the coordinates of the points within the corresponding cells of the point cloud. The center location of the point cloud within the characterization cell. Describe the degree of dispersion and shape orientation of point clouds around the center within a unit; For each unit, its probability density function is: By solving the optimal pose transformation matrix Achieve point cloud matching: The matching score is obtained in the following way: in: For points in a real-time point cloud, For points in the prior map, The optimal transformation matrix is... The Score value represents the number of point clouds. A smaller Score value indicates a better matching quality. When the Score value is lower than the preset threshold, the matching is considered successful.

5. The AGV multi-sensor tightly coupled positioning method based on adaptive factor graph optimization according to claim 4, characterized in that, The method for evaluating GNSS positioning quality based on positioning status flags and positioning accuracy factors, which dynamically determines the noise level of GNSS observations to assess GNSS positioning quality, includes: The positioning status flag is obtained by parsing the status word output by the GNSS receiver. The positioning accuracy factor is determined by obtaining the standard deviation of position and the standard deviation of heading given by the GNSS receiver. By combining the positioning status flag and the positioning accuracy factor, the noise level of GNSS observations is dynamically determined through preset mapping rules.

6. The AGV multi-sensor tightly coupled positioning method based on adaptive factor graph optimization according to claim 5, characterized in that, The step of dynamically determining the noise level of GNSS observations by combining the positioning status flag and the positioning accuracy factor through a preset mapping rule includes: The location uncertainty of GNSS observations is determined by mapping based on the positioning status flag. ; The heading uncertainty of GNSS observations is determined based on the positioning accuracy factor. .

7. The AGV multi-sensor tightly coupled positioning method based on adaptive factor graph optimization according to claim 6, characterized in that, The process of converting the positioning quality metrics of the lidar sensor and GNSS into a Gaussian noise covariance matrix and automatically assigning sensor fusion weights includes: Based on the positioning quality indicators of the aforementioned photoradar sensor and GNSS, a Gaussian noise covariance matrix is ​​dynamically constructed. : Wherein: For GNSS sensors, its noise matrix The parameter settings are as follows, position components From the above Determine the attitude components. From the above Sure; For lidar sensors, their noise matrix The parameter settings adopt a unified strategy: mapping the matching score to a unified noise parameter. And assign the value to all six diagonal elements, that is: When the sensor data quality is high, the evaluation results are... The value is small, the stated The diagonal element values ​​are smaller, and its inverse matrix is ​​smaller. If the corresponding element value is large, the residual term of the sensor factor is given a high weight in the optimization objective function, and the optimizer will prioritize satisfying this constraint. When the sensor data quality is low, the evaluation results are... The value is large, the stated The diagonal element with the largest value has the largest inverse matrix. The residual terms of the sensor factor with smaller corresponding element values ​​are given lower weights, and their influence in the optimization is automatically reduced. When the sensor fails or the data quality is extremely poor, the evaluation results are... The value is extremely large, the stated When the element value approaches zero, the weight of this factor in the optimization is automatically reduced to an extremely low level, which is equivalent to being smoothed out, without the need to design additional failure detection and switching logic.

8. The AGV multi-sensor tightly coupled positioning method based on adaptive factor graph optimization according to claim 7, characterized in that, The process of transforming the multi-sensor observation information, after quality assessment and adaptive weighting, into optimal pose estimation through a probabilistic graphical model includes: A factor graph model is constructed to express the constraint relationship between the pose state of an automated guided vehicle (AGV) and observations from various sensors. The factor graph model includes variable nodes and factor nodes; each variable node... Represents the AGV at any given moment The pose with 6 degrees of freedom in the global coordinate system, where For location, For the pose, all pose nodes to be estimated constitute a state set. Factor nodes represent constraints on state variables, including prior factors, which provide an absolute baseline for the optimization problem, provided by the first reliable GNSS pose at system startup or manually specified values; GNSS factors, which impose absolute position constraints on the corresponding pose nodes, with observations from the GNSS receiver; and lidar factors, which impose absolute pose constraints on the corresponding pose nodes, with observations from the matching results of the lidar point cloud and the prior map. When constructing each GNSS factor or lidar factor, the following is adopted: and By assigning corresponding factors, the constraint strength can be adjusted adaptively in real time.

9. The AGV multi-sensor tightly coupled positioning method based on adaptive factor graph optimization according to claim 8, characterized in that, The method used when constructing each GNSS factor or lidar factor is as follows: and By assigning corresponding factors, real-time adaptive adjustment of constraint strength can be achieved, including: The fusion localization problem is formalized as maximum a posteriori probability estimation: Assuming that all observations are independent, the objective function for a factor graph containing GNSS factors, lidar factors, and prior factors can be specifically expressed as: in: For observation models, This represents the initial state. Let i be the state at time i. Let j be the state at time j. For prior observations, These are LiDAR observations. These are GNSS observations. Represents the Mahalanobis distance, its weights are... Decide, and A noise model dynamically generated based on real-time quality assessment; Combining the sliding window mechanism, only the poses within the most recent N keyframes are optimized. When a new pose is added, the oldest pose is removed using edge detection, and its information is transformed into prior constraints about the remaining states. in: To preserve the state vector, The information matrix block represents the state to be marginalized. For the information matrix block that retains the state, For cross-information matrix blocks, Cross-information matrix blocks, Information vector of the state to be marginalized Preserve the information vector of the state; After optimization, the system outputs the optimal estimates of all pose nodes within the sliding window. ; The latest pose is encapsulated as a ROS message and published, and the coordinate system transformation is broadcast through a TF tree to provide high-precision and robust positioning information for the AGV's navigation, planning and other upper-level modules.

10. An AGV multi-sensor tightly coupled positioning system based on adaptive factor graph optimization, characterized in that, include: The data acquisition and processing module is used to acquire and preprocess the output data from the lidar sensor, GNSS receiver, and IMU sensor, respectively. The point cloud matching module is used to match the preprocessed real-time point cloud with a pre-built prior point cloud map to obtain a point cloud matching score, which is used to evaluate the positioning quality of the lidar sensor. The GNSS positioning quality assessment module is used to assess GNSS positioning quality based on positioning status flags and positioning accuracy factors. It dynamically determines the noise level of GNSS observations to evaluate GNSS positioning quality. An adaptive weight allocation module is used to convert the positioning quality indicators of the LiDAR sensor and GNSS into a Gaussian noise covariance matrix and automatically allocate sensor fusion weights. The pose optimization module is used to transform multi-sensor observation information, after quality assessment and adaptive weighting, into the optimal pose estimate through a probabilistic graphical model.

Images