Multi-robot dynamic complex environment local obstacle avoidance system and method based on pigeon flock escape behavior

Abstract

The invention discloses a multi-robot dynamic complex environment local obstacle avoidance system and method based on pigeon flock escape behaviors. The method comprises the steps of 1, initializing a robot model and sensing scene setting; 2, constructing a robot dynamic limitation perception model; 3, designing a dynamic judgment method simulating pigeon flock escape; 4, designing a behavior simulating a pigeon flock escape dynamic decision; 5, designing a robot motion controller; and 6, outputting a dynamic planning obstacle avoidance result of the robot. The system comprises 1) a robot dynamic limit sensing module, 2) a dynamic judgment module simulating pigeon flock escape, 3) a behavior design module simulating pigeon flock escape dynamic decision, and 4) a multi-robot motion controller module. The method has the advantages that (1) the system framework is reasonable and efficient, and the local obstacle avoidance task effect of multiple robots in the dynamic complex environment can be met; (2) the robustness of use in an unknown space is enhanced, and the efficiency in the dynamic complex environment is guaranteed; and 3) the implementation is strong, the real-time performance is good, and the actual task requirement is met.

Description

Technical Field

[0001] The present invention discloses a multi-robot dynamic complex environment local obstacle avoidance system and method based on the escape behavior of pigeon flocks, belonging to the field of multi-robot autonomous navigation and cooperative control. Background Art

[0002] With the development of intelligent and autonomous equipment, the application field of intelligent robot equipment has been widely expanded. In complex indoor environments, the requirements for the decision-making ability, behavioral logic and robustness of intelligent robots are relatively high, especially in certain specific emergency and complex environments, such as rescue robots in fire environments, exploration robots in mines, robots in extreme environments such as underwater or deep underground, etc., such environments cannot obtain reliable global map information, so the real-time and robustness requirements for robot behavior decisions are more stringent. Especially under the above-mentioned specific emergency conditions, the robot's surrounding environment is complex and has the characteristics of rapid changes. Therefore, the credibility of the prior map condition information is poor, and it mainly depends on the robot's own sensors to perceive the surrounding environmental information as the robot's input, so as to complete the robot's behavioral logic decision. The present invention aims to propose a local obstacle avoidance method in a dynamic and complex environment with simple principle, efficient behavior, direct and flexible, so as to improve the efficiency of the robot's local path planning and the ability level of task execution.

[0003] The local path planning algorithm of the robot in a dynamic and complex environment can be regarded as a combination of dynamic environmental perception and analysis, and the robot's motion control. The current mainstream local path planning algorithms include the dynamic window method, the time elastic band method, the A* algorithm and its derivative algorithms, etc., but these algorithms are mostly used in the path planning process in a static environment. The real-time performance of the algorithm is poor, and the global map is mostly used to generate the global path, which is difficult to apply in the above-mentioned dynamic and complex environment. As for the artificial potential field method, it may fall into the problem of local optimality during the process of path planning of the robot, and the coefficients of its attraction and repulsion need to be adjusted according to the specific environment, which is particularly difficult in a dynamic unknown environment and has a greater impact on the planning effect.

[0004] Pigeons are a typical gregarious animal in nature. The group flight behavior of pigeons not only shows how animal groups can be organized into large groups without obvious command, but also has important significance for the study of animal group behavior. Pigeons fly at a high speed and cannot stay in the air due to lift factors. Therefore, pigeons have a short decision-making time in a complex environment and will choose the most favorable way to avoid obstacles during flight. This also reflects the flexible characteristics of pigeons during flight. Under this logic, they fly to their destination.

[0005] In summary, the present invention proposes a multi-robot dynamic complex environment local obstacle avoidance system and method based on the escape behavior of pigeon flocks, which maps the flight decision-making behavior of pigeons in a complex environment to the indoor robot obstacle avoidance model in a dynamic complex environment, and is used in an obstacle avoidance environment in a dynamic complex environment based on visual perception. The control decision-making method is simple, efficient, and real-time, which conforms to the actual application scenario and has certain reference significance. Summary of the invention

[0006] The purpose of the present invention is to provide a multi-robot complex environment local obstacle avoidance system and method based on the escape behavior of pigeons, aiming to solve the problem of motion control and decision-making in the local range of the robot path planning process under limited visual or radar perception conditions. By drawing on the flight escape behavior of pigeons in nature, a robot information model under local perception is established, which is mapped to the robot decision-making process, providing a new solution for the local path decision and planning problems of robots in dynamic and complex environments.

[0007] The present invention proposes a multi-robot dynamic complex environment local obstacle avoidance system based on the pigeon flock escape behavior, which is as follows:

[0008] System framework diagram Figure 2 As shown in the figure, the multi-robot dynamic complex environment local obstacle avoidance system based on the pigeon flock escape behavior consists of four parts, namely: 1) robot dynamic limitation perception module, 2) dynamic judgment module imitating the pigeon flock escape, 3) behavior design module imitating the dynamic decision of the pigeon flock escape, and 4) multi-robot motion controller module.

[0009] 1) The robot dynamic limit perception module is used to limit sensor information, determine whether the detection conditions are met, record the returned distance value, and store the sensor detection results. Among them, limit the sensor information, establish the actual sensor limit conditions, and limit the robot's dynamic local obstacle avoidance process. Whether the detection conditions are met, record the returned obstacle distance value according to whether the sensor can detect the conditions. If the detection conditions are met, record the returned distance value; store the sensor detection results, and store the returned distance value recorded by each robot in the register to provide information for the subsequent dynamic decision-making judgment of the robot.

[0010] 2) A dynamic judgment module that simulates the escape of a flock of pigeons is used to read the robot's sensor information and posture information, determine the heading direction, determine the obstacle in the target point direction, determine the obstacle in the forward direction, and determine the direction of the gap. Among them, the robot sensor information and posture information are read, and the result data perceived by the sensor and its own posture information data are integrated through the transformation relationship among the sensor coordinate system, the body coordinate system and the ground coordinate system; the heading direction is judged, according to the relationship between the target point direction and the robot's forward yaw angle direction, the result is judged as the target point is within the heading range and the target point is outside the heading range; the target point direction obstacle judgment, through the information detected by the sensor, the obstacle information in the target point direction is judged, and the result is judged as the existence of obstacles in the target point direction and the absence of obstacles in the target point direction; if there is an obstacle in the target point direction, the forward direction obstacle judgment is performed, and according to the obstacle information in the current robot's forward yaw angle direction, the result is judged as the existence of obstacles in the forward direction and the absence of obstacles in the forward direction; if there is an obstacle in the forward direction, the gap direction judgment is performed, and according to the result data information in each direction of the robot sensor, the result is judged as the existence of gap direction and the absence of gap direction; the above judgment results are all transmitted to the behavior design module of the dynamic decision-making of the simulated pigeon flock escape.

[0011] 3) A behavior design module for dynamic decision-making of pigeon flock escape, which is used to integrate the judgment results, output the behavior decision results of pigeon flock escape judgment, judge the robot safety distance and output the cluster collaborative planning decision results of pigeon flock interaction. Among them, the judgment results are integrated, receiving the judgment results from the dynamic judgment method system module of pigeon flock escape; outputting the behavior decision results of pigeon flock escape judgment, and outputting the expected yaw angle of each robot according to the judgment results; judging the robot safety distance, comparing the actual distance between each robot with the safety distance, and generating a cluster collision avoidance input if the actual distance is less than the safety distance; outputting the cluster collaborative planning decision results of pigeon flock interaction, and transmitting the generated cluster collision avoidance input to the multi-robot motion controller.

[0012] 4) Multi-robot motion controller module, including forward controller, steering controller, output limiting and updating robot posture information. Among them, the forward controller receives the robot's expected position and interaction input output by the decision-making behavior module, and outputs the robot's expected acceleration; the steering controller receives the robot's expected yaw angle output by the decision-making behavior module, and outputs the robot's expected angular velocity; the output limiting limits the expected acceleration and expected angular velocity output by the forward controller and the steering controller; the robot posture information is updated, and the control quantity is received to act on the robot's motion model to update all the state quantity information of the drone in real time.

[0013] A multi-robot local obstacle avoidance method in a dynamic complex environment based on the escape behavior of pigeon flocks. The specific implementation steps are as follows:

[0014] Step 1: Initialize the robot model and perception scene settings

[0015] S11. Initialize complex map scene model

[0016] The present invention draws on the scene when a flock of pigeons escapes and returns to their nests, where the pigeon nest is the target point of the pigeons' movement, and the trees in the forest are obstacles that the pigeons need to avoid during flight. By analogy with the local obstacle avoidance process of a robot in a dynamic and complex environment, each individual in the pigeon flock is regarded as each robot, and the trees in the forest are regarded as obstacles that the robot needs to avoid during movement, such as Figure 1 Since the present invention aims to study the local obstacle avoidance method of ground robots in dynamic and complex environments, the task map is regarded as a two-dimensional space

[0017] S12. Initialize the space coordinate system and the initial state of the robot

[0018] Since the robot needs to constantly change its position and posture information during movement, and the coordinate system is a mathematical tool used to describe the position and posture of the robot, different coordinate systems provide descriptions of the robot state from different perspectives. The following three coordinate systems are defined: ground coordinate system O g X g Y g , body coordinate system O b X b Y b , sensor coordinate system O s X s Y s .

[0019] Among them, the ground coordinate system O g X g Y g Take a certain point on the ground as the origin O g , the horizontal direction is O g X g Axis, vertical direction is O g Y g Axis; body coordinate system O b X b Y b The center of the robot is taken as the origin O b , the robot moves in the direction of O b X b Axis, left direction is O b Y b Axis; sensor coordinate system O s X s Ys The center of the sensor body is taken as the origin O s , the sensor front is O s X s Axis, left direction is O s Y s The relationship between different coordinate systems is described using the rotation matrix R and the translation vector t.

[0020] p i =R ij p j +t ij (i,j=g,b,s) (1)

[0021]

[0022] In the formula, p i,j are a certain point in the coordinate system O i X i Y i and coordinate system O j X j Y j The coordinates below; θ ij is the coordinate system O i X i Y i and coordinate system O j X j Y j The angle between ij , Δy ij The coordinate system O i X i Y i and coordinate system O j X j Y j The offset between the two axes.

[0023] Step 2: Building a robot dynamic limit perception model

[0024] S21. Establishing a limiting sensor model

[0025] like Figure 1 The process of the pigeons and robots perceiving the surrounding environment shown in the figure requires simulating the behavior of the pigeons in acquiring visual information in a simulated experimental environment. The robot moves in an unknown environment and needs to continuously receive information from sensors to update its own behavior judgment conditions. In reality, radar sensors or depth cameras can be used to receive distance information from the surrounding environment. Set the perception angle to 2β, that is, the range in the sensor coordinate system is [-β, +β], the angle division value is Δβ, and the perception range radius is R s .

[0026] S22. Sensor perception methods in dynamic environments

[0027] After setting the sensors carried by the robot, it is necessary to collect information about the surrounding environment at each planning moment. When equation (3) is satisfied, it can be considered that the sensors carried by the robot can detect obstacle pixels in the target direction, so as to collect information to decide on the robot's local obstacle avoidance method that simulates the escape of a flock of pigeons.

[0028]

[0029] In the formula, ob i (i=1,2,…) is the obstacle pixel in the robot’s external environment; ||·|| is the norm of the vector; <·,·> is the angle between two vectors in Euclidean space; (·,·) is the inner product of two vectors in Euclidean space; the space satisfying the above formula is defined as the sensor’s detection space Ω, and the distance detected by the sensor is stored in result_sensor.

[0030] result_sensor(m)m∈Β={-β,-β+Δβ,-β+2Δβ,...,+β-Δβ,+β} ​​(4)

[0031] Step 3: Design of dynamic judgment method for escaping pigeon flocks

[0032] When a pigeon is flying in a complex environment, it has a short decision-making time. It also needs to consider the safety and efficiency of the flight process. It will choose the most favorable way to avoid obstacles during the flight. Therefore, it is necessary to classify and judge the received information so that each pigeon can choose a strategy based on the judgment results. The following summarizes four judgment conditions, namely, heading direction judgment, target point direction obstacle judgment, forward direction obstacle judgment, and gap direction judgment, which are mapped to the dynamic judgment behavior of the robot.

[0033] S31, heading direction judgment

[0034] When the pigeon flock is flying, it needs to regard the pigeon nest as the overall mission destination. Therefore, it is necessary to judge the deviation between the current forward direction and the destination direction, so as to continuously adjust the forward direction and drive the robot to the mission destination. The mathematical model is established as follows.

[0035]

[0036]

[0037] In the formula is the coordinate of the target point target in the body coordinate system of robot i; p g,target is the coordinate of the punctuation point target in the ground coordinate system; (xi ,y i ,ψ i ) is the position information of robot i, which are the two-axis coordinates and yaw angle in the ground coordinate system; tar i is the deflection angle between the target point target and the navigation of robot i; It is the judgment result of robot i in the heading direction judgment stage.

[0038] S32, target point direction obstacle judgment

[0039] When the pigeons are flying back to their nests, they need to determine whether there are obstacles between themselves and the nests. If there are obstacles, they need to avoid them. When the direction of the target point is not within the detection range of robot i at this time, robot i needs to change to judge the direction closest to the target point within the detection range. The mathematical model of the robot's judgment process in local path planning is established as follows.

[0040]

[0041] In the formula, when result_sensor i When the distance stored in is the maximum distance detected by the sensor, it can be considered that there is no obstacle in this direction at this time; It is the angle value needed to judge the obstacle in the target point direction; It is the judgment result of robot i in the obstacle judgment stage of the target point direction.

[0042] S33, obstacle judgment in the forward direction

[0043] When the pigeons are flying back to the nest, they need to judge whether there are obstacles between themselves and the nest, and whether there are obstacles in their direction. If there are obstacles in their direction, they need to avoid them, which is similar to the obstacle judgment stage at the target point. i In result_sensor i The corresponding angle is 0, and the mathematical model of the robot's judgment process in local path planning is established as follows.

[0044]

[0045] In the formula, It is the judgment result of robot i in the obstacle judgment stage in the forward direction.

[0046] S34, gap direction judgment

[0047] When flying, pigeons need to judge whether there are obstacles in certain directions. In some cases, they also need to judge whether there are obstacles in all directions in front of them, that is, whether they can cross the obstacles in front of them. When the forward direction is blocked by obstacles, they need to adjust the forward direction in time. The mathematical model of this part is established as follows.

[0048]

[0049] m∈Β={-β,-β+Δβ,-β+2Δβ,...,+β-Δβ,+β} ​​(13)

[0050] In the formula, It is the judgment result of robot i in the gap direction judgment stage.

[0051] Step 4: Behavior design of dynamic decision-making of pigeon flock escape

[0052] S41, behavioral decision-making of imitating the escape judgment of pigeon flocks

[0053] Based on the results of the dynamic judgment method for simulating the escape of a flock of pigeons described in step three, a decision is made on the behavior of each individual in the flock of pigeons.

[0054]

[0055] In the formula, is the expected yaw angle of robot i; ~ means it is independent of the value of this independent variable; With α i The definition of is as follows.

[0056]

[0057] S42. Cluster collaborative planning and decision-making based on pigeon group interaction

[0058] When the flock of pigeons fly back to the nest, in addition to considering the relationship between each individual pigeon and obstacles, it is also necessary to consider the mutual influence between the pigeons in the flock to avoid collisions between the pigeons. When multiple robots perform tasks in the same mission area, it is also necessary to consider the anti-collision measures between multiple robots and set the safety distance of the robot to R. safe , when the distance between the robots is less than R safe This will have an effect, making them move away from each other and avoid collision, as shown below.

[0059]

[0060] In the formula, k safe is the aircraft collision avoidance effect coefficient; It is the input of the cluster collision avoidance effect of the pigeon flock interaction.

[0061] Step 5: Robot motion controller design

[0062] The robot's movement process includes two parts: forward control and steering control, as shown in the following formula.

[0063]

[0064] In the formula, is the position of robot i in the ground coordinate system; x i ,y i are the components of the position of robot i along the x-axis and y-axis in the ground coordinate system; V i , i are the forward speed and yaw angle of robot i in the ground coordinate system; k V , k ψ are the gain parameters of the forward controller and the steering controller respectively; are the expected inputs of the forward controller and the steering controller of robot i respectively.

[0065] The constraints imposed on the robot motion model are shown below.

[0066]

[0067] Step 6: Output the robot's dynamic planning obstacle avoidance results

[0068] The model adopts a discrete update mechanism, defines the decision time interval Δt, updates the status information of each robot, and repeats the above steps. When all robots reach the target point, the task is considered completed. During the simulation, the perception process of each robot and the decision results of the robot can be displayed, and the trajectory diagram of the robot's dynamic local obstacle avoidance can be output.

[0069]

[0070] The present invention proposes a multi-robot dynamic complex environment local obstacle avoidance system and method based on the pigeon flock escape behavior, and its advantages and functions are: 1) The present invention provides a multi-robot dynamic complex environment local obstacle avoidance system framework and its workflow based on the pigeon flock escape behavior. The system framework is reasonable and efficient, and can meet the multi-robot dynamic complex environment local obstacle avoidance task effect 2) The present invention constructs a robot dynamic restricted perception model, supports the robot to dynamically perceive the surrounding environment information in an unknown environment, limits the acceptance amount of simulated environment information, enhances the robustness of the method in unknown space, designs a dynamic judgment behavior method that simulates the escape of a pigeon flock, dynamically plans the local path of the robot, and ensures the efficiency of the method in a dynamic complex environment; 3) The present invention constructs a cluster collaborative planning strategy that simulates the interaction of a pigeon flock, ensures the task coordination of multiple robots in the same task environment, has strong feasibility and good real-time performance, and meets the actual task requirements. BRIEF DESCRIPTION OF THE DRAWINGS

[0071] Figure 1 This is a scenario diagram of a multi-robot local obstacle avoidance system and method in a dynamic complex environment based on the escape behavior of pigeon flocks.

[0072] Figure 2 This is a framework diagram of the multi-robot dynamic complex environment local obstacle avoidance system based on the escape behavior of pigeon flocks.

[0073] Figure 3 This is a flow chart of the local obstacle avoidance method for multiple robots in dynamic complex environments based on the escape behavior of pigeon flocks.

[0074] Figure 4a , 4b ,4c is a schematic diagram of local perception and decision-making of multiple robots in a dynamic and complex environment.

[0075] Figure 5 Scenario diagram of multi-robot sensor detection data for the entire simulation process.

[0076] Figure 6 It is a curve diagram of multi-robot yaw angle data during the whole simulation process.

[0077] Figure 7 This is a curve chart of real-time trajectory data of multiple robots avoiding obstacles during the whole simulation process.

[0078] The numbers and symbols in the figure are explained as follows:

[0079] x i (t), y i (t), V i (t),ψ i (t)——horizontal and vertical coordinates, speed, and yaw angle of robot i at time t;

[0080] ——Dynamic decision-making behavior output information;

[0081] V. ——The output of the robot's forward and steering controllers;

[0082] ——Expected input of forward controller, expected input of steering controller, input of cluster collision avoidance action;

[0083] t——simulation time;

[0084] Δt——simulation step size. DETAILED DESCRIPTION

[0085] See Figures 1 to 7 , the effectiveness of the multi-robot dynamic complex environment local obstacle avoidance system and method based on the pigeon flock escape behavior proposed in the present invention is verified by a specific example. In this example, the number of robots set to explore the task area is 5, and path planning is performed without prior map information. The hardware conditions of the simulation environment configuration of this example are Intel (R) Core (TM) i7-10750H CPU, 2.60GHz main frequency, 16G memory, and the software conditions are MATLAB R2020a version.

[0086] A multi-robot local obstacle avoidance system in dynamic complex environment based on the escape behavior of pigeon flocks, as follows:

[0087] System framework diagram Figure 2 As shown in the figure, the multi-robot dynamic complex environment local obstacle avoidance system based on the pigeon flock escape behavior consists of four parts, namely: 1) robot dynamic limitation perception module, 2) dynamic judgment module imitating pigeon flock escape, 3) behavior design module imitating dynamic decision-making of pigeon flock escape, and 4) multi-robot motion controller module.

[0088] 1) The robot dynamic limit perception module is used to limit sensor information, determine whether the detection conditions are met, record the returned distance value, and store the sensor detection results. Among them, limit the sensor information, establish the actual sensor limit conditions, and limit the robot's dynamic local obstacle avoidance process. Whether the detection conditions are met, record the returned obstacle distance value according to whether the sensor can detect the conditions. If the detection conditions are met, record the returned distance value; store the sensor detection results, and store the returned distance value recorded by each robot in the register to provide information for the subsequent dynamic decision-making judgment of the robot.

[0089] 2) A dynamic judgment module that simulates the escape of a flock of pigeons is used to read the robot's sensor information and posture information, determine the heading direction, determine the obstacle in the target point direction, determine the obstacle in the forward direction, and determine the direction of the gap. Among them, the robot sensor information and posture information are read, and the result data perceived by the sensor and its own posture information data are integrated through the transformation relationship among the sensor coordinate system, the body coordinate system and the ground coordinate system; the heading direction is judged, according to the relationship between the target point direction and the robot's forward yaw angle direction, the result is judged as the target point is within the heading range and the target point is outside the heading range; the target point direction obstacle judgment, through the information detected by the sensor, the obstacle information in the target point direction is judged, and the result is judged as the existence of obstacles in the target point direction and the absence of obstacles in the target point direction; if there is an obstacle in the target point direction, the forward direction obstacle judgment is performed, and according to the obstacle information in the current robot's forward yaw angle direction, the result is judged as the existence of obstacles in the forward direction and the absence of obstacles in the forward direction; if there is an obstacle in the forward direction, the gap direction judgment is performed, and according to the result data information in each direction of the robot sensor, the result is judged as the existence of gap direction and the absence of gap direction; the above judgment results are all transmitted to the behavior design module of the dynamic decision-making of the simulated pigeon flock escape.

[0090] 3) The behavior design module for dynamic decision-making of pigeon flock escape is used to integrate the judgment results, output the behavior decision results of pigeon flock escape judgment, judge the robot safety distance and output the cluster collaborative planning decision results of pigeon flock interaction. Among them, the judgment results are integrated, the judgment results from the dynamic judgment method system module of pigeon flock escape are received; the behavior decision results of pigeon flock escape judgment are output, and the expected yaw angle of each robot is output according to the judgment results. The output result diagram is as follows: Figure 4a , 4b , as shown in 4c; determine the robot safety distance, compare the actual distance between each robot with the safety distance, and generate a cluster collision avoidance input if the actual distance is smaller than the safety distance; output the cluster collaborative planning decision result of the pigeon group interaction, and transmit the generated cluster collision avoidance input to the multi-robot motion controller.

[0091] 4) Multi-robot motion controller module, including forward controller, steering controller, output limiting and updating robot posture information. Among them, the forward controller receives the robot's expected position and interaction input output by the decision-making behavior module, and outputs the robot's expected acceleration; the steering controller receives the robot's expected yaw angle output by the decision-making behavior module, and outputs the robot's expected angular velocity; the output limiting limits the expected acceleration and expected angular velocity output by the forward controller and the steering controller; the robot posture information is updated, and the control quantity is received to act on the robot's motion model to update all the state quantity information of the drone in real time.

[0092] The multi-robot local obstacle avoidance method in dynamic complex environment based on the pigeon flock escape behavior is shown in the following flowchart: Figure 3 As shown, the specific practical steps of this example are as follows:

[0093] Step 1: Initialize the robot model and perception scene settings

[0094] S11. Initialize complex map scene model

[0095] In the ground coordinate system, select a point in the lower left corner of the map as the origin O g , set the target point position coordinates to (100,100) m, randomly distribute cylindrical obstacles in the task area, and set the number of obstacles n ob is 25, the obstacle center coordinates and obstacle radius (x ob,i ,y ob,i ,r ob,i ) are (10,20,4.5)m, (30,10,4.5)m, (40,40,9.5)m, (15,50,4.5)m, (80,20,9.5)m, (40,70,9.5)m, (70,50,9) respectively. .5)m, (60,70,4.5)m, (50,20,4.5)m, (80,80,4.5)m, (90,70,4.5)m, (70,90,4.5)m, (20,38,2.5)m, (30,24,2 .5)m, (20,80,4.5)m, (60,30,2.5)m, (90,40,4.5)m, (50,55,2.5)m, (50,90,4.5)m, (75,68,2.5)m, (18,8,2.5)m, (35,92,2.5)m, (60,10,2.5)m, (93,55,2.5)m, (20,65,2.5)m. Considering the actual volume of the robot in practice, the expansion radius r is set to 0.5m for each obstacle.

[0096] S12. Initialize the space coordinate system and the initial state of the robot

[0097] In the ground coordinate system, five robots are set to perform tasks, with their respective coordinates, initial yaw angles and initial velocities (x i ,y i ,ψ i ,v i ) are respectively The body coordinate system of each robot is fixedly connected to its own body.

[0098] Step 2: Building a robot dynamic limit perception model

[0099] S21. Establishing a limiting sensor model

[0100] Assume that the sensor is installed on the robot with a constant posture, that is, assume that the body coordinate system coincides with the sensor coordinate system, and set the sensor's perception angle to Right now The graduation value is The sight radius is R s =10m.

[0101] S22. Sensor perception methods in dynamic environments

[0102] After setting the sensors carried by the robot, the information of the surrounding environment is collected at each planning moment according to formula (3), and the results are stored in result_sensor.

[0103] Step 3: Design of dynamic judgment method for escaping pigeon flocks

[0104] S31, heading direction judgment

[0105] According to formula (7) and formula (8), Determine the result of robot i's heading direction at the current moment.

[0106] S32, target point direction obstacle judgment

[0107] According to formula (9) and formula (10), Determine the result of robot i's obstacle in the direction of the target point at the current moment.

[0108] S33, obstacle judgment in the forward direction

[0109] According to formula (11), Determine the result of robot i's response to obstacles in its forward direction at the current moment.

[0110] S34, gap direction judgment

[0111] According to formula (12) and formula (13), Determine the result of robot i for the gap direction at the current moment.

[0112] Step 4: Behavior design of dynamic decision-making of pigeon flock escape

[0113] S41, behavioral decision-making of imitating the escape judgment of pigeon flocks

[0114] According to the results calculated in step 3, the behavior of each robot is decided, and the expected yaw angle of each robot is calculated by substituting into equations (14) and (15):

[0115] S42. Cluster collaborative planning and decision-making based on pigeon group interaction

[0116] Set your own safety distance R safe =5m, machine-to-machine collision avoidance effect coefficient k safe =0.1, and the cluster collision avoidance force of the pigeon flock interaction is calculated according to formula (16).

[0117] Step 5: Robot motion controller design

[0118] Set the forward controller gain parameter k V =0.8, steering controller gain parameter k ψ = 0.5, according to the calculation of each robot in step 4 and Substituting the value of into equation (17) and equation (18) to obtain the output of each robot controller And according to formula (19), part of the control output is limited.

[0119] Step 6: Output the robot's dynamic planning obstacle avoidance results

[0120] Set the decision time interval Δt = 0.2s, update the posture state information of each robot according to formula (20), repeat the above steps, and when all robots reach the target point, the task is considered completed and the task process ends. During the simulation process, the perception process of each robot and the decision result of the robot can be displayed, and the trajectory diagram of the robot's dynamic local obstacle avoidance can be output.

[0121] Figures 5 to 7 is a simulation result diagram of this embodiment. Figure 5 Scenario diagram of multi-robot sensor detection data for the whole simulation process; Figure 6 It is a curve diagram of multi-robot yaw angle data during the whole simulation process; Figure 7 The following is a curve diagram of the real-time trajectory data of multi-robot obstacle avoidance during the whole simulation process. The simulation ends at the 731st decision step, and all five robots reach the target point without colliding with obstacles, that is, the whole simulation process takes 146.2 seconds.

[0122] Robot No. 4 reaches the target point at 117.8s, robot No. 5 reaches the target point at 119.2s, robot No. 1 reaches the target point at 129.8s, robot No. 3 reaches the target point at 130.4s, and robot No. 2 reaches the target point at the last 146.2s of the simulation.

Claims

1. A multi-robot local obstacle avoidance method in a dynamic complex environment based on the escape behavior of pigeon flocks, characterized by: The steps of this method are as follows: Step 1: Initialize the robot model and perception scene settings, including initializing the complex map scene model, initializing the spatial coordinate system and the initial state of the robot; Step 2: Construct the robot's dynamic constraint perception model, including establishing a constraint sensor model and a sensor perception method in a dynamic environment. After setting the sensors carried by the robot, it is necessary to collect information about the surrounding environment at each planning moment. Step 3: Design of dynamic judgment method for escaping pigeon flocks The received information is classified and judged. Four judgment conditions are summarized as follows: heading direction judgment, target point direction obstacle judgment, forward direction obstacle judgment, and gap direction judgment, which are mapped to the dynamic judgment behavior of the robot; Step 4: behavioral design of dynamic decision-making for escaping a flock of pigeons, including behavioral decision-making for escaping a flock of pigeons according to the dynamic judgment of escaping a flock of pigeons described in step 3, and cluster collaborative planning decision-making for interacting a flock of pigeons, wherein the cluster collaborative planning decision-making for interacting a flock of pigeons is an anti-collision measure between multiple robots when multiple robots perform tasks in the same task area; Step 6: Output the robot's dynamic planning obstacle avoidance results The model adopts a discrete update mechanism, defines the decision time interval Δt, updates the state information of each robot, and repeats the above steps. When all robots reach the target point, the task is considered completed.

2. The method according to claim 1, characterized in that: The specific process of step three is as follows: S31, heading direction judgment Determine the deviation between the current forward direction and the destination direction, so as to continuously adjust the forward direction and drive the robot to the mission destination. The mathematical model is established as follows: In the formula, is the coordinate of the target point target in the body coordinate system of robot i; p g,target is the coordinate of the punctuation point target in the ground coordinate system; (x i ,y i ,ψ i ) is the position information of robot i, which are the two-axis coordinates and yaw angle in the ground coordinate system; tar i is the deflection angle between the target point target and the navigation of robot i; is the judgment result of robot i in the heading direction judgment stage; S32, target point direction obstacle judgment When the pigeons are flying back to their nests, they need to determine whether there are obstacles between themselves and the nests. If there are obstacles, they need to avoid them. When the direction of the target point is not within the detection range of robot i at this time, robot i needs to change to judge the direction closest to the target point within the detection range. The mathematical model of the robot's judgment process in local path planning is established as follows: In the formula, when result_sensor i When the distance stored in is the maximum distance detected by the sensor, it is considered that there is no obstacle in this direction at this time; It is the angle value needed to judge the obstacle in the target point direction; is the judgment result of robot i in the obstacle judgment stage of the target point direction; S33, obstacle judgment in the forward direction When there is an obstacle in the direction of its own advance, it needs to avoid the obstacle, which is similar to the obstacle judgment stage in the direction of the target point. At this time, ψ i In result_sensor i The corresponding angle is 0, and the mathematical model of the robot's judgment process in local path planning is established as follows: In the formula, is the judgment result of robot i in the obstacle judgment stage in the forward direction; S34, gap direction judgment Determine whether there are obstacles in all directions ahead, that is, whether the obstacles ahead can be crossed at this time. When the forward direction is blocked by obstacles, the forward direction needs to be adjusted in time. The mathematical model of this part is established as follows: m∈Β={-β,-β+Δβ,-β+2Δβ,…,+β-Δβ,+β} ​​(9) In the formula, It is the judgment result of robot i in the gap direction judgment stage.

3. The method according to claim 1, characterized in that The specific process of step 4 is as follows: S41, behavioral decision-making of imitating the escape judgment of pigeon flocks Based on the results of the dynamic judgment method for escaping pigeons described in step 3, make decisions on the behavior of each individual in the pigeon flock: In the formula, is the expected yaw angle of robot i; ~ means it is independent of the value of this independent variable; With α i The definition is as follows: S42. Cluster collaborative planning and decision-making based on pigeon group interaction When multiple robots are performing tasks in the same task area, it is also necessary to consider the collision avoidance measures between multiple robots and set the safety distance of the robot to R. safe , when the distance between the robots is less than R safe It will have an effect, making them move away from each other to avoid collision, as shown below: In the formula, k safe is the aircraft collision avoidance effect coefficient; It is the input of the cluster collision avoidance effect of the pigeon flock interaction.

4. The method according to claim 1, characterized in that: The two parts of step 5, forward control and steering control, are shown in the following formula: In the formula, is the position of robot i in the ground coordinate system; x i ,y i are the components of the position of robot i along the x-axis and y-axis in the ground coordinate system; V i , i are the forward speed and yaw angle of robot i in the ground coordinate system; k V , k ψ are the gain parameters of the forward controller and the steering controller respectively; are the expected inputs of the forward controller and the steering controller of robot i respectively; The constraints imposed on the robot motion model are as follows:

5. The method according to claim 1, characterized in that The step six outputs the robot's dynamic planning obstacle avoidance result, specifically:

6. A multi-robot dynamic complex environment local obstacle avoidance system based on pigeon flock escape behavior, used to implement the method according to any one of claims 1 to 5, characterized in that: The system includes: 1) The robot dynamic limit perception module is used to limit sensor information, determine whether the detection conditions are met, record the returned distance value and store the detection results of the sensor; among them, the sensor information is limited, the actual sensor limit conditions are established, and the receiving information of the robot's dynamic local obstacle avoidance process is limited; whether the detection conditions are met is determined, and the returned obstacle distance value is recorded according to whether the sensor detection conditions are met; if the detection conditions are met, the returned distance value is recorded; the detection results of the sensor are stored, and the returned distance value recorded by each robot is stored in the register to provide information for the subsequent dynamic decision-making judgment of the robot; 2) A dynamic judgment module for escaping pigeon flocks, which is used to read the robot's sensor information and posture information, heading direction judgment, target point direction obstacle judgment, forward direction obstacle judgment and gap direction judgment; among them, the robot's sensor information and posture information are read, and the result data perceived by the sensor and its own posture information data are integrated through the transformation relationship among the sensor coordinate system, the body coordinate system and the ground coordinate system; the heading direction judgment, according to the relationship between the target point direction and the robot's forward yaw angle direction, the result is judged as the target point being within the heading range or the target point being outside the heading range; the target point direction obstacle judgment, through the sensor detection Information, judge the obstacle information in the direction of the target point, and judge the result as the situation that there is an obstacle in the direction of the target point or there is no obstacle in the direction of the target point; if there is an obstacle in the direction of the target point, then make a forward direction obstacle judgment, and according to the obstacle information in the direction of the current robot's forward yaw angle, judge the result as the situation that there is an obstacle in the forward direction or there is no obstacle in the forward direction; if there is an obstacle in the forward direction, make a gap direction judgment, and according to the result data information in each direction of the robot sensor, judge the result as the situation that there is a gap direction or there is no gap direction; all the above judgment results are passed to the behavior design module of dynamic decision-making of simulated pigeon flock escape; 3) A behavior design module for dynamic decision-making of pigeon flock escape, which is used to integrate the judgment results, output the behavior decision results of pigeon flock escape judgment, judge the robot safety distance and output the cluster collaborative planning decision results of pigeon flock interaction; wherein, the judgment results are integrated, and the judgment results from the dynamic judgment method system module of pigeon flock escape are received; the behavior decision results of pigeon flock escape judgment are output, and the expected yaw angle of each robot is output according to the judgment results; the robot safety distance is judged, and the actual distance between each robot is compared with the safety distance. If the actual distance is less than the safety distance, a cluster collision avoidance input is generated; the cluster collaborative planning decision results of pigeon flock interaction are output, and the generated cluster collision avoidance input is transmitted to the multi-robot motion controller; 4) A multi-robot motion controller module, including a forward controller, a steering controller, an output limiter and an update of the robot's posture information; wherein the forward controller receives the robot's desired position and interaction input output by the decision-making behavior module, and outputs the robot's desired acceleration; the steering controller receives the robot's desired yaw angle output by the decision-making behavior module, and outputs the robot's desired angular velocity; the output limiter limits the desired acceleration and desired angular velocity output by the forward controller and the steering controller; the robot's posture information is updated, and the control quantity is received to act on the robot's motion model to update all state quantity information of the drone in real time.

Images