A trajectory optimization method, device and a robot

Abstract

Embodiments of the present application provide a trajectory optimization method and device and a robot, and relate to the technical field of robots. In the process that the robot moves based on a preset global path, when the current position of the robot satisfies a preset optimization condition, first, a specified starting point and a specified ending point are determined in the global path based on the current position of the robot; then, the path between the specified starting point and the specified ending point in the global path is subjected to trajectory optimization to obtain a target path; thereafter, the robot is controlled to move according to the target path, and when the current position of the robot satisfies the preset optimization condition, the step of determining the specified starting point and the specified ending point in the global path based on the current position of the robot is returned to until the robot moves to a preset ending point of the global path. Compared with related technologies, application of the scheme provided by the embodiments of the present application can improve the moving efficiency of the robot when moving according to the preset global path.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of robot technology, and in particular, to a trajectory optimization method and device and a robot. BACKGROUND

[0002] With the development of science and technology, robots are widely used in various scenes of production and life, for example, robots are used in factories to patrol production workshops, AGVs (Automated Guided Vehicles) are used in cargo distribution centers to transport goods, etc. In the process of robot movement, the movement path needs to be planned so that the robot moves from the starting point to the ending point according to the planned movement path.

[0003] In related technologies, when planning a path for a robot, the starting point and the ending point of the path to be planned can be preset, and then a global path starting from the preset starting point and ending at the preset ending point is planned on a map including the preset starting point and the preset ending point.

[0004] However, there may be many inflection points, non-smooth turning points, and paths close to preset obstacles on the map used to plan the global path in the planned global path, so that when the robot moves according to the above paths, the movement speed of the robot will slow down due to the non-smooth paths, and the robot may also collide with the preset obstacles, thereby causing the robot to move inefficiently according to the planned global path.

[0005] Therefore, there is a need for a trajectory optimization method to optimize the trajectory of the planned global path, improve the smoothness and safety of the planned path, and thereby improve the movement efficiency of the robot when moving according to the preset global path. SUMMARY

[0006] The purpose of the embodiments of the present application is to provide a trajectory optimization method, device and robot to improve the movement efficiency of the robot. The specific technical solutions are as follows:

[0007] In a first aspect, the embodiments of the present application provide a trajectory optimization method, which comprises:

[0008] In the process of moving the robot based on the preset global path, when the current position of the robot satisfies a preset optimization condition, a specified starting point and a specified ending point are determined in the global path based on the current position of the robot;

[0009] The path between the specified starting point and the specified ending point in the global path is trajectory optimized to obtain a target path;

[0010] controlling the robot to move according to the target path, and returning to the step of determining the specified start point and the specified end point in the global path based on the current position of the robot when the current position of the robot satisfies the preset optimization condition until the robot moves to the preset end point of the global path.

[0011] Optionally, in a specific implementation, the step of determining the specified start point and the specified end point in the global path based on the current position of the robot when the current position of the robot satisfies the preset optimization condition during the movement of the robot based on the preset global path comprises:

[0012] determining the specified start point and the specified end point in the global path based on the preset start point when the robot is located at the preset start point of the preset global path;

[0013] determining the specified start point and the specified end point in the global path based on the current position of the robot when the robot moves to the path midpoint of the target path during the movement of the robot based on the target path obtained by the trajectory optimization; or

[0014] determining the specified start point and the specified end point in the global path based on the current position of the robot when the robot moves for a time length corresponding to a preset period during the movement of the robot based on the global path.

[0015] Optionally, in a specific implementation, the step of determining the specified start point and the specified end point in the global path based on the current position of the robot comprises:

[0016] determining a point in the global path with the minimum distance from the current position of the robot as the specified start point;

[0017] determining the specified end point in the global path based on the specified start point.

[0018] Optionally, in a specific implementation, the step of determining the specified end point in the global path based on the specified start point comprises:

[0019] determining a target distance based on the current speed of the robot;

[0020] if the distance between the specified start point and the preset end point of the global path is not greater than the target distance, taking the preset end point of the global path as the specified end point;

[0021] if the distance between the specified start point and the preset end point of the global path is greater than the target distance, determining a specified point in the global path as the specified end point;

[0022] The specified point includes a point closest to the target distance in a reference distance from the specified starting point, a point closest to the target distance in a reference distance less than the target distance, or a point closest to the target distance in a reference distance greater than the target distance.

[0023] Optionally, in a specific implementation, before the specified point in the global path is determined as the specified end point, the method further includes:

[0024] determining whether a distance between the specified point and a preset end point of the global path is not greater than a preset distance threshold;

[0025] if yes, determining the preset end point of the global path as the specified end point;

[0026] otherwise, determining the specified point in the global path as the specified end point.

[0027] Optionally, in a specific implementation, the controlling the robot to move according to the target path includes:

[0028] determining a calibration distance for path planning when a current position of the robot satisfies a preset planning condition;

[0029] determining a target point for local path planning from the global path based on the calibration distance and the current position of the robot;

[0030] selecting multiple groups of candidate moving speeds from a preset speed range corresponding to a current time, and determining a candidate path corresponding to each group of candidate moving speeds;

[0031] determining a target evaluation score of each candidate path, and determining a candidate path whose determined target evaluation score satisfies a preset evaluation condition as a local path; wherein the target evaluation score of each candidate path is used to represent a score of a deviation degree of the candidate path relative to the target point, a closeness degree of the candidate path to the global path, and an obstacle avoidance ability;

[0032] controlling the robot to move along the local path according to the candidate moving speed corresponding to the local path, and returning to the step of determining the calibration distance for path planning when the current position of the robot satisfies the preset planning condition.

[0033] Optionally, in a specific implementation, the determining the target evaluation score of each candidate path includes:

[0034] For each candidate path, an initial evaluation score of the candidate path with respect to each evaluation function is calculated by using a preset respective target evaluation function, and a target evaluation score of the candidate path is calculated based on the respective initial evaluation scores;

[0035] The respective target evaluation functions include respective first-type evaluation functions related to a deviation degree of a path deviation from a target point, respective second-type evaluation functions related to a path closeness degree, and respective third-type evaluation functions related to an obstacle avoidance capability, the first-type evaluation functions include an azimuth angle evaluation function, the second-type evaluation functions include a path following evaluation function, and the third-type evaluation functions include an obstacle distance evaluation function.

[0036] Optionally, in a specific implementation, the respective target evaluation functions further include a fourth-type evaluation function related to a speed, and the fourth-type evaluation function includes a linear speed evaluation function.

[0037] In a second aspect, an embodiment of the present application provides a trajectory optimization device, and the device comprises:

[0038] A determination module is configured to, when a current position of a robot meets a preset optimization condition during movement of the robot based on a preset global path, determine a specified starting point and a specified ending point in the global path based on the current position of the robot.

[0039] An optimization module is configured to perform trajectory optimization on a path between the specified starting point and the specified ending point in the global path to obtain a target path.

[0040] A control module is configured to control the robot to move according to the target path, and trigger the determination module when the current position of the robot meets the preset optimization condition, until the robot moves to a preset ending point of the global path.

[0041] Optionally, in a specific implementation, the determination module is specifically configured to:

[0042] When the robot is located at a preset starting point of a preset global path, determine a specified starting point and a specified ending point in the global path based on the preset starting point.

[0043] During movement of the robot based on a target path obtained through trajectory optimization, when the robot moves to a path midpoint of the target path, determine a specified starting point and a specified ending point in the global path based on a current position of the robot; or

[0044] During movement of the robot based on the global path, when the robot moves for a time length corresponding to a preset period, a specified start point and a specified end point are determined in the global path based on a current position of the robot.

[0045] Optionally, in a specific implementation, the determining module comprises:

[0046] a start point determining sub-module, configured to determine a point in the global path having a minimum distance from the current position of the robot as the specified start point;

[0047] an end point determining sub-module, configured to determine a specified end point in the global path based on the specified start point.

[0048] Optionally, in a specific implementation, the end point determining sub-module is specifically configured to:

[0049] determine a target distance based on a current speed of the robot;

[0050] if a distance between the specified start point and a preset end point of the global path is not greater than the target distance, determine the preset end point of the global path as the specified end point;

[0051] if the distance between the specified start point and the preset end point of the global path is greater than the target distance, determine a specified point in the global path as the specified end point;

[0052] wherein the specified point comprises a point having a reference distance closest to the target distance, a point having a reference distance less than the target distance and closest to the target distance, or a point having a reference distance greater than the target distance and closest to the target distance.

[0053] Optionally, in a specific implementation, the apparatus further comprises:

[0054] a distance determining module, configured to, before the specified point in the global path is determined as the specified end point, determine whether a distance between the specified point and a preset end point of the global path is not greater than a preset distance threshold; if yes, determine the preset end point of the global path as the specified end point; otherwise, trigger the end point determining sub-module.

[0055] Optionally, in a specific implementation, the control module comprises:

[0056] a distance determining sub-module, configured to, when the current position of the robot satisfies a preset planning condition, determine a calibration distance for path planning;

[0057] a target point determination sub-module, configured to determine a target point for local path planning from the global path based on the calibration distance and a current position of the robot;

[0058] a candidate path determination sub-module, configured to select a plurality of groups of candidate moving speeds from a preset speed range corresponding to a current time, and determine a candidate path corresponding to each group of candidate moving speeds;

[0059] a local path determination sub-module, configured to determine a target evaluation score of each candidate path, and determine a candidate path whose determined target evaluation score satisfies a preset evaluation condition as a local path; wherein the target evaluation score of each candidate path is used to represent a score of a deviation degree of the candidate path relative to the target point, a path proximity degree of the candidate path relative to the global path, and an obstacle avoidance capability of the candidate path;

[0060] a moving sub-module, configured to control the robot to move along the local path according to a candidate moving speed corresponding to the local path, and trigger the distance determination sub-module when a current position of the robot satisfies a preset planning condition.

[0061] Optionally, in a specific implementation, the local path determination sub-module is specifically configured to:

[0062] for each candidate path, calculate an initial evaluation score of the candidate path with respect to each evaluation function by using preset target evaluation functions, and calculate a target evaluation score of the candidate path based on the initial evaluation scores;

[0063] wherein the target evaluation functions include: a plurality of first type evaluation functions related to a deviation degree of path deviation relative to the target point, a plurality of second type evaluation functions related to the path proximity degree, and a plurality of third type evaluation functions related to the obstacle avoidance capability, the first type evaluation functions include: an azimuth angle evaluation function; the second type evaluation functions include: a path following evaluation function; and the third type evaluation functions include: an obstacle distance evaluation function.

[0064] Optionally, in a specific implementation, the target evaluation functions further include: a fourth type evaluation function related to the speed; wherein the fourth type evaluation function includes: a linear speed evaluation function.

[0065] In a third aspect, an embodiment of the present application provides a robot, comprising a processor, a communication interface, a memory and a communication bus, wherein the processor, the communication interface and the memory complete mutual communication through the communication bus;

[0066] the memory is used to store a computer program;

[0067] A processor is configured to implement the steps of any of the above method embodiments when executing a program stored in a memory.

[0068] In a fourth aspect, an embodiment of the present application provides a computer readable storage medium, wherein a computer program is stored in the computer readable storage medium, and the computer program, when executed by a processor, implements the steps of any of the above method embodiments.

[0069] In a fifth aspect, an embodiment of the present application further provides a computer program product containing instructions, which, when executed on a computer, cause the computer to perform the steps of any of the above method embodiments.

[0070] The embodiments of the present application have the following beneficial effects:

[0071] As can be seen above, when it is desired to move a robot from a preset starting point to a preset ending point, the global path from the preset starting point to the preset ending point can be determined in advance according to the scheme provided by the embodiments of the present application. Then, when the current position of the robot satisfies the preset optimization condition during the movement of the robot based on the global path, the specified starting point and the specified ending point can be determined in the global path based on the current position of the robot. After that, the path optimization can be performed on the path between the specified starting point and the specified ending point in the global path to obtain a target path. Then, the robot can be controlled to move according to the determined target path, and when the current position of the robot satisfies the preset optimization condition, the step of determining the specified starting point and the specified ending point in the global path based on the current position of the robot is returned to until the robot moves to the preset ending point of the global path.

[0072] Therefore, when it is desired to move a robot from a preset starting point to a preset ending point, the global path from the preset starting point to the preset ending point can be determined after global path planning according to the scheme provided by the embodiments of the present application. Then, during the movement of the robot based on the global path, the global path can be segmented and trajectory optimized according to the current position of the robot to achieve trajectory optimization of the global path. Then, the robot can move according to the global path after the segmented trajectory optimization during the movement based on the global path. In this way, since each target path obtained by the segmented trajectory optimization can be smooth and can avoid the preset obstacles, the global path followed by the robot during the movement from the preset starting point to the preset ending point can be smooth and can avoid the preset obstacles. Therefore, the robot can avoid deceleration due to the problems such as too many inflection points in the global path, non-smooth path segments in the global path, and collision with the preset obstacles, and thus the movement efficiency of the robot during the movement can be improved. BRIEF DESCRIPTION OF DRAWINGS

[0073] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiments or prior art description. Obviously, the drawings in the following description only represent some of the embodiments of the present application, and other embodiments can also be obtained by those skilled in the art based on these drawings.

[0074] Figure 1 A flowchart of a trajectory optimization method provided by an embodiment of the present application;

[0075] Figures 2(a)-2(e) A schematic diagram of a specific example of determining a specified terminal point provided by an embodiment of the present application;

[0076] Figure 3 A flowchart of local path planning provided by an embodiment of the present application;

[0077] Figure 4 A schematic diagram of a specific example of selecting a target point provided by an embodiment of the present application;

[0078] Figure 5 A schematic diagram of a plurality of groups of candidate moving speeds corresponding to moving trajectories provided by an embodiment of the present application;

[0079] Figure 6 A schematic diagram of a loss map provided by an embodiment of the present application;

[0080] Figure 7 A flowchart of a specific example of a trajectory optimization method provided by an embodiment of the present application;

[0081] Figure 8 A structural schematic diagram of a trajectory optimization device provided by an embodiment of the present application;

[0082] Figure 9 A structural schematic diagram of a robot provided by an embodiment of the present application. DETAILED DESCRIPTION

[0083] The technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments only represent some of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art based on the present application also belong to the scope of protection of the present application.

[0084] In the related art, when planning a path for a robot, a starting point and an ending point of the path to be planned can be preset, and then a global path starting from the preset starting point and ending at the preset ending point is planned on a map including the preset starting point and ending point. However, there can be a section with many inflection points, a turning point that is not smooth, and a section that is close to a preset obstacle on the map used to plan the global path in the planned global path. Thus, when the robot moves according to the above section, the moving speed of the robot is slowed down due to the non-smooth section, and the robot can collide with the preset obstacle, thereby causing the robot to move according to the planned global path with low efficiency. Therefore, a trajectory optimization method is needed to optimize the trajectory of the planned global path, improve the smoothness and safety of the planned path, and thereby improve the moving efficiency of the robot when moving according to the preset global path.

[0085] To solve the above technical problem, an embodiment of the present application provides a trajectory optimization method.

[0086] The method can be applied to various application scenarios that need to optimize the trajectory of the moving path of the robot, for example, in a factory, an AGV that patrols a workshop is optimized to move along a trajectory, in a home scenario, a household sweeping robot is optimized to move along a trajectory, and the like. Moreover, the method can be applied to a robot, for example, a trajectory optimization device is arranged on the robot to enable the trajectory optimization device to execute the method, or the method can be applied to other electronic devices in communication with the robot, for example, a server that provides a trajectory optimization scheme for the robot. Therefore, the application scenario and the execution subject of the method are not limited in the embodiment of the present application.

[0087] The trajectory optimization method provided by the embodiment of the present application can include the following steps:

[0088] During the movement of the robot based on the preset global path, when the current position of the robot satisfies a preset optimization condition, a specified starting point and a specified ending point are determined in the global path based on the current position of the robot;

[0089] The trajectory of the path between the specified starting point and the specified ending point in the global path is optimized to obtain a target path;

[0090] The robot is controlled to move according to the target path, and when the current position of the robot satisfies the preset optimization condition, the step of determining the specified starting point and the specified ending point in the global path based on the current position of the robot is returned until the robot moves to the preset ending point of the global path.

[0091] As can be seen from the above, when it is desired to move a robot from a preset starting point to a preset ending point, the global path from the preset starting point to the preset ending point can be determined in advance according to the scheme provided by the embodiments of the present application. Then, when the current position of the robot satisfies the preset optimization condition during the movement of the robot based on the global path, the specified starting point and the specified ending point can be determined in the global path based on the current position of the robot. After that, the path optimization can be performed on the path between the specified starting point and the specified ending point in the global path to obtain a target path. Then, the robot can be controlled to move according to the determined target path, and when the current position of the robot satisfies the preset optimization condition, the step of determining the specified starting point and the specified ending point in the global path based on the current position of the robot is returned to until the robot moves to the preset ending point of the global path.

[0092] Therefore, when it is desired to move a robot from a preset starting point to a preset ending point, the global path from the preset starting point to the preset ending point can be determined after global path planning according to the scheme provided by the embodiments of the present application. Then, during the movement of the robot based on the global path, the global path can be segmented and trajectory optimized according to the current position of the robot to achieve trajectory optimization of the global path. Then, the robot can move according to the global path after the segmented trajectory optimization during the movement based on the global path. In this way, since each segment of the target path obtained by the segmented trajectory optimization can be smooth and can avoid the preset obstacles, the global path followed by the robot during the movement from the preset starting point to the preset ending point can be smooth and can avoid the preset obstacles. Then, the robot can be prevented from slowing down due to the problems such as too many inflection points in the global path, non-smooth path segments in the global path, and collision with the preset obstacles, and the movement efficiency of the robot during the movement can be improved.

[0093] Next, a trajectory optimization method provided by the embodiments of the present application will be described in detail with reference to the accompanying drawings.

[0094] Figure 1 A flowchart of the trajectory optimization method provided by the embodiments of the present application is shown in FIG. 1, which can include the following steps S101-S103. Figure 1

[0095] S101: During the movement of a robot based on a preset global path, when the current position of the robot satisfies a preset optimization condition, a specified starting point and a specified ending point are determined in the global path based on the current position of the robot.

[0096] When it is desired to move a robot from a preset starting point to a preset ending point, the global path from the preset starting point to the preset ending point can be determined in advance according to the scheme provided by the embodiments of the present application. Then, when the current position of the robot satisfies the preset optimization condition during the movement of the robot based on the global path, the specified starting point and the specified ending point can be determined in the global path based on the current position of the robot. After that, the path optimization can be performed on the path between the specified starting point and the specified ending point in the global path to obtain a target path. Then, the robot can be controlled to move according to the determined target path, and when the current position of the robot satisfies the preset optimization condition, the step of determining the specified starting point and the specified ending point in the global path based on the current position of the robot is returned to until the robot moves to the preset ending point of the global path. ​

[0097] In the case that the preset start point and the preset end point are acquired, the global path planning algorithm can be used to plan a global path with the preset start point as the start point and the preset end point as the end point on a map including the preset start point, the preset end point and preset obstacles, and the robot can avoid the preset obstacles in the map when moving according to the global path.

[0098] The global path planning algorithm can be an A-Star algorithm, an ant colony algorithm or other algorithms, which are all reasonable and are not specifically limited in the embodiment of the present application.

[0099] Optionally, the A-Star algorithm is used to plan a global path with the preset start point as the start point and the preset end point as the end point on a grid map including preset obstacles, the preset start point and the preset end point.

[0100] Optionally, when the execution subject of global path planning is the robot, the robot can plan a global path by itself based on the preset start point and the preset end point using the global path planning algorithm, or the robot can acquire the global path from a server in communication with the robot.

[0101] Optionally, when the execution subject of global path planning is the server in communication with the robot, the server can acquire the global path planned by the robot based on the preset start point and the preset end point, or the server can plan a global path based on the preset start point and the preset end point using the global path planning algorithm.

[0102] Optionally, when the execution subject of global path planning is the server in communication with the robot, the server can acquire the global path planned by the robot based on the preset start point and the preset end point, or the server can plan a global path based on the preset start point and the preset end point using the global path planning algorithm.

[0103] However, the global path can have many inflection points, be not smooth or have a segment close to the preset obstacle. When moving to a segment with many inflection points or not smooth, the robot can need to slow down to pass the segment. When moving to a segment close to the preset obstacle, the robot can collide with the preset obstacle, thereby stopping moving or even causing damage. Therefore, it is necessary to optimize the trajectory of the global path to make the global path more suitable for the robot to move. In the trajectory optimization of the global path, the global path can be segmented based on the current position of the robot, so that the trajectory optimization of each part of the path obtained by segmentation is completed to optimize the trajectory of the global path.

[0104] In order to achieve the above solution, a preset optimization condition for triggering the segmented trajectory optimization in the robot movement process can be preset.

[0105] The preset optimization condition can be that the robot reaches any position within a specified range centered on a preset segmented point on the global path, or that the robot moves to any of the following positions: a midpoint of the segmented path after trajectory optimization, an endpoint of the segmented path after trajectory optimization, and a three-quarter point of the segmented path after trajectory optimization. It is reasonable for the robot to move for a corresponding duration of a preset period, and the embodiments of the present application do not make specific limitations.

[0106] Optionally, a plurality of preset segmented points can be selected in the global path, so that the global path is divided into a plurality of segmented paths according to the plurality of preset segmented points. In this way, when the robot reaches a position within a specified range centered on a preset segmented point on the global path during movement based on the global path, the specified starting point and the specified endpoint can be determined in the global path based on the current position of the robot.

[0107] The number and position of the plurality of preset segmented points can be determined according to the actual path condition of the global path and the actual demand of the robot during movement. For example, the number of the plurality of preset segmented points can be 8, 15, etc., which are all reasonable, and the embodiments of the present application do not limit the specific segmentation of the global path. In addition, the specified range can be a circular area centered on the preset segmented point, and the radius of the circular area can be set according to the actual path condition of the global path and the actual demand of the robot during movement.

[0108] In this way, during the control of the robot moving based on the global path, it can be detected whether the current position of the robot satisfies the preset optimization condition. When it is detected that the current position of the robot satisfies the preset optimization condition, the segmented trajectory optimization of the global path in the robot movement process can be triggered.

[0109] Generally, the movement path of the robot can include a starting point and an endpoint. Therefore, when the global path is segmented and trajectory optimized during the movement of the robot, the starting point and the endpoint of the path to be segmented and trajectory optimized in the global path can be determined first, that is, the specified starting point and the specified endpoint for segmented trajectory optimization can be determined in the global path.

[0110] Wherein, when determining the specified starting point and the specified ending point in the global path, the specified starting point and the specified ending point can be determined simultaneously, or the specified starting point can be determined first, and then the specified ending point is determined based on the specified starting point. Moreover, after the specified starting point is determined, when the specified ending point is determined, the point with a distance from the specified starting point satisfying a given distance can be taken as the specified ending point, or the specified ending point can be determined based on the specified starting point and the current speed of the robot, which are all reasonable, and the determination manner of the specified starting point and the specified ending point is not specifically limited in the embodiments of the present application.

[0111] Optionally, the global path can be divided into multiple path segments in advance, and then for each path segment, the corresponding specified starting point and specified ending point of the path segment can be set, so that when the current position of the robot satisfies the preset optimization condition, the specified starting point and the specified ending point corresponding to the path where the current position of the robot is located can be obtained.

[0112] Optionally, the point on the global path closest to the current position of the robot can be taken as the specified starting point, and then the point on the global path with a distance from the specified starting point equal to a given distance can be taken as the specified ending point. Wherein, the given distance can be 3 meters or 6 meters, which are all reasonable, and are not specifically limited in the embodiments of the present application.

[0113] Optionally, according to the actual path situation of the global path and the actual demand of the robot in the movement process, a plurality of representative position points can be selected in the global path, such as the midpoint between two adjacent inflection points, the midpoint between two adjacent points close to the obstacle, etc., so that the position point on the global path closest to the current position of the robot is taken as the specified starting point, and then the specified ending point is determined based on the specified starting point and the current speed of the robot. Wherein, the preset optimization distance can be 1 meter or 10 meters, which are all reasonable, and are not specifically limited in the embodiments of the present application.

[0114] In order to make the writing clear, the preset optimization condition and the execution manner of the step S101 will be illustrated in the following.

[0115] S102: Trajectory optimization is performed on the path between the specified starting point and the specified ending point in the global path to obtain a target path;

[0116] After the specified starting point and the specified ending point for segment trajectory optimization are determined in the global path based on the current position of the robot, a path between the specified starting point and the specified ending point in the global path can be determined as a path to be trajectory optimized, and then a trajectory optimization method can be used to perform trajectory optimization on the path to obtain a target path after trajectory optimization.

[0117] Optionally, when the trajectory optimization method provided by the embodiment of the present application is executed by a robot, the robot can determine a path between the specified starting point and the specified end point in the global path based on the specified starting point and the specified end point, and perform trajectory optimization on the path by using a trajectory optimization algorithm to obtain a target path.

[0118] Optionally, when the trajectory optimization method provided by the embodiment of the present application is executed by a server in communication with the robot, the server can determine a path between the specified starting point and the specified end point in the global path based on the specified starting point and the specified end point, and perform trajectory optimization on the path by using a trajectory optimization algorithm to obtain a target path, and send the target path to the robot.

[0119] The trajectory optimization method can be a potential field method, a gradient descent method, or other trajectory optimization methods, which are all reasonable and are not specifically limited in the embodiment of the present application.

[0120] Optionally, when the trajectory optimization is performed by using the potential field method, it can be assumed that there is a potential field between the specified starting point and the specified end point, which is a resultant force field formed by superimposing a repulsive force field assumed to be generated by a preset obstacle and an attractive force field assumed to be generated by the specified end point. When the robot moves into the potential field, the robot can be regarded as an object in the potential field. At this time, the robot will be subjected to a repulsive force from the preset obstacle and an attractive force from the specified end point, and the robot will move in the direction of the resultant force of the repulsive force and the attractive force and finally reach the specified end point.

[0121] The attractive force field function and the repulsive force field function can be represented as follows, respectively.

[0122] U A (X)=0.5ρ 2 (X,X L )

[0123]

[0124] U A (X) is the attractive force field potential energy; X is the coordinate of the current position of the robot; X L is the coordinate of the specified end point; ρ(X,X L ) is the distance from the specified end point to the robot; U B (X) is the repulsive force field potential energy; σ is the repulsive force field gain coefficient; X z is the position coordinate of the preset obstacle; ρ(X,X z ) is the distance from the preset obstacle to the robot; ρ0 is the repulsive force action range of the preset obstacle.

[0125] The repulsive field potential is greater when the robot is closer to the preset obstacle, and is smaller when the robot is farther away from the preset obstacle; when the shortest distance between the robot and the preset obstacle is greater than the influence distance of the preset obstacle, the repulsive field potential is 0.

[0126] The force acting on the robot can be obtained by taking the negative gradient of the attractive field function and the repulsive field function:

[0127]

[0128]

[0129] wherein, is the attractive force acting on the robot; and ε is the attractive field gain coefficient; is the repulsive force acting on the robot.

[0130] The attractive force acting on the robot monotonically increases linearly with the decrease of the distance ρ(X, X L ) between the robot and the specified end point, and the direction of the attractive force is from the robot to the specified end point; the repulsive force acting on the robot increases with the decrease of the distance ρ(X, X z ) between the robot and the preset obstacle, and the direction of the repulsive force is from the preset obstacle to the robot, when the distance between the robot and the preset obstacle is greater than the repulsive force action range of the preset obstacle, the repulsive force acting on the robot is 0.

[0131] The resultant force acting on the robot at any time t is:

[0132]

[0133] After the resultant force acting on the robot is calculated by the above equation, the moving direction of the robot is determined by the direction of the resultant force, and then the robot moves in the potential field according to the direction of the resultant force, so that a relatively smooth moving trajectory can be obtained.

[0134] Optionally, when the gradient descent method is used for trajectory optimization, the path between the specified start point and the specified end point on the global path is taken as the original path, the direction from the specified start point to the specified midpoint is set as the negative direction of the gradient, and the optimization step and the error threshold are set according to the actual needs of the robot. Then, based on the optimization step, a plurality of optimization nodes are determined, and an optimization path that satisfies the minimum deviation of the optimized path from the original path, the minimum deviation between adjacent nodes in the optimized path and the gradient descent value less than the error threshold is solved by an algorithm, and then a smooth path with the specified start point as the start point and the specified end point as the end point and reducing the turning points and large-angle turns is obtained.

[0135] S103: controlling the robot to move according to the target path, and returning to the step S101 when the preset optimization condition is met at the current position of the robot, until the robot moves to the preset end point of the global path.

[0136] After the path optimization in the global path between the specified start point and the specified end point is performed to obtain the target path, the robot can be controlled to move according to the target path. During the movement of the robot according to the target path, it is detected whether the preset optimization condition for triggering the path optimization is met again at the current position of the robot. When it is detected that the preset optimization condition is met again at the current position of the robot, the path optimization in the movement of the robot can be triggered again. In this way, when the robot moves to the position meeting the preset optimization condition again, the new target path can be obtained again according to the steps S101-S102, and the robot can be controlled to move according to the obtained target path again, and the cycle is repeated until the robot moves to the preset end point of the global path.

[0137] As can be seen from the above, when it is desired to move the robot from the preset start point to the preset end point, the global path from the preset start point to the preset end point can be obtained after the global path planning is performed. During the movement of the robot based on the global path, the global path can be segmented and path-optimized according to the current position of the robot, so as to realize the path optimization of the global path. Then, the robot can move according to the global path after the segmented path optimization during the movement based on the global path. In this way, since each target path obtained by the segmented path optimization can be smooth and can avoid the preset obstacle, the global path for the robot to move from the preset start point to the preset end point can be smooth and can avoid the preset obstacle. In this way, the robot can be prevented from slowing down due to the problems such as too many inflection points in the global path, the unsmooth path in the global path, and the collision with the preset obstacle, and the moving efficiency of the robot during the movement can be improved.

[0138] Next, the specific implementation manner of determining the specified start point and the specified end point in the global path based on the current position of the robot when the current position of the robot meets the preset optimization condition during the movement of the robot based on the preset global path is described by way of example.

[0139] In the process that the robot moves based on the global path, the global path can be segmented and trajectory-optimized based on a preset period, so that when the robot moves for a time length corresponding to the preset period, it is determined that the current position of the robot satisfies a preset optimization condition. The global path can also be segmented and trajectory-optimized based on a preset moving distance, so that when the robot moves a preset distance from the preset starting point, it is determined that the current position of the robot satisfies the preset optimization condition. Both are reasonable and are not specifically limited in the embodiments of the present application.

[0140] Optionally, in a specific implementation, the step S101 can include steps 11-12.

[0141] Step 11: When the robot is located at a preset starting point of a preset global path, the specified starting point and the specified ending point are determined in the global path based on the preset starting point.

[0142] Step 12: In the process that the robot moves based on the target path, when the robot moves to the path midpoint of the target path, the specified starting point and the specified ending point are determined in the global path based on the current position of the robot.

[0143] In this specific implementation, in the process that the robot moves based on the preset global path, the global path can be segmented and trajectory-optimized from the preset starting point. Then, when it is determined that the current position of the robot satisfies the preset optimization condition when the robot is located at the preset starting point of the global path, the specified starting point and the specified ending point for the segmented trajectory optimization can be determined in the global path based on the preset starting point.

[0144] That is, when the global path is segmented and trajectory-optimized, the preset starting point of the global path can be used as the specified starting point for the segmented trajectory optimization. Then, based on the specified starting point, the specified ending point for the segmented trajectory optimization can be determined in the global path.

[0145] When the robot moves based on the global path, the robot can move according to the target path obtained by trajectory optimization. Thus, when the robot moves to the path midpoint of the target path, the specified starting point and the specified ending point for the trajectory optimization can be determined in the global path based on the current position of the robot.

[0146] Optionally, since the robot may deviate from the target path during movement along the target path, the robot may approach the target path but not move along the target path, at which time the robot may pass a point near the midpoint of the target path but not pass the midpoint of the target path. Based on this, a range can be set with the midpoint of the target path as the center, and when the robot reaches the range, it can be considered that the robot has moved to the path center of the target path. At this time, it can be determined that the current position of the robot satisfies the preset optimization condition, and the specified start point and the specified end point for trajectory optimization can be further determined in the global path based on the current position of the robot. The range with the midpoint of the target path as the center can be a circle with the midpoint of the target path as the center and a given distance as the radius, or a square with the midpoint of the target path as the center and a diagonal of 10 meters, which are both reasonable and are not specifically limited in the present application.

[0147] Optionally, in one specific implementation, the step S101 can include steps 21-22:

[0148] Step 21: When the robot is located at the preset start point of the preset global path, determining the specified start point and the specified end point in the global path based on the preset start point;

[0149] The same as step 11, step 21 will not be described here.

[0150] Step 22: During movement of the robot based on the global path, when the robot moves for a time corresponding to a preset period each time, determining the specified start point and the specified end point in the global path based on the current position of the robot.

[0151] In the present specific implementation, during movement of the robot based on the global path, when the robot moves for a time corresponding to a preset period each time, it is determined that the current position of the robot satisfies the preset optimization condition, trajectory optimization can be performed, and then the specified start point and the specified end point for trajectory optimization can be determined in the global path based on the current position of the robot.

[0152] The preset period can be limited according to the movement requirements of the robot in actual applications, for example, it can be 30 seconds as a preset period, or it can be 2 minutes as a preset period, which are both reasonable and are not specifically limited in the present application. For example, when 30 seconds is taken as a preset period, when the robot moves for 30 seconds based on the global path, it can be determined that the current position of the robot satisfies the preset optimization condition, and trajectory optimization can be performed once, that is, when the robot moves for 30 seconds, the specified start point and the specified end point for trajectory optimization can be determined in the global path based on the current position of the robot.

[0153] In the process that the robot moves based on the global path, when the current position of the robot satisfies a preset optimization condition, the global path can be segmented trajectory optimized based on the current position of the robot. Before the segmented trajectory optimization, a specified starting point and a specified ending point for trajectory optimization need to be determined, and then a path to be trajectory optimized is determined in the global path based on the determined specified starting point and specified ending point.

[0154] Optionally, in an embodiment, the step S101 of determining the specified starting point and the specified ending point in the global path based on the current position of the robot can include steps 31-32:

[0155] Step 31: determining a point in the global path with the minimum distance from the current position of the robot as the specified starting point;

[0156] Step 32: determining the specified ending point in the global path based on the specified starting point.

[0157] In this embodiment, in the process that the robot moves based on the global path, when the current position of the robot satisfies a preset optimization condition, the current position of the robot can be determined, and then a point in the global path with the minimum distance from the current position of the robot is determined as the specified starting point for trajectory optimization. Then, based on the specified starting point, a specified ending point for trajectory optimization can be determined in the global path.

[0158] As shown in FIG. 2(a), where R is a robot, S is a preset starting point, F is a preset ending point, and a straight line is a global path, the robot moves from the preset starting point S to the preset ending point F based on the global path. When the current position of the robot satisfies a preset optimization condition, a point Q in the global path with the minimum distance from the current position of the robot can be determined as the specified starting point.

[0159] The point in the global path with the distance from the specified starting point satisfying a preset optimization distance condition can be determined as the specified ending point. The preset optimization distance condition can be equal to a target distance, can be greater than the target distance and closest to the target distance, or can be less than the target distance and closest to the target distance, which are all reasonable and are not specifically limited in the embodiments of the present application. In addition, the target distance can be calculated based on the current speed of the robot or can be a preset specific value, for example, 10 meters is preset as the target distance between the specified starting point and the specified ending point, which are all reasonable and are not specifically limited in the embodiments of the present application.

[0160] Optionally, in an embodiment, the step 32 can include steps 321-323:

[0161] Step 321: determining a target distance based on the current speed of the robot;

[0162] Step 322: if the distance between the specified starting point and the preset ending point of the global path is not greater than the target distance, taking the preset ending point of the global path as the specified ending point;

[0163] Step 323: if the distance between the specified starting point and the preset ending point of the global path is greater than the target distance, determining the specified point in the global path as the specified ending point;

[0164] The specified point includes: a point with a reference distance closest to the target distance between the specified starting point, a point with a reference distance less than the target distance and closest to the target distance, or a point with a reference distance greater than the target distance and closest to the target distance.

[0165] In the specific implementation, during movement of the robot based on the global path, when the current position of the robot satisfies a preset optimization condition, a specified starting point for segment trajectory optimization in the global path can be determined, then the current speed of the robot can be obtained, and then based on the current speed, a target distance for determining a specified ending point in the global path can be determined.

[0166] When the target distance is determined based on the current speed of the robot, a product of the current speed of the robot and a first preset parameter can be calculated, the product and a second preset sum value are taken as the target distance; or a sum of the robot and a third preset parameter can be calculated, and the sum is taken as the target distance; or a monomial quadratic equation system of the current speed can be calculated, and the result is taken as the target distance, which are all reasonable and are not specifically limited in the embodiment of the application.

[0167] Optionally, the target distance can be calculated using the following formula:

[0168]

[0169] Wherein, d C is the target distance; k, d min are preset parameters, which can be set according to actual needs; v t is the current speed of the robot.

[0170] When the global path is subjected to segment trajectory optimization based on the current position of the robot, the specified starting point of the first path to be subjected to trajectory optimization in the global path can be a preset starting point of the global path, at this time, the current position of the robot is close to the preset starting point, and the current speed of the robot can be 0, and then the formula group can be simplified as follows:

[0171] d C=k·v t +d min

[0172] At the robot's current speed v t When the value is 0, the distance to the target is d. min .

[0173] After determining the target distance, you can compare whether the distance between the specified starting point and the preset endpoint of the global path is not greater than the target distance.

[0174] Specifically, when the distance between the specified starting point and the preset endpoint of the global path is not greater than the target distance, since the preset endpoint is the last point in the global path, the distance between the preset endpoint and the specified starting point is the farthest among all points on the global path before trajectory optimization. Therefore, the distance between the specified starting point and the preset endpoint is closest to the target distance. Based on this, the preset endpoint can be used as the specified endpoint for trajectory optimization.

[0175] As shown in Figure 2(b), R represents the robot, S is the preset starting point, F is the preset ending point, and the straight line is the global path. The robot moves from the preset starting point S to the preset ending point F based on the global path. When the robot's current position meets the preset optimization conditions, the point H with the smallest distance from the robot's current position in the global path can be determined as the designated starting point. The distance between H and h is the target distance determined based on the robot's current speed. At this time, the distance between H and the preset ending point F is less than the target distance, so F is taken as the designated ending point.

[0176] When the distance between the specified starting point and the preset ending point of the global path is greater than the target distance mentioned above, a specified point can be determined in the global path and used as the specified ending point.

[0177] The specified point may include: a point whose reference distance from the specified starting point is closest to the target distance, a point whose reference distance is less than the target distance but is closest to the target distance, or a point whose reference distance is greater than the target distance and is closest to the target distance.

[0178] In other words, the point corresponding to the reference distance closest to the target distance can be used as the designated endpoint; the point corresponding to the reference distance closest to the target distance but less than the target distance can also be used as the designated endpoint; and the point corresponding to the reference distance closest to the target distance but greater than the target distance can also be used as the designated endpoint.

[0179] As shown in FIG. 2(c), where R is a robot, S is a preset starting point, F is a preset ending point, and a straight line is a global path, the robot moves from the preset starting point S to the preset ending point F based on the global path, and when a current position of the robot satisfies a preset optimization condition, a point H in the global path that has a minimum distance from the current position of the robot can be determined as a specified starting point. A distance between H and h is a target distance determined based on a current speed of the robot, and at this time, a distance between H and the preset ending point F is greater than the target distance.

[0180] When a point in the global path that is closest to the target distance is taken as the specified ending point, the O point is the specified ending point.

[0181] When a point in the global path that is less than the target distance and closest to the target distance is taken as the specified ending point, the O point is the target point.

[0182] When a point in the global path that is greater than the target distance and closest to the target distance is taken as the specified ending point, the V point is the target point.

[0183] When a distance between the specified starting point and the preset ending point of the global path is greater than the target distance, a specified point in the global path can be taken as the specified ending point, at this time, a distance between the specified starting point and the specified point is less than a distance between the specified starting point and the preset ending point of the global path, but the specified point can be close to the preset ending point, at this time, the distance between the specified starting point and the specified point is close to the distance between the specified starting point and the preset ending point, after the specified point is taken as the specified ending point of the path to be optimized, a remaining path in the global path can be short, at this time, in order to save computing resources and improve the efficiency of trajectory optimization, a path between the specified point and the preset ending point can be incorporated into the path to be segmented and optimized, that is, the preset ending point is taken as the specified ending point of the path to be segmented and optimized.

[0184] Optionally, in a specific implementation, before the specified point in the global path is determined as the specified ending point in step 323, steps 323A-323C are further included:

[0185] Step 323A: determining whether a distance between the specified point and the preset ending point of the global path is not greater than a preset distance threshold; if yes, step 323B is performed; otherwise, step 323C is performed.

[0186] Step 323B: determining the preset ending point of the global path as the specified ending point.

[0187] Step 323C: determining the specified point in the global path as the specified ending point.

[0188] In the specific implementation, when the distance between the specified starting point and the preset ending point of the global path is greater than the target distance, the specified point in the global path can be determined based on the specified starting point and the target distance. Then, before the specified point is determined as the specified ending point, it can be determined whether the distance between the specified point and the preset ending point of the global path is not greater than the preset distance threshold.

[0189] When the distance between the specified point and the preset ending point of the global path is not greater than the preset distance threshold, it can be considered that the specified point is close to the preset ending point of the global path. At this time, the preset ending point can be determined as the specified ending point, and the remaining global path with the specified starting point as the starting point and the preset ending point as the ending point is regarded as the last path to be optimized in trajectory. Then, the trajectory optimization is performed.

[0190] When the distance between the specified point and the preset ending point of the global path is greater than the preset distance threshold, it can be considered that the specified point is still a certain distance away from the preset ending point of the global path. At this time, the specified point can be determined as the specified ending point.

[0191] The distance threshold can be 30 cm, 1 m, or 30 m, which are all reasonable and can be set according to actual needs, and the embodiments of the present application are not limited in this regard.

[0192] As shown in FIG. 2(d), R is the current position of the robot, S is the preset starting point, F is the preset ending point, and the straight line is the global path. The robot moves from the preset starting point S to the preset ending point F based on the global path. When the current position of the robot meets the preset optimization condition, the point H with the minimum distance to the current position of the robot in the global path can be determined as the specified starting point. The distance between H and h is the target distance determined based on the current speed of the robot. At this time, the distance between H and the preset ending point F is greater than the target distance. When the point O corresponding to the reference distance closest to the target distance in the global path is determined as the specified point, the distance between the preset specified point O and the point z is the preset distance threshold. At this time, the distance between the specified point O and the preset ending point F is greater than the preset distance threshold. The specified point O is determined as the specified ending point.

[0193] As shown in FIG. 2(e), R represents the current position of the robot, S represents the preset starting point, F represents the preset ending point, and the straight line represents the global path. The robot moves from the preset starting point S to the preset ending point F based on the global path. When the current position of the robot meets the preset optimization condition, the point H with the minimum distance to the current position of the robot in the global path can be determined as the designated starting point. The distance between H and h is the target distance determined based on the current speed of the robot. At this time, the distance between H and the preset ending point F is greater than the target distance. When the point O corresponding to the reference distance closest to the target distance in the global path is taken as the designated point, the distance between the preset designated point O and the point z is the preset distance threshold. At this time, the distance between the designated point O and the preset ending point F is less than the preset distance threshold, and the preset ending point F is taken as the designated ending point.

[0194] In the process of controlling the robot to move according to the target path, a new obstacle that does not exist in the map used to plan the global path can appear on the target path. At this time, in order to enable the robot to avoid the new obstacle, local path optimization can be performed according to the current position of the robot.

[0195] Optionally, in an embodiment, as shown in FIG. 3, the step S103 of controlling the robot to move according to the target path can include the following steps S301-S305: Figure 3

[0196] S301: When the current position of the robot meets the preset planning condition, a calibration distance for path planning is determined.

[0197] In this embodiment, in the process of controlling the robot to move according to the target path, in order to enable the robot to be re-planned for local path planning during the movement of the robot based on the target path, a preset planning condition for triggering the local path planning during the movement of the robot can be set in advance.

[0198] In this way, in the process of controlling the robot to move according to the target path, when the robot moves to each position, it can be detected whether the current position of the robot meets the preset planning condition. Further, when it is detected that the current position of the robot meets the preset planning condition, the local path planning during the movement of the robot can be triggered, so that the calibration distance for local path planning can be determined.

[0199] The preset optimization condition can be that the robot moves for a time corresponding to a preset planning period, or that the robot detects a new obstacle, which are both reasonable and are not limited in the embodiment.

[0200] ​That is, the local path planning can be periodically performed in the process that the robot moves according to the target path, so that the current position of the robot is determined to satisfy the preset planning condition when the robot moves for a length of time corresponding to a preset planning period; or the local path planning can be performed when the robot detects a new obstacle, so that the current position of the robot is determined to satisfy the preset planning condition when the robot detects the new obstacle, which is reasonable.

[0201] Optionally, in the process that the robot moves according to the target path, a calibration distance for path planning is determined when the robot moves for a length of time corresponding to a preset planning period.

[0202] In the process that the robot moves according to the target path, the local path planning can be periodically performed, that is, the robot can be planned a path once when the robot moves for a length of time corresponding to a preset planning period. Therefore, in the process that the robot moves according to the target path, a calibration distance for path planning can be determined when the robot moves for a length of time corresponding to a preset planning period.

[0203] The preset planning period can be limited according to the movement demand of the robot in actual application, for example, it can be 30 seconds as a preset planning period, or it can be 2 minutes as a preset planning period, which is reasonable and is not limited in the embodiment of the application. For example, when 30 seconds is taken as a preset planning period, the robot can be planned a path once when the robot moves for 30 seconds, that is, a calibration distance for path planning can be determined when the robot moves for 30 seconds.

[0204] Optionally, in the process that the robot moves according to the target path, a calibration distance for path planning is determined when the robot detects a new obstacle;

[0205] The robot can detect obstacles on the front path in real time during movement, so that the local path planning can be performed when the robot detects a new obstacle.

[0206] In the global path planning, the map for global path planning includes the currently known obstacles, so that the global path planned can avoid the obstacles appearing on the map, but when the robot moves according to the target path, a new obstacle that does not appear on the map can appear on the target path. Since the global path has not planned the new obstacle, the target path obtained by performing the segment trajectory optimization on the global path also does not plan the new obstacle. Therefore, when the robot moves according to the target path, the new obstacle cannot be avoided.

[0207] Therefore, during the movement of the robot along the target path, when the robot detects a new obstacle in the front path, the robot can be locally path planned. Before path planning, the calibration distance for local path planning can be determined first.

[0208] In addition, the calibration distance for path planning can be a specified distance, for example, when it is detected that the current position of the robot satisfies the specified condition, the specified distance is taken as the calibration distance; or the calibration distance can be a distance calculated by using the current moving speed of the robot, for example, when it is detected that the current position of the robot satisfies the specified condition, the calibration distance is determined by using the current moving speed of the robot.

[0209] Optionally, the calibration distance for path planning is determined by using the current moving speed of the robot. When the robot moves along the target path, when the current position of the robot satisfies the preset planning condition, the current moving speed of the robot can be determined first, and then the calibration distance for path planning is determined by using the current moving speed of the robot.

[0210] For example, the product of the current moving speed and the time change amount can be calculated, and the sum of the product and a preset parameter is taken as the calibration distance.

[0211] Optionally, the calibration distance can be calculated by using the following formula:

[0212] d t =k1·v t 2 +k2·v t +b

[0213] wherein d t is the calibration distance; k1, k2 and b are preset parameters, which can be set according to actual needs; v t is the current moving speed of the robot.

[0214] The k1, k2 and b are known quantities, the v t is a known quantity, and the d t is an unknown quantity. Thus, by analyzing the formula, the specific value of d t can be solved, and thus the specific value of d t is the calibration distance.

[0215] S302: determining a target point for local path planning from the global path based on the calibration distance and the current position of the robot;

[0216] Generally, the candidate path of the robot can include a start point and an end point. Thus, the local path planning can be performed with the current position of the robot as the start point, and in order to make the planned local path close to the global path, the end point of the planned local path can be as close as possible to the global path, and thus, the end point of the local path can be set on the global path. Meanwhile, in order to avoid the newly added obstacle and improve the obstacle avoidance capability of the robot during movement, the end point of the global path cannot be directly determined as the end point of the local path. Based on this, the calibration distance can be used as a reference factor for the distance between the start point and the end point of the planned local path, and thus, based on the calibration distance, the path point on the global path that is the end point of the local path can be determined.

[0217] In this way, after the calibration distance is determined, the target point for path planning can be determined from the plurality of path points included in the global path based on the calibration distance and the current position of the robot.

[0218] For example, the path point in the global path that has a distance equal to the calibration distance from the current position of the robot can be determined as the target point.

[0219] However, the path points in the global path can be discrete, and thus, among the path points in the global path, there can be no path point that has a distance equal to the calibration distance from the current position of the robot. Based on this, the distances between the path points in the global path and the current position of the robot can be determined, and thus, according to the relationship between the determined distances and the calibration distance, the target point can be selected.

[0220] Optionally, when the calibration distance is not less than a first distance between the current position and a preset end point, the preset end point is determined as the target point; when the calibration distance is less than a target distance, the path point in the global path that satisfies a preset planning distance condition is determined as the target point; wherein the preset planning distance condition includes any one of the following conditions:

[0221] the second distance from the current position is closest to the calibration distance, the second distance from the current position is not greater than the calibration distance and is closest to the calibration distance, and the second distance from the current position is not less than the calibration distance and is closest to the calibration distance.

[0222] In the specific implementation manner, the distance between the current position of the robot and the preset end point can be determined as the first distance, and the distances between the current position and the path points can be determined as the second distances. In this way, when the target point for local path planning is determined, the numerical relationship between the calibration distance and the first distance can be determined first.

[0223] When the calibration distance is not less than the first distance, the distance from the robot's current position to the preset endpoint is not greater than the calibration distance. Since the preset endpoint is the last path point in the global path, the preset endpoint is the farthest from the robot's current position among the path points that the robot has not passed in the global path. Therefore, the distance between the robot's current position and the preset endpoint is closest to the calibration distance. Based on this, the preset endpoint can be used as the target point for local path planning.

[0224] When the calibration distance is less than the first distance, the distance from the robot's current position to the preset endpoint is greater than the calibration distance. After the robot has traveled the calibration distance, it still cannot reach the preset endpoint. Based on this, a path point that meets the preset planning distance condition can be selected in the global path as the target point for local path planning.

[0225] The preset planning distance condition can be any one of the following: the second distance from the current location is closest to the calibration distance; the second distance from the current location is not greater than the calibration distance and is closest to the calibration distance; or the second distance from the current location is not less than the calibration distance and is closest to the calibration distance.

[0226] In other words, the path point corresponding to the second distance closest to the calibration distance can be used as the target point; the path point corresponding to the second distance that is not greater than the calibration distance and is closest to the calibration distance can also be used as the target point; and the path point corresponding to the second distance that is not less than the calibration distance and is closest to the calibration distance can also be used as the target point.

[0227] For example, such as Figure 4 As shown, assume that: the straight line is the global path, the robot moves towards point d, points a and ad are path points on the global path, d is the end point of the global path, point A is the current position of the robot, and the distance between point A and point B is the calibration distance, and the distance between point e and point B is less than the distance between point B and point C.

[0228] When the path point corresponding to the second distance closest to the calibration distance is taken as the target point, then point e is the target point;

[0229] When the path point corresponding to the second distance that is no greater than the calibration distance and is closest to the calibration distance is taken as the target point, then point e is the target point;

[0230] When the path point corresponding to the second distance that is not less than the calibration distance and is closest to the calibration distance is taken as the target point, then point c is the target point.

[0231] S303: Select multiple candidate movement speeds from the preset speed range corresponding to the current time, and determine the candidate path corresponding to each candidate movement speed;

[0232] After determining the calibration distance for local path planning and determining the target point for local path planning based on the calibration distance, a preset speed range corresponding to the current time can be determined, so that a plurality of groups of candidate moving speeds can be selected in the determined preset speed range, and then for each group of candidate moving speeds, a moving track that can be formed when the robot moves at the group of candidate moving speeds can be determined based on the motion model of the robot, and the moving track is taken as a candidate path corresponding to the group of candidate moving speeds.

[0233] The preset speed range can be a preset speed range of the robot, a speed range determined based on the mechanical performance of the robot, or a speed range determined based on the current moving speed of the robot and the distance between the robot and the obstacle, which are all reasonable and are not specifically limited in the embodiments of the present application.

[0234] When selecting a plurality of groups of candidate moving speeds, a plurality of groups of candidate moving speeds can be randomly selected from the preset speed range corresponding to the current time, a plurality of groups of candidate moving speeds can be selected from the preset speed range corresponding to the current time according to a preset sampling interval, or a plurality of groups of candidate moving speeds that can make the robot have specific motion characteristics can be selected from the preset speed range corresponding to the current time, which are all reasonable and are not specifically limited in the embodiments of the present application.

[0235] Optionally, when selecting a plurality of groups of candidate moving speeds from the preset range corresponding to the current time, the preset speed range can include at least one of a first speed range, a second speed range, and a third speed range.

[0236] That is, the preset speed range can be the first speed range, the second speed range, or the third speed range, i.e., the plurality of groups of candidate moving speeds satisfy the first speed range, the second speed range, or the third speed range; the preset speed range can be the first speed range and the second speed range, i.e., the plurality of groups of candidate moving speeds satisfy the first speed range and the second speed range; the preset speed range can be the first speed range and the third speed range, i.e., the plurality of groups of candidate moving speeds satisfy the first speed range and the third speed range; the preset speed range can be the second speed range and the third speed range, i.e., the plurality of groups of candidate moving speeds satisfy the second speed range and the third speed range; and the preset speed range can be the first speed range, the second speed range, and the third speed range, i.e., the plurality of groups of candidate moving speeds satisfy the first speed range, the second speed range, and the third speed range.

[0237] The first speed range is a speed range constituted by a first maximum speed and a first minimum speed that the robot can reach. The robot itself has a first maximum speed and a first minimum speed that it can reach, or in the application of the robot, the first maximum speed and the first minimum speed that the robot can reach can be preset according to actual needs, and then the speed range constituted by the first maximum speed and the first minimum speed is taken as the first speed range.

[0238] For example, the first maximum speed can be 0 m / s, 10 m / s or 50 m / s, and the first minimum speed can be 0 m / s, which are all reasonable, and the embodiments of the present application are not limited specifically.

[0239] Optionally, the robot can rotate and move longitudinally, and cannot move laterally, so that the current moving speed of the robot includes two speed components of linear speed and angular speed. Thus, the first maximum linear speed, the first minimum linear speed, the first maximum angular speed and the first minimum angular speed that the robot can reach can be preset, so that the first speed range of the robot can be:

[0240] v i ∈[v min , v max ], ω i ∈[ω min , ω max ]

[0241] v max is the first maximum linear speed; v min is the first minimum linear speed; ω max is the first maximum angular speed; and ω min is the first minimum angular speed.

[0242] The second speed range is a speed range constituted by a second maximum speed and a second minimum speed determined based on the current moving speed of the robot, the preset maximum acceleration of the robot, the preset maximum deceleration of the robot and the preset speed change duration. Due to the motor torque limitation of the robot, the robot has a maximum acceleration and a maximum deceleration that it can reach. The maximum acceleration and the minimum deceleration can be set according to the motor torque of the robot, and the embodiments of the present application are not limited specifically.

[0243] Based on the current moving speed of the robot, the maximum acceleration and the maximum deceleration, the second maximum speed and the second minimum speed that the robot can reach after a preset speed change duration at the current time can be determined. Then, the speed range composed of the second maximum speed and the second minimum speed is taken as the second speed range. The preset speed change duration can be 5 seconds, 10 seconds or 1 minute, which are all reasonable, and the embodiment of the application does not make specific limitation.

[0244] Optionally, the robot can rotate and move longitudinally, and cannot move laterally, so that the current moving speed of the robot includes two speed components of linear speed and angular speed. The preset second speed range can be:

[0245] v i ∈[v t -a max ·Δt,v t +a max ·Δt],ω i ∈[ω t -α max ·Δt,ω t +α max ·Δt]

[0246] wherein a max is the preset maximum linear acceleration of the robot; -a max is the preset maximum linear deceleration of the robot; v t is the current moving linear speed of the robot; ω t is the current moving angular speed of the robot; α max is the preset maximum angular acceleration of the robot; -α max is the preset maximum angular deceleration of the robot; and Δt is the preset speed change duration.

[0247] The third speed range is a speed range composed of the first minimum speed and the third maximum speed determined based on the distance between the robot and the newly added obstacle and the maximum deceleration. When the robot detects an obstacle that does not appear in the map used to determine the global path, the distance between the robot and the newly added obstacle can be determined as the maximum braking distance first. In order to avoid the robot colliding with the newly added obstacle, the maximum speed that the robot can reach at the current position can be such that the robot starts to decelerate at the maximum deceleration and the speed is zero when moving the maximum braking distance, so that the maximum speed that the robot can reach at the current position, i.e., the third maximum speed, can be determined based on the maximum deceleration of the robot and the maximum braking distance.

[0248] Optionally, the robot can rotate and move longitudinally, and cannot move laterally, so that the current moving speed of the robot includes two speed components of linear speed and angular speed. The preset third speed range can be:

[0249]

[0250] wherein D is the distance between the robot and the newly added obstacle, and is the maximum braking distance, |-a max is the absolute value of the maximum linear acceleration, and |-a max is the absolute value of the maximum angular acceleration.

[0251] Moreover, the moving trajectory corresponding to different candidate moving speeds can be different, and thus the candidate path corresponding to different candidate moving speeds can be different.

[0252] In addition, the robot can be an omnidirectional robot, or can be a robot that can rotate and move longitudinally, and cannot move laterally. Thus, when the type of the robot is different, the motion model corresponding to the robot is also different, and thus the current moving speed of the robot is also different, so that the moving trajectories of the multiple groups of candidate moving speeds determined based on the motion model are also different.

[0253] For example, when the robot is an omnidirectional robot, the moving speed of the robot includes lateral linear speed V1, longitudinal linear speed V2 and angular speed ω 全 When the robot is a robot that can rotate and move longitudinally, and cannot move laterally, the moving speed of the robot includes longitudinal linear speed v and angular speed ω.

[0254] For example, as shown in FIG. 1, Figure 5 the black circular object in the figure is a robot that can rotate and move longitudinally, and cannot move laterally, and the moving speed of the robot includes linear speed v and angular speed ω. When multiple groups of candidate moving speeds (v, ω) are selected, multiple groups of candidate moving speeds (v, ω) can be selected from the preset speed range corresponding to the current time, and the moving trajectory of each group of candidate moving speeds is determined based on the corresponding motion model, as the candidate path corresponding to the group of candidate linear speed and angular speed. The preset speed range includes the speed range of linear speed v and the speed range of angular speed ω.

[0255] For the i-th group of candidate moving speeds (v i , ω i ), based on the kinematic model of the robot, the displacement of the robot within T time can be represented by the following formula, wherein the formula is:

[0256]

[0257] wherein xi , y i is the position of the robot in the coordinate system after the robot moves for T time at the candidate moving speed (v i , ω i ); x t , y t is the current position of the robot in the coordinate system; v i is a candidate linear speed in the ith set of candidate moving speeds; ω i is a candidate angular speed in the ith set of candidate moving speeds; θ t is the current rotation angle of the robot; θ i is the rotation angle of the robot after the robot moves for T time at the candidate moving speed (v i , ω i ), 1≤i≤n, and n is the total number of sets of candidate moving speeds.

[0258] Further, the candidate moving trajectories corresponding to different candidate moving angles (v i , ω i ) in each set are also different. In the figure, trajectories 1-7 are candidate paths 1-7 corresponding to the first set to the seventh set of candidate moving speeds, respectively.

[0259] S304: Determine the target evaluation score of each candidate path, and determine the candidate path whose determined target evaluation score meets a preset evaluation condition as a local path.

[0260] The target evaluation score of each candidate path is used to represent the degree of deviation of the candidate path relative to the target point, the degree of closeness of the candidate path to the global path, and the score of the obstacle avoidance ability.

[0261] When selecting a path from the multiple candidate paths as a local path, a preset evaluation condition can be set, and then the candidate path that meets the preset evaluation condition is selected as the local path.

[0262] After determining the candidate path corresponding to each set of candidate moving speeds, the target evaluation score for evaluating the degree of deviation of each candidate path relative to the target point, the degree of closeness to the global path, and the obstacle avoidance ability can be determined, and then the candidate path whose determined target evaluation score meets a preset evaluation condition is determined as a local path.

[0263] The preset evaluation condition can be that the target evaluation score is the largest, or the target evaluation score is closest to the average of the target evaluation scores of the multiple candidate paths, or the score value representing the degree of closeness of each candidate path to the global path is the highest, all of which are reasonable and are not specifically limited in the embodiments of the present application.

[0264] Optionally, the candidate path with the highest target evaluation score can be identified as a local path.

[0265] In this specific implementation, for each candidate path, the target evaluation score of the candidate path can be determined. Then, the candidate path with the highest target evaluation score can be determined as the local path.

[0266] Optionally, for each candidate path, a target evaluation score can be determined for that candidate path. Then, the average of the target evaluation scores of multiple candidate paths can be calculated, and the candidate path whose target evaluation score is closest to the average value can be determined as a local path.

[0267] Optionally, for each candidate path, a target evaluation score can be determined. Then, using the target evaluation score, a score representing the closeness of each candidate path to the global path can be determined, and the candidate path with the highest score can be determined as the local path.

[0268] Optionally, for each candidate path, a target evaluation score can be determined. Then, using the target evaluation score, a score representing the obstacle avoidance capability of each candidate path can be determined, and the candidate path with the highest score can be determined as the local path.

[0269] As the robot moves along the target path, local path planning can be triggered when its current position meets preset planning conditions. Thus, as the robot moves along the planned local path, it may move again to a position that meets the specified conditions, triggering local path planning again. This cycle continues until the robot meets the preset stop path planning conditions. For example, the robot may move to a preset endpoint or receive a stop path planning command.

[0270] like Figure 5 As shown, after selecting the first to seventh groups of candidate movement speeds, candidate paths 1-7 corresponding to the first to seventh groups of candidate movement speeds are determined. Then, the target evaluation score of each of the above candidate paths is determined, and the candidate path 4 with the highest target evaluation score is determined as the local path.

[0271] S305: Control the robot to move along the local path according to the candidate moving speed corresponding to the local path. When the robot's current position meets the preset planning conditions, return to step S301 to determine the calibration distance used for path planning.

[0272] In the specific implementation, after determining the local path, the robot can move along the local path according to the candidate moving speed corresponding to the local path. Further, in the process of moving of the robot, the robot can move to a position satisfying the specified condition again, and then the local path planning is triggered again. In this way, when the robot can move to a position satisfying the specified condition again, the steps S301-S304 can be repeated again until the robot satisfies the preset condition of stopping the path planning.

[0273] Optionally, when the robot moves to the preset terminal point, the local path planning for the robot is stopped.

[0274] Optionally, when the instruction of stopping the path planning is received, the local path planning for the robot is stopped.

[0275] Optionally, when it is detected that the robot moves to another global path, the steps S301-S304 can be repeated on the other global path to perform the local path planning for the robot.

[0276] Optionally, in a specific implementation, the step S304 of determining the target evaluation score of each candidate path can include the step 41.

[0277] The step 41 comprises: calculating, for each candidate path, an initial evaluation score of the candidate path with respect to each evaluation function by using the preset evaluation function, and calculating a target evaluation score of the candidate path based on the initial evaluation scores.

[0278] The evaluation functions include: a first type of evaluation function about the offset degree of the path offset relative to the target point, a second type of evaluation function about the path closeness degree, and a third type of evaluation function about the obstacle avoidance ability. The first type of evaluation function includes an azimuth angle evaluation function. The second type of evaluation function includes a path following evaluation function. The third type of evaluation function includes an obstacle distance evaluation function.

[0279] In the specific implementation, after a plurality of groups of candidate moving speeds are selected and a candidate path corresponding to each group of candidate moving speeds is determined, an initial evaluation score of the candidate path with respect to each evaluation function can be calculated based on a preset target evaluation function, and then a target evaluation score of the candidate path can be calculated based on the initial evaluation scores, and then the candidate path satisfying a preset evaluation condition can be determined as the target path based on the target evaluation score. The target evaluation functions can include a first type of evaluation function related to the offset degree of the path offset with respect to the target point, a second type of evaluation function related to the path closeness, and a third type of evaluation function related to the obstacle avoidance capability. The first type of evaluation function can include a heading angle evaluation function. The second type of evaluation function can include a path following evaluation function. The third type of evaluation function can include an obstacle distance evaluation function.

[0280] For example, for each candidate path, after the initial evaluation scores of the candidate path with respect to each evaluation function are determined by using the preset target evaluation functions, the sum of the initial evaluation scores can be calculated and determined as the target evaluation score of the candidate path. The initial evaluation scores can be weighted and summed, and the sum can be determined as the target evaluation score of the candidate path. The initial evaluation scores can also be normalized and then summed, and the sum can be determined as the target evaluation score of the candidate path, which is reasonable.

[0281] The target evaluation functions are used to evaluate the closeness of each candidate path to the global path and the obstacle avoidance capability, and can include a first type of evaluation function related to the offset degree of the path offset with respect to the target point, a second type of evaluation function related to the path closeness, and a third type of evaluation function related to the obstacle avoidance capability.

[0282] The first type of evaluation function can include at least one of a heading angle evaluation function and a target point distance evaluation function. The second type of evaluation function can include a path following evaluation function. The third type of evaluation function can include an obstacle distance evaluation function.

[0283] The azimuth angle (Target heading) evaluation function can be used to evaluate the angle difference between the orientation of the robot when reaching the evaluation position while moving along the candidate path to be evaluated and the trajectory formed by the start point of the candidate path to the target point. The smaller the angle difference, the higher the initial evaluation score, and the larger the angle difference, the lower the initial evaluation score. That is, the smaller the angle between the orientation of the robot when reaching the evaluation position while moving along the candidate path to be evaluated and the trajectory formed by the start point of the candidate path to the target point, the closer the candidate path to the trajectory formed by the start point of the candidate path to the target point. Since the target point is a path point on the global path, when the candidate path is close to the trajectory including the target point, the candidate path is also close to the global path.

[0284] The azimuth angle evaluation function can be expressed in terms of the velocity components of the robot. For example, the current moving speed of the robot includes a linear velocity component and an angular velocity component. Thus, the azimuth angle evaluation function can be expressed as f(v i , ω i ).

[0285] The target distance (Target distance) evaluation function is used to evaluate the degree to which the candidate path to be evaluated points to the target point for path planning. When the robot is far away from the global path during the movement of the robot along the candidate path, the initial evaluation score of the above path following evaluation function can be 0. At this time, using the target distance evaluation function to evaluate the candidate path can determine whether the candidate path points to the target point for path planning, that is, whether the robot is oriented towards the target point when moving along the candidate path.

[0286] Based on this, using the target distance evaluation function to evaluate the candidate path can determine the distance between the robot and the target point. The closer the distance between the robot and the target point, the higher the initial evaluation score of the candidate path.

[0287] The target distance evaluation function can be expressed in terms of the velocity components of the robot. For example, the current moving speed of the robot includes a linear velocity component and an angular velocity component. Thus, for each set of candidate moving distance (v i , ω i ), the initial evaluation score of the target distance evaluation function of the candidate moving distance can be expressed as f(v i , ω i ) d :

[0288] f(v i , ω i ) d =M d -k d ·d m

[0289] wherein M d is the maximum score; k d is a preset parameter; d m is the distance between the robot and the target point.

[0290] The path following evaluation function can be used to evaluate how close the candidate path to be evaluated is to the global path. When the robot moves along the candidate path to be evaluated, the initial evaluation score of the candidate path is full score if the robot is on the global path; the farther the robot is from the global path, the lower the initial evaluation score of the candidate path; when the distance between the robot and the global path is beyond the preset following threshold, the initial evaluation score of the candidate path is 0.

[0291] The evaluation function can be expressed in terms of the velocity components of the robot, for example, the current moving speed of the robot includes a linear velocity component and an angular velocity component, so that for each set of candidate moving distance (v i , ω i ), the path following evaluation function of the candidate moving distance can be expressed as f(v i , ω i ) p :

[0292]

[0293] wherein M p is the maximum score; k p1 , k p2 is a preset parameter; d is the distance between the robot and the global reference path; d1 and d2 are preset following thresholds respectively.

[0294] When d is less than d1, the initial evaluation score of the path following evaluation function is M p -k p1 ·d; when d is greater than d1 and less than d2, the initial evaluation score of the path following evaluation function is M p -k p2 ·d, and in other cases, the initial evaluation score of the path following evaluation function is 0. Wherein d1 and d2 can be set according to actual needs, but d1 should be less than d2.

[0295] In using the path following evaluation function to evaluate the candidate path to be evaluated, the global path and the preset following threshold can be set as Figure 6The costmap shown is used to determine the score of a candidate path on the loss map as the robot moves along it and the global map. Each pixel on the loss map represents the initial evaluation score of the candidate path at that pixel. When evaluating a candidate path using a path-following evaluation function, the initial evaluation score of the candidate path can be read from the loss map, thus saving computation time.

[0296] like Figure 6 As shown, the black circular objects in the loss map represent the robot, the five trajectories are candidate paths determined by the robot moving at different candidate speeds, the black squares marked with 100 are the squares containing the global path, and the black squares marked with 0 are preset obstacles. When the robot moves along a candidate path to be evaluated, its current position is determined in the loss map. The farther the robot's current position is from the global path, the lower the initial evaluation score of that candidate path. When the robot's current position is the location of a preset obstacle, its initial evaluation score is 0. In other words, when the robot is on the black square marked 100, the initial evaluation score of the candidate path is 100 points; when the robot is on the square marked 90, the initial evaluation score of the candidate path is 90 points; when the robot is on the white square marked 0, which is far from the global path, the initial evaluation score of the candidate path is 0 points; when the robot is on the square containing the preset obstacle (i.e., the black square marked 0), although the candidate path is close to the global path, the initial evaluation score of the candidate path is also 0 points.

[0297] The obstacle distance (Clearance) evaluation function is used to evaluate the obstacle avoidance capability of a candidate path. The robot moves along the candidate path, and when it passes an obstacle, the minimum distance between the robot and the obstacle is determined without the robot touching it. If this minimum distance is not less than a preset evaluation threshold, the initial evaluation score for the candidate path is full; if the minimum distance is less than the preset evaluation threshold, the initial evaluation score decreases as the minimum distance decreases. This obstacle distance evaluation function can be represented by the robot's velocity components. For example, the robot's current moving speed includes linear velocity and angular velocity components. Thus, for each set of candidate moving distances (v... i ω i The obstacle distance evaluation function for this candidate movement distance can be expressed as dist(v i ω i ).

[0298] In addition, based on the above-mentioned various evaluation functions including the first type of evaluation function, the second type of evaluation function and the third type of evaluation function, optionally, in a specific implementation, each target evaluation function can further include: a fourth type of evaluation function about speed.

[0299] The fourth type of evaluation function includes a linear speed evaluation function.

[0300] In this specific implementation, when using each target evaluation function to evaluate the closeness to the global path and the obstacle avoidance capability of each candidate path, the fourth type of evaluation function about speed can also be used.

[0301] The above-mentioned fourth type of evaluation function can include at least one of a linear speed (Velocity) evaluation function and an angular speed (Angular velocity) evaluation function.

[0302] The linear speed evaluation function is used to evaluate the moving linear speed of the robot when moving along the candidate path to be evaluated, and the greater the moving speed of the robot, the higher the initial evaluation score of the candidate path. The linear speed evaluation function can be expressed by the speed component of the robot, for example, for each group of candidate moving distances (v i , ω i ), the linear speed evaluation function of the candidate moving distance can be expressed as velo(v i , ω i ).

[0303] The angular speed evaluation function is used to evaluate the moving angular speed of the robot when moving along the candidate path to be evaluated, and the greater the moving angular speed of the robot, the more serious the oscillation of the robot, and the lower the initial evaluation score of the candidate path. The angular speed evaluation function can be expressed by the speed component of the robot, for example, the current moving speed of the robot includes a linear speed component and an angular speed component, so that for each group of candidate moving distances (v i , ω i ), the angular speed evaluation function of the candidate moving distance can be expressed as f(v i , ω i ) a .

[0304] f(v i , ω i ) a =M a -k a ·ω i

[0305] wherein M a is a maximum score; k a is a proportionality coefficient set artificially.

[0306] And, in calculating the target evaluation score of the candidate moving path based on the respective initial evaluation scores, the respective initial evaluation scores can be normalized, the product of the weight of each evaluation function and the normalized initial evaluation score of the evaluation function is calculated, and the sum of the respective products is calculated as the target evaluation score of the candidate path.

[0307] In the specific implementation, for each candidate path, after the initial evaluation scores of the candidate path with respect to the respective evaluation functions are determined, since the scales of the respective evaluation functions can be different, in order to unify the scales of the respective initial evaluation scores, the respective initial evaluation scores can be first normalized, then the product of the weight of each evaluation function and the normalized initial evaluation score of the evaluation function is calculated based on the weight of each evaluation function, the sum of the respective products is calculated, and the sum is taken as the target evaluation score of the candidate path.

[0308] Optionally, the respective initial evaluation scores can be normalized by using the following formula:

[0309]

[0310] wherein, avgg j (v i , ω i ) is the jth initial evaluation score of the candidate path corresponding to the ith group of candidate moving speeds after normalization; g j (v i , ω i ) is the jth initial evaluation score of the candidate path corresponding to the ith group of candidate moving speeds; sum(g j (v i , ω i )) is the sum of scores of all initial evaluation scores of the ith group of candidate moving speeds; 1≤i≤n, n is the total number of groups of the plurality of groups of candidate moving speeds, 1≤j≤m, m is the number of all initial evaluation scores of the ith group of candidate moving speeds.

[0311] The weights of the respective evaluation functions can be set according to actual needs, when a higher degree of closeness of the candidate path to the preset global path is required, the weights of the azimuth angle evaluation function, the path following evaluation function and the target point distance evaluation function can be set to be higher; when a stronger obstacle avoidance ability of the candidate path is required, the weight of the obstacle distance evaluation function can be set to be higher; when a higher stability of the robot moving along the candidate path is required, the angular velocity evaluation function can be set to be higher, which are all reasonable and are not specifically limited in the embodiment of the application.

[0312] For example, when evaluating candidate paths using the path following evaluation function, the target point distance evaluation function, and the obstacle distance evaluation function, the target evaluation score can be calculated using the following formula:

[0313] G(v i ω i )=γf(v i ω i ) p +δf(v i ω i ) d +σdist(v i ω i )

[0314] Wherein G(v i ω i ) represents the target evaluation score; γ represents the weight of the path following evaluation function; δ represents the weight of the target point distance evaluation function; and σ represents the weight of the obstacle distance evaluation function.

[0315] Optionally, after determining the initial evaluation scores of each evaluation function, the sum of the initial evaluation scores can be calculated as the target evaluation score for the candidate path.

[0316] Optionally, after determining the initial evaluation score of each evaluation function, the product of the weight of each evaluation function and the initial evaluation score of that evaluation function can be calculated, and the sum of the obtained products can be used as the target evaluation score of the candidate path.

[0317] Optionally, after determining the initial evaluation scores of each evaluation function, the initial evaluation scores can be normalized, and then the sum of the normalized initial evaluation scores can be calculated and used as the target evaluation score for the candidate path.

[0318] To facilitate understanding of the trajectory optimization method provided in this embodiment of the invention, the following is combined with... Figure 7 , and will be explained in detail.

[0319] like Figure 7 As shown, the trajectory optimizer is used to determine a specified start point and a specified end point in the global path when the robot's current position meets the preset optimization conditions during the robot's movement based on the robot's current position; then, it optimizes the trajectory of the path between the specified start point and the specified end point in the global path to obtain the target path; and then sends the target path to the robot.

[0320] The local planner is configured to determine a calibration distance for path planning when a preset planning condition is met at a current position of the robot, determine a target point for local path planning from the global path based on the calibration distance and the current position of the robot, select a plurality of groups of candidate moving speeds from a preset speed range corresponding to a current time, and determine a candidate path corresponding to each group of candidate moving speeds, determine a target evaluation score of each candidate path, and determine a local path for which the determined target evaluation score meets a preset evaluation condition, and send a candidate moving speed corresponding to the local path.

[0321] The executor is configured to control the robot to move along the local path at the candidate moving speed corresponding to the local path after receiving the local path, and return a current moving speed of the robot in real time, so that the trajectory optimizer can determine a specified starting point and a specified ending point for the segmented trajectory optimization of the global path based on the current speed of the robot.

[0322] In this way, during movement of the robot based on the preset global path, when a preset optimization condition is met at a current position of the robot, the current position of the robot can be sent to the trajectory optimizer, the trajectory optimizer receives the current position of the robot, and determines a point closest to the current position of the robot on the global path as a specified starting point for segmented trajectory optimization based on the current position of the robot, and then calculates a target distance for trajectory optimization based on the current speed of the robot. Furthermore, based on the specified starting point and the target distance, a specified ending point for trajectory optimization can be determined on the global path. In this way, based on the specified starting point and the specified ending point, the path between the specified starting point and the specified ending point in the global path is subjected to trajectory optimization, and an optimized target path is obtained. Then, the trajectory optimizer sends the target path to the robot, the robot receives the target path and moves according to the target path, and when the current position of the robot again meets the preset optimization condition, the current position of the robot is again sent to the trajectory optimizer, until the specified ending point of the target trajectory coincides with the preset ending point of the global path.

[0323] And, in the process that the robot moves according to the target path, when a current position of the robot satisfies a preset planning condition, the robot sends the current position to a local planner, the local planner receives the current position sent by the robot, and determines a local path close to the global path for the robot. In the process of local path planning, the local planner can determine a calibration distance for local path planning based on the current position of the robot. Further, based on the calibration distance and the current position of the robot, a target point for local path planning is determined in the global path. Then, a plurality of groups of candidate moving speeds are selected from a preset speed range corresponding to a current time, and a candidate path corresponding to each group of candidate moving speeds is determined; a target evaluation score of each candidate path is determined, and a candidate path whose determined target evaluation score satisfies a preset evaluation condition is determined as a local path; then, a candidate moving speed corresponding to the local path is sent to an executor, and the executor controls the robot to move along the local path according to the candidate moving speed corresponding to the local path, and returns a current moving speed of the robot in real time, so that the trajectory optimizer can determine the specified starting point and the specified ending point for segmenting and optimizing the global path again based on the current speed of the robot.

[0324] Based on the same inventive concept, the application provides a trajectory optimization method as shown in the embodiment of the application Figure 1 The application also provides a trajectory optimization device.

[0325] Figure 8 A structural schematic diagram of a trajectory optimization device provided by the application is shown in the figure Figure 8 The device can include the following modules:

[0326] The determination module 810 is configured to, in the process that the robot moves based on a preset global path, when a current position of the robot satisfies a preset optimization condition, determine a specified starting point and a specified ending point in the global path based on the current position of the robot.

[0327] The optimization module 820 is configured to perform trajectory optimization on a path between the specified starting point and the specified ending point in the global path to obtain a target path.

[0328] The control module 830 is configured to control the robot to move according to the target path, and trigger the determination module 810 when a current position of the robot satisfies the preset optimization condition, until the robot moves to a preset ending point of the global path.

[0329] It can be seen from the above that, when the robot is expected to move from the preset starting point to the preset ending point, after global path planning is performed and a global path from the preset starting point to the preset ending point is obtained, the global path can be segmented and trajectory optimized according to the current position of the robot during movement of the robot based on the global path, so as to achieve trajectory optimization of the global path. Then, the robot can move according to the global path after trajectory optimization during movement based on the global path. In this way, since each segment of the target path obtained by trajectory optimization can be smooth and can avoid the preset obstacle, the global path on which the robot moves from the preset starting point to the preset ending point can be smooth and can avoid the preset obstacle. Then, the robot can be prevented from slowing down due to a large number of inflection points in the global path, a non-smooth path segment in the global path, and collision with the preset obstacle, and the movement efficiency of the robot during movement can be improved.

[0330] Optionally, in a specific implementation manner, the determining module 810, specifically used for:

[0331] when the robot is located at the preset starting point of the preset global path, determining a specified starting point and a specified ending point in the global path based on the preset starting point;

[0332] during movement of the robot based on the target path obtained by trajectory optimization, when the robot moves to a path midpoint of the target path, determining a specified starting point and a specified ending point in the global path based on the current position of the robot; or, during movement of the robot based on the global path, when the robot moves for a time length corresponding to a preset period each time, determining a specified starting point and a specified ending point in the global path based on the current position of the robot.

[0333] Optionally, in a specific implementation manner, the determining module 810 includes:

[0334] a starting point determining sub-module, used for determining a point with a minimum distance between the global path and the current position of the robot as a specified starting point;

[0335] an ending point determining sub-module, used for determining a specified ending point in the global path based on the specified starting point.

[0336] Optionally, in a specific implementation manner, the ending point determining sub-module, specifically used for:

[0337] determining a target distance based on the current speed of the robot;

[0338] if the distance between the specified start point and the preset end point of the global path is not greater than the target distance, taking the preset end point of the global path as the specified end point;

[0339] if the distance between the specified start point and the preset end point of the global path is greater than the target distance, determining a specified point in the global path as the specified end point;

[0340] wherein the specified point comprises a point with a reference distance closest to the target distance, a point with a reference distance less than the target distance and closest to the target distance, or a point with a reference distance greater than the target distance and closest to the target distance.

[0341] Optionally, in an implementation manner, the apparatus further includes:

[0342] a distance judgment module, configured to, before the specified point in the global path is determined as the specified end point, judge whether a distance between the specified point and a preset end point of the global path is not greater than a preset distance threshold; if yes, taking the preset end point of the global path as the specified end point; otherwise, triggering the end point determination submodule.

[0343] Optionally, in an implementation manner, the control module 830 includes:

[0344] a distance determination submodule, configured to determine a calibration distance for path planning when the current position of the robot satisfies a preset planning condition;

[0345] a target point determination submodule, configured to determine a target point for local path planning from the global path based on the calibration distance and the current position of the robot;

[0346] a candidate path determination submodule, configured to select multiple groups of candidate moving speeds from a preset speed range corresponding to a current time, and determine a candidate path corresponding to each group of candidate moving speeds;

[0347] a local path determination submodule, configured to determine a target evaluation score of each candidate path, and determine a candidate path with a determined target evaluation score satisfying a preset evaluation condition as a local path; wherein the target evaluation score of each candidate path is used to represent a score of a deviation degree of the candidate path relative to the target point, a closeness degree of the candidate path to the global path, and an obstacle avoidance capability;

[0348] a moving submodule, configured to control the robot to move along the local path according to a candidate moving speed corresponding to the local path, and trigger the distance determination submodule when the current position of the robot satisfies a preset planning condition.

[0349] Optionally, in an implementation form, the local path determining submodule is specifically configured to:

[0350] For each candidate path, an initial evaluation score of the candidate path with respect to each evaluation function is calculated by using a preset respective target evaluation function, and a target evaluation score of the candidate path is calculated based on the respective initial evaluation scores;

[0351] The respective target evaluation functions include respective first-type evaluation functions with respect to a deviation degree of a path deviation from a target point, respective second-type evaluation functions with respect to a path closeness degree, and respective third-type evaluation functions with respect to an obstacle avoidance capability, the first-type evaluation functions include an azimuth angle evaluation function, the second-type evaluation functions include a path following evaluation function, and the third-type evaluation functions include an obstacle distance evaluation function.

[0352] Optionally, in an implementation form, the respective target evaluation functions further include a fourth-type evaluation function with respect to a speed, and the fourth-type evaluation function includes a linear speed evaluation function.

[0353] The application also provides a robot, as shown in Figure 9 The processor 901, the communication interface 902 and the memory 903 can communicate with each other through the communication bus 904.

[0354] The memory 903 is configured to store a computer program.

[0355] The processor 901 is configured to execute the program stored in the memory 903, and implement the steps of any trajectory optimization method provided by the application.

[0356] The communication bus mentioned above can be a Peripheral Component Interconnect (PCI) bus or an Extended Industry Standard Architecture (EISA) bus, etc. The communication bus can be divided into an address bus, a data bus, a control bus, etc. For the convenience of representation, only one thick line is shown in the figure, but it does not mean that there is only one bus or only one type of bus.

[0357] The communication interface is configured to communicate between the robot and other devices.

[0358] The memory can include a random access memory (RAM) and can also include a non-volatile memory (NVM), such as at least one disk memory. Optionally, the memory can also be at least one storage device located remotely from the aforementioned processor.

[0359] The processor described above can be a general processor, including a central processing unit (CPU), a network processor (NP), etc.; can also be a digital signal processor (DSP), an application specific integrated circuit (ASIC), a field-programmable gate array (FPGA) or other programmable logic device, a discrete gate or transistor logic device, a discrete hardware component.

[0360] In yet another embodiment provided by the present application, a computer readable storage medium is provided, and the computer readable storage medium stores a computer program. The computer program is executed by a processor to implement the steps of any of the above method embodiments.

[0361] In yet another embodiment provided by the present application, a computer program product containing instructions, which, when run on a computer, causes the computer to perform the steps of any of the method embodiments described above.

[0362] In the embodiments described above, all or some of the steps can be implemented by software, hardware or firmware, or any combination thereof. When implemented by software, all or some of the steps can be implemented in the form of one or more computer programs. The computer program can be stored in any computer readable medium, and loaded into the computer for execution. The computer readable medium includes computer storage media and communication media. The computer storage media includes any tangible or physical medium for storing or transmitting the program. The computer storage media can be a volatile (such as RAM) or non-volatile (such as ROM, disk, or CD) storage medium. The communication media typically include computer readable instructions, data structures, program modules or other data in a modulated data signal such as a carrier wave or other transport mechanism, and includes any information delivery media. The computer readable medium can be a computer program product.

[0363] It should be noted that, in the present document, the terms such as first and second are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Also, the terms "comprising", "containing", or any other variant thereof are intended to cover a non-exclusive inclusion, such that a process, method, article or apparatus that comprises a list of elements does not only include those elements, but can also include other elements not expressly listed or inherent to such process, method, article or apparatus. Without more limitations, an element defined by the statement "comprising a" does not exclude the presence of additional identical elements in the process, method, article or apparatus that includes the element.

[0364] Each of the embodiments in the present specification is described in a related manner, and the same or similar parts between the embodiments can be referred to each other. Each embodiment focuses on the difference from other embodiments. In particular, for the device embodiment, the robot embodiment, the computer readable storage medium embodiment, and the computer program product embodiment, since they are basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the part of the method embodiment.

[0365] The above merely describes the preferred embodiments of the present application, but is not used to limit the protection scope of the present application. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A trajectory optimization method, characterized by, The method comprises: In the process that the robot moves based on a preset global path, when a current position of the robot satisfies a preset optimization condition, a specified starting point and a specified ending point are determined in the global path based on the current position of the robot; Trajectory optimization is performed on a path in the global path between the specified starting point and the specified ending point to obtain a target path; The robot is controlled to move according to the target path, and when the current position of the robot satisfies the preset optimization condition, the step of determining the specified starting point and the specified ending point in the global path based on the current position of the robot is returned until the robot moves to a preset ending point of the global path; The step of determining the specified starting point and the specified ending point in the global path based on the current position of the robot comprises: A point in the global path with the minimum distance from the current position of the robot is determined as the specified starting point; A target distance is determined based on a current speed of the robot; If a distance between the specified starting point and the preset ending point of the global path is not greater than the target distance, the preset ending point of the global path is determined as the specified ending point; If the distance between the specified starting point and the preset ending point of the global path is greater than the target distance, a specified point in the global path is determined as the specified ending point; The specified point comprises a point with a reference distance closest to the target distance, a point with a reference distance less than the target distance and closest to the target distance, or a point with a reference distance greater than the target distance and closest to the target distance.

2. The method of claim 1, wherein, The step of determining the specified starting point and the specified ending point in the global path based on the current position of the robot in the process that the robot moves based on the preset global path when the current position of the robot satisfies the preset optimization condition comprises: When the robot is located at a preset starting point of the preset global path, the specified starting point and the specified ending point are determined in the global path based on the preset starting point; When the robot moves to a path midpoint of the target path in the process that the robot moves based on the target path obtained through trajectory optimization, the specified starting point and the specified ending point are determined in the global path based on the current position of the robot; or When the robot moves for a time corresponding to a preset period in the process that the robot moves based on the global path, the specified starting point and the specified ending point are determined in the global path based on the current position of the robot.

3. The method of claim 1, wherein, Before the specified point in the global path is determined as the specified ending point, the method further comprises: It is judged whether a distance between the specified point and a preset ending point of the global path is not greater than a preset distance threshold; If yes, the preset ending point of the global path is determined as the specified ending point; Otherwise, the specified point in the global path is determined as the specified ending point.

4. The method of claim 1, wherein, The step of controlling the robot to move according to the target path comprises: When a current position of the robot satisfies a preset planning condition, a calibration distance for path planning is determined; determine a target point for local path planning from the global path based on the calibrated distance and a current position of the robot; select a plurality of groups of candidate moving speeds from a preset speed range corresponding to a current time, and determine a candidate path corresponding to each group of candidate moving speeds; determine a target evaluation score of each candidate path, and determine a candidate path whose target evaluation score meets a preset evaluation condition as a local path; wherein the target evaluation score of each candidate path is used to represent a score of a deviation degree of the candidate path relative to the target point, a closeness degree of the candidate path to the global path, and an obstacle avoidance capability; control the robot to move along the local path according to the candidate moving speed corresponding to the local path, and return to the step of determining the calibrated distance for path planning when a current position of the robot meets a preset planning condition.

5. The method of claim 4, wherein, The method further includes: for each candidate path, calculating an initial evaluation score of the candidate path with respect to each evaluation function by using a preset evaluation function, and calculating a target evaluation score of the candidate path based on the initial evaluation scores; wherein the evaluation functions include: a first type of evaluation function for a deviation degree of a path deviation relative to a target point, a second type of evaluation function for a closeness degree of a path, and a third type of evaluation function for an obstacle avoidance capability, the first type of evaluation function includes an azimuth angle evaluation function, the second type of evaluation function includes a path following evaluation function, and the third type of evaluation function includes an obstacle distance evaluation function.

6. The method of claim 5, wherein, The evaluation functions further include a fourth type of evaluation function for a speed, and the fourth type of evaluation function includes a linear speed evaluation function.

7. A trajectory optimization apparatus characterized by comprising: The device includes: a determination module configured to, when a current position of a robot meets a preset optimization condition during movement of the robot based on a preset global path, determine a specified starting point and a specified ending point in the global path based on the current position of the robot; an optimization module configured to perform trajectory optimization on a path in the global path between the specified starting point and the specified ending point to obtain a target path; a control module configured to control the robot to move according to the target path, and trigger the determination module when the current position of the robot meets the preset optimization condition, until the robot moves to a preset ending point of the global path; The determination module includes: a starting point determination sub-module configured to determine a point in the global path that has a minimum distance from the current position of the robot as the specified starting point; an ending point determination sub-module configured to determine the specified ending point in the global path based on the specified starting point; The ending point determination sub-module is specifically configured to: determine a target distance based on a current speed of the robot; if a distance between the specified starting point and a preset ending point of the global path is not greater than the target distance, determine the preset ending point of the global path as the specified ending point. If a distance between the specified start point and a preset end point of the global path is greater than the target distance, a specified point in the global path is determined as the specified end point; The specified point includes a point with a reference distance closest to the target distance, a point with a reference distance less than the target distance and closest to the target distance, or a point with a reference distance greater than the target distance and closest to the target distance.

8. The apparatus of claim 7, wherein, The determining module is specifically configured to: determine a specified start point and a specified end point in the global path based on the preset start point when the robot is located at the preset start point of the global path; determine a specified start point and a specified end point in the global path based on the current position of the robot when the robot moves to a path midpoint of the target path in the process of moving based on the target path obtained through trajectory optimization; or determine a specified start point and a specified end point in the global path based on the current position of the robot when the robot moves for a preset period of time in the process of moving based on the global path. The device further includes:

9. The apparatus of claim 7, wherein, a distance determining module configured to determine whether a distance between a specified point and a preset end point of the global path is not greater than a preset distance threshold before the specified point in the global path is determined as the specified end point; if yes, the preset end point of the global path is determined as the specified end point; otherwise, the end point determining submodule is triggered. The control module includes:

10. The apparatus of claim 7, wherein, a distance determining submodule configured to determine a calibration distance for path planning when the current position of the robot meets a preset planning condition; a target point determining submodule configured to determine a target point for local path planning from the global path based on the calibration distance and the current position of the robot; a candidate path determining submodule configured to select multiple groups of candidate moving speeds from a preset speed range corresponding to a current time, and determine a candidate path corresponding to each group of candidate moving speeds; a local path determining submodule configured to determine a target evaluation score of each candidate path, and determine a candidate path with a determined target evaluation score meeting a preset evaluation condition as a local path; wherein the target evaluation score of each candidate path is used to represent a degree of deviation of the candidate path relative to the target point, a degree of closeness of the candidate path to the global path, and a score of obstacle avoidance capability; a moving submodule configured to control the robot to move along the local path according to a candidate moving speed corresponding to the local path, and trigger the distance determining submodule when the current position of the robot meets a preset planning condition. The local path determining submodule is specifically configured to:

11. The apparatus of claim 10, wherein, for each candidate path, calculate an initial evaluation score of the candidate path with respect to each evaluation function using a preset target evaluation function, and calculate a target evaluation score of the candidate path based on the initial evaluation scores. ​ The target evaluation functions include a first type of evaluation function about the deviation degree of the path deviation from the target point, a second type of evaluation function about the path closeness degree, and a third type of evaluation function about the obstacle avoidance capability.

12. The apparatus of claim 11, wherein, The target evaluation functions further include a fourth type of evaluation function about the speed.

13. A robot, characterized in that The computer readable storage medium stores a computer program, and the computer program is executed by the processor to implement the method steps of any one of claims 1-6. The computer readable storage medium stores a computer program, and the computer program is executed by the processor to implement the method steps of any one of claims 1-6. The computer readable storage medium stores a computer program, and the computer program is executed by the processor to implement the method steps of any one of claims 1-6.

14. A computer-readable storage medium, characterized in that, ​

Images