Robot cooperative positioning method and system based on infinite dimension parameter model

Abstract

The invention provides a robot cooperative positioning method and system based on an infinite-dimensional parameter model, and relates to the field of intelligent robots, and the method comprises the steps: for a robot cluster composed of a plurality of robots, each robot in the cluster obeys an infinite-dimensional ARX model; for each robot and any given moment, constructing a regression vector of a historical position state and historical control input; each robot uses the estimated value of the current moment, the information matrix, the regression vector and the observation output of the next moment to calculate an intermediate estimated value and an intermediate information matrix of the next moment in combination with a recursive least square algorithm; and each robot performs information interaction with neighbors in the neighborhood topology at the current moment, and obtains an updated estimated value and an updated information matrix according to a convex combination of neighbor information weights so as to output a three-dimensional position state and an estimated value of control input. The method can solve the problems of insufficient expression ability, low monomer estimation precision and the like of a traditional method.

Description

Technical Field

[0001] The present invention relates to the field of intelligent robots, and in particular, to a robot cooperative localization method and system based on an infinite-dimensional parameter model. Background Art

[0002] With the rapid development of intelligent robot systems towards clustering and autonomy, multi-robot systems have been widely used in industrial automation, disaster rescue, intelligent logistics and other fields. As a fundamental part to ensure group collaboration, cooperative localization technology faces core challenges such as dynamic environment perception, group behavior modeling and real-time calculation. In typical localization scenarios such as mobile robot SLAM and UAV formation control, the dynamic characteristics of the system often present the following features: (1) High-dimensional parameter space: When using a deep neural network to model the dynamics of a robot, the number of hidden layer nodes tends to infinity, resulting in an explosion of parameter dimensions; in the online learning scenario, the characteristic parameters generated by environmental interaction show a continuous growth characteristic, forming an infinite-dimensional dynamic characteristic. (2) Distributed coupling characteristics: In tasks such as multi-robot system collaborative handling and group search, the single-body dynamic models are coupled with each other through the communication topology, forming a joint parameter space whose dimension grows exponentially with the number of nodes. (3) Stochastic dynamic time-variation: In unstructured environments such as field patrol and disaster rescue, interference factors such as ground friction coefficient and air turbulence constitute a stochastic excitation source with time-varying statistical characteristics. Traditional methods are mainly based on finite-dimensional parameter models, and realize localization estimation by presetting the dimension of the system state space. It is difficult to accurately represent infinite-dimensional characteristics such as the distribution of unstructured obstacles and non-Gaussian noise interference in a dynamic environment. For example, the statistical characteristics of the multi-modal observation data generated by lidar in an unstructured scenario are difficult to be completely characterized by a parameter model with a fixed dimension, resulting in the accumulation of localization errors. And the cooperative localization method based on the SLAM framework needs to pre-establish an environmental map parameter model. When unexpected structural changes occur in the scenario, the parameter model needs to be reconstructed, and it is difficult to achieve real-time adaptive control in a dynamic environment. Therefore, traditional robot cooperative localization methods face technical problems such as insufficient complex environment modeling and poor adaptability. Summary of the Invention

[0003] The present invention aims to solve at least one of the technical problems existing in the prior art or related technologies, and discloses a robot cooperative localization method and system based on an infinite-dimensional parameter model, which can realize infinite-dimensional parameter modeling and distributed online estimation of robot cluster dynamic interaction, solve problems such as insufficient expression ability and low single-body estimation accuracy of traditional methods, and effectively improve the robustness and adaptability of the algorithm.

[0004] The first aspect of the present invention discloses a robot cooperative localization method based on an infinite-dimensional parameter model, including: constructing a stochastic dynamic system model with infinite parameters:

[0005] For a robot swarm consisting of n robots, each robot in the swarm obeys the following infinite-dimensional ARX model:

[0006] , ,

[0007] = 0, ,

[0008] where, represents the three-dimensional position state of the -th robot at time , represents the three-dimensional position state of the -th robot at time ;

[0009] represents the -dimensional control input of the -th robot at time ;

[0010] represents the disturbance noise term;

[0011] represents the infinite-dimensional autoregressive coefficient matrix to be estimated for the robot swarm, which is used to characterize the linear influence weight of the robot's historical output on the current output;

[0012] represents the exogenous input coefficient matrix to be estimated for the robot swarm, which is used to reflect the dynamic influence of the robot's control instruction on the observed output;

[0013] Define = as the coefficients of the infinite-dimensional ARX model to be estimated. Define as the position state and control input of the -th robot at all past historical moments. Here, T represents the matrix transpose. Then the infinite-dimensional ARX model is transformed into:

[0014] + ;

[0015] Construct a distributed infinite-parameter adaptive estimation algorithm:

[0016] For each robot and any given time , construct the following regression vector of the historical position state and the historical control input :

[0017] ;

[0018] Among them, T represents matrix transpose, is a positive integer function that increases with time and ;

[0019] Each robot uses the estimated value at the current time information matrix regression vector and the observed output at the time , and combines the recursive least squares algorithm to calculate the intermediate estimated value and the intermediate information matrix at the next time:

[0020] ,

[0021] ;

[0022] Among them, is a scalar representing the algorithm gain coefficient;

[0023] Each robot exchanges information with its neighbors within the neighborhood topology at the current time, and obtains the updated estimated value and the updated information matrix according to the convex combination of the neighbor information weights:

[0024]

[0025]

[0026] Among them, represents the inverse of the matrix, and the output estimated value is .

[0027] According to the robot cooperative positioning method based on the infinite-dimensional parameter model disclosed in the present invention, preferably, each robot in the cluster is only allowed to exchange information with its neighbors within the neighborhood topology at the current time.

[0028] According to the robot cooperative positioning method based on the infinite-dimensional parameter model disclosed in the present invention, preferably, the calculation process for determining the neighbors within the neighborhood topology at the current time includes:

[0029] Define the information exchange topology of the robots as an undirected topology where represents the set composed of these robots, The communication relationship at a moment, that is indicating the robot is capable of transmitting information to the robot The weight of information exchange between robots indicating the weight of information transmission from the th robot indicating the th robot to the th robot The weight of the transmitted information;

[0030] If indicating that the robot is a neighbor of the robot at the moment, if indicating that the robot is not a neighbor of the robot ; The set of all neighbors of the robot

[0031] is Since the robot system is subject to physical constraints and communication constraints, robots can only perform local information interaction, that is, a robot is only allowed to exchange information with its topological neighborhood For the robot collaborative positioning method based on an infinite-dimensional parameter model disclosed in the present invention, preferably,

[0032] The - dimensional control input at least includes: motor speed data and inertial measurement unit data.

[0033]

[0034] A second aspect of the present invention discloses a robot collaborative positioning system based on an infinite-dimensional parameter model, including: a memory for storing program instructions; a processor for calling the program instructions stored in the memory to implement the robot collaborative positioning method based on an infinite-dimensional parameter model as in any of the above technical solutions.

[0035] A third aspect of the present invention discloses a computer-readable storage medium storing program code for implementing the robot collaborative positioning method based on an infinite-dimensional parameter model as in any of the above technical solutions.

[0035] Compared with the prior art, the beneficial effects of the present invention at least include: The present invention realizes the distributed adaptive estimation of a random dynamic model with infinite parameters. First, by establishing an infinite-dimensional parameter model for the dynamic interaction of a robot swarm, the modeling bottleneck of complex dynamics by traditional finite-dimensional models is broken through. Secondly, by designing a dynamic index function that characterizes the upper bound of the model order and based on a local information fusion strategy, a distributed adaptive estimation algorithm suitable for an infinite-parameter random dynamic model is constructed. Finally, by introducing a sufficient excitation strategy for the parameter information matrix of a large-scale system and using methods such as double-index martingale theory, a convergence theory of the algorithm is established without the assumptions of data independence and stationarity. The infinite-dimensional parameter modeling and distributed online estimation of the dynamic interaction of the robot swarm are realized, the problems such as insufficient expression ability of traditional finite-dimensional models and low estimation accuracy of single-body estimation are solved, and the robustness and adaptability of the algorithm are effectively improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 FIG. shows a schematic algorithm flow diagram of a robot cooperative positioning method based on an infinite-dimensional parameter model according to an embodiment of the present invention.

[0037] Figure 2 FIG. shows a schematic block diagram of a robot cooperative positioning system based on an infinite-dimensional parameter model according to an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0038] In order to more clearly understand the above objects, features and advantages of the present invention, the present invention will be further described in detail below with reference to the drawings and specific embodiments. Many specific details are set forth in the following description in order to fully understand the present invention. However, the present invention may also be implemented in other ways different from those described herein. Therefore, the present invention is not limited by the limitations of the specific embodiments disclosed below.

[0039] In the process of implementing the present invention, the applicant found that traditional ARX models and state-space models rely on finite-dimensional parameter assumptions, and reduce the model complexity by truncating high-order terms or simplifying coupling relationships. However, in the robot dynamic interaction scenario, such simplifications will lead to model mismatch and cause degradation of control performance. When a single robot independently estimates the system model parameters, the obtained observation data is incomplete due to limited local observation ability, resulting in low estimation accuracy of the single-robot algorithm.

[0040] In addition, the existing centralized robot control architecture needs to upload all robot observation data to the central node, which is likely to cause the following problems: Non-scalable communication load: When the number of robots increases, the network bandwidth becomes a bottleneck. Single-point failure risk: The failure of the central node will cause the global model to collapse.

[0041] Existing distributed algorithms mainly target finite-dimensional parameter models, making it difficult to adapt to the continuous expansion characteristics of the parameter space during the online learning of neural networks, and also unable to effectively handle the modeling requirements of complex systems with long-range dependence relationships. At the same time, traditional optimization theory is difficult to guarantee the convergence of distributed algorithms in an infinite-dimensional parameter space.

[0042] Based on the above considerations, the present invention discloses a robot cooperative positioning method based on an infinite-dimensional parameter model, including: S1, establishing a stochastic dynamic system model with infinite parameters; S2, constructing a distributed infinite-parameter adaptive estimation algorithm. Specifically:

[0043] S1, establishing a stochastic dynamic system model with infinite parameters:

[0044] Considering a multi-robot cluster system composed of robots, each robot follows the following infinite-dimensional ARX model:

[0045] , ,

[0046] =0, ,

[0047] where represents the three-dimensional position state of the th robot at time , represents the th robot's -dimensional control input at time , including motor speed and inertial measurement unit (IMU) data, represents the disturbance noise term, represents the infinite-dimensional autoregressive coefficient matrix to be estimated for the robot cluster, used to characterize the linear influence weight of the robot's historical output on the current output. represents the exogenous input coefficient matrix to be estimated for the robot cluster, used to reflect the dynamic influence of the robot's control instructions on the observed output.

[0048] Let = be the infinite-dimensional model coefficients to be estimated;

[0049] , represents the states and control inputs of the th robot at all past historical moments, where represents the matrix transpose symbol. Then the above infinite-dimensional ARX model is transformed into:

[0050] +

[0051] Suppose that the information exchange topology of this set of robots is characterized by an undirected topology where represents the set of these robots; represents the communication relationship between robots at time, that is means that robot can transmit information to robot represents the weight of information exchange between robots represents the th robot transmitting to the th robot the weight of the transmitted information. If , it means that robot at time is a neighbor of robot ; otherwise, if it means that robot is not a neighbor of robot . Denote the set of all neighbors of robot as . Due to the physical and communication constraints of the robot system, robots can only perform local information interaction, that is, assume that robot is only allowed to exchange information with its topological neighborhood .

[0052] S2. Construct a distributed infinite parameter adaptive estimation algorithm:

[0053] First, for each robot , and any given time , construct the following regression vector of the historical position state and the historical control input:

[0054] ,

[0055] where represents the matrix transpose symbol, is a positive integer function that increases with time and .

[0056] Second, each robot uses the estimated value at the current time , the information matrix , the regression vector and Observation output at a moment , and the intermediate estimate value at the next moment is calculated by combining with the recursive least - squares algorithm and the intermediate information matrix :

[0057]

[0058]

[0059] where is the scalar representing the algorithm gain coefficient.

[0060] Finally, each robot exchanges information with its neighbors within the neighborhood topology at the current moment, and the updated estimate value and the updated information matrix are obtained according to the convex combination of the neighbor information weights

[0061]

[0062]

[0063] where represents the inverse of the matrix. The output estimate value .

[0064] As Figure 1 shown, in this embodiment, a distributed infinite - parameter adaptive estimation method based on the ARX model is proposed to predict the three - dimensional position information and control input information of each robot in the robot cluster. The algorithm pseudocode corresponding to this method is as follows:

[0065] Initialization: For each robot , and for any given moment , construct the following regression vector:

[0066] , ;

[0067] where is the given termination moment, and any initial estimate value and any initial positive - definite matrix are given;

[0068] For all , at each moment , each robot performs the following operations:

[0069] The first step: Adapt the update process, which specifically includes:

[0070]

[0071]

[0072]

[0073] The second step: the information exchange process, specifically including:

[0074]

[0075]

[0076] Output the estimated value: 。

[0077] According to another embodiment of the present invention, an effectiveness verification experiment of the above robot cooperative positioning method based on an infinite-dimensional parameter model is also disclosed, including:

[0078] Calculate the estimation error:

[0079] ;

[0080] Among them, the symbol represents the Euclidean norm, represents the input-output tail term;

[0081] = is the step unknown coefficient vector to be estimated.

[0082] From the above estimation error formula, it is necessary for the noise term to satisfy certain conditions to analyze the convergence property of the estimation error. For this reason, it is assumed that the noise satisfies the following conditions:

[0083] Noise condition: Let for any be a martingale difference sequence, where represents a non-decreasing algebra, and satisfies:

[0084] ,[[]] a.s. (almost everywhere), =o( ), where represents a deterministic increasing positive sequence and satisfies: , the symbol represents the conditional expectation, and the symbol represents an infinitesimal of infinite order.

[0085] Then, using the double-index martingale theorem, the following convergence rate of the estimation error can be obtained:

[0086] ,

[0087] wherein = represents the minimum eigenvalue of the matrix, represents the diameter of the network topology graph of, represents the sum of the input-output norms of the entire robot cluster over historical time, + represents the last term of the real coefficient, is a constant greater than 0.

[0088] It can be seen that in special cases: (1) the input and output are bounded; (2) the noise is Gaussian independent white noise, then the expression of the estimation error can be simplified as: . Therefore, if the non-persistent excitation condition is satisfied, the estimation error converges to 0, which verifies the effectiveness of the distributed infinite-parameter adaptive estimation algorithm, and further shows that the distributed least squares algorithm under the finite-order model is a special case of the above-mentioned distributed infinite-parameter adaptive estimation algorithm.

[0089] As Figure 2 shown, according to another embodiment of the present invention, a robot cooperative positioning system 200 based on an infinite-dimensional parameter model is also disclosed, including: a memory 201 for storing program instructions; a processor 202 for calling the program instructions stored in the memory to implement the robot cooperative positioning method based on the infinite-dimensional parameter model as in the above embodiment.

[0090] According to another embodiment of the present invention, a computer-readable storage medium is also disclosed, and the computer-readable storage medium stores program code for implementing the robot cooperative positioning method based on the infinite-dimensional parameter model as in the above embodiment.

[0091] In summary, the present invention proposes a distributed adaptive estimation method based on infinite-dimensional ARX model parameters, realizes infinite-dimensional parameter modeling and distributed online estimation of dynamic interaction of robot clusters, solves the problems of insufficient expression ability of traditional finite-dimensional models and low single-unit estimation accuracy, avoids the problems of high communication load and poor robustness of traditional centralized architecture, and effectively improves the adaptability and accuracy of the algorithm. The present invention realizes distributed adaptive estimation of random dynamic models with infinite parameters. First, by establishing an infinite-dimensional parameter model of dynamic interaction of robot clusters, the bottleneck of modeling complex dynamics of traditional finite-dimensional models is broken through. Secondly, by designing a dynamic index function that describes the upper bound of the model order, and based on the local information fusion strategy, a distributed adaptive estimation algorithm suitable for random dynamic models with infinite parameters is constructed. Finally, a full incentive strategy for large-scale system parameter information matrix is ​​introduced, and the algorithm convergence theory without data independence and stationarity assumptions is established by using methods such as double index martingale theory. Through the implementation method of this project, infinite-dimensional parameter modeling and distributed online estimation of dynamic interaction of robot clusters can be realized, and the problems of insufficient expression ability of traditional finite-dimensional models and low single-unit estimation accuracy can be solved, and the robustness and adaptability of the algorithm can be effectively improved.

[0092] All or part of the steps in the various methods of the above embodiments can be completed by controlling the relevant hardware through a program, and the program can be stored in a readable storage medium, and the storage medium includes a read-only memory (ROM), a random access memory (RAM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), a one-time programmable read-only memory (OTPROM), an electronically erasable programmable read-only memory (EEPROM), a compact disc (CD-ROM) or other optical disc storage, magnetic disk storage, magnetic tape storage, or any other readable medium that can be used to carry or store data.

[0093] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A robot cooperative localization method based on an infinite-dimensional parameter model, characterized in that Including: Construct a random dynamic system model with infinite parameters: For a robot cluster composed of n robots, each robot in the cluster obeys the following infinite-dimensional ARX model: ， ， =0， ， Among them, represents the th robot's three-dimensional position state at the moment , represents the th robot's three-dimensional position state at the moment ; Denote the -th robot's dimensional control input at the moment ; denotes the disturbance noise term; It represents the infinite-dimensional autoregressive coefficient matrix to be estimated for the robot swarm, and is used to characterize the linear influence weight of the historical output of the robot on the current output; It represents the exogenous input coefficient matrix to be estimated for the robot swarm, which is used to reflect the dynamic influence of the robot control instructions on the observation output; Definition = is defined as the coefficients of the infinite-dimensional ARX model to be estimated. Define as the position states and control inputs of the th robot at all past historical moments. Where T represents the matrix transpose, then the infinite-dimensional ARX model is transformed into: + ； Construct a distributed infinite-parameter adaptive estimation algorithm: For each robot and any given moment , construct the following regression vectors of the historical position state and the historical control input : : ； where T represents matrix transpose, is a positive integer function that increases with time and ; Each robot uses the estimated value at the current moment of information matrix regression vector and the observed output at the moment , and combines the recursive least squares algorithm to calculate the intermediate estimated value at the next moment and the intermediate information matrix : ， ； Among them, is the scalar representation algorithm gain coefficient; Each robot interacts with its neighbors within the neighborhood topology at the current moment and obtains an updated estimate based on the convex combination of the neighbor information weights and an updated information matrix : Among them, represents the inverse of the matrix and outputs the estimated value .

2. The robot collaborative positioning method based on the infinite-dimensional parameter model according to claim 1, wherein Each robot in the cluster is only allowed to interact with neighbors within the neighborhood topology at the current moment.

3. The robot cooperative positioning method based on an infinite-dimensional parameter model according to claim 2, characterized in that, The calculation process for determining neighbors within the neighborhood topology at the current moment includes: Definition The information exchange topology of the robots is an undirected topology , where denotes the set composed of these robots, denotes the communication relationship between robots at time, that is denotes that robot can transmit information to robot ; denotes the weight of information exchange between robots, , where denotes the th robot transmitting information to the th robot ; If it means that the robot is a neighbor of the robot at the moment. If it means that the robot is not a neighbor of the robot; Robot The set composed of all neighbors is , due to the physical and communication constraints of the robot system, only local information interaction can be carried out between robots, that is, a robot is only allowed to communicate with its topological neighborhood for information exchange.

4. The robot cooperative positioning method based on an infinite-dimensional parameter model according to any one of claims 1 to 3, characterized in that, The said The multi-dimensional control input at least includes: motor speed data and inertial measurement unit data.

5. A robot cooperative positioning system based on an infinite-dimensional parameter model, characterized in that, Including: A memory for storing program instructions; A processor for calling the program instructions stored in the memory to implement the robot cooperative localization method based on an infinite-dimensional parameter model as described in any one of claims 1 to 4.

6. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores program code for implementing the robot cooperative localization method based on an infinite-dimensional parameter model as described in any one of claims 1 to 4.

Images