CN108520233A - A kind of extension zonotopes collection person Kalman mixed filtering methods - Google Patents

Abstract

The present invention proposes a kind of extension zonotopes collection person Kalman mixed filtering methods, the optimal filter for carrying out system state variables for nonlinear system model calculates, Taylor series Polynomial Expansion is implemented to approach nonlinear system function, obtains system linearization equivalence model；Focus on handling the high-order truncation errors of linearisation operation, the higher order term error of Taylor series linearisation is utilized zonotopes approximation computation, carries out zonotopes set-membership filtering and calculates；The Gaussian noise of system state variables is calculated still with traditional Kalman filter, realizes that zonotopes and Kalman mixed filterings calculate.Present invention improves non-linear system status variable parameter optimal estimation precision and system-computed stability to compare via SLAM system simulation experiments with conventional Extension Kalman filter algorithm, and the present invention has preferable calculating advantage and calculates efficiency.

Description

A kind of extension zonotopes collection person Kalman mixed filtering methods

Technical field

The present invention relates to the technical fields of navigational guidance and control in air line information processing science, more particularly to one kind Zonotopes collection person's Kalman mixed filtering methods are extended, autonomous mobile robot positioning immediately and map structure are can be applied to It builds in system (Simultaneous Localization And Mapping, SLAM) problem, realizes SLAM SYSTEM ERROR MODELs The optimal filter of state parameter calculates.

Background technology

Estimation problem is divided into two major classes type：One type is to assume method based on random noise, such as Kalman filter and expansion Kalman filter algorithm is opened up, such methods require known to noise statistics or known to part of properties；Another kind of is to be based on making an uproar Sound statistical property it is unknown but its demarcation known to (Unknown But Bounded, UBB) situation, most study is exactly to collect at present Member's filtering (Set-Membership Estimation, SME) theory and algorithm, this kind of filtering algorithm, which requires nothing more than system noise, to be had Boundary, and the priori in relation to noise statistics that need not know for sure.For nonlinear filtering theoretical algorithm problem, it is wide It is general to be present in numerous science and Practical Project field, it needs according to non-linear system status equation and observational equation, according to choosing The optimal estimation that fixed estimation criterion carries out non-linear system status variable calculates, and Kalman filter theory is only applicable to linearly Thus system proposes that the Kalman filter algorithm based on Taylor series expansions, core are exactly to apply nonlinear system model Linearisation equivalence model equation obtain filter covariance matrix and Kalman gain matrixs, but use single order extend line Property EKF algorithm computational accuracies it is very poor, thus there has been proposed UT Unscented transforms theory come approach non-linear system status become The posterior probability density of amount, to construct UKF algorithms.UKF algorithms require known system refine model, and require be The statistical property for state variable of uniting is accurately known, this is difficult to obtain in engineering.With as the UKF classes of algorithms also have CDKF algorithms, CKF and GHKF algorithms etc..And set-membership filtering theory then provides the effective way of such issues that processing a kind of, it is system noise The sound even uncertainty description of system model are the additive property noise of unknown distribution but bounded, compared with traditional filtering algorithm, System noise bounded is required nothing more than, without knowing the accurate statistical property in relation to noise, therefore widely applicable and strong robustness.Mesh Before, set-membership filtering algorithm has been widely applied to the fields such as state estimation, parameter identification and PREDICTIVE CONTROL.

What set-membership filtering rationale system feasible set used, which approaches shape, can be divided into ellipsoid collection person algorithm, box collection person Zonotopes collection person's algorithm of algorithm, hyperpolyhedron collection person algorithm and newly-developed.Traditional set-membership filtering algorithm is general Processing linear system, but it and classical Kalman filter it is theoretical as, it is also desirable to handle non-linear in practical application System.

Invention content

For the existing higher technical problem of filtering method computation complexity, the present invention proposes a kind of more born of the same parents of extension holohedral symmetry Shape collection person's Kalman mixed filtering methods handle the high-order truncation errors of linearisation operation, the linearisation of Taylor series Higher order term error utilize zonotopes set-membership filtering approximation computation, for system state variables Gaussian noise still with Traditional Kalman filter calculates, and realizes SLAM SYSTEM ERROR MODEL state parameter optimal filters, improves computational efficiency.

In order to achieve the above object, the technical proposal of the invention is realized in this way：A kind of extension zonotopes collection Member's Kalman mixed filtering methods, its step are as follows：

Step 1：The random spatial model of discretization of SLAM mission nonlinears, including state equation and observational equation are established, It is initially state variable；

Step 2：Linearisation etc. is implemented to the model equation of SLAM mission nonlinears based on Taylor series polynomial nature Valence converts, and obtains the equivalent linearization system model equation of equivalence SLAM system linearizations.

Step 3：Zonotopes parameter, system noise variance and the sight of known kth step system stochastic regime variable Noise variance and its covariance are surveyed, it is real to the generating operator matrix of the zonotopes of the system stochastic regime variable of kth step Dimensionality reduction calculating is applied, determines the variance matrix of zonotopes；K=1,2,；

Step 4：The prediction for carrying out zonotopes calculates, and calculates zonotopes collection person's Kalman mixed filterings Device gain matrix；

Step 5：The zonotopes observation update of development system stochastic regime variable calculates, and obtains system random like The central value updates of the zonotopes of state variable, generating operator matrix update, zonotopes varivance matrix are more The update of new and Gaussian noise varivance matrix calculates, to complete zonotopes collection person's Kalman mixed filtering iteration meters Calculation process.

The random spatial model of the discretization of the SLAM systems is：

Wherein,It is the n of kth step_xTie up system state variables,It is the n of kth step_zSystematic observation is tieed up to become Amount,WithSystem Gaussian process noise and observation noise, and v={ v are indicated respectively_k}_k∈NWith w={ w_n}_n∈N All it is zero-mean independence or combines independently of system state variables initial value x₀, system state variables x_kShow as Markov Chain Process, (x_k,y_k) it is a hidden equine husband chain process with independent noise, f () and g () indicate system model respectively Nonlinear dynamic process function and observation function；

Nonlinear system state variables are made of zonotopes and gaussian random noise, are expressed as：X=c+z+ G, z ∈ (<0,R>),C indicates that the center of zonotopes, z indicate that the linearisation operation of Taylor series obtains High-order remainder error, R indicate zonotopes form matrix operator, g indicate stochastic regime variable Gaussian error item, Q indicates Gaussian noise variance matrix, then system stochastic regime variable can be expressed as gatheringSystem initial shape State variable：x₀=c₀+z₀+g₀, z₀∈(<0,R₀>),

Taylor series Polynomial Expansions are implemented to the model equation of discrete non-linear SLAM systems, can be obtained：

Wherein,Indicate the partial differential operator of nonlinear function,Δ x=x-x_k, E_g,kv_g,kItem indicates that system state variables meet the system random noise of Gaussian Profile, E_g,kExpression system Gaussian noise matrix, and Gaussian noise v_g,kMeetF_g,kw_g,kItem indicates that observation vector meets the observation random noise of Gaussian Profile, F_g,kIndicate observation Gaussian noise matrix, and Gaussian noise w_g,kMeet

Arrange the equivalent linearization system model equation that above formula obtains LPV structures：

Wherein, noise varianceObey zonotopes distribution characteristics, state-transition matrix A_kFor：A_k =A_0,k+Δ_x,Uncertainty δ is in a n_δIn the hypersphere of dimension；E_z,kAnd v_z,kTable respectively Show the system uncertain noise matrix and its zonotopes noise of zonotopes description；Observe transfer matrix C_kMeet C_k=C_0,k+Δ_y,Uncertainty ε is in a n_εIt ties up on hypersphere；F_z,kAnd w_z,kIt indicates respectively The observation vector uncertain noise matrix and its zonotopes observation noise of zonotopes description.

Using Luenberger Observer Structures, observation gain matrix G is introduced_k, observational equation can get：

It is hereby achieved that every more new-standard cement in stochastic variable is：

c_k+1=(A_k-G_kC_k)c_k+G_ky_k,

z_k+1=(A_k-G_kC_k)z_k+(E_z,kv_z,k-G_kF_z,kw_z,k),

g_k+1=(A_k-G_kC_k)g_k+(E_g,_kv_g,_k-G_kF_g,_kw_g,_k)；

So stochastic variable is updated to：

Wherein,

R_k+1=[(A_k-G_kC_k)(↓_qR_k), E_z,k,-G_kF_z,k],

It is expressed as to the system stochastic variable of+1 step of kth

The system state variables of known kth step in the step 3：x_k=c_k+z_k+g_k, and it meets

Zonotopes variance is：

To zonotopes operator matrix R_kImplement dimensionality reduction calculating：↓ q indicates dimensionality reduction operator；If Operator Moment Battle array R_kThere are p column vectors, successively decreases by row and be decomposed into R_k=[r_1,k,…,r_j,k,…,r_p,k], | | r_j,k||₂≥||r_j+1,k||₂；If p≤ Q, then ↓ qR_k=R_k, otherwise ↓ qR_k=[R_{＞, k},b(R_{＜, k})], wherein R_{＞, k}=[r_1,k,…,r_q-_n,k], R_{＜, k}= [r_q-_n+1,k,…,r_p,_k]；

And then zonotopes variance matrix is reduced to：

Calculate the zonotopes error variance and Gaussian noise variance matrix in equivalent linearization system model equation：

Q_v,k=(1- η) Q_vz,k+ηQ_vg,k

Q_w,k=(1- η) Q_wz,k+ηQ_wg,k；

Wherein,η∈(0, 1] it is scale factor.

The maximum number of lines of q representing matrixes ↓ qR in the dimensionality reduction operator ↓ q meetsIf generating operator matrix R is represented by R=[r₁,r₂,…r_j,…,r_p], and according to | | r_j||₂≥||r_j+1||₂It is arranged to make up, if p≤q, ↓ qR=R；It is no Then ↓ qR=[R_＞,b(R_＜)], wherein R_＞=[r₁,r₂,…,r_q-n], R_＜=[r_q-n+1,r_q-n+2,…,r_p]。

The optimal method for observing gain matrix G* is calculated in the step 4 is：

It solvesMulti-goal optimizing function J_k=(1- η) J_z,k+ηJ_g,k, whereinJ_g,_k=tr (Q_k) respectively indicate zonotopes variance matrix and Gaussian noise variance square Battle array；

It acquires：Wherein,

Zonotopes observation update is calculated as calculating observation error vector implementation predicted operation meter in the step 5 It calculates：ε_k=y_k-C_kc_k；Zonotopes Center Prediction operates：State-transition matrix predicted operation：Zonotopes dimension dilation procedure：

System stochastic regime variable update operates：The update of zonotopes center calculates：It is complete right Claim the operation of polytope generating operator matrix update：Systematic variance matrix update calculating operation：Gaussian noise variance matrix update operates：

The final filtering for obtaining+1 step of kth calculates data：c_k+1、R_k+1、P_k+1And Q_k+1, complete iterative process.

Beneficial effects of the present invention：The optimal filter for carrying out system state variables for nonlinear system model calculates, real Taylor series Polynomial Expansion has been applied to approach nonlinear system function, has obtained system linearization equivalence model；Focus on to line The high-order truncation errors processing of propertyization operation, is approached the higher order term error that Taylor series linearizes using zonotopes It calculates, carries out zonotopes set-membership filtering and calculate；For system state variables Gaussian noise still with traditional Kalman filter calculates, and realizes that zonotopes and Kalman mixed filterings calculate.The present invention is based on the system model sides SLAM The nonlinear case of journey obtains SLAM system equivalence inearized model equations using Taylor series expansions, and wherein high-order blocks Error term is defined as zonotopes situation, to which SLAM system state variables are defined as zonotopes error and height This noise error algebraical sum；Optimal filter gain matrix, zonotopes error variance are obtained by introducing scale factor Matrix and Gaussian noise varivance matrix, and then carry out zonotopes collection person's Kalman mixed filterings and calculate, to reach To the requirement for improving non-linear system status variable parameter optimal estimation precision and system-computed stability.It is imitative via SLAM systems True experiment, and compared with conventional Extension Kalman filter algorithm, demonstrate the calculating advantage and calculating efficiency of the present invention.

Description of the drawings

In order to more clearly explain the embodiment of the invention or the technical proposal in the existing technology, to embodiment or will show below There is attached drawing needed in technology description to be briefly described, it should be apparent that, the accompanying drawings in the following description is only this Some embodiments of invention for those of ordinary skill in the art without creative efforts, can be with Obtain other attached drawings according to these attached drawings.

Fig. 1 is the flow chart of the present invention.

Fig. 2 is application example of the present invention in robot SLAM simulated environment scene sets, describes acquisition of the present invention Carrier movement smoothed curve and actual motion track comparison diagram, curve is as shown in caption in figure.

Fig. 3 is the robot location's error and robot x and y both direction that the present invention obtains in robot SLAM systems Standard deviation data curve.

Fig. 4 is to utilize EKF filter algorithm calculating robot SLAM system smooth motion trajectories curves and true fortune Dynamic rail trace curve comparison diagram.

Fig. 5 is to utilize EKF filter algorithm calculating robot SLAM system position errors and the directions robot x and y Standard deviation data curve.

Specific implementation mode

Following will be combined with the drawings in the embodiments of the present invention, and technical solution in the embodiment of the present invention carries out clear, complete Site preparation describes, it is clear that described embodiments are only a part of the embodiments of the present invention, instead of all the embodiments.It is based on Embodiment in the present invention, those of ordinary skill in the art are obtained every other under the premise of not making the creative labor Embodiment shall fall within the protection scope of the present invention.

A kind of extension zonotopes collection person Kalman compound filter methods, for non-linear system status spatial mode The nonlinear function of type implements Taylor series expansions, to which system state variables or observational variable are described as Gaussian noise Part and Taylor series higher order terms are defined as uncertain linearisation noise section, and then carry out to nonlinear system and linearize Kalman filter calculates and zonotopes set-membership filtering calculates.Consider to design a kind of novel filtering in this calculating process Gain modulation algorithm, to realize that the accurate square to system mode function calculates, final acquisition non-linear system status variable Optimal filter result.

In order to complete the design objective of the present invention, discussion is made to the mathematical tool used in the present invention first.The present invention The higher order term minor of nonlinear system function is described using a kind of zonotopes of novel n-dimensional space, holohedral symmetry is more Born of the same parents' Shape definition is：

<c,R>=c+Rs, | | s | |_∞≤1} (1)

Wherein, variableIndicate the center of zonotopes, generator matrixIndicate that the unit tieed up by p surpasses Ball [- 1,1]^pLinear Mapping defines the polyhedron set come, and indicates that the holohedral symmetry that a center is zero is more if c=0 Born of the same parents' shape.Zonotopes meet following several calculating properties：Minkowski and calculating：

Linear Mapping calculates：There are a matrixes

L⊙<c,R>=<Lc,LR> (3)；

In addition, zonotopes meet a critical nature,B (R)=diag (| R | I), I For unit matrix.This property illustrate zonotopes can be closed in one byThe alignment box of definition In son, but this property be applied to outside zonotopes delimit approach in it is overly conservative, therefore use in the present invention A kind of dimensionality reduction operator computational methods.Dimensionality reduction operator definitions are symbol ↓ q, wherein the maximum number of lines of q representing matrixes ↓ qR meetsIf matrix R is represented by：R=[r₁,r₂,…r_j,…,r_p], and according to | | r_j||₂≥||r_j+1||₂Arrange structure At, if p≤q, ↓ qR=R；Otherwise ↓ qR=[R_＞,b(R_＜)], wherein R_＞=[r₁,r₂,…,r_q-n], R_＜=[r_q-n+1, r_q-n+2,…,r_p]。

For zonotopes<c,R>Its variance is defined as：

Cov(<c,R>)=RR^T(4)；

Wherein, Cov () indicates that variance calculates.The Frobenius models of matrix R can be utilized by describing zonotopes size It counts to indicate, referred to as F- radiuses：

Wherein, tr () indicates the mark of calculating matrix.

For nonlinear system function model, the stochastic state space model with SLAM system problem discretizations is：

Wherein,It is the n of kth step_xTie up system state variables,It is the n of kth step_zSystematic observation is tieed up to become Amount,WithSystem Gaussian process noise and observation noise, and v={ v are indicated respectively_k}_k∈NWith w={ w_n}_n∈N All it is zero-mean independence or combines independently of system state variables initial value x₀, system state variables x_kShow as Markov Chain (Markov Chain, MC) process, (x_k,y_k) it is a hidden equine husband chain (Hidden Markov with independent noise Chain, HMC-IN) process, f () and g () indicate the nonlinear dynamic process function and observation function of system model respectively.

Assuming that non-linear system status variable is made of zonotopes and gaussian random noise, one can be described as SetWherein, c indicates that zonotopes center, R indicate the form matrix operator of zonotopes, Q It indicates Gaussian noise variance matrix, is described as using formula：X=c+z+g, z ∈ (<0,R>),So system shape The initial value of state variable is represented by

Assuming that in system state variables x_kPlace obtains in nonlinear system model first using Taylor series expansion methods Non-linear process function linearization approaches expression formula：

Wherein,Indicate the partial differential operator of nonlinear function,Δ x=x-x_k, E_g,kv_g,kItem indicates that system state variables meet the system random noise of Gaussian Profile, E_g,kExpression system Gaussian noise matrix, and Gaussian noise v_g,kMeetNonlinear function is done in linearisation operating process using formula (7), can be obtained Jacobi (Jacobian) matrix of certainty single order, and remaining higher order term may be considered in system model linearisation Indeterminate, it is possible thereby to obtain the LPV equations of structure of non-linear system status equation：

x_k+1=A_kx_k+E_z,kv_z,k+E_g,kv_g,k (8)

Wherein, noise varianceZonotopes distribution characteristics is obeyed, considers system model linearisation In uncertainty, state-transition matrix A_kIt can be described as：A_k=A_0,k+Δ_x,It is uncertain δ is measured in a n_δIn the hypersphere of dimension, to may know that it is inaccurately known that the uncertainty of Δ results in state-transition matrix. E_z,kAnd v_z,kThe system uncertain noise matrix and its zonotopes noise of zonotopes description are indicated respectively.

Same method can obtain the LPV structure lienarized equations of systematic observation equation,

y_k=C_kx_k+F_z,kw_z,k+F_g,kw_g,k (9)

Wherein, observation transfer matrix C_kMeet C_k=C_0,k+Δ_y,Uncertainty ε is one A n_εIt ties up on hypersphere.F_z,kAnd w_z,kThe observation vector uncertain noise matrix of zonotopes description and its complete is indicated respectively Simple polytope observation noise.

Consideration system stochastic variable belongs to zonotopes and Gaussian Mixture variable, is expressed as a setAnd stochastic variable can be expressed as zonotopes central value and polytope error and Gaussian error item algebraically With：

Using Luenberger Observer Structures, observer gain G is introduced, can be obtained by formula (8) and (9)：

Wherein, G_kThe optimal observation gain matrix of kth step is indicated, it is hereby achieved that every update table in stochastic variable It is up to formula：

c_k+1=(A_k-G_kC_k)c_k+G_ky_k (12)

z_k+1=(A_k-G_kC_k)z_k+(E_z,kv_z,k-G_kF_z,kw_z,k) (13)

g_k+1=(A_k-G_kC_k)g_k+(E_g,kv_g,k-G_kF_g,kw_g,k) (14)

So stochastic variable expression formula (10) is updated to：

Wherein,

R_k+1=[(A_k-G_kC_k)(↓_qR_k), E_z,k,-G_kF_z,k] (16)

It is represented by the system stochastic variable of+1 step of kth

The nonlinear system model linearisation operation of the present invention causes the uncertain error of system model and Gaussian noise to miss Difference is needed to observing gain matrix G_kCombined optimization measure is made, the multiple-objection optimization for thus proposing observation gain matrix is calculated Method, multi-goal optimizing function are defined as：

J_k=(1- η) J_z,k+ηJ_g,k (18)

Wherein,J_{G, k}=tr (Q_k) respectively indicate zonotopes variance matrix and Gaussian noise variance matrix, scale operator η meet η ∈ (0,1], if η=0, in system only contain zonotopes error , so that it may the estimation to be carried out system state variables using zonotopes set-membership filtering algorithm is calculated；If η=1, system In only include Gaussian noise error term, so that it may estimated with carrying out system state variables using traditional EKF filter algorithm Count calculating process；Consider that two kinds of error sources calculate influence to the filtering of system state variables otherwise needing to combine, then Optimum gain matrixCalculating formula can be expressed as

Wherein,Q_x,k=(1- η) P_k+ηQ_k, Q_w,k=(1- η) Q_wz,k+η Q_wg,k, Q_vw,k=(1- η) Q_vzwz,k+ηQ_vgwg,k,

In summary rudimentary knowledge, the present invention carry out Taylor series Polynomial Expansions behaviour for nonlinear system model Make, obtain the equivalent linearity model equation of nonlinear system model, is zonotopes system state variables error separate The uncertain error item and process noise error term of description, to obtain system state variables mixing expression, introduce scale because Son two kinds of error terms of description construct a kind of zonotopes collection person Kalman mixed filterings to observing the influence of gain Method carries out the iteration meter that parity price Linear system model equation implements zonotopes collection person's Kalman mixed filtering methods It calculates, obtains the filtering data of system state variables, further improve SLAM system state variables optimal estimation of parameters to reach Required precision.Specific implementation step is：Initially set up the state equation and observational equation of SLAM system problems；Carry out SLAM systems The equivalent linearization map function of system obtains SLAM system equivalence transformation model equations, and then carries out the complete of state variable parameter Simple polytope collection person's Kalman mixed filterings calculate, and the optimal filter result that iterative calculation acquisition T steps are walked via n=T is estimated Count mean value m_T|TWith estimate variance matrix P_T|T, the system status parameters variable to complete equivalence transformation SLAM system problems estimates Count calculating task.

A kind of extension zonotopes collection person Kalman compound filter methods, as shown in Figure 1, its specific implementation step For：

Step 1：The random spatial model of discretization of SLAM mission nonlinears, including state equation and observational equation are established, It is initially state variable.

The spatial model for building SLAM systems is formula (6), and parameter according to formula (15) as previously mentioned, and be initially state Variable：

Meet

Step 2：Linearisation etc. is implemented to the model equation of SLAM mission nonlinears based on Taylor series polynomial nature Valence converts, and obtains the equivalent linearization system model equation of equivalence SLAM system linearizations.

Taylor series Polynomial Expansions are implemented to the model equation of Discrete Nonlinear SLAM systems, can be obtained：

And then formula (21) is arranged, the equivalent linearization system model equation of LPV structures is obtained,

In the system model linearisation operation for containing zonotopes description in equivalent linearization system model equation High-order indeterminate error v_z,kAnd w_z,kAnd the Gaussian noise error term v separated_g,kAnd w_g,k, parameter characteristic is such as It is noted earlier.

Step 3：The zonotopes parameter and system noise variance of the system stochastic regime variable of known kth step with And observation noise variance and its covariance, to the generating operator square of the zonotopes of the system stochastic regime variable of kth step Battle array implements dimensionality reduction calculating, determines the variance matrix of zonotopes.

Assuming that the system state variables of known kth step：

x_k=c_k+z_k+g_k (23)

And it meetsZonotopes variance is：

To zonotopes operator matrix R_kImplement dimensionality reduction calculating：

And then it obtains

Calculate the zonotopes error variance and Gaussian noise variance matrix in equivalent linearization system model equation：

Q_v,k=(1- η) Q_vz,k+ηQ_vg,k (30)

Q_w,k=(1- η) Q_wz,k+ηQ_wg,k (31)

Step 4：The prediction for carrying out zonotopes calculates, and calculates zonotopes collection person's Kalman mixed filterings Device gain matrix.

Calculate optimal observation gain matrix G^*,

Wherein,Matrix Q subscripts are different, are meant that the same , in addition subscript x indicates the noise variance for variable x.

Step 5：The zonotopes observation update of development system stochastic regime variable calculates, and obtains system random like The central value updates of the zonotopes of state variable, generating operator matrix update, zonotopes varivance matrix are more The update of new and Gaussian noise varivance matrix calculates, to complete zonotopes collection person's Kalman mixed filtering iteration meters Calculation process.

Calculating observation error vector implements predicted operation calculating：

ε_k=y_k-C_kc_k (33)

And zonotopes Center Prediction operation：

With state-transition matrix predicted operation：

With zonotopes dimension dilation procedure：

System state variables update operation：The update of zonotopes center calculates：

Zonotopes generating operator matrix update operates：

Systematic variance matrix update calculating operation：

Gaussian noise variance matrix update operates：

The final filtering for obtaining+1 step of kth calculates data：c_k+1、R_k+1、P_k+1And Q_k+1, complete iterative process.

Specific example：The SLAM problems for considering robot motion's carrier can provide carrier fortune under cartesian coordinate system Dynamic equation is：

Here the state vector of SLAM systems is x_k=[x_k,y_k,φ_k]^T, respectively indicate kth step carrier positions coordinate and Orientation；V is bearer rate, and G indicates that carrier steering angle, parameter WB indicate carrier wheelspan, noise vector v_kIt is Gaussian process noise, v_k~N (0, Q_k), wherein Q_kIndicate noise variance.

Robot motion's carrier is equipped with distance and bearing sensor, can within the scope of azimuth ± 30 ° perceived distance Target object within the scope of 30m, it is possible thereby to which the observational equation for obtaining robot SLAM systems is：

Wherein, (r_i,x,r_i,y) it is the road sign position coordinate that sensor perceives, w_kIt is observation white noise, meets distribution w_k~N (0,R_k), wherein R_kIndicate observation noise variance.So, the initial parameter of SLAM systems is set as：Initial velocity V₀=3m/s, G=± 30 °, WB=4m, velocity standard difference σ_V=0.3m/s, steering angle standard deviation sigma_G=3 °, criterion distance difference σ_r=0.1m, side Position standard deviation sigma_B=1 °.Initial state vector x₀=0, initial variance P₀=diag { 10^-10,10^-10,10^-10}.Thus expansion emulation Work is verified, and extension zonotopes collection person Kalman mixed filterings algorithm and extension Kalman smoothing algorithms are calculated Effectiveness Comparison, as shown in Fig. 2,3,4,5.Comparison diagram 2 and Fig. 4, it is evident that the method for the present invention and robot are true in two kinds of algorithms Running orbit data fitting degree is preferable, and the fitting degree for extending Kalman smoother algorithms is more poor；From Fig. 3 and Fig. 5 It can compare and find out, calculating standard deviation of the invention is smaller, and error information curve smoothing is stablized, and extends Kalman smoothers The position error data variation that algorithm obtains is violent, or even data scatter phenomenon occurs, hence it is evident that error information is bigger, accordingly Standard deviation data is larger, carries out the emulation experiment of robot SLAM systems by using both smoother algorithms, acquisition Experimental data illustrates that calculating efficiency of the invention shows extension holohedral symmetry better than conventional extension Kalman smoothing algorithms The calculating advantage of polytope collection person's Kalman mixed filtering algorithms.

The foregoing is merely illustrative of the preferred embodiments of the present invention, is not intended to limit the invention, all essences in the present invention With within principle, any modification, equivalent replacement, improvement and so on should all be included in the protection scope of the present invention god.

Claims (8)

1. a kind of extension zonotopes collection person Kalman mixed filtering methods, which is characterized in that its step are as follows：

Step 1：The random spatial model of discretization of SLAM mission nonlinears, including state equation and observational equation are established, initially For state variable；

Step 2：Linearisation change of equal value is implemented to the model equation of SLAM mission nonlinears based on Taylor series polynomial nature It changes, obtains the equivalent linearization system model equation of equivalence SLAM system linearizations.

Step 3：Zonotopes parameter, system noise variance and the observation of known kth step system stochastic regime variable are made an uproar Sound variance and its covariance implement drop to the generating operator matrix of the zonotopes of the system stochastic regime variable of kth step Dimension calculates, and determines the variance matrix of zonotopes；K=1,2,；

Step 4：The prediction for carrying out zonotopes calculates, and calculates zonotopes collection person's Kalman compound filters and increases Beneficial matrix；

Step 5：The zonotopes observation update of development system stochastic regime variable calculates, and obtains system stochastic regime and becomes The central value updates of the zonotopes of amount, the update of generating operator matrix update, zonotopes varivance matrix and The update of Gaussian noise varivance matrix calculates, and is iterated to calculate to complete zonotopes collection person's Kalman mixed filterings Journey.

2. extension zonotopes collection person Kalman mixed filtering methods according to claim 1, which is characterized in that institute The random spatial model for stating the discretization of SLAM systems is：

Wherein,It is the n of kth step_xTie up system state variables,It is the n of kth step_zSystematic observation variable is tieed up,WithSystem Gaussian process noise and observation noise, and v={ v are indicated respectively_k}_k∈NWith w={ w_n}_n∈NAll it is Zero-mean independence is combined independently of system state variables initial value x₀, system state variables x_kMarkov chain process is shown as, (x_k,y_k) it is a hidden equine husband chain process with independent noise, f () and g () indicate the non-linear of system model respectively Dynamic process function and observation function；

Nonlinear system state variables are made of zonotopes and gaussian random noise, are expressed as：X=c+z+g, z ∈(<0, R ＞),C indicates that the center of zonotopes, z indicate what the linearisation operation of Taylor series obtained High-order remainder error, R indicate that the form matrix operator of zonotopes, g indicate the Gaussian error item of stochastic regime variable, Q Gaussian noise variance matrix is indicated, then system stochastic regime variable can be expressed as gatheringSystem initial shape State variable：x₀=c₀+z₀+g₀, z₀∈ (＜ 0, R₀>),

3. extension zonotopes collection person Kalman mixed filtering methods according to claim 1, which is characterized in that right The model equation of discrete non-linear SLAM systems implements Taylor series Polynomial Expansions, can obtain：

Wherein,Indicate the partial differential operator of nonlinear function,Δ x=x-x_k, E_{g, k}v_g,kItem indicates that system state variables meet the system random noise of Gaussian Profile, E_g,kExpression system Gaussian noise matrix, and it is high This noise v_g,kMeetF_g,kw_g,kItem indicates that observation vector meets the observation random noise of Gaussian Profile, F_g,k Indicate observation Gaussian noise matrix, and Gaussian noise w_g,kMeet

Arrange the equivalent linearization system model equation that above formula obtains LPV structures：

Wherein, noise varianceObey zonotopes distribution characteristics, state-transition matrix A_kFor：A_k=A_0,k +Δ_x,Uncertainty δ is in a n_δIn the hypersphere of dimension；E_z,kAnd v_z,kIndicate complete right respectively Claim the system uncertain noise matrix and its zonotopes noise of polytope description；Observe transfer matrix C_kMeet C_k=C_0,k +Δ_y,Uncertainty ε is in a n_εIt ties up on hypersphere；F_z,kAnd w_z,kHolohedral symmetry is indicated respectively The observation vector uncertain noise matrix and its zonotopes observation noise of polytope description.

4. extension zonotopes collection person Kalman mixed filtering methods according to claim 3, which is characterized in that profit With Luenberger Observer Structures, observation gain matrix G is introduced_k, observational equation can get：

It is hereby achieved that every more new-standard cement in stochastic variable is：

c_k+1=(A_k-G_kC_k)c_k+G_ky_k,

z_k+1=(A_k-G_kC_k)z_k+(E_z,kv_z,k-G_kF_z,kw_z,k),

g_k+1=(A_k-G_kC_k)g_k+(E_g,kv_g,k-G_kF_g,kw_g,k)；

So stochastic variable is updated to：

x_k+1=c_k+1+z_k+1+g_k+1, z_k+1∈(<0,R_k+1>),

Wherein,

R_k+1=[(A_k-G_kC_k)(↓_qR_k), E_z,k,-G_kF_z,k],

It is expressed as to the system stochastic variable of+1 step of kth

5. extension zonotopes collection person Kalman mixed filtering methods according to claim 3, which is characterized in that institute State the system state variables of known kth step in step 3：x_k=c_k+z_k+g_k, and it meets

Zonotopes variance is：

To zonotopes operator matrix R_kImplement dimensionality reduction calculating：↓ q indicates dimensionality reduction operator；If operator matrix R_k There are p column vectors, successively decreases by row and be decomposed into R_k=[r_1,k,…,r_j,k,…,r_p,k], | | r_j,k||₂≥||r_j+1,k||₂；If p≤q, that ↓ qR_k=R_k, otherwise ↓ qR_k=[R_{＞, k},b(R_{＜, k})], wherein R_{＞, k}=[r_1,k,…,r_q-n,k], R_{＜, k}=[r_q-n+1,k,…, r_p,k]；

And then zonotopes variance matrix is reduced to：

Calculate the zonotopes error variance and Gaussian noise variance matrix in equivalent linearization system model equation：

Q_v,k=(1- η) Q_vz,k+ηQ_vg,k

Q_w,k=(1- η) Q_wz,k+ηQ_wg,k；

Wherein,η ∈ (0,1] be Scale factor.

6. extension zonotopes collection person Kalman mixed filtering methods according to claim 4, which is characterized in that institute The maximum number of lines of q representing matrixes ↓ qR in dimensionality reduction operator ↓ q is stated, is metIf generating operator matrix R is represented by R =[r₁,r₂,…r_j,…,r_p], and according to | | r_j||₂≥||r_j+1||₂It is arranged to make up, if p≤q, ↓ qR=R；Otherwise ↓ qR= [R_＞,b(R_＜)], wherein R_＞=[r₁,r₂,…,r_q-n], R_＜=[r_q-n+1,r_q-n+2,…,r_p]。

7. extension zonotopes collection person Kalman mixed filtering methods according to claim 4, which is characterized in that institute It states and calculates optimal observation gain matrix G in step 4^*Method be：

It solvesMulti-goal optimizing function J_k=(1- η) J_z,k+ηJ_g,k, wherein J_g,k=tr (Q_k) respectively indicate zonotopes variance matrix and Gaussian noise variance matrix；

It acquires：Wherein,

8. extension zonotopes collection person Kalman mixed filtering methods according to claim 6, which is characterized in that institute It states zonotopes observation update in step 5 and is calculated as the implementation predicted operation calculating of calculating observation error vector：ε_k=y_k- C_kc_k；Zonotopes Center Prediction operates：State-transition matrix predicted operation： Zonotopes dimension dilation procedure：

System stochastic regime variable update operates：The update of zonotopes center calculates：The more born of the same parents of holohedral symmetry Shape generating operator matrix update operates：Systematic variance matrix update calculating operation：Gaussian noise variance matrix update operates：

The final filtering for obtaining+1 step of kth calculates data：c_k+1、R_k+1、P_k+1And Q_k+1, complete iterative process.