CN109877836A - Paths planning method, device, mechanical arm controller and readable storage medium storing program for executing - Google Patents
Paths planning method, device, mechanical arm controller and readable storage medium storing program for executing Download PDFInfo
- Publication number
- CN109877836A CN109877836A CN201910190556.4A CN201910190556A CN109877836A CN 109877836 A CN109877836 A CN 109877836A CN 201910190556 A CN201910190556 A CN 201910190556A CN 109877836 A CN109877836 A CN 109877836A
- Authority
- CN
- China
- Prior art keywords
- tree
- path
- length
- node
- random
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Landscapes
- Feedback Control In General (AREA)
- Manipulator (AREA)
Abstract
This application involves a kind of paths planning method, device, mechanical arm controller and readable storage medium storing program for executing.Method includes: to play the terminal position shape of point shape and task terminal in joint space in the joint space of mechanical arm according to the sample space of joint space, the task starting point of mechanical arm, and preset first step-length and the second step-length, random tree growth process is carried out, multiple tree nodes of random tree are obtained；The step-length that a length of new tree node of the first step is grown to sampled point, the step-length that a length of new tree node of second step is grown to target tree node；During carrying out random tree growth process, the judgement of local minimum state is carried out, when judging that random tree growth falls into local minimum state, adjusts the first step-length and the second step-length, so that the first step is long long greater than second step；According to multiple tree nodes of random tree, initial path of the task starting point to task terminal in joint space is obtained.Local minimum state can be escaped out in random tree growth course using this method.
Description
Technical field
This application involves robotic technology fields, more particularly to a kind of paths planning method, device, mechanical arm controller
And readable storage medium storing program for executing.
Background technique
Robot can be widely applied to old man accompany and attend to, medical operating assist, amusement household and industrial production etc. it is many
Field, and the path planning of mechanical arm is one of the core of robot research content.Path planning refer to for it is determining mostly from
By the mechanical arm spent, when given beginning and end, the lesser path of path cost for meeting constraint is calculated, so that mechanical arm
It can be moved to terminal without collision from starting point.
A kind of paths planning method of mechanical arm is: carrying out parametric modeling to mechanical arm and is establishing positive inverse kinematics mould
After type, barrier in mechanical arm and environment is established using AABB (Axis-aligned bounding box) bounding box method
AABB bounding box (is filtered before accurate collision detection) for being rapidly performed by collision detection or carrying out, and is then used
RRT (rapidly exploring random tree, rapid discovery random tree) algorithm cooks up collisionless under constraint condition
Path.
However, when, there are when the barriers such as spill barrier, above-mentioned paths planning method is difficult to search between beginning and end
Rope goes out active path, and mechanical arm is caused to be difficult to avoiding obstacles.
Summary of the invention
Based on this, it is necessary to which in view of the above technical problems, providing one kind, there are spill barriers between beginning and end
It remains to search out the paths planning method of active path, device, mechanical arm controller and readable storage medium storing program for executing when equal barriers.
In a first aspect, a kind of paths planning method, which comprises
Determine the task starting point of mechanical arm in the joint space of mechanical arm rise point shape, mechanical arm task terminal exist
Terminal position shape in the joint space, and determine the sample space of the joint space；
According to the sample space, point shape and terminal position shape and preset first step-length and the second step-length are played, is carried out
Random tree growth process obtains multiple tree nodes of random tree；What a length of new tree node of the first step was grown to sampled point
Step-length, the step-length that a length of new tree node of second step is grown to target tree node, the sampled point are empty from the sampling
Between middle sample obtain, the target tree node is described to play point shape or terminal position shape；
During carrying out random tree growth process, the judgement of local minimum state is carried out, is fallen into judging that random tree is grown
When local minimum state, first step-length and second step-length are adjusted, so that the first step is long to be greater than the second step
It is long；
According to multiple tree nodes of the random tree, the task starting point is obtained to the task terminal in joint space
Initial path.
In one of the embodiments, the method also includes:
When judging that random tree growth does not fall into local minimum state, first step-length and second step-length are adjusted,
So that first step-length is less than second step-length.
The progress local minimum state judgement in one of the embodiments, comprising:
At least one tree node before newest tree node and the newest tree node is obtained, is calculated described newest
Tree node and at least one described tree node between at least one nodal distance；
When at least one described nodal distance is respectively less than or is equal to pre-determined distance threshold value, determine that the random tree growth is fallen into
Enter local minimum state；
When at least one described nodal distance is greater than pre-determined distance threshold value, determine that the random tree growth does not fall into part
Minimum state.
In one of the embodiments, the method also includes:
Using initial path of the task starting point to the task terminal in joint space as current path, road is carried out
Diameter optimization processing obtains new path；
Wherein, the path optimization, which is handled, includes:
Calculate the path cost of the current path；
According to the path cost, described point shape and terminal position shape, determine a spheroid space as institute
State the new sample space of joint space；Each point and described point shape in the new sample space, terminal position shape
Sum of the distance is less than or equal to the path cost；
According to the new sample space, random tree growth process is carried out, it is whole to the task to obtain the task starting point
Undetermined path of the point in joint space, and when the path cost in the path undetermined is less than the path generation of the current path
When valence, using the path undetermined as new path.
In one of the embodiments, the method also includes:
Using the new path as current path, continue path optimization's processing, until the new path meets
Preset condition obtains the final new path；
Wherein it is determined that the new path meets preset condition includes:
At least one path before newest path and the newest path is obtained, the newest path is calculated
The difference of at least one path cost between at least one described path；
When the difference of at least one path cost is respectively less than or is equal to preset path cost threshold value, determine described new
Path meet preset condition.
In one of the embodiments, the method also includes:
The newest path is optimized according to package barrier method, the destination path after being optimized.
In one of the embodiments, it is described according to the sample space, play point shape and terminal position shape and preset
First step-length and the second step-length carry out random tree growth process, comprising:
Using terminal position shape as the target tree node of the first random tree, and described will rise point shape as second with
The target tree node of machine tree；
It is directed to first random tree and second random tree respectively, according to the sample space, the preset first step
Long and the second step-length carries out random tree growth process, until the newest tree node of the first random tree and the second random tree is newest
Line between tree node meets collisionless condition.
It is described according to the sample space, preset first step-length and the second step-length in one of the embodiments, it carries out
Random tree growth process, comprising:
Sampling obtains sampled point from the sample space of the joint space；
Each tree node in the random tree is traversed, the determining and sampled point is apart from the smallest tree node as father node；
The random tree is first random tree or second random tree；
According to the father node, sampled point, target tree node and first step-length and second step-length, determine
The child node grown from the father node；
When the line between the child node and the father node meets collisionless condition, using the child node as institute
State the new tree node of random tree.
Multiple tree nodes according to the random tree in one of the embodiments, obtain the task starting point extremely
Path of the task terminal in joint space, comprising:
It is associated with according to the newest tree node of first random tree with the father and son of each tree node in first random tree
System, determines multiple first tree nodes in first random tree from described in point shape to the path of the newest tree node
With the sequence of the multiple first tree node；
It is associated with according to the newest tree node of second random tree with the father and son of each tree node in second random tree
System, determines in second random tree from multiple second tree nodes on the newest tree node to the path of terminal position shape
With the sequence of the multiple second tree node；
According to the sequence and the multiple second burl of the multiple first tree node and the multiple first tree node
The sequence of point and the multiple second tree node determines multiple trees from described in point shape to the path of terminal position shape
The sequence of node and the multiple tree node, the road as the task starting point to the task terminal in joint space
Diameter.
Second aspect, a kind of path planning apparatus, described device include:
Initialization module, for determining that the task starting point of mechanical arm plays point shape, machine in the joint space of mechanical arm
Terminal position shape of the task terminal of tool arm in the joint space, and determine the sample space of the joint space；
Random tree pop-in upgrades, for according to the sample space, point shape and terminal position shape and preset first
Step-length and the second step-length carry out random tree growth process, obtain multiple tree nodes of random tree；The a length of new tree of the first step
The step-length that node is grown to sampled point, the step-length that a length of new tree node of second step is grown to target tree node are described to adopt
Sampling point is to sample to obtain from the sample space, and the target tree node is described point shape or terminal position shape；
First step size adjusting module, for carrying out the judgement of local minimum state during carrying out random tree growth process,
When judging that random tree growth falls into local minimum state, first step-length and second step-length are adjusted, so that described the
One step-length is greater than second step-length；
Initial path obtains module and obtains the task starting point to institute for multiple tree nodes according to the random tree
State initial path of the task terminal in joint space.
The third aspect, a kind of machinery arm controller, including memory and processor, the memory are stored with computer journey
Sequence, the processor perform the steps of when executing the computer program
Determine the task starting point of mechanical arm in the joint space of mechanical arm rise point shape, mechanical arm task terminal exist
Terminal position shape in the joint space, and determine the sample space of the joint space；
According to the sample space, point shape and terminal position shape and preset first step-length and the second step-length are played, is carried out
Random tree growth process obtains multiple tree nodes of random tree；What a length of new tree node of the first step was grown to sampled point
Step-length, the step-length that a length of new tree node of second step is grown to target tree node, the sampled point are empty from the sampling
Between middle sample obtain, the target tree node is described to play point shape or terminal position shape；
During carrying out random tree growth process, the judgement of local minimum state is carried out, is fallen into judging that random tree is grown
When local minimum state, first step-length and second step-length are adjusted, so that the first step is long to be greater than the second step
It is long；
According to multiple tree nodes of the random tree, the task starting point is obtained to the task terminal in joint space
Initial path.
Fourth aspect, a kind of readable storage medium storing program for executing are stored thereon with computer program, and the computer program is by processor
It is performed the steps of when execution
Determine the task starting point of mechanical arm in the joint space of mechanical arm rise point shape, mechanical arm task terminal exist
Terminal position shape in the joint space, and determine the sample space of the joint space；
According to the sample space, point shape and terminal position shape and preset first step-length and the second step-length are played, is carried out
Random tree growth process obtains multiple tree nodes of random tree；What a length of new tree node of the first step was grown to sampled point
Step-length, the step-length that a length of new tree node of second step is grown to target tree node, the sampled point are empty from the sampling
Between middle sample obtain, the target tree node is described to play point shape or terminal position shape；
During carrying out random tree growth process, the judgement of local minimum state is carried out, is fallen into judging that random tree is grown
When local minimum state, first step-length and second step-length are adjusted, so that the first step is long to be greater than the second step
It is long；
According to multiple tree nodes of the random tree, the task starting point is obtained to the task terminal in joint space
Initial path.
Above-mentioned paths planning method, device, mechanical arm controller and readable storage medium storing program for executing, mechanical arm controller can pass through
Determine the task starting point of mechanical arm in the joint space of mechanical arm rise point shape, mechanical arm task terminal in the joint
Terminal position shape in space, and determine the sample space of the joint space；According to the sample space, play point shape and end
Point shape and preset first step-length and the second step-length carry out random tree growth process, obtain multiple burls of random tree
Point；The step-length that a length of new tree node of the first step is grown to sampled point, a length of new tree node of second step is to target
The step-length of tree node growth；According to multiple tree nodes of the random tree, obtains the task starting point to the task terminal and exist
Initial path in joint space；Most importantly, during carrying out random tree growth process, local minimum state is carried out
Judgement adjusts first step-length and second step-length, so that institute when judging that random tree growth falls into local minimum state
It is long greater than second step-length to state the first step, is grown so that random tree is biased to sampled point, escapes out local minimum state, therefore can
To ensure to search path of the task starting point to the task terminal in joint space.
Detailed description of the invention
Fig. 1 is the applied environment figure of paths planning method in one embodiment；
Fig. 2 is the flow diagram of paths planning method in one embodiment；
Fig. 3 a is in one embodiment by the flow diagram of optimization sample space path optimizing；
Fig. 3 b is the schematic diagram of the sample space optimized in one embodiment；
Fig. 3 c is the schematic diagram that barrier method path optimizing is wrapped up in one embodiment；
Fig. 4 is the flow diagram of random two-way tree growth in one embodiment；
Fig. 5 a is the flow diagram of random two-way tree growth in another embodiment；
Fig. 5 b is the schematic diagram of random two-way tree growth in one embodiment；
Fig. 6 is the flow diagram of paths planning method in one embodiment；
Fig. 7 is the structural block diagram of path planning apparatus in one embodiment；
Fig. 8 is the structural block diagram of path planning apparatus in one embodiment.
Specific embodiment
It is with reference to the accompanying drawings and embodiments, right in order to which the objects, technical solutions and advantages of the application are more clearly understood
The application is further elaborated.It should be appreciated that specific embodiment described herein is only used to explain the application, not
For limiting the application.
Paths planning method provided by the embodiments of the present application can be applied in mechanical arm as shown in Figure 1.Wherein, should
Mechanical arm can be the mechanical arm on conventional meaning, be also possible to robot or other machinery for needing to carry out path planning
Structure.Mechanical arm is generally made of multiple diarthrosis and at least one actuator, each joint can be rotated, slide or its
Its driving method, therefore mechanical arm has multiple degrees of freedom, such as 4DOF, 6DOF etc.；Specifically, mechanical arm may include
Mechanical arm controller, mechanical arm controller can control each joint motions of motor driven, carry out control actuator from task starting point
It is moved to task terminal.However, task starting point is moved in task terminal, there may be barrier, locked-up point, itself interference etc.
Region can not be passed through, it is therefore desirable to carry out the planning of paths planning method described in the present embodiment from task starting point to task terminal
Path, mechanical arm controller according to each joint of the path drives mechanical arm, can realize the actuator of mechanical arm from task
Starting point is mobile to the collisionless of task terminal.It is understood that mechanical arm controller can be presented as computer equipment, monolithic
Machine control system and other devices that can carry out data processing.
In one embodiment, as shown in Fig. 2, providing a kind of paths planning method, it is applied in Fig. 1 in this way
It is illustrated for mechanical arm controller, comprising the following steps:
S201 determines that the task starting point of mechanical arm plays point shape, the task of mechanical arm in the joint space of mechanical arm
Terminal position shape of the terminal in the joint space, and determine the sample space of the joint space.
Mechanical arm controller can store the forward and reverse kinematical equation of established mechanical arm.Wherein, positive kinematics are
When referring to size between each joint type known, each adjacent segment, the size of adjacent segment relative variation, solves actuator and exist
Position and direction in basis coordinates system (fixed coordinate system)；Inverse kinematics is with positive kinematics on the contrary, each joint type known, each phase
The position and direction of the information such as the size between adjacent joint and actuator in basis coordinates system (fixed coordinate system) solve phase
The size of adjacent joint relative variation.Therefore, above-mentioned forward and reverse kinematical equation can characterize the big of adjacent segment relative variation
The relationship of the position and direction of small (each joint variable parameter) and actuator in basis coordinates system, is typically embodied as transformation matrix
Form；If the position and direction parameter of the actuator of mechanical arm constitutes cartesian space, each joint variable parameter structure of mechanical arm
At joint space, then forward and reverse kinematical equation is equivalent to the cartesian space of mechanical arm and one kind of the joint space of mechanical arm is reflected
Penetrate relationship.
Illustratively, a kind of D-H modeling method proposed by Denavit and Hartenberg, can be in mechanical arm
Establish joint coordinate system on each joint, according to the DH parameter of multi-degree-of-freemechanical mechanical arm (DH parameter list, it is each for describing robot
Relationship between joint coordinate system) and basis coordinates system, it can determine above-mentioned forward and reverse kinematical equation.
In one embodiment, mechanical arm controller can be according to preset forward and reverse kinematical equation, and input
Task starting point and task terminal determine parameter of the task starting point in joint space, that is, play point shape, and described in determination
Parameter of the task terminal in joint space, i.e. terminal position shape；Illustratively, when the mechanical arm is N freedom degree, described
Point shape and terminal position shape can be N-dimensional parameter respectively, and the variable parameter in some joint is respectively corresponded per one-dimensional parameter.
Certainly, above-mentioned mechanical arm controller can also directly receive point shape and the terminal position shape of input, or obtain
The point shape in preset calculation document or terminal position shape, the present embodiment are not intended to limit this.
The sample space of above-mentioned joint space can be preset sample space, which is only each joint of limitation
The value range of variable parameter, such as certain variable parameters are the relatively variety angle degree in joint, and there is locked angle, therefore
It needs to be defined the value range of the variable parameter；Furthermore the setting of sample space is carried out in order to interior in a limited space
Sampling, to reduce calculation amount.
S202 according to the sample space, plays point shape and terminal position shape and preset first step-length and second step
It is long, random tree growth process is carried out, multiple tree nodes of random tree are obtained；The a length of new tree node of the first step is to sampled point
The step-length of growth, the step-length that a length of new tree node of second step is grown to target tree node, the sampled point is from described
Sampling obtains in sample space, and the target tree node is described point shape or terminal position shape.
In random tree growth process algorithm, mechanical arm controller can will play point shape or terminal position shape in joint space
In starting tree node of any point as random tree carry out unidirectional random tree growth, i.e., using another point as target tree node
Since originating tree node, in sample space sampling obtain sampled point, then choose random tree in any tree node as father
Node (most starts to originate tree node), and according to first step-length and second step-length, the father node is calculated
Child node.Illustratively, to play point shape xstartAs starting tree node, with terminal position shape xgoalAs target tree node, adopt
Sampling point is xrand, father node xf, then child node xnewAre as follows:
Wherein, ρ1Indicate the first step-length that child node (i.e. new tree node) is grown to sampled point direction, ρ2Indicate child node
The step-length grown to goal tree node direction.Generally, above-mentioned sampled point is obtained through stochastical sampling, is random node；Certainly,
Above-mentioned sampling is also possible to the various method of samplings, the present embodiment such as Gauss sampling and is not intended to limit to this.Therefore, according to above-mentioned random
Set growth process, can will new tree node be added random tree in obtain new random tree, further according to new random tree continue into
The above-mentioned growth of row is touched for unidirectional growth until the line between new tree node and the target tree node meets nothing
Hit condition.
It is understood that needing during above-mentioned random tree growth process between above-mentioned child node and father node
Line carry out collision detection；It, just can be with when the line between the child node and the father node meets collisionless condition
Using the child node as the new tree node of the random tree.Specifically, can by the child node and the father node it
Between line be divided into n node, collision detection is carried out respectively to this n node and the child node, if not colliding,
Meet collisionless condition, is then added child node as new tree node in random tree, the son is abandoned if colliding
Node.The acquisition for repeating above-mentioned child node and collision detection process are until adding a collisionless new burl into random tree
Point.Above-mentioned collision detection can be carried out using various bounding box methods, and which is not described herein again.
After determining that above-mentioned child node is collisionless new tree node, the new tree node and institute can also be connected
State target tree node；If the line between two o'clock is unsatisfactory for collisionless condition, illustrate that random tree needs continued growth；If two o'clock
Between line meet collisionless condition, then random tree can stop growing.
It should be noted that above-mentioned father node can be any tree node in above-mentioned random tree, it is also possible to above-mentioned random
It is also possible to sampled point nearest with target tree node in random tree apart from nearest tree node with sampled point in tree, it can be with
It is newest tree node (i.e. most newly generated new tree node) etc. in random tree；The present embodiment is not intended to limit this.
Illustratively, the first step-length and the second step-length can be using D as scale, and one as D is a small amount of；Such as first step-length
It can be 0.025D, second step a length of 0.05D, D have been the distance between point shape and terminal position shape.In the present embodiment, away from
From can be Euclidean distance, or other norms.
S203 carries out the judgement of local minimum state during carrying out random tree growth process, is judging random tree growth
When falling into local minimum state, first step-length and second step-length are adjusted, so that the first step is long to be greater than described the
Two step-lengths.
It is understood that new tree node can be biased to goal tree node direction when second step is long long greater than the first step,
The difference of second step-length and the first step-length is bigger, and new tree node is more biased to target tree node, and the path planned at this time can more connect
Closely ignore the shortest path after barrier；Not barrier ideally this be a kind of effectively method, but it is existing
Very likely there is obstacle between task starting point and task terminal in real environment, if planning algorithm sets always second step and grows up
In the first step-length, local minimum will be fallen near spill barrier, can not cook up feasible path.And generally, initially
The generally long first step that is greater than of setting second step is long when setting, to carry out path planning as efficiently as possible, but it is possible that on
State the state for falling into local minimum.
Therefore, the paths planning method of the present embodiment can carry out local pole during carrying out random tree growth process
Small state judgement adjusts first step-length and second step-length when judging that random tree growth falls into local minimum state,
So that the first step is long to be greater than second step-length.
Optionally, in one embodiment, the number of the tree node of the available random tree of mechanical arm controller,
When the number of the tree node is greater than preset threshold, it is meant that random tree growth may fall into local minimum, then judgement is random
Tree growth falls into local minimum state.
Optionally, in one embodiment, if continuous several tree nodes are (according to the suitable of obtained new tree node
Sequence) in the narrow regions in configuration space, it is likely that nearby there are a local minimums.Illustratively, mechanical arm control
At least one tree node before the available newest tree node of device processed and the newest tree node calculates described newest
Tree node and at least one described tree node between at least one nodal distance；When at least one described nodal distance is small
When pre-determined distance threshold value, determine that the random tree growth falls into local minimum state；When at least one described node
When distance is greater than pre-determined distance threshold value, determine that the random tree growth does not fall into local minimum state.Wherein, newest tree node
For newest obtained new tree node, the new tree that is obtained according at least one tree node before the newest tree node
New tree node for the sequence of node, before the newest tree node.For example, if for certain small positive number εm
(pre-determined distance threshold value), exists | | xi-xi+1| | < εm, | | xi-xi+2| | < εm, and | | xi-xi+3| | < εm, then can recognize
Local minimum state is fallen into for the growth of random tree at this time, wherein xiFor the new tree node in random tree.
S204 obtains the task starting point to the task terminal in joint according to multiple tree nodes of the random tree
Initial path in space.
It is understood that being deposited in multiple tree nodes of obtained random tree after carrying out above-mentioned random tree growth process
Between the tree node of part line composition from the task starting point to the initial path between the task terminal.Work as target
When tree node has been point shape, specifically, because the cut-off condition of random tree growth process is usually newest tree node and mesh
When line between mark tree node meets collisionless condition, therefore can be from the target tree node and the newest tree node
Start, using the target tree node and the newest tree node as the first two path node in the initial path, so
Father node (i.e. there are the tree nodes of set membership with the newest tree node) conduct of the newest tree node is found afterwards
Third path node in the initial path, and then there are the burls of set membership with the third path node for searching
Point is used as the 4th path node ... ..., is so added to up to terminal position shape as the last one path node described；Cause
This, the subpath of second path node is directed toward from first path node, is referred to from second path node
To the subpath ... ... of the third path node, the task starting point is formed by each subpath that each path node forms
To initial path of the task terminal in joint space.
The paths planning method of the present embodiment, mechanical arm controller pass through the task starting point for determining mechanical arm in mechanical arm
Terminal position shape of the task terminal of point shape, mechanical arm in joint space in the joint space, and described in determination
The sample space of joint space；According to the sample space, play point shape and terminal position shape and preset first step-length and the
Two step-lengths carry out random tree growth process, obtain multiple tree nodes of random tree；The a length of new tree node of the first step is to adopting
The step-length of sampling point growth, the step-length that a length of new tree node of second step is grown to target tree node；According to the random tree
Multiple tree nodes, obtain initial path of the task starting point to the task terminal in joint space；Mostly important
It is during carrying out random tree growth process, to carry out the judgement of local minimum state, falls into local pole judging that random tree is grown
When small state, first step-length and second step-length are adjusted, so that the first step is long to be greater than second step-length, so that
Random tree is biased to sampled point growth, escapes out local minimum state, it is hereby ensured that search task starting point to the task
Path of the terminal in joint space.
It is understood that if planning algorithm is set always, the first step is long to be greater than long, the new tree node meeting of the first step
It is biased to sampled point always, this will lead to the inefficient problem of path planning algorithm.The first step-length can be dynamically arranged in the present embodiment
With the second step-length, assuming initially that there is no barrier between task starting point and task terminal, the long first step that is greater than of setting second step is long,
It persistently extends so that random tree is biased to goal tree node direction, until judging that random tree growth has fallen into local minimum, then sets
Set that the first step is long to be greater than that second step is long, so that the direction that new tree node is biased to sampled point grows to flee from local minimum state；
When judging that local minimum state is fled from random tree growth, it is long long greater than the first step that second step can be set dynamically again.Therefore exist
In the present embodiment, mechanical arm controller can be when judging that random tree growth does not fall into local minimum state, adjustment described first
Step-length and second step-length, so that first step-length is less than second step-length.This method had both remained flexible avoidance
The advantages of enhance the efficiency of search again.
In one embodiment, it is related to can specifically include by the process of optimization sample space realizing route optimization: will
The task starting point, as current path, carries out at path optimization to initial path of the task terminal in joint space
Reason, obtains new path；Wherein, referring to shown in Fig. 3 a, path optimization's processing is comprised the following processes:
S301 calculates the path cost of the current path.
Path cost can be the index in assessment path, can be any effective index；For example, path cost can be
Path length, specifically, the length for the sum of the length of each subpath in composition path, each subpath are corresponding for the subpath
The distance between two path nodes, i.e. the path cost characterizes the variation of the joint according to caused by the path clustering mechanical arm
Amount；When path cost minimum, joint variable quantity is minimum, and joint wear is minimum, can be with the prolonged mechanical arm service life.Certainly, path
Cost also uses other evaluation criterions.
S302 determines that a spheroid space is made according to the path cost, described point shape and terminal position shape
For the new sample space of the joint space；Each point and described point shape, the terminal position in the new sample space
The sum of the distance of shape is less than or equal to the path cost.
New sample space after this optimization can be represented by the following formula:
Xf*={ x ∈ Xfree|||xstart-x||2+||x-xgoal||2≤cmax}
This is exactly the expression formula of spheroid (super ellipsoids body), so can claim optimize space constraint again by current path
In the spheroid of construction, Xf*Sample space after indicating optimization, XfreeIndicate collisionless sample space, i.e., the described joint space
Sample space, cmaxIndicate the path cost of current path.
The 2 d plane picture of sample space after optimizing referring to shown in Fig. 3 b；cminFor starting tree node and target burl
Ignore the linear distance after barrier, initial tree nodes X between pointstartWith goal tree nodes XgoalRespectively two of spheroid
Focus, cmaxFor the long axis of spheroid,For the short axle of spheroid.
S303 carries out random tree growth process, obtains the task starting point to described according to the new sample space
Undetermined path of the business terminal in joint space, and when the path cost in the path undetermined is less than the road of the current path
When diameter cost, using the path undetermined as new path.
According to the property of spheroid it is found that the sampled point sampled in the sample space respectively with starting tree node,
The sum of the distance of target tree node is less than path cost cmax, therefore it is relatively former according to the sampled point of the sample space after the optimization
Sample space is more excellent, it is ensured that is sampled out of space that may improve current path cost；Therefore the path generation in new path
Valence there is a strong possibility relative to current path path cost it is less than normal, realizing route optimization；That is, according to current path structure
Spheroid is built to optimize sample space, it can be ensured that algorithm is only sampled from can optimize in the space of current path, so that search road
Diameter levels off to optimal, improves the search efficiency of algorithm.
Optionally, the method also includes: using the new path as current path, continue at path optimization
Reason obtains the final new path until the new path meets preset condition；Wherein it is determined that the new path
Meeting preset condition includes: at least one path before obtaining newest path and the newest path, described in calculating
The difference of at least one path cost between newest path and at least one described path；When at least one described path generation
When the difference of valence is respectively less than or is equal to preset path cost threshold value, determine that the new path meets preset condition.
It is understood that in the present embodiment, can since initial path, using initial path as current path,
New path after being optimized according to above-mentioned path optimization's treatment process can continue to optimize, realizing route it is continuous progressive
Optimization.Above-mentioned optimization process should be noted how to judge that path has leveled off to optimal, and the present embodiment can use as follows
Method judges it is optimal whether path has leveled off to: for certain small positive εc(εcValue and the size of sample space be in
It is positively correlated), exist:
Wherein, ciIndicate that the path cost of the i-th paths, i are the integer more than or equal to 4；Illustratively, when continuous
When the path cost of 4 paths is close, then avoiding repetitive operation, therefore can it is considered that path has leveled off to optimal
To improve operation efficiency.
Optionally, when path optimization's approach is optimal, the newest path can be carried out according to package barrier method
Optimization, the destination path after being optimized.Wherein, the newest path is the optimal road that the path optimization is handled
Diameter.It, can be using another optimal way: package barrier when also being difficult to effectively be optimized by way of optimizing sample space
Hinder object method, optimize again, obtains destination path.
Referring to the schematic diagram for wrapping up barrier method shown in Fig. 3 c, indicate in two-dimensional surface by approach package barrier come
The process of path optimizing, dark black line indicates the path (figure (a)) originally planned in figure, after light black line indicates double optimization
Path (figure (f)), dark rectangular indicates barrier.It assumes initially that through initial path P0There are four path node (figures altogether
(a)) path node for, needing to optimize is the node other than Origin And Destination, i.e. node x2And x3, the optimization of each node
Process is divided into two steps.Step 1: discrete nodes x2And x3Between path, obtain the new discrete nodes of n, they be denoted as x2i
(i ∈ (1, n)), then by these discrete nodes successively with x2Previous node be connected, to this line carry out collision detection,
First node for finding the detection that collides is denoted as x'2(figure (b))；Step 2: discrete x2Previous node and node x'2
Between path, obtain the new discrete nodes of n, they be denoted as x'2i(i ∈ (1, n)), then successively by these discrete nodes
With x3It is connected, collision detection is carried out to this line, first node for finding the detection that collides is denoted as x2new(figure (c))；Through
Crossing this two-step pretreatment can be by node x2It is substituted for new node x2new.Node x3Treatment process it is identical (figure (d), scheme (e)),
By node x3It is substituted for new node x3new.Path P according to Triangle inequality, after secondary treatment1(x1→x2new→x3new→
x4) it is substantially better than initial path P0(x1→x2→x3→x4).Likewise it is possible to by package barrier method to the optimal road
Each path node in diameter carries out suboptimization again, the target road that the path node directed connection after obtaining new optimization is constituted
Diameter.
It is understood that can choose obtained to S204 first because package barrier method operand is larger
Beginning path carries out the optimization of package barrier method, is then carried out by the way of the lesser optimization sample space of calculation amount to path
It continues to optimize, until it is optimal to be becoming tight, obtains newest path, finally newest path is carried out using package barrier method again excellent
Change, obtain destination path, has not only avoided increasing the collision detection time because excessive use wraps up barrier method, but also improve algorithm effect
Rate.
In one embodiment, referring to shown in Fig. 4, the present embodiment is related to carrying out path rule using the growth of random two-way tree
It draws, can specifically include:
S401 using terminal position shape as the target tree node of the first random tree, and described will play the conduct of point shape
The target tree node of second random tree.
When using random two-way tree growth algorithm, can using point shape as the starting tree node of the first random tree,
Using terminal position shape as the target tree node of the first random tree, the growth process of the first random tree is carried out；Using terminal position shape as
The starting tree node of second random tree carries out the second random tree to play point shape as the target tree node of the second random tree
Growth process.
S402 is directed to first random tree and second random tree respectively, according to the sample space, preset the
One step-length and the second step-length carry out random tree growth process, until the newest tree node and the second random tree of the first random tree
Line between newest tree node meets collisionless condition.
When the line between the newest tree node of the first random tree and the newest tree node of the second random tree meets collisionless
When condition, it is meant that the first random tree and the growth of the second random tree are completed, can be true according to the first random tree and the second random tree
Determine path.
Specifically, described according to the sample space, preset first step-length and second step referring to shown in Fig. 5 a and Fig. 5 b
It is long, random tree growth process is carried out, may include:
S501, sampling obtains sampled point from the sample space of the joint space；
S502 traverses each tree node in the random tree, and the determining and sampled point is apart from the smallest tree node as father
Node；The random tree is first random tree or second random tree；
S503, according to the father node, sampled point, target tree node and first step-length and second step-length,
Determine the child node grown from the father node；
S504, when the line between the child node and the father node meets collisionless condition, by the child node
New tree node as the random tree.
The process of above-mentioned random two-way tree growth is referred to the process of above-mentioned unidirectional random tree growth, no longer superfluous here
It states.It should be noted that the first random tree TsChild nodes xnewsIt is following to indicate:
Second random tree TgChild nodes xnewgIt is following to indicate:
Wherein, xstartAs starting tree node, xgoalAs target tree node, sampled point xrand, father node is
xnearest。
Optionally, multiple tree nodes according to the random tree obtain the task starting point to the task terminal
Path in joint space may include: in newest tree node and first random tree according to first random tree
Father and son's incidence relation of each tree node determines in first random tree from described point shape to the road of the newest tree node
The sequence of multiple first tree nodes and the multiple first tree node on diameter；According to the newest tree node of second random tree
With father and son's incidence relation of each tree node in second random tree, determine in second random tree from the newest tree node
The sequence of multiple second tree nodes and the multiple second tree node on to the path of terminal position shape；According to the multiple
The sequence and the multiple second tree node of first tree node and the multiple first tree node and the multiple second burl
The sequence of point determines multiple tree nodes and the multiple burl from described in point shape to the path of terminal position shape
The sequence of point, the path as the task starting point to the task terminal in joint space.
The random two-way tree referring to shown in Fig. 5 b grows schematic diagram, wherein rectangular area, delta-shaped region etc. are obstacle
Object area, the first random tree TsFor multiple tree node compositions interconnected in the lower left corner in figure, the second random tree TgIt is right in figure
Multiple tree node compositions interconnected at upper angle；It is understood that between two tree nodes of connection relationship, there are fathers
Sub- incidence relation.In figure 5b, as the newest tree node x of the first random treenewsWith the newest tree node x of the second random treenewgIt
Between line when meeting collisionless condition, can according to father and son's incidence relation of tree node each in the first random tree, determine described in
From the newest tree node x in first random treenewsMultiple first tree nodes on to the path for playing point shape and described
The sequence of multiple first tree nodes: xnews→xnewsFather node →... ... → xstartChild node → xstart, and then can be anti-
Sequentially, i.e., from described point shape to the newest tree node xnewsPath on multiple first tree nodes and the multiple
The sequence of one tree node: xstart→xstartChild node →... ... → xnewsFather node → xnews.Likewise it is possible to direct root
According to father and son's incidence relation of each tree node in the second random tree, determine from the newest tree node to the path of terminal position shape
On multiple second tree nodes and the multiple second tree node sequence: xnewg→xnewgFather node →... ... → xgoal's
Child node → xgoal.May finally obtain multiple tree nodes from described in point shape to the path of terminal position shape and
The sequence of the multiple tree node: xstart→xstartChild node →... ... → xnewsFather node → xnews→xnewg→xnewg
Father node →... ... → xgoalChild node → xgoal, as the task starting point to the task terminal in joint space
Path.
Referring to shown in Fig. 6, a kind of paths planning method based on the growth of random two-way tree is shown from another angle
Schematic diagram may include:
S601, stochastical sampling obtains the sampled point of the first random tree in sample space, then executes S602；
S602, the first random tree of traversal obtain the nearest point of sampled point of the first random tree in distance S601 as father's section
Point, then executes S603；
S603, judges whether the growth of the first random tree falls into local minimum state；If it is not, then executing S604；If so,
Execute S605；
S604, according to the step-length adjustment mechanism under preset non local minimum state, so that the first step-length is less than second step
It is long, and the child node of father node in S602 is obtained according to the first step-length and second step length, then execute S606；
S605, according to the step-length adjustment mechanism under preset local minimum state, so that the long second step that is greater than of the first step is long,
And the child node of father node in S602 is obtained according to the first step-length and second step length, then execute S606；
S606 carries out collision detection to the line between the child node and the father node, judges whether to collide；
If so, executing S601；If it is not, then executing S607；
S607, by the child node be added the first random tree in, the tree node new as one, then execute S608 and
S613；
S608, stochastical sampling obtains the sampled point of the second random tree in sample space, then executes S609；
S609, the second random tree of traversal obtain the nearest point of sampled point of the second random tree in distance S608 as father's section
Point, then executes S610；
S610, judges whether the growth of the second random tree falls into local minimum state；If it is not, then according to preset non local
Step-length adjustment mechanism under minimum state, so that the first step-length is less than the second step-length, if so, according to preset local minimum shape
Step-length adjustment mechanism under state, so that the first step is long long greater than second step；It is obtained in S609 according to the first step-length and second step length
The child node of father node, then executes S611；
S611 carries out collision detection to the line between the child node and the father node, judges whether to collide；
If so, executing S608；If it is not, then executing S612；
The child node is added in the second random tree S612, then the tree node new as one executes S613；
S613 connects the new tree node of first random tree and the new tree node of the second random tree, then executes
S614；
The line of S614, the new tree node of new tree node and the second random tree to first random tree touch
Detection is hit, judges whether to collide；If so, executing S601；If it is not, then executing S615；
S615 judges whether to have obtained initial path, if it is not, then executing S616；If so, executing S619；
S616, according to the new of the first random tree not collided in the first random tree and the second random tree and S614
Tree node and the second random tree new tree node, generate initial path P0, then execute S617；
S617 carries out package barrier optimization to initial path, obtains P1, then execute S618；
S618 optimizes sample space, then executes using P1 as current path according to the path cost of current path
S601；
S619, judges whether the process for optimizing sample space progress path optimization tends to be optimal, if it is not, then executing S620；
If so, executing S621；
S620 optimizes sample space, then executes S601 according to the path cost of current path；
S621 carries out package barrier optimization to current path, obtains destination path.
Although should be understood that Fig. 2,3a, 4,5a, each step in 6 flow chart according to arrow instruction successively
It has been shown that, but these steps are not that the inevitable sequence according to arrow instruction successively executes.Unless expressly state otherwise herein,
There is no stringent sequences to limit for the execution of these steps, these steps can execute in other order.Moreover, Fig. 2,3a,
At least part step in 4,5a, 6 may include that perhaps these sub-steps of multiple stages or stage be not for multiple sub-steps
Completion necessarily is executed in synchronization, but can be executed at different times, the execution in these sub-steps or stage is suitable
Sequence, which is also not necessarily, successively to be carried out, but can be at least one of the sub-step or stage of other steps or other steps
Minute wheel stream alternately executes.
In one embodiment, as shown in fig. 7, providing a kind of path planning apparatus, comprising: initialization module 71, with
Machine tree pop-in upgrades 72, the first step size adjusting module 73 and initial path obtain module 74, in which:
Initialization module 71, for determine the task starting point of mechanical arm in the joint space of mechanical arm rise point shape,
Terminal position shape of the task terminal of mechanical arm in the joint space, and determine the sample space of the joint space；
Random tree pop-in upgrades 72, for according to the sample space, play point shape and terminal position shape and preset the
One step-length and the second step-length carry out random tree growth process, obtain multiple tree nodes of random tree；The first step is a length of new
The step-length that tree node is grown to sampled point, the step-length that a length of new tree node of second step is grown to target tree node are described
Sampled point is to sample to obtain from the sample space, and the target tree node is described point shape or the terminal position
Shape；
First step size adjusting module 73 is sentenced for during carrying out random tree growth process, carrying out local minimum state
It is disconnected, when judging that random tree growth falls into local minimum state, first step-length and second step-length are adjusted, so that described
The first step is long to be greater than second step-length；
Initial path obtains module 74 and obtains the task starting point extremely for multiple tree nodes according to the random tree
Initial path of the task terminal in joint space.
Optionally, referring to shown in Fig. 8, described device can also include:
Second step size adjusting module 75, for when judging that random tree growth does not fall into local minimum state, described in adjustment
First step-length and second step-length, so that first step-length is less than second step-length.
Optionally, the progress local minimum state judgement, comprising: obtain newest tree node and the newest tree
At least one tree node before node calculates at least one between the newest tree node and at least one described tree node
A nodal distance；When at least one described nodal distance is respectively less than or is equal to pre-determined distance threshold value, determine that the random tree is raw
Length falls into local minimum state；When at least one described nodal distance is greater than pre-determined distance threshold value, determine that the random tree is raw
Length does not fall into local minimum state.
Optionally, referring to shown in Fig. 8, described device can also include:
First path optimization module 76, for the task starting point is initial in joint space to the task terminal
Path carries out path optimization's processing, obtains new path as current path；Wherein, path optimization's processing includes: to calculate
The path cost of the current path；According to the path cost, described point shape and terminal position shape, determine one it is ellipse
New sample space of the sphere space as the joint space；Each point and described point shape in the new sample space,
The sum of the distance of terminal position shape is less than or equal to the path cost；According to the new sample space, random tree is carried out
Growth process obtains undetermined path of the task starting point to the task terminal in joint space, and when described undetermined
When the path cost in path is less than the path cost of the current path, using the path undetermined as new path.
Further, referring to shown in Fig. 8, described device can also include:
Optimal control module 77, for continuing path optimization's processing, directly using the new path as current path
Meet preset condition to the new path, obtains the final new path；Wherein it is determined that the new path meets in advance
If condition includes: at least one path before obtaining newest path and the newest path, calculate described newest
The difference of at least one path cost between path and at least one described path；When the difference of at least one path cost
When value is respectively less than or is equal to preset path cost threshold value, determine that the new path meets preset condition.
Further, referring to shown in Fig. 8, described device can also include:
Second path optimization's module 78 is obtained for being optimized according to package barrier method to the newest path
Destination path after optimization.
Optionally, referring to shown in Fig. 8, the random tree pop-in upgrades 72 may include:
Random two-way tree setting unit 721, for using terminal position shape as the target tree node of the first random tree, with
And using described point shape as the target tree node of the second random tree；
Random two-way tree growing element 722, for being directed to first random tree and second random tree respectively, according to
The sample space, preset first step-length and the second step-length carry out random tree growth process, until the first random tree is newest
Line between tree node and the newest tree node of the second random tree meets collisionless condition.
Optionally, random two-way tree growing element 722 is specifically used for sampling from the sample space of the joint space
To sampled point；Each tree node in the random tree is traversed, the determining and sampled point is apart from the smallest tree node as father node；
The random tree is first random tree or second random tree；According to the father node, sampled point, target tree node,
And first step-length and second step-length, determine the child node grown from the father node；When the child node and
When line between the father node meets collisionless condition, using the child node as the new tree node of the random tree.
Further, referring to shown in Fig. 8, the initial path obtains module 74 and may include:
First path acquiring unit 741, for first according to the newest tree node of first random tree and described first
Father and son's incidence relation of each tree node in random tree determines in first random tree from described point shape to the newest tree
The sequence of multiple first tree nodes and the multiple first tree node on the path of node；
Second path acquiring unit 742, for according to the newest tree node of second random tree and described second random
Father and son's incidence relation of each tree node in tree determines in second random tree from the newest tree node to terminal position shape
Path on multiple second tree nodes and the multiple second tree node sequence；
Initial path acquiring unit 743, for according to the multiple first tree node and the multiple first tree node
Sequentially and the sequence of the multiple second tree node and the multiple second tree node, determine from described point shape to institute
The sequence for stating the multiple tree nodes and the multiple tree node on the path of terminal position shape, as the task starting point to described
Path of the task terminal in joint space.
Specific about path planning apparatus limits the restriction that may refer to above for paths planning method, herein not
It repeats again.Modules in above-mentioned path planning apparatus can be realized fully or partially through software, hardware and combinations thereof.On
Stating each module can be embedded in the form of hardware or independently of in the processor in computer equipment, can also store in a software form
In memory in computer equipment, the corresponding operation of the above modules is executed in order to which processor calls.
In one embodiment, a kind of mechanical arm controller, including memory and processor are provided, is stored in memory
There is computer program, which performs the steps of when executing computer program
Determine the task starting point of mechanical arm in the joint space of mechanical arm rise point shape, mechanical arm task terminal exist
Terminal position shape in the joint space, and determine the sample space of the joint space；
According to the sample space, point shape and terminal position shape and preset first step-length and the second step-length are played, is carried out
Random tree growth process obtains multiple tree nodes of random tree；What a length of new tree node of the first step was grown to sampled point
Step-length, the step-length that a length of new tree node of second step is grown to target tree node, the sampled point are empty from the sampling
Between middle sample obtain, the target tree node is described to play point shape or terminal position shape；
During carrying out random tree growth process, the judgement of local minimum state is carried out, is fallen into judging that random tree is grown
When local minimum state, first step-length and second step-length are adjusted, so that the first step is long to be greater than the second step
It is long；
According to multiple tree nodes of the random tree, the task starting point is obtained to the task terminal in joint space
Initial path.
In one embodiment, a kind of readable storage medium storing program for executing is provided, computer program, computer program are stored thereon with
It is performed the steps of when being executed by processor
Determine the task starting point of mechanical arm in the joint space of mechanical arm rise point shape, mechanical arm task terminal exist
Terminal position shape in the joint space, and determine the sample space of the joint space；
According to the sample space, point shape and terminal position shape and preset first step-length and the second step-length are played, is carried out
Random tree growth process obtains multiple tree nodes of random tree；What a length of new tree node of the first step was grown to sampled point
Step-length, the step-length that a length of new tree node of second step is grown to target tree node, the sampled point are empty from the sampling
Between middle sample obtain, the target tree node is described to play point shape or terminal position shape；
During carrying out random tree growth process, the judgement of local minimum state is carried out, is fallen into judging that random tree is grown
When local minimum state, first step-length and second step-length are adjusted, so that the first step is long to be greater than the second step
It is long；
According to multiple tree nodes of the random tree, the task starting point is obtained to the task terminal in joint space
Initial path.
Those of ordinary skill in the art will appreciate that realizing all or part of the process in above-described embodiment method, being can be with
Relevant hardware is instructed to complete by computer program, the computer program can be stored in a non-volatile computer
In read/write memory medium, the computer program is when being executed, it may include such as the process of the embodiment of above-mentioned each method.Wherein,
To any reference of memory, storage, database or other media used in each embodiment provided herein,
Including non-volatile and/or volatile memory.Nonvolatile memory may include read-only memory (ROM), programming ROM
(PROM), electrically programmable ROM (EPROM), electrically erasable ROM (EEPROM) or flash memory.Volatile memory may include
Random access memory (RAM) or external cache.By way of illustration and not limitation, RAM is available in many forms,
Such as static state RAM (SRAM), dynamic ram (DRAM), synchronous dram (SDRAM), double data rate sdram (DDRSDRAM), enhancing
Type SDRAM (ESDRAM), synchronization link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM
(RDRAM), direct memory bus dynamic ram (DRDRAM) and memory bus dynamic ram (RDRAM) etc..
Each technical characteristic of above embodiments can be combined arbitrarily, for simplicity of description, not to above-described embodiment
In each technical characteristic it is all possible combination be all described, as long as however, the combination of these technical characteristics be not present lance
Shield all should be considered as described in this specification.
The several embodiments of the application above described embodiment only expresses, the description thereof is more specific and detailed, but simultaneously
It cannot therefore be construed as limiting the scope of the patent.It should be pointed out that coming for those of ordinary skill in the art
It says, without departing from the concept of this application, various modifications and improvements can be made, these belong to the protection of the application
Range.Therefore, the scope of protection shall be subject to the appended claims for the application patent.
Claims (12)
1. a kind of paths planning method, which is characterized in that the described method includes:
Determine the task starting point of mechanical arm in the joint space of mechanical arm rise point shape, mechanical arm task terminal described
Terminal position shape in joint space, and determine the sample space of the joint space；
According to the sample space, point shape and terminal position shape and preset first step-length and the second step-length are played, is carried out random
Growth process is set, multiple tree nodes of random tree are obtained；The step-length that a length of new tree node of the first step is grown to sampled point,
The step-length that a length of new tree node of second step is grown to target tree node, the sampled point is adopted from the sample space
What sample obtained, the target tree node is described point shape or terminal position shape；
During carrying out random tree growth process, the judgement of local minimum state is carried out, falls into part judging that random tree is grown
When minimum state, first step-length and second step-length are adjusted, so that the first step is long to be greater than second step-length；
According to multiple tree nodes of the random tree, it is first in joint space to the task terminal to obtain the task starting point
Beginning path.
2. the method according to claim 1, wherein the method also includes:
When judging that random tree growth does not fall into local minimum state, first step-length and second step-length are adjusted, so that
First step-length is less than second step-length.
3. the method according to claim 1, wherein the progress local minimum state judgement, comprising:
At least one tree node before newest tree node and the newest tree node is obtained, the newest tree is calculated
At least one nodal distance between node and at least one described tree node；
When at least one described nodal distance is respectively less than or is equal to pre-determined distance threshold value, determine that the random tree growth falls into office
The minimum state in portion；
When at least one described nodal distance is greater than pre-determined distance threshold value, determine that the random tree growth does not fall into local minimum
State.
4. the method according to claim 1, wherein the method also includes:
Using initial path of the task starting point to the task terminal in joint space as current path, it is excellent to carry out path
Change processing, obtains new path；
Wherein, the path optimization, which is handled, includes:
Calculate the path cost of the current path；
According to the path cost, described point shape and terminal position shape, determine a spheroid space as the pass
Save the new sample space in space；The distance of each point and described point shape, terminal position shape in the new sample space
The sum of be less than or equal to the path cost；
According to the new sample space, random tree growth process is carried out, the task starting point to the task terminal is obtained and exists
Path undetermined in joint space, and when the path cost in the path undetermined is less than the path cost of the current path
When, using the path undetermined as new path.
5. according to the method described in claim 4, it is characterized in that, the method also includes:
Using the new path as current path, continue path optimization's processing, is preset until the new path meets
Condition obtains the final new path；
Wherein it is determined that the new path meets preset condition includes:
At least one path before newest path and the newest path is obtained, the newest path and institute are calculated
State the difference of at least one path cost between at least one path；
When the difference of at least one path cost is respectively less than or is equal to preset path cost threshold value, the new road is determined
Diameter meets preset condition.
6. according to the method described in claim 5, it is characterized in that, the method also includes:
The newest path is optimized according to package barrier method, the destination path after being optimized.
7. method according to claim 1 to 6, which is characterized in that described according to the sample space, starting point
Position shape and terminal position shape and preset first step-length and the second step-length, carry out random tree growth process, comprising:
Using terminal position shape as the target tree node of the first random tree, and point shape described will be played as the second random tree
Target tree node；
Respectively be directed to first random tree and second random tree, according to the sample space, preset first step-length and
Second step-length carries out random tree growth process, until the newest tree node of the first random tree and the newest burl of the second random tree
Line between point meets collisionless condition.
8. the method according to the description of claim 7 is characterized in that described according to the sample space, preset first step-length
With the second step-length, random tree growth process is carried out, comprising:
Sampling obtains sampled point from the sample space of the joint space；
Each tree node in the random tree is traversed, the determining and sampled point is apart from the smallest tree node as father node；It is described
Random tree is first random tree or second random tree；
According to the father node, sampled point, target tree node and first step-length and second step-length, determine from institute
State the child node that father node is grown；
When the line between the child node and the father node meets collisionless condition, using the child node as it is described with
The new tree node of machine tree.
9. according to the method described in claim 8, it is characterized in that, multiple tree nodes according to the random tree, obtain
Path of the task starting point to the task terminal in joint space, comprising:
According to father and son's incidence relation of each tree node in the newest tree node of first random tree and first random tree, really
Multiple first tree nodes and institute in fixed first random tree from described in point shape to the path of the newest tree node
State the sequence of multiple first tree nodes；
According to father and son's incidence relation of each tree node in the newest tree node of second random tree and second random tree, really
In fixed second random tree from the newest tree node to the path of terminal position shape multiple second tree nodes and institute
State the sequence of multiple second tree nodes；
According to the sequence and the multiple second tree node of the multiple first tree node and the multiple first tree node and
The sequence of the multiple second tree node determines multiple tree nodes from described in point shape to the path of terminal position shape
And the sequence of the multiple tree node, the path as the task starting point to the task terminal in joint space.
10. a kind of path planning apparatus, which is characterized in that described device includes:
Initialization module, for determining that the task starting point of mechanical arm plays point shape, mechanical arm in the joint space of mechanical arm
Terminal position shape of the task terminal in the joint space, and determine the sample space of the joint space；
Random tree pop-in upgrades, for according to the sample space, point shape and terminal position shape and preset first step-length
With the second step-length, random tree growth process is carried out, multiple tree nodes of random tree are obtained；The a length of new tree node of the first step
The step-length grown to sampled point, the step-length that a length of new tree node of second step is grown to target tree node, the sampled point
It is to sample to obtain from the sample space, the target tree node is described point shape or terminal position shape；
First step size adjusting module, for carrying out the judgement of local minimum state, sentencing during carrying out random tree growth process
When disconnected random tree growth falls into local minimum state, first step-length and second step-length are adjusted, so that the first step
It is long to be greater than second step-length；
Initial path obtains module and obtains the task starting point to described for multiple tree nodes according to the random tree
Initial path of the business terminal in joint space.
11. a kind of machinery arm controller, including memory and processor, the memory are stored with computer program, feature
It is, the step of processor realizes any one of claims 1 to 9 the method when executing the computer program.
12. a kind of readable storage medium storing program for executing, is stored thereon with computer program, which is characterized in that the computer program is processed
The step of device realizes method described in any one of claims 1 to 9 when executing.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910190556.4A CN109877836B (en) | 2019-03-13 | 2019-03-13 | Path planning method and device, mechanical arm controller and readable storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910190556.4A CN109877836B (en) | 2019-03-13 | 2019-03-13 | Path planning method and device, mechanical arm controller and readable storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109877836A true CN109877836A (en) | 2019-06-14 |
| CN109877836B CN109877836B (en) | 2021-06-08 |
Family
ID=66932316
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910190556.4A Active CN109877836B (en) | 2019-03-13 | 2019-03-13 | Path planning method and device, mechanical arm controller and readable storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109877836B (en) |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110509279A (en) * | 2019-09-06 | 2019-11-29 | 北京工业大学 | A kind of trajectory path planning method and system of apery mechanical arm |
| CN110646006A (en) * | 2019-09-02 | 2020-01-03 | 平安科技（深圳）有限公司 | Assembly path planning method and related device |
| CN110653805A (en) * | 2019-10-10 | 2020-01-07 | 西安科技大学 | Task constraint path planning method for seven-degree-of-freedom redundant manipulator in Cartesian space |
| CN111113428A (en) * | 2019-12-31 | 2020-05-08 | 深圳市优必选科技股份有限公司 | Robot control method, robot control device and terminal equipment |
| CN111531550A (en) * | 2020-07-09 | 2020-08-14 | 浙江大华技术股份有限公司 | Motion planning method and device, storage medium and electronic device |
| CN112192566A (en) * | 2020-09-25 | 2021-01-08 | 武汉联影智融医疗科技有限公司 | Control method for end adapter of mechanical arm |
| CN113119116A (en) * | 2021-03-22 | 2021-07-16 | 深圳市优必选科技股份有限公司 | Mechanical arm motion planning method and device, readable storage medium and mechanical arm |
| CN113433954A (en) * | 2021-06-17 | 2021-09-24 | 江苏科技大学 | Underwater robot three-dimensional global path planning method based on improved RRT algorithm |
| CN113459087A (en) * | 2021-05-28 | 2021-10-01 | 北京精密机电控制设备研究所 | Path planning method capable of limiting deflection angle based on minimum potential energy algorithm |
| CN114174009A (en) * | 2019-09-30 | 2022-03-11 | 西门子（中国）有限公司 | Method, device and system for controlling robot, storage medium and terminal |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20100174435A1 (en) * | 2009-01-07 | 2010-07-08 | Samsung Electronics Co., Ltd. | Path planning apparatus of robot and method thereof |
| US20110035087A1 (en) * | 2009-08-10 | 2011-02-10 | Samsung Electronics Co., Ltd. | Method and apparatus to plan motion path of robot |
| CN107234617A (en) * | 2017-07-10 | 2017-10-10 | 北京邮电大学 | A kind of obstacle-avoiding route planning method of the unrelated Artificial Potential Field guiding of avoidance task |
| CN108876024A (en) * | 2018-06-04 | 2018-11-23 | 清华大学深圳研究生院 | Path planning, path real-time optimization method and device, storage medium |
| CN108983780A (en) * | 2018-07-24 | 2018-12-11 | 武汉理工大学 | One kind is based on improvement RRT*The method for planning path for mobile robot of algorithm |
| CN108981704A (en) * | 2018-07-13 | 2018-12-11 | 昆明理工大学 | A kind of two-way RRT paths planning method of target gravitation based on dynamic step length |
-
2019
- 2019-03-13 CN CN201910190556.4A patent/CN109877836B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20100174435A1 (en) * | 2009-01-07 | 2010-07-08 | Samsung Electronics Co., Ltd. | Path planning apparatus of robot and method thereof |
| US20110035087A1 (en) * | 2009-08-10 | 2011-02-10 | Samsung Electronics Co., Ltd. | Method and apparatus to plan motion path of robot |
| CN107234617A (en) * | 2017-07-10 | 2017-10-10 | 北京邮电大学 | A kind of obstacle-avoiding route planning method of the unrelated Artificial Potential Field guiding of avoidance task |
| CN108876024A (en) * | 2018-06-04 | 2018-11-23 | 清华大学深圳研究生院 | Path planning, path real-time optimization method and device, storage medium |
| CN108981704A (en) * | 2018-07-13 | 2018-12-11 | 昆明理工大学 | A kind of two-way RRT paths planning method of target gravitation based on dynamic step length |
| CN108983780A (en) * | 2018-07-24 | 2018-12-11 | 武汉理工大学 | One kind is based on improvement RRT*The method for planning path for mobile robot of algorithm |
Cited By (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110646006A (en) * | 2019-09-02 | 2020-01-03 | 平安科技（深圳）有限公司 | Assembly path planning method and related device |
| CN110509279A (en) * | 2019-09-06 | 2019-11-29 | 北京工业大学 | A kind of trajectory path planning method and system of apery mechanical arm |
| CN114174009B (en) * | 2019-09-30 | 2023-07-21 | 西门子（中国）有限公司 | Method, device, system, storage medium and terminal for controlling robot |
| CN114174009A (en) * | 2019-09-30 | 2022-03-11 | 西门子（中国）有限公司 | Method, device and system for controlling robot, storage medium and terminal |
| CN110653805A (en) * | 2019-10-10 | 2020-01-07 | 西安科技大学 | Task constraint path planning method for seven-degree-of-freedom redundant manipulator in Cartesian space |
| CN111113428A (en) * | 2019-12-31 | 2020-05-08 | 深圳市优必选科技股份有限公司 | Robot control method, robot control device and terminal equipment |
| CN111113428B (en) * | 2019-12-31 | 2021-08-27 | 深圳市优必选科技股份有限公司 | Robot control method, robot control device and terminal equipment |
| CN111531550A (en) * | 2020-07-09 | 2020-08-14 | 浙江大华技术股份有限公司 | Motion planning method and device, storage medium and electronic device |
| CN112192566B (en) * | 2020-09-25 | 2022-03-01 | 武汉联影智融医疗科技有限公司 | Control method for end adapter of mechanical arm |
| CN112192566A (en) * | 2020-09-25 | 2021-01-08 | 武汉联影智融医疗科技有限公司 | Control method for end adapter of mechanical arm |
| CN113119116A (en) * | 2021-03-22 | 2021-07-16 | 深圳市优必选科技股份有限公司 | Mechanical arm motion planning method and device, readable storage medium and mechanical arm |
| WO2022198994A1 (en) * | 2021-03-22 | 2022-09-29 | 深圳市优必选科技股份有限公司 | Robot arm motion planning method and apparatus, and readable storage medium and robot arm |
| CN113459087A (en) * | 2021-05-28 | 2021-10-01 | 北京精密机电控制设备研究所 | Path planning method capable of limiting deflection angle based on minimum potential energy algorithm |
| CN113459087B (en) * | 2021-05-28 | 2022-07-05 | 北京精密机电控制设备研究所 | Path planning method capable of limiting deflection angle based on minimum potential energy algorithm |
| CN113433954A (en) * | 2021-06-17 | 2021-09-24 | 江苏科技大学 | Underwater robot three-dimensional global path planning method based on improved RRT algorithm |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109877836B (en) | 2021-06-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109877836A (en) | Paths planning method, device, mechanical arm controller and readable storage medium storing program for executing | |
| US10365110B2 (en) | Method and system for determining a path of an object for moving from a starting state to an end state set avoiding one or more obstacles | |
| CN108549385B (en) | Robot dynamic path planning method combining A-x algorithm and VFH obstacle avoidance algorithm | |
| CN109945873B (en) | Hybrid path planning method for indoor mobile robot motion control | |
| US8774965B2 (en) | Method and device for controlling a manipulator | |
| CN107883961A (en) | A kind of underwater robot method for optimizing route based on Smooth RRT algorithms | |
| CN113172626B (en) | Intelligent robot group control method based on three-dimensional gene regulation and control network | |
| CN109491389A (en) | A kind of robot trace tracking method with constraint of velocity | |
| JP2021175590A (en) | Robot optimization operation planning initial stage reference generation | |
| CN111015656A (en) | Control method and device for robot to actively avoid obstacle and storage medium | |
| JP2008105132A (en) | Method and apparatus for producing path of arm in joint space | |
| CN114633258A (en) | Method for planning mechanical arm movement track in tunnel environment and related device | |
| CN110275528B (en) | Improved path optimization method for RRT algorithm | |
| JP2020004421A (en) | Methods and systems for determining a path of an object moving from an initial state to final state set while avoiding one or more obstacle | |
| CN112828889A (en) | Six-axis cooperative mechanical arm path planning method and system | |
| Yang et al. | A novel path planning algorithm for warehouse robots based on a two-dimensional grid model | |
| Zhao et al. | Improved manipulator obstacle avoidance path planning based on potential field method | |
| CN114939872B (en) | MIRRT-Connect algorithm-based intelligent storage redundant mechanical arm dynamic obstacle avoidance motion planning method | |
| Masehian et al. | An improved particle swarm optimization method for motion planning of multiple robots | |
| US20210245364A1 (en) | Method And Control System For Controlling Movement Trajectories Of A Robot | |
| CN116698069A (en) | Goods picking path optimization method based on chaotic particle swarm optimization algorithm | |
| WO2022149278A1 (en) | Machining program correction device, numerical control device, machining program correction method, and machine learning device | |
| Hirashima et al. | A new reinforcement learning for group-based marshaling plan considering desired layout of containers in port terminals | |
| CN113146637B (en) | Robot Cartesian space motion planning method | |
| CN114986501A (en) | Mechanical arm path planning method and system and mechanical arm |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |