CN113485323B - Flexible formation method for cascading multiple mobile robots - Google Patents

Abstract

The invention provides a flexible formation method of cascading multiple mobile robots, which is based on a strategy gradient algorithm combining priori nonlinear distance-angle-heading formation control knowledge and continuous control, avoids blind exploration of the mobile robots, improves training convergence speed, avoids a fussy coefficient tuning process, and simultaneously introduces a near-end strategy to optimize flexible obstacle avoidance capability of independently training a single mobile robot to cope with local static and dynamic obstacles. The method is divided into a training stage and an reasoning stage, a complex online resolving process is transferred to offline, a formation and a flexible obstacle avoidance strategy are independently trained based on course learning ideas, and meanwhile, a pre-training strategy is flexibly called in a reasoning link, so that the whole formation has higher autonomy and flexibility.

Description

A flexible formation method for cascaded multiple mobile robots

Technical Field

The present invention belongs to the field of multi-mobile robots, and specifically relates to a multi-mobile robot flexible formation method based on a cascade architecture, and specifically relates to a cascade multi-mobile robot formation method based on reinforcement learning and prior nonlinear distance-angle-heading formation control.

Background technique

With the development of robot technology, multi-mobile robot formation operations have effectively improved operating efficiency with their collaborative capabilities, gradually replacing traditional single-machine operations. For example, multiple underwater robots conduct searches through collaborative formations. In the military, drone clusters, multi-ground mobile robots for mine clearance, search and rescue, reconnaissance, etc. all reflect the advantages of multi-machine formations. Recently, in order to use disinfection mobile robots to replace traditional manual methods for hospital disinfection work, many domestic hospitals have effectively improved the efficiency of single-machine operations through formation collaboration of multiple disinfection mobile robots.

The distance-angle-heading-based leader-follower formation strategy is one of the commonly used technologies to achieve formation tracking of multiple mobile robots. Compared with the traditional leader-follower formation strategy, this method has better flexibility and scalability. The basic idea of this strategy is to preset one robot as the leader and the other robots as followers, and then determine the relative distance, relative angle and heading between the leader robot and the follower robot by presetting the formation, and then design the formation control strategy.

At present, the mainstream methods for achieving distance-angle-heading pilot following include nonlinear control and nonlinear model predictive control. The former includes input-output feedback linearization control, feedback control, etc. Due to the introduction of more performance gain parameters, the cumbersome parameter adjustment process cannot be avoided; while the latter is highly dependent on accurate models and has high requirements for online solution speed. On the other hand, the robustness of the traditional pilot following formation model needs to be improved, and it lacks certain flexible obstacle avoidance and formation recovery capabilities.

With the development of artificial intelligence technology, deep reinforcement learning technology has also been widely used in end-to-end mobile robot related tasks due to its advantages such as model-free and offline training, but most of them are in the single robot field; this end-to-end implementation method in the multi-machine field has strict requirements on the performance of sensors and actuators, and the dimensions of the state and action space are high. In the process of implementing actual mobile robots, the training cost is too high and the reasoning reproduction is difficult.

Summary of the invention

The present invention aims to address the above deficiencies in the prior art and to provide a method for flexible formation of multiple mobile robots with certain flexible obstacle avoidance and formation recovery capabilities.

The present invention adopts the following technical scheme. A cascaded multi-mobile robot flexible formation method is provided, including: based on the selected formation, according to the distance, angle and heading between the robots, determining the dynamic model; according to the dynamic model and the dynamic model constraints, determining the prior controller of the reinforcement learning architecture in the flexible formation method of the nonlinear mobile robot; determining the action space based on the hyperparameters of the mobile robot posture vector, the action space includes the formation tracking action space of two adjacent mobile robots and the action space required for each mobile robot to independently and flexibly avoid obstacles; determining the state space based on the tracking error of the mobile robot posture and speed, the state space includes: the state space of the tracking error of each mobile robot tracking the corresponding virtual mobile robot at the current time step, the state space between adjacent mobile robots, and the state space required for each mobile robot to describe the surrounding environment information; setting the reward function of reinforcement learning, the reward function includes the formation reward function and the obstacle avoidance reward function;

Based on the a priori controller, by interacting with the environment, reinforcement learning training is performed according to the action space, state space and reward function, and after the training is completed, a cascaded multi-mobile robot flexible formation method including a formation strategy and a flexible obstacle avoidance strategy is obtained.

Further, the kinetic equation is described as follows:

Where η = [x, y, θ] ^T represents the pose vector of each mobile robot, where (x, y) is the position of each mobile robot and θ is the angle of each mobile robot; is the speed of the mobile robot, /> ω is the current angular velocity of the mobile robot, v _r and v _l represent the velocities of the left and right wheels of the mobile robot respectively;

The constraints of the dynamic model are as follows:

Furthermore, the method for determining the a priori controller of the reinforcement learning architecture in the flexible formation method of the nonlinear mobile robot specifically includes: S31. Determine the expected trajectory of the virtual expected mobile robot as η _r =[x _r ,y _r ,θ _r ] ^T , (x _r ,y _r ) is the position of the virtual expected mobile robot, θ _r is the angle of the virtual expected mobile robot, and the tracking error of the posture and speed of the mobile robot determined according to the virtual expected trajectory is expressed as:

e _x is the position tracking error in the x direction; e _y is the position tracking error in the y direction; e _θ is the azimuth tracking error; are the velocity tracking errors in the x-direction and y-direction respectively;/> is the angular velocity tracking error; /> is the expected angular velocity of the virtual robot;

S32. Determine the desired formation model between the distance, angle and heading between adjacent mobile robots, which is described as follows:

Wherein, _v1 , _v2 represent the virtual robot objects that the adjacent mobile robots need to track, denoted as virtual robot 1 and virtual robot 2, ( _xv1 , _yv1 ) is the position of virtual robot 1, ( _xv2 , _yv2 ) is the position of virtual robot 2, _θv1 is the angle of virtual robot 1, _θv2 is the angle of virtual robot 2; _dv2v1 is the relative distance between adjacent mobile robots v1 and v2; _φv2v1 is the relative angle between adjacent mobile robots v1 and v2; _βv2v1 is the angle correction of the mobile robots that maintain the same azimuth;

S33. Combining (1)-(4) with feedback linearization nonlinear control theory, the prior description of the formation control of adjacent mobile robots is as follows:

Wherein, _v1 is the speed of virtual robot 1 that meets the preset formation requirements, _v2 is the speed of virtual robot 2 that meets the preset formation requirements, _w1 is the angular velocity of virtual robot 1 that meets the preset formation requirements, _w2 is the angular velocity of virtual robot 2 that meets the preset formation requirements, are the performance hyperparameters of the nonlinear formation prior controller of virtual robot 1, are the performance hyperparameters of the nonlinear formation a priori controller of the virtual robot 2. The performance hyperparameters directly determine the control performance of the a priori controller.

Furthermore, the formation tracking action space of the two adjacent mobile robots is represented as follows;

in, Performance hyperparameters of a priori controller for nonlinear formation tracking of virtual robots 1 for mobile robots,/> The performance hyperparameters of the a priori controller for the nonlinear formation of the virtual robot 2 for the neighboring mobile robots to track the virtual robot,

The action space required for each mobile robot to avoid obstacles independently and flexibly is expressed as follows:

Among them, v _discrete and ω _discrete are the discrete velocity command and angular velocity command of the mobile robot respectively.

Furthermore, the state space representation of the tracking error of each mobile robot tracking the corresponding virtual mobile robot at the current time step is as follows:

The state space between adjacent mobile robots is represented as follows:

in, The position tracking error of the virtual robot 1 in the x direction is tracked by the mobile robot; /> The position tracking error of the virtual robot 1 in the y direction is tracked by the mobile robot; /> The tracking error of the virtual robot 1 in azimuth is tracked by the mobile robot; /> is the position tracking error of the virtual robot 2 in the x direction of the mobile robot's adjacent mobile robot tracking, Position tracking error of the virtual robot 2 in the y direction for tracking the adjacent mobile robots of the mobile robot; /> is the tracking error of the mobile robot's adjacent mobile robot tracking virtual robot 2 in azimuth; _e1 is the tracking error of the mobile robot tracking virtual robot 1, and _e2 is the tracking error of the mobile robot's adjacent robot tracking virtual robot 2;

and/> Respectively represent the distance, angle, and heading of the formation state between adjacent mobile robots at each time step t; ||u ₁ || ₂ ,||u ₂ || ₂ ,/> Respectively represent the relative values of the speed, angular velocity and acceleration of robot 1 and robot 2 relative to the virtual robot. The purpose of this item is to hope that the mobile robot runs with a continuous and smooth speed and acceleration. Among them, ||u ₁ || ₂ is the speed value of mobile robot 1 relative to the virtual robot, including speed and angular velocity; /> is the acceleration value of robot 1 relative to the virtual robot, including acceleration and angular acceleration; ||u ₂ || ₂ is the velocity value of mobile robot 2 relative to the virtual robot, including velocity and angular velocity; /> It is the acceleration value of mobile robot 2 relative to the virtual robot, including acceleration and angular acceleration.

The state space required for each mobile robot to describe the surrounding environment information is represented as follows:

Among them, _ηt is the pose vector of the mobile robot at the current moment, _dr is the distance between the mobile robot and its expected virtual mobile robot position at the current moment, _dob is the distance vector of the mobile robot to obstacles within the safety threshold at the current moment, and |Δθ| is a vector containing two elements, which are the difference between the speed of the mobile robot at the current moment and the speed at the previous moment, and the difference between the angular velocity of the mobile robot at the current moment and the angular velocity of the mobile robot at the previous moment.

Furthermore, the formation reward function between two adjacent mobile robots is specifically described as follows:

Where ε _thresh is the set threshold, R _{error_1} in the reward function is the sum of the penalty terms of the tracking errors of the two mobile robots for the expected virtual mobile robot, which is used to inspire the robot to reduce the tracking error of the expected position as much as possible; r is the reward or penalty value, R _formation is the reward or penalty function, which is used to guide the robot to maintain the consistency of the formation. If the dynamic change range of the formation is within the set threshold, a positive reward value is fed back, otherwise a negative penalty value is returned; R _velocity is used to guide the mobile robot to maintain the consistency of velocity and acceleration and maintain a continuous and smooth motion mode.

Furthermore, the obstacle avoidance reward function is specifically in the following form:

Among them, the reward function R _{error_2} is the penalty term of the tracking error of the mobile robot i for the expected virtual mobile robot, guiding the robot's formation recovery; R _avoid guides the mobile robot to perform autonomous obstacle avoidance, ε _safe is the safety threshold, r ₁ is the penalty value when the distance between the robot and the nearest obstacle is within the safety threshold but has not yet completely collided. r ₂ is the penalty value when the robot collides with the obstacle; R _{delta_yaw} is the penalty value for the change of the angular direction of the mobile robot in adjacent time steps, so as to control the change of the angular direction of the mobile robot i and make the overall motion trajectory smoother.

Furthermore, during the training process, two subtasks, formation tracking and flexible obstacle avoidance, are trained independently. The specific method includes the following steps:

For the formation tracking task, the action space is selected as the formation tracking action space a ¹ _space of two adjacent mobile robots, and the state space is based on the state space of the tracking error of each mobile robot tracking the corresponding virtual mobile robot at the current time step The state space between adjacent mobile robots/>

The action value network outputs an evaluation of the current action, uses the Q value of the evaluation output by the current action value network as the weight, and updates the action network based on the policy gradient;

The specific update description of the action value network is as follows

Among them, w _i is the priority sampling weight calculated based on the priority experience replay algorithm at the current moment i; r _i is the reward signal at the current moment i; γ is the discount factor; Q _θ′ (s _i+1 , μ′(s _i+1 )) is the evaluation of the target action value network for the target action μ′(s _i+1 ) at the next moment i+1, s _i is the state value of the robot i at the current moment, s _i+1 is the state value of the robot i+1 at the next moment, a _i is the action of the robot i at the current moment, and N is the number of samples in the small batch sampling; Q _θ (s _i , a _i ) is the evaluation value of the current action value network for the state and action instructions of the robot i at the current moment. .

For the flexible obstacle avoidance task, a proximal strategy optimization algorithm architecture based on discrete action space is adopted to select the action space required for each mobile robot to independently and flexibly avoid obstacles. Select the state space as the state space of the tracking error of each mobile robot tracking the corresponding virtual mobile robot at the current time step/> The state space required for each mobile robot to describe the surrounding environment information/>

Furthermore, the target action value network is updated by using the updated online action network and the online action value network parameters after each training of a small batch. The specific description is as follows:

η′←τη”+(1-τ)η′ (14)

Among them, η′ and η” represent the target network parameters and the current network parameters, and τ is used to control the update ratio.

Furthermore, the method also includes a local collision detection step, which is used to detect the safe distance between the local obstacle and the robot. If the returned safe distance meets the safety state requirements, the mobile robot individual exits the flexible obstacle avoidance strategy and restores the formation strategy.

The beneficial technical effects achieved by the present invention;

A cascaded multi-mobile robot flexible formation method provided by the present invention is a cascaded multi-mobile robot flexible formation method based on reinforcement learning and prior nonlinear distance-angle-heading formation control, so that multiple mobile robots can adaptively adjust key parameters in the formation control algorithm to improve the stability of the formation and the accuracy of tracking; at the same time, flexible obstacle avoidance strategies are independently trained so that each robot in the formation has a certain degree of flexible obstacle avoidance ability, thereby improving the flexibility and autonomy of each mobile robot in the formation.

The formation tracking architecture of the algorithm designed by the present invention is based on a deep deterministic policy gradient algorithm, which further improves the performance and efficiency of the algorithm by simplifying the random exploration process and introducing a priority experience replay mechanism. By introducing a priori nonlinear distance-angle-heading formation controller information, blind exploration is avoided, making the training process more targeted, thereby improving the speed of algorithm convergence, and in the application process of reasoning, the a priori formation controller information controller can avoid abnormal behaviors that damage the actuator in the end-to-end process, thereby improving the robustness of the overall formation.

The obstacle avoidance architecture of the algorithm designed in the present invention is based on the proximal strategy optimization algorithm. When flexibly avoiding obstacles, the action space of the mobile robot is discretized to reduce the search space and reduce the training complexity. By introducing a collision detection function module, the obstacle distance is monitored in real time to determine whether it is possible to return to the formation tracking mode.

Preferably, the training of the two sets of architectures are independent of each other, and they complement each other in the process of reasoning formation to jointly complete the flexible formation of multiple mobile robots.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG1 is a schematic diagram of an overall framework of a specific embodiment of the present invention;

FIG2 is a schematic diagram of a training phase of a specific embodiment of the present invention;

FIG. 3 is a schematic diagram of flexible formation based on reasoning according to a specific embodiment of the present invention.

Detailed ways

The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

Embodiment: A cascaded multi-mobile robot flexible formation method mainly comprises the following steps: S1. Selecting a formation from a formation library, and confirming the priority and specific position of each robot in the team according to the distance-angle-heading formation mode;

S2. Determine the dynamic model according to the robot type;

S3. According to the constraints of the dynamic model, combined with the relative distance constraints, relative angle constraints, and heading constraints between robots, the expected prior trajectories of the virtual leader and virtual follower are designed, and the actual robot formation problem is transformed into a number of tracking problems for tracking the trajectories of virtual mobile robots. The corresponding nonlinear formation tracking prior controller of the angular velocity is designed as the knowledge prior in the entire reinforcement learning framework.

S4. Design a collision detection module for the entire formation algorithm to detect the safe distance between local obstacles and the robot;

S5. Design the action space part of the entire formation algorithm architecture, which is mainly divided into two parts: one is the velocity space containing the velocity and angular velocity of the mobile robot, and the other is the parameter space containing all performance parameters of the prior nonlinear tracking control knowledge;

S6. Design the state space part of the entire formation algorithm architecture, mainly including the position and posture of each robot and the obstacle information in the environment;

S7. Design a reward function to guide the robot formation to learn and flexibly avoid obstacles, which mainly includes formation reward function, tracking reward function and obstacle avoidance reward function;

S8. Build a simulation environment for training, so that the intelligent agent can learn to enable multiple mobile robots to complete flexible and stable formation process strategies and flexible obstacle avoidance strategies based on distance-angle-direction through trial and error interaction with the environment under the condition of prior nonlinear formation control knowledge.

Furthermore, in step S1, the type of each robot is isomorphic, and the number of robots N ≥ 2;

Furthermore, in step S2, taking a two-wheel differential mobile robot as an example, its dynamic equation is described as follows:

Where η = [x, y, θ] ^T represents the pose vector of each mobile robot; is the speed of the mobile robot, /> is the angular velocity of the mobile robot, v _r and v _l represent the speeds of the left and right wheels respectively; it should be noted that the two-wheel drive mobile robot has a non-holonomic constraint, which makes the mobile robot only move forward and backward, not left and right. The constraint form is as follows:

Furthermore, taking the distance-angle-heading formation of multiple mobile robots as an example, the specific design steps of the prior formation control knowledge of mobile robots in S3 are as follows:

S31. Design a tracking controller between the mobile robot and the virtual desired mobile robot. The desired trajectory of the virtual mobile robot is defined as η _r = [x _r , y _r , θ _r ] ^T . The tracking error of its posture and velocity is:

S32. Design the expected formation model between the distance, angle and heading between adjacent mobile robots. The specific description is as follows:

Among them, v1 and v2 represent the virtual robot objects that the adjacent mobile robots need to track, denoted as virtual robot 1 and virtual robot 2 respectively. d _v2v1 , φ _v2v1 , and β _v2v1 are state quantities under the distance-angle-heading formation architecture, indicating the distance angle and direction between v1 and v2.

S33. Combining (1)-(4) with feedback linearization nonlinear control theory, the prior description of the formation control of adjacent mobile robots is as follows:

Among them, v ₁ is the speed of virtual robot 1 that meets the preset formation requirements, v ₂ is the speed of virtual robot 2 that meets the preset formation requirements, w ₁ is the angular velocity of virtual robot 1 that meets the preset formation requirements, w ₂ is the angular velocity of virtual robot 2 that meets the preset formation requirements, [K _x ,K _y ,K _θ ] is the performance hyperparameter of the nonlinear formation control prior of the mobile robot, and its value directly determines the quality of formation tracking;

Further, in S4, the collision detection function module determines the distance to the obstacle through the mobile robot's own sensor and outputs a Boolean collision warning flag;

Furthermore, in S5, the action space is mainly composed of two parts, one is the action space required for formation tracking, and the other is the action space required for flexible obstacle avoidance when detecting local obstacles. The specific design is described as follows:

S51. Design the formation tracking action space of two adjacent mobile robots. The specific method is based on the nonlinear formation knowledge prior involved in S33, and its action space is as follows:

Where [K _x ,K _y ,K _θ ] are the prior performance hyperparameters of nonlinear formation control of mobile robots;

S52. Design the action space required for each mobile robot to avoid obstacles independently and flexibly:

Among them, v _discrete and ω _discrete are the discrete velocity command and angular velocity command of the mobile robot respectively;

Furthermore, in S6, the state space is mainly composed of three parts: one part is the state space describing the tracking error of each mobile robot tracking the corresponding virtual robot, one part is the state space describing the distance-angle-heading formation between adjacent mobile robots, and one part is the state space required to describe the surrounding environment information. The specific design description is as follows:

S61. Taking two adjacent mobile robots as an example, the state space describing the tracking error of each mobile robot tracking the corresponding virtual mobile robot at the current time step is designed as follows:

S62. Taking two adjacent mobile robots as an example, the state space between the adjacent mobile robots that satisfies the distance-angle-heading formation architecture is designed as follows:

Where d, φ, and β represent the distance, angle, and heading of the formation state between adjacent mobile robots at each time step, respectively; ||u ₁ || ₂ ,||u ₂ || ₂ , Respectively represent the relative values of the speed, angular velocity, and acceleration of Robot 1 and Robot 2 relative to the virtual robot. The purpose of this item is to enable the mobile robot to run with a continuous and smooth speed and acceleration;

S63. Design the state space required for each mobile robot to describe the surrounding environment information as follows:

Among them, _ηt is the pose vector of the mobile robot at the current moment, _dr is the distance between the mobile robot and its expected virtual mobile robot position at the current moment, _dob is the distance vector of the mobile robot to obstacles within the safety threshold at the current moment, and |Δθ| is a vector containing two elements, which are the difference between the speed of the mobile robot at the current moment and the speed at the previous moment, and the difference between the angular velocity of the mobile robot at the current moment and the angular velocity of the mobile robot at the previous moment.

Furthermore, the reward function design in S7 can be divided into two sub-reward function designs, one for the formation tracking subtask and the other for the flexible obstacle avoidance and formation recovery subtask, namely:

S71. Design the reward function for the formation tracking subtask.

The specific description of the formation reward function between two adjacent mobile robots is as follows:

The R _error in the reward function is the sum of the penalty terms of the tracking errors of the two mobile robots for the desired virtual mobile robot, which is used to inspire the robot to reduce the tracking error of the desired position as much as possible; R _formation is used to guide the robot to maintain the consistency of the formation. If the dynamic change range of the formation is within the threshold, a positive reward is fed back, otherwise a negative penalty is returned; R _velocity is used to guide the mobile robot to maintain the consistency of velocity and acceleration and maintain a continuous and smooth motion mode;

S72. Design a flexible obstacle avoidance reward function for mobile robot i. The specific form is as follows:

Among them, the reward function R _error is the penalty term of the tracking error of the mobile robot i for the expected virtual mobile robot, guiding the robot's formation recovery; R _avoid guides the mobile robot to perform autonomous obstacle avoidance; R _{delta_yaw} limits the direction angle change of the mobile robot i to save energy;

Optionally, the reward function designed in S72 only occurs in the obstacle avoidance task stage, and is used to inspire the mobile robot to quickly avoid local obstacles. When it is determined by S4 that it is far away from the obstacle, it exits the obstacle avoidance task stage, switches to the formation tracking subtask, and restores and maintains the formation under the guidance of the reward function in S71;

Furthermore, in S8, during the training process, two subtasks, formation tracking and flexible obstacle avoidance, are trained independently, as described below:

S81. For the formation tracking task, a deterministic policy gradient algorithm architecture based on continuous action space is adopted. The action space is selected as a ¹ _space and the state space is based on With/> The algorithm generally follows the "actor-critic" model, but unlike other reinforcement learning algorithms, the biggest advantage of this algorithm is that the output of the action network is a certain action rather than a strategy distribution.

On the other hand, the action value network outputs the evaluation of the current action, and then uses the Q value of the evaluation output by the current action value network as the weight, and updates the action network based on the policy gradient. The update of the action value network is based on the offline target action network and target action value network. The advantage of this method is that the parameters of the target network change little, making the training process more stable.

The specific update description of the action value network is as follows

Where w _i is the priority sampling weight calculated based on the priority experience replay algorithm; _ri is the current reward signal; γ is the discount factor; Q _θ′ (s _i+1 ,μ′(s _i+1 )) is the evaluation of the target action value network for the next target action μ′(s _i+1 )

Preferably, the target network is updated based on a soft update strategy. After each small batch of training, the parameters of the online action network and the online action value network are updated using the updated parameters. The specific description is as follows:

η′←τη”+(1-τ)η′ (14)

Among them, η′ and η” represent the target network parameters and the current network parameters, and τ is used to control the update ratio.

τ is used to control the update ratio. This soft update method reduces the impact of abnormal parameters and avoids abnormal parameter jumps during the parameter update process.

S82. For the flexible obstacle avoidance task, a proximal strategy optimization algorithm architecture based on discrete action space is adopted, and the action space is selected as Select the state space as/> With/>

The proximal policy optimization algorithm optimizes the problems of slow parameter update and low data utilization in the online strategy of the traditional policy gradient algorithm. Based on the generalized advantage evaluation algorithm, the algorithm introduces a resampling mechanism to transform the online strategy into an offline strategy to improve data utilization. At the same time, it constrains the parameter update range based on KL divergence or pruning operations to obtain a more stable training process.

Furthermore, including S9, according to the formation strategy learned offline in S7, the entire inference-based cascade formation control algorithm is built.

In S9, the formation and flexible obstacle avoidance strategies trained in S8 are used to build a mobile robot flexible formation algorithm architecture based on reasoning. The specific process is described as follows:

S91. Determine formation requirements and mission environment;

S92. The mobile robot formation is loaded with the prior-based formation strategy and flexible obstacle avoidance strategy pre-trained in S8;

S93. The mobile robot formation uses a formation tracking strategy to track the formation based on the interaction information with the environment, and each individual robot performs local collision detection;

S94. If flexible obstacle avoidance is required, the mobile robot formation switches to an individual flexible obstacle avoidance strategy and performs real-time obstacle avoidance based on the interactive information with the environment;

S95. If the local collision detection function module returns to a safe state, the mobile robot individual exits the flexible obstacle avoidance strategy and quickly restores the formation state;

S96. Repeat S93 to S95 until reaching the target point.

The present invention provides a cascade multi-mobile robot flexible formation method based on reinforcement learning and prior nonlinear distance-angle-heading formation control. The method transfers the computing power consumption of online solution to offline through reinforcement learning, thereby realizing the flexible formation of multiple mobile robots based on reasoning.

During the training phase, the formation tracking strategy and flexible obstacle avoidance strategy are trained independently, which reduces the difficulty of training. At the same time, the nonlinear distance-angle-heading formation control prior is introduced to improve the training speed and avoid the complicated parameter tuning process. During the inference phase, independent strategies based on offline training are combined to achieve the task requirements of autonomous stable formation and flexible obstacle avoidance. Compared with the existing formation tracking control algorithm based on pilot-tracking, it gives the robot formation the ability to track autonomously while giving each mobile robot the ability to avoid local static and dynamic obstacles independently, which is autonomous, stable, efficient and flexible.

The overall framework of this embodiment is shown in Figure 1, where 1 is an offline independent training framework, 2 is an inference flexible formation framework, 21 is a flexible formation obstacle avoidance strategy, and 3 is a simulation interaction environment.

First, in the first training phase, the formation strategy and the flexible obstacle avoidance strategy are trained respectively; the formation strategy training is based on prior formation experience, which accelerates the training process and convergence speed, prevents blind exploration of multiple mobile robots, and improves the stability of the formation; after the training, the parameters of the two strategies are stored;

Then, in the reasoning stage 2, multiple mobile robots flexibly call the experience-based flexible formation obstacle avoidance strategy 21 to perform autonomous formation tracking and flexible obstacle avoidance. This method migrates the online computing process to the offline, which is more efficient and stable.

The framework of the training phase is shown in Figure 2. The overall strategy of training is based on the idea of curriculum learning, that is, the training environment is simple to complex, and the policy performance is gradually improved. In Figure 2, 1 is the formation training environment based on the distance-angle-heading formation control prior; 2 is the environment for flexible obstacle avoidance for each mobile robot; 3 is the continuous deterministic policy gradient algorithm agent; 4 is the discrete proximal policy optimization algorithm agent; 5 is the formation strategy parameters and flexible obstacle avoidance strategy parameters stored offline after training. The specific process is described as follows:

The formation strategy training process is described as follows:

First, according to the idea of course learning, configure a variety of training environments from simple to complex, such as training two mobile robots in a formation environment and then gradually increasing the number of robots in the formation;

Next, for each preset simulation environment, the action network, target action network, action value network, and target action value network parameters are initialized in the continuous deterministic policy gradient algorithm agent 3 in Figure 2; for each iteration cycle, the formation training environment 1 based on the distance-angle-heading formation control prior is initialized, and then in each time step:

Step 1: Select an action from the threshold range of the action space according to the strategy, and add random Gaussian noise to improve the random exploration performance;

Step 2: Interact with the formation training environment. Specifically, the selected deterministic action is input into the cascade priori formation controller of multiple mobile robots designed in combination with the preset formation mode and the geometric relationship between distance, angle and heading. The specific description is as shown in the above formula (5). Then, the upper control speed and angular velocity instructions of the priori formation are input into each mobile robot. When the robot completes the instruction, the environment updates the current state and feeds back the state value, reward value and Boolean flag indicating whether the task is ended or interrupted.

Step 3: Store the environmental feedback information and the calculated priority into the experience pool as training data;

Step 4: When the experience pool overflows, sample according to priority, train and update the action value network and action network; specifically:

Step 4-1: Input the sampled state value into the action network to obtain the action;

Step 4-2: Input the action and the sampled state value into the action value network to obtain the Q value;

Step 4-3: Based on the Q value, back propagation updates the action network according to the policy gradient;

Step 4-4: Repeat steps 4-2 and 4-3 to obtain the Q value of the updated action calculated by the action value network;

Step 4-5: Input the next state value in the sampled experience into the target action network to obtain the target action;

Step 4-6: Input the action and the state into the target state network to obtain the target Q value;

Step 4-7: According to formula (13), the target Q value and the Q value calculated in step 4-4 are combined with the priority weight coefficient to update the action value network;

Step 5: Repeat steps 1-4;

Step 6: After a certain time step is met, the target action network and the target action value network are soft updated according to formula (14);

Step 7: Store the network parameters of the final formation strategy for next training or inference.

The training process of individual autonomous flexible obstacle avoidance strategy is as follows:

First, according to the idea of course learning, configure a variety of training environments from simple to complex, such as training the obstacle avoidance strategy of the mobile robot in a static obstacle environment, and then training the obstacle avoidance strategy of the mobile robot in a dynamic obstacle environment;

Next, for each preset simulation environment, first initialize the policy network and value network in 2-4; in each iteration cycle, initialize the corresponding environment, and then, in each time step:

Step 1: In the policy network, input the environment state information, obtain the policy distribution, and sample an action in the discrete action space based on this distribution;

Step 2: Input the action into the environment, interact with the flexible obstacle avoidance environment, and the environment updates the state, feeding back the state value, reward value, and a Boolean flag indicating whether the task is ended or interrupted;

Step 3: Repeat step 1, sample a certain amount of experience, and store it;

Step 4: Input the state of the last step in step 3 into the value network to get the state value, and then backtrack to calculate the discounted reward value under so many time steps;

Step 5: Input all stored experiences into the value network and calculate the advantage value using generalized advantage evaluation;

Step 6: Back propagate and update the value network based on the calculated advantage value;

Step 7: Input all state values in the stored experience into the policy network and the past policy network to obtain different policy distributions. Use resampling to convert the same policy into a different policy, and back-propagate to update the policy network.

Step 8: Repeat steps 5-6, and then use the policy network parameters to update the past policy network parameters;

Step 9: Repeat steps 1-8 to store the network parameters of the final flexible obstacle avoidance strategy for the next training or inference.

This embodiment provides a cascaded multi-mobile robot flexible formation method based on reinforcement learning and prior nonlinear distance-angle-heading formation control. When actually deployed and applied, the steps shown in FIG3 are followed:

Step 1: Obtain the expected trajectory of the upper-level motion planning for formation tracking;

Step 2: Clarify the specific formation form requirements of the formation task, obtain prior formation control information, and clarify the task environment;

Step 3: Load the offline formation tracking strategy and flexible obstacle avoidance strategy pre-trained in the training phase;

Step 4: According to the pre-trained formation strategy, after obtaining the state of the mobile robot, the action network feeds back the action, and the mobile robot performs the formation tracking task according to the action;

Step 5: Perform local collision detection to ensure the safety of formation tracking. If there is an obstacle within the safety threshold of a mobile robot, jump to step 6, otherwise go to step 7;

Step 6: The corresponding mobile robot calls the offline flexible formation strategy pre-trained in the training phase. The mobile robot samples discrete actions from the distribution output by the strategy network, performs local obstacle avoidance, and quickly returns to its position in the formation with the smallest possible error, and continues to track the corresponding virtual mobile robot in the formation mode.

Step 7: Check whether the target point of the expected trajectory has been reached. If not, return to step 4 and continue formation tracking.

The method provided by the present invention is based on a policy gradient algorithm that combines a priori nonlinear distance-angle-heading formation control knowledge with continuous control, which avoids blind exploration of mobile robots, improves the speed of training convergence, avoids tedious coefficient tuning processes, and introduces proximal policy optimization to independently train a single mobile robot to cope with local static and dynamic obstacles. The method is divided into training and reasoning stages, which migrates the complex online solution process to offline, independently trains formations and flexible obstacle avoidance strategies based on course learning ideas, and flexibly calls pre-training strategies in the reasoning link, so that the entire formation has higher autonomy and flexibility.

Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented in one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program code.

The present application is described with reference to the flowchart and/or block diagram of the method, device (system) and computer program product according to the embodiment of the present application. It should be understood that each process and/or box in the flowchart and/or block diagram, and the combination of the process and/or box in the flowchart and/or block diagram can be realized by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device produce a device for realizing the function specified in one process or multiple processes in the flowchart and/or one box or multiple boxes in the block diagram.

These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer-readable memory produce a manufactured product including an instruction device that implements the functions specified in one or more processes in the flowchart and/or one or more boxes in the block diagram.

These computer program instructions may also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, whereby the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in one or more processes in the flowchart and/or one or more boxes in the block diagram.

The embodiments of the present invention are described above in conjunction with the accompanying drawings, but the present invention is not limited to the above-mentioned specific implementation methods. The above-mentioned specific implementation methods are merely illustrative and not restrictive. Under the enlightenment of the present invention, ordinary technicians in this field can also make many forms without departing from the scope of protection of the purpose of the present invention and the claims, which all fall within the protection of the present invention.

Claims (7)

1. A cascaded multi-mobile robot flexible formation method, characterized in that it includes: based on the selected formation, according to the distance, angle and heading between the robots, determining the dynamic model; according to the dynamic model and the constraints of the dynamic model, determining the prior controller of the reinforcement learning architecture in the flexible formation method of the nonlinear mobile robot; determining the action space based on the hyperparameters of the mobile robot posture vector, the action space includes the formation tracking action space of two adjacent mobile robots and the action space required for each mobile robot to independently and flexibly avoid obstacles; determining the state space according to the tracking error of the mobile robot posture and speed, the state space includes: the state space of the tracking error of each mobile robot tracking the corresponding virtual mobile robot at the current time step, the state space between adjacent mobile robots and the state space required for each mobile robot to describe the surrounding environment information; setting the reward function of reinforcement learning, the reward function includes the formation reward function and the obstacle avoidance reward function; Based on the a priori controller, by interacting with the environment, reinforcement learning training is performed according to the action space, state space and reward function, and after the training is completed, a cascade multi-mobile robot flexible formation method including a formation strategy and a flexible obstacle avoidance strategy is obtained; The kinetic equation of the kinetic model is described as follows: Where η = [x, y, θ] ^T represents the pose vector of each mobile robot, where (x, y) is the position of each mobile robot and θ is the angle of each mobile robot; is the speed of the mobile robot, /> ω is the current angular velocity of the mobile robot, v _r and v _l represent the velocities of the left and right wheels of the mobile robot respectively; The constraints of the dynamic model are as follows: The method for determining the a priori controller of the reinforcement learning architecture in the flexible formation method of the nonlinear mobile robot specifically includes: S31. Determine the expected trajectory of the virtual expected mobile robot and define it as η _r =[x _r ,y _r ,θ _r ] ^T , (x _r ,y _r ) is the position of the virtual expected mobile robot, θ _r is the angle of the virtual expected mobile robot, and the tracking error of the posture and the tracking error of the speed of the mobile robot determined according to the virtual expected trajectory are expressed as: e _x is the position tracking error in the x direction; e _y is the position tracking error in the y direction; e _θ is the azimuth tracking error; They are the speed tracking error in the x direction and the speed tracking error in the y direction respectively;/> is the angular velocity tracking error; /> is the expected angular velocity of the virtual robot; S32. Determine the desired formation model between the distance, angle and heading between adjacent mobile robots, which is described as follows: Wherein, _v1 , _v2 represent the virtual robot objects that the adjacent mobile robots need to track, denoted as virtual robot 1 and virtual robot 2, ( _xv1 , _yv1 ) is the position of virtual robot 1, ( _xv2 , _yv2 ) is the position of virtual robot 2, _θv1 is the angle of virtual robot 1, _θv2 is the angle of virtual robot 2; _dv2v1 is the relative distance between adjacent mobile robots v1 and v2; _φv2v1 is the relative angle between adjacent mobile robots v1 and v2; _βv2v1 is the angle correction of the mobile robots that maintain the same azimuth; S33. Combining (1)-(4) with feedback linearization nonlinear control theory, the prior description of the formation control of adjacent mobile robots is as follows: Wherein, _v1 is the speed of virtual robot 1 that meets the preset formation requirements, _v2 is the speed of virtual robot 2 that meets the preset formation requirements, _w1 is the angular velocity of virtual robot 1 that meets the preset formation requirements, _w2 is the angular velocity of virtual robot 2 that meets the preset formation requirements, The performance hyperparameters of the nonlinear formation prior controller of virtual robot 1,/> is the performance hyperparameter of the a priori controller for the nonlinear formation of the virtual robot 2. The performance hyperparameter directly determines the control performance of the a priori controller. During the training process, two subtasks, formation tracking and flexible obstacle avoidance, are trained independently. The specific method includes the following steps: For the formation tracking task, the action space is selected as the formation tracking action space a ¹ _space of two adjacent mobile robots, and the state space is based on the state space of the tracking error of each mobile robot tracking the corresponding virtual mobile robot at the current time step The state space between adjacent mobile robots/> The action value network outputs an evaluation of the current action, uses the Q value of the evaluation output by the current action value network as the weight, and updates the action network based on the policy gradient; The specific update description of the action value network is as follows Wherein, w _i is the priority sampling weight calculated based on the priority experience replay algorithm at the current moment i; r _i is the reward signal at the current moment i; γ is the discount factor; Q _θ′ (s _i+1 , μ′(s _i+1 )) is the evaluation of the target action value network for the target action μ′(s _i+1 ) at the next moment i+1, s _i is the state value of the robot i at the current moment, s _i+1 is the state value of the robot i+1 at the next moment, a _i is the action of the robot i at the current moment, and N is the number of samples in the small batch sampling; Q _θ (s _i , a _i ) is the evaluation value of the current action value network for the state and action instructions of the robot i at the current moment; For the flexible obstacle avoidance task, a proximal strategy optimization algorithm architecture based on discrete action space is adopted to select the action space required for each mobile robot to independently and flexibly avoid obstacles. Select the state space as the state space of the tracking error of each mobile robot tracking the corresponding virtual mobile robot at the current time step/> The state space required for each mobile robot to describe the surrounding environment information/> 2. A cascaded multi-mobile robot flexible formation method according to claim 1, characterized in that the formation tracking action space of the two adjacent mobile robots is expressed as follows; in, Performance hyperparameters of a priori controller for nonlinear formation tracking of virtual robots 1 for mobile robots,/> The performance hyperparameters of the a priori controller for the nonlinear formation of the virtual robot 2 for the neighboring mobile robots to track the virtual robot, The action space required for each mobile robot to avoid obstacles independently and flexibly is expressed as follows: Among them, v _discrete and ω _discrete are the discrete velocity command and angular velocity command of the mobile robot respectively. 3. A cascaded multi-mobile robot flexible formation method according to claim 1, characterized in that the state space of the tracking error of each mobile robot tracking the corresponding virtual mobile robot at the current time step is expressed as follows: The state space between adjacent mobile robots is represented as follows: in, The position tracking error of the virtual robot 1 in the x direction is tracked by the mobile robot; /> The position tracking error of the virtual robot 1 in the y direction is tracked by the mobile robot; /> The tracking error of the virtual robot 1 in azimuth is tracked by the mobile robot; /> The position tracking error of the virtual robot 2 in the x direction is tracked by the adjacent mobile robot of the mobile robot,/> Position tracking error of the virtual robot 2 in the y direction for tracking the adjacent mobile robots of the mobile robot; /> is the tracking error of the mobile robot's adjacent mobile robot tracking virtual robot 2 in azimuth; _e1 is the tracking error of the mobile robot tracking virtual robot 1, and _e2 is the tracking error of the mobile robot's adjacent robot tracking virtual robot 2; The formation state quantity representing the distance between adjacent mobile robots at each time step t, /> The formation state quantity representing the angle between adjacent mobile robots at each time step t, /> and/> Represents the formation state quantity of the heading between adjacent mobile robots at each time step t; ||u ₁ || ₂ is the velocity value of mobile robot 1 relative to the virtual robot, including velocity and angular velocity; /> is the acceleration value of robot 1 relative to the virtual robot, including acceleration and angular acceleration; ||u ₂ || ₂ is the velocity value of mobile robot 2 relative to the virtual robot, including velocity and angular velocity; /> is the acceleration value of the mobile robot 2 relative to the virtual robot, including acceleration and angular acceleration; The state space required for each mobile robot to describe the surrounding environment information is represented as follows: Among them, _ηt is the pose vector of the mobile robot at the current moment, _dr is the distance between the mobile robot and its expected virtual mobile robot position at the current moment, _dob is the distance vector of the mobile robot to obstacles within the safety threshold at the current moment, and |Δθ| is a vector containing two elements, which are the difference between the speed of the mobile robot at the current moment and the speed at the previous moment, and the difference between the angular velocity of the mobile robot at the current moment and the angular velocity of the mobile robot at the previous moment. 4. A cascaded multi-mobile robot flexible formation method according to claim 3, characterized in that the formation reward function between two adjacent mobile robots is specifically described in the following form: Where ε _thresh is the set threshold, R _{error_1} in the reward function is the sum of the penalty terms of the tracking errors of the two mobile robots for the expected virtual mobile robot, which is used to inspire the robot to reduce the tracking error of the expected position as much as possible; r is the reward or penalty value, R _formation is the reward or penalty function, which is used to guide the robot to maintain the consistency of the formation. If the dynamic change range of the formation is within the set threshold, a positive reward value is fed back, otherwise a negative penalty value is returned; R _velocity is used to guide the mobile robot to maintain the consistency of velocity and acceleration and maintain a continuous and smooth motion mode. 5. A cascaded multi-mobile robot flexible formation method according to claim 3, characterized in that the obstacle avoidance reward function is specifically in the following form: Among them, the reward function R _{error_2} is the penalty term of the tracking error of the mobile robot i for the expected virtual mobile robot, guiding the robot's formation recovery; R _avoid guides the mobile robot to perform autonomous obstacle avoidance, ε _safe is the safety threshold, r ₁ is the penalty value when the distance between the robot and the nearest obstacle is within the safety threshold but has not yet completely collided; r ₂ is the penalty value when the robot collides with the obstacle; R _{delta_yaw} is the penalty value for the change of the angular direction of the mobile robot in adjacent time steps, so as to control the change of the angular direction of the mobile robot i and make the overall motion trajectory smoother. 6. A cascaded multi-mobile robot flexible formation method according to claim 5, characterized in that the target action value network is updated by using the updated online action network and the online action value network parameters after each small batch of training. The specific description form is as follows: η′←τη”+(1-τ)η′ (14) Among them, η′ and η” represent the target network parameters and the current network parameters, and τ is used to control the update ratio. 7. A cascaded multi-mobile robot flexible formation method according to claim 1, characterized in that the method also includes a local collision detection step, and the local collision detection step is used to detect the safe distance between the local obstacle and the robot. If the returned safe distance meets the safety state requirements, the mobile robot individual exits the flexible obstacle avoidance strategy and restores the formation strategy.