Motion control method for mobile robot and computer program product

Abstract

A motion control method for a mobile robot (1) is proposed, comprising: an initial planning step S1, wherein a first planning trajectory containing time information capable of enabling the mobile robot (1) to complete a predetermined first path (2) in the shortest time is obtained, a target point being 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 in the first planning trajectory and an expected arrival time at which the mobile robot (1) is expected to reach the target point are determined; 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 a planned arrival time and the time information of the first planning trajectory is changed to obtain a second planning trajectory, so that the mobile robot (1) can reach the target point at the planned arrival time according to the second planning trajectory. A corresponding computer program product is also proposed. By means of the present application, the punctuality of the mobile robot can be improved.

Description

Technical Field

[0001] This invention relates to the field of mobile robots, and more particularly to the field of motion control for mobile robots, specifically to a motion control method for mobile robots and a computer program product. Background Technology

[0002] With rapid economic growth and rising labor costs, mobile robots are being used more and more widely in various industrial and domestic environments. For example, Automated Guided Vehicles (AGVs), Autonomous Mobile Robots (AMRs), and forklifts are key pieces of equipment in modern logistics systems. Mobile robots can move and stop at target locations according to path planning and operational requirements to complete tasks such as material handling and transportation.

[0003] Typically, mobile robots need to reach specific locations (task points) due to the tasks they are tasked with. For example, a mobile robot used to transport goods in a warehouse may need to move to a specific receiving location to pick up the items to be transported and then transport them to a designated delivery location. This can be achieved through methods such as specific local path planning, enabling the mobile robot to move to the designated location.

[0004] In some scenarios, mobile robots also need to arrive at specific locations on time.

[0005] Existing technologies still fall short in ensuring the timely arrival of mobile robots. Summary of the Invention

[0006] The purpose of this invention is to provide an improved motion control method for mobile robots to improve the timeliness of mobile robots.

[0007] According to a first aspect of the present invention, a motion control method for a mobile robot is provided, wherein the motion control method includes the following steps: an initial planning step S1, wherein a first planned trajectory containing time information is obtained, which enables the mobile robot to complete a predetermined first path in the shortest time, and a target point is located on the first path; a time determination step S2, wherein a first arrival time of the mobile robot to reach the target point in the first planned trajectory and an expected arrival time of the mobile robot to reach the target point are determined; and a replanning step S3, wherein if the first arrival time is earlier than the expected arrival time, the expected arrival time is used as the planned arrival time and the time information of the first planned trajectory is changed to obtain a second planned trajectory, such that the mobile robot can reach the target point at the planned arrival time according to the second planned trajectory.

[0008] In an exemplary embodiment, the initial planning step S1 includes: performing velocity planning on the mobile robot based on the curve equation of the first path, and determining a planned trajectory containing time information that allows the mobile robot to complete the first path in the shortest time while satisfying the kinematic and dynamic constraints of the mobile robot as the first planned trajectory; the first planned trajectory satisfies the motion equation. in, The position of the mobile robot is represented by the function G(t). The functional relationship with time t.

[0009] In an exemplary embodiment, the first path is a local path obtained by performing local path planning on the mobile robot; the first path is in the form of a Bézier curve of order 3 or higher and can be represented by the following formula:

[0010]

[0011] in, This represents the position of the mobile robot. As the variable s increases from 0 to 1, the corresponding... This represents the position of the mobile robot along the first path from the starting point to the ending point; in the first planned trajectory, s = g(t). The function g(t) represents the functional relationship between the variable s and time t.

[0012] In one exemplary embodiment, the kinematic and dynamic constraints of the mobile robot include: the velocity of the mobile robot at the starting point of the first path is equal to a predetermined initial velocity; the velocity of the mobile robot in the first planned trajectory is below a predetermined maximum velocity; 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, and the kinematic and dynamic constraints of the mobile robot include: at the starting point of the first path, the initial velocities of the first drive wheel and the second drive wheel of the mobile robot are equal to predetermined initial velocities of the left wheel and the right wheel, respectively; the velocities of the first drive wheel and the second drive wheel of the mobile robot in the first planned trajectory are below predetermined maximum wheel velocities; and the accelerations of the first drive wheel and the second drive wheel of the mobile robot in the first planned trajectory are below predetermined maximum wheel accelerations.

[0013] In one exemplary embodiment, velocity planning is performed according to a T-shaped planning method.

[0014] In an exemplary embodiment, the replanning step S3 includes: determining the ratio of a first duration from the start time to the first arrival time of the first planned trajectory to a planned duration from the start time to the planned arrival time of the first planned trajectory as a scaling ratio k; scaling the velocity plan corresponding to the first planned trajectory proportionally according to the scaling ratio to obtain a second planned trajectory, such that the second planned trajectory satisfies the equation of motion.

[0015] In an exemplary embodiment, the replanning step S3 further includes: adjusting the adjustment segment of the second planned trajectory obtained by scaling, starting from its starting point, such that at the starting point of the second planned trajectory, the speeds of the first drive wheel and the second drive wheel of the mobile robot are equal to the predetermined initial speeds of the left wheel and the right wheel, respectively, and at the end point of the adjustment segment, the speeds of the first drive wheel and the second drive wheel of the mobile robot and the position of the mobile robot are the same as before the adjustment.

[0016] In one exemplary embodiment, the target point is a task point related to the task to be performed by the mobile robot, and the expected arrival time is determined based on the task.

[0017] In one exemplary embodiment, the motion control method includes controlling a mobile robot to move along a first planned trajectory if the first arrival time is not earlier than the expected arrival time.

[0018] In an exemplary embodiment, the target point is the location where the mobile robot will collide with the moving dynamic obstacle according to the first planned trajectory; the time determination step S2 includes: determining the expected arrival time based on the motion state of the dynamic obstacle, such that if the mobile robot arrives at the target point at a time no earlier than the expected arrival time, the mobile robot will not collide with the dynamic obstacle.

[0019] In one exemplary embodiment, the motion control method further includes a detour planning step, which includes: determining a third path that enables the mobile robot to detour around a dynamic obstacle without collision by passing at least one front detour point, wherein the start and end points 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 the third path, according to the third planned trajectory, the mobile robot will pass the intersection of the third path and the movement path of the dynamic obstacle before the dynamic obstacle; if the generation of the third planned trajectory is successful, then the third planned trajectory is used as one of the candidate detour trajectories.

[0020] In an exemplary embodiment, a leading bypass point is determined based on the outline of the mobile robot, the outline of the dynamic obstacle, and the speed and direction of the dynamic obstacle. An expected bypass time corresponding to the leading bypass point is determined such that the mobile robot can bypass the dynamic obstacle without collision as long as it passes the leading bypass point along the third path before the expected bypass time. A planned trajectory containing time information is generated as the third planned trajectory, enabling the mobile robot to complete the third path in the shortest time. According to the third planned trajectory, the mobile robot will pass the leading bypass point at the corresponding leading bypass time. If the leading bypass time for each leading bypass point is correspondingly before the expected bypass time, the generation of the third planned trajectory is successful; otherwise, the position of the leading bypass point is changed to redetermine the third path and the third planned trajectory, or the generation of the third planned trajectory fails.

[0021] In an exemplary embodiment, the third path is in the form of a Bézier curve of order four or higher, wherein, when the third path passes through m front bypass points, the third path is in the form of a Bézier curve of order m+3; in the third path, the velocity direction of the mobile robot at the front bypass point is perpendicular to the dynamic obstacle and will be perpendicular to the velocity direction of the mobile robot moving according to the first planned trajectory when colliding with it.

[0022] In one exemplary embodiment, the motion control method further includes a detour planning step, which includes: determining a fourth path that enables the mobile robot to detour around the dynamic obstacle without collision by passing at least one rear detour point, wherein the start and end points of the fourth path are both located on the first path; generating a fourth planned trajectory containing time information that enables the mobile robot to move along the fourth path, wherein, according to the third planned trajectory, the mobile robot will pass the intersection of the fourth path and the movement path of the dynamic obstacle after the dynamic obstacle; and if the generation of the fourth planned trajectory is successful, then the fourth planned trajectory is used as one of the candidate detour trajectories.

[0023] In an exemplary embodiment, a rear bypass point is determined based on the outline of the mobile robot, the outline of the dynamic obstacle, and the speed and direction of the dynamic obstacle. The expected bypass time corresponding to the rear bypass point is determined such that the mobile robot can bypass the dynamic obstacle without collision as long as it passes the rear bypass point along the fourth path after the expected bypass time. If the rear bypass time is after the expected bypass time for each rear bypass point, the fourth planned trajectory is successfully generated; otherwise, the position of the rear bypass point is changed to redetermine the fourth path and the fourth planned trajectory, or the generation of the fourth planned trajectory fails.

[0024] In an exemplary embodiment, the fourth path is in the form of a Bézier curve of order four or higher. When the fourth path passes through n rear loop points, the fourth path is in the form of a Bézier curve of order n+3. In the fourth path, the velocity direction of the mobile robot at the rear loop point is perpendicular to the dynamic obstacle. When the mobile robot collides with the obstacle moving according to the first planned trajectory, the velocity direction will be perpendicular to the obstacle.

[0025] In one exemplary embodiment, the mobile robot is controlled to move along a second planned trajectory and a candidate detour trajectory to bypass the dynamic obstacle in the shortest time.

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

[0027] The positive effect of this invention is that, through this motion control method, the mobile robot can arrive at the target point on time when its motion capability allows. Attached Figure Description

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

[0029] Figure 1 This illustration schematically demonstrates the control of a mobile robot using a motion control method according to an exemplary embodiment of the present invention;

[0030] Figure 2 A flowchart illustrating a motion control method for a mobile robot according to an exemplary embodiment of the present invention is shown schematically.

[0031] Figure 3 The diagram schematically illustrates the intermediate variable s, the velocity v of the mobile robot, and the velocity v of the first drive wheel in the first planned trajectory. L The speed v of the second drive wheel R Curve showing change over time;

[0032] Figure 4 The illustration schematically shows a comparison between a second planned trajectory obtained by scaling and a first planned trajectory in an exemplary embodiment according to the present invention; and

[0033] Figure 5 The illustration schematically shows the adjustment of the adjustment segment of the second planning trajectory in an exemplary embodiment according to the present invention;

[0034] Figure 6The illustration schematically depicts the control of a mobile robot using a motion control method according to an exemplary embodiment of the present invention. Detailed Implementation

[0035] To make the technical problems to be solved, the technical solutions, and the beneficial technical effects of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and several exemplary embodiments. It should be understood that the specific embodiments described herein are for illustrative purposes only and are not intended to limit the scope of protection of this invention.

[0036] This invention applies to mobile robots, which can be any robot capable of autonomous spatial movement, such as AGVs and AMRs. The mobile robot can be used to perform various tasks, such as as a warehouse robot, cleaning robot, home companion robot, or welcoming robot.

[0037] It should be understood that the terms "first," "second," etc., used herein are for descriptive purposes only and should not be construed as indicating or implying relative importance, nor should they be construed as implicitly specifying the number of technical features indicated. Features specified as "first" or "second" may expressly or implicitly indicate that at least one of those features is included.

[0038] The following is combined Figure 1 and Figure 2 The motion control method of the present invention is illustrated by way of example. Figure 1 The illustration schematically shows the control of a mobile robot 1 by a motion control method according to an exemplary embodiment of the present invention. Figure 2 A flowchart illustrating a motion control method for a mobile robot 1 according to an exemplary embodiment of the present invention is shown schematically.

[0039] exist Figure 1 In the illustrated embodiment, the mobile robot 1 is, for example, a differential robot, meaning that the mobile robot 1 has a differential wheel motion system that includes at least a first drive wheel and a second drive wheel. Alternatively, the mobile robot 1 can also be other types of robots, such as a single-steering-wheel robot or a dual-steering-wheel robot. Accordingly, the mobile robot 1 may include, for example, a dual-steering-wheel motion system.

[0040] The mobile robot 1 includes, for example, sensors; a communication device for communicating with other devices, such as a scheduling control system; and a controller for controlling components of the mobile robot 1, such as a differential wheel motion system, sensors, and the communication device. The controller can also receive operating status or detection data from relevant components, such as sensors, via communication lines to monitor or control the operation of the mobile robot 1. Motion control methods can be executed, for example, by means of the controller, or by means of another device capable of exchanging data with the controller, such as a scheduling control system.

[0041] Mobile robot 1 needs to arrive at a specific location at a specific time in certain scenarios. This specific location can be a task point related to the task that mobile robot 1 is to perform. The expected arrival time can be determined based on the task. For example, mobile robot 1 may need to arrive at target point P at the expected arrival time. t At target point P t The system receives items to be transported and then transports them to a designated location. For example, mobile robot 1 may include a barcode scanner and be able to arrive at the target point P at the desired arrival time. t And at target point P t The task is to scan a QR code.

[0042] Therefore, the following proposals were put forward: Figure 2 The motion control method for a mobile robot 1 is shown. The motion control method includes the following steps:

[0043] Initial planning step S1, wherein a first planned trajectory including time information is obtained, which enables the mobile robot 1 to complete the predetermined first path 2 in the shortest time, with target point P. t Located on path 2;

[0044] In step S2, the time determination step involves determining when the mobile robot 1 reaches the target point P along the first planned trajectory. t First arrival time and expected arrival time of mobile robot 1 at target point P t The expected arrival time; and

[0045] In step S3, if the first arrival time is earlier than the expected arrival time, the expected arrival time is used as the planned arrival time, and the time information of the first planned trajectory is changed to obtain the second planned trajectory, so that the mobile robot 1 can reach the target point P at the planned arrival time according to the second planned trajectory. t .

[0046] This motion control method enables the mobile robot 1 to arrive at the target point P on time, provided its motion capabilities allow. tCompared to the first planned trajectory, the second planned trajectory does not change the motion path of the mobile robot 1, but still follows the first path.

[0047] Preferably, the speed of the mobile robot in the second planned trajectory is continuous. Continuous speed means that the speed does not change abruptly in the second planned trajectory.

[0048] In an exemplary embodiment of the present invention, the motion control method further includes: if the first arrival time is not earlier than the expected arrival time, controlling the mobile robot 1 to move according to a first planned trajectory. In this case, the motion capability of the mobile robot 1 is insufficient to enable it to arrive at the target point P on time. t However, mobile robot 1 is still able to move to the target point P in the shortest possible time. t .

[0049] exist Figure 1 In the diagram, the global path 3 for the mobile robot 1 is shown as a dashed curve, and the first path 2 is shown as a solid curve. In this embodiment, the first path 2 is a local path obtained by performing local path planning based on the global path 3.

[0050] The first path 2 can take the form of a Bézier curve of order 3 or higher and can be expressed by the following formula:

[0051]

[0052] in, This represents the position of mobile robot 1. As s increases from 0 to 1, the corresponding position is... This represents the position of mobile robot 1 along the first path 2 from the starting point to the ending point. This is particularly advantageous for differential robots. The first path 2 has continuous second derivatives, which is particularly advantageous for adapting to the motion characteristics of differential robots. In particular, the first path 2 can have continuous curvature. This makes the changes in the velocity and acceleration of mobile robot 1 smoother.

[0053] More specifically, the curve of the first path 2 can be represented by the following formula:

[0054]

[0055] Where i = 0, 1, ..., N, N ≥ 3, This represents the coordinates of the control points of the Bézier curve.

[0056] The initial planning step S1 can be performed as follows: Based on the curve equation of the first path 2, velocity planning is performed on the mobile robot 1, and the planned trajectory containing time information of the mobile robot 1 that completes the first path 2 in the shortest time while satisfying the kinematic and dynamic constraints of the mobile robot 1 is determined as the first planned trajectory, wherein the first planned trajectory satisfies the motion equation. in, The position of mobile robot 1 is represented by the function G(t). The functional relationship with time t.

[0057] The specific execution process of the initial planning step S1 is as follows:

[0058] First, determine the curve equation of the first path 2. The equation of the curve is decomposed into two directions, x and y, and expressed as follows:

[0059]

[0060] The x and y directions define the plane on which the mobile robot 1 moves. (P) x (s0),P y (s0) can represent the position of mobile robot 1 at s = s0. At this position, the slope of the trajectory direction of mobile robot 1 can be expressed as f. y ′(s0) / f x ′(s0).

[0061] It should be understood that decomposing the curve equation into x and y directions is not necessary. In other embodiments, this decomposition can be omitted. For example, the distance traveled by the mobile robot 1 along the first path 2 can be represented by distance L, simplifying the two-dimensional plane xy 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 once the distance L is determined. The first derivative of distance L is the magnitude of the velocity of the mobile robot 1.

[0062] Then, based on the curve equation of the first path 2, speed planning is performed on the mobile robot 1 to determine s = g(t), such that... The function g(t) represents the functional relationship between the variable s and time t. To determine the relationship between the intermediate variable s and time t, we can first plan the derivative of s, s′=g′(t), and then obtain s=g(t) through integration.

[0063] The velocity v of mobile robot 1 in the x and y directions x (t), v y (t) and acceleration a x (t), a y (t) are as follows:

[0064]

[0065] Where g′(t) and g″(t) are the first and second derivatives of s, respectively. The speed of mobile robot 1 can also be expressed as

[0066] During velocity planning, the kinematic and dynamic constraints that need to be satisfied for the mobile robot 1 include: the velocity of the mobile robot 1 at the starting point of the first path 2 is equal to the predetermined initial velocity; the velocity of the mobile robot 1 in the first planned trajectory is below the predetermined maximum velocity; and the acceleration of the mobile robot 1 in the first planned trajectory is below the predetermined maximum acceleration. For example, the kinematic and dynamic constraints of the mobile robot 1 may include at least:

[0067]

[0068] in, and These are the maximum permissible speeds of mobile robot 1 in the x and y directions, respectively. and These are the maximum allowable accelerations of mobile robot 1 in the x and y directions, respectively, and v0 is the predetermined initial velocity of mobile robot 1 at the starting point of the first path 2.

[0069] In the case where the mobile robot 1 is a differential wheel robot, the kinematic and dynamic constraints of the mobile robot 1 also include, in particular, that at the starting point of the first path 2, the initial velocities of the first and second drive wheels of the mobile robot 1 are respectively equal to the predetermined initial velocity v of the left wheel. L0 And the predetermined initial velocity v of the right wheel R0 In the first planned trajectory, the speeds of the first and second drive wheels of the mobile robot 1 are respectively below the predetermined maximum wheel speed; in the first planned trajectory, the accelerations of the first and second drive wheels of the mobile robot 1 are respectively below the predetermined maximum wheel acceleration.

[0070] Figure 2 The image shows the target point P located on the first path 2. t The target point P can be obtained through the curve equation. t The corresponding s = s n According to the first planned trajectory, mobile robot 1 will arrive at the first arrival time t. opt Reaching target point P t .

[0071] Figure 3The diagram schematically illustrates the intermediate variable s, the velocity v of the mobile robot 1, and the velocity v of the first drive wheel in the first planned trajectory. L The speed v of the second drive wheel R The curves showing the change over time, where only the path from the starting point of the first planned trajectory to the target point P are shown. t The part. Here, t starts from t=0 for the sake of simplicity.

[0072] In the time determination step S2, the first arrival time t can be determined. opt The expected arrival time t of the expected mobile robot 1 to reach the target point n In comparison. If the first arrival time t opt Earlier than expected arrival time t n Then, the replanning step S3 is executed. In the replanning step S3, the expected arrival time is used as the planned arrival time, and the time information of the first planned trajectory is changed to obtain the second planned trajectory, so that the mobile robot 1 can reach the target point P at the planned arrival time according to the second planned trajectory at a continuous speed. t .

[0073] The replanning step S3 may include: determining the ratio of the first duration from the start time to the first arrival time of the first planned trajectory to the planned duration from the start time to the planned arrival time of the first planned trajectory as a scaling ratio k; and scaling the velocity plan corresponding to the first planned trajectory proportionally according to the scaling ratio k to obtain a second planned trajectory, such that the second planned trajectory satisfies the equation of motion.

[0074] therefore, Substituting t = ku into s = g(t) in the first planning trajectory, we get: Here, u represents the time variable in the second planned trajectory. Accordingly, it can be seen that the second planned trajectory satisfies the equation of motion.

[0075] The velocity v of the mobile robot 1 in the second planned trajectory can be further determined using the following formula:

[0076]

[0077] Furthermore, the speed v of the first drive wheel of the mobile robot 1 can be determined by the following formula. L The speed v of the second drive wheel R Size:

[0078]

[0079] Where p(s) is determined based on the curve equation of the first path 2 and the following formula:

[0080]

[0081] Where b represents the wheelbase of mobile robot 1, and R represents the rotation radius of mobile robot 1, which can be determined by the curve equation of the first path 2. The rotation radius R has a one-to-one correspondence with the variable s.

[0082] By scaling the parameters as described above, the relationship between the time-varying values ​​of s and u for the second planned trajectory can be obtained. Correspondingly, the position of mobile robot 1 can also be determined. Relationship over time The relationship between the velocity v of mobile robot 1 and time, v(u), and the velocity v of the first drive wheel. L Relationship between v and time L (u), the speed of the second drive wheel v R Relationship between v and time R (u).

[0083] In this document, to more clearly describe the replanning steps, the letter “u” is used to represent the time variable in the second planning trajectory, but those skilled in the art will understand that “t” can be used equivalently to represent the time variable here.

[0084] Figure 4 The illustration schematically shows a comparison between a second planned trajectory obtained by scaling and a first planned trajectory in an exemplary embodiment of the present invention. Figure 4 The first planned trajectory is shown in dashed lines, and the second planned trajectory is shown in solid lines.

[0085] As can be seen, according to the second planned trajectory, mobile robot 1 will arrive at the expected time t. n Reaching s = s n The corresponding location, i.e., the target point P t At the same s value, the velocity v of the mobile robot 1 and the velocity v of the first drive wheel in the second planned trajectory are... L The speed v of the second drive wheel R Relative to the velocity v of mobile robot 1 in the second planned trajectory and the velocity v of the first drive wheel L The speed v of the second drive wheel R The trajectory is scaled down proportionally accordingly. This scaling process ensures that the second planned trajectory still meets the constraints of not exceeding the predetermined maximum speed and acceleration of the mobile robot 1, as well as the predetermined maximum wheel speed and maximum wheel acceleration. Furthermore, the speed v of the mobile robot 1 and the speed v of the first drive wheel... L The speed v of the second drive wheel R It can maintain its original trend of change over time without undergoing abrupt changes.

[0086] like Figure 4 As shown, at the starting point of the second planned trajectory obtained by scaling, the speed v of the first drive wheel is... L The speed v of the second drive wheel R They were reduced to and

[0087] To make the speed v of the first drive wheel at the starting point L The speed v of the second drive wheel R The initial velocity of the left wheel is equal to the predetermined initial velocity v. L0 And the predetermined initial velocity v of the right wheel R0 It can adjust the adjustment segment starting from its starting point of the second planning trajectory obtained by scaling. Figure 5 Taking the first drive wheel as an example, this schematically illustrates the adjustment of an adjustment segment of the second planned trajectory according to an exemplary embodiment of the present invention. Here, the adjustment segment corresponds to the time period from t0 to t2. The adjustment segment can be divided into a first part and a second part, the first part corresponding to the time period from t0 to t1, and the second part corresponding to the time period from t1 to t2. The first part starts from the starting point of the second planned trajectory. Optionally, in the first part, the speed v of the first drive wheel... L The speed v of the second drive wheel R The initial velocity of the left wheel is always equal to the predetermined initial velocity v. L0 And the predetermined initial velocity v of the right wheel R0 In the second part, the speed v of the first drive wheel L The speed v of the second drive wheel R Increase. At the end of the adjustment section, the speed of the first and second drive wheels of mobile robot 1 and the position of mobile robot 1 are the same as before the adjustment.

[0088] Figure 6 The illustration schematically depicts a mobile robot 1 controlled by a motion control method according to an exemplary embodiment of the present invention. In this embodiment, the target point is the location where the mobile robot 1 will collide with a moving dynamic obstacle 4 according to a first planned trajectory. In other words, at the first arrival time of the mobile robot 1 moving to the target point according to the first planned trajectory, the dynamic obstacle 4 will also move to an adjacent or identical position and collide with the mobile robot 1. The mobile robot 1 may include a detection system for detecting the dynamic obstacle 4. The detection system may include at least one sensor, such as radar or a camera, for detecting the possible presence of the dynamic obstacle 4. The detection system may analyze the presence (absence) of the dynamic obstacle 4, the motion state and / or contour of the dynamic obstacle 4, and the predicted motion trajectory of the dynamic obstacle 4 based on the detection results of the sensors.

[0089] In this embodiment, the time determination step S2 may include: determining the expected arrival time based on the motion state of the dynamic obstacle 4, such that if the mobile robot 1 arrives at the target point no earlier than the expected arrival time, the mobile robot 1 will not collide with the dynamic obstacle 4. In this case, the first arrival time will be earlier than the expected arrival time. Then, the replanning step S3 is performed to obtain a second planned trajectory. According to the second planned trajectory, the mobile robot 1 will arrive at the target point at the expected arrival time, so that the mobile robot 1 will not collide with the dynamic obstacle 4. Thus, the mobile robot 1 can avoid the dynamic obstacle 4 by decelerating and yielding. During the deceleration and yielding, the speed of the mobile robot 1 will not change abruptly. "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 less than a predetermined safe distance.

[0090] Optionally, the motion control method further includes a detour planning step, which includes: determining a third path 6 that enables the mobile robot 1 to bypass the dynamic obstacle 4 without collision by passing at least one front detour point 5, wherein the start and end points 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 the third planned trajectory, the mobile robot 1 will pass 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, then using the third planned trajectory as one of the candidate detour trajectories. The third path 6 may optionally have the same start and end points as the first path 2.

[0091] The third path 6 can be in the form of a Bézier curve of order four or higher. In particular, when the third path 6 needs to pass through m leading loop points 5, the third path 6 is in the form of a Bézier curve of order m+3. Thus, it is possible to generate the third path 6 in an advantageous manner that ensures it passes through all leading loop points 5.

[0092] Optionally, in the third path 6, the velocity direction of the mobile robot 1 at the leading-edge loop point 5 is perpendicular to the dynamic obstacle 4. When the mobile robot 1 collides with the obstacle moving according to the first planned trajectory, its velocity direction will be perpendicular to the obstacle. This facilitates fast and safe obstacle avoidance.

[0093] The leading detour point 5 can be determined based on the outline of the mobile robot 1, the outline of the dynamic obstacle 4, and the speed and direction of movement of the dynamic obstacle 4. In the detour planning step, the expected detour time corresponding to the leading detour point 5 is determined, so that as long as the mobile robot 1 passes the leading detour point 5 along the third path 6 before the expected detour time, it can bypass the dynamic obstacle 4 without collision. According to the third planned trajectory, the mobile robot 1 will pass the leading detour point 5 at the corresponding leading detour time.

[0094] If for each front bypass point 5, the front bypass time is correspondingly earlier than the expected bypass time, then the generation of the third planned trajectory is successful; otherwise, the position of the front bypass point 5 is changed to redetermine the third path 6 and the third planned trajectory, or the generation of the third planned trajectory fails.

[0095] Alternatively or additionally, the detour planning step includes: determining a fourth path 8 that enables the mobile robot 1 to detour around the dynamic obstacle 4 without collision via at least one rear detour point 7, wherein the start and end points of the fourth path 8 are both located on the first path 2; generating a fourth planned trajectory containing time information that enables the mobile robot 1 to move along the fourth path 8, wherein, according to the third planned trajectory, the mobile robot 1 will pass the intersection of the fourth path 8 and the movement path of the dynamic obstacle 4 after the dynamic obstacle 4; and if the generation of the fourth planned trajectory is successful, then using the fourth planned trajectory as one of the candidate detour trajectories. The fourth path 8 may optionally have the same start and end points as the first path 2.

[0096] Similarly, the fourth path 8 can be in the form of a Bézier curve of order four or higher. In particular, when the fourth path 8 needs to pass through n back loop points 7, the fourth path 8 is in the form of a Bézier curve of order n+3. Thus, it is possible to generate the fourth path 8 in an advantageous manner that ensures it passes through all back loop points 7.

[0097] Optionally, in the fourth path 8, the mobile robot 1, at the rear bypass point 7, will have a velocity direction perpendicular to the dynamic obstacle 4 when colliding with the mobile robot 1 moving according to the first planned trajectory. This facilitates fast and safe obstacle avoidance.

[0098] The rear bypass point 7 can be determined based on the outline of the mobile robot 1, the outline of the dynamic obstacle 4, and the movement speed and direction of the dynamic obstacle 4. In the detour planning step, the expected detour time corresponding to the rear bypass point 7 is determined, so that as long as the mobile robot 1 passes the rear bypass point 7 along the fourth path 8 after the expected detour time, it can bypass the dynamic obstacle 4 without collision. According to the third planned trajectory, the mobile robot 1 will pass the front bypass point 5 at the corresponding rear bypass time or later.

[0099] If for each back-end bypass point 7, the back-end bypass time is correspondingly after the expected bypass time, then the generation of the fourth planned trajectory is successful; otherwise, the position of the back-end bypass point 7 is changed to redetermine the fourth path 8 and the fourth planned trajectory, or the generation of the fourth planned trajectory fails.

[0100] In one exemplary embodiment, the front bypass point 5 and / or the rear bypass point 7 are determined based on the position of a dynamic obstacle 4 that will collide with the mobile robot 1 moving along the first planned trajectory, such that the mobile robot 1 bypasses the dynamic obstacle 4 in front of and / or behind it without collision. Thus, the third path 6 and / or the fourth path 8 can deviate from the first path 2 as little as possible. The front bypass point 5 and / or the rear bypass point 7 may include points located on the predicted trajectory of the dynamic obstacle 4.

[0101] Optionally, the motion control method further includes controlling the mobile robot 1 to move along a trajectory that bypasses the dynamic obstacle 4 in the shortest time, between a second planned trajectory and a candidate bypass trajectory.

[0102] In addition, the present invention relates to a computer program product comprising computer program instructions which, when executed by one or more processors, enable the processors to perform the motion control method according to the present invention.

[0103] In this invention, 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 hard disks, RAM, plug-in hard disks, smart memory cards, secure digital cards, flash memory cards, at least one disk storage device, flash memory device, or other volatile solid-state storage devices. The processor 10 may be a central processing unit, or other general-purpose processors, digital signal processors, application-specific integrated circuits (ASICs), off-the-shelf programmable gate arrays (OPGs) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or any conventional processor.

[0104] The start and end points of a trajectory or path only indicate the beginning and end positions of the trajectory or path, and do not indicate the beginning and end positions of the movement of mobile robot 1. The velocity of mobile robot 1 at the start point, target point, and / or end point may not be 0.

[0105] Although specific embodiments of the invention have been described in detail herein, they are given for illustrative purposes only and should not be construed as limiting the scope of the invention. Various substitutions, alterations, and modifications can be conceived without departing from the spirit and scope of the invention.

Claims

1. A motion control method for a mobile robot (1), wherein, The motion control method includes the following steps: In the initial planning step S1, a first planned trajectory containing time information is obtained, which enables the mobile robot (1) to complete the predetermined first path (2) in the shortest time. The target point is located on the first path (2), and the first planned trajectory satisfies the motion equation. in, The position of the mobile robot (1) is represented by the function G(t), which represents the position. The functional relationship with time t; Time determination step S2, wherein the first arrival time of the mobile robot (1) to the target point in the first planned trajectory and the expected arrival time of the mobile robot (1) to the target point are determined; and The replanning step S3 includes the following steps: If the first arrival time is earlier than the expected arrival time, the expected arrival time is used as the planned arrival time, and the time information of the first planned trajectory is changed to obtain the second planned trajectory, so that the mobile robot (1) can reach the target point at the planned arrival time according to the second planned trajectory. The replanning step S3 includes: The scaling ratio k is determined as the ratio of the first duration from the start time to the first arrival time of the first planned trajectory to the planned duration from the start time to the planned arrival time of the first planned trajectory. The velocity plan corresponding to the first planned trajectory is scaled proportionally according to the scaling ratio to obtain the second planned trajectory, such that the second planned trajectory satisfies the equation of motion. And based on this equation of motion, the velocity v of the first drive wheel of the mobile robot (1) is determined. L The speed v of the second drive wheel R Relationship between v and time L (u) and v R (u), where u represents time; The speed v of the first drive wheel obtained by scaling L The speed v of the second drive wheel R Relationship between v and time L (u) and v R (u) Adjustments are made starting from the starting point in the adjustment section, which is divided into a first part and a second part. The first part starts from the starting point of the second planned trajectory. In the first part, the speed of the first drive wheel is v. L The speed v of the second drive wheel R The initial velocity of the left wheel is always equal to the predetermined initial velocity v. L0 And the predetermined initial velocity v of the right wheel R0 In the second part, the speed v of the first drive wheel L The speed v of the second drive wheel R Increase until the speed of the first and second drive wheels and the position of the mobile robot (1) are the same as before the adjustment at the end of the second part.

2. The motion control method according to claim 1, wherein, The initial planning step S1 includes: based on the curve equation of the first path (2), performing velocity planning on the mobile robot (1), and determining the planned trajectory containing time information of the mobile robot (1) to complete the first path (2) in the shortest time under the condition of satisfying the kinematic constraints and dynamic constraints of the mobile robot (1) as the first planned trajectory.

3. The motion control method according to claim 2, wherein, The first path (2) is a local path obtained by performing local path planning on the mobile robot (1); The first path (2) takes the form of a Bézier curve of order 3 or higher and can be expressed by the following formula: in, This represents the position of the mobile robot (1). When the variable s increases from 0 to 1, the corresponding position is... This indicates the position of the mobile robot (1) along the first path (2) from the starting point to the end point; In the first planned trajectory, s = g(t), The function g(t) represents the functional relationship between the variable s and time t.

4. The motion control method according to claim 2 or 3, wherein, The kinematic and dynamic constraints of the mobile robot (1) include: the velocity of the mobile robot (1) at the starting point of the first path (2) is equal to a predetermined initial velocity; the velocity of the mobile robot (1) in the first planned trajectory is below a predetermined maximum velocity; 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. The kinematic and dynamic constraints of the mobile robot (1) include: at the starting point of the first path (2), the initial velocities of the first drive wheel and the second drive wheel of the mobile robot (1) are equal to the predetermined initial velocities of the left wheel and the right wheel, respectively; in the first planned trajectory, the velocities of the first drive wheel and the second drive wheel of the mobile robot (1) are below the predetermined maximum wheel velocities; in the first planned trajectory, the accelerations of the first drive wheel and the second drive wheel of the mobile robot (1) are below the predetermined maximum wheel accelerations, respectively.

5. The motion control method according to claim 2 or 3, wherein, Velocity planning is performed using the T-planar planning method.

6. The motion control method according to any one of claims 1-3, wherein, The target point is a task point related to the task to be performed by the mobile robot (1), and the expected arrival time is determined according to the task.

7. The motion control method according to any one of claims 1-3, wherein, The motion control method includes controlling the mobile robot (1) to move according to a first planned trajectory if the first arrival time is not earlier than the expected arrival time.

8. The motion control method according to any one of claims 1-3, wherein, The target point is the location where the mobile robot (1) will collide with the moving dynamic obstacle (4) according to the first planned trajectory; The time determination step S2 includes: determining the expected arrival time based on the motion state of the dynamic obstacle (4) so ​​that if the mobile robot (1) arrives at the target point at a time no earlier than the expected arrival time, the mobile robot (1) will not collide with the dynamic obstacle (4).

9. The motion control method according to claim 8, wherein, The motion control method further includes a detour planning step, which includes: A third path (6) is determined that enables the mobile robot (1) to bypass the dynamic obstacle (4) without collision by passing through at least one front bypass point (5), wherein the start and end points of the third path (6) are both located on the first path (2). A third planned trajectory containing time information is generated that enables the mobile robot (1) to move along the third path (6). According to the third planned trajectory, the mobile robot (1) will pass the intersection of the third path (6) and the movement path of the dynamic obstacle (4) before the dynamic obstacle (4). If the third planned trajectory is successfully generated, it will be used as one of the candidate detour trajectories.

10. The motion control method according to claim 9, wherein, Based on the outline of the mobile robot (1), the outline of the dynamic obstacle (4), the speed and direction of the dynamic obstacle (4), determine the front bypass point (5); Determine the expected bypass time corresponding to the front bypass point (5) so that as long as the mobile robot (1) passes the front bypass point (5) before the expected bypass time along the third path (6), it can bypass the dynamic obstacle (4) without collision. A time-informed planning trajectory is generated that enables the mobile robot (1) to complete the third path (6) in the shortest time as the third planning trajectory. According to the third planning trajectory, the mobile robot (1) will pass the front-end bypass point (5) at the corresponding front-end bypass time. If for each front bypass point (5), the front bypass time is correspondingly earlier than the expected bypass time, then the generation of the third planned trajectory is successful; otherwise, the position of the front bypass point (5) is changed to redetermine the third path (6) and the third planned trajectory, or the generation of the third planned trajectory fails.

11. The motion control method according to claim 9 or 10, wherein, The third path (6) takes the form of a Bézier curve of order four or higher. In the case that the third path (6) passes through m front-end bypass points (5), the third path (6) takes the form of a Bézier curve of order m+3. In the third path (6), the velocity direction of the mobile robot (1) at the front bypass point (5) is perpendicular to the dynamic obstacle (4) when it collides with the mobile robot (1) moving according to the first planned trajectory.

12. The motion control method according to claim 9 or 10, wherein, The motion control method further includes a detour planning step, which includes: Determine a fourth path (8) that enables the mobile robot (1) to bypass the dynamic obstacle (4) without collision by passing at least one rear bypass point (7), wherein the start and end points of the fourth path (8) are both located on the first path (2). A fourth planned trajectory containing time information is generated that enables the mobile robot (1) to move along the fourth path (8). According to the third planned trajectory, 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). If the fourth planned trajectory is successfully generated, it will be used as one of the candidate detour trajectories.

13. The motion control method according to claim 12, wherein, Based on the outline of the mobile robot (1), the outline of the dynamic obstacle (4), the speed and direction of the dynamic obstacle (4), determine the rear bypass point (7); Determine the expected bypass time corresponding to the back bypass point (7) so that as long as the mobile robot (1) passes the back bypass point (7) after the expected bypass time along the fourth path (8), it can bypass the dynamic obstacle (4) without collision. If for each back-end bypass point (7), the back-end bypass time is correspondingly after the expected bypass time, then the generation of the fourth planning trajectory is successful; otherwise, the position of the back-end bypass point (7) is changed to redetermine the fourth path (8) and the fourth planning trajectory, or the generation of the fourth planning trajectory fails.

14. The motion control method according to claim 12, wherein, The fourth path (8) takes the form of a Bézier curve of order four or higher. When the fourth path (8) passes through n rear loop points (7), the fourth path (8) takes the form of a Bézier curve of order n+3. In the fourth path (8), the velocity direction of the mobile robot (1) at the rear bypass point (7) is perpendicular to the dynamic obstacle (4) when it collides with the mobile robot (1) moving according to the first planned trajectory.

15. The motion control method according to claim 11, wherein, Control the mobile robot (1) to move along one of the second planned trajectories and the candidate detour trajectories to bypass the dynamic obstacle (4) in the shortest time.

16. A computer program product comprising computer program instructions, wherein, When the computer program instructions are executed by one or more processors, the processors are capable of performing the motion control method according to any one of claims 1-15.

Images