CN115993815A - Motion control method for mobile robot and computer program product - Google Patents
Motion control method for mobile robot and computer program product Download PDFInfo
- Publication number
- CN115993815A CN115993815A CN202111208623.4A CN202111208623A CN115993815A CN 115993815 A CN115993815 A CN 115993815A CN 202111208623 A CN202111208623 A CN 202111208623A CN 115993815 A CN115993815 A CN 115993815A
- Authority
- CN
- China
- Prior art keywords
- mobile robot
- path
- detour
- time
- point
- Prior art date
- Legal status (The legal status 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 status listed.)
- Pending
Links
- 230000033001 locomotion Effects 0.000 title claims abstract description 92
- 238000000034 method Methods 0.000 title claims abstract description 56
- 238000004590 computer program Methods 0.000 title claims abstract description 13
- 230000001133 acceleration Effects 0.000 claims description 17
- 230000006870 function Effects 0.000 description 9
- 230000008859 change Effects 0.000 description 8
- 230000036962 time dependent Effects 0.000 description 6
- 238000001514 detection method Methods 0.000 description 5
- 238000004891 communication Methods 0.000 description 3
- 230000004888 barrier function Effects 0.000 description 2
- 239000007787 solid Substances 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000014509 gene expression Effects 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000035772 mutation Effects 0.000 description 1
- 230000008092 positive effect Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 239000000758 substrate Substances 0.000 description 1
- 238000010408 sweeping Methods 0.000 description 1
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
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)
- Manipulator (AREA)
Abstract
A motion control method for a mobile robot (1) is proposed, comprising: an initial planning step S1, wherein a first planning track containing time information enabling the mobile robot (1) to complete a preset first path (2) in the shortest time is acquired, and a target point is located on the first path (2); a time determination step S2, wherein a first arrival time of the mobile robot (1) reaching the target point and a desired arrival time of the mobile robot (1) reaching the target point in the first planned trajectory are determined; and a re-planning step S3, wherein if the first arrival time is earlier than the expected arrival time, the expected arrival time is taken as the planned arrival time, and the time information of the first planned trajectory is changed to acquire a second planned trajectory, so that the mobile robot (1) can arrive at the target point at the planned arrival time according to the second planned trajectory. A corresponding computer program product is also presented. By means of the invention, the punctuality of the mobile robot can be improved.
Description
Technical Field
The present invention relates to the field of mobile robots, in particular to the field of motion control of mobile robots, in particular to a motion control method for a mobile robot and a computer program product.
Background
With rapid economic growth and gradual rise in human costs, mobile robots are increasingly being used in a variety of industrial and home environments. For example, automatic Guided Vehicles (AGVs), autonomous Mobile Robots (AMR), forklift, and like mobile robots are one of the key devices of modern logistics systems. The mobile robot can move and stop to a target place according to path planning and operation requirements so as to complete tasks such as material handling and conveying.
In general, a mobile robot needs to reach a specific location (task point), for example, due to the task it is to perform. For example, a mobile robot for transporting goods in a warehouse may need to move to a specific receiving location to receive the goods to be transported, and then transport the goods to be transported to a designated delivery location. For this purpose, the mobile robot can be moved to a specific position by a specific local path planning method or the like.
Mobile robots also need to reach a particular location at a particular time in certain scenarios.
The prior art still has shortcomings in on-time arrival for mobile robots.
Disclosure of Invention
An object of the present invention is to provide an improved motion control method for a mobile robot to improve punctuality of the mobile robot.
According to a first aspect of the present invention, there is provided a motion control method for a mobile robot, wherein the motion control method comprises the steps of: an initial planning step S1, wherein a first planning track containing time information capable of enabling the mobile robot to complete a preset first path in the shortest time is obtained, and a target point is located on the first path; a time determination step S2, in which a first arrival time of the mobile robot at the target point in the first planned trajectory and a desired arrival time of the mobile robot at the target point are determined; and a re-planning step S3, wherein if the first arrival time is earlier than the expected arrival time, the expected arrival time is taken as the planned arrival time, and the time information of the first planned trajectory is changed to acquire a second planned trajectory, so that the mobile robot can arrive at the target point at the planned arrival time according to the second planned trajectory.
In one exemplary embodiment, the initial planning step S1 includes: based on a curve equation of the first path, carrying out speed planning on the mobile robot, and determining that the mobile robot completes a planning track containing time information of the first path in the shortest time as a first planning track under the condition that the kinematic constraint and the dynamic constraint of the mobile robot are met; the first planning track satisfies the equation of motion 
wherein ,/>
Representing the position of the mobile robot, function G (t) representing the position +.>
As a function of time t.
In one exemplary embodiment, the first path is a local path obtained by local path planning of the mobile robot; the first path is in the form of a 3 rd order or higher bezier curve and can be expressed by the following formula:
wherein ,
representing the position of the mobile robot, when the variable s increases from 0 to 1, corresponding +.>
Representing a position of the mobile robot along the first path from the start point to the end point; in the first planned trajectory, the first trajectory is a trajectory,
the function g (t) represents the variable s as a function of time t.
In one exemplary embodiment, the kinematic and dynamic constraints of the mobile robot include: the speed of the mobile robot at the start of the first path is equal to a predetermined initial speed; the speed of the mobile robot in the first planned trajectory is below a predetermined maximum speed; the acceleration of the mobile robot in the first planned trajectory is below a predetermined maximum acceleration; and/or the mobile robot is a differential wheel robot, the kinematic and kinetic constraints of the mobile robot include: the initial speeds of a first driving wheel and a second driving wheel of the mobile robot at the starting point of the first path are respectively equal to a preset left wheel initial speed and a preset right wheel initial speed; the speeds of a first driving wheel and a second driving wheel of the mobile robot in the first planned track are respectively below a preset maximum wheel speed; acceleration of a first driving wheel and a second driving wheel of the mobile robot in the first planned trajectory is respectively below a predetermined maximum wheel acceleration.
In one exemplary embodiment, the speed planning is performed according to a T-type planning method.
In one exemplary embodiment, the re-planning step S3 includes: determining a ratio of a first duration from a start time of the first track to a first arrival time to a planned duration from the start time of the first track to a planned arrival time as a scaling ratio k; the speed planning corresponding to the first planning track is scaled according to the scaling ratio to obtain a second planning track, so that the second planning track meets the motion equation
In an exemplary embodiment, the re-planning step S3 further includes: the adjustment section of the second planned trajectory obtained by scaling is adjusted from the start point thereof such that the speeds of the first and second drive wheels of the mobile robot at the start point of the second planned trajectory are equal to the predetermined left and right wheel initial speeds, respectively, and the speeds of the first and second drive wheels of the mobile robot at the end point of the adjustment section and the position of the mobile robot are the same as before the adjustment.
In an exemplary embodiment, the target point is a task point related to a task to be performed by the mobile robot, the desired arrival time being determined from the task.
In one exemplary embodiment, the motion control method includes: and if the first arrival time is not earlier than the expected arrival time, controlling the mobile robot to move according to the first planned track.
In one exemplary embodiment, the target point is a position at which the mobile robot will collide with the moving dynamic obstacle according to the first planned trajectory; the time determination step S2 includes: the expected arrival time is determined according to the motion state of the dynamic obstacle so that the mobile robot does not collide with the dynamic obstacle if the time at which the mobile robot arrives at the target point is not earlier than the expected arrival time.
In an exemplary embodiment, the motion control method further comprises a detour planning step comprising: determining a third path capable of enabling the mobile robot to pass in front of the dynamic obstacle through at least one front end detour point without collision, wherein a starting point and an ending point of the third path are both located on the first path; generating a third planned trajectory containing time information that enables the mobile robot to move along a third path, according to which the mobile robot will pass through an intersection of the third path and a movement path of the dynamic obstacle before the dynamic obstacle; if the generation of the third planned trajectory is successful, the third planned trajectory is taken as one of the candidate detour trajectories.
In one exemplary embodiment, the front end detour point is determined according to the profile of the mobile robot, the profile of the dynamic obstacle, the movement speed and movement direction of the dynamic obstacle; determining an expected detour time corresponding to the front end detour point such that the mobile robot can detour the dynamic obstacle without collision as long as the mobile robot passes the front end detour point along the third path before the expected detour time; generating a planned track containing time information, which enables the mobile robot to complete a third path in a shortest time, as a third planned track, wherein the mobile robot will pass through a front-end detour point at a corresponding front-end detour time according to the third planned track; if the front-end detour time is correspondingly before the expected detour time for each front-end detour point, the generation of the third planned trajectory is successful, otherwise, the re-determination of the third path and the third planned trajectory by changing the position of the front-end detour point or the generation of the third planned trajectory fails.
In one exemplary embodiment, the third path is in the form of a bezier curve of fourth order or more, wherein, in case the third path passes through m front-end detour points, the third path is in the form of a bezier curve of m+3 order; in the third path, the mobile robot will have a velocity direction perpendicular to the velocity direction that the dynamic obstacle would have if it were to collide with the mobile robot moving according to the first planned trajectory at the front end detour point.
In an exemplary embodiment, the motion control method further comprises a detour planning step comprising: determining a fourth path capable of enabling the mobile robot to pass through at least one rear end detour point to detour from the rear of the dynamic obstacle without collision, wherein the starting point and the ending point of the fourth path are both located on the first path; generating a fourth planned trajectory including time information that enables the mobile robot to move along a fourth path, according to which the mobile robot will pass through an intersection of the fourth path and a movement path of the dynamic obstacle after the dynamic obstacle; and if the fourth planning track is successfully generated, taking the fourth planning track as one of candidate detour tracks.
In one exemplary embodiment, the back-end detour point is determined according to the profile of the mobile robot, the profile of the dynamic obstacle, the movement speed and movement direction of the dynamic obstacle; determining an expected detour time corresponding to the back-end detour point such that the mobile robot can detour the dynamic obstacle without collision as long as the mobile robot passes the back-end detour point after the expected detour time along the fourth path; if the back-end detour time is correspondingly after the expected detour time for each back-end detour point, the fourth planned track is generated successfully, otherwise, the position of the back-end detour point is changed to redetermine the fourth path and the fourth planned track or the fourth planned track is generated in failure.
In one exemplary embodiment, the fourth path is in the form of a bezier curve of fourth order or more, and in the case where the fourth path passes through n back-end detour points, the fourth path is in the form of a bezier curve of n+3 order; in the fourth path, the mobile robot will have a velocity direction perpendicular to the velocity direction that the dynamic obstacle would have if it were to collide with the mobile robot moving according to the first planned trajectory at the rear-end detour point.
In one exemplary embodiment, the mobile robot is controlled to move according to one of the second planned trajectory and the candidate detour trajectory that detours around the dynamic obstacle in the shortest time.
According to a second aspect of the present invention there is provided a computer program product comprising computer program instructions, wherein the computer program instructions, when executed by one or more processors, are capable of performing the motion control method according to the present invention.
The invention has the positive effects that: by this motion control method, the mobile robot can be made to arrive at the target point at the desired arrival time, with its motion capabilities allowed.
Drawings
The principles, features and advantages of the present invention may be better understood by describing the present invention in more detail with reference to the drawings. The drawings include:
Fig. 1 schematically illustrates controlling a mobile robot by a motion control method according to an exemplary embodiment of the present invention;
fig. 2 schematically shows a flow chart of a motion control method for a mobile robot according to an exemplary embodiment of the invention;
fig. 3 schematically shows the intermediate variable s, the speed v of the mobile robot, the speed v of the first driving wheel in the first planned trajectory L And the speed v of the second driving wheel R A time-dependent profile;
FIG. 4 schematically illustrates a comparison of a second planned trajectory with a first planned trajectory by scaling in an exemplary embodiment according to the present invention; and
FIG. 5 schematically illustrates an adjustment of an adjustment section of a second planned trajectory in an exemplary embodiment according to the invention
Fig. 6 schematically illustrates controlling a mobile robot by a motion control method according to an exemplary embodiment of the present invention.
Detailed Description
In order to make the technical problems, technical solutions and advantageous technical effects to be solved by the present invention more apparent, the present invention will be further described in detail with reference to the accompanying drawings and a plurality of exemplary embodiments. It should be understood that the detailed description and specific examples are intended for purposes of illustration only and are not intended to limit the scope of the invention.
The present invention is applicable to mobile robots, which may be any robot capable of autonomous spatial movement, such as AGVs, AMRs, etc. The mobile robot may be used to perform various tasks, such as for example as a warehouse robot, a sweeping robot, a home attendant robot, a greeting robot, etc.
It should be appreciated that the expressions "first", "second", etc. are used herein for descriptive purposes only and are not to be construed as indicating or implying relative importance or as implying any particular order of number of technical features indicated. Features defining "first", "second" or "first" may be expressed or implied as including at least one such feature.
The motion control method of the present invention is exemplarily described below with reference to fig. 1 and 2. Fig. 1 schematically illustrates a mobile robot 1 controlled by a motion control method according to an exemplary embodiment of the present invention. Fig. 2 schematically shows a flow chart of a motion control method for a mobile robot 1 according to an exemplary embodiment of the invention.
In the embodiment shown in fig. 1, the mobile robot 1 is, for example, a differential robot, i.e. the mobile robot 1 has a differential motion system comprising at least a first driving wheel and a second driving wheel. Alternatively, the mobile robot 1 may also be another type of robot, such as a single steering wheel robot or a double steering wheel robot, etc. Accordingly, the mobile robot 1 may comprise a double steering wheel movement system, for example.
The mobile robot 1 includes, for example: a sensor; communication means for communicating with other devices, such as a dispatch control system; and a controller for controlling components for the mobile robot 1, including, for example, differential wheel movement systems, sensors, communication devices, and the like. The controller may also receive operational status or detection data of the corresponding components, e.g. sensors, via the communication lines for monitoring or controlling the operation of the mobile robot 1. The motion control method may be performed, for example, by means of a controller, or by means of a further device, such as a dispatch control system, capable of exchanging data with the controller.
The mobile robot 1 needs to reach a specific location at a specific time in some scenarios. The specific location may be a task point related to a task to be performed by the mobile robot 1. The desired arrival time may be determined from the task. For example, the mobile robot 1 may need to reach the target point P at the desired arrival time t To at the target point P t Where the item to be transported is received and then transported to a designated location. As another example, the mobile robot 1 may comprise a code scanner and have a function of reaching the target point P at the desired arrival time t And at the target point P t The task of scanning the two-dimensional code is located.
For this purpose, a motion control method for the mobile robot 1 as shown in fig. 2 is proposed. The motion control method comprises the following steps:
an initial planning step S1, wherein a first planning track containing time information enabling the mobile robot 1 to complete a predetermined first path 2 in a shortest time is acquiredTrace, target point P t On the first path 2;
a time determination step S2, in which it is determined that the mobile robot 1 arrives at the target point P in the first planned trajectory t Is determined by the first arrival time of the mobile robot 1 at the target point P t Is set to the expected arrival time of (1); and
a re-planning step S3 in which, if the first arrival time is earlier than the expected arrival time, the expected arrival time is taken as the planned arrival time and the time information of the first planned trajectory is changed to acquire a second planned trajectory so that the mobile robot 1 can arrive at the target point P at the planned arrival time in accordance with the second planned trajectory t 。
By this motion control method, the mobile robot 1 can be made to arrive at the target point P at the desired arrival time with its motion capability allowed t . The second planned trajectory does not change the path of movement of the mobile robot 1 compared to the first planned trajectory, but still follows the first path.
Preferably, the speed of the mobile robot in the second planned trajectory is continuous. The continuous speed means that no abrupt change in speed occurs in the second planned trajectory.
In an exemplary embodiment according to the present invention, the motion control method further includes: if the first arrival time is not earlier than the desired arrival time, the mobile robot 1 is controlled to move according to the first planned trajectory. In this case, the movement capability of the mobile robot 1 cannot be made to arrive at the target point P on time t The mobile robot 1 is still able to move to the target point P in as short a time as possible t 。
In fig. 1, the global path 3 for the mobile robot 1 is shown in dashed curves and the first path 2 is shown in solid curves. In this embodiment, the first path 2 is a local path obtained by local path planning on the basis of the global path 3.
The first path 2 may be in the form of a 3 rd order or more bezier curve and can be expressed by the following formula:
wherein ,
representing the position of the mobile robot 1, when s increases from 0 to 1, corresponding +.>
The position of the mobile robot 1 along the first path 2 from the start point to the end point is shown. This is particularly advantageous for differential robots. The first path 2 having a continuous second derivative can particularly advantageously adapt to the motion characteristics of the differential robot. In particular, the first path 2 can have a continuous curvature. This makes the change in the speed and acceleration of the mobile robot 1 more gradual.
More specifically, the curve of the first path 2 may be represented by the following formula:
wherein i=0, 1, …, N, N is not less than 3,
representing the coordinates of the control points of the bezier curve.
The initial planning step S1 may be performed by: based on the curve equation of the first path 2, the mobile robot 1 is subjected to speed planning, and the planned track of the first path 2 containing time information is determined to be used as a first planned track by the mobile robot 1 in the shortest time under the condition that the kinematic constraint and the dynamic constraint of the mobile robot 1 are met, wherein the first planned track meets the motion equation
wherein ,/>
The position of the mobile robot 1 is represented, and the function G (t) represents the position
As a function of time t.
The specific implementation procedure of the initial planning step S1 is as follows, for example:
first, a curve equation of the first path 2 is determined
The curve equation is decomposed into two directions x and y, expressed as:
the x-direction and the y-direction may define a plane along which the movement path of the mobile robot 1 is located. (P) x (s 0 ),P y (s 0 ) A) may represent that the mobile robot 1 is moving at s=s 0 A location at which to locate. At this position, the slope of the trajectory direction of the mobile robot 1 can be correspondingly denoted as f y ′(s 0 )/f x ′(s 0 )。
It should be appreciated that the step of decomposing the curve equation into x and y directions is not necessary. In other embodiments this decomposition may not be performed. For example, the distance of movement of the mobile robot 1 along the first path 2 may be expressed as a distance L, and the two-dimensional plane x-y is reduced to a one-dimensional space L. In other words, based on the first path 2, the position of the corresponding mobile robot 1 can be determined as long as the distance L is determined. The first derivative of the distance L is the speed of the mobile robot 1.
Then, based on the curve equation of the first path 2, the mobile robot 1 is speed-planned so as to determine s=g (t) such that
The function g (t) represents the function dependence of the variable s on the time tIs tied up. To determine the variation of the intermediate variable s with time t, the derivative s '=g' (t) of s may be first planned and then integrated to obtain s=g (t).
Velocity v of mobile robot 1 in x and y directions x (t)、v y (t) and acceleration a x (t)、a y (t) respectively as follows:
wherein g '(t) and g' (t) are the first and second derivatives of s, respectively,
the speed of the mobile robot 1 can also be expressed as +.>
The kinematic and dynamic constraints of the mobile robot 1 that need to be satisfied during the speed planning process include: the speed of the mobile robot 1 at the start of the first path 2 is equal to a predetermined initial speed; the speed of the mobile robot 1 in the first planned trajectory is below a predetermined maximum speed; the acceleration of the mobile robot 1 in the first planned trajectory is below a predetermined maximum acceleration. For example, the kinematic and dynamic constraints of the mobile robot 1 may include at least:
wherein ,
and />
The maximum speeds allowed for the mobile robot 1 in the x-direction and the y-direction respectively,
and />
Maximum allowable acceleration, v, of the mobile robot 1 in the x-direction and the y-direction, respectively 0 Is a predetermined initial velocity of the mobile robot 1 at the start of the first path 2.
In the case where the mobile robot 1 is a differential-wheel robot, the kinematic and dynamic constraints of the mobile robot 1 include, among others: the initial speeds of the first and second driving wheels of the mobile robot 1 at the start of the first path 2 are respectively equal to the predetermined left wheel initial speed v L0 And a predetermined right wheel initial speed v R0 The method comprises the steps of carrying out a first treatment on the surface of the The speeds of the first driving wheel and the second driving wheel of the mobile robot 1 in the first planned trajectory are respectively below a predetermined maximum wheel speed; the accelerations of the first and second driving wheels of the mobile robot 1 in the first planned trajectory are respectively below a predetermined maximum wheel acceleration.
In fig. 2 the target point P is shown on the first path 2 t . The target point P can be obtained by a curve equation t Corresponding s=s n . According to the first planned trajectory, the mobile robot 1 will be at a first arrival time t opt Reach the target point P t 。
Fig. 3 schematically shows the intermediate variable s, the speed v of the mobile robot 1, the speed v of the first driving wheel in the first planned trajectory L And the speed v of the second driving wheel R A time-dependent curve, wherein only the starting point of the first planned trajectory to the target point P is shown t Is a part of the same. Here, t starts from t=0 in order to simplify the description.
In the time determination step S2, the first arrival time t may be determined opt Time t of arrival expected from expected mobile robot 1 to reach target point n Comparison. If the first arrival time t opt Earlier than the expected arrival time t n A re-planning step S3 is performed. In the re-planning step S3, the desired arrival time is taken as the planned arrival time and the first planned trajectory is changedInformation to obtain a second planned trajectory such that the mobile robot 1 is able to reach the target point P at the planned arrival time at a continuous speed according to the second planned trajectory t 。
The re-planning step S3 may include: determining a ratio of a first duration from a start time of the first track to a first arrival time to a planned duration from the start time of the first track to a planned arrival time as a scaling ratio k; and scaling the speed plan corresponding to the first planned track according to the scaling ratio k to obtain a second planned track, so that the second planned track meets the motion equation
Thus, the first and second substrates are bonded together,
substituting t=ku for s=g (t) in the first planned trajectory, it is possible to obtain: 
Here, u represents a time variable in the second planned trajectory. Accordingly, it can be known that the second planned trajectory satisfies the equation of motion +.>
The speed v of the mobile robot 1 in the second planned trajectory may be further determined by:
further, the speed v of the first driving wheel of the mobile robot 1 may be determined by L And the speed v of the second driving wheel R Is of the size of (2):
wherein p(s) is determined based on the curve equation of the first path 2 and the following equation:
where b denotes the tread of the mobile robot 1, and R denotes the radius of rotation of the mobile robot 1, which can be determined by the curve equation of the first path 2. The radius of rotation R has a one-to-one correspondence with the variable s.
By the above scaling, the time-dependent change relation s (u) of s of the second planned trajectory can be obtained. Accordingly, the position of the mobile robot 1 can also be determined
Relation of change over time->
The time-dependent change v (u) of the speed v of the mobile robot 1, the speed v of the first driving wheel L Time-dependent relationship v L (u), speed v of the second drive wheel R Time-dependent relationship v R (u)。
For purposes of describing the re-planning step more clearly herein, the letter "u" is used to represent a time variable in the second planned trajectory, but those skilled in the art will appreciate that the time variable may be equivalently represented herein by "t".
Fig. 4 schematically shows a comparison of a second planned trajectory with a first planned trajectory by scaling in an exemplary embodiment according to the invention. The first planned trajectory is shown in dashed lines and the second planned trajectory is shown in solid lines in fig. 4.
It can be seen that according to the second planned trajectory, the mobile robot 1 will be at the desired arrival time t n Arrive s=s n Corresponding position, i.e. target point P t . At the same value of s, the speed v of the mobile robot 1, the speed v of the first driving wheel in the second planned trajectory L Speed v of the second drive wheel R Relative to the speed of the mobile robot 1 in the second planned trajectoryv speed v of first drive wheel L Speed v of the second drive wheel R And correspondingly scaled down. This scaling procedure can ensure that the second planned trajectory still meets the constraint that the predetermined maximum speed and the predetermined maximum acceleration of the mobile robot 1 and the predetermined maximum wheel speed and maximum wheel acceleration are not exceeded. The speed v of the mobile robot 1 and the speed v of the first driving wheel L Speed v of the second drive wheel R Can keep the original trend of change with time without mutation.
As shown in fig. 4, at the start of the second planned trajectory by scaling, the speed v of the first driving wheel L And the speed v of the second driving wheel R Respectively is reduced to
and />
In order to make the speed v of the first driving wheel at the starting point L And the speed v of the second driving wheel R Respectively equal to the preset initial velocity v of the left wheel L0 And a predetermined right wheel initial speed v R0 The adjustment section of the second planned trajectory, which is obtained by scaling, starting from its start point may be adjusted. Fig. 5 schematically shows an adjustment of the adjustment section of the second planned trajectory in an exemplary embodiment according to the invention, taking the first driving wheel as an example. The adjustment section corresponds to a range from t to 0 to t 2 Is a time period of (a). The adjustment section may be divided into a first portion and a second portion, the first portion corresponding to a range from t to 0 1 The second part corresponds to the time period from t to t 1 To t 2 Is a time period of (a). The first portion starts from the start of the second planned trajectory. Optionally, the speed v of the first driving wheel in the first section L And the speed v of the second driving wheel R Respectively always equal to the preset left wheel initial speed v L0 And a predetermined right wheel initial speed v R0 In the second section, the speed v of the first driving wheel L And a second driving wheelVelocity v of (2) R Increasing. The speed of the first and second drive wheels of the mobile robot 1 and the position of the mobile robot 1 at the end of the adjustment section are the same as before the adjustment.
Fig. 6 schematically illustrates controlling the mobile robot 1 by a motion control method according to an exemplary embodiment of the present invention. In this embodiment, the target point is the position at which the mobile robot 1 will collide with the moving dynamic obstacle 4 according to the first planned trajectory. In other words, at the first arrival time at which the mobile robot 1 moves to the target point according to the first planned trajectory, the dynamic barrier 4 will also move to an adjacent or same position and collide with the mobile robot 1. The mobile robot 1 may comprise a detection system for detecting dynamic obstacles 4. The detection system may comprise at least one sensor, such as a radar or a camera or the like, for detecting dynamic obstacles 4 that may be present. The detection system may analyze the presence (absence) of the dynamic obstacle 4, the motion state and/or profile of the dynamic obstacle 4, and the predicted motion profile of the dynamic obstacle 4 based on the detection results of the sensors.
In this embodiment, the time determining step S2 may include: the expected arrival time is determined according to the motion state of the dynamic obstacle 4 so that the mobile robot 1 does not collide with the dynamic obstacle 4 if the time at which the mobile robot 1 arrives at the target point is not earlier than the expected arrival time. In this case, the first arrival time will be earlier than the desired arrival time. Then, a re-planning step S3 is performed to obtain a second planned trajectory. According to the second planned trajectory, the mobile robot 1 arrives at the target point with the desired arrival time so that the mobile robot 1 does not collide with the dynamic obstacle 4. This allows the mobile robot 1 to avoid the dynamic obstacle 4 by decelerating and yielding. During deceleration let-off, no abrupt change in the speed of the mobile robot 1 occurs. "the mobile robot 1 collides with the dynamic obstacle 4" means that the distance between the mobile robot 1 and the dynamic obstacle 4 is smaller than a predetermined safety distance.
Optionally, the motion control method further includes a detour planning step, and the detour planning step includes: determining a third path 6 capable of enabling the mobile robot 1 to pass through at least one front end detour point 5 to detour in front of the dynamic obstacle 4 without collision, wherein the start point and the end point of the third path 6 are both located on the first path 2; generating a third planned trajectory containing time information that enables the mobile robot 1 to move along the third path 6, according to which the mobile robot 1 will pass through the intersection of the third path 6 and the movement path of the dynamic obstacle 4 before the dynamic obstacle 4; and if the generation of the third planned trajectory is successful, taking the third planned trajectory as one of the candidate detour trajectories. The third path 6 optionally has the same start and end points as the first path 2.
The third path 6 may be in the form of a bezier curve of more than fourth order. In particular, in case the third path 6 needs to pass through m front end detour points 5, the third path 6 is in the form of an m+3 order bezier curve. Thereby, a third path 6 can be generated in an advantageous manner, which ensures that all front-end detour points 5 are traversed.
Alternatively, in the third path 6, the mobile robot 1 will have a velocity direction perpendicular to the velocity direction that the dynamic barrier 4 would have if it were to collide with the mobile robot 1 moving according to the first planned trajectory, at the front end detour point 5. This facilitates quick and safe obstacle avoidance.
The front-end detour point 5 may be determined according to the contour of the mobile robot 1, the contour of the dynamic obstacle 4, the movement speed and the movement direction of the dynamic obstacle 4. In the detour planning step, the desired detour time corresponding to the front end detour point 5 is determined such that the dynamic obstacle 4 can be detoured without collision as long as the mobile robot 1 passes the front end detour point 5 before the desired detour time along the third path 6. According to the third planned trajectory, the mobile robot 1 will pass the front-end detour point 5 at the respective front-end detour time.
If for each front-end detour point 5 the front-end detour time is correspondingly before the desired detour time, the generation of the third planned trajectory is successful, otherwise, changing the position of the front-end detour point 5 re-determines the third path 6 and the third planned trajectory or the generation of the third planned trajectory fails.
Alternatively or additionally, the detour planning step comprises: determining a fourth path 8 capable of enabling the mobile robot 1 to pass through at least one rear end detour point 7 to detour from the rear of the dynamic obstacle 4 without collision, wherein the start point and the end point of the fourth path 8 are both located on the first path 2; generating a fourth planned trajectory including time information that enables the mobile robot 1 to move along the fourth path 8, according to which the mobile robot 1 will pass through an intersection of the fourth path 8 and a movement path of the dynamic obstacle 4 after the dynamic obstacle 4; and if the fourth planned trajectory is generated successfully, taking the fourth planned trajectory as one of candidate detour trajectories. The fourth path 8 optionally has the same start and end points as the first path 2.
Similarly, the fourth path 8 may be in the form of a bezier curve of fourth order or more. In particular, in the case where the fourth path 8 needs to pass through n trailing-end detour points 7, the fourth path 8 is in the form of an n+3 order bezier curve. Thereby, a fourth path 8 can be generated in an advantageous manner, which ensures that all backend detours through the point 7.
Alternatively, in the fourth path 8, the velocity direction of the mobile robot 1 at the rear end detour point 7 is perpendicular to the velocity direction that the dynamic obstacle 4 would have if it were to collide with the mobile robot 1 moving according to the first planned trajectory. This facilitates quick and safe obstacle avoidance.
The back-end detour point 7 may be determined from the contour of the mobile robot 1, the contour of the dynamic obstacle 4, the speed and direction of movement of the dynamic obstacle 4. In the detour planning step, the desired detour time corresponding to the back-end detour point 7 is determined such that the dynamic obstacle 4 can be detoured without collision as long as the mobile robot 1 passes the back-end detour point 7 after the desired detour time along the fourth path 8. According to the third planned trajectory, the mobile robot 1 will pass the front end detour point 5 at or after the respective back end detour time.
If for each back-end detour point 7 the back-end detour time is correspondingly after the desired detour time, the generation of the fourth planned trajectory is successful, otherwise, changing the position of the back-end detour point 7 re-determines the fourth path 8 and the fourth planned trajectory or the generation of the fourth planned trajectory fails.
In an exemplary embodiment, the front end detour point 5 and/or the rear end detour point 7 are determined based on the position of the dynamic obstacle 4 that will collide with the mobile robot 1 that is to move with the first planned trajectory such that the mobile robot 1 detours from just in front of and/or behind the dynamic obstacle 4 located at that position without collision. Thereby, the third path 6 and/or the fourth path 8 can deviate from the first path 2 as little as possible. The front-end detour point 5 and/or the rear-end detour point 7 may comprise points located on the predicted trajectory of motion of the dynamic obstacle 4.
Optionally, the motion control method further includes controlling the mobile robot 1 to move according to one of the second planned trajectory and the candidate detour trajectory that detours around the dynamic obstacle 4 in the shortest time.
Furthermore, the invention relates to a computer program product comprising computer program instructions which, when executed by one or more processors, are capable of performing the motion control method according to the invention.
In the present invention, the computer program product may be stored in a computer readable storage medium. The computer readable storage medium may include, for example, high speed random access memory, but may also include non-volatile memory, such as a hard disk, memory, a plug-in hard disk, a smart memory card, a secure digital card, a flash memory card, at least one magnetic disk storage device, a flash memory device, or other volatile solid state storage device. The processor 10 may be a central processing unit, but may also be other general purpose processors, digital signal processors, application specific integrated circuits, off-the-shelf programmable gate arrays or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, or the like. The general purpose processor may be a microprocessor or may be any conventional processor or the like.
The start point and the end point of the trajectory or path represent only the positions of the start and end of the trajectory or path, and do not represent the positions of the start and end movements of the mobile robot 1. The speed of the mobile robot 1 at the start point, the target point and/or the end point may not be 0.
Although specific embodiments of the invention have been described in detail herein, they are presented for purposes of illustration only and are not to be construed as limiting the scope of the invention. Various substitutions, alterations, and modifications can be made without departing from the spirit and scope of the invention.
Claims (18)
1. A motion control method for a mobile robot (1), wherein the motion control method comprises the steps of:
an initial planning step S1, wherein a first planning track containing time information enabling the mobile robot (1) to complete a preset first path (2) in the shortest time is acquired, and a target point is located on the first path (2);
a time determination step S2, wherein a first arrival time of the mobile robot (1) reaching the target point and a desired arrival time of the mobile robot (1) reaching the target point in the first planned trajectory are determined; and
and a re-planning step S3, wherein if the first arrival time is earlier than the expected arrival time, the expected arrival time is taken as the planned arrival time, and the time information of the first planned trajectory is changed to acquire a second planned trajectory, so that the mobile robot (1) can arrive at the target point at the planned arrival time according to the second planned trajectory.
2. The motion control method according to claim 1, wherein,
the initial planning step S1 includes: based on a curve equation of the first path (2), carrying out speed planning on the mobile robot (1), and determining that the mobile robot (1) completes a planning track containing time information of the first path (2) as a first planning track in the shortest time under the condition that the kinematic constraint and the dynamic constraint of the mobile robot (1) are met;
The first planning track satisfies the equation of motion
wherein ,/>
The position of the mobile robot (1) is indicated, and the function G (t) indicates the position +.>
As a function of time t.
3. The motion control method according to claim 2, wherein,
the first path (2) is a local path obtained by planning a local path of the mobile robot (1);
the first path (2) is in the form of a 3 rd order or more Bezier curve and can be expressed by the following formula:
wherein ,
representing the position of the mobile robot (1), when the variable s increases from 0 to 1, the corresponding +.>
Representing the position of the mobile robot (1) along the first path (2) from the start point to the end point;
in the first planned trajectory, s=g (t),
the function g (t) represents the variable s as a function of time t.
4. A motion control method according to claim 2 or 3, wherein,
the kinematic and dynamic constraints of the mobile robot (1) include: the speed of the mobile robot (1) at the start of the first path (2) is equal to a predetermined initial speed; the speed of the mobile robot (1) in the first planned trajectory is below a predetermined maximum speed; the acceleration of the mobile robot (1) in the first planned trajectory is below a predetermined maximum acceleration; and/or
The mobile robot (1) is a differential wheel robot, and the kinematic constraint and the dynamic constraint of the mobile robot (1) comprise: initial speeds of a first driving wheel and a second driving wheel of the mobile robot (1) at the starting point of the first path (2) are respectively equal to a predetermined left wheel initial speed and a predetermined right wheel initial speed; the speeds of a first driving wheel and a second driving wheel of the mobile robot (1) in the first planned track are respectively below a preset maximum wheel speed; acceleration of a first driving wheel and a second driving wheel of the mobile robot (1) in a first planned trajectory is below a predetermined maximum wheel acceleration, respectively.
5. The motion control method according to any one of claims 2 to 4, wherein,
the speed planning is performed according to a T-type planning method.
6. The motion control method according to any one of claims 2 to 5, wherein,
the re-planning step S3 includes:
determining a ratio of a first duration from a start time of the first track to a first arrival time to a planned duration from the start time of the first track to a planned arrival time as a scaling ratio k;
the speed planning corresponding to the first planning track is scaled according to the scaling ratio to obtain a second planning track, so that the second planning track meets the motion equation
7. The motion control method according to claim 6, wherein,
the re-planning step S3 further comprises:
the adjustment section of the second planned trajectory obtained by scaling is adjusted from its start point such that the speeds of the first and second drive wheels of the mobile robot (1) at the start point of the second planned trajectory are equal to the predetermined left and right wheel initial speeds, respectively, and the speeds of the first and second drive wheels of the mobile robot (1) at the end point of the adjustment section and the position of the mobile robot (1) are the same as before the adjustment.
8. The motion control method according to any one of claims 1 to 7, wherein,
the target point is a task point related to a task to be performed by the mobile robot (1), from which the desired arrival time is determined.
9. The motion control method according to any one of claims 1 to 8, wherein,
the motion control method comprises the following steps: if the first arrival time is not earlier than the expected arrival time, the mobile robot (1) is controlled to move according to the first planned trajectory.
10. The motion control method according to any one of claims 1 to 7, wherein,
the target point is a position where the mobile robot (1) collides with the moving dynamic obstacle (4) according to the first planning track;
the time determination step S2 includes: the expected arrival time is determined according to the motion state of the dynamic obstacle (4) so that the mobile robot (1) does not collide with the dynamic obstacle (4) if the time at which the mobile robot (1) arrives at the target point is not earlier than the expected arrival time.
11. The motion control method according to claim 10, wherein,
the motion control method further comprises a detour planning step, the detour planning step comprising:
determining a third path (6) which enables the mobile robot (1) to bypass in front of the dynamic obstacle (4) through at least one front-end bypass point (5), wherein the start point and the end point of the third path (6) are both located on the first path (2);
Generating a third planned trajectory containing time information enabling the mobile robot (1) to move along a third path (6), according to which the mobile robot (1) will pass through an intersection of the third path (6) and a movement path of the dynamic obstacle (4) before the dynamic obstacle (4);
if the generation of the third planned trajectory is successful, the third planned trajectory is taken as one of the candidate detour trajectories.
12. The motion control method according to claim 11, wherein,
determining a front end detour point (5) according to the outline of the mobile robot (1), the outline of the dynamic obstacle (4), the movement speed and the movement direction of the dynamic obstacle (4);
determining a desired detour time corresponding to the front end detour point (5) such that the dynamic obstacle (4) can be detoured without collision as long as the mobile robot (1) passes the front end detour point (5) along the third path (6) before the desired detour time;
generating a planned trajectory containing time information enabling the mobile robot (1) to complete the third path (6) in a shortest time as a third planned trajectory, wherein, according to the third planned trajectory, the mobile robot (1) will pass the front end detour point (5) at a respective front end detour time;
if for each front-end detour point (5) the front-end detour time is correspondingly before the desired detour time, the generation of the third planned trajectory is successful, otherwise, the re-determination of the third path (6) and the third planned trajectory by changing the position of the front-end detour point (5) or the generation of the third planned trajectory fails.
13. The motion control method according to claim 11 or 12, wherein,
the third path (6) is in the form of a Bezier curve with more than four orders, wherein when the third path (6) passes through m front end detour points (5), the third path (6) is in the form of a Bezier curve with m+3 orders;
in the third path (6), the velocity direction of the mobile robot (1) at the front end detour point (5) is perpendicular to the velocity direction that the dynamic obstacle (4) would have if it were to collide with the mobile robot (1) moving according to the first planned trajectory.
14. The motion control method according to any one of claims 10 to 13, wherein,
the motion control method further comprises a detour planning step, the detour planning step comprising:
determining a fourth path (8) which enables the mobile robot (1) to bypass behind the dynamic obstacle (4) through at least one rear end bypass point (7) without collision, wherein the start point and the end point of the fourth path (8) are both located on the first path (2);
generating a fourth planned trajectory containing time information, which enables the mobile robot (1) to move along a fourth path (8), according to which the mobile robot (1) will pass through the intersection of the fourth path (8) and the movement path of the dynamic obstacle (4) after the dynamic obstacle (4);
And if the fourth planning track is successfully generated, taking the fourth planning track as one of candidate detour tracks.
15. The motion control method according to claim 14, wherein,
determining a rear end detour point (7) according to the contour of the mobile robot (1), the contour of the dynamic obstacle (4), the movement speed and the movement direction of the dynamic obstacle (4);
determining an expected detour time corresponding to the back-end detour point (7) such that the dynamic obstacle (4) can be detoured without collision as long as the mobile robot (1) passes the back-end detour point (7) after the expected detour time along the fourth path (8);
if for each back-end detour point (7) the back-end detour time is correspondingly after the desired detour time, the generation of the fourth planned trajectory is successful, otherwise, the re-determination of the fourth path (8) and the fourth planned trajectory by changing the position of the back-end detour point (7) or the generation of the fourth planned trajectory fails.
16. The motion control method according to claim 14 or 15, wherein,
the fourth path (8) is in the form of a Bezier curve with more than fourth order, and when the fourth path (8) passes through n rear end detour points (7), the fourth path (8) is in the form of a Bezier curve with n+3 order;
in the fourth path (8), the velocity direction of the mobile robot (1) at the rear end detour point (7) is perpendicular to the velocity direction that the dynamic obstacle (4) would have if it were to collide with the mobile robot (1) moving according to the first planned trajectory.
17. The motion control method according to any one of claims 11 to 16, wherein,
the mobile robot (1) is controlled to move according to one track which bypasses the dynamic obstacle (4) in the shortest time in the second planning track and the candidate detour track.
18. A computer program product comprising computer program instructions, wherein the computer program instructions, when executed by one or more processors, are capable of performing the motion control method according to any one of claims 1-17.
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111208623.4A CN115993815A (en) | 2021-10-18 | 2021-10-18 | Motion control method for mobile robot and computer program product |
| PCT/CN2022/123037 WO2023066012A1 (en) | 2021-10-18 | 2022-09-30 | Motion control method for mobile robot, and computer program product |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111208623.4A CN115993815A (en) | 2021-10-18 | 2021-10-18 | Motion control method for mobile robot and computer program product |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115993815A true CN115993815A (en) | 2023-04-21 |
Family
ID=85992657
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111208623.4A Pending CN115993815A (en) | 2021-10-18 | 2021-10-18 | Motion control method for mobile robot and computer program product |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN115993815A (en) |
| WO (1) | WO2023066012A1 (en) |
Family Cites Families (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH1139592A (en) * | 1997-07-23 | 1999-02-12 | Toyota Motor Corp | Vehicle traveling controller |
| CN106843223B (en) * | 2017-03-10 | 2020-05-05 | 武汉理工大学 | Intelligent obstacle avoidance AGV trolley system and obstacle avoidance method |
| CN108664020A (en) * | 2018-04-11 | 2018-10-16 | 上海大学 | A kind of unmanned boat dynamic obstacle avoidance algorithm based on Speed Obstacles method and dynamic window method |
| JP2020004095A (en) * | 2018-06-28 | 2020-01-09 | 株式会社Ｓｏｋｅｎ | Autonomous mobile body controller and autonomous mobile body |
| CN109163728B (en) * | 2018-10-31 | 2020-08-28 | 山东大学 | Dynamic environment obstacle avoidance method, controller and robot |
| CN109960261B (en) * | 2019-03-22 | 2020-07-03 | 北京理工大学 | Dynamic obstacle avoiding method based on collision detection |
| CN110187706A (en) * | 2019-05-28 | 2019-08-30 | 上海钛米机器人科技有限公司 | A kind of speed planning method, apparatus, electronic equipment and storage medium |
| CN112256023B (en) * | 2020-09-28 | 2022-08-19 | 南京理工大学 | Bezier curve-based airport border patrol robot local path planning method and system |
-
2021
- 2021-10-18 CN CN202111208623.4A patent/CN115993815A/en active Pending
-
2022
- 2022-09-30 WO PCT/CN2022/123037 patent/WO2023066012A1/en unknown
Also Published As
| Publication number | Publication date |
|---|---|
| WO2023066012A1 (en) | 2023-04-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Mehami et al. | Smart automated guided vehicles for manufacturing in the context of Industry 4.0 | |
| CN106022274B (en) | Obstacle avoidance method, obstacle avoidance device and unmanned machine | |
| US8521415B2 (en) | Traveling vehicle and method of controlling the same | |
| EP3403772B1 (en) | Robot motion planning for avoiding collision with moving obstacles | |
| WO2014156502A1 (en) | Position identification device and mobile robot provided with same | |
| CN106950972A (en) | A kind of automatic guided vehicle AGV and its route method for correcting error | |
| CN112334907A (en) | Controlling movement of autonomous devices | |
| JP2018527689A (en) | Virtual line following method and modification method for autonomous vehicles | |
| JP2024020457A (en) | Information processing device, information processing method, computer program, and information processing system | |
| US20220180749A1 (en) | Mobile object control apparatus, mobile object control method, and computer readable recording medium | |
| CN112828880A (en) | State machine for dynamic path planning | |
| Behrje et al. | An autonomous forklift with 3d time-of-flight camera-based localization and navigation | |
| CN111445723A (en) | Vehicle path identification | |
| CN113031581A (en) | Robot, method for controlling travel of robot, electronic device, and storage medium | |
| CN115698889A (en) | Autonomous mobile robot linkage system and autonomous mobile robot | |
| CN114012732B (en) | Robot control method, device, equipment and medium | |
| CN109839930B (en) | Obstacle avoidance device, system and method | |
| EP3265752B1 (en) | Bubble logic for ride vehicle control | |
| CN115808918A (en) | Global path planning method, motion control method and computer program product | |
| CN115993815A (en) | Motion control method for mobile robot and computer program product | |
| CN113751330A (en) | Method, system, device and storage medium for sorting articles | |
| RU2749202C1 (en) | Method for planning robot movement and mobile robot | |
| CN116414116A (en) | Path planning method, motion control method and computer program product | |
| JPWO2019186815A1 (en) | Self-propelled device, self-propelled method, and program | |
| US20230022637A1 (en) | Information processing apparatus, information processing method, and program |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |