Multi-robot trajectory planning method and computer program product

Abstract

The application provides a multi-robot trajectory planning method, which comprises: a preliminary planning step S21, wherein a plurality of planning trajectories containing time information for a plurality of mobile robots are obtained, the plurality of planning trajectories being planning trajectories generated by performing a time-optimal trajectory planning method on the plurality of mobile robots respectively; a conflict identification step S22, wherein a conflict point between two planning trajectories in the plurality of planning trajectories in the spatial and time dimensions is identified, the conflict point indicating that the mobile robots moving according to the two planning trajectories will arrive at the same position at the same time; and a conflict resolution step S23, wherein the conflict is resolved by adjusting time information of one of the two planning trajectories. A corresponding computer program product is provided. According to the application, the plurality of mobile robots can reach respective destinations without collision in the shortest time.

Description

Technical Field

[0001] The present invention relates to the field of mobile robots, in particular to the field of motion control of mobile robots, and specifically to a multi-robot trajectory planning method and a corresponding computer program product. Background Art

[0002] With rapid economic growth and rising labor costs, mobile robots are increasingly being used in various industrial and domestic environments. For example, mobile robots such as automated guided vehicles (AGVs), autonomous mobile robots (AMRs), and forklifts are key components of modern logistics systems. Mobile robots can move according to planned paths and operational requirements, docking at target locations to complete tasks such as material handling and transportation. Trajectory planning is crucial for mobile robot motion control.

[0003] When multiple mobile robots work together, each plans its own path and moves according to the assigned task. This can easily lead to conflicting trajectories. Typically, obstacle avoidance is used to prevent these robots from moving around. However, in confined working environments, these robots often encounter blocked paths and are unable to navigate.

[0004] The existing technology still has many deficiencies in trajectory planning for multiple mobile robots. Summary of the Invention

[0005] An object of the present invention is to provide an improved multi-robot trajectory planning method and a corresponding computer program product to overcome at least one deficiency of the prior art.

[0006] According to a first aspect of the present invention, a multi-robot trajectory planning method is provided, wherein the multi-robot trajectory planning method includes: a preliminary planning step S21, wherein a plurality of planned trajectories containing time information corresponding to a plurality of mobile robots are obtained, and the plurality of planned trajectories are planned trajectories generated by performing a time optimal trajectory planning method on the plurality of mobile robots respectively; a conflict identification step S22, wherein a conflict point between two of the plurality of planned trajectories in space and time dimensions is identified, and the conflict point indicates that the mobile robots moving according to the two planned trajectories will arrive at the same position at the same time; and a conflict resolution step S23, wherein the conflict is resolved by adjusting the time information of one of the two planned trajectories.

[0007] “Time information” refers to information that can represent the relationship between the position of the mobile robot on the path and time. Since the path is fixed, “time information” can also represent the speed of the mobile robot at each position on the path.

[0008] In an example embodiment, the conflict identification step S22 and the conflict resolution step S23 are repeatedly performed until there is no conflict point between any two of the plurality of planning trajectories.

[0009] In an example embodiment, the conflict identification step S22 comprises: finding all intersection points of each two of the plurality of planning trajectories in spatial dimension; and for each intersection point, checking a time interval between time information of the relevant planning trajectories at the intersection point, and identifying the corresponding intersection point as a conflict point if the time interval is less than a predetermined time interval threshold.

[0010] In an example embodiment, the conflict resolution step S23 comprises:

[0011] sub-step S231 : selecting a conflict point to be resolved and a planning trajectory to be adjusted from the identified conflict points and the planning trajectories having conflict points, wherein the planning trajectory to be adjusted is one of two planning trajectories associated with the conflict point to be resolved or the conflict point to be resolved is one of the conflict points having the planning trajectory to be adjusted,

[0012] sub-step S232: adjusting time information of the planning trajectory to be adjusted at the conflict point to be resolved in a manner of postponing time information of the planning trajectory to be adjusted at the conflict point, so that a time interval between time information of the relevant two planning trajectories at the conflict point is greater than or equal to the time interval threshold,

[0013] sub-step S233: updating time information of a part of the planning trajectory to be adjusted after the conflict point correspondingly based on the adjusted time information of the planning trajectory to be adjusted at the conflict point.

[0014] In an example embodiment, the sub-step S232 and the sub-step S233 are performed in the following manner: postponing the time information of the planning trajectory to be adjusted at the conflict point and the time information of the part of the planning trajectory to be adjusted after the conflict point equally; and / or the postponed time amount is a time point at which another planning trajectory associated with the conflict point to be resolved enters the conflict point to be resolved minus a time point at which the planning trajectory to be adjusted exits the conflict point to be resolved plus the predetermined time interval threshold.

[0015] In an example embodiment, the conflict identification step S22 and the conflict resolution step S23 are performed in the following manner:

[0016] the conflict identification step S22 is performed to identify all conflict points between the plurality of planning trajectories;

[0017] The planning trajectories with conflict points are sorted in at least one of the following manners:

[0018] in order of priority of their corresponding tasks from high to low;

[0019] in order of number of their conflict points from few to many;

[0020] in order of conflict duration of their conflict points from few to many;

[0021] in order of time of their entering conflict points;

[0022] selecting the planning trajectory with the earliest sorting order as a fixed planning trajectory, determining one by one conflict points of the fixed planning trajectory as conflict points to be adjusted, and accordingly determining planning trajectories that conflict with the fixed planning trajectory at the conflict points to be adjusted as planning trajectories to be adjusted, so as to eliminate all conflict points of the fixed planning trajectory; and

[0023] re-executing the conflict identification step S22 to re-identify all conflict points among the planning trajectories.

[0024] In an exemplary embodiment, the conflict identification step S22 and the conflict elimination step S23 are executed in the following manner:

[0025] executing the conflict identification step S22 to identify all conflict points among the planning trajectories;

[0026] sorting the conflict points in order of their occurring time,

[0027] selecting the conflict point with the earliest sorting order as a conflict point to be eliminated, and selecting the planning trajectory that enters the conflict point later among the two planning trajectories associated with the conflict point to be eliminated as a planning trajectory to be adjusted, so as to eliminate the conflict point to be eliminated; and

[0028] re-executing the conflict identification step S22 to re-identify all conflict points among the planning trajectories.

[0029] In an exemplary embodiment, the time-optimal trajectory planning method can perform speed planning on a mobile robot according to a determined path to determine a planning trajectory containing time information that enables the mobile robot to move along the path, and the trajectory planning method comprises:

[0030] determining one of at least two drive wheels of the mobile robot as a constrained wheel, so that as long as the constrained wheel satisfies kinematic and dynamic constraints, other drive wheels that move in coordination with the constrained wheel will satisfy kinematic and dynamic constraints;

[0031] determining a speed of the constrained wheel in such a way that the constrained wheel has one of a maximum speed and a maximum acceleration at any point that satisfies the kinematic and dynamic constraints thereof and the constraints of the path;

[0032] speed planning the other driving wheels in such a way that the determined speed of the constrained wheel is met.

[0033] In one example embodiment, a T-shaped planning method is employed in the process of speed planning the constrained wheel.

[0034] In one example embodiment, the kinematic and dynamic constraints include that the magnitude of the speed of a driving wheel is below a predetermined limit wheel speed for the driving wheel and the magnitude of the acceleration of the driving wheel is below a predetermined limit wheel acceleration for the driving wheel.

[0035] In one example embodiment, the path is speed planned in a sectional manner, and the following steps are performed for at least one section of the path respectively: for a first control point being a starting point of the section, determining one of the at least two driving wheels as a constrained wheel in the section according to the path shape of the section, the motion state of each driving wheel at the first control point, and the kinematic and dynamic constraints of the driving wheels, the constrained wheel being a driving wheel that reaches a limit value of the kinematic or dynamic constraint first in the section according to the path shape of the section and the motion state of each driving wheel at the first control point; speed planning the constrained wheel to determine the speed of the constrained wheel in the section; and determining the speed of the other driving wheels in the section in such a way that the determined speed of the constrained wheel is met.

[0036] In one example embodiment, the mobile robot is a dual-differential-wheel robot, and the at least two driving wheels are a first driving wheel and a second driving wheel arranged symmetrically, wherein the first driving wheel and the second driving wheel are subject to the same kinematic and dynamic constraints.

[0037] In one example embodiment, the constrained wheel in each section is determined in the following manner:

[0038] obtaining a first initial speed v L0 and a second initial speed v R0 of the first driving wheel and the second driving wheel at the first control point;

[0039] determining a value k1 of a speed ratio k determined by the path at a second control point being an end point of the section, the speed ratio k representing a ratio of the speed of the second driving wheel to the speed of the first driving wheel;

[0040] Determine the first maximum speed v of the first driving wheel and the second driving wheel at the second control point respectively Lmax and the second maximum speed v Rmax , the first maximum speed and the second maximum speed respectively represent the maximum speeds that satisfy the kinematic and dynamic constraints of each driving wheel and the restrictions of the path without considering the speeds of the first driving wheel and the second driving wheel before reaching the second control point;

[0041] The first driving wheel is moved from the first initial velocity v at the first control point L0 The speed obtained by accelerating to the second control point with the limit wheel acceleration of the first driving wheel is determined as the first acceleration terminal speed v La , change the second driving wheel from the second initial velocity v at the first control point R0 The speed obtained by accelerating to the second control point with the limit wheel acceleration of the second driving wheel is determined as the second acceleration terminal speed v Ra ;

[0042] The first maximum velocity v at the second control point Lmax With the first acceleration final velocity v La The smaller of the two is determined as the first terminal velocity v L , the second maximum velocity v at the second control point Rmax With the second acceleration final velocity v Ra The smaller of the two is determined as the second terminal velocity v R ;as well as

[0043] The second terminal velocity v R With the first terminal velocity v L The ratio is compared with the speed ratio k1 at the second control point, and the constrained wheel in the section is determined based on the comparison result.

[0044] In an exemplary embodiment, if the second terminal velocity v R With the first terminal velocity v L The ratio of the second final velocity v is greater than the velocity ratio k1 at the second control point, then the first driving wheel is determined to be the constrained wheel in the section; if the second final velocity v R With the first terminal velocity v L The ratio of the second final velocity v is less than the velocity ratio k1 at the second control point, then the second driving wheel is determined to be the constrained wheel in the section; if the second final velocity v R With the first terminal velocity v L If the ratio of the speed ratios φ and φ is equal to the speed ratio k1 at the second control point, then one of the first driving wheel and the second driving wheel is determined to be the constrained wheel in the section.

[0045] In an exemplary embodiment, the motion duration corresponding to each segment is equal to the predetermined control period t, and the first acceleration terminal velocity v Laand a second maximum speed v Ra is determined according to:

[0046] v La = v L0 + a*t

[0047] v Ra = v R0 + a*t

[0048] wherein a denotes a limit wheel acceleration of the first drive wheel and of the second drive wheel.

[0049] In an exemplary embodiment, a first maximum speed v Lmax and a second maximum speed v Rmax are determined according to at least one of the following constraints:

[0050] - a first constraint based on a limit wheel speed v lim : v Lmax ≤ v lim ,

[0051] - a second constraint based on a limit wheel speed v lim and a speed ratio k determined by the path: v Lmax ≤ v lim / k,

[0052] - a third constraint based on a limit wheel acceleration a and a speed ratio change rate k' determined by the path: wherein k'≠0; and

[0053] a first maximum speed v Lmax and a second maximum speed v Rmax at any point on the path of the first drive wheel and of the second drive wheel satisfy: v Rmax = v Lmax *k.

[0054] In an exemplary embodiment, a first maximum speed v Lmax and a second maximum speed v Rmax are determined additionally according to a fourth constraint:

[0055] The determination assumes that the first drive wheel moves along the path at a maximum speed determined by the at least one of the first constraint, the second constraint and the third constraint resulting in at least one of a first preliminary maximum speed varying with a distance of movement L L of the first drive wheel and a second preliminary maximum speed varying with a distance of movement L R of the second drive wheel;

[0056] determining a maximum point and a minimum point of the at least one of the first preliminary maximum velocity and the second preliminary maximum velocity;

[0057] The first maximum velocity v at any point Lmax and / or a second preliminary maximum speed v Rmax satisfy:

[0058] If the arbitrary point is behind the minimum point closest to the arbitrary point, then:

[0059] and / or

[0060] If the arbitrary point is in front of the minimum point closest to the arbitrary point, then:

[0061] and / or

[0062] Among them, L L and L R represents the movement distance of the first driving wheel and the second driving wheel to the arbitrary point, v1 represents the first preliminary maximum speed or the second preliminary maximum speed of the minimum point closest to the arbitrary point, L L1 and L R1 They respectively represent the movement distances of the first driving wheel and the second driving wheel to the nearest minimum point.

[0063] In an exemplary embodiment, the path is a global path determined by performing global path planning based on at least one task point of the mobile robot, and the at least one task point is located on the global path; and / or the path is in the form of a Bezier curve of order 3 or above.

[0064] According to a second aspect of the present invention, a computer program product is provided, comprising computer program instructions, wherein when the computer program instructions are executed by one or more processors, the processors are capable of performing the multi-robot trajectory planning method according to the present invention.

[0065] The positive effect of the present invention is that when multiple mobile robots are working in the same working environment, without collision between the multiple mobile robots, the multiple mobile robots can reach their respective destinations as a whole in the shortest possible time. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] The present invention will be described in more detail below with reference to the accompanying drawings, so that the principles, features and advantages of the present invention can be better understood. The accompanying drawings include:

[0067] Figure 1A mobile robot and its path implementing a trajectory planning method according to an example embodiment of the present application are schematically shown;

[0068] Figure 2 A flowchart of a trajectory planning method for a mobile robot according to an example embodiment of the present application is schematically shown;

[0069] Figure 3 A flowchart of a path partitioned for speed planning according to an example embodiment is schematically shown;

[0070] Figure 4A A plot of the radius of curvature and the curvature on the path in an example embodiment of the present application is schematically shown;

[0071] Figure 4B A plot of the speed ratio and the first and second maximum speeds satisfying the first and second constraints on the path in an example embodiment of the present application is schematically shown;

[0072] Figure 4C and Figure 4D A plot of the speed ratio and the first and second maximum speeds satisfying the first and second constraints on the path in an example embodiment of the present application is schematically shown; Figure 4B A plot of the speed ratio and the first and second maximum speeds satisfying the first and second constraints on the path in an example embodiment of the present application is schematically shown;

[0073] Figure 4E A plot of the speed ratio and the first and second maximum speeds satisfying the first and second constraints on the path in an example embodiment of the present application is schematically shown;

[0074] Figure 4F A plot of the speed ratio and the first and second maximum speeds satisfying the first and second constraints on the path in an example embodiment of the present application is schematically shown;

[0075] Figure 5A A path in an example embodiment of the present application is schematically shown;

[0076] Figure 5B A plot of the speed ratio and the first and second maximum speeds satisfying the first and second constraints on the path in an example embodiment of the present application is schematically shown; Figure 5A A plot of the speed ratio and the first and second maximum speeds satisfying the first and second constraints on the path in an example embodiment of the present application is schematically shown;

[0077] Figure 5C A plot of the speed ratio and the first and second maximum speeds satisfying the first and second constraints on the path in an example embodiment of the present application is schematically shown; Figure 5A A plot of the speed ratio and the first and second maximum speeds satisfying the first and second constraints on the path in an example embodiment of the present application is schematically shown;

[0078] Figure 5D A plot of the speed ratio and the first and second maximum speeds satisfying the first and second constraints on the path in an example embodiment of the present application is schematically shown; Figure 5Athe first maximum speed and the second maximum speed in the exemplary embodiment shown satisfy the first constraint;

[0079] Figure 5E schematically shown in Figure 5A the first maximum speed and the second maximum speed in the exemplary embodiment shown satisfy the first constraint and the second constraint;

[0080] Figure 5F schematically shown in Figure 5A the first maximum speed and the second maximum speed in the exemplary embodiment shown satisfy the first constraint, the second constraint and the third constraint;

[0081] Figures 5G-5H a curve of the first preliminary maximum speed of the first drive wheel in relation to the movement distance of the first drive wheel in one exemplary embodiment is schematically shown in

[0082] Figure 5I schematically shown in Figure 5A the first maximum speed and the second maximum speed in the exemplary embodiment shown satisfy the first constraint, the second constraint, the third constraint and the fourth constraint;

[0083] Figure 6 a flow chart of a multi-robot trajectory planning method according to one exemplary embodiment of the present application is schematically shown;

[0084] Figure 7 five paths for five mobile robots are schematically shown respectively;

[0085] Figure 8 intersection points and conflict points in one exemplary embodiment of the present application are schematically shown; and

[0086] Figure 9 schematically shown in Figure 8 intersection points and conflict points after the conflict point "1-2" is resolved in the exemplary embodiment shown. DETAILED DESCRIPTION

[0087] In order to make the technical problems to be solved by the present application, the technical solutions and the beneficial technical effects more clearly, the present application will be further described in detail below in conjunction with the drawings and multiple exemplary embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application, but not used to limit the protection scope of the present application.

[0088] The present application is applicable to mobile robots, which can be any robot capable of autonomous spatial movement, such as AGV, AMR, etc. The mobile robots can be used to perform various tasks, such as used as warehouse robots, cleaning robots, home companion robots, welcome robots, etc.

[0089] It should be understood that the expressions "first", "second" and the like as used herein merely denote descriptive purposes and should not be understood to indicate or imply relative importance, nor should they be understood to imply a specific number of the technical features indicated.

[0090] The motion control method of the present application is exemplarily explained below. Figure 1 and Figure 2 The motion control method of the present application is exemplarily explained below. Figure 1 A mobile robot 1 and a path 2 thereof implementing a trajectory planning method according to one exemplary embodiment of the present application are schematically shown. Figure 2 A flowchart of a trajectory planning method for a mobile robot 1 according to one exemplary embodiment of the present application is schematically shown.

[0091] In Figure 1 In the embodiment shown, the mobile robot 1 is, for example, a differential robot, i.e. the mobile robot 1 has a differential wheel motion system comprising a first drive wheel (hereinafter exemplarily explained with the left wheel as the first drive wheel) and a second drive wheel (hereinafter exemplarily explained with the right wheel as the second drive wheel). Alternatively, the mobile robot 1 can also be another type of robot, such as a dual-steering wheel robot, etc. Accordingly, the mobile robot 1 can comprise, for example, a dual-steering wheel motion system.

[0092] In the trajectory planning method, the mobile robot 1 is velocity planned according to the determined path 2 to determine a planning trajectory containing time information enabling the mobile robot 1 to move along the path 2. The path 2 can be a global path determined by global path planning according to at least one task point of the mobile robot 1, the at least one task point being located on the global path.

[0093] The path 2 can be in the form of a 3rd order or higher Bezier curve and can be expressed by the following equation:

[0094]

[0095] wherein, denotes the position of the mobile robot 1, i = 0, 1,..., N, N > 3, denotes the coordinates of the control points of the Bezier curve. When s increases from 0 to 1, the corresponding denotes the position of the mobile robot 1 along the path 2 from the start point to the end point. This is particularly advantageous for differential robots. The path 2 having a continuous 2nd derivative can particularly advantageously adapt to the motion characteristics of the differential robot. In particular, the path 2 can have a continuous curvature. This makes the changes in velocity and acceleration of the mobile robot 1 more gradual.Figure 1 Path 2 is shown in the form of a 4th order Bezier curve. However, it should be understood that path 2 can also have other shapes.

[0096] As shown in FIG. 1, the trajectory planning method comprises: step S11, determining one of the at least two drive wheels of the mobile robot 1 as a constrained wheel, such that as long as the constrained wheel satisfies kinematic and dynamic constraints, the other drive wheels that move in coordination with the constrained wheel will satisfy the kinematic and dynamic constraints; step S12, based on the path 2, performing velocity planning for the constrained wheel under the condition that the kinematic and dynamic constraints of the constrained wheel are satisfied, to determine the velocity of the constrained wheel; and step S13, performing velocity planning for the other drive wheels other than the constrained wheel in a manner that coordinates the determined velocity of the constrained wheel. Figure 2 Thus, an alternative trajectory planning method is provided, which can reliably plan a trajectory for the mobile robot 1 that satisfies kinematic and dynamic constraints, among other things.

[0097] In the process of performing velocity planning for the constrained wheel, the constrained wheel is given one of the maximum velocity and the maximum acceleration under the condition that the kinematic and dynamic constraints thereof are satisfied and the constraints of the path 2 are satisfied. Thus, a time-optimal trajectory can be planned for the mobile robot 1.

[0098] The kinematic and dynamic constraints can include that the magnitude of the velocity of the drive wheel is below a predetermined limit wheel velocity for the drive wheel, and the magnitude of the acceleration of the drive wheel is below a predetermined limit wheel acceleration for the drive wheel. The limit wheel velocity and the limit wheel acceleration are limited by the configuration of the mobile robot 1 itself and are independent of the constraints of the path 2. The limit wheel velocity and the limit wheel acceleration are determined by, for example, the motor used to drive the corresponding drive wheel. Alternatively, the kinematic and dynamic constraints can also include that the jerk of the drive wheel is below a predetermined limit wheel jerk for the drive wheel. Alternatively, the first drive wheel and the second drive wheel of the mobile robot 1 can be symmetrically arranged, so as to have the same limit wheel velocity and limit wheel acceleration.

[0099]

[0100] ​The constrained wheel can be determined as the drive wheel that, in the course of the motion, reaches the limit wheel acceleration first from the current control time to the next control time according to the path 2 of the mobile robot 1 and the motion state of the mobile robot 1 at the current time, under the condition that the kinematic and dynamic constraints are satisfied and the wheel speed is as large as possible. Here, the drive wheel that reaches the limit wheel acceleration first means the drive wheel that, in the case that the mobile robot is expected to accelerate as fast as possible along the path from the motion state at the current time as the initial state to the maximum speed allowed by the kinematic and dynamic constraints, will cause the mobile robot to be unable to accelerate at a larger acceleration due to reaching the limit wheel acceleration. For example, if the acceleration of the mobile robot is large to a certain extent, the first drive wheel has reached its limit wheel acceleration first, while the wheel acceleration of the second drive wheel is still below its limit wheel acceleration, then the mobile robot will be unable to accelerate at a larger acceleration due to the first drive wheel reaching the limit wheel acceleration. Therefore, the first drive wheel can be determined as the constrained wheel. If the current motion state of the mobile robot 1 has reached the maximum speed along the path allowed by the kinematic and dynamic constraints, then any drive wheel can be considered as the constrained wheel, or it can also be considered that there is no constrained wheel in this state.

[0101] Optionally, the path 2 is segmented for speed planning, and the following steps are performed for at least one segment of the path 2 respectively: for a first control point as the starting point of the segment, determining one of the at least two drive wheels as the constrained wheel in the segment according to the path shape of the segment, the motion state of each drive wheel at the first control point, and the kinematic and dynamic constraints of the drive wheels, the constrained wheel being the drive wheel that reaches the limit value of the kinematic and dynamic constraints first in the segment according to the path shape of the segment and the motion state of each drive wheel at the first control point; speed planning for the constrained wheel to determine the speed of the constrained wheel within the segment; and determining the speed of the other drive wheels within the segment in a manner coordinated with the determined speed of the constrained wheel. Here, the drive wheel that reaches the limit value of the kinematic and dynamic constraints first means that this drive wheel will reach the limit value of the kinematic and dynamic constraints before or at the same time as the other drive wheels.

[0102] The motion duration corresponding to each segment can be a predetermined control period t. The control period t can be set to be very short, for example, to be a time in the order of milliseconds, for example, less than 10 ms.

[0103] The following will be described in combination with Figure 3 The process of segmenting the path 2 for speed planning will be further described. Figure 3The speed planning of the segments of the path 2 is schematically shown according to an example embodiment. In this example embodiment, the starting point of the path 2 is taken as the first control point, and the speed planning of the segment of the path 2 corresponding to the control period is determined starting from the current control point, then the end point of the segment is taken as the next control point, and the speed planning is continued until the end point of the entire path 2 is reached.

[0104] For each segment, the first initial speed v L0 and the second initial speed v R0 of the first driving wheel and the second driving wheel at the first control point are first obtained. L0 The initial speeds of the first driving wheel and the second driving wheel at the starting point of the path 2 are known. For segments other than the first segment starting from the starting point of the path 2, the first initial speed v R0 and the second initial speed v R0 of the first driving wheel and the second driving wheel at the first control point can be derived from the planning results of the previous segment. In the description herein, the "speed" (also referred to as "wheel speed") of the driving wheel is exemplarily described in terms of linear speed. Since the size of the driving wheel is determined, the relationship between the linear speed and the angular speed of the driving wheel is also determined.

[0105] In addition, the value k1 of the speed ratio k determined by the path 2 at the second control point, which is the end point of the segment, is determined, where the speed ratio k represents the ratio of the speed of the second driving wheel to the speed of the first driving wheel. For a differential wheel motion system, the speed ratio k and the radius of curvature R of the path 2 satisfy: where b represents the wheel base of the first driving wheel and the second driving wheel, the radius of curvature R and the curvature are derivatives of each other, and the curvature of the path 2 at any point is determined. Therefore, for a determined path 2, the speed ratio k at any point on the path 2 is determined. For example, for a path in the form of a Bezier curve, the speed ratio k can be expressed as a function of the variable s: k = g(s). Accordingly, the speed ratio change rate k' can also be determined: k' = g'(s). In the case where the speed ratio k at any point on the path 2 has been determined, the speed ratio k1 at the second control point can be obtained by methods known in the art.

[0106] Furthermore, the first maximum speed v Lmax and the second maximum speed v Rmax of the first driving wheel and the second driving wheel at the second control point are respectively determined, which respectively represent the maximum wheel speed satisfying the kinematic and dynamic constraints of the respective driving wheels and satisfying the constraints of the path 2 without considering the speeds of the first driving wheel and the second driving wheel before reaching the second control point.

[0107] Then, the first driving wheel is accelerated from the first initial speed v L0The wheel speed obtained by starting to accelerate at the limit wheel acceleration of the first driving wheel to the second control point is determined as a first acceleration end speed v La The wheel speed obtained by starting to accelerate at the limit wheel acceleration of the second driving wheel to the second control point is determined as a second acceleration end speed v R0 The wheel speed obtained by starting to accelerate at the limit wheel acceleration of the second driving wheel to the second control point is determined as a second acceleration end speed v Ra In the case where the control period t of the section corresponds to the control period t, the first acceleration end speed v La And the second acceleration end speed v Ra Can be determined according to the following formula:

[0108] v La = v l0 + a*t

[0109] v Ra = v R0 + a*t

[0110] Wherein, a represents the limit wheel acceleration of the first driving wheel and the second driving wheel.

[0111] The smaller of the first maximum speed v Lmax At the second control point and the first acceleration end speed v La Is determined as the first end speed v L , and the smaller of the second maximum speed v Rmax At the second control point and the second acceleration end speed v Ra Is determined as the second end speed v R .

[0112] Then, the ratio of the second end speed v R And the first end speed v L Is compared with the speed ratio k1 at the second control point, and the constrained wheel in the section is determined according to the comparison result.

[0113] After determining the constrained wheel in the section, if the end point of path 2 has not been reached, the current second control point is taken as the first control point of the next section, and the speed planning of the next section is continued.

[0114] Alternatively, the constrained wheel is determined according to the comparison result of the ratio of the second end speed v R And the first end speed v L With the speed ratio k1 at the second control point in the following way: if the ratio of the second end speed v R And the first end speed v L Is greater than the speed ratio k1 at the second control point, the first driving wheel is determined as the constrained wheel in the section; if the ratio of the second end speed v R And the first end speed v LThe ratio of the second final velocity v is less than the velocity ratio k1 at the second control point, then the second driving wheel is determined to be the constrained wheel in the section; if the second final velocity v R With the first terminal velocity v L If the ratio of the speed ratios k1 to 2 is equal to the speed ratio k1 at the second control point, it can be determined that either the first driving wheel or the second driving wheel is the constrained wheel in the section.

[0115] The following combination Figures 4A-4F Detailed description Determine the first maximum speed v of the first drive wheel and the second drive wheel at any point on the path 2 Lmax and the second maximum speed v Rmax process.

[0116] Figure 4A Schematically illustrates a curve showing the variation of the curvature radius R and the curvature к on a path 2 according to an exemplary embodiment of the present invention. In this exemplary embodiment, the path 2 is a fourth-order Bezier curve, and the coordinates of its five Bezier curve control points are (0, -1), (0, 0), (1, 2), (4, 2), and (5, 3).

[0117] According to the path 2, the curvature к at any point on the path 2 can be determined as follows:

[0118]

[0119] Among them, P x ′(s), P y ′(s), P x ″(s), P y ″(s) ​​are The first-order derivative, ordinate and second-order derivative, ordinate. Accordingly, the curvature radius R at any point on the path 2 can be determined.

[0120] Furthermore, the speed ratio k at any point on path 2 can be obtained. Figure 4B The bottom portion of FIG2 schematically shows a variation curve of the speed ratio k on path 2 .

[0121] In this exemplary embodiment, the wheel speed v lim The first constraint and the limit wheel speed v lim The first maximum speed v of the first driving wheel and the second driving wheel at any point on path 2 is determined by the second constraint of the speed ratio determined by path 2. Lmax and the second maximum speed v Rmax .

[0122] The first constraint states that the speed of the driving wheel cannot exceed its limit wheel speed. Therefore, the first maximum speed v of the first driving wheel is Lmax Constraints must be met: v Lmax≤ v lim In this embodiment, the limit wheel speed v lim is pre-set to 1.5 (m / s).

[0123] The second constraint means that the speed of any one of the first and second drive wheels needs to be such that the other drive wheel also cannot exceed its limit wheel speed. Thus, the first maximum speed v Lmax of the first drive wheel needs to satisfy the constraint: v Lmax ≤ v lim / k.

[0124] The first maximum speed v Lmax and the second maximum speed v Rmax of the first and second drive wheels at any point on path 2 also satisfy the constraint: v Rmax = v Lmax *k.

[0125] Thus, the first maximum speed v Lmax and the second maximum speed v Rmax are the maximum wheel speeds that satisfy the constraint:

[0126]

[0127] It can be derived that v Lmax = min(v lim , v lim / k), v Rma = min(v lim , v lim *k), where min(a, b) denotes the smaller of a and b. The derived first maximum speed v Lmax and second maximum speed v Rmax are schematically shown at the top and middle of path 2. Figure 4B

[0128] It can be seen that in determining the first maximum speed v Lmax and the second maximum speed v Rmax , the actual or planned speeds of the first and second drive wheels prior to reaching the arbitrary point are not taken into account in limiting the speeds that they can achieve at the arbitrary point.

[0129] Assuming that the first and second drive wheels move along path 2 at the first maximum speed v Lmax and the second maximum speed v Rmax all the time, the corresponding movement speed v r and movement distance L of the mobile robot 1 will be as follows: Figure 4C ​The time stamp t of the movement of the mobile robot 1 along the path 2 and the first and second required accelerations a Lneed and a Rneed are obtained accordingly. Figure 4D

[0130] In this embodiment, the limit wheel acceleration a of the first and second driving wheels is exemplarily pre-set to 0.5 (m / s 2 ). It can be seen from Figure 4D that the second driving wheel exceeds the limit of the limit wheel acceleration a in the interval of s Figure 4E . Therefore, it is necessary to re-plan this, for example, as follows: starting from the minimum point of the speed in the section exceeding the limit of the limit wheel acceleration, re-plan on both sides. In this embodiment, re-plan from s = 0 in the direction of increasing s. The acceleration is forced to be the limit wheel acceleration a, as shown in Figure 4F . That is, from s = 0, the second driving wheel accelerates at a = 0.5, and the first driving wheel moves in coordination with the second driving wheel to meet the speed ratio determined by the path 2. Obviously, the first driving wheel needs to decelerate first (the magnitude of its acceleration will be below the limit wheel acceleration a) and then accelerate. Figure 4F It can be seen that at the position of s = s2, the first and second driving wheels will reach the first maximum speed v Lmax and the second maximum speed v Rmax . Here, s2> s1. After s = s2, the first and second driving wheels can move along the path 2 at the first maximum speed v Lmax and the second maximum speed v Rmax .

[0131] In the re-planned section, the second driving wheel can be determined as a constrained wheel, and the speed of the second driving wheel is planned to determine the speed of the second driving wheel. Then, the speed of the first driving wheel is planned in a manner that coordinates with the determined speed of the second driving wheel. The process of determining the second driving wheel as a constrained wheel can refer to Figure 3 . In the re-planned section, v R0 + a * t < v Rmax , so v R = v R0 + a * t. And v L0 + a * t > v Lmax , so v L = v Lmax . Further, v R / v L ​<k,因此,以第二驱动轮(即右轮)为约束轮。

[0132] In the above exemplary description, it is assumed that the first driving wheel and the second driving wheel of the mobile robot 1 have a first maximum speed v at the starting point of the path 2. Lmax and the second maximum speed v Rmax When the wheel speeds of the first driving wheel and the second driving wheel at the starting point of the path 2 are other values, the mobile robot 1 needs to go through an acceleration phase that satisfies the limit of the limit wheel acceleration to reach the first maximum speed v Lmax and the second maximum speed v Rmax .

[0133] The following combination Figures 5A-5F DETAILED DESCRIPTION In an exemplary embodiment according to the present invention, a first maximum speed v of the first drive wheel and the second drive wheel at any point on the path 2 is determined. Lmax and the second maximum speed v Rmax process.

[0134] Figure 5A A path 2 in an exemplary embodiment according to the present invention is schematically shown. In this exemplary embodiment, the path 2 satisfies the curve equation: y=sin(π / 2*x). Figure 5B The curve of the curvature к on the path 2 in this exemplary embodiment is schematically shown. Figure 5C The variation curve of the speed ratio k on the path 2 is schematically shown. The process of determining the curvature к and the speed ratio k on the path 2 according to the path 2 can refer to the above description of Figures 4A-4F Description.

[0135] In this exemplary embodiment, Figures 4A-4F Similarly to the embodiment shown, the first maximum speed v of the first driving wheel and the second driving wheel at any point on the path 2 is Lmax and the second maximum speed v Rmax The first and second constraints must be satisfied.

[0136] Figure 5D The first maximum speed v that satisfies the first constraint in this exemplary embodiment is schematically shown. Lmax and the second maximum speed v Rmax The first constraint states that the speed of the driving wheel cannot exceed its limit wheel speed. Therefore, the first maximum speed v Lmax and the second maximum speed v Rmax Constraints must be met: v Lma ≤v lim , v Rmax ≤v lim In this embodiment, the limit wheel speed v of the first driving wheel and the second driving wheellim is set to 1.2 (m / s) in advance.

[0137] Figure 5E The first maximum speed v Lmax and the second maximum speed v Rmax satisfying the first constraint and the second constraint in this example embodiment are schematically shown. Lmax and the second maximum speed v Rmax . The second constraint indicates that the speed of either one of the first drive wheel and the second drive wheel needs to be such that the other drive wheel also cannot exceed its limit wheel speed satisfying the speed ratio k. Therefore, the first maximum speed v Lmax of the first drive wheel needs to satisfy the constraint: v Lmax ≤ v lim / k; and the second maximum speed v Rmax of the second drive wheel needs to satisfy the constraint: v Rmax = v lim *k.

[0138] Thus, the maximum wheel speeds satisfying the first constraint and the second constraint can be obtained as: v Lmax = min(v lim , v lim / k), and v Rmax = min(v lim , v lim *k).

[0139] The first maximum speed v Lmax and the second maximum speed v Rmax of the first drive wheel and the second drive wheel at any point on the path 2 also satisfy the constraint: v Rmax = v Lmax *k.

[0140] In this embodiment, the first maximum speed v Lmax and the second maximum speed v Rmax of the first drive wheel and the second drive wheel at any point on the path 2 can additionally satisfy a third constraint based on the limit wheel acceleration a and the speed ratio change rate k' determined by the path 2: where k'≠0.

[0141] The principle of the third constraint will be described in detail below. As described above, after the path 2 is determined, the speed ratio k and the speed ratio change rate k' at any point on the path 2 can be determined. Assume that the first drive wheel and the second drive wheel of the mobile robot 1 moving along the path 2 have speeds v L0 and v R0 at a certain point on the path 2, and the speed ratio k0=v R0 / v L0 .

[0142] Since the wheel accelerations of the first and second drive wheels cannot exceed the limit wheel acceleration a, after the mobile robot 1 has moved along a path segment of the path 2 for a small time period t, the speed ratio k has a value in the range:

[0143] Then, at this point, the speed ratio change rate k' should satisfy the following equation:

[0144]

[0145] where L represents the movement distance of the mobile robot 1 and the small displacement amount dL represents the displacement of the mobile robot 1 in the small time period t. The small displacement amount dL is equal to the arithmetic mean of the displacements of the first and second drive wheels, i.e. Thus, the above equation can be simplified as:

[0146]

[0147] When v R0 + v L0 is not equal to 0, the above equation can be simplified as:

[0148]

[0149] Thus, when the speed ratio change rate k' ≠ 0, the wheel speed of the first drive wheel should satisfy: Correspondingly, the wheel speed of the second drive wheel should satisfy:

[0150] It should be understood that the speed ratio change rate k' = 0 means that the mobile robot 1 performs a straight line movement or a circular movement at this point. In the case of straight line movement (k' = 0 and k = 1), either of the first and second drive wheels can be determined as the constrained wheel. In the case of circular movement (k' = 0 and k ≠ 1), the outer wheel can be determined as the constrained wheel, i.e. the second drive wheel is determined as the constrained wheel if k > 1, and the first drive wheel is determined as the constrained wheel if k < 1.

[0151] Figure 5F The first maximum speed v Lmax and the second maximum speed v Rmax satisfying the first constraint, the second constraint and the third constraint in this exemplary embodiment are schematically shown.

[0152] Alternatively, the first maximum speed v Lmax and the second maximum speed v Rmax of the first and second drive wheels at any point on the path 2 can also be set with the following fourth constraint.

[0153] The following description is made with reference to Figures 5G-5HTo illustrate the fourth constraint. First, assume that the first drive wheel moves along the path 2 at a maximum speed determined by the first constraint, the second constraint and the third constraint, and that the distance L L the first preliminary maximum speed of the first drive wheel as a function of L Figures 5G-5H the distance L L the first preliminary maximum speed of the first drive wheel as a function of L

[0154] Then, all the maximum points and minimum points of the first preliminary maximum speed can be determined. From each minimum point to the adjacent maximum point (if there is any) on both sides at the limit wheel acceleration a until the curves obtained by accelerating in the same way to the adjacent minimum points on both sides intersect. Then, all the intersection points are connected by curves between adjacent minimum points, i.e. the acceleration constraint curve of the left wheel under the acceleration constraint is obtained.

[0155] Figure 5G illustrates the distance L L = L1, the minimum point (L1, v1) at L L = L2, then: L

[0156]

[0157] where v2 represents the wheel speed of the first drive wheel at L L = L2 at a minimum distance dL L tends to 0,

[0158] Thus, we have:

[0159]

[0160] Correspondingly, the fourth constraint is set to be for the point behind the nearest minimum point on path 2:

[0161]

[0162] It should be understood that in this paper, "front" and "back" refer to the direction of movement of the mobile robot 1 on the path 2.

[0163] ​Figure 5H The minimum point (L1, v1) at L L is accelerated to the adjacent maximum point to the right (i.e. forward) with the limit wheel acceleration a by a minimum distance dL L of L L = L2, then:

[0164]

[0165] where v2 represents the wheel speed of the first driving wheel at L L = L2 with a minimum distance, p represents the slope of the wheel speed of the first driving wheel increasing from the point (L1, v1) to the point (L2, v2) with the limit wheel acceleration a. When dL L approaches 0,

[0166] Thus, it can be obtained that:

[0167]

[0168] Correspondingly, the fourth constraint is set to be for the point on the path 2 in front of the nearest minimum point:

[0169]

[0170] Similarly, the fourth constraint can be similarly set for the second maximum speed v Rmax of the second driving wheel.

[0171] Therefore, according to the fourth constraint, the first maximum speed v Lmax and / or the second preliminary maximum speed v Rmax at an arbitrary point satisfy: if the arbitrary point is behind the nearest minimum point to the arbitrary point, then:

[0172] and / or if the arbitrary point is in front of the nearest minimum point to the arbitrary point, then:

[0173] and / or where L L and L R respectively represent the movement distance of the first driving wheel and the second driving wheel to the arbitrary point, v1 respectively represents the first preliminary maximum speed or the second preliminary maximum speed of the nearest minimum point to the arbitrary point, L L1 and L R1 respectively represent the movement distance of the first driving wheel and the second driving wheel to the nearest minimum point.

[0174] Figure 5I The first maximum speed v Figure 5F is further satisfied on the basis of the fourth constraint. Lmax The second maximum speed v Rmax is further satisfied on the basis of the fourth constraint. In other words, Figure 5I The first maximum speed v Lmax and the second maximum speed v Rmax satisfy the first constraint, the second constraint, the third constraint and the fourth constraint simultaneously.

[0175] Another aspect of the present application proposes a multi-robot trajectory planning method, which can be executed independently of the trajectory planning method described above, and can also preferably be executed in combination with the trajectory planning method described above.

[0176] Figure 6 A multi-robot trajectory planning method according to one exemplary embodiment of the present application is schematically shown.

[0177] As shown in Figure 6 , the multi-robot trajectory planning method at least comprises the following steps: a preliminary planning step S21, in which a plurality of planning trajectories containing time information for a plurality of mobile robots 1 are obtained, the planning trajectories being planning trajectories generated by performing a time-optimal trajectory planning method on the plurality of mobile robots 1 respectively; a conflict identification step S22, in which a conflict point between two planning trajectories in the plurality of planning trajectories in the spatial and temporal dimensions is identified, the conflict point indicating that the mobile robots 1 moving according to the two planning trajectories will arrive at the same position at the same time; and a conflict resolution step S23, in which the conflict is resolved by adjusting the time information of one of the two planning trajectories.

[0178] In this way, when a plurality of mobile robots 1 work in the same working environment, the plurality of mobile robots 1 can reach their respective destinations in the shortest possible time as a whole without collisions between the plurality of mobile robots 1.

[0179] This method can divide the multi-robot trajectory planning method into two layers, i.e., a time-optimal global trajectory planning and a time-adjusted (or speed-adjusted) local trajectory planning. In the time-optimal global trajectory planning, a global path without time information is planned for each mobile robot 1 according to a certain global path planning algorithm, and then a speed planning is performed for each mobile robot 1 in such a way that the mobile robot 1 moves at its maximum motion capability (maximum speed, maximum acceleration, maximum jerk) to obtain a planned trajectory with time information. In the time-adjusted local trajectory planning, based on the planned trajectory with time information obtained in the last layer of time-optimal global trajectory planning, the motion time (i.e., the motion speed) of each planned trajectory is adjusted so that there is no collision point between the planned trajectories. Thus, the multiple planned trajectories obtained can ensure that the multiple mobile robots 1 do not collide with each other and reach their respective destinations in the shortest time as a whole.

[0180] It should be understood that the collision point is not limited to the case where the planned trajectories collide at a single point, but also includes the case where the planned trajectories have overlapping trajectory segments (see Figure 7 ). In this context, obtaining the multiple planned trajectories includes both obtaining the existing multiple planned trajectories in the form of receiving data or reading data, and obtaining the corresponding multiple planned trajectories by trajectory planning for the multiple mobile robots through the trajectory planning method.

[0181] In an exemplary embodiment, the multiple planned trajectories are the planned trajectories generated through the trajectory planning method described above.

[0182] As Figure 6 indicated, the collision identification step S22 and the collision removal step S23 can be repeatedly performed until there is no collision point between any two of the multiple planned trajectories.

[0183] Specifically, in the collision identification step S22, all intersection points in the spatial dimension between each two of the multiple planned trajectories can be first searched. Then, for each intersection point, the time interval between the time information of the relevant planned trajectories at the intersection point is checked, and if the time interval is less than a predetermined time interval threshold, the corresponding intersection point is identified as a collision point. The intersection point represents a point where the paths of the multiple planned trajectories intersect, i.e., a spatial position passed by at least two planned trajectories.

[0184] The conflict resolution step S23 may, for example, include: sub-step S231: selecting a conflict point to be resolved and an adjusted planned trajectory from the identified conflict points and the conflicting planned trajectories, wherein the adjusted planned trajectory is one of the two planned trajectories associated with the conflict point to be resolved or the conflict point to be resolved is one of the conflict points with the adjusted planned trajectory; sub-step S232: adjusting the time information of the adjusted planned trajectory at the conflict point to be resolved by delaying the time information of the adjusted planned trajectory at the conflict point, so that the time interval between the time information of the two associated planned trajectories at the conflict point is greater than or equal to a time interval threshold; and sub-step S233: updating the time information of the portion of the adjusted planned trajectory after the conflict point based on the adjusted time information of the adjusted planned trajectory at the conflict point. In this way, the conflict can be resolved with fewer adjustments.

[0185] The following combination Figure 7 and Figure 8 Exemplary embodiments according to the present invention are further described. Figure 7 Schematically shows five paths for five mobile robots 1. Curves numbered 1-5 correspond to the first to fifth paths of the first to fifth mobile robots 1. The intersection or overlap of the paths indicates the intersection point of the trajectories of the corresponding mobile robots 1 in the spatial dimension. Figure 7 It can be seen that there are five intersection points between the first planned trajectory for the first mobile robot 1 and other planned trajectories, which are the intersection points between the first planned trajectory and the second, fifth, third, fourth and fifth planned trajectories in sequence. Obviously, Figure 7 The time information of the corresponding planned trajectory is not shown.

[0186] After all intersection points are found, for each intersection point, the time information of the relevant planned trajectory entering and exiting the intersection point can be determined, and based on the time interval between the time information of the relevant planned trajectory at the intersection point, it is determined whether each intersection point is a conflict point.

[0187] Figure 8 Schematically illustrates the intersection points and conflict points in an exemplary embodiment according to the present invention. Figure 8 In the figure, for the sake of intuitiveness, each planned trajectory is abstractly illustrated as a horizontal axis. Each horizontal axis corresponds to both a time scale and the distance moved by the mobile robot 1. A point on each horizontal axis represents the position to which the mobile robot 1 will move at the corresponding time according to the time-optimal global trajectory plan. Figure 8In the example shown in FIG. 6, intersection points are identified on the lateral axis of each mobile robot 1 in the form of a rectangular grid, where the lateral axes labeled 1-5 correspond to the planned trajectories of the first through fifth mobile robots 1. The numbers of the mobile robots 1 that intersect at each intersection point are listed in each rectangular grid. For example, the intersection points between the first planned trajectory and the second, fifth, third, fourth, and fifth planned trajectories are labeled "1-2", "1-5", "1-3", "1-4", and "1-5", respectively. The position of a rectangular grid on the lateral axis indicates the time period during which the planned trajectory represented by the lateral axis continues to move at the intersection point represented by the rectangular grid, and the width of the rectangular grid along the lateral axis indicates the length of time during which the planned trajectory continues to move at the intersection point. For example, the first planned trajectory continues to move at the intersection point "1-4" for a shorter period of time than it continues to move at the intersection point "4-5".

[0188] Then, for each intersection point, the time interval between the time information of the relevant planned trajectories at the intersection point can be checked to determine whether the intersection point is a conflict point. If the time interval is less than a predetermined time interval threshold, the corresponding intersection point is identified as a conflict point. The predetermined time interval threshold can be set to 0, for example. For safety, the predetermined time interval threshold can also be set to be greater than 0. Figure 8 The conflict points identified from the intersection points are marked on the lowermost lateral axis in FIG. 6. Taking the intersection point "1-2" as an example, the interval between the time period during which the first planned trajectory continues to move at the intersection point "1-2" and the time period during which the second planned trajectory continues to move at the intersection point "1-2" is less than 0, i.e., the two time periods overlap. Therefore, the intersection point "1-2" is a conflict point. Taking the intersection point "2-5" as another example, the interval between the time period during which the second planned trajectory continues to move at the intersection point "2-5" and the time period during which the fifth planned trajectory continues to move at the intersection point "2-5" is greater than 0, i.e., the two time periods are completely offset. Therefore, the intersection point "2-5" is not a conflict point.

[0189] The intersection points between the planned trajectories can be represented in the form of a matrix. For example, the intersection points between the ith planned trajectory and the other planned trajectories can be represented in the following matrix:

[0190] X i = [T1...T j ...T n ], i = 1, 2,..., n

[0191] where T j represents the intersection point between the ith planned trajectory and the jth planned trajectory, and n represents the number of planned trajectories. In general, T j can be represented in the following form:

[0192]

[0193] where m > 0, and Tijm+1denote the time when the (m+1)th intersection point between the ith planned trajectory and the jth planned trajectory is entered and left, respectively. When there is no intersection point between the ith planned trajectory and the jth planned trajectory, or j = i, it is defined that T j = 0.

[0194] For convenience of representation, the shortest planned trajectory motion time is taken as the normalized time 1, and the other planned trajectories are scaled according to the motion time length. The intersection points between the above 5 planned trajectories can be represented as follows:

[0195]

[0196]

[0197]

[0198]

[0199]

[0200] The subscripts 0 and 1 of the numbers in the above matrix are used to mark that the number is the time when the corresponding planned trajectory enters and leaves the intersection point, respectively. The superscripts 0 and 1 of the numbers are used to mark that the number is the time information of the 1st and 2nd intersection points between the ith planned trajectory and the jth planned trajectory, respectively. For the case where there are more intersection points between the ith planned trajectory and the jth planned trajectory, the above can be extended by analogy. In the case where there is only one intersection point between the ith planned trajectory and the jth planned trajectory, the superscripts of the numbers are omitted.

[0201] For example, the traversal method can be used to search for the conflict points from the intersection points. Starting from the intersection point X1 of the 1st planned trajectory, T2, T3, T4, T5 of X1 are compared with T1 of X2, X3, X4, X5, respectively, and the intersection points with time overlap are marked. Then, T1, T3, T4, T5 of the intersection point X2 of the 2nd planned trajectory are compared with T2 of X1, X3, X4, X5, respectively, and the intersection points with time overlap are marked. Such a cycle is repeated until all the planned trajectories are traversed.

[0202] Through traversal, 5 conflict points between the above 5 planned trajectories can be found: "1-2", "3-5", "1-3", "3-4" and "4-5". In the above matrix, each conflict point is marked in bold. In Figure 8 , the 5 conflict points are shown in the form of rectangular grids in the last horizontal axis direction.

[0203] After finding the conflict point, the conflict resolution step S23 can be executed. Preferably, in the conflict resolution step S23, the time information of the adjusted planned trajectory at the conflict point and the time information of the part of the adjusted planned trajectory after the conflict point are delayed by an equal amount. Since the multiple planned trajectories themselves are time-optimal trajectory planning, this method can ensure that the adjusted planned trajectories still meet the kinematic and dynamic constraints of the mobile robot 1, and enable the multiple mobile robots 1 to reach their respective destinations as soon as possible as a whole without collision. Because the time-optimal global trajectory planning of the previous layer represents the maximum motion capability of the mobile robot 1, when resolving the conflict, only the time when the adjusted planned trajectory enters the conflict point to be resolved is postponed. This backward delay will correspondingly affect all time information of the adjusted planned trajectory after the conflict point to be resolved.

[0204] In one exemplary embodiment, the conflict points to be resolved and the planned trajectories to be adjusted are selected based on the priority (or importance) of the tasks corresponding to the planned trajectories. If different tasks performed by the mobile robot 1 have different priorities, when a conflict point exists, the planned trajectories associated with the higher-priority tasks may be fixed first. The conflict points of the fixed planned trajectories may then be resolved by adjusting the conflicting planned trajectories.

[0205] Specifically, the conflict identification step S22 and the conflict resolution step S23 are performed in the following manner: first, all conflict points between the multiple planned trajectories are identified; the planned trajectories with conflict points are sorted in descending order according to the priority of their corresponding tasks; the planned trajectory with the highest sorting point is selected as the fixed planned trajectory, and the conflict points of the fixed planned trajectory are determined one by one as the conflict points to be adjusted, and accordingly, the planned trajectory that conflicts with the fixed planned trajectory at the conflict point to be adjusted is determined as the adjusted planned trajectory to resolve all conflict points of the fixed planned trajectory; then, the conflict identification step S22 is performed again to re-identify all conflict points between the multiple planned trajectories.

[0206] by Figure 8 Take the five planned trajectories shown as an example. All five planned trajectories have conflict points. Sort them in descending order based on the priority of their corresponding tasks. If the priority of the tasks corresponding to these five planned trajectories is ranked from high to low as 1 > 2 > 3 > 4 > 5, then fix the first planned trajectory first. The conflict points "1-2" and "1-3" of the first planned trajectory are identified as conflict points to be resolved one by one. The corresponding planned trajectories to be adjusted are the second and third planned trajectories, respectively.

[0207] Here, the second planned trajectory is adjusted to remove the conflict point "1-2" between the second planned trajectory and the first planned trajectory. It should be understood that the third planned trajectory can also be adjusted to remove the conflict point "1-3" between the third planned trajectory and the first planned trajectory.

[0208] As described above, the conflict point can be removed in a manner that equally delays the time information of the second planned trajectory at the conflict point "1-2" and the time information of the portion of the second planned trajectory after the conflict point "1-2". The amount of time delayed is the time point at which the other party of the conflict, the first planned trajectory, enters the conflict point "1-2" minus the time point at which the adjusted planned trajectory, i.e., the second planned trajectory, exits the conflict point "1-2" to be removed plus the predetermined time interval threshold, which is 0.3-0.275+0=0.025 in this example.

[0209] Thus, the times at which the second planned trajectory enters and exits each intersection point will change as follows:

[0210]

[0211] Figure 9 The intersection points and conflict points after the conflict point "1-2" is removed are schematically shown. As shown in Figure 9 the time information of the second planned trajectory at the conflict point "1-2" and after will be delayed in its entirety, and the total time of the second planned trajectory will also be correspondingly lengthened. The other trajectories remain unchanged.

[0212] Then, the third planned trajectory is adjusted to remove the conflict point "1-3" between the third planned trajectory and the first planned trajectory.

[0213] Here, the time information of the third planned trajectory at the conflict point "1-3" and the time information of the portion of the third planned trajectory after the conflict point "1-3" are both equally delayed by 0.67-0.58+0=0.09. The times at which the third planned trajectory enters and exits each intersection point will be updated as follows:

[0214]

[0215] After the conflict points "1-2" and "1-3" of the fixed first planned trajectory are removed, the conflict identification step S22 is performed again to re-determine the conflict points between the planned trajectories. As shown in the following matrix:

[0216]

[0217]

[0218]

[0219]

[0220]

[0221] It can be seen that there are three conflict points between the above five planning trajectories: "3-5", "4-5" and "3-5". The planning trajectories with conflict points are sorted in descending order of the priority of the corresponding task as: 3>4>5. Then, the third planning trajectory is fixed. The fifth planning trajectory with conflict points with the third planning trajectory is adjusted to eliminate the conflict points between the third planning trajectory and the fifth planning trajectory. Here, there are two conflict points between the third planning trajectory and the fifth planning trajectory, and the first conflict point between the third planning trajectory and the fifth planning trajectory is eliminated in time sequence.

[0222] After eliminating the first conflict point between the third planning trajectory and the fifth planning trajectory, the time information of the third trajectory is updated:

[0223]

[0224] It can be seen that while the first conflict point between the third planning trajectory and the fifth planning trajectory is eliminated, the second conflict point between the third planning trajectory and the fifth planning trajectory is also eliminated because the time information of the part of the fifth planning trajectory after the conflict point is updated accordingly.

[0225] Here, the conflict identification step S22 is performed, and the identification result is shown in the following matrix:

[0226]

[0227]

[0228]

[0229]

[0230]

[0231] There is still one conflict point between the above five planning trajectories: "4-5". The planning trajectories with conflict points are sorted in descending order of the priority of the corresponding task as: 4>5. Then, the fourth planning trajectory is fixed. The fifth planning trajectory with conflict points with the fourth planning trajectory is adjusted to eliminate the conflict points between the fourth planning trajectory and the fifth planning trajectory.

[0232] After eliminating the conflict points of the fourth planning trajectory, the conflict identification step S22 is performed again. The time information of the intersection points between the planning trajectories is as follows:

[0233]

[0234]

[0235]

[0236]

[0237]

[0238] At this point, there are no longer any conflict points between the planned trajectories.

[0239] In another exemplary embodiment according to the present invention, conflict points to be resolved and planned trajectories to be adjusted are selected in order of the number of conflict points of the planned trajectories from least to most, so that planned trajectories with earlier order are fixed first so that they do not need to be adjusted.

[0240] Specifically, the conflict identification step S22 and the conflict resolution step S23 are performed as follows: first, all conflict points between the multiple planned trajectories are identified; the planned trajectories with conflict points are sorted in order from least to most according to the number of their conflict points; the planned trajectory with the highest order is selected as the fixed planned trajectory, and the conflict points of the fixed planned trajectory are determined one by one as the conflict points to be adjusted, and accordingly, the planned trajectory that conflicts with the fixed planned trajectory at the conflict point to be adjusted is determined as the adjusted planned trajectory to resolve all conflict points of the fixed planned trajectory; then, the conflict identification step S22 is performed again to re-identify all conflict points between the multiple planned trajectories. When there are multiple planned trajectories with the same and least number of conflict points, the adjusted planned trajectory can be selected based on the time sequence of entering the conflict point.

[0241] by Figure 8 For example, the five planned trajectories shown have five conflict points: "1-2," "3-5," "1-3," "3-4," and "4-5." These five planned trajectories all have conflict points. They are sorted from least to most: 2 < 1 = 4 = 5 < 3. First, fix the second planned trajectory. This fixed trajectory has only one conflict point with the first planned trajectory. Therefore, adjust the first planned trajectory to resolve the conflict point "1-2" between the first and second planned trajectories. The delay is 0.325 - 0.25 + 0 = 0.075.

[0242] After executing the first conflict resolution step S23, the time information of the intersection points between the planned trajectories is as follows:

[0243]

[0244]

[0245]

[0246]

[0247]

[0248] Again, the conflict identification step S22 is performed, and it is identified that there are 4 conflict points between the planning trajectories: "1-5", "3-4", "3-5", and "4-5". The 1st, 3rd, 4th, and 5th planning trajectories with conflict points are sorted in order of the number of conflict points from few to many: 1<3=4<5. The 1st planning trajectory is fixed first. Then, the 5th planning trajectory is adjusted to eliminate the conflict point "1-5" between the 5th planning trajectory and the 1st planning trajectory. The amount of time delayed is 0.65-0.625+0=0.025.

[0249] After the second conflict elimination step S23 is performed, the time information of the intersection points between the planning trajectories is as follows:

[0250]

[0251]

[0252]

[0253]

[0254]

[0255] Again, the conflict identification step S22 is performed, and it is identified that there are 3 conflict points between the planning trajectories: "3-4", "3-5", and "4-5". The 3rd, 4th, and 5th planning trajectories with conflict points are sorted in order of the number of conflict points from few to many: 3=4=5. At this time, there are 3 planning trajectories with the same number of conflict points and the least number of conflict points, and the planning trajectories to be adjusted can be selected according to the time order of entering the conflict points. For example, the planning trajectories with the same number of conflict points and the least number of conflict points are sorted according to the time order of entering the conflict points: 3<5<4. Therefore, the 3rd planning trajectory is fixed first. Then, the conflict points "3-4" and "3-5" of the 3rd planning trajectory are adjusted one by one.

[0256] First, the 5th planning trajectory is adjusted to eliminate the conflict point "3-5" between the 5th planning trajectory and the 3rd planning trajectory. The amount of time delayed is 0.0425.

[0257] The time information of the 5th trajectory is updated, and it is obtained that:

[0258]

[0259] Then, the 4thplanned trajectory is adjusted to eliminate the conflict point "3-4" between the 4thplanned trajectory and the 3rdplanned trajectory. The amount of time delayed is 0.01.

[0260] After the conflict point of the 3rdplanned trajectory is eliminated, the time information of the intersection points between the planned trajectories is as follows:

[0261]

[0262]

[0263]

[0264]

[0265]

[0266] The conflict identification step S22 is performed again, and it is identified that there is one conflict point "4-5" between the planned trajectories. The 4thplanned trajectory and the 5thplanned trajectory, which have the same number of conflict points and the least number of conflict points according to the time order of entering the conflict points, are sorted: 4<5. Therefore, the 4thplanned trajectory can be fixed first. Then, the 5thplanned trajectory is adjusted to eliminate the conflict point "4-5" between the 5thplanned trajectory and the 4thplanned trajectory. The amount of time delayed is 0.0425.

[0267] After the conflict point of the 4thplanned trajectory is eliminated, the time information of the intersection points between the planned trajectories is as follows:

[0268]

[0269]

[0270]

[0271]

[0272]

[0273] At this time, the planned trajectories no longer have conflict points.

[0274] In still another exemplary embodiment according to the present application, the conflict point to be eliminated and the planned trajectory to be adjusted are selected in order of the conflict duration of the conflict points that the planned trajectories have from the least to the most, so that the planned trajectory sorted earlier is preferentially fixed without being adjusted.

[0275] Specifically, the conflict identification step S22 and the conflict resolution step S23 are performed in the following manner: first, all the conflict points between the multiple planning trajectories are identified; the planning trajectories with conflict points are sorted in order of the conflict duration of the conflict points from less to more; the planning trajectory at the top of the sorting is selected as a fixed planning trajectory, and the conflict points of the fixed planning trajectory are determined as to-be-adjusted conflict points one by one, and the planning trajectories that conflict with the fixed planning trajectory at the to-be-adjusted conflict points are determined as adjusted planning trajectories, so as to resolve all the conflict points of the fixed planning trajectory; then, the conflict identification step S22 is performed again to re-identify all the conflict points between the multiple planning trajectories. When there are multiple planning trajectories with the same number of conflict points and the least number, the adjusted planning trajectory can be selected according to the time sequence of entering the conflict points.

[0276] With Figure 8 For example, there are five conflict points between the five planning trajectories shown in Table 1: “1-2”, “3-5”, “1-3”, “3-4” and “4-5”. The five planning trajectories all have conflict points, and they are sorted in order of the conflict duration of the conflict points from less to more: 2<1<3<4<5. The second planning trajectory is fixed first. The fixed second planning trajectory only has one conflict point with the first planning trajectory. Therefore, the first planning trajectory is adjusted to resolve the conflict point “1-2” between the first planning trajectory and the second planning trajectory. The amount of time delay is 0.325-0.25+0=0.075.

[0277] After performing the first conflict resolution step S23, the time information of the intersection points between the planning trajectories is as follows:

[0278]

[0279]

[0280]

[0281]

[0282]

[0283] The conflict identification step S22 is performed again, and it is identified that there are four conflict points between the planning trajectories: "1-5", "3-4", "3-5", and "4-5". Among the first, third, fourth, and fifth planning trajectories having the conflict points, the conflict duration of the conflict point of the first planning trajectory is the shortest. The first planning trajectory is fixed first. Then, the fifth planning trajectory is adjusted to eliminate the conflict point "1-5" between the fifth planning trajectory and the first planning trajectory. The amount of time delayed is 0.65-0.625+0=0.025.

[0284] After the second conflict elimination step S23 is performed, the time information of the intersection points between the planning trajectories is as follows:

[0285]

[0286]

[0287]

[0288]

[0289]

[0290] The conflict identification step S22 is performed again, and it is identified that there are three conflict points between the planning trajectories: "3-4", "3-5", and "4-5". The third, fourth, and fifth planning trajectories having the conflict points are ordered in the order of the conflict duration of the conflict points from short to long: 3<4<5. Therefore, the third planning trajectory can be fixed first. Then, the conflict points "3-4" and "3-5" are eliminated one by one.

[0291] For example, the fourth planning trajectory can be adjusted first to eliminate the conflict point "3-4" between the fourth planning trajectory and the third planning trajectory. The amount of time delayed is 0.01.

[0292] Then, the fifth planning trajectory is adjusted to eliminate the conflict point "3-5" between the fifth planning trajectory and the third planning trajectory. The amount of time delayed is 0.0425.

[0293] After the conflict points of the third planning trajectory are eliminated, the time information of the intersection points between the planning trajectories is as follows:

[0294]

[0295]

[0296]

[0297]

[0298]

[0299] At this time, there is a conflict point between each of the planned trajectories: "4-5". The 4th and 5th planned trajectories are sorted according to the time order of entering the conflict point: 4 < 5, since the number of conflict points is the same and is the least. Therefore, the 4th planned trajectory can be fixed first. Then, the 5th planned trajectory is adjusted to remove the conflict point "4-5" between the 5th planned trajectory and the 4th planned trajectory. The amount of time delayed is 0.0425.

[0300] After that, the time information of the intersection points between each of the planned trajectories is as follows:

[0301]

[0302]

[0303]

[0304]

[0305]

[0306] At this time, there is no longer a conflict point between each of the planned trajectories.

[0307] In another exemplary embodiment according to the present application, the conflict point to be removed and the planned trajectory to be adjusted are selected according to the time order of occurrence of the conflict point.

[0308] Specifically, the conflict identifying step S22 and the conflict removing step S23 are performed in the following manner: first, the conflict identifying step S22 is performed to identify all the conflict points between the planned trajectories; the conflict points are sorted according to the time order of occurrence of the conflict points, the conflict point at the head of the sorted list is selected as the conflict point to be removed, and the planned trajectory entering the conflict point later among the two planned trajectories associated with the conflict point to be removed is selected as the planned trajectory to be adjusted to remove the conflict point; and then the conflict identifying step S22 is performed again to identify all the conflict points between the planned trajectories.

[0309] In this way, Figure 8The five planning trajectories shown are taken as an example, and there are five conflict points among the planning trajectories: "1-2", "3-5", "1-3", "3-4" and "4-5". The conflict points are sorted in the order of their occurrence time as: "1-2", "3-5", "1-3", "3-4" and "4-5". Thus, "1-2" is determined as the conflict point to be resolved. In the first planning trajectory and the second planning trajectory participating in the conflict point "1-2", the second planning trajectory enters the conflict point "1-2" later, and thus the second planning trajectory is determined as the planning trajectory to be adjusted. Then, the second planning trajectory is adjusted to resolve the conflict point "1-2" between the second planning trajectory and the first planning trajectory. The amount of time delay is 0.3-0.275+0=0.025.

[0310] After performing the first conflict resolution step S23, the time information of the intersection points among the planning trajectories is as follows:

[0311]

[0312]

[0313]

[0314]

[0315]

[0316] The conflict identification step S22 is performed again, and it is identified that there are four conflict points among the planning trajectories: "1-3", "3-4", "3-5" and "4-5". The conflict points are sorted in the order of their occurrence time as: "3-5", "1-3", "3-4" and "4-5". Thus, the fifth planning trajectory is adjusted to resolve the conflict point "3-5" between the fifth planning trajectory and the third planning trajectory. The amount of time delay is 0.0425.

[0317] After performing the second conflict resolution step S23, the time information of the intersection points among the planning trajectories is as follows:

[0318]

[0319]

[0320]

[0321]

[0322]

[0323] Then, the conflict identification step S22 and the conflict resolution step S23 are repeatedly performed as described above until no conflict point exists between any two of the five planned trajectories.

[0324] Alternatively, the conflict identification step S22 and the conflict resolution step S23 may be performed in other ways. For example, after identifying all conflict points between the multiple planned trajectories, the planned trajectories with conflict points are sorted in at least one of the following ways: by the priority of their corresponding tasks from high to low; by the number of conflict points from small to large; by the duration of the conflict points from small to large; or by the time they entered the conflict point. Then, the top-ranked planned trajectory with a conflict point is selected as the adjusted planned trajectory, and the time information of the adjusted planned trajectory is adjusted to resolve the conflict point between the adjusted planned trajectory and the top-ranked planned trajectory.

[0325] In addition, the present invention also relates to a computer program product comprising computer program instructions. When the computer program instructions are executed by one or more processors, the processors are capable of executing the trajectory planning method and / or the multi-robot trajectory planning method according to the present invention.

[0326] In the present invention, a computer program product may be stored in a computer-readable storage medium. The computer-readable storage medium may include, for example, high-speed random access memory, and may also include non-volatile memory, such as a hard disk, internal memory, a plug-in hard disk, a smart memory card, a secure digital card, a flash memory card, at least one magnetic disk storage device, a flash memory device, or other volatile solid-state memory device. The processor may be a central processing unit, or may also be other general-purpose processors, digital signal processors, application-specific integrated circuits, off-the-shelf programmable gate arrays or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or any conventional processor, etc.

[0327] Although specific embodiments of the present invention are described in detail herein, they are provided for illustrative purposes only and should not be considered to limit the scope of the present invention. Various substitutions, changes, and modifications may be conceived without departing from the spirit and scope of the present invention.

Claims

1. A multi-robot trajectory planning method, wherein: The multi-robot trajectory planning method comprises: A preliminary planning step S21, wherein a plurality of planned trajectories containing time information corresponding to the plurality of mobile robots (1) are obtained, wherein the plurality of planned trajectories are planned trajectories generated by performing a time-optimal trajectory planning method on the plurality of mobile robots (1); a conflict identification step S22, wherein a conflict point in space and time dimensions is identified between two planned trajectories among the plurality of planned trajectories, the conflict point indicating that the mobile robot (1) moving along the two planned trajectories will arrive at the same position at the same time; and Conflict resolution step S23, wherein the conflict is resolved by adjusting the time information of one of the two planned trajectories. The time-optimal trajectory planning method is capable of performing velocity planning on a mobile robot (1) according to a determined path (2) to determine a planned trajectory containing time information that enables the mobile robot (1) to move along the path (2). The trajectory planning method comprises: determining that one of at least two driving wheels of the mobile robot (1) is a constrained wheel, so that as long as the constrained wheel satisfies kinematic and dynamic constraints, the other driving wheels that move in coordination with the constrained wheel will satisfy the kinematic and dynamic constraints; The path (2) is segmented and velocity planning is performed, wherein the constrained wheels are: in the segment, according to the path shape of the segment and the motion state of each driving wheel at the first control point, the driving wheels that preferentially reach the limit value of the kinematic or dynamic constraint, The constrained wheels in each segment are determined as follows: Get the first initial velocity v of the first driving wheel and the second driving wheel at the first control point L0 and the second initial velocity v R0 ; Determine a value k1 of a speed ratio k determined by the path (2) at a second control point serving as an end point of the segment, wherein the speed ratio k represents a ratio of the speed of the second driving wheel to the speed of the first driving wheel; Determine the first maximum speed v of the first driving wheel and the second driving wheel at the second control point respectively Lmax and the second maximum speed v Rmax , the first maximum speed and the second maximum speed respectively represent the maximum speeds that satisfy the kinematic and dynamic constraints of each driving wheel and the constraints of the path (2) without considering the speeds of the first driving wheel and the second driving wheel before reaching the second control point; The first driving wheel is moved from the first initial velocity v at the first control point L0 The speed obtained by accelerating to the second control point with the limit wheel acceleration of the first driving wheel is determined as the first acceleration terminal speed v La , change the second driving wheel from the second initial velocity v at the first control point R0 The speed obtained by accelerating to the second control point with the limit wheel acceleration of the second driving wheel is determined as the second acceleration terminal speed v Ra ; The first maximum velocity v at the second control point Lmax With the first acceleration final velocity v La The smaller of the two is determined as the first terminal velocity v L , the second maximum velocity v at the second control point Rmax With the second acceleration final velocity v Ra The smaller of the two is determined as the second terminal velocity v R ; The second terminal velocity v R With the first terminal velocity v L The ratio is compared with the speed ratio k1 at the second control point, and the constrained wheel in the section is determined based on the comparison result.

2. The multi-robot trajectory planning method according to claim 1, wherein: The conflict identification step S22 and the conflict resolution step S23 are repeatedly performed until no conflict point exists between any two planned trajectories among the plurality of planned trajectories.

3. The multi-robot trajectory planning method according to claim 1 or 2, wherein: The conflict identification step S22 includes: Find all intersection points of each two planned trajectories in the plurality of planned trajectories in the spatial dimension; and For each intersection point, the time interval between the time information of the relevant planned trajectories at the intersection point is checked. If the time interval is less than a predetermined time interval threshold, the corresponding intersection point is identified as a conflict point.

4. The multi-robot trajectory planning method according to claim 3, wherein: The conflict resolution step S23 includes: Sub-step S231: selecting a conflict point to be resolved and an adjusted planned trajectory from the identified conflict points and conflicting planned trajectories, wherein the adjusted planned trajectory is one of the two planned trajectories associated with the conflict point to be resolved or the conflict point to be resolved is one of the conflict points with the adjusted planned trajectory, Sub-step S232: Adjusting the time information of the adjusted planned trajectory at the conflict point to be resolved by delaying the time information of the adjusted planned trajectory at the conflict point, so that the time interval between the time information of the two associated planned trajectories at the conflict point is greater than or equal to the time interval threshold, Sub-step S233: Based on the adjusted time information of the adjusted planned trajectory at the conflict point, the time information of the portion of the adjusted planned trajectory after the conflict point is updated accordingly.

5. The multi-robot trajectory planning method according to claim 4, wherein: Sub-steps S232 and S233 are performed in the following manner: the time information of the adjusted planned trajectory at the conflict point and the time information of the portion of the adjusted planned trajectory after the conflict point are delayed by equal amounts; and / or The amount of time for the delay is the time point at which another planned trajectory associated with the conflict point to be resolved enters the conflict point to be resolved minus the time point at which the adjusted planned trajectory leaves the conflict point to be resolved plus a predetermined time interval threshold.

6. The multi-robot trajectory planning method according to claim 4 or 5, wherein: The conflict identification step S22 and the conflict resolution step S23 are performed in the following manner: executing a conflict identification step S22 to identify all conflict points between the plurality of planned trajectories; The planned trajectories with conflict points are sorted in at least one of the following ways: In order of priority of their corresponding tasks from high to low; In order from the least to the most number of conflict points; In order of the duration of the conflict at their conflict points from least to most; According to the order of their entry into the conflict point; Selecting the first-ranked planned trajectory as the fixed planned trajectory, determining the conflict points of the fixed planned trajectory one by one as the conflict points to be adjusted, and correspondingly determining the planned trajectory that conflicts with the fixed planned trajectory at the conflict points to be adjusted as the adjusted planned trajectory, so as to resolve all the conflict points of the fixed planned trajectory; The conflict identification step S22 is then performed to re-identify all conflict points between the plurality of planned trajectories.

7. The multi-robot trajectory planning method according to claim 4 or 5, wherein: The conflict identification step S22 and the conflict resolution step S23 are performed in the following manner: executing a conflict identification step S22 to identify all conflict points between the plurality of planned trajectories; Sort the conflict points in the order of their occurrence time. Selecting the conflict point with the highest order as the conflict point to be resolved, and selecting the planned trajectory that enters the conflict point later than the two planned trajectories associated with the conflict point to be resolved as the adjusted planned trajectory to resolve the conflict point to be resolved; The conflict identification step S22 is then performed to re-identify all conflict points between the plurality of planned trajectories.

8. The multi-robot trajectory planning method according to any one of claims 1-2, 4-5, wherein: The trajectory planning method comprises: Performing velocity planning on the constrained wheel based on the path (2) while satisfying the kinematic and dynamic constraints of the constrained wheel, so as to determine the velocity of the constrained wheel so that the constrained wheel has one of a maximum velocity and a maximum acceleration at any point while satisfying its kinematic and dynamic constraints and satisfying the constraints of the path (2); The speeds of the driving wheels other than the constrained wheels are planned in a manner that matches the determined speeds of the constrained wheels.

9. The multi-robot trajectory planning method according to claim 8, wherein: In the process of speed planning for the constrained wheel, the T-shaped planning method is adopted.

10. The multi-robot trajectory planning method according to claim 8, wherein: Kinematic and dynamic constraints include: The magnitude of the speed of the driving wheel is below a predetermined limit wheel speed for the driving wheel; The magnitude of the acceleration of the drive wheels is less than a limit wheel acceleration predetermined for the drive wheels.

11. The multi-robot trajectory planning method according to claim 10, wherein: The following steps are respectively performed on at least one section of the path (2): For a first control point serving as a starting point of the segment, determining, based on a path shape of the segment, a motion state of each driving wheel at the first control point, and kinematic and dynamic constraints of the driving wheels, one of the at least two driving wheels as a constrained wheel in the segment; performing speed planning on the constrained wheel to determine a speed of the constrained wheel within the segment; The speed of the other driven wheels within the segment is determined in a manner coordinated with the determined speed of the constrained wheel.

12. The multi-robot trajectory planning method according to claim 11, wherein: The mobile robot (1) is a dual-differential wheel robot, wherein the at least two driving wheels are a first driving wheel and a second driving wheel which are symmetrically arranged, wherein the first driving wheel and the second driving wheel are subject to the same kinematic and dynamic constraints.

13. The multi-robot trajectory planning method according to any one of claims 1-2, 4-5, and 9-12, wherein: If the second terminal velocity v R With the first terminal velocity v L If the ratio of the speeds is greater than the speed ratio k1 at the second control point, the first driving wheel is determined to be the constrained wheel in the section; If the second terminal velocity v R With the first terminal velocity v L If the ratio of the speeds is less than the speed ratio k1 at the second control point, the second driving wheel is determined to be the constrained wheel in the section; If the second terminal velocity v R With the first terminal velocity v L If the ratio of the speed ratios φ and φ is equal to the speed ratio k1 at the second control point, then one of the first driving wheel and the second driving wheel is determined to be the constrained wheel in the section.

14. The multi-robot trajectory planning method according to any one of claims 1-2, 4-5, and 9-12, wherein: The motion duration of each segment is equal to the predetermined control period t, and the first acceleration terminal velocity v La and the second acceleration terminal velocity v Ra Determine according to the following formula: v La =v L0 +a*t v Ra =v R0 +a*t Wherein, a represents the limit wheel acceleration of the first driving wheel and the second driving wheel.

15. The multi-robot trajectory planning method according to any one of claims 1-2, 4-5, and 9-12, wherein: The first maximum speed v of the first driving wheel and the second driving wheel at any point on the path (2) Lmax and the second maximum speed v Rmax Determined according to at least one of the following constraints: -Based on the limit wheel speed v lim The first constraint: v Lma ≤v lim , -Based on the limit wheel speed v lim and the second constraint on the speed ratio determined by path (2): v Lma ≤v lim / k, - The third constraint based on the limit wheel acceleration a and the speed ratio change rate k' determined by path (2): Among them, k ′ ≠0; and The first maximum speed v of the first driving wheel and the second driving wheel at any point on the path (2) Lmax and the second maximum speed v Rmax Satisfaction: v Rmax =v Lma *k.

16. The multi-robot trajectory planning method according to claim 15, wherein: The first maximum speed v of the first driving wheel and the second driving wheel at any point on the path (2) Lmax and the second maximum speed v Rmax Additionally, it is determined according to the following fourth constraint: Determine the movement distance L of the first driving wheel obtained by assuming that the first driving wheel moves at the maximum speed determined by at least one of the first constraint, the second constraint and the third constraint along the path (2). L The first initial maximum speed changes and the movement distance L of the second driving wheel R at least one of the second preliminary maximum speeds of the variation; determining a maximum point and a minimum point of the at least one of the first preliminary maximum velocity and the second preliminary maximum velocity; The first maximum velocity v at any point Lmax and / or a second preliminary maximum speed v Rmax satisfy: If the arbitrary point is behind the minimum point closest to the arbitrary point, then: and / or If the arbitrary point is in front of the minimum point closest to the arbitrary point, then: and / or Among them, L L and L R represents the movement distance of the first driving wheel and the second driving wheel to the arbitrary point, v1 represents the first preliminary maximum speed or the second preliminary maximum speed of the minimum point closest to the arbitrary point, L L1 and L R1 They respectively represent the movement distances of the first driving wheel and the second driving wheel to the nearest minimum point.

17. The multi-robot trajectory planning method according to any one of claims 1-2, 4-5, 9-12, and 16, wherein: The path (2) is a global path determined by performing global path planning based on at least one mission point of the mobile robot (1), the at least one mission point being located on the global path; and / or The path (2) is in the form of a Bezier curve of order 3 or higher.

18. A computer program product comprising computer program instructions, wherein: When the computer program instructions are executed by one or more processors, the processors are capable of performing the multi-robot trajectory planning method according to any one of claims 1-17.

Images