A method for multi-robot formation transformation and dynamic obstacle avoidance

Abstract

This invention relates to a multi-robot formation transformation and dynamic obstacle avoidance method, belonging to the field of robotics technology. The method uses a maximum priority assignment algorithm and a Hungarian algorithm to allocate robot positions, minimizing the total path distance for other robots while ensuring the longest path distance is minimized. Then, a reciprocal velocity obstacle method is used to optimize the time lost by robots farther from the target point during obstacle avoidance, further shortening the formation transformation time. Finally, an improved formation velocity obstacle method is used for formation, improving the problem of frequent formation changes in a short period during obstacle avoidance.

Description

Technical Field

[0001] The invention belongs to the technical field of robots and relates to a multi-robot formation transformation and dynamic obstacle avoidance method. Background Art

[0002] With the development and maturity of robot technology, robot formation has become a hot research direction. Since robot formations often need to work in obstacle environments, the obstacle avoidance problem of robot formations must be considered. Unlike single robot obstacle avoidance, robot formations can adapt to the constraints of the external environment by changing formations. The efficiency of formation changes and the quality of formation obstacle avoidance algorithms affect the safety of robot formations. Summary of the invention

[0003] In view of this, the purpose of the present invention is to provide a multi-robot formation transformation and dynamic obstacle avoidance method, using the maximum priority assignment algorithm and the Hungarian algorithm to provide a solution to the position allocation problem; using the reciprocal speed obstacle method to shorten the time to complete the formation transformation; using the improved formation speed obstacle method to form a formation, improving the problem of frequent switching of formations in a short time during the obstacle avoidance process.

[0004] In order to achieve the above object, the present invention provides the following technical solutions:

[0005] A multi-robot formation transformation and dynamic obstacle avoidance method uses the maximum priority assignment algorithm and the Hungarian algorithm to assign positions to robots, and minimizes the total path distance of other robots while ensuring that the longest path distance is minimized; then the reciprocal speed obstacle method is used to optimize the time lost by robots that are far away from the target point during the obstacle avoidance process, further shortening the time to complete the formation transformation; finally, the improved formation speed obstacle method is used to form the formation, which improves the problem of frequent formation switching in a short time during the obstacle avoidance process.

[0006] The method specifically comprises the following steps:

[0007] S1: According to the pilot-following method Formation model design formation description matrix;

[0008] S2: Use the maximum priority assignment algorithm and the Hungarian algorithm to assign positions to the robots, and use the improved reciprocal speed obstacle method to optimize the time lost by robots that are far from the target point during obstacle avoidance;

[0009] S3: Use the improved formation speed obstacle method to form a formation, and improve the problem of frequent formation switching in a short period of time during the obstacle avoidance process;

[0010] S4: Modeling of robot formations, obstacles, and controllers;

[0011] S5: Use the formation controller to control the robot formation and evaluate the operation efficiency and obstacle avoidance effect of the robot formation.

[0012] Further, in step S1, define the position matrix To describe the position of a robot in the desired formation, where R id is the robot's ID number, used to identify the robot; R lid is the ID number of the leader robot in the formation; l id , are the distance and angle between the robot and the leading robot in the expected formation.

[0013] Furthermore, in step S1, the robot formation is usually composed of n wheeled mobile robots with the same kinematic model, and its position coordinate matrix in the global coordinate system ∑XOY can be expressed as: P l =[x l y l θ l ] T , where x l and l They represent the coordinates of the midpoint of the line connecting the two driving wheels of the lth robot on the X-axis and the Y-axis, respectively. l It represents the angle between the velocity direction of the lth robot and the X-axis of the inertial coordinate system.

[0014] Furthermore, in step S2, the time consumption matrix and resource consumption matrix of the robot formation are A N×M and B N×M When the robot reaches the target position, the elements in the time consumption matrix and resource consumption matrix are: ij =b ij =|G j -R i |,1≤i≤N,1≤j≤M; where R i and G j are the position coordinates of the robot and the target point respectively, and N represents the order of the time consumption matrix and resource consumption matrix of the robot formation.

[0015] Further, in step S2, the maximum priority assignment algorithm can be used to obtain the minimum maximum consumption time: t max =max{a ij ,i=1,2,…,N,j=solve[i],j≠0}, where solve[RawIndex]=ColumnIndex; reconstruct the square matrix C (N+n)×(M+m) , the Hungarian algorithm is used to solve the dispatch result with the minimum sum of paths under the condition of the minimum maximum consumption time constraint, the matrix C (N+n)×(M+m)for:

[0016]

[0017] Among them, n and m represent the order that is generally greater than the original matrix.

[0018] Further, in step S2, the reciprocal speed barrier method is improved, specifically including: for two robots A and B with the same performance and volume, the current position vector of A is P A , the position vector of the target point is G A , the current position vector of B is P B , the position vector of the target point is G B , then the proportion of responsibility that robot A needs to bear during the obstacle avoidance process is for:

[0019]

[0020] The proportion of responsibility that robot B needs to bear during the obstacle avoidance process for:

[0021]

[0022] For robot A, the speed obstacle caused by robot B for:

[0023]

[0024] Among them, V A 、V B Respectively represent the speed of robots A and B, V A ′ represents the speed of robot A in the speed obstacle, Indicates the speed obstacle of obstacle B to robot A. The robot with less responsibility will have less change in speed and extra walking distance during obstacle avoidance, and can reach the target position faster. Another robot with more responsibility for obstacle avoidance will spend more time on obstacle avoidance.

[0025] Further, in step S3, the improved formation speed obstacle method specifically includes: formation speed obstacles caused by obstacles in all directions for:

[0026]

[0027] Among them, v θ is the unit velocity along the direction θ, v o represents the speed of obstacle O, τ represents the time range, t - ,t + represents the lower and upper limits of the collision time, po represents the position of the obstacle O in the coordinate system centered on the formation bounding box, and h represents half of the height of the bounding box;

[0028] The formation evaluation function E(F) is:

[0029] E(F)=p F (v F ·v pref )

[0030] Among them, p F Indicates that each formation is artificially given a priority, v F represents the actual speed of the robot formation, v pref represents the expected speed of the robot formation;

[0031] The comprehensive evaluation function E of the selected speed is:

[0032] E=-α 1 ·Δstr+α 2 P-α 3 ·D v

[0033] Among them, Δstr represents the structural deformation index, P represents the formation priority of the formation to which the speed to be evaluated belongs, and its value is equal to p F , D v represents the Euclidean distance between the candidate speed and the desired speed, α 1 , α 2 and α 3 is the weight of each performance indicator.

[0034] Furthermore, in step S4, the ID of the leader robot is k. By combining the position matrices of all robots, any formation consisting of n robots can be represented by a formation description matrix with 4 rows and n columns:

[0035]

[0036] The beneficial effects of the present invention are as follows: the present invention uses the maximum priority assignment algorithm and the Hungarian algorithm to provide a solution to the position allocation problem; uses the reciprocal speed obstacle method to optimize the time wasted by robots that are far away from the target point during the obstacle avoidance process, further shortening the time to complete the formation change; uses the improved formation speed obstacle method to form a formation, improving the problem of frequent switching of formations in a short period of time during the obstacle avoidance process.

[0037] Other advantages, objectives and features of the present invention will be described in the following description to some extent, and to some extent, will be obvious to those skilled in the art based on the following examination and study, or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can be realized and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention will be described in detail below in conjunction with the accompanying drawings, wherein:

[0039] Figure 1 This is a flow chart of the multi-robot formation transformation and dynamic obstacle avoidance method of the present invention;

[0040] Figure 2 To improve the robot trajectory of the formation speed obstacle method under static obstacles;

[0041] Figure 3 To improve the formation speed obstacle method formation bounding box center trajectory under static obstacles;

[0042] Figure 4 To improve the robot trajectory of the formation speed obstacle method under dynamic obstacles;

[0043] Figure 5 It is the center trajectory of the formation bounding box of the improved formation speed obstacle method under dynamic obstacles. DETAILED DESCRIPTION

[0044] The following describes the embodiments of the present invention by specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments only illustrate the basic concept of the present invention in a schematic manner, and the following embodiments and features in the embodiments can be combined with each other without conflict.

[0045] Among them, the drawings are only used for illustrative explanations, and they only represent schematic diagrams rather than actual pictures, and should not be understood as limitations on the present invention. In order to better illustrate the embodiments of the present invention, some parts of the drawings may be omitted, enlarged or reduced, and do not represent the size of actual products. For those skilled in the art, it is understandable that some well-known structures and their descriptions in the drawings may be omitted.

[0046] The same or similar numbers in the drawings of the embodiments of the present invention correspond to the same or similar parts; in the description of the present invention, it should be understood that if the terms "upper", "lower", "left", "right", "front", "rear", etc. indicate the orientation or position relationship, they are based on the orientation or position relationship shown in the drawings, which is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operate in a specific orientation. Therefore, the terms describing the position relationship in the drawings are only used for illustrative purposes and cannot be understood as limiting the present invention. For ordinary technicians in this field, the specific meanings of the above terms can be understood according to specific circumstances.

[0047] See also Figure 1 to Figure 5 , is a multi-robot formation transformation and dynamic obstacle avoidance method, such as Figure 1 As shown, the specific steps of the method are as follows:

[0048] 1) When using a leader robot and two follower robots and selecting a triangle formation as the formation, the formation description matrix is:

[0049]

[0050] 2) A robot formation usually consists of n wheeled mobile robots with the same kinematic model. For the lth wheeled mobile robot in the formation, its position coordinate matrix in the global coordinate system ∑XOY can be expressed as:

[0051] P l =[x l y l θ l ] T

[0052] Among them, x l and l They represent the coordinates of the midpoint of the line connecting the two driving wheels of the robot on the X-axis and the Y-axis, respectively. l It represents the angle between the robot's velocity direction and the X-axis of the inertial coordinate system. The position coordinate matrix of the virtual robot can be obtained from the distance-angle relationship model of the pilot-follower method:

[0053]

[0054] The error between the follower robot and the virtual robot can be expressed as:

[0055]

[0056] Establish a local coordinate system ∑X at the center of the follower robot f O f Y f, e fx and e fy They represent the X coordinates of the follower robot and the virtual robot in the local coordinate system. f Axis and Y f The error component of the axis, then the posture error equation of the follower robot can be obtained as:

[0057]

[0058] 3) In this embodiment, five mobile robots are used, one of which is a pilot robot and the other four are follower robots. The robot's starting speed is 0 m / s, the maximum speed is 0.5 m / s, the robot's radius is 0.2 m / s, and a triangle formation is used.

[0059] The starting position of the leader robot is (0,0), and the starting positions of the other follower robots are (0,1) and (0,-1) respectively. The robot formation moves along the guide line, and the return point is uniformly set to (30,0). After returning to the guide line, the formation continues to move along the guide line. The controller parameters are k1=4.2 and k2=2.7.

[0060] The obstacle is a static obstacle with a position of (20,0) and a radius of 3m. Using the improved formation speed obstacle method, the expansion radius of the bounding box is adjusted according to the distance between the robot formation and the obstacle. The formation path is as follows: Figure 2 The movement trajectory of the center of the formation bounding box is shown in Figure 3 shown.

[0061] from Figure 2 and Figure 3 As can be seen in the figure, the robot formation starts to deviate from the prescribed route to avoid obstacles at position (5.1,0) and returns to the prescribed route at position (29.8,0). The distance traveled on the prescribed route is 21.3m, and the length of deviation from the prescribed route is 24.7m. Compared with the original formation speed obstacle method, the length of deviation from the prescribed path is reduced by 1.2m. At the same time, due to the introduction of the minimum safety distance, the robot formation maintains a minimum interval of 0.8m with the obstacle during obstacle avoidance, ensuring the safety of the robot formation.

[0062] 4) Change the obstacle to a dynamic obstacle, with the starting position of the obstacle at (25,0), a radius of 3m, and a speed of 0.1m / s. Figure 4 The movement trajectory of the center of the formation bounding box is shown in Figure 5 As shown in the figure, the robot formation starts to deviate from the prescribed route to avoid obstacles at position (8.57,0) and returns to the prescribed route at position (30,0). The distance traveled on the prescribed route is 24.57m.

[0063] According to the above experiments, the use of the improved formation speed obstacle method can significantly reduce the path length of the robot formation deviating from the prescribed route, while allowing the robot formation to maintain a certain distance from obstacles, ensuring the safety of the robot formation.

[0064] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solution of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solution, which should be included in the scope of the claims of the present invention.

Claims

1. A multi-robot formation transformation and dynamic obstacle avoidance method, characterized in that: Use the maximum priority assignment algorithm and the Hungarian algorithm to assign positions to the robots, and minimize the total path distance of other robots while ensuring that the longest path distance is minimized; then use the reciprocal speed obstacle method to optimize the time lost by robots that are far from the target point during the obstacle avoidance process; Finally, the improved formation speed obstacle method is used to form a formation, which can improve the problem of frequent formation switching in a short period of time during the obstacle avoidance process. The method specifically comprises the following steps: S1: According to the pilot-following method The formation model designs the formation description matrix, and uses it to design the formation controller; S2: Use the maximum priority assignment algorithm and the Hungarian algorithm to assign positions to the robots, and use the improved reciprocal speed obstacle method to optimize the time lost by robots that are far from the target point during obstacle avoidance; S3: Use the improved formation speed obstacle method to form a formation, and improve the problem of frequent formation switching in a short period of time during the obstacle avoidance process; S4: Modeling of robot formations, obstacles, and controllers; In step S1, define the position matrix To describe the position of a robot in the desired formation, where R id is the robot's ID number, used to identify the robot; R lid is the ID number of the leader robot in the formation; l id , are the distance and angle between the robot and the leading robot in the expected formation, respectively; In step S1, the robot formation consists of n wheeled mobile robots with the same kinematic model, and their position coordinate matrix in the global coordinate system ∑XOY is expressed as: P l =[x l y l θ l ] r , where x l and l Respectively represent the coordinates of the midpoint of the line connecting the two driving wheels of the ,th robot on the X-axis and the Y-axis, θ l Represents the angle between the velocity direction of the lth robot and the X-axis of the inertial coordinate system; The time consumption matrix and resource consumption matrix of the robot formation are A N×M and B N×M When the robot reaches the target position, the elements in the time consumption matrix and resource consumption matrix are: ij =b ij =|G j -R i |, 1≤i≤N, 1≤j≤M; where R i and G j are the position coordinates of the robot and the target point, respectively. N and M represent the order of the time consumption matrix and resource consumption matrix of the robot formation. Using the maximum priority assignment algorithm, the minimum maximum consumption time is: t max =max{a ij ,i=1,2,…,N,j=solve[i],j≠0},where solve[RawIndex]=ColumnIndex; reconstruct the square matrix C (N+n)×(M+m) , the Hungarian algorithm is used to solve the dispatch result with the minimum sum of paths under the condition of the minimum maximum consumption time constraint, the matrix C (N+n)×(M+m) for: Among them, n and m represent the order that is generally greater than the original matrix; The improved reciprocal speed barrier method specifically includes: for two robots A and B with the same performance and volume, the current position vector of A is P A , the position vector of the target point is G A , the current position vector of B is P B , the position vector of the target point is G B , then the proportion of responsibility that robot A needs to bear during the obstacle avoidance process is for: The proportion of responsibility that robot B needs to bear during the obstacle avoidance process for: For robot A, the speed obstacle caused by robot B for: Among them, V A 、V B Respectively represent the speed of robots A and B, V A ' represents the speed of robot A in the speed obstacle, represents the speed obstacle of obstacle B to robot A; In step S3, the improved formation speed obstacle method specifically includes: formation speed obstacles caused by obstacles in all directions for: Among them, v θ is the unit velocity along the direction θ, v o represents the speed of obstacle O, τ represents the time range, t - ,t + represents the lower and upper limits of the collision time, p o represents the position of the obstacle O in the coordinate system centered on the formation bounding box, and h represents half of the height of the bounding box; The formation evaluation function E(F) is: E(F)=p F (v F ·v pref ) Among them, p F Indicates that each formation is artificially given a priority, v F represents the actual speed of the robot formation, v pref represents the expected speed of the robot formation; The comprehensive evaluation function E of the selected speed is: E=-α1·Δstr+α2·P-α3·D v Among them, Δstr represents the structural deformation index, P represents the formation priority of the formation to which the speed to be evaluated belongs, and its value is equal to p F , D v It represents the Euclidean distance between the candidate speed and the expected speed, and α1, α2 and α3 are the weights of each performance indicator.

2. The multi-robot formation transformation and dynamic obstacle avoidance method according to claim 1, characterized in that: In step S4, the ID of the leader robot is k. By combining the position matrices of all robots, any formation consisting of n robots can be represented by a formation description matrix with 4 rows and n columns:

Images