An Omnidirectional Mobile Robot Formation Rolling Optimization Control Method against Unknown Disturbances

Abstract

The present invention belongs to the field of control methods, and particularly relates to an omnidirectional mobile robot formation rolling optimization control method for resisting unknown disturbances. An omnidirectional mobile robot formation rolling optimization control method for resisting unknown disturbances, which includes the following steps: preparatory work; model establishment and initialization; iterative calculation; result output. The present invention has the following beneficial effects compared with the prior art: The present invention proposes an omnidirectional mobile robot formation rolling optimization method in the presence of unknown disturbances. Under the multi-leader-follower formation model, the distributed rolling optimization control based on the particle swarm optimizer is combined with the double closed-loop feedback compensation to optimize the control input, effectively reducing the influence of unknown disturbances and realizing distributed formation control under unknown disturbances.

Description

Technical Field

[0001] The present invention belongs to the field of control methods, and particularly relates to an omnidirectional mobile robot formation rolling optimization control method for resisting unknown disturbances. Background Technique

[0002] With the rapid development of industrial technologies, multi-robot systems have received extensive attention. Multi-robots can optimize their own states through information interaction and complete tasks that are difficult for a single robot to complete. Formation control refers to the process in which multi-robots maintain a formation and meet environmental constraints (such as avoiding collisions between robots and avoiding obstacles) during the process of tracking a certain desired trajectory. It is an important prerequisite for tasks such as collaborative handling, encirclement and capture, and reconnaissance, and is an important part of the cooperative control of multi-robot systems, which can significantly improve the task execution success rate and the robustness of the system.

[0003] The Mecanum wheel is a special omnidirectional wheel composed of a wheel hub and several rollers that form a 45-degree angle with the wheel hub. The omnidirectional mobile robot with Mecanum wheels consists of a vehicle body, four Mecanum wheels symmetrically distributed around the geometric center of the vehicle body, and four independent DC motors for driving the wheels, as shown in the appendix. Figure 1 Different from conventional wheeled robots, such robots can simultaneously and independently perform translational and rotational movement functions, move to any position without changing their direction, and thus complete tasks that are difficult for conventional robots due to physical constraints. Therefore, it is widely used in various operation scenarios, such as collaborative handling in intelligent factories, flexible transportation in narrow hospital environments, collaborative operations of spacecraft, etc. The formation control of omnidirectional mobile robots has broad application prospects.

[0004] Rolling optimization control is a method of continuously solving discrete-time optimization problems within a finite prediction time domain to obtain optimal control inputs. The basic idea of this method is: at each sampling moment, solve the finite-time domain optimization problem to obtain an optimal prediction control input sequence including the current and future moments, but only execute the first control input, and repeat the solution of the finite-time domain optimization problem based on new state observations at subsequent moments. Rolling optimization control has currently been widely applied in multi-robot formations, but it has been less applied in the formations of multi-omnidirectional mobile robots.

[0005] The Particle Swarm Optimization (PSO) is a swarm intelligence computing technology proposed by Dr. Eberhart and Dr. Kennedy in 1995. This algorithm is derived from the study of the foraging behavior of bird flocks and is an intelligent stochastic optimization algorithm proposed by simulating the migration and collaborative behavior in the foraging process of bird flocks. Its basic idea is: starting from a random solution, continuously update the position and velocity of particle individuals through iteration, and at the same time judge the adaptability of particles according to the objective function, and then search for the optimal solution. The particle swarm optimization algorithm has attracted the attention of the academic community due to its advantages such as easy implementation, high accuracy, and fast convergence, and has demonstrated its special superiority in solving problems such as formation, path planning, and trajectory tracking.

[0006] At present, some achievements have been made in the formation control research of omnidirectional mobile robots, but the defects of the existing technologies are mainly reflected in: 1) Some existing research results adopt centralized control, which requires global information. However, in the actual formation operation process, due to the limitations of the detection range and computing power, not all robots can perceive the target information or reference trajectory, and centralized computing often fails to meet the real-time requirements. 2) Some research results only consider the formation control of the robot model under uncertain disturbances, and do not consider the collision avoidance and obstacle avoidance constraints that must be satisfied during the formation process. Therefore, there is an urgent need to develop a distributed anti-interference collision avoidance formation trajectory tracking method for multi-omnidirectional mobile robots that simultaneously considers physical constraints and collision avoidance constraints. Summary of the Invention

[0007] The purpose of the present invention is to provide an omnidirectional mobile robot formation rolling optimization control method against unknown disturbances in view of the defects of the existing technologies.

[0008] The specific technical solution adopted by the present invention is as follows: An omnidirectional mobile robot formation rolling optimization control method against unknown disturbances, which includes the following steps:

[0009] Preparation work; establishing a model and initializing; iterative calculation; outputting results.

[0010] The said preparation work includes setting or inputting the following parameters: the total running time is T, the sampling interval is t s , setting the mass of each robot as m b , the length of the vehicle body is 2a, the width is 2b, and the moment of inertia of the vehicle body is I b , the moment of inertia of each wheel is I w , the radius of each wheel is R w ,, the control input range is [u min u max , the communication range is R d , the critical collision avoidance distance is R colland the critical obstacle avoidance distance R obs , the reference trajectory is x r , the number of robots in the formation is set to M, and the desired formation pattern is P d , the prediction horizon of the rolling optimization control is set to N p , the weighting matrices are Q and R, and the population size N of the particle swarm optimization solver is set pop , the maximum number of iterations t pmax , the minimum inertia weight coefficient ω pmin ∈(0, 1), the maximum inertia weight coefficient ω pmax ∈(ω pmin , 1), and the feedback compensation gains K1 and K2 are set; the above information is known for subsequent calculations, and the values of some parameters are directly input externally, and some parameters are the results obtained by pre-calculation

[0011] The establishment of the model and initialization includes the following steps:

[0012] Step 1: According to the desired formation pattern P d , the self-state x of each robot i , the reference trajectory x r , considering collision avoidance and obstacle avoidance constraints, construct a mathematical model of the multi-leader-follower formation to obtain the desired state vector of each robot where the self-state x of each robot i , the reference trajectory x r are all externally input;

[0013] Step 2: Establish a kinematic model of the robot according to the behavior decomposition, establish a dynamic model of the robot according to the Lagrangian equation, on this basis, construct a kinematic-dynamic ideal integrated model, and use this model as the non-linear prediction model of the rolling optimization control. Further, considering the external disturbances and modeling uncertainties existing in the model, establish the actual integrated model under unknown disturbances;

[0014] Step 3: Set the initial time k = 0, and initialize the starting state quantity x of each robot i in the global coordinate system X a O a Y a at the starting velocity quantity i (0),

[0015] The iterative calculation includes the following steps:

[0016] Step 4: Each robot i receives the assumed predictive control input sequence of each neighbor robot j within the communication range R d and its own assumed predictive control input sequence and Send to all neighbor robots within the communication range R d ;

[0017] Step Five: Each robot i detects whether there are obstacles within the communication range R d If there are, save the nearest point p of each obstacle j obs,j ;

[0018] Step Six: Each robot i calculates its predicted expected reference state within [k, k + N p and combines with the prediction model in Step Two to establish a distributed rolling optimization control framework;

[0019] Step Seven: Each robot combines with a particle swarm optimizer to independently solve the rolling optimization problem at time k, obtaining its own optimal predicted control input sequence at that time

[0020] The above-mentioned Step Seven includes the following specific execution process:

[0021] Step Eight: Let the iteration number t of the particle swarm algorithm p = 1. Each robot initializes N pop particles. The position of each particle corresponds to a predicted control input sequence, and the dimension of each particle is 4×N p , sets the search range as [u min u max , sets the search speed limit as [v min v max , randomly determines the speed and position of each particle, and the value of v min v max is input from the outside;

[0022] Step Nine: Each robot calculates the cost function J of each particle id , id = 1,..., N pop , records the current particle position as pbest id (t p ), records the position of the optimal particle as gbest(t p ), and the described optimal particle is the particle with the smallest cost function value in the entire population; p

[0023] Step Ten: Update the positions and speeds of all particles in the population, limit the positions within [u min u max , and limit the speeds within [v min v max

[0024] Step Eleven: Each particle compares the updated cost function value with the value stored in pbest id ​(t p ) Compare the particle cost function values, and update the solution with the smaller cost function value to the local optimal solution pbest id (t p + 1), and select the optimal particle from it to update the global optimal solution gbest(t p + 1), that is, update to obtain the optimal predictive control input sequence searched so far;

[0025] Step Twelve: Let t p = t p + 1, and judge whether t p > t pmax is satisfied. If it is satisfied, stop the search and save the global optimal solution gbest(t p + 1), and store this optimal solution in the optimal predictive control input sequence Otherwise, return to Step Ten to continue the search,

[0026] The iterative calculation described above also includes the following steps:

[0027] Step Thirteen: Each robot will Substitute it into the ideal integrated model in Step Two and update to obtain the optimal state estimate and the optimal wheel angular velocity estimate

[0028] Step Fourteen: Each robot will The first term in Substitute it into the actual integrated model in Step Two to obtain the true state estimate and the true wheel angular velocity estimate

[0029] Step Fifteen: Each robot i obtains the compensation control input u c,i through the double - closed - loop feedback compensation controller, and then obtains the actual control input u a,i , and substitute it into the ideal integrated model in Step Two to update the state quantity x i (k + 1) and the velocity quantity

[0030] The output result described above includes the following steps:

[0031] Let k = k + 1, repeat Steps Four to Fifteen, and judge whether is satisfied. If it is satisfied, the formation stops moving.

[0032] An omnidirectional mobile robot formation rolling optimization control method against unknown disturbances as described above, where,

[0033] Among them, in the preparatory work, x r= [x t , y r , θ r T is the reference trajectory state vector of the robot formation, where x r , y r , and θ r are respectively the X-axis coordinate, Y-axis coordinate, and direction angle of the reference trajectory in the global coordinate system X a O a Y a .

[0034] Among them, in the preparation work, the expected formation pattern is:

[0035] P d = [h1, h2,..., h i i = 1,..., M (1)

[0036] In the above formula, h i = [h x,i , h y,i , 0] T is the expected formation vector of robot i, where h x,i and h y,i are respectively the X-axis coordinate and Y-axis coordinate of robot i in the expected formation.

[0037] Among them, in step one, the self-state of robot i is x i = [x i , y i , θ i T , where x i , y i , and θ i are respectively the X-axis coordinate, Y-axis coordinate, and direction angle of robot i in X a O a Y a , and the expected state vector is calculated as follows:

[0038]

[0039] In the above formula, N i is the neighbor set of robot i within the communication range R d , N i is the number of neighbor robots of robot i within R d , d i is whether the reference trajectory can be sensed. For the leader robot, d i = 1; for the follower robot, d i = 0.

[0040] Among them, in step one, the formation model is defined as follows:

[0041]

[0042]

[0043] In the above formula, p i =[x i , y i T is the position vector of robot i. ||p i - p j || > R coll and ||p i - p obs,j || > R obs are distance state constraints, which ensure collision avoidance and obstacle avoidance between individuals.

[0044] Among them, in step two, the kinematic model of the omnidirectional mobile robot i is as follows:

[0045]

[0046] In the above formula, is the rotation matrix from the robot coordinate system X b O b Y b to the world coordinate system X a O a Y a . is the inertia matrix. is the velocity vector of robot i in X a O a Y a . represents the angular velocities of the four wheels. a and b are the semi-major axis and semi-minor axis lengths of the vehicle body respectively.

[0047] Among them, in step two, the dynamic model of the omnidirectional mobile robot i is as follows:

[0048]

[0049] In the above formula, is the inertia matrix. is the viscous friction coefficient matrix of the wheels. λ1 to λ4 are the viscous friction coefficients of each wheel. u i =[u i1 , u i2 , u i3 , u i4 T is the output torque vector of the four motors.

[0050] ​​Among them, in step two, the ideal integrated model of the omnidirectional mobile robot i is as follows:

[0051]

[0052] In the above formula, J is the pseudo-inverse matrix of J + ,

[0053] Among them, in step two, the actual integrated model under uncertain disturbances is as follows:

[0054]

[0055] In the above formula, ΔJ, ΔM, and ΔD are model uncertainty terms, u d is the input disturbance, f d is the total external disturbance including static friction,

[0056] Among them, in step four, the assumed predictive control input sequence of robot i is as follows:

[0057]

[0058] In the above formula, k + s|k represents the predictive control input from the current time k to the future time k + s,

[0059] Among them, in step six, the rolling optimization equation is as follows:

[0060]

[0061]

[0062] In the above formula, J i is the cost function of robot i, x i (k + s + 1|k) = f(x i (k + s|k), u i (k + s|k)) represents the prediction of the state value based on the model, is the assumed predictive position vector of the neighbor robot j, U is the set of feasible control inputs, Q and R are positive semi-definite symmetric matrices, is the expected state vector at the prediction time k + s + 1, which is expressed as follows:

[0063]

[0064] Among them, in step seven, the optimal predictive control input sequence within the prediction horizon N p of robot i is as follows:

[0065]

[0066] Among them, in Step Nine and Step Eleven, pbest id (t p ) = [pbest i1 , pbest i2 , …, pbest iD represents the optimal solution of the i-th (i = 1, 2, …, N p ) particle up to the t-th pop generation, that is, the optimal predictive control input sequence corresponding to the i-th particle up to the t-th p generation, and gbest id (t p ) = [gbest i1 , gbest i2 , …, gbest iD represents the global optimal solution up to the t-th p generation, that is, the optimal predictive control input sequence corresponding to the particle i with the minimum fitness value up to the t-th p generation.

[0067] Among them, in Step Ten, the position and velocity update formulas of the particle are as follows:

[0068]

[0069] In the above formula, c1 ∈ [1, 2] and c2 ∈ [1, 2] represent the acceleration constants, ω p represents the inertia weight, rand() represents a random number between (0, 1), and the calculation formula of the inertia weight ω p is as follows:

[0070]

[0071] Among them, in Step Thirteen, the estimated value of the optimal wheel angular velocity at the next moment is calculated as follows:

[0072]

[0073] Among them, in Step Fourteen, the compensation control input u c,i is calculated as follows:

[0074]

[0075] Among them, in Step Fourteen, the actual control input is as follows:

[0076]

[0077] The present invention has the following beneficial effects compared with the prior art: The present invention proposes an omnidirectional mobile robot formation rolling optimization method under unknown disturbances of the model. Under the multi-leader-follower formation model, the distributed rolling optimization control based on the particle swarm optimizer is combined with the double closed-loop feedback compensation to optimize the control input, effectively reducing the influence of unknown disturbances and realizing the distributed formation control under unknown disturbances. BRIEF DESCRIPTION OF THE DRAWINGS

[0078] Figure 1 It is a schematic diagram of the structure of an omnidirectional mobile robot and the coordinate transformation relationship.

[0079] Figure 2 It is a schematic diagram of formation trajectory tracking.

[0080] Figure 3 It is a flow chart of distributed rolling optimization control.

[0081] Figure 4 It is a flow chart of double closed-loop feedback compensation control.

[0082] Figure 5 It is a formation operation trajectory diagram.

[0083] Figure 6 It is a curve diagram of the average tracking error in the X direction.

[0084] Figure 7 It is a curve diagram of the average tracking error in the Y direction.

[0085] Figure 8 It is a curve diagram of the average tracking error of the direction angle θ.

[0086] Figure 9 It is a curve diagram of the average position consistency error.

[0087] Figure 10a and Figure 10b It is a curve diagram of the change in the mutual distance between robots. DETAILED DESCRIPTION OF THE INVENTION

[0088] The present invention will be further described and explained below in conjunction with the drawings and specific embodiments.

[0089] Preparation work: Set the total running time as T and the sampling interval as t s , and set the mass of each robot as m b , the length of the vehicle body is 2a, the width is 2b, and the moment of inertia of the vehicle body is I b , the moment of inertia of each wheel is I w , the radius of each wheel is R w ,, the control input range is [u min u max , and the communication range is R d, the critical collision avoidance distance R coll and the critical obstacle avoidance distance R obs , the reference trajectory is x r , the number of robots in the formation is set to M, and the desired formation pattern is P d , the prediction horizon of the rolling optimization control is set to N p , the weighting matrices are Q and R, and the population size N of the particle swarm optimization solver is set pop , the maximum number of iterations t pmax , the minimum inertia weight coefficient ω pmin ∈(0, 1), the maximum inertia weight coefficient ω pmax ∈(ω pmin , 1), and the feedback compensation gains K1 and K2 are set. The above information is known for subsequent calculations. The values of some parameters are directly input externally, and some parameters are the results obtained by prior calculations.

[0090] Step 1: According to the desired formation pattern P d , the self-state x of each robot i , the reference trajectory x r , considering the collision avoidance and obstacle avoidance constraints, construct a mathematical model of the multi-leader-follower formation to obtain the desired state vector of each robot where the self-state x of each robot i , the reference trajectory x r are both externally input.

[0091] This step can be achieved by those skilled in the art using existing technologies, that is, the desired state vector of each robot can be calculated using the multi-leader-follower formation model

[0092] Step 2: Establish a kinematic model of the robot according to the behavior decomposition, establish a dynamic model of the robot according to the Lagrange equation, and on this basis, construct a kinematic-dynamic ideal integrated model and use this model as the non-linear prediction model of the rolling optimization control. Furthermore, considering the external disturbances and modeling uncertainties existing in the model, establish an actual integrated model under unknown disturbances.

[0093] This step can be achieved by those skilled in the art using existing technologies.

[0094] Step 3: Set the initial time k = 0, and initialize the starting state quantity x a O a Y a of each robot i in the global coordinate system X i (0), the starting velocity quantity

[0095] Step 4: Each robot i receives the assumed predictive control input sequences of each neighboring robot j within the communication range R d and sends its own assumed predictive control input sequence to all neighboring robots within the communication range R . d

[0096] Step 5: Each robot i detects whether there are obstacles within the communication range R d . If there are obstacles, it saves the closest point p of each obstacle j obs,j .

[0097] Step 6: Each robot i calculates its predicted desired reference state within [k, k+N p and establishes a distributed receding horizon optimization control framework in combination with the prediction model in Step 2 .

[0098] Step 7: Each robot combines the particle swarm optimizer to independently solve the receding horizon optimization problem at time k, and obtains its own optimal predicted control input sequence at this moment . The specific steps are shown in Steps 8 to 12

[0099] Step 8: Let the iteration number t of the particle swarm algorithm p =1. Each robot initializes N pop particles. The position of each particle corresponds to an assumed predictive control input sequence, and the dimension of each particle is 4×N p . Set the search range as [u min u max , and the search speed limit as [v min v max . Randomly determine the speed and position of each particle. The value of v min v max is input from the outside

[0100] Step 9: Each robot calculates the cost function J of each particle id , id = 1,..., N pop . Denote the current particle position as pbest id (t p ) and denote the position of the optimal particle as gbest(t p ). The optimal particle described saves the particle with the smallest cost function value in the entire population

[0101] Step 10: Update the positions and speeds of all particles in the population, limit the positions within [u min u max and limit the speeds within [v min v max .​

[0102] Step Eleven: Each particle compares the updated cost function value with the cost function value of the particle in pbest id (t p ) and updates the solution with a smaller cost function value to the local optimal solution pbest id (t p + 1), and selects the optimal particle from it to update the global optimal solution gbest(t p + 1), that is, updates to obtain the optimal predictive control input sequence found so far.

[0103] Step Twelve: Let t p = t p + 1, and judge whether t p > t pmax . If it is satisfied, stop the search and save the global optimal solution gbest(t p + 1), and store this optimal solution in the optimal predictive control input sequence Otherwise, return to Step Ten to continue the search.

[0104] Step Thirteen: Each robot will substitute it into the ideal integrated model in Step Two and update to obtain the optimal state estimate and the optimal wheel angular velocity estimate

[0105] Step Fourteen: Each robot will substitute the first item in it into the actual integrated model in Step Two to obtain the true state estimate and the true wheel angular velocity estimate

[0106] Step Fifteen: Each robot i obtains the compensation control input u c,i through the double - closed - loop feedback compensation controller, and then obtains the actual control input u a,i , and substitutes it into the ideal integrated model in Step Two to update the state quantity x i (k + 1) and the velocity quantity

[0107] Step Sixteen: Let k = k + 1, repeat Steps Four to Fifteen, and judge whether it satisfies If it is satisfied, the formation stops moving.

[0108] Supplementary Note:

[0109] Among them, in the preparatory work, x r = [x t , y r , θr T is the reference trajectory state vector of the robot formation, where x r , y r , and θ r are the X-axis coordinate, Y-axis coordinate, and direction angle of the reference trajectory in the global coordinate system X a O a Y a respectively.

[0110] Among them, in the preparation work, the expected formation pattern is:

[0111] P d = [h1, h2,..., h i for i = 1,..., M (1)

[0112] In the above formula, h i = [h x,i , h y,i , 0] T is the expected formation vector of robot i, and h x,i and h y,i are the X-axis coordinate and Y-axis coordinate of robot i in the expected formation respectively.

[0113] Among them, in step one, the self-state of robot i is x i = [x i , y i , θ i T , where x i , y i , and θ i are the X-axis coordinate, Y-axis coordinate, and direction angle of robot i in X a O a Y a respectively, and the expected state vector is calculated as follows:

[0114]

[0115] In the above formula, N i is the neighbor set of robot i within the communication range R d , N i is the number of neighbor robots of robot i within R d , d i is whether the reference trajectory can be sensed. For the leader robot, d i = 1; for the follower robot, d i = 0.

[0116] Among them, in step one, the formation model is defined as follows:

[0117] ​​

[0118]

[0119] In the above formula, p i = [x i , y i T is the position vector of robot i, ||p i - p j || > R coll and ||p i - p obs,j || > R obs are distance state constraints, which ensure collision avoidance and obstacle avoidance between individuals.

[0120] Among them, in step two, the kinematic model of the omnidirectional mobile robot i is as follows:

[0121]

[0122] In the above formula, is the rotation matrix from the robot coordinate system X b O b Y b to the world coordinate system X a O a Y a , is the inertia matrix, is the velocity vector of robot i in X a O a Y a , represents the angular velocities of the four wheels, and a and b are the lengths of the semi-major axis and semi-minor axis of the vehicle body respectively.

[0123] Among them, in step two, the dynamic model of the omnidirectional mobile robot i is as follows:

[0124]

[0125] In the above formula, is the inertia matrix, is the viscous friction coefficient matrix of the wheels, λ1~λ4 are the viscous friction coefficients of each wheel, and u i = [u i1 , u i2 , u i3 , u i4 T is the output torque vector of the four motors.

[0126] Among them, in step two, the ideal integrated model of the omnidirectional mobile robot i is as follows: ​​

[0127]

[0128] In the above formula, J is the pseudo-inverse matrix of J + .

[0129] Among them, in step two, the actual integrated model under uncertain disturbances is as follows:

[0130]

[0131] In the above formula, ΔJ, ΔM, ΔD are model uncertainty terms, u d is the input disturbance, and f d is the total external disturbance including static friction.

[0132] Among them, in step four, the assumed predictive control input sequence of robot i is as follows:

[0133]

[0134] In the above formula, k + s|k represents the predictive control input from the current moment k to the future moment k + s.

[0135] Among them, in step six, the rolling optimization equation is as follows:

[0136]

[0137]

[0138] In the above formula, J i is the cost function of robot i, x i (k + s + 1|k) = f(x i (k + s|k), u i (k + s|k)) represents the prediction of the state value based on the model, is the assumed predictive position vector of neighbor robot j, U is the set of feasible control inputs, Q and R are positive semi-definite symmetric matrices, is the expected state vector at the prediction moment k + s + 1, which is expressed as follows:

[0139]

[0140] Among them, in step seven, robot i obtains the optimal predictive control input sequence within the prediction horizon N p as follows:

[0141]

[0142] Among them, in step nine, the position of each particle is expressed as x id (t p ) = [xi1 , x i2 ,..., x iD , the cost function of each particle is calculated as follows:

[0143]

[0144] Among them, in Steps Nine and Eleven, pbest id (t p ) = [pbest i1 , pbest i2 , …, pbest iD represents the optimal solution of particle i (i = 1, 2, …, N p ) up to the t pop -th generation, that is, the optimal predictive control input sequence corresponding to particle i up to the t p -th generation, and gbest id (t p ) = [gbest i1 , gbest i2 ,..., gbest iD represents the global optimal solution up to the t p -th generation, that is, the optimal predictive control input sequence corresponding to the particle i with the minimum fitness value up to the t p -th generation.

[0145] Among them, in Step Ten, the position and velocity update formulas of the particle are as follows:

[0146]

[0147] In the above formula, c1 ∈ [1, 2] and c2 ∈ [1, 2] represent the acceleration constants, ω p represents the inertia weight, and rand() represents a random number between (0, 1). The calculation formula of the inertia weight ω p is as follows:

[0148]

[0149] Among them, in Step Thirteen, the estimated value of the optimal wheel angular velocity at the next moment is calculated as follows:

[0150]

[0151] Among them, in Step Fourteen, the compensation control input ui c, is calculated as follows:

[0152]

[0153] Among them, in Step Fourteen, the actual control input is as follows:

[0154]

[0155] A specific example is given below.

[0156] Set the total running time to T = 70 s and the sampling interval to t s = 0.025 s, and set the mass of each robot to m b = 5 kg, the semi-major axis length of the vehicle body a = 0.145, the semi-minor axis length b = 0.145, and the moment of inertia of the vehicle body I b = 0.1875, the moment of inertia of each wheel I w = 0.00625, the radius of each wheel R w = 0.05 m, the lower bound u of the control input (output torque of 4 motors) min = -5 N·m, the upper bound u of the control input max = 5 N·m, the communication range R d = 3 m, the critical collision avoidance distance and the critical obstacle avoidance distance R coll = R obs = 0.5 m, the reference trajectory is as follows:

[0157]

[0158] Set the number of robots in the formation to M = 4, and the desired formation pattern to a square. Set the prediction horizon of the rolling optimization control to N p = 7, and the weighting matrix is Set the population size N of the particle swarm optimization solver pop = 50, the maximum number of iterations t pmax = 5, the minimum inertia weight coefficient ω pmin ∈(0, 1), the maximum inertia weight coefficient ω pmax ∈(ω pmin , 1), and set the feedback compensation gain K1 = -10 3 、K2 = -1. Set the obstacles in the environment to be circular, and the centroid positions are: (5.5, 3.5) T 、(8, 10) T , and the radii are all 1.

[0159] Step 1: Set the leader robot to robot 1, construct the mathematical model of the distributed formation, and derive the expressions of the desired states of robots 1 to 4 as follows:

[0160]

[0161] Step 2: Set the uncertainty parameters to: [aij 3×4 ,[b ij 3×4 ,[c ij 3×4 ,|a ij | < 0.1, |b ij | < 0.1, |c ij | < 0.1, set the unknown disturbance as: τ d =[d ij 4×1 ,|d ij | < 0.5, f d =|e ij | 4×1 ,|e ij | < 0.5, construct the ideal integrated kinematics-dynamics model and the actual integrated model under unknown disturbances.

[0162] Step 3: Set the initial time k = 0, and initialize the starting state variables x a O a Y a of 4 robots at i (0) and the starting velocity variables as follows:

[0163]

[0164] Step 4: Each robot i receives the assumed predictive control input sequences d of each neighbor robot j within R and sends its own assumed predictive control input sequences to the neighbor robots within R d .

[0165] Step 5: Each robot i detects whether there are obstacles within R d . If there are, save the closest point p obs,j of each obstacle j.

[0166] Step 6: Let the iteration number t p = 1. Each robot initializes 50 particles, the dimension of each particle is 28, set the search range as [-5 5], the search speed limit as [-1 1], and randomly determine the speed and position of each particle.

[0167] Step 7: Each robot calculates the cost function J id , id = 1,..., N pop , store the current particle position in pbest id (t p ) and store the position of the optimal particle in gbest(t​​​​p ) Among them, the described optimal particle is the particle with the smallest cost function value in the entire population.

[0168] Step Eight: Update the positions and velocities of all particles in the population and perform amplitude limiting processing. The position and velocity update formulas of the particles are as follows:

[0169]

[0170] In the above formula, rand() represents a random number between (0, 1), and the inertia weight ω p The calculation formula is as follows:

[0171]

[0172] Step Nine: Each particle compares the updated cost function value with the cost function value of the particle stored in pbest id (t p ) and updates the solution with a smaller cost function value to the local optimal solution pbest id (t p + 1), and selects the optimal particle from it to update the global optimal solution gbest(t p + 1), that is, updates to obtain the optimal predictive control input sequence searched so far.

[0173] Step Ten: Let t p = t p + 1, and judge whether t p > t pmax is satisfied. If it is satisfied, stop the search and save the global optimal solution gbest(t p + 1), and store this optimal solution in the optimal predictive control input sequence Otherwise, return to Step Eight to continue the search.

[0174] Step Eleven: Each robot will The first item in is substituted into formula (6) and the optimal state estimate and the optimal wheel angular velocity estimate

[0175] Step Twelve: Each robot will The first item in is substituted into formula (7) to obtain the true state estimate and the true wheel angular velocity estimate

[0176] Step Thirteen: Through the double closed-loop feedback compensation controller, the compensation control input is obtained as follows:

[0177]

[0178] Furthermore, the actual control input is obtained. And substitute it into Equation (6) to update the state quantity x i (k + 1) and the velocity quantity

[0179] Step 14: Let k = k + 1, repeat Steps 4 to 13, and judge whether it satisfies If it satisfies, the formation stops moving.

[0180] Step 15: Define the average tracking errors e x , e y , e θ and the position consistency error e h to measure the quality of the formation trajectory tracking effect, and the specific definitions are as follows:

[0181]

[0182] In the above formula, is the desired position vector of robot i.

[0183] Appendix Figure 5 shows the multi-robot formation trajectory. It can be seen that the multi-robots can successfully complete the functions of formation formation, maintenance, smooth obstacle avoidance and trajectory tracking, without obvious trajectory chattering. Appendix Figure 6 , 7 , 8 show the average trajectory tracking error curves in three directions. It can be seen that the distributed rolling optimization framework based on particle swarm optimizer (PSO-DMPC) proposed in the present invention has higher solution quality and better average tracking performance compared with the traditional distributed rolling optimization framework based on sequential quadratic programming solver (SQP-DMPC). Appendix Figure 9 is the average position consistency error curve graph. It can be seen that both methods can quickly form and maintain the formation. When avoiding obstacles, PSO-DMPC has a faster formation convergence speed compared with SQP-DMPC. Appendix Figure 10a and Figure 10b show the mutual distances between the robots. It can be seen that compared with SQP-DMPC, PSO-DMPC can ensure that the inter-robot distance is strictly greater than the collision avoidance distance, and it has smaller distance fluctuations when avoiding obstacles. From Appendix Figure 5 to Appendix Figure 10b it is shown that the PSO-DMPC with disturbance compensation is an effective distributed formation control method for omnidirectional mobile robots against unknown disturbances.

Claims

1. An omnidirectional mobile robot formation rolling optimization control method against unknown disturbances, characterized in that, It includes the following steps: Preparation work; model establishment and initialization; iterative calculation; result output; The preparation work described above includes setting or inputting the following parameters: the total running time is T, and the sampling interval is t s , set the mass of each robot as m b , the length of the vehicle body is 2a, the width is 2b, and the moment of inertia of the vehicle body is I b , the moment of inertia of each wheel is I w , the radius of each wheel is R w , the control input range is [u min u max , the communication range is R d , the critical collision avoidance distance is R coll and the critical obstacle avoidance distance is R obs , the reference trajectory is x r , set the number of robots in the formation as M, and the expected formation pattern as P d , set the prediction horizon of the rolling optimization control as N p , the weighting matrices are Q and R, set the population size of the particle swarm optimization solver as N pop , the maximum number of iterations is t pmax , the minimum inertia weight coefficient ω pmin ∈ (0, 1), the maximum inertia weight coefficient ω pmax ∈ (ω pmin , 1), set the feedback compensation gains K1 and K2; the above information is known for subsequent calculations, the values of some parameters are directly input externally, and some parameters are the results obtained from previous calculations; The model establishment and initialization mentioned above includes the following steps: Step 1: According to the expected formation pattern P d , the self-state x of each robot i , the reference trajectory x r , considering the collision avoidance and obstacle avoidance constraints, construct a mathematical model for multi-leader-follower formation to obtain the expected state vector of each robot where the self-state x of each robot i , the reference trajectory x r are both external inputs; Step 2: Establish a robot kinematic model according to behavior decomposition, establish a robot dynamic model according to Lagrange's equation. On this basis, construct a kinematics-dynamics ideal integrated model, and use this model as the non-linear prediction model for rolling optimization control. Furthermore, considering the external disturbances and modeling uncertainties existing in the model, establish an actual integrated model under unknown disturbances; Step 3: Set the initial time \(k = 0\), and initialize the starting state quantity \(x^{(0)}\) of each robot \(i\) in the global coordinate system \(XOY\), and the starting velocity quantity a O a Y a under i (0), and the starting velocity quantity The iterative calculation mentioned above includes the following steps: Step 4: Each robot i receives the hypothetical predictive control input sequences of each neighbor robot j within the communication range R d and sends its own hypothetical predictive control input sequence to all neighbor robots within the communication range R which is d sent to all neighbor robots within the communication range R; Step 5: Each robot i detects whether there are obstacles within the communication range R d If there are, save the nearest point p of each obstacle j obs,j ; Step 6: Each robot i calculates its predicted expected reference state within [k, k+N p , and combines the prediction model in Step 2 to establish a distributed rolling optimization control framework; s ∈ [0, N p -1], and combines the prediction model in Step 2 to establish a distributed rolling optimization control framework; Step Seven: Each robot combines with a particle swarm optimizer to independently solve the rolling optimization problem at time k, and obtains its own optimal predictive control input sequence at this time The specific execution process of the step 7 mentioned above includes the following: Step 7.1: Set the iteration number t of the particle swarm algorithm p = 1, and initialize N pop particles for each robot. The position of each particle corresponds to a predictive control input sequence, and the dimension of each particle is 4×N p , set the search range as [u min u max , and set the search speed limit as [v min v max . Randomly determine the speed and position of each particle. The value of v min v max is input externally; Step 7.2: Each robot calculates the cost function J of each particle id , id = 1, ..., N pop , and records the current particle position as pbest id (t p ), and records the position of the optimal particle as gbest(t p ), where the optimal particle described is the particle with the minimum cost function value in the entire population; Step 7.3: Update the positions and velocities of all particles in the population, limit the positions within [u min u max , and limit the velocities within [v min v max ​ Step 7.4: Each particle compares the updated cost function value with the cost function value of the particle stored in pbest id (t p ), and updates the solution with a smaller cost function value to the local optimal solution pbest id (t p +1), and selects the optimal particle from it to update the global optimal solution gbest(t p +1), that is, updates to obtain the optimal predictive control input sequence searched so far; Step 7.5: Let t p = t p + 1, and determine whether it satisfies t p > t pmax . If it is satisfied, stop the search and save the global optimal solution gbest(t p + 1), and store this optimal solution in the optimal predictive control input sequence Otherwise, return to Step 7.3 to continue the search The iterative calculation also includes the following steps: Step 8: Each robot will bring it into the ideal integrated model in Step 2 and update to obtain the optimal state estimator at the next moment and the optimal wheel angular velocity estimator Step Nine: Each robot takes the first item in and inputs it into the actual integrated model in Step Two to obtain the true state estimator and the true wheel angular velocity estimator Step Ten: Each robot i obtains the compensation control input u through the double closed-loop feedback compensation controller c,i , and further obtains the actual control input u a,i , and substitutes it into the ideal integrated model in Step Two to update the state quantity x i (k + 1) and the velocity quantity The result output mentioned above includes the following steps: Let k = k + 1, repeat steps four to ten, and determine whether it satisfies If it satisfies, the formation stops moving; Among them, in step 6, the rolling optimization equation is as follows: In the above formula, J i is the cost function of robot i, x i (k + s + 1|k) = f(x i (k + s|k), u i (k + s|k)) represents the prediction of the state value based on the model, is the hypothetical predicted position vector of neighbor robot j, U is the set of feasible control inputs, Q and R are positive semi - definite symmetric matrices, is the expected state vector at the prediction time of k + s + 1, which is expressed as follows:

2. A method for rolling optimization control of an omnidirectional mobile robot formation against unknown disturbances as described in claim 1, wherein: Among them, In the preparation work, x r =[x r , y r , θ r T is the reference trajectory state vector of the robot formation. x r , y r , θ r are the X-axis coordinate, Y-axis coordinate and direction angle of the reference trajectory in the global coordinate system X a O a Y a respectively,​ Among them, in the preparation work, the desired formation pattern is: P d = [h1, h2,..., h i i = 1,..., M (3) In the above formula, h i = [h x,i , h y,i , 0] T is the desired formation vector of robot i, and h x,i and h y,i are the X-axis coordinate and Y-axis coordinate of robot i in the desired formation, respectively. Among them, in step one, the self - state of robot i is x i =[x i ,y i ,θ i T , x i , y i , θ i are respectively the X - axis coordinate, Y - axis coordinate and direction angle of robot i under X a O a Y a . The desired state vector is calculated as follows:​ In the above formula, N i is the number of neighbor robots of robot i within R d , d i is whether the reference trajectory can be sensed. For the leader robot, d i = 1; for the follower robot, d i = 0. Among them, in step 1, the formation model is defined as follows: In the above formula, p i = [x i , y i T is the position vector of robot i, ||p i - p j || > R coll and ||p i - p obs,j || > R obs are distance state constraints, which ensure collision avoidance between individuals and obstacle avoidance​ Among them, in step 2, the kinematic model of omnidirectional mobile robot i is as follows: In the above formula, is the rotation matrix from the X-axis of the robot coordinate system b O b Y b to the X-axis of the world coordinate system a O a Y a , is the inertia matrix, is the velocity vector of robot i under X a O a Y a , represents the angular velocities of the four wheels, where a and b are the half major axis and half minor axis lengths of the vehicle body respectively. Among them, in step 2, the dynamic model of omnidirectional mobile robot i is as follows: In the above formula, is the inertia matrix, is the viscous friction coefficient matrix of the wheels, λ1 to λ4 are the viscous friction coefficients of each wheel, and u i = [u i1 , u i2 , u i3 , u i4 T is the output torque vector of the four motors.​ Among them, in step 2, the ideal integrated model of omnidirectional mobile robot i is as follows: In the above formula, J is the pseudo-inverse matrix of J + and Among them, in step 2, the actual integrated model under uncertainty disturbances is as follows: In the above formula, △J, △M, and △D are model uncertainty terms, and u d is the input disturbance, and f d is the total external disturbance including static friction. Among them, in step 4, the assumed predictive control input sequence of robot i is as follows: In the above formula, k+s|k represents the predictive control input from the current moment k to the future moment k+s, Among them, in step seven, robot i obtains the optimal predictive control input sequence within the prediction horizon N p as shown below: Among them, in steps 7.2 and 7.4, pbest id (t p ) = [pbest i1 , pbest i2 , …, pbest iD represents the optimal solution of particle i (i = 1, 2, …, N p ) up to the t pop -th generation, that is, the optimal predictive control input sequence corresponding to particle i up to the t p -th generation, and gbest id (t p ) = [gbest i1 , gbest i2 , …, gbest iD represents the global optimal solution up to the t p -th generation, that is, the optimal predictive control input sequence corresponding to the particle i with the minimum fitness value up to the t p -th generation. Among them, in step 7.3, the position and velocity update formulas of the particles are as follows: In the above formula, c1 ∈ [1, 2] and c2 ∈ [1, 2] represent acceleration constants, ω p represents the inertia weight, rand() represents a random number between (0, 1), and the inertia weight ω p is calculated as follows: Among them, in step eight, the estimated value of the optimal wheel angular velocity at the next moment is calculated as follows: Among them, in step nine, the compensation control input u c,i is calculated as follows: Among them, in step 14, the actual control input is as follows:

Images