Fixed node assisted mobile agent slam absolute positioning method

Abstract

The application discloses a kind of fixed node assisted mobile intelligent agent SLAM absolute positioning method, according to the pose transformation information of mobile robot, obtain factor graph, and determine covariance;According to the bias of the factor graph estimation IMU, update the trajectory and map of mobile robot;According to the bias of the IMU, the positioning state of mobile robot is determined.The application can use the fixed node with known absolute position to assist when mobile robot is mapping and positioning, so as to improve the accuracy of mapping and positioning;Using the method of fixed node assistance, the positioning of mobile robot is monitored in real time, error correction can be timely, improve the robustness and reliability of mobile robot;It can be integrated in a variety of functional sensor networks, easy to deploy.

Description

Technical Field

[0001] This invention belongs to the field of sensor network-assisted mobile robot localization technology, specifically relating to a fixed node-assisted SLAM absolute localization method for mobile intelligent agents. Background Technology

[0002] Mobile robots are intelligent agents capable of autonomous mapping, navigation, and performing transportation or human-robot interaction tasks, and their applications are becoming increasingly widespread. To accomplish these tasks, improving the positioning accuracy of mobile robots and developing methods for real-time detection and correction of positioning failures are crucial.

[0003] To achieve high-precision positioning, odometry based on LiDAR and visual sensors plays a crucial role, while SLAM algorithms assisted by inertial navigation units also function effectively in various application scenarios. However, since the positioning obtained by these algorithms is derived from the accumulation of relative pose transformations, prolonged operation inevitably leads to severe cumulative errors, ultimately causing positioning failure. Although GPS can provide global absolute position estimation, it is unusable in many obstructed environments or indoor scenarios. Summary of the Invention

[0004] The purpose of the embodiments in this specification is to provide a fixed-node-assisted SLAM absolute positioning method for mobile intelligent agents.

[0005] This invention provides a fixed-node-assisted SLAM absolute localization method for mobile intelligent agents, the method comprising:

[0006] The factor map is obtained based on the pose transformation information of the mobile robot, and the covariance is determined.

[0007] Estimate the IMU bias based on the factor graph, and update the trajectory and map of the mobile robot;

[0008] The positioning status of the mobile robot is determined based on the deviation of the IMU.

[0009] In one embodiment, obtaining the factor map based on the pose transformation information of the mobile robot specifically involves: determining the odometry factor, the pre-integration factor, and the fixed node factor, and determining the factor map based on the odometry factor, the pre-integration factor, and the fixed node factor.

[0010] In one embodiment, determining the odometer factor specifically involves obtaining the odometer factor through the sensors of the mobile robot.

[0011] In one embodiment, determining the pre-integration factor specifically involves determining the IMU pre-integration and using it as the pre-integration factor.

[0012] In one embodiment, determining the fixed node factor specifically involves: the mobile agent being able to communicate with multiple fixed nodes simultaneously, obtaining relative poses from each, calculating weights based on the strength of the communication signals, and optimizing the mobile agent's pose based on fixed node constraints, with the objective function being:

[0013] Where, ρ i The weight is the signal strength I. i The function of ρ i =h(I i The optimization results are used as fixed node factors.

[0014] In one embodiment, the fixed node is installed at a known location in the environment and has wireless communication capabilities with the mobile agent.

[0015] In one embodiment, obtaining the factor graph based on the pose transformation information of the mobile robot specifically involves: the factor graph using the mobile robot pose as nodes and the pose transformation relationships between nodes as edges, including odometry factors, pre-integration factors, and fixed node factors, with the objective function being:

[0016] Among them, e ij For node x i With node x j The error between observations, Ω ij Let be the covariance matrix of the observations, q be the precision coefficient of the fixed node factor, and the optimization variables be:

[0017] In one embodiment, the pose of the mobile robot is estimated using the fixed nodes. The pose estimated by the nearest fixed node is taken as the estimation result. The product of the variance of the estimates from multiple fixed nodes and the scaling factor k is used as the fixed node factor accuracy coefficient. The weight of the fixed node factor in the factor graph optimization is adjusted as follows: q = kσ 2 Where k is the proportionality coefficient, σ 2 The variance of the estimated values ​​for multiple fixed nodes.

[0018] In one embodiment, estimating the IMU bias based on the factor graph specifically involves: adding fixed node factors to the factor graph, performing weighted square summation using node covariance as weight, optimizing the robot pose using an iterative method, and simultaneously estimating the IMU bias.

[0019] In one embodiment, determining the positioning status of the mobile robot based on the IMU deviation specifically involves: if the estimated value of the IMU deviation is less than a first threshold, the mobile robot is accurately positioned and no error correction is needed; or, if the estimated value of the IMU deviation is greater than the first threshold and less than a second threshold, the mobile robot has a large positioning deviation, and the SLAM system is initialized using the weighted result of fixed node position estimation and loop closure detection; or, if the estimated value of the IMU deviation is greater than the second threshold, the SLAM system is initialized using fixed node position estimation.

[0020] Compared with existing technologies, this invention can use fixed nodes with known absolute positions to assist in the mapping and localization of mobile robots, thereby improving the accuracy of mapping and localization; by using the fixed node assistance method, the localization status of the mobile robot can be monitored in real time, and errors can be corrected in a timely manner, improving the robustness and reliability of the mobile robot; and it can be integrated into a multi-functional sensor network, making it easy to deploy. Attached Figure Description

[0021] To more clearly illustrate the technical solutions in the embodiments or prior art of this specification, the drawings used in the description of the embodiments or prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in this specification. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0022] Figure 1 A schematic diagram of the positioning system provided in this application;

[0023] Figure 2 A flowchart illustrating the fixed-node-assisted SLAM absolute localization method for mobile intelligent agents provided in this application;

[0024] Figure 3 A schematic diagram of the factor plot provided in this application. Detailed Implementation

[0025] To enable those skilled in the art to better understand the technical solutions in this specification, the technical solutions in the embodiments of this specification will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this specification, and not all embodiments. Based on the embodiments in this specification, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of this specification.

[0026] In the following description, specific details such as particular system architectures and techniques are set forth for illustrative purposes and not for limitation, in order to provide a thorough understanding of the embodiments of this application. However, those skilled in the art will understand that this application may also be implemented in other embodiments without these specific details. In other instances, detailed descriptions of well-known systems, apparatuses, circuits, and methods have been omitted so as not to obscure the description of this application with unnecessary detail.

[0027] Various modifications and variations can be made to the specific embodiments described in this application without departing from the scope or spirit of this application, as will be apparent to those skilled in the art. Other embodiments derived from this application will be obvious to those skilled in the art. This application specification and embodiments are merely exemplary.

[0028] The terms “include,” “including,” “have,” “contain,” etc., used in this article are all open-ended terms, meaning that they include but are not limited to.

[0029] The present invention will be further described in detail below with reference to the accompanying drawings and embodiments.

[0030] like Figure 1 As shown, the present invention addresses a fixed-node-assisted SLAM absolute positioning system for mobile intelligent agents. This system includes a mobile robot and fixed sensor nodes, and the corresponding assisted positioning method includes a fixed-node-assisted SLAM algorithm and a fixed-node-assisted error correction algorithm.

[0031] The fixed sensor node has known absolute coordinates, communicates wirelessly with the mobile robot, and is able to obtain the relative pose between the two.

[0032] A mobile robot is a highly reliable and scalable mobile intelligent agent that can move autonomously on the ground. Equipped with a high-precision inertial navigation unit, multi-line LiDAR, visual sensors, and wireless communication modules, it can perform tasks such as mapping, navigation, transportation, and human-machine interaction.

[0033] like Figure 2 As shown, the fixed-node-assisted SLAM algorithm is deployed on a mobile robot, using the position of the fixed nodes to help the mobile agent obtain accurate absolute position information.

[0034] This invention provides a fixed-node-assisted SLAM absolute localization method for mobile intelligent agents, which is implemented through the following steps:

[0035] Step 101: Obtain the factor map based on the pose transformation information of the mobile robot and determine the covariance;

[0036] Specifically, the odometer factor, pre-integration factor, and fixed node factor are determined, and a factor graph is determined based on the odometer factor, pre-integration factor, and fixed node factor.

[0037] The determination of the odometer factor specifically involves obtaining the odometer factor through the sensors of the mobile robot.

[0038] The determination of the pre-integration factor specifically involves: determining the IMU pre-integration and using it as the pre-integration factor.

[0039] The determination of the fixed node factor specifically involves: the mobile agent being able to communicate with multiple fixed nodes simultaneously, obtaining relative poses from each, calculating weights based on the strength of the communication signals, and optimizing the mobile agent's pose based on fixed node constraints. The objective function is: Where, ρ i The weight is the signal strength I. i The function of ρ i =h(I i The optimization results are used as fixed node factors.

[0040] The fixed node is installed at a known location in the environment and has the ability to communicate wirelessly with the mobile intelligent agent.

[0041] like Figure 3 As shown, the factor graph uses the mobile robot pose as nodes and the pose transformation relationships between nodes as edges. It includes odometry factors, pre-integration factors, and fixed node factors. The objective function is: Among them, e ij For node x i With node x j The error between observations, Ω ij Let be the covariance matrix of the observations, q be the precision coefficient of the fixed node factor, and the optimization variables be:

[0042] The pose of the mobile robot is estimated using the fixed nodes. The pose estimated by the nearest fixed node is taken as the estimation result. The product of the variance of multiple fixed node estimates and the scaling factor k is used as the fixed node factor accuracy coefficient. The weight of the fixed node factor in the factor graph optimization is adjusted as follows: q = kσ 2 Where k is the proportionality coefficient, σ 2 The variance of the estimated values ​​for multiple fixed nodes.

[0043] Step 102: Estimate the IMU bias based on the factor map, and update the trajectory and map of the mobile robot;

[0044] Specifically, fixed node factors are added to the factor graph, and the node covariance is used as the weight for weighted square summation. An iterative method is used to optimize the robot pose and estimate the IMU bias.

[0045] Step 103: Determine the positioning status of the mobile robot based on the deviation of the IMU.

[0046] Specifically, if the estimated value of the IMU bias is less than the first threshold, the mobile robot is accurately positioned and no error correction is needed; or, if the estimated value of the IMU bias is greater than the first threshold but less than the second threshold, the mobile robot has a large positioning deviation, and the SLAM system is initialized using the weighted result of fixed node position estimation and loop closure detection; or, if the estimated value of the IMU bias is greater than the second threshold, the SLAM system is initialized using fixed node position estimation.

[0047] It should be noted that the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus. Unless otherwise specified, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes that element.

[0048] The various embodiments in this specification are described in a progressive manner. Similar or identical parts between embodiments can be referred to interchangeably. Each embodiment focuses on describing the differences from other embodiments. In particular, the system embodiments are basically similar to the method embodiments, so the description is relatively simple; relevant parts can be referred to the descriptions in the method embodiments.

Claims

1. A fixed node assisted mobile agent SLAM absolute localization method, characterized in that, The method comprises: According to the pose transformation information of the mobile robot, a factor graph is obtained, and a covariance is determined, specifically: determining an odometer factor, a pre-integration factor, and a fixed node factor, and determining a factor graph according to the odometer factor, the pre-integration factor, and the fixed node factor; The odometer factor is determined, specifically: obtaining the odometer factor through the sensor of the mobile robot; The pre-integration factor is determined, specifically: determining an IMU pre-integration and taking it as the pre-integration factor; The determining the fixed node factor specifically comprises: the mobile agent can simultaneously communicate with multiple fixed nodes, the relative poses are respectively obtained, the weights are calculated according to the communication signal strengths, the mobile agent pose is optimized based on the fixed node constraint, and the target function is: wherein, The weight is a function of the signal strength , that is, , and the optimization result is used as the fixed node factor; According to the factor graph, the bias of the IMU is estimated, and the trajectory and the map of the mobile robot are updated; According to the bias of the IMU, the positioning state of the mobile robot is determined.

2. The fixed node-assisted mobile agent SLAM absolute positioning method according to claim 1, characterized in that, The fixed node is installed at a known position in the environment and has wireless communication capability with the mobile agent.

3. The fixed node-assisted mobile agent SLAM absolute localization method according to any one of claims 1-2, characterized in that, The factor graph is obtained according to pose transformation information of the mobile robot, specifically, the factor graph is a mobile robot pose as a node, a pose transformation relationship between nodes as an edge, contains an odometer factor, a pre-integration factor and a fixed node factor, and a target function is: wherein, is a node and an error observed between nodes , is a covariance matrix of observation, is a fixed node factor precision coefficient, and an optimization variable is: .

4. The fixed node-assisted mobile agent SLAM absolute positioning method according to claim 3, characterized in that, The pose of the mobile robot is estimated by the fixed nodes, and the pose estimated by the closest fixed node is taken as the estimation result, and the product of the variance of the multiple fixed node estimation values and the proportionality coefficient is taken as the fixed node factor accuracy coefficient, and the weight of the fixed node factor in the factor graph optimization is adjusted as follows: , wherein, is the proportionality coefficient, is the variance of the multiple fixed node estimation values.​ 5. The fixed node-assisted mobile agent SLAM absolute positioning method according to claim 4, characterized in that, According to the factor graph, the bias of the IMU is estimated, specifically: adding the fixed node factor to the factor graph, taking the node covariance as the weight for weighted sum of squares, using an iterative method to optimize and solve the robot pose, and estimating the IMU bias.

6. The fixed node-assisted mobile agent SLAM absolute positioning method according to claim 5, characterized in that, According to the bias of the IMU, the positioning state of the mobile robot is determined, specifically: when the estimated value of the IMU bias is less than a first threshold value, the positioning of the mobile robot is accurate and no error correction is needed; or when the estimated value of the IMU bias is greater than the first threshold value and less than a second threshold value, the positioning of the mobile robot has a large deviation, and the SLAM system is initialized through the weighted results of the fixed node position estimation and loop detection; or when the estimated value of the IMU bias is greater than the second threshold value, the SLAM system is initialized through the fixed node position estimation.

Images