Summary of the invention
The intelligent barrier avoiding method based on timing node that the object of the present invention is to provide a kind of, according to the travel path of target
Carry out risk anticipation, if it exists risk then planning path again, can jam situation to avoid target at same crossing, by macro
The anticipation of sight rationally utilize it is all can passage path, improve efficiency, reduce the problems such as paralysing caused by waiting phenomenon and waiting.This
The purpose of invention is achieved through the following technical solutions:
A kind of intelligent barrier avoiding method based on timing node, mainly includes the following steps,
Step 1: precise positioning: precise positioning is carried out to the real time position of target, obtain t moment coordinates of targets (x, y,
t);
Step 2: planning path:
A) plane is divided into square net, divides coordinate points, simplify region of search;
B) No. 1 target will from A to B, draw it is all from A to B can passage path, multilevel iudge goes out a fastest path,
And this fastest path is stored in work library;
C) No. 2 targets are marched into the arena, repeat No. 1 target selection path process, and by the path and work library in path into
Row compares, and such as there is coincidence point, then judges the time difference, and as too small such as the time difference, there are risk of collision, then programme path again.
Further, the specific steps of above-mentioned steps two are as follows:
1) assume in t0At the moment, No. 1 target is from starting point A to terminal B is wanted, and at current time, No. 1 target will be done searches as follows
Rope: since point A, as a point deposit one " opening list " to be processed, carving, only one element in table at the beginning,
But element increases later, forms a grid list to be checked;Mark the direction of travel of current goal;It is left up and down to find starting point
Passable grid and unlatching list is added in right four direction, skip can not by grid;It is deleted a little from opening in list
A is added to one " closing list ", saves all grids for not needing to check again in list;Then, selection is opened in list
Close on grid, repeat the process of front;
2) in t1Time, No. 2 targets enter near-earth region, by the number in the optimal three-dimensional array of No. 2 targets and work library
According to being compared, phase near point if it exists, then the point be marked as No. 2 targets can not progress point, programme path is until new again
Coordinates measurement;
3) repetition is above-mentioned 1) with 2) process, carries out path planning to the target of any time.
Further, above-mentioned steps one specifically comprise the following steps:
1) it using mixing least square method, directly handles the pulse signal issued by UWB label and receives pulse letter to base station
Number time, obtain positioning initial value;
2) with the point of Lagrange interpolation polynomial processing short distance, the accuracy of coordinate is improved;
It 3) will be because bad data caused by extraneous factor interference be rejected using outlier detection algorithm;
4) real-time coordinates of various discrete label are obtained with moving average filter, reaches the effect of optimization of positioning accuracy.
Further, above-mentioned intelligent barrier avoiding method further includes path detection, and the set time receives a near-earth region institute
There is the two-dimensional coordinate of target, these coordinates added into temporal information, is compared with the route data of each target in work library,
It sends a warning when target deviation route.
Compared with prior art, beneficial effects of the present invention:
Target is accurately positioned in intelligent barrier avoiding method of the invention, and it is pre- to carry out risk according to the travel path of target
Sentence, if it exists risk then planning path again, can jam situation to avoid target at same crossing, the anticipation for passing through macroscopic view closes
Reason using it is all can passage path, improve efficiency, reduce the problems such as paralysing caused by waiting phenomenon and waiting.
Specific embodiment
Further detailed description is done to the present invention combined with specific embodiments below, but embodiments of the present invention are not limited to
This.
The present embodiment is illustrated the intelligent barrier avoiding method by taking unmanned plane as an example.
It counts 2~40 times within DW1000 mono- second, therefore its smallest chronomere isThat is 16.65ps.DW1000 will
Real-time capture sends and receives timestamp, and accurately timestamp is before point-to-point accurate ranging is realized between label and base station
Mention, label between base station at a distance from be that the flight time tof (Timeof Flignt) transmitted a signal to by label up to base station multiplies
It is completed with the light velocity.That is:
D=tof × c
Due to the ultra wide band characteristic of itself, the temporal resolution of uwb is very high, and multiplied by the light velocity, range accuracy substantially may be used
It effectively keeps to a very small extent, to realize three-dimensional localization, needing to find out by the XYZ three-dimensional coordinate of positioning device, in base station
When erection, need especially to pull open the difference in height of Z axis, to ensure the accuracy on Z axis.Use the mode of TOF, three bases
Three-dimensional localization can be completed by standing.Ground set up can using set up mounting rod in the way of, for acquisition accurately three-dimensional localization data,
The mode for installing base station unmanned plane progress spot hover additional can be used to carry out the measurement to Z axis height.
Accurately the time is the key factor for obtaining targeting accuracy coordinate, and existing DW1000 can exist control errors
Within 30cm, error can be further reduced by being further processed for data.In this part, first using mixing minimum two
Multiplication directly handles the time that the pulse signal issued by UWB label receives pulse signal to base station, obtains positioning initial value;So
Afterwards with the point of Lagrange interpolation polynomial processing short distance, the accuracy of coordinate is improved;Use outlier detection algorithm will again
Because bad data caused by extraneous factor interference is rejected;Finally the reality of various discrete label is obtained with moving average filter
When coordinate, reach the effect of optimization of positioning accuracy.This positioning and optimizing model further increases positioning accuracy, and can mention significantly
The anti-jamming effectiveness of high positioning system, effectively increases location efficiency.Specific block diagram is as shown in Figure 1.
The data of four points are taken to draw line chart as shown in Figure 2.Observe Fig. 2, using interpolation method find out distance and error it
Between mapping relations.3 Lagrange interpolation polynomial P are sought by above 4 nodes3(x).Calculation formula is
Wherein
Have at node
L3(xj)=f (xj) (j=0,1,2,3)
And on other aspects, it is the approximation of f (x).Note
R3(x)=f (x)-Ln(x)
Claim R3It (x) is the remainder of interpolation polynomial.
By 3 Lagrange interpolation polynomials being calculated are as follows:
Y=-5.96 × 10-13·x3+3.175×10-9·x3-50824×10-6·x2+0.004034·x+0.4263
Matched curve is as shown in Figure 3.Fig. 3 is observed it can be found that 3 Lagrange interpolation polynomial methods can achieve very well
Fitting effect.It therefore, only need to be by the distance received by above formula progress for the label point in two meters of base station
Conversion, can be obtained the position coordinates that accuracy greatly promotes.These points are carried out together with remote label into one below
The processing of step.
It can count 40 times within DW1000 mono- second, i.e., each label is per second 40 dynamic coordinates.These coordinates are accurate
It floats value left and right.It is found by actual observation, partial coordinates differ larger with true value, analyze reason it is found that label and base station
Between information transmitting be electromagnetic wave, when there is strong interference in the external world (for example, apart from it is close when, someone is walking up and down), propagate
Time will receive influence, so institute's ranging from and true value compared to biggish deviation can be generated.Number biggish for these deviations
According to, rejected, main thought be delete from the farther away data of average value.A critical value K is set, when data and averagely
When the gap of value is more than critical value K, it is determined as outlier and rejects.Specific formula is as follows:
Wherein ri=1 indicates that the data will not be removed, ri=0 indicates that the data can be removed.In order to determine that k's is specific
Value, be by the thought of least square method, and specific flow chart is referring to Fig. 4.
There are many kinds of current alternative filtering algorithms, such as limit filtration, middle position value filtering method, digital averaging filtering
Method, moving average filter method, anti-impulse disturbances average filter method etc..Moving average filter is selected, it can be good at adapting to
In the higher-order of oscillation system of this paper, and it is smooth all high, there is good inhibiting effect to PERIODIC INTERFERENCE.
The specific method of moving average filter is that N number of sampled value will continuously be taken to regard a queue as, and the length of queue is solid
It is set to N.Sampling a new data is put into tail of the queue every time, and throws away a data of original head of the queue, (first in first out),
N number of data in queue carry out arithmetic average operation, that is, obtain the flow chart of new filter result algorithm referring to Fig. 5.
Unmanned plane does not need obstacle in high-altitude flight, but UWB can reach when unmanned plane during flying is under lower height
More accurately position.In this context, path planning is carried out to the flight path of unmanned plane.
Basic thought are as follows: there are also when certain horizontal distance, unmanned plane directly carries out in high-altitude straight from target level point
Line flight is lowered vertically into when entering in the range of target point apart from ground 8m, then carries out intelligent obstacle, and most Zhongdao
Up to above the 8m of target point (hereinafter referred to near-earth region), then vertical landing.Simplified schematic diagram is as shown in Figure 6.
Observe Fig. 6, it can be seen that three-dimensional coordinate is reduced to two-dimensional coordinate.Using unmanned plane position and the time three
Tie up coordinate (x1,x2,x3), A* intelligence pathfinding algorithm is used for reference to plan flight path of the unmanned plane in low latitude.
All instructions of unmanned plane are all issued by turn-key system, and turn-key system is divided into two subsystems: pathfinding and avoidance system
System, monitoring system.The two respectively works independently, system block diagram such as Fig. 7.
Path planning and barrier-avoiding method based on time point, advantage: it is more macroscopical, because the position of all unmanned planes is all
Know, it is possible to calculate the time that unmanned plane is put by some, evacuation is made in all passage path adjustment in advance.Similar high moral is navigated
When route recommendation.Congestion is avoided to avoid.
Plane is divided into square net first, artificially divides coordinate points, simplifies region of search.In conjunction with real-time positioning, it is
System has been simplified to the Three-Dimensional Dynamic array A=[a being made of coordinate and time1(x1,x2,x3),a2(x1,x2,x3),…,am
(x1,x2,x3)], wherein m is integer, with the quantity and route dynamic change of unmanned plane;x1,x2Indicate the position coordinates of unmanned plane,
x3Indicate time coordinate.Each element of array is a square of grid, and there are two types of states for square: can by with can not lead to
It crosses.Path is described as the set of the square passed through from A to B.Once path is found, unmanned plane is just from square center of a lattice (I
Be known as " node ") move towards another, until arriving at the destination, the characteristics of this system be the entire traveling process of unmanned plane without
It needs to pause.
Assuming that in t0At the moment, No. 1 unmanned plane is from starting point A to terminal B is wanted, and at current time, No. 1 will do following search:
Since point A, and using it as a point deposit one " opening list " to be processed.It carves at the beginning, in table
Only one element, but later will be more and more.The grid that No. 1 final path may include by it, it is also possible to will not.
Substantially, this is a grid list to be checked.
Mark the direction of travel of current unmanned plane, direction of travel be up and down one of those, if unmanned plane rotation 90
The consuming of degree is C1, the consuming for rotating 180 degree is C2(C1, C2It for fixed value, is set according to actual conditions).
It finds starting point and passable grid and unlatching list is added in they in four direction up and down, skipping can not lead to
The grid crossed.All these grids save paternal lattice of the point as A, with the description in path later.
Point A is deleted in list from opening, he is added to one " closing list ", saves all do not need again in list
The grid of inspection.
Then, selection, which is opened in list, closes on grid, substantially the repeatedly process of front.The selection of target grid is main
Concern.Provide path score value
F=G+H+C1+C2
Here, G is indicated from starting point A, along the path of generation, is moved on grid and is specified the mobile consuming of grid, this
Process unmanned plane only makes a move, so G=1.H indicates the consuming for being moved to final target point B from specified grid.
Euclidean distance and manhatton distance are introduced, wherein Euclidean distance are as follows:
Manhatton distance are as follows:
As can be seen that H is the process of an anticipation, the path that it can minimize unmanned plane is expended.
After above step generates No. 1 optimal travel path of unmanned plane, a three-dimensional array is obtained, each point contains
No. 1 unmanned plane marches to position and the time of the point.All elements in array are deposited into a new array, referred to as " work
Library ".The wherein calculation formula of time are as follows:
Wherein, t0Represent unmanned plane departure time, n90Represent the number of right-angled bend in the paths, n180Delegated path
In the number that turns through 180 degree, τtIt represents right-angled bend and expends the time, S delegated path total length, V represents unmanned plane average speed.
In t1Time, No. 2 unmanned planes enter near-earth region, repeat above-mentioned algorithm at this time, when obtaining the optimal of No. 2 unmanned planes
After three-dimensional array, it is compared with the data in work library, if it exists phase near point, then the point is marked as No. 2 unmanned planes not
Can progress point, programme path is until new coordinates measurement again.Regulation, under the premise of position coordinates are identical, when two points
When time difference was less than 2 seconds, the two points are considered as phase near point.
It repeats the above process, path planning is carried out to the unmanned plane that any time will land.
Real-time route monitors system:
Fully consider that the unmanned plane continuation of the journey being likely to occur in reality is insufficient, the emergency cases such as individual unmanned plane disconnectings.
A monitoring system, which must be designed, to carry out real-time position finding to the unmanned plane in full wafer near-earth region, when nothing of finding the problem
When man-machine, the location information of System Reports UAV targets is monitored, in order to which timely correction guarantees that turn-key system can continue to
Operation without any confusion.
Specifically: on the basis of the time of turn-key system, monitoring system every five seconds receives all unmanned planes in near-earth region
Two-dimensional coordinate, the route data by these coordinates plus each unmanned plane in temporal information at this time, and work library compares,
It sends a warning when target deviation route.Deviation detection algorithm specific formula is as follows:
Wherein, k1=1, k2=0.2, it is proportionality coefficient.x1,x2,x3Certain the unmanned plane coordinate received for the currently monitored system
Information.N indicates the grid number that the route of this unmanned plane passes through.As warning=1, system is sounded an alarm, and is not otherwise sent out
Alarm out.By the test of actual scene, setting is more than 1 meter or time deviation is more than five seconds when unmanned plane deviation route, it is believed that
Unmanned plane breaks down, can be with sets itself by adjusting the numerical value of proportionality coefficient.
As a result show: use above-mentioned algorithm, take at random two path plannings result as shown in Figure 8 (in figure 2 indicate can not
Pass through, 0 indicates to pass through).From figure 8, it is seen that whole system can operation without any confusion, can not only see nothing in figure
Man-machine route, moreover it is possible to which information at the time of predicting on route, in the case where map barrier is not very much, any time appoints
Meaning route unmanned plane can find arrival level point of the appropriate route without pause.
The above content is a further detailed description of the present invention in conjunction with specific preferred embodiments, and it cannot be said that
Specific implementation of the invention is only limited to these instructions.For those of ordinary skill in the art to which the present invention belongs, exist
Under the premise of not departing from present inventive concept, a number of simple deductions or replacements can also be made, all shall be regarded as belonging to of the invention
Protection scope.