CN116257042A

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

Info

Publication number: CN116257042A
Application number: CN202111497327.0A
Authority: CN
Inventors: 赵安; 邸兴超; 赵雨辰; 成鹏; 武文博
Original assignee: Lingdong Technology Beijing Co Ltd
Current assignee: Lingdong Technology Beijing Co Ltd
Priority date: 2021-12-09
Filing date: 2021-12-09
Publication date: 2023-06-13
Also published as: WO2023103554A1

Abstract

The track planning method for the mobile robot performs speed planning on the mobile robot according to the determined path to determine a planned track containing time information, and comprises the following steps: determining one of at least two drive wheels of the mobile robot as a constrained wheel such that the other drive wheel that moves in coordination with the constrained wheel satisfies the kinematic and dynamic constraints as long as the constrained wheel satisfies the kinematic and dynamic constraints; planning the speed of the constrained wheel based on the path while satisfying the kinematic and kinetic constraints of the constrained wheel to determine the speed of the constrained wheel; the other driving wheels are speed programmed in a manner that matches the determined speed of the constrained wheel. A corresponding computer program product is presented. By means of the invention, an alternative trajectory planning method for a mobile robot is provided, which in particular enables a trajectory meeting kinematic and kinetic constraints to be reliably planned for the mobile robot.

Description

Trajectory planning method for mobile robot and computer program product

Technical Field

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

Background

With rapid economic growth and gradual rise in human costs, mobile robots are increasingly being used in a variety of industrial and home environments. For example, automatic Guided Vehicles (AGVs), autonomous Mobile Robots (AMR), forklift, and like mobile robots are one of the key devices of modern logistics systems. The mobile robot can move and stop to a target place according to the planned path and the operation requirement so as to complete tasks such as material carrying and conveying. Trajectory planning is a key in motion control of mobile robots.

In the trajectory planning process, a speed planning for the mobile robot is required based on the determined path. In order to determine the velocity profile of a mobile robot over a path and to obtain as optimal a movement trajectory as possible, for example the movement trajectory with the shortest time, it is often necessary to solve a system of differential equations. For example: in the existing algorithm, when the motion trail is planned based on the path, a constraint optimization problem with time as an optimization target can be defined, and meanwhile, the speed constraint and the acceleration constraint of the mobile robot are considered. For mobile robots with a plurality of drive wheels, the kinematic fit between the drive wheels is also taken into account. This requires both a large memory and a considerable amount of computation.

The prior art still has a number of disadvantages in path planning for mobile robots.

Disclosure of Invention

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

According to a first aspect of the present invention, there is provided a trajectory planning method for a mobile robot, wherein the mobile robot is speed-planned according to a determined path to determine a planned trajectory containing time information enabling the mobile robot to move along the path, the trajectory planning method comprising: determining one of at least two drive wheels of the mobile robot as a constrained wheel such that the other drive 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; planning the speed of the constrained wheel based on the path 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.

"time information" means information capable of characterizing the relationship between the position of the mobile robot on the path and time. Since the path is deterministic, the "time information" can also characterize the speed of the mobile robot at various locations on the path.

In one exemplary embodiment, in 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 path constraints.

In one exemplary embodiment, a T-shape planning method is employed in the process of speed planning of a constrained wheel.

In one exemplary embodiment, the kinematic and dynamic 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.

In an exemplary embodiment, the path is speed planned in sections, and the following steps are performed for at least one section of the path: 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.

In one exemplary embodiment, the mobile robot is a dual differential robot, the at least two drive wheels are a first drive wheel and a second drive wheel symmetrically arranged, wherein the first drive wheel and the second drive wheel are subject to the same kinematic and kinetic constraints.

In one exemplary embodiment, 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 at a second control point which is an end point of the section, the speed ratio k representing a ratio of 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 maximum speeds that satisfy the kinematic and kinetic constraints of the respective drive wheels and satisfy the constraints of the path irrespective of the speeds of the first and second drive 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 firstAcceleration of 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 The method comprises the steps of carrying out a first treatment on the surface of the And

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.

In an exemplary embodiment, 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.

In an exemplary embodiment, the respective segment corresponds to a movement duration equal to a predetermined control period t, a 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.

In one exemplary embodiment, the first maximum speed v of the first and second drive wheels at any point on the path _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 speed ratio determined by the path: 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:

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 _Lmax And a second maximum velocity v _Rmax The method meets the following conditions: v _Rmax ＝v _Lmax *k。

In one exemplary embodiment, the first maximum speed v of the first and second drive wheels at any point on the path _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 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 ₁ Respectively representing a 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.

In one exemplary embodiment, the path is a global path determined by global path planning from at least one task point of the mobile robot, the at least one task point being located on the global path; and/or the path is in the form of a bezier curve of order 3 or more.

According to a second aspect of the present invention there is provided 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 according to the present invention.

The invention has the positive effects that: an alternative trajectory planning method is provided which is particularly capable of reliably planning trajectories for mobile robots that meet kinematic and kinetic constraints.

Drawings

The principles, features and advantages of the present invention may be better understood by describing the present invention in more detail with reference to the drawings. The drawings include:

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

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

FIG. 3 schematically illustrates a flow chart for speed planning of a path in segments according to an example embodiment;

FIG. 4A schematically illustrates a radius of curvature and a curve of curvature over a path in an exemplary embodiment in accordance with the present invention;

FIG. 4B schematically illustrates a speed ratio on a path and a variation of a first maximum speed and a second maximum speed satisfying a first constraint and a second constraint in an exemplary embodiment according to the present invention;

fig. 4C and 4D schematically illustrate movement speeds and movement distances of the mobile robot corresponding to the first maximum speed and the second maximum speed illustrated in fig. 4B, and first and second required accelerations required for the first and second driving wheels and time stamps of movement of the mobile robot along the path;

FIG. 4E schematically illustrates first and second required accelerations of the first and second drive wheels after re-planning and time stamps of the movement of the mobile robot along the path;

FIG. 4F schematically illustrates a speed ratio k and speed profiles of the first and second drive wheels after rescheduling;

FIG. 5A schematically illustrates a path in an exemplary embodiment in accordance with the present invention;

FIG. 5B schematically illustrates a curve of curvature over the path in the exemplary embodiment illustrated in FIG. 5A;

FIG. 5C schematically illustrates a variation of the speed ratio on the path in the exemplary embodiment shown in FIG. 5A;

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

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 illustrated in FIG. 5A;

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

fig. 5G-5H schematically show curves of a first preliminary maximum speed of the first drive wheel as a function of the movement distance of the first drive 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 illustrated in FIG. 5A;

FIG. 6 schematically illustrates a flow chart of a multi-robot trajectory planning method according to an exemplary embodiment of the invention;

Fig. 7 schematically shows 5 paths for 5 mobile robots, respectively;

FIG. 8 schematically illustrates intersection points and conflict points in an exemplary embodiment in accordance with the present invention; and

fig. 9 schematically illustrates the intersection points and the conflict points after the conflict points "1-2" are released in the exemplary embodiment illustrated in fig. 8.

Detailed Description

In order to make the technical problems, technical solutions and advantageous technical effects to be solved by the present invention more apparent, the present invention will be further described in detail with reference to the accompanying drawings and a plurality of exemplary embodiments. It should be understood that the detailed description and specific examples are intended for purposes of illustration only and are not intended to limit the scope of the invention.

The present invention is applicable to mobile robots, which may be any robot capable of autonomous spatial movement, such as AGVs, AMRs, etc. The mobile robot may be used to perform various tasks, such as for example as a warehouse robot, a sweeping robot, a home attendant robot, a greeting robot, etc.

It should be appreciated that the expressions "first", "second", etc. are used herein for descriptive purposes only and are not to be construed as indicating or implying relative importance or as implying any particular order of number of technical features indicated. Features defining "first", "second" or "first" may be expressed or implied as including at least one such feature.

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

In the embodiment shown in fig. 1, the mobile robot 1 is, for example, a differential robot, i.e., the mobile robot 1 has a differential wheel motion system including a first drive wheel (hereinafter, exemplarily described with respect to a left wheel as a first drive wheel) and a second drive wheel (hereinafter, exemplarily described with respect to a right wheel as a second drive wheel). Alternatively, the mobile robot 1 may also be another type of robot, such as a double steering wheel robot or the like. Accordingly, the mobile robot 1 may comprise a double steering wheel movement system, for example.

In the trajectory planning method, the mobile robot 1 is speed-planned according to the determined path 2 to determine a planned trajectory containing time information enabling the mobile robot 1 to move along the path 2. The path 2 may be 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.

Path 2 may be in the form of a 3 rd order or higher bezier curve and can be represented by the following equation:

wherein, the liquid crystal display device comprises a liquid crystal display device,

indicating the position of mobile robot 1, i=0, 1, …, N is > 3, < ->

Representing the coordinates of the control points of the bezier curve. When s increases from 0 to 1, the corresponding +.>

The position of the mobile robot 1 along the path 2 from the start point to the end point is shown. This is particularly advantageous for differential robots. The path 2 having a continuous second derivative can particularly advantageously adapt to the motion characteristics of the differential robot. In particular, path 2 can have a continuous curvature. This makes the change in the speed and acceleration of the mobile robot 1 more gradual. Path 2 in the form of a 4 th order bezier curve is shown in fig. 1. It should be understood that path 2 may have other shapes.

As shown in fig. 2, the trajectory planning method includes: step S11, determining one of at least two driving wheels of the mobile robot 1 as a constrained wheel, so that the other driving wheel which moves cooperatively with the constrained wheel can meet the kinematic and dynamic constraints as long as the constrained wheel meets the kinematic and dynamic constraints; step S12, planning the speed of the constrained wheel based on the path 2 under the condition that the kinematics and dynamics constraint of the constrained wheel are met, so as to determine the speed of the constrained wheel; and step S13, performing speed planning on other driving wheels except the constrained wheel in a mode of matching the determined speed of the constrained wheel.

An alternative trajectory planning method is thus provided, which in particular enables a trajectory meeting kinematic and kinetic constraints to be planned reliably for the mobile robot 1.

In the speed planning of the constrained wheel, the constrained wheel is given 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. This makes it possible to plan a time-optimal trajectory for the mobile robot 1.

Kinematic and kinetic constraints may 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. The limit wheel speed and limit wheel acceleration are limited by the construction of the mobile robot 1 itself irrespective of the limitation of the path 2. The limit wheel speeds and limit wheel accelerations are determined, for example, by motors for driving the respective drive wheels. Alternatively, the kinematic and dynamic constraints may also include that the jerk of the drive wheel is below a predetermined limit wheel jerk for said drive wheel. Alternatively, the first driving wheel and the second driving wheel of the mobile robot 1 may be symmetrically disposed so as to have the same limit wheel speed and limit wheel acceleration.

The constrained wheel may be determined as a driving wheel that reaches a limit wheel acceleration first during movement from a current control moment to a next control moment, in accordance with the path 2 of the mobile robot 1 and the movement state of the mobile robot 1 at the current moment, while satisfying kinematic and kinetic constraints and moving at a wheel speed as large as possible. Here, the driving wheel that reaches the limit wheel acceleration first means a driving wheel that will not accelerate at a greater acceleration due to the limit wheel acceleration being reached in a case where the mobile robot is expected to accelerate along a path to a maximum speed allowed by kinematic and dynamic constraints with the motion state at the current time as a starting state. For example, if the acceleration of the mobile robot is large to a certain extent, the first driving wheel has first reached its limit wheel acceleration, while the wheel acceleration of the second driving wheel is still below its limit wheel acceleration, the mobile robot will not accelerate with a larger acceleration anymore because the first driving wheel reaches the limit wheel acceleration. Thus, the first drive wheel may be determined to be a constrained wheel. If the current state of motion of the mobile robot 1 has reached the maximum speed allowed by the kinematic and dynamic constraints, which corresponds to the path, either one of the driving wheels can be considered as a constrained wheel, or it can 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 for at least one section of the path 2: 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 limit values of kinematic and dynamic constraints 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; and determining the speed of the other drive wheels within the segment in a manner that matches the determined speed of the constrained wheel. The drive wheel that preferably reaches the limit value for the kinematic and dynamic constraints here means that the drive wheel will reach the limit value for the kinematic and dynamic constraints before or simultaneously with the other drive wheels.

The movement duration corresponding to each segment may be a predetermined control period t. The control period t may be set to a very short time, for example to a time of the order of milliseconds, for example less than 10ms.

The process of speed planning the path 2 in segments is further described below in connection with fig. 3. Fig. 3 schematically shows a speed planning of the path 2 in sections according to an exemplary embodiment. In this exemplary embodiment, the start point of the path 2 is taken as the first control point, and it is determined that the speed planning is performed on the section of the path 2 corresponding to the control period from the current control point, and then the speed planning is continued with the end point of the section as the next control point until the end point of the entire path 2 is reached.

For each section, a first initial speed v of the first and second drive wheels at a first control point is first obtained _L0 And a second initial velocity v _R0 . At the start of path 2, the initial speeds of the first and second drive wheels are known. For a section other than the first section starting from the start of path 2, a first initial speed v of the first and second drive wheels at a first control point _L0 And a second initial velocity v _R0 Can be derived from the planning result of the previous section. In the description herein, the "speed" of the drive wheel (also referred to as "wheel speed") is illustrated by way of example as a linear speed. Since the size of the driving wheel is determined, the relation between the linear velocity and the angular velocity of the driving wheel is also determined.

In addition, a value k1 of a speed ratio k, which represents a ratio of speeds of the second driving wheel to the first driving wheel, determined by the path 2 at a second control point, which is an end point of the section, is determined. For differential motion systems, the speed ratio k and the radius of curvature R of path 2 satisfy:

where b represents the track width of the first and second drive wheels, 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. Thus, for a determined path 2, the speed ratio k at any point on path 2 is determined. For example, for a form in the form of a Bezier curve, the speed ratio k may be expressed as a function of the variable s: k=g(s). Accordingly, the rate of change of the speed ratio k' may also be determined: k '=g'(s). In case the speed ratio k at any point on path 2 has been determined, the speed ratio k1 at the second control point can be found by methods known in the art.

Furthermore, a first maximum speed v of the first and the second drive wheel at the second control point is determined respectively _Lmax And a second maximum velocity v _Rmax The first and second maximum speeds represent the maximum wheel speeds, respectively, 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.

Then, the first driving wheel is driven from the first initial speed v at the first control point _L0 The wheel 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 Wheel obtained by starting acceleration to the second control point at the limit wheel acceleration of the second driving wheelThe speed is determined as the second acceleration final speed v _Ra . In the case of a corresponding control period t of said segment, a first acceleration final speed v _La And a second acceleration final speed v _Ra The determination may be made according to the following equation:

v _La ＝v _L0 +a*t

v _Ra ＝v _R0 +a*t

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 And 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 。

Then, the second final velocity 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.

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

Alternatively, according to the second final speed v in the following manner _R And a first final velocity v _L The comparison of the ratio with the speed ratio k1 at the second control point determines the constrained wheel: 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 either of the first and second drive wheels may be determined to be the constrained wheel in the section.

The followingDetermining a first maximum speed v of the first and second drive wheels at any point on the path 2 is described in detail in connection with fig. 4A-4F _Lmax And a second maximum velocity v _Rmax Is a process of (2).

Fig. 4A schematically shows a variation of the radius of curvature R and the curvature k on the path 2 in an exemplary embodiment according to the invention. In this exemplary embodiment, the path 2 is a four-order Bezier curve with five Bezier curve control point coordinates of (0, -1), (0, 0), (1, 2) (4, 2), and (5, 3), respectively.

From the path 2, the curvature k at any point on the path 2 can be determined as follows:

wherein P is _x ′(s)、P _y ′(s)、P _x ″(s)、P _y "(s) are respectively

A first order abscissa and a second order abscissa. Accordingly, the radius of curvature R at any point on path 2 may be determined.

Further, the speed ratio k at any point on path 2 can be derived. The bottom of fig. 4B schematically shows the variation of the speed ratio k on path 2.

In the exemplary embodiment, according to the limit-based wheel speed v _lim Is based on the first constraint of the limit wheel speed v _lim And determining a first maximum speed v of the first and second drive wheels at any point on the path 2 from a second constraint of the speed ratio determined by the path 2 _Lmax And a second maximum velocity v _Rmax 。

The first constraint indicates that the speed of the drive wheel cannot exceed its limit wheel speed. Thus, a first maximum speed v of the first driving wheel _Lmax The constraints need to be satisfied: v _Lmax ≤v _lim . In this embodiment, the limit wheel speeds v of the first and second drive wheels _lim Is pre-arranged in advance ofSet to 1.5 (m/s).

The second constraint indicates that the speed of either one of the first and second drive wheels is such that the other drive wheel meeting the speed ratio k cannot exceed its limit wheel speed. Thus, a first maximum speed v of the first driving wheel _Lmax The constraints need to be satisfied: v _Lmax ≤v _lim /k。

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 Constraints are also satisfied: v _Rmax ＝v _Lmax *k。

Thus, a first maximum velocity v _Lmax And a second maximum velocity v _Rmax Maximum wheel speed to meet the following constraints:

can be derived from v _Lmax ＝min(v _lim ,v _lim /k),v _Rmax ＝min(v _lim ,v _lim * k) Wherein min (a, b) represents the smaller of a and b. The derived first maximum velocity v _Lmax And a second maximum velocity v _Rmax Schematically shown at the top and middle of fig. 4B.

It can be seen that in determining the first maximum velocity v _Lmax And a second maximum velocity v _Rmax The limits on the speed that the first and second drive wheels can reach at said arbitrary point are not considered for the actual or planned speed before said arbitrary point is reached.

It is assumed that the first and second drive wheels are always at the first maximum speed v _Lmax And a second maximum velocity v _Rmax Along path 2, then the corresponding mobile robot 1 moves at speed v _r And the movement distance L will be shown in fig. 4C. Accordingly, a time stamp t of the movement of the mobile robot 1 along the path 2 and a first required acceleration a required for the first and second driving wheels can be obtained _Lneed And a second required acceleration a _Rneed As shown in FIG. 4D 。

In this embodiment, the limit wheel acceleration a of the first and second driving wheels is illustratively preset to 0.5 (m/s ² ). As can be seen from fig. 4D, the second drive wheel is at s<The limit of the limit wheel acceleration a is exceeded in the s1 interval. Therefore, this needs to be re-planned, for example as follows: the re-planning starts to both sides starting from the speed minimum point in the section exceeding the limit wheel acceleration limit. In the present embodiment, the re-programming is started from s=0 to the s increasing direction. The acceleration is forcibly set to the limit wheel acceleration a as shown in fig. 4E. That is, starting at s=0, the second drive wheel accelerates at an acceleration a=0.5, and the first drive wheel moves in coordination with the second drive wheel to meet the speed ratio determined by path 2. Obviously, the first driving wheel here needs to be decelerated first (its acceleration will be below the limit wheel acceleration a) and then accelerated. Fig. 4F again shows the speed ratio k and the speed profile of the first and second drive wheels after the re-planning. As can be seen from fig. 4F, at s=s ₂ The first and second drive wheels will reach a first maximum speed v _Lmax And a second maximum velocity v _Rmax . Here, s ₂ >s ₁ . At s=s ₂ Thereafter, the first and second drive wheels may be operated at a first maximum speed v _Lmax And a second maximum velocity v _Rmax Along path 2.

In the re-planned section, the second drive wheel may be determined to be a constrained wheel and a speed plan may be performed on the second drive wheel to determine a speed of the second drive wheel. The first drive wheel is then speed programmed in a manner that matches the determined speed of the second drive wheel. The process of determining the second drive wheel as a constrained wheel may be referred to in fig. 3. In the re-planned section, v _R0 +a*t<v _Rmax Thus v _R ＝v _R0 +a×t. And v _L0 +a*t>v _Lmax Thus v _L ＝v _Lmax . Further, v _R/ v _L <k, thus the second driving wheel (i.e. the right wheel) is taken as the constraining wheel.

The above exemplary descriptionIn the procedure described, it is assumed that the first and second driving wheels of the mobile robot 1 have a first maximum speed v at the start of the path 2, respectively _Lmax And a second maximum velocity v _Rmax . In case the wheel speeds of the first and second driving wheels at the start of the path 2 are other values, the mobile robot 1 needs to go through an acceleration phase satisfying the limit of the limit wheel acceleration to reach the first maximum speed v _Lmax And a second maximum velocity v _Rmax 。

The determination of the first maximum speed v of the first and second drive wheels at any point on the path 2 in an exemplary embodiment according to the present invention is described in detail below in connection with fig. 5A-5F _Lmax And a second maximum velocity v _Rmax Is a process of (2).

Fig. 5A schematically shows path 2 in an exemplary embodiment according to the invention. In this exemplary embodiment, the path 2 satisfies the curve equation: y=sin (pi/2*x). Fig. 5B schematically shows a change curve of the curvature k on the path 2 in this exemplary embodiment. Fig. 5C schematically shows a variation of the speed ratio k on this path 2. The process of determining the curvature k and the speed ratio k on path 2 from path 2 may be described above with respect to fig. 4A-4F.

In this exemplary embodiment, similar to the embodiment shown in fig. 4A-4F, the first maximum speed v of the first and second drive wheels at any point on path 2 _Lmax And a second maximum velocity v _Rmax The first constraint and the second constraint need to be satisfied.

Fig. 5D schematically shows a first maximum velocity v satisfying a first constraint in this exemplary embodiment _Lmax And a second maximum velocity v _Rmax . The first constraint indicates that the speed of the drive wheel cannot exceed its limit wheel speed. Thus, a first maximum velocity v _Lmax And a second maximum velocity v _Rmax The constraints need to be satisfied: v _Lmax ≤v _lim ，v _Rmax ≤v _lim . In this embodiment, the limit wheel speeds v of the first and second drive wheels _lim Is preset to 1.2 (m/s).

FIG. 5E schematicallyShowing a first maximum velocity v satisfying the first constraint and the second constraint in the exemplary embodiment _Lmax And a second maximum velocity v _Rmax . The second constraint indicates that the speed of either one of the first and second drive wheels is such that the other drive wheel meeting the speed ratio k cannot exceed its limit wheel speed. Thus, a first maximum speed v of the first driving wheel _Lmax The constraints need to be satisfied: v _Lmax ≤v _lim K; second maximum speed v of second driving wheel _Rmax The constraints need to be satisfied: v _Rmax ＝v _lim *k。

Thereby, the maximum wheel speed 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)。

In this embodiment, the 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 A third constraint based on the limit wheel acceleration a and the rate of change k' of the speed ratio determined by path 2 may be additionally satisfied:

wherein k' noteq0.

The principle of the third constraint is specifically described below. As described above, after the path 2 is determined, the speed ratio k and the speed ratio change rate k' at any point on the path 2 can be determined. It is assumed that at a certain point on the path 2, the first and second driving wheels of the mobile robot 1 moving along the path 2 have a speed v, respectively _L0 And v _R0 Speed ratio k ₀ ＝v _R0 /v _L0 。

Since the wheel accelerations of the first and second driving wheels cannot exceed the limit wheel acceleration a, the mobile robot 1 follows the path 2 for a minute timeAfter the segment t moves through a path segment, the value range of the speed ratio k is as follows:

then, at this point, the speed ratio change rate k' should satisfy the following equation:

where L represents the movement distance of the mobile robot 1, and the minute displacement dL represents the displacement of the mobile robot 1 in the minute time period t. The small displacement dL being equal to the arithmetic average of the displacements of the first and second driving wheels, i.e

Thus, the above formula can be simplified as:

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

from this, it can be seen that when the rate of change k' +.0 of the speed ratio, the wheel speed of the first driving wheel needs to satisfy:

accordingly, the wheel speed of the second driving wheel needs to satisfy: />

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

FIG. 5F schematically shows a first maximum velocity v satisfying the first constraint, the second constraint, and the third constraint in this exemplary embodiment _Lmax And a second maximum velocity v _Rmax 。

Alternatively, it may also be 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 following fourth constraint is set.

The fourth constraint is described below with reference to fig. 5G-5H. First, it is determined that the first driving wheel is supposed to move along the path 2 at the maximum speed determined by the first constraint, the second constraint and the third constraint, resulting in a movement distance L with the first driving wheel _L A first preliminary maximum speed of the varying first drive wheel. In FIGS. 5G-5H, the movement distance L with the first drive wheel in an exemplary embodiment is schematically illustrated _L A curve of a first preliminary maximum speed of the varying first drive wheel. It should be appreciated that in some embodiments, the first preliminary maximum speed may also be the maximum speed movement of the first drive wheel that satisfies the first constraint and the second constraint but does not take into account the third constraint.

Then, all maximum points and minimum points of the first preliminary maximum speed may be determined. Acceleration is performed from each minimum point to adjacent maximum points on both sides (if present) with the limit wheel acceleration a until the intersection with a curve obtained by acceleration in the same manner as the adjacent minimum points on both sides. And then connecting curves among all adjacent minimum value points of the intersection points to obtain an acceleration constraint curve of the left wheel under acceleration constraint.

FIG. 5G schematically shows the form of L _L ＝L ₁ Minimum point at (L ₁ ，v ₁ ) For example, acceleration is performed from this minimum point to the phase at an adjacent maximum point to the left (i.e., rearward) of the limiting wheel acceleration aFrom a minimum distance dL _L L of (2) _L ＝L ₂ And (3) if so, then:

wherein v is ₂ Indicating acceleration to a very small distance L _L ＝L ₂ The wheel speed of the first drive wheel at which p represents the wheel speed of the first drive wheel at the point (L ₁ ，v ₁ ) At the limit wheel acceleration a to the point (L ₂ ，v ₂ ) An increased slope. When dL is _L Is close to the point of 0 and the like,

thereby, can obtain:

accordingly, the fourth constraint is set to a point on path 2 that is behind the nearest minimum point:

it should be understood that in this context, "forward" and "rearward" are with reference to the direction of movement of the mobile robot 1 on the path 2.

FIG. 5H schematically illustrates a slave L _L ＝L ₁ Minimum point at (L ₁ ，v ₁ ) Acceleration is performed to a minimum distance dL from an adjacent maximum point to the right (i.e., forward) of the limit wheel acceleration a _L L of (2) _L ＝L ₂ And (3) if so, then:

thereby, can obtain:

accordingly, the fourth constraint is set to a point on path 2 that is forward of the nearest minimum point:

Similarly, a second maximum speed v for the second drive wheel _Rmax The fourth constraint may be similarly set.

Thus, according to a fourth constraint, a first maximum velocity v at any 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 +.>

and/or +.>

FIG. 5I schematically shows a first maximum velocity v further satisfying a fourth constraint on the basis of FIG. 5F _Lmax And a second maximum velocity v _Rmax . In other words, the first maximum velocity v shown in FIG. 5I _Lmax And a second maximum velocity v _Rmax While satisfying the first constraint, the second constraint, the third constraint, and the fourth constraint.

Another aspect of the invention proposes a multi-robot trajectory planning method that may be performed independently of, or preferably in combination with, the planning method described above.

Fig. 6 schematically illustrates a multi-robot trajectory planning method according to an exemplary embodiment of the invention.

As shown in fig. 6, the multi-robot trajectory planning method at least includes the following steps: a preliminary planning step S21, in which a plurality of planned trajectories including time information for a plurality of mobile robots 1, respectively, which are planned trajectories generated by performing a time-optimal trajectory planning method on the plurality of mobile robots 1, respectively, are acquired; a collision recognition step S22 in which a collision point in the space and time dimensions between two of the plurality of planned trajectories is recognized, the collision point indicating that the mobile robot 1 moving in accordance with the two planned trajectories will arrive at the same position at the same time; and a conflict resolution step S23, wherein the conflict is resolved by adjusting the time information of one of the two planned tracks. In this way, when the plurality of mobile robots 1 are operated in the same operating environment, the plurality of mobile robots 1 can be brought to their respective destinations in a time as short as possible without collision between the plurality of mobile robots 1.

The method can divide the multi-robot track planning method into two layers of time-optimal global track planning and time-adjusted (or speed-adjusted) local track 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 some global path planning rule, and then each mobile robot 1 is speed-planned in such a manner that the mobile robot 1 moves with its own maximum movement capability (maximum speed, maximum acceleration, maximum jerk) to obtain a planned trajectory that contains time information. In the time-adjusted local trajectory planning, the movement time (i.e., the movement speed) of each planned trajectory is adjusted based on the planned trajectory including time information obtained by the global trajectory planning with the optimal time of the previous layer, so that no conflict point exists between the planned trajectories. The provision of a plurality of planned trajectories thus ensures that no collisions occur between the plurality of mobile robots 1 and that the plurality of mobile robots 1 as a whole reach their respective destinations in as short a time as possible.

It should be understood that the conflict points are not limited to the case where the planned trajectories collide at a single point, but also include the case where there are overlapping trajectory sections between the planned trajectories (see fig. 7). In this context, acquiring the plurality of planned trajectories includes acquiring the existing plurality of planned trajectories in a manner of receiving data or reading data, and also includes performing trajectory planning for the plurality of mobile robots by a trajectory planning method to acquire a corresponding plurality of planned trajectories.

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

As shown in fig. 6, the collision recognition step S22 and the collision resolution step S23 may be repeatedly performed until no collision point exists between any two of the plurality of planned trajectories.

Specifically, in the collision recognition step S22, all the intersection points of each two of the plurality of planned trajectories in the spatial dimension may be first found. Then, for each intersection, checking the time interval between the time information of the relevant planned trajectory at the intersection, and if the time interval is smaller than a predetermined time interval threshold value, identifying the corresponding intersection as a conflict point. The intersection point represents a point at which paths of the plurality of planned trajectories intersect, i.e., a spatial position traversed by at least two planned trajectories.

The conflict resolution step S23 may include, for example: substep S231: selecting a conflict point to be relieved and an adjusted planning track from the identified conflict points and the conflict planning tracks, wherein the adjusted planning track is one of two planning tracks associated with the conflict point to be relieved or the conflict point to be relieved is one of the conflict points of the adjusted planning track; substep S232: adjusting the time information of the adjusted planned track at the conflict point to be relieved in a mode of delaying the time information of the adjusted planned track at the conflict point, so that the time interval between the time information of the two associated planned tracks at the conflict point is larger than or equal to a time interval threshold; substep S233: based on the adjusted time information of the adjusted planned trajectory at the conflict point, the time information of the portion of the adjusted planned trajectory after the conflict point is updated accordingly. Thus, the conflict can be resolved with less adjustment.

Exemplary embodiments according to the present invention are further described below in conjunction with fig. 7 and 8. Fig. 7 schematically shows 5 paths for 5 mobile robots 1 accordingly. The curves numbered 1-5 correspond to the 1 st-5 th paths of the 1 st-5 th mobile robot 1. The path intersection or overlap represents the intersection point of the corresponding mobile robot 1 trajectory in the spatial dimension. As can be seen from fig. 7, there are 5 intersection points between the 1 st planned trajectory and the other planned trajectories for the 1 st mobile robot 1, which are the intersection points between the 1 st planned trajectory and the 2 nd, 5 th, 3 rd, 4 th and 5 th planned trajectories, respectively. Obviously, the time information of the corresponding planned trajectory is not shown in fig. 7.

After finding all the intersection points, for each intersection point, the time information of the relevant planned track entering and exiting the intersection point can be determined, and whether each intersection point is a conflict point or not is determined according to the time interval between the time information of the relevant planned track at the intersection point.

Fig. 8 schematically illustrates the intersection points and the conflict points in an exemplary embodiment according to the present invention. In fig. 8, each of the planned trajectories is schematically shown in abstraction with one transverse axis for the sake of visualization. Each transverse axis corresponds to both a time scale and a distance of movement of the mobile robot 1. The points on each lateral axis represent a global trajectory plan that is optimal in terms of time, to which the mobile robot 1 will move at the corresponding time. In fig. 8, the intersection points are identified on the lateral axis of each mobile robot 1 in the form of a rectangular grid, wherein the lateral axes numbered 1-5 correspond to the planned trajectories of the 1 st-5 th mobile robot 1. The numbers of the mobile robots 1 where the intersections occur are listed in each rectangular grid. For example, the intersection points between the 1 st and 2 nd, 5 th, 3 rd, 4 th and 5 th 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 transverse axis represents the period of time that the planned trajectory shown by the transverse axis continues to move at the intersection point shown by the rectangular grid, and the width of the rectangular grid along the transverse axis represents the length of time that the planned trajectory continues to move at the intersection point. For example, the 4 th planned trajectory continues to move at intersection points "1-4" for less time than it does at intersection points "4-5".

Then, for each intersection, the time interval between the time information of the associated planned trajectory at the intersection may be checked to determine whether the intersection is a conflict. If the time interval is less than a predetermined time interval threshold, the corresponding intersection point is identified as a conflict point. The predetermined time interval threshold may be set to 0, for example. The predetermined time interval threshold may also be set to be greater than 0 for safety. The lowest transverse axis in fig. 8 identifies the conflict point from the intersection point. Taking the intersection point "1-2" as an example, the interval between the time periods during which the 1 st and 2 nd planned trajectories continuously move at the intersection point "1-2" is smaller than 0, i.e., the two time periods overlap. Thus, the intersection points "1-2" are conflict points. Taking the intersection point of 2-5 as an example, the interval between the time periods of continuous movement of the 2 nd planning track and the 5 th planning track at the intersection point of 2-5 is larger than 0, namely the two time periods are completely staggered. Thus, the intersection points "2-5" are not conflict points.

The intersection points between the planned trajectories may be represented in the form of a matrix. For example, the intersection points between the ith track and other tracks may be represented in the following matrix:

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

Wherein T is _j Represents the intersection point between the ith and jth planned trajectories, and n represents the number of planned trajectories. In general, T _j Can be expressed in the following form:

wherein, m is more than or equal to 0,

and->

The time at which the ith planned trajectory enters and leaves its mth+1th intersection with the jth planned trajectory is shown, respectively. When there is no intersection point between the ith and jth planned trajectories, or j=i, provision T _j ＝0。

For convenience of representation, the shortest planned trajectory movement time is taken as normalized time 1, and other planned trajectories are converted proportionally according to the movement time length. The intersection points between the 5 planned trajectories can be expressed as follows:

subscripts

0 and 1 of the numbers in the above matrix are used to mark the numbers as the times when the corresponding planned trajectories enter and leave the intersection point, respectively, and

subscripts

0 and 1 of the numbers are used to mark the numbers as the time information of the 1 st and 2 nd intersection points between the ith and jth planned trajectories, respectively. And so on for the case that there are more intersection points between the ith and jth planned trajectories. In the case where there is only one intersection between the ith and jth planned trajectories, the superscript of the numeral is omitted.

For example, a traversal method may be employed to search for conflict points from intersection points. From intersection point X of 1 st planned trajectory ₁ Initially, X is taken ₁ T of (2) ₂ 、T ₃ 、T ₄ 、T ₅ Respectively with X ₂ 、X ₃ 、X ₄ 、X ₅ T of (2) ₁ In comparison, intersection points with time overlap are marked. Then, the intersection point X of the 2 nd planning track ₂ T of (2) ₁ 、T ₃ 、T ₄ 、T ₅ Respectively with X ₁ 、X ₃ 、X ₄ 、X ₅ T of (2) ₂ Compared, and marked with time overlapping intersection points. And circulating in this way until all the planning tracks are traversed.

Through traversal, 5 conflict points among the 5 planning tracks can be found out: "1-2", "3-5", "1-3", "3-4" and "4-5". Each conflict point is marked in the matrix in a thickening mode. In fig. 8, these 5 conflict points are shown in the form of a rectangular grid in the axial direction of the last transverse line.

After the conflict point is found, a conflict resolution step S23 may be performed. Preferably, the time information of the adjusted planned trajectory at the conflict point and the time information of the portion of the adjusted planned trajectory after the conflict point are delayed by an equal amount in the conflict resolution step S23. Since the plurality of planned trajectories is itself a time-optimal trajectory plan, this way it is ensured that the adjusted planned trajectories still meet the kinematic and kinetic constraints of the mobile robot 1 and that the plurality of mobile robots 1 as a whole reach their respective destination in as short a time as possible without collision. Since the global trajectory planning of the upper time optimization represents the maximum motion capability of the mobile robot 1, only the time when the adjusted planned trajectory enters the conflict point to be relieved is postponed when the conflict is relieved. This backward delay will correspondingly affect all time information of the adjusted planned trajectory after the conflict point to be relieved.

In an exemplary embodiment, the conflict points to be relieved and the adjusted planned trajectories are selected according to the priority (or importance) of the tasks corresponding to the planned trajectories. If the tasks executed by different mobile robots 1 have different priorities, when a conflict point exists, the planned track with the task with high priority can be preferentially fixed, and the conflict point of the fixed planned track can be relieved by adjusting the planned track which conflicts with the fixed planned track in sequence.

Specifically, the collision recognition step S22 and the collision resolution step S23 are performed in the following manner: firstly, identifying all conflict points among the plurality of planning tracks; sequencing the planning tracks with the conflict points according to the priority of the corresponding tasks from high to low; selecting the forefront planning track as a fixed planning track, determining the conflict points of the fixed planning track as conflict points to be adjusted one by one, and correspondingly determining the planning track which conflicts with the fixed planning track at the conflict points to be adjusted as an adjusted planning track so as to release all the conflict points of the fixed planning track; then, a conflict recognition step S22 is performed again to re-recognize all conflict points between the plurality of planned trajectories.

Take 5 planned trajectories as shown in fig. 8 as an example. These 5 planned trajectories all have conflict points, ordering them in order of priority of their corresponding tasks from high to low. If the priorities of the corresponding tasks of the 5 planned tracks are ordered sequentially from high to low: 1>2>3>4>5, the 1 st planning track is fixed first. And determining the conflict points '1-2' and '1-3' of the 1 st planning track as conflict points to be relieved one by one, wherein the correspondingly adjusted planning tracks are the 2 nd planning track and the 3 rd planning track respectively.

Here, the 2 nd planned track is adjusted first to release the conflict point "1-2" between the 2 nd planned track and the 1 st planned track. It should be appreciated that the 3 rd scheduled track may also be adjusted first to eliminate the conflict points "1-3" between the 3 rd scheduled track and the 1 st scheduled track.

As described above, the conflict point may be released in such a manner that the time information of the 2 nd planning track at the conflict point "1-2" and the time information of the portion of the 2 nd planning track after the conflict point "1-2" are equally delayed. The amount of time delayed is the point in time when the other 1 st planned track of the conflict enters conflict point "1-2", minus the point in time when the adjusted planned track (i.e., 2 nd planned track) leaves conflict point "1-2" to be relieved, plus a predetermined time interval threshold, here exemplified by 0.3-0.275+0=0.025.

Thus, the time for entry and exit of the 2 nd planned trajectory from each intersection will vary as follows:

fig. 9 schematically shows the intersection points and the conflict points after the conflict points "1-2" are released. As shown in fig. 9, the time information of the 2 nd planned track at and after the conflict point "1-2" will be delayed overall, and the total time of the 2 nd planned track will be correspondingly prolonged. The other trajectories remain unchanged.

Then, the 3 rd prescribed trajectory is adjusted to release the conflict points "1-3" between the 3 rd prescribed trajectory and the 1 st prescribed trajectory.

Here, the time information of the 3 rd track at the conflict point "1-3" and the time information of the portion of the 3 rd track after the conflict point "1-3" are both equally delayed by 0.67-0.58+0=0.09. The time for entry and exit of the 3 rd planned trajectory to each intersection will be updated as:

after the conflict points "1-2" and "1-3" of the 1 st planned trajectory are released, the conflict recognition step S22 is performed again to redetermine the conflict points between the planned trajectories. The following matrix is shown:

it can be seen that at this time, there are 3 conflict points between the 5 planned tracks: "3-5", "4-5" and "3-5". The planned tracks with the conflict points are orderly sequenced from high to low according to the priorities of the corresponding tasks: 3>4>5. Then, the 3 rd planning track is fixed. And adjusting the 5 th planned track with the conflict point between the 3 rd planned track and the 5 th planned track to release the conflict point between the 3 rd planned track and the 5 th planned track. Here, two conflict points exist between the 3 rd and 5 th planned tracks, and the first conflict point between the 3 rd and 5 th planned tracks can be sequentially released according to the time sequence of occurrence of the conflict.

And updating the time information of the 3 rd track after the first conflict point between the 3 rd track and the 5 th track is released:

it can be seen that, at the same time as the first conflict point between the 3 rd and 5 th prescribed tracks is released, since the time information of the portion of the 5 th prescribed track after the conflict point is updated accordingly, the second conflict point between the 3 rd and 5 th prescribed tracks is also released.

The collision recognition step S22 is performed here, the recognition result being shown in the following matrix:

there are also 1 conflict point between the 5 planned tracks: "4-5". The planned tracks with the conflict points are orderly sequenced from high to low according to the priorities of the corresponding tasks: 4>5. Then, the 4 th planned track is fixed. And adjusting the 5 th planned track with the conflict point between the 4 th planned track and the 5 th planned track to release the conflict point between the 4 th planned track and the 5 th planned track.

After the conflict point of the 4 th planned track is released, the conflict recognition step S22 is performed again. The time information of the intersection points between the planned trajectories is as follows:

at this time, no conflict point exists between the planned trajectories.

In another exemplary embodiment according to the present invention, the conflict points to be relieved and the adjusted planned trajectories are selected in order of the number of conflict points the planned trajectories have from less to more so as to preferentially fix the earlier ordered planned trajectories without adjustment.

Specifically, the collision recognition step S22 and the collision resolution step S23 are performed in the following manner: firstly, identifying all conflict points among the plurality of planning tracks; sequencing the planned track with the conflict points according to the sequence of the number of the conflict points from the small to the high; selecting the forefront planning track as a fixed planning track, determining the conflict points of the fixed planning track as conflict points to be adjusted one by one, and correspondingly determining the planning track which conflicts with the fixed planning track at the conflict points to be adjusted as an adjusted planning track so as to release all the conflict points of the fixed planning track; then, a conflict recognition step S22 is performed again to re-recognize all conflict points between the plurality of planned trajectories. When there are as many and as few as there are conflict points for a plurality of planned trajectories, the adjusted planned trajectory may be selected according to the time sequence of entering the conflict points.

Taking the 5 planned trajectories shown in fig. 8 as an example, there are 5 conflict points between these planned trajectories: "1-2", "3-5", "1-3", "3-4" and "4-5". These 5 planned trajectories all have conflict points, which are ordered in order of their number of conflict points from a few to a many: 2<1 =4= 5<3. The 2 nd planning track is fixed first. There is only one conflict between the fixed 2 nd and 1 st planned tracks. Thus, the 1 st prescribed trajectory is adjusted to release the conflict points "1-2" between the 1 st prescribed trajectory and the 2 nd prescribed trajectory. The amount of time delayed is 0.325-0.25+0=0.075.

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

again, the conflict recognition step S22 is performed, and it can be recognized that there are 4 conflict points between these planned trajectories: "1-5", "3-4", "3-5" and "4-5". The 1 st, 3 rd, 4 th and 5 th planned tracks with the conflict points are ordered in the order of the number of the conflict points from the small to the high: 1<3 = 4<5. The 1 st planning track is fixed first. Then, the 5 th planned track is adjusted to release the conflict points "1-5" between the 5 th planned track and the 1 st planned track. The amount of time delayed is 0.65-0.625+0 = 0.025.

After the second collision resolution step S23 is performed, the time information of the intersection point between each planned trajectory is as follows:

/>

again, the conflict recognition step S22 is performed, and it can be recognized that 3 conflict points exist between the planned trajectories: "3-4", "3-5" and "4-5". The 3 rd, 4 th and 5 th planned tracks with the conflict points are ordered in the order of the number of the conflict points from the small to the high: 3=4=5. At this time, the number of the conflict points with 3 planning tracks is the same and the minimum, and the adjusted planning tracks can be selected according to the time sequence of entering the conflict points. For example, the same and minimum number of planned trajectories are ordered according to the chronological order of entry into the conflict points: 3<5<4. Therefore, the 3 rd planning track can be fixed first. Then, the conflict points "3-4" and "3-5" of the 3 rd planning track are one by one.

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

Updating the time information of the 5 th track, and obtaining:

then, the 4 th planned track is adjusted to release the conflict point "3-4" between the 4 th planned track and the 3 rd planned track. The amount of time delayed was 0.01.

After the conflict point of the 3 rd planned track is released, the time information of the intersection point between the planned tracks is as follows:

/>

again, the conflict recognition step S22 is performed, and it can be recognized that there are 1 conflict points between the planned trajectories: "4-5". Sorting the 4 th and 5 th planned tracks with the same and minimum number of the conflict points according to the time sequence of entering the conflict points: 4<5. Therefore, the 4 th planned track can be fixed first. Then, the 5 th planned track is adjusted to release the conflict point "4-5" between the 5 th planned track and the 4 th planned track. The amount of time delayed was 0.0425.

After the conflict point of the 4 th planned track is released, the time information of the intersection point between the planned tracks is as follows:

at this time, no conflict point exists between the planned trajectories.

In a further exemplary embodiment according to the present invention, the conflict points to be relieved and the adjusted planned trajectories are selected in order of the conflict duration of the conflict points that the planned trajectories have from as few as many, so that the planned trajectories with the earlier fixed order of preference are not adjusted.

Specifically, the collision recognition step S22 and the collision resolution step S23 are performed in the following manner: firstly, identifying all conflict points among the plurality of planning tracks; ordering the planned trajectories with the conflict points in order of from least to most conflict duration of the conflict points; selecting the forefront planning track as a fixed planning track, determining the conflict points of the fixed planning track as conflict points to be adjusted one by one, and correspondingly determining the planning track which conflicts with the fixed planning track at the conflict points to be adjusted as an adjusted planning track so as to release all the conflict points of the fixed planning track; then, a conflict recognition step S22 is performed again to re-recognize all conflict points between the plurality of planned trajectories. When there are as many and as few as there are conflict points for a plurality of planned trajectories, the adjusted planned trajectory may be selected according to the time sequence of entering the conflict points.

Taking the 5 planned trajectories shown in fig. 8 as an example, there are 5 conflict points between these planned trajectories: "1-2", "3-5", "1-3", "3-4" and "4-5". These 5 planned trajectories all have conflict points, which are ordered in order of their conflict duration from as few as many: 2<1<3<4<5. The 2 nd planning track is fixed first. There is only one conflict between the fixed 2 nd and 1 st planned tracks. Thus, the 1 st prescribed trajectory is adjusted to release the conflict points "1-2" between the 1 st prescribed trajectory and the 2 nd prescribed trajectory. The amount of time delayed is 0.325-0.25+0=0.075.

again, the conflict recognition step S22 is performed, and it can be recognized that there are 4 conflict points between these planned trajectories: "1-5", "3-4", "3-5" and "4-5". Of the 1 st, 3, 4, 5 th planned trajectories having the conflict points, the conflict duration of the conflict point of the 1 st planned trajectory is the shortest. The 1 st planning track is fixed first. Then, the 5 th planned track is adjusted to release the conflict points "1-5" between the 5 th planned track and the 1 st planned track. The amount of time delayed is 0.65-0.625+0 = 0.025.

/>

again, the conflict recognition step S22 is performed, and it can be recognized that 3 conflict points exist between the planned trajectories: "3-4", "3-5" and "4-5". The 3 rd, 4 th and 5 th planned tracks with the conflict points are ordered in the order of the conflict duration of the conflict points from at least to at most: 3<4<5. Therefore, the 3 rd planning track can be fixed first. Then, the conflict points "3-4" and "3-5" are released one by one.

For example, the 4 th planned track may be first adjusted to release the conflict point "3-4" between the 4 th planned track and the 3 rd planned track. The amount of time delayed was 0.01.

Then, the 5 th planned track is adjusted to release the conflict point "3-5" between the 5 th planned track and the 3 rd planned track. The amount of time delayed was 0.0425.

at this time, there are 1 conflict points between each planned trajectory: "4-5". Sorting the 4 th and 5 th planned tracks with the same and minimum number of the conflict points according to the time sequence of entering the conflict points: 4<5. Therefore, the 4 th planned track can be fixed first. Then, the 5 th planned track is adjusted to release the conflict point "4-5" between the 5 th planned track and the 4 th planned track. The amount of time delayed was 0.0425.

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

/>

at this time, no conflict point exists between the planned trajectories.

In a further exemplary embodiment according to the present invention, the conflict points to be relieved and the adjusted planned trajectory are selected in the order of the occurrence times of the conflict points.

Specifically, the collision recognition step S22 and the collision resolution step S23 are performed in the following manner: first, a conflict recognition step S22 is performed to recognize all conflict points between the plurality of planned trajectories; sorting the conflict points according to the sequence of occurrence time, selecting the conflict point with the forefront sorting as the conflict point to be released, and selecting the planning track with the later time for entering the conflict point from the two planning tracks associated with the conflict point to be released as the planning track to be adjusted so as to release the conflict point to be released of the conflict point; the conflict recognition step S22 is then performed to re-recognize all conflict points between the plurality of planned tracks.

Taking the 5 planned trajectories shown in fig. 8 as an example, there are 5 conflict points between these planned trajectories: "1-2", "3-5", "1-3", "3-4" and "4-5". The conflict points are ordered according to the sequence of the occurrence time, and the order is as follows: "1-2", "3-5", "1-3", "3-4" and "4-5". Thus, "1-2" may be determined as the point of conflict to be relieved. Of the 1 st and 2 nd planned trajectories participating in the conflict point "1-2", the 2 nd planned trajectory enters the conflict point "1-2" later, and thus, the 2 nd planned trajectory is determined as the adjusted planned trajectory. Then, the 2 nd planned track is adjusted to release the conflict point "1-2" between the 2 nd planned track and the 1 st planned track. The amount of time delayed is 0.3-0.275+0=0.025.

/>

again, the conflict recognition step S22 is performed, and it can be recognized that there are 4 conflict points between these planned trajectories: "1-3", "3-4", "3-5" and "4-5". The conflict points are ordered according to the sequence of the occurrence time, and the conflict points are: "3-5", "1-3", "3-4" and "4-5". Thus, the 5 th prescribed trajectory is adjusted to release the conflict point "3-5" between the 5 th prescribed trajectory and the 3 rd prescribed trajectory. The amount of time delayed was 0.0425.

then, the conflict recognition step S22 and the conflict resolution step S23 are repeatedly performed as described above until no conflict point exists between any two of the 5 planned trajectories.

Alternatively, the collision recognition step S22 and the collision resolution step S23 may be performed in other manners. For example, after identifying all of the conflict points between the plurality of planned trajectories, the planned trajectories with the conflict points are ordered in at least one of the following ways: according to the priority of the corresponding tasks from high to low; in order of from a small number to a large number of conflict points; in order of from least to most conflict duration of conflict points; in the order of the times at which they entered the conflict points. Then, the one of the planned tracks having the conflict points between the planned track and the planned track having the highest ranking is selected as the adjusted planned track, and the time information of the adjusted planned track is adjusted to release the conflict point between the adjusted planned track and the planned track having the highest ranking.

Furthermore, the invention relates to a computer program product comprising computer program instructions which, when executed by one or more processors, are capable of performing the trajectory planning method and/or the multi-robot trajectory planning method according to the 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, high speed random access memory, but may also include non-volatile memory, such as a hard disk, memory, a plug-in hard disk, a smart memory card, a secure digital card, a flash memory card, at least one magnetic disk storage device, a flash memory device, or other volatile solid state storage device. The processor may be a central processing unit, but also 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 or the like.

Although specific embodiments of the invention have been described in detail herein, they are presented for purposes of illustration only and are not to be construed as limiting the scope of the invention. Various substitutions, alterations, and modifications can be made without departing from the spirit and scope of the invention.

Claims

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:

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;

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 ；

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

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 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;

and/or +.>

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.