CN102306399A - Three-dimensional space path planning method based on movement property of elastic rope - Google Patents

Abstract

The invention provides a three-dimensional space path planning method based on the movement property of an elastic rope, which comprises the steps of: initializing a retrieving environment and a parameter, computing the displacement of a point on the elastic rope due to the action of the elastic rope; computing the displacement of the point on the elastic rope due to the action of the elastic rope and a barrier; computing a position after movement; judging whether all points on the elastic rope move once, judging whether the elastic rope reaches a balance position and the like. The three-dimensional space path planning method based on the movement property of the elastic rope, provided by the invention, has the advantages of small operation quantity, high routing speed and simple environment expression relative to other path planning methods; only a normal direction that the surface of the barrier points to the outer part of the barrier is known; a space is not divided into the form of cells; and a found path has better smoothness and accuracy.

Description

A kind of three dimensions paths planning method based on the elastic threads kinetic property

Technical field

The invention belongs to the Computational intelligence technology field, moving under the obstacle environment through the virtual space elastic threads proposes a kind of three-dimensional sky footpath planing method of asking the way.

Background technology

Along with the development of air-sea cause, three-dimensional path planning more and more receives people's attention, and it plays crucial effects in effective utilization of resource and time.But mostly existing paths planning method is to propose to two-dimensional space; Existing three-dimensional sky is asked the way directly, and mostly planing method is the popularization of two-dimension method to three-dimensional method; Because three-dimensional complexity improves; In three-dimensional popularization process, must bring problems by two dimension, as adopted at present to three dimensions paths planning method Artificial Potential Field method, A* search procedure, based on the rationalistic method of case and genetic algorithm etc., wherein potential field method inevitably can be sunk into local minimum; And when adopting complicated optimization criterion, potential field method can not directly be promoted; The A* algorithm can be used in the higher-dimension problem, but along with the increase of dimension, the space and time requirement of A* algorithm will be difficult to be met; Method based on reasoning by cases is adjusted the path according to the barrier of part, can not obtain the path of global optimum sometimes; Genetic algorithms use mode at random produces initial path, utilizes the genetic operator operation constantly the path to be improved.When environmental baseline was fairly simple, algorithm can be accomplished planning, and when circumstance complication, algorithm will be difficult to the feasible path that finds one to satisfy constraint condition.These problems are the constructional of algorithm, want good solution, need the proposition of new theory and new method.

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 ₁, 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 ₁＞0 is constant; The iteration stopping threshold values that judges whether balance is that ζ does

C ₂＞0 is constant, Distance for the pathfinding origin-to-destination;

Step 2: make A ₁'=A ₁, 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 ₁Effect;

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:

$\stackrel{\→}{A_i{A}_i^{\′}}=\stackrel{\→}{S_{A_i}}+\stackrel{\→}{S_{O_j{A}_i}^{\′}}+\stackrel{\→}{S_{O_j{A}_i}^{\′\′}}+\stackrel{\→}{S_{O_j{A}_i}^{\′\′\′}}$

$=\stackrel{\→}{S_{A_i}}+\stackrel{\→}{S_{O_j{A}_i}^{\′\′\′}}$

$=\stackrel{\→}{S_{A_i}}+\frac{\stackrel{\→}{O_j{A}_i}}{\left|\stackrel{\→}{O_j{A}_i}\right|}*L^{\′}$

Wherein Change step 8 then over to; If

Be false, then A _iPoint gets into step 5 not in barrier; Wherein, $\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;\&prime;}}=\frac{\stackrel{\&RightArrow;}{O_j{A}_i}}{\left|\stackrel{\&RightArrow;}{O_j{A}_i}\right|}*L^{\&prime;}*g\left(\right|\stackrel{\&RightArrow;}{O_j{A}_i}|-R_j),$ $g\left(x\right)=\left\{\begin{array}{cc}0& x\&GreaterEqual;0\\ 1& x<0\end{array}\right.;$

$\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;}}=-\frac{\stackrel{\&RightArrow;}{O_j{A}_i}}{{\left|\stackrel{\&RightArrow;}{O_j{A}_i}\right|}^2}*(\stackrel{\&RightArrow;}{S_{A_i}}\&CenterDot;\stackrel{\&RightArrow;}{O_j{A}_i})*f(\stackrel{\&RightArrow;}{S_{A_i}}\&CenterDot;\stackrel{\&RightArrow;}{O_j{A}_i})*f(R_j-|\stackrel{\&RightArrow;}{O_j{A}_i}\left|\right)*f\left(\right|\stackrel{\&RightArrow;}{O_j{A}_i}|-R_j-\mathrm{\&xi;}),f\left(x\right)=\left\{\begin{array}{cc}0& x>0\\ 1& x\&le;0\end{array}\right.;$ $\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;}}=\frac{\stackrel{\&RightArrow;}{S_{A_i}}}{\left|\stackrel{\&RightArrow;}{S_{A_i}}\right|}*(L-|\stackrel{\&RightArrow;}{S_{A_i}}|-\mathrm{\&xi;})*g(R_j+\mathrm{\&xi;}-|\stackrel{\&RightArrow;}{O_j{A}_i}\left|\right)*g\left(\right|\stackrel{\&RightArrow;}{O_j{A}_i}+\stackrel{\&RightArrow;}{S_{A_i}}|-R_j),g\left(x\right)=\left\{\begin{array}{cc}0& x\&GreaterEqual;0\\ 1& x<0\end{array}\right.;$

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

$\stackrel{\&RightArrow;}{A_i{A}_i^{\&prime;}}=\stackrel{\&RightArrow;}{S_{A_i}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;\&prime;}}$

Calculate A _iPoint is at elastic threads and barrier O _jResultant displacement under the acting in conjunction

$=\stackrel{\&RightArrow;}{S_{A_i}}-\frac{\stackrel{\&RightArrow;}{O_j{A}_i}}{|\stackrel{\&RightArrow;}{O_j{A}_i}{|}^2}*(\stackrel{\&RightArrow;}{S_{A_i}}\&CenterDot;\stackrel{\&RightArrow;}{O_j{A}_i})$

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

$\stackrel{\&RightArrow;}{A_i{A}_i^{\&prime;}}=\stackrel{\&RightArrow;}{S_{A_i}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;\&prime;}}$

$=\stackrel{\&RightArrow;}{S_{A_i}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;}}$

$=\stackrel{\&RightArrow;}{S_{A_i}}+\frac{\stackrel{\&RightArrow;}{S_{A_i}}}{\left|\stackrel{\&RightArrow;}{S_{A_i}}\right|}*(L-\stackrel{\&RightArrow;}{S_{A_i}}-\mathrm{\&xi;})$

$=\frac{\stackrel{\&RightArrow;}{S_{A_i}}}{\left|\stackrel{\&RightArrow;}{S_{A_i}}\right|}*(L-\mathrm{\&xi;})$

Wherein $L=-\frac{\stackrel{\&RightArrow;}{S_{A_i}}\&CenterDot;\stackrel{\&RightArrow;}{O_j{A}_i}}{\left|\stackrel{\&RightArrow;}{S_{A_i}}\right|}-\sqrt{{\left(\frac{\stackrel{\&RightArrow;}{S_{A_i}}\&CenterDot;\stackrel{\&RightArrow;}{O_j{A}_i}}{\left|\stackrel{\&RightArrow;}{S_{A_i}}\right|}\right)}^2-{\left|\stackrel{\&RightArrow;}{O_j{A}_i}\right|}^2+R_j^2},$ 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.

Description of drawings

Fig. 1: when point is outside barrier on the elastic threads, receive the displacement profile figure of obstacle effect;

Fig. 2: point receives the displacement profile figure of obstacle effect on the elastic threads when obstacles borders;

Fig. 3: when point is in barrier on the elastic threads, receive the displacement profile figure of obstacle effect;

Fig. 4: the process flow diagram of the present invention proposes a kind of three dimensions paths planning method based on the elastic threads kinetic property;

Fig. 5: the convergence result that single elastic threads uses that the present invention proposes based on the three dimensions paths planning method of elastic threads kinetic property;

Fig. 6: the convergence result that five elastic threads use that the present invention proposes based on the three dimensions paths planning method of elastic threads kinetic property.

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:

${\stackrel{\&RightArrow;}{F}}_a=-\frac{\stackrel{\&RightArrow;}{a}}{{\left|\stackrel{\&RightArrow;}{a}\right|}^2}*(\stackrel{\&RightArrow;}{F}\&CenterDot;\stackrel{\&RightArrow;}{a})*f(\stackrel{\&RightArrow;}{F}\&CenterDot;\stackrel{\&RightArrow;}{a})---\left(1\right)$

Wherein:

$f\left(x\right)=\left\{\begin{array}{cc}0& x>0\\ 1& x\&le;0\end{array}\right.---\left(2\right)$

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 ₁, O ₂, radius is R ₁, R ₂Then have

$\left|\stackrel{\&RightArrow;}{O_1{O}_2}\right|-(R_1+R_2)>0---\left(3\right)$

Wherein η is a constant.

According to displacement formula:

$s=\frac{1}{2}a{t}^2=\frac{1}{2}\frac{F}{m}{t}^2=F\frac{t^2}{2m}---\left(4\right)$

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 ₁, A ₂..., A _n, through next moment point after once moving be followed successively by A ' ₁, A ' ₂..., A ' _n(so because end points is fixed A ' ₁=A ₁, A ' _n=A _n), an optional end points is a starting point, like A ₁, at first move and terminal A ₁Adjacent some A ₂,, calculate A according to the definition of elastic threads ₂At A ' ₁Graviational interaction under displacement do

At A ₃Graviational interaction under displacement do

And A ₃The displacement of doing jointly under using of point is A ₂The caused displacement of effect that receives elastic threads is:

$\stackrel{\&RightArrow;}{S_{A_2}}=k*\stackrel{\&RightArrow;}{A_2{A}_1^{\&prime;}}+k*\stackrel{\&RightArrow;}{A_2{A}_3}---\left(5\right)$

A ₂Next position constantly does

${A_2}^{\&prime;}=A_2+\stackrel{\&RightArrow;}{S_{A_2}}---\left(6\right)$

With identical method, move { A successively ₃, A ₄, A ₅..., A _N-1, so obtain on the elastic threads removing the every bit of end points do through the position after once moving

${A_i}^{\&prime;}=A_i+\stackrel{\&RightArrow;}{S_{A_i}}(i=\mathrm{2,3}...,n-1)---\left(7\right)$

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)

$\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;}}=\frac{\stackrel{\&RightArrow;}{S_{A_i}}}{\left|\stackrel{\&RightArrow;}{S_{A_i}}\right|}*(L-|\stackrel{\&RightArrow;}{S_{A_i}}|-\mathrm{\&xi;})*g(R_j+\mathrm{\&xi;}-|\stackrel{\&RightArrow;}{O_j{A}_i}\left|\right)*g\left(\right|\stackrel{\&RightArrow;}{O_j{A}_i}+\stackrel{\&RightArrow;}{S_{A_i}}|-R_j)---\left(8\right)$

Wherein $g\left(x\right)=\left\{\begin{array}{cc}0& x\&GreaterEqual;0\\ 1& x<0\end{array}\right.;$

$L=-\frac{\stackrel{\&RightArrow;}{S_{A_i}}\&CenterDot;\stackrel{\&RightArrow;}{O_j{A}_i}}{\left|\stackrel{\&RightArrow;}{S_{A_i}}\right|}-\sqrt{{\left(\frac{\stackrel{\&RightArrow;}{S_{A_i}}\&CenterDot;\stackrel{\&RightArrow;}{O_j{A}_i}}{\left|\stackrel{\&RightArrow;}{S_{A_i}}\right|}\right)}^2-{\left|\stackrel{\&RightArrow;}{O_j{A}_i}\right|}^2+R_j^2}$ (when $\left|\stackrel{\&RightArrow;}{O_j{A}_i}\right|>R_j+\mathrm{\&xi;}\&|\stackrel{\&RightArrow;}{O_j{A}_i}+\stackrel{\&RightArrow;}{S_{A_i}}|<R_j$ 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

$\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;}}=-\frac{\stackrel{\&RightArrow;}{O_j{A}_i}}{{\left|\stackrel{\&RightArrow;}{O_j{A}_i}\right|}^2}*(\stackrel{\&RightArrow;}{S_{A_i}}\&CenterDot;\stackrel{\&RightArrow;}{O_j{A}_i})*f(\stackrel{\&RightArrow;}{S_{A_i}}\&CenterDot;\stackrel{\&RightArrow;}{O_j{A}_i})*f(R_j-|\stackrel{\&RightArrow;}{O_j{A}_i}\left|\right)*f\left(\right|\stackrel{\&RightArrow;}{O_j{A}_i}|-R_j-\mathrm{\&xi;})---\left(9\right)$

Wherein $f\left(x\right)=\left\{\begin{array}{cc}0& x>0\\ 1& x\&le;0\end{array}\right.;$

(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

$\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;\&prime;}}=\frac{\stackrel{\&RightArrow;}{O_j{A}_i}}{\left|\stackrel{\&RightArrow;}{O_j{A}_i}\right|}*L^{\&prime;}*g\left(\right|\stackrel{\&RightArrow;}{O_j{A}_i}|-R_j)(L^{\&prime;}>0)---\left(10\right)$

Wherein $g\left(x\right)=\left\{\begin{array}{cc}0& x\&GreaterEqual;0\\ 1& x<0\end{array}\right.;$ 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

$\stackrel{\&RightArrow;}{A_i{A}_i^{\&prime;}}=\stackrel{\&RightArrow;}{S_{A_i}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;\&prime;}}j\&Element;\{\mathrm{1,2},...,m\}---\left(11\right)$

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

$\underset{1\&le;i\&le;n}{\max }\left[\max \left(\stackrel{\&RightArrow;}{A_i{A}_i^{\&prime;}}\right)\right]\&le;\mathrm{\&zeta;}---\left(12\right)$

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 ₁, 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

$\mathrm{\&xi;}=\frac{R_j}{C_1}---\left(13\right)$

R in the formula _jBe the radius of j barrier, C ₁＞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

$\mathrm{\&zeta;}=\frac{\left|\stackrel{\&RightArrow;}{A_1{A}_n}\right|}{C_2*n}---\left(14\right)$

Wherein n is the number of putting on the elastic threads, C ₂＞0 is constant, C ₂Bigger 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 ₁'=A ₁, 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

$\stackrel{\&RightArrow;}{S_{A_i}}=k*\stackrel{\&RightArrow;}{A_i{A_{i-1}}^{\&prime;}}+k*\stackrel{\&RightArrow;}{A_i{A}_{i+1}}---\left(15\right)$

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 ₁Effect;

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

$\stackrel{\&RightArrow;}{A_i{A}_i^{\&prime;}}=\stackrel{\&RightArrow;}{S_{A_i}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;\&prime;}}$

$=\stackrel{\&RightArrow;}{S_{A_i}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;\&prime;}}---\left(16\right)$

$=\stackrel{\&RightArrow;}{S_{A_i}}+\frac{\stackrel{\&RightArrow;}{O_j{A}_i}}{\left|\stackrel{\&RightArrow;}{O_j{A}_i}\right|}*L^{\&prime;}$

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

$\stackrel{\&RightArrow;}{A_i{A}_i^{\&prime;}}=\stackrel{\&RightArrow;}{S_{A_i}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;\&prime;}}$

$=\stackrel{\&RightArrow;}{S_{A_i}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;}}---\left(17\right)$

$=\stackrel{\&RightArrow;}{S_{A_i}}-\frac{\stackrel{\&RightArrow;}{O_j{A}_i}}{{\left|\stackrel{\&RightArrow;}{O_j{A}_i}\right|}^2}*(\stackrel{\&RightArrow;}{S_{A_i}}\&CenterDot;\stackrel{\&RightArrow;}{O_j{A}_i})$

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:

$\stackrel{\&RightArrow;}{A_i{A}_i^{\&prime;}}=\stackrel{\&RightArrow;}{S_{A_i}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;\&prime;}}$

$=\stackrel{\&RightArrow;}{S_{A_i}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;}}$

$=\stackrel{\&RightArrow;}{S_{A_i}}+\frac{\stackrel{\&RightArrow;}{S_{A_i}}}{\left|\stackrel{\&RightArrow;}{S_{A_i}}\right|}*(L-\stackrel{\&RightArrow;}{S_{A_i}}-\mathrm{\&xi;})---\left(18\right)$

$=\frac{\stackrel{\&RightArrow;}{S_{A_i}}}{\left|\stackrel{\&RightArrow;}{S_{A_i}}\right|}*(L-\mathrm{\&xi;})$

Wherein $L=-\frac{\stackrel{\&RightArrow;}{S_{A_i}}\&CenterDot;\stackrel{\&RightArrow;}{O_j{A}_i}}{\left|\stackrel{\&RightArrow;}{S_{A_i}}\right|}-\sqrt{{\left(\frac{\stackrel{\&RightArrow;}{S_{A_i}}\&CenterDot;\stackrel{\&RightArrow;}{O_j{A}_i}}{\left|\stackrel{\&RightArrow;}{S_{A_i}}\right|}\right)}^2-{\left|\stackrel{\&RightArrow;}{O_j{A}_i}\right|}^2+R_j^2},$ 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 ₁=(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 ₁Expression path (being the final position of elastic threads), this moment, elastic threads length did $L_0=\underset{i=1}{\overset{n-1}{\mathrm{\&Sigma;}}}\left|\stackrel{\&RightArrow;}{A_i{A}_{i+1}}\right|=878.3917$

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 ₁, L ₂, L ₃, L ₄, L ₅Be respectively

{L ₁，L ₂，L ₃，L ₄，L ₅}＝{879.9699?879.9588?878.3692?879.8403?878.3485}

Path P among Fig. 6 ₃By elastic threads L ₃And L ₅Overlapping forming, path P ₂By elastic threads L ₁, L ₂, L ₄Overlapping 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 ₃And L ₅L among institute convergent equilibrium position and Fig. 5 ₀Institute 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.

Claims (4)

1. three dimensions paths planning method based on the elastic threads kinetic property is characterized in that: specifically comprise following step:

Step 1: establish on the elastic threads point and be { A _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 ₁, 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, k is a resistance coefficient; Judge whether adjacent threshold parameter is ξ for elastic threads and barrier, and the iteration stopping threshold values that judges whether balance is ζ;

Step 2: make A ₁'=A ₁, 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 ₁Effect;

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:

$\stackrel{\&RightArrow;}{A_i{A}_i^{\&prime;}}=\stackrel{\&RightArrow;}{S_{A_i}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;\&prime;}}$

$=\stackrel{\&RightArrow;}{S_{A_i}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;\&prime;}}$

$=\stackrel{\&RightArrow;}{S_{A_i}}+\frac{\stackrel{\&RightArrow;}{O_j{A}_i}}{\left|\stackrel{\&RightArrow;}{O_j{A}_i}\right|}*L^{\&prime;}$

Wherein

Change step 8 then over to,

The expression barrier centre of sphere is to A _iDistance,

Distance for the pathfinding origin-to-destination; If

Be false, then A _iPoint gets into step 5 not in barrier; Wherein,

$\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;\&prime;}}=\frac{\stackrel{\&RightArrow;}{O_j{A}_i}}{\left|\stackrel{\&RightArrow;}{O_j{A}_i}\right|}*L^{\&prime;}*g\left(\right|\stackrel{\&RightArrow;}{O_j{A}_i}|-R_j),g\left(x\right)=\left\{\begin{array}{cc}0& x\&GreaterEqual;0\\ 1& x<0\end{array}\right.;$

$\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;}}=-\frac{\stackrel{\&RightArrow;}{O_j{A}_i}}{{\left|\stackrel{\&RightArrow;}{O_j{A}_i}\right|}^2}*(\stackrel{\&RightArrow;}{S_{A_i}}\&CenterDot;\stackrel{\&RightArrow;}{O_j{A}_i})*f(\stackrel{\&RightArrow;}{S_{A_i}}\&CenterDot;\stackrel{\&RightArrow;}{O_j{A}_i})*f(R_j-|\stackrel{\&RightArrow;}{O_j{A}_i}\left|\right)*f\left(\right|\stackrel{\&RightArrow;}{O_j{A}_i}|-R_j-\mathrm{\&xi;}),f\left(x\right)=\left\{\begin{array}{cc}0& x>0\\ 1& x\&le;0\end{array}\right.;$

$\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;}}=\frac{\stackrel{\&RightArrow;}{S_{A_i}}}{\left|\stackrel{\&RightArrow;}{S_{A_i}}\right|}*(L-|\stackrel{\&RightArrow;}{S_{A_i}}|-\mathrm{\&xi;})*g(R_j+\mathrm{\&xi;}-|\stackrel{\&RightArrow;}{O_j{A}_i}\left|\right)*g\left(\right|\stackrel{\&RightArrow;}{O_j{A}_i}+\stackrel{\&RightArrow;}{S_{A_i}}|-R_j),g\left(x\right)=\left\{\begin{array}{cc}0& x\&GreaterEqual;0\\ 1& x<0\end{array}\right.;$

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 Wherein ξ is the threshold parameter that is used to judge that elastic threads and barrier be whether adjacent;

$\stackrel{\&RightArrow;}{A_i{A}_i^{\&prime;}}=\stackrel{\&RightArrow;}{S_{A_i}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;\&prime;}}$

Calculate A _iPoint is at elastic threads and barrier O _jResultant displacement under the acting in conjunction

$=\stackrel{\&RightArrow;}{S_{A_i}}-\frac{\stackrel{\&RightArrow;}{O_j{A}_i}}{{\left|\stackrel{\&RightArrow;}{O_j{A}_i}\right|}^2}*(\stackrel{\&RightArrow;}{S_{A_i}}\&CenterDot;\stackrel{\&RightArrow;}{O_j{A}_i})$

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

$\stackrel{\&RightArrow;}{A_i{A}_i^{\&prime;}}=\stackrel{\&RightArrow;}{S_{A_i}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;\&prime;\&prime;}}$

$=\stackrel{\&RightArrow;}{S_{A_i}}+\stackrel{\&RightArrow;}{S_{O_j{A}_i}^{\&prime;}}$

$=\stackrel{\&RightArrow;}{S_{A_i}}+\frac{\stackrel{\&RightArrow;}{S_{A_i}}}{\left|\stackrel{\&RightArrow;}{S_{A_i}}\right|}*(L-\stackrel{\&RightArrow;}{S_{A_i}}-\mathrm{\&xi;})$

$=\frac{\stackrel{\&RightArrow;}{S_{A_i}}}{\left|\stackrel{\&RightArrow;}{S_{A_i}}\right|}*(L-\mathrm{\&xi;})$

Wherein $L=-\frac{\stackrel{\&RightArrow;}{S_{A_i}}\&CenterDot;\stackrel{\&RightArrow;}{O_j{A}_i}}{\left|\stackrel{\&RightArrow;}{S_{A_i}}\right|}-\sqrt{{\left(\frac{\stackrel{\&RightArrow;}{S_{A_i}}\&CenterDot;\stackrel{\&RightArrow;}{O_j{A}_i}}{\left|\stackrel{\&RightArrow;}{S_{A_i}}\right|}\right)}^2-{\left|\stackrel{\&RightArrow;}{O_j{A}_i}\right|}^2+R_j^2},$ 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 is the path that institute will seek, and wherein ζ is the iteration stopping threshold values of judgement balance, otherwise does not reach the equilibrium position, makes A _i=A _i' (i=1,2 ..., n), and return step 2.

2. according to claim 1 three dimensions paths planning method based on the elastic threads kinetic property, it is characterized in that: the value of described resistance coefficient k is k ∈ (0,1).

3. according to claim 1 three dimensions paths planning method based on the elastic threads kinetic property, it is characterized in that: describedly be used to judge whether adjacent threshold parameter ξ does for elastic threads and barrier R _jBe the radius of j barrier, C ₁＞0 is constant.

4. according to claim 1 three dimensions paths planning method based on the elastic threads kinetic property, it is characterized in that: the described iteration stopping threshold values ζ of balance that judges whether does

C ₂＞0 is constant, and n is the number of putting on the elastic threads,

Distance for the pathfinding origin-to-destination.

Images