CN116257042A - Trajectory planning method for mobile robot and computer program product - Google Patents

Abstract

A trajectory planning method for a mobile robot is proposed, which performs speed planning on the mobile robot according to a determined path to determine a planned trajectory containing time information, and the trajectory planning method includes: determining at least two driving wheels of the mobile robot One is a constrained wheel such that as long as the constrained wheel satisfies the kinematic and dynamic constraints, the other driving wheel moving in coordination with the constrained wheel will satisfy the kinematic and dynamic constraints; Under the condition of mechanical and dynamic constraints, the speed planning of the constrained wheel is carried out to determine the speed of the constrained wheel; the speed planning of the other driving wheels is carried out in a manner that matches the determined speed of the constrained wheel. A corresponding computer program product is proposed. Through the present invention, an alternative trajectory planning method for mobile robots is provided, which can especially reliably plan trajectories satisfying kinematics and dynamics constraints for mobile robots.

Description

Trajectory planning method and computer program product for mobile robot

Technical Field

The present invention relates to the field of mobile robots, in particular to the field of motion control of mobile robots, and specifically to a trajectory planning method for mobile robots and a computer program product.

Background Art

With the rapid economic growth and the gradual increase in labor costs, mobile robots are increasingly used in various industrial and domestic environments. For example, mobile robots such as automatic guided vehicles (AGVs), autonomous mobile robots (AMRs), and forklifts are one of the key devices in modern logistics systems. Mobile robots can move and dock at the target location according to the planned path and operation requirements to complete tasks such as material handling and transportation. Trajectory planning is the key to the motion control of mobile robots.

In the process of trajectory planning, it is necessary to plan the speed of 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 set of differential equations. For example, in the existing algorithms, when planning the motion trajectory based on the path, a constrained optimization problem with time as the optimization target 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 must also be considered. This requires both a large amount of memory and a considerable amount of computation.

The existing technology still has many deficiencies in path planning for mobile robots.

Summary of the invention

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

According to a first aspect of the present invention, a trajectory planning method for a mobile robot is provided, wherein speed planning is performed on the mobile robot according to a determined path to determine a planned trajectory containing time information that enables the mobile robot to move along the path, and the trajectory planning method comprises: determining that one of at least two driving wheels of the mobile robot is a constrained wheel, so that as long as the constrained wheel satisfies kinematic and dynamic constraints, the other driving wheels that move in coordination with the constrained wheel will satisfy the kinematic and dynamic constraints; based on the path, speed planning is performed on the constrained wheel while satisfying the kinematic and dynamic constraints of the constrained wheel to determine the speed of the constrained wheel; and speed planning is performed on the other driving wheels other than the constrained wheel in a manner that matches the determined speed of the constrained wheel.

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

In an exemplary embodiment, during velocity planning of the constrained wheel, the velocity of the constrained wheel is determined so that the constrained wheel has one of a maximum velocity and a maximum acceleration at any point while satisfying its kinematic and dynamic constraints and satisfying the constraints of the path.

In an exemplary embodiment, a T-shaped planning method is used during velocity planning of the constrained wheel.

In an exemplary embodiment, the kinematic and dynamic constraints include: a magnitude of a speed of a driving wheel is below a limit wheel speed predetermined for the driving wheel; a magnitude of an acceleration of a driving wheel is below a limit wheel acceleration predetermined for the driving wheel.

In an exemplary embodiment, speed planning is performed on the path segment by segment, and the following steps are performed on at least one segment of the path: for a first control point serving as the starting point of the segment, one of the at least two driving wheels is determined to be a constrained wheel in the segment according to the path shape of the segment, the motion state of each driving wheel at the first control point, and the kinematic and dynamic constraints of the driving wheels, and the constrained wheel is: in the segment, according to the path shape of the segment and the motion state of each driving wheel at the first control point, the driving wheel preferentially reaches the limit value of the kinematic or dynamic constraints; speed planning is performed on the constrained wheel to determine the speed of the constrained wheel in the segment; and the speed of the other driving wheels in the segment is determined in a manner that matches the determined speed of the constrained wheel.

In an exemplary embodiment, the mobile robot is a dual-differential wheel robot, and the at least two driving wheels are a first driving wheel and a second driving wheel that are symmetrically arranged, wherein the first driving wheel and the second driving wheel are subject to the same kinematic and dynamic constraints.

In one exemplary embodiment, the constrained wheels in each segment are determined in the following manner:

Obtain a first initial velocity v _L0 and a second initial velocity v _R0 of the first driving wheel and the second driving wheel at the first control point;

Determine a value k1 of a speed ratio k determined by the path at a second control point as an end point of the section, wherein the speed ratio k represents a ratio of a speed of the second driving wheel to a speed of the first driving wheel;

Determine a first maximum speed v _Lmax and a second maximum speed v _Rmax of the first driving wheel and the second driving wheel at the second control point, respectively, wherein the first maximum speed and the second maximum speed respectively represent maximum speeds that satisfy kinematic and dynamic constraints of each driving wheel and satisfy the constraints of the path without considering the speeds of the first driving wheel and the second driving wheel before reaching the second control point;

The speed obtained by accelerating the first driving wheel from the first initial speed v _L0 at the first control point to the second control point at the limit wheel acceleration of the first driving wheel is determined as the first acceleration terminal speed v _La , and the speed obtained by accelerating the second driving wheel from the second initial speed v _R0 at the first control point to the second control point at the limit wheel acceleration of the second driving wheel is determined as the second acceleration terminal speed v _Ra ;

The smaller of the first maximum speed v _Lmax and the first acceleration terminal speed v _La at the second control point is determined as the first terminal speed v _L , and the smaller of the second maximum speed v _Rmax and the second acceleration terminal speed v _Ra at the second control point is determined as the second terminal speed v _R ; and

The ratio of the second terminal velocity v _R to the first terminal velocity v _L is compared with the velocity ratio k1 at the second control point, and the constrained wheel in the section is determined based on the comparison result.

In an exemplary embodiment, if the ratio of the second terminal velocity v _R to the first terminal velocity v _L is greater than the speed ratio k1 at the second control point, the first drive wheel is determined to be the constrained wheel in the section; if the ratio of the second terminal velocity v _R to the first terminal velocity v _L is less than the speed ratio k1 at the second control point, the second drive wheel is determined to be the constrained wheel in the section; if the ratio of the second terminal velocity v _R to the first terminal velocity v _L is equal to the speed ratio k1 at the second control point, one of the first drive wheel and the second drive wheel is determined to be the constrained wheel in the section.

In an exemplary embodiment, the motion duration corresponding to each segment is equal to the predetermined control period t, and the first acceleration terminal velocity v _La and the second acceleration terminal velocity v _Ra are determined according to the following formula:

v _La = v _L0 + a*t

v _Ra = v _R0 + a*t

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

In an exemplary embodiment, the first maximum speed v _Lmax and the second maximum speed v _Rmax of the first drive wheel and the second drive wheel at any point on the path are determined according to at least one of the following constraints:

- The first constraint based on the limit wheel speed v _lim : v _Lmax ≤ _{v lim} ,

- A second constraint based on the limit wheel speed v _lim and the speed ratio determined by the path: v _Lmax ≤ _{v lim} /k,

其中，k′≠0；并且- The third constraint based on the limit wheel acceleration a and the speed ratio change rate k' determined by the path:

where k′≠0; and

The first maximum speed v _Lmax and the second maximum speed v _Rmax of the first driving wheel and the second driving wheel at any point on the path satisfy: v _Rmax =v _Lmax *k.

In an exemplary embodiment, the first maximum speed v _Lmax and the second maximum speed v _Rmax of the first drive wheel and the second drive wheel at any point on the path are additionally determined according to the following fourth constraint:

Determine at least one of a first preliminary maximum speed varying with a movement distance _{LL of the first driving wheel and a second preliminary maximum speed varying with a movement distance LR of} the second driving wheel assuming that the first driving wheel moves along the path at a maximum speed determined by the at least one of the first constraint, the second constraint, and the third constraint;

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

The first maximum speed v _Lmax and/or the second preliminary maximum speed v _Rmax at any point satisfy:

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

和/或

and/or

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

和/或

and/or

Among them, _LL and _LR respectively represent the movement distances of the first driving wheel and the second driving wheel to the arbitrary point, _v1 correspondingly represents the first preliminary maximum speed or the second preliminary maximum speed of the minimum point closest to the arbitrary point, and _LL1 and _LR1 respectively represent the movement distances of the first driving wheel and the second driving wheel to the nearest minimum point.

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

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

The positive effect of the present invention is that it provides an alternative trajectory planning method, which can especially reliably plan a trajectory for a mobile robot that satisfies kinematic and dynamic constraints.

BRIEF DESCRIPTION OF THE DRAWINGS

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

FIG1 schematically shows a mobile robot and its path implementing a trajectory planning method according to an exemplary embodiment of the present invention;

FIG2 schematically shows a flow chart of a trajectory planning method for a mobile robot according to an exemplary embodiment of the present invention;

FIG3 schematically shows a flow chart of performing speed planning on a path segment by segment according to an exemplary embodiment;

FIG4A schematically shows a curve of a change in the radius of curvature and the curvature on a path according to an exemplary embodiment of the present invention;

4B schematically shows a variation curve of a speed ratio on a path and a first maximum speed and a second maximum speed satisfying a first constraint and a second constraint in an exemplary embodiment of the present invention;

4C and 4D schematically illustrate the movement speed and movement distance of the mobile robot corresponding to the first maximum speed and the second maximum speed shown in FIG. 4B , as well as the first required acceleration and the second required acceleration required by the first driving wheel and the second driving wheel, and the timestamp of the mobile robot moving along the path;

FIG4E schematically shows the first required acceleration and the second required acceleration required by the first driving wheel and the second driving wheel after replanning and the timestamp of the mobile robot moving along the path;

FIG4F schematically shows the speed ratio k and the speed variation curves of the first driving wheel and the second driving wheel after replanning;

FIG5A schematically shows a path according to an exemplary embodiment of the present invention;

FIG5B schematically shows a curve of curvature variation on the path in the exemplary embodiment shown in FIG5A ;

FIG5C schematically shows a curve of a change in speed ratio on a path in the exemplary embodiment shown in FIG5A ;

FIG5D schematically illustrates a first maximum speed and a second maximum speed satisfying a first constraint in the exemplary embodiment shown in FIG5A ;

FIG. 5E schematically illustrates a first maximum speed and a second maximum speed satisfying the first constraint and the second constraint in the exemplary embodiment shown in FIG. 5A ;

FIG. 5F schematically illustrates a first maximum speed and a second maximum speed that satisfy the first constraint, the second constraint, and the third constraint in the exemplary embodiment shown in FIG. 5A ;

5G-5H schematically show a curve of a first preliminary maximum speed of the first driving wheel as a function of the movement distance of the first driving wheel in an exemplary embodiment;

FIG. 5I schematically illustrates a first maximum speed and a second maximum speed satisfying the first constraint, the second constraint, the third constraint, and the fourth constraint in the exemplary embodiment shown in FIG. 5A ;

FIG6 schematically shows a flow chart of a multi-robot trajectory planning method according to an exemplary embodiment of the present invention;

FIG7 schematically shows five paths for five mobile robots respectively;

FIG8 schematically illustrates intersection points and conflict points according to an exemplary embodiment of the present invention; and

FIG. 9 schematically shows intersection points and conflict points after the conflict point “ 1 - 2 ” is resolved in the exemplary embodiment shown in FIG. 8 .

DETAILED DESCRIPTION

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

The present invention is applicable to mobile robots, which may be any robot capable of autonomously moving in space, such as AGV, AMR, etc. The mobile robot may be used to perform various tasks, such as a storage robot, a cleaning robot, a home care robot, a welcoming robot, etc.

It should be understood that, in this document, the expressions "first", "second", etc. are used for descriptive purposes only and should not be understood as indicating or implying relative importance, nor should they be understood as implicitly indicating the number of technical features indicated. Features defined as "first" or "second" may explicitly or implicitly indicate that at least one of the features is included.

The motion control method of the present invention is exemplarily described below in conjunction with Figures 1 and 2. Figure 1 schematically shows a mobile robot 1 and its path 2 for implementing a trajectory planning method according to an exemplary embodiment of the present invention. Figure 2 schematically shows a flow chart of a trajectory planning method for a mobile robot 1 according to an exemplary embodiment of the present invention.

In the embodiment shown in FIG1 , the mobile robot 1 is, for example, a differential robot, that is, the mobile robot 1 has a differential wheel motion system including a first drive wheel (hereinafter, the left wheel is exemplarily used as the first drive wheel for explanation) and a second drive wheel (hereinafter, the right wheel is exemplarily used as the second drive wheel for explanation). Alternatively, the mobile robot 1 may also be another type of robot, such as a dual-steering wheel robot. Accordingly, the mobile robot 1 may, for example, include a dual-steering wheel motion system.

In the trajectory planning method, velocity planning is performed on the mobile robot 1 according to the determined path 2 to determine a planned trajectory including time information that enables the mobile robot 1 to move along the path 2. The path 2 may be a global path determined by performing global path planning according to at least one task point of the mobile robot 1, and the at least one task point is located on the global path.

The path 2 may be in the form of a Bezier curve of degree 3 or higher and can be expressed as follows:

表示移动机器人1的位置，i＝0,1,…,N，N≥3，

表示贝塞尔曲线的控制点的坐标。当s从0增大到1，对应的

表示移动机器人1沿着路径2从起点到终点的位置。这对于差速机器人而言是尤其有利的。路径2具有连续的二阶导数能够特别有利地适应差速机器人的运动特性。特别是，路径2能够具有连续的曲率。这使得移动机器人1的速度和加速度的变化更平缓。图1中示出了呈4阶贝塞尔曲线形式的路径2。但应理解，路径2也可具有其它形状。in,

represents the position of 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

represents the position of the mobile robot 1 along the path 2 from the starting point to the end point. This is particularly advantageous for a differential robot. The continuous second-order derivative of the path 2 can be particularly advantageous for adapting to the motion characteristics of the differential robot. In particular, the path 2 can have a continuous curvature. This makes the change of the speed and acceleration of the mobile robot 1 smoother. FIG. 1 shows the path 2 in the form of a 4th-order Bezier curve. However, it should be understood that the path 2 may also have other shapes.

As shown in Figure 2, the trajectory planning method includes: step S11, determining that one of the at least two driving wheels of the mobile robot 1 is a constrained wheel, so that as long as the constrained wheel satisfies the kinematic and dynamic constraints, the other driving wheels that move in coordination with the constrained wheel will satisfy the kinematic and dynamic constraints; step S12, based on the path 2, the constrained wheel is speed-planned while satisfying the kinematic and dynamic constraints of the constrained wheel to determine the speed of the constrained wheel; step S13, the speed of other driving wheels other than the constrained wheel is planned in a manner that matches the determined speed of the constrained wheel.

Thus, an alternative trajectory planning method is provided, which can reliably plan a trajectory for the mobile robot 1 that satisfies kinematic and dynamic constraints.

In the process of velocity planning for the constrained wheel, the constrained wheel is made to have at any point one of the maximum velocity and the maximum acceleration under the condition of satisfying its kinematic and dynamic constraints and satisfying the restrictions of the path 2. Thus, a time-optimal trajectory can be planned for the mobile robot 1.

The kinematic and dynamic constraints may include: the magnitude of the speed of the driving wheel is below the limit wheel speed predetermined for the driving wheel; the magnitude of the acceleration of the driving wheel is below the limit wheel acceleration predetermined for the driving wheel. The limit wheel speed and the limit wheel acceleration are limited by the construction of the mobile robot 1 itself without considering the limitation of the path 2. The limit wheel speed and the limit wheel acceleration are determined, for example, by the motor used to drive the corresponding driving wheel. Optionally, the kinematic and dynamic constraints may also include the acceleration of the driving wheel being below the limit wheel jerk predetermined for the driving wheel. Optionally, the first driving wheel and the second driving wheel of the mobile robot 1 may be symmetrically arranged so as to have the same limit wheel speed and limit wheel acceleration.

The constrained wheel can be determined as the driving wheel that first reaches the limit wheel acceleration during the motion process, from the current control moment to the next control moment, according to the path 2 of the mobile robot 1 and the motion state of the mobile robot 1 at the current moment, while satisfying the kinematic and dynamic constraints and moving at the maximum possible wheel speed. Here, the driving wheel that first reaches the limit wheel acceleration means that when the mobile robot takes the motion state at the current moment as the starting state and expects to accelerate to the maximum speed allowed by the kinematic and dynamic constraints as soon as possible along the path, the driving wheel that will cause the mobile robot to be unable to accelerate with 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 driving wheel has reached its limit wheel acceleration first, and the wheel acceleration of the second driving wheel is still below its limit wheel acceleration, then the mobile robot will not be able to accelerate with a greater acceleration due to the first driving wheel reaching the limit wheel acceleration. Therefore, the first driving wheel can be determined as a constrained wheel. If the current motion state of the mobile robot 1 has reached the maximum speed allowed by the kinematic and dynamic constraints and in line with the path, then any driving wheel can be used as a constrained wheel, or it can also be considered that there is no constrained wheel in this state.

Optionally, the path 2 is speed-planned in sections, and the following steps are respectively performed on at least one section of the path 2: for a first control point as the starting point of the section, according to the path shape of the section, the motion state of each driving wheel at the first control point, and the kinematic and dynamic constraints of the driving wheel, one of the at least two driving wheels is determined to be a constrained wheel in the section, and the constrained wheel is: in the section, according to the path shape of the section and the motion state of each driving wheel at the first control point, the driving wheel preferentially reaches the limit value of the kinematic and dynamic constraints; the constrained wheel is speed-planned to determine the speed of the constrained wheel in the section; and the speed of the other driving wheels in the section is determined in a manner that matches the speed of the determined constrained wheel. Here, the driving wheel that preferentially reaches the limit value of the kinematic and dynamic constraints means that the driving wheel will reach the limit value of the kinematic and dynamic constraints before or at the same time as the other driving wheels.

The exercise duration corresponding to each section may be a predetermined control period t. The control period t may be set to a very short time, such as a millisecond time, such as less than 10 ms.

The process of performing speed planning on the path 2 in sections is further described below in conjunction with FIG3. FIG3 schematically shows the speed planning on the path 2 in sections according to an exemplary embodiment. In the exemplary embodiment, the starting point of the path 2 is taken as the first control point, and it is determined that starting from the current control point, the speed planning is performed on the section of the path 2 corresponding to the control cycle, and then the end point of the section is taken as the next control point, and the speed planning is continued until the end point of the entire path 2 is reached.

For each section, first obtain the first initial velocity v _L0 and the second initial velocity v _R0 of the first drive wheel and the second drive wheel at the first control point. At the starting point of path 2, the initial velocities of the first drive wheel and the second drive wheel are known. For sections other than the first section starting from the starting point of path 2, the first initial velocity v _L0 and the second initial velocity v _R0 of the first drive wheel and the second drive wheel at the first control point can be obtained from the planning result of the previous section. In the description of this article, the "speed" (also called "wheel speed") of the drive wheel is explained by taking the linear velocity as an example. Since the size of the drive wheel is determined, the relationship between the linear velocity and the angular velocity of the drive wheel is also determined.

其中，b表示第一驱动轮与第二驱动轮的轮距，曲率半径R与曲率互为导数，而路径2在任一点处的曲率是确定的。因此，对于确定的路径2，路径2上的任一点处的速度比k是确定的。例如，对于呈贝塞尔曲线的形式，速度比k可表示为变量s的函数：k＝g(s)。相应地，还可确定速度比变化率k’：k′＝g′(s)。在已经确定路径2上的任一点处的速度比k的情况下，第二控制点处的速度比k1可通过现有技术已知的方法求取。In addition, a value k1 of the speed ratio k determined by the path 2 at the second control point as the end point of the segment is determined, wherein the speed ratio k represents the speed ratio of the second driving wheel to the first driving wheel. For the differential wheel motion system, the speed ratio k and the curvature radius R of the path 2 satisfy:

Wherein, b represents the wheelbase 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 the determined path 2, the speed ratio k at any point on the path 2 is determined. For example, for the form of a Bezier curve, the speed ratio k can be expressed as a function of the variable s: k = g(s). Correspondingly, the speed ratio change rate k' can also be determined: k' = g'(s). When 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 prior art.

In addition, a first maximum speed v _Lmax and a second maximum speed v _Rmax of the first drive wheel and the second drive wheel at the second control point are respectively determined, wherein the first maximum speed and the second maximum speed respectively represent the maximum wheel speeds that satisfy the kinematic and dynamic constraints of each drive wheel and satisfy the restrictions of the path 2 without considering the speeds of the first drive wheel and the second drive wheel before reaching the second control point.

Then, the wheel speed obtained by accelerating the first driving wheel from the first initial speed v _L0 at the first control point to the second control point with the limit wheel acceleration of the first driving wheel is determined as the first acceleration terminal speed v _La , and the wheel speed obtained by accelerating the second driving wheel from the second initial speed v _R0 at the first control point to the second control point with the limit wheel acceleration of the second driving wheel is determined as the second acceleration terminal speed v _Ra . In the case where the section corresponds to the control period t, the first acceleration terminal speed v _La and the second acceleration terminal speed v _Ra can be determined according to the following formula:

v _La = v _L0 + a*t

v _Ra = v _R0 + a*t

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

The smaller of the first maximum speed v _Lmax and the first acceleration terminal speed v _La at the second control point is determined as the first terminal speed v _L , and the smaller of the second maximum speed v _Rmax and the second acceleration terminal speed v _Ra at the second control point is determined as the second terminal speed v _R .

Then, the ratio of the second terminal velocity v _R to the first terminal velocity v _L is compared with the velocity ratio k1 at the second control point, and the constrained wheel in the section is determined based on the comparison result.

After determining the constrained wheels in the segment, if the end point of path 2 has not been reached, the current second control point is used as the first control point of the next segment, and speed planning for the next segment continues.

Optionally, the constrained wheel is determined according to the comparison result of the ratio of the second terminal velocity v _R to the first terminal velocity v _L and the speed ratio k1 at the second control point in the following manner: if the ratio of the second terminal velocity v _R to the first terminal velocity v _L is greater than the speed ratio k1 at the second control point, the first drive wheel is determined to be the constrained wheel in the section; if the ratio of the second terminal velocity v _R to the first terminal velocity v _L is less than the speed ratio k1 at the second control point, the second drive wheel is determined to be the constrained wheel in the section; if the ratio of the second terminal velocity v _R to the first terminal velocity v _L is equal to the speed ratio k1 at the second control point, either the first drive wheel or the second drive wheel can be determined to be the constrained wheel in the section.

The process of determining the first maximum speed v _Lmax and the second maximum speed v _Rmax of the first driving wheel and the second driving wheel at any point on the path 2 is described in detail below in conjunction with FIGS. 4A-4F .

Fig. 4A schematically shows a variation curve of the curvature radius R and the curvature к on the path 2 according to an exemplary embodiment of the present invention. In this exemplary embodiment, the path 2 is a fourth-order Bezier curve, and the coordinates of its five Bezier curve control points are (0, -1), (0, 0), (1, 2) (4, 2) and (5, 3) respectively.

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

的一阶导横、纵坐标和二阶导横、纵坐标。相应地，可确定路径2上的任一点处的曲率半径R。Among them, P _x ′(s), P _y ′(s), P _x ″(s), and P _y ″(s) are

The first-order derivative, ordinate and second-order derivative, ordinate. Accordingly, the radius of curvature R at any point on the path 2 can be determined.

Then, the speed ratio k at any point on the path 2 can be obtained. The bottom of FIG4B schematically shows the variation curve of the speed ratio k on the path 2.

In this exemplary embodiment, the first maximum speed v _Lmax and the second maximum speed v Rmax of the first and second driving wheels at any point on Path 2 are determined according to a first constraint based on the limit wheel speed v _lim and a second constraint based on the limit wheel speed v _lim and the speed _ratio determined by Path 2 .

The first constraint indicates that the speed of the driving wheel cannot exceed its limit wheel speed. Therefore, the first maximum speed v _Lmax of the first driving wheel must satisfy the constraint: v _Lmax ≤v _lim . In this embodiment, the limit wheel speed v _lim of the first driving wheel and the second driving wheel is preset to 1.5 (m/s).

The second constraint indicates that the speed of any one of the first driving wheel and the second driving wheel must be such that the other driving wheel that satisfies the speed ratio k cannot exceed its limit wheel speed. Therefore, the first maximum speed v _Lmax of the first driving wheel must satisfy the constraint: v _Lmax ≤v _lim /k.

The first maximum speed v _Lmax and the second maximum speed v _Rmax of the first driving wheel and the second driving wheel at any point on the path 2 also satisfy the constraint: v _Rmax =v _Lmax *k.

Therefore, the first maximum speed v _Lmax and the second maximum speed v _Rmax are the maximum wheel speeds that satisfy the following constraints:

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) represents the smaller of a and b. The derived first maximum speed v _Lmax and second maximum speed v _Rmax are schematically shown in the top and middle of FIG. 4B .

It can be seen that in the process of determining the first maximum speed v _Lmax and the second maximum speed v _Rmax , the actual speed or planned speed of the first driving wheel and the second driving wheel before reaching the arbitrary point is not considered to limit the speed that can be achieved at the arbitrary point.

Assuming that the first driving wheel and the second driving wheel always move along the path 2 at the first maximum speed v _Lmax and the second maximum speed v _Rmax , the corresponding movement speed v _r and movement distance L of the mobile robot 1 will be as shown in Figure 4C. Accordingly, the timestamp t of the movement of the mobile robot 1 along the path 2 and the first required acceleration a _Lneed and the second required acceleration a _Rneed required by the first driving wheel and the second driving wheel can be obtained, as shown in Figure 4D.

In this embodiment, the limit wheel acceleration a of the first drive wheel and the second drive wheel is pre-set to 0.5 (m/s ² ) by way of example. As can be seen from FIG. 4D , the second drive wheel exceeds the limit wheel acceleration a in the interval s<s1. Therefore, it is necessary to replan this, and the replanning process is as follows: replanning is started from the speed minimum point in the section exceeding the limit wheel acceleration limit to both sides. In this embodiment, replanning is started from s=0 in the direction of increasing s. The acceleration is forcibly set to the limit wheel acceleration a, as shown in FIG. 4E . That is, starting from s=0, the second drive wheel accelerates with an acceleration a=0.5, and the first drive wheel cooperates with the second drive wheel to move to meet the speed ratio determined by path 2. Obviously, the first drive wheel needs to decelerate first (the magnitude of its acceleration will be below the limit wheel acceleration a) and then accelerate. FIG. 4F again shows the speed ratio k and the speed change curves of the first drive wheel and the second drive wheel after replanning. As can be seen from FIG4F , at the position s=s ₂ , the first drive wheel and the second drive wheel will reach the first maximum speed v _Lmax and the second maximum speed v _Rmax . Here, s ₂ >s _1. After s=s ₂ , the first drive wheel and the second drive wheel can move along the path 2 at the first maximum speed v _Lmax and the second maximum speed v _Rmax .

In the replanned section, the second driving wheel may be determined as a constrained wheel, and the speed of the second driving wheel may be planned to determine the speed of the second driving wheel. Then, the speed of the first driving wheel may be planned in a manner that matches the determined speed of the second driving wheel. The process of determining the second driving wheel as a constrained wheel may refer to FIG. 3 . In the replanned section, v _R0 +a*t<v _Rmax , therefore, v _R ＝v _R0 +a*t. And v _L0 +a*t>v _Lmax , therefore, v _L ＝v _Lmax . Furthermore, v _R/ v _L <k, therefore, the second driving wheel (i.e., the right wheel) is the constrained wheel.

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

5A-5F , a process of determining the first maximum speed v _Lmax and the second maximum speed v _Rmax of the first driving wheel and the second driving wheel at any point on the path 2 according to an exemplary embodiment of the present invention will be described in detail below.

FIG. 5A schematically shows a path 2 in an exemplary embodiment according to the present invention. In the exemplary embodiment, the path 2 satisfies the curve equation: y=sin(π/2*x). FIG. 5B schematically shows a curve of a change in the curvature к on the path 2 in the exemplary embodiment. FIG. 5C schematically shows a curve of a change in the speed ratio k on the path 2. The process of determining the curvature к and the speed ratio k on the path 2 according to the path 2 can refer to the description of FIGS. 4A-4F above.

In this exemplary embodiment, similar to the embodiment shown in FIGS. 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 any point on the path 2 need to satisfy the first constraint and the second constraint.

FIG5D schematically shows the first maximum speed v _Lmax and the second maximum speed v _Rmax satisfying the first constraint in this exemplary embodiment. The first constraint indicates that the speed of the driving wheel cannot exceed its limit wheel speed. Therefore, the first maximum speed v _Lmax and the second maximum speed v _Rmax need to satisfy the constraints: v _Lmax ≤v _lim , v _Rmax ≤v _lim . In this embodiment, the limit wheel speed v _lim of the first driving wheel and the second driving wheel is preset to 1.2 (m/s).

FIG5E schematically shows the first maximum speed v _Lmax and the second maximum speed v _Rmax satisfying the first constraint and the second constraint in this exemplary embodiment. The second constraint indicates that the speed of any one of the first driving wheel and the second driving wheel must be such that the other driving wheel satisfying the speed ratio k cannot exceed its limit wheel speed. Therefore, the first maximum speed v _Lmax of the first driving wheel must satisfy the constraint: v _Lmax ≤v _lim /k; the second maximum speed v _Rmax of the second driving wheel must satisfy the constraint: v _Rmax ＝v _lim *k.

Thus, the maximum wheel speeds satisfying the first constraint and the second constraint can be obtained: v _Lmax =min(v _lim ,v _lim /k), v _Rmax =min(v _lim ,v _lim *k).

The first maximum speed v _Lmax and the second maximum speed v _Rmax of the first driving wheel and the second driving wheel at any point on the path 2 also satisfy the constraint: v _Rmax =v _Lmax *k.

其中，k′≠0。In this embodiment, the first maximum speed v _Lmax and the second maximum speed v _Rmax of the first driving wheel and the second driving wheel at any point on the path 2 may additionally satisfy a third constraint based on the limit wheel acceleration a and the speed ratio change rate k' determined by the path 2:

Among them, k′≠0.

The principle of the third constraint is described in detail below. As described above, after path 2 is determined, the speed ratio k and speed ratio change rate k' at any point on path 2 can be determined. Assume that at a certain point on path 2, the first driving wheel and the second driving wheel of the mobile robot 1 moving along path 2 have speeds v _L0 and v _R0 respectively, and the speed ratio k ₀ =v _R0 /v _L0 .

Since the wheel accelerations of the first driving wheel and the second driving wheel cannot exceed the limit wheel acceleration a, after the mobile robot 1 moves along the path 2 for a small period of time t through a path segment, the value range of the speed ratio k is:

Then, at this point, the speed ratio change rate k' should satisfy the following formula:

由此，上式可简化为：Wherein, L represents the movement distance of the mobile robot 1, and the small displacement dL represents the displacement of the mobile robot 1 in a small time period t. The small displacement dL is equal to the arithmetic mean of the displacements of the first driving wheel and the second driving wheel, that is,

Therefore, the above formula can be simplified to:

When v _R0 +v _L0 is not equal to 0, the above formula can be simplified to:

相应地，第二驱动轮的轮速度需满足：

It can be seen from this that when the speed ratio change rate k′≠0, the wheel speed of the first driving wheel must satisfy:

Accordingly, the wheel speed of the second driving wheel must satisfy:

It should be understood that the speed ratio change rate k'=0 means that the mobile robot 1 performs linear motion or circular motion at this point. In the case of linear motion (k'=0 and k=1), any one of the first drive wheel and the second drive wheel can be determined as a constrained wheel. In the case of circular motion (k'=0 and k≠1), the outer wheel can be determined as a constrained wheel, that is, if k>1, the second drive wheel is determined as a constrained wheel, and if k<1, the first drive wheel is determined as a constrained wheel.

FIG. 5F schematically illustrates a first maximum speed v _Lmax and a second maximum speed v _Rmax that satisfy the first constraint, the second constraint, and the third constraint in this exemplary embodiment.

Optionally, the following fourth constraint may be set for the first maximum speed v _Lmax and the second maximum speed v _Rmax of the first driving wheel and the second driving wheel at any point on the path 2 .

The fourth constraint is described below with reference to FIGS. 5G-5H. First, a first preliminary maximum speed of the first driving wheel that varies with the movement distance _LL of the first driving wheel is determined by assuming that the first driving wheel moves at the maximum speed determined by the first constraint, the second constraint, and the third constraint along the path 2. FIGS. 5G-5H schematically show a curve of the first preliminary maximum speed of the first driving wheel that varies with the movement distance _LL of the first driving wheel in an exemplary embodiment. It should be understood that in some embodiments, the first preliminary maximum speed may also be the maximum speed movement of the first driving wheel that satisfies the first constraint and the second constraint but does not consider the third constraint.

Then, all the maximum and minimum points of the first preliminary maximum speed can be determined. Accelerate from each minimum point to the adjacent maximum points on both sides (if any) with the limiting wheel acceleration a until the curve obtained by accelerating in the same way with the adjacent minimum points on both sides intersects. Then, connect the curves between the adjacent minimum points at all intersections to obtain the acceleration constraint curve of the left wheel under the acceleration constraint.

FIG5G schematically shows that taking the minimum point (L ₁ , v ₁ ) at L _L =L ₁ as an example, the wheel is accelerated from the minimum point to the adjacent maximum point on the left (i.e., the rear) at the limit wheel acceleration a to L _L =L ₂ at the minimum distance dL _L , then:

Wherein, _v2 represents the wheel speed of the first driving wheel accelerated to the minimum distance L _L = L ₂ , and p represents the slope of the wheel speed of the first driving wheel increasing from point (L ₁ , v ₁ ) to point (L ₂ , v ₂ ) at the limit wheel acceleration a. When dL _L approaches 0,

Thus we can get:

Accordingly, the fourth constraint is set for points on path 2 that are behind the nearest minimum point:

It should be understood that, in this article, “front” and “back” refer to the direction of movement of the mobile robot 1 on the path 2 .

FIG5H schematically shows that the minimum point ( _L1 , _v1 ) at L _L = L ₁ is accelerated toward the adjacent maximum point on the right (i.e., in front) at the limit wheel acceleration a to L _L = L ₂ at the minimum distance dL _L , then:

Wherein, _v2 represents the wheel speed of the first driving wheel accelerated to the minimum distance L _L = L ₂ , and p represents the slope of the wheel speed of the first driving wheel increasing from point (L ₁ , v ₁ ) to point (L ₂ , v ₂ ) at the limit wheel acceleration a. When dL _L approaches 0,

Thus we can get:

Accordingly, the fourth constraint is set for the points on path 2 that are ahead of the nearest minimum point:

Likewise, for the second maximum speed v _Rmax of the second driving wheel, the fourth constraint may be set similarly.

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 satisfies: if the arbitrary point is behind the minimum point closest to the arbitrary point, then:

和/或

如果所述任意点在距离所述任意点最近的极小值点的前方，则：

and/or

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

和/或

其中，L_L和L_R分别表示第一驱动轮和第二驱动轮运动到所述任意点处的运动距离，v₁相应地表示距离所述任意点最近的极小值点的第一初步最大速度或第二初步最大速度，L_L1和L_R1分别表示第一驱动轮和第二驱动轮运动到所述最近的极小值点处的运动距离。

and/or

Among them, _LL and _LR respectively represent the movement distances of the first driving wheel and the second driving wheel to the arbitrary point, _v1 correspondingly represents the first preliminary maximum speed or the second preliminary maximum speed of the minimum point closest to the arbitrary point, and _LL1 and _LR1 respectively represent the movement distances of the first driving wheel and the second driving wheel to the nearest minimum point.

Fig. 5I schematically shows the first maximum speed v _Lmax and the second maximum speed v _Rmax that further satisfy the fourth constraint based on Fig. 5F. In other words, the first maximum speed v _Lmax and the second maximum speed v _Rmax shown in Fig. 5I satisfy the first constraint, the second constraint, the third constraint and the fourth constraint at the same time.

Another aspect of the present invention provides a multi-robot trajectory planning method, which can be executed independently of the planning method described above, and can also be preferably executed in combination with the planning method described above.

FIG6 schematically shows a multi-robot trajectory planning method according to an exemplary embodiment of the present invention.

As shown in FIG6 , the multi-robot trajectory planning method includes at least the following steps: a preliminary planning step S21, wherein a plurality of planning trajectories including time information corresponding to a plurality of mobile robots 1 are obtained, wherein the plurality of planning trajectories are planning trajectories generated by respectively performing the time optimal trajectory planning method on the plurality of mobile robots 1; a conflict identification step S22, wherein a conflict point in the space and time dimensions between two planning trajectories among the plurality of planning trajectories is identified, wherein the conflict point indicates that the mobile robots 1 moving along the two planning trajectories will reach the same position at the same time; a conflict resolution step S23, wherein the conflict is resolved by adjusting the time information of one of the two planning trajectories. Thus, when the plurality of mobile robots 1 are working in the same working environment, the plurality of mobile robots 1 can reach their respective destinations as a whole in the shortest possible time without collision between the plurality of mobile robots 1.

This method can divide the multi-robot trajectory planning method into two layers: time-optimal global trajectory planning and time-adjusted (or speed-adjusted) local trajectory planning. In the time-optimal global trajectory planning, a global path for each mobile robot 1 that does not contain time information is planned according to a certain global path planning algorithm, and then the speed of each mobile robot 1 is planned in a way that the mobile robot 1 moves at its maximum motion capacity (maximum speed, maximum acceleration, maximum jerk) to obtain a planned trajectory containing time information. In the time-adjusted local trajectory planning, based on the planned trajectory containing time information obtained by the time-optimal global trajectory planning of the previous layer, the movement time of each planned trajectory is adjusted (that is, the movement speed is adjusted), so that there are no conflict points between the planned trajectories. The multiple planned trajectories obtained in this way can ensure that there is no collision between the multiple mobile robots 1, and enable the multiple mobile robots 1 as a whole to reach their respective destinations in the shortest possible time.

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 article, obtaining the multiple planned trajectories includes both obtaining the existing multiple planned trajectories by receiving data or reading data, and also includes performing trajectory planning for the multiple mobile robots through a trajectory planning method to obtain the corresponding multiple planned trajectories.

In an exemplary embodiment, the plurality of planned trajectories are planned trajectories generated by the trajectory planning method described above.

As shown in FIG. 6 , the conflict identification step S22 and the conflict resolution step S23 may be repeatedly performed until no conflict point exists between any two planned trajectories among the plurality of planned trajectories.

Specifically, in the conflict identification step S22, all intersection points of each two planned trajectories in the spatial dimension may be first found. Then, for each intersection point, the time interval between the time information of the relevant planned trajectories at the intersection point is checked. If the time interval is less than a predetermined time interval threshold, the corresponding intersection point is identified as a conflict point. The intersection point represents the point where the paths of the multiple planned trajectories intersect, that is, the spatial position passed by at least two planned trajectories.

The conflict resolution step S23 may include, for example, the following sub-steps: S231: selecting the conflict point to be resolved and the adjusted planning trajectory from the identified conflict points and the conflicting planning trajectories, wherein the adjusted planning trajectory is one of the two planning trajectories associated with the conflict point to be resolved or the conflict point to be resolved is one of the conflict points with the adjusted planning trajectory; sub-step S232: adjusting the time information of the adjusted planning trajectory at the conflict point to be resolved by delaying the time information of the adjusted planning trajectory at the conflict point, so that the time interval between the time information of the two associated planning trajectories at the conflict point is greater than or equal to the 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 part of the adjusted planning trajectory after the conflict point. Thus, the conflict can be resolved with less adjustment.

The exemplary embodiment according to the present invention is further described below in conjunction with FIG. 7 and FIG. 8. FIG. 7 schematically shows five paths corresponding to five mobile robots 1. The curves numbered 1-5 correspond to the 1st to 5th paths of the 1st to 5th mobile robots 1. The intersection or overlap of the paths represents the intersection points of the trajectories of the corresponding mobile robots 1 in the spatial dimension. As can be seen from FIG. 7, there are five intersection points between the first planned trajectory for the first mobile robot 1 and the other planned trajectories, which are the intersection points between the first planned trajectory and the second, fifth, third, fourth and fifth planned trajectories in sequence. Obviously, the time information of the corresponding planned trajectories is not shown in FIG. 7.

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

FIG8 schematically shows the intersection points and conflict points in an exemplary embodiment of the present invention. In FIG8 , for the sake of intuitiveness, each planned trajectory is schematically illustrated in an abstract manner as a transverse axis. Each transverse axis corresponds to both a time scale and a distance moved by the mobile robot 1. The point on each transverse axis represents the position to which the mobile robot 1 will move at the corresponding time according to the global trajectory planning that is optimal in time. In FIG8 , the intersection points are marked on the transverse axis of each mobile robot 1 in the form of rectangular grids, wherein the transverse axes numbered 1-5 correspond to the planned trajectories of the 1st to 5th mobile robots 1. The numbers of the mobile robots 1 that intersect are listed in each rectangular grid. For example, the intersection points between the 1st planned trajectory and the 2nd, 5th, 3rd, 4th and 5th planned trajectories are marked as "1-2", "1-5", "1-3", "1-4" and "1-5" respectively. The position of the rectangular grid on the horizontal axis indicates the time period during which the planned trajectory shown by the horizontal axis continuously moves at the intersection point shown by the rectangular grid, and the width of the rectangular grid along the horizontal axis indicates the length of time during which the planned trajectory continuously moves at the intersection point. For example, the time during which the 4th planned trajectory continuously moves at the intersection point "1-4" is less than the time during which it continuously moves at the intersection point "4-5".

Then, for each intersection point, the time interval between the time information of the relevant planned trajectory at the intersection point can be checked to determine whether the intersection point is a conflict point. If the time interval is less than the 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 reasons, the predetermined time interval threshold can also be set to be greater than 0. The conflict points identified from the intersection points are marked on the bottom horizontal axis in Figure 8. Taking the intersection point "1-2" as an example, the interval between the time period in which the first planned trajectory and the second planned trajectory continue to move at the intersection point "1-2" is less than 0, that is, the two time periods overlap. Therefore, the intersection point "1-2" is a conflict point. Taking the intersection point "2-5" as an example, the interval between the time period in which the second planned trajectory and the fifth planned trajectory continue to move at the intersection point "2-5" is greater than 0, that is, the two time periods are completely staggered. Therefore, the intersection point "2-5" is not a conflict point.

The intersection points between each planned trajectory can be represented in the form of a matrix. For example, the intersection points between the i-th trajectory and other trajectories can be represented by the following matrix:

X _i = [T ₁ … T _j … T _n ], i = 1, 2, …, n

Where _Tj represents the intersection point between the i-th planned trajectory and the j-th planned trajectory, and n represents the number of planned trajectories. In general, _Tj can be expressed in the following form:

和

分别表示第i条规划轨迹进入和离开其与第j条规划轨迹之间的第m+1个交集点的时间。当第i条规划轨迹与第j条规划轨迹之间不存在交集点时，或者j＝i时，规定T_j＝0。Where m≥0,

and

They represent the time when the ith planned trajectory enters and leaves the m+1th intersection point between it and the jth planned trajectory. When there is no intersection point between the ith planned trajectory and the jth planned trajectory, or when j=i, T _j =0.

For the convenience of representation, the shortest planned trajectory movement time is taken as normalized time 1, and other planned trajectories are converted in proportion to the length of the movement time. The intersection point between the above five planned trajectories can be expressed as follows:

The subscripts 0 and 1 of the numbers in the above matrix are used to mark 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 the time information of the first and second intersection points between the i-th drawn trajectory and the j-th planned trajectory, respectively. The same can be applied to the case where there are more intersection points between the i-th drawn trajectory and the j-th planned trajectory. In the case where there is only one intersection point between the i-th drawn trajectory and the j-th planned trajectory, the superscript of the number is omitted.

For example, the traversal method can be used to search for conflict points from the intersection points. Starting from the intersection point _X1 of the first planned trajectory, _T2 , _T3 , _T4 , and _T5 of X1 are compared with _T1 of _X2 , _X3 , _X4 , and _X5 , and the intersection points with time overlap are marked. Then _{, T1} , _T3 , _T4 , and _T5 of the intersection point _X2 of the second planned trajectory are compared with _T2 of _X1 , _X3 , _X4 , and _X5 , and the intersection points with time overlap are marked. This cycle is repeated until all planned trajectories are traversed.

After traversing, five conflict points between the five planned trajectories can be found: "1-2", "3-5", "1-3", "3-4" and "4-5". Each conflict point is marked in bold in the above matrix. In Figure 8, these five conflict points are shown in the form of rectangular grids along the axis of the last horizontal line.

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

In an exemplary embodiment, the conflict point to be resolved and the adjusted planned trajectory are selected according to the priority (or importance) of the task corresponding to the planned trajectory. If the tasks performed by different mobile robots 1 have different priorities, when there is a conflict point, the planned trajectory with the high priority task can be fixed first, and the conflict point of the fixed planned trajectory can be resolved by adjusting the planned trajectory that conflicts with the fixed planned trajectory in turn.

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

Take the five planning trajectories shown in Figure 8 as an example. These five planning trajectories all have conflict points, and they are sorted in order from high to low according to the priority of their corresponding tasks. If the priorities of the tasks corresponding to these five planning trajectories are sorted from high to low as follows: 1>2>3>4>5, then fix the first planning trajectory first. The conflict points "1-2" and "1-3" of the first planning trajectory are determined one by one as the conflict points to be resolved, and the corresponding planning trajectories to be adjusted are the second planning trajectory and the third planning trajectory.

Here, the second planned trajectory is adjusted first to resolve the conflict point "1-2" between the second planned trajectory and the first planned trajectory. It should be understood that the third planned trajectory may also be adjusted first to resolve the conflict point "1-3" between the third planned trajectory and the first planned trajectory.

As described above, the conflict point can be resolved by delaying the time information of the second planned trajectory at the conflict point "1-2" and the time information of the part of the second planned trajectory after the conflict point "1-2" by an equal amount. The amount of time delayed is the time point when the other party of the conflict, the first planned trajectory, enters the conflict point "1-2" minus the time point when the adjusted planned trajectory (i.e., the second planned trajectory) leaves the conflict point "1-2" to be resolved plus a predetermined time interval threshold, which is 0.3-0.275+0=0.025 in this exemplary embodiment.

Therefore, the time when the second planned trajectory enters and leaves each intersection point will change as follows:

FIG9 schematically shows the intersection point and conflict point after the conflict point "1-2" is resolved. As shown in FIG9, the time information of the second planned trajectory at and after the conflict point "1-2" will be delayed as a whole, and the total time of the second planned trajectory will be extended accordingly. The other trajectories remain unchanged.

Then, the third planning trajectory is adjusted to resolve the conflict point “1-3” between the third planning trajectory and the first planning trajectory.

Here, the time information of the third planned trajectory at the conflict point "1-3" and the time information of the third planned trajectory after the conflict point "1-3" are both delayed by 0.67-0.58+0=0.09. The time when the third planned trajectory enters and leaves each intersection point will be updated as:

After the conflict points "1-2" and "1-3" of the first planning trajectory are released, the conflict identification step S22 is performed again to redetermine the conflict points between the planning trajectories. As shown in the following matrix:

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 from high to low according to the priority of the corresponding tasks: 3>4>5. Then, the third planning trajectory is fixed. The fifth planning trajectory with a conflict point with the third planning trajectory is adjusted to resolve the conflict point 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. The first conflict point between the third planning trajectory and the fifth planning trajectory can be resolved in the order of the time when the conflicts occur.

After resolving the first conflict point between the third and fifth planned trajectories, update the time information of the third trajectory:

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

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

There is a conflict point between the above five planning trajectories: "4-5". The planning trajectories with conflict points are sorted from high to low according to the priority of the corresponding tasks: 4>5. Then, the fourth planning trajectory is fixed. The fifth planning trajectory that has a conflict point with the fourth planning trajectory is adjusted to resolve the conflict point between the fourth and fifth planning trajectories.

After the conflict point of the fourth planned trajectory is resolved, the conflict identification step S22 is performed again. The time information of the intersection points between the planned trajectories is as follows:

At this point, there are no more conflict points between the planned trajectories.

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

Specifically, the conflict identification step S22 and the conflict resolution step S23 are performed in the following manner: first, all conflict points between the multiple planning trajectories are identified; the planning trajectories with conflict points are sorted in order from least to most according to the number of their conflict points; the planning trajectory with the highest sorting is selected as the fixed planning trajectory, and the conflict points of the fixed planning trajectory are determined as the conflict points to be adjusted one by one, and the planning trajectory that conflicts with the fixed planning trajectory at the conflict points to be adjusted is correspondingly determined as the adjusted planning trajectory 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. When there are multiple planning trajectories with the same and least number of conflict points, the adjusted planning trajectory can be selected according to the time sequence of entering the conflict point.

Take the five planning trajectories shown in Figure 8 as an example. There are five conflict points between these planning trajectories: "1-2", "3-5", "1-3", "3-4" and "4-5". These five planning trajectories all have conflict points. Sort them in order from the least to the most in terms of the number of their conflict points: 2<1=4=5<3. Fix the second planning trajectory first. The fixed second planning trajectory has only one conflict point with the first planning trajectory. Therefore, adjust the first planning trajectory to resolve the conflict point "1-2" between the first planning trajectory and the second planning trajectory. The amount of time delayed is 0.325-0.25+0=0.075.

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

Executing the conflict identification step S22 again, it can be identified that there are 4 conflict points between these 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 from the least to the most in terms of the number of their conflict points: 1<3=4<5. First fix the 1st planning trajectory. Then, adjust the 5th planning trajectory to resolve 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.

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

Executing the conflict identification step S22 again, it can be identified that there are three conflict points between these planned trajectories: "3-4", "3-5" and "4-5". The 3rd, 4th and 5th planned trajectories with conflict points are sorted in order from the least to the most in terms of the number of their conflict points: 3=4=5. At this time, there are 3 planned trajectories with the same and least number of conflict points. The adjusted planned trajectory can be selected based on the time sequence of entering the conflict points. For example, the planned trajectories with the same and least number of conflict points are sorted according to the time sequence of entering the conflict points: 3<5<4. Therefore, the 3rd planned trajectory can be fixed first. Then, the conflict points "3-4" and "3-5" of the 3rd planned trajectory are checked one by one.

First, the 5th planned trajectory is adjusted to resolve the conflict point "3-5" between the 5th planned trajectory and the 3rd planned trajectory. The amount of time delayed is 0.0425.

Update the time information of the fifth trajectory and get:

Then, the fourth planned trajectory is adjusted to resolve the conflict point "3-4" between the fourth planned trajectory and the third planned trajectory. The amount of time delayed is 0.01.

After the conflict point of the third planned trajectory is resolved, the time information of the intersection points between the planned trajectories is as follows:

Executing the conflict identification step S22 again, it can be identified that there is a conflict point "4-5" between these planned trajectories. The 4th and 5th planned trajectories with the same and least number of conflict points are sorted according to the time sequence of entering the conflict point: 4<5. Therefore, the 4th planned trajectory can be fixed first. Then, the 5th planned trajectory is adjusted to resolve the conflict point "4-5" between the 5th planned trajectory and the 4th planned trajectory. The amount of time delayed is 0.0425.

After the conflict point of the fourth planned trajectory is resolved, the time information of the intersection points between the planned trajectories is as follows:

At this point, there are no more conflict points between the planned trajectories.

In another exemplary embodiment according to the present invention, the conflict points to be resolved and the planned trajectories to be adjusted are selected in order of the conflict durations of the conflict points of the planned trajectories from least to most, so that the planned trajectories with earlier fixed rankings are preferentially not adjusted.

Specifically, the conflict identification step S22 and the conflict resolution step S23 are performed in the following manner: first, all conflict points between the multiple planning trajectories are identified; the planning trajectories with conflict points are sorted in order from least to most according to the conflict duration of the conflict points; the planning trajectory with the highest sorting is selected as the fixed planning trajectory, and the conflict points of the fixed planning trajectory are determined as the conflict points to be adjusted one by one, and the planning trajectory that conflicts with the fixed planning trajectory at the conflict points to be adjusted is correspondingly determined as the adjusted planning trajectory 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. When there are multiple planning trajectories with the same and least number of conflict points, the adjusted planning trajectory can be selected according to the time sequence of entering the conflict points.

Taking the five planning trajectories shown in Figure 8 as an example, there are five conflict points between these planning trajectories: "1-2", "3-5", "1-3", "3-4" and "4-5". These five planning trajectories all have conflict points, and they are sorted in order from least to most in terms of the conflict duration of their conflict points: 2<1<3<4<5. Fix the second planning trajectory first. The fixed second planning trajectory has only one conflict point with the first planning trajectory. Therefore, adjust the first planning trajectory to resolve the conflict point "1-2" between the first planning trajectory and the second planning trajectory. The amount of time delayed is 0.325-0.25+0=0.075.

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

Executing the conflict identification step S22 again, it can be identified that there are 4 conflict points between these planned trajectories: "1-5", "3-4", "3-5" and "4-5". Among the 1st, 3rd, 4th and 5th planned trajectories with conflict points, the conflict duration of the conflict point of the 1st planned trajectory is the shortest. First fix the 1st planned trajectory. Then, adjust the 5th planned trajectory to resolve the conflict point "1-5" between the 5th planned trajectory and the 1st planned trajectory. The amount of time delayed is 0.65-0.625+0=0.025.

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

Executing the conflict identification step S22 again, it can be identified that there are three conflict points between these planned trajectories: "3-4", "3-5" and "4-5". The 3rd, 4th and 5th planned trajectories with conflict points are sorted in order from least to most in terms of the conflict duration of their conflict points: 3<4<5. Therefore, the 3rd planned trajectory can be fixed first. Then, the conflict points "3-4" and "3-5" are resolved one by one.

For example, the fourth planned trajectory may be adjusted first to resolve the conflict point "3-4" between the fourth planned trajectory and the third planned trajectory. The amount of time delayed is 0.01.

Then, the fifth planned trajectory is adjusted to resolve the conflict point "3-5" between the fifth planned trajectory and the third planned trajectory. The amount of time delayed is 0.0425.

After the conflict point of the third planned trajectory is resolved, the time information of the intersection points between the planned trajectories is as follows:

At this time, there is a conflict point "4-5" between the planned trajectories. Sort the 4th and 5th planned trajectories with the same and least number of conflict points according to the time sequence of entering the conflict point: 4<5. Therefore, the 4th planned trajectory can be fixed first. Then, the 5th planned trajectory is adjusted to resolve the conflict point "4-5" between the 5th planned trajectory and the 4th planned trajectory. The amount of time delayed is 0.0425.

Afterwards, the time information of the intersection points between the planned trajectories is as follows:

At this point, there are no more conflict points between the planned trajectories.

In another exemplary embodiment according to the present invention, the conflict points to be resolved and the adjusted planned trajectory are selected in the order of occurrence time of the conflict points.

Specifically, the conflict identification step S22 and the conflict resolution step S23 are performed in the following manner: first, the conflict identification step S22 is executed to identify all conflict points between the multiple planned trajectories; the conflict points are sorted in chronological order of their occurrence time, and the conflict point at the front of the sort is selected as the conflict point to be resolved, and the planned trajectory that enters the conflict point later than the two planned trajectories associated with the conflict point to be resolved is selected as the adjusted planned trajectory to resolve the conflict point to be resolved; and the conflict identification step S22 is executed again to re-identify all conflict points between the multiple planned trajectories.

Taking the five planning trajectories shown in Figure 8 as an example, there are five conflict points between these 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". Therefore, "1-2" can be determined as the conflict point to be resolved. Among the first planning trajectory and the second planning trajectory participating in the conflict point "1-2", the second planning trajectory enters the conflict point "1-2" later, so the second planning trajectory is determined as the adjusted planning trajectory. Then, the second planning trajectory is adjusted to resolve the conflict point "1-2" between the second planning trajectory and the first planning trajectory. The amount of delayed time is 0.3-0.275+0=0.025.

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

Executing the conflict identification step S22 again, it can be identified that there are 4 conflict points between these planned trajectories: "1-3", "3-4", "3-5" and "4-5". The conflict points are sorted in the order of their occurrence time: "3-5", "1-3", "3-4" and "4-5". Therefore, the fifth planned trajectory is adjusted to resolve the conflict point "3-5" between the fifth planned trajectory and the third planned trajectory. The amount of time delayed is 0.0425.

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

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 planned trajectories.

Alternatively, the conflict identification step S22 and the conflict resolution step S23 may also be performed in other ways. For example, after identifying all conflict points between the plurality of planned trajectories, the planned trajectories with conflict points are sorted in at least one of the following ways: in order of the priority of the tasks corresponding to them from high to low; in order of the number of their conflict points from small to large; in order of the conflict duration of their conflict points from small to large; in order of the time of entering the conflict point. Then, the first-ranked planned trajectory among the planned trajectories with conflict points with the first-ranked planned trajectory is selected as the adjusted planned trajectory, and the time information of the adjusted planned trajectory is adjusted to resolve the conflict points between the adjusted planned trajectory and the first-ranked planned trajectory.

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

In the present invention, the computer program product may be stored in a computer-readable storage medium. The computer-readable storage medium may include, for example, a high-speed random access memory, and may 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 memory device, or other volatile solid-state storage devices. The processor may be a central processing unit, or may be other general-purpose processors, digital signal processors, application-specific integrated circuits, off-the-shelf programmable gate arrays or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or may be any conventional processor, etc.

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

Claims (13)

1. A trajectory planning method for a mobile robot (1), wherein the mobile robot (1) is speed-planned according to a determined path (2) to determine a planned trajectory comprising time information enabling the mobile robot (1) to move along the path (2), the trajectory planning method comprising:

determining one of at least two driving wheels of the mobile robot (1) as a constrained wheel such that the other driving wheel moving in coordination with the constrained wheel satisfies the kinematic and dynamic constraints as long as the constrained wheel satisfies the kinematic and dynamic constraints;

-speed planning the constrained wheel on the basis of said path (2) while satisfying the kinematic and kinetic constraints of the constrained wheel, to determine the speed of the constrained wheel;

the other driving wheels than the constrained wheel are speed programmed in a manner that matches the determined speed of the constrained wheel.

2. The trajectory planning method of claim 1, wherein,

in the speed planning of the constrained wheel, the speed of the constrained wheel is determined such that the constrained wheel has one of a maximum speed and a maximum acceleration at any point that satisfies its kinematic and kinetic constraints and satisfies the constraints of the path (2).

3. The trajectory planning method of claim 2, wherein,

in the process of speed planning of the constrained wheel, a T-shaped planning method is adopted.

4. The trajectory planning method of claim 2, wherein,

kinematic and kinetic constraints include:

the magnitude of the speed of the drive wheel is below a predetermined limit wheel speed for the drive wheel;

the magnitude of the acceleration of the drive wheel is below a predetermined limit wheel acceleration for the drive wheel.

5. The trajectory planning method of claim 4, wherein,

-speed planning the path (2) in sections, -performing the following steps for at least one section of the path (2) respectively:

for a first control point that is a starting point of the segment, determining one of the at least two drive wheels as a constrained wheel in the segment according to a path shape of the segment, a motion state of each drive wheel at the first control point, and kinematic and dynamic constraints of the drive wheels, the constrained wheel being: in the section, driving wheels that preferentially reach a limit value of kinematic or dynamic constraint according to the path shape of the section and the motion state of each driving wheel at a first control point;

Planning the speed of the constrained wheel to determine the speed of the constrained wheel within the section;

the speed of the other drive wheels within the segment is determined in coordination with the determined speed of the constrained wheel.

6. The trajectory planning method of claim 5, wherein,

the mobile robot (1) is a double differential wheel robot, the at least two driving wheels are a first driving wheel and a second driving wheel which are symmetrically arranged, wherein the first driving wheel and the second driving wheel are constrained by the same kinematics and dynamics.

7. The trajectory planning method of claim 6, wherein,

the constrained wheels in each section are determined in the following manner:

acquiring a first initial velocity v of the first driving wheel and the second driving wheel at a first control point _L0 And a second initial velocity v _R0 ；

Determining a value k1 of a speed ratio k determined by the path (2) at a second control point, which is the end point of the section, the speed ratio k representing the ratio of the speeds of the second driving wheel to the first driving wheel;

determining a first maximum speed v of the first and second drive wheels, respectively, at the second control point _Lmax And a second maximum velocity v _Rmax The first and second maximum speeds respectively represent the maximum speeds that satisfy the kinematic and kinetic constraints of the respective driving wheels and satisfy the constraints of the path (2) irrespective of the speeds of the first and second driving wheels before reaching the second control point;

A first initial speed v of the first driving wheel from a first control point _L0 The speed obtained by starting acceleration to the second control point at the limit wheel acceleration of the first drive wheel is determined as the first acceleration final speed v _La A second initial speed v of the second driving wheel from the first control point _R0 The speed obtained by starting acceleration to the second control point at the limit wheel acceleration of the second drive wheel is determined as the second acceleration final speed v _Ra ；

The first maximum speed v at the second control point _Lmax With the first accelerating final velocity v _La The smaller of (a) is determined as a first final speed v _L A second maximum speed v at a second control point _Rmax And a second acceleration final velocity v _Ra The smaller of (a) is determined as the second final speed v _R ；

Second final speed v _R And a first final velocity v _L The ratio is compared with a speed ratio k1 at the second control point and the constrained wheel in the section is determined from the comparison.

8. The trajectory planning method of claim 7, wherein,

if the second final speed v _R And a first final velocity v _L The ratio is greater than the speed ratio k1 at the second control point, determining the first drive wheel as the constrained wheel in the section;

if the second final speed v _R And a first final velocity v _L Determining the second drive wheel as a constrained wheel in the section if the ratio is less than the speed ratio k1 at the second control point;

If the second final speed v _R And a first final velocity v _L The ratio is equal to the speed ratio k1 at the second control point, then one of the first and second drive wheels is determined to be the constrained wheel in the section.

9. The trajectory planning method according to claim 7 or 8, wherein,

the movement duration corresponding to each section is equal to a preset control period t, and the first acceleration final speed v _La And a second acceleration final speed v _Ra The determination is made according to the following equation:

v _La ＝v _L0 +a*t

v _Ra ＝v _R0 +a*t

where a represents the limit wheel acceleration of the first and second drive wheels.

10. A trajectory planning method according to any one of claims 7 to 9, wherein,

a first maximum speed v of the first and second drive wheels at any point on the path (2) _Lmax And a second maximum velocity v _Rmax Determined according to at least one of the following constraints:

based on the limit wheel speed v _lim Is a first constraint of (a): v _Lmax ≤v _lim ，

Based on the limit wheel speed v _lim And a second constraint of the speed ratio determined by path (2): v _lmax ≤v _lim /k，

-a third constraint based on the limit wheel acceleration a and the rate of change k' of the speed ratio determined by the path (2):

wherein k' noteq0; and is also provided with

A first maximum speed v of the first and second drive wheels at any point on the path (2) _Lmax And a second maximum velocity v _Rmax The method meets the following conditions: v _Rmax ＝v _Lmax *k。

11. The trajectory planning method of claim 10, wherein,

a first maximum speed v of the first and second drive wheels at any point on the path (2) _Lmax And a second maximum velocity v _Rmax Additionally determined according to the fourth constraint:

determining that the first drive wheel is assumed to move along the path (2) at a maximum speed determined by the at least one of the first constraint, the second constraint and the third constraint resulting in a movement distance L with the first drive wheel _L Varying a first preliminary maximum speed and a distance of movement L with the second drive wheel _R At least one of the second preliminary maximum speeds of variation;

determining maximum and minimum points of the at least one of the first and second preliminary maximum speeds;

a first maximum velocity v at said arbitrary point _Lmax And/or a second preliminary maximum velocity v _Rmax The method meets the following conditions:

if the arbitrary point is behind the minimum point closest to the arbitrary point, then:

and/or +.>

If the arbitrary point is in front of the minimum point nearest to the arbitrary point, then:

and/or +.>

Wherein L is _L And L _R Representing the movement distance, v, of the first and second drive wheels to said arbitrary point, respectively ₁ Representing distance accordinglyA first preliminary maximum speed or a second preliminary maximum speed of a minimum value point nearest to the arbitrary point, L _L1 And L _R1 Representing the movement distance of the first and second drive wheels to the nearest minimum point, respectively.

12. The trajectory planning method according to any one of claims 1 to 10, wherein,

the path (2) is a global path determined by global path planning from 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 bezier curve of 3 rd order or more.

13. A computer program product comprising computer program instructions, wherein the computer program instructions, when executed by one or more processors, are capable of performing the trajectory planning method of any one of claims 1-12.

Images