A GNSS-based robot cooperative positioning system and method

Abstract

The application provides a GNSS-based robot cooperative positioning system and method. A central server of the application dynamically selects anchor node robots and user node robots, constructs an extended Kalman filter for each robot, uses the extended Kalman filter of each robot for prediction and measurement update to obtain the position of each robot at the next moment, the measurement update includes ionosphere-free combined measurement and double-difference measurement, and the position of each user node robot before and after the double-difference measurement at the next moment and the corresponding covariance matrix are used to determine whether each user node robot accepts the double-difference measurement update through a static game model. The user node robot of the application benefits from the high-precision position information of the anchor node robot, the high-precision position information is propagated through double-difference measurement, and the robustness module composed of the random model and the static game model can effectively detect measurement gross errors and weaken the influence.

Description

Technical Field

[0001] This invention belongs to the field of robot positioning technology, and in particular relates to a GNSS-based robot cooperative positioning system and method. Background Technology

[0002] Autonomous robots have been widely used in many modern intelligent systems, such as delivery, rescue, and patrol. Accurate and consistent positioning is crucial for improving the efficiency and redundancy of autonomous mobile robots. Global Navigation Satellite Systems (GNSS) play a vital role in addressing outdoor global positioning challenges. The main GNSS positioning technologies can be divided into two groups: point positioning and relative positioning relying on nearby base stations. Today, Precise Point Positioning (PPP) is gaining increasing attention due to its advantages in global availability and privacy protection. However, PPP convergence can take several minutes, and its accuracy may degrade in challenging GNSS scenarios.

[0003] With the development of communication technology, robots are able to share information with each other. Information sharing among robots makes it possible to improve their localization performance. Utilizing the advantages of robot swarm communication, existing GNSS-based cooperative localization methods include two categories: relative localization and absolute localization. The first category uses differential pseudorange and carrier phase observations to obtain accurate relative positions between robots; however, relative localization methods do not consider the global position of the robots during cooperative localization. The second category, absolute localization methods, uses double-difference pseudorange measurements to improve the absolute localization accuracy of the swarm network; however, localization consistency is still affected by changes in the surrounding environment and cannot be guaranteed. Current robot GNSS cooperative localization methods suffer from low localization accuracy and low global consistency. Summary of the Invention

[0004] To address the aforementioned technical problems, this invention proposes a GNSS-based robot cooperative positioning system and method.

[0005] The technical solution of the present invention is a GNSS-based robot cooperative positioning system, comprising: a central server and multiple robots;

[0006] The central server is connected to each robot in sequence;

[0007] The central server sequentially calculates the trace of the uncertainty matrix for each robot at the current moment, determining the anchor node robot and the user node robot. The central server constructs an extended Kalman filter for each robot. Prediction is performed using the extended Kalman filter for each robot to obtain the predicted state vector and corresponding covariance matrix for the next moment. Measurement updates are performed using the extended Kalman filter for each robot to obtain its position at the next moment. The covariance matrix of the state vector for the next moment is then obtained using the extended Kalman filter for each robot. The position before and after the double-difference measurement of each user node robot at the next moment, along with the corresponding covariance matrix, is used to calculate the gain parameter of the static game model. The gain parameter is used to determine whether each user node robot accepts the double-difference measurement update.

[0008] The technical solution of this invention is a GNSS-based robot cooperative localization method, which specifically includes the following steps:

[0009] Step 1: Each robot wirelessly transmits its current position, uncertainty matrix, and raw GNSS observations to the central server.

[0010] Step 2: The central server calculates the trace of the uncertainty matrix of each robot at the current time in turn, selects the robot with the smallest trace of the uncertainty matrix at the current time as the anchor node robot, and uses the remaining robots as multiple user node robots.

[0011] Step 3: The central server constructs an extended Kalman filter for each robot, obtains the current state vector and covariance matrix of the current state vector of each robot; uses the extended Kalman filter of each robot to make predictions, and obtains the predicted state vector of each robot at the next moment; the covariance matrix of the current state vector of each robot is transformed using the extended Kalman filter of each robot to obtain the covariance matrix of the predicted state vector of each robot at the next moment.

[0012] Step 4: The central server updates the raw GNSS observations of each robot at the current moment through the extended Kalman filter of each robot to obtain the position of each robot at the next moment; and updates the covariance matrix of the predicted state vector of each robot at the next moment using the extended Kalman filter of each robot to obtain the covariance matrix of the state vector of each robot at the next moment.

[0013] Step 5: The central server verifies the covariance matrix of the state vector of each robot at the next time step using a stochastic model, obtaining the stochastically verified covariance matrix of the state vector of each robot at the next time step. After the stochastic model verification, the central server calculates the gain parameter of the static game model using the position before and after the double difference measurement of each user node robot at the next time step and the corresponding covariance matrix. The judgment of the gain parameter determines whether each user node robot accepts the double difference measurement update.

[0014] Preferably, in step 4, the central server updates the raw GNSS observations of each robot at the current moment using the extended Kalman filter of each robot. The specific process is as follows:

[0015] The central server combines the raw GNSS observations of each robot at the current moment into ionospheric-free combined observations. Using the ionospheric-free combined observations, the server updates the measurements of each robot through an extended Kalman filter to obtain the robot's position before double-difference measurement. The central server also calculates the virtual position of each user node robot using the raw GNSS observations of the anchor node robot and each user node robot at the current moment through a double-difference model. Using the virtual position, the server updates the measurements of each user node robot through an extended Kalman filter to obtain the user robot's position after double-difference measurement.

[0016] Preferably, step 5 involves verifying the position of each robot before and after the double-difference measurement and the corresponding covariance matrix at the next moment using a static game model. The specific process is as follows:

[0017]

[0018] Among them, g k,t+1 This represents the gain parameter of the robot at time t+1 for the k-th user node, where min indicates taking the minimum value. k,t+1 ,y k,t+1 ,z k,t+1 ) T Let X represent the coordinates of the k-th user node robot in the X, Y, and Z directions in the Earth-centered Earth-fixed system before the double-difference measurement at time t+1. This represents the coordinates of the object in the X, Y, and Z directions under the Earth-centered Earth-solid system after double-difference measurement. (a) k,t+1 ,b k,t+1 ,c k,t+1 ) T Let represent the eigenvalue vectors of the covariance matrices along the X, Y, and Z axes of the robot at the k-th user node before the double-difference measurement at time t+1, in the Earth-centered Earth-fixed system. The eigenvectors represent the covariance matrices of the k-th user node robot in the X, Y, and Z directions in the Earth-centered Earth-solid system after double-difference measurement at time t+1.

[0019] Preferably, step 5 involves determining whether each user node robot should accept double-difference measurement updates by judging the gain parameter, as detailed below:

[0020] If g k,t+1 >1 indicates that the game has reached a unique Nash equilibrium, and therefore accepts the double difference measurement update. The position of the robot at time t+1 of the k-th user node is:

[0021] If g k,t+1 ≤1 indicates that the game has not reached a unique Nash equilibrium, therefore the double difference measurement update is rejected. The position of the robot at time t+1 of the k-th user node is: (x k,t+1 ,y k,t+1 ,z k,t+1 ) T ;

[0022] One or more technical solutions provided in this invention have at least the following technical effects or advantages:

[0023] If a robot swarm has been running for a period of time and converged to a high-precision positioning result, after applying the cooperative positioning method of this invention, the absolute position of newly added robots will benefit from the high-precision position reference of the swarm, and the accuracy and consistency of robot swarm positioning will be significantly improved. This is because the user node robot benefits from the high-precision position information of the anchor node robot, which is propagated through position constraints obtained through double-difference measurement. Although double-difference measurement is susceptible to multipath errors, the two-stage robust module can effectively detect inconsistent double-difference measurements and eliminate their influence, thus still achieving significant performance gains.

[0024] Furthermore, this invention is less affected by the interval of information sharing between robots. When each robot maintains a certain time interval in communicating with other robots, the overall positioning accuracy is good and stable. This is because in the method of this invention, each robot can maintain high-precision positioning for a period of time, and the fusion of GNSS information between robots can improve the robustness to possible positioning errors. Considering the trade-off between positioning accuracy and communication cost, choosing a suitable communication interval can reduce communication costs and improve the positioning accuracy of the cluster. Attached Figure Description

[0025] Figure 1 : Flowchart of the method according to an embodiment of the present invention. Detailed Implementation

[0026] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0027] In specific implementation, the method proposed in the technical solution of this invention can be automatically executed by those skilled in the art using computer software technology. System devices for implementing the method, such as computer-readable storage media storing the corresponding computer program of the technical solution of this invention and computer equipment including the computer program running the corresponding computer program, should also be within the protection scope of this invention.

[0028] The following is combined with Figure 1 The technical solution described in this invention is a GNSS-based robot cooperative positioning system and method.

[0029] The technical solution of the system in this embodiment of the invention is a GNSS-based robot cooperative positioning system, comprising: a central server and multiple robots;

[0030] The central server is connected to each robot in sequence;

[0031] The central server sequentially calculates the trace of the uncertainty matrix for each robot at the current moment, determining the anchor node robot and the user node robot. The central server constructs an extended Kalman filter for each robot. Prediction is performed using the extended Kalman filter for each robot to obtain the predicted state vector and corresponding covariance matrix for the next moment. Measurement updates are performed using the extended Kalman filter for each robot to obtain its position at the next moment. The covariance matrix of the state vector for the next moment is then obtained using the extended Kalman filter for each robot. The position before and after the double-difference measurement of each user node robot at the next moment, along with the corresponding covariance matrix, is used to calculate the gain parameter of the static game model. The gain parameter is used to determine whether each user node robot accepts the double-difference measurement update.

[0032] The central server is selected as a four-wheeled robot, with a U-blox F9P GNSS receiver and an AMD Ryzen 5 4500U central processor; the robot is selected as a four-wheeled robot, with a U-blox F9P GNSS receiver and an ARM Cortex-A75 central processor.

[0033] All robots autonomously collect GNSS measurements using their respective GNSS receivers and share their positioning information via a communication network. The robot with the least uncertainty is selected as the anchor node robot, and the remaining robots become user node robots. The anchor node robot's GNSS measurements are shared within the network. All robots update their positioning information using GNSS ionospheric-free combined measurements. Simultaneously, user node robots calculate their virtual positions using GNSS double-difference measurement equations established with the anchor node robots and update their positioning information using the virtual position solution. All robots use a stochastic model-based verification method to eliminate gross errors in the measurements. Furthermore, user robots use a static game-theoretic model-based verification method to decide whether to accept the virtual position update. This entire process is independent of a central server, requiring only point-to-point communication. GNSS ionospheric-free combined measurements allow each robot to obtain independent positioning information, GNSS double-difference combined measurements improve the accuracy and consistency of user node robot positioning results, and the verification methods based on stochastic and static game models eliminate gross errors, enhancing the reliability of the positioning information. This solves the technical problem of low accuracy and consistency in existing GNSS collaborative positioning technologies.

[0034] The technical solution adopted in the embodiments of the present invention is a GNSS-based robot cooperative localization method, such as... Figure 1 As shown, the specific steps are as follows:

[0035] Step 1: Each robot wirelessly transmits its current position, uncertainty matrix, and raw GNSS observations to the central server.

[0036] Step 2: The central server calculates the trace of the uncertainty matrix of each robot at the current time in turn, selects the robot with the smallest trace of the uncertainty matrix at the current time as the anchor node robot, and uses the remaining robots as multiple user node robots.

[0037] Step 3: The central server constructs an extended Kalman filter for each robot, obtains the current state vector and covariance matrix of the current state vector of each robot; uses the extended Kalman filter of each robot to make predictions, and obtains the predicted state vector of each robot at the next moment; the covariance matrix of the current state vector of each robot is transformed using the extended Kalman filter of each robot to obtain the covariance matrix of the predicted state vector of each robot at the next moment.

[0038] Step 4: The central server updates the raw GNSS observations of each robot at the current moment through the extended Kalman filter of each robot to obtain the position of each robot at the next moment; and updates the covariance matrix of the predicted state vector of each robot at the next moment using the extended Kalman filter of each robot to obtain the covariance matrix of the state vector of each robot at the next moment.

[0039] In step 4, the central server updates the raw GNSS observations of each robot at the current moment using the extended Kalman filter of each robot. The specific process is as follows:

[0040] The central server combines the raw GNSS observations of each robot at the current moment into ionospheric-free combined observations. Using the ionospheric-free combined observations, the server updates the measurements of each robot through an extended Kalman filter to obtain the robot's position before double-difference measurement. The central server also calculates the virtual position of each user node robot using the raw GNSS observations of the anchor node robot and each user node robot at the current moment through a double-difference model. Using the virtual position, the server updates the measurements of each user node robot through an extended Kalman filter to obtain the user robot's position after double-difference measurement.

[0041] Step 5: The central server verifies the covariance matrix of the state vector of each robot at the next time step using a stochastic model, obtaining the stochastically verified covariance matrix of the state vector of each robot at the next time step. After the stochastic model verification, the central server calculates the gain parameter of the static game model using the position before and after the double difference measurement of each user node robot at the next time step and the corresponding covariance matrix. The judgment of the gain parameter determines whether each user node robot accepts the double difference measurement update.

[0042] Step 5 involves verifying the position of each robot before and after the double-difference measurement and the corresponding covariance matrix at the next time step using a static game model. The specific process is as follows:

[0043]

[0044] Among them, g k,t+1 This represents the gain parameter of the robot at time t+1 for the k-th user node, where min indicates taking the minimum value. k,t+1 ,y k,t+1 ,z k,t+1 ) T Let X represent the coordinates of the k-th user node robot in the X, Y, and Z directions in the Earth-centered Earth-fixed system before the double-difference measurement at time t+1. This represents the coordinates of the object in the X, Y, and Z directions under the Earth-centered Earth-solid system after double-difference measurement. (a) k,t+1 ,b k,t+1 ,c k,t+1 ) T Let represent the eigenvalue vectors of the covariance matrices along the X, Y, and Z axes of the robot at the k-th user node before the double-difference measurement at time t+1, in the Earth-centered Earth-fixed system. The eigenvectors represent the covariance matrices of the k-th user node robot in the X, Y, and Z directions in the Earth-centered Earth-solid system after double-difference measurement at time t+1.

[0045] Step 5, which determines whether each user node robot should accept double-difference measurement updates by judging the gain parameter, is as follows:

[0046] If g k,t+1 >1 indicates that the game has reached a unique Nash equilibrium, and therefore accepts the double difference measurement update. The position of the robot at time t+1 of the k-th user node is:

[0047] If g k,t+1 ≤1 indicates that the game has not reached a unique Nash equilibrium, therefore the double difference measurement update is rejected. The position of the robot at time t+1 of the k-th user node is: (x k,t+1 ,y k,t+1 ,z k,t+1 ) T ;

[0048] It should be understood that any parts not described in detail in this specification belong to the prior art.

[0049] It should be understood that the above description of the preferred embodiments is quite detailed, but it should not be considered as a limitation on the scope of protection of this invention. Those skilled in the art, under the guidance of this invention, can make substitutions or modifications without departing from the scope of protection of the claims of this invention, and all such substitutions or modifications fall within the scope of protection of this invention. The scope of protection of this invention should be determined by the appended claims.

Claims

1. A GNSS-based robot cooperative localization method, applied to a GNSS-based robot cooperative localization system, the robot cooperative localization system comprising a central server and multiple robots, characterized in that, The robot cooperative localization method includes the following steps: Step 1: Each robot wirelessly transmits its current position, uncertainty matrix, and raw GNSS observations to the central server. Step 2: The central server calculates the trace of the uncertainty matrix of each robot at the current time in turn, selects the robot with the smallest trace of the uncertainty matrix at the current time as the anchor node robot, and uses the remaining robots as multiple user node robots. Step 3: The central server constructs an extended Kalman filter for each robot, obtains the current state vector and the covariance matrix of the current state vector of each robot; and calculates the predicted state vector and the covariance matrix of the predicted state vector for the next time step for each robot through the extended Kalman filter. Step 4: The central server updates the raw GNSS observations of each robot at the current moment through the extended Kalman filter of each robot to obtain the position of each robot at the next moment; and updates the covariance matrix of the predicted state vector of each robot at the next moment using the extended Kalman filter of each robot to obtain the covariance matrix of the state vector of each robot at the next moment. The central server updates the raw GNSS observations of each robot at the current moment using the extended Kalman filter of each robot, specifically including: The central server combines the raw GNSS observations of each robot at the current moment into ionospheric-free combined observations. Using the ionospheric-free combined observations, the server updates the measurements of each robot through an extended Kalman filter to obtain the robot's position before double-difference measurement. The central server also calculates the virtual position of each user node robot using the raw GNSS observations of the anchor node robot and each user node robot at the current moment through a double-difference model. Using the virtual position, the server updates the measurements of each user node robot through an extended Kalman filter to obtain the user node robot's position after double-difference measurement. Step 5: The central server verifies the covariance matrix of the state vector of each robot at the next time step using a stochastic model, obtaining the stochastically verified covariance matrix of the state vector of each robot at the next time step. After the stochastic model verification, the central server calculates the gain parameter of the static game model using the position before and after the double difference measurement of each user node robot at the next time step and the corresponding covariance matrix. The judgment of the gain parameter determines whether each user node robot accepts the double difference measurement update. The formula for calculating the gain parameter is as follows: in, This represents the gain parameter of the robot at time t+1 for the k-th user node, where min indicates taking the minimum value. Let X represent the coordinates of the k-th user node robot in the X, Y, and Z directions in the Earth-centered Earth-fixed system before the double-difference measurement at time t+1. This indicates the coordinates of the device measured using the double-difference method in the X-axis, Y-axis, and Z-axis directions within the Earth-centered Earth-solid system. Let represent the eigenvalue vectors of the covariance matrices along the X, Y, and Z axes in the Earth-centered Earth-fixed system before the double-difference measurement of the k-th user node robot at time t+1. The eigenvectors represent the covariance matrices of the k-th user node robot in the X, Y, and Z directions in the Earth-centered Earth-solid system after double-difference measurement at time t+1. The gain parameter is used to determine whether each user node robot should accept double-difference measurement updates. Specifically, this includes: like >1 indicates that the game has reached a unique Nash equilibrium, and therefore accepts the double difference measurement update. The position of the robot at time t+1 of the k-th user node is: ; like ≤1 indicates that the game has not reached a unique Nash equilibrium, therefore the double difference measurement update is rejected. The position of the robot at time t+1 of the k-th user node is: .

2. The GNSS-based robot cooperative localization method according to claim 1, characterized in that: Step 3, which involves calculation using an extended Kalman filter, is detailed below: The predicted state vector of each robot at the next moment is obtained by using the extended Kalman filter of each robot. The covariance matrix of the current state vector of each robot is then transformed using the extended Kalman filter of each robot to obtain the covariance matrix of the predicted state vector of each robot at the next moment.

Images