A method and apparatus for path planning of a robot

Abstract

A path planning method and device of a robot, by acquiring a starting point and an ending point, and defining a growing tree and a growing rule to iteratively update the growing tree, thereby acquiring a motion path from the starting point to the ending point; through the application. In the case of knowing the starting point and the ending point, a motion path can be automatically planned by the robot.

Description

Technical Field

[0001] This invention relates to the field of robot control, and more specifically to a method and apparatus for robot path planning. Background Technology

[0002] Two important issues are involved in robot motion control: path planning and how to move along the path. In many robot control processes, robot teaching is used to guide the robot along a fixed trajectory. However, in unstructured environments, if the surrounding environment changes, and the robot continues to move along a fixed trajectory, collisions or even irreversible equipment damage may occur. Summary of the Invention

[0003] To address the above problems, this invention provides a path planning method and apparatus for robots, which are described in detail below.

[0004] According to a first aspect, one embodiment provides a path planning method for a robot, comprising:

[0005] Obtain relevant trajectory information of the robot in the pre-built space; the relevant trajectory information includes initial path nodes, the initial path nodes include a first initial path node and a second initial path node, wherein: the first initial path node and the second initial path node are the start point and the end point, or vice versa;

[0006] The initial path node is used as a seed node to construct a growth tree, and the growth tree is iteratively updated based on the growth rules until a stopping condition is reached, so as to obtain a path from the first initial path node to the second initial path node based on the iteratively updated growth tree.

[0007] The iterative update of the growth tree based on growth rules includes:

[0008] For the current growth tree to be updated, a random point is generated in the pre-built space;

[0009] Search for the node closest to the random point in the current reference tree, where the reference tree is also a growth tree obtained based on the growth rule;

[0010] Based on the random point and the nearest node, determine the candidate nodes of the growth tree that are currently to be updated;

[0011] Obtain the node to be grown in the current growth tree to be updated, and determine whether there is an obstacle between the node and the candidate node. If there is no obstacle, the candidate node is used as the child node of the current node to be grown to update the current growth tree until the stopping condition is reached. If there is an obstacle, the candidate node is discarded and the growth tree is iteratively updated based on the growth rules.

[0012] In one embodiment, the reference tree and the current growth tree to be updated are the same growth tree; the growth tree uses the first initial path node as the seed node;

[0013] The node to be grown is the node in the current growth tree that is closest to the random point.

[0014] In one embodiment, determining the candidate nodes of the growth tree to be updated based on the random point and the nearest node includes:

[0015] Based on the random point and the nearest node, a first direction from the nearest node to the random point is determined;

[0016] The point that is the closest node in the first direction to the node with a preset step size is selected as the candidate node.

[0017] In one embodiment, the iterative update of the growth tree based on growth rules further includes:

[0018] After updating the growth tree by treating the candidate node as a child node of the current node to be grown, it is further determined whether the candidate node is within the preset neighborhood of the second initial path node and whether there are obstacles between them: if the candidate node is not within the preset neighborhood of the second initial path node or there are obstacles between them, a random point is generated in the pre-built space to continue iteratively updating the growth tree based on the growth rules; if the candidate node is within the preset neighborhood of the second initial path node and there are no obstacles between them, it is determined that the stopping condition has been reached.

[0019] In one embodiment, constructing a growth tree using the initial path node as a seed node includes: constructing a first growth tree using the first initial path node as a seed node, and constructing a second growth tree using the second initial path node as a seed node.

[0020] In the iterative update of the growth tree based on growth rules:

[0021] If the current growth tree to be updated is the first growth tree, then the current reference tree is the second growth tree, and the current node to be grown is the last node in the first growth tree. Furthermore, if there are no obstacles and the candidate node is used as a child node of the current node to be grown to update the current growth tree, and the stopping condition is not reached, then while iteratively updating the growth tree based on the growth rules, the first growth tree remains the growth tree to be updated. If there are obstacles and the candidate node is abandoned while iteratively updating the growth tree based on the growth rules, then the first growth tree is switched to the reference tree, and the second growth tree is switched to the growth tree to be updated.

[0022] If the current growth tree to be updated is the second growth tree, then the current reference tree is the first growth tree, and the current node to be grown is the last node in the second growth tree. Furthermore, if there are no obstacles and the candidate node is used as a child node of the current node to be grown to update the current growth tree, and if the stopping condition is not reached, then while iterating and updating the growth tree based on the growth rules, the second growth tree will remain the growth tree to be updated. If there are obstacles and the candidate node is abandoned while iterating and updating the growth tree based on the growth rules continues, then the second growth tree will be switched as the reference tree, and the first growth tree will be switched as the growth tree to be updated.

[0023] In one embodiment, determining the candidate nodes of the growth tree to be updated based on the random point and the nearest node includes:

[0024] Based on the random point and the nearest node, a second direction from the random point to the nearest node is determined;

[0025] A point that is a preset step size away from the current node to be grown in the second direction is selected as the candidate node.

[0026] In one embodiment, the iterative update of the growth tree based on growth rules further includes:

[0027] After updating the growth tree by using the candidate node as a child node of the current node to be grown, it is further determined whether there is a first target node and a second target node between the currently updated growth tree and the reference tree that are less than a preset distance and have no obstacles between them. If they exist, it is determined that the stopping condition has been reached; otherwise, it is determined that the stopping condition has not been reached.

[0028] According to a second aspect, one embodiment provides a path planning device for a robot, comprising:

[0029] Memory, used to store programs;

[0030] A processor for implementing the method as described in any of the embodiments herein by executing a program stored in the memory.

[0031] According to a third aspect, one embodiment provides a computer program product that, when run on a processor, causes the processor to execute to implement the method as described in any of the embodiments herein.

[0032] According to a fourth aspect, one embodiment provides a computer-readable storage medium including a program that can be executed by a processor to perform the methods as described in any of the embodiments herein.

[0033] According to the robot path planning method, apparatus, computer program product, and computer-readable storage medium of the above embodiments, a motion path from the starting point to the ending point is obtained by iteratively updating the growth tree by acquiring the starting point and ending point and defining the growth tree and growth rules; through this application, a motion path can be automatically planned by the robot when the starting point and ending point are known. Attached Figure Description

[0034] Figure 1 A flowchart illustrating a path planning method for a robot according to one embodiment;

[0035] Figure 2 This is a flowchart illustrating an embodiment of iteratively updating a growth tree based on growth rules.

[0036] Figure 3 This is a schematic diagram of the growth tree and movement path in one embodiment;

[0037] Figure 4 This is a flowchart illustrating motion control of each segment of the path, as shown in one embodiment. Detailed Implementation

[0038] The present invention will now be described in further detail with reference to specific embodiments and accompanying drawings. Similar elements in different embodiments are referred to by associated similar element reference numerals. In the following embodiments, many details are described to facilitate a better understanding of this application. However, those skilled in the art will readily recognize that some features may be omitted in different situations, or may be replaced by other elements, materials, or methods. In some cases, certain operations related to this application are not shown or described in the specification. This is to avoid obscuring the core parts of this application with excessive description. For those skilled in the art, detailed description of these related operations is not necessary; they can fully understand the related operations based on the description in the specification and general technical knowledge in the art.

[0039] Furthermore, the features, operations, or characteristics described in the specification can be combined in any suitable manner to form various embodiments. At the same time, the steps or actions in the method description can be rearranged or adjusted in a manner obvious to those skilled in the art. Therefore, the various orders in the specification and drawings are only for the clear description of a particular embodiment and do not imply a necessary order, unless otherwise stated that a particular order must be followed.

[0040] The serial numbers assigned to components in this document, such as "first" and "second," are used only to distinguish the described objects and have no sequential or technical meaning. The terms "connection" and "linkage" used in this application, unless otherwise specified, include both direct and indirect connections (linkages).

[0041] The robot involved in this application is a robot in a broad sense, not just referring to humanoid devices. It can also refer to machine parts such as robotic arms; that is, the robot involved in this application refers to a machine that can move or move.

[0042] The main purpose of this application is to address the autonomous path planning capabilities of robots in unstructured environments, which are described in detail below.

[0043] Please refer to Figure 1 Some embodiments of the robot path planning method include the following steps:

[0044] Step 1100: Obtain the relevant trajectory information of the robot in the pre-built space. In some embodiments, the relevant trajectory information includes initial path nodes, which include a first initial path node and a second initial path node, wherein the first initial path node and the second initial path node are the start point and the end point, or vice versa.

[0045] Step 1200: Iteratively update the growth tree to obtain the path between the starting point and the ending point. For example, in step 1200, the initial path node is used as the seed node to construct the growth tree, and the growth tree is iteratively updated based on the growth rules until a stopping condition is reached, so as to obtain a path from the first initial path node to the second initial path node based on the iteratively updated growth tree.

[0046] The following explains the iterative update of the growth tree based on growth rules involved in step 1200.

[0047] Please refer to Figure 2 In some embodiments, step 1200 iteratively updates the growth tree based on growth rules, including the following steps:

[0048] Step 1210: For the current growth tree T to be updated rn A random point is generated in the pre-constructed space described above.

[0049] Step 1220: In the current reference tree T rf Search for the node closest to the random point in the reference tree T. rf This is also a growth tree obtained based on the above growth rules.

[0050] Step 1230: Determine the current growth tree T to be updated based on the above random points and the nearest node. rn Candidate nodes.

[0051] Step 1240: Determine whether to apply the growth tree T based on the candidate nodes. rn Perform the update. Specifically, step 1240 obtains the current growth tree T to be updated. rn The current node to be grown is selected, and based on this node and the aforementioned candidate nodes, it is determined whether there is an obstacle between them. If there is no obstacle, the aforementioned candidate nodes are made into child nodes of the current node to be grown, in order to update the current growth tree T. rn Update the tree until the above stopping condition is reached; if there is an obstacle, discard the candidate node and continue to iteratively update the growth tree based on the growth rules.

[0052] In some embodiments, the reference tree T involved in step 1200 rf Compared with the current growth tree T that needs to be updated rn For the same growth tree; the growth tree takes the first initial path node as the seed node; the node to be grown in step 1200 is the node in the current growth tree to be updated that is closest to the above random point.

[0053] In some embodiments, step 1230 determines the current growth tree T to be updated based on a random point and the nearest node.rf The candidate nodes include: determining a first direction from the nearest node to the random point based on the random point and the nearest node, and selecting a point in the first direction that is a preset step size away from the nearest node as the candidate node.

[0054] In some embodiments, step 1200 involves treating the aforementioned candidate nodes as child nodes of the node currently to be grown, in order to update the current growth tree T. rf After the update, it is further determined whether the candidate node is within the preset neighborhood of the second initial path node and whether there are obstacles between them: if the candidate node is within the preset neighborhood of the second initial path node and there are no obstacles between them, it is determined that the above stopping condition has been reached; otherwise, if the candidate node is not within the preset neighborhood of the second initial path node or there are obstacles between them, a random point is generated in the pre-constructed space to continue iteratively updating the growth tree based on the growth rules.

[0055] Therefore, an example could be like this:

[0056] The reference tree T involved in step 1200 rf Compared with the current growth tree T that needs to be updated rn Let's denote the same growth tree as T. In this case, the initial path node, such as the first initial path node, is used as the seed node to construct the growth tree T. Step 1200 iteratively updates the growth tree based on the growth rules, including:

[0057] Step 1210: For the current growth tree T to be updated, generate a random point in the pre-constructed space mentioned above.

[0058] Step 1220: Search for the node in the current growth tree T1 that is closest to the above random point.

[0059] Step 1230: Based on the above-mentioned random point and the above-mentioned nearest node, determine the candidate nodes of the growth tree T to be updated. For example, Step 1230 determines the first direction from the above-mentioned nearest node to the above-mentioned random point based on the above-mentioned random point and the above-mentioned nearest node, and selects the point in the first direction that is a preset step size away from the above-mentioned nearest node as the candidate node.

[0060] Step 1240: Determine whether there is an obstacle between the nearest node and the candidate node mentioned above.

[0061] If there are obstacles, the candidate nodes are abandoned and the growth tree T is iteratively updated based on the growth rules. The iteration is restarted from step 1210, and a random point is generated in the pre-constructed space to continue iteratively updating the growth tree T based on the growth rules.

[0062] If there are no obstacles, the candidate node is used as a child node of the nearest node to update the current growth tree T until the stopping condition is reached. In some embodiments, after updating the current growth tree T by using the candidate node as a child node of the nearest node, it is further determined whether the candidate node is within the preset neighborhood of the second initial path node and whether there are no obstacles between them. If the candidate node is not within the preset neighborhood of the second initial path node or there are obstacles between them, a random point is generated in the pre-built space to continue iteratively updating the growth tree T based on the growth rules. If the candidate node is within the preset neighborhood of the second initial path node and there are no obstacles between them, it is determined that the stopping condition has been reached.

[0063] In some embodiments, step 1200 constructs a growth tree using the initial path node as the seed node, including: constructing a first growth tree T1 using the first initial path node as the seed node, and constructing a second growth tree T2 using the second initial path node as the seed node. In such embodiments, the first growth tree T1 and the second growth tree T2 are iteratively updated alternately based on growth rules: when iteratively updating the first growth tree T1, the first growth tree T1 is the current growth tree T to be updated. rn Correspondingly, at this time, the second growth tree T2 is the current reference tree T. rf Similarly, when iteratively updating the second growth tree T2, the second growth tree T2 is the current growth tree T to be updated. rn Correspondingly, at this time, the first growing tree T1 is the current reference tree T. rf Therefore, some embodiments are as follows:

[0064] In the current growth tree T that is yet to be updated rn If the first growing tree is T1, then the current reference tree T rf Let T2 be the second growth tree, and the node to be grown is the last node in the first growth tree T1. If there are no obstacles, and the candidate node is used as a child node of the node to be grown to update the current growth tree T1, and the stopping condition is not reached, then while continuing to iterate and update the growth tree based on the growth rules, the first growth tree T1 will remain the growth tree to be updated. If there are obstacles, and the candidate node is abandoned while continuing to iterate and update the growth tree based on the growth rules, then the first growth tree T1 will be switched to the reference tree T. rfSwitch the second growth tree T2 to the growth tree T to be updated. rn ;

[0065] In the current growth tree T that is yet to be updated rn In the case of the second growing tree T2, the current reference tree T rf Let T1 be the first growth tree, and the node to be grown is the last node in the second growth tree T2. Furthermore, after updating the current growth tree T2 by making a candidate node a child of the node to be grown when there are no obstacles, if the stopping condition has not been reached, then while continuing to iteratively update the growth tree based on the growth rules, the second growth tree T2 will remain the growth tree to be updated. rn If an obstacle exists, causing the candidate node to be abandoned and the growth tree to continue iteratively updated based on the growth rules, then the second growth tree T2 is switched as the reference tree T. rf Switch the first growth tree T1 to the growth tree T to be updated. rn .

[0066] During the iterative update of the first growth tree T1 and the second growth tree T2 based on growth rules, step 1230 determines the current growth tree T to be updated based on a random point and the nearest node. rf The candidate nodes may include: determining a second direction from the random point to the nearest node based on the random point and the nearest node, and selecting a point in the second direction that is a preset step size away from the node to be grown as a candidate node.

[0067] In some embodiments, after step 1200 updates the growth tree by using the candidate node as a child node of the current node to be grown, it further determines whether there is a first target node and a second target node between the currently updated growth tree and the current reference tree that are less than a preset distance apart and have no obstacles between them. That is, whether there is a first target node in the currently updated growth tree and a second target node in the current reference tree, and whether the distance between these two nodes is less than a preset distance apart and there are no obstacles between them. If there are such a first target node and a second target node in the currently updated growth tree and the current reference tree, it is determined that the stopping condition has been reached. Otherwise, it is determined that the stopping condition has not been reached and it is necessary to continue iteratively updating the growth tree based on the growth rules.

[0068] The above is a description of the path planning method in this application.

[0069] Figure 3Here is an example. The black circles and pentagons in the figure represent obstacles. Node 1 represents the starting point and node 13 represents the ending point. Nodes 1 to 13 represent the nodes of the growth tree. Based on the growth tree after reaching the stopping condition, a movement path from the starting point 1 to the ending point 13 can be obtained, which is the movement path represented by "node 1-node 2-node 4-node 7-node 8-node 9-node 12-node 13" in the figure.

[0070] This application proposes a path planning method that enables a robot to automatically plan a motion path given the starting point and the ending point.

[0071] After obtaining the motion path, it can be seen that it can include several path segments. For example, in the figure, "node 1-node 2-node 4-node 7-node 8-node 9-node 12-node 13" can represent a path segment between two adjacent nodes.

[0072] In some embodiments, after determining the motion path, the control quantities during the intermediate process of the robot can be estimated further, given the starting and ending points of the path and the state variables. In some embodiments, the solution of this application makes the robot's motion as smooth as possible and minimizes energy loss.

[0073] Please refer to Figure 4 In some embodiments, the path planning method further includes step 2000, performing motion control on each segment of the path; in some embodiments, step 2000 includes the following steps:

[0074] Step 2100: For each path segment, obtain the state value of the starting point, the state value of the ending point, and the constraint duration T of the path segment R (hereinafter referred to as path R).

[0075] In some embodiments, the state variables include one or more of position parameters, velocity parameters, and acceleration parameters.

[0076] In some embodiments, the values ​​of the state variables at the starting point and the ending point include the values ​​of position parameters; in some embodiments, the values ​​of the state variables at the starting point and the ending point also include the values ​​of velocity parameters; in some embodiments, the values ​​of the state variables at the starting point and the ending point also include the values ​​of acceleration parameters.

[0077] Step 2200: Construct control variables based on state variables.

[0078] In some embodiments, the control quantities include one or more of velocity parameters, acceleration parameters, and jerk parameters.

[0079] In some embodiments, step 2200 involves differentiating the state quantity to obtain the control quantity. In some embodiments, when the state quantity includes a position parameter, the control quantity includes a velocity parameter; in some embodiments, when the state quantity includes a velocity parameter, the control quantity includes an acceleration parameter; in some embodiments, when the state quantity includes an acceleration parameter, the control quantity includes a jerk parameter.

[0080] Step 2300: Construct an objective function based on state variables and control variables. The objective function is used to represent the energy loss of the robot during its movement along the path segment.

[0081] In some embodiments, step 2300 constructs an objective function H(s(t),i(t),λ(t)), where s(t) represents the state variable, u(t) represents the control variable, λ(t) represents the coefficient of u(t), and t is a time variable.

[0082] Step 2400: Solve for the optimal control quantity based on the objective function and constraints, so as to obtain the values ​​of the control parameters at each position point or at each time in the path segment R based on the optimal control quantity; wherein, the optimal control quantity minimizes the energy loss of the robot during the movement of the path segment R, and the constraints include: the values ​​of the state quantities at the start and / or end of the path segment R, and the constraint duration T.

[0083] In some embodiments, step 2400, which solves for the optimal control quantity based on the objective function and constraints to obtain the values ​​of the control parameters at each position point or at each time point in the path R, can be performed as follows:

[0084] Step 2400 performs the first calculation on the objective function H(s(t), u(t), λ(t)) to obtain the expression of λ(t) with respect to time t, where the expression contains the variable t and the constant to be solved.

[0085] In some embodiments, step 2410 takes the partial derivative of the objective function H(s(t), u(t), λ(t)) with respect to the state variable s(t) to obtain the partial derivative of λ(t). Then with respect to the partial derivative Integrating over time t yields an expression for λ(t) with respect to time t.

[0086] Step 2400: In the objective function H(s(t), u(t), λ(t)), where s(t) is the optimal state variable, solve for the optimal control variable to obtain the simplified objective expression; where the simplified objective expression is an expression for the optimal control parameter and λ(t).

[0087] In some embodiments, the optimal control parameter is the optimal jerk parameter;

[0088] Step 2400: Set the partial derivative of the above objective expression with respect to the above optimal control parameters to zero, and solve for the expression of the above optimal control parameters, which includes the variable t and the constant to be solved in λ(t).

[0089] Step 2400 involves integrating the above-mentioned optimal control parameters once or multiple times over time t to obtain an expression for the optimal state quantity, which includes the variable t and the constant to be solved in λ(t).

[0090] In the expression of the optimal state quantity, step 2400 takes the value of the state quantity at the starting point of the path R as the value of the optimal state quantity at t=0, and takes the value of the state quantity at the starting point of the path as the value of the optimal state quantity at t=T, and solves for the value of the constant to be solved in λ(t).

[0091] Step 2400 substitutes the values ​​of the constants that have been solved in λ(t) back into the expression of the optimal state quantity, thereby obtaining the values ​​of the control parameters at each position point or at each time in the path R segment.

[0092] Step 2500: Control the robot to move on the path segment R based on the values ​​of the control parameters at each position point or at each time point.

[0093] The motion path obtained in step 1000 may include one or more path segments. Step 2000 is performed for any one of these path segments to obtain motion control for the entire motion path.

[0094] Let's denote the state variables as s(t) and take the state variables s(t) = [p(t), v(t), a(t)], where p(t), v(t), and a(t) represent position, velocity, and acceleration, respectively, as an example for illustration.

[0095] Step 2100: Obtain the value of the state variable s(t) at the starting point of path R, the value of the state variable s(t) at the ending point, and the constraint duration T of the path R segment; where the state variable s(t) = [p(t), v(t), a(t)], p(t), v(t) and a(t) represent position, velocity and acceleration, respectively.

[0096] In some embodiments, the values ​​of the starting point state variable s(t) and the ending point state variable s(t) obtained in step 2100 include the value of p(t); in some embodiments, the values ​​of the starting point state variable s(t) and the ending point state variable s(t) obtained in step 2100 also include the value of v(t); in some embodiments, the values ​​of the starting point state variable s(t) and the ending point state variable s(t) obtained in step 2100 also include the value of a(t).

[0097] Step 2200: Differentiate the state variable s(t) = [p(t), v(t), a(t)] to construct the control variable u(t) = [v(t), a(t), j(t)], where j(t) is the jerk.

[0098] Step 2300: Based on the state variables s(t) = [p(t), v(t), a(t)] and the control variables u(t) = [v(t), a(t), j(t)], construct the objective function H(s(t), u(t), λ(t)), where λ(t) represents the coefficient of u(t); the objective function is used to represent the energy loss of the robot during its movement along the path R.

[0099] Step 2400 solves for the optimal control quantity u based on the objective function H(s(t), u(t), λ(t)) and constraints. * (t) is used to obtain the values ​​of the control parameters at each location point or at each time point in the path R; where the optimal control quantity u is... * The constraint (t) minimizes the energy loss of the robot during its movement along the path R. The constraints include the values ​​of the state variables s(t) at the start and / or end of the path R, and the constraint duration T.

[0100] In some embodiments, step 2400 involves taking the partial derivative of the objective function H(s(t), u(t), λ(t)) with respect to the state variable s(t) to obtain the partial derivative of λ(t). Step 2400 with respect to the partial derivative Integrating over time t yields an expression for λ(t) with respect to time t, which includes the variable t and the constants α, β, and γ to be solved in λ(t); Step 2400: In the objective function H(s(t), u(t), λ(t)), s(t) is the optimal state variable s. * (t)=[p * (t), v * (t), a * Given [t], solve for the optimal control quantity u. * (t)=[v * (t), a * (t), j * [t], thus obtaining the simplified objective formula:

[0101] j * (t)=arg min j(t) H(s * (t), j(t), λ(t));

[0102] Step 2400 addresses H(s) in the target formula. * Taking the partial derivative of j(t), j(t), and λ(t) with respect to λ(t), and setting it equal to zero, we can obtain the optimal jerk j.* The expression for (t) includes the variable t and the constants α, β, and γ to be solved; Step 2400: Calculate the optimal jerk j. * The optimal acceleration a is obtained by performing a first-order integral on (t). * (t), perform a second-order integral to obtain the optimal velocity v. * (t), and perform a third-order integral to obtain the optimal position p. * (t), thus obtaining the expression for the optimal state quantity s. * (t)=[p * (t), v * (t), a * [(t)], the expression includes the variable t and the constants α, β, γ to be solved; step 2400 takes the value of the state quantity s(t) at the starting point of the path R as the value of the optimal state quantity at t=0, and takes the value of the state quantity s(t) at the starting point of the path R as the value of the optimal state quantity at t=T, and substitutes it into the expression of the optimal state quantity s * (t)=[p * (t), v * (t), a * (t)], solve for the values ​​of constants α, β, and γ; step 2400 substitutes the values ​​of constants α, β, and γ obtained from the solution back into the expression s of the optimal state quantity. * (t)=[p * (t), v * (t), a * [(t)], to obtain the values ​​of the control parameters at the location points or at each time point in the path R segment.

[0103] Let's take the objective function constructed in step 2300 as an example. To further illustrate, the process of solving for the control parameters in step 2400 will be explained in more detail.

[0104] Taking the partial derivative of the objective function H(s(t), u(t), λ(t)) with respect to the state variable s(t), we obtain the partial derivative of λ(t): Among them, s * (t) and u * (t) represent the optimal state variable and the optimal control variable, respectively; the specific process can be as follows:

[0105] Since the control variables include velocity, acceleration, and jerk, λ(t) also correspondingly includes the coefficients λ1(t) for velocity, λ2(t) for acceleration, and λ3(t) for jerk; therefore, the objective function...

[0106] The partial derivatives of the objective function with respect to the state variables are as follows: The derivatives are taken with respect to the positions of the state variables. Differentiate the velocity in the state variables. Differentiate the acceleration in the state variables.

[0107] because Therefore, λ1(t) is a constant, and without loss of generality, let... Understandably, this is just one example of the constructed λ1(t), and in practice, users can construct other forms of λ1(t) according to their needs.

[0108] And because Therefore, integrating over t yields...

[0109] And because Therefore, integrating over t yields...

[0110] This can be expressed as an expression for λ(t) with respect to time t:

[0111]

[0112] Next, the optimal u(t) is solved based on the optimal state of s(t). The specific process can be as follows:

[0113] s at t=0 and t=T * (0) and s * (T) is known—that is, the value of the state variable s(t) at the starting point of the path R obtained in step 2100, and the value of the state variable s(t) at the ending point;

[0114] Since s(t) = [p(t), v(t), a(t)] and u(t) = [v(t), a(t), j(t)], when s(t) = [p(t), v(t), a(t)] is optimal, p(t), v(t), and a(t) are also optimal. Since u(t) = [v(t), a(t), j(t)], we only need to find the optimal j(t). Therefore, the problem is solved by u... * (t)=arg min u(t) H(s * The transformation of (t), u(t), λ(t)) into:

[0115] j * (t)=arg min j(t) H(s * (t), j(t), λ(t));

[0116] Based on the expression for λ(t) with respect to time t obtained from the above solution, we can further obtain:

[0117]

[0118] Let the target formula be After taking the partial derivative of j(t), set it to zero, that is... Therefore, the solution is obtained as follows:

[0119]

[0120] Next, based on the solved j * (t), and s * (0) and s * Given these two quantities (T), we can solve for the values ​​of the three variables α, β, and γ; the specific process can be as follows:

[0121] Regarding the jerk j * (t) is integrated first-orderly to obtain the acceleration a. * (t), the velocity v is obtained by performing a second-order integral. * (t), and perform a third-order integral to obtain the position p. * (t), and when t = 0, p * (t), v * (t), a * (t) is known and is considered optimal; therefore, we obtain:

[0122]

[0123] In the formula, p0, v0, and a0 represent the initial positions p. * (0), velocity v * (0) and acceleration a * (0);

[0124] Let t = T, and the position of the endpoint p of path R. * (T), velocity v * (T) and acceleration a * (T) represents p f v f a f Substituting the known quantities into the above expression, we can obtain:

[0125]

[0126] The three variables α, β, and γ have exactly three equations, therefore they can be solved to obtain:

[0127]

[0128] in, Δv=v f-v0-a0T, Δa=a f -a0.

[0129] Therefore, based on the solved α, β, and γ, the values ​​of the control parameters at the position points or at each time point in the path segment R can be obtained. For example, the solved α, β, and γ can be substituted back into s. * From (t), the optimal state variables (control variables) at each time t can be obtained:

[0130]

[0131] The above is based on the known location p * (0), velocity v * (0) and acceleration a * (0), and knowing the location p * (T), velocity v * (T) and acceleration a * In the case of (T), this is an example of controlling the three control variables—position, velocity, and acceleration—separately during the robot's motion; in some embodiments, only position and velocity can be controlled among the three, in which case only the position p needs to be known. * (0), velocity v * (0), p * (T) and velocity v * (T) is sufficient; in some embodiments, only position can be controlled among position, velocity, and acceleration. In this case, only the position p needs to be known. * (0) and p * (T) is sufficient.

[0132] For example, if only position and velocity are controlled among position, velocity, and acceleration, then:

[0133]

[0134] Since the acceleration is not controlled, let λ3(t) = -αt 2 -2βt-2γ=0, therefore, by combining the equations, we can obtain:

[0135]

[0136] Therefore, we can obtain:

[0137]

[0138] For example, when only position is controlled among position, velocity, and acceleration, since velocity and acceleration are not controlled (i.e., not required), we can let λ3(t) = -αt.2 -2βt-2γ=0 and λ2(t)=2αt+2β=0, therefore we can obtain:

[0139]

[0140] Therefore, we can obtain:

[0141]

[0142] This document describes various exemplary embodiments with reference to them. However, those skilled in the art will recognize that changes and modifications can be made to the exemplary embodiments without departing from the scope of this document. For example, various operational steps and components for performing operational steps can be implemented in different ways depending on the specific application or considering any number of cost functions associated with the operation of the system (e.g., one or more steps can be deleted, modified, or combined with other steps).

[0143] In the above embodiments, implementation can be achieved, in whole or in part, by software, hardware, firmware, or any combination thereof. Furthermore, as those skilled in the art will understand, the principles herein can be reflected in a computer program product on a computer-readable storage medium pre-loaded with computer-readable program code. Any tangible, non-transitory computer-readable storage medium may be used, including magnetic storage devices (hard disks, floppy disks, etc.), optical storage devices (CD-ROMs, DVDs, Blu-ray discs, etc.), flash memory, and / or the like. These computer program instructions can be loaded onto a general-purpose computer, special-purpose computer, or other programmable data processing apparatus to form a machine, such that instructions executing on the computer or other programmable data processing apparatus can generate means for implementing a specified function. These computer program instructions can also be stored in a computer-readable storage medium that can instruct the computer or other programmable data processing apparatus to operate in a particular manner, such that instructions stored in the computer-readable storage medium can form an article of manufacture including means for implementing the specified function. The computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to perform a series of operational steps on the computer or other programmable apparatus to produce a computer-implemented process, such that instructions executing on the computer or other programmable apparatus can provide steps for implementing the specified function.

[0144] While the principles herein have been illustrated in various embodiments, numerous modifications to the structure, arrangement, proportions, elements, materials, and components, particularly suited to specific environmental and operational requirements, may be used without departing from the principles and scope of this disclosure. These modifications and other alterations or alterations will be included within the scope of this document.

[0145] The foregoing specific descriptions have been described with reference to various embodiments. However, those skilled in the art will recognize that various modifications and changes can be made without departing from the scope of this disclosure. Therefore, considerations for this disclosure are to be illustrative rather than restrictive, and all such modifications are to be included within its scope. Similarly, advantages, other advantages, and solutions to problems with respect to various embodiments have been described above. However, benefits, advantages, solutions to problems, and any elements that produce these, or make them more explicit, should not be construed as critical, essential, or necessary. The term “comprising” and any other variations thereof as used herein are non-exclusive inclusion, meaning that a process, method, article, or apparatus that includes a list of elements includes not only those elements but also other elements not expressly listed or not part of the process, method, system, article, or apparatus. Furthermore, the term “coupled” and any other variations thereof as used herein refer to physical connections, electrical connections, magnetic connections, optical connections, communication connections, functional connections, and / or any other connections.

[0146] Those skilled in the art will recognize that many changes can be made to the details of the above embodiments without departing from the basic principles of the invention. Therefore, the scope of the invention should be determined only by the claims.

Claims

1. A path planning method for a robot, characterized in that, include: Obtain the relevant trajectory information of the robot in the pre-built space; The relevant trajectory information includes initial path nodes, which include a first initial path node and a second initial path node, wherein the first initial path node and the second initial path node are the start point and the end point, or vice versa; The initial path node is used as a seed node to construct a growth tree, and the growth tree is iteratively updated based on the growth rules until a stopping condition is reached, so as to obtain a path from the first initial path node to the second initial path node based on the iteratively updated growth tree. Wherein, when the current growth tree to be updated is not the same growth tree as the current reference tree, the iterative update of the growth tree based on growth rules includes: For the current growth tree to be updated, a random point is generated in the pre-built space; Search for the node closest to the random point in the current reference tree, where the reference tree is also a growth tree obtained based on the growth rule; Based on the random point and the nearest node, determine the candidate nodes of the growth tree that are currently to be updated; Obtain the node currently to be grown in the growth tree to be updated, and determine whether there is an obstacle between the node and the candidate node. If there is no obstacle, the candidate node is used as a child node of the node currently to be grown to update the growth tree. If the stopping condition is not reached after the update, the growth tree to be updated is kept as the growth tree to be updated while continuing to iterate and update the growth tree based on the growth rules. If there is an obstacle, the candidate node is abandoned, and the roles of the growth tree to be updated and the reference tree are swapped while continuing to iterate and update the growth tree based on the growth rules.

2. The path planning method as described in claim 1, characterized in that, When the reference tree and the current growth tree to be updated are the same growth tree, the growth tree uses the first initial path node as the seed node. The node to be grown is the node in the current growth tree that is closest to the random point.

3. The path planning method as described in claim 2, characterized in that, The step of determining the candidate nodes of the growth tree to be updated based on the random point and the nearest node includes: Based on the random point and the nearest node, a first direction from the nearest node to the random point is determined; The point that is the closest node in the first direction to the node with a preset step size is selected as the candidate node.

4. The path planning method as described in claim 2, characterized in that, The iterative update of the growth tree based on growth rules also includes: After updating the growth tree by treating the candidate node as a child node of the current node to be grown, it is further determined whether the candidate node is within the preset neighborhood of the second initial path node and whether there are obstacles between them: if the candidate node is not within the preset neighborhood of the second initial path node or there are obstacles between them, a random point is generated in the pre-built space to continue iteratively updating the growth tree based on the growth rules; if the candidate node is within the preset neighborhood of the second initial path node and there are no obstacles between them, it is determined that the stopping condition has been reached.

5. The path planning method as described in claim 1, characterized in that, The step of constructing a growth tree using the initial path node as a seed node includes: constructing a first growth tree using the first initial path node as a seed node, and constructing a second growth tree using the second initial path node as a seed node. In the iterative update of the growth tree based on growth rules: If the current growth tree to be updated is the first growth tree, then the current reference tree is the second growth tree, and the current node to be grown is the last node in the first growth tree. Furthermore, if there are no obstacles and the candidate node is used as a child node of the current node to be grown to update the current growth tree, and the stopping condition is not reached, then while iteratively updating the growth tree based on the growth rules, the first growth tree remains the growth tree to be updated. If there are obstacles and the candidate node is abandoned while iteratively updating the growth tree based on the growth rules, then the first growth tree is switched to the reference tree, and the second growth tree is switched to the growth tree to be updated. If the current growth tree to be updated is the second growth tree, then the current reference tree is the first growth tree, and the current node to be grown is the last node in the second growth tree. Furthermore, if there are no obstacles and the candidate node is used as a child node of the current node to be grown to update the current growth tree, and if the stopping condition is not reached, then while iterating and updating the growth tree based on the growth rules, the second growth tree will remain the growth tree to be updated. If there are obstacles and the candidate node is abandoned while iterating and updating the growth tree based on the growth rules continues, then the second growth tree will be switched as the reference tree, and the first growth tree will be switched as the growth tree to be updated.

6. The path planning method as described in claim 5, characterized in that, The step of determining the candidate nodes of the growth tree to be updated based on the random point and the nearest node includes: Based on the random point and the nearest node, a second direction from the random point to the nearest node is determined; A point that is a preset step size away from the current node to be grown in the second direction is selected as the candidate node.

7. The path planning method as described in claim 5, characterized in that, The iterative update of the growth tree based on growth rules also includes: After updating the growth tree by using the candidate node as a child node of the current node to be grown, it is further determined whether there is a first target node and a second target node between the currently updated growth tree and the reference tree that are less than a preset distance and have no obstacles between them. If they exist, it is determined that the stopping condition has been reached; otherwise, it is determined that the stopping condition has not been reached.

8. A path planning device for a robot, characterized in that, include: Memory, used to store programs; A processor for implementing the method as described in any one of claims 1 to 7 by executing a program stored in the memory.

9. A computer program product, characterized in that, When the computer program product is run on a processor, the processor is caused to execute to implement the method as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, Includes a program that can be executed by a processor to implement the method as described in any one of claims 1 to 7.

Images