WO2023103553A1 - 多机器人轨迹规划方法及计算机程序产品 - Google Patents

多机器人轨迹规划方法及计算机程序产品 Download PDF

Info

Publication number
WO2023103553A1
WO2023103553A1 PCT/CN2022/123038 CN2022123038W WO2023103553A1 WO 2023103553 A1 WO2023103553 A1 WO 2023103553A1 CN 2022123038 W CN2022123038 W CN 2022123038W WO 2023103553 A1 WO2023103553 A1 WO 2023103553A1
Authority
WO
WIPO (PCT)
Prior art keywords
conflict
point
speed
wheel
planned
Prior art date
Application number
PCT/CN2022/123038
Other languages
English (en)
French (fr)
Inventor
赵安
边铁栋
张传发
邸兴超
赵雨辰
王鹏飞
Original Assignee
灵动科技（北京）有限公司
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 灵动科技（北京）有限公司 filed Critical 灵动科技（北京）有限公司
Publication of WO2023103553A1 publication Critical patent/WO2023103553A1/zh

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

Definitions

  • the invention relates to the field of mobile robots, in particular to the field of motion control of mobile robots, and in particular to a multi-robot trajectory planning method and a corresponding computer program product.
  • mobile robots are more and more widely used in various industrial and domestic environments.
  • mobile robots such as automated guided vehicles (AGVs), autonomous mobile robots (AMRs), and forklifts are one of the key equipment in modern logistics systems.
  • the mobile robot can move and dock to the target location according to the planned path and job requirements, so as to complete tasks such as material handling and delivery.
  • Trajectory planning is the key in the motion control of mobile robots.
  • each mobile robot performs path planning and movement according to the received tasks. This is prone to situations where the motion trajectories of mobile robots conflict with each other.
  • the obstacle avoidance function of mobile robots can be relied on to realize the avoidance between multiple mobile robots.
  • the mobile robot faces the situation that the road is blocked and cannot pass.
  • the object of the present invention is to provide an improved multi-robot trajectory planning method and a corresponding computer program product, so as to overcome at least one deficiency of the prior art.
  • a multi-robot trajectory planning method wherein the multi-robot trajectory planning method includes: a preliminary planning step S21, wherein, correspondingly used for a plurality of mobile robots containing time information A plurality of planned trajectories, the plurality of planned trajectories are planned trajectories generated by performing a time-optimal trajectory planning method on the plurality of mobile robots respectively; conflict identification step S22, wherein, identifying any of the plurality of planned trajectories A conflict point between the two planned trajectories in the space and time dimensions, the conflict point represents that the mobile robot moving according to the two planned trajectories will arrive at the same position at the same time; and the conflict resolution step S23, wherein, The conflict is resolved by adjusting the time information of one of the two planned trajectories.
  • Time information means information capable of characterizing the relationship between the position of the mobile robot on the route and time. Since the path is deterministic, "time information" can also characterize the speed of the mobile robot at various positions on the path.
  • the conflict identification step S22 and the conflict resolution step S23 are repeatedly performed until there is no conflict point between any two planned trajectories among the plurality of planned trajectories.
  • the conflict identification step S22 includes: finding all intersection points of every two planned trajectories in the plurality of planned trajectories in the spatial dimension; and for each intersection point, checking the relevant planned trajectories in The time interval between the time information at the intersection point, if the time interval is less than a predetermined time interval threshold, the corresponding intersection point is identified as a conflict point.
  • the conflict resolution step S23 includes:
  • Sub-step S231 Select the conflict point to be resolved and the adjusted planned trajectory from the identified conflict point and the conflicted planned trajectory, wherein the adjusted planned trajectory is two points associated with the conflicted point to be resolved.
  • One of the planned trajectories or the conflict point to be resolved is one of the conflict points with the adjusted planned trajectory
  • Sub-step S232 Adjust the time information of the adjusted planned trajectory at the conflict point to be resolved by delaying the time information of the adjusted planned trajectory at the conflict point, so that the two associated planned trajectories are in conflict
  • the time interval between the time information at the point is greater than or equal to the time interval threshold
  • Sub-step S233 Based on the adjusted time information of the adjusted planned trajectory at the conflict point, correspondingly update the time information of the part of the adjusted planned trajectory after the conflict point.
  • sub-step S232 and sub-step S233 are executed in the following manner: the time information of the adjusted planned trajectory at the conflict point and the time of the adjusted planned trajectory after the conflict point are delayed by an equal amount information; and/or the delayed amount of time is the time point at which another planned trajectory associated with the conflict point to be resolved enters the conflict point to be resolved minus the time point at which the adjusted planned trajectory leaves the conflict point to be resolved plus a predetermined time interval threshold.
  • the conflict identification step S22 and the conflict resolution step S23 are performed in the following manner:
  • the conflict identification step S22 and the conflict resolution step S23 are performed in the following manner:
  • the time-optimal trajectory planning method can perform speed planning on the mobile robot according to the determined path, so as to determine a planned trajectory including time information that enables the mobile robot to move along the path
  • the Trajectory planning methods include:
  • One of the at least two driving wheels of the mobile robot is determined to be a constrained wheel such that as long as the constrained wheel satisfies the kinematics and dynamics constraints, the other driving wheels moving in cooperation with the constrained wheel will satisfy the kinematics and dynamics constraints. dynamic constraints;
  • speed planning is performed on the constrained wheel while satisfying the kinematics and dynamics constraints of the constrained wheel, so as to determine the speed of the constrained wheel so that the constrained wheel has the kinematics and dynamics satisfying its kinematics and dynamics at any point one of a maximum velocity and a maximum acceleration subject to the constraints of the path and satisfying the constraints of the path;
  • the speeds of the driven wheels other than the constrained wheel are programmed in a manner that matches the determined speed of the constrained wheel.
  • a T-shaped planning method is used in the process of speed planning for the constrained wheels.
  • the kinematic and dynamic constraints include: the magnitude of the speed of the driving wheel is below a predetermined limit wheel speed for the driving wheel; the magnitude of the acceleration of the driving wheel is below the predetermined limit for the driving wheel below the wheel acceleration.
  • the speed planning is performed on the path section by section, and the following steps are respectively performed on at least one section of the path: for the first control point as the starting point of the section, according to the determining one of the at least two drive wheels as the constrained drive wheel in the segment based on the path shape of the segment, the state of motion of each drive wheel at the first control point, and the kinematic and dynamic constraints of the drive wheels
  • the constrained wheels are: 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 limit value of the kinematics or dynamics constraint is preferentially reached a drive wheel; performing speed programming on the constrained wheel to determine the constrained wheel's speed in the segment; and determining the speed of the other drive wheel in the segment in a manner compatible with the determined constrained wheel's speed .
  • the mobile robot is a robot with double differential wheels, and the at least two driving wheels are symmetrically arranged first driving wheels and second driving wheels, wherein the first driving wheels and the second driving wheels subject to the same kinematic and dynamic constraints.
  • the constrained wheels in each segment are determined in the following manner:
  • said first maximum speed and second maximum speed represent respectively without considering the first the maximum speed of the drive wheel and the second drive wheel satisfying the kinematic and dynamic constraints of each drive wheel and satisfying the constraints of said path without reaching the speed before 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 final acceleration speed v La
  • the first The speed obtained by accelerating the two driving wheels 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 final acceleration speed v Ra ;
  • the smaller of the first maximum speed v Lmax at the second control point and the first acceleration final speed v La is determined as the first final speed v L
  • the second maximum speed v Rmax at the second control point and the first final speed v La are determined as the first final speed v L
  • the smaller of the two acceleration final speeds v Ra is determined as the second final speed v R ;
  • the ratio of the second final speed vR to the first final speed vL is compared with the speed ratio k1 at the second control point, and the constrained wheels in the segment are determined from the result of the comparison.
  • the first drive wheel is determined to be constrained in the segment if the ratio of the second final speed v R to the first final speed v L is greater than the speed ratio k1 at the second control point wheel; if the ratio of the second final speed v R to the first final speed v L is less than the speed ratio k1 at the second control point, then determine the second driving wheel as the constrained wheel in the section; if the second final The ratio of the speed v R to the first final speed v L is equal to the speed ratio k1 at the second control point, then one of the first and second driving wheels is determined to be the constrained wheel in the segment.
  • the motion duration corresponding to each section is equal to a predetermined control period t, and the first final acceleration velocity v La and the second final acceleration velocity v Ra are determined according to the following formula:
  • a represents the limit wheel acceleration of the first driving wheel and the second driving wheel.
  • 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 maximum speed vLmax and the second maximum speed vRmax 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:
  • the first maximum velocity v Lmax and/or the second preliminary maximum velocity v Rmax at said arbitrary point satisfy:
  • LL and LR respectively represent the movement distances of the first driving wheel and the second driving wheel to the arbitrary point
  • v 1 correspondingly represents the first preliminary maximum value of the minimum value point closest to the arbitrary point
  • Velocities or second preliminary maximum velocities, L L1 and LR1 respectively represent the movement distances of the first drive wheel and the second drive wheel to the nearest minimum point.
  • the path is a global path determined by performing global path planning according to at least one mission point of the mobile robot, and the at least one mission point is located on the global path; and/or the path It is in the form of a Bezier curve of degree 3 or higher.
  • a computer program product comprising computer program instructions, wherein, when said computer program instructions are executed by one or more processors, said processors are capable of performing Invented multi-robot trajectory planning method.
  • the positive effect of the present invention is: when a plurality of mobile robots are working in the same working environment, under the condition that no collision occurs between the plurality of mobile robots, the plurality of mobile robots can be integrated in the shortest possible distance. time to reach their respective destinations.
  • Fig. 1 schematically shows a mobile robot and its path implementing a trajectory planning method according to an exemplary embodiment of the present invention
  • Fig. 2 schematically shows a flow chart of a trajectory planning method for a mobile robot according to an exemplary embodiment of the present invention
  • Fig. 3 schematically shows a flow chart of performing speed planning on a path segment by segment according to an exemplary embodiment
  • Fig. 4A schematically shows the change curve of the radius of curvature and the curvature on the path in an exemplary embodiment according to the present invention
  • Fig. 4B schematically shows the change curves of the speed ratio on the path and the first maximum speed and the second maximum speed satisfying the first constraint and the second constraint in an exemplary embodiment according to the present invention
  • Figure 4C and Figure 4D schematically show the moving speed and moving distance of the mobile robot corresponding to the first maximum speed and the second maximum speed shown in Figure 4B and the required first driving wheel and the second driving wheel A required acceleration and a second required acceleration and the time stamp of the mobile robot moving along the path;
  • FIG. 4E schematically shows the first required acceleration and the second required acceleration required by the replanned first driving wheel and the second driving wheel and the time stamp of the mobile robot moving along the path;
  • Fig. 4F schematically shows the speed ratio and the speed change curves of the first driving wheel and the second driving wheel after re-planning
  • Figure 5A schematically shows a path according to an exemplary embodiment of the present invention
  • Fig. 5B schematically shows the variation curve of the curvature on the path in the exemplary embodiment shown in Fig. 5A;
  • Fig. 5C schematically shows the change curve of the speed ratio on the path in the exemplary embodiment shown in Fig. 5A;
  • FIG. 5D schematically shows a first maximum speed and a second maximum speed satisfying the first constraint in the exemplary embodiment shown in FIG. 5A;
  • FIG. 5E schematically shows 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 shows a first maximum speed and a second maximum speed satisfying the first constraint, the second constraint and the third constraint in the exemplary embodiment shown in Fig. 5A;
  • 5G-5H schematically illustrate the curve of the first preliminary maximum speed of the first drive wheel as a function of the distance of movement of the first drive wheel in an exemplary embodiment
  • Fig. 5I schematically shows the first maximum speed and the 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;
  • Fig. 6 schematically shows a flow chart of a multi-robot trajectory planning method according to an exemplary embodiment of the present invention
  • Fig. 7 schematically shows 5 paths correspondingly for 5 mobile robots
  • Figure 8 schematically shows intersection points and conflict points according to an exemplary embodiment of the present invention.
  • FIG. 9 schematically shows the intersection point and the conflict point after the conflict point "1-2" is resolved in the exemplary embodiment shown in FIG. 8 .
  • the present invention is applicable to mobile robots, which can be any robots capable of autonomous space movement, such as AGV, AMR and so on.
  • the mobile robot can be used to perform various tasks, such as a storage robot, a cleaning robot, a family escort robot, a welcome robot, and the like.
  • Fig. 1 schematically shows a mobile robot 1 and its path 2 implementing a trajectory planning method according to an exemplary embodiment of the present invention.
  • Fig. 2 schematically shows a flowchart of a trajectory planning method for a mobile robot 1 according to an exemplary embodiment of the present invention.
  • the mobile robot 1 is, for example, a differential robot, that is, the mobile robot 1 has a first driving wheel (hereinafter, the left wheel is exemplarily described as the first driving wheel) and a second driving wheel. (Hereinafter, the right wheel is exemplarily described as the second drive wheel) differential wheel kinematic system.
  • the mobile robot 1 may also be other types of robots, such as a robot with dual steering wheels.
  • the mobile robot 1 can include, for example, a dual steering wheel kinematic system.
  • speed 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 mission point of the mobile robot 1, and the at least one mission point is located on the global path.
  • Path 2 may be in the form of a Bezier curve of degree 3 or higher and can be represented by the following equation:
  • path 2 has a continuous second derivative can be adapted particularly advantageously to the kinematics of the differential robot. In particular, path 2 can have a continuous curvature. This makes changes in the speed and acceleration of the mobile robot 1 more gradual.
  • Path 2 is shown in FIG. 1 in the form of a 4th order Bezier curve. It should be understood, however, that path 2 may also have other shapes.
  • 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 kinematics and dynamics constraints, the same as the constrained wheel The other driving wheels that the wheels move in cooperation will satisfy the kinematics and dynamics constraints; step S12, based on the path 2, carry out speed planning to the constrained wheels under the condition of satisfying the kinematics and dynamics constraints of the constrained wheels, To determine the speed of the constrained wheel; step S13 , to plan the speed of other driving wheels other than the constrained wheel in a manner that matches the determined speed of the constrained wheel.
  • an alternative trajectory planning method is provided, which is particularly capable of reliably planning a trajectory for the mobile robot 1 that satisfies kinematic and dynamic constraints.
  • the kinematic and dynamic constraints may include: the magnitude of the speed of a driven wheel being below a predetermined limit wheel speed for said drive wheel; the magnitude of the acceleration of a drive wheel being below a predetermined limit wheel acceleration for said drive wheel.
  • the limit wheel speed and limit wheel acceleration are limited by the construction of the mobile robot 1 itself without considering the limitation of the path 2 .
  • the limiting wheel speed and the limiting wheel acceleration are determined, for example, by the electric motors for driving the corresponding drive wheels.
  • 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.
  • the first driving wheel and the second driving wheel of the mobile robot 1 may be arranged symmetrically so as to have the same limit wheel speed and limit wheel acceleration.
  • the constrained wheel can be determined to satisfy the kinematics and dynamics constraints and to satisfy the kinematics and dynamics constraints 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.
  • the driving wheel first reaches the limit wheel acceleration.
  • “the driving wheel that reaches the limit wheel acceleration first” means that when the mobile robot takes the current motion state as the initial state and expects to accelerate along the path to the maximum speed allowed by the kinematics and dynamics constraints as soon as possible, the Drive wheels for which the mobile robot cannot accelerate with greater acceleration due to reaching the limit wheel acceleration.
  • the first drive wheel can be determined as the constrained wheel. If the current state of motion of the mobile robot 1 has reached the maximum speed allowed by the kinematics and dynamics constraints and conforms to the path, then any driving wheel can be regarded as a constrained wheel, or it can also be considered that there is no constrained wheel in this state. wheel.
  • the speed planning is performed on the path 2 in sections, and the following steps are respectively performed on at least one section of the path 2: for the first control point as the starting point of the section, according to the section the path shape of the segment, the state of motion of each drive wheel at the first control point, and the kinematics and dynamics constraints of the drive wheels, determining one of the at least two drive wheels as the constrained wheel in the segment,
  • the constrained wheels are: 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 wheels that preferentially reach the limit value of the kinematics and dynamics constraints ; performing speed planning on the constrained wheel to determine the constrained wheel's speed in the segment; and determining the speed of the other driven wheels in the segment in a manner compatible with the determined constrained wheel's speed.
  • the drive wheel which first reaches the limit value of the kinematic and dynamic constraints means that this drive wheel will reach the limit value of the kinematic and dynamic constraints before or simultaneously with the
  • the motion duration corresponding to each section may be a predetermined control period t.
  • the control period t can be set to a very short time, for example, a millisecond time, for example, less than 10 ms.
  • Fig. 3 schematically shows the section-by-section speed planning of the path 2 according to an exemplary embodiment.
  • the starting point of path 2 is taken as the first control point, and it is judged that starting from the current control point, the section of path 2 corresponding to the control cycle is speed planned, and then the end point of the section is As the next control point, speed planning continues until the end of the entire path 2 is reached.
  • the first initial speed v L0 and the second initial speed v R0 of the first driving wheel and the second driving wheel at the first control point are obtained first.
  • the initial speeds of the first and second drive wheels are known.
  • the first initial velocity v L0 and the second initial velocity v R0 of the first driving wheel and the second driving wheel at the first control point can be determined by The planning results of a section are obtained.
  • the "speed" of the driving wheel (also referred to as "wheel speed”) is described by taking linear velocity as an example. Since the size of the driving wheel is determined, the relationship between the linear velocity and the angular velocity of the driving wheel is also determined.
  • the speed ratio k representing the speed ratio between the second drive wheel and the first drive wheel Compare.
  • the speed ratio k and the radius of curvature R of path 2 satisfy:
  • b represents the wheelbase of the first driving wheel and the second driving wheel
  • the radius of curvature R and the curvature are mutual derivatives
  • the curvature of the path 2 at any point is determined. Therefore, for a determined path 2, the speed ratio k at any point on the path 2 is determined.
  • the speed ratio k1 at the second control point can be obtained by methods known in the prior art.
  • first maximum speed v Lmax and the second maximum speed v Rmax of the first driving wheel and the second driving wheel at the second control point are respectively determined, said first maximum speed and second maximum speed respectively denoting that regardless of The first and second drive wheels meet the kinematic and dynamic constraints of the respective drive wheels and satisfy the constraints of the path 2 at speeds up to the second control point.
  • 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 at the limit wheel acceleration of the first driving wheel is determined as the first final acceleration speed v La
  • 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 at the limit wheel acceleration of the second driving wheel is determined as the second final acceleration speed v Ra .
  • the first final acceleration speed vLa and the second final acceleration speed vRa can be determined according to the following formula:
  • 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 at the second control point and the first acceleration final speed v La is determined as the first final speed v L
  • the second maximum speed v Rmax at the second control point and The smaller of the second acceleration final speeds v Ra is determined as the second final speed v R .
  • the ratio of the second final speed vR to the first final speed vL is compared with the speed ratio k1 at the second control point, and the constrained wheels in the section are determined from the result of the comparison.
  • the current second control point is used as the first control point of the next section, and the speed planning for the next section is continued .
  • the constrained wheel is determined from the comparison of the ratio of the second final speed v R to the first final speed v L with the speed ratio k1 at the second control point in the following manner: if the second final speed v R a ratio of final speeds v L greater than the speed ratio k1 at the second control point, the first driven wheel is determined to be the constrained wheel in the segment; if the ratio of the second final speed v R to the first final speed v L If the ratio is less than the speed ratio k1 at the second control point, the second driven wheel is determined to be the constrained wheel in the section; if the ratio of the second final speed v R to the first final speed v L is equal to the second control point With a speed ratio k1 at , either of the first drive wheel and the second drive wheel can be determined to be the constrained wheel in the segment.
  • FIG. 4A schematically shows the variation curves of the radius of curvature R and the curvature ⁇ on the path 2 in an exemplary embodiment according to the present invention.
  • 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) respectively (4, 2) and (5, 3).
  • the curvature ⁇ at any point on the path 2 can be determined as follows:
  • P x ′(s), P y ′(s), P x ′′(s), P y ′′(s) are respectively The first-order abscissa and ordinate and the second-order abscissa and ordinate of . Accordingly, the radius of curvature R at any point on the path 2 can be determined.
  • FIG. 4B schematically shows the profile of the velocity ratio k on path 2 .
  • the first and second driven wheels 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 states that the speed of a driven wheel cannot exceed its limit wheel speed. Therefore, the first maximum speed v Lmax of the first driving wheel needs to satisfy the constraint: v Lmax ⁇ v lim .
  • the limit wheel speed v lim of the first drive wheel and the second drive wheel is preset to 1.5 (m/s).
  • the second constraint means 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 needs to satisfy the constraint: v Lmax ⁇ v lim /k.
  • the first maximum speed v Lmax and the second maximum speed v Rmax are the maximum wheel speeds satisfying the following constraints:
  • v Lmax min(v lim ,v lim /k)
  • v Rmax min(v lim ,v lim *k)
  • min(a,b) represents the smaller of a and b.
  • the resulting first and second maximum speeds v Lmax and v Rmax are schematically shown at the top and middle of Fig. 4B.
  • the second drive wheel may be determined to be the constrained wheel, and a speed program may be performed on the second drive wheel to determine the speed of the second drive wheel.
  • the speed of the first drive wheel is then programmed in a manner adapted to the determined speed of the second drive wheel.
  • v R/ v L ⁇ k therefore, the second driving wheel (ie, the right wheel) is used as the constraining wheel.
  • the first and second driving wheels of the mobile robot 1 have a first maximum velocity v Lmax and a second maximum velocity v Rmax respectively at the start of the path 2 .
  • the mobile robot 1 needs to go through an acceleration phase satisfying the limitation of the limit wheel acceleration to reach the first maximum speed v Lmax and the second 2 Maximum speed v Rmax .
  • FIG. 5A schematically shows path 2 in an exemplary embodiment according to the present invention.
  • FIG. 5B schematically shows the variation curve of the curvature ⁇ on the path 2 in this exemplary embodiment.
  • FIG. 5C schematically shows the variation curve of 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 may refer to the above description for FIGS. 4A-4F .
  • the first maximum speed v Lmax and the second maximum speed of the first drive wheel and the second drive wheel at any point on the path 2 v Rmax needs to satisfy the first constraint and the second constraint.
  • Fig. 5D schematically shows a first maximum speed vLmax and a second maximum speed vRmax satisfying the first constraint in this exemplary embodiment.
  • the first constraint states that the speed of a driven 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 .
  • the limit wheel speed v lim of the first drive wheel and the second drive wheel is preset to 1.2 (m/s).
  • Fig. 5E schematically shows a first maximum speed v Lmax and a second maximum speed v Rmax satisfying the first and second constraints in this exemplary embodiment.
  • 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 drive wheel and the second drive wheel at any point on the path 2 can additionally satisfy the limit wheel acceleration a based on the path 2 Determine the third constraint on the velocity ratio k': Among them, k' ⁇ 0.
  • L represents the movement distance of the mobile robot 1
  • the small displacement dL represents the displacement of the mobile robot 1 within a small time period t.
  • the small displacement dL is equal to the arithmetic mean value of the displacements of the first driving wheel and the second driving wheel, that is Therefore, the above formula can be simplified as:
  • linear motion either one of the first drive wheel and the second drive wheel may be determined as the constrained wheel.
  • circular motion the outer wheel can be determined as the constrained wheel, i.e. if k>1 then the second drive wheel is determined as the constrained wheel, if k ⁇ 1 then The first driven wheel is identified as the constrained wheel.
  • Fig. 5F schematically shows the first maximum speed v Lmax and the second maximum speed v Rmax satisfying the first constraint, the second constraint and the third constraint in this exemplary embodiment.
  • the following fourth constraint can also 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 .
  • the first driving wheel moves along the path 2 at the maximum speed determined by the first constraint, the second constraint and the third constraint
  • the first driving wheel that varies with the moving distance L of the first driving wheel is obtained
  • the first preliminary maximum speed of .
  • the first preliminary maximum speed of the first drive wheel as a function of the movement distance L L of the first drive wheel in one exemplary embodiment is schematically shown in FIGS. 5G-5H . It should be understood that, in some embodiments, the first preliminary maximum speed may also be the maximum speed motion of the first drive wheel satisfying the first and second constraints but not considering the third constraint.
  • all maximum and minimum points of the first preliminary maximum velocity can be determined. Accelerate from each minimum point to the adjacent maximum points on both sides (if any) at the limit wheel acceleration a until it intersects the curve obtained by accelerating in the same way to the adjacent minimum points on both sides. Then, connect all the curves between the adjacent minimum points of the intersection points to obtain the acceleration constraint curve of the left wheel under the acceleration constraint.
  • the fourth constraint is set for points on path 2 behind the nearest minimum point:
  • front and rear refer to the moving direction of the mobile robot 1 on the path 2 .
  • the fourth constraint is set to for points on path 2 in front of the nearest minimum point:
  • the fourth constraint can be similarly set.
  • the first maximum velocity v Lmax and/or the second preliminary maximum velocity v Rmax at any point satisfy:
  • LL and LR respectively represent the movement distances of the first driving wheel and the second driving wheel to the arbitrary point
  • v 1 correspondingly represents the first preliminary maximum value of the minimum value point closest to the arbitrary point
  • Velocities or second preliminary maximum velocities, L L1 and LR1 respectively represent the movement distances of the first drive wheel and the second drive 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 on the basis of FIG. 5F .
  • the first maximum speed v Lmax and the second maximum speed v Rmax shown in FIG. 5I simultaneously satisfy the first constraint, the second constraint, the third constraint and the fourth constraint.
  • Another aspect of the present invention proposes a multi-robot trajectory planning method, which can be executed independently of the trajectory planning method described above, and can also preferably be performed in combination with the trajectory planning method described above.
  • Fig. 6 schematically shows a multi-robot trajectory planning method according to an exemplary embodiment of the present invention.
  • the multi-robot trajectory planning method at least includes the following steps: a preliminary planning step S21, wherein a plurality of planning trajectories containing time information corresponding to a plurality of mobile robots 1 are acquired, and the plurality of The planned trajectory is a planned trajectory generated by performing a time-optimal trajectory planning method on the plurality of mobile robots 1 respectively; the conflict identification step S22, wherein, identifying the space between two planned trajectories in the plurality of planned trajectories and the conflict point on the time dimension, the conflict point represents that the mobile robot 1 moving according to the two planned trajectories will arrive at the same position at the same time; conflict resolution step S23, wherein, by adjusting the two planned trajectories The time information of a planned trajectory in , resolves the conflict.
  • This method can divide the multi-robot trajectory planning method into two layers: time-optimized global trajectory planning and time-adjusted (or speed-adjusted) local trajectory planning.
  • time-optimal global trajectory planning a global path that does not contain time information for each mobile robot 1 is planned according to a certain global path planning algorithm, and then the mobile robot 1 can use its maximum motion capability (maximum speed, maximum Acceleration, maximum jerk) movement mode is to carry out speed planning for each mobile robot 1 to obtain a planned trajectory including time information.
  • 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 each planning There are no conflict points between trajectories.
  • obtaining multiple planned trajectories can ensure that no collision occurs between the multiple mobile robots 1 , and make the multiple mobile robots 1 reach their respective destinations in as short a time as possible as a whole.
  • obtaining the multiple planned trajectories includes obtaining the existing multiple planned trajectories by receiving or reading data, and also includes performing trajectory planning for the multiple mobile robots through the trajectory planning method so as to obtain the corresponding multiple planned trajectories.
  • the plurality of planned trajectories are planned trajectories generated by the trajectory planning method described above.
  • the conflict identification step S22 and the conflict resolution step S23 may be repeatedly performed until there is no conflict point between any two planned trajectories among the plurality of planned trajectories.
  • all intersection points in the spatial dimension of every two planned trajectories among the plurality of planned trajectories may be searched first. Then, for each intersection point, check the time interval between the time information of the relevant planned trajectory at the intersection point, if the time interval is less than a predetermined time interval threshold, identify the corresponding intersection point as a conflict point .
  • the intersection point represents a point where paths of multiple planned trajectories intersect, that is, a spatial position passed by at least two planned trajectories.
  • the conflict resolution step S23 may include, for example: Sub-step S231: Select the conflict point to be resolved and the adjusted planned trajectory from the identified conflict points and the conflicted planned trajectory, wherein the adjusted planned trajectory is the same as the planned trajectory to be resolved.
  • One of the two planned trajectories associated with the resolved conflict point or the conflict point to be resolved is one of the conflict points with the adjusted planned trajectory;
  • conflicts can be resolved with less adjustments.
  • FIG. 7 schematically shows five paths for five mobile robots 1 .
  • the curves labeled 1-5 correspond to the 1st-5th paths of the 1st-5th mobile robot 1 .
  • Path intersections or overlaps represent points of intersection of corresponding mobile robot 1 trajectories in the spatial dimension. It can be seen from Figure 7 that there are 5 intersection points between the first planned trajectory for the first mobile robot 1 and other planned trajectories, which are respectively the first planned trajectory and the second and fifth , the intersection point between the 3rd, 4th and 5th planned trajectories. Obviously, time information of the corresponding planned trajectory is not shown in FIG. 7 .
  • the time information of the relevant planned trajectory entering and leaving 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, determine Whether each intersection point is a conflict point.
  • Fig. 8 schematically shows intersection points and conflict points according to an exemplary embodiment of the present invention.
  • each planned trajectory is abstractly shown schematically with a horizontal axis.
  • Each horizontal axis not only corresponds to the time scale, but also corresponds to the moving distance of the mobile robot 1 .
  • the points on each horizontal axis represent the optimal global trajectory planning according to the time, and the mobile robot 1 will move to the position at the corresponding time.
  • the intersection points are marked on the transverse axis of each mobile robot 1 in the form of a rectangular grid, wherein the transverse axes labeled 1-5 correspond to the planned trajectories of the 1st-5th mobile robot 1 .
  • the number of the mobile robot 1 where the intersection occurs is listed in each rectangular grid.
  • the intersection points between the 1st planned trajectory and the 2nd, 5th, 3rd, 4th and 5th planned trajectory are marked as "1-2", “1-5" accordingly , "1-3", "1-4", and "1-5".
  • the position of the rectangular grid on the horizontal axis indicates the time period during which the planned trajectory shown by the horizontal axis continues to move at the intersection point shown by the rectangular grid, and the width of the rectangular grid along the horizontal axis indicates that the planned trajectory continues to move at the intersection point length of time.
  • the time for the fourth planned trajectory to move continuously at the intersection point "1-4" is shorter than the time it lasts to move at the intersection point "4-5".
  • the time interval between the time information of the relevant planned trajectories at the intersection point may be checked to determine whether the intersection point is a conflict point. If the time interval is smaller than a predetermined time interval threshold, the corresponding intersection point is identified as a conflict point.
  • the predetermined time interval threshold can be set to 0, for example. For safety reasons, the predetermined time interval threshold may also be set to be greater than 0.
  • the conflict points identified from the intersection points are marked on the lowermost transverse axis in Fig. 8 . Taking the intersection point "1-2" as an example, the interval between the time period during 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 exist overlapping.
  • intersection point "1-2" is a conflict point.
  • the interval between the time period during 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 Completely staggered. Therefore, the intersection point "2-5" is not a conflict point.
  • intersection points between the planned trajectories can be expressed in the form of a matrix.
  • intersection points between the i-th trajectory and other trajectories can be represented by the following matrix:
  • T j represents the intersection point between the i-th planned trajectory and the j-th planned trajectory, and n represents the number of planned trajectories.
  • T j can be expressed in the following form:
  • m ⁇ 0 and respectively represent the time when the i-th planned trajectory enters and leaves the m+1th intersection point between it and the j-th planned trajectory.
  • T j 0 is specified.
  • the shortest planned trajectory movement time is taken as the normalized time 1, and other planned trajectories are converted proportionally according to the length of movement time.
  • the intersection points 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 number as the time when the corresponding planned trajectory enters and leaves the intersection point, and the superscripts 0 and 1 of the number are used to mark the number as the ith trajectory
  • the time information of the first and second intersection points with the j-th planned trajectory For the case where there are more intersection points between the i-th planned trajectory and the j-th planned trajectory, the same can be deduced. In the case where there is only one intersection point between the i-th planned trajectory and the j-th planned trajectory, the superscript of the number is omitted.
  • a traversal method can be used to search for conflict points from intersection points.
  • Starting from the intersection point X 1 of the first planned trajectory compare T 2 , T 3 , T 4 , and T 5 of X 1 with T 1 of X 2 , X 3 , X 4 , and X 5 respectively, and there will be time Overlapping intersection points are marked.
  • T 1 , T 3 , T 4 , T 5 of the intersection point X 2 of the second planned trajectory are compared with T 2 of X 1 , X 3 , X 4 , and X 5 respectively, and marked with time overlap intersection point. This loops until all planned trajectories have been traversed.
  • the conflict resolution step S23 can be performed.
  • the time information of the adjusted planned trajectory at the conflict point and the time information of the part of the adjusted planned trajectory after the conflict point are delayed by an equal amount in the conflict resolution step S23. Since the multiple planned trajectories themselves are time-optimal trajectory planning, this method can ensure that the adjusted planned trajectories still satisfy the kinematics and dynamics constraints of the mobile robot 1, and make the multiple mobile robots 1 In the event of a collision, the respective destination is generally reached in the shortest possible time. Because the time-optimized global trajectory planning of the previous layer represents the maximum motion capability of the mobile robot 1, when the conflict is resolved, only the time when the adjusted planned trajectory enters the conflict point to be resolved is delayed. This backward delay will correspondingly affect all time information of the adjusted planned trajectory after the conflict point to be resolved.
  • 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 fixed trajectory can be released by adjusting the planned trajectory that conflicts with the fixed planned trajectory in turn. The conflict point of the planned trajectory.
  • the conflict identification step S22 and the conflict resolution step S23 are performed in the following manner: first, identify all conflict points among the plurality of planned trajectories; Sorting in order from high to low; select the planning trajectory at the top of the ranking as the fixed planning trajectory, determine the conflict points of the fixed planning trajectory one by one as the conflict points to be adjusted, and correspondingly separate the conflict points at the conflict points to be adjusted.
  • the planning trajectory that conflicts with the fixed planning trajectory is determined as the adjusted planning trajectory, so as to remove all conflict points of the fixed planning trajectory; then, perform the conflict identification step S22 to re-identify between the multiple planning trajectories all points of conflict.
  • the five planned trajectories shown in Figure 8 Take the five planned trajectories shown in Figure 8 as an example. These five planning trajectories all have conflict points, and they are sorted according to the priority of their corresponding tasks from high to low. If the priorities of the tasks corresponding to the five planning trajectories are sorted from high to low: 1>2>3>4>5, the first planning trajectory is fixed 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 adjusted planning trajectory is the second planning trajectory and the third planning trajectory respectively.
  • the second planned trajectory is first adjusted to remove the conflict point "1-2" between the second planned trajectory and the first planned trajectory.
  • the third planned trajectory may also be adjusted first to remove the conflict point “1-3” between the third planned trajectory and the first planned trajectory.
  • the time information of the second planned trajectory at the conflict point "1-2" and the time information of the second planned trajectory after the conflict point “1-2” can be delayed by an equal amount. way to resolve conflict points.
  • the delayed amount of time is the time point when the first planned trajectory of the other party to the conflict enters the conflict point "1-2" minus the adjusted planned trajectory (that is, the second planned trajectory) leaving the conflict point "1" to be resolved.
  • Fig. 9 schematically shows the intersection point and the conflict point after the conflict point "1-2" is resolved.
  • 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.
  • the time when the third planned trajectory enters and leaves each intersection point will be updated as:
  • the conflict identification step S22 is executed again to re-determine the conflict points among the planned trajectories. As shown in the following matrix:
  • the conflict identification step S22 is executed here, and the identification result is shown in the following matrix:
  • the planning trajectory with conflict points is sorted according to the priority of the corresponding task from high to low: 4>5. Then, fix the fourth planned trajectory. Adjust the fifth planned trajectory that has conflict points with the fourth planned trajectory to remove the conflict points between the fourth planned trajectory and the fifth planned trajectory.
  • the conflict identification step S22 is executed again.
  • the time information of the intersection points between the planned trajectories is as follows:
  • the conflict points to be resolved and the adjusted planned trajectory are selected according to the order of the number of conflict points in the planned trajectory from the fewest to the largest, so as to give priority to the planned trajectory with a fixed ranking earlier so that it does not need to be adjusted.
  • the conflict identification step S22 and the conflict resolution step S23 are performed in the following manner: first, identify all conflict points between the plurality of planned trajectories; The order of the sequence is sorted; the planning trajectory at the top of the 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 conflict points to be adjusted are correspondingly combined with the fixed
  • the planned trajectory in which the planned trajectory conflicts is determined to be the adjusted planned trajectory, so as to remove all conflict points of the fixed planned trajectory; point of conflict.
  • the adjusted planned trajectory can be selected according to the time sequence of entering the conflict points.
  • the time information of the intersection points between the planned trajectories is as follows:
  • the time information of the intersection points between the planned trajectories is as follows:
  • the adjusted planned trajectory can be selected according to the time sequence of entering the conflict points. For example, sort the planned trajectories with the same number of conflict points and the least number according to the time sequence of entering conflict points: 3 ⁇ 5 ⁇ 4. Therefore, the third planned trajectory can be fixed first. Then, the conflict points "3-4" and "3-5" of the third planned trajectory one by one.
  • the time information of the intersection points between the planned trajectories is as follows:
  • the time information of the intersection points between the planned trajectories is as follows:
  • the conflict points to be resolved and the adjusted planned trajectory are selected according to the order of the conflict duration of the conflict points in the planned trajectory from the least to the longest, so as to give priority to fixing the earlier ones.
  • the planned trajectory is not adjusted.
  • the conflict identification step S22 and the conflict resolution step S23 are performed in the following manner: first, identify all conflict points among the plurality of planned trajectories; Sequence from least to most; select the planning trajectory at the top of the ranking as the fixed planning trajectory, determine the conflict points of the fixed planning trajectory one by one as the conflict points to be adjusted, and correspondingly match the conflict points at the conflict points to be adjusted
  • the planning trajectory that conflicts with the fixed planning trajectory is determined as the adjusted planning trajectory, so as to remove all conflict points of the fixed planning trajectory; then, perform the conflict identification step S22 to re-identify between the multiple planning trajectories all points of conflict.
  • the adjusted planned trajectory can be selected according to the time sequence of entering the conflict points.
  • the time information of the intersection points between the planned trajectories is as follows:
  • the time information of the intersection points between the planned trajectories is as follows:
  • the fourth planned trajectory can be adjusted first to remove the conflict point "3-4" between the fourth planned trajectory and the third planned trajectory.
  • the amount of time to delay is 0.01.
  • the time information of the intersection points between the planned trajectories is as follows:
  • the fourth planned trajectory can be fixed first. Then, adjust the fifth planned trajectory to remove the conflict point "4-5" between the fifth planned trajectory and the fourth planned trajectory. The amount of time delayed is 0.0425.
  • the conflict points to be resolved and the adjusted planned trajectory are selected according to the order of occurrence time of the conflict points.
  • the conflict identification step S22 and the conflict resolution step S23 are performed in the following manner: first, the conflict identification step S22 is performed to identify all conflict points between the plurality of planned trajectories; Sorting, select the conflict point at the top of the sort as the conflict point to be resolved, and select the planning trajectory that enters the conflict point later in the two planning trajectories associated with the conflict point to be resolved as the adjusted planning trajectory, to remove The conflict points to be resolved; the conflict identification step S22 is performed to re-identify all conflict points among the plurality of planned trajectories.
  • the time information of the intersection points between the planned trajectories is as follows:
  • the time information of the intersection points between the planned trajectories is as follows:
  • the conflict identification step S22 and the conflict resolution step S23 are repeatedly executed as described above until there is no conflict point between any two planned trajectories among the five planned trajectories.
  • the conflict identification step S22 and the conflict resolution step S23 may also be performed in other ways.
  • the planned trajectories with conflicting points are sorted in at least one of the following ways: according to the order of priority of their corresponding tasks from high to low ;According to the order of the number of conflict points from the least to the largest; according to the order of the conflict duration of the conflict points from the least to the largest; according to the order of the time when they entered the conflict point.
  • the invention also relates to a computer program product comprising computer program instructions which, when executed by one or more processors, enable said processors to perform the trajectory planning method and and/or multi-robot trajectory planning methods.
  • the computer program product can be stored in a computer-readable storage medium.
  • the computer-readable storage medium may include, for example, high-speed random access memory, and may also include non-volatile memory, such as a hard disk, internal memory, plug-in hard disk, smart memory card, secure digital card, flash memory card, at least one magnetic disk storage device, Flash memory devices, or other volatile solid-state storage devices.
  • the processor may be a central processing unit, or 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, and the like.
  • a general-purpose processor may be a microprocessor or any conventional processor or the like.

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

一种多机器人（1）轨迹规划方法包括：初步规划步骤（S21），其中获取相应地用于多个移动机器人（1）的包含时间信息的多条规划轨迹，多条规划轨迹是通过对多个移动机器人（1）分别进行时间最优轨迹规划方法而生成的规划轨迹；冲突识别步骤（S22），其中识别多条规划轨迹中的两条规划轨迹之间在空间和时间维度上的冲突点，冲突点表示按照两条规划轨迹运动的移动机器人（1）将在相同的时间到达相同的位置；冲突解除步骤（S23），其中通过调整两条规划轨迹中的一条规划轨迹的时间信息解除冲突。相应的计算机程序产品。通过多机器人（1）轨迹规划方法，能够使多个移动机器人（1）以最短的时间不发生碰撞地到达各自的目的地。

Description

多机器人轨迹规划方法及计算机程序产品 技术领域
本发明涉及移动机器人领域、尤其是移动机器人的运动控制领域，具体涉及一种多机器人轨迹规划方法及相应的计算机程序产品。
背景技术
随着经济快速增长、人力成本逐渐上升，移动机器人越来越广泛地应用于各种工业和家庭环境中。例如，自动引导车(AGV)、自主移动机器人(AMR)、叉车等移动机器人是现代物流系统的关键设备之一。移动机器人能够根据规划好的路径和作业要求运动并停靠到目标地点，以完成物料搬运输送等任务。轨迹规划是移动机器人的运动控制中的关键。
在多个移动机器人协同工作的场景中，各个移动机器人分别根据接收到的任务进行路径规划与运动。这很容易发生移动机器人的运动轨迹相互冲突的情况。通常，可依靠移动机器人的避障功能来实现多个移动机器人之间的避让。然而，在工作环境空间比较局促的场景下，常常发生移动机器人面临道路阻塞无法通行的局面。
现有技术在对多个移动机器人进行的轨迹规划方面仍然存在诸多不足。
发明内容
本发明的目的在于提供一种改进的多机器人轨迹规划方法及相应的计算机程序产品，以克服现有技术的至少一个不足。
根据本发明的第一方面，提供了一种多机器人轨迹规划方法，其中，所述多机器人轨迹规划方法包括：初步规划步骤S21，其中，获取相应地用于多个移动机器人的包含时间信息的多条规划轨迹，所述多条规划轨迹是通过对所述多个移动机器人分别进行时间最优轨迹规划方法而生成的规划轨迹；冲突识别步骤S22，其中，识别所述多条规划轨迹中的两条规划轨迹之间在空间和时间维度上的冲突点，所述冲突点表示按照所述两条规划轨迹运动的移动机器人将在相同的时间到达相同的位置；以及冲突解除步骤S23，其中，通过调整所述两条规划轨迹中的一条规划轨迹的时间信息解除所述冲突。
“时间信息”表示能够表征移动机器人在路径上的位置与时间的关系的信息。由于路径是确定的，因此“时间信息”也能表征移动机器人在路径上的各位置处的速度。
在一个示例性实施例中，重复执行冲突识别步骤S22和冲突解除步骤S23直到所述多条规划轨迹中的任意两条规划轨迹之间都不存在冲突点。
在一个示例性实施例中，冲突识别步骤S22包括：查找所述多条规划轨迹中的每两条规划轨迹在空间维度上的所有交集点；以及针对每个交集点，检查相关的规划轨迹在所述交集点处的时间信息之间的时间间隔，若所述时间间隔小于预定的时间间隔阈值，则将相应的交集点识别为冲突点。
在一个示例性实施例中，冲突解除步骤S23包括：
子步骤S231：从识别出的冲突点和发生冲突的规划轨迹中选择待解除的冲突点和被调整的规划轨迹，其中，所述被调整的规划轨迹为与待解除的冲突点相关联的两条规划轨迹中的一条或者所述待解除的冲突点为与被调整的规划轨迹所具有的冲突点中的一个，
子步骤S232：以延后被调整的规划轨迹在冲突点处的时间信息的方式调整被调整的规划轨迹在待解除的冲突点处的时间信息，使得所述相关联的两条规划轨迹在冲突点处的时间信息之间的时间间隔大于或等于时间间隔阈值，
子步骤S233：基于被调整的规划轨迹在冲突点处的调整后的时间信息，相应地更新被调整的规 划轨迹在冲突点之后的部分的时间信息。
在一个示例性实施例中，子步骤S232和子步骤S233以下述方式执行：等量地延后被调整的规划轨迹在冲突点处的时间信息和被调整的规划轨迹在冲突点之后的部分的时间信息；和/或延后的时间量为与待解除的冲突点相关联的另一规划轨迹进入待解除的冲突点的时间点减去被调整的规划轨迹离开待解除的冲突点的时间点再加上预定的时间间隔阈值。
在一个示例性实施例中，冲突识别步骤S22和冲突解除步骤S23以下述方式执行：
执行冲突识别步骤S22以识别所述多条规划轨迹之间的所有冲突点；
将具有冲突点的规划轨迹以下述方式中的至少一者进行排序：
按其对应的任务的优先级从高至低的顺序；
按其冲突点的数量从少至多的顺序；
按其冲突点的冲突持续时间从少至多的顺序；
按其进入冲突点的时间的先后顺序；
选择排序最前的规划轨迹作为固定的规划轨迹，逐一将所述固定的规划轨迹的冲突点确定为待调整的冲突点，相应地将在待调整的冲突点处与所述固定的规划轨迹发生冲突的规划轨迹确定为被调整的规划轨迹，以解除所述固定的规划轨迹的所有冲突点；以及
再此执行冲突识别步骤S22以重新识别所述多条规划轨迹之间的所有冲突点。
在一个示例性实施例中，冲突识别步骤S22和冲突解除步骤S23以下述方式执行：
执行冲突识别步骤S22以识别所述多条规划轨迹之间的所有冲突点；
将冲突点按照其发生时间的先后顺序进行排序，
选择排序最前的冲突点作为待解除的冲突点，选择与待解除的冲突点相关联的两条规划轨迹中进入冲突点的时间较晚的规划轨迹作为被调整的规划轨迹，以解除所述冲突点待解除的冲突点；以及
再此执行冲突识别步骤S22以重新识别所述多条规划轨迹之间的所有冲突点。
在一个示例性实施例中，所述时间最优轨迹规划方法能够根据确定的路径对移动机器人进行速度规划，以确定使移动机器人能够沿着所述路径运动的包含时间信息的规划轨迹，所述轨迹规划方法包括：
确定移动机器人的至少两个驱动轮中的一个为受约束轮，使得只要所述受约束轮满足运动学和动力学约束，与受约束轮相配合地运动的其它驱动轮就会满足运动学和动力学约束；
基于所述路径在满足受约束轮的运动学和动力学约束的情况下对受约束轮进行速度规划，以确定受约束轮的速度，使得受约束轮在任意点都具有满足其运动学和动力学约束并且满足所述路径的限制的情况下的最大的速度和最大的加速度中的一者；
以配合所确定的受约束轮的速度的方式，对受约束轮之外的其它驱动轮进行速度规划。
在一个示例性实施例中，在对受约束轮进行速度规划的过程中，采用T形规划方法。
在一个示例性实施例中，运动学和动力学约束包括：驱动轮的速度的大小在针对所述驱动轮预定的极限轮速度以下；驱动轮的加速度的大小在针对所述驱动轮预定的极限轮加速度以下。
在一个示例性实施例中，对所述路径分区段地进行速度规划，对所述路径的至少一个区段分别执行下述步骤：针对作为所述区段的起点的第一控制点，根据所述区段的路径形状、各驱动轮在第一控制点处的运动状态以及驱动轮的运动学和动力学约束，确定所述至少两个驱动轮中的一个为所述区段中的受约束轮，所述受约束轮为：在所述区段中，根据所述区段的路径形状和各驱动轮在第一控制点处的运动状态，优先达到运动学或动力学约束的极限值的驱动轮；对受约束轮进行速度规划以确定受约束轮在所述区段内的速度；以及以配合所确定的受约束轮的速度的方式，确定所述其它驱动轮在区段内的速度。
在一个示例性实施例中，移动机器人为双差速轮机器人，所述至少两个驱动轮为对称地布置的第一驱动轮和第二驱动轮，其中，第一驱动轮和第二驱动轮受到相同的运动学和动力学约束。
在一个示例性实施例中，以下述方式确定各个区段中的受约束轮：
获取第一驱动轮和第二驱动轮在第一控制点处的第一初速度v L0和第二初速度v R0；
确定作为所述区段的终点的第二控制点处的由所述路径决定的速度比k的取值k1，所述速度比k表示第二驱动轮与第一驱动轮的速度之比；
分别确定第一驱动轮和第二驱动轮在第二控制点处的第一最大速度v Lmax和第二最大速度v Rmax，所述第一最大速度和第二最大速度分别表示在不考虑第一驱动轮和第二驱动轮在达到第二控制点之前的速度的情况下满足各驱动轮的运动学和动力学约束并且满足所述路径的限制的最大的速度；
将第一驱动轮从第一控制点处的第一初速度v L0开始以第一驱动轮的极限轮加速度加速至第二控制点所得到的速度确定为第一加速终速度v La，将第二驱动轮从第一控制点处的第二初速度v R0开始以第二驱动轮的极限轮加速度加速至第二控制点所得到的速度确定为第二加速终速度v Ra；
将第二控制点处的第一最大速度v Lmax与第一加速终速度v La中的较小者确定为第一终速度v L，将第二控制点处的第二最大速度v Rmax与第二加速终速度v Ra中的较小者确定为第二终速度v R；以及
将第二终速度v R与第一终速度v L之比与第二控制点处的速度比k1相比较，并根据比较结果确定所述区段中的受约束轮。
在一个示例性实施例中，如果第二终速度v R与第一终速度v L之比大于第二控制点处的速度比k1，则确定第一驱动轮为所述区段中的受约束轮；如果第二终速度v R与第一终速度v L之比小于第二控制点处的速度比k1，则确定第二驱动轮为所述区段中的受约束轮；如果第二终速度v R与第一终速度v L之比等于第二控制点处的速度比k1，则确定第一驱动轮与第二驱动轮中的一者为所述区段中的受约束轮。
在一个示例性实施例中，各区段对应的运动时长等于预定的控制周期t，第一加速终速度v La和第二加速终速度v Ra根据下式来确定：
v La＝v L0+a*t
v Ra＝v R0+a*t
其中，a表示第一驱动轮和第二驱动轮的极限轮加速度。
在一个示例性实施例中，第一驱动轮和第二驱动轮在路径上的任意点处的第一最大速度v Lmax和第二最大速度v Rmax根据下述约束中的至少一者来确定：
-基于极限轮速度v lim的第一约束：v Lmax≤v lim，
-基于极限轮速度v lim和由路径决定的速度比的第二约束：v Lmax≤v lim/k，
-基于极限轮加速度a和由路径决定的速度比变化率k’的第三约束：
Figure PCTCN2022123038-appb-000001
其中，k′≠0；并且
第一驱动轮和第二驱动轮在路径上的任意点处的第一最大速度v Lmax和第二最大速度v Rmax满足：v Rmax＝v Lmax*k。
在一个示例性实施例中，第一驱动轮和第二驱动轮在路径上的任意点处的第一最大速度v Lmax和第二最大速度v Rmax附加地根据下述第四约束来确定：
确定假设沿着所述路径第一驱动轮以由第一约束、第二约束和第三约束中的所述至少一者确定的最大速度运动而得到随第一驱动轮的运动距离L L变化的第一初步最大速度和随第二驱动轮的运动距离L R变化的第二初步最大速度中的至少一者；
确定第一初步最大速度和第二初步最大速度中的所述至少一者的极大值点和极小值点；
所述任意点处的第一最大速度v Lmax和/或第二初步最大速度v Rmax满足：
如果所述任意点在距离所述任意点最近的极小值点的后方，则：
Figure PCTCN2022123038-appb-000002
和/或
Figure PCTCN2022123038-appb-000003
如果所述任意点在距离所述任意点最近的极小值点的前方，则：
Figure PCTCN2022123038-appb-000004
和/或
Figure PCTCN2022123038-appb-000005
其中，L L和L R分别表示第一驱动轮和第二驱动轮运动到所述任意点处的运动距离，v 1相应地表示距离所述任意点最近的极小值点的第一初步最大速度或第二初步最大速度，L L1和L R1分别表示第一驱动轮和第二驱动轮运动到所述最近的极小值点处的运动距离。
在一个示例性实施例中，所述路径是通过根据移动机器人的至少一个任务点进行全局路径规划而确定的全局路径，所述至少一个任务点位于所述全局路径上；和/或所述路径呈3阶以上贝塞尔曲线的形式。
根据本发明的第二方面，提供了一种计算机程序产品，其包括计算器程序指令，其中，当所述计算机程序指令被一个或多于一个处理器执行时，所述处理器能够执行根据本发明的多机器人轨迹规划方法。
本发明的积极效果在于：能够在多个移动机器人在同一工作环境下工作时，在所述多个移动机器人之间不发生碰撞的情况下，使所述多个移动机器人整体上以尽量短的时间达到各自的目的地。
附图说明
下面，通过参看附图更详细地描述本发明，可以更好地理解本发明的原理、特点和优点。附图包括：
图1示意性地示出了实施根据本发明的一个示例性实施例的轨迹规划方法的移动机器人及其路径；
图2示意性地示出了根据本发明的一个示例性实施例的用于移动机器人的轨迹规划方法的流程图；
图3示意性的示出了根据一个示例性实施例对路径分区段地进行速度规划的流程图；
图4A示意性地示出了在根据本发明的一个示例性实施例中路径上的曲率半径和曲率的变化曲线；
图4B示意性地示出了在根据本发明的一个示例性实施例中路径上的速度比和满足第一约束和第二约束的第一最大速度和第二最大速度的变化曲线；
图4C和图4D示意性地示出了与图4B所示的第一最大速度和第二最大速度对应的移动机器人的运动速度和运动距离以及第一驱动轮和第二驱动轮所需的第一需求加速度和第二需求加速度和移动机器人沿路径运动的时间戳；
图4E示意性地示出了经过重新规划后的第一驱动轮和第二驱动轮所需的第一需求加速度和第二需求加速度和移动机器人沿路径运动的时间戳；
图4F示意性地示出了速度比以及经过重新规划后的第一驱动轮和第二驱动轮的速度变化曲线；
图5A示意性地示出了根据本发明的一个示例性实施例中的路径；
图5B示意性地示出了在图5A所示的示例性实施例中的路径上的曲率的变化曲线；
图5C示意性地示出了在图5A所示的示例性实施例中的路径上的速度比的变化曲线；
图5D示意性地示出了在图5A所示的示例性实施例中满足第一约束的第一最大速度和第二最大速度；
图5E示意性地示出了在图5A所示的示例性实施例中满足第一约束和第二约束的第一最大速度和第二最大速度；
图5F示意性地示出了在图5A所示的示例性实施例中满足第一约束、第二约束和第三约束的第一最大速度和第二最大速度；
图5G-5H中示意性地示出了在一个示例性实施例中随第一驱动轮的运动距离变化的第一驱动轮的第一初步最大速度的曲线；
图5I示意性地示出了在图5A所示的示例性实施例中满足第一约束、第二约束、第三约束和第四约束的第一最大速度和第二最大速度；
图6示意性地示出了根据本发明的一个示例性实施例的多机器人轨迹规划方法的流程图；
图7示意性地示出了相应地用于5个移动机器人的5条路径；
图8示意性地示出了根据本发明的一个示例性实施例中的交集点和冲突点；以及
图9示意性地示出了在图8所示的示例性实施例中解除冲突点“1-2”之后的交集点和冲突点。
具体实施方式
为了使本发明所要解决的技术问题、技术方案以及有益的技术效果更加清楚明白，以下将结合附图以及多个示例性实施例对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用于解释本发明，而不是用于限定本发明的保护范围。
本发明适用于移动机器人，其可以是任何能够自主地进行空间移动的机器人，例如AGV、AMR等。所述移动机器人可用于执行各种任务，例如用作仓储机器人、清扫型机器人、家庭陪护机器人、迎宾机器人等。
应理解，在本文中，表述“第一”、“第二”等仅用于描述性目的，而不应理解为指示或暗示相对重要性，也不应理解为隐含指明所指示的技术特征的数量。限定有“第一”、“第二”的特征可以明示或者隐含地表示包括至少一个该特征。
下面结合图1和图2示例性地说明本发明的运动控制方法。图1示意性地示出了实施根据本发明的一个示例性实施例的轨迹规划方法的移动机器人1及其路径2。图2示意性地示出了根据本发明的一个示例性实施例的用于移动机器人1的轨迹规划方法的流程图。
在图1所示的实施例中，移动机器人1例如为差速机器人，即移动机器人1具有包括第一驱动轮(在下文中示例性地以左轮为第一驱动轮进行说明)和第二驱动轮(在下文中示例性地以右轮为第二驱动轮进行说明)的差速轮运动系统。替代地，移动机器人1也可以是其它类型的机器人，例如双舵轮机器人等。相应地，移动机器人1例如可包括双舵轮运动系统。
在轨迹规划方法中，根据确定的路径2对移动机器人1进行速度规划，以确定使移动机器人1能够沿着所述路径2运动的包含时间信息的规划轨迹。所述路径2可以是通过根据移动机器人1的至少一个任务点进行全局路径规划而确定的全局路径，所述至少一个任务点位于所述全局路径上。
路径2可呈3阶以上贝塞尔曲线的形式并且能够以下式表示：
Figure PCTCN2022123038-appb-000006
其中，
Figure PCTCN2022123038-appb-000007
表示移动机器人1的位置，i＝0,1,…,N，N≥3，
Figure PCTCN2022123038-appb-000008
表示贝塞尔曲线的控制点的坐标。当s从0增大到1，对应的
Figure PCTCN2022123038-appb-000009
表示移动机器人1沿着路径2从起点到终点的位置。这对于差速机器人而言是尤其有利的。路径2具有连续的二阶导数能够特别有利地适应差速机器人的运动特性。特别是，路径2能够具有连续的曲率。这使得移动机器人1的速度和加速度的变化更平缓。图1中示出了呈4阶贝塞尔曲线形式的路径2。但应理解，路径2也可具有其它形状。
如图2所示，轨迹规划方法包括：步骤S11、确定移动机器人1的至少两个驱动轮中的一个为受约束轮，使得只要所述受约束轮满足运动学和动力学约束，与受约束轮相配合地运动的其它驱动轮就会满足运动学和动力学约束；步骤S12、基于所述路径2在满足受约束轮的运动学和动力学约束的情况下对受约束轮进行速度规划，以确定受约束轮的速度；步骤S13、以配合所确定的受约束轮的速度的方式，对受约束轮之外的其它驱动轮进行速度规划。
由此，提供了一种可供选择的轨迹规划方法，其尤其能够可靠地为移动机器人1规划出满足运动学和动力学约束的轨迹。
在对受约束轮进行速度规划的过程中，使受约束轮在任意点都具有满足其运动学和动力学约束并且满足所述路径2的限制的情况下的最大的速度和最大的加速度中的一者。由此，能够为移动机器人1规划出时间最优的轨迹。
运动学和动力学约束可包括：驱动轮的速度的大小在针对所述驱动轮预定的极限轮速度以下； 驱动轮的加速度的大小在针对所述驱动轮预定的极限轮加速度以下。所述极限轮速度和极限轮加速度受移动机器人1本身的构造限制而不考虑路径2的限制。极限轮速度和极限轮加速度例如由用于驱动相应的驱动轮的电机决定。可选地，运动学和动力学约束也可包括驱动轮的加加速度在针对所述驱动轮预定的极限轮加加速度以下。可选地，移动机器人1的第一驱动轮和第二驱动轮可对称地设置，从而具有相同的极限轮速度和极限轮加速度。
受约束轮可被确定为在运动过程中，从当前控制时刻到下一控制时刻，根据移动机器人1的路径2和当前时刻的移动机器人1的运动状态，在满足运动学和动力学约束并且以尽可能大的轮速度运动的情况下，先达到极限轮加速度的驱动轮。在此，“先达到极限轮加速度的驱动轮”表示在移动机器人以当前时刻的运动状态为起始状态而期望沿着路径尽快加速到运动学和动力学约束允许的最大速度的情况下，将由于达到极限轮加速度而导致移动机器人无法以更大的加速度加速的驱动轮。例如，如果移动机器人的加速度大到一定程度，第一驱动轮已经率先达到其极限轮加速度，而第二驱动轮的轮加速度仍在其极限轮加速度以下，那么移动机器人将由于第一驱动轮达到极限轮加速度而不能再以更大的加速度加速。因此，可将第一驱动轮确定为受约束轮。如果移动机器人1的当前运动状态已经达到了运动学和动力学约束允许的、符合路径的最大速度，那么可以将任一驱动轮作为受约束轮，或者也可以认为在该状态下不存在受约束轮。
可选地，对所述路径2分区段地进行速度规划，对所述路径2的至少一个区段分别执行下述步骤：针对作为所述区段的起点的第一控制点，根据所述区段的路径形状、各驱动轮在第一控制点处的运动状态以及驱动轮的运动学和动力学约束，确定所述至少两个驱动轮中的一个为所述区段中的受约束轮，所述受约束轮为：在所述区段中，根据所述区段的路径形状和各驱动轮在第一控制点处的运动状态，优先达到运动学和动力学约束的极限值的驱动轮；对受约束轮进行速度规划以确定受约束轮在区段内的速度；以及以配合所确定的受约束轮的速度的方式，确定所述其它驱动轮在区段内的速度。在此，优先达到运动学和动力学约束的极限值的驱动轮表示该驱动轮将在其它驱动轮之前或与其它驱动轮同时地达到运动学和动力学约束的极限值。
各区段对应的运动时长可以是预定的控制周期t。所述控制周期t可设定为非常短的时间，例如设定为毫秒级的时间，例如可小于10ms。
下面结合图3进一步说明对所述路径2分区段地进行速度规划的过程。图3示意性的示出了根据一个示例性实施例对所述路径2分区段地进行速度规划。在该示例性实施例中，将路径2的起点作为首个控制点，并判断从当前控制点开始，对控制周期所对应的路径2的区段进行速度规划，然后以所述区段的终点作为下一控制点，继续进行速度规划，直到达到整个路径2的终点。
对于每个区段，首先获取第一驱动轮和第二驱动轮在第一控制点处的第一初速度v L0和第二初速度v R0。在路径2的起点处，第一驱动轮和第二驱动轮的初始速度是已知的。对于从路径2的起点开始的第一个区段之外的区段，第一驱动轮和第二驱动轮在第一控制点处的第一初速度v L0和第二初速度v R0可由上一区段的规划结果得出。在本文的描述中，驱动轮的“速度”(也称为“轮速度”)以线速度为例进行说明。由于驱动轮的尺寸是确定的，因此驱动轮的线速度与角速度之间的关系也是确定的。
另外，确定作为所述区段的终点的第二控制点处的由所述路径2决定的速度比k的取值k1，所述速度比k表示第二驱动轮与第一驱动轮的速度之比。对于差速轮运动系统，速度比k与路径2的曲率半径R满足：
Figure PCTCN2022123038-appb-000010
其中，b表示第一驱动轮与第二驱动轮的轮距，曲率半径R与曲率互为导数，而路径2在任一点处的曲率是确定的。因此，对于确定的路径2，路径2上的任一点处的速度比k是确定的。例如，对于呈贝塞尔曲线的形式，速度比k可表示为变量s的函数：k＝g(s)。相应地，还可确定速度比变化率k’：k′＝g′(s)。在已经确定路径2上的任一点处的速度比k的情况下，第二控制点处的速度比k1可通过现有技术已知的方法求取。
此外，分别确定第一驱动轮和第二驱动轮在第二控制点处的第一最大速度v Lmax和第二最大速度v Rmax，所述第一最大速度和第二最大速度分别表示在不考虑第一驱动轮和第二驱动轮在达到第二控制点之前的速度的情况下满足各驱动轮的运动学和动力学约束并且满足所述路径2的限制的最大的轮速度。
然后，将第一驱动轮从第一控制点处的第一初速度v L0开始以第一驱动轮的极限轮加速度加速至第二控制点所得到的轮速度确定为第一加速终速度v La，将第二驱动轮从第一控制点处的第二初速度v R0开始以第二驱动轮的极限轮加速度加速至第二控制点所得到的轮速度确定为第二加速终速度v Ra。在所述区段对应控制周期t的情况下，第一加速终速度v La和第二加速终速度v Ra可根据下式来确定：
v La＝v L0+a*t
v Ra＝v R0+a*t
其中，a表示第一驱动轮和第二驱动轮的极限轮加速度。
将第二控制点处的第一最大速度v Lmax与第一加速终速度v La中的较小者确定为第一终速度v L，并且将第二控制点处的第二最大速度v Rmax与第二加速终速度v Ra中的较小者确定为第二终速度v R。
然后，将第二终速度v R与第一终速度v L之比与第二控制点处的速度比k1相比较，并根据比较结果确定所述区段中的受约束轮。
在确定该区段中的受约束轮之后，如果还未到达路径2的终点，则将当前的第二控制点作为下一区段的第一控制点，并继续对下一区段进行速度规划。
可选地，以下述方式根据第二终速度v R与第一终速度v L之比与第二控制点处的速度比k1的比较结果确定受约束轮：如果第二终速度v R与第一终速度v L之比大于第二控制点处的速度比k1，则确定第一驱动轮为所述区段中的受约束轮；如果第二终速度v R与第一终速度v L之比小于第二控制点处的速度比k1，则确定第二驱动轮为所述区段中的受约束轮；如果第二终速度v R与第一终速度v L之比等于第二控制点处的速度比k1，则可确定第一驱动轮与第二驱动轮中的任一者为所述区段中的受约束轮。
下面结合图4A-4F详细说明确定第一驱动轮和第二驱动轮在路径2上的任意点处的第一最大速度v Lmax和第二最大速度v Rmax的过程。
图4A示意性地示出了在根据本发明的一个示例性实施例中路径2上的曲率半径R和曲率к的变化曲线。在该示例性实施例中，所述路径2为四阶贝塞尔曲线，其五个贝塞尔曲线控制点坐标分别为(0，-1)、(0，0)、(1，2)(4，2)和(5，3)。
根据所述路径2，可确定路径2上的任一点处的曲率к如下：
Figure PCTCN2022123038-appb-000011
其中，P x′(s)、P y′(s)、P x″(s)、P y″(s)分别是
Figure PCTCN2022123038-appb-000012
的一阶导横、纵坐标和二阶导横、纵坐标。相应地，可确定路径2上的任一点处的曲率半径R。
进而，可得出路径2上的任一点处的速度比k。图4B的底部示意性地示出了路径2上的速度比k的变化曲线。
在该示例性实施例中，根据基于极限轮速度v lim的第一约束和基于极限轮速度v lim和由路径2决定的速度比的第二约束来确定第一驱动轮和第二驱动轮在路径2上的任意点处的第一最大速度v Lmax和第二最大速度v Rmax。
第一约束表示驱动轮的速度不能超过其极限轮速度。因此，第一驱动轮的第一最大速度v Lmax需满足约束：v Lmax≤v lim。在该实施例中，第一驱动轮和第二驱动轮的极限轮速度v lim被预先设定为1.5(m/s)。
第二约束表示第一驱动轮和第二驱动轮中的任一个驱动轮的速度需使得满足速度比k的另一个驱动轮也不能超过其极限轮速度。因此，第一驱动轮的第一最大速度v Lmax需满足约束：v Lmax≤v lim/k。
第一驱动轮和第二驱动轮在路径2上的任意点处的第一最大速度v Lmax和第二最大速度v Rmax还满足约束：v Rmax＝v Lmax*k。
因此，第一最大速度v Lmax和第二最大速度v Rmax为满足下述约束的最大轮速度：
Figure PCTCN2022123038-appb-000013
可得出v Lmax＝min(v lim,v lim/k),v Rmax＝min(v lim,v lim*k)，其中，min(a,b)表示a与b中的较小者。所得出的第一最大速度v Lmax和第二最大速度v Rmax在图4B的顶部和中部示意性地示出。
可以看出，在确定第一最大速度v Lmax和第二最大速度v Rmax的过程中，没有考虑第一驱动轮和第二驱动轮在达到所述任意点之前的实际速度或规划速度对其在所述任意点处所能达到的速度的限制。
假设第一驱动轮和第二驱动轮始终以第一最大速度v Lmax和第二最大速度v Rmax沿着路径2运动，那么相应的移动机器人1的运动速度v r和运动距离L将如图4C所示。相应地，可得到移动机器人1沿路径2运动的时间戳t以及第一驱动轮和第二驱动轮所需的第一需求加速度a Lneed和第二需求加速度a Rneed，如图4D所示。
在该实施例中，第一驱动轮和第二驱动轮的极限轮加速度a示例性地被预先设定为0.5(m/s 2)。从图4D可见，第二驱动轮在s<s1区间内超过了极限轮加速度a的限制。因此，需要对此进行重新规划，重新规划过程例如如下：以超过极限轮加速度限制的区段中的速度最小值点为起点向两侧开始重新规划。在本实施例中，从s＝0处开始向s增大方向重新规划。将加速度强制设定为极限轮加速度a，如图4E所示。即，从s＝0处开始，第二驱动轮以加速度a＝0.5加速，第一驱动轮配合第二驱动轮运动，以满足由路径2决定的速度比。显然，第一驱动轮在此需要先减速(其加速度的大小将在极限轮加速度a以下)，然后再加速。图4F再此示出了速度比k以及经过重新规划后的第一驱动轮和第二驱动轮的速度变化曲线。从图4F可见，在s＝s 2的位置，第一驱动轮和第二驱动轮将达到第一最大速度v Lmax和第二最大速度v Rmax。在此，s 2>s 1。在s＝s 2之后，第一驱动轮和第二驱动轮可以以第一最大速度v Lmax和第二最大速度v Rmax沿着路径2运动。
在被重新规划的区段中，可确定第二驱动轮为受约束轮，并对第二驱动轮进行速度规划以确定第二驱动轮的速度。然后，以配合所确定的第二驱动轮的速度的方式，对第一驱动轮进行速度规划。确定第二驱动轮为受约束轮的过程可参照图3。在被重新规划的区段中，v R0+a*t<v Rmax，因此，v R＝v R0+a*t。而v L0+a*t>v Lmax，因此，v L＝v Lmax。进而，v R/v L<k，因此，以第二驱动轮(即右轮)为约束轮。
在上述示例性描述的过程中，假设了移动机器人1的第一驱动轮和第二驱动轮在路径2的起点处分别具有第一最大速度v Lmax和第二最大速度v Rmax。当第一驱动轮和第二驱动轮在路径2的起点处的轮速度为其它值的情况下，移动机器人1需经历满足极限轮加速度的限制的加速阶段以达到第一最大速度v Lmax和第二最大速度v Rmax。
下面结合图5A-5F详细说明在根据本发明的示例性实施例中确定第一驱动轮和第二驱动轮在路径2上的任意点处的第一最大速度v Lmax和第二最大速度v Rmax的过程。
图5A示意性地示出了在根据本发明的一个示例性实施例中的路径2。在该示例性实施例中，所述路径2满足曲线方程：y＝sin(π/2*x)。图5B示意性地示出了在该示例性实施例中的路径2上的曲率к的变化曲线。图5C示意性地示出了该路径2上的速度比k的变化曲线。根据路径2确定路径2上的曲率к和速度比k的过程可参考上文针对图4A-4F的描述。
在该示例性实施例中，与图4A-4F所示的实施例类似地，第一驱动轮和第二驱动轮在路径2上的任意点处的第一最大速度v Lmax和第二最大速度v Rmax需满足第一约束和第二约束。
图5D示意性地示出了在该示例性实施例中满足第一约束的第一最大速度v Lmax和第二最大速度v Rmax。第一约束表示驱动轮的速度不能超过其极限轮速度。因此，第一最大速度v Lmax和第二最大速度v Rmax需满足约束：v Lmax≤v lim，v Rmax≤v lim。在该实施例中，第一驱动轮和第二驱动轮的极限轮速度v lim被预先设定为1.2(m/s)。
图5E示意性地示出了在该示例性实施例中满足第一约束与第二约束的第一最大速度v Lmax和第二最大速度v Rmax。第二约束表示第一驱动轮和第二驱动轮中的任一个驱动轮的速度需使得满足速度比k的另一个驱动轮也不能超过其极限轮速度。因此，第一驱动轮的第一最大速度v Lmax需满足约束：v Lmax≤v lim/k；第二驱动轮的第二最大速度v Rmax需满足约束：v Rmax＝v lim*k。
由此，可得到满足第一约束和第二约束的最大轮速度：v Lmax＝min(v lim,v lim/k),v Rmax＝min(v lim,v lim*k)。
第一驱动轮和第二驱动轮在路径2上的任意点处的第一最大速度v Lmax和第二最大速度v Rmax还满足约束：v Rmax＝v Lmax*k。
在该实施例中，第一驱动轮和第二驱动轮在路径2上的任意点处的第一最大速度v Lmax和第二最大速度v Rmax可附加地满足基于极限轮加速度a和由路径2决定的速度比变化率k’的第三约束：
Figure PCTCN2022123038-appb-000014
其中，k′≠0。
下面具体介绍第三约束的原理。如上文所述，在路径2被确定之后，就能够确定路径2上任意点处的速度比k和速度比变化率k’。假设在路径2上某一点处，沿着路径2运动的移动机器人1的第一驱动轮和第二驱动轮分别具有速度v L0和v R0，速度比k 0＝v R0/v L0。
由于第一驱动轮和第二驱动轮的轮加速度不能超过极限轮加速度a，因此，在移动机器人1沿路径2以一微小的时间段t运动经过一路径段之后，速度比k的取值范围为：
Figure PCTCN2022123038-appb-000015
那么，在该点处，速度比变化率k’应满足下式：
Figure PCTCN2022123038-appb-000016
其中，L表示移动机器人1的运动距离，微小位移量dL表示移动机器人1在微小的时间段t内的位移。微小位移量dL等于第一驱动轮和第二驱动轮的位移的算术平均值，即
Figure PCTCN2022123038-appb-000017
由此，上式可简化为：
Figure PCTCN2022123038-appb-000018
当v R0+v L0不等于0时，上式可简化为：
Figure PCTCN2022123038-appb-000019
由此可知，当速度比变化率k′≠0时，第一驱动轮的轮速度需满足：
Figure PCTCN2022123038-appb-000020
相应地，第二驱动轮的轮速度需满足：
Figure PCTCN2022123038-appb-000021
应理解，速度比变化率k′＝0意味着移动机器人1在该点处进行直线运动或圆周运动。在直线运动(k′＝0并且k＝1)的情况下，可将第一驱动轮和第二驱动轮中的任一个确定为受约束轮。在圆周运动(k′＝0并且k≠1)的情况下，可将外侧轮确定为受约束轮，即，如果k>1则将第二驱动轮确定为受约束轮，如果k<1则将第一驱动轮确定为受约束轮。
图5F示意性地示出了在该示例性实施例中满足第一约束、第二约束和第三约束的第一最大速度v Lmax和第二最大速度v Rmax。
可选地，还可为第一驱动轮和第二驱动轮在路径2上的任意点处的第一最大速度v Lmax和第二最大速度v Rmax设置下述第四约束。
下面参照图5G-5H来说明第四约束。首先，确定假设沿着所述路径2第一驱动轮以由第一约束、第二约束和第三约束确定的最大速度运动而得到随第一驱动轮的运动距离L L变化的第一驱动轮的第一初步最大速度。图5G-5H中示意性地示出了在一个示例性实施例中随第一驱动轮的运动距离L L变化的第一驱动轮的第一初步最大速度的曲线。应理解，在一些实施例中，第一初步最大速度也可以是满足第一约束和第二约束但不考虑第三约束的第一驱动轮的最大速度运动。
然后，可确定第一初步最大速度的所有极大值点和极小值点。从每个极小值点向两侧的相邻极大值点(如果存在)以极限轮加速度a进行加速，直到与两侧的相邻极小值点以相同方式进行加速得到的曲线相交。然后，将所有的交点相邻极小值点间的曲线相连，即得到左轮在加速度约束下的加速度约束曲线。
图5G示意性地示出了以L L＝L 1处的极小值点(L 1，v 1)为例，从该极小值点以极限轮加速度a向左侧(即后方)的相邻极大值点进行加速至相距极小距离dL L的L L＝L 2处，则：
Figure PCTCN2022123038-appb-000022
其中，v 2表示加速至相距极小距离的L L＝L 2处的第一驱动轮的轮速度，p表示第一驱动轮的轮速度在点(L 1，v 1)处以极限轮加速度a向点(L 2，v 2)增大的斜率。当dL L趋近于0，
Figure PCTCN2022123038-appb-000023
从而可得：
Figure PCTCN2022123038-appb-000024
相应地，第四约束设定为对于路径2上在最近的极小值点的后方的点：
Figure PCTCN2022123038-appb-000025
应理解，在本文中，“前方”和“后方”是参照移动机器人1在路径2上运动方向而言。
图5H示意性地示出了从L L＝L 1处的极小值点(L 1，v 1)以极限轮加速度a向右侧(即前方)的相邻极大值点进行加速至相距极小距离dL L的L L＝L 2处，则：
Figure PCTCN2022123038-appb-000026
其中，v 2表示加速至相距极小距离的L L＝L 2处的第一驱动轮的轮速度，p表示第一驱动轮的轮速度在点(L 1，v 1)处以极限轮加速度a向点(L 2，v 2)增大的斜率。当dL L趋近于0，
Figure PCTCN2022123038-appb-000027
从而可得：
Figure PCTCN2022123038-appb-000028
相应地，第四约束设定为对于路径2上在最近的极小值点的前方的点：
Figure PCTCN2022123038-appb-000029
同理，针对第二驱动轮的第二最大速度v Rmax，第四约束可类似地设定。
因此，根据第四约束，任意点处的第一最大速度v Lmax和/或第二初步最大速度v Rmax满足：
如果所述任意点在距离所述任意点最近的极小值点的后方，则：
Figure PCTCN2022123038-appb-000030
和/或
Figure PCTCN2022123038-appb-000031
如果所述任意点在距离所述任意点最近的极小值点的前方，则：
Figure PCTCN2022123038-appb-000032
和/或
Figure PCTCN2022123038-appb-000033
其中，L L和L R分别表示第一驱动轮和第二驱动轮运动到所述任意点处的运动距离，v 1相应地表示距离所述任意点最近的极小值点的第一初步最大速度或第二初步最大速度，L L1和L R1分别表示第一驱动轮和第二驱动轮运动到所述最近的极小值点处的运动距离。
图5I示意性地示出了在图5F的基础上进一步满足第四约束的第一最大速度v Lmax和第二最大速度v Rmax。换言之，图5I所示的第一最大速度v Lmax和第二最大速度v Rmax同时满足第一约束、第二约束、第三约束和第四约束。
本发明的另一方面提出了一种多机器人轨迹规划方法，其可以独立于上文所述的轨迹规划方法被执行，也可优选地与上文所述的轨迹规划方法相结合地执行。
图6示意性地示出了根据本发明的一个示例性实施例的多机器人轨迹规划方法。
如图6所示，所述多机器人轨迹规划方法至少包括下述步骤：初步规划步骤S21，其中，获取相应地用于多个移动机器人1的包含时间信息的多条规划轨迹，所述多条规划轨迹是通过对所述多个移动机器人1分别进行时间最优轨迹规划方法而生成的规划轨迹；冲突识别步骤S22，其中，识别所述多条规划轨迹中的两条规划轨迹之间在空间和时间维度上的冲突点，所述冲突点表示按照所述两条规划轨迹运动的移动机器人1将在相同的时间到达相同的位置；冲突解除步骤S23，其中，通过调整所述两条规划轨迹中的一条规划轨迹的时间信息解除所述冲突。
由此，能够在多个移动机器人1在同一工作环境下工作时，在所述多个移动机器人1之间不发生碰撞的情况下，使所述多个移动机器人1整体上以尽量短的时间达到各自的目的地。
这种方法能够将多机器人轨迹规划方法分为时间最优的全局轨迹规划和时间调整(或速度调整)的局部轨迹规划两层。在时间最优的全局轨迹规划中，根据某种全局路径规划算法规划出用于各移动机器人1的不包含时间信息的全局路径，然后以使移动机器人1以自身最大运动能力(最大速度、最大加速度、最大加加速度)运动的方式为各移动机器人1进行速度规划以获得包含时间信息的规划轨迹。在时间调整的局部轨迹规划中，基于上一层时间最优的全局轨迹规划所获得的包含时间信息的规划轨迹，调整每条规划轨迹的运动时间(即，调整运动速度)，从而使各规划轨迹之间不存在冲突点。由此获得多条规划轨迹能够保证所述多个移动机器人1之间不发生碰撞，并且使所述多个移动机器人1整体上以尽量短的时间达到各自的目的地。
应理解，冲突点并不限于规划轨迹在某一单独的点发生碰撞的情况，而是也包括规划轨迹之间具有重叠的轨迹区段的情况(参见图7)。在本文中，获取所述多条规划轨迹既包括以接收数据或读取数据的方式获取已有的多条规划轨迹，也包括通过轨迹规划方法为所述多个移动机器人进行轨迹规划从而获取相应的多条规划轨迹。
在一个示例性实施例中，所述多条规划轨迹是通过上文描述的轨迹规划方法生成的规划轨迹。
如图6所述，可重复执行冲突识别步骤S22和冲突解除步骤S23直到所述多条规划轨迹中的任意两条规划轨迹之间都不存在冲突点。
具体地说，在冲突识别步骤S22中，可首先查找所述多条规划轨迹中的每两条规划轨迹在空间维度上的所有交集点。然后，针对每个交集点，检查相关的规划轨迹在所述交集点处的时间信息之间的时间间隔，若所述时间间隔小于预定的时间间隔阈值，则将相应的交集点识别为冲突点。交集点表示多条规划轨迹的路径相交的点，即被至少两条规划轨迹经过的空间位置。
冲突解除步骤S23例如可包括：子步骤S231：从识别出的冲突点和发生冲突的规划轨迹中选择待解除的冲突点和被调整的规划轨迹，其中，所述被调整的规划轨迹为与待解除的冲突点相关联的两条规划轨迹中的一条或者所述待解除的冲突点为与被调整的规划轨迹所具有的冲突点中的一个；子步骤S232：以延后被调整的规划轨迹在冲突点处的时间信息的方式调整被调整的规划轨迹在待解除的冲突点处的时间信息，使得所述相关联的两条规划轨迹在冲突点处的时间信息之间的时间间隔大于或等于时间间隔阈值；以及子步骤S233：基于被调整的规划轨迹在冲突点处的调整后的时间信息，相应地更新被调整的规划轨迹在冲突点之后的部分的时间信息。由此，能够以较少的调整解除 冲突。
下面结合图7和图8进一步说明根据本发明的示例性实施例。图7示意性地示出了相应地用于5个移动机器人1的5条路径。标号1-5的曲线对应第1-5个移动机器人1的第1-5条路径。路径相交或重叠表示相应的移动机器人1轨迹在空间维度上的交集点。从图7中可以看出，用于第1个移动机器人1的第1条规划轨迹与其它规划轨迹之间具有5个交集点，分别为第1条规划轨迹先后与第2个、第5个、第3个、第4个和第5个条规划轨迹之间的交集点。显然，图7中没有示出相应的规划轨迹的时间信息。
在找到所有交集点之后，针对每个交集点，可确定相关的规划轨迹进出所述交集点的时间信息，并根据相关的规划轨迹在所述交集点处的时间信息之间的时间间隔，确定各交集点是否为冲突点。
图8示意性地示出了根据本发明的一个示例性实施例中的交集点和冲突点。在图8中，为了直观，将每条规划轨迹抽象地以一条横向轴线示意性地示出。每条横向轴线既对应时间刻度，又可对应移动机器人1运动的距离。每条横向轴线上的点表示按照时间最优的全局轨迹规划，该移动机器人1在对应时间将运动到该位置。在图8中，交集点以矩形格的形式被标识在每个移动机器人1的横向轴线上，其中，标号1-5的横向轴线对应第1-5移动机器人1的规划轨迹。各矩形格中列出了发生交集的移动机器人1的编号。例如，第1条规划轨迹先后与第2条、第5条、第3条、第4条和第5条规划轨迹之间的交集点相应地标记为“1-2”、“1-5”、“1-3”、“1-4”和“1-5”。矩形格在横向轴线上的位置表示该横向轴线所示的规划轨迹在矩形格所示的交集点处持续运动的时间段，矩形格沿横向轴线的宽度表示该规划轨迹在该交集点处持续运动的时间长度。例如，第4条规划轨迹在交集点“1-4”处持续运动的时间小于其在交集点“4-5”处持续运动的时间。
然后，可针对每个交集点，检查相关的规划轨迹在所述交集点处的时间信息之间的时间间隔，来判断交集点是否为冲突点。若所述时间间隔小于预定的时间间隔阈值，则将相应的交集点识别为冲突点。预定的时间间隔阈值例如可设定为0。为了安全起见，所述预定的时间间隔阈值也可设定为大于0。图8中最下方的横向轴线上标出了从交集点中识别出的冲突点。以交集点“1-2”为例，第1条规划轨迹与第2条规划轨迹在交集点“1-2”处持续运动的时间段之间的间隔小于0，即这两个时间段存在重叠。因此，交集点“1-2”为冲突点。再以交集点“2-5”为例，第2条规划轨迹与第5条规划轨迹在交集点“2-5”处持续运动的时间段之间的间隔大于0，即这两个时间段完全错开。因此，交集点“2-5”不是冲突点。
各规划轨迹之间的交集点可以以矩阵的形式表示。例如，第i条轨迹与其它轨迹之间的交集点可以以下述矩阵表示：
X i＝[T 1…T j…T n]，i＝1,2,…,n
其中，T j表示第i条规划轨迹与第j条规划轨迹之间的交集点，n表示规划轨迹的数量。一般情况下，T j可以以如下形式表示：
Figure PCTCN2022123038-appb-000034
其中，m≥0，
Figure PCTCN2022123038-appb-000035
和
Figure PCTCN2022123038-appb-000036
分别表示第i条规划轨迹进入和离开其与第j条规划轨迹之间的第m+1个交集点的时间。当第i条规划轨迹与第j条规划轨迹之间不存在交集点时，或者j＝i时，规定T j＝0。
为了便于表示，以最短规划轨迹运动时间为归一化时间1，其它规划轨迹根据运动时间长度按比例进行折算。上述5条规划轨迹之间的交集点可表示如下：
Figure PCTCN2022123038-appb-000037
Figure PCTCN2022123038-appb-000038
Figure PCTCN2022123038-appb-000039
Figure PCTCN2022123038-appb-000040
Figure PCTCN2022123038-appb-000041
以上矩阵中的数字的下标0和1分别用于标记该数字为对应的规划轨迹进入和离开该交集点的时间，数字的上标0和1分别用于标记该数字为第i条划轨迹与第j条规划轨迹之间的第1个和第2个交集点的时间信息。对于第i条划轨迹与第j条规划轨迹之间存在更多交集点的情况可依此类推。在第i条划轨迹与第j条规划轨迹之间仅存在一个交集点的情况下，省略了数字的上标。
例如，可采用遍历法从交集点中搜索冲突点。从第1条规划轨迹的交集点X 1开始，将X 1的T 2、T 3、T 4、T 5分别与X 2、X 3、X 4、X 5的T 1相比较，将有时间重叠的交集点进行标记。然后，将第2条规划轨迹的交集点X 2的T 1、T 3、T 4、T 5分别与X 1、X 3、X 4、X 5的T 2相比较，并标记有时间重叠的交集点。如此循环，直到遍历完所有规划轨迹。
经过遍历，可找出上述5条规划轨迹之间的5个冲突点：“1-2”、“3-5”、“1-3”、“3-4”和“4-5”。在上述矩阵中以加粗的方式标记出各冲突点。在图8中，这5个冲突点在最后一条横线轴向上以矩形格的形式示出。
在找出冲突点之后，可执行冲突解除步骤S23。优选地，在冲突解除步骤S23中等量地延后被调整的规划轨迹在冲突点处的时间信息和被调整的规划轨迹在冲突点之后的部分的时间信息。由于所述多条规划轨迹本身是时间最优的轨迹规划，这种方式能够保证调整后的规划轨迹仍然满足移动机器人1的运动学和动力学约束，并且使所述多个移动机器人1在不发生碰撞的情况下整体上以尽量短的时间达到各自的目的地。因为上一层时间最优的全局轨迹规划代表了移动机器人1的最大运动能力，因此在解除冲突时，只将被调整的规划轨迹进入待解除的冲突点处的时间向后推迟。这种向后延迟将相应地影响到被调整的规划轨迹在该待解除的冲突点之后的所有时间信息。
在一个示例性实施例中，按规划轨迹对应的任务的优先级(或者说重要程度)选择待解除的冲突点和被调整的规划轨迹。如果不同的移动机器人1执行的任务具有不同的优先级，则当存在冲突点时，可优先固定具有高优先级任务的规划轨迹，依次通过调整与固定的规划轨迹发生冲突的规划轨迹来解除固定的规划轨迹的冲突点。
具体地说，冲突识别步骤S22和冲突解除步骤S23以下述方式执行：首先，识别所述多条规划轨迹之间的所有冲突点；将具有冲突点的规划轨迹按其对应的任务的优先级从高至低的顺序排序；选择排序最前的规划轨迹作为固定的规划轨迹，逐一将所述固定的规划轨迹的冲突点确定为待调整的冲突点，相应地将在待调整的冲突点处与所述固定的规划轨迹发生冲突的规划轨迹确定为被调整的规划轨迹，以解除所述固定的规划轨迹的所有冲突点；然后，再此执行冲突识别步骤S22以重新识别所述多条规划轨迹之间的所有冲突点。
以图8所示的5条规划轨迹为例。这5条规划轨迹都具有冲突点，将它们按其对应的任务的优先级从高至低的顺序排序。如果这5条规划轨迹的对应的任务的优先级从高到低依次排序为：1>2>3>4>5，则先固定第1条规划轨迹。逐一将第1条规划轨迹的冲突点“1-2”和“1-3”确定为待解除的冲突点，相应地被调整的规划轨迹分别为第2条规划轨迹和第3条规划轨迹。
在此，先调整第2条规划轨迹以解除第2条规划轨迹与第1条规划轨迹之间的冲突点“1-2”。 应理解，也可先调整第3条规划轨迹以解除第3条规划轨迹与第1条规划轨迹之间的冲突点“1-3”。
如上文所述，可采用等量地延后第2条规划轨迹在冲突点“1-2”处的时间信息和第2条规划轨迹在冲突点“1-2”之后的部分的时间信息的方式来解除冲突点。延后的时间量为冲突的另一方第1条规划轨迹进入冲突点“1-2”的时间点减去被调整的规划轨迹(即，第2条规划轨迹)离开待解除的冲突点“1-2”的时间点再加上预定的时间间隔阈值，在此示例性地为0.3-0.275+0＝0.025。
因此，第2条规划轨迹进入和离开各交集点的时间将发生变化，如下所示：
Figure PCTCN2022123038-appb-000042
图9示意性地示出了在解除冲突点“1-2”之后的交集点和冲突点。如图9所示，第2条规划轨迹在冲突点“1-2”处以及之后的时间信息将整体延后，第2条规划轨迹的总时间也相应地延长。其它轨迹保持不变。
然后，调整第3条规划轨迹以解除第3条规划轨迹与第1条规划轨迹之间的冲突点“1-3”。
在此，第3条规划轨迹在冲突点“1-3”处的时间信息和第3条规划轨迹在冲突点“1-3”之后的部分的时间信息都等量地延后0.67-0.58+0＝0.09。第3条规划轨迹进入和离开各交集点的时间将更新为：
Figure PCTCN2022123038-appb-000043
在解除固定的第1条规划轨迹的冲突点“1-2”和“1-3”之后，再次执行冲突识别步骤S22，以重新确定各规划轨迹之间的冲突点。如下述矩阵所示：
Figure PCTCN2022123038-appb-000044
Figure PCTCN2022123038-appb-000045
Figure PCTCN2022123038-appb-000046
Figure PCTCN2022123038-appb-000047
Figure PCTCN2022123038-appb-000048
可以看出，此时上述5条规划轨迹之间存在3个冲突点：“3-5”、“4-5”和“3-5”。具有冲突点的规划轨迹按对应的任务的优先级从高到低依次排序为：3>4>5。然后，固定第3条规划轨迹。调整与第3条规划轨迹之间存在冲突点的第5条规划轨迹以解除第3条规划轨迹与第5条规划轨迹之间的冲突点。在此，第3条规划轨迹与第5条规划轨迹之间存在两个冲突点，可按冲突发生的时间顺序依次解除第3条规划轨迹与第5条规划轨迹之间的第一个冲突点。
在解除第3条规划轨迹与第5条规划轨迹之间的第一个冲突点，更新第3条轨迹的时间信息：
Figure PCTCN2022123038-appb-000049
可以看出，在解除第3条规划轨迹与第5条规划轨迹之间的第一个冲突点的同时，由于第5条规划轨迹在该冲突点之后的部分的时间信息被相应地更新，第3条规划轨迹与第5条规划轨迹之间的第二个冲突点也被解除。
在此执行冲突识别步骤S22，识别结果如下述矩阵所示：
Figure PCTCN2022123038-appb-000050
Figure PCTCN2022123038-appb-000051
Figure PCTCN2022123038-appb-000052
Figure PCTCN2022123038-appb-000053
Figure PCTCN2022123038-appb-000054
上述5条规划轨迹之间还存在1个冲突点：“4-5”。具有冲突点的规划轨迹按对应的任务的优先级从高到低依次排序为：4>5。然后，固定第4条规划轨迹。调整与第4条规划轨迹之间存在冲突点的第5条规划轨迹以解除第4条规划轨迹与第5条规划轨迹之间的冲突点。
在解除第4条规划轨迹的冲突点之后，再次执行冲突识别步骤S22。各规划轨迹之间的交集点的时间信息如下所示：
Figure PCTCN2022123038-appb-000055
Figure PCTCN2022123038-appb-000056
Figure PCTCN2022123038-appb-000057
Figure PCTCN2022123038-appb-000058
Figure PCTCN2022123038-appb-000059
此时，各规划轨迹之间不再具有冲突点。
在根据本发明的另一示例性实施例中，按规划轨迹具有的冲突点的数量从少至多的顺序，选择待解除的冲突点和被调整的规划轨迹，以优先固定排序较前的规划轨迹使其无需被调整。
具体地说，冲突识别步骤S22和冲突解除步骤S23以下述方式执行：首先，识别所述多条规划轨迹之间的所有冲突点；将具有冲突点的规划轨迹按其冲突点的数量从少至多的顺序排序；选择排序最前的规划轨迹作为固定的规划轨迹，逐一将所述固定的规划轨迹的冲突点确定为待调整的冲突点，相应地将在待调整的冲突点处与所述固定的规划轨迹发生冲突的规划轨迹确定为被调整的规划轨迹，以解除所述固定的规划轨迹的所有冲突点；然后，再此执行冲突识别步骤S22以重新识别所述多条规划轨迹之间的所有冲突点。当有多条规划轨迹的冲突点数量一样且最少时，可依据进入冲突点的时间顺序来选择被调整的规划轨迹。
以图8所示的5条规划轨迹为例，这些规划轨迹之间存在5个冲突点：“1-2”、“3-5”、“1-3”、“3-4”和“4-5”。这5条规划轨迹都具有冲突点，将它们按其冲突点的数量从少至多的顺序排序：2<1＝4＝5<3。先固定第2条规划轨迹。被固定的第2条规划轨迹仅与第1条规划轨迹之间存在一个冲突点。因此，调整第1条规划轨迹以解除第1条规划轨迹与第2条规划轨迹之间的冲突点“1-2”。延后的时间量为0.325-0.25+0＝0.075。
在执行第一次冲突解除步骤S23之后，各规划轨迹之间的交集点的时间信息如下所示：
Figure PCTCN2022123038-appb-000060
Figure PCTCN2022123038-appb-000061
Figure PCTCN2022123038-appb-000062
Figure PCTCN2022123038-appb-000063
Figure PCTCN2022123038-appb-000064
再次执行冲突识别步骤S22，可识别出这些规划轨迹之间存在4个冲突点：“1-5”、“3-4”、“3-5”和“4-5”。对具有冲突点的第1、3、4、5条规划轨迹按其冲突点的数量从少至多的顺序排序：1<3＝4<5。先固定第1条规划轨迹。然后，调整第5条规划轨迹以解除第5条规划轨迹与第1条规划轨迹之间的冲突点“1-5”。延后的时间量为0.65-0.625+0＝0.025。
在执行第二次冲突解除步骤S23之后，各规划轨迹之间的交集点的时间信息如下所示：
Figure PCTCN2022123038-appb-000065
Figure PCTCN2022123038-appb-000066
Figure PCTCN2022123038-appb-000067
Figure PCTCN2022123038-appb-000068
Figure PCTCN2022123038-appb-000069
再次执行冲突识别步骤S22，可识别出这些规划轨迹之间存在3个冲突点：“3-4”、“3-5”和“4-5”。对具有冲突点的第3、4、5条规划轨迹按其冲突点的数量从少至多的顺序排序：3＝4＝5。此时，有3条规划轨迹的冲突点数量一样且最少，可依据进入冲突点的时间顺序来选择被调整的规划轨迹。例如，根据进入冲突点的时间顺序将冲突点数量一样且最少的规划轨迹排序：3<5<4。因此，可先固定第3条规划轨迹。然后，逐一第3条规划轨迹的冲突点“3-4”和“3-5”。
首先，调整第5条规划轨迹以解除第5条规划轨迹与第3条规划轨迹之间的冲突点“3-5”。延后的时间量为0.0425。
更新第5条轨迹的时间信息，可得：
Figure PCTCN2022123038-appb-000070
然后，调整第4条规划轨迹以解除第4条规划轨迹与第3条规划轨迹之间的冲突点“3-4”。延后的时间量为0.01。
在解除第3条规划轨迹的冲突点之后，各规划轨迹之间的交集点的时间信息如下所示：
Figure PCTCN2022123038-appb-000071
Figure PCTCN2022123038-appb-000072
Figure PCTCN2022123038-appb-000073
Figure PCTCN2022123038-appb-000074
Figure PCTCN2022123038-appb-000075
再次执行冲突识别步骤S22，可识别出这些规划轨迹之间存在1个冲突点：“4-5”。根据进入冲突点的时间顺序将冲突点数量一样且最少的第4条和第5条规划轨迹排序：4<5。因此，可先固定第4条规划轨迹。然后，调整第5条规划轨迹以解除第5条规划轨迹与第4条规划轨迹之间的冲突点“4-5”。延后的时间量为0.0425。
在解除第4条规划轨迹的冲突点之后，各规划轨迹之间的交集点的时间信息如下所示：
Figure PCTCN2022123038-appb-000076
Figure PCTCN2022123038-appb-000077
Figure PCTCN2022123038-appb-000078
Figure PCTCN2022123038-appb-000079
Figure PCTCN2022123038-appb-000080
此时，各规划轨迹之间不再具有冲突点。
在根据本发明的又一示例性实施例中，按规划轨迹具有的冲突点的冲突持续时间从少至多的顺序，选择待解除的冲突点和被调整的规划轨迹，以优先固定排序较前的规划轨迹不被调整。
具体地说，冲突识别步骤S22和冲突解除步骤S23以下述方式执行：首先，识别所述多条规划轨迹之间的所有冲突点；将具有冲突点的规划轨迹按具有的冲突点的冲突持续时间从少至多的顺序排序；选择排序最前的规划轨迹作为固定的规划轨迹，逐一将所述固定的规划轨迹的冲突点确定为待调整的冲突点，相应地将在待调整的冲突点处与所述固定的规划轨迹发生冲突的规划轨迹确定为被调整的规划轨迹，以解除所述固定的规划轨迹的所有冲突点；然后，再此执行冲突识别步骤S22以重新识别所述多条规划轨迹之间的所有冲突点。当有多条规划轨迹的冲突点数量一样且最少时，可依据进入冲突点的时间顺序来选择被调整的规划轨迹。
以图8所示的5条规划轨迹为例，这些规划轨迹之间存在5个冲突点：“1-2”、“3-5”、“1-3”、“3-4”和“4-5”。这5条规划轨迹都具有冲突点，将它们按其冲突点的冲突持续时间从少至多的顺序排序：2<1<3<4<5。先固定第2条规划轨迹。被固定的第2条规划轨迹仅与第1条规划轨迹之间存在一个冲突点。因此，调整第1条规划轨迹以解除第1条规划轨迹与第2条规划轨迹之间的冲突点“1-2”。延后的时间量为0.325-0.25+0＝0.075。
在执行第一次冲突解除步骤S23之后，各规划轨迹之间的交集点的时间信息如下所示：
Figure PCTCN2022123038-appb-000081
Figure PCTCN2022123038-appb-000082
Figure PCTCN2022123038-appb-000083
Figure PCTCN2022123038-appb-000084
Figure PCTCN2022123038-appb-000085
再次执行冲突识别步骤S22，可识别出这些规划轨迹之间存在4个冲突点：“1-5”、“3-4”、“3-5”和“4-5”。在具有冲突点的第1、3、4、5条规划轨迹中，第1条规划轨迹的冲突点的冲突持续时间最短。先固定第1条规划轨迹。然后，调整第5条规划轨迹以解除第5条规划轨迹与第1条规划轨迹之间的冲突点“1-5”。延后的时间量为0.65-0.625+0＝0.025。
在执行第二次冲突解除步骤S23之后，各规划轨迹之间的交集点的时间信息如下所示：
Figure PCTCN2022123038-appb-000086
Figure PCTCN2022123038-appb-000087
Figure PCTCN2022123038-appb-000088
Figure PCTCN2022123038-appb-000089
Figure PCTCN2022123038-appb-000090
再次执行冲突识别步骤S22，可识别出这些规划轨迹之间存在3个冲突点：“3-4”、“3-5”和“4-5”。对具有冲突点的第3、4、5条规划轨迹按其冲突点的冲突持续时间从少至多的顺序排序：3<4<5。因此，可先固定第3条规划轨迹。然后，逐一解除冲突点“3-4”和“3-5”。
例如，可首先调整第4条规划轨迹以解除第4条规划轨迹与第3条规划轨迹之间的冲突点“3-4”。延后的时间量为0.01。
然后，调整第5条规划轨迹以解除第5条规划轨迹与第3条规划轨迹之间的冲突点“3-5”。延后的时间量为0.0425。
在解除第3条规划轨迹的冲突点之后，各规划轨迹之间的交集点的时间信息如下所示：
Figure PCTCN2022123038-appb-000091
Figure PCTCN2022123038-appb-000092
Figure PCTCN2022123038-appb-000093
Figure PCTCN2022123038-appb-000094
Figure PCTCN2022123038-appb-000095
此时，各规划轨迹之间存在1个冲突点：“4-5”。根据进入冲突点的时间顺序将冲突点数量一样且最少的第4条和第5条规划轨迹排序：4<5。因此，可先固定第4条规划轨迹。然后，调整第5条规划轨迹以解除第5条规划轨迹与第4条规划轨迹之间的冲突点“4-5”。延后的时间量为0.0425。
之后，各规划轨迹之间的交集点的时间信息如下所示：
Figure PCTCN2022123038-appb-000096
Figure PCTCN2022123038-appb-000097
Figure PCTCN2022123038-appb-000098
Figure PCTCN2022123038-appb-000099
Figure PCTCN2022123038-appb-000100
此时，各规划轨迹之间不再具有冲突点。
在根据本发明的另外的示例性实施例中，按冲突点的发生时间的先后顺序，选择待解除的冲突点和被调整的规划轨迹。
具体地说，冲突识别步骤S22和冲突解除步骤S23以下述方式执行：首先，执行冲突识别步骤S22以识别所述多条规划轨迹之间的所有冲突点；将冲突点按照其发生时间的先后顺序进行排序，选择排序最前的冲突点作为待解除的冲突点，选择与待解除的冲突点相关联的两条规划轨迹中进入冲突点的时间较晚的规划轨迹作为被调整的规划轨迹，以解除所述冲突点待解除的冲突点；再此执行冲突识别步骤S22以重新识别所述多条规划轨迹之间的所有冲突点。
以图8所示的5条规划轨迹为例，这些规划轨迹之间存在5个冲突点：“1-2”、“3-5”、“1-3”、“3-4”和“4-5”。按冲突点按其发生时间的先后顺序进行排序为：“1-2”、“3-5”、“1-3”、“3-4”和“4-5”。由此，可确定“1-2”为待解除的冲突点。在参与冲突点“1-2”的第1条规划轨迹和第2条规划轨迹中，第2条规划轨迹较晚进入冲突点“1-2”，因此，将第2条规划轨迹确定为被调整的规划轨迹。然后，调整第2条规划轨迹以解除第2条规划轨迹与第1条规划轨迹之间的冲突点“1-2”。延后的时间量为0.3-0.275+0＝0.025。
在执行第一次冲突解除步骤S23之后，各规划轨迹之间的交集点的时间信息如下所示：
Figure PCTCN2022123038-appb-000101
Figure PCTCN2022123038-appb-000102
Figure PCTCN2022123038-appb-000103
Figure PCTCN2022123038-appb-000104
Figure PCTCN2022123038-appb-000105
再次执行冲突识别步骤S22，可识别出这些规划轨迹之间存在4个冲突点：“1-3”、“3-4”、“3-5”和“4-5”。冲突点按其发生时间的先后顺序进行排序为：“3-5”、“1-3”、“3-4”和“4-5”。因此，调整第5条规划轨迹以解除第5条规划轨迹与第3条规划轨迹之间的冲突点“3-5”。延后的时间量为0.0425。
在执行第二次冲突解除步骤S23之后，各规划轨迹之间的交集点的时间信息如下所示：
Figure PCTCN2022123038-appb-000106
Figure PCTCN2022123038-appb-000107
Figure PCTCN2022123038-appb-000108
Figure PCTCN2022123038-appb-000109
Figure PCTCN2022123038-appb-000110
然后，如上所述地重复执行冲突识别步骤S22和冲突解除步骤S23，直到这5条规划轨迹中的任意两条规划轨迹之间都不存在冲突点。
替代地，也可以以其它方式执行冲突识别步骤S22和冲突解除步骤S23。例如，在识别所述多条规划轨迹之间的所有冲突点之后，将具有冲突点的规划轨迹以下述方式中的至少一者进行排序：按其对应的任务的优先级从高至低的顺序；按其冲突点的数量从少至多的顺序；按其冲突点的冲突持续时间从少至多的顺序；按其进入冲突点的时间的先后顺序。然后，选择在与排序最前的规划轨迹之间存在冲突点的规划轨迹中排序最前的一条作为被调整的规划轨迹，并且调整被调整的规划轨迹的时间信息以解除被调整的规划轨迹与排序较前的规划轨迹之间的冲突点。
另外，本发明还涉及一种计算机程序产品，其包括计算器程序指令，当所述计算机程序指令被一个或多于一个处理器执行时，所述处理器能够执行根据本发明的轨迹规划方法和/或多机器人轨迹规划方法。
在本发明中，计算机程序产品可存储在计算机可读存储介质中。计算机可读存储介质例如可包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡，安全数字卡，闪存卡、至少一个磁盘存储器件、闪存器件、或其它易失性固态存储器件。处理器可 以是中央处理单元，还可以是其它通用处理器、数字信号处理器、专用集成电路、现成可编程门阵列或者其它可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者也可以是任何常规的处理器等。
尽管这里详细描述了本发明的特定实施方式，但它们仅仅是为了解释的目的而给出的，而不应认为它们对本发明的范围构成限制。在不脱离本发明精神和范围的前提下，各种替换、变更和改造可被构想出来。

Claims (19)

  1. 一种多机器人轨迹规划方法，其中，所述多机器人轨迹规划方法包括：
    初步规划步骤S21，其中，获取相应地用于多个移动机器人(1)的包含时间信息的多条规划轨迹，所述多条规划轨迹是通过对所述多个移动机器人(1)分别进行时间最优轨迹规划方法而生成的规划轨迹；
    冲突识别步骤S22，其中，识别所述多条规划轨迹中的两条规划轨迹之间在空间和时间维度上的冲突点，所述冲突点表示按照所述两条规划轨迹运动的移动机器人(1)将在相同的时间到达相同的位置；以及
    冲突解除步骤S23，其中，通过调整所述两条规划轨迹中的一条规划轨迹的时间信息解除所述冲突。
  2. 根据权利要求1所述的多机器人轨迹规划方法，其中，
    重复执行冲突识别步骤S22和冲突解除步骤S23直到所述多条规划轨迹中的任意两条规划轨迹之间都不存在冲突点。
  3. 根据权利要求1或2所述的多机器人轨迹规划方法，其中，
    冲突识别步骤S22包括：
    查找所述多条规划轨迹中的每两条规划轨迹在空间维度上的所有交集点；以及
    针对每个交集点，检查相关的规划轨迹在所述交集点处的时间信息之间的时间间隔，若所述时间间隔小于预定的时间间隔阈值，则将相应的交集点识别为冲突点。
  4. 根据权利要求3所述的多机器人轨迹规划方法，其中，
    冲突解除步骤S23包括：
    子步骤S231：从识别出的冲突点和发生冲突的规划轨迹中选择待解除的冲突点和被调整的规划轨迹，其中，所述被调整的规划轨迹为与待解除的冲突点相关联的两条规划轨迹中的一条或者所述待解除的冲突点为与被调整的规划轨迹所具有的冲突点中的一个，
    子步骤S232：以延后被调整的规划轨迹在冲突点处的时间信息的方式调整被调整的规划轨迹在待解除的冲突点处的时间信息，使得所述相关联的两条规划轨迹在冲突点处的时间信息之间的时间间隔大于或等于时间间隔阈值，
    子步骤S233：基于被调整的规划轨迹在冲突点处的调整后的时间信息，相应地更新被调整的规划轨迹在冲突点之后的部分的时间信息。
  5. 根据权利要求4所述的多机器人轨迹规划方法，其中，
    子步骤S232和子步骤S233以下述方式执行：等量地延后被调整的规划轨迹在冲突点处的时间信息和被调整的规划轨迹在冲突点之后的部分的时间信息；和/或
    延后的时间量为与待解除的冲突点相关联的另一规划轨迹进入待解除的冲突点的时间点减去被调整的规划轨迹离开待解除的冲突点的时间点再加上预定的时间间隔阈值。
  6. 根据权利要求4或5所述的多机器人轨迹规划方法，其中，
    冲突识别步骤S22和冲突解除步骤S23以下述方式执行：
    执行冲突识别步骤S22以识别所述多条规划轨迹之间的所有冲突点；
    将具有冲突点的规划轨迹以下述方式中的至少一者进行排序：
    按其对应的任务的优先级从高至低的顺序；
    按其冲突点的数量从少至多的顺序；
    按其冲突点的冲突持续时间从少至多的顺序；
    按其进入冲突点的时间的先后顺序；
    选择排序最前的规划轨迹作为固定的规划轨迹，逐一将所述固定的规划轨迹的冲突点确定为待调整的冲突点，相应地将在待调整的冲突点处与所述固定的规划轨迹发生冲突的规划轨迹确定为被调整的规划轨迹，以解除所述固定的规划轨迹的所有冲突点；
    再此执行冲突识别步骤S22以重新识别所述多条规划轨迹之间的所有冲突点。
  7. 根据权利要求4或5所述的多机器人轨迹规划方法，其中，
    冲突识别步骤S22和冲突解除步骤S23以下述方式执行：
    执行冲突识别步骤S22以识别所述多条规划轨迹之间的所有冲突点；
    将冲突点按照其发生时间的先后顺序进行排序，
    选择排序最前的冲突点作为待解除的冲突点，选择与待解除的冲突点相关联的两条规划轨迹中进入冲突点的时间较晚的规划轨迹作为被调整的规划轨迹，以解除所述冲突点待解除的冲突点；
    再此执行冲突识别步骤S22以重新识别所述多条规划轨迹之间的所有冲突点。
  8. 根据权利要求1-7中任一项所述的多机器人轨迹规划方法，其中，
    所述时间最优轨迹规划方法能够根据确定的路径(2)对移动机器人(1)进行速度规划，以确定使移动机器人(1)能够沿着所述路径(2)运动的包含时间信息的规划轨迹，所述轨迹规划方法包括：
    确定移动机器人(1)的至少两个驱动轮中的一个为受约束轮，使得只要所述受约束轮满足运动学和动力学约束，与受约束轮相配合地运动的其它驱动轮就会满足运动学和动力学约束；
    基于所述路径(2)在满足受约束轮的运动学和动力学约束的情况下对受约束轮进行速度规划，以确定受约束轮的速度，使得受约束轮在任意点都具有满足其运动学和动力学约束并且满足所述路径(2)的限制的情况下的最大的速度和最大的加速度中的一者；
    以配合所确定的受约束轮的速度的方式，对受约束轮之外的其它驱动轮进行速度规划。
  9. 根据权利要求8所述的多机器人轨迹规划方法，其中，
    对受约束轮进行速度规划的过程中，采用T形规划方法。
  10. 根据权利要求8或9所述的多机器人轨迹规划方法，其中，
    运动学和动力学约束包括：
    驱动轮的速度的大小在针对所述驱动轮预定的极限轮速度以下；
    驱动轮的加速度的大小在针对所述驱动轮预定的极限轮加速度以下。
  11. 根据权利要求10所述的多机器人轨迹规划方法，其中，
    对所述路径(2)分区段地进行速度规划，对所述路径(2)的至少一个区段分别执行下述步骤：
    针对作为所述区段的起点的第一控制点，根据所述区段的路径形状、各驱动轮在第一控制点处的运动状态以及驱动轮的运动学和动力学约束，确定所述至少两个驱动轮中的一个为所述区段中的受约束轮，所述受约束轮为：在所述区段中，根据所述区段的路径形状和各驱动轮在第一控制点处的运动状态，优先达到运动学或动力学约束的极限值的驱动轮；
    对受约束轮进行速度规划以确定受约束轮在所述区段内的速度；
    以配合所确定的受约束轮的速度的方式，确定所述其它驱动轮在区段内的速度。
  12. 根据权利要求11所述的多机器人轨迹规划方法，其中，
    移动机器人(1)为双差速轮机器人，所述至少两个驱动轮为对称地布置的第一驱动轮和第二驱动轮，其中，第一驱动轮和第二驱动轮受到相同的运动学和动力学约束。
  13. 根据权利要求12所述的多机器人轨迹规划方法，其中，
    以下述方式确定各个区段中的受约束轮：
    获取第一驱动轮和第二驱动轮在第一控制点处的第一初速度v L0和第二初速度v R0；
    确定作为所述区段的终点的第二控制点处的由所述路径(2)决定的速度比k的取值k1，所述速度比k表示第二驱动轮与第一驱动轮的速度之比；
    分别确定第一驱动轮和第二驱动轮在第二控制点处的第一最大速度v Lmax和第二最大速度v Rmax，所述第一最大速度和第二最大速度分别表示在不考虑第一驱动轮和第二驱动轮在达到第二控制点之前的速度的情况下满足各驱动轮的运动学和动力学约束并且满足所述路径(2)的限制的最大的速度；
    将第一驱动轮从第一控制点处的第一初速度v L0开始以第一驱动轮的极限轮加速度加速至第二控制点所得到的速度确定为第一加速终速度v La，将第二驱动轮从第一控制点处的第二初速度v R0开始以第二驱动轮的极限轮加速度加速至第二控制点所得到的速度确定为第二加速终速度v Ra；
    将第二控制点处的第一最大速度v Lmax与第一加速终速度v La中的较小者确定为第一终速度v L，将第二控制点处的第二最大速度v Rmax与第二加速终速度v Ra中的较小者确定为第二终速度v R；
    将第二终速度v R与第一终速度v L之比与第二控制点处的速度比k1相比较，并根据比较结果确定所述区段中的受约束轮。
  14. 根据权利要求13所述的多机器人轨迹规划方法，其中，
    如果第二终速度v R与第一终速度v L之比大于第二控制点处的速度比k1，则确定第一驱动轮为所述区段中的受约束轮；
    如果第二终速度v R与第一终速度v L之比小于第二控制点处的速度比k1，则确定第二驱动轮为所述区段中的受约束轮；
    如果第二终速度v R与第一终速度v L之比等于第二控制点处的速度比k1，则确定第一驱动轮与第二驱动轮中的一者为所述区段中的受约束轮。
  15. 根据权利要求13或14所述的多机器人轨迹规划方法，其中，
    各区段对应的运动时长等于预定的控制周期t，第一加速终速度v La和第二加速终速度v Ra根据下式来确定：
    v La＝v L0+a*t
    v Ra＝v R0+a*t
    其中，a表示第一驱动轮和第二驱动轮的极限轮加速度。
  16. 根据权利要求13-15中任一项所述的多机器人轨迹规划方法，其中，
    第一驱动轮和第二驱动轮在路径(2)上的任意点处的第一最大速度v Lmax和第二最大速度v Rmax根据下述约束中的至少一者来确定：
    -基于极限轮速度v lim的第一约束：v Lmax≤v lim，
    -基于极限轮速度v lim和由路径(2)决定的速度比的第二约束：v Lmax≤v lim/k，
    -基于极限轮加速度a和由路径(2)决定的速度比变化率k’的第三约束：
    Figure PCTCN2022123038-appb-100001
    其中，k′≠0；并且
    第一驱动轮和第二驱动轮在路径(2)上的任意点处的第一最大速度v Lmax和第二最大速度v Rmax满足：v Rmax＝v Lma*k。
  17. 根据权利要求16所述的多机器人轨迹规划方法，其中，
    第一驱动轮和第二驱动轮在路径(2)上的任意点处的第一最大速度v Lmax和第二最大速度v Rmax附加地根据下述第四约束来确定：
    确定假设沿着所述路径(2)第一驱动轮以由第一约束、第二约束和第三约束中的所述至少一者确定的最大速度运动而得到随第一驱动轮的运动距离L L变化的第一初步最大速度和随第二驱动轮的运动距离L R变化的第二初步最大速度中的至少一者；
    确定第一初步最大速度和第二初步最大速度中的所述至少一者的极大值点和极小值点；
    所述任意点处的第一最大速度v Lmax和/或第二初步最大速度v Rmax满足：
    如果所述任意点在距离所述任意点最近的极小值点的后方，则：
    Figure PCTCN2022123038-appb-100002
    和/或
    Figure PCTCN2022123038-appb-100003
    如果所述任意点在距离所述任意点最近的极小值点的前方，则：
    Figure PCTCN2022123038-appb-100004
    和/或
    Figure PCTCN2022123038-appb-100005
    其中，L L和L R分别表示第一驱动轮和第二驱动轮运动到所述任意点处的运动距离，v 1相应地表示距离所述任意点最近的极小值点的第一初步最大速度或第二初步最大速度，L L1和L R1分别表示第一驱动轮和第二驱动轮运动到所述最近的极小值点处的运动距离。
  18. 根据权利要求8-17中任一项所述的多机器人轨迹规划方法，其中，
    所述路径(2)是通过根据移动机器人(1)的至少一个任务点进行全局路径规划而确定的全局路径，所述至少一个任务点位于所述全局路径上；和/或
    所述路径(2)呈3阶以上贝塞尔曲线的形式。
  19. 一种计算机程序产品、特别是计算机可读存储介质，其包括计算器程序指令，其中，当所述计算机程序指令被一个或多于一个处理器执行时，所述处理器能够执行根据权利要求1-18中任一项所述的多机器人轨迹规划方法。
PCT/CN2022/123038 2021-12-09 2022-09-30 多机器人轨迹规划方法及计算机程序产品 WO2023103553A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202111498229.9 2021-12-09
CN202111498229.9A CN116257044A (zh) 2021-12-09 2021-12-09 多机器人轨迹规划方法及计算机程序产品

Publications (1)

Publication Number Publication Date
WO2023103553A1 true WO2023103553A1 (zh) 2023-06-15

Family

ID=86681168

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2022/123038 WO2023103553A1 (zh) 2021-12-09 2022-09-30 多机器人轨迹规划方法及计算机程序产品

Country Status (2)

Country Link
CN (1) CN116257044A (zh)
WO (1) WO2023103553A1 (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102011081995A1 (de) * 2011-09-01 2012-10-25 Siemens Ag Fahrtoptimierungsmodul
CN103310118A (zh) * 2013-07-04 2013-09-18 文超 高速铁路列车运行冲突预测方法
CN107085432A (zh) * 2017-06-22 2017-08-22 星际（重庆）智能装备技术研究院有限公司 一种移动机器人的目标轨迹跟踪方法
CN112034842A (zh) * 2020-01-23 2020-12-04 沈阳工业大学 适用于不同使用者的服务机器人速度约束跟踪控制方法
CN112193244A (zh) * 2020-09-30 2021-01-08 浙江大学 基于线性约束的自动驾驶车辆运动规划方法
CN113156977A (zh) * 2021-05-21 2021-07-23 山东大学 基于时间优化的多机器人复杂交错轨迹规划方法及系统

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102011081995A1 (de) * 2011-09-01 2012-10-25 Siemens Ag Fahrtoptimierungsmodul
CN103310118A (zh) * 2013-07-04 2013-09-18 文超 高速铁路列车运行冲突预测方法
CN107085432A (zh) * 2017-06-22 2017-08-22 星际（重庆）智能装备技术研究院有限公司 一种移动机器人的目标轨迹跟踪方法
CN112034842A (zh) * 2020-01-23 2020-12-04 沈阳工业大学 适用于不同使用者的服务机器人速度约束跟踪控制方法
CN112193244A (zh) * 2020-09-30 2021-01-08 浙江大学 基于线性约束的自动驾驶车辆运动规划方法
CN113156977A (zh) * 2021-05-21 2021-07-23 山东大学 基于时间优化的多机器人复杂交错轨迹规划方法及系统

Also Published As

Publication number Publication date
CN116257044A (zh) 2023-06-13

Similar Documents

Publication Publication Date Title
CN110989570B (zh) 一种多agv防碰撞协同路径规划方法
CN110530369B (zh) 基于时间窗的agv任务调度方法
CN107203190B (zh) 一种基于复杂路径的惯性导航agv调度方法及系统
CN108762268B (zh) 多agv无碰撞路径规划算法
CN110268457B (zh) 用于确定至少两个车辆的集体操纵的方法、计算机程序产品、计算机能读取的介质、控制器和包括该控制器的车辆
CN111474926B (zh) 一种基于多agv时间窗路径优化算法的废烟回收方法
Guney et al. Dynamic prioritized motion coordination of multi-AGV systems
JP2024020457A (ja) 情報処理装置、情報処理方法、コンピュータプログラム及び情報処理システム
WO2020007855A1 (en) An agv system and a method of controlling an agv system
US9703292B2 (en) Automatic traveling vehicle system, control method and program
CN111445723A (zh) 车辆路径识别
Xiao et al. A collision and deadlock prevention method with traffic sequence optimization strategy for UGN-based AGVs
CN115328113A (zh) 基于改进时间窗算法的多物流机器人路径规划方法
CN110412990B (zh) 一种用于工厂环境下的agv避碰方法
CN114407929A (zh) 无人驾驶绕障处理方法、装置、电子设备及存储介质
Solichudin et al. Conflict-free dynamic route multi-agv using dijkstra Floyd-warshall hybrid algorithm with time windows
WO2023103553A1 (zh) 多机器人轨迹规划方法及计算机程序产品
WO2022032444A1 (zh) 一种多智能主体避障方法、系统和计算机可读存储介质
WO2023103554A1 (zh) 用于移动机器人的轨迹规划方法及计算机程序产品
CN115547023A (zh) 一种车辆控制方法、装置及系统
US20210004023A1 (en) Self-propelled device, self-propelling method, and recording medium
JP6656296B2 (ja) コントローラ、自動走行車システム、制御方法及びプログラム
Keppler et al. SAFESTOP: Disturbance handling in prioritized multi-robot trajectory planning
Li Task Assignment and Path Planning for Autonomous Mobile Robots in Stochastic Warehouse Systems
Dobrzanska et al. Simulation modelling of material handling using AGV

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 22902978

Country of ref document: EP

Kind code of ref document: A1