Summary of the invention
To the problem that exists in the prior art; This patent has proposed a kind of three dimensions paths planning method based on the elastic threads kinetic property; The thought of the searching space path of this method derives from the kinetic property of elastic threads in the real world, the elastic threads that two ends are fixing; Give initial position of elastic threads arbitrarily; Under the holding power and the elastic force effect of self of the damping force of air, barrier, it always can be stabilized on the shortest locus that possibly move to, and the locus when elastic threads is stablized so is a comparatively ideal space path.
The present invention proposes a kind of three-dimensional sky based on elastic threads kinetic property footpath planing method of asking the way, and specifically comprises following step (mould of following symbol
expression vector is long):
Step 1: environment and parameter are recovered in initialization:
If point is { A on the elastic threads
i, after taking place once to move be A '
i, (i=1,2 ... n), n is the number of putting on the elastic threads, n>=3; The coordinate of starting point and terminal point is respectively A in the space
1, A
n, the displacement of starting point and terminal point is always zero; The centre of sphere and the radius of barrier are respectively { O
j, { R
j(j=1,2 ... m), the number m of barrier>=0; Resistance coefficient k ∈ (0,1).Be used to judge whether adjacent threshold parameter ξ does for elastic threads and barrier
R
jBe the radius of j barrier, C
1>0 is constant; The iteration stopping threshold values that judges whether balance is that ζ does
C
2>0 is constant,
Distance for the pathfinding origin-to-destination;
Step 2: make A
1'=A
1, A
n'=A
nCurrent motor point A on the elastic threads
iSubscript i=2, elastic threads is from second some setting in motion;
Step 3: according to formula
Calculate A
iPoint receives the caused displacement of effect of elastic threads
Wherein k is a resistance coefficient, A
I-1' expression A
I-1Point after once moving, A
iAt A
I+1Graviational interaction under displacement do
A
iAt A
I-1Graviational interaction under displacement do
The subscript j=1 of order expression barrier calculates A
iPoint and barrier O
1Effect;
Step 4: judge A
iWhether some O in barrier
j, if
Set up, then A
iPoint calculates A in barrier
iPoint elasticity rope and barrier O
jResultant displacement under the acting in conjunction
For:
Wherein
Change step 8 then over to; If
Be false, then A
iPoint gets into step 5 not in barrier; Wherein,
Step 5: judge A
iWhether point is surperficial at barrier, if
Set up, then A
iPoint is on the barrier surface, and this moment is if having
Set up, then calculate A
iPoint is at elastic threads and barrier O
jResultant displacement under the acting in conjunction
Calculate A
iPoint is at elastic threads and barrier O
jResultant displacement under the acting in conjunction
Get into step 8 then; If
Be false, then directly get into step 7; If
Be false, then A
iPoint on the barrier surface, does not get into step 6;
Step 6: judge A
iPoint receives the caused displacement of effect of elastic threads
Whether can move to barrier O down
jIn, if
Set up, then A
iPoint can move to barrier O
jIn, then calculate A
iPoint is at elastic threads and barrier O
jResultant displacement under the acting in conjunction
Wherein
Calculate A
iPoint is at elastic threads and barrier O
jResultant displacement under the acting in conjunction
Get into step 8;
If
Be false, A then is described
iPoint can not move to barrier O
jIn, directly get into step 7;
Step 7: A
iPut the effect that only receives the effect of elastic threads and do not receive barrier, A
iPoint is at elastic threads and barrier O
jResultant displacement under the acting in conjunction
The subscript j of order expression barrier adds 1 then, whether judges j again greater than m, if greater than m, gets into step 8, otherwise returns step 4;
Step 8: calculate A
iPosition after point moves
Make current motor point A
iSubscript i add 1, and whether judge i, if i more than or equal to n, gets into step 9, otherwise returns step 3 more than or equal to n;
Step 9: judge whether elastic threads reaches the equilibrium position, if
Set up, say that then elastic threads reaches the equilibrium position, stop pathfinding, the residing position of elastic threads this moment be the path that will seek, otherwise do not reach the equilibrium position, make A
i=A
i' (i=1,2 ..., n), and return step 2.
The invention has the advantages that:
1, the present invention proposes a kind of three dimensions paths planning method based on the elastic threads kinetic property, has proposed a kind of new method of seeking the space optimal path, and this method has convergence, and elastic threads convergent extreme position is part or globally optimal solution;
2, the present invention proposes a kind of three dimensions paths planning method based on the elastic threads kinetic property, and little with respect to other paths planning method operand, pathfinding speed is fast;
3, the present invention proposes a kind of three dimensions paths planning method based on the elastic threads kinetic property, and environment representation is simple, points to the outer normal direction of barrier as long as know the barrier surface;
4, the present invention proposes a kind of three dimensions paths planning method based on the elastic threads kinetic property, need spatial be slit into the form of cell, the path flatness that is found, and accuracy is better.
Embodiment
To combine accompanying drawing that the present invention is elaborated below:
The barrier surface is smooth, and the contraction of elastic threads is unreasonable to be thought, so establish barrier with smooth curved surface parcel, described smooth being meant: establishing on the elastic threads is A with surperficial any that contacts of barrier arbitrarily; This some place normal to a surface direction is
(outer by pointing to barrier in the barrier); The A point receives the A point of making a concerted effort for
then except that this power barrier and receives the acting force of barrier and be:
Wherein:
Order
So
and
The force is always associated with surface normal
orthogonal.
Barrier is represented with mutual non-conterminous smooth sphere, described non-conterminous being interpreted as: the centre of sphere of establishing any two barriers is O
1, O
2, radius is R
1, R
2Then have
Wherein η is a constant.
According to displacement formula:
Wherein, s representes displacement; A representes acceleration magnitude; The t express time; F representes the external force size; M representes quality.As time t and quality m fixedly the time, displacement s and external force F are directly proportional, i.e. the big more displacement of external force is big more, so when calculating discretize, represent to put on the elastic threads stressed size with the displacement size.
Definition elastic threads: the fixing elastic threads of forming by the individual identical point of n (n>=3) of end points; Any two consecutive point; And have only adjacent point always to attract each other; Two consecutive point A of non-end points on the elastic threads; B; A then; B has under the effect of attractive interaction and caused by movement resistance: some displacement that A did is: the displacement that B did of
point for
wherein k ∈ (0,1) be resistance coefficient.
Can know that by elastic threads definition the big more displacement of doing of adjacent 2 distances is big more on the elastic threads, represent 2 stressed big more, this conforms to more greatly with the big more gravitation of elastic threads elongation.Point among the present invention all refers to the point in the three dimensions, and coordinate promptly all belongs to R
1 * 3
The displacement method (in the absence of barrier) of definition elastic threads: establish on the elastic threads point and be followed successively by { A
1, A
2..., A
n, through next moment point after once moving be followed successively by A '
1, A '
2..., A '
n(so because end points is fixed A '
1=A
1, A '
n=A
n), an optional end points is a starting point, like A
1, at first move and terminal A
1Adjacent some A
2,, calculate A according to the definition of elastic threads
2At A '
1Graviational interaction under displacement do
At A
3Graviational interaction under displacement do
And A
3The displacement of doing jointly under using of point is A
2The caused displacement of effect that receives elastic threads is:
A
2Next position constantly does
With identical method, move { A successively
3, A
4, A
5..., A
N-1, so obtain on the elastic threads removing the every bit of end points do through the position after once moving
Make A
i=A '
i(i=1,2 ..., n), repeat above-mentioned moving, carry out such moving repeatedly, till having a few all no longer motion on the elastic threads, promptly elastic threads reaches the equilibrium position.
Define the effect of barrier to elastic threads: the centre of sphere of establishing certain barrier is O
j, radius is R
j, A
iFor non-end points on the elastic threads certain a bit, taking place once, position, motion back is A '
i, i ∈ 2,3 ... and n-1}, when satisfying
The time, A
iPoint is outside barrier; When satisfying
The time, A
iPoint contacts with barrier, when satisfying
The time, A
iPoint in barrier, (R wherein
j>>ξ>0), A
iPoint receives the caused displacement of gravitation of elastic threads to do
Be the threshold parameter that is used to judge that elastic threads and barrier be whether adjacent,
The expression barrier centre of sphere is to A
iDistance.
(1) works as A
iPoint is outside barrier, and is as shown in Figure 1, and in displacement
After can get into barrier O
jIn, promptly satisfy
The time, but A
iPoint only moves to the barrier surface just to be stopped, and calculates the caused displacement of resistance of barrier
For (IN, B, OUT represent A respectively among Fig. 1
iPoint is in barrier inside, border, outside)
Wherein
(when
The time,
Wherein L is A
iThe point edge
Direction is to barrier O
jThe distance on surface);
(2) work as A
iPoint contacts with barrier, and
The component of (barrier surface normal direction) direction is not more than zero, promptly satisfies
The time, as shown in Figure 2, A receives the caused displacement of holding power of this ball
Be parallel to A
iThe tangent plane of the place's of selecting sphere) does
Wherein
(3) work as A
iPoint is in certain barrier, and is as shown in Figure 3, for A
iPoint is got rid of outside barrier, obstacle ball O
jTo A
iThe caused displacement of repulsion of point
For
Wherein
L ' expression barrier O
jTo A
iThe size of the caused displacement of repulsion of point, L ' can be set to constant, also can be set to
Or barrier radius R
jRelevant variable, L ' is set among the present invention
According to the mode of action of barrier to elastic threads, 1 A of non-end points on the elastic threads is established in the motion of elastic threads when barrier being arranged in can computer memory
iTaking place once, position, motion back is A '
i, wherein i ∈ 2,3 ... n-1}, and postulated point A
iWith barrier O
jHave an effect (promptly
Be not equal to zero), then elastic threads and barrier O
jResultant displacement under the acting in conjunction
For
O wherein
jBe the centre of sphere of j barrier, m representes the number of barrier, therefore can be calculated when barrier is arranged the position after the point of non-end points moves at every turn on the elastic threads by formula (11)
So just can carry out elastic threads and barrier O after iterative computation moves at every turn according to the iterative manner of the displacement method of elastic threads
jResultant displacement under the acting in conjunction.
The definition equilibrium position: by n the elastic threads formed of point, when the displacement size of this n point in each iteration all is zero, some stress balances arbitrarily on the elastic threads, then putting residing position on the elastic threads at this moment is n point equilibrium position, the abbreviation equilibrium position.Regulation satisfies in actual computation
The time, elastic threads reaches the equilibrium position, iteration stopping, and wherein ζ>0 is for judging whether the iteration stopping threshold values of balance, the maximal value of scalar in max () the expression bracket; A
iThe expression elastic threads on more arbitrarily, A
i' expression A
iPairing point takes place once to move.
Definition path: put the elastic threads form by n, have a few on it all outside barrier, and be evenly distributed; Be that adjacent 2 distances are equal arbitrarily on the elastic threads; Be referred to as even elastic threads, claim that then the broken line that the consecutive point line is formed on this elastic threads is n point path, be called for short the path; The length in path be the length of consecutive point line add with.
The present invention proposes a kind of three dimensions paths planning method based on the elastic threads kinetic property, and is as shown in Figure 4, specifically comprises following step:
Step 1: environment and parameter are recovered in initialization:
If point is { A on the elastic threads
i, after taking place once to move be A '
i, (i=1,2 ... n), n>=3; The coordinate of starting point and terminal point is respectively A in the space
1, A
n, the displacement of starting point and terminal point is always zero; The initial position of putting on the elastic threads can be arranged on the line of starting point and terminal point, also can be provided with at random; The thing of placing obstacles, the present invention representes barrier with non-conterminous spheroid, the centre of sphere and the radius of barrier are respectively { O
j, { R
j(j=1,2 ... m), the number m of barrier>=0; Resistance coefficient k ∈ (0,1) (according to Banach compression reflection principle, and the estimation of error formula of calculating fixed point, calculate and learn that the pathfinding process is a convergent when k ∈ (0,1), and work as
(k ∈ (0,1)) } time, the speed of convergence of elastic threads is the fastest, and wherein n is the number of putting on the elastic threads); Be used to judge whether adjacent threshold parameter ξ does for elastic threads and barrier
R in the formula
jBe the radius of j barrier, C
1>0 is constant, confirms the size of barrier adjacent area according to the size of barrier; The iteration stopping threshold values that judges whether balance is that ζ does
Wherein n is the number of putting on the elastic threads, C
2>0 is constant, C
2Bigger judgement for the equilibrium position is accurate more, is convenient to control, and iteration stopping threshold values ξ all is suitable under different length unit,
The expression barrier centre of sphere is to A
iDistance;
Step 2: make A
1'=A
1, A
n'=A
nCurrent motor point A on the elastic threads
iSubscript i=2, elastic threads is from second some setting in motion;
Step 3: according to formula
Calculate A
iPoint receives the caused displacement of effect of elastic threads
Wherein k is a resistance coefficient, A
I-1' expression A
I-1Point after once moving, A
iAt A
I+1Graviational interaction under displacement do
A
iAt A
I-1Graviational interaction under displacement do
The subscript j=1 of order expression barrier at first calculates A
iPoint and barrier O
1Effect;
Step 4: judge A
iWhether some O in barrier
j, if
Set up, then A
iPoint obtains formula (16) according to (11) formula in barrier
Wherein
The expression barrier centre of sphere is to A
iDistance,
Be the distance of pathfinding origin-to-destination, calculate A
iPoint is at elastic threads and barrier O
jResultant displacement under the acting in conjunction
Change step 8 over to; If
Be false, then A
iPoint gets into step 5 not in barrier;
Step 5: judge A
iWhether point is surperficial at barrier, if
Set up, then A
iPoint is on the barrier surface, and this moment is if having
Set up, some A are described
iAt current resultant displacement
Following meeting and barrier surface action are for making the some A of this moment
iSlide on the barrier surface, calculate A according to (11) formula
iPoint is at elastic threads and barrier O
jResultant displacement under the acting in conjunction
Calculate A
iPoint is at elastic threads and barrier O
jResultant displacement under the acting in conjunction
Get into step 8 then; If
Be false, then do not calculate A
iPoint is at elastic threads and barrier O
jResultant displacement under the acting in conjunction
Directly get into step 7; If
Be false, then A
iPoint on the barrier surface, does not get into step 6;
Step 6: judge A
iPoint is at current resultant displacement
Whether can move to barrier O down
jIn, if
Set up, A then is described
iPoint can move to barrier O
jIn, obtain according to (11) formula:
Wherein
Calculate A
iPoint is at elastic threads and barrier O
jResultant displacement under the acting in conjunction
Get into step 8;
If
Be false, A then is described
iPoint can not move to barrier O
jIn, do not calculate, directly get into step 7;
Step 7: A
iPut the effect that only receives the effect of elastic threads and do not receive barrier, A
iPoint is at elastic threads and barrier O
jResultant displacement under the acting in conjunction
The subscript j of order expression barrier adds 1 then, whether judges j greater than m, if explain that all barriers are to A
iThe effect of point has all been considered, gets into step 8, otherwise returns step 4;
Step 8: calculate A
iPosition after point moves
Make current motor point A
iSubscript i add 1, whether judge i more than or equal to n, if institute have a few and has all taken place once to move on the explanation elastic threads, entering step 9, otherwise return step 3;
Step 9: judge whether elastic threads reaches the equilibrium position, if
Set up, say that then elastic threads reaches the equilibrium position, stop pathfinding (the residing position of elastic threads this moment reaches the path for we inquired for), otherwise explanation does not reach the equilibrium position, makes A
i=A
i' (i=1,2 ..., n), return step 2.
Embodiment:
If the number n of putting on the elastic threads=52; The coordinate of starting point and terminal point is respectively A in the space
1=(000), A
n=(500 500 500); Unit is 1; The initial position of putting on the elastic threads is arranged on the line of starting point and terminal point; The number m=60 of obstacle ball carries out iteration according to the step that the three dimensions paths planning method based on the elastic threads kinetic property of the present invention's proposition is realized, can test convergence effect such as Fig. 5, and spheroid is represented barrier among Fig. 5, P
1Expression path (being the final position of elastic threads), this moment, elastic threads length did
Make the pathfinding simultaneously of five elastic threads, their initial position is provided with at random, and other condition is the same, and the pathfinding result is as shown in Figure 6, five length L that elastic threads is final
1, L
2, L
3, L
4, L
5Be respectively
{L
1,L
2,L
3,L
4,L
5}={879.9699?879.9588?878.3692?879.8403?878.3485}
Path P among Fig. 6
3By elastic threads L
3And L
5Overlapping forming, path P
2By elastic threads L
1, L
2, L
4Overlapping forming can be found out, two equilibrium positions have been found in pathfinding this time, and their length differs about 1.5, and the equilibrium position of this explanation elastic threads is not unique.L wherein
3And L
5L among institute convergent equilibrium position and Fig. 5
0Institute convergent equilibrium position is identical.When a large amount of experiments show that general selection initial position is on the line of starting point and terminal point, finally restrain the path of finding, all shorter with respect to other path.
In the experiment in the mode of motion of elastic threads and kinetic property and the reality mode of motion of elastic threads and kinetic property similar; The motion of elastic threads is convergent always, and the equilibrium position that elastic threads converges under the identical environment is not unique, and method promptly of the present invention might be absorbed in locally optimal solution; A large amount of experiments show when the obstacle ball non-conterminous, when having a few on the elastic threads all outside barrier, in iterative process, put on the elastic threads and can not get in the barrier, and this conforms to the movement effects of elastic threads in the reality; Because this method has been given up and algorithm has once been arranged with the three-dimensional mode of grid representation; The flatness in path and accuracy are decided by the number of putting on the elastic threads; Rather than by representing that three-dimensional grid number decides, this algorithm greatly reduces the computing cost in the process that improves path accuracy like this.