Global path planning method, motion control method and computer program product

Abstract

The present invention proposes a global path planning method for a mobile robot, wherein the global path planning method includes: an initial planning step for obtaining an initial global path for the mobile robot; a local path planning step, wherein the following sub-steps are performed: selecting at least a portion of the global path as an initial local path segment, generating an optimized local path such that the optimized local path is a curve with continuous second-order derivatives, and the optimized local path has the same starting point, end point, starting velocity direction, end velocity direction, starting curvature, and end curvature as the initial local path segment; replacing the initial local path segment with the optimized local path to form a new global path; and a global path determination step for determining the new global path as the final global path. A motion control method for a mobile robot and a computer program product are also proposed. With the aid of the present invention, the motion trajectory of a mobile robot can be optimized.

Description

Technical Field

[0001] The present invention relates to the field of mobile robots, in particular to the field of motion control of mobile robots, and specifically to a global path planning method for a mobile robot, a motion control method for a mobile robot, and a computer program product. Background Art

[0002] With rapid economic growth and rising labor costs, mobile robots are increasingly being used in various industrial and domestic environments. For example, mobile robots such as automated guided vehicles (AGVs), autonomous mobile robots (AMRs), and forklifts are key components of modern logistics systems. Mobile robots can move and dock at target locations based on planned paths and operational requirements to complete tasks such as material handling and transportation. Path planning is crucial for mobile robot motion control.

[0003] When using a global path planning algorithm (such as the A* algorithm) to plan a global path for a mobile robot, a smooth path is often not guaranteed. Furthermore, although a global path planning algorithm considers multiple factors to obtain an optimized global path, new factors that need to be considered may arise during the movement of the mobile robot.

[0004] The existing technology still has many deficiencies in path planning and motion control of mobile robots. Summary of the Invention

[0005] The object of the present invention is to provide an improved global path planning method and motion control method for a mobile robot, so as to optimize the motion trajectory of the mobile robot.

[0006] According to a first aspect of the present invention, a global path planning method for a mobile robot is provided, wherein the global path planning method comprises the following steps:

[0007] Initial planning step S11, wherein an initial global path for the mobile robot is obtained;

[0008] The local path planning step S12 includes the following sub-steps:

[0009] In the first sub-step, at least a portion of the global path is selected as an initial local path segment;

[0010] A second sub-step of generating an optimized local path, wherein the optimized local path is a curve having a continuous second-order derivative, and the optimized local path has the same starting point, ending point, starting point velocity direction, ending point velocity direction, starting point curvature, and ending point curvature as the initial local path segment; and

[0011] A third sub-step is to replace the initial local path segment with the optimized local path to form a new global path; and

[0012] A global path determination step S13 is performed, wherein the new global path is determined as the final global path.

[0013] Optionally, the local path planning step S12 is performed in at least one of the following ways:

[0014] In the local path planning step S12, initial local path segments are selected in such a way that the starting point of each optimized local path is located on the final global path;

[0015] The length of the initial local path segment is less than a predetermined length threshold;

[0016] The local path planning step S12 is repeatedly performed until any point in the new global path is located on at least one optimized local path, and then the final determination step S13 is performed.

[0017] Optionally, the local path planning step S12 includes a first local path planning step. In the first local path planning step, the optimized local path is a third-order Bezier curve, and the third-order Bezier curve is represented by the following formula:

[0018]

[0019] in, represents the coordinates of the control points of the Bezier curve and Among them, the control point The coordinates of are determined as follows:

[0020] are the coordinates of the starting point of the initial local path segment, are the coordinates of the end point of the initial local path segment;

[0021] Respectively Same positive and negative, where is the unit vector representing the velocity direction of the starting point of the initial local path segment;

[0022] Respectively Same positive and negative, where is the unit vector representing the terminal velocity direction of the initial local path segment;

[0023] Substituting s=0 and К(0)=К0 into , where К0 represents the path starting curvature of the initial local path segment, They are The first-order derivative, ordinate and the second-order derivative, ordinate;

[0024] Substituting s=1 and К(1)=К1 into , where К1 represents the path end curvature of the initial local path segment.

[0025] Optionally, the local path planning step S12 includes a second local path planning step. In a first sub-step of the second local path planning step, a set of waypoints is determined. Waypoints represent points that the mobile robot needs to pass through. The waypoint set is a set of m waypoints adjacent to the initial local path segment, where m ≥ 1. In a second sub-step of the second local path planning step, an optimized local path is generated that passes through all waypoints in the waypoint set.

[0026] Optionally, in the second local path planning step, the optimized local path is an m+3 order Bezier curve, and the first control point and the m+4th control point of the m+3 order Bezier curve are respectively the starting point and the end point of the optimized local path.

[0027] Optionally, in the second sub-step of the second local path planning step, the optimized local path is generated so that the speed direction at at least one waypoint in the set of waypoints satisfies one of the following conditions: the same as the speed direction of the point closest to the corresponding waypoint on the initial local path segment; the same as the direction from the start point to the end point of the initial local path segment; and the task requirements of the mobile robot at the corresponding waypoint are met.

[0028] Optionally, in the second local path planning step, the start and end points of the initial local path segment are determined such that:

[0029] The starting point of the initial local path segment is located before all points on the global path that are respectively closest to each of the waypoints in the set of waypoints, along the direction of the global path; and / or

[0030] Along the direction of the global path, the end point of the initial local path segment is located after all points on the global path that are respectively closest to each of the waypoints in the set of waypoints; and / or

[0031] The distance between the waypoint in the waypoint set that is closest to the start point of the initial local path segment and the start point of the initial local path segment is equal to the distance between the waypoint in the waypoint set that is closest to the end point of the initial local path segment and the end point of the initial local path segment.

[0032] Optionally, in the first sub-step of the second local path planning step, the waypoints in the waypoint set include obstacle avoidance waypoints, and the obstacle avoidance waypoints are determined in the following manner: determining an obstacle that conflicts with the global path; and determining at least one obstacle avoidance waypoint based on the position of the obstacle, so that the generated optimized local path can pass through the at least one obstacle avoidance waypoint and bypass the obstacle.

[0033] Optionally, the positions of obstacles that conflict with the global path are determined by obtaining the edges of the obstacles from the obstacle costmap.

[0034] Optionally, the at least one obstacle avoidance waypoint is determined with the aid of an obstacle cost map.

[0035] Optionally, in the first sub-step of the second local path planning step, the start and end points of the initial local path segment are determined so that: the distance from the start point of the initial local path segment to the obstacle is greater than a predetermined first distance threshold; and / or the distance from the end point of the initial local path segment to the obstacle is greater than a predetermined second distance threshold.

[0036] Optionally, in the second sub-step of the second local path planning step, the optimized local path is generated so that the velocity direction at at least one obstacle avoidance waypoint in the waypoint set satisfies one of the following conditions: the same as the velocity direction of the point on the initial local path segment that is closest to the corresponding obstacle avoidance waypoint; the same as the direction from the start point to the end point of the initial local path segment; perpendicular to the direction of descent of the obstacle cost gradient at the point on the edge of the obstacle that is closest to the corresponding obstacle avoidance waypoint.

[0037] Optionally, the mobile robot is a differential robot.

[0038] According to a second aspect of the present invention, a motion control method for a mobile robot is provided, wherein the motion control method comprises the following steps:

[0039] A first movement step S21 , wherein the mobile robot is moved along the planned path;

[0040] In the real-time local path planning step S22, the following sub-steps are performed:

[0041] In the first sub-step, at least a portion of the planned path is selected as the original local path segment;

[0042] A second sub-step is to generate a real-time optimized local path, such that the real-time optimized local path is a curve with a continuous second-order derivative, and the real-time optimized local path has the same starting point, end point, starting point speed direction, end point speed direction, starting point curvature, and end point curvature as the original local path segment; and

[0043] The third sub-step is to replace the original local path segment in the planned path with the real-time optimized local path; and

[0044] The second movement step S23 , wherein the mobile robot is controlled to move according to the replaced planned path.

[0045] Optionally, in the first sub-step of the real-time local path planning step S22, the current position point of the mobile robot is used as the starting point of the original local path segment.

[0046] Optionally, the real-time local path planning step S22 includes a first real-time local path planning step S22. In the first real-time local path planning step S22, the real-time optimized local path is a third-order Bezier curve, and the third-order Bezier curve is represented by the following formula:

[0047]

[0048] in, represents the coordinates of the control points of the Bezier curve and Among them, the control point The coordinates of are determined as follows:

[0049] are the coordinates of the starting point of the original local path segment, is the coordinate of the end point of the original local path segment;

[0050] Respectively Same positive and negative, where is the unit vector representing the velocity direction of the starting point of the original local path segment;

[0051] Respectively Same positive and negative, where is the unit vector representing the direction of the terminal velocity of the original local path segment;

[0052] Substituting s=0 and К(0)=К0 into Among them, К0 represents the curvature of the path starting point of the original local path segment, P x ′(s), P y ′(s), P x ″(s), P y ″(s) ​​are The first-order derivative, ordinate and the second-order derivative, ordinate;

[0053] Substituting s=1 and К(1)=К1 into , where К1 represents the path end curvature of the original local path segment.

[0054] Optionally, the real-time local path planning step S22 includes a second real-time local path planning step S22. In a first sub-step of the second real-time local path planning step S22, a set of waypoints is determined. At least one waypoint located within a predetermined distance from the mobile robot's current location is detected, the waypoint representing a point that the mobile robot needs to pass through. The waypoint set is a set of m waypoints adjacent to the initial local path segment, where m ≥ 1. In a second sub-step of the second real-time local path planning step S22, a real-time optimized local path is generated that passes through all waypoints in the waypoint set.

[0055] Optionally, in the second real-time local path planning step S22, the real-time optimized local path is an m+3 order Bezier curve, and the first control point and the m+4th control point of the m+3 order Bezier curve are respectively the starting point and the end point of the real-time optimized local path.

[0056] Optionally, in the second sub-step of the second real-time local path planning step S22, the real-time optimized local path is generated so that the speed direction at at least one waypoint in the set of waypoints satisfies one of the following conditions: the same as the speed direction of the point closest to the corresponding waypoint on the original local path segment; the same as the direction from the start point to the end point of the original local path segment; and the task requirements of the mobile robot at the corresponding waypoint are met.

[0057] Optionally, in the second real-time local path planning step S22, the end point of the original local path segment is determined such that:

[0058] Along the direction of the planned path, the end point of the original local path segment is located after all points on the planned path that are closest to each of the waypoints in the set of waypoints; and / or

[0059] The distance between the waypoint in the waypoint set that is closest to the starting point of the original local path segment and the starting point of the original local path segment is equal to the distance between the waypoint in the waypoint set that is closest to the end point of the original local path segment and the end point of the original local path segment.

[0060] Optionally, in the first sub-step of the second real-time local path planning step S22, the waypoints in the waypoint set include obstacle avoidance waypoints, and the obstacle avoidance waypoints are determined in the following manner: determining an obstacle that conflicts with the planned path; and determining at least one obstacle avoidance waypoint based on the position of the obstacle, so that the generated real-time optimized local path can pass through the at least one obstacle avoidance waypoint and bypass the obstacle.

[0061] Optionally, the positions of obstacles that conflict with the planned path are determined by obtaining the edges of the obstacles from the obstacle cost map.

[0062] Optionally, the at least one obstacle avoidance waypoint is determined with the aid of an obstacle cost map.

[0063] Optionally, in the first sub-step of the second real-time local path planning step S22, the start and end points of the original local path segment are determined so that: the distance from the start point of the original local path segment to the obstacle is greater than a predetermined third distance threshold; and / or the distance from the end point of the original local path segment to the obstacle is greater than a predetermined fourth distance threshold.

[0064] Optionally, in the second sub-step of the second real-time local path planning step S22, the real-time optimized local path is generated so that the speed direction at at least one obstacle avoidance waypoint in the waypoint set satisfies one of the following conditions: the same as the speed direction of the point closest to the corresponding obstacle avoidance waypoint on the original local path segment; the same as the direction from the start point to the end point of the original local path segment; perpendicular to the direction of the obstacle cost gradient descent at the point closest to the corresponding obstacle avoidance waypoint located on the edge of the obstacle.

[0065] Optionally, the mobile robot is a differential robot.

[0066] According to a third aspect of the present invention, a computer program product is provided, which includes computer program instructions, wherein, when the computer program instructions are executed by one or more processors, the processor is capable of executing the global path planning method according to the present invention or the motion control method according to the present invention.

[0067] The present invention has the advantage of enabling optimization with minimal deviation from the original global path, thereby smoothing the path while maintaining the advantages of the original global path. In particular, because the original starting point, end point, starting point velocity direction, end point velocity direction, starting point curvature, and end point curvature are retained during the optimization process, the optimized local path can be directly and smoothly replaced with the global path. Furthermore, the planning process can be simplified, reducing the computational effort. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0069] Figure 1 A flow chart of a global path planning method for a mobile robot according to an exemplary embodiment of the present invention is shown;

[0070] Figure 2 Schematically illustrating replacing an initial local path segment with an optimized local path to form a new global path in a global path planning method according to an exemplary embodiment of the present invention;

[0071] Figure 3 Schematically illustrating an optimized local path according to an exemplary embodiment of the present invention;

[0072] Figure 4 schematically illustrates an optimized local path according to an exemplary embodiment of the present invention; and

[0073] Figure 5 A motion control method for a mobile robot according to an exemplary embodiment of the present invention is shown. DETAILED DESCRIPTION

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

[0075] The present invention is applicable to a mobile robot 1, which can be any robot capable of autonomous spatial movement, such as an AGV, an AMR, etc. The mobile robot 1 can be used to perform various tasks, such as a warehouse robot, a cleaning robot, a home care robot, a welcoming robot, etc.

[0076] It should be understood that, in this document, the terms "first," "second," etc., are used for descriptive purposes only and should not be construed as indicating or implying relative importance, nor as implicitly specifying the quantity of the technical features being referenced. A feature specified as "first" or "second" may explicitly or implicitly indicate that at least one of the features is included.

[0077] Figure 1 A flow chart of a global path planning method for a mobile robot 1 according to an exemplary embodiment of the present invention is shown. The global path planning method comprises the following steps:

[0078] Initial planning step S11 , wherein an initial global path for the mobile robot 1 is obtained;

[0079] The local path planning step S12 includes the following sub-steps:

[0080] In a first sub-step, at least a portion of the (current) global path is selected as an initial local path segment;

[0081] A second sub-step is to generate an optimized local path 2, so that the optimized local path 2 is a curve with a continuous second-order derivative, and the optimized local path 2 has the same starting point, end point, starting speed direction, end speed direction, starting curvature, and end curvature as the initial local path segment; and

[0082] In a third sub-step, the initial local path segment is replaced with the optimized local path 2 to form a new global path; and

[0083] A global path determination step S13 is performed, wherein the new global path is determined as the final global path.

[0084] The initial global path can be obtained using any applicable known method. For example, the initial global path can be planned by the A* algorithm. Generally, the initial global path is a better path that has taken into account the constraints and optimization goals for the mobile robot 1. For example, the initial global path may be the shortest path from the starting point to the end point. However, the initial global path often cannot guarantee the smoothness of the path. The global path planning method according to the present invention is optimized on the basis of less deviation from the initial global path, thereby smoothing the global path while maintaining the advantages of the initial global path, such as short path length. In particular, since the optimized local path 2 has the same starting point, end point, starting point speed direction, end point speed direction, starting point curvature and end point curvature as the initial local path segment, the optimized local path 2 can be directly and smoothly replaced in the global path. In addition, the planning process can be simplified and the amount of calculation can be reduced.

[0085] Those skilled in the art will understand that the start point, end point, start point velocity direction, end point velocity direction, start point curvature and end point curvature of the initial local path segment can be derived from the initial global path.

[0086] Figure 2 Schematically illustrating that an initial local path segment is replaced by an optimized local path 2 to form a new global path in a global path planning method according to an exemplary embodiment of the present invention.

[0087] exist Figure 2 In the illustrated embodiment, the mobile robot 1 is, for example, a differential robot, that is, the mobile robot 1 includes a differential wheel motion system. For a differential robot, optimizing the local path 2 with a continuous second-order derivative can be particularly advantageous for adapting to the motion characteristics of the differential robot. In particular, the optimized local path 2 can have a continuous curvature. This allows the speed and acceleration of the mobile robot 1 to change more smoothly. Alternatively, the mobile robot 1 can also be another type of robot, such as a single-steering wheel robot or a dual-steering wheel robot. Accordingly, the mobile robot 1 can, for example, include a dual-steering wheel motion system.

[0088] Mobile robot 1, for example, includes a communication device for communicating with other devices, such as a dispatch control system. In the initial planning step S11, the communication device can be used to receive an initial global path from the other devices. Mobile robot 1 may also include sensors that allow it to obtain required information, such as its current location.

[0089] Mobile robot 1, for example, also includes a controller. The controller is used to control components of mobile robot 1, such as a differential wheel motion system, sensors, and communication devices. The controller can also receive operating status or detection data from corresponding components, such as sensors, via communication lines to monitor or control the operation of mobile robot 1. For example, the controller can also plan an initial global path in the initial planning step S11. The global path planning method can be executed, for example, by the controller or by another device capable of exchanging data with the controller, such as a scheduling control system.

[0090] exist Figure 2 In FIG, the global path obtained in the initial planning step S11 is partially shown in dashed lines. Figure 2 It can be seen that the global path is not smooth at points S1 and S2. From the global path, the portion from P0 to P3 adjacent to points S1 and S2 is selected as the initial local path segment.

[0091] Based on the initial local path segment, an optimized local path 2 (shown as a solid line) is generated, such that the optimized local path 2 is a curve having a continuous second-order derivative and has the same starting point, end point, starting point velocity direction, end point velocity direction, starting point curvature, and end point curvature as the initial local path segment. The starting point velocity direction and the end point velocity direction represent the velocity direction of the mobile robot 1 at the starting point and end point of the corresponding path, respectively. The starting point curvature and the end point curvature represent the curvature of the corresponding path at the starting point and end point, respectively.

[0092] Then, the initial local path segment is replaced by the optimized local path 2 to form a new global path. Finally, the new global path can be determined as the final global path.

[0093] The local path planning step S12 may be repeatedly performed until any point in the new global path is located on at least one optimized local path 2 , and then the final determination step is performed.

[0094] In the local path planning step S12, initial local path segments are selected so that the starting point of each optimized local path 2 is located on the final global path. When the local path planning step S12 is repeatedly executed, initial local path segments are selected so that the initial local path segments do not contain the starting point of the previously generated optimized local path 2. For example, initial local path segments are selected one by one along the global path from the starting point to the end point of the global path, so that the starting point of each initial local path segment selected later is closer to the end point than the starting point of each initial local path segment selected earlier.

[0095] Optionally, the length of the initial local path segment may be set to be less than a predetermined length threshold, thereby helping to avoid excessive deviation from the initial global path.

[0096] Figure 2 It is shown in FIG. 1 that the local path planning step S12 may include a first local path planning step, in which the optimized local path 2 is a third-order Bezier curve.

[0097] The third-order Bezier curve can be expressed by the following formula:

[0098]

[0099] in, represents the coordinates of the control points of the Bezier curve and

[0100] The following is an example of how to determine the control points. process.

[0101] are the coordinates of the starting point of the initial local path segment, The coordinates of the end point of the initial local path segment are as follows:

[0102] The direction of the line connecting the first control point and the second control point is along the velocity direction of the starting point of the initial local path segment. The velocity direction of the starting point of the initial local path segment is replaced by a unit vector If , then the first control point and the second control point have the following relationship:

[0103]

[0104] in And (x1-x0), (y1-y0) are respectively The same sign and negative are used. This allows the optimized partial path 2 to have the same starting velocity direction as the initial partial path segment. This is particularly advantageous for differential robots, as the velocity direction of a differential robot can only be along the forward direction of the differential robot itself. If the differential robot has a given starting pose at the starting point, the optimized partial path 2 thus planned will have a starting velocity direction that matches the given pose.

[0105] Similarly, Respectively Same positive and negative, where is a unit vector representing the terminal velocity direction of the initial local path segment. This allows the planned optimized local path 2 to have a predetermined terminal velocity direction. This is particularly advantageous for differential robots. If the differential robot has a given terminal pose at its endpoint, the planned optimized local path 2 will have a terminal velocity direction that matches the given pose.

[0106] Taking into account the curvature of the path starting point, substitute s = 0 and К(0) = К0 into Where К0 represents the path starting curvature of the initial local path segment, P x ′(s), P y ′(s), P x ″(s), P y ″(s) ​​are The first-order derivative transverse and ordinate and the second-order derivative transverse and ordinate.

[0107] For differential robots, the curvature of the motion trajectory can be obtained by the ratio of the angular velocity to the linear velocity. By setting the linear velocity at the starting point P0 to V0 and the angular velocity to ω0, the curvature of the path starting point can be set to Similarly, the predetermined path end curvature at the end point can be obtained.

[0108] Similarly, substitute s=1 and К(1)=К1 into , where К1 represents the path end curvature of the initial local path segment.

[0109] Therefore, in the first local path planning step, the coordinates of each control point of the optimized local path 2 can be determined according to the starting point, end point, starting speed direction, end speed direction, starting curvature and end curvature of the initial local path segment, and then the optimized local path 2 can be directly determined.

[0110] Alternatively, the optimized local path 2 may also be another type of smooth curve, such as a polynomial curve or a B-spline curve, in particular a clamped-B-spline curve, such as a NURBS curve.

[0111] Figure 3 An optimized local path 2 according to an exemplary embodiment of the present invention is schematically shown.

[0112] exist Figure 3In the illustrated embodiment, the local path planning step S12 includes a second local path planning step. In the first sub-step of this second local path planning step, a set of waypoints is determined. Waypoints represent points that the mobile robot 1 needs to pass through. The set of waypoints is composed of m waypoints adjacent to the initial local path segment, where m ≥ 1. In the second sub-step of this second local path planning step, an optimized local path 2 is generated that passes through all waypoints in the set of waypoints.

[0113] In the second local path planning step, the optimized local path 2 may be an m+3 order Bezier curve, where the first control point and the m+4th control point of the m+3 order Bezier curve are the starting point and the end point of the optimized local path 2, respectively. Figure 3 The case where m = 1 is exemplarily shown. Accordingly, the optimized local path 2 is a fourth-order Bezier curve.

[0114] In this case, in the curve determination step S12, the curve can be expressed by the following equation:

[0115]

[0116] Where i = 0, 1, ..., m + 3, represents the coordinates of the control points of the Bezier curve and

[0117] Control Point The coordinates of can be determined as follows:

[0118] is the coordinate of the starting point, is the coordinate of the end point;

[0119] Respectively Same positive and negative, where is a unit vector indicating the direction of the predetermined starting velocity;

[0120] Respectively Same positive and negative, where is a unit vector indicating the direction of the predetermined terminal velocity; in, represents the coordinates of the j-th waypoint in the at least one waypoint, j=1, 2, ..., m, Indicates the value of s corresponding to the j-th waypoint;

[0121] and The direction is the same, where is a unit vector representing the direction of the velocity of the predetermined waypoint at the jth waypoint, and P′(s) represents The coordinates of the first derivative of express exist The coordinates of the first derivative at ;

[0122] Substituting s=0 and К(0)=К0 into , where К0 represents the predetermined curvature of the starting point of the path; and

[0123] Substituting s=1 and К(1)=К1 into Wherein, К1 represents the predetermined curvature of the path end point.

[0124] Optionally, in the second sub-step of the second local path planning step, the optimized local path 2 is generated so that the velocity direction at at least one waypoint in the set of waypoints satisfies one of the following conditions:

[0125] The velocity direction of the point closest to the waypoint corresponding to the distance on the initial local path segment is the same;

[0126] The same direction as the start point to the end point of the initial local path segment;

[0127] Satisfy the mission requirements of the mobile robot 1 at the corresponding waypoints.

[0128] For example, in some scenarios, mobile robot 1 needs to precisely pass through a specific location to complete a task. For example, mobile robot 1 may include a barcode scanner and be tasked with scanning a QR code at a specific location. This specific location can be used as a waypoint, ensuring that the optimized local path 2 must pass through it in order for mobile robot 1 to perform the corresponding task. Such a waypoint is also referred to herein as a "task waypoint."

[0129] When m>1, the method of determining each control point is the same as that for Figure 3 The method described for the case of m = 1 is similar. The main difference is that when the number of waypoints is greater than 1, two additional constraints are added for each additional waypoint, based on the waypoint coordinates and the velocity direction at the waypoint. Accordingly, the order of the Bezier curve increases by one, requiring the coordinates of an additional control point to be determined. This is achieved by adding two additional constraints.

[0130] In the second local path planning step, the start and end points of the initial local path segments are determined such that:

[0131] The starting point of the initial local path segment is located before all points on the global path that are respectively closest to each of the waypoints in the set of waypoints, along the direction of the global path; and / or

[0132] Along the direction of the global path, the end point of the initial local path segment is located after all points on the global path that are respectively closest to each of the waypoints in the set of waypoints; and / or

[0133] The distance between the waypoint in the waypoint set that is closest to the start point of the initial local path segment and the start point of the initial local path segment is equal to the distance between the waypoint in the waypoint set that is closest to the end point of the initial local path segment and the end point of the initial local path segment.

[0134] The first and second local path planning steps described above can be understood as different types of local path planning steps S12. Obviously, the local path planning steps S12 repeatedly executed in the global path planning method may include local path planning steps S12 of the same type or different types. In other words, the repeatedly executed local path planning steps S12 may include the first local path planning step and / or the second real-time local path planning step S22. The final global path can be composed of Bezier curves of different orders.

[0135] Figure 4 The optimized local path 2 according to an exemplary embodiment of the present invention is schematically shown. Figure 4 In the embodiment shown, the local path planning step S12 includes a second local path planning step. In the first sub-step of the second local path planning step, the waypoints in the waypoint set include obstacle avoidance waypoints ( Figure 4 Marked as P v ).

[0136] The obstacle avoidance waypoints can be determined by: determining an obstacle 3 that conflicts with the global path; and determining at least one obstacle avoidance waypoint based on the position of the obstacle 3, so that the generated optimized local path 2 can pass through the at least one obstacle avoidance waypoint and bypass the obstacle 3. Figure 4 As shown, the global path partially shown with a dashed line collides with obstacle 3. Accordingly, an obstacle avoidance waypoint is determined.

[0137] Optionally, the position of the obstacle 3 that conflicts with the global path is determined by obtaining the edge of the obstacle 3 from an obstacle costmap.

[0138] Optionally, the at least one obstacle avoidance waypoint can be determined using an obstacle cost map. For example, the point with the highest cost in the initial local path segment can be moved along the direction of the decreasing cost gradient of obstacle 3 at that point until it reaches a position a certain distance away from obstacle 3, and then this position is determined as the obstacle avoidance waypoint. For another example, the point in the initial local path segment that intersects the edge of obstacle 3 can be moved along the direction of the decreasing cost gradient of obstacle 3 until it reaches a position a certain distance away from obstacle 3, and then this position is determined as the obstacle avoidance waypoint.

[0139] exist Figure 4 In FIG, an example is shown in which one obstacle avoidance waypoint is set so that the generated optimized local path 2 avoids the obstacle 3. When setting one obstacle avoidance waypoint cannot avoid the obstacle 3, multiple waypoints can be added.

[0140] Optionally, in the second sub-step of the second local path planning step, the optimized local path 2 is generated so that the velocity direction at at least one obstacle avoidance waypoint in the waypoint set satisfies one of the following conditions: the same as the velocity direction of the point on the initial local path segment that is closest to the corresponding obstacle avoidance waypoint; the same as the direction from the start point to the end point of the initial local path segment; perpendicular to the direction of descent of the cost gradient of obstacle 3 at the point on the edge of obstacle 3 that is closest to the corresponding obstacle avoidance waypoint.

[0141] In the first sub-step of the second local path planning step, the start and end points of the initial local path segment are determined such that: the distance from the start point of the initial local path segment to obstacle 3 is greater than a predetermined first distance threshold; and / or the distance from the end point of the initial local path segment to obstacle 3 is greater than a predetermined second distance threshold. This helps optimize local path 2 avoid obstacle 3.

[0142] Figure 5 A motion control method for a mobile robot 1 according to an exemplary embodiment of the present invention is shown. The motion control method comprises the following steps:

[0143] A first movement step S21 , wherein the mobile robot 1 is moved along the planned path;

[0144] In the real-time local path planning step S22, the following sub-steps are performed:

[0145] In the first sub-step, at least a portion of the (current) planned path is selected as the original local path segment;

[0146] A second sub-step is to generate a real-time optimized local path 2, so that the real-time optimized local path 2 is a curve with a continuous second-order derivative, and the real-time optimized local path 2 has the same starting point, end point, starting point speed direction, end point speed direction, starting point curvature, and end point curvature as the original local path segment; and

[0147] The third sub-step is to replace the original local path segment in the planned path with the real-time optimized local path 2; and

[0148] The second movement step S23 , wherein the mobile robot 1 is controlled to move according to the replaced planned path.

[0149] As can be seen, the motion control method shares similar characteristics with the aforementioned global path planning, and therefore offers corresponding advantages. Particularly advantageously, after a path is planned for mobile robot 1, factors such as the environment in which mobile robot 1 moves may change before mobile robot 1 begins to move. The motion control method can optimize the motion path with minimal deviation from the original path, making the motion path more adaptable to the real-time environment of mobile robot 1 as it moves.

[0150] The original planned path can be obtained using any suitable known method. For example, the original planned path can be planned using the A-star algorithm. The motion control method according to the present invention optimizes the original planned path based on minimal deviation from the original planned path, thereby maintaining the advantages of the original planned path, such as a short path length. Furthermore, the planning process can be simplified, reducing the amount of computation required.

[0151] The motion control method can be applied to the mobile robot 1 described above, in particular to a differential robot. The motion control method can be executed, for example, by means of a controller of the mobile robot 1 .

[0152] In the first sub-step of the real-time local path planning step S22 , the current position point of the mobile robot 1 is used as the starting point of the original local path segment.

[0153] Optionally, the real-time local path planning step S22 includes a first real-time local path planning step S22. In the first real-time local path planning step S22, the real-time optimized local path 2 is a third-order Bezier curve, and the third-order Bezier curve is expressed by the following formula:

[0154]

[0155] in, represents the coordinates of the control points of the Bezier curve and

[0156] Determine control points The coordinates of can be similar to those described above for the global path planning method:

[0157] are the coordinates of the starting point of the original local path segment, is the coordinate of the end point of the original local path segment;

[0158] Respectively Same positive and negative, where is the unit vector representing the velocity direction of the starting point of the original local path segment;

[0159] Respectively Same positive and negative, where is the unit vector representing the direction of the terminal velocity of the original local path segment;

[0160] Substituting s=0 and К(0)=К0 into Where, К0 represents the curvature of the path starting point of the original local path segment, P x ′(s), P y ′(s), P x ″(s), P y ″(s) ​​are The first-order derivative, ordinate and the second-order derivative, ordinate;

[0161] Substituting s=1 and К(1)=К1 into , where К1 represents the path end curvature of the original local path segment.

[0162] Optionally, the real-time local path planning step S22 includes a second real-time local path planning step S22. In a first sub-step of the second real-time local path planning step S22, a set of waypoints is determined. At least one waypoint located within a predetermined distance from the current position of the mobile robot 1 is detected, the waypoint representing a point that the mobile robot 1 needs to pass through. The waypoint set is a set of m waypoints adjacent to the initial local path segment, where m ≥ 1. In a second sub-step of the second real-time local path planning step S22, a real-time optimized local path 2 is generated that passes through all waypoints in the waypoint set.

[0163] In the second real-time local path planning step S22, the real-time optimized local path 2 is an m+3 order Bezier curve, and the first control point and the m+4th control point of the m+3 order Bezier curve are the starting point and the end point of the real-time optimized local path 2 respectively. The method of calculating the coordinates of can also be similar to that described above for the global path planning method, and will not be repeated here.

[0164] Optionally, in the second sub-step of the second real-time local path planning step S22, the real-time optimized local path 2 is generated so that the speed direction at at least one waypoint in the waypoint set satisfies one of the following conditions: the same as the speed direction of the point closest to the corresponding waypoint on the original local path segment; the same as the direction from the start point to the end point of the original local path segment; and the task requirements of the mobile robot 1 at the corresponding waypoint are met.

[0165] Optionally, in the second real-time local path planning step S22, the end point of the original local path segment is determined so that: along the direction of the planned path, the end point of the original local path segment is located after all points on the planned path that are respectively closest to each waypoint in the waypoint set; and / or the distance between the waypoint in the waypoint set that is closest to the starting point of the original local path segment and the starting point of the original local path segment is equal to the distance between the waypoint in the waypoint set that is closest to the end point of the original local path segment and the end point of the original local path segment.

[0166] Optionally, in the first sub-step of the second real-time local path planning step S22, the waypoints in the waypoint set include obstacle avoidance waypoints, which are determined by: determining an obstacle 3 that conflicts with the planned path; and determining at least one obstacle avoidance waypoint based on the position of the obstacle 3, so that the generated real-time optimized local path 2 can pass through the at least one obstacle avoidance waypoint and bypass the obstacle 3. When the mobile robot 1 is moving, the trajectory of the original planned path near the obstacle 3 may be uneven, or a new static obstacle 3 may appear on the original planned path that interferes with the original planned path. The obstacle avoidance waypoints enable the mobile robot 1 to avoid the obstacle 3 along a smooth trajectory. The method for determining the obstacle avoidance waypoints may be similar to that described above for the global path planning method.

[0167] In the first sub-step of the second real-time local path planning step S22, the start and end points of the original local path segment are determined so that: the distance from the start point of the original local path segment to the obstacle 3 is greater than a predetermined third distance threshold; and / or the distance from the end point of the original local path segment to the obstacle 3 is greater than a predetermined fourth distance threshold.

[0168] In the second sub-step of the second real-time local path planning step S22, the real-time optimized local path 2 is generated so that the speed direction at at least one obstacle avoidance waypoint in the waypoint set satisfies one of the following conditions: the same as the speed direction of the point closest to the corresponding obstacle avoidance waypoint on the original local path segment; the same as the direction from the start point to the end point of the original local path segment; perpendicular to the direction of the descent of the cost gradient of obstacle 3 at the point closest to the corresponding obstacle avoidance waypoint and located on the edge of obstacle 3.

[0169] The motion control method according to the present invention has corresponding characteristics and similar principles to the global path planning method according to the present invention. The features and advantages described above for the global path planning method can also be applied to the motion control method accordingly.

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

[0171] In the present invention, the computer program product may be stored in a computer-readable storage medium. The computer-readable storage medium may include, for example, a high-speed random access memory, and may also include a non-volatile memory, such as a hard disk, a memory card, 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 memory device. The processor 10 may be a central processing unit, or may be another general-purpose processor, a digital signal processor, an application-specific integrated circuit, an off-the-shelf programmable gate array or other programmable logic device, a discrete gate or transistor logic device, a discrete hardware component, or the like. The general-purpose processor may be a microprocessor or any conventional processor, or the like.

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

Claims

1. A global path planning method for a mobile robot (1), wherein: The global path planning method comprises the following steps: an initial planning step S11, wherein an initial global path for the mobile robot (1) is obtained; The local path planning step S12 includes the following sub-steps: In the first sub-step, at least a portion of the global path is selected as an initial local path segment; A second sub-step is to generate an optimized local path (2), so that the optimized local path (2) is a curve with a continuous second-order derivative, and the optimized local path (2) has the same starting point, ending point, starting point speed direction, ending point speed direction, starting point curvature, and ending point curvature as the initial local path segment; and A third sub-step is to replace the initial local path segment with the optimized local path (2) to form a new global path; and Global path determination step S13, wherein the new global path is determined as the final global path, The local path planning step S12 includes a second local path planning step, wherein: In a first sub-step of the second local path planning step, a set of waypoints is determined, where the waypoints represent points that the mobile robot (1) needs to pass through, and the set of waypoints is a set of m waypoints adjacent to the initial local path segment, where m≥1; In a second sub-step of the second local path planning step, the generated optimized local path (2) passes through all waypoints in the set of waypoints, wherein the optimized local path (2) is generated so that the speed direction at at least one waypoint in the set of waypoints satisfies one of the following conditions: The velocity direction of the point closest to the waypoint corresponding to the distance on the initial local path segment is the same; The same direction as the start point to the end point of the initial local path segment; Satisfy the mission requirements of the mobile robot (1) at the corresponding waypoints.

2. The global path planning method according to claim 1, wherein: The local path planning step S12 is performed in at least one of the following ways: In the local path planning step S12, the initial local path segments are selected in such a way that the starting point of each optimized local path (2) is located on the final global path; The length of the initial local path segment is less than a predetermined length threshold; The local path planning step S12 is repeatedly executed until any point in the new global path is located on at least one optimized local path (2), and then the final determination step S13 is executed.

3. The global path planning method according to claim 1 or 2, wherein: The local path planning step S12 includes a first local path planning step. In the first local path planning step, the optimized local path (2) is a third-order Bezier curve, and the third-order Bezier curve is represented by the following formula: in, represents the coordinates of the control points of the Bezier curve and Among them, the control point The coordinates of are determined as follows: are the coordinates of the starting point of the initial local path segment, are the coordinates of the end point of the initial local path segment; (x1-x0), (y1-y0) are respectively Same positive and negative, where is the unit vector representing the velocity direction of the starting point of the initial local path segment; (x3-x2), (y3-y2) are respectively Same positive and negative, where is the unit vector representing the terminal velocity direction of the initial local path segment; Substituting s=0 and К(0)=К0 into Where К0 represents the path starting curvature of the initial local path segment, P x ′(s), P y ′(s), P x ″(s), P y ″(s) ​​are The first-order derivative, ordinate and the second-order derivative, ordinate; Substituting s=1 and К(1)=К1 into , where К1 represents the path end curvature of the initial local path segment.

4. The global path planning method according to claim 1 or 2, wherein: In the second local path planning step, the optimized local path (2) is an m+3 order Bezier curve, and the first control point and the m+4th control point of the m+3 order Bezier curve are respectively the starting point and the end point of the optimized local path (2).

5. The global path planning method according to claim 1 or 2, wherein: In the second local path planning step, the start and end points of the initial local path segments are determined such that: The starting point of the initial local path segment is located before all points on the global path that are respectively closest to each of the waypoints in the set of waypoints, along the direction of the global path; and / or Along the direction of the global path, the end point of the initial local path segment is located after all points on the global path that are respectively closest to each of the waypoints in the set of waypoints; and / or The distance between the waypoint in the waypoint set that is closest to the start point of the initial local path segment and the start point of the initial local path segment is equal to the distance between the waypoint in the waypoint set that is closest to the end point of the initial local path segment and the end point of the initial local path segment.

6. The global path planning method according to claim 1 or 2, wherein: In the first sub-step of the second local path planning step, the waypoints in the waypoint set include obstacle avoidance waypoints, and the obstacle avoidance waypoints are determined in the following manner: Identify obstacles that conflict with the global path (3); and At least one obstacle avoidance waypoint is determined according to the position of the obstacle (3), so that the generated optimized local path (2) can pass through the at least one obstacle avoidance waypoint and bypass the obstacle (3).

7. The global path planning method according to claim 6, wherein: Determining the position of the obstacle (3) that conflicts with the global path by obtaining the edge of the obstacle (3) from the obstacle cost map; and / or The at least one obstacle avoidance waypoint is determined with the aid of an obstacle cost map.

8. The global path planning method according to claim 6, wherein: In the first sub-step of the second local path planning step, the start and end points of the initial local path segments are determined such that: The distance from the starting point of the initial local path segment to the obstacle (3) is greater than a predetermined first distance threshold; and / or The distance from the end point of the initial local path segment to the obstacle (3) is greater than a predetermined second distance threshold.

9. The global path planning method according to claim 6, wherein: In the second sub-step of the second local path planning step, the optimized local path (2) is generated so that the velocity direction at at least one obstacle avoidance waypoint in the set of waypoints satisfies one of the following conditions: The velocity direction of the point closest to the obstacle avoidance waypoint corresponding to the distance on the initial local path segment is the same; The same direction as the start point to the end point of the initial local path segment; The direction of the gradient descent of the obstacle (3) cost at the point on the edge of the obstacle (3) closest to the corresponding obstacle avoidance waypoint is perpendicular.

10. The global path planning method according to any one of claims 1-2 and 7-9, wherein: The mobile robot (1) is a differential robot.

11. A motion control method for a mobile robot (1), wherein: The motion control method comprises the following steps: a first movement step S21 , wherein the mobile robot (1) is moved along a planned path; In the real-time local path planning step S22, the following sub-steps are performed: In the first sub-step, at least a portion of the planned path is selected as the original local path segment; A second sub-step is to generate a real-time optimized local path (2), so that the real-time optimized local path (2) is a curve with a continuous second-order derivative, and the real-time optimized local path (2) has the same starting point, end point, starting point speed direction, end point speed direction, starting point curvature, and end point curvature as the original local path segment; and The third sub-step is to replace the original local path segment in the planned path with the real-time optimized local path (2); and The second movement step S23, wherein the mobile robot (1) is controlled to move according to the replaced planned path, The real-time local path planning step S22 includes a second real-time local path planning step S22, wherein: In a first sub-step of the second real-time local path planning step S22, a waypoint set is determined, wherein at least one waypoint located within a predetermined distance range relative to a current position point of the mobile robot (1) is detected, the waypoint representing a point that the mobile robot (1) needs to pass through, and the waypoint set is a set consisting of m waypoints adjacent to the original local path segment, where m≥1; In the second sub-step of the second real-time local path planning step S22, the generated real-time optimized local path (2) passes through all the waypoints in the waypoint set, and the real-time optimized local path (2) is generated so that the speed direction at at least one waypoint in the waypoint set satisfies one of the following conditions: The velocity direction of the point closest to the waypoint corresponding to the distance on the original local path segment is the same; The direction from the start point to the end point of the original local path segment is the same; Satisfy the mission requirements of the mobile robot (1) at the corresponding waypoints.

12. The motion control method according to claim 11, wherein: In the first sub-step of the real-time local path planning step S22, the current position point of the mobile robot (1) is used as the starting point of the original local path segment.

13. The motion control method according to claim 11 or 12, wherein: The real-time local path planning step S22 includes a first real-time local path planning step S22. In the first real-time local path planning step S22, the real-time optimized local path (2) is a third-order Bezier curve, and the third-order Bezier curve is represented by the following formula: in, represents the coordinates of the control points of the Bezier curve and Among them, the control point The coordinates of are determined as follows: are the coordinates of the starting point of the original local path segment, is the coordinate of the end point of the original local path segment; (x1-x0), (y1-y0) are respectively Same positive and negative, where is the unit vector representing the velocity direction of the starting point of the original local path segment; (x3-x2), (y3-y2) are respectively Same positive and negative, among them, is the unit vector representing the direction of the terminal velocity of the original local path segment; Substituting s=0 and К(0)=К0 into Where, К0 represents the curvature of the path starting point of the original local path segment, P x ′(s), P y ′(s), P x ″(s), P y ″(s) ​​are The first-order derivative, ordinate and the second-order derivative, ordinate; Substituting s=1 and К(1)=К1 into , where К1 represents the path end curvature of the original local path segment.

14. The motion control method according to claim 11 or 12, wherein: In the second real-time local path planning step S22, the real-time optimized local path (2) is an m+3 order Bezier curve, and the first control point and the m+4th control point of the m+3 order Bezier curve are the starting point and the end point of the real-time optimized local path (2), respectively.

15. The motion control method according to claim 11 or 12, wherein: In the second real-time local path planning step S22, the end point of the original local path segment is determined such that: Along the direction of the planned path, the end point of the original local path segment is located after all points on the planned path that are closest to each of the waypoints in the set of waypoints; and / or The distance between the waypoint in the waypoint set that is closest to the starting point of the original local path segment and the starting point of the original local path segment is equal to the distance between the waypoint in the waypoint set that is closest to the end point of the original local path segment and the end point of the original local path segment.

16. The motion control method according to claim 11 or 12, wherein: In the first sub-step of the second real-time local path planning step S22, the waypoints in the waypoint set include obstacle avoidance waypoints, and the obstacle avoidance waypoints are determined by: Identify obstacles that conflict with the planned path (3); and At least one obstacle avoidance waypoint is determined according to the position of the obstacle (3), so that the generated real-time optimized local path (2) can pass through the at least one obstacle avoidance waypoint and bypass the obstacle (3).

17. The motion control method according to claim 16, wherein: Determining the position of the obstacle (3) that conflicts with the planned path by obtaining the edge of the obstacle (3) from the obstacle cost map; and / or The at least one obstacle avoidance waypoint is determined with the aid of an obstacle cost map.

18. The motion control method according to claim 16, wherein: In the first sub-step of the second real-time local path planning step S22, the start and end points of the original local path segment are determined such that: The distance from the starting point of the original local path segment to the obstacle (3) is greater than a predetermined third distance threshold; and / or The distance from the end point of the original local path segment to the obstacle (3) is greater than a predetermined fourth distance threshold.

19. The motion control method according to claim 16, wherein: In the second sub-step of the second real-time local path planning step S22, the real-time optimized local path (2) is generated so that the velocity direction at at least one obstacle avoidance waypoint in the set of waypoints satisfies one of the following conditions: The velocity direction of the point closest to the obstacle avoidance waypoint corresponding to the distance on the original local path segment is the same; The direction from the start point to the end point of the original local path segment is the same; The direction of the gradient descent of the obstacle (3) cost at the point on the edge of the obstacle (3) closest to the corresponding obstacle avoidance waypoint is perpendicular.

20. The motion control method according to any one of claims 11-12, 17-19, wherein: The mobile robot (1) is a differential robot.

21. 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 global path planning method according to any one of claims 1-10 or the motion control method according to any one of claims 11-19.

Images