CN104406588A - Air route planning method based on guide speed field in threat environment - Google Patents

Abstract

The invention discloses an air route planning method based on guide speed field in threat environment, and belongs to the field of air route planning. Subsistent threat affecting is fully considered, the threat is equivalent to a circular obstacle in a two-dimensional plane through the calculation, then traction field and avoidance field environment is established, the guide speed field is specially designed, controllability of the algorithm is improved, and the fast advantage of artificial potential field method is maintained. According to the method, planning speed can be directly obtained by calculation, the problem of incapability of reaching or entry barriers of the artificial potential field method can be overcome, and a smooth two-dimensional obstacle avoidance air route can be planned in real time. The air route planning method has the advantages of simple terrain modeling and small amount of calculation, and is easy to realize.

Description

Based on the Route planner of guiding velocity field under a kind of threatening environment

Technical field

The invention belongs to routeing field, specifically refer to the Route planner based on guiding velocity field under a kind of threatening environment.

Background technology

Intelligent body, as study hotspot in recent years, more and more causes the extensive concern of every country in the world.The most significantly mark of intelligent body is exactly possess independence, and independence research field important branch is routeing.What routeing relied on for a long time is that ground staff has made by hand, and this method not only takes time and effort, and often also can not get best air route.No matter on ground, under water or sky, due to the variation of intelligent body figure, its environment of executing the task also becomes increasingly complex, and the reinforcement of such as ground air defense firepower and the enhancing of radar system captured target ability, the reaction time leaving intelligent body for also shortens thereupon.

Current Route planner has a lot, the feature that different algorithms has them different.Such as A* algorithm has stronger convergence and rapidity, but it must rely on and stress and strain model, and the density of stress and strain model affects the precision of algorithm, and air route is level and smooth not; And have well of overall importance with the genetic algorithm of random character and ant group algorithm, often also can find optimum solution, but its speed of convergence is slow, is unfavorable for engineer applied; Artificial Potential Field Method algorithm is simple, and computing velocity is fast and be easy to realize, and have good real-time, but it can not process the impact of threat, and only have repulsion and Domain of Attraction, can cause the failure of routeing when potential field balances.

Performing along with intelligent body of task becomes increasingly complex, and hazardous location gets more and more, and traditional manual manufacture air route more and more can not adapt to the demand for development of intelligent body contexture by self.The trend of current routeing is with rapidity and optimality for target, and to practical implementation development, therefore design is applicable to the real-time planing method under threatening environment, will expand out more practical, more valuable broad space for intelligent body routeing.

For this reason, the present invention with routeing problem for background, take into full account that in esse threat affects, establish traction field and evade an environment, devise guiding velocity field especially, improve the controllability of algorithm, and maintain the rapidity advantage of Artificial Potential Field Method, for the real-time routeing under threatening environment proposes a kind of brand-new resolving ideas and method, finally for the independence research of intelligent body provides stronger technical support.

Summary of the invention

For problems of the prior art, the object of the invention is to propose the Route planner based on guiding velocity field under a kind of threatening environment, can exist in the environment threatened by this method, generate that two dimension is smooth keeps away barrier air route fast.

To achieve these goals, the technical solution adopted in the present invention is the Route planner based on guiding velocity field under a kind of threatening environment; First it carry out pre-service for threat and threat information be converted into static-obstacle, calculates haulage speed afterwards, evades speed and guiding speed, utilize superposition principle to sue for peace to speed, finally according to simulation step length determination flight route; Concrete steps are as follows:

Threat Equivalent Calculation is the circular static obstacle of certain height by step one: carry out pre-service to threat information;

If intelligent body carries out two-dimensional movement in a certain plane, intelligent body in this plane with threaten centre distance be r; Maximum effect radius R on α direction, each sight line angle of depression and the pass of α are R=f (α), and namely it is spatially the envelope surface threatening maximum effect space; If the envelope height being r place with the horizontal range of missile emplacements is Δ h, the geometric relationship between intelligent body and threat is:

$\left\{\begin{array}{c}\mathrm{\Δh}=R\sin \mathrm{\α}=f\left(\mathrm{\α}\right)\sin \mathrm{\α}\\ r=R\cos \mathrm{\α}=f\left(\mathrm{\α}\right)\cos \mathrm{\α}\end{array}\right.$

Probability P is shot down in introducing _kfor correction factor, revise the height of envelope surface at r place, revised curved surface is as equivalent terrain surface;

$\left\{\begin{array}{c}{\mathrm{\Δh}}_{\mathrm{td}}={\mathrm{KP}}_k\mathrm{\Δh}={\mathrm{KP}}_{k}f\left(\mathrm{\α}\right)\sin \mathrm{\α}={\mathrm{KP}}_{k/\mathrm{clos}}{K}_{0}f\left(\mathrm{\α}\right){\sin }^2\mathrm{\α}\\ r=f\left(\mathrm{\α}\right)\cos \mathrm{\α}\end{array}\right.$

In formula, Δ h _tdfor threatening equivalent height, K is correction factor, P _kfor the fallen probability of aircraft, P _k/closfor being shot down probability under the clear condition of sight line; Wherein use formula:

$P_{\mathrm{clos}}=K_0\frac{{\mathrm{\&Delta;h}}_{\mathrm{AS}}}{\mathrm{Rs}}=K_0\sin \mathrm{\&alpha;},0^0<\mathrm{\&alpha;}<{90}^0$

P _closfor the clear probability of sight line, Δ h _aS, Rs is respectively the height of aircraft on missile emplacements and the oblique distance between them, K ₀for scale-up factor; In α=90 ⁰time (i.e. r=0), shoot down probability P _kmaximum, for safety make this some correction anterior and posterior height equal, namely have

KP _k/closK ₀f(90 ⁰)sin ²90 ⁰＝f(90 ⁰)sin90 ⁰

Comprehensive above variously to obtain:

$K=\frac{1}{P_{k/\mathrm{clos}}{K}_0}$

The terrain surface parametric equation that substitution can obtain threat equivalence is:

$\left\{\begin{array}{c}{\mathrm{\&Delta;h}}_{\mathrm{td}}=f\left(\mathrm{\&alpha;}\right){\sin }^2\mathrm{\&alpha;}\\ r=f\left(\mathrm{\&alpha;}\right)\cos \mathrm{\&alpha;}\end{array}\right.$

If the coordinate of missile emplacements is (x ₀, y ₀), then have bring above formula into and can obtain equivalent landform surface equation

${\mathrm{\&Delta;h}}_{\mathrm{td}}=\left\{\begin{array}{c}T(x,y),0<r<R_{\max }\\ 0,r>R_{\max }\end{array}\right.$

In formula, R _maxfor the maximal value of f (α); Suppose that the radius of action of guided missile in all directions is R ₀, now the envelope surface of action space is hemisphere, i.e. f (α)=R ₀, substitute into above formula cancellation α can obtain equivalent landform surface equation

${\mathrm{\&Delta;h}}_{\mathrm{td}}=\left\{\begin{array}{c}\frac{R_0^2-{\left({x-x}_0\right)}^2-{(y-y_0)}^2}{R_0},0<r<R_0\\ 0,r>R_0\end{array}\right.$

After pre-service is carried out in threat, equivalent landform is a rotary paraboloid, and its shape approximation is in a mountain; When threatening the plane of movement distance with intelligent body to be Δ h _tdtime, then the radius after threatening equivalence is the center of circle is (x ₀, y ₀), then in any two dimensional surface, threaten and be equivalent to static circular obstacle;

Step 2: calculate target to the haulage speed of intelligent body;

The routeing task of intelligent body shows as and arrives impact point G (x in vector field _t, y _t), target is traction field field source, and traction field action produces haulage speed V on planning point _t; The traction field action region that impact point provides is the overall situation, and size is definite value, and direction is by planning a sensing impact point:

${\stackrel{\&RightArrow;}{V}}_T=w\&CenterDot;{\stackrel{\&RightArrow;}{r}}_t$

Wherein represent haulage speed, w represents traction field weight, represent the long unit direction vector of target; In two-dimensional space, expression formula as follows:

$\left\{\begin{array}{c}{r}_{\mathrm{tx}}=\frac{x_t-x_c}{\sqrt{{(x_t-x_c)}^2+{(y_t-y_c)}^2}}\\ r_{\mathrm{ty}}=\frac{y_t-y_c}{\sqrt{{(x_t-x_c)}^2+{(y_t-y_c)}^2}}\end{array}\right.$

Wherein (x _c, y _c) represent when preplanning point coordinate, (x _t, y _t) represent coordinate of ground point, r _txrepresent the component of traction field unit direction vector along x-axis, r _tyrepresent the component of traction field unit direction vector along y-axis;

Step 3: dyscalculia evades speed to intelligent body;

According to the threat pre-service of step one, threaten and be all converted to circular obstacle with terrain obstruction, should avoid entering the circular obstacle generation of this border circular areas and evade field and guiding field in routeing process, evade field action generation on planning point and evade speed V _p, guiding field action produces guiding speed V on planning point _g, what this step calculated is evade speed, if it is as follows to evade speed:

${\stackrel{\&RightArrow;}{V}}_P^i=\left\{\begin{array}{cc}0& d_i>r_i+\mathrm{\&Delta;R}\\ \frac{w_p}{(1+{((d_i-r_i)/L)}^2)}\&CenterDot;{\stackrel{\&RightArrow;}{r}}_p^i& r_i\&le;d_i\&le;r_i+\mathrm{\&Delta;R}\\ \frac{w_p}{{(d_i/r_i)}^2}\&CenterDot;{\stackrel{\&RightArrow;}{r}}_p^i& d_i<r\end{array}\right.$

Wherein represent i-th obstacle to planning point produce evade speed, w _prepresent the weight coefficient evading field, L represents the function coefficient evading field, d _irepresent the distance between i-th obstacle center and planning point, r _irepresent the radius of i-th barrier zone, Δ R represents that barrier zone evades the scope of field action, represent and evade a unit direction vector;

Make its field intensity size be α times that guides field field intensity in barrier zone field intensity active edge, make at barrier zone edge its field intensity size be β times that guides field, if the expression formula of weight and threat field action coefficient:

$w_p=\mathrm{\&beta;}\&CenterDot;w,L=\frac{\mathrm{\&Delta;R}}{\sqrt{\mathrm{\&alpha;}/(\mathrm{\&beta;}-1)}}$

Parameter beta representative, when barrier rims, evades a field intensity and the ratio of guiding field field intensity suffered by planning point, in order to prevent planning point barriers to entry region, demand fulfillment β > 1, when β is larger, in planning process, the track distance of planning point motion threatens edge far away;

Parameter alpha representative is when obstructive action edges of regions, a field intensity and the ratio of guiding field field intensity is evaded suffered by planning point, the concussion of field action edge is being evaded in order to prevent planning point, demand fulfillment α > 1, α value generally needs smaller, planning point is evaded before and after field action entering, and field intensity change is mild, thus makes air route smoother;

Step 4: guiding field with evade field and produce simultaneously, calculate guiding speed according to evading speed;

The position of hypothetical target point is p _t, planning point current location is p _c, then haulage speed evade speed with guiding speed guiding speed is:

${\stackrel{\&RightArrow;}{V}}_g=\mathrm{\&epsiv;}\left|{\stackrel{\&RightArrow;}{V}}_p\right|\&CenterDot;{\stackrel{\&RightArrow;}{r}}_g$

Wherein ε represents guiding field and evades the ratio of a size, gets ε=1 here, represents guiding field size and to evade field equal, represent guiding field unit direction vector;

Guiding velocity reversal vertically evades speed, and the direction of guiding speed is by impact point, perpendicular to evading speed and pointing to the side of impact point; The determination methods of guiding field is as follows:

${\stackrel{\&RightArrow;}{r}}_g=\left\{\begin{array}{cc}{\stackrel{\&RightArrow;}{V}}_g=R\left(\frac{\mathrm{\&pi;}}{2}\right)\&CenterDot;{\stackrel{\&RightArrow;}{r}}_p& \mathrm{\&theta;}({\stackrel{\&RightArrow;}{V}}_g^{\&prime;},{\stackrel{\&RightArrow;}{V}}_t)\&le;\frac{\mathrm{\&pi;}}{2}\\ {\stackrel{\&RightArrow;}{V}}_g^{\&prime;}=R(-\frac{\mathrm{\&pi;}}{2})\&CenterDot;{\stackrel{\&RightArrow;}{r}}_p& \mathrm{\&theta;}({\stackrel{\&RightArrow;}{V}}_g,{\stackrel{\&RightArrow;}{V}}_t)>\frac{\mathrm{\&pi;}}{2}\end{array}\right.$

Wherein $R\left(\mathrm{\&phi;}\right)=\left(\begin{array}{cc}\cos \mathrm{\&phi;}& -\sin \mathrm{\&phi;}\\ \sin \mathrm{\&phi;}& \cos \mathrm{\&phi;}\end{array}\right)$ Represent rotation matrix, by this rotation matrix of unit vector premultiplication, the unit vector after dextrorotation gyration φ can be obtained; represent vector with between angle; Evade the guiding velocity that speed obtains perpendicular to evading speed after over-rotation ± pi/2 with select the guiding field pointing to impact point side, represent with γ with angle, γ ' expression with angle, known γ+γ '=π; Therefore, right direction choose and also can be judged by following formula:

${\stackrel{\&RightArrow;}{r}}_g=\left\{\begin{array}{cc}{\stackrel{\&RightArrow;}{V}}_g& \mathrm{sgn}({\stackrel{\&RightArrow;}{V}}_g\&CenterDot;{\stackrel{\&RightArrow;}{V}}_t)=1\\ {\stackrel{\&RightArrow;}{V}}_g^{\&prime;}& \mathrm{sgn}({\stackrel{\&RightArrow;}{V}}_g^{\&prime;}\&CenterDot;{\stackrel{\&RightArrow;}{V}}_t)=1\end{array}\right.$

Wherein the dot product of expression amount vector, when the angle of two vectors is obtuse angle, two vector dot results are less than 0; Sgn (m) represents the symbol of peek value m, and computing formula is:

$\mathrm{sgn}\left(m\right)=\left\{\begin{array}{cc}-1& m<0\\ 0& m=0\\ 1& m>0\end{array}\right.$

Step 5: to haulage speed, evade speed and guiding speed carries out vector summing, and calculate subsequent time way point;

First to haulage speed, evade speed and guiding speed carries out vector summing, obtain total planning speed afterwards, carry out integration to planning speed to obtain planning air route.

Step 6: repeat step 2 ~ step 5, calculate a series of way point, complete routeing.

The size of described guiding field equals to evade field, and impact point, perpendicular to evading field, is pointed in direction.

Described planning speed is carried out integration and is adopted runge kutta method.

The invention has the advantages that:

(1) the present invention proposes the Route planner based on guiding velocity field under a kind of threatening environment, it can be the circular obstacle in two dimensional surface by threat transforming, adopt the form calculus haulage speed of field intensity driving, evade speed and guiding speed, smooth two dimension can be cooked up in real time and keep away barrier air route.

(2) the present invention proposes the Route planner based on guiding velocity field under a kind of threatening environment, and the parameter of adjustment is less, directly calculating field intensity speed, and then obtains air route.

(3) the present invention proposes the Route planner based on guiding velocity field under a kind of threatening environment, compared to Artificial Potential Field Method, be not easy to be absorbed in local minimum, the planning failure problem brought through obstacle symcenter can be solved, and air route is smooth, real-time is good.

Accompanying drawing explanation

Fig. 1 threatens Equivalent Calculation schematic diagram in the present invention;

Fig. 2 is barrier rims and obstructive action area schematic in the present invention;

Fig. 3 is haulage speed in the present invention, evades speed and guiding speed relativeness schematic diagram.

Embodiment

First carry out pre-service for threat and threat information is converted into static-obstacle, calculate haulage speed afterwards, evade speed and guiding speed, utilize superposition principle to sue for peace to speed, finally according to simulation step length determination flight route.Specifically comprise the steps:

Threat Equivalent Calculation is the circular static obstacle of certain height by step one: carry out pre-service to threat information.Concrete grammar is as follows:

If intelligent body carries out two-dimensional movement in a certain plane, intelligent body in this plane with threaten centre distance be r.Maximum effect radius R on α direction, each sight line angle of depression and the pass of α are R=f (α), and namely it is spatially the envelope surface threatening maximum effect space.If the envelope height being r place with the horizontal range of missile emplacements is Δ h, then in Fig. 1 there is following geometric relationship in intelligent body and threat:

$\left\{\begin{array}{c}\mathrm{\&Delta;h}=R\sin \mathrm{\&alpha;}=f\left(\mathrm{\&alpha;}\right)\sin \mathrm{\&alpha;}\\ r=R\cos \mathrm{\&alpha;}=f\left(\mathrm{\&alpha;}\right)\cos \mathrm{\&alpha;}\end{array}\right.$

Probability P is shot down in introducing _kfor correction factor, revise the height of envelope surface at r place, revised curved surface is as equivalent terrain surface.

$\left\{\begin{array}{c}{\mathrm{\&Delta;h}}_{\mathrm{td}}={\mathrm{KP}}_k\mathrm{\&Delta;h}={\mathrm{KP}}_{k}f\left(\mathrm{\&alpha;}\right)\sin \mathrm{\&alpha;}={\mathrm{KP}}_{k/\mathrm{clos}}{K}_{0}f\left(\mathrm{\&alpha;}\right){\sin }^2\mathrm{\&alpha;}\\ r=f\left(\mathrm{\&alpha;}\right)\cos \mathrm{\&alpha;}\end{array}\right.$

In formula, Δ h _tdfor threatening equivalent height, K is correction factor, P _kfor the fallen probability of aircraft, P _k/closfor being shot down probability under the clear condition of sight line.Wherein use formula:

$P_{\mathrm{clos}}=K_0\frac{{\mathrm{\&Delta;h}}_{\mathrm{AS}}}{\mathrm{Rs}}=K_0\sin \mathrm{\&alpha;},0^0<\mathrm{\&alpha;}<{90}^0$

P _closfor the clear probability of sight line, Δ h _aS, Rs is respectively the height of aircraft on missile emplacements and the oblique distance between them, K ₀for scale-up factor.In α=90 ⁰time (i.e. r=0), shoot down probability P _kmaximum, for safety make this some correction anterior and posterior height equal, namely have

KP _k/closK ₀f(90 ⁰)sin ²90 ⁰＝f(90 ⁰)sin90 ⁰

Comprehensive above variously to obtain:

$K=\frac{1}{P_{k/\mathrm{clos}}{K}_0}$

The terrain surface parametric equation that substitution can obtain threat equivalence is:

$\left\{\begin{array}{c}{\mathrm{\&Delta;h}}_{\mathrm{td}}=f\left(\mathrm{\&alpha;}\right){\sin }^2\mathrm{\&alpha;}\\ r=f\left(\mathrm{\&alpha;}\right)\cos \mathrm{\&alpha;}\end{array}\right.$

If the coordinate of missile emplacements is (x ₀, y ₀), then have bring above formula into and can obtain equivalent landform surface equation

${\mathrm{\&Delta;h}}_{\mathrm{td}}=\left\{\begin{array}{c}T(x,y),0<r<R_{\max }\\ 0,r>R_{\max }\end{array}\right.$

In formula, R _maxfor the maximal value of f (α).Such as, suppose that the radius of action of guided missile in all directions is R ₀, now the envelope surface of action space is hemisphere, i.e. f (α)=R ₀, substitute into above formula cancellation α can obtain equivalent landform surface equation

${\mathrm{\&Delta;h}}_{\mathrm{td}}=\left\{\begin{array}{c}\frac{R_0^2-{\left({x-x}_0\right)}^2-{(y-y_0)}^2}{R_0},0<r<R_0\\ 0,r>R_0\end{array}\right.$

After pre-service is carried out in threat, equivalent landform is a rotary paraboloid, and its shape approximation is in a mountain.When threatening the plane of movement distance with intelligent body to be Δ h _tdtime, then the radius after threatening equivalence is the center of circle is (x ₀, y ₀).Thus, in any two dimensional surface, threat can be equivalent to static circular obstacle.

Step 2: calculate target to the haulage speed of intelligent body.

The routeing task of intelligent body shows as and arrives impact point G (x in vector field _t, y _t), target is traction field field source, and traction field action produces haulage speed V on planning point _t.The traction field action region that impact point provides is the overall situation, and size is definite value, and direction is by planning a sensing impact point:

${\stackrel{\&RightArrow;}{V}}_T=w\&CenterDot;{\stackrel{\&RightArrow;}{r}}_t$

Wherein represent haulage speed, w represents traction field weight, represent the long unit direction vector of target.In two-dimensional space, expression formula as follows:

$\left\{\begin{array}{c}{r}_{\mathrm{tx}}=\frac{x_t-x_c}{\sqrt{{(x_t-x_c)}^2+{(y_t-y_c)}^2}}\\ r_{\mathrm{ty}}=\frac{y_t-y_c}{\sqrt{{(x_t-x_c)}^2+{(y_t-y_c)}^2}}\end{array}\right.$

Wherein (x _c, y _c) represent when preplanning point coordinate, (x _t, y _t) represent coordinate of ground point, r _txrepresent the component of traction field unit direction vector along x-axis, r _tyrepresent the component of traction field unit direction vector along y-axis.

Step 3: dyscalculia evades speed to intelligent body.

According to the threat pre-service of step one, threaten and be all converted to circular obstacle with terrain obstruction, should avoid entering this border circular areas in routeing process.Field and guiding field are evaded in circular obstacle generation, evade field action generation on planning point and evade speed V _p, guiding field action produces guiding speed V on planning point _g.What this step calculated is evade speed, evades speed and stops planning point close to circular obstacle.Evade field to set up needs and consider some:

(1) in order to reduce computation burden and facilitate environmental model to upgrade, evading field should arrange reach, exceedes this regional extent, and this obstacle is inoperative to planning point.

(2) size of evading field should to planning point to threatening the distance in district relevant.Distance more near field is strong larger, otherwise then reduces.Evade field action planning point produce evade velocity reversal should stop planning point close to this region.

(3) evade the concussion of field action edge in order to avoid planning air route, need the change evading field to be the process of continuous gradation.

Within the scope of obstructive action, the square distance evading a size and intelligent body current location and barrier rims is inversely proportional to, and intelligent body is pointed to by obstacle center in direction, and therefore to evade speed as follows in design:

${\stackrel{\&RightArrow;}{V}}_P^i=\left\{\begin{array}{cc}0& d_i>r_i+\mathrm{\&Delta;R}\\ \frac{w_p}{(1+{((d_i-r_i)/L)}^2)}\&CenterDot;{\stackrel{\&RightArrow;}{r}}_p^i& r_i\&le;d_i\&le;r_i+\mathrm{\&Delta;R}\\ \frac{w_p}{{(d_i/r_i)}^2}\&CenterDot;{\stackrel{\&RightArrow;}{r}}_p^i& d_i<r\end{array}\right.$

Wherein represent i-th obstacle to planning point produce evade speed.W _prepresent the weight coefficient evading field, L represents the function coefficient evading field, d _irepresent the distance between i-th obstacle center and planning point, r _irepresent the radius of i-th barrier zone, Δ R represents that barrier zone evades the scope of field action, represent and evade a unit direction vector.

As shown in Figure 2, make its field intensity size be α times that guides field field intensity in barrier zone field intensity active edge, make at barrier zone edge its field intensity size be β times that guides field.Regulation weight and the expression formula threatening field action coefficient:

$w_p=\mathrm{\&beta;}\&CenterDot;w,L=\frac{\mathrm{\&Delta;R}}{\sqrt{\mathrm{\&alpha;}/(\mathrm{\&beta;}-1)}}$

Parameter beta representative, when barrier rims, evades a field intensity and the ratio of guiding field field intensity suffered by planning point, in order to prevent planning point barriers to entry region, demand fulfillment β > 1, when β is larger, in planning process, the track distance of planning point motion threatens edge far away.

Parameter alpha representative is when obstructive action edges of regions, a field intensity and the ratio of guiding field field intensity is evaded suffered by planning point, the concussion of field action edge is being evaded in order to prevent planning point, demand fulfillment α > 1, α value generally needs smaller, planning point is evaded before and after field action entering, and field intensity change is mild, thus makes air route smoother.

Step 4: guiding field with evade field and produce simultaneously, calculate guiding speed according to evading speed.

Guiding field produces with evading field, and how its guiding plan point avoids barrier zone.The design of guiding field makes to evade a little to be avoided barrier zone and has certain orientation, thus reduces the probability that planning point is absorbed in local minimum area largely, and this guides the sharpest edges that tachometric method is different from Artificial Potential Field Method.

The size of guiding field equals to evade field, and impact point, perpendicular to evading field, is pointed in direction.The position of hypothetical target point is p _t, planning point current location is p _c, then haulage speed evade speed with guiding speed respectively as shown in Figure 3.Guiding speed is as follows:

${\stackrel{\&RightArrow;}{V}}_g=\mathrm{\&epsiv;}\left|{\stackrel{\&RightArrow;}{V}}_p\right|\&CenterDot;{\stackrel{\&RightArrow;}{r}}_g$

Wherein ε represents guiding field and evades the ratio of a size, gets ε=1 here, represents guiding field size and to evade field equal. represent guiding field unit direction vector.

Guiding velocity reversal vertically evades speed.The direction of guiding speed is by impact point, perpendicular to evading speed and pointing to the side of impact point.The determination methods of guiding field is as follows:

${\stackrel{\&RightArrow;}{r}}_g=\left\{\begin{array}{cc}{\stackrel{\&RightArrow;}{V}}_g=R\left(\frac{\mathrm{\&pi;}}{2}\right)\&CenterDot;{\stackrel{\&RightArrow;}{r}}_p& \mathrm{\&theta;}({\stackrel{\&RightArrow;}{V}}_g^{\&prime;},{\stackrel{\&RightArrow;}{V}}_t)\&le;\frac{\mathrm{\&pi;}}{2}\\ {\stackrel{\&RightArrow;}{V}}_g^{\&prime;}=R(-\frac{\mathrm{\&pi;}}{2})\&CenterDot;{\stackrel{\&RightArrow;}{r}}_p& \mathrm{\&theta;}({\stackrel{\&RightArrow;}{V}}_g,{\stackrel{\&RightArrow;}{V}}_t)>\frac{\mathrm{\&pi;}}{2}\end{array}\right.$

Wherein $R\left(\mathrm{\&phi;}\right)=\left(\begin{array}{cc}\cos \mathrm{\&phi;}& -\sin \mathrm{\&phi;}\\ \sin \mathrm{\&phi;}& \cos \mathrm{\&phi;}\end{array}\right)$ Represent rotation matrix, by this rotation matrix of unit vector premultiplication, the unit vector after dextrorotation gyration φ can be obtained. represent vector with between angle.Evade the guiding velocity that speed obtains perpendicular to evading speed after over-rotation ± pi/2 with now need the guiding field selecting to point to impact point side.Represent with γ with angle, γ ' expression with angle, known γ+γ '=π.Therefore, right direction choose and also can be judged by following formula:

${\stackrel{\&RightArrow;}{r}}_g=\left\{\begin{array}{cc}{\stackrel{\&RightArrow;}{V}}_g& \mathrm{sgn}({\stackrel{\&RightArrow;}{V}}_g\&CenterDot;{\stackrel{\&RightArrow;}{V}}_t)=1\\ {\stackrel{\&RightArrow;}{V}}_g^{\&prime;}& \mathrm{sgn}({\stackrel{\&RightArrow;}{V}}_g^{\&prime;}\&CenterDot;{\stackrel{\&RightArrow;}{V}}_t)=1\end{array}\right.$

Wherein the dot product of expression amount vector, when the angle of two vectors is obtuse angle, two vector dot results are less than 0.Sgn (m) represents the symbol of peek value m, and computing formula is:

$\mathrm{sgn}\left(m\right)=\left\{\begin{array}{cc}-1& m<0\\ 0& m=0\\ 1& m>0\end{array}\right.$

Step 5: to haulage speed, evade speed and guiding speed carries out vector summing, and calculate subsequent time way point.

First to haulage speed, evade speed and guiding speed carries out vector summing, obtain total planning speed afterwards, need to carry out integration to planning speed to obtain planning air route.In engineering, adopt runge kutta method to the integration of speed, high-order runge kutta method can improve the precision solved, and in order to improve solving speed, single order Runge Kutta method can be adopted to carry out the renewal of subsequent time location point:

P _k+1(x _k+1,y _k+1)＝P _k(x _k,y _k)+V(x _k,y _k)·Δt

Wherein Δ t represents the cycle length of iteration.Due to the planning speed V (x in planning algorithm _k, y _k) smaller, the precision single order Runge Kutta for position can meet planning requirement, in order to improve the quick performance of algorithm, adopts single order runge kutta method.

Step 6: repeat step 2 ~ step 5, calculate a series of way point, complete routeing.

Claims (3)

1. under a threatening environment based on guiding velocity field Route planner; It is characterized in that: first it carry out pre-service for threat and threat information is converted into static-obstacle, calculate haulage speed afterwards, evade speed and guiding speed, utilize superposition principle to sue for peace to speed, finally according to simulation step length determination flight route; Concrete steps are as follows:

Threat Equivalent Calculation is the circular static obstacle of certain height by step one: carry out pre-service to threat information;

Step 2: calculate target to the haulage speed of intelligent body;

The routeing task of intelligent body shows as and arrives impact point G (x in vector field _t, y _t), target is traction field field source, and traction field action produces haulage speed V on planning point _t; The traction field action region that impact point provides is the overall situation, and size is definite value, and direction is by planning a sensing impact point:

${\stackrel{\&RightArrow;}{V}}_T=w\&CenterDot;{\stackrel{\&RightArrow;}{r}}_t$

Wherein represent haulage speed, w represents traction field weight, represent the long unit direction vector of target; In two-dimensional space, expression formula as follows:

$\left\{\begin{array}{c}{r}_{\mathrm{tx}}=\frac{x_t-x_c}{\sqrt{{(x_t-x_c)}^2+{(y_t-y_c)}^2}}\\ r_{\mathrm{ty}}=\frac{y_t-y_c}{\sqrt{{(x_t-x_c)}^2+{(y_t-y_c)}^2}}\end{array}\right.$

Wherein (x _c, y _c) represent when preplanning point coordinate, (x _t, y _t) represent coordinate of ground point, r _txrepresent the component of traction field unit direction vector along x-axis, r _tyrepresent the component of traction field unit direction vector along y-axis;

Step 3: dyscalculia evades speed to intelligent body;

Evade field action generation on planning point and evade speed V _p, guiding field action produces guiding speed V on planning point _gif it is as follows to evade speed:

${\stackrel{\&RightArrow;}{V}}_P^i=\left\{\begin{array}{cc}0& d_i>r_i+\mathrm{\&Delta;R}\\ \frac{w_p}{(1+{((d_i-r_i)/L)}^2)}\&CenterDot;{\stackrel{\&RightArrow;}{r}}_p^i& r_i\&le;d_i\&le;r_i+\mathrm{\&Delta;R}\\ \frac{w_p}{{(d_i/r_i)}^2}\&CenterDot;{\stackrel{\&RightArrow;}{r}}_p^i& d_i<r\end{array}\right.$

Wherein represent i-th obstacle to planning point produce evade speed, w _prepresent the weight coefficient evading field, L represents the function coefficient evading field, d _irepresent the distance between i-th obstacle center and planning point, r _irepresent the radius of i-th barrier zone, Δ R represents that barrier zone evades the scope of field action, represent and evade a unit direction vector;

Make its field intensity size be α times that guides field field intensity in barrier zone field intensity active edge, make at barrier zone edge its field intensity size be β times that guides field, if the expression formula of weight and threat field action coefficient:

$w_p=\mathrm{\&beta;}\&CenterDot;w,L=\frac{\mathrm{\&Delta;R}}{\sqrt{\mathrm{\&alpha;}/(\mathrm{\&beta;}-1)}}$

Parameter beta representative, when barrier rims, evades a field intensity and the ratio of guiding field field intensity, β > 1 suffered by planning point, when β is larger, in planning process, the track distance threat edge of planning point motion is far away;

Parameter alpha representative, when obstructive action edges of regions, evades a field intensity and the ratio of guiding field field intensity, α > 1 suffered by planning point, α value generally needs smaller, planning point is evaded before and after field action entering, and field intensity change is mild, thus makes air route smoother;

Step 4: guiding field with evade field and produce simultaneously, calculate guiding speed according to evading speed;

The position of hypothetical target point is p _t, planning point current location is p _c, then haulage speed evade speed with guiding speed guiding speed is:

${\stackrel{\&RightArrow;}{V}}_g=\mathrm{\&epsiv;}\left|{\stackrel{\&RightArrow;}{V}}_p\right|\&CenterDot;{\stackrel{\&RightArrow;}{r}}_g$

Wherein ε represents guiding field and evades the ratio of a size, gets ε=1 here, represents guiding field size and to evade field equal, represent guiding field unit direction vector;

Guiding velocity reversal vertically evades speed, and the direction of guiding speed is by impact point, perpendicular to evading speed and pointing to the side of impact point; The determination methods of guiding field is as follows:

${\stackrel{\&RightArrow;}{r}}_g=\left\{\begin{array}{cc}{\stackrel{\&RightArrow;}{V}}_g=R\left(\frac{\mathrm{\&pi;}}{2}\right)\&CenterDot;{\stackrel{\&RightArrow;}{r}}_p& \mathrm{\&theta;}({\stackrel{\&RightArrow;}{V}}_g^{\&prime;},{\stackrel{\&RightArrow;}{V}}_t)\&le;\frac{\mathrm{\&pi;}}{2}\\ {\stackrel{\&RightArrow;}{V}}_g^{\&prime;}=R(-\frac{\mathrm{\&pi;}}{2})\&CenterDot;{\stackrel{\&RightArrow;}{r}}_p& \mathrm{\&theta;}({\stackrel{\&RightArrow;}{V}}_g,{\stackrel{\&RightArrow;}{V}}_t)>\frac{\mathrm{\&pi;}}{2}\end{array}\right.$

Wherein $R\left(\mathrm{\&phi;}\right)=\left(\begin{array}{cc}\cos \mathrm{\&phi;}& -\sin \mathrm{\&phi;}\\ \sin \mathrm{\&phi;}& \cos \mathrm{\&phi;}\end{array}\right)$ Represent rotation matrix, by this rotation matrix of unit vector premultiplication, the unit vector after dextrorotation gyration φ can be obtained; represent vector with between angle; Evade the guiding velocity that speed obtains perpendicular to evading speed after over-rotation ± pi/2 with select the guiding field pointing to impact point side, represent with γ with angle, γ ' expression with angle, known γ+γ '=π; Therefore, right direction choose and also can be judged by following formula:

${\stackrel{\&RightArrow;}{r}}_g=\left\{\begin{array}{cc}{\stackrel{\&RightArrow;}{V}}_g& \mathrm{sgn}({\stackrel{\&RightArrow;}{V}}_g\&CenterDot;{\stackrel{\&RightArrow;}{V}}_t)=1\\ {\stackrel{\&RightArrow;}{V}}_g^{\&prime;}& \mathrm{sgn}({\stackrel{\&RightArrow;}{V}}_g^{\&prime;}\&CenterDot;{\stackrel{\&RightArrow;}{V}}_t)=1\end{array}\right.$

Wherein the dot product of expression amount vector, when the angle of two vectors is obtuse angle, two vector dot results are less than 0; Sgn (m) represents the symbol of peek value m, and computing formula is:

$\mathrm{sgn}\left(m\right)=\left\{\begin{array}{cc}-1& m<0\\ 0& m=0\\ 1& m>0\end{array}\right.$

Step 5: to haulage speed, evade speed and guiding speed carries out vector summing, and calculate subsequent time way point;

First to haulage speed, evade speed and guiding speed carries out vector summing, obtain total planning speed afterwards, carry out integration to planning speed to obtain planning air route;

Step 6: repeat step 2 ~ step 5, calculate a series of way point, complete routeing.

2. under a kind of threatening environment according to claim 1 based on guiding velocity field Route planner; It is characterized in that: the size of described guiding field equals to evade field, impact point, perpendicular to evading field, is pointed in direction.

3. under a kind of threatening environment according to claim 1 based on guiding velocity field Route planner; It is characterized in that: described planning speed is carried out integration and adopted runge kutta method.