CN106197432B - A kind of UAV Landing method based on FastSLAM algorithm - Google Patents
A kind of UAV Landing method based on FastSLAM algorithm Download PDFInfo
- Publication number
- CN106197432B CN106197432B CN201610766065.6A CN201610766065A CN106197432B CN 106197432 B CN106197432 B CN 106197432B CN 201610766065 A CN201610766065 A CN 201610766065A CN 106197432 B CN106197432 B CN 106197432B
- Authority
- CN
- China
- Prior art keywords
- road sign
- unmanned plane
- particle
- environment road
- environment
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
Abstract
The invention discloses a kind of UAV Landing methods based on FastSLAM algorithm, belong to field of navigation technology.The method includes following steps: step 1, establishes UAV Landing section system model；Step 2 designs UAV Landing section FastSLAM algorithm；Step 3 obtains velocity estimation using arc tangent Nonlinear Tracking Differentiator.The present invention utilizes FastSLAM algorithm, particle filter and Extended Kalman filter is respectively adopted to estimate unmanned plane path and environmental characteristic, constructing environment map, realize update of the unmanned plane to self poisoning, realize independent navigation, have the characteristics that navigation accuracy is high, can satisfy the positioning accuracy and requirement of real-time of UAV Landing section.
Description
Technical field
The present invention relates to a kind of UAV Landing methods based on FastSLAM algorithm, belong to field of navigation technology.
Background technique
Landing is the stage that is extremely important and being easy to appear failure during unmanned plane execution task, due to manipulation is complicated,
Ground disturbing factor mostly Frequent Accidents, therefore high-precision navigation and guidance system are the important guarantors of unmanned plane safe landing
Card.Landing Guidance System used at present mainly has microwave landing system and instrument-landing-system, in navigation accuracy and performance
The requirement of aircraft landing can be met, but expensive, the unmanned plane for being not suitable for relative low price, needing frequent transition.
The airmanship that UAV Landing uses at present specifically include that inertial navigation system, GPS navigation system, inertia/
GPS integrated navigation system, computer vision navigation system etc..Inertial navigation belongs to independent navigation, can provide a variety of navigation ginsengs
Number, but error passage at any time and dissipate, landing phase, which only relies on inertial navigation, can bring biggish navigation error.GPS navigation tool
Have the advantages that precision it is high, using simple, but if navigation accuracy can be seriously affected by being interfered in landing mission.Computer
Vision has the advantages that big visual field, non-contact, abundant information, but image procossing needs a large amount of calculating and storage, is landing
Real-time is difficult to be guaranteed in journey.
Summary of the invention
The purpose of the present invention is to solve problems of the prior art, provide a kind of based on FastSLAM algorithm
FastSLAM algorithm is used for UAV Landing by UAV Landing method, the method, using particle filter and spreading kalman
The path of unmanned plane and environment road sign are estimated in filtering, specifically include following steps:
Step 1 establishes UAV Landing section system model.
Step 2 designs UAV Landing section FastSLAM algorithm.
Step 3 obtains velocity estimation using arc tangent Nonlinear Tracking Differentiator.
The present invention has the advantages that
(1) particle filter and Extended Kalman filter is respectively adopted to estimate unmanned plane path and environment road sign, structure
Environmental map is built, update of the unmanned plane to self poisoning is realized, realizes independent navigation.
(2) have the characteristics that navigation accuracy is high, can satisfy the required precision of UAV Landing section.
Detailed description of the invention
Fig. 1 is the UAV Landing method flow diagram of the invention based on FastSLAM algorithm.
Fig. 2 is environment road sign and landing path schematic diagram in embodiment.
It is 95s that Fig. 3, which is in simulation time, three shaft position estimation error curves in the case of different particles.
It is 95s that Fig. 4, which is in simulation time, three axle speed estimation error curves in the case of different particles.
It is 95s, grade comparison curves in the case of different particles that Fig. 5, which is in simulation time,.
It is 95s, three shaft position estimation error curves in the case of varying environment road sign that Fig. 6, which is in simulation time,.
It is 95s that Fig. 7, which is in simulation time, 18 particles, in the case of 134 environment road signs, environment road sign covariance matrix
Trace curve.
Specific embodiment
Below in conjunction with drawings and examples, the present invention is described in further detail.
The present invention provides a kind of UAV Landing method based on FastSLAM algorithm, and process is as shown in Figure 1, include following
Several steps:
Step 1 establishes UAV Landing section system model, the system model packet under the geographic coordinate system of northeast day
Include the motion model and observation model of unmanned plane；
The state vector x=[x, y, z, ψ, θ] of unmanned planeT, wherein x, y, z respectively indicate unmanned plane in earth axes
Three shaft position coordinates under oxyz, wherein o be chosen on ground a bit, x, y, z respectively refer to eastwards, north, day direction, ψ is nothing
For man-machine velocity vector in the angle of xoy plane projection and x-axis, θ is the angle of unmanned plane velocity vector and xoy plane, is referred to as
Unmanned plane Velocity Azimuth angle.Several environment road signs are distributed around in UAV Landing track, can be obtained using airborne laser radar
Obtain the distance between unmanned plane and environment road sign and azimuth.The motion model and observation model of unmanned plane can be with containing high
The nonlinear equation of this noise indicates are as follows:
Z=h (x (t), m (t), t)+εt (2)
Wherein, t is the time, and x (t) indicates the state vector of the unmanned plane of t moment,Indicate the micro- of drone status vector
Point, z indicates observed quantity of the unmanned plane to environment road sign, δtIndicate the Gaussian noise in motion model, motion model covariance square
Battle array is Qt,εtIt is the observation noise in observation model, observation model covariance matrix is Rt, u (t)=v is that airborne ins obtain
The unmanned plane speed of t moment, m (t) are position vector of the environment road sign under earth axes, and environment road sign position is fixed, no
T changes at any time, and form is as follows after motion model discretization:
L refers to that first of sampling walks, and Δ t is sampling time interval, and Δ θ is the increment of θ in two neighboring sampling step, and Δ ψ is
The increment of ψ, δ in two neighboring sampling stept1、δt2、δt3、δt4、δt5It is right to respectively indicate each quantity of state of unmanned plane (x, y, z, ψ and θ)
The Gaussian noise answered, v indicate the velocity magnitude for the current time unmanned plane that airborne ins obtain.It is selected on UAV Landing track
Several destinations are selected, if calculating unmanned plane current location and to up to distance between destination initially to be first destination up to destination,
If being less than given threshold Rmin, then to become next destination up to destination.Velocity Azimuth angle increment Δ θ, Δ ψ calculate as follows:
Wherein wpx,wpy,wpzIt is wait reach position coordinates of the destination under earth axes, x (l), y (l), z (l), ψ respectively
(l), θ (l) is respectively the quantity of state that first of sampling walks lower unmanned plane.
Unmanned plane can be expressed as z=[r, α, β] to the vector form of the observed quantity of environment road signT, concrete form is as follows:
Wherein, r be the distance between unmanned plane and environment road sign, α for unmanned plane and environment road sign between position vector and
Current time unmanned plane velocity vector is respectively and the difference of the angle of horizontal plane, β are the differences of two angles, and first angle is for nobody
Projection of the position vector on the face xoy and x-axis angle between machine and environment terrestrial reference, second angle is Velocity Azimuth angle ψ；x,y,
Z is respectively the current position coordinates of unmanned plane, xm,ym,zmRespectively three shaft position coordinates of environment road sign.
Step 2 designs UAV Landing section FastSLAM algorithm.
Specifically include following steps:
1) drone status updates；
Drone status is estimated using N number of particle filter, initialization particle weights coefficient is 1/N, initialization
Destination and environment road sign, initial state vector x0=[x0,y0,z0,ψ0,θ0]T, x0Each element be respectively unmanned plane initial bit
It sets and the azimuth of initial velocity.
Predict the perfect condition of the next sampling step of unmanned plane:
Wherein, x (l+1), y (l+1), z (l+1), ψ (l+1), θ (l+1) are respectively the pre- of unmanned plane the l+1 sampling step
Survey state, Velocity Azimuth angle increment Δ ψ, Δ θ define same formula (4), (5).
Below to speed and Velocity Azimuth angle increment adding procedure noise, the covariance matrix of motion model is Qt, prediction
Each particle state.
To the covariance matrix Q of motion modeltCarry out Cholesky factorization, i.e. Qt=SST；Define q=[v, Δ ψ, Δ θ
]T, X=randn (3) is random number vector, qn=STX+q, then qnIt is the unmanned plane speed after adding random process noise and side
Parallactic angle increment, enables qn=[vn,Δψn,Δθn]T, wherein vnIt is the speed added after random process noise, Δ ψnIt is that addition is random
The increment of azimuth ψ after process noise, Δ θnIt is the increment for adding the azimuth angle theta after random process noise.
Each particle indicates a kind of possible state of unmanned plane, to each particle, carries out state recursion, there is following l
Relationship between step and l+1 step state vector:
Wherein, xp,yp,zpThe respectively current position of particle, ψp,θpFor the current Velocity Azimuth angle of particle.
If reaching observation cycle, airborne laser radar detects environment road sign, obtains laser radar detection distance
Interior all environment road sign observed quantities and environment road sign sequence store the environment road sign sequence detected, if for the first time
The environment road sign detected, environment road sign observed quantity are stored in new road sign collection znIn, if not the environment road detected for the first time
Mark, then the observed quantity of environment road sign is stored in non-new road sign collection zfIn；Otherwise, unmanned plane predicted state is regained.
2) more new environment road sign；
If not new road sign collection zfIt is not empty set, to non-new road sign collection zfIn environment road sign, calculate environment road sign observed quantity
Predicted value provides intermediate variable dx, dy, dz, d and d first1It is as follows:
Dx=xf-xp, dy=yf-yp, dz=zf-zp
Wherein xp,yp,zpIt is the location components in particle shape state value,For the predicted value of environment road sign observed quantity, xf,yf,
zfRespectively non-new road sign collection zfThree shaft position coordinates of middle environment road sign.
It is updated, can be obtained using mean value and variance of the Extended Kalman filter to environment road sign:
Kt=Pf,t-1Hf(Rt+HfPf,t-1Hf T)-1
Pf,t=(I-KtHf)Pf,t-1
Wherein, lower footnote t indicates t moment, and lower footnote f expression is directed to non-new road sign collection；RtFor the association of observation noise
Variance matrix, KtIt is the filtering gain matrix of Kalman filtering, HfIt is observational equation (2) to environment road sign coordinate (xf,yf,zf)
Jacobian matrix, I are the unit matrix that dimension is 3, Pf,tIt is the covariance matrix of environment road sign, μtFor the mean value of environment road sign, zt
For environment road sign observed quantity.
The Jacobian matrix HfIt indicates are as follows:
Definition: Lt=HfPf,tHf T+Rt (12)
The weight coefficient of k-th of particleIt is as follows:
Subscript [k] indicates k-th of particle, and k range is 1~N, and z is observation vector of the unmanned plane to environment road sign.
If new road sign collection znIt is not empty set, shows to observe new environment road sign, then need to extend environmental map；
If the state vector of k-th of particle is x[k]=[xp [k],yp [k] ,zp [k],ψp [k],θp [k]]T, for new road sign collection znIn
Observed quantity z=[r, α, β]T, define intermediate variable:
S_phi=sin (α+ψ[k])
C_phi=cos (α+ψ[k])
S_thi=sin (β+θ[k])
C_thi=cos (β+θ[k]) (14)
Δ x=r*c_thi*c_phi
Δ y=r*c_thi*s_phi
Δ z=r*s_thi
It enables
xf=[xp [k]+Δx,yp [k]+Δy,zp [k]+Δz]T (16)
Pf,t=Hz*Rt*Hz T (17)
Pf,tIt is the covariance matrix of newly-increased environment road sign, xfIt is the estimated value of environment road sign position, r is unmanned plane and environment
The distance between road sign, Δ x are the differences of the x-axis component of environment road sign estimated value and the x-axis component of particle state, and Δ y is environment
The difference of the y-axis component of the y-axis component and particle state of road sign estimated value, Δ z are the z-axis component and particle of environment road sign estimated value
The difference of the z-axis component of state.
3) particle resampling；
Formula (13) has been obtained for the weight coefficient before the normalization of k-th of particleIt is now to calculate each particle
Homogenization weight coefficient, the weight coefficient after the normalization of k-th of particle is as follows:
Calculate number of effective particlesEffective population threshold N is setminIf Neff < Nmin,
Following resampling steps are then carried out, resampling is not otherwise needed.
The step of resampling, is as follows:
The uniform random number (the wherein number that N is above-mentioned particle) between N number of 0 to 1 is generated, is set as select [1] ...,
Select [j] ..., select [N], if the weight coefficient ω of k-th of particle[k]> select [j], it is all to meet above formula ω[k]>
The subscript j of select [j] constitutes set j；Definition sampling array sap [1]~sap [N], is initialized as 0, then k-th of particle exists
It is k that element disposition in set j is designated as under sampling array, similarly, by all particle weights coefficients compared with uniformly random array
And aforesaid operations are carried out, final sampling array can be obtained, the particle number after resampling is sampling array sequence.
Particle state mean value can obtain unmanned plane location estimation after finding out resampling.
Step 3 obtains velocity estimation using arc tangent Nonlinear Tracking Differentiator；
It allows the estimation of three shaft position of unmanned plane by Nonlinear Tracking Differentiator respectively, the velocity estimation of unmanned plane can be obtained.Due to
The Nonlinear Tracking Differentiator of arc tangent form can take into account tracking rapidity and transient process stationarity, and have preferable filter effect,
Parameter to be regulated is less, using arc tangent form Nonlinear Tracking Differentiator obtain unmanned plane velocity estimation, arc tangent form with
Track differentiator is as follows:
Wherein a1>0,a2>0,l1>0,l2> 0, R > 0 is design parameter, and v (t) is the speed of the unmanned plane t moment of input, x1
(t) and x2(t) be respectively input speed estimation and velocity differentials estimation.
Embodiment:
In landing glide section, downslide point height is 100m, and flight path angle is -3.5 °, and lifting speed is -2m/s, is being evened up
Section, touchdown elevation 0.7m, lifting speed -0.08m/s even up Terminal Track tilt angle gamma2=-1 °, glide a little wait fly away from
From for 1985.1m, the lifting speed for evening up section changes to -0.08m/s by -2m/s with exponential form.The flight time of downslide section
For 35s, the flight time for evening up section is 60s.The distributional environment road sign around track, relatively environment road sign is to estimation essence below
It spends in the example of comparison, environment road sign number takes 69 and 134 respectively.Unmanned plane speed control noise is ± 0.3m/s, angle
Controlling noise is ± 0.3 °, system communication cycle 0.05s, and the maximum detectable range of airborne laser radar is 50m, observed range
Noise is ± 0.1m, and observation angle noise is ± 0.3 °, observation cycle 0.4s.The Nonlinear Tracking Differentiator of x, y, z axle speed estimation
Parameter designing difference is as follows:
Rx=10, a1x=2, l1x=3, a2x=2, l2x=3
Ry=20, a1y=2, l1y=3, a2y=2, l2y=3
Rz=5, a1z=2, l1z=3, a2z=2, l2z=3
Three axis initial position errors are respectively 0.2m, 0.3m, 0.5m, choose 12 destinations from landing path and constitute expection
Route, landing path, environment road sign, the schematic diagram of destination are as shown in Figure 2.
In order to investigate influence of the number of particles to path evaluated error, 12,18 and 24 particles are taken respectively, in identical environment
It is emulated under road sign, road sign number is 134.Simulation result is as shown in Figure 3.As can be seen that the lateral deviation of unmanned plane is away from (X
Axis error) and height error (Z axis error) be held within the scope of 1m, the navigation accuracy that can satisfy the UAV Landing stage is wanted
It asks.In addition, number of particles increase advantageously reduces the position estimation error of unmanned plane.Since FastSLAM algorithm is filtered using particle
Wave estimates that the path of unmanned plane, Path selection sample can be increased by increasing population, and more particle samplers are conducive to force
The true path of nearly unmanned plane, thus advantageously reduce path estimation error.
The speed estimation error of three axes is as shown in Figure 4 in the case of different particles, it can be seen that in landing mission, 3
The speed estimation error of axis is substantially in the smaller range close to 0.Grade is more as shown in Figure 5, it can be seen that grade
Error is smaller between estimation and true value, and downslide section grade is constant during UAV Landing, evens up a section grade and gradually drops
As low as close to 0.
Influence for investigation environment road sign quantity to path evaluated error, chooses 69 and 134 environment road signs respectively,
It is emulated in the case of 18 particles, as a result as shown in Figure 6.As can be seen that environment road sign quantity increases, nobody is advantageously reduced
The path estimation error of machine.Environment road sign increases, and unmanned plane can obtain more observed quantities, conducive to the positioning for reducing unmanned plane
Error.Region especially more by environment road sign when unmanned plane, can obtain a large amount of observation informations, be conducive to correction positioning and miss
Difference.
For the convergence for verifying the map for observing that environment road sign is constituted, the curve of environment road sign covariance matrix mark is made such as
Shown in Fig. 7.It can be seen that the mark of environment road sign covariance matrix will increase after observing new environment road sign every time, show map
Uncertainty can all increase, and be gradually reduced then as the continuing to move of unmanned plane, converge to 0.
Due to emulation in control noise and observation noise be randomly generated, in order to reduce randomness to simulation result
It influences, more accurately assessment algorithm performance, to 134 environment road signs, the case where 12,18 and 24 particles carries out 20 times solely respectively
It is vertical to repeat to emulate, take the absolute value of average position estimation error to be compared algorithm performance, the results are shown in Table 1.It can see
Three shaft position evaluated error absolute values can satisfy the position accuracy demand of landing phase navigation all within the scope of 0.6m out.
Mean place evaluated error absolute value compares in the case of the different populations of table 1
| Population | X-axis position estimation error absolute value | Y-axis position estimation error absolute value | Z axis position estimation error absolute value |
| 12 | 0.53m | 0.31m | 0.53m |
| 18 | 0.48m | 0.30m | 0.51m |
| 24 | 0.46m | 0.30m | 0.51m |
Claims (3)
1. a kind of UAV Landing method based on FastSLAM algorithm, which is characterized in that including the following steps:
Step 1 establishes UAV Landing section system model, the system model includes nothing under the geographic coordinate system of northeast day
Man-machine motion model and observation model, is embodied as:
Z=h (x (t), m (t), t)+εt (2)
Wherein, t is the time, and x (t) indicates the state vector of the unmanned plane of t moment,Indicate the differential of drone status vector, nothing
Man-machine state vector x=[x, y, z, ψ, θ]T, wherein x, y, z respectively indicate three axis of the unmanned plane at earth axes oxyz
Position coordinates, wherein o be chosen on ground a bit, x, y, z respectively refer to eastwards, north, day direction, ψ be unmanned plane velocity vector
In the angle of xoy plane projection and x-axis, θ is the angle of unmanned plane velocity vector and xoy plane, is referred to as unmanned plane speed side
Parallactic angle；Several environment road signs are distributed around in UAV Landing track, obtain unmanned plane and environment road using airborne laser radar
The distance between mark and azimuth；Z indicates observed quantity of the unmanned plane to environment road sign, δtIndicate that the Gauss in motion model makes an uproar
Sound, motion model covariance matrix are Qt,εtIt is the observation noise in observation model, observation model covariance matrix is Rt, u (t)
=v is the unmanned plane speed for the t moment that airborne ins obtain, and m (t) is position vector of the environment road sign under earth axes,
Form is as follows after motion model discretization:
L refers to that first of sampling walks, and Δ t is sampling time interval, and Δ θ is the increment of θ in two neighboring sampling step, and Δ ψ is adjacent
The increment of ψ, δ in two sampling stepst1、δt2、δt3、δt4、δt5Respectively indicate each quantity of state x, y, z of unmanned plane, ψ and the corresponding height of θ
This noise, v indicate the velocity magnitude for the current time unmanned plane that airborne ins obtain；It is selected on UAV Landing track several
A destination, if unmanned plane current location is calculated and to up to distance between destination initially to be first destination up to destination, if being less than
Given threshold Rmin, then to become next destination up to destination；Velocity Azimuth angle increment Δ θ, Δ ψ calculate as follows:
Wherein wpx,wpy,wpzBe respectively to up to destination earth axes under position coordinates, x (l), y (l), z (l), ψ (l),
θ (l) is respectively the quantity of state that first of sampling walks lower unmanned plane；
Unmanned plane is expressed as z=[r, α, β] to the vector form of the observed quantity of environment road signT, concrete form is as follows:
Wherein, r be the distance between unmanned plane and environment road sign, α be between unmanned plane and environment terrestrial reference position vector and nobody
Machine velocity vector is respectively and the difference of the angle of horizontal plane, β are throwing of the position vector on the face xoy between unmanned plane and environment terrestrial reference
The difference of shadow and x-axis angle and Velocity Azimuth angle ψ, x, y, z are respectively the current position coordinates of unmanned plane, xm,ym,zmRespectively ring
Three shaft position coordinates of border road sign；
Step 2 designs UAV Landing section FastSLAM algorithm；
Specifically include following steps:
1) drone status updates；
Drone status is estimated using N number of particle filter, initialization particle weights coefficient is 1/N, initializes destination
With environment road sign, initial state vector x0=[x0,y0,z0,ψ0,θ0]T, x0Each element be respectively unmanned plane initial position with
And the azimuth of initial velocity；
Predict the perfect condition of the next sampling step of unmanned plane:
Wherein, x (l+1), y (l+1), z (l+1), ψ (l+1), θ (l+1) are respectively the prediction shape of unmanned plane the l+1 sampling step
State；
To speed and Velocity Azimuth angle increment adding procedure noise, to each particle, carry out state recursion, have following l step and
L+1 walks the relationship between state vector:
Wherein, xp,yp,zpThe respectively current position of particle, ψp,θpFor the current Velocity Azimuth angle of particle；vnIt is that addition is random
Speed after process noise, Δ ψnIt is the increment for adding the azimuth ψ after random process noise, Δ θnIt is that addition random process is made an uproar
The increment of azimuth angle theta after sound；
If reaching observation cycle, airborne laser radar detects environment road sign, obtains in laser radar detection distance
All environment road sign observed quantities and environment road sign sequence store the environment road sign sequence detected, if detecting for the first time
The environment road sign arrived, environment road sign observed quantity are stored in new road sign collection znIn, if not the environment road sign detected for the first time, then
Environment road sign observed quantity is stored in non-new road sign collection zfIn；Otherwise, unmanned plane predicted state is regained；
2) more new environment road sign；
If not new road sign collection zfIt is not empty set, to non-new road sign collection zfIn environment road sign, calculate the observed quantity of environment road sign prediction
Value, provides intermediate variable dx, dy, dz, d and d first1It is as follows:
Wherein xp,yp,zpIt is the location components in particle shape state value,For the predicted value of environment road sign observed quantity, xf,yf,zfRespectively
For non-new road sign collection zfThree shaft position coordinates of middle environment road sign；
It is updated, is obtained using mean value and variance of the Extended Kalman filter to environment road sign:
Wherein, lower footnote t indicates t moment, and lower footnote f expression is directed to non-new road sign collection；RtFor the covariance square of observation noise
Battle array, KtIt is the filtering gain matrix of Kalman filtering, HfIt is observational equation (2) to environment road sign coordinate (xf,yf,zf) Jacobi
Matrix, I are the unit matrix that dimension is 3, Pf,tIt is the covariance matrix of environment road sign, μtFor the mean value of environment road sign, ztFor environment
Road sign observed quantity；
The Jacobian matrix HfIt indicates are as follows:
Definition: Lt=HfPf,tHf T+Rt (12)
The weight coefficient of k-th of particleIt is as follows:
Subscript [k] indicates k-th of particle, and k range is 1~N, and z is observation vector of the unmanned plane to environment road sign；
If new road sign collection znIt is not empty set, shows to observe new environment road sign, then need to extend environmental map；
If the state vector of k-th of particle is x[k]=[xp [k],yp [k],zp [k],ψp [k],θp [k]]T, for new road sign collection znIn sight
It measures z=[r, α, β]T, define intermediate variable:
It enables
xf=[xp [k]+Δx,yp [k]+Δy,zp [k]+Δz]T (16)
Pf,t=Hz*Rt*Hz T (17)
Pf,tIt is the covariance matrix of newly-increased environment road sign, xfThe estimated value of environment road sign position, r be unmanned plane and environment road sign it
Between distance, Δ x is the difference of the x-axis component of environment road sign estimated value and the x-axis component of particle state, and Δ y is that environment road sign is estimated
The difference of the y-axis component of the y-axis component and particle state of evaluation, Δ z are the z-axis component and particle state of environment road sign estimated value
The difference of z-axis component；
3) particle resampling；
Formula (13) has been obtained for the weight coefficient of k-th of particleIt is now to calculate the homogenization weight system of each particle
Number, the normalized weight coefficient of k-th of particle are as follows:
Calculate number of effective particlesEffective population threshold N is setminIf Neff < Nmin, then into
Row resampling steps find out particle state mean value and then acquisition unmanned plane location estimation after resampling；
Otherwise resampling is not needed；
Step 3 obtains velocity estimation using arc tangent Nonlinear Tracking Differentiator.
2. a kind of UAV Landing method based on FastSLAM algorithm according to claim 1, it is characterised in that: step
It is as follows the step of resampling described in two:
The uniform random number between N number of 0 to 1 is generated, is set as select [1] ..., select [j] ..., select [N], if kth
The weight coefficient ω of a particle[k]> select [j], it is all to meet formula ω[k]The subscript j of > select [j] constitutes set j；It is fixed
Justice sampling array sap [1]~sap [N], is initialized as 0, then k-th of particle is designated as in set j at element in the case where sampling array
It is set to k, similarly, all particle weights coefficients compared with uniformly random array and is subjected to aforesaid operations, obtain final sampling
Array, the particle number after resampling are sampling array sequence.
3. a kind of UAV Landing method based on FastSLAM algorithm according to claim 1, it is characterised in that: step
Arc tangent Nonlinear Tracking Differentiator described in three is as follows:
Wherein a1> 0, a2> 0, l1> 0, l2> 0, R > 0 is design parameter, and v (t) is the speed of the unmanned plane t moment of input, x1
(t) and x2(t) be respectively input speed estimation and velocity differentials estimation.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610766065.6A CN106197432B (en) | 2016-08-30 | 2016-08-30 | A kind of UAV Landing method based on FastSLAM algorithm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610766065.6A CN106197432B (en) | 2016-08-30 | 2016-08-30 | A kind of UAV Landing method based on FastSLAM algorithm |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN106197432A CN106197432A (en) | 2016-12-07 |
| CN106197432B true CN106197432B (en) | 2018-12-21 |
Family
ID=58088578
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610766065.6A Expired - Fee Related CN106197432B (en) | 2016-08-30 | 2016-08-30 | A kind of UAV Landing method based on FastSLAM algorithm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN106197432B (en) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108195376B (en) * | 2017-12-13 | 2021-06-18 | 天津津航计算技术研究所 | Autonomous navigation positioning method for small unmanned aerial vehicle |
| CN108387236B (en) * | 2018-02-08 | 2021-05-07 | 北方工业大学 | Polarized light SLAM method based on extended Kalman filtering |
| CN108616302B (en) * | 2018-04-28 | 2020-10-30 | 中国人民解放军陆军工程大学 | Unmanned aerial vehicle multiple coverage model under power control and deployment method |
| CN109032169B (en) * | 2018-06-15 | 2024-03-26 | 岭南师范学院 | Unmanned aerial vehicle landing device based on laser conduction |
| US11572079B2 (en) * | 2019-04-25 | 2023-02-07 | WeRide Corp. | Apparatus and method for controlling velocity of autonomous driving vehicle, and storage medium |
| CN115235475B (en) * | 2022-09-23 | 2023-01-03 | 成都凯天电子股份有限公司 | MCC-based EKF-SLAM back-end navigation path optimization method |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101625572A (en) * | 2009-08-10 | 2010-01-13 | 浙江大学 | FastSLAM algorithm based on improved resampling method and particle selection |
| US20140126769A1 (en) * | 2012-11-02 | 2014-05-08 | Qualcomm Incorporated | Fast initialization for monocular visual slam |
| CN105737832A (en) * | 2016-03-22 | 2016-07-06 | 北京工业大学 | Distributed SLAM (simultaneous localization and mapping) method on basis of global optimal data fusion |
-
2016
- 2016-08-30 CN CN201610766065.6A patent/CN106197432B/en not_active Expired - Fee Related
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101625572A (en) * | 2009-08-10 | 2010-01-13 | 浙江大学 | FastSLAM algorithm based on improved resampling method and particle selection |
| US20140126769A1 (en) * | 2012-11-02 | 2014-05-08 | Qualcomm Incorporated | Fast initialization for monocular visual slam |
| CN105737832A (en) * | 2016-03-22 | 2016-07-06 | 北京工业大学 | Distributed SLAM (simultaneous localization and mapping) method on basis of global optimal data fusion |
Non-Patent Citations (2)
| Title |
|---|
| A Modified FastSLAM with Simple Particle Swarm Optimization and Consistent Mapping;Lulu Ying etc.;《OCEANS 2014-TAIPE》;20141124;第1-5页 * |
| 一种小型无人机的FastSLAM算法;丛楚滢等;《华中科技大学学报:自然科学版》;20151031;第43卷;第420-425,427页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN106197432A (en) | 2016-12-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106197432B (en) | A kind of UAV Landing method based on FastSLAM algorithm | |
| CN106643737B (en) | Four-rotor aircraft attitude calculation method in wind power interference environment | |
| Bryson et al. | Observability analysis and active control for airborne SLAM | |
| Sabatini et al. | Low-cost sensors data fusion for small size unmanned aerial vehicles navigation and guidance | |
| CN109445449B (en) | A kind of high subsonic speed unmanned plane hedgehopping control system and method | |
| Petrich et al. | On-board wind speed estimation for uavs | |
| CN105644785A (en) | Unmanned aerial vehicle landing method based on optical flow method and horizon line detection | |
| CN109683628B (en) | Spacecraft relative position control method based on finite time distributed speed observer | |
| CN109856972A (en) | A kind of unmanned helicopter robust Fault-Tolerant tracking and controlling method | |
| CN105912019A (en) | Powered parafoil system's air-drop wind field identification method | |
| Guo et al. | Algorithm for geomagnetic navigation and its validity evaluation | |
| Yu et al. | Observability-based local path planning and obstacle avoidance using bearing-only measurements | |
| Zahran et al. | Hybrid Machine Learning VDM for UAVs in GNSS‐denied Environment | |
| Kang et al. | Finite-memory-structured online training algorithm for system identification of unmanned aerial vehicles with neural networks | |
| CN113126647B (en) | Collaborative guidance method based on leader and follower principle | |
| Crocoll et al. | Quadrotor inertial navigation aided by a vehicle dynamics model with in-flight parameter estimation | |
| CN116719239A (en) | Trace underactuated satellite incomplete information tracking game control method | |
| Zheng et al. | UAV attitude measurement in the presence of wind disturbance | |
| Guan et al. | A new integrated navigation system for the indoor unmanned aerial vehicles (UAVs) based on the neural network predictive compensation | |
| Kannan et al. | Vision-based tracking of uncooperative targets | |
| Li et al. | Small UAV autonomous localization based on multiple sensors fusion | |
| Wang et al. | Attitude estimation for UAV with low-cost IMU/ADS based on adaptive-gain complementary filter | |
| Ramirez et al. | Stability analysis of a vision-based UAV controller: An application to autonomous road following missions | |
| Lee et al. | Analysis on the performance of VIO according to Trajectory Planning of UAV | |
| Tang et al. | Unscented Kalman filter for position estimation of UAV by using image information |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CF01 | Termination of patent right due to non-payment of annual fee | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20181221 Termination date: 20190830 |