WO2023036044A1 - 全局路径规划方法、运动控制方法及计算机程序产品 - Google Patents

全局路径规划方法、运动控制方法及计算机程序产品 Download PDF

Info

Publication number
WO2023036044A1
WO2023036044A1 PCT/CN2022/116465 CN2022116465W WO2023036044A1 WO 2023036044 A1 WO2023036044 A1 WO 2023036044A1 CN 2022116465 W CN2022116465 W CN 2022116465W WO 2023036044 A1 WO2023036044 A1 WO 2023036044A1
Authority
WO
WIPO (PCT)
Prior art keywords
path
point
partial
segment
optimized
Prior art date
Application number
PCT/CN2022/116465
Other languages
English (en)
French (fr)
Inventor
赵安
张传发
邸兴超
唐文庆
武文博
赵雨辰
Original Assignee
灵动科技（北京）有限公司
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 灵动科技（北京）有限公司 filed Critical 灵动科技（北京）有限公司
Publication of WO2023036044A1 publication Critical patent/WO2023036044A1/zh

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions

Definitions

  • the present invention relates to the field of mobile robots, especially the field of motion control of mobile robots, in particular to a global path planning method for mobile robots, a motion control method for mobile robots and a computer program product.
  • mobile robots are more and more widely used in various industrial and domestic environments.
  • mobile robots such as automated guided vehicles (AGVs), autonomous mobile robots (AMRs), and forklifts are one of the key equipment in modern logistics systems.
  • AGVs automated guided vehicles
  • AMRs autonomous mobile robots
  • forklifts are one of the key equipment in modern logistics systems.
  • Mobile robots can move and dock to target locations according to path planning and job requirements to complete tasks such as material handling and delivery.
  • Path planning is the key in the motion control of mobile robots.
  • the object of the present invention is to provide an improved global path planning method and motion control method for a mobile robot to optimize the motion trajectory of the mobile robot.
  • a global path planning method for a mobile robot includes the following steps:
  • Initial planning step S11 wherein an initial global path for the mobile robot is acquired
  • Partial path planning step S12 wherein the following sub-steps are performed:
  • the first sub-step is to select at least a part of the global path as an initial local path segment
  • the second sub-step is to generate an optimized local path, so 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 point velocity direction, and end point as the initial partial path section Velocity direction, start and end curvatures;
  • a third sub-step replacing the initial local path segment with the optimized local path to form a new global path
  • the global path determination step S13 wherein the new global path is determined as the final global path.
  • the partial path planning step S12 is performed in at least one of the following ways:
  • an initial local path section is selected in such a way that the starting point of each optimized local path is located on the final global path;
  • the length of the initial partial path segment is less than a predetermined length threshold
  • the local path planning step S12 is repeatedly executed, and the final determination step S13 is not executed until any point in the new global path is located on at least one optimized local path.
  • the partial path planning step S12 includes a first partial path planning step, in which the optimized partial path is a third-order Bezier curve, and the third-order Bezier curve is given by the following formula express:
  • the partial path planning step S12 includes a second partial path planning step.
  • a set of waypoints is determined, the waypoints represent the points that the mobile robot needs to pass through, and the set of waypoints is composed of m waypoints adjacent to the initial local path section The set of m ⁇ 1.
  • the generated optimized partial path passes through all the waypoints in the set of waypoints.
  • the optimized partial path is an m+3 order Bezier curve, the first control point and the m+4th order of the m+3 order Bezier curve
  • the control points are the starting point and the ending point of the optimized local path, respectively.
  • the optimized partial path is generated such that the velocity direction at at least one passing point in the passing point set satisfies one of the following conditions: the same as the initial The speed direction of the point closest to the corresponding passing point on the local path section is the same; it is the same as the direction from the start point to the end point of the initial local path section; and the task requirements of the mobile robot at the corresponding passing point are met.
  • the start and end points of the initial partial path segments are determined such that:
  • the starting point of the initial local path segment is located before all the points on the global path that are respectively closest to the respective via points in the set of via points;
  • 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;
  • the distance between the closest waypoint in the set of waypoints to the start point of the initial local path segment and the start point of the initial local path section is equal to the distance between the waypoint in the set of waypoints closest to the end point of the initial local path segment and the initial local path area The distance between the end points of the segment.
  • the waypoints in the set of waypoints include obstacle avoidance waypoints, and the obstacle avoidance waypoints are determined by Conflicting obstacles; and determining at least one obstacle-avoiding waypoint according to the position of the obstacle, so that the generated optimized local path can pass through the at least one obstacle-avoiding waypoint and bypass the obstacle.
  • the position of the obstacle that conflicts with the global path is determined by obtaining the edge of the obstacle from the obstacle cost map.
  • the at least one obstacle avoidance waypoint is determined by means of an obstacle cost map.
  • the starting point and the ending point of the initial partial path section are determined such that: the distance from the starting point of the initial partial path section to the obstacle is greater than a predetermined first distance threshold; and/or the distance from the end point of the initial partial path segment to the obstacle is greater than a predetermined second distance threshold.
  • the optimized partial path is generated such that the velocity direction at at least one obstacle avoidance passing point in the set of passing points satisfies one of the following conditions:
  • the velocity direction of the point closest to the corresponding obstacle avoidance pass point on the initial local path segment is the same as the velocity direction; it is the same as the direction from the start point to the end point of the initial partial path segment;
  • the direction of gradient descent of the obstacle cost at the point of the edge is vertical.
  • the mobile robot is a differential robot.
  • a motion control method for a mobile robot includes the following steps:
  • the first movement step S21 wherein the mobile robot is moved along the planned path
  • Real-time local path planning step S22 wherein the following sub-steps are performed:
  • the first sub-step is to select at least a part of the planned path as the original partial path section;
  • the second sub-step is to generate a real-time optimized partial path, so that the real-time optimized partial path is a curve with continuous second-order derivatives, and the real-time optimized partial path has the same starting point, end point, and starting speed as the original partial path section Direction, End Velocity Direction, Start Curvature, and End Curvature; and
  • the third sub-step is replacing the original partial path section in the planned path with the real-time optimized partial path.
  • the mobile robot is controlled to move according to the replaced planned path.
  • the current position of the mobile robot is used as the starting point of the original local path section.
  • the real-time partial path planning step S22 includes a first real-time partial path planning step S22.
  • the real-time optimized partial path is a third-order Bezier curve, and the third-order Bezier curve
  • the Serre curve is represented by the following formula:
  • the real-time partial path planning step S22 includes a second real-time partial path planning step S22.
  • a set of waypoints is determined, wherein at least one waypoint located within a predetermined distance range relative to the current location point of the mobile robot is detected, said waypoint Indicates the points that the mobile robot needs to pass through, the set of passing points is a set consisting of m passing points adjacent to the initial local path segment, m ⁇ 1.
  • the generated real-time optimized partial path passes through all the waypoints in the set of waypoints.
  • the real-time optimized partial path is an m+3 order Bezier curve, the first control point and the mth order of the m+3 order Bezier curve
  • the +4 control points are the start and end points of the real-time optimized local path.
  • the real-time optimized partial path is generated such that the velocity direction at at least one passing point in the set of passing points satisfies one of the following conditions :
  • the velocity direction of the point closest to the corresponding passing point on the original local path segment is the same; the direction is the same as the direction from the start point to the end point of the original local path segment; meeting the task requirements of the mobile robot at the corresponding passing point.
  • the end point of the original partial path segment is determined such that:
  • the end point of the original partial route segment is located after all points on the planned route that are respectively closest to the respective route points in the route point set;
  • the distance between the transit point closest to the starting point of the original local route segment in the transit point set and the starting point of the original partial route segment is equal to the distance between the transit point in the transit point set and the closest transit point to the end point of the original partial route segment in the original local route region The distance between the end points of the segment.
  • the waypoints in the set of waypoints include obstacle avoidance waypoints
  • the obstacle avoidance waypoints are determined by the following methods: determination and planning Obstacles with conflicting paths; and determining at least one obstacle-avoiding waypoint according to the position of the obstacle, so that the generated real-time optimized local path can pass through the at least one obstacle-avoiding waypoint and bypass the obstacle.
  • the position of the obstacle that conflicts with the planned path is determined by obtaining the edge of the obstacle from the obstacle cost map.
  • the at least one obstacle avoidance waypoint is determined by means of an obstacle cost map.
  • the starting point and the ending point of the original partial path section are determined such that: the distance from the starting point of the original partial path section to the obstacle is greater than the predetermined first Three distance thresholds; and/or the distance from the end point of the original partial path segment to the obstacle is greater than a predetermined fourth distance threshold.
  • the real-time optimized partial path is generated such that the velocity direction at at least one obstacle avoidance passing point in the set of passing points satisfies one of the following conditions:
  • the velocity direction of the point closest to the corresponding obstacle avoidance passing point on the original local path section is the same; the direction is the same as the direction from the start point to the end point of the original local path section;
  • the direction of gradient descent of the obstacle cost at points located on the edge of the obstacle is vertical.
  • the mobile robot is a differential robot.
  • a computer program product comprising computer program instructions, wherein, when said computer program instructions are executed by one or more processors, said processors are capable of performing The inventive global path planning method or the motion control method according to the present invention.
  • the positive effect of the present invention is that optimization can be performed on the basis of less deviation from the original global path, thereby smoothing the path while maintaining the advantages of the original global path.
  • the optimized local path can be directly and smoothly replaced into the global path.
  • the planning process can be simplified and the calculation amount can be reduced.
  • Fig. 1 shows the flowchart of the global path planning method for mobile robot according to an exemplary embodiment of the present invention
  • Fig. 2 schematically shows that in the global path planning method according to an exemplary embodiment of the present invention, an initial local path section is replaced with an optimized local path to form a new global path;
  • FIG. 3 schematically shows an optimized partial path according to an exemplary embodiment of the present invention
  • Fig. 4 schematically shows an optimized partial path according to an exemplary embodiment of the present invention.
  • Fig. 5 shows a motion control method for a mobile robot according to an exemplary embodiment of the present invention.
  • the present invention is applicable to the mobile robot 1, which can be any robot capable of autonomous spatial movement, such as AGV, AMR and so on.
  • the mobile robot 1 can be used to perform various tasks, such as a storage robot, a cleaning robot, a family escort robot, a welcome robot, and the like.
  • Fig. 1 shows a flowchart of a global path planning method for a mobile robot 1 according to an exemplary embodiment of the present invention.
  • the global path planning method includes the following steps:
  • Partial path planning step S12 wherein the following sub-steps are performed:
  • At least a part of the (current) global path is selected as an initial local path section
  • the second sub-step is to generate an optimized partial path 2, so that the optimized partial path 2 is a curve with continuous second-order derivatives, and the optimized partial path 2 has the same starting point, end point, and starting speed as the initial partial path section Direction, End Velocity Direction, Start Curvature, and End Curvature; and
  • a third sub-step replacing the initial local path segment with the optimized local path 2 to form a new global path
  • the global path determination step S13 wherein the new global path is determined as the final global path.
  • the initial global path can be obtained using any suitable known method.
  • the initial global path can be planned through the A-star (A*) algorithm.
  • the initial global path is a better path that has taken into account the constraints and optimization goals for the mobile robot 1 .
  • the initial global path may be the shortest path from the start point to the end point.
  • the initial global path often cannot guarantee the smoothness of the path.
  • the global path planning method according to the present invention performs optimization 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.
  • the optimized local path 2 since the optimized local path 2 has the same start point, end point, start speed direction, end point speed direction, start curvature, and end point curvature as the initial local path segment, the optimized local path 2 can be directly and smoothly replaced into the global path.
  • the planning process can be simplified and the calculation amount can be reduced.
  • start point, end point, start velocity direction, end velocity direction, start curvature, and end curvature of the initial local path segment may be derived from the initial global path.
  • Fig. 2 schematically shows that in the global path planning method according to an exemplary embodiment of the present invention, the optimized local path 2 is used to replace the initial local path section to form a new global path.
  • the mobile robot 1 is, for example, a differential robot, that is, the mobile robot 1 includes a differential wheel motion system.
  • the optimal partial path 2 with a continuous second derivative can be adapted particularly advantageously to the kinematic behavior of the differential robot.
  • the optimized partial path 2 can have a continuous curvature. This makes changes in the speed and acceleration of the mobile robot 1 more gradual.
  • the mobile robot 1 may also be other types of robots, such as a single steering wheel robot or a double steering wheel robot.
  • the mobile robot 1 can include, for example, a dual steering wheel kinematic system.
  • the mobile robot 1 includes, for example, a communication device for communicating with other devices, such as a dispatch control system. In the initial planning step S11, an initial global path may be received from other devices through a communication means.
  • the mobile robot 1 can also include sensors, through which the mobile robot 1 can obtain required information, such as the current position of the mobile robot 1 .
  • the mobile robot 1 further includes, for example, a controller.
  • the controller is used to move the components of the robot 1 , including for example the differential wheel kinematic system, sensors, communication means and the like.
  • the controller can also receive working status or detection data of corresponding components, such as sensors, through communication lines, so as to monitor or control the operation of the mobile robot 1 .
  • the controller may also plan an initial global path in the initial planning step S11.
  • the global route planning method can be executed, for example, by means of a controller, or by means of another device capable of exchanging data with the controller, such as a dispatch control system.
  • the global path obtained in the initial planning step S11 is partially shown in dotted lines. It can be seen from Fig. 2 that the global path is not smooth at point S1 and point S2 . From the global path, select the part from P0 to P3 adjacent to point S1 and point S2 as the initial local path segment.
  • an optimized local path 2 (shown in solid line) is generated such that the optimized local path 2 is a curve with continuous second-order derivatives, and the optimized local path 2 is identical to the initial local path section Have the same start point, end point, start velocity direction, end velocity direction, start curvature, and end curvature.
  • the start velocity direction and the end velocity direction represent the velocity directions of the mobile robot 1 at the start and end points of the corresponding paths, respectively.
  • the starting point curvature and the ending point curvature represent the curvatures of the corresponding path at the starting point and the ending point of the path, respectively.
  • the initial local path segment is replaced with the optimized local path 2 to form a new global path.
  • the new global path can be determined as the final global path.
  • the local path planning step S12 may be executed repeatedly, and the final determination step is not executed until any point in the new global path is located on at least one optimized local path 2 .
  • initial partial path sections are selected in such a way that the starting point of each optimized partial path 2 is located on the final global path.
  • the initial partial path section is selected in such a way that the initial partial path section does not contain the starting point of the previously generated optimized partial path 2 .
  • the initial local path segments are sequentially selected segment by segment, so that the starting point of the initial local path segment selected later is closer to the starting point of the initial local path segment selected earlier. close to the termination point.
  • the length of the initial partial path segment may be set to be smaller than a predetermined length threshold. This helps to avoid deviating too much from the original global path.
  • the partial path planning step S12 may include a first partial path planning step in which the optimized partial path 2 is a third-order Bezier curve.
  • the third-order Bezier curve can be represented by the following formula:
  • the optimized partial path 2 thus has the same starting point and end point as the initial partial path section.
  • 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 optimized partial path 2 and the initial partial path section can have the same starting speed direction. This is especially advantageous for differential robots, since the direction of velocity of a differential robot can only be in the forward direction of the differential robot itself. In the case that the differential robot has a given starting point pose at the starting point, the optimized local path 2 thus planned has a starting point velocity direction that matches said given pose.
  • make (x 3 -x 2 ), (y 3 -y 2 ) and Same positive and negative, among them, is a unit vector representing the direction of the terminal velocity of the initial local path segment.
  • the planned optimized partial path 2 can have a predetermined terminal speed direction. This is especially advantageous for differential robots. In case the differential robot has a given end point pose at the end point, the optimized local path 2 thus planned has an end point velocity direction matching said given pose.
  • the curvature of the trajectory can be obtained from the ratio of the angular velocity to the linear velocity.
  • the linear velocity at the starting point P 0 as V 0
  • the angular velocity as ⁇ 0 you can set the predetermined starting point curvature of the path Similarly, the predetermined curvature of the end point of the path at the end point can be obtained.
  • the coordinates of each control point of the optimized partial path 2 can be determined according to the starting point, the end point, the starting speed direction, the ending point speed direction, the starting point curvature and the ending point curvature of the initial partial path section, and then Directly determine the optimized local path 2
  • the optimized partial path 2 may also be other types of smooth curves, such as polynomial curves or B-spline curves, especially clamped-B-spline curves, such as NURBS curves.
  • Fig. 3 schematically shows an optimized partial path 2 in an exemplary embodiment according to the present invention.
  • the partial path planning step S12 includes a second partial path planning step.
  • a set of passing points is determined, the passing points represent the points that the mobile robot 1 needs to pass through, and the set of passing points consists of m passing points adjacent to the initial local path section The set composed of m ⁇ 1.
  • the generated optimized partial path 2 passes through all the waypoints in the set of waypoints.
  • the optimized partial path 2 may be an m+3 order Bezier curve, the first control point and the m+4th control point of the m+3 order Bezier curve are the start and end points of the optimized local path 2, respectively.
  • the optimized local path 2 is a fourth-order Bezier curve.
  • the curve in the curve determining step S12, the curve can be represented by the following formula:
  • control point The coordinates of can be determined by:
  • the optimized partial path 2 is generated such that the velocity direction at at least one passing point in the passing point set satisfies one of the following conditions:
  • the mobile robot 1 needs to accurately pass a specific location in some scenarios to complete a task.
  • the mobile robot 1 may include a code scanner and have the task of scanning a two-dimensional code at a specific location.
  • the specific position can be used as a waypoint, so that the optimized local path 2 must pass through the specific position, so that the mobile robot 1 can perform corresponding tasks.
  • Such waypoints are also referred to herein as "mission waypoints”.
  • the main difference is that when the number of passing points is greater than 1, for each additional passing point, two additional constraints are added according to the coordinates of the passing point and the speed direction at the passing point.
  • the order of the Bezier curve is increased by one, it is necessary to determine the coordinates of an additional control point, which can be achieved by adding two additional constraints.
  • the start and end points of the initial partial path segments are determined such that:
  • the starting point of the initial local path segment is located before all the points on the global path that are respectively closest to the respective via points in the set of via points;
  • 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;
  • the distance between the closest waypoint in the set of waypoints to the start point of the initial local path segment and the start point of the initial local path section is equal to the distance between the waypoint in the set of waypoints closest to the end point of the initial local path segment and the initial local path area The distance between the end points of the segments.
  • the above first partial path planning step and the second partial path planning step can be understood as different types of partial path planning step S12.
  • the repeated execution of the local path planning step S12 in the global path planning method may include the same type of local path planning steps S12, and may also include different types of local path planning steps S12.
  • the repeated partial path planning step S12 may include the first partial path planning step and/or the second real-time partial path planning step S22.
  • the final global path can be spliced by Bezier curves of different orders.
  • Fig. 4 schematically shows an optimized partial path 2 in an exemplary embodiment according to the present invention.
  • the partial path planning step S12 includes a second partial path planning step.
  • the waypoints in the set of waypoints include obstacle avoidance waypoints (marked as P v in FIG. 4 ).
  • the obstacle avoidance waypoint can be determined in the following manner: determine an obstacle 3 that conflicts with the global path; and determine at least one obstacle avoidance waypoint 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 .
  • the global path partially shown in dashed lines collides with obstacle 3 .
  • an obstacle avoidance passing point is determined.
  • 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 (costmap).
  • costmap obstacle costmap
  • the at least one obstacle avoidance waypoint can be determined by means of an obstacle cost map.
  • the point with the largest cost value in the initial local path section can be moved along the direction of the obstacle 3 cost gradient drop at this point until it reaches a position that is a certain distance away from the range of the obstacle 3, and then the position is determined as Obstacle avoidance points.
  • the point intersecting the edge of the obstacle 3 in the initial local path segment can be moved along the direction of the gradient of the obstacle 3 cost gradient until it reaches a position a certain distance away from the obstacle 3, and then determine the position Passing point for obstacle avoidance.
  • FIG. 4 it exemplarily shows the situation that the generated optimized local path 2 avoids the obstacle 3 by setting an obstacle avoidance waypoint.
  • the generated optimized local path 2 avoids the obstacle 3 by setting an obstacle avoidance waypoint.
  • multiple waypoints can be added.
  • the optimized partial path 2 is generated such that the velocity direction at at least one obstacle avoidance passing point in the set of passing points satisfies one of the following conditions : same as the velocity direction of the point on the initial local path section closest to the corresponding obstacle avoidance passing point; same as the direction from the start point to the end point of the initial local path section;
  • the direction of the gradient descent of the obstacle 3 cost at the point on the edge of the object 3 is vertical.
  • the starting point and the ending point of the initial partial path section are determined such that: the distance from the starting point of the initial partial path section to the obstacle 3 is greater than a predetermined first distance threshold; and /or the distance from the end point of the initial partial path section to the obstacle 3 is greater than a predetermined second distance threshold.
  • Fig. 5 shows a motion control method for a mobile robot 1 according to an exemplary embodiment of the present invention.
  • the motion control method includes the following steps:
  • the first movement step S21 wherein the mobile robot 1 is moved along the planned path
  • Real-time local path planning step S22 wherein the following sub-steps are performed:
  • the first sub-step is to select at least a part of the (current) planned path as the original local path section;
  • the second sub-step is to generate a real-time optimized partial path 2, so that the real-time optimized partial path 2 is a curve with continuous second-order derivatives, and the real-time optimized partial path 2 has the same starting point and end point as the original partial path section , starting velocity direction, ending velocity direction, starting curvature, and ending curvature; and
  • the third sub-step is to replace the original partial path section in the planned path with the real-time optimized partial path 2;
  • the mobile robot 1 is controlled to move according to the replaced planned path.
  • the motion control method has similar characteristics to the above-mentioned global path planning, and thus has corresponding advantages. It is particularly advantageous that after the path is planned for the mobile robot 1, factors such as the environment in which the mobile robot 1 moves may change before the mobile robot 1 moves, and the motion control method can be optimized on the basis of less deviation from the original path.
  • the motion path makes the motion path more adaptable to the real-time environment when the mobile robot 1 moves.
  • the original planned path can be obtained using any suitable known method.
  • the original planned path can be planned by the A star algorithm.
  • the motion control method according to the present invention performs optimization on the basis of less deviation from the original planned path, and can maintain the advantages of the original planned path, such as short path length.
  • the planning process can be simplified and the calculation amount can be reduced.
  • the motion control method is applicable to the mobile robot 1 described above, in particular to a differential speed robot.
  • the motion control method can be carried out, for example, by means of a controller of the mobile robot 1 .
  • the current location of the mobile robot 1 is used as the starting point of the original local path section.
  • the real-time partial path planning step S22 includes a first real-time partial path planning step S22.
  • the real-time optimized partial path 2 is a third-order Bezier curve
  • the third-order A Bezier curve is represented by the following formula:
  • the real-time partial path planning step S22 includes a second real-time partial path planning step S22.
  • a set of waypoints is determined, wherein at least one waypoint located within a predetermined distance range relative to the current position point of the mobile robot 1 is detected, the waypoint A point represents a point that the mobile robot 1 needs to pass through, and the set of passing points is a set consisting of m passing points adjacent to the initial local path segment, where m ⁇ 1.
  • the generated real-time optimized partial path 2 passes through all the waypoints in the set of waypoints.
  • the real-time optimized partial path 2 is an m+3 order Bezier curve, the first control point and the m+4th order of the m+3 order Bezier curve
  • the control points are the starting point and the ending point of the real-time optimized local path 2 respectively. Determining the control points of the m+3 order Bezier curve
  • the way of the coordinates of can also be similar to that described above for the global path planning method, and will not be repeated here.
  • the real-time optimized partial path 2 is generated such that the velocity direction at at least one passing point in the set of passing points satisfies one of the following conditions Or: the velocity direction of the point closest to the corresponding passing point on the original local path section is the same; the direction from the starting point to the end point of the original local path section is the same; meeting the task requirements of mobile robot 1 at the corresponding passing point .
  • the end point of the original partial path section is determined such that: along the direction of the planned path, the end point of the original partial path section is located at all distances traveled by the planned path After the nearest point of each waypoint in the point set; and/or the distance between the closest waypoint in the waypoint set and the starting point of the original local path segment is equal to the distance in the waypoint set from the original The distance between the nearest via point of the end point of the partial route segment and the end point of the original partial route segment.
  • the waypoints in the set of waypoints include obstacle avoidance waypoints, and the obstacle avoidance waypoints are determined by the following methods: determination and planning An obstacle 3 with conflicting paths; and according to the position of the obstacle 3, determine at least one obstacle avoidance waypoint, 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 .
  • the mobile robot 1 moves, it may occur that the trajectory of the original planned path near the obstacle 3 is not smooth, or a new static obstacle 3 that interferes with the original planned path appears on the original planned path.
  • the mobile robot 1 can avoid the obstacle 3 along a smooth trajectory.
  • the way of determining the obstacle avoidance waypoints may be similar to that described above for the global path planning method.
  • the starting point and the ending point of the original partial path section are determined such that: the distance from the starting point of the original partial path section to the obstacle 3 is greater than a predetermined third distance threshold ; and/or the distance from the end point of the original partial path segment to the obstacle 3 is greater than a predetermined fourth distance threshold.
  • the real-time optimized partial path 2 is generated such that the velocity direction at at least one obstacle avoidance passing point in the set of passing points satisfies one of the following conditions:
  • the velocity direction of the point closest to the corresponding obstacle-avoiding passing point on the original local path section is the same as the direction of velocity from the start point to the end point of the original local path section;
  • the 3 edge points are perpendicular to the obstacle 3 in the direction of the cost gradient descent.
  • 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 also apply correspondingly for the motion control method.
  • the present invention also relates to a computer program product comprising computer program instructions, which, when executed by one or more processors, are capable of performing the global path planning method according to the present invention or motion control methods.
  • the computer program product can be stored in a computer-readable storage medium.
  • the computer-readable storage medium may include, for example, high-speed random access memory, and may also include non-volatile memory, such as a hard disk, internal memory, plug-in hard disk, smart memory card, secure digital card, flash memory card, at least one magnetic disk storage device, Flash memory devices, or other volatile solid-state storage devices.
  • the processor 10 may be a central processing unit, or other general-purpose processors, digital signal processors, application-specific integrated circuits, off-the-shelf programmable gate arrays or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, and the like.
  • a general-purpose processor may be a microprocessor or any conventional processor or the like.

Landscapes

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

Abstract

本发明提出了一种用于移动机器人的全局路径规划方法，其中，所述全局路径规划方法包括：初始规划步骤，获取用于移动机器人的初始的全局路径；局部路径规划步骤，其中执行下述子步骤：选取全局路径中的至少一部分作为初始局部路径区段，生成优化局部路径使得所述优化局部路径是具有连续的二阶导数的曲线，并且所述优化局部路径与初始局部路径区段具有相同的起点、终点、起点速度方向、终点速度方向、起点曲率和终点曲率，用优化局部路径替换初始局部路径区段以形成新的全局路径；全局路径确定步骤，将新的全局路径确定为最终全局路径。提出了一种用于移动机器人的运动控制方法及一种计算机程序产品。借助于本发明，能够优化移动机器人的运动轨迹。

Description

全局路径规划方法、运动控制方法及计算机程序产品 技术领域
本发明涉及移动机器人领域、尤其是移动机器人的运动控制领域，具体涉及一种用于移动机器人的全局路径规划方法、一种用于移动机器人的运动控制方法以及一种计算机程序产品。
背景技术
随着经济快速增长、人力成本逐渐上升，移动机器人越来越广泛地应用于各种工业和家庭环境中。例如，自动引导车(AGV)、自主移动机器人(AMR)、叉车等移动机器人是现代物流系统的关键设备之一。移动机器人能够根据路径规划和作业要求运动并停靠到目标地点，以完成物料搬运输送等任务。路径规划是移动机器人的运动控制中的关键。
在使用全局路径规划算法(例如A*算法)为移动机器人规划全局路径时，往往不能保证路径是平滑的。另一方面，尽管在使用全局路径规划算法为移动机器人规划全局路径时，考虑了多种因素以获得优化的全局路径，但在移动机器人运动时，可能会出现新的需要考虑的因素。
现有技术在对于移动机器人的路径规划和运动控制方面仍然存在诸多不足。
发明内容
本发明的目的在于提供一种改进的用于移动机器人的全局路径规划方法和运动控制方法，以优化移动机器人的运动轨迹。
根据本发明的第一方面，提供了一种用于移动机器人的全局路径规划方法，其中，所述全局路径规划方法包括以下步骤：
初始规划步骤S11，其中，获取用于移动机器人的初始的全局路径；
局部路径规划步骤S12，其中执行下述子步骤：
第一子步骤，选取全局路径中的至少一部分作为初始局部路径区段；
第二子步骤，生成优化局部路径，使得所述优化局部路径是具有连续的二阶导数的曲线，并且所述优化局部路径与初始局部路径区段具有相同的起点、终点、起点速度方向、终点速度方向、起点曲率和终点曲率；以及
第三子步骤，用优化局部路径替换初始局部路径区段以形成新的全局路径；以及
全局路径确定步骤S13，其中，将新的全局路径确定为最终全局路径。
可选地，以下述方式中的至少一者执行局部路径规划步骤S12：
在局部路径规划步骤S12，以使得每个优化局部路径的起点都位于最终全局路径上的方式选取初始局部路径区段；
初始局部路径区段的长度小于预定的长度阈值；
重复执行局部路径规划步骤S12，直到新的全局路径中的任一点都位于至少一个优化局部路径上之后，才执行最终确定步骤S13。
可选地，局部路径规划步骤S12包括第一局部路径规划步骤，在所述第一局部路径规划步骤中，优化局部路径是三阶贝塞尔曲线，所述三阶贝塞尔曲线由下式表示：
Figure PCTCN2022116465-appb-000001
其中，
Figure PCTCN2022116465-appb-000002
表示贝塞尔曲线的控制点的坐标并且
Figure PCTCN2022116465-appb-000003
其中，控制点
Figure PCTCN2022116465-appb-000004
的坐标通过以下方式来确定：
Figure PCTCN2022116465-appb-000005
为初始局部路径区段的起点的坐标，
Figure PCTCN2022116465-appb-000006
为初始局部路径区段的终点的坐标；
Figure PCTCN2022116465-appb-000007
(x 1-x 0)、(y 1-y 0)分别与
Figure PCTCN2022116465-appb-000008
同正负，其中，
Figure PCTCN2022116465-appb-000009
是表示初始局部路径区段的起点速度方向的单位矢量；
Figure PCTCN2022116465-appb-000010
(x 3-x 2)、(y 3-y 2)分别与
Figure PCTCN2022116465-appb-000011
同正负，其中，
Figure PCTCN2022116465-appb-000012
是表示初始局部路径区段的终点速度方向的单位矢量；
将s＝0和К(0)＝К 0代入
Figure PCTCN2022116465-appb-000013
中，其中，К 0表示初始局部路径区段的路径起点曲率，P x′(s)、P y′(s)、P x″(s)、P y″(s)分别是
Figure PCTCN2022116465-appb-000014
的 一阶导横、纵坐标和二阶导横、纵坐标；
将s＝1和К(1)＝К 1代入
Figure PCTCN2022116465-appb-000015
中，其中，К 1表示初始局部路径区段的路径终点曲率。
可选地，局部路径规划步骤S12包括第二局部路径规划步骤。在所述第二局部路径规划步骤的第一子步骤中，确定途经点集合，途经点表示移动机器人需要经过的点，所述途经点集合为由邻近初始局部路径区段的m个途经点组成的集合，m≥1。在所述第二局部路径规划步骤的第二子步骤中，生成的优化局部路径经过途经点集合中的所有途经点。
可选地，在所述第二局部路径规划步骤中，优化局部路径是m+3阶贝塞尔曲线，所述m+3阶贝塞尔曲线的第一个控制点和第m+4个控制点分别为优化局部路径的起点和终点。
可选地，在所述第二局部路径规划步骤的第二子步骤中，优化局部路径被生成为使得途经点集合中的至少一个途经点处的速度方向满足以下条件中的一者：与初始局部路径区段上的距离相应的途经点最近的点的速度方向相同；与初始局部路径区段的起点到终点的方向相同；满足移动机器人在相应的途经点处的任务需求。
可选地，在第二局部路径规划步骤中，初始局部路径区段的起点和终点被确定为使得：
沿着全局路径的方向，初始局部路径区段的起点位于全局路径上的所有分别距离途经点集合中的各途经点最近的点之前；和/或
沿着全局路径的方向，初始局部路径区段的终点位于全局路径上的所有分别距离途经点集合中的各途经点最近的点之后；和/或
途经点集合中距离初始局部路径区段的起点最近的途经点与初始局部路径区段的起点之间的距离等于途经点集合中距离初始局部路径区段的终点最近的途经点与初始局部路径区段的终点之间的距离。
可选地，在所述第二局部路径规划步骤的第一子步骤中，途经点集合中的途经点包括避障途经点，所述避障途经点通过下述方式确定：确定与全局路径相冲突的障碍物；以及根据所述障碍物的位置，确定至少一个避 障途经点，使得生成的优化局部路径能够经过所述至少一个避障途经点并绕过障碍物。
可选地，通过由障碍物代价图获取障碍物的边缘来确定与全局路径相冲突的障碍物的位置。
可选地，借助于障碍物代价图确定所述至少一个避障途经点。
可选地，在第二局部路径规划步骤的第一子步骤中，初始局部路径区段的起点和终点被确定为使得：初始局部路径区段的起点到障碍物的距离大于预定的第一距离阈值；和/或初始局部路径区段的终点到障碍物的距离大于预定的第二距离阈值。
可选地，在所述第二局部路径规划步骤的第二子步骤中，优化局部路径被生成为使得途经点集合中的至少一个避障途经点处的速度方向满足以下条件中的一者：与初始局部路径区段上的距离相应的避障途经点最近的点的速度方向相同；与初始局部路径区段的起点到终点的方向相同；与距离相应的避障途经点最近的位于障碍物边缘的点处的障碍物代价梯度下降的方向垂直。
可选地，移动机器人为差速机器人。
根据本发明的第二方面，提供了一种用于移动机器人的运动控制方法，其中，所述运动控制方法包括以下步骤：
第一运动步骤S21，其中，使移动机器人沿着规划路径运动；
实时局部路径规划步骤S22，其中执行下述子步骤：
第一子步骤，选取规划路径中的至少一部分作为原局部路径区段；
第二子步骤，生成实时优化局部路径，使得所述实时优化局部路径是具有连续的二阶导数的曲线，并且所述实时优化局部路径与原局部路径区段具有相同的起点、终点、起点速度方向、终点速度方向、起点曲率和终点曲率；以及
第三子步骤，用实时优化局部路径替换规划路径中的原局部路径区段；以及
第二运动步骤S23，其中，控制移动机器人按照替换后的规划路径运动。
可选地，在实时局部路径规划步骤S22的第一子步骤中，以移动机器 人的当前位置点作为原局部路径区段的起点。
可选地，实时局部路径规划步骤S22包括第一实时局部路径规划步骤S22，在所述第一实时局部路径规划步骤S22中，实时优化局部路径是三阶贝塞尔曲线，所述三阶贝塞尔曲线由下式表示：
Figure PCTCN2022116465-appb-000016
其中，
Figure PCTCN2022116465-appb-000017
表示贝塞尔曲线的控制点的坐标并且
Figure PCTCN2022116465-appb-000018
其中，控制点
Figure PCTCN2022116465-appb-000019
的坐标通过以下方式来确定：
Figure PCTCN2022116465-appb-000020
为原局部路径区段的起点的坐标，
Figure PCTCN2022116465-appb-000021
为原局部路径区段的终点的坐标；
Figure PCTCN2022116465-appb-000022
(x 1-x 0)、(y 1-y 0)分别与
Figure PCTCN2022116465-appb-000023
同正负，其中，
Figure PCTCN2022116465-appb-000024
是表示原局部路径区段的起点速度方向的单位矢量；
Figure PCTCN2022116465-appb-000025
(x 3-x 2)、(y 3-y 2)分别与
Figure PCTCN2022116465-appb-000026
同正负，其中，
Figure PCTCN2022116465-appb-000027
是表示原局部路径区段的终点速度方向的单位矢量；
将s＝0和К(0)＝К 0代入
Figure PCTCN2022116465-appb-000028
中，其中，К 0表示原局部路径区段的路径起点曲率，P x′(s)、P y′(s)、P x″(s)、P y″(s)分别是
Figure PCTCN2022116465-appb-000029
的一阶导横、纵坐标和二阶导横、纵坐标；
将s＝1和К(1)＝К 1代入
Figure PCTCN2022116465-appb-000030
中，其中，К 1表示原局部路径区段的路径终点曲率。
可选地，实时局部路径规划步骤S22包括第二实时局部路径规划步骤S22。在所述第二实时局部路径规划步骤S22的第一子步骤中，确定途经点集合，其中，检测位于相对于移动机器人的当前位置点的预定距离范围内的至少一个途经点，所述途经点表示移动机器人需要经过的点，所述途经点集合为由邻近初始局部路径区段的m个途经点组成的集合，m≥1。在所述第二实时局部路径规划步骤S22的第二子步骤中，生成的实时优化局部路径经过途经点集合中的所有途经点。
可选地，在所述第二实时局部路径规划步骤S22中，实时优化局部路径是m+3阶贝塞尔曲线，所述m+3阶贝塞尔曲线的第一个控制点和第m+4个控制点分别为实时优化局部路径的起点和终点。
可选地，在所述第二实时局部路径规划步骤S22的第二子步骤中，实时优化局部路径被生成为使得途经点集合中的至少一个途经点处的速度方向满足以下条件中的一者：与原局部路径区段上的距离相应的途经点最近的点的速度方向相同；与原局部路径区段的起点到终点的方向相同；满足移动机器人在相应的途经点处的任务需求。
可选地，在第二实时局部路径规划步骤S22中，原局部路径区段的终点被确定为使得：
沿着规划路径的方向，原局部路径区段的终点位于规划路径上的所有分别距离途经点集合中的各途经点最近的点之后；和/或
途经点集合中距离原局部路径区段的起点最近的途经点与原局部路径区段的起点之间的距离等于途经点集合中距离原局部路径区段的终点最近的途经点与原局部路径区段的终点之间的距离。
可选地，在所述第二实时局部路径规划步骤S22的第一子步骤中，途经点集合中的途经点包括避障途经点，所述避障途经点通过下述方式确定：确定与规划路径相冲突的障碍物；以及根据所述障碍物的位置，确定至少一个避障途经点，使得生成的实时优化局部路径能够经过所述至少一个避障途经点并绕过障碍物。
可选地，通过由障碍物代价图获取障碍物的边缘来确定与规划路径相冲突的障碍物的位置。
可选地，借助于障碍物代价图确定所述至少一个避障途经点。
可选地，在第二实时局部路径规划步骤S22的第一子步骤中，原局部路径区段的起点和终点被确定为使得：原局部路径区段的起点到障碍物的距离大于预定的第三距离阈值；和/或原局部路径区段的终点到障碍物的距离大于预定的第四距离阈值。
可选地，在所述第二实时局部路径规划步骤S22的第二子步骤中，实时优化局部路径被生成为使得途经点集合中的至少一个避障途经点处的速度方向满足以下条件中的一者：与原局部路径区段上的距离相应的避障途 经点最近的点的速度方向相同；与原局部路径区段的起点到终点的方向相同；与距离相应的避障途经点最近的位于障碍物边缘的点处的障碍物代价梯度下降的方向垂直。
可选地，移动机器人为差速机器人。
根据本发明的第三方面，提供了一种计算机程序产品，其包括计算器程序指令，其中，当所述计算机程序指令被一个或多于一个处理器执行时，所述处理器够执行根据本发明的全局路径规划方法或根据本发明的运动控制方法。
本发明的积极效果在于：能够在较少地偏离原有的全局路径的基础上进行优化，从而使路径平滑化，同时保持原有的全局路径的优点。特别是，由于在优化过程中保留了原有的起点、终点、起点速度方向、终点速度方向、起点曲率和终点曲率，因此经过优化的局部路径能够直接平滑地替换到全局路径中。另外，能够简化规划过程，减少计算量。
附图说明
下面，通过参看附图更详细地描述本发明，可以更好地理解本发明的原理、特点和优点。附图包括：
图1示出了根据本发明的一个示例性实施例的用于移动机器人的全局路径规划方法的流程图；
图2示意性地示出了在根据本发明的一个示例性实施例的全局路径规划方法中用优化局部路径替换初始局部路径区段以形成新的全局路径；
图3示意性地示出根据本发明的一个示例性实施例中的优化局部路径；
图4示意性地示出根据本发明的一个示例性实施例中的优化局部路径；以及
图5示出了根据本发明的一个示例性实施例的用于移动机器人的运动控制方法。
具体实施方式
为了使本发明所要解决的技术问题、技术方案以及有益的技术效果更加清楚明白，以下将结合附图以及多个示例性实施例对本发明进行进一步 详细说明。应当理解，此处所描述的具体实施例仅用于解释本发明，而不是用于限定本发明的保护范围。
本发明适用于移动机器人1，其可以是任何能够自主地进行空间移动的机器人，例如AGV、AMR等。所述移动机器人1可用于执行各种任务，例如用作仓储机器人、清扫型机器人、家庭陪护机器人、迎宾机器人等。
应理解，在本文中，表述“第一”、“第二”等仅用于描述性目的，而不应理解为指示或暗示相对重要性，也不应理解为隐含指明所指示的技术特征的数量。限定有“第一”、“第二”的特征可以明示或者隐含地表示包括至少一个该特征。
图1示出了根据本发明的一个示例性实施例的用于移动机器人1的全局路径规划方法的流程图。所述全局路径规划方法包括以下步骤：
初始规划步骤S11，其中，获取用于移动机器人1的初始的全局路径；
局部路径规划步骤S12，其中执行下述子步骤：
第一子步骤，选取(当前的)全局路径中的至少一部分作为初始局部路径区段；
第二子步骤，生成优化局部路径2，使得所述优化局部路径2是具有连续的二阶导数的曲线，并且所述优化局部路径2与初始局部路径区段具有相同的起点、终点、起点速度方向、终点速度方向、起点曲率和终点曲率；以及
第三子步骤，用优化局部路径2替换初始局部路径区段以形成新的全局路径；以及
全局路径确定步骤S13，其中，将新的全局路径确定为最终全局路径。
初始的全局路径可利用任何适用的已知方法来获取。例如，可通过A星(A*)算法来规划初始的全局路径。通常，初始的全局路径是已经考虑了对于移动机器人1的约束条件和优化目标的较优的路径。例如，初始的全局路径可能是从起始点到终止点的最短路径。然而，初始的全局路径往往不能保证路径的平滑性。根据本发明的全局路径规划方法在较少地偏离初始的全局路径的基础上进行优化，从而使全局路径平滑化，同时保持初始的全局路径的优点、例如短路径长度。特别是，由于优化局部路径2与初始局部路径区段具有相同的起点、终点、起点速度方向、终点速度方向、 起点曲率和终点曲率，因此优化局部路径2能够直接平滑地替换到全局路径中。另外，能够简化规划过程，减少计算量。
本领域技术人员将理解，初始局部路径区段的起点、终点、起点速度方向、终点速度方向、起点曲率和终点曲率可由初始的全局路径得出。
图2示意性地示出了在根据本发明的一个示例性实施例的全局路径规划方法中用优化局部路径2替换初始局部路径区段以形成新的全局路径。
在图2所示的实施例中，移动机器人1例如为差速机器人，即移动机器人1包括差速轮运动系统。对于差速机器人而言，优化局部路径2具有连续的二阶导数能够特别有利地适应差速机器人的运动特性。特别是，优化局部路径2能够具有连续的曲率。这使得移动机器人1的速度和加速度的变化更平缓。替代地，移动机器人1也可以是其它类型的机器人，例如单舵轮机器人或双舵轮机器人等。相应地，移动机器人1例如可包括双舵轮运动系统。
移动机器人1例如包括用于与其它设备、例如调度控制系统进行通信的通信装置。在初始规划步骤S11中，可通过通信装置从其它设备接收初始的全局路径。移动机器人1还可包括传感器，移动机器人1可通过所述传感器获取所需的信息、如移动机器人1的当前位置。
移动机器人1例如还包括控制器。控制器用于移动机器人1的部件，所述部件例如包括差速轮运动系统、传感器、通信装置等。控制器还可以通过通信线路接收相应部件、例如传感器的工作状态或检测数据，用以监测或控制移动机器人1的操作。控制器例如还可在初始规划步骤S11中，规划初始的全局路径。全局路径规划方法例如可借助于控制器来执行，也可借助于能够与控制器进行数据交换的另外的设备、例如调度控制系统来执行。
在图2中，初始规划步骤S11中获取的全局路径以虚线部分地示出。从图2可以看出，全局路径在点S 1和点S 2处不平滑。从全局路径中，选取邻近点S 1和点S 2的从P 0到P 3之间的部分作为初始局部路径区段。
根据初始局部路径区段，生成优化局部路径2(以实线示出)，使得所述优化局部路径2是具有连续的二阶导数的曲线，并且所述优化局部路径2与初始局部路径区段具有相同的起点、终点、起点速度方向、终点速度方 向、起点曲率和终点曲率。起点速度方向和终点速度方向分别表示移动机器人1在相应的路径的起点和终点处的速度方向。起点曲率和终点曲率分别表示相应的路径在路径的起点和终点处的曲率。
然后，用优化局部路径2替换初始局部路径区段以形成新的全局路径。最后，可将新的全局路径确定为最终全局路径。
局部路径规划步骤S12可被重复执行，直到新的全局路径中的任一点都位于至少一个优化局部路径2上之后，才执行最终确定步骤。
在局部路径规划步骤S12中，以使得每个优化局部路径2的起点都位于最终全局路径上的方式选取初始局部路径区段。在重复执行局部路径规划步骤S12的情况下，以使得初始局部路径区段不包含之前生成的优化局部路径2的起点的方式选取初始局部路径区段。例如，沿着全局路径从全局路径的起始点到终止点的方向依次逐段选取初始局部路径区段，使得后选取的初始局部路径区段的起点比先选取的初始局部路径区段的起点更靠近终止点。
可选地，初始局部路径区段的长度可设定为小于预定的长度阈值。由此，可有助于避免过多地偏离初始的全局路径。
图2中示出了局部路径规划步骤S12可包括第一局部路径规划步骤，在所述第一局部路径规划步骤中，优化局部路径2是三阶贝塞尔曲线。
所述三阶贝塞尔曲线可由下式表示：
Figure PCTCN2022116465-appb-000031
其中，
Figure PCTCN2022116465-appb-000032
表示贝塞尔曲线的控制点的坐标并且
Figure PCTCN2022116465-appb-000033
下面将示例性地描述确定控制点
Figure PCTCN2022116465-appb-000034
的过程。
Figure PCTCN2022116465-appb-000035
为初始局部路径区段的起点的坐标，
Figure PCTCN2022116465-appb-000036
为初始局部路径区段的终点的坐标。由此，优化局部路径2与初始局部路径区段具有相同的起点和终点。
使第一控制点与第二控制点连线方向沿着初始局部路径区段的起点速度方向。将初始局部路径区段的起点速度方向用单位矢量
Figure PCTCN2022116465-appb-000037
表示，则第一控制点和第二控制点具有以下关系：
Figure PCTCN2022116465-appb-000038
其中
Figure PCTCN2022116465-appb-000039
且(x 1-x 0)、(y 1-y 0)分别与
Figure PCTCN2022116465-appb-000040
同正负。由此，可使得 优化局部路径2与初始局部路径区段具有相同的起点速度方向。这对于差速机器人而言尤为有利，因为差速机器人的速度方向只能沿着差速机器人本身的前方方向。在差速机器人在起点处具有给定的起点位姿的情况下，由此规划的优化局部路径2具有与所述给定的位姿相匹配的起点速度方向。
类似地，使
Figure PCTCN2022116465-appb-000041
(x 3-x 2)、(y 3-y 2)分别与
Figure PCTCN2022116465-appb-000042
同正负，其中，
Figure PCTCN2022116465-appb-000043
是表示初始局部路径区段的终点速度方向的单位矢量。由此，可使得规划的优化局部路径2具有预定的终点速度方向。这对于差速机器人而言尤为有利。在差速机器人在终点处具有给定的终点位姿的情况下，由此规划的优化局部路径2具有与所述给定的位姿相匹配的终点速度方向。
考虑到路径起点曲率，将s＝0和К(0)＝К 0代入
Figure PCTCN2022116465-appb-000044
中，其中，К 0表示初始局部路径区段的路径起点曲率，P x′(s)、P y′(s)、P x″(s)、P y″(s)分别是
Figure PCTCN2022116465-appb-000045
的一阶导横、纵坐标和二阶导横、纵坐标。
对于差速机器人，运动轨迹的曲率可通过角速度大小与线速度大小之比得出。通过设定起点P 0处的线速度大小为V 0，角速度大小为ω 0，可设定预定的路径起点曲率
Figure PCTCN2022116465-appb-000046
同理可得终点处的预定的路径终点曲率。
类似地，将s＝1和К(1)＝К 1代入
Figure PCTCN2022116465-appb-000047
中，其中，К 1表示初始局部路径区段的路径终点曲率。
因此，在第一局部路径规划步骤中，根据初始局部路径区段的起点、终点、起点速度方向、终点速度方向、起点曲率和终点曲率，可确定优化局部路径2的各控制点的坐标，进而直接确定优化局部路径2
可选地，优化局部路径2也可以是其它类型的光滑曲线，例如多项式曲线或B样条曲线、特别是clamped-B样条曲线、例如NURBS曲线。
图3示意性地示出根据本发明的一个示例性实施例中的优化局部路径2。
在图3所示的实施例中，局部路径规划步骤S12包括第二局部路径规划步骤。在所述第二局部路径规划步骤的第一子步骤中，确定途经点集合，途经点表示移动机器人1需要经过的点，所述途经点集合为由邻近初始局部路径区段的m个途经点组成的集合，m≥1。在所述第二局部路径规划步骤的第二子步骤中，生成的优化局部路径2经过途经点集合中的所有途经点。
在所述第二局部路径规划步骤中，优化局部路径2可以是m+3阶贝塞尔曲线，所述m+3阶贝塞尔曲线的第一个控制点和第m+4个控制点分别为优化局部路径2的起点和终点。例如，图3示例性地示出了m＝1的情况。相应的，优化局部路径2为四阶贝塞尔曲线。
在这种情况下，在曲线确定步骤S12中，曲线可由下式表示：
Figure PCTCN2022116465-appb-000048
其中，i＝0,1,…,m+3，
Figure PCTCN2022116465-appb-000049
Figure PCTCN2022116465-appb-000050
表示贝塞尔曲线的控制点的坐标并且
Figure PCTCN2022116465-appb-000051
控制点
Figure PCTCN2022116465-appb-000052
的坐标可通过以下方式来确定：
Figure PCTCN2022116465-appb-000053
为起点的坐标，
Figure PCTCN2022116465-appb-000054
为终点的坐标；
Figure PCTCN2022116465-appb-000055
(x 1-x 0)、(y 1-y 0)分别与
Figure PCTCN2022116465-appb-000056
同正负，其中，
Figure PCTCN2022116465-appb-000057
是表示预定的起点速度方向的单位矢量；
Figure PCTCN2022116465-appb-000058
(x m+3-x m+2)、(y m+3-y m+2)分别与
Figure PCTCN2022116465-appb-000059
Figure PCTCN2022116465-appb-000060
同正负，其中，
Figure PCTCN2022116465-appb-000061
是表示预定的终点速度方向的单位矢量；
Figure PCTCN2022116465-appb-000062
其中，
Figure PCTCN2022116465-appb-000063
表示所述至少一个途经点中的第j个途经点的坐标，j＝1,2,…,m，
Figure PCTCN2022116465-appb-000064
表示第j个途经点对应的s的取值；
Figure PCTCN2022116465-appb-000065
与
Figure PCTCN2022116465-appb-000066
的方向相同，其中，
Figure PCTCN2022116465-appb-000067
是表示在第j个途经点处的预定的途经点速度方向的单位矢量，P′(s)表示
Figure PCTCN2022116465-appb-000068
的一阶导的坐标，
Figure PCTCN2022116465-appb-000069
表示
Figure PCTCN2022116465-appb-000070
在
Figure PCTCN2022116465-appb-000071
处的一阶导的坐标；
将s＝0和К(0)＝К 0代入
Figure PCTCN2022116465-appb-000072
中，其中，К 0表示预定的路径起点曲率；以及
将s＝1和К(1)＝К 1代入
Figure PCTCN2022116465-appb-000073
中，其中，К 1表示预定的路径终点曲率。
可选地，在第二局部路径规划步骤的第二子步骤中，优化局部路径2被生成为使得途经点集合中的至少一个途经点处的速度方向满足以下条件中的一者：
与初始局部路径区段上的距离相应的途经点最近的点的速度方向相同；
与初始局部路径区段的起点到终点的方向相同；
满足移动机器人1在相应的途经点处的任务需求。
例如，移动机器人1在某些场景下需要精确经过特定位置以完成任务。例如，移动机器人1可包括扫码器，并具有在特定位置处扫描二维码的任务。可将该特定位置可作为途经点，使得优化局部路径2必然经过该特定位置，以便移动机器人1执行相应的任务。这种途经点在本文中也称为“任务途经点”。
当m>1时，确定各控制点的方式与上文针对图3所示的m＝1的情况所描述的方式类似。不同之处主要在于，当途经点的数量大于1时，每增加一个途经点，则根据途经点坐标和途经点处的速度方向增加两个附加约束条件。相应地，贝塞尔曲线的阶数增加一阶，需要多确定一个附加的控制点的坐标，这可通过增加两个附加约束条件来实现。
在第二局部路径规划步骤中，初始局部路径区段的起点和终点被确定为使得：
沿着全局路径的方向，初始局部路径区段的起点位于全局路径上的所有分别距离途经点集合中的各途经点最近的点之前；和/或
沿着全局路径的方向，初始局部路径区段的终点位于全局路径上的所有分别距离途经点集合中的各途经点最近的点之后；和/或
途经点集合中距离初始局部路径区段的起点最近的途经点与初始局部路径区段的起点之间的距离等于途经点集合中距离初始局部路径区段的终点最近的途经点与初始局部路径区段的终点之间的距离。
上述第一局部路径规划步骤和第二局部路径规划步骤可理解为不同类型的局部路径规划步骤S12。显然，在全局路径规划方法中重复执行的局部路径规划步骤S12可包括同类型的局部路径规划步骤S12，也可包括不同类型的局部路径规划步骤S12。换言之，重复执行的局部路径规划步骤S12可包括第一局部路径规划步骤和/或第二实时局部路径规划步骤S22。最终全局路径可由不同阶的贝塞尔曲线拼接而成。
图4示意性地示出根据本发明的一个示例性实施例中的优化局部路径2。在图4所示的实施例中，局部路径规划步骤S12包括第二局部路径规划步骤。在所述第二局部路径规划步骤的第一子步骤中，途经点集合中的途经点包括避障途经点(图4中标记为P v)。
所述避障途经点可通过下述方式确定：确定与全局路径相冲突的障碍物3；以及根据所述障碍物3的位置，确定至少一个避障途经点，使得生成的优化局部路径2能够经过所述至少一个避障途经点并绕过障碍物3。如图4所示，以虚线部分地示出的全局路径与障碍物3相冲突。相应地，确定一个避障途经点。
可选地，通过由障碍物代价图(costmap)获取障碍物3的边缘来确定与全局路径相冲突的障碍物3的位置。
可选地，所述至少一个避障途经点可借助于障碍物代价图来确定。例如，可将初始局部路径区段中代价值最大的点沿着该点处的障碍物3代价梯度下降的方向移动，直到到达离开障碍物3的范围一定距离的位置，然后将该位置确定为避障途经点。又如，可将初始局部路径区段中与障碍物3的边缘相交的点沿着障碍物3代价梯度下降的方向移动，直到到达离开障碍物3的范围一定距离的位置，然后将该位置确定为避障途经点。
在图4中，示例性地示出了通过设定一个避障途经点，使得生成的优化局部路径2避开障碍物3的情况。当设定一个避障途经点无法避开障碍 物3时，可添加多个途经点。
可选地，在所述第二局部路径规划步骤的第二子步骤中，优化局部路径2被生成为使得途经点集合中的至少一个避障途经点处的速度方向满足以下条件中的一者：与初始局部路径区段上的距离相应的避障途经点最近的点的速度方向相同；与初始局部路径区段的起点到终点的方向相同；与距离相应的避障途经点最近的位于障碍物3边缘的点处的障碍物3代价梯度下降的方向垂直。
在第二局部路径规划步骤的第一子步骤中，初始局部路径区段的起点和终点被确定为使得：初始局部路径区段的起点到障碍物3的距离大于预定的第一距离阈值；和/或初始局部路径区段的终点到障碍物3的距离大于预定的第二距离阈值。由此，可有助于使优化局部路径2避开障碍物3。
图5示出了根据本发明的一个示例性实施例的用于移动机器人1的运动控制方法。所述运动控制方法包括以下步骤：
第一运动步骤S21，其中，使移动机器人1沿着规划路径运动；
实时局部路径规划步骤S22，其中执行下述子步骤：
第一子步骤，选取(当前的)规划路径中的至少一部分作为原局部路径区段；
第二子步骤，生成实时优化局部路径2，使得所述实时优化局部路径2是具有连续的二阶导数的曲线，并且所述实时优化局部路径2与原局部路径区段具有相同的起点、终点、起点速度方向、终点速度方向、起点曲率和终点曲率；以及
第三子步骤，用实时优化局部路径2替换规划路径中的原局部路径区段；以及
第二运动步骤S23，其中，控制移动机器人1按照替换后的规划路径运动。
可以看出，运动控制方法与上述全局路径规划具有相似的特征，因此也具有与之相应的优势。特别有利的是，在为移动机器人1规划好路径之后，到移动机器人1运动之前，移动机器人1运动的环境等因素可能发生变化，而运动控制方法能够在较少地偏离原路径的基础上优化运动路径，使得运动路径更适应移动机器人1移动时的实时环境。
原规划路径可利用任何适用的已知方法来获取。例如，原规划路径可通过A星算法来规划。根据本发明的运动控制方法在较少地偏离原规划路径的基础上进行优化，能够保持原规划路径的优点、例如短路径长度。另外，能够简化规划过程，减少计算量。
运动控制方法可适用于上文描述的移动机器人1、特别是适用于差速机器人。运动控制方法例如可借助于移动机器人1的控制器来执行。
在实时局部路径规划步骤S22的第一子步骤中，以移动机器人1的当前位置点作为原局部路径区段的起点。
可选地，实时局部路径规划步骤S22包括第一实时局部路径规划步骤S22，在所述第一实时局部路径规划步骤S22中，实时优化局部路径2是三阶贝塞尔曲线，所述三阶贝塞尔曲线由下式表示：
Figure PCTCN2022116465-appb-000074
其中，
Figure PCTCN2022116465-appb-000075
表示贝塞尔曲线的控制点的坐标并且
Figure PCTCN2022116465-appb-000076
确定控制点
Figure PCTCN2022116465-appb-000077
的坐标可与上文针对全局路径规划方法所描述的类似：
Figure PCTCN2022116465-appb-000078
为原局部路径区段的起点的坐标，
Figure PCTCN2022116465-appb-000079
为原局部路径区段的终点的坐标；
Figure PCTCN2022116465-appb-000080
分别与
Figure PCTCN2022116465-appb-000081
同正负，其中，
Figure PCTCN2022116465-appb-000082
是表示原局部路径区段的起点速度方向的单位矢量；
Figure PCTCN2022116465-appb-000083
(x 3-x 2)、(y 3-y 2)分别与
Figure PCTCN2022116465-appb-000084
同正负，其中，
Figure PCTCN2022116465-appb-000085
是表示原局部路径区段的终点速度方向的单位矢量；
将s＝0和К(0)＝К 0代入
Figure PCTCN2022116465-appb-000086
中，其中，К 0表示原局部路径区段的路径起点曲率，P x′(s)、P y′(s)、P x″(s)、P y″(s)分别是
Figure PCTCN2022116465-appb-000087
的一阶导横、纵坐标和二阶导横、纵坐标；
将s＝1和К(1)＝К 1代入
Figure PCTCN2022116465-appb-000088
中，其中，К 1表示原局部路径区段的路径终点曲率。
可选地，实时局部路径规划步骤S22包括第二实时局部路径规划步骤S22。在所述第二实时局部路径规划步骤S22的第一子步骤中，确定途经点集合，其中，检测位于相对于移动机器人1的当前位置点的预定距离范围内的至少一个途经点，所述途经点表示移动机器人1需要经过的点，所述途经点集合为由邻近初始局部路径区段的m个途经点组成的集合，m≥1。在所述第二实时局部路径规划步骤S22的第二子步骤中，生成的实时优化局部路径2经过途经点集合中的所有途经点。
在所述第二实时局部路径规划步骤S22中，实时优化局部路径2是m+3阶贝塞尔曲线，所述m+3阶贝塞尔曲线的第一个控制点和第m+4个控制点分别为实时优化局部路径2的起点和终点。确定述m+3阶贝塞尔曲线的控制点
Figure PCTCN2022116465-appb-000089
的坐标的方式也可与上文针对全局路径规划方法所描述的类似，在此不再赘述。
可选地，在所述第二实时局部路径规划步骤S22的第二子步骤中，实时优化局部路径2被生成为使得途经点集合中的至少一个途经点处的速度方向满足以下条件中的一者：与原局部路径区段上的距离相应的途经点最近的点的速度方向相同；与原局部路径区段的起点到终点的方向相同；满足移动机器人1在相应的途经点处的任务需求。
可选地，在第二实时局部路径规划步骤S22中，原局部路径区段的终点被确定为使得：沿着规划路径的方向，原局部路径区段的终点位于规划路径上的所有分别距离途经点集合中的各途经点最近的点之后；和/或途经点集合中距离原局部路径区段的起点最近的途经点与原局部路径区段的起点之间的距离等于途经点集合中距离原局部路径区段的终点最近的途经点与原局部路径区段的终点之间的距离。
可选地，在所述第二实时局部路径规划步骤S22的第一子步骤中，途经点集合中的途经点包括避障途经点，所述避障途经点通过下述方式确定：确定与规划路径相冲突的障碍物3；以及根据所述障碍物3的位置，确定至少一个避障途经点，使得生成的实时优化局部路径2能够经过所述至少一个避障途经点并绕过障碍物3。在移动机器人1运动时，可能出现原规划路 径在障碍物3附近的轨迹不平滑，或者在原规划路径上出现了与原规划路径干涉的新的静态障碍物3。通过避障途经点，能够使移动机器人1沿着平滑的轨迹避开障碍物3。关于避障途经点的确定方式可与上文针对全局路径规划方法所描述的类似。
在第二实时局部路径规划步骤S22的第一子步骤中，原局部路径区段的起点和终点被确定为使得：原局部路径区段的起点到障碍物3的距离大于预定的第三距离阈值；和/或原局部路径区段的终点到障碍物3的距离大于预定的第四距离阈值。
在所述第二实时局部路径规划步骤S22的第二子步骤中，实时优化局部路径2被生成为使得途经点集合中的至少一个避障途经点处的速度方向满足以下条件中的一者：与原局部路径区段上的距离相应的避障途经点最近的点的速度方向相同；与原局部路径区段的起点到终点的方向相同；与距离相应的避障途经点最近的位于障碍物3边缘的点处的障碍物3代价梯度下降的方向垂直。
根据本发明的运动控制方法与根据本发明的全局路径规划方法具有对应的特性和相似的原理。上文中针对全局路径规划方法所描述的特征和优势，也可相应地适用于运动控制方法。
另外，本发明还涉及一种计算机程序产品，其包括计算器程序指令，当所述计算机程序指令被一个或多于一个处理器执行时，所述处理器能够执行根据本发明的全局路径规划方法或运动控制方法。
在本发明中，计算机程序产品可存储在计算机可读存储介质中。计算机可读存储介质例如可包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡，安全数字卡，闪存卡、至少一个磁盘存储器件、闪存器件、或其它易失性固态存储器件。处理器10可以是中央处理单元，还可以是其它通用处理器、数字信号处理器、专用集成电路、现成可编程门阵列或者其它可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者也可以是任何常规的处理器等。
尽管这里详细描述了本发明的特定实施方式，但它们仅仅是为了解释的目的而给出的，而不应认为它们对本发明的范围构成限制。在不脱离本 发明精神和范围的前提下，各种替换、变更和改造可被构想出来。

Claims (25)

  1. 一种用于移动机器人(1)的全局路径规划方法，其中，所述全局路径规划方法包括以下步骤：
    初始规划步骤S11，其中，获取用于移动机器人(1)的初始的全局路径；
    局部路径规划步骤S12，其中执行下述子步骤：
    第一子步骤，选取全局路径中的至少一部分作为初始局部路径区段；
    第二子步骤，生成优化局部路径(2)，使得所述优化局部路径(2)是具有连续的二阶导数的曲线，并且所述优化局部路径(2)与初始局部路径区段具有相同的起点、终点、起点速度方向、终点速度方向、起点曲率和终点曲率；以及
    第三子步骤，用优化局部路径(2)替换初始局部路径区段以形成新的全局路径；以及
    全局路径确定步骤S13，其中，将新的全局路径确定为最终全局路径。
  2. 根据权利要求1所述的全局路径规划方法，其中，
    以下述方式中的至少一者执行局部路径规划步骤S12：
    在局部路径规划步骤S12，以使得每个优化局部路径(2)的起点都位于最终全局路径上的方式选取初始局部路径区段；
    初始局部路径区段的长度小于预定的长度阈值；
    重复执行局部路径规划步骤S12，直到新的全局路径中的任一点都位于至少一个优化局部路径(2)上之后，才执行最终确定步骤S13。
  3. 根据权利要求1或2所述的全局路径规划方法，其中，
    局部路径规划步骤S12包括第一局部路径规划步骤，在所述第一局部路径规划步骤中，优化局部路径(2)是三阶贝塞尔曲线，所述三阶贝塞尔曲线由下式表示：
    Figure PCTCN2022116465-appb-100001
    其中，
    Figure PCTCN2022116465-appb-100002
    表示贝塞尔曲线的控制点的坐标并且
    Figure PCTCN2022116465-appb-100003
    其中， 控制点
    Figure PCTCN2022116465-appb-100004
    的坐标通过以下方式来确定：
    Figure PCTCN2022116465-appb-100005
    为初始局部路径区段的起点的坐标，
    Figure PCTCN2022116465-appb-100006
    为初始局部路径区段的终点的坐标；
    Figure PCTCN2022116465-appb-100007
    (x 1-x 0) 、(y 1-y 0)分别与
    Figure PCTCN2022116465-appb-100008
    同正负，其中，
    Figure PCTCN2022116465-appb-100009
    是表示初始局部路径区段的起点速度方向的单位矢量；
    Figure PCTCN2022116465-appb-100010
    (x 3-x 2)、(y 3-y 2)分别与
    Figure PCTCN2022116465-appb-100011
    同正负，其中，
    Figure PCTCN2022116465-appb-100012
    是表示初始局部路径区段的终点速度方向的单位矢量；
    将s＝0和К(0)＝К 0代入
    Figure PCTCN2022116465-appb-100013
    中，其中，К 0表示初始局部路径区段的路径起点曲率，P x′(s)、P y′(s)、P x″(s)、P y″(s)分别是
    Figure PCTCN2022116465-appb-100014
    的一阶导横、纵坐标和二阶导横、纵坐标；
    将s＝1和К(1)＝К 1代入
    Figure PCTCN2022116465-appb-100015
    中，其中，К 1表示初始局部路径区段的路径终点曲率。
  4. 根据权利要求1-3中任一项所述的全局路径规划方法，其中，
    局部路径规划步骤S12包括第二局部路径规划步骤，其中，
    在所述第二局部路径规划步骤的第一子步骤中，确定途经点集合，途经点表示移动机器人(1)需要经过的点，所述途经点集合为由邻近初始局部路径区段的m个途经点组成的集合，m≥1；
    在所述第二局部路径规划步骤的第二子步骤中，生成的优化局部路径(2)经过途经点集合中的所有途经点。
  5. 根据权利要求4所述的全局路径规划方法，其中，
    在所述第二局部路径规划步骤中，优化局部路径(2)是m+3阶贝塞尔曲线，所述m+3阶贝塞尔曲线的第一个控制点和第m+4个控制点分别为优化局部路径(2)的起点和终点。
  6. 根据权利要求4或5所述的全局路径规划方法，其中，
    在所述第二局部路径规划步骤的第二子步骤中，优化局部路径(2)被生成为使得途经点集合中的至少一个途经点处的速度方向满足以下条件中的一者：
    与初始局部路径区段上的距离相应的途经点最近的点的速度方向相同；
    与初始局部路径区段的起点到终点的方向相同；
    满足移动机器人(1)在相应的途经点处的任务需求。
  7. 根据权利要求4-6中任一项所述的全局路径规划方法，其中，
    在第二局部路径规划步骤中，初始局部路径区段的起点和终点被确定为使得：
    沿着全局路径的方向，初始局部路径区段的起点位于全局路径上的所有分别距离途经点集合中的各途经点最近的点之前；和/或
    沿着全局路径的方向，初始局部路径区段的终点位于全局路径上的所有分别距离途经点集合中的各途经点最近的点之后；和/或
    途经点集合中距离初始局部路径区段的起点最近的途经点与初始局部路径区段的起点之间的距离等于途经点集合中距离初始局部路径区段的终点最近的途经点与初始局部路径区段的终点之间的距离。
  8. 根据权利要求4-7中任一项所述的全局路径规划方法，其中，
    在所述第二局部路径规划步骤的第一子步骤中，途经点集合中的途经点包括避障途经点，所述避障途经点通过下述方式确定：
    确定与全局路径相冲突的障碍物(3)；以及
    根据所述障碍物(3)的位置，确定至少一个避障途经点，使得生成的优化局部路径(2)能够经过所述至少一个避障途经点并绕过障碍物(3)。
  9. 根据权利要求8所述的全局路径规划方法，其中，
    通过由障碍物代价图获取障碍物(3)的边缘来确定与全局路径相冲突的障碍物(3)的位置；和/或
    借助于障碍物代价图确定所述至少一个避障途经点。
  10. 根据权利要求8或9所述的全局路径规划方法，其中，
    在第二局部路径规划步骤的第一子步骤中，初始局部路径区段的起点和终点被确定为使得：
    初始局部路径区段的起点到障碍物(3)的距离大于预定的第一距离阈值；和/或
    初始局部路径区段的终点到障碍物(3)的距离大于预定的第二距离阈值。
  11. 根据权利要求8-10中任一项所述的全局路径规划方法，其中，
    在所述第二局部路径规划步骤的第二子步骤中，优化局部路径(2)被生成为使得途经点集合中的至少一个避障途经点处的速度方向满足以下条件中的一者：
    与初始局部路径区段上的距离相应的避障途经点最近的点的速度方向相同；
    与初始局部路径区段的起点到终点的方向相同；
    与距离相应的避障途经点最近的位于障碍物(3)边缘的点处的障碍物(3)代价梯度下降的方向垂直。
  12. 根据权利要求1-11中任一项所述的全局路径规划方法，其中，
    移动机器人(1)为差速机器人。
  13. 一种用于移动机器人(1)的运动控制方法，其中，所述运动控制方法包括以下步骤：
    第一运动步骤S21，其中，使移动机器人(1)沿着规划路径运动；
    实时局部路径规划步骤S22，其中执行下述子步骤：
    第一子步骤，选取规划路径中的至少一部分作为原局部路径区段；
    第二子步骤，生成实时优化局部路径(2)，使得所述实时优化局部路径(2)是具有连续的二阶导数的曲线，并且所述实时优化局部路径(2) 与原局部路径区段具有相同的起点、终点、起点速度方向、终点速度方向、起点曲率和终点曲率；以及
    第三子步骤，用实时优化局部路径(2)替换规划路径中的原局部路径区段；以及
    第二运动步骤S23，其中，控制移动机器人(1)按照替换后的规划路径运动。
  14. 根据权利要求13所述的运动控制方法，其中，
    在实时局部路径规划步骤S22的第一子步骤中，以移动机器人(1)的当前位置点作为原局部路径区段的起点。
  15. 根据权利要求13或14所述的运动控制方法，其中，
    实时局部路径规划步骤S22包括第一实时局部路径规划步骤S22，在所述第一实时局部路径规划步骤S22中，实时优化局部路径(2)是三阶贝塞尔曲线，所述三阶贝塞尔曲线由下式表示：
    Figure PCTCN2022116465-appb-100016
    其中，
    Figure PCTCN2022116465-appb-100017
    表示贝塞尔曲线的控制点的坐标并且
    Figure PCTCN2022116465-appb-100018
    其中，控制点
    Figure PCTCN2022116465-appb-100019
    的坐标通过以下方式来确定：
    Figure PCTCN2022116465-appb-100020
    为原局部路径区段的起点的坐标，
    Figure PCTCN2022116465-appb-100021
    为原局部路径区段的终点的坐标；
    Figure PCTCN2022116465-appb-100022
    (x 1-x 0) 、(y 1-y 0)分别与
    Figure PCTCN2022116465-appb-100023
    同正负，其中，
    Figure PCTCN2022116465-appb-100024
    是表示原局部路径区段的起点速度方向的单位矢量；
    Figure PCTCN2022116465-appb-100025
    (x 3-x 2)、(y 3-y 2)分别与
    Figure PCTCN2022116465-appb-100026
    同正负，其中，
    Figure PCTCN2022116465-appb-100027
    是表示原局部路径区段的终点速度方向的单位矢量；
    将s＝0和К(0)＝К 0代入
    Figure PCTCN2022116465-appb-100028
    中，其中，К 0表示原局部路径区段的路径起点曲率，P x′(s)、P y′(s)、P x″(s)、P y″(s)分别是
    Figure PCTCN2022116465-appb-100029
    的一阶导横、纵坐标和二阶导横、纵坐标；
    Figure PCTCN2022116465-appb-100030
  16. 根据权利要求13-15中任一项所述的运动控制方法，其中，
    实时局部路径规划步骤S22包括第二实时局部路径规划步骤S22，其中，
    在所述第二实时局部路径规划步骤S22的第一子步骤中，确定途经点集合，其中，检测位于相对于移动机器人(1)的当前位置点的预定距离范围内的至少一个途经点，所述途经点表示移动机器人(1)需要经过的点，所述途经点集合为由邻近原局部路径区段的m个途经点组成的集合，m≥1；
    在所述第二实时局部路径规划步骤S22的第二子步骤中，生成的实时优化局部路径(2)经过途经点集合中的所有途经点。
  17. 根据权利要求16所述的运动控制方法，其中，
    在所述第二实时局部路径规划步骤S22中，实时优化局部路径(2)是m+3阶贝塞尔曲线，所述m+3阶贝塞尔曲线的第一个控制点和第m+4个控制点分别为实时优化局部路径(2)的起点和终点。
  18. 根据权利要求16或18所述的运动控制方法，其中，
    在所述第二实时局部路径规划步骤S22的第二子步骤中，实时优化局部路径(2)被生成为使得途经点集合中的至少一个途经点处的速度方向满足以下条件中的一者：
    与原局部路径区段上的距离相应的途经点最近的点的速度方向相同；
    与原局部路径区段的起点到终点的方向相同；
    满足移动机器人(1)在相应的途经点处的任务需求。
  19. 根据权利要求16-19中任一项所述的运动控制方法，其中，
    在第二实时局部路径规划步骤S22中，原局部路径区段的终点被确定为使得：
    沿着规划路径的方向，原局部路径区段的终点位于规划路径上的所有 分别距离途经点集合中的各途经点最近的点之后；和/或
    途经点集合中距离原局部路径区段的起点最近的途经点与原局部路径区段的起点之间的距离等于途经点集合中距离原局部路径区段的终点最近的途经点与原局部路径区段的终点之间的距离。
  20. 根据权利要求16-19中任一项所述的运动控制方法，其中，
    在所述第二实时局部路径规划步骤S22的第一子步骤中，途经点集合中的途经点包括避障途经点，所述避障途经点通过下述方式确定：
    确定与规划路径相冲突的障碍物(3)；以及
    根据所述障碍物(3)的位置，确定至少一个避障途经点，使得生成的实时优化局部路径(2)能够经过所述至少一个避障途经点并绕过障碍物(3)。
  21. 根据权利要求20所述的运动控制方法，其中，
    通过由障碍物代价图获取障碍物(3)的边缘来确定与规划路径相冲突的障碍物(3)的位置；和/或
    借助于障碍物代价图确定所述至少一个避障途经点。
  22. 根据权利要求21所述的运动控制方法，其中，
    在第二实时局部路径规划步骤S22的第一子步骤中，原局部路径区段的起点和终点被确定为使得：
    原局部路径区段的起点到障碍物(3)的距离大于预定的第三距离阈值；和/或
    原局部路径区段的终点到障碍物(3)的距离大于预定的第四距离阈值。
  23. 根据权利要求20-22中任一项所述的运动控制方法，其中，
    在所述第二实时局部路径规划步骤S22的第二子步骤中，实时优化局部路径(2)被生成为使得途经点集合中的至少一个避障途经点处的速度方向满足以下条件中的一者：
    与原局部路径区段上的距离相应的避障途经点最近的点的速度方向相同；
    与原局部路径区段的起点到终点的方向相同；
    与距离相应的避障途经点最近的位于障碍物(3)边缘的点处的障碍物(3)代价梯度下降的方向垂直。
  24. 根据权利要求13-23中任一项所述的运动控制方法，其中，移动机器人(1)为差速机器人。
  25. 一种计算机程序产品、特别是计算机可读存储介质，其包括计算器程序指令，其中，当所述计算机程序指令被一个或多于一个处理器执行时，所述处理器够执行根据权利要求1-12中任一项所述的全局路径规划方法或根据权利要求13-24中任一项所述的运动控制方法。
PCT/CN2022/116465 2021-09-13 2022-09-01 全局路径规划方法、运动控制方法及计算机程序产品 WO2023036044A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202111066416.X 2021-09-13
CN202111066416.XA CN115808918A (zh) 2021-09-13 2021-09-13 全局路径规划方法、运动控制方法及计算机程序产品

Publications (1)

Publication Number Publication Date
WO2023036044A1 true WO2023036044A1 (zh) 2023-03-16

Family

ID=85481016

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2022/116465 WO2023036044A1 (zh) 2021-09-13 2022-09-01 全局路径规划方法、运动控制方法及计算机程序产品

Country Status (2)

Country Link
CN (1) CN115808918A (zh)
WO (1) WO2023036044A1 (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116149340A (zh) * 2023-04-23 2023-05-23 季华实验室 差速轮底盘机器人自适应路径跟随方法及其相关设备
CN117346791A (zh) * 2023-12-01 2024-01-05 北京科技大学 基于视觉图像的智能轮椅路径规划方法及系统

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20160375901A1 (en) * 2015-06-29 2016-12-29 Mitsubishi Electric Research Laboratories, Inc. System and Method for Controlling Semi-Autonomous Vehicles
CN109253735A (zh) * 2018-11-30 2019-01-22 奇瑞汽车股份有限公司 路径规划方法、装置及存储介质
CN109799822A (zh) * 2019-01-30 2019-05-24 中国石油大学(华东) 移动机器人全局平滑路径规划方法
CN112362074A (zh) * 2020-10-30 2021-02-12 重庆邮电大学 一种结构化环境下的智能车辆局部路径规划方法
CN112683278A (zh) * 2021-01-08 2021-04-20 东南大学 一种基于改进a*算法和贝塞尔曲线的全局路径规划方法
CN113031592A (zh) * 2021-02-25 2021-06-25 杭州国辰机器人科技有限公司 一种基于五阶贝塞尔曲线的机器人路径平滑方法及系统
CN113341991A (zh) * 2021-06-18 2021-09-03 重庆大学 一种基于动态窗口和冗余节点过滤的路径优化方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20160375901A1 (en) * 2015-06-29 2016-12-29 Mitsubishi Electric Research Laboratories, Inc. System and Method for Controlling Semi-Autonomous Vehicles
CN109253735A (zh) * 2018-11-30 2019-01-22 奇瑞汽车股份有限公司 路径规划方法、装置及存储介质
CN109799822A (zh) * 2019-01-30 2019-05-24 中国石油大学(华东) 移动机器人全局平滑路径规划方法
CN112362074A (zh) * 2020-10-30 2021-02-12 重庆邮电大学 一种结构化环境下的智能车辆局部路径规划方法
CN112683278A (zh) * 2021-01-08 2021-04-20 东南大学 一种基于改进a*算法和贝塞尔曲线的全局路径规划方法
CN113031592A (zh) * 2021-02-25 2021-06-25 杭州国辰机器人科技有限公司 一种基于五阶贝塞尔曲线的机器人路径平滑方法及系统
CN113341991A (zh) * 2021-06-18 2021-09-03 重庆大学 一种基于动态窗口和冗余节点过滤的路径优化方法

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116149340A (zh) * 2023-04-23 2023-05-23 季华实验室 差速轮底盘机器人自适应路径跟随方法及其相关设备
CN117346791A (zh) * 2023-12-01 2024-01-05 北京科技大学 基于视觉图像的智能轮椅路径规划方法及系统
CN117346791B (zh) * 2023-12-01 2024-03-22 北京科技大学 基于视觉图像的智能轮椅路径规划方法及系统

Also Published As

Publication number Publication date
CN115808918A (zh) 2023-03-17

Similar Documents

Publication Publication Date Title
WO2023036044A1 (zh) 全局路径规划方法、运动控制方法及计算机程序产品
CN110260867B (zh) 一种机器人导航中位姿确定、纠正的方法、设备及装置
WO2023016194A1 (zh) 用于移动机器人的路径规划方法及程序产品
CN109434831B (zh) 机器人运行方法、装置、机器人、电子设备及可读介质
US8831880B2 (en) Estimating positions of a device and at least one target in an environment
Digani et al. Obstacle avoidance for industrial AGVs
Patil et al. Gaussian belief space planning with discontinuities in sensing domains
CN108983603A (zh) 一种机器人与物体的对接方法及其机器人
WO2022252869A1 (zh) 移动设备绕障方法、移动设备及存储介质
KR20180076815A (ko) Qr 마커와 레이저 스캐너를 이용하여 광범위 실내 공간에서 로봇의 위치를 인식하기 위한 방법 및 장치
Lu et al. Lane marking based vehicle localization using particle filter and multi-kernel estimation
CN113867347A (zh) 机器人路径规划方法、装置、管理系统及计算机存储介质
Damas et al. Avoiding moving obstacles: the forbidden velocity map
WO2023124339A1 (zh) 路径规划方法、运动控制方法及计算机程序产品
Zhang et al. An efficient centralized planner for multiple automated guided vehicles at the crossroad of polynomial curves
KR102183830B1 (ko) 무인 자율주행차량의 경로제어장치 및 경로제어방법
US20200080850A1 (en) Self-propelled device, and method and travel control computer program of controlling traveling of self-propelled device
JP7235060B2 (ja) 経路計画装置、経路計画方法、及びプログラム
JP7160110B2 (ja) 経路計画装置、経路計画方法、及びプログラム
WO2023279918A1 (zh) 双舵轮式自主移动设备的运动控制方法和系统及程序产品
US11614750B2 (en) Self-propelled device, self-propelling method, and recording medium
CN113156962A (zh) 运动控制方法、装置、机器人及存储介质
Luo et al. Boundary aware navigation and mapping for a mobile automaton
CN112518744A (zh) 机器人轨迹规划方法、装置、机器人
Bošnak et al. Obstacle avoidance for line-following AGV with local maps

Legal Events

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

Ref document number: 22866508

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE