Trajectory planning method for mobile robot and computer program product

Abstract

A trajectory planning method for a mobile robot is proposed, which plans a speed of the mobile robot according to a determined path to determine a planning trajectory containing time information, the trajectory planning method comprising: 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 moving in coordination with the constrained wheel will satisfy the kinematic and dynamic constraints; planning a speed of the constrained wheel based on the path under the condition that the kinematic and dynamic constraints of the constrained wheel are satisfied to determine the speed of the constrained wheel; planning a speed of the other drive wheels in a manner of coordinating with the determined speed of the constrained wheel. A corresponding computer program product is proposed. Through the present application, an alternative trajectory planning method for a mobile robot is provided, which can reliably plan a trajectory satisfying kinematic and dynamic constraints for the mobile robot.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of mobile robots, in particular to the field of motion control of mobile robots, and specifically to a trajectory planning method for a mobile robot and a computer program product. BACKGROUND

[0002] With the rapid economic growth and the gradual increase of labor costs, mobile robots are increasingly widely used in various industrial and home environments. For example, mobile robots such as automatic guided vehicles (AGV), autonomous mobile robots (AMR), and forklifts are one of the key equipment in modern logistics systems. Mobile robots can move and stop at target locations according to planned paths and job requirements to complete tasks such as material handling and transportation. Trajectory planning is a key in the motion control of mobile robots.

[0003] In the process of trajectory planning, speed planning needs to be performed for the mobile robot based on the determined path. In order to determine the speed distribution of the mobile robot on the path and obtain the optimal motion trajectory as much as possible, such as the motion trajectory with the shortest time, it is usually necessary to solve a system of differential equations. For example, in existing algorithms, when planning a motion trajectory based on a path, a constraint optimization problem with time as the optimization objective can be defined, while considering the speed constraint and acceleration constraint of the mobile robot. For a mobile robot with multiple drive wheels, the motion coordination between the drive wheels also needs to be considered. This requires not only a large amount of memory, but also a considerable amount of calculation.

[0004] The prior art still has many deficiencies in the path planning of mobile robots. SUMMARY

[0005] The object of the present application is to provide an improved trajectory planning method for a mobile robot and a corresponding computer program product in order to overcome at least one deficiency of the prior art.

[0006] According to a first aspect of the present application, a trajectory planning method for a mobile robot is provided, wherein a speed planning is performed for the mobile robot based on a determined path to determine a planning trajectory containing time information that enables the mobile robot to move along the path, the trajectory planning method comprising: determining one of at least two drive wheels of the mobile robot 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; performing a speed planning for the constrained wheel based on the path under the condition that the kinematic and dynamic constraints of the constrained wheel are satisfied to determine a speed of the constrained wheel; performing a speed planning for the other drive wheels other than the constrained wheel in a manner that coordinates with the determined speed of the constrained wheel.

[0007] "Time information" represents information capable of characterizing the relationship between the position of the mobile robot on the path and time. Since the path is determined, the "time information" can also characterize the speed of the mobile robot at each position on the path.

[0008] In one example embodiment, in the process of velocity planning for the constrained wheel, the velocity of the constrained wheel is determined to be one of the maximum velocity and the maximum acceleration of the constrained wheel at any point under the condition that the kinematic and dynamic constraints thereof are satisfied and the limits of the path are satisfied.

[0009] In one example embodiment, in the process of velocity planning for the constrained wheel, a T-shaped planning method is adopted.

[0010] In one example embodiment, the kinematic and dynamic constraints 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.

[0011] In one example embodiment, the path is segmented for velocity planning, and the following steps are performed for at least one segment of the path respectively: for a first control point being the starting point of the segment, determining one of the at least two drive wheels as a 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 or dynamic constraint 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; performing velocity planning for the constrained wheel to determine the velocity of the constrained wheel within the segment; determining the velocities of the other drive wheels within the segment in coordination with the determined velocity of the constrained wheel.

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

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

[0014] obtaining first initial velocities v L0 and second initial velocities v R0 of the first drive wheel and the second drive wheel at the first control point;

[0015] determining the value k1 of the speed ratio k determined by the path at a second control point being the end point of the segment, the speed ratio k representing the ratio of the velocity of the second drive wheel to the velocity of the first drive wheel.

[0016] Determine the first maximum speed v of the first drive wheel and the second drive wheel at the second control point respectively. Lmax Second maximum speed v Rmax The first maximum speed and the second maximum speed represent the maximum speeds that satisfy the kinematic and dynamic constraints of each drive wheel and the path constraints, respectively, without considering the speeds of the first and second drive wheels before reaching the second control point.

[0017] The first drive wheel starts from the first initial velocity v at the first control point. L0 The speed obtained by accelerating from the first drive wheel to the second control point using the maximum wheel acceleration is defined as the first final acceleration speed v. La The second drive wheel is moved from the second initial velocity v at the first control point. R0 The speed obtained by accelerating from the second drive wheel to the second control point using the ultimate wheel acceleration is defined as the second final acceleration speed v. Ra ;

[0018] The first maximum speed v at the second control point Lmax With the final velocity v of the first acceleration La The smaller of the two values ​​is determined as the first final velocity v. L The second maximum speed v at the second control point Rmax With the second acceleration final velocity v Ra The smaller of the two values ​​is determined as the second final velocity v. R ;as well as

[0019] The second final velocity v R With the first final 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.

[0020] In one exemplary embodiment, if the second final velocity v R With the first final velocity v L If the ratio is greater than the speed ratio k1 at the second control point, then the first drive wheel is determined to be the constrained wheel in the segment; if the second final speed v R With the first final velocity v L If the ratio is less than the speed ratio k1 at the second control point, then the second drive wheel is determined to be the constrained wheel in the segment; if the second final speed v R With the first final velocity v L If the ratio is equal to the speed ratio k1 at the second control point, then one of the first drive wheel and the second drive wheel is determined to be the constrained wheel in the segment.

[0021] In one exemplary embodiment, the motion duration of each segment corresponding to the predetermined control period t, the first acceleration final speed v La and the second acceleration final speed v Ra are determined according to the following formulae:

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

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

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

[0025] In one exemplary embodiment, 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 are determined according to at least one of the following constraints:

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

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

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

[0029] 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 satisfy: v Rmax = v Lmax *k.

[0030] In one exemplary embodiment, 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 are determined additionally according to a fourth constraint:

[0031] The determination assumes that the first drive wheel moves along the path at the maximum speed determined by the at least one of the first, second and third constraints, resulting in a first preliminary maximum speed that varies with the distance L L moved by the first drive wheel and a second preliminary maximum speed that varies with the distance L RAt least one of the second initial maximum rates of change;

[0032] Determine the maximum and minimum points of at least one of the first and second preliminary maximum velocities;

[0033] The first maximum velocity v at any point Lmax and / or the second initial maximum velocity v Rmax satisfy:

[0034] If any point is located behind the nearest minimum point, then:

[0035] and / or

[0036] If any point is in front of the nearest minimum point, then:

[0037] and / or

[0038] Among them, L L and L R Let v1 and v2 represent the distances traveled by the first and second drive wheels to the arbitrary point, respectively. v1 correspondingly represents the first or second initial maximum velocity at the nearest minimum point to the arbitrary point. L1 and L R1 These represent the distances traveled by the first drive wheel and the second drive wheel to the nearest minimum point, respectively.

[0039] In one exemplary embodiment, the path is a global path determined by global path planning based on at least one task point of the mobile robot, the at least one task point being located on the global path; and / or the path is in the form of a Bézier curve of order 3 or higher.

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

[0041] The positive effect of this invention is that it provides an alternative trajectory planning method, which is particularly capable of reliably planning trajectories that satisfy kinematic and dynamic constraints for mobile robots. Attached Figure Description

[0042] The invention will now be described in more detail with reference to the accompanying drawings, which will provide a better understanding of its principles, features, and advantages. The drawings include:

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

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

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

[0046] Figure 4A A variation curve of a radius of curvature and a curvature on a path in an example embodiment of the present application is schematically shown;

[0047] Figure 4B A variation curve of a speed ratio and a first maximum speed and a second maximum speed satisfying the first constraint and the second constraint on a path in an example embodiment of the present application is schematically shown;

[0048] Figure 4C and Figure 4D A motion speed and a motion distance of the mobile robot and a first required acceleration and a second required acceleration of the first drive wheel and the second drive wheel and a time stamp of the motion of the mobile robot along the path corresponding to the first maximum speed and the second maximum speed shown in Figure 4B

[0049] Figure 4E A first required acceleration and a second required acceleration of the first drive wheel and the second drive wheel and a time stamp of the motion of the mobile robot along the path after the re-planning are schematically shown;

[0050] Figure 4F A speed ratio k and a variation curve of the speed of the first drive wheel and the second drive wheel after the re-planning are schematically shown;

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

[0052] Figure 5B A variation curve of a curvature on a path in the example embodiment shown in Figure 5A

[0053] A variation curve of a speed ratio on a path in the example embodiment shown in Figure 5C Figure 5A A variation curve of a speed ratio on a path in the example embodiment shown in

[0054] Figure 5D A variation curve of a​​Figure 5A the first maximum speed and the second maximum speed in the example embodiment shown satisfy the first constraint;

[0055] Figure 5E schematically shows the intersection points and the conflict points in one example embodiment of the present application; Figure 5A the first maximum speed and the second maximum speed in the example embodiment shown satisfy the first constraint and the second constraint;

[0056] Figure 5F schematically shows the intersection points and the conflict points in one example embodiment of the present application; Figure 5A the first maximum speed and the second maximum speed in the example embodiment shown satisfy the first constraint, the second constraint and the third constraint;

[0057] Figures 5G-5H schematically shows the curve of the first preliminary maximum speed of the first drive wheel varying with the movement distance of the first drive wheel in one example embodiment;

[0058] Figure 5I schematically shows the intersection points and the conflict points in one example embodiment of the present application; Figure 5A the first maximum speed and the second maximum speed in the example embodiment shown satisfy the first constraint, the second constraint, the third constraint and the fourth constraint;

[0059] Figure 6 schematically shows the flow chart of the multi-robot trajectory planning method according to one example embodiment of the present application;

[0060] Figure 7 schematically shows the five paths respectively for the five mobile robots;

[0061] Figure 8 schematically shows the intersection points and the conflict points in one example embodiment of the present application; and

[0062] Figure 9 schematically shows the intersection points and the conflict points in one example embodiment of the present application; Figure 8 the intersection points and the conflict points after the conflict point “1-2” is resolved in the example embodiment shown. DETAILED DESCRIPTION

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

[0064] The present application is applicable to a mobile robot, which can be any robot capable of moving in space autonomously, such as an AGV, an AMR, etc. The mobile robot can be used to perform various tasks, such as serving as a warehouse robot, a cleaning robot, a home companion robot, a welcome robot, etc.

[0065] It should be understood that, in this document, the terms "first", "second", etc. are used only for descriptive purposes and should not be construed as indicating or implying relative importance, and should not be construed as implying a specific number of the technical features indicated.

[0066] The motion control method of the present application will be described below Figure 1 and Figure 2 The motion control method of the present application will be described below Figure 1 A mobile robot 1 and a path 2 thereof implementing a trajectory planning method according to one example 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 example embodiment of the present application is schematically shown.

[0067] 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 described as a left wheel as the first drive wheel) and a second drive wheel (hereinafter exemplarily described as a right wheel as the second drive wheel). Alternatively, the mobile robot 1 can also be other types of robots, such as a dual-steering wheel robot, etc. Accordingly, the mobile robot 1 may, for example, comprise a dual-steering wheel motion system.

[0068] In the trajectory planning method, a speed planning is performed on the mobile robot 1 according to the determined path 2 to determine a planning trajectory containing time information that enables the mobile robot 1 to move along the path 2. The path 2 can be a global path determined by performing a 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.

[0069] The path 2 can be in the form of a 3-order or higher Bezier curve and can be expressed in the following formula:

[0070]

[0071] wherein, represents the position of the mobile robot 1, i = 0, 1, …, N, N ≥ 3, represents the coordinates of the control points of the Bezier curve. When s increases from 0 to 1, the corresponding The path 2 represents 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 drive robots. The path 2 having a continuous second derivative can particularly advantageously adapt to the motion characteristics of a differential drive 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 The path 2 in the form of a 4th order Bezier curve is shown in Fig. 2. It is to be understood, however, that the path 2 can also have other shapes.

[0072] As shown in Fig. 3, the trajectory planning method comprises the steps of: 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 the kinematic and dynamic constraints, the other drive wheels moving in coordination with the constrained wheel will satisfy the kinematic and dynamic constraints; S12, based on the path 2, 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; S13, velocity planning for the other drive wheels other than the constrained wheel in a manner coordinated with the determined velocity of the constrained wheel. Figure 2 Thereby, an alternative trajectory planning method is provided which can reliably plan a trajectory for the mobile robot 1 that satisfies the kinematic and dynamic constraints, in particular.

[0073] In the process of velocity planning for the constrained wheel, the constrained wheel is provided with 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. Thereby, a time-optimal trajectory can be planned for the mobile robot 1.

[0074] The kinematic and dynamic constraints can comprise that the magnitude of the velocity of the drive wheel is below a limit wheel velocity predetermined for the drive wheel, and the magnitude of the acceleration of the drive wheel is below a limit wheel acceleration predetermined for the drive wheel. The limit wheel velocity and the limit wheel acceleration are limited by the construction 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, for example, by the electric motor used to drive the respective drive wheel. Alternatively, the kinematic and dynamic constraints can also comprise that the jerk of the drive wheel is below a limit wheel jerk predetermined for the drive wheel. Alternatively, the first drive wheel and the second drive wheel of the mobile robot 1 can be arranged symmetrically, such that they have the same limit wheel velocity and limit wheel acceleration.

[0075]

[0076] ​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 greater 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 greater 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.

[0077] 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.

[0078] 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.

[0079] 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.

[0080] 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.

[0081] 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.

[0082] 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.

[0083] 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 drive 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 drive 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 drive 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:

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

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

[0086] Wherein, a represents the limit wheel acceleration of the first drive wheel and the second drive wheel.

[0087] 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 .

[0088] 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.

[0089] 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.

[0090] 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 drive 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 LIf the ratio is less than the speed ratio k1 at the second control point, then the second drive wheel is determined to be the constrained wheel in the segment; if the second final speed v R With the first final velocity v L If the ratio is equal to the speed ratio k1 at the second control point, then either the first drive wheel or the second drive wheel can be determined as the constrained wheel in the segment.

[0091] The following is combined with Figures 4A-4F Detailed description of determining the first maximum speed v of the first and second drive wheels at any point on path 2. Lmax Second maximum speed v Rmax The process.

[0092] Figure 4A The diagram schematically illustrates the variation curves of the radius of curvature R and curvature к on path 2 according to an exemplary embodiment of the present invention. In this exemplary embodiment, path 2 is a fourth-order Bézier curve, and the coordinates of its five Bézier curve control points are (0, -1), (0, 0), (1, 2), (4, 2), and (5, 3), respectively.

[0093] Based on path 2, the curvature к at any point on path 2 can be determined as follows:

[0094]

[0095] Among them, P x ′(s), P y ′(s), P x "(s), P y "(s) are respectively The first-order and second-order guided x and y coordinates are given. Accordingly, the radius of curvature R at any point on path 2 can be determined.

[0096] Therefore, the speed ratio k at any point on path 2 can be obtained. Figure 4B The bottom of the diagram schematically shows the curve of the change in velocity ratio k on path 2.

[0097] In this exemplary embodiment, based on the limiting wheel speed v lim The first constraint and based on the limiting wheel speed v lim The first maximum speed v of the first and second drive wheels at any point on path 2 is determined by a second constraint on the speed ratio determined by path 2. Lmax Second maximum speed v Rmax .

[0098] The first constraint states that the speed of the drive wheel cannot exceed its limit wheel speed. Therefore, the first maximum speed v of the first drive wheel... 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).

[0099] 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.

[0100] 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.

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

[0102]

[0103] It can be derived that v Lmax = min(v lim , v lim / k), v Rmax = 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

[0104] 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.

[0105] 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 movement speed v r and the movement distance L of the corresponding 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

[0106] In this embodiment, the limit wheel acceleration a of the first and second driving wheels is exemplarily preset 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 < s1. 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 4E . 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 The speed ratio k and the speed change curves of the first and second driving wheels after re-planning are shown. It can be seen from Figure 4F 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 .

[0107] 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 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 ​< thus, the second driving wheel (i.e. the right wheel) is the constraining wheel.

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

[0109] The process of determining the first maximum speed v Rmax and the second maximum speed v Lmax of the first driving wheel and the second driving wheel at an arbitrary point on the path 2 in an exemplary embodiment according to the present application will be described in detail below with reference to Rmax .

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

[0111] In this exemplary embodiment, similarly to the embodiment illustrated in Figures 4A-4F , the first maximum speed v Lmax and the second maximum speed v Rmax of the first driving wheel and the second driving wheel at an arbitrary point on the path 2 need to satisfy the first constraint and the second constraint.

[0112] Figure 5D The first maximum speed v Lmax and the second maximum speed v Rmax satisfying the first constraint in this exemplary embodiment are schematically illustrated. The first constraint indicates that the speed of the driving wheel cannot exceed its limit wheel speed. Thus, the first maximum speed v Lmax and the second maximum speed v lim need to satisfy the constraint: v Rmax ≤ v lim In this embodiment, the limit wheel speeds v lim of the first driving wheel and the second driving wheel are the same.lim It is preset to 1.2 (m / s).

[0113] Figure 5E The first maximum speed v, satisfying the first constraint and the second constraint, is schematically shown in this exemplary embodiment. Lmax Second maximum speed v Rmax The second constraint states that the speed of either the first or second drive wheel must be such that the other drive wheel, satisfying the speed ratio k, cannot exceed its limit wheel speed. Therefore, the first maximum speed v of the first drive wheel... Lmax Constraints must be met: v Lmax ≤v lim / k; the second maximum speed v of the second drive wheel Rmax Constraints must be met: v Rmax =v lim *k.

[0114] Therefore, the maximum wheel speed satisfying the first and second constraints can be obtained: v Lmax =min(v lim ,v lim / k),v Rmax =min(v lim ,v lim *k).

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

[0116] In this embodiment, the first maximum speed v of the first drive wheel and the second drive wheel at any point on path 2 Lmax Second maximum speed v Rmax A third constraint can be additionally satisfied based on the limiting wheel acceleration a and the rate of change of velocity ratio k' determined by path 2: Where k′≠0.

[0117] The principle of the third constraint is explained in detail below. As mentioned above, once path 2 is determined, the velocity ratio k and the rate of change of velocity ratio k' at any point on path 2 can be determined. Assume that at a certain point on path 2, the first and second drive wheels of the mobile robot 1 moving along path 2 have velocities v and v', respectively. L0 and v R0 The speed ratio k0 = v R0 / v L0 .

[0118] 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:

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

[0120]

[0121] where L denotes the movement distance of the mobile robot 1 and the small displacement dL denotes the displacement of the mobile robot 1 in the small time period t. The small displacement 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:

[0122]

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

[0124]

[0125] 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:

[0126] It should be understood that the speed ratio change rate k' = 0 means that the mobile robot 1 performs a straight movement or a circular movement at this point. In the case of straight 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.

[0127] 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.

[0128] 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.

[0129] The following description is made with reference to Figures 5G-5HTo explain the fourth constraint, firstly, assume that the first drive wheel moves along path 2 at the maximum speed determined by the first, second, and third constraints, and the distance L traveled with the first drive wheel is obtained. L The first initial maximum speed of the first drive wheel changes. Figures 5G-5H The diagram schematically illustrates the distance L traveled with the first drive wheel in an exemplary embodiment. L The curve of the first initial maximum speed of the first drive wheel varies. It should be understood that in some embodiments, the first initial maximum speed may also be the maximum speed movement of the first drive wheel that satisfies the first and second constraints but does not consider the third constraint.

[0130] Then, all the maximum and minimum points of the first preliminary maximum velocity can be determined. From each minimum point, the wheel is accelerated towards the adjacent maximum points on both sides (if any) with the limiting wheel acceleration 'a' until it intersects the curve obtained by accelerating in the same way to the adjacent minimum points on both sides. Then, the curves between all the intersection points and adjacent minimum points are connected to obtain the acceleration constraint curve of the left wheel under acceleration constraints.

[0131] Figure 5G Schematic illustration of L L Taking the minimum point (L1, v1) at L1 as an example, from this minimum point, the wheel accelerates with the limiting wheel acceleration a towards the adjacent maximum point to the left (i.e., behind) until they are a minimum distance dL apart. L L L = at L2, then:

[0132]

[0133] Where v2 represents the acceleration to the minimum distance between L. L = The wheel velocity of the first driving wheel at point L2, where p represents the slope by which the wheel velocity of the first driving wheel increases from point (L1, v1) with the limiting wheel acceleration a towards point (L2, v2). When dL L Approaching 0,

[0134] Therefore, we can conclude that:

[0135]

[0136] Accordingly, the fourth constraint is set for points on path 2 that are after the nearest minimum point:

[0137]

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

[0139] Figure 5H The minimum point (L1, v1) at L L The first driven wheel is accelerated to the adjacent maximum value point on the right side (i.e. the front) of the minimum distance dL L The L L = L2, then:

[0140]

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

[0142] Thus, it can be obtained that:

[0143]

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

[0145]

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

[0147] 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 value point to the arbitrary point, then:

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

[0149] and / or wherein L L and L R respectively represent the movement distances of the first driven wheel and the second driven wheel moving to the arbitrary point, v1 respectively represents the first preliminary maximum speed or the second preliminary maximum speed of the nearest minimum value point to the arbitrary point, L L1 and L R1 respectively represent the movement distances of the first driven wheel and the second driven wheel moving to the nearest minimum value point.

[0150] Figure 5I It schematically shows that in Figure 5F Based on this, the first maximum velocity v that further satisfies the fourth constraint Lmax Second maximum speed v Rmax In other words, Figure 5I The first maximum speed v shown Lmax Second maximum speed v Rmax It simultaneously satisfies the first constraint, the second constraint, the third constraint, and the fourth constraint.

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

[0152] Figure 6 A multi-robot trajectory planning method according to an exemplary embodiment of the present invention is illustrated schematically.

[0153] like Figure 6 As shown, the multi-robot trajectory planning method includes at least the following steps: a preliminary planning step S21, wherein multiple planned trajectories containing time information are obtained for multiple mobile robots 1, the multiple planned trajectories being generated by performing time-optimal trajectory planning on the multiple mobile robots 1 respectively; a conflict identification step S22, wherein conflict points between two of the multiple planned trajectories are identified in the spatial and temporal dimensions, the conflict points indicating that mobile robots 1 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. Thus, when multiple mobile robots 1 are working in the same working environment, without collisions between the multiple mobile robots 1, the multiple mobile robots 1 can reach their respective destinations in the shortest possible time as a whole.

[0154] 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 conflict point between the planned trajectories. Thus, the multiple planned trajectories obtained can ensure that there is no collision between the multiple mobile robots 1, and the multiple mobile robots 1 can reach their respective destinations in the shortest time as a whole.

[0155] It should be understood that the conflict 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 performing trajectory planning for the multiple mobile robots through the trajectory planning method.

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

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

[0158] Specifically, in the conflict 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 conflict 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.

[0159] The conflict resolution step S23 may include, for example, the following sub-steps: Sub-step S231: Selecting a conflict point to be resolved and an adjusted planning trajectory from the identified conflict points and conflicting planning trajectories, wherein the adjusted planning trajectory 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 shared by the adjusted planning trajectory; Sub-step S232: Adjusting the time information of the adjusted planning trajectory at the conflict point by delaying the time information of the adjusted planning trajectory at the conflict point, such that the time interval between the time information of the two associated planning trajectories at the conflict point is greater than or equal to a time interval threshold; and Sub-step S233: Based on the adjusted time information of the adjusted planning trajectory at the conflict point, correspondingly updating the time information of the portion of the adjusted planning trajectory after the conflict point. Thus, conflicts can be resolved with fewer adjustments.

[0160] The following is combined Figure 7 and Figure 8 Further explanation is provided regarding exemplary embodiments of the present invention. Figure 7 The diagram schematically illustrates five paths corresponding to five mobile robots 1. Curves labeled 1-5 correspond to paths 1-5 of mobile robots 1. Path intersections or overlaps represent the spatial intersection points of the corresponding mobile robot 1 trajectories. Figure 7 As can be seen, the first planned trajectory for the first mobile robot 1 has five intersection points with the other planned trajectories, which are the intersection points between the first planned trajectory and the second, fifth, third, fourth, and fifth planned trajectories respectively. Clearly, Figure 7 The time information for the corresponding planned trajectory is not shown.

[0161] After finding all intersection points, for each intersection point, the time information of the relevant planned trajectory entering and leaving 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 can be determined whether each intersection point is a conflict point.

[0162] Figure 8 Intersection points and conflict points are schematically illustrated in an exemplary embodiment of the invention. Figure 8 For clarity, each planned trajectory is schematically represented by a horizontal axis. Each horizontal axis corresponds both to a time scale and the distance traveled by mobile robot 1. A point on each horizontal axis represents the position that mobile robot 1 will reach 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".

[0163] 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.

[0164] 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:

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

[0166] 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:

[0167]

[0168] 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.

[0169] 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:

[0170]

[0171]

[0172]

[0173]

[0174]

[0175] 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 similarly extended. 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.

[0176] 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. This cycle is repeated until all the planned trajectories are traversed.

[0177] 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, the conflict points are marked in bold. In Figure 8 , the 5 conflict points are shown in the form of rectangular grids in the last horizontal axis direction.

[0178] After identifying the conflict point, conflict resolution step S23 can be executed. Preferably, in conflict resolution step S23, the time information of the adjusted planned trajectory at the conflict point and the time information of the adjusted planned trajectory after the conflict point are equally delayed. Since the multiple planned trajectories are themselves time-optimal trajectory plans, this method can ensure that the adjusted planned trajectory still satisfies the kinematic and dynamic constraints of the mobile robot 1, and enables the multiple mobile robots 1 to reach their respective destinations as a whole in the shortest possible time without collisions. Because the time-optimal global trajectory plan 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 delayed. This delay will correspondingly affect all the time information of the adjusted planned trajectory after the conflict point to be resolved.

[0179] In one exemplary embodiment, the conflict points to be resolved and the planned trajectories to be adjusted are selected according to the priority (or importance) of the tasks corresponding to the planned trajectories. If different mobile robots 1 perform tasks with different priorities, when conflict points exist, the planned trajectories with higher priority tasks can be fixed first, and the conflict points of the fixed planned trajectories can be resolved by adjusting the planned trajectories that conflict with the fixed planned trajectories in turn.

[0180] Specifically, the conflict identification step S22 and the conflict resolution step S23 are performed as follows: First, all conflict points between the multiple planning trajectories are identified; the planning trajectories with conflict points are sorted in descending order of priority of their corresponding tasks; the planning trajectory with the highest priority is selected as the fixed planning trajectory, and the conflict points of the fixed planning trajectory are identified one by one as the conflict points to be adjusted. Accordingly, the planning trajectories that conflict with the fixed planning trajectory at the conflict points to be adjusted are identified as the planning trajectories to be adjusted, so as to resolve all conflict points of the fixed planning trajectory; then, the conflict identification step S22 is performed again to re-identify all conflict points between the multiple planning trajectories.

[0181] by Figure 8 Taking the five planning trajectories shown as an example, all five trajectories have conflict points. They are sorted in descending order of priority according to the tasks they correspond to. If the priority of the tasks corresponding to these five planning trajectories is sorted in descending order as 1>2>3>4>5, then the first planning trajectory is fixed first. The conflict points "1-2" and "1-3" of the first planning trajectory are identified one by one as conflict points to be resolved, and the planning trajectories that are adjusted accordingly are the second and third planning trajectories.

[0182] 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.

[0183] 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.

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

[0185]

[0186] 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 the conflict point "1-2" 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.

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

[0188] 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:

[0189]

[0190] 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:

[0191]

[0192]

[0193]

[0194]

[0195]

[0196] 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.

[0197] 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:

[0198]

[0199] 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.

[0200] The conflict identification step S22 is performed here, and the identification result is shown in the following matrix:

[0201]

[0202]

[0203]

[0204]

[0205]

[0206] 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.

[0207] 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:

[0208]

[0209]

[0210]

[0211]

[0212]

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

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

[0215] 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 ascending order of the number of conflict points; the planned trajectory at the top of the sorted list is selected as the fixed planned trajectory, and the conflict points of the fixed planned trajectory are identified one by one as the conflict points to be adjusted. Accordingly, the planned trajectories that conflict with the fixed planned trajectory at the conflict points to be adjusted are identified as the planned trajectories to be adjusted, thereby resolving 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 multiple planned trajectories have the same and minimum number of conflict points, the planned trajectory to be adjusted can be selected based on the time order in which the conflict points are entered.

[0216] by Figure 8 Taking the five planned trajectories shown as an example, there are five conflict points among them: "1-2", "3-5", "1-3", "3-4", and "4-5". All five trajectories have conflict points, and they are ordered from least to most conflict points: 2 < 1 = 4 = 5 < 3. First, fix the second planned trajectory. The fixed second planned 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 time delay is 0.325 - 0.25 + 0 = 0.075.

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

[0218]

[0219]

[0220]

[0221]

[0222]

[0223] 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.

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

[0225]

[0226]

[0227]

[0228]

[0229]

[0230] 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 trajectory 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.

[0231] 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.

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

[0233]

[0234] 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.

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

[0236]

[0237]

[0238]

[0239]

[0240]

[0241] Again, the conflict recognition step S22 is executed, and it is recognized 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, are ranked according to the time order of entering the conflict points: 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.

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

[0243]

[0244]

[0245]

[0246]

[0247]

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

[0249] 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 ranked earlier is preferentially fixed without being adjusted.

[0250] Specifically, the conflict identification step S22 and the conflict resolution step S23 are performed as follows: First, all conflict points between the multiple planning trajectories are identified; the planning trajectories with conflict points are sorted in ascending order of the duration of the conflict points; the planning trajectory with the highest sorting value is selected as the fixed planning trajectory, and the conflict points of the fixed planning trajectory are identified one by one as the conflict points to be adjusted. Accordingly, the planning trajectory that conflicts with the fixed planning trajectory at the conflict points to be adjusted is identified as the planning trajectory to be adjusted, thereby resolving all conflict points of the fixed planning trajectory; then, the conflict identification step S22 is performed again to re-identify all conflict points between the multiple planning trajectories. When multiple planning trajectories have the same number of conflict points and the minimum number, the planning trajectory to be adjusted can be selected based on the time order of entering the conflict points.

[0251] by Figure 8 Taking the five planned trajectories shown as an example, there are five conflict points among them: "1-2", "3-5", "1-3", "3-4", and "4-5". All five trajectories have conflict points, and they are ordered in ascending order of conflict duration: 2 < 1 < 3 < 4 < 5. First, fix the second planned trajectory. The fixed second planned 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 time is 0.325 - 0.25 + 0 = 0.075.

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

[0253]

[0254]

[0255]

[0256]

[0257]

[0258] 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.

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

[0260]

[0261]

[0262]

[0263]

[0264]

[0265] 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.

[0266] 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.

[0267] 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.

[0268] 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:

[0269]

[0270]

[0271]

[0272]

[0273]

[0274] 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.

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

[0276]

[0277]

[0278]

[0279]

[0280]

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

[0282] 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.

[0283] 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.

[0284] In this way Figure 8The 5 planning trajectories shown are taken as an example, and there are 5 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 1st planning trajectory and the 2nd planning trajectory participating in the conflict point "1-2", the 2nd planning trajectory enters the conflict point "1-2" later, and thus the 2nd planning trajectory is determined as the planning trajectory to be adjusted. Then, the 2nd planning trajectory is adjusted to resolve the conflict point "1-2" between the 2nd planning trajectory and the 1st planning trajectory. The amount of time delay is 0.3-0.275+0=0.025.

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

[0286]

[0287]

[0288]

[0289]

[0290]

[0291] The conflict identification step S22 is performed again, and it is identified that there are 4 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 5th planning trajectory is adjusted to resolve the conflict point "3-5" between the 5th planning trajectory and the 3rd planning trajectory. The amount of time delay is 0.0425.

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

[0293]

[0294]

[0295]

[0296]

[0297]

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

[0299] Alternatively, the conflict identification step S22 and the conflict resolution step S23 can also be performed in other ways. For example, after identifying all the conflict points between the multiple planning trajectories, the planning 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 of the number of their conflict points from few to many; in order of the conflict duration of their conflict points from little to much; in order of the time of their entering the conflict points. Then, one of the planning trajectories with conflict points between the planning trajectories sorted at the front is selected as an adjusted planning trajectory, and the time information of the adjusted planning trajectory is adjusted to resolve the conflict points between the adjusted planning trajectory and the planning trajectories sorted at the front.

[0300] In addition, the present application also relates to a computer program product comprising computer program instructions which, when executed by one or more processors, enable the processors to perform the trajectory planning method and / or the multi-robot trajectory planning method according to the present application.

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

[0302] Although specific embodiments of the present application are described in detail herein, they are merely for the purpose of explanation and should not be considered to limit the scope of the present application. Various substitutions, alterations and modifications can be conceived without departing from the spirit and scope of the present application.

Claims

1. A trajectory planning method for a mobile robot (1), wherein, The mobile robot (1) is segmentedly speed planned according to the determined path (2) to determine a time information contained planning trajectory enabling the mobile robot (1) to move along the path (2), the trajectory planning method comprising: 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 moving in coordination with the constrained wheel will satisfy the kinematic and dynamic constraints, the constrained wheel being the drive wheel that, in the segment, reaches the limit value of the kinematic or dynamic constraint first according to the path shape of the segment and the motion state of each drive wheel at the first control point; speed planning the constrained wheel based on the path (2) to determine the speed of the constrained wheel while satisfying the kinematic and dynamic constraints of the constrained wheel; speed planning the other drive wheels other than the constrained wheel in coordination with the determined speed of the constrained wheel, wherein the constrained wheel in each segment is determined in the following manner: acquiring a first initial velocity v of the first driving wheel and the second driving wheel at the first control point L0 and a second initial velocity v R0 ; determining the value k1 of the speed ratio k at the second control point as the end point of the segment, the speed ratio k representing the ratio of the speed of the second drive wheel to the speed of the first drive wheel, which is determined by the path (2); determining a first maximum speed v Lmax and a second maximum speed v Rmax of the first and second drive wheels at the second control point, respectively, which represent the maximum speeds that satisfy the kinematic and dynamic constraints of the respective drive wheels and the limits of the path (2) without considering the speeds of the first and second drive wheels before reaching the second control point, respectively; the first drive wheel is accelerated from the first initial speed v L0 the speed resulting from the start of acceleration of the first drive wheel to the second control point with the limit wheel acceleration of the first drive wheel is determined as the first acceleration end speed v La the second drive wheel is accelerated from the second initial speed v R0 the speed resulting from the start of acceleration of the second drive wheel to the second control point with the limit wheel acceleration of the second drive wheel is determined as the second acceleration end speed v Ra ; determined as the first terminal speed v Lmax determined as the first terminal speed v La determined as the first terminal speed v L determined as the second terminal speed v Rmax determined as the second terminal speed v Ra determined as the second terminal speed v R ; The second final velocity v R With the first final 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 trajectory planning method according to claim 1, wherein, in the process of speed planning the constrained wheel, the speed of the constrained wheel is determined to have one of the maximum speed and the maximum acceleration of the constrained wheel at any point under the condition that the constrained wheel satisfies its kinematic and dynamic constraints and satisfies the constraints of the path (2).

3. The trajectory planning method according to claim 2, wherein, in the process of speed planning the constrained wheel, a T-shaped planning method is adopted.

4. The trajectory planning method according to claim 2, wherein, the kinematic and dynamic constraints include: the magnitude of the speed of the drive wheel is below the limit wheel speed predetermined for the drive wheel; the magnitude of the acceleration of the drive wheel is below the limit wheel acceleration predetermined for the drive wheel.

5. The trajectory planning method according to claim 4, wherein, the following steps are respectively performed for at least one segment of the path (2): for the 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 wheel; speed planning the constrained wheel to determine the speed of the constrained wheel within the segment; determining the speed of the other drive wheels within the segment in coordination with the determined speed of the constrained wheel.

6. The trajectory planning method according to claim 5, wherein, the mobile robot (1) is a double differential wheel robot, and the at least two drive wheels are a first drive wheel and a second drive wheel arranged symmetrically, wherein the first drive wheel and the second drive wheel are subject to the same kinematic and dynamic constraints.

7. The trajectory planning method according to claim 1, wherein, if the second terminal speed v R is greater than the first terminal speed v L , and if the ratio of the first terminal speed v R to the second terminal speed v L is greater than the speed ratio k1 at the second control point, then the first drive wheel is determined to be the constrained wheel in the section. if the second terminal speed v R is less than the first terminal speed v L , then determining the second drive wheel as the constrained wheel in the section. If the second terminal speed v R is greater than the first terminal speed v L , then the ratio of the first terminal speed v R to the second terminal speed v L is equal to the speed ratio k1 at the second control point, then one of the first drive wheel and the second drive wheel is determined to be the constrained wheel in the section.

8. The trajectory planning method according to claim 1, wherein, The movement duration corresponding to each section is equal to a predetermined control period t, the first acceleration final speed v La and the second acceleration final speed v Ra is determined according to the following formula: v La = v L0 + a*t v Ra = v R0 + a*t wherein a represents a limit wheel acceleration of the first driving wheel and the second driving wheel. 9.The trajectory planning method according to any one of claims 1-8, wherein, a first maximum speed v Lmax and a second maximum speed v Rmax is determined according to at least one of the following constraints: - based on a first constraint on the limit wheel speed v lim : v Lmax ≤ v lim , - based on the limit wheel speed v lim and the second constraint on the speed ratio determined by the path (2): v Lmax ≤ v lim / k, - a third constraint based on the limit wheel acceleration a and the speed ratio variation rate k' determined by the path (2): where k ′ ≠ 0; and the first maximum speed v Lmax and the second maximum speed v Rmax satisfies: v Rmax = v Lmax *k. 10.The trajectory planning method according to claim 9, wherein, the first maximum speed v Lmax and the second maximum speed v Rmax Additionally, the determination is made in accordance with a fourth constraint as follows: determining that the first drive wheel is moving along the path (2) with a maximum speed determined by the at least one of the first constraint, the second constraint and the third constraint L a first preliminary maximum speed that varies and a distance of movement L of the second drive wheel R at least one of a second preliminary maximum speed that varies and a distance of movement L of the second drive wheel determining a maximum point and a minimum point of the at least one of the first preliminary maximum speed and the second preliminary maximum speed; a first maximum speed v at said arbitrary point Lmax and / or a second preliminary maximum speed v Rmax satisfies: if the arbitrary point is behind the minimum point closest to the arbitrary point: and / or if the arbitrary point is in front of the minimum point closest to the arbitrary point: and / or wherein L L and L R denote the movement distance of the first drive wheel and the second drive wheel, respectively, to said arbitrary point, and v1 denotes the first preliminary maximum speed or the second preliminary maximum speed, respectively, to the minimum point closest to said arbitrary point, L L1 and L R1 denote the movement distance of the first drive wheel and the second drive wheel, respectively, to said closest minimum point. 11.The trajectory planning method according to any one of claims 1-8, 10, wherein, the path (2) is 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; and / or the path (2) is in the form of a 3-order or higher Bezier curve.

12. A computer program product comprising computer program instructions, wherein, The computer program instructions, when executed by one or more processors, cause the processors to perform the trajectory planning method according to any one of claims 1-11.

Images