CN109597864A - Instant positioning and map constructing method and the system of ellipsoid boundary Kalman filtering - Google Patents
Instant positioning and map constructing method and the system of ellipsoid boundary Kalman filtering Download PDFInfo
- Publication number
- CN109597864A CN109597864A CN201811347634.9A CN201811347634A CN109597864A CN 109597864 A CN109597864 A CN 109597864A CN 201811347634 A CN201811347634 A CN 201811347634A CN 109597864 A CN109597864 A CN 109597864A
- Authority
- CN
- China
- Prior art keywords
- equation
- indicate
- state
- linear
- ellipsoid
- 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.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/20—Design optimisation, verification or simulation
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Computer Hardware Design (AREA)
- Evolutionary Computation (AREA)
- Geometry (AREA)
- General Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Feedback Control In General (AREA)
Abstract
Instant positioning and map constructing method and the system that the invention discloses a kind of based on ellipsoid boundary Kalman filtering, propose a kind of model comprising set-membership filtering and Kalman filtering feature, while considering that model is random and the uncertain factor of bounded.The model all considers bounded but uncertain noise in state transition equation and observational equation, has better uncertainty flow characteristic.It also retains the recurrence frame of stochastic uncertainty in Kalman filter, therefore the advantages of remain KF in an iterative process.In set-membership filtering frame, linearisation best should estimate that the nonlinear function on collection, only in state estimation point, obtains inearized model by minimizing the weighted square error between functional value and approximate linear function value by closing in entire state estimation collection without single by adaptive state.Reach the requirement for improving non-linear system status variable parameter optimal estimation precision and system-computed stability through the invention.
Description
Technical field
The invention belongs to navigational guidances and control field, are based on ellipsoid boundary Kalman filtering more particularly, to one kind
It is instant positioning with map constructing method and system.
Background technique
Rescue and not when the robot of an autonomous is in new environment, such as in building of collapsing
Know when being explored in environment, needs to search the route in environment using intelligent method.In this process, robot should
It is to be understood that the map of oneself current position and local environment, this problem is briefly termed as positioning and map structuring immediately
(Simultaneous Localization and Mapping, SLAM).The target of SLAM is to find machine from any initial position
The orientation of device people, while constructing the model of circumstances not known.SLAM problem can be described as: robot is in circumstances not known from one
Unknown position starts to move, and carries out self poisoning according to location estimation and map in moving process, while in self poisoning
On the basis of build increment type map, realize the autonomous positioning and navigation of robot.Therefore, the solution of many SLAM is recurrence
Type.At the position that determines robot and extraneous map, there are many uncertainties, noise jamming including sensor and are
System model error etc..In order to find optimal robot and map state estimation result under these disturbances, need to uncertain
Property makes the assumption that.
In in recent years, many novel SLAM methods are continuously emerged, but can be roughly divided into two types: the side of probability Estimation
The method of method and non-probability Estimation, wherein the method for probability Estimation occupies leading position.In these methods based on probability Estimation
In, the SLAM based on Extended Kalman filter (Extended Kalman Filter, EKF) is simple when handling nonlinear system
It is feasible, still it is widely used in land, aviation and underwater field so far, and be applied to the robot field of diversified forms.But
It is that EKF only with the Taylor expansion of single order carrys out approximate non-linear system, the precision of state estimation is limited, and discontented in noise
When sufficient Gaussian Profile is assumed, estimated result can generate obvious deviation.However set-membership filtering (the Set- in non-Probabilistic estimation
Membership Filter, SMF) provide another thinking, noise jamming and system model error can be assumed point
In a statistical property in unknown but limited region, therefore obtaining estimated result can also be limited in limited region cloth
Face.
In many cases, probabilistic bounded description is compared with random distribution hypothesis is more easily satisfied, this method examination
It is wider with face.Obviously it is compared with Kalman filtering, because requiring no knowledge about the statistical property of noise, set-membership filtering is to the general of noise
Rate distribution is simultaneously insensitive, so strong robustness.Classify according to the geometry expression of bounded domain, set-membership filtering limits not
There are mainly four types of methods for certainty: polyhedron, ellipsoid, more cell spaces and section.Polyhedron (Polytope) can obtain well
Estimated accuracy, but have the shortcomings that one very big, in high-dimensional nonlinear system, calculated load can be very big, this point
It also exists in more cell spaces (Zonotope).Ellipsoid is widely used since operation is simple, but Min of two ellipsoids can
Paderewski and be no longer ellipsoid, therefore, the popularization of related algorithm needs to solve optimization problem.
Summary of the invention
Aiming at the above defects or improvement requirements of the prior art, the present invention provides one kind is filtered based on ellipsoid boundary Kalman
Instant positioning and map constructing method and the system of wave, thus solve existing set-membership filtering and Kalman filtering instant positioning with
Accuracy in map structuring there is technical issues that.
To achieve the above object, according to one aspect of the present invention, it provides a kind of based on ellipsoid boundary Kalman filtering
It is instant positioning and map constructing method, comprising:
(1) it is disturbed, is established with the spheroid of Gaussian noise present in map structuring SLAM and bounded as positioning immediately
The nonlinear dynamical model of SLAM, wherein the nonlinear dynamical model includes state transition equation and observational equation；
(2) it closes in current elliposoidal state estimation collection by minimizing the nonlinear dynamical model and adaptive optimal control
Weighted square error between the target linear model of current elliposoidal state estimation collection obtains the target linear model,
In, the target linear model includes linear condition equation of transfer and Systems with Linear Observation equation；
(3) according to the linear condition equation of transfer, by the priori shape of the state parameter prediction subsequent time at current time
State parameter is updated the shape at current time by the observation at current time based on the Systems with Linear Observation equation and observation gain matrix
State parameter, wherein state parameter includes ellipsoid central point, Gaussian error covariance and ellipsoid error shape matrix；
(4) by the elder generation of the state parameter at updated current time, the optimal subsequent time for observing gain matrix and prediction
It tests state parameter and obtains the posteriority state parameter of subsequent time, and then positioned immediately by the posteriority state parameter and map
Building.
Preferably, the nonlinear dynamical model of the SLAM are as follows:
xk+1=fk(xk,uk,wk,ak) and yk=hk(xk,vk,bk), wherein xkIt is a n dimension state vector, ukIt is known
Input vector,It is that covariance isState transition equation Gaussian noise,It is shape
Shape matrix isUnknown but bounded spheroid disturbance, fkIndicate state transition function,It is covariance
ForObservational equation Gaussian noise,It is that form matrix isUnknown but bounded spheroid disturbance,
Subscript u and z in parameter respectively correspond state transition equation and observational equation, and k indicates moment, hkIndicate observation function, ykIt indicates
The observation at current time.
Preferably, the target linear model are as follows:
And
Wherein, Fx,kAnd Fa,kIndicate the spheroid coefficient of disturbance of unknown but bounded in state transition equation, Fw,kIndicate state transition equation
The linear coefficient of middle Gaussian error,Indicate the basic point in state transition equation, Hx,kAnd Hb,kIndicate observational equation in it is unknown but
The spheroid coefficient of disturbance of bounded, Hv,kIndicate the linear coefficient of Gaussian error in observational equation,Indicate the base in observational equation
Point.
Preferably for the linearized fashion of the state transition equation in the nonlinear dynamical model are as follows:
?Place carries out line to the state transition equation
Property, wherein
It indicates
The N number of estimation point taken in current elliposoidal state estimation set,It indicates to take in elliposoidal disturbance set
M estimation point, i=1 ..., N, j=1 ..., M, wi,jIt is preset weight,Indicate state estimation
Mean value SkThe ellipsoid set at place,Indicate ellipsoid cluster center.
Preferably for the linearized fashion of the observational equation in the nonlinear dynamical model are as follows:
?Place linearizes the observational equation, wherein It indicates current ellipse
The N number of estimation point taken in spherical state estimation set,Indicate the M estimation taken in elliposoidal disturbance set
Point, i=1 ..., N, j=1 ..., M, wi,jIt is preset weight.
Preferably, by the linear condition equation of transfer according to the posteriority ellipsoid central point at current timeGaussian error
CovarianceWith ellipsoid error shape matrixThe prior state parameter of the subsequent time of prediction are as follows:
,
Wherein,Indicate the priori ellipsoid central point of subsequent time,Indicate the priori Gaussian error association side of subsequent time
Difference,Indicate the Gaussian noise covariance in state transition equation,Indicate the priori ellipsoid error shape square of subsequent time
Battle array, tr () indicate the mark of calculating matrix, Fa,kIndicate the linear coefficient of Bounded Errors in state transition equation,Expression state
Bounded Noise form matrix in equation of transfer.
Preferably, by the Systems with Linear Observation equation according to the priori ellipsoid central point at current timeGaussian error association side
DifferenceWith spheroid-like matrixThe posteriority state parameter at the current time of prediction are as follows:
Wherein, KkTo observe gain matrix, β ∈ (0 ,+∞), y indicate observation known to current time,Indicate observation side
Journey Gaussian noise covariance, Hb,kIndicate the linear coefficient of observational equation Bounded Errors,Indicate observational equation Bounded Noise shape
Shape matrix.
Preferably, the optimal observation gain matrix are as follows: Kk(β*), wherein
WithRespectively
The size of Gaussian noise and Bounded Noise is represented, η indicates preset weight.
It is another aspect of this invention to provide that provide it is a kind of based on ellipsoid boundary Kalman filtering it is instant positioning and map
Building system, comprising:
Model construction module, for as positioning the ellipsoid with Gaussian noise present in map structuring SLAM and bounded immediately
Body disturbance, establishes the nonlinear dynamical model of SLAM, wherein the nonlinear dynamical model includes state transition equation and observation
Equation；
Linearization block, for closing in current elliposoidal state estimation collection by minimizing the Nonlinear Dynamic morphotype
Weighted square error between type and the target linear model of the current elliposoidal state estimation collection of adaptive optimal control obtains the mesh
Mark linear model, wherein the target linear model includes linear condition equation of transfer and Systems with Linear Observation equation；
Forecast updating module is used for according to the linear condition equation of transfer, by under the state parameter prediction at current time
The prior state parameter at one moment, based on the Systems with Linear Observation equation and observation gain matrix, by current time observation more
The state parameter at new current time, wherein state parameter includes ellipsoid central point, Gaussian error covariance and ellipsoid error shape
Matrix；
Posterior estimator module, for state parameter, optimal observation gain matrix and the prediction by updated current time
The prior state parameter of subsequent time obtain the posteriority state parameter of subsequent time, and then carried out by the posteriority state parameter
Immediately positioning and map structuring.
In general, through the invention it is contemplated above technical scheme is compared with the prior art, can obtain down and show
Beneficial effect:
When wider there are a kind of more reliable and adaptability when the uncertainty of unknown but bounded, is provided in SLAM system
Nonlinear system dynamic model；With standard Kalman filter the difference is that, the state of estimation be ellipsoid rather than
A single point, and each point estimated state having the same in an ellipsoid have better robustness to noise；Linearisation
Process is not single to be linearized using Taylor expansion, the non-linear letter for considering best to approach on state estimation collection
Number is without single only in state estimation point；Weight coefficient is introduced to obtain optimal filter gain matrix, ellipsoid error shape matrix,
Gaussian error covariance matrix, thus reach improve non-linear system status variable parameter optimal estimation precision and system-computed it is steady
Qualitatively require.Finally according to the SLAM system simulation experiments and conventional Extension Kalman filtering SLAM estimation knot on data set
Fruit compares, and demonstrates calculated performance superiority of the invention.
Detailed description of the invention
Fig. 1 is a kind of method flow schematic diagram based on ellipsoid boundary Kalman filtering provided in an embodiment of the present invention；
Fig. 2 is a kind of program circuit for positioning and building immediately figure applied to mobile robot provided in an embodiment of the present invention
Figure；
Fig. 3 is kinematics model schematic diagram in a kind of robot simulation environment provided in an embodiment of the present invention；
Fig. 4 is the robot estimated motion track that SKF of the invention is obtained, and observes road sign position and actual motion track,
The comparison diagram of true road sign position；
Fig. 5 is a kind of robot estimation movement rail obtained with Extended Kalman filter EKF provided in an embodiment of the present invention
Mark observes road sign position and actual motion track, the comparison diagram of true road sign position；
Fig. 6 is provided in an embodiment of the present invention a kind of SKF of the invention and expanded Kalman filtration algorithm EKF to be utilized to calculate
The direction robot x and y position, different moments are from actual position Euclidean distance correlation curve；
Fig. 7 is a kind of SKF of the invention provided in an embodiment of the present invention and expanded Kalman filtration algorithm EKF robot position
The average Euclidean distance error of estimation is set to change with time.
Specific embodiment
In order to make the objectives, technical solutions, and advantages of the present invention clearer, with reference to the accompanying drawings and embodiments, right
The present invention is further elaborated.It should be appreciated that the specific embodiments described herein are merely illustrative of the present invention, and
It is not used in the restriction present invention.As long as in addition, technical characteristic involved in the various embodiments of the present invention described below
Not constituting a conflict with each other can be combined with each other.
The invention proposes a kind of filter model, the characteristics of it contains set-membership filtering and Kalman filtering, examine simultaneously
Consider that model is random and the uncertain factor of bounded.Collection person's Kalman filtering (Set-Membership Kalman of the invention
Filter, SKF) in state transition equation and measurement equation all consider bounded but uncertain noise, therefore have more preferable
Uncertainty flow characteristic.It also retains the recurrence frame of stochastic uncertainty in Kalman filter, therefore in iteration mistake
The advantages of KF is remained in journey.In addition, linearisation best adaptive state estimation should collect non-in set-membership filtering frame
Linear function is without single only in state estimation point.Be not using estimation point at first order Taylor linearized,
But it is missed by being closed in entire state estimation collection by the weighted quadratic minimized between functional value and approximate linear function value
Difference obtains inearized model.Based on relatively reliable noise it is assumed that solving optimization problem in every step iteration to obtain more
Accurate SLAM state estimation result.
A kind of instant positioning and map constructing method, synthesis based on ellipsoid boundary Kalman filtering proposed by the present invention is examined
Bounded Perturbations present in SLAM system and Gaussian noise are considered, by approaching non-linear letter in entire state estimation set
Number obtains more accurate linear model；The recurrence frame for remaining stochastic uncertainty in Kalman filter is increased by observation
The mode of benefit obtains optimal system state variables estimated result.
In order to complete mentality of designing of the invention, some mathematical tools used are illustrated first.
One center existsN dimension spheroid can be with is defined as:
Wherein S is a positive definite matrix (S > 0), referred to as form matrix.
Known two spheroid E1=E (c1,S1) and E2=E (c2,S2), their Minkowski and is defined as:
Obviously the set after summation is not necessarily ellipsoid again, and the present invention indicates summed result with an outsourcing ellipsoid:
Available optimal outsourcing ellipsoid, takes at this time by way of minimizing markTr () table
Show the mark of calculating matrix.
Consider that mobile robot moves in circumstances not known, state variable includes two parts: robot itself pose, two dimension
It include abscissa, ordinate and rotational angle under motion conditions；The position of road sign is observed, each road sign has corresponding abscissa
And ordinate.
Referring to Fig.1, Initial state estimation center is inputted firstInitial estimation covariance matrixInitially estimate
Count spheroid-like matrix
The position and quantity for observing road sign can change as the pose of robot changes, it is assumed that in moment k, SLAM's
Nonlinear dynamical model are as follows:
xk+1=fk(xk,uk,wk,ak) (4)
yk=hk(xk,vk,bk) (5)
Wherein xkIt is a n dimension state vector, ukIt is known input vector,It is that covariance is
State transition equation Gaussian noise,It is that form matrix isUnknown but bounded spheroid disturbance,It is that covariance isObservational equation Gaussian noise,It is that form matrix isNot
Know but the spheroid of bounded disturbs.In embodiments of the present invention, the subscript u and z in parameter indicate that they respectively correspond state
Equation of transfer and observational equation, above-mentioned all symbolic representations are the information of moment k, and system initial state can be expressed as x0=c0+
r0+s0, c0Indicate original state center, r0~N (0, C0) it is Gaussian noise existing for state estimation, s0∈E(0,S0) it is to exist
Spheroid disturbance.
The Kalman filtering output of standard is usually the stochastic variable of Gaussian distributed, and it is distributed mean value and is considered as
Estimation point.In the Kalman filter based on ellipsoid boundary, obtained estimated result is one and includes be distributed mean value
Set.
According to obtained nonlinear dynamical model, the linear mould of an adaptive optimal control current state estimation collection can be obtained
Type:
First against state transition equation, in embodiments of the present invention, considerPlace is linearized.Particularly, wherein Gaussian error
Linear coefficient can be indicated using Jacobian matrix, such asIt is therein unknown but have
The coefficient of disturbance F on boundaryx,k,Fa,kAnd basic pointIt can be obtained by weighted least-squares method, process is as follows:
N number of estimation point is taken in current elliposoidal state estimation set E (x, S)It is disturbed in elliposoidal
Dynamic setIn take M estimation pointIt is defined as follows distance function:
Wherein i=1 ..., N, j=1 ..., M respectively represent nonlinear function fkWith linear approximation function in estimation point
The error at place.Related coefficient Fx,k,Fa,kAnd basic pointIt can be calculated by minimum average B configuration distance:
Wherein wi,jIt is preset weight, it can be determine according to actual needs.The solution of the above problem can be by following formulas
It indicates:
Wherein Mk,fkAnd WkIs defined as:
Wk=diag (w1,1,...,wN,M) (13)
For observational equation, considerPlace is linearized, and is obtained
To corresponding parameterParticularly, the wherein linear coefficient of Gaussian error, can use Jacobi square
Battle array is indicated, such asThe coefficient of disturbance H of unknown but bounded thereinx,k,Hb,kAnd basic pointIt can
To be obtained by weighted least-squares method, process is as follows:
N number of estimation point is taken in current elliposoidal state estimation set E (x, S)It is disturbed in elliposoidal
Dynamic setIn take M estimation pointIt is defined as follows distance function:
Wherein i=1 ..., N, j=1 ..., M respectively represent nonlinear function hkWith linear approximation function in estimation point
The error at place.Related coefficient Hx,k,Hb,kAnd basic pointIt can be calculated by minimum average B configuration distance:
Wherein wi,jIt is preset weight, it can be determine according to actual needs.The solution of the above problem can be by following formulas
It indicates:
Wherein Mk,fkAnd WkIs defined as:
Wk=diag (w1,1,...,wN,M) (19)
In moment k, the prior estimate of time of dayAnd Posterior estimatorIt is all stochastic variable, it is assumed that prior estimateExpectation and covariance be respectivelyWithPosterior estimatorExpectation and covariance be respectivelyWithIt is all
Experienced expectationCenter is formd to existAnd form matrix isEllipsoid, namelyEqually have
After obtaining linear condition equation of transfer (6), input prediction step parameter, the Posterior estimator point including current timeThe covariance of estimationAnd form matrixThe prior estimate result at next moment can be exported:
System stochastic variable belongs to spheroid and Gaussian Mixture variable, and state estimation result can express in the prediction step
Are as follows:
Wherein
According to Systems with Linear Observation equation (7) formula, the prior estimate parameter that input prediction step obtains, including prior estimate pointThe covariance of estimationAnd form matrixInput weight coefficient η can be filtered device and update step:
Introduce the weight coefficient η ∈ [0,1] of tradeoff a Bounded Noise and Gaussian noise, the Optimality Criteria letter being defined as follows
Number:
WhereinWithThe size of Gaussian noise and Bounded Noise has been respectively represented, if η=0, system
In it is only uncertain comprising Gaussian noise, ellipsoid boundary Kalman filtering at this time is just converted into traditional Kalman filtering；
As η=1, there is only Bounded Noise uncertainties for system, so that it may carry out system mode using the method for ellipsoid set-membership filtering
Variable estimation procedure.Consider interference of two kinds of noise sources simultaneously to system, problem can be converted into following optimization problem:
(25) formula of utilization, gives weight coefficient η, and adaptive optimum state observes gain matrix KkSolution can be passed throughObtained result indicates are as follows:
The mode of adoption status observer introduces observation gain matrix Kk, the Posterior estimator result of available state:
Particularly, β is the non-negative parameter that ellipsoid collection asks Minkowski and introducing, and value can according to actual needs really
It is fixed.Stochastic variable at this time can indicate are as follows:
Wherein,
Optimized parameter β can be solved by following optimization problems:
Obtain optimal parameter beta*, observe gain matrix Kk(β*), Gaussian noise covariance matrixEllipsoid noise
Form matrixη indicates preset weight, and value can be determine according to actual needs.
The final filtering for obtaining the k+1 moment calculates data:WithComplete iterative process.
Specific example:
Fig. 2, which gives, is applied to the tool that mobile robot positioned and built immediately figure based on ellipsoid boundary Kalman filtering algorithm
Body programming flowchart.
The SLAM problem for considering mobile robot plane motion can provide movement referring to Fig. 3 in cartesian coordinate system
Learn equation:
Here SLAM system mode vector is xk=[x (k) y (k) φ (k)]T, illustrate the position and side of robot
To noise vectorIt is Gaussian noise,It is unknown but bounded noise.
The speed v of robotcIt is to be measured by the encoder on rear-wheel, that is, has
veThe rear wheel rotation speed that encoder calculates, L is the distance between front and back wheel shaft, h be rear-wheel axis center and encoder away from
From b is vertical range of the rear-wheel axis center to laser sensor, and a is horizontal distance of the rear-wheel axis center to laser sensor.
Robot is by the observation road sign around laser sensor sensing, it is hereby achieved that the sight of robot SLAM system
Survey equation:
Wherein (xi,yi) indicate to observe the position of road sign, noise vectorIt is Gaussian noise,It is unknown but bounded noise.
The initial parameter of SLAM system is provided that original state is x0=[5.1048 π/180 4.2044-126]T, just
Beginning estimate covariance matrixOriginal shape matrix
Weight coefficient η=0.5；The Gaussian noise covariance matrix of state transition equation and observational equation is respectively
Form matrix is respectively
Simulation result is as shown in Figure 4, it is evident that robot SLAM motion profile and real trace are very close to and finally estimating
It counts obtained observation road sign and true road sign position deviation is smaller.Fig. 5 is with the EKF-SLAM motion profile obtained and road sign position
It sets, there are certain errors with true value.From fig. 6, it can be seen that the method for the present invention and robot really run rail in two kinds of algorithms
Mark data fitting degree is preferable, and the fitting degree of expanded Kalman filtration algorithm is more poor.In order to intuitively embody this hair
The ellipsoid boundary Kalman filtering (SKF) of bright proposition and the estimation effect difference of Extended Kalman filter (EKF) design following flat
Equal Euclidean distance error criterion:
The average Euclidean range error of two kinds of algorithms is as shown in fig. 7, demonstrate calculating efficiency of the invention better than routine
Spreading kalman algorithm.
As it will be easily appreciated by one skilled in the art that the foregoing is merely illustrative of the preferred embodiments of the present invention, not to
The limitation present invention, any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should all include
Within protection scope of the present invention.
Claims (9)
1. a kind of instant positioning and map constructing method based on ellipsoid boundary Kalman filtering characterized by comprising
(1) it is disturbed as positioning immediately with the spheroid of Gaussian noise present in map structuring SLAM and bounded, establishes SLAM's
Nonlinear dynamical model, wherein the nonlinear dynamical model includes state transition equation and observational equation；
(2) it is closed in current elliposoidal state estimation collection current by minimizing the nonlinear dynamical model and adaptive optimal control
Weighted square error between the target linear model of elliposoidal state estimation collection obtains the target linear model, wherein
The target linear model includes linear condition equation of transfer and Systems with Linear Observation equation；
(3) according to the linear condition equation of transfer, joined by the prior state of the state parameter prediction subsequent time at current time
Number is joined based on the Systems with Linear Observation equation and observation gain matrix by the state that the observation at current time updates current time
Number, wherein state parameter includes ellipsoid central point, Gaussian error covariance and ellipsoid error shape matrix；
(4) by the state parameter at updated current time, the priori shape of the optimal subsequent time for observing gain matrix and prediction
State parameter obtains the posteriority state parameter of subsequent time, and then carries out positioning immediately and map structure by the posteriority state parameter
It builds.
2. the method according to claim 1, wherein the nonlinear dynamical model of the SLAM are as follows:
xk+1=fk(xk, uk, wk, ak) and yk=hk(xk, vk, bk), wherein xkIt is a n dimension state vector, ukIt is known defeated
Incoming vector,It is that covariance isState transition equation Gaussian noise,It is shape square
Battle array beUnknown but bounded spheroid disturbance, fkIndicate state transition function,It is that anti-variance is
Observational equation Gaussian noise,It is that form matrix isUnknown but bounded spheroid disturbance, in parameter
Subscript u and z respectively correspond state transition equation and observational equation, k indicates moment, hkIndicate observation function, ykWhen indicating current
The observation at quarter.
3. according to the method described in claim 2, it is characterized in that, the target linear model are as follows:
AndWherein,
FX, kAnd FA, kIndicate the spheroid coefficient of disturbance of unknown but bounded in state transition equation, FW, kIndicate high in state transition equation
The linear coefficient of this error,Indicate the basic point in state transition equation, HX, kAnd HB, kIndicate unknown but bounded in observational equation
Spheroid coefficient of disturbance, HV, kIndicate the linear coefficient of Gaussian error in observational equation,Indicate the basic point in observational equation.
4. according to the method described in claim 3, it is characterized in that, for the state transfer side in the nonlinear dynamical model
The linearized fashion of journey are as follows:
?uk=uk, wk=0,Place linearizes the state transition equation,
Wherein, It indicates current
The N number of estimation point taken in elliposoidal state estimation set,Indicate that M taken in elliposoidal disturbance set are estimated
Enumeration, i=1 ..., N, j=1 ..., M, wI, jIt is preset weight,Indicate state estimation mean value SkInstitute
Ellipsoid set,Indicate ellipsoid cluster center.
5. the method according to claim 3 or 4, which is characterized in that for the observation side in the nonlinear dynamical model
The linearized fashion of journey are as follows:
?vk=0,Place linearizes the observational equation, wherein It indicates current ellipse
The N number of estimation point taken in spherical state estimation set,Indicate the M estimation taken in elliposoidal disturbance set
Point, i=1 ..., N, j=1 ..., M, wI, jIt is preset weight.
6. according to the method described in claim 5, it is characterized in that, by the linear condition equation of transfer according to current time
Posteriority ellipsoid central pointGaussian error covarianceWith ellipsoid error shape matrixThe priori of the subsequent time of prediction
State parameter are as follows: ,
Wherein,Indicate the priori ellipsoid central point of subsequent time,Indicate the priori Gaussian error covariance of subsequent time,
Indicate the Gaussian noise covariance in state transition equation,Indicate the priori ellipsoid error shape matrix of subsequent time, tr
() indicates the mark of calculating matrix, FA, kIndicate the linear coefficient of Bounded Errors in state transition equation,The transfer of expression state
Bounded Noise form matrix in equation.
7. according to the method described in claim 6, it is characterized in that, by the Systems with Linear Observation equation according to the priori at current time
Ellipsoid central pointGaussian error covarianceWith spheroid-like matrixThe posteriority state parameter at the current time of prediction
Are as follows: Wherein,
KkTo observe gain matrix, β ∈ (0 ,+∞), y indicate observation known to current time,Indicate observational equation Gaussian noise
Covariance, HB, kIndicate the linear coefficient of observational equation Bounded Errors,Indicate observational equation Bounded Noise form matrix.
8. the method according to the description of claim 7 is characterized in that the optimal observation gain matrix are as follows: Kk(β*), wherein WithIt respectively represents
The size of Gaussian noise and Bounded Noise, η indicate preset weight.
9. a kind of instant positioning and map structuring system based on ellipsoid boundary Kalman filtering characterized by comprising
Model construction module is disturbed for the spheroid as positioning immediately with Gaussian noise present in map structuring SLAM and bounded
It is dynamic, establish the nonlinear dynamical model of SLAM, wherein the nonlinear dynamical model includes state transition equation and observation side
Journey；
Linearization block, for current elliposoidal state estimation collection close by minimize the nonlinear dynamical model and
Weighted square error between the target linear model of the current elliposoidal state estimation collection of adaptive optimal control obtains the score
Property model, wherein the target linear model includes linear condition equation of transfer and Systems with Linear Observation equation；
Forecast updating module, for predicting lower a period of time by the state parameter at current time according to the linear condition equation of transfer
The prior state parameter at quarter is worked as based on the Systems with Linear Observation equation and observation gain matrix by the observation update at current time
The state parameter at preceding moment, wherein state parameter includes ellipsoid central point, Gaussian error covariance and ellipsoid error shape square
Battle array；
Posterior estimator module, under state parameter, optimal observation gain matrix and the prediction by updated current time
The prior state parameter at one moment obtains the posteriority state parameter of subsequent time, and then is carried out immediately by the posteriority state parameter
Positioning and map structuring.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811347634.9A CN109597864B (en) | 2018-11-13 | 2018-11-13 | Method and system for real-time positioning and map construction of ellipsoid boundary Kalman filtering |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811347634.9A CN109597864B (en) | 2018-11-13 | 2018-11-13 | Method and system for real-time positioning and map construction of ellipsoid boundary Kalman filtering |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109597864A true CN109597864A (en) | 2019-04-09 |
| CN109597864B CN109597864B (en) | 2020-10-16 |
Family
ID=65958264
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811347634.9A Active CN109597864B (en) | 2018-11-13 | 2018-11-13 | Method and system for real-time positioning and map construction of ellipsoid boundary Kalman filtering |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109597864B (en) |
Cited By (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110265054A (en) * | 2019-06-14 | 2019-09-20 | 深圳市腾讯网域计算机网络有限公司 | Audio signal processing method, device, computer readable storage medium and computer equipment |
| CN110276952A (en) * | 2019-06-27 | 2019-09-24 | 武汉中海庭数据技术有限公司 | A kind of traffic information analog acquisition method and device |
| CN110298124A (en) * | 2019-07-03 | 2019-10-01 | 江南大学 | A kind of industrial control system actuator method for parameter estimation based on filtering |
| CN110457863A (en) * | 2019-08-23 | 2019-11-15 | 江南大学 | The wind-driven generator pitch parameter of any subsystem estimation method of filtering is shunk based on ellipsoid |
| CN111427007A (en) * | 2020-04-24 | 2020-07-17 | 山东科技大学 | Mine personnel safety state estimation method based on centralized personnel filtering under incomplete measurement |
| CN111765889A (en) * | 2020-04-30 | 2020-10-13 | 江苏高科石化股份有限公司 | Pose positioning method of mobile robot in production workshop based on multi-cell-ellipsoid dual-filtering |
| CN112346461A (en) * | 2020-11-05 | 2021-02-09 | 上海海事大学 | Automatic guided vehicle collective filtering method considering unknown noise bounded characteristic |
| CN113160315A (en) * | 2021-04-16 | 2021-07-23 | 广东工业大学 | Semantic environment map representation method based on dual quadric surface mathematical model |
| CN113188444A (en) * | 2021-05-06 | 2021-07-30 | 上海航天测控通信研究所 | Uncertainty test experiment and calculation method for laser measurement system |
| CN113591020A (en) * | 2021-07-23 | 2021-11-02 | 江南大学 | Nonlinear system state estimation method based on axial symmetry box space filtering |
| CN113932815A (en) * | 2021-10-19 | 2022-01-14 | 北京京航计算通讯研究所 | Robustness optimized Kalman filtering method and device, electronic equipment and storage medium |
| CN114812636A (en) * | 2022-05-05 | 2022-07-29 | 合肥工业大学 | Local abnormal factor optimization ellipse fitting method for optical fiber vibration sensing phase demodulation |
| CN116518983A (en) * | 2023-07-05 | 2023-08-01 | 西安羚控电子科技有限公司 | Self-adaptive fusion method and device for mobile robot positioning |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101963512A (en) * | 2010-09-03 | 2011-02-02 | 哈尔滨工程大学 | Initial alignment method for marine rotary fiber-optic gyroscope strapdown inertial navigation system |
| CN103630137A (en) * | 2013-12-02 | 2014-03-12 | 东南大学 | Correction method used for attitude and course angles of navigation system |
| CN105222780A (en) * | 2015-09-07 | 2016-01-06 | 郑州轻工业学院 | A kind of ellipsoid set-membership filtering method of approaching based on Stirling interpolation polynomial |
| CN106767780A (en) * | 2016-11-28 | 2017-05-31 | 郑州轻工业学院 | Based on the extension ellipsoid set-membership filtering method that Chebyshev polynomial interopolations are approached |
| WO2018027206A1 (en) * | 2016-08-04 | 2018-02-08 | Reification Inc. | Methods for simultaneous localization and mapping (slam) and related apparatus and systems |
| CN108427282A (en) * | 2018-03-30 | 2018-08-21 | 华中科技大学 | A kind of solution of Inverse Kinematics method based on learning from instruction |
| CN108520233A (en) * | 2018-04-09 | 2018-09-11 | 郑州轻工业学院 | A kind of extension zonotopes collection person Kalman mixed filtering methods |
-
2018
- 2018-11-13 CN CN201811347634.9A patent/CN109597864B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101963512A (en) * | 2010-09-03 | 2011-02-02 | 哈尔滨工程大学 | Initial alignment method for marine rotary fiber-optic gyroscope strapdown inertial navigation system |
| CN103630137A (en) * | 2013-12-02 | 2014-03-12 | 东南大学 | Correction method used for attitude and course angles of navigation system |
| CN105222780A (en) * | 2015-09-07 | 2016-01-06 | 郑州轻工业学院 | A kind of ellipsoid set-membership filtering method of approaching based on Stirling interpolation polynomial |
| WO2018027206A1 (en) * | 2016-08-04 | 2018-02-08 | Reification Inc. | Methods for simultaneous localization and mapping (slam) and related apparatus and systems |
| CN106767780A (en) * | 2016-11-28 | 2017-05-31 | 郑州轻工业学院 | Based on the extension ellipsoid set-membership filtering method that Chebyshev polynomial interopolations are approached |
| CN108427282A (en) * | 2018-03-30 | 2018-08-21 | 华中科技大学 | A kind of solution of Inverse Kinematics method based on learning from instruction |
| CN108520233A (en) * | 2018-04-09 | 2018-09-11 | 郑州轻工业学院 | A kind of extension zonotopes collection person Kalman mixed filtering methods |
Non-Patent Citations (4)
| Title |
|---|
| BENJAMIN NOACK等: "State estimation considering negative information with switching Kalman and ellipsoidal filtering", 《IEEE》 * |
| BENJAMIN NOACK等: "Treatment of biased and dependent sensor data in graph-based SLAM", 《IEEE》 * |
| 许艳萍等: "线性系统在非线性等式约束下的集员卡尔曼滤波", 《东南大学学报》 * |
| 黄剑: "低轨空间目标雷达探测信息处理技术", 《中国博士学位论文全文数据库（信息科技辑）》 * |
Cited By (23)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110265054A (en) * | 2019-06-14 | 2019-09-20 | 深圳市腾讯网域计算机网络有限公司 | Audio signal processing method, device, computer readable storage medium and computer equipment |
| CN110265054B (en) * | 2019-06-14 | 2024-01-30 | 深圳市腾讯网域计算机网络有限公司 | Speech signal processing method, device, computer readable storage medium and computer equipment |
| CN110276952A (en) * | 2019-06-27 | 2019-09-24 | 武汉中海庭数据技术有限公司 | A kind of traffic information analog acquisition method and device |
| CN110276952B (en) * | 2019-06-27 | 2020-11-27 | 武汉中海庭数据技术有限公司 | Traffic information simulation acquisition method and device |
| CN110298124A (en) * | 2019-07-03 | 2019-10-01 | 江南大学 | A kind of industrial control system actuator method for parameter estimation based on filtering |
| CN110298124B (en) * | 2019-07-03 | 2020-10-27 | 江南大学 | Industrial control system actuator parameter estimation method based on filtering |
| CN110457863A (en) * | 2019-08-23 | 2019-11-15 | 江南大学 | The wind-driven generator pitch parameter of any subsystem estimation method of filtering is shunk based on ellipsoid |
| CN110457863B (en) * | 2019-08-23 | 2021-02-19 | 江南大学 | Parameter estimation method for wind driven generator pitch subsystem based on ellipsoid contraction filtering |
| CN111427007B (en) * | 2020-04-24 | 2021-03-19 | 山东科技大学 | Mine personnel safety state estimation method based on centralized personnel filtering under incomplete measurement |
| CN111427007A (en) * | 2020-04-24 | 2020-07-17 | 山东科技大学 | Mine personnel safety state estimation method based on centralized personnel filtering under incomplete measurement |
| CN111765889A (en) * | 2020-04-30 | 2020-10-13 | 江苏高科石化股份有限公司 | Pose positioning method of mobile robot in production workshop based on multi-cell-ellipsoid dual-filtering |
| CN111765889B (en) * | 2020-04-30 | 2024-04-16 | 江苏高科石化股份有限公司 | Pose positioning method of mobile robot in production workshop based on multi-cell-ellipsoid double filtering |
| CN112346461A (en) * | 2020-11-05 | 2021-02-09 | 上海海事大学 | Automatic guided vehicle collective filtering method considering unknown noise bounded characteristic |
| CN113160315A (en) * | 2021-04-16 | 2021-07-23 | 广东工业大学 | Semantic environment map representation method based on dual quadric surface mathematical model |
| CN113188444A (en) * | 2021-05-06 | 2021-07-30 | 上海航天测控通信研究所 | Uncertainty test experiment and calculation method for laser measurement system |
| CN113188444B (en) * | 2021-05-06 | 2023-01-13 | 上海航天测控通信研究所 | Uncertainty testing experiment and calculating method for laser measuring system |
| CN113591020A (en) * | 2021-07-23 | 2021-11-02 | 江南大学 | Nonlinear system state estimation method based on axial symmetry box space filtering |
| CN113591020B (en) * | 2021-07-23 | 2024-03-01 | 江南大学 | Nonlinear system state estimation method based on axisymmetric box spatial filtering |
| CN113932815A (en) * | 2021-10-19 | 2022-01-14 | 北京京航计算通讯研究所 | Robustness optimized Kalman filtering method and device, electronic equipment and storage medium |
| CN113932815B (en) * | 2021-10-19 | 2023-07-18 | 北京京航计算通讯研究所 | Robustness optimization Kalman filtering relative navigation method, device, equipment and storage medium |
| CN114812636A (en) * | 2022-05-05 | 2022-07-29 | 合肥工业大学 | Local abnormal factor optimization ellipse fitting method for optical fiber vibration sensing phase demodulation |
| CN116518983A (en) * | 2023-07-05 | 2023-08-01 | 西安羚控电子科技有限公司 | Self-adaptive fusion method and device for mobile robot positioning |
| CN116518983B (en) * | 2023-07-05 | 2023-10-17 | 西安羚控电子科技有限公司 | Self-adaptive fusion method and device for mobile robot positioning |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109597864B (en) | 2020-10-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109597864A (en) | Instant positioning and map constructing method and the system of ellipsoid boundary Kalman filtering | |
| Zhu et al. | Adapted error map based mobile robot UWB indoor positioning | |
| CN102752855B (en) | Indoor personnel positioning system and method based on path rule and prediction | |
| CN105263113A (en) | Wi-Fi location fingerprint map building method and system based on crowd-sourcing | |
| CN106714110A (en) | Auto building method and system of Wi-Fi position fingerprint map | |
| CN108645415A (en) | A kind of ship track prediction technique | |
| CN104507159A (en) | A method for hybrid indoor positioning based on WiFi (Wireless Fidelity) received signal strength | |
| Yan et al. | Extreme learning machine for accurate indoor localization using RSSI fingerprints in multifloor environments | |
| CN111970633A (en) | Indoor positioning method based on WiFi, Bluetooth and pedestrian dead reckoning fusion | |
| CN106403953B (en) | A method of for underwater independent navigation and positioning | |
| Hasan et al. | Automatic estimation of inertial navigation system errors for global positioning system outage recovery | |
| CN116908777A (en) | Multi-robot random networking collaborative navigation method based on explicit communication with tag Bernoulli | |
| Yu et al. | AI based location tracking in WiFi indoor positioning application | |
| Ye et al. | Robot indoor positioning and navigation based on improved wifi location fingerprint positioning algorithm | |
| Li et al. | A new RSS fingerprinting-based location discovery method under sparse reference point conditions | |
| Zhao et al. | Factor graph based multi-source data fusion for wireless localization | |
| Sun et al. | The application of indoor localization systems based on the improved Kalman filtering algorithm | |
| Li et al. | HIWL: An unsupervised learning algorithm for indoor wireless localization | |
| CN114339595B (en) | Ultra-wide band dynamic inversion positioning method based on multi-model prediction | |
| CN115950414A (en) | Adaptive multi-fusion SLAM method for different sensor data | |
| Li et al. | An enhanced direction calibration based on reinforcement learning for indoor localization system | |
| Kong et al. | Hybrid indoor positioning method of BLE and monocular VINS based smartphone | |
| Liu et al. | An UWB/PDR fusion algorithm based on improved square root unscented Kalman filter | |
| CN110763223B (en) | Sliding window based indoor three-dimensional grid map feature point extraction method | |
| Zhang et al. | The research for a kind of information fusion model based on BP neural network with multi position sources and big data selection |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |