Distributed multi-robot safety control method and controller in complex environment

Abstract

The invention relates to a distributed multi-robot safety control method in a complex environment and a controller. The method is used for carrying out multi-robot control under the uncertainty of observation of the robots and a model. Firstly, an upper planner provides a global reference trajectory with accurate time and speed requirements for each robot so as to provide optimality guidance. Then, analyzing and modeling the observation result and the uncertainty of the model, and establishing inequality constraints of the model and a state observer; and finally, on the basis of model prediction control, a control quantity is generated through adaptive sampling and an optimization function, and the control quantity is used for environment self-adaption and trajectory tracking. The method has a high success rate, and safety control needs to be provided for a plurality of robots in an uncertain environment.

Description

Technical Field

[0001] This application relates to the technical field of robot control, and particularly to a distributed multi-robot safety control method and controller in a complex environment. Background Art

[0002] Multi-robot systems have broad application prospects and have been continuously developing in recent years with the development of technologies such as sensing technology, control theory technology, and artificial intelligence. Safe and robust control is the key to the movement of robots in multi-robot systems. However, in practice, all factors bring uncertainties, which greatly affect the control of multiple robots. Therefore, a control framework that can operate safely in a complex environment considering uncertainties is needed.

[0003] In recent years, in order to further consider model constraints and the integration of planning and control, practical tracking work using Model Predictive Control (MPC for short) has received extensive attention. MPC is a control technology based on the iterative solution of optimization problems. It can be combined with an objective function to generate and track a trajectory that satisfies the kinematics, dynamics, and drive limits of a rolling horizon trajectory. Using the system model and the current state, an optimal control sequence can be generated. The system continuously executes the first control input in the optimal sequence and re-acquires environmental information for iterative optimization in the next step.

[0004] Regarding the uncertainty of external obstacles, in existing research results, control barrier functions are used to model pedestrians and added to MPC for obstacle avoidance. In Reference 1 (Hai Zhu and Javier Alonso-Mora. Chance-constrained collision avoidance for mavs in dynamic environments. IEEE Robotics and Automation Letters, 4(2):776–783, 2019), a probabilistic collision avoidance method for navigating between robots and moving obstacles is proposed. Based on strict constraints on the collision probability of each robot with obstacles, a chance-constrained nonlinear model predictive control problem (CCNMPC) is established. For the uncertainty of robot observation and modeling, a reinforcement learning and constraint-based control motion planner is proposed to achieve collision avoidance of static obstacles in the workspace. For nonlinear systems, Reference 2 (Haokun Wang, Haojia Li, Boyu Zhou, Fei Gao, and Shaojie Shen. Impact-aware planning and control for aerial robots with suspended payloads. IEEE Transactions on Robotics, 2024) proposes a new perception planning and control framework that uses the augmented Lagrangian method to solve optimization problems with nonlinear complementary constraints. A hybrid nonlinear model predictive control method is further proposed to solve the model mismatch problem in agile flight. However, these methods make it difficult to achieve real-time navigation and control of multiple robots in a chaotic environment. Summary of the Invention

[0005] Based on this, in view of the above technical problems, it is necessary to provide a distributed multi-robot safety control method and controller in a complex environment.

[0006] A distributed multi-robot safety control method in a complex environment, which is used for a multi-robot distributed control system to achieve multi-robot safety control. The multi-robot distributed control system includes: an upper-layer planner and multiple robots configured with improved model predictive control; the method includes: Upper-layer planner: Obtain obstacle information and future state predictions published by each robot.

[0007] According to the obstacle information, plan a global reference path for each robot with precise time and speed requirements.

[0008] Perform collision detection for each robot regularly.

[0009] Each robot: By analytically modeling the uncertainty of the observation results and the robot model, obtain the inequality constraints of the robot model and the state observer.

[0010] Use the method of time-adaptive sampling to select appropriate reference path points for its own global reference path, and then obtain the reference trajectory through trajectory fitting.

[0011] Estimate the uncertain state of the robot using a state estimator based on the obtained observation noise and the output of the robot model.

[0012] According to the reference trajectory, obstacle information, the uncertain state of the robot, the exchanged trajectory, and the inequality constraints of the robot model and the state observer, use a preset optimization function to track the iterative trajectory, generate the predicted future state and the control quantities for environment adaptation and trajectory tracking.

[0013] Publish the predicted future state to the upper-level planner.

[0014] A distributed multi-robot safety controller in a complex environment, the distributed multi-robot safety controller includes: an upper-level planner and multiple improved model predictive controllers.

[0015] The upper-level planner is used to obtain obstacle information and the predicted future state published by each robot; plan a global reference path with precise time and speed requirements for each robot according to the obstacle information; and is also used to perform collision detection for each robot regularly.

[0016] The improved model predictive controller is used to obtain the inequality constraints of the robot model and the state observer by analytically modeling the uncertainty of the observation results and the robot model; use the method of time-adaptive sampling to select appropriate reference path points for its own global reference path, and then obtain the reference trajectory through trajectory fitting; estimate the uncertain state of the robot using a state estimator based on the obtained observation noise and the output of the robot model; according to the reference trajectory, obstacle information, the uncertain state of the robot, the exchanged trajectory, and the inequality constraints of the robot model and the state observer, use a preset optimization function to track the iterative trajectory, generate the predicted future state and the control quantities for environment adaptation and trajectory tracking; and publish the predicted future state to the upper-level planner.

[0017] The above-mentioned distributed multi-robot safety control method and controller in a complex environment, where the method is used for multi-robot control under the uncertainty of the robot's own observations and models. First, the upper-level planner provides each robot with a global reference trajectory with precise time and speed requirements to provide optimality guidance. Then, by analytically modeling the uncertainty of the observations and models, inequality constraints for the models and state observers are established. Finally, based on model predictive control, control quantities are generated through adaptive sampling and optimization functions for environment adaptation and trajectory tracking. This method has a high success rate and is required to provide safe control for multiple robots in an uncertain environment. Brief Description of the Drawings

[0018] Figure 1 It is a schematic flow chart of the distributed multi-robot safety control method in a complex environment in an embodiment; Figure 2 It is a schematic diagram of a multi-robot distributed control system in another embodiment; Figure 3 It is a schematic diagram of the uncertainty of a robot with shape and speed in another embodiment, where Figure 3 (a) is a schematic diagram of the uncertainty of the robot when vx = 0.0 and vy = 0.0, (b) is a schematic diagram of the uncertainty of the robot when vx = 0.8 and vy = 0.5, (c) is a schematic diagram of the uncertainty of the robot when vx = 0.5 and vy = 0.0, (d) is a schematic diagram of the uncertainty of the robot when vx = 1.0 and vy = 0.0; Figure 4 It is a schematic diagram of a three-wheeled omnidirectional mobile robot model and its coordinate system in another embodiment; Figure 5 It is a schematic diagram of the navigation map of multiple robots in a complex environment in another embodiment; Figure 6 It is the tracking effect diagram of the x and y states in another embodiment, Figure 6 (a) is the tracking effect diagram of the x state, (b) is the tracking effect diagram of the y state. Detailed Embodiment

[0019] In order to make the purpose, technical solutions and advantages of this application clearer, the following further details this application in conjunction with the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain this application and are not used to limit this application.

[0020] The distributed multi-robot safety control method proposed in this application constructs a distributed multi-robot safety control framework (DMSCF) by combining MPC and studies the control problem of omnidirectional mobile robots on the ground in a dynamic environment. This application extends the number of robots to multiple robots and conducts simulation analysis while fully considering the observation and model uncertainties of the robots.

[0021] In one embodiment, as Figure 1 shown, a distributed multi-robot safety control method in a complex environment is provided. This method is used for a multi-robot distributed control system to achieve multi-robot safety control. As Figure 2 shown, the multi-robot distributed control system includes: an upper-layer planner and multiple robots configured with improved model predictive control; the method includes the following steps: Upper-layer planner: Step 100: Obtain obstacle information and future state predictions published by each robot.

[0022] Specifically, in this multi-robot distributed system, the unknown environment requires the robots to plan and control through local perception and communication.

[0023] Step 102: Plan a global reference path with precise time and speed requirements for each robot according to the obstacle information.

[0024] Specifically, a multi-robot system requires more precise planning and control than a single robot, and a small deviation from a single robot will greatly reduce the efficiency and safety of the entire system. During the operation of the entire system, the upper-layer planner first plans the initial global trajectory of each robot once with precise time and speed requirements.

[0025] Step 104: Regularly perform collision detection for each robot.

[0026] Each robot: Step 106: Obtain inequality constraints of the robot model and the state observer by analytically modeling the uncertainties of the observation results and the robot model.

[0027] Specifically, to particularly consider the influence of various uncertainties during the observation process, this method adds uncertainty constraints to the optimizer and adds a state estimator to filter the observation uncertainties.

[0028] Step 108: Select appropriate reference path points for its own global reference path by using the method of time-adaptive sampling, and then obtain a reference trajectory through trajectory fitting.

[0029] Specifically, the model predictive controller of the robot will perform adaptive sampling based on the global reference path, the current running time, and its sampling period to meet the requirements of trajectory time and speed.

[0030] Step 110: Estimate the uncertain state of the robot using a state estimator based on the obtained observation noise and the output of the robot model.

[0031] Specifically, the entire system faces challenges caused by external noise interference and inaccurate robot models.

[0032] Step 112: Track the iterative trajectory using a preset optimization function based on the reference trajectory, obstacle information, the uncertain state of the robot, the swapped trajectory, and the inequality constraints of the robot model and the state observer to generate predicted future states and control quantities for environment adaptation and trajectory tracking.

[0033] Specifically, the optimizer generates control commands and future state predictions, considering trajectory tracking errors, robot model constraints, external obstacle avoidance, and mutual collision avoidance between robots.

[0034] Step 114: Publish the predicted future states to the upper-level planner.

[0035] In the above-mentioned distributed multi-robot safety control method in a complex environment, the method is used for multi-robot control under the uncertainty of the robot's own observations and models; first, the upper-level planner provides each robot with a global reference trajectory with precise time and speed requirements to provide optimality guidance. Then, by analytically modeling the uncertainty of the observation results and the model, the inequality constraints of the model and the state observer are established. Finally, based on model predictive control, control quantities are generated through adaptive sampling and optimization functions for environment adaptation and trajectory tracking. This method has a high success rate and is required to provide safe control for multiple robots in an uncertain environment.

[0036] In one embodiment, step 106 includes: constructing a state equation for a system with state observation and robot model mismatch uncertainty, and the expression of the state equation is:

[0037] Wherein, 、 and are the observed states, 、 and are the observation noises respectively, A and B are two matrices related to the system model, and are the deviations related to the system model, uis a control variable, is the control torque.

[0038] The uncertainties of the observations and the robot model are uniformly represented as the uncertainty of the state. According to the state equation, a discrete-time nonlinear dynamic system is obtained; the expression of the discrete-time nonlinear dynamic system is:

[0039]

[0040] where, o k is the output vector, is the unknown output disturbance, is the unknown state disturbance containing , and influences, is the k-th control input, is the k-th time period, is the state quantity at the next time step, is the state quantity at the current k-th time step.

[0041] The state disturbance and the output disturbance are represented by an elliptical field with a bivariate Gaussian distribution to represent the uncertainty of the robot state; according to the uncertainties of the state disturbance and the output disturbance, the inequality constraints of the robot model and the state observer are determined; the expression of the inequality constraints is:

[0042] where, F and G are two known matrices, and g is a preset safety matrix.

[0043] In one embodiment, the state disturbance and the output disturbance are represented by an elliptical field with a bivariate Gaussian distribution to represent the uncertainty of the robot state, including: representing the state disturbance and the output disturbance in two ellipsoidal sets; the expressions of the two ellipsoidal sets are respectively:

[0044]

[0045] where, W and V are the ellipsoidal sets of the state disturbance and the output disturbance respectively, Q and R are known positive definite matrices, and are the elements in the sets of W and V respectively.

[0046] For a moving obstacle, the ellipsoidal sets of the discrete-time nonlinear dynamic system, state perturbation, and output perturbation change according to the state of the robot relative to the obstacle, and the uncertainty of the robot state is represented as an elliptical field; the form of the elliptical field is generated based on the radius, position, velocity, and operating period of the robot.

[0047] Specifically, in practice, the uncertainty comes from sensor observation errors and robot model errors. There are various forms of sensor noise: external noise is caused by factors such as obstacles, people, or natural disturbances in the environment, and internal noise mainly comes from relevant circuit components inside the sensor. The robot model is an abstract representation of the real world, and all models can only be approximate models that partially simulate the physical process of the robot. For uncertainty processing, in addition to further strengthening the hardware facilities, approximation also needs to be carried out in the algorithm to ensure real-time performance and effective calculation.

[0048] For a system with state observation and model mismatch uncertainty, the form of the state equation is as shown in the expression of the above state equation.

[0049] The unstructured environment and the performance of sensors with uncertainty will affect the perception of the robot. Combining with the control model of the robot, the uncertainty will propagate continuously, as shown in the expression of the above state equation. For uncertain planning and control, it will ultimately be manifested as trajectory deviation, speed, and acceleration deviation. The uncertainty of sensory information will lead to inaccuracies in environmental modeling, which in turn will affect the inaccuracies of behavior decision-making. Finally, the behavior of the robot will in turn have an uncertain impact on the environment.

[0050] However, considering the actual accuracy of existing sensors and models, the overall uncertainty cannot be expanded indefinitely. Since the uncertainty of observation and the uncertainty of the model can be uniformly represented as the uncertainty of the state, this work simplifies the small amount of uncertainty in the expression of the above state equation to obtain a discrete-time nonlinear dynamic system as shown in the expression of the above discrete-time nonlinear dynamic system.

[0051] According to the relevant work of statistical analysis, the known state perturbation and output perturbation can be within the ellipsoidal sets W and V. An elliptical field with a bivariate Gaussian distribution is introduced to represent the uncertainty, as Figure 3 shown, Figure 3 (a) is a schematic diagram of the uncertainty of the robot when vx = 0.0 and vy = 0.0, (b) is a schematic diagram of the uncertainty of the robot when vx = 0.8 and vy = 0.5, (c) is a schematic diagram of the uncertainty of the robot when vx = 0.5 and vy = 0.0, and (d) is a schematic diagram of the uncertainty of the robot when vx = 1.0 and vy = 0.0.

[0052] Compared with an ordinary round field, the elliptical field can further characterize the motion intention of the robot and provide more accurate guidance for the safe control of the robot. For a moving obstacle, the expressions of the discrete-time nonlinear dynamic system and the expressions of the two ellipsoidal sets can be changed according to the state of the robot relative to them, and the uncertainty of the robot state can still be represented in the Figure 3 form shown. The form of the elliptical field is generated based on the radius, position, speed, and running period of the robot, and different colors are used to represent the possibility that the robot will be at this position at the next moment. The circle represents the current position of the robot.

[0053] For the initial state , the observation can satisfy:

[0054] where , the initial uncertainty, is an ellipsoidal set. The system state and control input of the robot system are subject to the mixed constraints shown in the expressions of the above inequality constraints.

[0055] In one embodiment, step 108 includes: performing adaptive sampling on the global reference path of the robot itself using a time self-sampling method to obtain reference path points and corresponding timestamps; fitting a polynomial trajectory using the positions, speeds, and timestamps of adjacent reference path points in the global reference path of the robot itself, and converting the path point data into trajectory parameters; the expression of the trajectory parameters is:

[0056] where are the fitting trajectory coefficients, S ( ) is the sampling fitting function, is the current estimated state, is the current time of the reference trajectory, N is the number of trajectory points, dt is the sampling interval, P ref is the reference path point, T ref is the reference path point P ref corresponding timestamp, k is the time step.

[0057] In one embodiment, the construction process of the state estimator in step 110 includes: constructing an extended state observer; the expression of the extended state observer is:

[0058] where e is the state error, , is the state variable to be measured, is the state estimate, is the differential of the measured state, is the estimated disturbance, is the differential of the estimated disturbance, u is the corresponding control input, b is the control gain; l 1 and l 2 are two ESO gains.

[0059] Set the control gain to 1, and set the two ESO gains to and , and evenly configure the poles of the system to , and transform the extended state observer to obtain the final state estimator as:

[0060] where, are the characteristic roots of the equation, are the preset poles of the system.

[0061] In one embodiment, the expression of the optimization function preset in step 112 is:

[0062] where, is the tracking error weight, is the tracking error term, is the control input weight, is the control cost term, is the obstacle avoidance weight, is the obstacle avoidance cost term, is the prediction time domain, is the estimated state, is the measured state, is the control quantity, is the disturbance term, is the state at the 0th step, is the state at the 0th step, is the state error, is the elliptic matrix, is the initial state, is the lower limit of the state range, is the upper limit of the state range, is the lower limit of the control force, is the upper limit of the control force, and W and V are the ellipsoidal sets of state disturbance and output disturbance respectively, Q and R are known positive definite matrices, and are elements corresponding to the sets W and V respectively; F and G are two known matrices, and g is a preset safety matrix.

[0063] In one embodiment, the expression for optimizing the project composition is:

[0064]

[0065]

[0066] where is the k th path point, is the reference path point corresponding to the k th path point, and the reference state is calculated from the expression of the above trajectory parameters, is the safety distance, is the distance between the robot and the i th obstacle, M is the number of obstacles within the safety range.

[0067] Specifically, MPC can consider model constraints and optimization objectives to generate and track a rolling - horizon trajectory that satisfies kinematic, dynamic, and actuator limits.

[0068] To meet the actual requirements, in an environment lacking centralized guidance and lacking uncertainty, a safe and robust controller is essential for safe navigation. This method will estimate the uncertain state of the robot, then use a time - self - sampling method to select appropriate reference path points. Finally, an appropriate optimization function is used to accurately track the iterative trajectory. The specific process is shown in Algorithm 1.

[0069] Algorithm 1: Adaptive MPC in a Dynamic Environment Input: reference trajectory , current time , current observed state of the robot , current control input ; Output: control command u, predicted state s; 1: / / Parameter initialization; 2: if RefTrajReceived then; 3: / / Load the reference trajectory; 4: / / Read the trajectories of other robots; 5: / / State estimation; 6: / / Obtain trajectory parameters on demand; 7: While reachMaxIter do; 8: / / MPC solution; 9: end while; 10: SharedTraj(s) / / Share the predicted trajectory; 11: if isFeasible(u) then; 12: / / Limit the control force; 13: end if; 14: / / Smooth the control force; 15: Else; 16: ; 17: end if; 18: return u / / Output the current control force.

[0070] 1) State estimation method: In the nonlinear system studied in this method, the uncertainty of the robot state diffuses greatly in the whole system, and the uncertainty of observation and model is finally reflected in the state uncertainty.

[0071] For the first-order system, considering the important role of the accurate state of the robot, an extended state observer (ESO) as shown in the expression of the extended state observer is designed in this paper to reduce the uncertainty of the observed state. In the kinematic model, the control gain is set to b = 1, and the ESO gains are l 1 = 2 w and l 2 = w 2 , and the poles of the system are uniformly configured to − w , and the extended state observer is simplified to the state estimator shown in the above final state estimator.

[0072] 2) Time self-sampling method: In order to provide effective guidance for the multi-robot distributed system, this method calculates a global reference trajectory with accurate time and speed requirements for each robot.

[0073] To track the global reference trajectory as much as possible in a complex environment, this section uses the positions, velocities, and timestamps of adjacent path points in the reference trajectory to fit a polynomial trajectory, effectively converting the path point data into trajectory parameters as described in the above trajectory parameter expressions.

[0074] 3) Optimization problem construction: An MPC needs to combine environmental information to solve the optimization problem for a future period, thus becoming a robust controller that satisfies the model constraints and considers the future time domain. The specific optimization item composition is as shown in the expression of the above optimization item composition.

[0075] Finally, the optimization problem is constructed into an optimization function as shown in the expression of the above preset optimization function. A non-zero control input factor is designed to limit the solution space to balance the tracking accuracy and the smoothness of the control input.

[0076] In one of the embodiments, the robot is a three-wheel omnidirectional mobile robot, and the three-wheel omnidirectional mobile robot has the ability to move in any full posture and full angle; the expression of the robot model in step 106 is:

[0077]

[0078]

[0079]

[0080]

[0081] Among them, s is the system state variable, s = x , y , θ , x , y is the two-dimensional position in the world coordinate system, θ is the angle between the body coordinate system and the world coordinate system, u is the control variable, u = v 1, v 2, v 3], are respectively the speeds of the three wheels of the three-wheel omnidirectional robot, τ is the control torque, τ = τ 1, τ 2, τ 3], are respectively the torques of the three wheels of the three-wheel omnidirectional robot, mis the mass of the robot, r is the radius of the wheel, l is the distance from the center of the wheel to the center of the robot, I is the moment of inertia, and A and B are two matrices related to the system model, is the inherent mounting angle of the trolley.

[0082] Specifically, the omnidirectional mobile robot has the ability to move arbitrarily in all postures and all angles, and its mobility is extremely high. In trajectory planning, only a high-order differentiable path and the centroid yaw angle need to be planned to control the entire system, and it has a differential flatness similar to that of an unmanned aerial vehicle. This characteristic reduces the complexity of the problem solution. This means that in the iterative process, even if they do not converge to a local minimum, the optimization results meet their constraints.

[0083] The classic three-wheel omnidirectional mobile robot is as Figure 4 shown. The axes of its three wheels are at a fixed angle, ϕ = 120◦, and the centers of the omnidirectional wheels are evenly distributed on the same circle, and the wheel axes point to the center of the platform.

[0084] Preferably, the radius of the three-wheel omnidirectional mobile robot l = 0.04 m, θ is the angle between the body coordinate system (marked in blue) and the world coordinate system. The positive directions of the respective velocities (indicated in red).

[0085] In one embodiment, step 114 includes: the robot communicates by publishing the predicted future state to the upper-level planner, and sequentially processes the trajectory information constructed based on the future state through the priority mechanism set by the robot.

[0086] Specifically, in this communication mechanism, the low-priority robot needs to consider the trajectory information of the high-priority robot. In addition, observation noise and model noise are randomly added during the simulation process.

[0087] It should be understood that although Figure 1 the steps in the flowchart of Figure 1 are shown sequentially according to the arrows, these steps are not necessarily executed in the order indicated by the arrows. Unless there is a clear indication in this article, the execution of these steps has no strict order limit, and these steps can be executed in other orders. Moreover,

[0088] In a validation example, to verify the performance of the algorithm, we constructed a simulation environment, conducted a comparative analysis, and performed MPC optimization using the existing CasADi toolkit. Figure 5 Schematic diagram of the navigation of multiple robots in a complex environment Figure 5 Shows an example of how a robot responds to environmental changes while tracking its reference global path. Figure 5 Shows an example of five robots safely navigating in a simulated scenario containing static obstacles (squares) and dynamic obstacles (circles). The dotted coils of different colors around the robots represent their respective sensing ranges. The circles with different transparencies represent the positions of the obstacles at past moments.

[0089] (1) Experimental setup The expression for adding an extended state observer in the ESO in the equation is set to l 1 = 1 and l 2 = 0.25. In the equation, the preset optimization function of the model is configured as H = 5, and the time step used is dt = 0.1s. In addition, to better verify the effectiveness of the proposed framework in uncertain scenarios, we introduced relevant deviations (1Σ) of the robot in the simulation to simulate the uncertainty of the observed state, as Figure 6 shown, where Figure 6 (a) is the tracking effect diagram of the x state, and (b) is the tracking effect diagram of the y state. For general observations, we set a 1m deviation for the x and y states, θ a 0.1 ◦ deviation for the state, and an additional deviation for the dynamic obstacle . For the uncertainty of the model, we also introduced bounded random perturbations.

[0090] (2) Simulation experiment To verify the effectiveness of each component of the method, this example conducted a trajectory tracking test as Figure 6 shown and an ablation comparison as shown in Table 1. In the above trajectory parameter expression, the reference trajectory replaced the general MPC with an initial discrete trajectory. For variables without safety conditions (SC), the inequality constraints in the expression of the above preset optimization function will no longer be considered.

[0091] Table 1 shows the data comparison of different variables at different uncertainty levels. In a scenario similar to Figure 550 simulations were conducted in a random environment, including 5 robots, 6 static obstacles, and 2 dynamic obstacles. The metrics include success rate, swarm distance, dynamic distance, path length, and travel time. The average | minimum | standard deviation of each metric is shown, and the better-performing metrics are highlighted in bold.

[0092] Table 1: Data comparison of different variables at different uncertainty levels

[0093] In Figure 6 even if the observed states are scattered, the maximum deviation between the estimated states and the actual states of the x and y states is less than 0.3 m, allowing for effective control.

[0094] The data in Table 1 shows that, first, for a dynamic environment, especially when the speed of the dynamic obstacle is slightly higher than that of the robot, it is difficult to guarantee an absolute success rate even under ideal deterministic conditions (0 Σ). However, compared with general MPC, the proposed method can accurately track the reference trajectory. The success rate is significantly improved, and the travel time and path length are also improved. Second, in an uncertain environment, due to the lack of a state observer, the ability of the system to avoid obstacles is severely affected, increasing the complexity of real-time optimization. The controller may issue incorrect commands, resulting in a significant reduction in the success rate, as well as additional path and time consumption. The missing version of SC also leads to a reduction in the success rate. The method has significant advantages in terms of crossing success rate, dynamic distance, travel time, and path length. In addition, it also proves the effectiveness and necessity of each part of the method, especially in an environment considering uncertainty.

[0095] In one embodiment, a distributed multi-robot safety controller in a complex environment is provided. The distributed multi-robot safety controller includes: an upper-layer planner and a plurality of improved model predictive controllers, wherein: The upper-layer planner is configured to obtain obstacle information and future state predictions published by each robot; plan a global reference path with precise time and speed requirements for each robot according to the obstacle information; and is further configured to perform collision detection for each robot regularly.

[0096] An improved model predictive controller is used to analytically model the uncertainties of observations and the robot model to obtain inequality constraints for the robot model and the state observer; adopt a time-adaptive sampling method for its own global reference path to select appropriate reference path points, and then obtain a reference trajectory through trajectory fitting; estimate the uncertain state of the robot using a state estimator based on the acquired observation noise and the output of the robot model; according to the reference trajectory, obstacle information, the uncertain state of the robot, the switching trajectory, and the inequality constraints of the robot model and the state observer, use a preset optimization function to track the iterative trajectory, generate predicted future states and control quantities for environment adaptation and trajectory tracking; and publish the predicted future states to the upper-level planner.

[0097] In one embodiment, the improved model predictive controller is further configured to construct a state equation for a system with state observation and robot model mismatch uncertainty as shown in the state equation expression; uniformly represent the observed uncertainty and the robot model uncertainty as state uncertainty, and obtain a discrete-time nonlinear dynamic system as described in the discrete-time nonlinear dynamic system expression according to the state equation; represent the state perturbation and the output perturbation using an elliptical field with a bivariate Gaussian distribution to represent the robot state uncertainty; determine the inequality constraints for the robot model and the state observer as described in the above inequality constraints according to the uncertainties of the state perturbation and the output perturbation.

[0098] In one embodiment, the improved model predictive controller is further configured to represent the state perturbation and the output perturbation in two ellipsoidal sets; where the two ellipsoidal sets are as shown in the above ellipsoidal set expression; for a moving obstacle, the ellipsoidal sets of the discrete-time nonlinear dynamic system, the state perturbation, and the output perturbation change according to the robot's state relative to the obstacle, and represent the uncertainty of the robot state as an elliptical field; the form of the elliptical field is generated based on the radius, position, speed, and operating period of the robot.

[0099] In one embodiment, the improved model predictive controller is further configured to perform adaptive sampling on its own global reference path using a time self-sampling method to obtain reference path points and corresponding timestamps; use the positions, speeds, and timestamps of adjacent reference path points in the robot's own global reference path to fit a polynomial trajectory, and convert the path point data into trajectory parameters as shown in the trajectory parameter expression.

[0100] In one embodiment, the construction process of the state estimator in the improved model predictive controller includes: constructing an extended state observer as shown in the extended state observer expression; setting the control gain to 1, and setting the two ESO gains to and , the poles of the system are evenly configured as , the extended state observer is transformed to obtain the final state estimator as described in the above state estimator expression.

[0101] In one embodiment, the preset optimization function in the improved model predictive controller is as shown in the expression of the above preset optimization function.

[0102] In one embodiment, the composition of the optimization items in the improved model predictive controller is as shown in the expression of the above composition of the optimization items.

[0103] In one embodiment, the robot is a three-wheel omnidirectional mobile robot, and the three-wheel omnidirectional mobile robot has the ability to move arbitrarily in all postures and all angles; the robot model in the improved model predictive controller is as shown in the expression of the above robot model.

[0104] In one embodiment, the improved model predictive controller is further used for the robot to communicate by publishing the predicted future state to the upper-level planner, and sequentially process the trajectory information constructed based on the future state through the priority mechanism set by the robot.

[0105] For the specific limitations of the distributed multi-robot safety controller in a complex environment, reference can be made to the limitations of the distributed multi-robot safety controller method in the above text, which will not be elaborated here. Each module in the above distributed multi-robot safety controller in a complex environment can be implemented in whole or in part by software, hardware, and their combinations. The above modules can be embedded in the processor of the computer device in hardware form or independent of it, or stored in the memory of the computer device in software form, so that the processor can call and execute the operations corresponding to the above modules.

[0106] The technical features of the above embodiments can be combined arbitrarily. For the sake of brevity of description, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, it should be considered as the scope described in this specification.

[0107] The above-described embodiments only represent several implementation manners of the present application, and their descriptions are relatively specific and detailed, but they should not be construed as limiting the scope of the present application. It should be noted that for those of ordinary skill in the art, without departing from the concept of the present application, several deformations and improvements can still be made, and these all belong to the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the appended claims.

Claims

1. A distributed multi-robot safety control method in a complex environment, characterized in that: The method is used in a multi-robot distributed control system to realize multi-robot safety control, wherein the multi-robot distributed control system comprises: an upper-level planner and a plurality of robots configured with improved model predictive control; the method comprises: Upper planner: Obtain obstacle information and future state predictions published by each robot; Planning a global reference path with precise time and speed requirements for each robot based on the obstacle information; Regularly perform collision detection for each robot; Each robot: By analytically modeling the uncertainty of the observation results and the robot model, the inequality constraints of the robot model and the state observer are obtained; Selecting appropriate reference path points for the global reference path of the self by using a time adaptive sampling method, and then obtaining a reference trajectory by trajectory fitting; The state estimator is used to estimate the uncertain state of the robot according to the obtained observation noise and the output of the robot model; According to the reference trajectory, the obstacle information, the uncertainty state of the robot, the exchange trajectory, and the inequality constraints of the robot model and the state observer, a preset optimization function is used to track the iterative trajectory to generate a predicted future state and a control amount for environmental adaptation and trajectory tracking; The predicted future state is published to the upper-level planner.

2. The distributed multi-robot safety control method in a complex environment according to claim 1 is characterized in that: By analytically modeling the uncertainty of the observation results and the robot model, the inequality constraints of the robot model and the state observer are obtained, including: The state equation of the system with state observation and robot model mismatch uncertainty is constructed as: in, , and is the observation state, , and are observation noises, A and B are two matrices related to the system model, and is the deviation associated with the system model, u is the control variable, It is the control torque; The uncertainty of observation and the uncertainty of robot model are uniformly expressed as the uncertainty of state. According to the state equation, the discrete time nonlinear dynamic system is obtained as: in, o k is the output vector, is the unknown output disturbance, is included , and The unknown state disturbance that affects is the kth control input, is the kth time period, is the state quantity of the next time step, is the state quantity of the current k-th time step; An elliptical field with a binary Gaussian distribution is used to represent the robot state uncertainty for the state disturbance and the output disturbance; According to the uncertainty of the state disturbance and the output disturbance, the inequality constraints of the robot model and the state observer are determined as follows: Among them, F and G are two known matrices, and g is a preset security matrix.

3. The distributed multi-robot safety control method in a complex environment according to claim 2 is characterized in that: The state disturbance and the output disturbance are represented by an elliptical field with a binary Gaussian distribution to represent the robot state uncertainty, including: The state disturbance and the output disturbance are represented in two ellipsoid sets; wherein the two ellipsoid sets are respectively: Among them, W and V are the ellipsoid sets of state perturbations and output perturbations respectively, Q and R is a known positive definite matrix, and They are the elements in the corresponding W and V sets respectively; For a moving obstacle, the ellipsoid set of the discrete-time nonlinear dynamic system, the state perturbation and the output perturbation changes according to the robot's state relative to the obstacle, and the uncertainty of the robot's state is represented as an elliptical field; the form of the elliptical field is generated according to the robot's radius, position, speed and operation cycle.

4. The distributed multi-robot safety control method in a complex environment according to claim 1, characterized in that: The global reference path of the self is selected by a time adaptive sampling method for appropriate reference path points, and then a reference trajectory is obtained by trajectory fitting, including: Adaptively sample the robot's own global reference path using a time self-sampling method to obtain reference path points and corresponding timestamps; The position, velocity and time stamp of the adjacent reference path points in the global reference path of the robot itself are used to fit a polynomial trajectory and convert the path point data into trajectory parameters: in, is the fitting trajectory coefficient, S ( ) is the sampling fitting function, is the current estimated state, is the current time of the reference trajectory, N is the number of trajectory points, dt is the sampling interval, P ref is the reference path point, T ref is the reference path point P ref The corresponding timestamp, k is the time step.

5. The distributed multi-robot safety control method in a complex environment according to claim 1, characterized in that: The construction process of the state estimator includes: Construct the extended state observer as: in, e is the state error, , is the measured state variable, is the state estimate, is the differential of the measured state, is the estimated disturbance, is the differential of the estimated disturbance, u is the corresponding control input, b is the control gain; l 1 and l 2 is two ESO gains; Set the control gain to 1 and the two ESO gains to and , the poles of the system are evenly configured as , transform the extended state observer, and get the final state estimator as: in, is the characteristic root of the equation, It is the system preset extreme point.

6. The distributed multi-robot safety control method in a complex environment according to claim 1, characterized in that: The default optimization function is: in, is the tracking error weight, is the tracking error term, is the control input weight, is the control cost term, is the obstacle avoidance weight, is the obstacle avoidance cost, is the prediction time domain, is the estimated state, is the measurement state, is the control quantity, is a disturbance term, is the state at step 0, is the state at step 0, is the state error, is an elliptic matrix, is the initial state, is the lower limit of the state range, is the upper limit of the state range, is the lower limit of control, is the upper limit of the control force, W and V are the ellipsoid sets of state disturbance and output disturbance respectively, Q and R is a known positive definite matrix, and are the elements in the corresponding W and V sets respectively; F and G are two known matrices, and g is the preset security matrix.

7. The distributed multi-robot safety control method in a complex environment according to claim 6 is characterized in that: The optimization project consists of: in, It is k Waypoints, It is k The reference path points corresponding to the path points are It's a safe distance. It is a robot and i The distance between obstacles, M is the number of obstacles within the safety range.

8. The distributed multi-robot safety control method in a complex environment according to claim 1, characterized in that: The robot is a three-wheeled omnidirectional mobile robot, which has the ability to move arbitrarily in all postures and all angles; The kinematic and dynamic equations of the three-wheeled omnidirectional mobile robot are: in, s = [ x , y , θ ], x , y is the two-dimensional position in the world coordinate system, θ is the angle between the body coordinate system and the world coordinate system, u is the control variable, u = [ v 1, v 2, v 3], are the three wheel speeds of the three-wheeled omnidirectional robot, τ is the control torque, τ = [ τ 1, τ 2, τ 3], are the torques of the three wheels of the three-wheeled omnidirectional robot, m is the mass of the robot, r is the radius of the wheel, l is the distance from the center of the wheel to the center of the robot, I is the moment of inertia, A and B are two matrices related to the system model, It is the inherent installation angle of the trolley.

9. The distributed multi-robot safety control method in a complex environment according to claim 1, characterized in that: Publishing the predicted future state to the upper-level planner includes: The robot communicates by publishing the predicted future state to the upper-level planner, and sequentially processes the trajectory information constructed based on the future state through the priority mechanism set by the robot.

10. A distributed multi-robot safety controller in a complex environment, characterized in that: The distributed multi-robot safety controller includes: an upper-level planner and a plurality of improved model predictive controllers; The upper-level planner is used to obtain obstacle information and future state predictions issued by each robot; plan a global reference path with precise time and speed requirements for each robot based on the obstacle information; and is also used to periodically perform collision detection for each robot; The improved model predictive controller is used to obtain inequality constraints of the robot model and the state observer by analytically modeling the uncertainty of the observation results and the robot model; select appropriate reference path points for its own global reference path using a time adaptive sampling method, and then obtain a reference trajectory through trajectory fitting; estimate the uncertainty state of the robot using a state estimator based on the obtained observation noise and the output of the robot model; track the iterative trajectory using a preset optimization function based on the reference trajectory, the obstacle information, the uncertainty state of the robot, the exchange trajectory, and the inequality constraints of the robot model and the state observer, generate a predicted future state and a control amount for environmental adaptation and trajectory tracking; and publish the predicted future state to the upper-level planner.

Images