CN106197432A - A kind of UAV Landing method based on FastSLAM algorithm - Google Patents

Abstract

The invention discloses a kind of UAV Landing method based on FastSLAM algorithm, belong to field of navigation technology.Described method includes following step: step one, sets up UAV Landing section system model；Step 2, designs UAV Landing section FastSLAM algorithm；Step 3, uses arc tangent Nonlinear Tracking Differentiator to obtain velocity estimation.The present invention utilizes FastSLAM algorithm, it is respectively adopted particle filter and EKF unmanned plane path and environmental characteristic are estimated, constructing environment map, realize the unmanned plane renewal to self poisoning, realize independent navigation, have the advantages that navigation accuracy is high, it is possible to meet positioning precision and the requirement of real-time of UAV Landing section.

Description

A kind of UAV Landing method based on FastSLAM algorithm

Technical field

The present invention relates to a kind of UAV Landing method based on FastSLAM algorithm, belong to field of navigation technology.

Background technology

Landing is that unmanned plane performs the stage that is extremely important in task process and that easily break down, due to handle complicated, Ground interference factor is many and Frequent Accidents, the most high-precision navigation and guiding system are the important guarantors of unmanned plane safe landing Card.Currently used Landing Guidance System mainly has microwave landing system and instrument landing system, in navigation accuracy and performance The requirement of aircraft landing can be met, but expensive, be not suitable for relative low price, need the unmanned plane of frequent transition.

The airmanship that at present UAV Landing uses 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, it is provided that multiple navigation is joined Number, but error passage in time and dissipate, landing phase only relies on inertial navigation can bring bigger navigation error.GPS navigation has There is precision height, use simple advantage, but if be interfered in landing mission and can have a strong impact on navigation accuracy.Computer Vision has big visual field, noncontact, informative advantage, but image procossing needs a large amount of calculating and storage, is landing In journey, real-time is difficult to be guaranteed.

Summary of the invention

The invention aims to solve problems of the prior art, it is provided that a kind of based on FastSLAM algorithm UAV Landing method, FastSLAM algorithm is used for UAV Landing by described method, uses particle filter and spreading kalman Filter and path and the environment road sign of unmanned plane estimated, specifically include following step:

Step one, sets up UAV Landing section system model.

Step 2, designs UAV Landing section FastSLAM algorithm.

Step 3, uses arc tangent Nonlinear Tracking Differentiator to obtain velocity estimation.

It is an advantage of the current invention that:

(1) it is respectively adopted particle filter and EKF unmanned plane path and environment road sign are estimated, structure Build environmental map, it is achieved the unmanned plane renewal to self poisoning, it is achieved independent navigation.

(2) there is the feature that navigation accuracy is high, it is possible to meet the required precision of UAV Landing section.

Accompanying drawing explanation

Fig. 1 is the UAV Landing method flow diagram based on FastSLAM algorithm of the present invention.

Fig. 2 is the environment road sign in embodiment and landing path schematic diagram.

Fig. 3 is to be 95s at simulation time, three shaft position estimation difference curves in the case of different particles.

Fig. 4 is to be 95s at simulation time, three axle speed estimation error curves in the case of different particles.

Fig. 5 is to be 95s at simulation time, grade comparison curves in the case of different particles.

Fig. 6 is to be 95s at simulation time, three shaft position estimation difference curve in the case of varying environment road sign.

Fig. 7 is to be 95s at simulation time, 18 particles, in the case of 134 environment road signs, and environment road sign covariance matrix Trace curve.

Detailed description of the invention

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 flow process is as it is shown in figure 1, include following Several steps:

Step one, northeastward under sky geographic coordinate system, sets up UAV Landing section system model, described system model bag Include motion model and the observation model of unmanned plane；

State vector x=[x, y, z, ψ, θ] of unmanned plane^T, wherein x, y, z represent that unmanned plane is at earth axes respectively Three shaft position coordinates under oxyz, wherein o is a bit chosen on ground, and x, y, z are respectively directed to east, north, direction, sky, and ψ is nothing Man-machine velocity is the angle of unmanned plane velocity and xoy plane at the angle of xoy plane projection Yu x-axis, θ, is referred to as Unmanned plane Velocity Azimuth angle.It is distributed around some environment road signs at UAV Landing flight path, utilizes airborne laser radar to obtain Obtain the distance between unmanned plane and environment road sign and azimuth.The motion model of unmanned plane and observation model can be with containing height The nonlinear equation of this noise is expressed as:

$\stackrel{\·}{x}=f\left(x\right(t),u(t),t)+{\mathrm{\δ}}_t---\left(1\right)$

Z=h (x (t), m (t), t)+ε_t (2)

Wherein, t is the time, and x (t) represents the state vector of the unmanned plane of t,Represent the micro-of unmanned plane state vector Point, z represents the unmanned plane observed quantity to environment road sign, δ_tRepresent the Gaussian noise in motion model, motion model covariance square Battle array is Q_t,ε_tBeing the observation noise in observation model, observation model covariance matrix is R_t, u (t)=v is that airborne ins obtains The unmanned plane speed of t, m (t) is environment road sign position vector under earth axes, and environment road sign position is fixed, no T change in time, after motion model discretization, form is as follows:

$\left[\begin{array}{c}x(l+1)\\ y(l+1)\\ z(l+1)\\ \mathrm{\ψ}(l+1)\\ \mathrm{\θ}(l+1)\end{array}\right]=\left[\begin{array}{c}x\left(l\right)+v\mathrm{\Δ}tcos\left(\mathrm{\θ}\right(l)+\mathrm{\Δ}\mathrm{\θ})sin\left(\mathrm{\ψ}\right(l)+\mathrm{\Δ}\mathrm{\ψ})\\ y\left(l\right)+v\mathrm{\Δ}tcos\left(\mathrm{\θ}\right(l)+\mathrm{\Δ}\mathrm{\θ})cos\left(\mathrm{\ψ}\right(l)+\mathrm{\Δ}\mathrm{\ψ})\\ z\left(l\right)+v\mathrm{\Δ}t\sin \left(\mathrm{\θ}\right(l)+\mathrm{\Δ}\mathrm{\θ})\\ \mathrm{\ψ}\left(l\right)+\mathrm{\Δ}\mathrm{\ψ}\\ \mathrm{\θ}\left(l\right)+\mathrm{\Δ}\mathrm{\θ}\end{array}\right]+\left[\begin{array}{c}{\mathrm{\δ}}_{t1}\\ {\mathrm{\δ}}_{t2}\\ {\mathrm{\δ}}_{t3}\\ {\mathrm{\δ}}_{t4}\\ {\mathrm{\δ}}_{t5}\end{array}\right]---\left(3\right)$

L refers to the l sampling step, and Δ t is sampling time interval, and Δ θ is the increment of θ in adjacent two sampling steps, and Δ ψ is The increment of ψ, δ in adjacent two sampling steps_t1、δ_t2、δ_t3、δ_t4、δ_t5Represent that each quantity of state of unmanned plane (x, y, z, ψ and θ) is right respectively The Gaussian noise answered, v represents the velocity magnitude of the current time unmanned plane that airborne ins obtains.UAV Landing flight path selects Select several destinations, if destination to be reached is first destination, calculate the spacing of unmanned plane current location and destination to be reached, If less than setting threshold value R_min, destination to be reached becomes next destination.Velocity Azimuth angle increment Δ θ, Δ ψ is calculated as follows:

$\mathrm{\Δ}\mathrm{\ψ}=\arctan \left(\frac{{\mathrm{wp}}_y-y\left(l\right)}{{\mathrm{wp}}_x-x\left(l\right)}\right)-\mathrm{\ψ}\left(l\right)---\left(4\right)$

$\mathrm{\Δ}\mathrm{\θ}=\arctan \left(\frac{{\mathrm{wp}}_z-z\left(l\right)}{{({\mathrm{wp}}_x-x(l\left)\right)}^2+{({\mathrm{wp}}_y-y(l\left)\right)}^2}\right)-\mathrm{\θ}\left(l\right)---\left(5\right)$

Wherein wp_x,wp_y,wp_zIt is the destination to be reached position coordinates under earth axes respectively, x (l), y (l), z (l), ψ L (), θ (l) is respectively the quantity of state of the l lower unmanned plane of sampling step.

Unmanned plane can be expressed as z=[r, α, β] to the vector form of the observed quantity of environment road sign^T, concrete form is as follows:

$\left[\begin{array}{c}r\\ \mathrm{\α}\\ \mathrm{\β}\end{array}\right]=\left[\begin{array}{c}\sqrt{{(x-x_m)}^2+{(y-y_m)}^2+{(z-z_m)}^2}\\ \arctan \left(\frac{z-z_m}{\sqrt{{(x-x_m)}^2+{(y-y_m)}^2}}\right)-\mathrm{\θ}\\ \arctan \left(\frac{y-y_m}{x-x_m}\right)-\mathrm{\ψ}\end{array}\right]---\left(6\right)$

Wherein, r is the distance between unmanned plane and environment road sign, α be between unmanned plane and environment road sign position vector with Current time unmanned plane velocity respectively with the difference of the angle of horizontal plane, β is the difference of two angles, and first angle is unmanned Position vector projection on xoy face 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, x_m,y_m,z_mIt is respectively three shaft position coordinates of environment road sign.

Step 2, designs UAV Landing section FastSLAM algorithm.

Specifically include following step:

1) unmanned plane state updates；

Using N number of particle filter to estimate unmanned plane state, initializing particle weights coefficient is 1/N, initializes Destination and environment road sign, initial state vector is x₀=[x₀,y₀,z₀,ψ₀,θ₀]^T, x₀Each element be respectively unmanned plane initial bit Put and the azimuth of initial velocity.

The perfect condition of prediction unmanned plane next one sampling step:

$\left[\begin{array}{c}x(l+1)\\ y(l+1)\\ z(l+1)\\ \mathrm{\ψ}(l+1)\\ \mathrm{\θ}(l+1)\end{array}\right]=\left[\begin{array}{c}x\left(l\right)+v\mathrm{\Δ}tcos\left(\mathrm{\θ}\right(l)+\mathrm{\Δ}\mathrm{\θ})sin\left(\mathrm{\ψ}\right(l)+\mathrm{\Δ}\mathrm{\ψ})\\ y\left(l\right)+v\mathrm{\Δ}tcos\left(\mathrm{\θ}\right(l)+\mathrm{\Δ}\mathrm{\θ})cos\left(\mathrm{\ψ}\right(l)+\mathrm{\Δ}\mathrm{\ψ})\\ z\left(l\right)+v\mathrm{\Δ}t\sin \left(\mathrm{\θ}\right(l)+\mathrm{\Δ}\mathrm{\θ})\\ \mathrm{\ψ}\left(l\right)+\mathrm{\Δ}\mathrm{\ψ}\\ \mathrm{\θ}\left(l\right)+\mathrm{\Δ}\mathrm{\θ}\end{array}\right]---\left(7\right)$

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 Q_t, it was predicted that Each particle state.

Covariance matrix Q to motion model_tCarry out Cholesky factorization, i.e. Q_t=SS^T；Definition q=[v, Δ ψ, Δ θ ]^T, X=randn (3) is random number vector, q_n=S^TX+q, then q_nIt is the unmanned plane speed after adding stochastic process noise and side Parallactic angle increment, makes q_n=[v_n,Δψ_n,Δθ_n]^T, wherein v_nIt is the speed after adding stochastic process noise, Δ ψ_nIt is to add at random The increment of the azimuth ψ after process noise, Δ θ_nIt it is the increment adding the azimuth angle theta after stochastic process noise.

Each particle represents a kind of possible state of unmanned plane, to each particle, carries out state recursion, has following l Relation between step and l+1 step state vector:

$\left[\begin{array}{c}{x}_p(l+1)\\ y_p(l+1)\\ z_p(l+1)\\ {\mathrm{\ψ}}_p(l+1)\\ {\mathrm{\θ}}_p(l+1)\end{array}\right]=\left[\begin{array}{c}{x}_p\left(l\right)+v_n\mathrm{\Δ}tcos\left({\mathrm{\θ}}_p\right(l)+{\mathrm{\Δ\θ}}_n)sin\left({\mathrm{\ψ}}_p\right(l)+{\mathrm{\Δ\ψ}}_n)\\ y_p\left(l\right)+v_n\mathrm{\Δ}tcos\left({\mathrm{\θ}}_p\right(l)+{\mathrm{\Δ\θ}}_n)cos\left({\mathrm{\ψ}}_p\right(l)+{\mathrm{\Δ\ψ}}_n)\\ z_p\left(l\right)+v_n\mathrm{\Δ}t\sin \left({\mathrm{\θ}}_p\right(l)+{\mathrm{\Δ\θ}}_n)\\ {\mathrm{\ψ}}_p\left(l\right)+{\mathrm{\Δ\ψ}}_n\\ {\mathrm{\θ}}_p\left(l\right)+{\mathrm{\Δ\θ}}_n\end{array}\right]---\left(8\right)$

Wherein, x_p,y_p,z_pIt is respectively the position that particle is current, ψ_p,θ_pFor the Velocity Azimuth angle that particle is current.

If reaching observation cycle, environment road sign is detected by airborne laser radar, it is thus achieved that laser radar detection distance Interior all environment road sign observed quantities and environment road sign sequence, the environment road sign sequence that storage has detected, if for the first time The environment road sign detected, environment road sign observed quantity is stored in new road sign collection z_nIn, 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 z_fIn；Otherwise, unmanned plane predicted state is regained.

2) environment road sign is updated；

If not new road sign collection z_fIt is not empty set, to non-new road sign collection z_fIn environment road sign, computing environment road sign observed quantity Predictive value, first provides intermediate variable dx, dy, dz, d and d₁As follows:

Dx=x_f-x_p, dy=y_f-y_p, dz=z_f-z_p

$d=\sqrt{{\mathrm{dx}}^2+{\mathrm{dy}}^2+{\mathrm{dz}}^2},d_1=\sqrt{{\mathrm{dx}}^2+{\mathrm{dy}}^2}---\left(9\right)$

${\hat{z}}_t={\[d,\arctan \left(\frac{dy}{dx}\right)-\mathrm{\ψ},\arctan \left(\frac{dz}{d_1}\right)-\mathrm{\θ}\]}^T$

Wherein x_p,y_p,z_pIt is the location components in particle state value,For the predictive value of environment road sign observed quantity, x_f,y_f, z_fIt is respectively non-new road sign collection z_fThree shaft position coordinates of middle environment road sign.

Use EKF that average and the variance of environment road sign are updated, can obtain:

K_t=P_f,t-1H_f(R_t+H_fP_f,t-1H_f ^T)^-1

${\mathrm{\μ}}_t={\mathrm{\μ}}_{t-1}+K_t(z_t-{\hat{z}}_t)---\left(10\right)$

P_f,t=(I-K_tH_f)P_f,t-1

Wherein, lower footnote t represents that t, lower footnote f represent and is directed to non-new road sign collection；R_tAssociation for observation noise Variance matrix, K_tIt is the filtering gain matrix of Kalman filtering, H_fFor observational equation (2) to environment road sign coordinate (x_f,y_f,z_f) Jacobian matrix, I be dimension be the unit matrix of 3, P_f,tIt is the covariance matrix of environment road sign, μ_tFor the average of environment road sign, z_t For environment road sign observed quantity.

Described Jacobian matrix H_fIt is expressed as:

$H_f=\left[\begin{array}{ccc}\frac{dx}{d}& \frac{dy}{d}& \frac{dz}{d}\\ -\frac{dy}{d_1^2}& \frac{dx}{d_1^2}& 0\\ -\frac{dxdz}{d^2{d}_1}& -\frac{dydz}{d^2{d}_1}& \frac{d_1}{d^2}\end{array}\right]---\left(11\right)$

Definition: L_t=H_fP_f,tH_f ^T+R_t (12)

The weight coefficient of kth particleAs follows:

${\mathrm{\ω}}_t^{\[k\]}={\left|2{\mathrm{\πL}}_t^{\[k\]}\right|}^{-1/2}\exp (-\frac{1}{2}{\left(z-{\hat{z}}_t^{\[k\]}\right)}^T(L_t^{\[k\]}\left)\right(z-{\hat{z}}_t^{\[k\]}\left)\right)---\left(13\right)$

Subscript [k] represents kth particle, and k scope is 1～N, and z is the unmanned plane observation vector to environment road sign.

If new road sign collection z_nIt is not empty set, shows to observe new environment road sign, then need to extend environmental map；

If the state vector of kth particle is x^[k]=[x_p ^[k],y_p ^[k] _,z_p ^[k],ψ_p ^[k],θ_p ^[k]]^T, for new road sign collection z_nIn Observed quantity z=[r, α, β]^T, definition 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

Order $H_z=\left[\begin{array}{ccc}c\_thi*c\_phi& -rc\_thi*s\_phi& -rc\_thi*c\_phi\\ c\_thi*s\_phi& rc\_thi*c\_phi& -rs\_thi*s\_phi\\ s\_thi& 0& r*c\_thi\end{array}\right]---\left(15\right)$

x_f=[x_p ^[k]+Δx,y_p ^[k]+Δy,z_p ^[k]+Δz]^T (16)

P_f,t=H_z*R_t*H_z ^T (17)

P_f,tIt is the covariance matrix of newly-increased environment road sign, x_fBeing the estimated value of environment road sign position, r is unmanned plane and environment Distance between road sign, Δ x is the x-axis component difference with the x-axis component of particle state of environment road sign estimated value, and Δ y is environment The y-axis component of road sign estimated value and the difference of the y-axis component of particle state, Δ z is z-axis component and the 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 kth particleIt is now to calculate each particle Homogenization weight coefficient, the weight coefficient after the normalization of kth particle is as follows:

Calculate number of effective particlesEffective population threshold N is set_minIf, Neff < N_min, Then carry out following resampling steps, otherwise need not resampling.

The step of described resampling is as follows:

Generate the uniform random number between N number of 0 to 1 (wherein N is the number of above-mentioned particle), be set to select [1] ..., Select [j] ..., select [N], if the weight coefficient ω of kth particle^[k]> select [j], all meet above formula ω^[k]> Subscript j of select [j] constitutes set j；Definition sampling array sap [1]～sap [N], be initialized as 0, then kth particle exists It is designated as gathering in j under sampling array and is set to k at element, in like manner, all particle weights coefficients are compared with uniformly random array And carrying out aforesaid operations, available final sampling array, the particle numbering after resampling is sampling array sequence.

After obtaining resampling, particle state average can obtain unmanned plane location estimation.

Step 3, uses arc tangent Nonlinear Tracking Differentiator to obtain velocity estimation；

Unmanned plane three shaft position is allowed to estimate to pass through Nonlinear Tracking Differentiator respectively, it is possible to obtain the velocity estimation of unmanned plane.Due to The Nonlinear Tracking Differentiator of arc tangent form can take into account tracking rapidity and transient process stationarity, and has preferable filter effect, Parameter to be regulated is less, use arc tangent form Nonlinear Tracking Differentiator obtain unmanned plane velocity estimation, arc tangent form with Track differentiator is as follows:

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_1\left(t\right)=x_2\left(t\right)\\ {\stackrel{\&CenterDot;}{x}}_2\left(t\right)=R^2(-a_1\arctan (l_1\left(x_1\left(t\right)-v\left(t\right)\right))-a_2\arctan (l_2{x}_2\left(t\right)/R\left)\right)\end{array}\right.---\left(18\right)$

Wherein a₁>0,a₂>0,l₁>0,l₂> 0, R > 0 it is design parameter, v (t) is the speed of the unmanned plane t of input, x₁ (t) and x₂T () is estimation and the estimation of velocity differentials of input speed respectively.

Embodiment:

In landing glide section, the some height that glides is 100m, and flight path angle is-3.5 °, and rising or falling speed is-2m/s, is evening up Section, touchdown elevation is 0.7m, and rising or falling speed-0.08m/s evens up Terminal Track tilt angle gamma₂=-1 °, glide point wait fly away from From for 1985.1m, the rising or falling speed of the section of evening up is changed to-0.08m/s by-2m/s with exponential form.The flight time of downslide section For 35s, the flight time of the section of evening up is 60s.At flight path environment distributed about road sign, compare environment road sign below to estimating essence In the example of degree contrast, environment road sign number takes 69 and 134 respectively.Unmanned plane speed controlling noise is ± 0.3m/s, angle Controlling noise is ± 0.3 °, and system communication cycle is 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 °, and observation cycle is 0.4s.The Nonlinear Tracking Differentiator of x, y, z axle velocity estimation Parameter designing is as follows:

R_x=10, a_1x=2, l_1x=3, a_2x=2, l_2x=3

R_y=20, a_1y=2, l_1y=3, a_2y=2, l_2y=3

R_z=5, a_1z=2, l_1z=3, a_2z=2, l_2z=3

Three axle initial position errors are respectively 0.2m, 0.3m, 0.5m, choose 12 destinations and constitute expection from landing path Route, landing path, environment road sign, destination schematic diagram as shown in Figure 2.

In order to investigate the number of particles impact on path estimation difference, take 12,18 and 24 particles respectively, at equivalent environment Emulating under road sign, road sign number is 134.Simulation result is as shown in Figure 3.It can be seen that the lateral deviation of unmanned plane is away from (X Axis error) and in the range of height error (Z axis error) is held in 1m, the navigation accuracy that can meet the UAV Landing stage is wanted Ask.Additionally, number of particles increase advantageously reduces the position estimation error of unmanned plane.Owing to FastSLAM algorithm uses particle filter The path of unmanned plane is estimated by ripple, increases population and can increase Path selection sample, and more particle sampler is beneficial to force The true path of nearly unmanned plane, thus advantageously reduce path estimation error.

In the case of different particles, the speed estimation error of three axes is as shown in Figure 4, it can be seen that in landing mission, and 3 The speed estimation error of axle is substantially in the smaller range of 0.Grade compares as shown in Figure 5, it can be seen that grade Estimating that error is less between actual value, during UAV Landing, downslide section grade is constant, and the section of evening up grade gradually drops Low to close to 0.

For investigating the impact on path estimation difference of the environment road sign quantity, choose 69 and 134 environment road signs respectively, Emulating in the case of 18 particles, result is as shown in Figure 6.It can be seen that environment road sign quantity increases, advantageously reduce unmanned The path estimation error of machine.Environment road sign increases, and unmanned plane can obtain more observed quantity, is beneficial to reduce the location of unmanned plane Error.Especially when unmanned plane is through the more region of environment road sign, it is possible to obtain a large amount of observation information, it is beneficial to correction location by mistake Difference.

Observe the convergence of map that environment road sign constitutes for checking, make the curve of environment road sign covariance matrix mark such as Shown in Fig. 7.Can be seen that, after observing new environment road sign, the mark of environment road sign covariance matrix can increase, and shows map every time Uncertainty all can increase, and is gradually reduced then as the continuation campaign of unmanned plane, converges to 0.

Owing to the control noise in emulation and observation noise randomly generate, in order to reduce randomness to simulation result Impact, assessment algorithm performance more accurately, to 134 environment road signs, the situation of 12,18 and 24 particles carries out 20 times solely respectively Vertical repetition emulates, and algorithm performance is compared by the absolute value being averaged position estimation error, and result is as shown in table 1.Can see Go out three shaft position estimation difference absolute values all in the range of 0.6m, it is possible to meet the position accuracy demand of landing phase navigation.

In the case of the different population of table 1, mean place estimation difference absolute value compares

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 UAV Landing method based on FastSLAM algorithm, it is characterised in that include following step:

Step one, northeastward under sky geographic coordinate system, sets up UAV Landing section system model, and described system model includes nothing Man-machine motion model and observation model, be embodied as:

$\stackrel{\&CenterDot;}{x}=f\left(x\right(t),u(t),t)+{\mathrm{\&delta;}}_t---\left(1\right)$

Z=h (x (t), m (t), t)+ε_t (2)

Wherein, t is the time, and x (t) represents the state vector of the unmanned plane of t,Represent the differential of unmanned plane state vector, nothing Man-machine state vector x=[x, y, z, ψ, θ]^T, wherein x, y, z represent the unmanned plane three axles under earth axes oxyz respectively Position coordinates, wherein o is a bit chosen on ground, and x, y, z are respectively directed to east, north, direction, sky, and ψ is unmanned plane velocity At the angle of xoy plane projection Yu x-axis, θ is the angle of unmanned plane velocity and xoy plane, is referred to as unmanned plane speed side Parallactic angle；It is distributed around some environment road signs at UAV Landing flight path, utilizes airborne laser radar to obtain unmanned plane and environment road Distance between mark and azimuth；Z represents the unmanned plane observed quantity to environment road sign, δ_tRepresent that the Gauss in motion model makes an uproar Sound, motion model covariance matrix is Q_t,ε_tBeing the observation noise in observation model, observation model covariance matrix is R_t, u (t) =v is the unmanned plane speed of the t that airborne ins obtains, and m (t) is environment road sign position vector under earth axes, After motion model discretization, form is as follows:

$\left[\begin{array}{c}x(l+1)\\ y(l+1)\\ z(l+1)\\ \mathrm{\&psi;}(l+1)\\ \mathrm{\&theta;}(l+1)\end{array}\right]=\left[\begin{array}{c}x\left(l\right)+v\mathrm{\&Delta;}t\cos \left(\mathrm{\&theta;}\right(l)+\mathrm{\&Delta;}\mathrm{\&theta;})\sin \left(\mathrm{\&psi;}\right(l)+\mathrm{\&Delta;}\mathrm{\&psi;})\\ y\left(l\right)+v\mathrm{\&Delta;}t\cos \left(\mathrm{\&theta;}\right(l)+\mathrm{\&Delta;}\mathrm{\&theta;})\cos \left(\mathrm{\&psi;}\right(l)+\mathrm{\&Delta;}\mathrm{\&psi;})\\ z\left(l\right)+v\mathrm{\&Delta;}t\sin \left(\mathrm{\&theta;}\right(l)+\mathrm{\&Delta;}\mathrm{\&theta;})\\ \mathrm{\&psi;}\left(l\right)+\mathrm{\&Delta;}\mathrm{\&psi;}\\ \mathrm{\&theta;}\left(l\right)+\mathrm{\&Delta;}\mathrm{\&theta;}\end{array}\right]+\left[\begin{array}{c}{\mathrm{\&delta;}}_{t1}\\ {\mathrm{\&delta;}}_{t2}\\ {\mathrm{\&delta;}}_{t3}\\ {\mathrm{\&delta;}}_{t4}\\ {\mathrm{\&delta;}}_{t5}\end{array}\right]---\left(3\right)$

L refers to the l sampling step, and Δ t is sampling time interval, and Δ θ is the increment of θ in adjacent two sampling steps, and Δ ψ is adjacent The increment of ψ, δ in two sampling steps_t1、δ_t2、δ_t3、δ_t4、δ_t5Represent each quantity of state of unmanned plane (x, y, z, ψ and θ) correspondence respectively Gaussian noise, v represents the velocity magnitude of the current time unmanned plane that airborne ins obtains；If selecting on UAV Landing flight path Dry destination, if destination to be reached is first destination, calculates the spacing of unmanned plane current location and destination to be reached, if little In setting threshold value R_min, destination to be reached becomes next destination；Velocity Azimuth angle increment Δ θ, Δ ψ is calculated as follows:

$\mathrm{\&Delta;}\mathrm{\&psi;}=\arctan \left(\frac{{\mathrm{wp}}_y-y\left(l\right)}{{\mathrm{wp}}_x-x\left(l\right)}\right)-\mathrm{\&psi;}\left(l\right)---\left(4\right)$

$\mathrm{\&Delta;}\mathrm{\&theta;}=\arctan \left(\frac{{\mathrm{wp}}_z-z\left(l\right)}{{({\mathrm{wp}}_x-x(l\left)\right)}^2+{({\mathrm{wp}}_y-y(l\left)\right)}^2}\right)-\mathrm{\&theta;}\left(l\right)---\left(5\right)$

Wherein wp_x,wp_y,wp_zIt is the position coordinates under the earth axes of destination to be reached respectively, x (l), y (l), z (l), ψ (l), θ (l) is respectively the quantity of state of the l lower unmanned plane of sampling step；

Unmanned plane is expressed as z=[r, α, β] to the vector form of the observed quantity of environment road sign^T, concrete form is as follows:

$\left[\begin{array}{c}r\\ \mathrm{\&alpha;}\\ \mathrm{\&beta;}\end{array}\right]=\left[\begin{array}{c}\sqrt{{(x-x_m)}^2+{(y-y_m)}^2+{(z-z_m)}^2}\\ \arctan \left(\frac{z-z_m}{\sqrt{{(x-x_m)}^2+{(y-y_m)}^2}}\right)-\mathrm{\&theta;}\\ \arctan \left(\frac{y-y_m}{x-x_m}\right)-\mathrm{\&psi;}\end{array}\right]---\left(6\right)$

Wherein, r is the distance between unmanned plane and environment road sign, α be between unmanned plane and environment terrestrial reference position vector with unmanned Motor speed vector respectively with the difference of the angle of horizontal plane, β is position vector throwing on xoy face between unmanned plane and environment terrestrial reference Shadow and x-axis angle and the difference of Velocity Azimuth angle ψ, x, y, z are respectively the current position coordinates of unmanned plane, x_m,y_m,z_mIt is respectively ring Three shaft position coordinates of border road sign；

Step 2, designs UAV Landing section FastSLAM algorithm；

Specifically include following step:

1) unmanned plane state updates；

Using N number of particle filter to estimate unmanned plane state, initializing particle weights coefficient is 1/N, initializes destination With environment road sign, initial state vector is x₀=[x₀,y₀,z₀,ψ₀,θ₀]^T, x₀Each element be respectively unmanned plane initial position with And the azimuth of initial velocity；

The perfect condition of prediction unmanned plane next one sampling step:

$\left[\begin{array}{c}x(l+1)\\ y(l+1)\\ z(l+1)\\ \mathrm{\&psi;}(l+1)\\ \mathrm{\&theta;}(l+1)\end{array}\right]=\left[\begin{array}{c}x\left(l\right)+v\mathrm{\&Delta;}t\cos \left(\mathrm{\&theta;}\right(l)+\mathrm{\&Delta;}\mathrm{\&theta;})\sin \left(\mathrm{\&psi;}\right(l)+\mathrm{\&Delta;}\mathrm{\&psi;})\\ y\left(l\right)+v\mathrm{\&Delta;}t\cos \left(\mathrm{\&theta;}\right(l)+\mathrm{\&Delta;}\mathrm{\&theta;})\cos \left(\mathrm{\&psi;}\right(l)+\mathrm{\&Delta;}\mathrm{\&psi;})\\ z\left(l\right)+v\mathrm{\&Delta;}t\sin \left(\mathrm{\&theta;}\right(l)+\mathrm{\&Delta;}\mathrm{\&theta;})\\ \mathrm{\&psi;}\left(l\right)+\mathrm{\&Delta;}\mathrm{\&psi;}\\ \mathrm{\&theta;}\left(l\right)+\mathrm{\&Delta;}\mathrm{\&theta;}\end{array}\right]---\left(7\right)$

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 Relation between l+1 step state vector:

$\left[\begin{array}{c}{x}_p(l+1)\\ y_p(l+1)\\ z_p(l+1)\\ {\mathrm{\&psi;}}_p(l+1)\\ {\mathrm{\&theta;}}_p(l+1)\end{array}\right]=\left[\begin{array}{c}{x}_p\left(l\right)+v_n\mathrm{\&Delta;}t\cos \left({\mathrm{\&theta;}}_p\right(l)+{\mathrm{\&Delta;\&theta;}}_n)\sin \left({\mathrm{\&psi;}}_p\right(l)+{\mathrm{\&Delta;\&psi;}}_n)\\ y_p\left(l\right)+v_n\mathrm{\&Delta;}t\cos \left({\mathrm{\&theta;}}_p\right(l)+{\mathrm{\&Delta;\&theta;}}_n)\cos \left({\mathrm{\&psi;}}_p\right(l)+{\mathrm{\&Delta;\&psi;}}_n)\\ z_p\left(l\right)+v_n\mathrm{\&Delta;}t\sin \left({\mathrm{\&theta;}}_p\right(l)+{\mathrm{\&Delta;\&theta;}}_n)\\ {\mathrm{\&psi;}}_p\left(l\right)+{\mathrm{\&Delta;\&psi;}}_n\\ {\mathrm{\&theta;}}_p\left(l\right)+{\mathrm{\&Delta;\&theta;}}_n\end{array}\right]---\left(8\right)$

Wherein, x_p,y_p,z_pIt is respectively the position that particle is current, ψ_p,θ_pFor the Velocity Azimuth angle that particle is current；v_nIt is to add at random Speed after process noise, Δ ψ_nIt is the increment adding the azimuth ψ after stochastic process noise, Δ θ_nIt is to add stochastic process to make an uproar The increment of the azimuth angle theta after sound；

If reaching observation cycle, environment road sign is detected by airborne laser radar, it is thus achieved that in laser radar detection distance All environment road sign observed quantities and environment road sign sequence, the environment road sign sequence that storage has detected, if detection for the first time The environment road sign arrived, environment road sign observed quantity is stored in new road sign collection z_nIn, if not the environment road sign detected for the first time, then Environment road sign observed quantity is stored in non-new road sign z_fIn；Otherwise, unmanned plane predicted state is regained；

2) environment road sign is updated；

If not new road sign collection z_fIt is not empty set, to non-new road sign collection z_fIn environment road sign, the prediction of computing environment road sign observed quantity Value, first provides intermediate variable dx, dy, dz, d and d₁As follows:

$\begin{array}{c}dx=x_f-x_p,dy=y_f-y_p,dz=z_f-z_p\\ d=\sqrt{{\mathrm{dx}}^2+{\mathrm{dy}}^2+{\mathrm{dz}}^2},d_1=\sqrt{{\mathrm{dx}}^2+{\mathrm{dy}}^2}\\ {\hat{z}}_t={\&lsqb;d,\arctan \left(\frac{dy}{dx}\right)-\mathrm{\&psi;},\arctan \left(\frac{dz}{d_1}\right)-\mathrm{\&theta;}\&rsqb;}^T\end{array}---\left(9\right)$

Wherein x_p,y_p,z_pIt is the location components in particle state value,For the predictive value of environment road sign observed quantity, x_f,y_f,z_fRespectively For non-new road sign collection z_fThree shaft position coordinates of middle environment road sign；

Use EKF that average and the variance of environment road sign are updated:

$\begin{array}{c}{K}_t=P_{f,t-1}{H}_f{(R_t+H_f{P}_{f,t-1}{H_f}^T)}^{-1}\\ {\mathrm{\&mu;}}_t={\mathrm{\&mu;}}_{t-1}+K_t(z_t-{\hat{z}}_t)\\ P_{f,t}=(I-K_t{H}_f)P_{f,t-1}\end{array}---\left(10\right)$

Wherein, lower footnote t represents that t, lower footnote f represent and is directed to non-new road sign collection；R_tCovariance square for observation noise Battle array, K_tIt is the filtering gain matrix of Kalman filtering, H_fFor observational equation (2) to environment road sign coordinate (x_f,y_f,z_f) Jacobi Matrix, I be dimension be the unit matrix of 3, P_f,tIt is the covariance matrix of environment road sign, μ_tFor the average of environment road sign, z_tFor environment Road sign observed quantity；

Described Jacobian matrix H_fIt is expressed as:

$H_f=\left[\begin{array}{ccc}\frac{dx}{d}& \frac{dy}{d}& \frac{dz}{d}\\ -\frac{dy}{d_1^2}& \frac{dx}{d_1^2}& 0\\ -\frac{dxdz}{d^2{d}_1}& -\frac{dydz}{d^2{d}_1}& \frac{d_1}{d^2}\end{array}\right]---\left(11\right)$

Definition: L_t=H_fP_f,tH_f ^T+R_t (12)

The weight coefficient of kth particleAs follows:

${\mathrm{\&omega;}}_t^{\&lsqb;k\&rsqb;}={\left|2{\mathrm{\&pi;L}}_t^{\&lsqb;k\&rsqb;}\right|}^{-1/2}\exp (-\frac{1}{2}{\left(z-{\hat{z}}_t^{\&lsqb;k\&rsqb;}\right)}^T(L_t^{\&lsqb;k\&rsqb;}\left)\right(z-{\hat{z}}_t^{\&lsqb;k\&rsqb;}\left)\right)---\left(13\right)$

Subscript [k] represents kth particle, and k scope is 1～N, and z is the unmanned plane observation vector to environment road sign；

If new road sign collection z_nIt is not empty set, shows to observe new environment road sign, then need to extend environmental map；

If the state vector of kth particle is x^[k]=[x_p ^[k],y_p ^[k],z_p ^[k],ψ_p ^[k],θ_p ^[k]]^T, for new road sign collection z_nIn sight Measure z=[r, α, β]^T, definition intermediate variable:

$\begin{array}{c}s\_phi=\sin (\mathrm{\&alpha;}+{\mathrm{\&psi;}}^{\&lsqb;k\&rsqb;})\\ c\_phi=\cos (\mathrm{\&alpha;}+{\mathrm{\&psi;}}^{\&lsqb;k\&rsqb;})\\ s\_thi=\sin (\mathrm{\&beta;}+{\mathrm{\&theta;}}^{\&lsqb;k\&rsqb;})\\ c\_thi=\cos (\mathrm{\&beta;}+{\mathrm{\&theta;}}^{\&lsqb;k\&rsqb;})\\ \mathrm{\&Delta;}x=r*c\_thi*c\_phi\\ \mathrm{\&Delta;}y=r*c\_thi*s\_phi\\ \mathrm{\&Delta;}z=r*s\_thi\end{array}---\left(14\right)$

Order

x_f=[x_p ^[k]+Δx,y_p ^[k]+Δy,z_p ^[k]+Δz]^T (16)

P_f,t=H_z*R_t*H_z ^T (17)

P_f,tIt is the covariance matrix of newly-increased environment road sign, x_fThe 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 x-axis component and the particle state of environment road sign estimated value, and Δ y is that environment road sign is estimated The y-axis component of evaluation and the difference of the y-axis component of particle state, Δ z is z-axis component and the 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 kth particleIt is now to calculate the homogenization weight system of each particle Number, the normalized weight coefficient of kth particle is as follows:

Calculate number of effective particlesEffective population threshold N is set_minIf, Neff < N_min, then carry out Resampling steps, obtains particle state average and then acquisition unmanned plane location estimation after resampling；

Otherwise need not resampling；

Step 3, uses arc tangent Nonlinear Tracking Differentiator to obtain velocity estimation.

A kind of UAV Landing method based on FastSLAM algorithm the most according to claim 1, it is characterised in that: step The step of resampling described in two is as follows:

Generate the uniform random number between N number of 0 to 1, be set to select [1] ..., select [j] ..., select [N], if kth The weight coefficient ω of individual particle^[k]> select [j], all meet formula ω^[k]> select [j] subscript j constitute set j；Definition Sampling array sap [1]～sap [N], be initialized as 0, then kth particle is designated as gathering element disposal in j under sampling array For k, in like manner, all particle weights coefficients compared with uniformly random array and carries out aforesaid operations, obtaining final hits Group, the particle numbering after resampling is sampling array sequence.

A kind of UAV Landing method based on FastSLAM algorithm the most according to claim 1, it is characterised in that: step Arc tangent Nonlinear Tracking Differentiator described in three is as follows:

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_1\left(t\right)=x_2\left(t\right)\\ {\stackrel{\&CenterDot;}{x}}_2\left(t\right)=R^2(-a_1\arctan (l_1\left(x_1\left(t\right)-v\left(t\right)\right))-a_2\arctan (l_2{x}_2\left(t\right)/R\left)\right)\end{array}\right.---\left(18\right)$

Wherein a₁>0,a₂>0,l₁>0,l₂> 0, R > 0 it is design parameter, v (t) is the speed of the unmanned plane t of input, x₁(t) and x₂T () is estimation and the estimation of velocity differentials of input speed respectively.