CN109877836B

CN109877836B - Path planning method and device, mechanical arm controller and readable storage medium

Info

Publication number: CN109877836B
Application number: CN201910190556.4A
Authority: CN
Inventors: 骆思宇; 汪鹏飞; 卢维; 殷俊
Original assignee: Zhejiang Dahua Technology Co Ltd
Current assignee: Zhejiang Dahua Technology Co Ltd
Priority date: 2019-03-13
Filing date: 2019-03-13
Publication date: 2021-06-08
Anticipated expiration: 2039-03-13
Also published as: CN109877836A

Abstract

The application relates to a path planning method, a path planning device, a mechanical arm controller and a readable storage medium. The method comprises the following steps: according to the sampling space of the joint space, the starting point position shape of the task starting point of the mechanical arm in the joint space of the mechanical arm, the end point position shape of the task end point in the joint space, a preset first step length and a preset second step length, carrying out random tree growth processing to obtain a plurality of tree nodes of a random tree; the first step length is the step length of the new tree node growing to the sampling point, and the second step length is the step length of the new tree node growing to the target tree node; during the process of random tree growth processing, judging a local minimum state, and adjusting a first step length and a second step length when judging that the random tree growth falls into the local minimum state, so that the first step length is larger than the second step length; and obtaining an initial path from the task starting point to the task ending point in the joint space according to a plurality of tree nodes of the random tree. The method can escape from the local minimum state in the random tree growth process.

Description

Path planning method and device, mechanical arm controller and readable storage medium

Technical Field

The present application relates to the field of robotics, and in particular, to a path planning method, apparatus, manipulator controller, and readable storage medium.

Background

The robot can be widely applied to the fields of old people accompanying and attending, medical operation assistance, entertainment home, industrial production and the like, and path planning of the mechanical arm is one of the cores of research contents of the robot. Path planning refers to calculating a path with less cost satisfying constraints when a starting point and an end point are given for a mechanical arm with a certain degree of freedom, so that the mechanical arm can move from the starting point to the end point without collision.

A path planning method of a mechanical arm comprises the following steps: after parametric modeling is performed on the mechanical arm and a positive and negative kinematics model is established, an AABB (Axis-aligned bounding box) bounding box method is adopted to establish an AABB bounding box of the mechanical arm and obstacles in the environment (used for performing collision detection quickly or performing filtering before performing accurate collision detection), and then an RRT (rapid exploration random tree) algorithm is used to plan a collision-free path under a constraint condition.

However, when an obstacle such as a concave obstacle exists between the starting point and the end point, the above path planning method has difficulty in finding an effective path, resulting in difficulty in avoiding the obstacle by the robot arm.

Disclosure of Invention

In view of the above, it is necessary to provide a path planning method, a device, a robot arm controller, and a readable storage medium, which can search for an effective path even when an obstacle such as a concave obstacle exists between a start point and an end point.

In a first aspect, a method of path planning, the method comprising:

determining a starting point configuration of a task starting point of a mechanical arm in a joint space of the mechanical arm, an end point configuration of a task end point of the mechanical arm in the joint space, and determining a sampling space of the joint space;

carrying out random tree growth processing according to the sampling space, the starting point position and the end point position, and a preset first step length and a preset second step length to obtain a plurality of tree nodes of a random tree; the first step length is the step length of a new tree node growing to a sampling point, the second step length is the step length of the new tree node growing to a target tree node, the sampling point is obtained by sampling from the sampling space, and the target tree node is the starting point configuration or the end point configuration;

during the process of random tree growth processing, judging a local minimum state, and when judging that the random tree growth falls into the local minimum state, adjusting the first step length and the second step length to enable the first step length to be larger than the second step length;

and obtaining an initial path from the task starting point to the task end point in a joint space according to the plurality of tree nodes of the random tree.

In one embodiment, the method further comprises:

and when the random tree growth is judged not to fall into the local minimum state, adjusting the first step length and the second step length to enable the first step length to be smaller than the second step length.

In one embodiment, the performing the local minimum state determination includes:

acquiring a latest tree node and at least one tree node before the latest tree node, and calculating at least one node distance between the latest tree node and the at least one tree node;

when the distance of the at least one node is smaller than or equal to a preset distance threshold, determining that the random tree growth falls into a local minimum state;

and when the distance of the at least one node is greater than a preset distance threshold value, determining that the random tree growth does not fall into a local minimum state.

In one embodiment, the method further comprises:

taking an initial path from the task starting point to the task end point in a joint space as a current path, and performing path optimization processing to obtain a new path;

wherein the path optimization process includes:

calculating the path cost of the current path;

determining an ellipsoid space as a new sampling space of the joint space according to the path cost, the starting point configuration and the end point configuration; the sum of the distances between each point in the new sampling space and the starting point configuration and the ending point configuration is less than or equal to the path cost;

and performing random tree growth processing according to the new sampling space to obtain an undetermined path from the task starting point to the task ending point in the joint space, and when the path cost of the undetermined path is less than that of the current path, taking the undetermined path as a new path.

In one embodiment, the method further comprises:

taking the new path as the current path, and continuing to perform path optimization processing until the new path meets preset conditions to obtain the final new path;

wherein determining that the new path satisfies a preset condition comprises:

acquiring a latest path and at least one path before the latest path, and calculating a difference value of at least one path cost between the latest path and the at least one path;

and when the difference value of the at least one path cost is less than or equal to a preset path cost threshold value, determining that the new path meets a preset condition.

In one embodiment, the method further comprises:

and optimizing the latest path according to a wrapping obstacle method to obtain an optimized target path.

In one embodiment, the performing a random tree growth process according to the sampling space, the start site configuration and the end site configuration, and a preset first step size and a preset second step size includes:

taking the ending point configuration as a target tree node of a first random tree and taking the starting point configuration as a target tree node of a second random tree;

and respectively carrying out random tree growth processing on the first random tree and the second random tree according to the sampling space, a preset first step length and a preset second step length until a connecting line between the latest tree node of the first random tree and the latest tree node of the second random tree meets a non-collision condition.

In one embodiment, the performing random tree growth according to the sampling space, a preset first step size and a preset second step size includes:

sampling from a sampling space of the joint space to obtain a sampling point;

traversing each tree node in the random tree, and determining the tree node with the minimum distance to the sampling point as a father node; the random tree is the first random tree or the second random tree;

determining child nodes growing from the father nodes according to the father nodes, the sampling points, the target tree nodes and the first step length and the second step length;

and when the connecting line between the child node and the father node meets the non-collision condition, taking the child node as a new tree node of the random tree.

In one embodiment, the obtaining a path from the task start point to the task end point in the joint space according to the plurality of tree nodes of the random tree includes:

determining the order of a plurality of first tree nodes and a plurality of first tree nodes on a path from the starting point bitmap to the latest tree node in the first random tree according to the latest tree node of the first random tree and the parent-child incidence relation of each tree node in the first random tree;

determining the sequence of a plurality of second tree nodes and a plurality of second tree nodes on a path from the latest tree node to the terminal configuration in the second random tree according to the latest tree node of the second random tree and the parent-child association relationship of each tree node in the second random tree;

determining the sequence of the plurality of tree nodes and the plurality of tree nodes on the path from the starting point configuration to the ending point configuration as the path from the task starting point to the task ending point in the joint space according to the sequence of the plurality of first tree nodes and the sequence of the plurality of second tree nodes and the plurality of second tree nodes.

In a second aspect, a path planning apparatus, the apparatus comprising:

the system comprises an initialization module, a data processing module and a data processing module, wherein the initialization module is used for determining a starting point configuration of a task starting point of a mechanical arm in a joint space of the mechanical arm, an end point configuration of a task end point of the mechanical arm in the joint space, and determining a sampling space of the joint space;

the random tree growing module is used for carrying out random tree growing processing according to the sampling space, the starting point position and the end point position, and a preset first step length and a preset second step length to obtain a plurality of tree nodes of the random tree; the first step length is the step length of a new tree node growing to a sampling point, the second step length is the step length of the new tree node growing to a target tree node, the sampling point is obtained by sampling from the sampling space, and the target tree node is the starting point configuration or the end point configuration;

the first step length adjusting module is used for judging a local minimum state in the process of random tree growth processing, and adjusting the first step length and the second step length when judging that the random tree growth falls into the local minimum state, so that the first step length is larger than the second step length;

and the initial path acquisition module is used for acquiring an initial path from the task starting point to the task ending point in a joint space according to the plurality of tree nodes of the random tree.

In a third aspect, a robot arm controller comprises a memory and a processor, the memory storing a computer program, the processor implementing the following steps when executing the computer program:

In a fourth aspect, a readable storage medium, having stored thereon a computer program which, when executed by a processor, performs the steps of:

According to the path planning method, the path planning device, the mechanical arm controller and the readable storage medium, the mechanical arm controller can determine the starting point position shape of the task starting point of the mechanical arm in the joint space of the mechanical arm, the end point position shape of the task end point of the mechanical arm in the joint space, and determine the sampling space of the joint space; carrying out random tree growth processing according to the sampling space, the starting point position and the end point position, and a preset first step length and a preset second step length to obtain a plurality of tree nodes of a random tree; the first step length is the step length of the new tree node growing to the sampling point, and the second step length is the step length of the new tree node growing to the target tree node; obtaining an initial path from the task starting point to the task end point in a joint space according to a plurality of tree nodes of the random tree; most importantly, in the process of random tree growth processing, local minimum state judgment is carried out, when the random tree growth is judged to be in the local minimum state, the first step length and the second step length are adjusted, the first step length is larger than the second step length, the random tree is grown towards a sampling point, and the local minimum state is escaped, so that the path from a task starting point to a task end point in a joint space can be searched.

Drawings

FIG. 1 is a diagram of an exemplary path planning method;

FIG. 2 is a flow diagram illustrating a method for path planning in one embodiment;

FIG. 3a is a schematic flow chart of optimizing a path by optimizing a sampling space in one embodiment;

FIG. 3b is a schematic diagram of an optimized sample space in one embodiment;

FIG. 3c is a schematic diagram of a wrap-around obstacle method optimized path in one embodiment;

FIG. 4 is a schematic flow chart of bidirectional random tree growth in one embodiment;

FIG. 5a is a schematic flow chart of bidirectional random tree growth in another embodiment;

FIG. 5b is a schematic diagram of bidirectional random tree growth in one embodiment;

FIG. 6 is a flow diagram illustrating a method for path planning in one embodiment;

FIG. 7 is a block diagram of a path planning apparatus according to an embodiment;

fig. 8 is a block diagram showing the structure of a route planning apparatus according to an embodiment.

Detailed Description

In order to make the objects, technical solutions and advantages of the present application more apparent, the present application is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the present application and are not intended to limit the present application.

The path planning method provided by the embodiment of the application can be applied to the mechanical arm shown in fig. 1. The mechanical arm can be a mechanical arm in a conventional sense, a robot or other mechanical structures needing path planning. A mechanical arm generally comprises a plurality of movable joints and at least one actuator, and each joint can be rotated, slid or driven in other manners, so that the mechanical arm has multiple degrees of freedom, such as 4 degrees of freedom, 6 degrees of freedom and the like; specifically, the mechanical arm may include a mechanical arm controller, and the mechanical arm controller may control the motor to drive each joint to move, so as to control the actuator to move from the task starting point to the task ending point. However, since there may be an obstacle, a dead lock point, self interference, and other areas that cannot pass through when the task start point moves to the task end point, it is necessary to plan a path from the task start point to the task end point by using the path planning method described in this embodiment, and the robot arm controller may drive each joint of the robot arm according to the path, thereby achieving collision-free movement of the actuator of the robot arm from the task start point to the task end point. It will be appreciated that the robot arm controller may be embodied as a computer device, a single chip computer control system, and other means by which data may be processed.

In one embodiment, as shown in fig. 2, a path planning method is provided, which is described by taking the example that the method is applied to the manipulator controller in fig. 1, and includes the following steps:

s201, determining a starting point configuration of a task starting point of the mechanical arm in a joint space of the mechanical arm, an end point configuration of a task end point of the mechanical arm in the joint space, and determining a sampling space of the joint space.

The manipulator controller may store established equations of forward and inverse kinematics of the manipulator. Wherein, the positive kinematics refers to solving the position and the direction of the actuator in a base coordinate system (a fixed coordinate system) when the type of each joint, the size between each adjacent joint and the relative variation of the adjacent joints are known; the inverse kinematics is opposite to the positive kinematics, and the relative variation of the adjacent joints is solved by knowing the type of each joint, the size between each adjacent joint and other information and the position and the direction of the actuator in a base coordinate system (a fixed coordinate system). Therefore, the above-mentioned forward and inverse kinematics equation can represent the relationship between the magnitude of the relative variation of adjacent joints (variable parameter of each joint) and the position and direction of the actuator in the base coordinate system, and is generally embodied in the form of a transformation matrix; if the position and the direction parameters of the actuator of the mechanical arm form a Cartesian space, and the variable parameters of each joint of the mechanical arm form a joint space, the positive-inverse kinematics equation is equivalent to a mapping relation between the Cartesian space of the mechanical arm and the joint space of the mechanical arm.

Illustratively, through a D-H modeling method proposed by Denavit and Hartenberg, a joint coordinate system can be established on each joint of the robot arm, and the above-mentioned positive and negative kinematic equations can be determined according to DH parameters (DH parameter table, used to describe the relationship between the joint coordinate systems of the robot) of the multi-degree-of-freedom robot arm and a base coordinate system.

In one embodiment, the manipulator controller may determine a parameter of the task starting point in the joint space, i.e., a starting point configuration, and a parameter of the task ending point in the joint space, i.e., an ending point configuration, according to a preset positive and negative kinematic equation and the input task starting point and task ending point; for example, when the mechanical arm has N degrees of freedom, the starting point configuration and the ending point configuration may be N-dimensional parameters, and each dimensional parameter corresponds to a variable parameter of a certain joint.

Of course, the above-mentioned mechanical arm controller may also directly accept the input start position form and end position form, or obtain the start position form or end position form in the preset calculation file, which is not limited in this embodiment.

The sampling space of the joint space may be a preset sampling space, and the sampling space is only used for limiting the value range of the variable parameter of each joint, for example, some variable parameters are relative change angles of the joints, and a lock angle exists, so that the value range of the variable parameter needs to be limited; in addition, the sampling space is set to perform sampling in a limited space so as to reduce the calculation amount.

S202, performing random tree growth processing according to the sampling space, the starting point position and the end point position, and a preset first step length and a preset second step length to obtain a plurality of tree nodes of a random tree; the first step length is the step length of a new tree node growing to a sampling point, the second step length is the step length of the new tree node growing to a target tree node, the sampling point is obtained by sampling from the sampling space, and the target tree node is the starting point configuration or the end point configuration.

In the random tree growth processing algorithm, the mechanical arm controller may use any one point of a start point configuration or an end point configuration in a joint space as a start tree node of a random tree, and use another point as a target tree node to perform unidirectional random tree growth, that is, sampling is performed in a sampling space from the start tree node to obtain a sampling point, then any tree node in the random tree is selected as a parent node (the start tree node), and a child node of the parent node is obtained by calculation according to the first step length and the second step length. Illustratively, form x with the starting position_startAs the starting tree node, the end point bit shape x_go_alAs a target tree node, the sampling point is x_randThe parent node is x_fThen child node x_newComprises the following steps:

where ρ is₁A first step size, p, representing the growth of a child node (i.e. a new tree node) in the direction of the sample point₂And representing the step size of the growth of the child node towards the target tree node. Generally, the sampling points are obtained by random sampling and are random nodes; of course, the sampling may be various sampling methods such as gaussian sampling, and the present embodiment is not limited thereto. Therefore, according to the random tree growing process, new tree nodes can be added into the random tree to obtain a new random tree, and then the random tree is grown continuously according to the new random tree, and for unidirectional growth, the tree nodes are grown until a connecting line between the new tree node and the target tree node meets a non-collision condition.

It can be understood that, in the process of the random tree growth, collision detection needs to be performed on the connecting line between the child node and the parent node; when the connection line between the child node and the parent node satisfies a no-collision condition, the child node may be used as a new tree node of the random tree. Specifically, a connecting line between the child node and the parent node may be equally divided into n nodes, collision detection may be performed on the n nodes and the child node, if no collision occurs, that is, a collision-free condition is satisfied, the child node is added to the random tree as a new tree node, and if a collision occurs, the child node is discarded. The above child node acquisition and collision detection process is repeated until a new tree node without collision is added to the random tree. The collision detection can be performed by various bounding box methods, which are not described in detail herein.

After determining that the child node is a new collision-free tree node, connecting the new tree node with the target tree node; if the connecting line between the two points does not meet the no-collision condition, the random tree needs to continue growing; if the connecting line between the two points meets the no-collision condition, the random tree can stop growing.

It should be noted that the parent node may be any tree node in the random tree, or may be a tree node closest to a sampling point in the random tree, or may be a sampling point closest to a target tree node in the random tree, or may be a latest tree node (i.e., a newly generated new tree node) in the random tree, or the like; the present embodiment does not limit this.

Illustratively, the first step size and the second step size may be scaled by D as a small amount of D; for example, the first step size may be 0.025D, the second step size may be 0.05D, and D is the distance between the start and end configurations. In this embodiment, the distance may be an euclidean distance, or may be other norms.

S203, judging a local minimum state in the process of random tree growth treatment, and adjusting the first step length and the second step length to enable the first step length to be larger than the second step length when judging that the random tree growth falls into the local minimum state.

It can be understood that, when the second step length is longer than the first step length, the new tree node will be biased to the target tree node direction, and the larger the difference between the second step length and the first step length is, the more the new tree node is biased to the target tree node, and at this time, the closer the planned path will be to the shortest path after the obstacle is ignored; in an ideal situation without obstacles, the method is very effective, but in a real environment, obstacles are very likely to exist between a task starting point and a task ending point, if the planning algorithm sets the second step length to be larger than the first step length all the time, a local minimum will be trapped near a concave obstacle, and a feasible path cannot be planned. Generally, the second step size is set to be larger than the first step size in the initial setting so as to perform path planning as efficiently as possible, but the above-mentioned state of falling into local minimum may occur.

Therefore, the path planning method of this embodiment may perform local minimum state determination during the process of performing random tree growth, and adjust the first step size and the second step size when it is determined that the random tree growth falls into the local minimum state, so that the first step size is larger than the second step size.

Alternatively, in one embodiment, the mechanical arm controller may obtain the number of tree nodes of the random tree, and when the number of tree nodes is greater than a preset threshold, it means that the random tree growth may be trapped in a local minimum, and then it is determined that the random tree growth is trapped in a local minimum state.

Alternatively, in one embodiment, if several consecutive tree nodes (in the order of the resulting new tree nodes) are within a small region in bitmap space, there is likely to be a local minimum in the vicinity. For example, the robotic arm controller may obtain a latest tree node and at least one tree node preceding the latest tree node, calculate at least one node distance between the latest tree node and the at least one tree node; when the distance of the at least one node is smaller than or equal to a preset distance threshold, determining that the random tree growth falls into a local minimum state; and when the distance of the at least one node is greater than a preset distance threshold value, determining that the random tree growth does not fall into a local minimum state. And the latest tree node is a newly obtained new tree node, and at least one tree node before the latest tree node is a new tree node before the latest tree node according to the sequence of the obtained new tree nodes. For example, if for some slight positive number ε_m(preset distance threshold), presence | | | x_i-x_i+1||＜ε_m，||x_i-x_i+2||＜ε_mAnd | | | x_i-x_i+3||＜ε_mThen it can be considered that random tree growth at this point falls into a local minimum state, where x_iIs a new tree node in the random tree.

S204, obtaining an initial path from the task starting point to the task ending point in the joint space according to the plurality of tree nodes of the random tree.

It can be understood that after the random tree growth process is performed, an initial path from the task start point to the task end point, which is composed of the connecting lines between part of the tree nodes, exists in the plurality of tree nodes of the obtained random tree. When the target tree node is the starting point configuration, specifically, because the cut-off condition of the random tree growing process is generally that the connecting line between the latest tree node and the target tree node satisfies the no-collision condition, starting from the target tree node and the latest tree node, the target tree node and the latest tree node may be taken as the first two path nodes in the initial path, and then the parent node of the latest tree node (i.e., the tree node having a parent-child relationship with the latest tree node) may be found as the third path node in the initial path, and further the tree node having a parent-child relationship with the third path node may be found as the fourth path node, … …, until the ending point configuration is added to the random tree node as the last path node; thus, a sub-path from the first path node to the second path node, a sub-path from the second path node to the third path node, … …, each sub-path composed of path nodes constitutes an initial path in joint space from the task start point to the task end point.

In the path planning method of this embodiment, the manipulator controller determines a start point configuration of a task start point of the manipulator in a joint space of the manipulator and an end point configuration of a task end point of the manipulator in the joint space, and determines a sampling space of the joint space; carrying out random tree growth processing according to the sampling space, the starting point position and the end point position, and a preset first step length and a preset second step length to obtain a plurality of tree nodes of a random tree; the first step length is the step length of the new tree node growing to the sampling point, and the second step length is the step length of the new tree node growing to the target tree node; obtaining an initial path from the task starting point to the task end point in a joint space according to a plurality of tree nodes of the random tree; most importantly, in the process of random tree growth processing, local minimum state judgment is carried out, when the random tree growth is judged to be in the local minimum state, the first step length and the second step length are adjusted, the first step length is larger than the second step length, the random tree is grown towards a sampling point, and the local minimum state is escaped, so that the path from a task starting point to a task end point in a joint space can be searched.

It can be understood that if the planning algorithm always sets the first step size to be larger than the first step size, the new tree node always leans to the sampling point, which may cause the problem of inefficiency of the path planning algorithm. The method can dynamically set a first step length and a second step length, firstly, assuming that no barrier exists between a task starting point and a task ending point, setting the second step length to be larger than the first step length, enabling the random tree to continuously expand towards the direction of a target tree node until the random tree is judged to grow into a local minimum, and then setting the first step length to be larger than the second step length, enabling a new tree node to grow towards the direction of a sampling point to escape from the local minimum state; when the random tree is judged to be grown to escape from the local minimum state, the second step length can be dynamically set to be larger than the first step length. Therefore, in this embodiment, the mechanical arm controller may adjust the first step size and the second step size when it is determined that the random tree growth does not fall into the local minimum state, so that the first step size is smaller than the second step size. The method not only retains the advantages of flexible obstacle avoidance, but also enhances the searching efficiency.

In an embodiment, the process related to implementing path optimization by optimizing a sampling space may specifically include: taking an initial path from the task starting point to the task end point in a joint space as a current path, and performing path optimization processing to obtain a new path; referring to fig. 3a, the path optimization process includes the following steps:

and S301, calculating the path cost of the current path.

The path cost can be an index of an evaluation path and can be any effective index; for example, the path cost may be a path length, specifically, a sum of lengths of sub-paths constituting the path, where the length of each sub-path is a distance between two path nodes corresponding to the sub-path, that is, the path cost represents a joint variation caused by controlling the mechanical arm according to the path; when the path cost is minimum, the joint variation is minimum, the joint abrasion is minimum, and the service life of the mechanical arm can be prolonged. Of course, other evaluation criteria are also used for the path cost.

S302, determining an ellipsoid space as a new sampling space of the joint space according to the path cost, the starting point configuration and the end point configuration; the sum of the distances between each point in the new sampling space and the starting point configuration and the ending point configuration is less than or equal to the path cost.

This optimized new sampling space can be represented by the following equation:

X_f*＝{x∈X_free|||x_start-x||₂+||x-x_goal||₂≤c_max}

this is exactly the expression for an ellipsoid (hyper-ellipsoid) and so can be said to constrain the optimization space within the ellipsoid constructed by the current path, X_f*Representing the optimized sampling space, X_freeRepresenting a collision-free sampling space, i.e. a sampling space of the joint space, c_maxRepresenting the path cost of the current path.

Referring to the two-dimensional plan view of the optimized sample space shown in FIG. 3 b; c. C_minThe straight-line distance between the starting tree node and the target tree node after the obstacle is ignored is the starting tree node X_startAnd target tree node X_goalTwo focal points of an ellipsoid, respectively, c_maxIs the long axis of the ellipsoid,

the minor axis of the ellipsoid.

S303, according to the new sampling space, performing random tree growth processing to obtain an undetermined path from the task starting point to the task ending point in a joint space, and when the path cost of the undetermined path is less than that of the current path, taking the undetermined path as a new path.

According to the property of the ellipsoid, the sum of the distances between the sampling point sampled in the sampling space and the initial tree node and the distance between the sampling point and the target tree node are respectively less than the path cost c_maxTherefore, the sampling point of the optimized sampling space is better than the original sampling space, and the sampling is ensured to be performed in the space which can improve the cost of the current path; therefore, the path cost of the new path is very high and is possibly smaller than that of the current path, and path optimization is realized; that is to say, an ellipsoid is constructed according to the current path to optimize the sampling space, so that the algorithm can be ensured to sample only from the space capable of optimizing the current path, the search path approaches to the optimum, and the search efficiency of the algorithm is improved.

Optionally, the method further comprises: taking the new path as the current path, and continuing to perform path optimization processing until the new path meets preset conditions to obtain the final new path; wherein determining that the new path satisfies a preset condition comprises: acquiring a latest path and at least one path before the latest path, and calculating a difference value of at least one path cost between the latest path and the at least one path; and when the difference value of the at least one path cost is less than or equal to a preset path cost threshold value, determining that the new path meets a preset condition.

It can be understood that, in this embodiment, the initial path may be used as the current path from the initial path, and the optimized new path is obtained according to the path optimization processing procedure, so that the optimization can be continued to realize the continuous and gradual optimization of the path. The optimization process needs to pay attention to how to determine that the path has approached the optimum, and the present embodiment may adopt the following method to determine whether the path has approached the optimumOptimally: for some slight positive epsilon_c(ε_cThe value of (a) is positively correlated with the size of the sampling space), there are:

wherein, c_iRepresenting the path cost of the ith path, wherein i is an integer greater than or equal to 4; illustratively, when the path costs of 4 consecutive paths are close, the paths can be considered to be already close to the optimum, and repeated operation is avoided, so that the operation efficiency can be improved.

Optionally, when the path optimization approaches to the optimal path, the latest path may be optimized according to a parcel barrier method to obtain an optimized target path. And the latest path is the optimal path obtained by the path optimization processing. When it is difficult to perform effective optimization by optimizing the sampling space, another optimization method may be adopted: and (4) wrapping the obstacles, and optimizing again to obtain a target path.

Referring to the schematic diagram of the parcel obstacle method shown in fig. 3c, the process of optimizing a path by approaching a parcel obstacle in a two-dimensional plane is shown, in which the dark black line represents the originally planned path (fig. (a)), the light black line represents the path after secondary optimization (fig. (f)), and the dark rectangle represents the obstacle. First assume that an initial path P is traversed₀A total of four path nodes (graph (a)), and the path nodes to be optimized are nodes except the starting point and the end point, namely node x₂And x₃The optimization process of each node is divided into two steps. The first step is as follows: discrete node x₂And x₃The path between the nodes is obtained, n new discrete nodes are obtained and are marked as x_2i(i e (1, n)), and then sequentially connecting the discrete nodes with x₂Is connected, collision detection is carried out on the connecting line, and the first node which is found to have collision detection is recorded as x'₂(FIG. (b)); the second step is that: discrete x₂Of the preceding node and node x'₂Get n new discrete nodes, note them as x'_2i(i e (1, n)), and then sequentially connecting the discrete nodes with x₃Connecting, performing collision detection on the connecting line, and recording the first node with collision detection as x_2new(FIG. (c)); through the two steps, the node x can be processed₂Replacement by a new node x_2new. Node x₃The same processing procedure as in (d) and (e), node x is assigned₃Replacement by a new node x_3new. Path P after secondary processing according to the principle of triangle inequality₁(x₁→x_2new→x_3new→x₄) Is obviously superior to the initial path P₀(x₁→x₂→x₃→x₄). Similarly, each path node in the optimal path can be optimized again through a wrapped obstacle method, and a new target path formed by directed connection of the optimized path nodes is obtained.

It can be understood that, because the wrapping barrier method has a large calculation amount, the initial path obtained in S204 may be selected to be optimized by the wrapping barrier method, and then the path is continuously optimized in a sampling space optimizing manner with a small calculation amount until the path is optimized to obtain the latest path, and finally the latest path is optimized by the wrapping barrier method to obtain the target path, which not only avoids increasing the collision detection time due to excessive use of the wrapping barrier method, but also improves the algorithm efficiency.

In an embodiment, referring to fig. 4, the present embodiment relates to performing path planning by using bidirectional random tree growth, which may specifically include:

s401, the terminal site shape is used as a target tree node of a first random tree, and the starting site shape is used as a target tree node of a second random tree.

When a bidirectional random tree growth algorithm is adopted, the growing processing of the first random tree can be carried out by taking the starting point configuration as a starting tree node of the first random tree and taking the end point configuration as a target tree node of the first random tree; and taking the end point configuration as a starting tree node of the second random tree and the starting point configuration as a target tree node of the second random tree to carry out the growth processing of the second random tree.

S402, respectively aiming at the first random tree and the second random tree, carrying out random tree growth processing according to the sampling space, a preset first step length and a preset second step length until a connecting line between the latest tree node of the first random tree and the latest tree node of the second random tree meets a non-collision condition.

When a connecting line between the latest tree node of the first random tree and the latest tree node of the second random tree satisfies a no-collision condition, meaning that the first random tree and the second random tree are grown completely, a path may be determined according to the first random tree and the second random tree.

Specifically, as shown in fig. 5a and 5b, the performing the random tree growing process according to the sampling space, the preset first step size and the preset second step size may include:

s501, sampling from a sampling space of the joint space to obtain a sampling point;

s502, traversing each tree node in the random tree, and determining the tree node with the minimum distance to the sampling point as a father node; the random tree is the first random tree or the second random tree;

s503, determining child nodes growing from the father nodes according to the father nodes, the sampling points, the target tree nodes, the first step length and the second step length;

s504, when the connecting line between the child node and the father node meets the non-collision condition, the child node is used as a new tree node of the random tree.

The process of the bidirectional random tree growth may refer to the process of the unidirectional random tree growth, which is not described herein again. It should be noted that the first random tree T_sNeutron node x_newsAs follows:

second random tree T_gNeutron node x_newgAs follows:

wherein x is_startAs starting tree node, x_goalAs a target tree node, the sampling point is x_randThe parent node is x_nearest。

Optionally, the obtaining a path from the task start point to the task end point in the joint space according to the plurality of tree nodes of the random tree may include: determining the order of a plurality of first tree nodes and a plurality of first tree nodes on a path from the starting point bitmap to the latest tree node in the first random tree according to the latest tree node of the first random tree and the parent-child incidence relation of each tree node in the first random tree; determining the sequence of a plurality of second tree nodes and a plurality of second tree nodes on a path from the latest tree node to the terminal configuration in the second random tree according to the latest tree node of the second random tree and the parent-child association relationship of each tree node in the second random tree; determining the sequence of the plurality of tree nodes and the plurality of tree nodes on the path from the starting point configuration to the ending point configuration as the path from the task starting point to the task ending point in the joint space according to the sequence of the plurality of first tree nodes and the sequence of the plurality of second tree nodes and the plurality of second tree nodes.

Referring to FIG. 5b, a schematic diagram of a bi-directional random tree growing is shown, wherein the rectangular regions, the triangular regions, etc. are all barrier regions, and the first random tree T_sA second random tree T consisting of a plurality of interconnected tree nodes at the lower left corner of the figure_gThe tree node is composed of a plurality of interconnected tree nodes at the upper right corner of the graph; it will be appreciated that there is a parent-child association relationship between two tree nodes in a connected relationship. In FIG. 5b, when the last tree node x of the first random tree is_newsAnd the latest tree node x of the second random tree_newgWhen the connection line between the first random tree and the second random tree meets the collision-free condition, determining the parent-child association relationship of each tree node in the first random treeFrom the latest tree node x in the first random tree_newsAn order of a plurality of first tree nodes and the plurality of first tree nodes on a path to the starting bitmap: x is the number of_news→x_newsParent node of → … … → x_startChild node of → x_startAnd further in reverse order, i.e. from the starting bitmap to the newest tree node x_newsAnd an order of the plurality of first tree nodes on the path of: x is the number of_start→x_startChild node of → … … → x_newsParent node of → x_news. Similarly, the order of the plurality of second tree nodes and the plurality of second tree nodes on the path from the latest tree node to the end point configuration can be determined directly according to the parent-child association relationship of each tree node in the second random tree: x is the number of_newg→x_newgParent node of → … … → x_goalChild node of → x_goal. Finally, a plurality of tree nodes on the path from the starting point configuration to the end point configuration and the sequence of the tree nodes can be obtained: x is the number of_start→x_startChild node of → … … → x_newsParent node of → x_news→x_newg→x_newgParent node of → … … → x_goalChild node of → x_goalAs a path in joint space from the task start point to the task end point.

Referring to fig. 6, a schematic diagram of a path planning method based on bidirectional random tree growth is shown from another perspective, and may include:

s601, randomly sampling in a sampling space to obtain a sampling point of a first random tree, and then executing S602;

s602, traversing the first random tree to obtain a point which is closest to the sampling point of the first random tree in the S601 as a father node, and then executing S603;

s603, judging whether the growth of the first random tree falls into a local minimum state; if not, executing S604; if yes, executing S605;

s604, enabling the first step length to be smaller than the second step length according to a preset step length adjusting mechanism in a non-local minimum state, obtaining child nodes of the father node in the S602 according to the first step length and the second step length, and then executing S606;

s605, according to a preset step length adjusting mechanism in a local minimum state, enabling a first step length to be larger than a second step length, obtaining a child node of a father node in the S602 according to the first step length and the second step length, and then executing S606;

s606, performing collision detection on a connecting line between the child node and the father node, and judging whether collision occurs or not; if yes, executing S601; if not, executing S607;

s607, adding the child node into the first random tree as a new tree node, and then executing S608 and S613;

s608, randomly sampling in the sampling space to obtain sampling points of a second random tree, and then executing S609;

s609, traversing the second random tree to obtain a point which is closest to the sampling point of the second random tree in the S608 as a father node, and then executing S610;

s610, judging whether the growth of the second random tree falls into a local minimum state; if not, enabling the first step length to be smaller than the second step length according to a preset step length adjusting mechanism in a non-local minimum state, and if yes, enabling the first step length to be larger than the second step length according to a preset step length adjusting mechanism in a local minimum state; obtaining a child node of the parent node in S609 according to the first step size and the second step size, and then executing S611;

s611, performing collision detection on a connecting line between the child node and the father node, and judging whether collision occurs or not; if yes, go to S608; if not, executing S612;

s612, adding the child node into the second random tree as a new tree node, and then executing S613;

s613, connecting the new tree node of the first random tree and the new tree node of the second random tree, and then performing S614;

s614, carrying out collision detection on a connecting line of the new tree node of the first random tree and the new tree node of the second random tree, and judging whether collision occurs or not; if yes, executing S601; if not, executing S615;

s615, judging whether an initial path is obtained or not, if not, executing S616; if yes, go to S619;

s616, generating an initial path P according to the first random tree and the second random tree, and the new tree nodes of the first random tree and the new tree nodes of the second random tree which are not collided in S614₀Then, S617 is performed;

s617, optimizing wrapping obstacles on the initial path to obtain P₁Then, S618 is executed;

s618, taking P1 as a current path, optimizing a sampling space according to the path cost of the current path, and then executing S601;

s619, judging whether the process of optimizing the sampling space for path optimization tends to be optimal, if not, executing S620; if yes, go to S621;

s620, optimizing a sampling space according to the path cost of the current path, and then executing S601;

and S621, optimizing the wrapping barrier of the current path to obtain a target path.

It should be understood that although the various steps in the flow charts of fig. 2,3a,4,5a,6 are shown in sequence as indicated by the arrows, the steps are not necessarily performed in sequence as indicated by the arrows. The steps are not performed in the exact order shown and described, and may be performed in other orders, unless explicitly stated otherwise. Moreover, at least some of the steps in fig. 2,3a,4,5a,6 may include multiple sub-steps or multiple stages, which are not necessarily performed at the same time, but may be performed at different times, and the order of performing the sub-steps or stages is not necessarily sequential, but may be performed alternately or alternatingly with other steps or at least some of the sub-steps or stages of other steps.

In one embodiment, as shown in fig. 7, there is provided a path planning apparatus, including: an initialization module 71, a random tree growing module 72, a first step size adjusting module 73, and an initial path obtaining module 74, wherein:

an initialization module 71, configured to determine a starting point configuration of a task starting point of the mechanical arm in a joint space of the mechanical arm, an end point configuration of a task end point of the mechanical arm in the joint space, and determine a sampling space of the joint space;

a random tree growing module 72, configured to perform random tree growing processing according to the sampling space, the start point configuration and the end point configuration, and the preset first step length and second step length, to obtain a plurality of tree nodes of a random tree; the first step length is the step length of a new tree node growing to a sampling point, the second step length is the step length of the new tree node growing to a target tree node, the sampling point is obtained by sampling from the sampling space, and the target tree node is the starting point configuration or the end point configuration;

a first step size adjusting module 73, configured to perform local minimum state judgment during a process of performing random tree growth, and adjust the first step size and the second step size when it is judged that the random tree growth falls into a local minimum state, so that the first step size is larger than the second step size;

an initial path obtaining module 74, configured to obtain an initial path in a joint space from the task start point to the task end point according to the plurality of tree nodes of the random tree.

Optionally, as shown in fig. 8, the apparatus may further include:

and a second step size adjusting module 75, configured to adjust the first step size and the second step size when it is determined that the random tree growth does not fall into the local minimum state, so that the first step size is smaller than the second step size.

Optionally, the performing the local minimum state judgment includes: acquiring a latest tree node and at least one tree node before the latest tree node, and calculating at least one node distance between the latest tree node and the at least one tree node; when the distance of the at least one node is smaller than or equal to a preset distance threshold, determining that the random tree growth falls into a local minimum state; and when the distance of the at least one node is greater than a preset distance threshold value, determining that the random tree growth does not fall into a local minimum state.

Optionally, as shown in fig. 8, the apparatus may further include:

a first path optimization module 76, configured to perform path optimization processing by using an initial path in the joint space from the task start point to the task end point as a current path, so as to obtain a new path; wherein the path optimization process includes: calculating the path cost of the current path; determining an ellipsoid space as a new sampling space of the joint space according to the path cost, the starting point configuration and the end point configuration; the sum of the distances between each point in the new sampling space and the starting point configuration and the ending point configuration is less than or equal to the path cost; and performing random tree growth processing according to the new sampling space to obtain an undetermined path from the task starting point to the task ending point in the joint space, and when the path cost of the undetermined path is less than that of the current path, taking the undetermined path as a new path.

Further, referring to fig. 8, the apparatus may further include:

an optimization control module 77, configured to take the new path as a current path, and continue to perform path optimization processing until the new path meets a preset condition, so as to obtain a final new path; wherein determining that the new path satisfies a preset condition comprises: acquiring a latest path and at least one path before the latest path, and calculating a difference value of at least one path cost between the latest path and the at least one path; and when the difference value of the at least one path cost is less than or equal to a preset path cost threshold value, determining that the new path meets a preset condition.

Further, referring to fig. 8, the apparatus may further include:

and a second path optimization module 78, configured to optimize the latest path according to a parcel barrier method, so as to obtain an optimized target path.

Optionally, referring to fig. 8, the random tree growing module 72 may include:

a bidirectional random tree setting unit 721 for setting the ending bitmap as a target tree node of a first random tree and the starting bitmap as a target tree node of a second random tree;

and a bidirectional random tree growing unit 722, configured to perform random tree growing processing on the first random tree and the second random tree respectively according to the sampling space, a preset first step size, and a preset second step size, until a connection line between a latest tree node of the first random tree and a latest tree node of the second random tree satisfies a collision-free condition.

Optionally, the bidirectional random tree growing unit 722 is specifically configured to obtain sampling points from a sampling space of the joint space; traversing each tree node in the random tree, and determining the tree node with the minimum distance to the sampling point as a father node; the random tree is the first random tree or the second random tree; determining child nodes growing from the father nodes according to the father nodes, the sampling points, the target tree nodes and the first step length and the second step length; and when the connecting line between the child node and the father node meets the non-collision condition, taking the child node as a new tree node of the random tree.

Further, referring to fig. 8, the initial path obtaining module 74 may include:

a first path obtaining unit 741, configured to determine, according to a latest tree node of the first random tree and a parent-child association relationship of each tree node in the first random tree, an order of a plurality of first tree nodes and the plurality of first tree nodes on a path from the starting point configuration to the latest tree node in the first random tree;

a second path obtaining unit 742, configured to determine, according to a latest tree node of the second random tree and a parent-child association relationship of each tree node in the second random tree, an order of a plurality of second tree nodes and the plurality of second tree nodes on a path from the latest tree node to the destination bit pattern in the second random tree;

an initial path obtaining unit 743, configured to determine, according to an order of the plurality of first tree nodes and an order of the plurality of second tree nodes and the plurality of second tree nodes, the plurality of tree nodes and the order of the plurality of tree nodes on a path from the start point configuration to the end point configuration as a path in a joint space from the task start point to the task end point.

For the specific definition of the path planning device, reference may be made to the above definition of the path planning method, which is not described herein again. The modules in the path planning device can be wholly or partially implemented by software, hardware and a combination thereof. The modules can be embedded in a hardware form or independent from a processor in the computer device, and can also be stored in a memory in the computer device in a software form, so that the processor can call and execute operations corresponding to the modules.

In one embodiment, there is provided a robot arm controller comprising a memory and a processor, the memory having stored therein a computer program, the processor when executing the computer program implementing the steps of:

In one embodiment, a readable storage medium is provided, having stored thereon a computer program which, when executed by a processor, performs the steps of:

It will be understood by those skilled in the art that all or part of the processes of the methods of the embodiments described above can be implemented by hardware instructions of a computer program, which can be stored in a non-volatile computer-readable storage medium, and when executed, can include the processes of the embodiments of the methods described above. Any reference to memory, storage, database, or other medium used in the embodiments provided herein may include non-volatile and/or volatile memory, among others. Non-volatile memory can include read-only memory (ROM), Programmable ROM (PROM), Electrically Programmable ROM (EPROM), Electrically Erasable Programmable ROM (EEPROM), or flash memory. Volatile memory can include Random Access Memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in a variety of forms such as Static RAM (SRAM), Dynamic RAM (DRAM), Synchronous DRAM (SDRAM), Double Data Rate SDRAM (DDRSDRAM), Enhanced SDRAM (ESDRAM), Synchronous Link DRAM (SLDRAM), Rambus Direct RAM (RDRAM), direct bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).

The technical features of the above embodiments can be arbitrarily combined, and for the sake of brevity, all possible combinations of the technical features in the above embodiments are not described, but should be considered as the scope of the present specification as long as there is no contradiction between the combinations of the technical features.

The above-mentioned embodiments only express several embodiments of the present application, and the description thereof is more specific and detailed, but not construed as limiting the scope of the invention. It should be noted that, for a person skilled in the art, several variations and modifications can be made without departing from the concept of the present application, which falls within the scope of protection of the present application. Therefore, the protection scope of the present patent shall be subject to the appended claims.

Claims

1. A method of path planning, the method comprising:

carrying out random tree growth processing according to the sampling space, the starting point position and the end point position, and a preset first step length and a preset second step length to obtain a plurality of tree nodes of a random tree; the first step size is the step size of the new tree node growing to the sampling point,

the second step length is the step length of the new tree node growing to the target tree node, the sampling point is obtained by sampling from the sampling space, and the target tree node is the starting point configuration or the end point configuration;

obtaining an initial path from the task starting point to the task end point in a joint space according to a plurality of tree nodes of the random tree;

taking an initial path from the task starting point to the task end point in a joint space as a current path, and performing path optimization processing to obtain a new path; wherein the path optimization processing includes: calculating the path cost of the current path; determining an ellipsoid space as a new sampling space of the joint space according to the path cost, the starting point configuration and the end point configuration; the sum of the distance from any point in the new sampling space to the starting point configuration and the distance from the point in the new sampling space to the end point configuration is less than or equal to the path cost; and performing random tree growth processing according to the new sampling space to obtain an undetermined path from the task starting point to the task ending point in the joint space, and when the path cost of the undetermined path is less than that of the current path, taking the undetermined path as a new path.

2. The method of claim 1, further comprising:

3. The method of claim 1, wherein the performing local minimum state determination comprises:

4. The method of claim 1, wherein the new sampling space is represented by:

representing an optimized sampling space

A sample space representing collision-free, i.e. the joint space,

represents the path cost of the current path,

the straight line distance between the starting tree node and the target tree node after the obstacles are ignored is adopted as the starting tree node

And target tree nodes

Are respectively two focus points of the ellipsoid,

is the long axis of the ellipsoid,

the minor axis of the ellipsoid.

5. The method of claim 4, further comprising:

taking the new path as the current path, and continuing path optimization processing until the new path meets preset conditions to obtain a final new path;

wherein determining that the new path satisfies a preset condition comprises:

6. The method of claim 5, further comprising:

and optimizing the final new path according to a wrapping obstacle method to obtain an optimized target path.

7. The method according to any one of claims 1-6, wherein the performing a random tree growing process according to the sampling space, the start site configuration and the end site configuration, and the preset first step size and second step size comprises:

8. The method according to claim 7, wherein the performing a random tree growing process according to the sampling space, the preset first step size and the preset second step size comprises:

sampling from a sampling space of the joint space to obtain a sampling point;

traversing each tree node in the random tree, and determining the tree node with the minimum distance to the sampling point as a father node;

the random tree is the first random tree or the second random tree;

9. The method of claim 8, wherein the deriving a path in joint space from the task start point to the task end point from a plurality of tree nodes of the stochastic tree comprises:

10. A path planning apparatus, the apparatus comprising:

the initial path acquisition module is used for acquiring an initial path from the task starting point to the task end point in a joint space according to a plurality of tree nodes of the random tree;

the new path acquisition module is used for taking an initial path from the task starting point to the task ending point in a joint space as a current path and performing path optimization processing to obtain a new path; wherein the path optimization processing includes: calculating the path cost of the current path; determining an ellipsoid space as a new sampling space of the joint space according to the path cost, the starting point configuration and the end point configuration; the sum of the distance from any point in the new sampling space to the starting point configuration and the distance from the point in the new sampling space to the end point configuration is less than or equal to the path cost; and performing random tree growth processing according to the new sampling space to obtain an undetermined path from the task starting point to the task ending point in the joint space, and when the path cost of the undetermined path is less than that of the current path, taking the undetermined path as a new path.

11. A robot arm controller comprising a memory and a processor, the memory storing a computer program, characterized in that the processor realizes the steps of the method according to any of claims 1 to 9 when executing the computer program.

12. A readable storage medium, on which a computer program is stored, which, when being executed by a processor, carries out the steps of the method of any one of claims 1 to 9.