A Sequential Constraint Sampling Path Planning Method for a Robot Arm

Abstract

The present invention discloses a robotic arm sequence constraint sampling path planning method, including: Step 1, determining effective sampling points by ellipse heuristic; Step 2, turning neighboring points to sampling points under the manifold structure heuristic strategy; Step 3, turning neighboring points to the manifold intersection under the manifold structure heuristic strategy; Step 4, re-layout of the RRT* tree; Step 5, generating the next constrained space RRT* virtual root node. The present invention uses a heuristic prior-assisted planning method to effectively enhance the effectiveness of the nodes in the RRT* tree, enhance the guiding effect of the sequence constraint intersection sub-goal, and at the same time accelerate the convergence speed of the algorithm and improve the real-time performance.

Description

Technical Field

[0001] The present invention belongs to the field of sampling path planning algorithms for robotic arm motion planning, and relates to a method for sampling path planning with sequential constraints for a robotic arm, in particular, a method for sampling path planning with sequential constraints for a robotic arm based on heuristic prior knowledge. Background Art

[0002] Sequential constraint path planning is a sequential sub-problem of robotic arm motion planning, which is a combination of sequential manifold constraint satisfaction and path planning.

[0003] Given a representation of the target manifold constraint, sequential constraint path planning needs to achieve the satisfaction of all path points in a path for the current constraint, obstacle avoidance, and the selection of sub-goals, so that the path evaluation index is as high as possible. For example, when a robotic arm performs the task of placing a water-filled cup on a table, it is necessary to plan a path that satisfies the sequential constraints, that is: 1. Keep the cup as horizontal as possible; 2. Place the cup horizontally on the table. At the same time, the planning process needs to intelligently determine a configuration between the two constraints so that the cup can be placed horizontally on the table, which is the sub-goal of the first sequential constraint process. In addition, the path still needs to satisfy the requirements of relevant functions and indicators as much as possible, such as obstacle avoidance, planning time, path cost, constraint satisfaction metric, etc.

[0004] Sampling-based robotic arm path planning algorithms are mainly divided into graph-based sampling path planning algorithms and tree-based sampling path planning algorithms. Among them, the graph-based sampling path planning algorithm is based on PRM, and gradually exposes the search space by performing incremental search sampling in the configuration space, and establishes a search tree for planning. It has high accuracy but low speed and is not suitable for real-time planning tasks. The tree-based path planning algorithm is a single-query mechanism, which can better adapt to dynamic environments and high-dimensionality. Therefore, it greatly speeds up the planning speed and is easy to apply to actual projects. As a tree-based path planning algorithm with an optimization process, RRT* has significant advantages in asymptotic optimality. Since RRT* compares the optimal node of a point based on the neighboring nodes of a point in the tree and re-wires for each node, it is difficult to maintain the original real-time performance while ensuring asymptotic optimality. In addition, in order to ensure that the RRT* path nodes satisfy the constraints, an iterative projection process needs to be introduced for the nodes, which further destroys the time performance of RRT*. Summary of the Invention

[0005] Aiming at the above-mentioned prior art, the technical problem to be solved by the present invention is to provide a method for sampling path planning with sequential constraints for a robotic arm based on heuristic prior knowledge, which uses the method of heuristic prior-assisted planning to effectively enhance the effectiveness of the nodes in the RRT* tree, enhance the guiding role of the sequential constraint intersection sub-goals, and at the same time accelerate the convergence speed of the algorithm and improve the real-time performance.

[0006] To solve the above technical problems, the present invention provides a robotic arm sequential constraint sampling path planning method, comprising:

[0007] Step 1: uniformly sample the sampling points q in the configuration space rand If there is no planning path result in the current constraint stage, the tree adjacent point q is determined based on the sampling point near , jump to step 2; if there is a planning result in the current constraint stage, perform operation a: then construct an ellipse heuristic prior based on the current path result cost value to determine whether the point has an asymptotically optimal effect on the planned path. If q rand If the ellipse heuristic prior is satisfied, the sampling point is retained and the tree adjacent point q is determined based on the sampling point. near , jump to step 2; if the ellipse heuristic prior is not satisfied, the point is discarded, and it is determined whether the set maximum number of sampling times is reached. If so, the planning is terminated; otherwise, uniform sampling is performed again in the configuration space to obtain a new sampling point, and the operation a is returned to be executed;

[0008] Step 2: q near The corresponding q rand Perform a uniform sampling of (0, 1) to obtain the probability parameter a. If a is less than the set probability parameter b, proceed to step 4, otherwise proceed to step 3.

[0009] Step 3: q obtained in step 1 near Under the manifold structure heuristic strategy, the corresponding sampling point q rand Expand one step to get the extension point q extend ; Simplify the expansion step to a straight line, the manifold tangent space corresponding to the current constraint limits q near The direction of the step is based on the end point of the step and q rand The distance is the cost, with the initial point as q near , the expansion step satisfies the current constraint and performs the optimization process as the constraint condition, and obtains the expansion step and expansion point q extend , execute step five;

[0010] Step 4: q obtained in step 1 near Under the manifold structure heuristic strategy, the intersection M of the current constraint and the next constraint in the sequence is calculated. i ∩M i+1 Expand one step to get the extension point q extend , simplifying the expansion step to a straight line, the manifold tangent space corresponding to the current constraint limits q near The direction of the step is determined by the measurement of whether the end point of the step satisfies the next constraint. The initial point is q near , the expansion step satisfies the current constraint and performs the optimization process as the constraint condition, and obtains the expansion step and expansion point q extend, execute step five;

[0011] Step 5: Add the extension point q generated in step 3 or step 4 extend Perform judgment processing and project it into the corresponding constraint space to expand the RRT* tree: If q extend If the current constraint is satisfied, it is projected into the current constraint space, otherwise it is projected into the space corresponding to the next constraint, and q is obtained. new , change q new Perform optimal comparison of adjacent points with the existing RRT* tree and rewiring to complete the expansion of the RRT* tree, increase the number of current RRT* nodes, and execute step 6;

[0012] Step 6: Determine q new Check whether the next constraint is met. If so, record the node and determine whether the number of nodes in the constraint space reaches the expected number. If not, return to step 1 for the constraint. If so, mark the recorded node as a virtual root node and determine whether there is a next constraint. If so, switch the current constraint to the next constraint and return to step 1. If there is no next constraint, the planning ends.

[0013] Furthermore, the ellipse heuristic prior is specifically:

[0014] c(q rand )=g(q rand )+h(q rand )

[0015] where g(q rand ) represents the measurement to the starting point, h(q rand ) is expressed as the estimated metric to the target, Among them, M i+1 represents the i+1th manifold, Indicates M i+1 Jacobian matrix, is the constraint function corresponding to the next constraint, i.e. the i+1 constraint;

[0016] When c(q eand )<c best When the ellipse heuristic prior is satisfied, otherwise it is not satisfied, c best is the shortest path cost between the starting point and the target point of the robot arm in the planning task.

[0017] Furthermore, the expansion step obtained in step 3 is specifically as follows:

[0018]

[0019] Among them, M i represents the i-th manifold, J Mi Indicates Mi The Jacobian matrix, where V contains singular vectors that span the right null space;

[0020] Normalize d and use it as a single-step extension of the newly generated point:

[0021]

[0022] where α is the set step size.

[0023] Furthermore, the specific extension step obtained in Step 4 is as follows:

[0024] Obtain d through a system of linear equations:

[0025]

[0026] where λ is the Lagrange variable, representing M i+1 the Jacobian matrix, J Mi representing M i the Jacobian matrix, is the next constraint, that is, the constraint function corresponding to the (i + 1)-th constraint;

[0027] Normalize d and use it as a single-step extension of the newly generated point:

[0028]

[0029] where α is the set step size.

[0030] Advantages of the present invention: The main problem solved by the present invention is the low real-time performance and sub-goal uncertainty problems that occur when RRT* is used for sequential constraint path planning tasks. For this reason, the present invention provides a method for using heuristic priors to assist in planning, which can effectively enhance the effectiveness of nodes in the RRT* tree, enhance the guiding role of the sequential constraint intersection sub-goals, and at the same time accelerate the convergence speed of the algorithm and improve real-time performance.

[0031] Compared with the prior art, the present invention has the following advantages:

[0032] 1. The present invention solves the problem of poor real-time performance caused by the ineffectiveness of some sampling points in the RRT* algorithm for asymptotic path optimization by means of elliptical heuristic priors.

[0033] 2. The prior of the manifold structure is introduced in the process of obtaining the expansion point, thereby enhancing the success rate of the expansion step of the RRT* algorithm and the projection iteration speed, and can solve the problem of sub-goal uncertainty in the sequential constraint planning problem.

[0034] 3. The probability-based steering strategy is used to assist RRT* in spatial exploration, accelerating the convergence speed of the RRT* algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 is a flowchart of a method for robotic arm sequential sampling path planning based on heuristic prior of the present invention;

[0036] Figure 2 is an elliptical heuristic schematic diagram of a method for robotic arm sequential sampling path planning based on heuristic prior of the present invention;

[0037] Figure 3 is a heuristic evaluation schematic diagram of a method for robotic arm sequential sampling path planning based on heuristic prior of the present invention;

[0038] Figure 4 is a prior steering strategy structure diagram of a manifold structure for a method for robotic arm sequential sampling path planning based on heuristic prior of the present invention;

[0039] Figure 5 is a schematic diagram of virtual root node generation for a method for robotic arm sequential sampling path planning based on heuristic prior of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0040] The present invention will be further described below with reference to the accompanying drawings of the specification and embodiments.

[0041] Sequential constrained path planning requires establishing sub-goals in the sequential planning process and satisfying the indicators and functions of the planned paths in each sub-phase. RRT* depends on a random sampling process that gradually reveals the entire configuration space based on probability and introduces a re-wiring mechanism to optimally select neighboring nodes. By continuously performing the planning process, an asymptotically optimal planned path can be obtained. To meet the constraint conditions, a Jacobi projection mechanism is added to project the expansion point onto the corresponding constraint manifold. In this constrained RRT* algorithm, the satisfaction of the constraint projection and the core planning algorithm are independent of each other. The present invention mainly improves the sampling module and the steering module in the RRT* algorithm, uses the constraint function value as a measure of the non-deterministic goal to meet the requirements of sequential constrained path planning problems and improve the performance of the original algorithm. An elliptical prior is introduced to screen effective sampling points, and the effectiveness of the point is judged by the prior path cost, which accelerates the convergence of the asymptotically optimal process. Further, a prior of the constraint manifold structure is introduced to assist the algorithm in determining the steering direction. By using the tangent space straight line model, the projection iteration steps are reduced, and at the same time, the optimization cost of the expansion point is consistent with the non-deterministic goal measure, enabling the RRT* algorithm to have the ability to determine non-deterministic sub-goals in sequential constrained planning problems.

[0042] The present invention is implemented according to the following process:

[0043] Step 1: Determine effective sampling points using elliptical heuristics, specifically:

[0044] Uniformly sample in the configuration space to obtain the sampling point q rand , if there is no planned path result in the current constraint stage, determine the tree neighbor point q according to this sampling point near . If there is already a planned result in the current constraint stage, construct an elliptical heuristic prior according to the cost value of the current path result to judge whether this point has an asymptotically optimal effect on the planned path. If q rand satisfies this elliptical heuristic prior, retain this sampling point and execute the subsequent steps under the condition of no planned path result. If it does not satisfy this elliptical heuristic prior, discard this point, uniformly sample again in the configuration space to obtain a new sampling point, continue to judge whether it satisfies the elliptical heuristic prior, and perform the subsequent steps. After obtaining q near , perform a uniform sampling of (0, 1) to obtain the probability parameter a. If a is less than the set probability parameter b, enter Step 3, otherwise enter Step 2

[0045] Step 2: Neighbor point turns to sampling point under the manifold structure heuristic strategy

[0046] The q obtained in Step 1 near Expand one step towards the corresponding sampling point q under the manifold structure heuristic strategy rand to obtain the expansion point q extend . Simplify this expansion step into a straight line. The tangent space of the manifold corresponding to the current constraint limits the direction in which q near can step. Taking the distance between the stepping end point and q rand as the cost, with the initial point being q earr , execute the optimization process with the expansion step satisfying the current constraint as the constraint condition to obtain this expansion step and the expansion point q extend . Execute Step 4

[0047] Step 3: Neighbor point turns to the manifold intersection under the manifold structure heuristic strategy

[0048] The q obtained in Step 1 near Expand one step towards the intersection M of the current constraint and the next constraint in the sequence under the manifold structure heuristic strategy i ∩M i+1 to obtain the expansion point q extend . The stepping model assumption and the constraint conditions of the optimization problem are the same as in Step 2. In this process, select the measure of whether the stepping end point satisfies the next constraint as the cost, execute the optimization process to obtain this expansion step and the expansion point q extend . Execute Step 4

[0049] Step 4: Re-layout the RRT* tree, specifically:

[0050] The expansion point q generated in Step 2 or Step 3 extendPerform a judgment process, project it onto the corresponding constraint space, and expand the RRT* tree. If q extend satisfies the current constraint, project it onto the current constraint space; otherwise, project it onto the space corresponding to the next constraint to obtain q new . Compare q new with the existing RRT* tree for the optimal comparison of neighboring points and rewire it to complete the expansion of the RRT* tree and increase the number of current RRT* nodes. Execute Step Five.

[0051] Step Five: Generate the RRT* virtual root node for the next constraint space, specifically:

[0052] Judge whether q new satisfies the next constraint. If it does, record this node. Judge whether the number of nodes in this constraint space reaches the expected number. If it has not reached, still execute Step One for this constraint. If it has reached, mark the recorded node as the virtual root node, judge whether there is a next constraint. If there is a next constraint, switch the current constraint to the next constraint and then return to Step One. If there is no next constraint, the planning ends.

[0053] The following is an embodiment in conjunction with the accompanying drawings:

[0054] The present invention includes the following steps:

[0055] 1. Determine effective sampling points by ellipse heuristic

[0056] The schematic diagram of the ellipse prior is as shown in Figure 2 . The ellipse prior means that the sum of the distances from the points on the ellipse to the two foci of the ellipse is the same. The sum of the distances from the points outside the ellipse to the two foci is greater than the sum of the distances from the points on the ellipse to the two foci, and vice versa for the points inside the ellipse. After RRT* has obtained a feasible path, the sampling space can be shrunk to an elliptical region, and the points outside the region have no practical value for shortening the length of the planned path. The shortest path is used as c best , and the distance between the starting point and the ending point is used as c min to construct the ellipse in this way. When there is no planning result yet, c best is inf, which is no different from the RRT* planning process.

[0057] For the heuristic metric c(q rand ) = g(q rand ) + h(q rand ), where g(q rand ) represents the metric to the starting point, and h(q rand)Denoted as the estimated metric to the target. In this problem, the starting point is a point in the configuration space, so the corresponding Euclidean distance is selected as this metric. For the target position, this target is often implicitly represented by the intersection between the current constraint and the next constraint. Since the planning algorithm ensures that each node satisfies the current constraint, the positive definite form of the constraint function of the next constraint is selected as the metric from this sampling point to the target, that is: Where is the constraint function corresponding to the next constraint, is its Jacobian, and its schematic diagram is as shown in Figure 3 Figure.

[0058] 2. Manifold Structure Heuristic Strategy for Steering Sampling Point Expansion

[0059] In this process, the problem can be described as obtaining a curve from q near to q rand in the constraint space where q rand is the sampling point, is the manifold space satisfying the i-th constraint, and q near is the neighboring point of q rand on the constraint manifold M i . This problem is formulated as a constrained optimization problem for finding the curve, and its mathematical representation is as follows:

[0060] min||γ(1)-q rand || 2

[0061] s.t.γ(0)=q near

[0062]

[0063] Due to the non-linear constraints, this problem is difficult to solve. The steering operation is called multiple times in the inner loop of the algorithm. This method selects a simple curve representation and only calculates an approximate solution to this problem. The curve is approximately simplified using the straight-line process where the direction d is selected as the orthogonal projection of q rand -q near onto the tangent space of the manifold at q near . Applying the first-order Taylor formula for expansion to the constraint function gives This problem can be solved as:

[0064] ]>

[0065] where V contains singular vectors spanning the right null space.

[0066] Normalize d in the algorithm as a single-step expansion of the newly generated point. The new expansion point lies on the tangent space of the manifold at configuration q near and thus requires only a few projection steps before it is added to the tree.

[0067] 3. The manifold structure heuristic strategy turns to constrained manifold intersection expansion

[0068] This turning step expands the tree from q near to the intersection of the current and the next manifold M i ∩M i+1 which can be expressed as an optimization problem:

[0069]

[0070] s.t. γ(0) = q near

[0071]

[0072] The difference from the turning sampling point problem is that the loss function is now specified according to the distance to the next manifold This cost pulls the robot's configuration towards the intersection of the adjacent sequence constraint spaces. Similar to the simplification process in the turning sampling point procedure, a straight-line process is used to approximate the curve and the first-order Taylor expansion is applied to the non-linear terms, which leads to the simplification of the problem for easy solution:

[0073]

[0074]

[0075] d can be obtained by the following system of linear equations:

[0076]

[0077] where λ is the Lagrange variable. Similarly, the new expansion point lies on the tangent space of the manifold at configuration q near and can be uniformly expressed as: where α is the set step size.

[0078] 4. RRT* tree rewiring

[0079] Project the obtained q extend onto the corresponding constraint space to get q new and expand this point into the RRT* tree through cost iteration comparison and rewiring. For q extend, verify whether it satisfies the next constraint. According to the verification result, project its Jacobian pseudoinverse into the corresponding constraint space. For the judged constraint space, there is a constraint function f(q) corresponding to it, its Jacobian matrix is J(q), and the Jacobian pseudoinverse matrix is J + (q), continuously iterate and execute q extend = q extend - J + (q extend )f(q extend ) until the corresponding constraint is satisfied, and a new extended point q new that satisfies the constraint can be obtained.

[0080] For the expandable point q new , the RRT* tree determines neighboring nodes in a set radius range near it, searches for the optimal parent node among them, calculates the current cost of the alternative parent nodes within the range plus the path cost from the alternative parent nodes to the expandable point in turn, and selects the node with the minimum cost as the parent node. After determining the parent node, a rewiring process is carried out. This process can be described as for the new extended point q new , if this point can further reduce the cost of the neighboring nodes, then adjust the parent node of the neighboring nodes to the new extended point q new .

[0081] 5. Generate the virtual root node of the next constraint space in RRT*

[0082] As the loop logic of this algorithm, it includes judging whether the desired number of nodes in the current constraint space has been explored and judging whether there is still a next constraint space. If the current constraint space has been explored and there is a next constraint, establish a virtual root node for the nodes recorded in the intersection part of the constraints to facilitate marking the new stage and algorithm call. The schematic diagram of the virtual root node is as shown in Figure 5 . The green part is the previous constraint space, and the red part is the next constraint space connected to it. This algorithm can naturally determine the intersection nodes during the exploration process and mark the nodes in the intersection as the virtual root nodes of the next constraint space.

Claims

1. A method for sampling path planning of a robotic arm sequence constraint, characterized in that including: Step 1: Uniformly sample in the configuration space to obtain a sampling point q rand , if there is no planned path result in the current constraint stage, then determine the tree neighbor point q based on this sampling point near , and jump to Step 2; If there is a planning result in the current constraint stage, perform operation a: construct an elliptical heuristic prior according to the cost value of the current path result to determine whether this point has an asymptotically optimal effect on the planned path. If q rand satisfies the elliptical heuristic prior, retain this sampling point, and determine the tree neighbor point q according to this sampling point near , and jump to step 2; if it does not satisfy the elliptical heuristic prior, discard this point, determine whether the set maximum sampling times have been reached, if so, end the planning; otherwise, perform uniform sampling in the configuration space again to obtain a new sampling point, and return to execute operation a; Step 2. For q near The corresponding q rand Perform a uniform sampling of (0, 1) to obtain the probability parameter a. If a is less than the set probability parameter b, go to Step 4; otherwise, go to Step 3. Step 3. Take the q obtained in Step 1 near Under the manifold structure heuristic strategy, expand it by one step towards the corresponding sampling point q rand to obtain the expansion point q extend ; Simplify the expansion step to a straight line. The tangent space of the manifold corresponding to the current constraint restricts the direction in which q near can step. Taking the distance between the stepping end point and q rand as the cost and the initial point as q near , perform the optimization process with the condition that the expansion step satisfies the current constraint to obtain the expansion step and the expansion point q extend , and execute Step 5; Step 4: Take the q obtained in Step 1 near Under the manifold structure heuristic strategy, expand the intersection M i ∩M i+1 by one step to obtain the expansion point q extend , simplify the expansion step to a straight line, and the tangent space of the manifold corresponding to the current constraint limits the direction in which q near can step. Take the measure of whether the step end point satisfies the next constraint as the cost, and take the initial point as q near , and execute the optimization process with the expansion step satisfying the current constraint as the constraint condition to obtain the expansion step and the expansion point q extend , and execute Step 5; Step 5: Add the extension point q generated in step 3 or step 4 extend Perform judgment processing and project it into the corresponding constraint space to expand the RRT* tree: If q extend If the current constraint is satisfied, it is projected into the current constraint space, otherwise it is projected into the space corresponding to the next constraint, and q is obtained. new , q new Perform optimal comparison of adjacent points with the existing RRT* tree and rewiring to complete the expansion of the RRT* tree, increase the number of current RRT* nodes, and execute step 6; Step 6. Determine q new whether it meets the next constraint. If it does, record the node, and determine whether the number of nodes in this constraint space reaches the expected number. If it has not reached, still return to Step 1 for this constraint; if it has reached, mark the recorded node as the virtual root node, determine whether there is a next constraint. If there is a next constraint, switch the current constraint to the next constraint and then return to Step 1; if there is no next constraint, the planning ends.

2. A robotic arm sequence constraint sampling path planning method according to claim 1, characterized in that: The elliptical heuristic prior specifically is: c(q rand ) = g(q rand ) + h(q rand ) where g(q rand ) represents the metric to the starting point, h(q rand ) represents the estimated metric to the target, where M i+1 represents the (i + 1)-th manifold, represents the Jacobian matrix of M i+1 , and f Mi+1 is the next constraint, that is, the constraint function corresponding to the (i + 1)-th constraint; When c(q rand ) < c best , the elliptical heuristic prior is satisfied; otherwise, it is not satisfied. c best is the cost of the shortest path between the starting point and the target point of the robotic arm in the planning task.

3. A method for planning a sampling path with sequence constraints for a robotic arm according to claim 1, characterized in that: The specific way to obtain the expansion step in Step 3 is: Among them, M i represents the i-th manifold, and J Mi represents the Jacobian matrix of M i , and V contains singular vectors that span the right null space; Normalize d and use it as the single-step expansion of the newly generated point: where α is the set step size.

4. A robotic arm sequence constraint sampling path planning method according to claim 1, characterized in that: The specific way to obtain the expansion step in Step 4 is: Solve for d through a system of linear equations: where λ is the Lagrangian variable, denotes M i+1 Jacobian matrix, J Mi denotes M i Jacobian matrix, is the next constraint, i.e., the constraint function corresponding to the (i + 1)-th constraint; Normalize d and use it as the single-step expansion of the newly generated point: where α is the set step size.

Images