A distributed robust belief propagation method and device with odometer pre-integration for multi-robot cooperative localization

Abstract

The application provides a kind of distributed robust belief propagation method and equipment with odometer pre-integration for multi-robot cooperative positioning, belongs to the technical field of multi-robot cooperative positioning.The method first proposes an odometer pre-integration measurement model, based on which the messages from the odometer pre-integration factor node are calculated;Then the probability density of the external measurement factor node is modeled as a student t distribution, and the improved measurement noise covariance matrix is derived using variational inference, based on which the messages from the absolute measurement and relative measurement factor nodes are calculated;Finally, the beliefs of all variable nodes in the sliding window are iteratively updated using the messages from the odometer pre-integration factor node, the absolute measurement factor node and the relative measurement factor node.The distributed robust belief propagation method with odometer pre-integration proposed by the application can significantly reduce the influence of measurement nonlinearity and measurement outliers, improve the accuracy of pose estimation, and has practical engineering application significance.

Description

Technical Field

[0001] The invention relates to the field of multi-robot cooperative localization technology, and in particular to a distributed robust belief propagation method and device with odometry pre-integration for multi-robot cooperative localization. Background Technology

[0002] Multi-robot systems are renowned for their exceptional efficiency and play a crucial role in applications such as search and rescue, underwater operations, and logistics management. Accurate attitude estimation is essential for the effective execution of these tasks. In environments where precise absolute information (such as GNSS signals, landmark references, etc.) is always available, multi-robot systems can achieve accurate attitude estimation. However, in real-world scenarios, the presence of obstacles, electromagnetic interference, or other factors may render absolute information only intermittently available or completely unavailable, significantly reducing the robot's positioning accuracy. Cooperative localization, which integrates measurements from both robots, has proven effective in mitigating this problem and has become an indispensable technology for multi-robot systems.

[0003] Existing cooperative localization methods are mainly divided into two categories: centralized methods and distributed methods. Centralized methods require a central node in the cluster to run a common filter to estimate the pose of all robots; however, these methods suffer from poor flexibility and a huge communication burden. In distributed methods, each robot runs its own filter, enabling independent measurement and information sharing, which alleviates the problems of centralized methods to some extent.

[0004] Gaussian belief propagation, with its advantages of being asynchronous, flexible, scalable, and having low communication overhead, has become a future trend in distributed cooperative localization. However, existing distributed cooperative localization methods based on Gaussian belief propagation perform poorly in environments with measurement outliers and nonlinear measurement, severely hindering their practical application. There are two main reasons for these problems: firstly, the presence of measurement outliers causes measurement noise to deviate from a Gaussian distribution, violating the Gaussian noise assumption and affecting the accuracy of attitude estimation; secondly, existing distributed cooperative localization methods based on Gaussian belief propagation are susceptible to the effects of nonlinear measurements, which may affect the accuracy of their pose estimation. Summary of the Invention

[0005] To overcome the problems of existing methods, this invention provides a distributed robust belief propagation method with odometry pre-integration to reduce the impact of measurement outliers and measurement nonlinearity on pose estimation. The technical solution adopted in this invention is as follows: An odometry pre-integration model is proposed, based on which messages from odometry factor nodes are calculated. The iterative update characteristics of Gaussian belief propagation are utilized to reduce the impact of measurement nonlinearity on pose estimation. A method is proposed to model the probability density of external measurement factors as a Student's t-distribution, and the measurement covariance matrix is ​​improved based on variational inference. Based on this, messages from external measurement factor nodes are accurately calculated, reducing the impact of measurement outliers on pose estimation.

[0006] This invention provides a distributed robust belief propagation method with odometry pre-integration for multi-robot cooperative localization, comprising:

[0007] Step 1: Establish the kinematic and measurement models of the multi-robot system;

[0008] Step 2: Calculate the covariance matrix of the odometer pre-integration measurement and the corresponding measurement noise using the odometer pre-integration;

[0009] Step 3: When a robot detects a landmark or another robot, or is detected by another robot, it calculates the message from the odometry factor node based on the odometry pre-integration measurement and the measurement noise covariance matrix.

[0010] Step 4: The robot uses the camera to detect landmarks or other robots, uses the variational inference process to estimate the noise covariance matrix of the improved measurement, and calculates the messages from the measurement factor nodes.

[0011] Step 5: Based on the messages from the odometry factor nodes and the measurement factor nodes obtained in Steps 3 and 4, calculate the robot's belief information vector and information matrix.

[0012] Step 6: Construct a Gaussian belief iterative update process. Iteratively update the beliefs of the variable nodes within the sliding window using the information vector and information matrix of the robot's beliefs received, and output the robot's pose estimate at the current moment.

[0013] Furthermore, in step 1, the pose calculation of the kinematic model... Calculations are performed using the linear and angular velocities output from the odometer:

[0014]

[0015] in, Represents robots At any moment The pose state. and Indicates location, Indicates direction; and These represent the linear velocity and angular velocity output by the odometer, respectively. Indicates the discretization time; Represents the state transition function; The noise representing the linear and angular velocities output by the odometer obeys... ;

[0016] When the robot Landmarks were detected using the onboard camera. At that time, the absolute measurement model is:

[0017]

[0018] in, Represents robots At any moment Detection landmarks The obtained absolute measurement; Indicates landmark Location; Represents an absolute measurement function; and For absolute measurement noise The components represent the noise in distance measurement and azimuth measurement, respectively; For pose estimation;

[0019] When the robot The robot was detected using the onboard camera. At that time, the relative measurement model is:

[0020]

[0021] in, Represents robots The robot was detected using the onboard camera. The obtained relative measurements; Represents a relative measurement function; and It is relative measurement noise The components represent the noise in distance measurement and azimuth measurement, respectively.

[0022] Furthermore, in step 2, the odometer pre-integration measurement and the corresponding measurement noise covariance matrix are calculated using pre-integration, specifically as follows:

[0023] Step 2.1: Computational Robot exist Direction angle at time :

[0024]

[0025] in, Represents robots exist The direction angle at that moment; Represents robots exist angular velocity at time t; express Time's up The time of a moment;

[0026] Step 2.2: Calculation Time to the present Rotation matrix at time :

[0027]

[0028] in, Represents the rotation matrix. ;

[0029] Step 2.3: Calculate the odometer pre-integration measurement :

[0030]

[0031] in, ;

[0032] The corresponding measurement equation is:

[0033]

[0034] in, This represents the mileage measurement function. This represents the odometer noise, which follows a Gaussian distribution. .

[0035] Step 2.4: Calculate the odometer pre-integration measurement covariance matrix :

[0036]

[0037] in, ; ; and These represent the noise covariances of the linear velocity and angular velocity output by the odometer, respectively. , This indicates the operation of taking an antisymmetric matrix; intermediate variables .

[0038] Furthermore, in step 3, if the robot When a landmark or other robot is detected, or when a robot detects a landmark, the information vector and information matrix of the message from the odometry factor node are calculated, specifically:

[0039] Step 3.1: Calculate the odometer readings related to the state. Jacobian matrix :

[0040]

[0041] Step 3.2: Calculate the odometer readings related to the state. Jacobian matrix :

[0042]

[0043] Step 3.3: Calculate the information vector of messages from the odometry pre-integration factor node. :

[0044]

[0045]

[0046]

[0047]

[0048]

[0049] in, This is the inverse matrix of the odometer pre-integration measurement covariance matrix. , , and express Time Robot The information vector and information matrix regarding one's own beliefs, this value is determined by the robot. exist All measurements are constantly integrated. and express Time Robot The information vector and information matrix regarding one's own beliefs are obtained from the pose calculation formula; , For mileage measurement function; and For robots from variable node Flow to odometry factor node The information vector and information matrix of the message.

[0050] Step 3.4: Calculate the information matrix of messages from the odometry pre-integration factor node. :

[0051]

[0052] .

[0053] Furthermore, in step 4, if the robot Landmarks detected using cameras The improved noise covariance matrix of the absolute measurement is estimated using a variational inference process, and the information vector and information matrix of the messages from the absolute measurement factor nodes are calculated; specifically:

[0054] Step 4.1.1: Set expectations Number of iterations ;

[0055] Step 4.1.2: Computational Robot Information vector of current belief With information matrix :

[0056]

[0057]

[0058] in, express Time and Variable Nodes Connected measurement factor nodes ;

[0059] Step 4.1.3: Calculate the measurement linearization point and , Indicates landmark Location;

[0060] Step 4.1.4: Calculate the measurement Jacobian matrix :

[0061]

[0062] in, , , Represents an identity matrix of appropriate dimensions. ;

[0063] Step 4.1.5: Calculate the inverse matrix of the estimated measurement noise covariance matrix. :

[0064]

[0065] in, Let be the nominal absolute measurement covariance matrix; These are the estimated parameters for the measurement covariance matrix;

[0066]

[0067]

[0068]

[0069]

[0070] in, Dimensions indicating absolute measurement; This represents the degrees of freedom of the student's t-distribution; This indicates a trace operation.

[0071] Step 4.1.6: Calculate the information vector of the message from the absolute measurement factor node. :

[0072]

[0073] Step 4.1.7: Calculate the information matrix of messages from the absolute measurement factor nodes. :

[0074]

[0075] Step 4.1.8: Repeat steps 4.1.2 to 4.1.7 above until the specified number of iterations is reached. , The number of iterations is for variational inference.

[0076] Furthermore, in step 4, if the robot Robot detected using camera The improved noise covariance matrix of the relative measurement is estimated using a variational inference process, and the information vector and information matrix of the messages from the absolute measurement factor nodes are calculated; specifically:

[0077] Step 4.2.1: Based on the robot The linearization point of the broadcast's belief calculation , and For robots At any moment The information vector and information matrix of the robot's beliefs It came via broadcast;

[0078] Step 4.2.2: Set expectations Number of iterations ;

[0079] Step 4.2.3: Calculate Measurements Regarding the linearization point Jacobian matrix :

[0080]

[0081] in, , Represents robots Compared to robots In the robot The relative position in the coordinate system, i.e. , Represents robots exist At the moment The first two dimensions of the pose state at the next iteration refer to the robot's position state. Location; It refers to robots Location, Represents the 2-norm;

[0082] Step 4.2.4: Computational Robot Information vector of current belief With information matrix :

[0083] Step 4.2.5: Calculate the measurement linearization point and ;

[0084] Step 4.2.6: Calculate the measurement Jacobian matrix :

[0085]

[0086] in, Represents an identity matrix of appropriate dimensions. ;

[0087] Step 4.2.7: Calculate the inverse matrix of the estimated measurement noise covariance matrix. :

[0088]

[0089]

[0090]

[0091]

[0092]

[0093] in, For measurement dimensions; Let be the degrees of freedom of the student's t-distribution; The nominal noise covariance for relative measurements;

[0094] Step 4.2.8: Calculate the information vector from the relative measurement factor node message. :

[0095]

[0096]

[0097]

[0098]

[0099] in, and For robots Broadcast's variable nodes The information vector and information matrix of the latest news obtained;

[0100] Step 4.2.9: Calculate the information matrix from the relative measurement factor node messages. :

[0101]

[0102]

[0103]

[0104] Step 4.2.10: Repeat steps 4.2.3 to 4.2.9 above until the specified number of iterations is reached. , The number of iterations is for variational inference.

[0105] Furthermore, in step 5, the robot Information vector of beliefs for:

[0106]

[0107] in, express Time and Variable Nodes Connected measurement factor nodes ;

[0108] The robot The matrix vector of beliefs for:

[0109] .

[0110] Furthermore, in step 6, each iteration involves calculating the latest belief of the corresponding variable node based on all messages received by the historical variable nodes within the sliding window; and combining the messages from the odometer factor nodes, absolute measurement factor nodes, and relative measurement factor nodes with the information vector from the odometer factor nodes from the future to the current time. With information matrix , Calculate the latest belief of the variable node; update the belief of the variable node sequentially from the current time to the starting point of the sliding window, update the belief of the variable node sequentially from the starting point of the sliding window to the current time, and output the latest belief of the variable node at the current time.

[0111] The present invention also provides a computer device / equipment / system, including a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the distributed robust belief propagation method with odometry pre-integration for multi-robot cooperative localization as described in any of the preceding claims.

[0112] The present invention also provides a computer-readable storage medium having a computer program / instructions stored thereon, which, when executed by a processor, implements the steps of the distributed robust belief propagation method with odometry pre-integration for multi-robot cooperative localization as described in any of the preceding claims.

[0113] The present invention also provides a computer program product, including a computer program / instructions that, when executed by a processor, implement the steps of the distributed robust belief propagation method with odometry pre-integration for multi-robot cooperative localization as described in any of the preceding claims.

[0114] The beneficial effects of this invention are as follows:

[0115] This invention provides a distributed robust belief propagation method with odometry pre-integration for multi-robot cooperative localization. This method can reduce the influence of measurement nonlinearity and measurement outliers, and improve the accuracy of pose estimation. In practical applications, ranging measurements are usually nonlinear, and electromagnetic interference, occlusion, and other factors often lead to outliers in the measurements. The proposed method can significantly solve these problems and has practical significance. Attached Figure Description

[0116] Figure 1A flowchart of a distributed robust belief propagation method with odometry pre-integration for multi-robot cooperative localization is provided for the implementation of this invention.

[0117] Figure 2 The parameter values ​​used in the implementation of this invention;

[0118] Figure 3 The accuracy of each robot position estimation provided for implementation of this invention;

[0119] Figure 4 The accuracy of each robot orientation estimation provided for the implementation of this invention. Detailed Implementation

[0120] The specific embodiments of the present invention will be described in further detail below with reference to the accompanying drawings and examples. The following examples are for illustrative purposes only and are not intended to limit the scope of the invention.

[0121] This invention discloses a distributed robust belief propagation method with odometry pre-integration for multi-robot cooperative localization, comprising:

[0122] Step 1: Define the robot's motion model and measurement model, and determine the relevant input parameters.

[0123] Step 2: Each robot uses the linear velocity and angular velocity output by the odometry to calculate its pose, and uses odometry pre-integration to calculate the odometry pre-integration measurement and the corresponding measurement noise covariance matrix.

[0124] Step 3: Robot When a landmark or other robot is detected, or when it is detected by another robot, it needs to utilize the information provided in step 2. and Calculate the message from the odometer factor node.

[0125] Step 4: If the robot Landmarks detected using cameras This requires using a variational inference process to estimate the noise covariance matrix of the improved absolute measurement. Based on this, it computes messages from the absolute measurement factor nodes.

[0126] Step 5: If the robot Robot detected using camera This requires using a variational inference process to estimate the improved noise covariance matrix of the relative measurement. Based on this, it computes messages from the relative measurement factor nodes.

[0127] Step 6: Based on the messages from the measurement factor nodes calculated in Steps 3-5, calculate the robot. Information vector of beliefs With information matrix .

[0128] Step 7: Iterative updates using Gaussian belief propagation reduce the impact of measurement nonlinearity on pose estimation. Each iteration calculates the latest belief of the corresponding variable node based on all messages received by the historical variable nodes within the sliding window. First, steps 3 and 4 are reused to calculate the messages from the odometry factor node, absolute measurement factor node, and relative measurement factor node. Second, the corresponding formulas in step 3 are used to calculate... and (b>k, indicating the distance between the variable node and the sliding window) Adjacent variable nodes Finally, the latest belief of the variable node is calculated using the formula in step 6. The belief of the variable node is updated sequentially from the current time to the starting point of the sliding window, and then sequentially from the starting point of the sliding window back to the current time. The latest belief of the variable node at the current time is then output.

[0129] Step 8: Proceed to step 3.

[0130] Example 1

[0131] A flowchart of a distributed robust belief propagation method with odometry pre-integration for multi-robot cooperative localization is shown below. Figure 1 As shown, it includes the following steps:

[0132] Step 1: Assume that in the cooperative localization scenario there are A robot and Each robot is equipped with a wheeled odometer for pose estimation and a camera to detect other landmarks (with precisely known locations) or other robots to obtain absolute or relative measurements. The pose estimation formula for each robot is as follows:

[0133]

[0134] in, Represents robots At any moment The pose state. and Indicates location, Indicates direction, and These represent the linear velocity and angular velocity output by the odometer, respectively. Indicates the discretization time. Represents the state transition function. The noise representing the linear and angular velocities output by the odometer obeys... .

[0135] When the robot Landmarks were detected using the onboard camera. At that time, the corresponding measurement was described as:

[0136]

[0137] in, Represents robots At any moment Detection landmarks The obtained absolute measurement; Indicates landmark Location; Represents an absolute measurement function; and It is absolute measurement noise The components represent the noise in distance measurement and azimuth measurement, respectively.

[0138] When the robot The robot was detected using the onboard camera. At that time, the corresponding measurement was described as:

[0139]

[0140] in, Represents robots The robot was detected using the onboard camera. The obtained relative measurements; Represents a relative measurement function; and It is relative measurement noise The components represent the noise in distance measurement and azimuth measurement, respectively.

[0141] Then, input the initial pose and covariance of each robot, and input the sliding window length. Number of iterations for variational inference Parameters such as these.

[0142] Step 2: Each robot uses the linear and angular velocities output by the odometry to calculate its pose, and uses pre-integration to calculate the covariance matrix of the odometry pre-integration measurement and the corresponding measurement noise. Let... =1, the specific method is as follows:

[0143] Step 2.1: Calculation ,in Represents robots exist angular velocity at time t, express Time's up The time of moment, and They represent robots exist and The direction angle at any given moment.

[0144] Step 2.2: Calculation Time's up Rotation matrix at time , The rotation matrix is ​​defined as follows:

[0145]

[0146] Step 2.3: Calculation ;

[0147] Step 2.4: Calculation ;

[0148] The corresponding measurement equation is:

[0149]

[0150] in, This represents the mileage measurement function. This represents the odometer noise, which follows a Gaussian distribution. .

[0151] Step 2.5: Calculation ,in ;

[0152] Step 2.6: Calculate the odometer pre-integration measurement covariance matrix:

[0153]

[0154] in, , , and Let represent the noise covariance of the linear velocity and angular velocity output by the odometer, respectively. , This indicates the operation of taking an antisymmetric matrix.

[0155] Step 2.7: Calculation .

[0156] Step 3: If the robot When a landmark or other robot is detected, or when it is detected by another robot, it needs to use the odometry pre-integration measurement provided in step 2. Covariance matrix of pre-integrated measurement with odometer Calculate the message from the odometry factor node; step 3 is to enable the robot to... (Other landmarks or robots detected) or other robots (robots) (Detected by other robots) Calculate the linearization point of the measurement.

[0157] Step 3.1: Calculate the odometer readings related to the state. Jacobian matrix:

[0158]

[0159] Step 3.2: Calculate the odometer readings related to the state. Jacobian matrix:

[0160]

[0161] Step 3.3: Calculate the information vector of the message from the odometry pre-integration factor node:

[0162]

[0163] Step 3.4: Calculate the information matrix of messages from the odometry pre-integration factor node:

[0164]

[0165]

[0166] in, It is the inverse matrix of the odometer pre-integration measurement covariance matrix calculated in step 2. , , and express Time Robot The information vector and information matrix regarding one's own beliefs, this value is determined by the robot. exist All measurements are constantly integrated. and express Time Robot The information vector and information matrix regarding one's own beliefs are obtained from the pose calculation formula; , For mileage measurement function; and For robots from variable node Flow to odometry factor node The information vector and information matrix of the message.

[0167] Step 4: If the robot Landmarks detected using cameras This requires estimating the improved noise covariance matrix of the absolute measurement using a variational inference process. Based on this, it calculates the messages from the absolute measurement factor nodes, with the following specific steps:

[0168] Step 4.1: Settings , Express the expectation; set the number of iterations. .

[0169] Step 4.2: Computational Robot Information vector of current belief With information matrix ,in express Time and Variable Nodes Connected measurement factor nodes It performs a fusion calculation on all the information vectors and information matrices obtained at the current moment.

[0170] Step 4.3: Calculate the measurement linearization point and

[0171] Step 4.4: Calculate the measurement Jacobian matrix , , Represents an identity matrix of appropriate dimensions. .

[0172] Step 4.5: Calculation .

[0173] Step 4.6: Calculate the inverse matrix of the estimated measurement noise covariance matrix. , Let be the nominal absolute measurement covariance matrix.

[0174] Step 4.7: Calculation and ,in Indicates the measurement dimension. This represents the degrees of freedom of the student's t-distribution. This indicates the trace operation.

[0175] Step 4.8: Calculate the estimated parameters of the measurement covariance matrix .

[0176] Step 4.9: Calculate the information vector of the message from the absolute measurement factor node:

[0177]

[0178] Step 4.10: Calculate the information matrix of messages from the absolute measurement factor nodes:

[0179]

[0180] Step 4.11: Repeat steps 4.2 to 4.10 until the specified number of iterations is reached. This step reduces the impact of outliers from landmark measurements on pose estimation through variational iteration.

[0181] Step 5: If the robot Robot detected using camera This requires estimating the improved noise covariance matrix of the relative measurement using a variational inference process. Based on this, it calculates the messages from the relative measurement factor nodes, with the following specific steps:

[0182] Step 5.1: Based on the robot The linearization point of the broadcast's belief calculation .

[0183] Step 5.2: Settings Set the number of iterations. .

[0184] Step 5.3: Calculate Measurements Regarding the linearization point Jacobian matrix ,in , Represents robots Compared to robots In the robot The relative position in the coordinate system, i.e. , Represents robots exist The first two dimensions of the pose state at any given moment, referring to the robot's... Location; It refers to robots Location, It represents the 2-norm.

[0185] Step 5.4: Computational Robot Information vector of current belief With information matrix It performs a fusion calculation on all the information vectors and information matrices obtained at the current moment.

[0186] Step 5.5: Calculate the measurement linearization point and .

[0187] Step 5.6: Calculate the measurement Jacobian matrix , Represents an identity matrix of appropriate dimensions. .

[0188] Step 5.7: Calculation .

[0189] Step 5.8: Calculate the inverse matrix of the estimated measurement noise covariance matrix. .

[0190] Step 5.9: Calculation and , For measurement dimensions; Let be the degrees of freedom of the student's t-distribution.

[0191] Step 5.10: Calculate the estimated parameters of the measurement covariance matrix .

[0192] Step 5.11: Calculate the information vector from the relative measurement factor node message:

[0193]

[0194]

[0195] in, and For robots The information vector and information matrix of the latest broadcast message.

[0196] Step 5.12: Calculate the information matrix from the relative measurement factor node messages:

[0197]

[0198]

[0199]

[0200] Step 5.13: Repeat steps 5.3 to 5.12 until the specified number of iterations is reached. This step reduces the impact of outliers from measurements from other robots on pose estimation through variational iteration.

[0201] Step 6: Based on the messages from the measurement factor nodes calculated in Steps 3-5, calculate the robot. Information vector of beliefs With information matrix , express Time and Variable Nodes Connected measurement factor nodes Then, perform fusion calculation on all the information vectors and information matrices obtained at the current moment, and the calculation method is the same as in steps 4.2 and 5.4.

[0202] Step 7: Reduce the impact of measurement nonlinearity on pose estimation by using iterative updates of Gaussian belief propagation. The specific steps are as follows:

[0203] Step 7.1: Calculate the information vector from the odometry factor nodes from the future time to the current time. With information matrix The calculation steps are the same as above. and .

[0204] Step 7.2: Fusion Computing Robot Information vector of all beliefs and information matrix , express Time and Variable Nodes Connected measurement factor nodes Then, perform fusion calculation on all the information vectors and information matrices obtained at the current moment, using the same calculation method as steps 6, 4.2 and 5.4.

[0205] Step 7.3: Using the messages received by the historical variable nodes within the sliding window, update the beliefs of the variable nodes sequentially from the current moment to the starting point of the sliding window.

[0206] Step 7.4: Based on the messages received by the historical variable nodes within the sliding window, update the beliefs of the variable nodes sequentially from the starting point of the sliding window to the current time. Multiple iterative updates reduce the impact of measurement nonlinearity on pose estimation.

[0207] Step 8: Output the current time. pose estimation, Proceed to step 3.

[0208] Step 9: Display the results, with the position estimation accuracy for each robot as shown below. Figure 3 As shown, the direction estimation accuracy is as follows: Figure 4 As shown. The parameters used in the simulation are listed below. Figure 2 .

[0209] It can be seen that in multi-robot cooperative localization scenarios with nonlinear measurements and outlier measurements, the proposed method can accurately estimate the robot's pose state, proving that the method is effective.

[0210] In particular, in some preferred embodiments of the present invention, a computer device is also provided, including a memory and a processor and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the distributed robust belief propagation method with odometry pre-integration for multi-robot cooperative localization as described in any of the above embodiments.

[0211] In some other preferred embodiments of the present invention, a computer-readable storage medium is also provided, on which a computer program / instruction is stored, wherein the computer program, when executed by a processor, implements the steps of the distributed robust belief propagation method with odometry pre-integration for multi-robot cooperative localization as described in any of the above embodiments.

[0212] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the above embodiment of a distributed robust belief propagation method with odometry pre-integration for multi-robot cooperative localization, which will not be repeated here.

[0213] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., refer to specific features, structures, materials, or characteristics described in connection with that embodiment or example, which are included in at least one embodiment or example of the present invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

[0214] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the description of this invention, "N" means at least two, such as two, three, etc., unless otherwise explicitly specified.

[0215] Any process or method description in the flowchart or otherwise herein can be understood as representing a module, segment, or portion of code comprising one or more N executable instructions for implementing custom logic functions or processes, and the scope of preferred embodiments of the invention includes additional implementations in which functions may be performed not in the order shown or discussed, including substantially simultaneously or in reverse order depending on the functions involved, as should be understood by those skilled in the art to which embodiments of the invention pertain.

[0216] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a sequenced list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (such as a computer-based system, a processor-included system, or other system that can fetch and execute instructions from, an instruction execution system, apparatus, or device). For the purposes of this specification, "computer-readable medium" can be any means that can contain, store, communicate, propagate, or transmit programs for use by, or in conjunction with, an instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of computer-readable media include: an electrical connection having one or more wires (electronic device), a portable computer disk drive (magnetic device), random access memory (RAM), read-only memory (ROM), erasable and editable read-only memory (EPROM or flash memory), fiber optic devices, and portable optical disc read-only memory (CDROM). Alternatively, the computer-readable medium may be paper or other suitable media on which the program can be printed, since the program can be obtained electronically, for example, by optically scanning the paper or other medium, followed by editing, interpreting, or otherwise processing as necessary, and then stored in a computer memory.

[0217] It should be understood that various parts of the present invention can be implemented in hardware, software, firmware, or a combination thereof. In the above embodiments, the N steps or methods can be implemented in software or firmware stored in memory and executed by a suitable instruction execution system. For example, if implemented in hardware as in another embodiment, it can be implemented using any one or a combination of the following techniques known in the art: discrete logic circuits having logic gates for implementing logical functions on data signals, application-specific integrated circuits (ASICs) having suitable combinational logic gates, programmable gate arrays (PGAs), field-programmable gate arrays (FPGAs), etc.

[0218] Those skilled in the art will understand that all or part of the steps of the methods in the above embodiments can be implemented by a program instructing related hardware. The program can be stored in a computer-readable storage medium, and when executed, the program includes one or a combination of the steps of the method embodiments.

[0219] Furthermore, the functional units in the various embodiments of the present invention can be integrated into a processing module, or each unit can exist physically separately, or two or more units can be integrated into a module. The integrated module can be implemented in hardware or as a software functional module. If the integrated module is implemented as a software functional module and sold or used as an independent product, it can also be stored in a computer-readable storage medium.

[0220] The storage medium mentioned above can be a read-only memory, a disk, or an optical disk, etc. Although embodiments of the present invention have been shown and described above, it is to be understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of the present invention.

Claims

1. A distributed robust belief propagation method with odometry pre-integration for multi-robot cooperative localization, characterized in that, include: Step 1: Establish the kinematic and measurement models of the multi-robot system; Pose estimation of the kinematic model Calculations are performed using the linear and angular velocities output from the odometer: ， in, Represents robots At any moment The pose state. and Indicates location, Indicates direction; and These represent the linear velocity and angular velocity output by the odometer, respectively. Indicates the discretization time; Represents the state transition function; The noise representing the linear and angular velocities output by the odometer obeys... ; When the robot Landmarks were detected using the onboard camera. At that time, the absolute measurement model is: ， in, Represents robots At any moment Detection landmarks The obtained absolute measurement; Indicates landmark Location; Represents an absolute measurement function; and For absolute measurement noise The components represent the noise in distance measurement and azimuth measurement, respectively; For pose estimation; When the robot The robot was detected using the onboard camera. At that time, the relative measurement model is: ， in, Represents robots The robot was detected using the onboard camera. The obtained relative measurements; Represents a relative measurement function; and It is relative measurement noise The components represent the noise in distance measurement and azimuth measurement, respectively; Step 2: Calculate the covariance matrix of the odometer pre-integration measurement and the corresponding measurement noise using pre-integration; Step 2.1: Computational Robot exist Direction angle at time : ， in, Represents robots exist The direction angle at that moment; Represents robots exist angular velocity at time t; express Time's up The time of a moment; Step 2.2: Calculation Time to the present Rotation matrix at time : ， in, Represents the rotation matrix. ; Step 2.3: Calculate the odometer pre-integration measurement : ， in, ; The corresponding measurement equation is: ， in, This represents the mileage measurement function. This represents the odometer noise, which follows a Gaussian distribution. ; Step 2.4: Calculate the odometer pre-integration measurement covariance matrix : ， in, ; ; and These represent the noise covariances of the linear velocity and angular velocity output by the odometer, respectively. , This indicates the operation of taking an antisymmetric matrix; ; Step 3: When a robot detects a landmark or another robot, or is detected by another robot, it calculates the message from the odometry factor node based on the odometry pre-integration measurement and the measurement noise covariance matrix. Step 4: When the robot detects a landmark or other robot using the camera, it estimates the improved noise covariance matrix of the measurement using a variational inference process and calculates the messages from the measurement factor nodes. If the robot Landmarks detected using cameras The improved noise covariance matrix of the absolute measurement is estimated using a variational inference process, and the information vector and information matrix of the messages from the absolute measurement factor nodes are calculated; specifically: Step 4.1.1: Set expectations Number of iterations ; Step 4.1.2: Computational Robot Information vector of current belief With information matrix : ， in, express Time and Variable Nodes Connected measurement factor nodes ; Step 4.1.3: Calculate the measurement linearization point and ; Step 4.1.4: Calculate the measurement Jacobian matrix : ， in, , , Represents an identity matrix of appropriate dimensions. ; Step 4.1.5: Calculate the inverse matrix of the estimated measurement noise covariance matrix. : ， in, Let be the nominal absolute measurement covariance matrix; These are the estimated parameters for the measurement covariance matrix; Calculate the first The estimated parameters of the measurement covariance matrix for the next iteration are: ， in, For measurement dimensions; Let be the degrees of freedom of the student's t-distribution; Step 4.1.6: Calculate the information vector of the message from the absolute measurement factor node. : Step 4.1.7: Calculate the information matrix of messages from the absolute measurement factor nodes. : Step 4.1.8: Repeat steps 4.1.2 to 4.1.7 above until the specified number of iterations is reached. , For variational inference, the number of iterations is determined; if the robot Robot detected using camera The improved noise covariance matrix of the relative measurement is estimated using a variational inference process, and the information vector and information matrix of the messages from the relative measurement factor nodes are calculated; specifically: Step 4.2.1: Based on the robot The linearization point of the broadcast's belief calculation ; Step 4.2.2: Set expectations Number of iterations ; Step 4.2.3: Calculate Measurements Regarding the linearization point Jacobian matrix : ， in, , Represents robots Compared to robots In the robot The relative position in the coordinate system, i.e. , Represents robots exist The first two dimensions of the pose state at any given moment, referring to the robot's... Location; It refers to robots Location, Represents the 2-norm; Step 4.2.4: Computational Robot Information vector of current belief With information matrix ; Step 4.2.5: Calculate the measurement linearization point and ; Step 4.2.6: Calculate the measurement Jacobian matrix : ， in, Represents an identity matrix of appropriate dimensions. ; Step 4.2.7: Calculate the inverse matrix of the estimated measurement noise covariance matrix. : ， in, For measurement dimensions; Let be the degrees of freedom of the student's t-distribution; The nominal noise covariance for relative measurements; Step 4.2.8: Calculate the information vector from the relative measurement factor node message. : ， in, and For robots Broadcast's variable nodes The information vector and information matrix of the latest news obtained; Step 4.2.9: Calculate the information matrix from the relative measurement factor node messages. : Step 4.2.10: Repeat steps 4.2.3 to 4.2.9 above until the specified number of iterations is reached. , For variational inference, the number of iterations is... Step 5: Based on the messages from the odometry factor nodes and the measurement factor nodes obtained in Steps 3 and 4, calculate the robot's belief information vector and information matrix. Step 6: Construct a Gaussian belief iterative update process. Iteratively update the beliefs of the variable nodes within the sliding window using the received robot belief information vector and information matrix, outputting the robot's pose estimate at the current moment. Each iteration calculates the latest belief of the corresponding variable node based on all messages received from historical variable nodes within the sliding window. This is done by combining the messages from odometry factor nodes, absolute measurement factor nodes, and relative measurement factor nodes with the information vector from odometry factor nodes from the future to the current moment. With information matrix , Calculate the latest belief of the variable node; update the belief of the variable node sequentially from the current time to the starting point of the sliding window, update the belief of the variable node sequentially from the starting point of the sliding window to the current time, and output the latest belief of the variable node at the current time.

2. The distributed robust belief propagation method with odometry pre-integration for multi-robot cooperative localization according to claim 1, characterized in that, In step 3, the robot When a landmark or other robot is detected, or when a robot detects a landmark, the information vector and information matrix of the message from the odometry factor node are calculated, specifically: Step 3.1: Calculate the odometer readings related to the state. Jacobian matrix : Step 3.2: Calculate the odometer readings related to the state. Jacobian matrix : Step 3.3: Calculate the information vector of messages from the odometry pre-integration factor node. : ， in, This is the inverse matrix of the odometer pre-integration measurement covariance matrix. , , and express Time Robot Information vectors and information matrices regarding one's own beliefs, provided by the robot exist All measurements are constantly integrated. and express Time Robot The information vector and information matrix regarding one's own beliefs are obtained from the pose calculation formula; , For mileage measurement function; and For robots from variable node Flow to odometry factor node The information vector and information matrix of the message; Step 3.4: Calculate the information matrix of messages from the odometry pre-integration factor node. : 。 3. The distributed robust belief propagation method with odometry pre-integration for multi-robot cooperative localization according to claim 1, characterized in that, In step 5, the robot Information vector of beliefs for: ， in, express Time and Variable Nodes Connected measurement factor nodes ; The robot Information matrix for: 。 4. A computer device, comprising a memory, a processor, and a computer program stored in the memory, characterized in that, The processor executes the computer program to implement the steps of the method according to any one of claims 1 to 3.

Images