CN108387236A - Polarized light S L AM method based on extended Kalman filtering - Google Patents
Polarized light S L AM method based on extended Kalman filtering Download PDFInfo
- Publication number
- CN108387236A CN108387236A CN201810128657.4A CN201810128657A CN108387236A CN 108387236 A CN108387236 A CN 108387236A CN 201810128657 A CN201810128657 A CN 201810128657A CN 108387236 A CN108387236 A CN 108387236A
- Authority
- CN
- China
- Prior art keywords
- road sign
- unmanned plane
- sign point
- moment
- indicate
- 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
- 238000000034 method Methods 0.000 title claims abstract description 24
- 238000001914 filtration Methods 0.000 title abstract description 7
- 239000011159 matrix material Substances 0.000 claims description 35
- 238000005259 measurement Methods 0.000 claims description 33
- 230000001133 acceleration Effects 0.000 claims description 6
- IAPHXJRHXBQDQJ-ODLOZXJASA-N jacobine Natural products O=C1[C@@]2([C@H](C)O2)C[C@H](C)[C@](O)(C)C(=O)OCC=2[C@H]3N(CC=2)CC[C@H]3O1 IAPHXJRHXBQDQJ-ODLOZXJASA-N 0.000 claims description 6
- 230000003716 rejuvenation Effects 0.000 claims description 6
- 230000010287 polarization Effects 0.000 claims description 5
- 238000012937 correction Methods 0.000 claims description 4
- 238000012546 transfer Methods 0.000 claims description 4
- 238000013459 approach Methods 0.000 claims description 3
- 238000012217 deletion Methods 0.000 claims description 3
- 230000037430 deletion Effects 0.000 claims description 3
- 238000005530 etching Methods 0.000 claims description 3
- 230000003287 optical effect Effects 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 230000009897 systematic effect Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 102000002274 Matrix Metalloproteinases Human genes 0.000 claims 1
- 108010000684 Matrix Metalloproteinases Proteins 0.000 claims 1
- 238000004088 simulation Methods 0.000 description 4
- 238000009825 accumulation Methods 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000007480 spreading Effects 0.000 description 2
- 240000007594 Oryza sativa Species 0.000 description 1
- 235000007164 Oryza sativa Nutrition 0.000 description 1
- 238000000149 argon plasma sintering Methods 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000005855 radiation Effects 0.000 description 1
- 235000009566 rice Nutrition 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Mathematical Analysis (AREA)
- Computational Mathematics (AREA)
- Pure & Applied Mathematics (AREA)
- Mathematical Optimization (AREA)
- Data Mining & Analysis (AREA)
- Theoretical Computer Science (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Databases & Information Systems (AREA)
- Computing Systems (AREA)
- Algebra (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a polarized light S L AM method based on extended Kalman filtering, which belongs to the field of autonomous navigation of unmanned aerial vehicles.
Description
Technical field
Position the scope for belonging to unmanned plane independent navigation with composition (SLAM) simultaneously the present invention relates to unmanned plane, and in particular to
How a kind of polarised light SLAM methods based on Extended Kalman filter, self-position and sensing external environment are determined to unmanned plane
The problem of, SLAM systems are intended to, by UAV system model, positioning and the week of unmanned plane be completed in conjunction with corresponding filtering method
The drafting in collarette border.
Background technology
The Simultaneous Localization and Mapping abbreviations that SLAM is, mean " while positioning and building
Figure ".It refers to information of the moving object according to sensor, calculates self-position, the process of one side constructing environment map on one side.Mesh
Before, SLAM technologies have been employed for the fields such as unmanned plane, unmanned, robot, AR, smart home.
SLAM researchs lay particular emphasis on the noise for the road sign point that movement posture and map are minimized using filter theory, and one
As using odometer input of the input as prediction process, the input of laser sensor exported as renewal process.Extension
Kalman filtering (Extended Kalman Filter, EKF) filtering algorithm is the filtering that most of SLAM methods use at present
Algorithm.The development of unmanned plane is very fast in recent years, and for unmanned plane, can to solve unmanned plane self-position not true by SLAM
It is fixed, in reckoning the case where site error accumulation, using self-contained sensor, the feature in environment is detected repeatedly, from
And the correction of self-position and feature locations is completed, while constructing environment map, without predicting cartographic information or relying on outer secondary
Help equipment, you can the structure of completion unmanned plane location information and ambient enviroment map, but the positioning accuracy of this SLAM schemes
Not high with composition accuracy, robustness is very poor.
More and more to the research of polarised light in recent years, the famous physicist's Rayleigh of Britain proposes Rayleigh scattering within 1871
Law, discloses light scattering characteristics, and subsequent people obtain omnimax atmospheric polarization distribution pattern based on Rayleigh's law of scattering.
Atmospheric polarization distribution pattern is stablized relatively, wherein containing abundant navigation information, many biologies can utilize in nature
Sky polarised light carries out navigation or assisting navigation.It is a kind of very effective navigation means to polarize navigation mechanism, has passive, nothing
The features such as radiation, good concealment, new solution route can be provided for the navigation task under complex environment.
Polarised light can be improved unmanned determining self-position and build ambient enviroment map for unmanned plane SLAM to be upper
Accuracy, solve the problems, such as that robustness is not high.
Invention content
Present invention mainly solves the problem of be：How polarization information to be applied in unmanned plane SLAM same to solve unmanned plane
Shi Dingwei determines the problem difficult, environmental suitability is poor, composition is inaccurate with self-position present in composition.
The technical solution adopted by the present invention is：A kind of polarised light SLAM methods based on Extended Kalman filter, including with
Lower step：
(1) posture, speed, position and the road sign point for choosing unmanned plane are system mode, establish the kinetic simulation of unmanned plane
Type；
(2) measurement model of laser radar is established；
(3) measurement model of polarized light sensor is established；
(4) system initialization, map initialization；
(5) road sign point matches；
(6) laser radar data and polarized light sensor data for utilizing road sign point, are filtered by distributed spreading kalman
Wave device, estimation unmanned plane position, speed, posture and road sign point position；
(7) map rejuvenation；
Wherein, posture, speed, position and the road sign point that the step (1) chooses unmanned plane are system mode, establish nobody
The kinetic model of machine；Using unmanned plane initial position as the world coordinate system of origin, i.e. w systems, using direct north as world coordinates
The positive direction of the x-axis of system determines world coordinates using due west direction as the positive direction of the y-axis of world coordinate system according to right hand rule
The z-axis positive direction of system；It establishes using unmanned plane body center as the body coordinate system of origin, i.e. b systems, is referred to being parallel to fuselage datum
To heading be body coordinate system x-axis positive direction, using be parallel to fuselage horizontal axis be directed toward left as body coordinate system y-axis just
Direction determines body coordinate system z-axis positive direction according to right hand rule；Choose the position of unmanned plane, speed, posture and road sign point
Position is as system mode；Then systematic state transfer equation is as follows：
Wherein
Indicate the system mode at k-1 moment,
Indicate coordinate of the k-1 moment unmanned plane under world coordinate system,Indicate that k-1 moment unmanned planes are alive respectively
X under boundary's coordinate system, y, the coordinate of tri- axis directions of z,Indicate k-1 moment unmanned planes in the world
Speed under coordinate system,Indicate that k-1 moment unmanned plane is directed toward x, y, z tri- under world coordinate system respectively
The speed of axis direction,The posture for indicating unmanned plane, is indicated in the form of Eulerian angles,Respectively roll angle, pitch angle and yaw angle,Indicate k-1 moment world coordinates
The coordinate of m road sign point under system,Indicate i-th,i=1,2 ..., m road sign point exists
Coordinate under world coordinate system,For the specific force acceleration under k-1 moment body coordinate systems,For k-1 moment body coordinates
Angular speed under system,For the heavy acceleration under k-1 moment world coordinates, △ t are sampling time interval,
For k-1 moment body coordinate system to the transformation matrix of world coordinate system, nk-1For system noise, noise is set as high
This white noise, Qk-1For system noise covariance matrix；
Wherein, the step (2) establishes the measurement model of laser radar；Choose the distance of lidar measurement, angle is made
For measured value, the measurement model of road sign point is provided for the data of laser radar：
Wherein
I=1,2 ..., m indicate the nonlinear function in the measurement equation of i-th of road sign point of k moment pair,
yk,i,lidar=[dk,i θk,i λk,i]TIndicate the measuring value of etching system when k, dk,iIndicate the unmanned plane of k moment lidar measurements
Barycenter is at a distance from i-th of road sign point, θk,iIndicate k moment lidar measurements unmanned plane barycenter and i-th road sign point away from
From pitch angle, λk,iIndicate the azimuth of the unmanned plane barycenter and i-th of road sign point of k moment lidar measurements, uk,lidarIt indicates
Laser radar measures noise, and noise is set as white Gaussian noise, Rk,lidarTo measure noise covariance matrix；
Wherein, the step (3) establishes the measurement model of polarized light sensor：Choose the body that polarized light sensor measures
The observation model for the metric data that the longitudinal axis and the meridianal angle amount of the sun are provided as measured value, for polarised light is as follows：
WhereinThe body long axis and the meridianal angle of the sun obtained for polarized light sensor,For solar azimuth,For altitude of the sun
Angle,For solar declination,For the latitude of observation point,For solar hour angle, uk,polarIndicate that polarized light sensor measures noise,
Noise is set as white Gaussian noise, Rk,polarTo measure noise covariance matrix；
Wherein, step (4) system initialization；Unmanned plane coordinate system is transformed into world coordinate system and by unmanned plane,
The system mode of given initial time is, initial covariance matrix is P0, and establish using unmanned plane initial position as the complete of origin
Local figure；
Wherein, step (5) the road sign point matching；To the data that k moment laser radars measure, and in global map
The road sign point of storage is matched using iteration closest approach (ICP) algorithm, for the road sign point of successful match, then by the road sign point
It is sent into the subsequent operation of corresponding subfilter progress the road sign point that it fails to match is then established corresponding subfilter, is
It carries out next iteration to prepare, for road sign point deletion its corresponding subfilter for failing to observe；
Wherein, the step (6) utilizes the laser radar data and polarized light sensor data of road sign point, passes through distribution
Extended Kalman filter estimates the position of unmanned plane, the position of speed, posture and road sign point；First by each of k-1 moment
The estimated state and covariance matrix of subfilterIn conjunction with the output data of the IMU at k-1 moment, carries out a step recursion and obtain
To the robot location extrapolated by IMU and state-transition matrix, then according to predicted value, laser sensor information and partially is added
The optical information that shakes carries out measuring newer calculating, exports update of each subfilter to correction value and the road sign point of unmanned plane position
Value, and corresponding covariance matrix is exported simultaneously, the estimated result of each subfilter is input in senior filter, according to each son
The covariance of filter determines the proportion that each subfilter accounts in senior filter and is normalized, to export unmanned plane
Position, speed, posture and road sign point position.It is as follows：
Time updates：
Measure update：
Senior filter weighted average：
WhereinIndicate the system state estimation at kth moment,Indicate the covariance matrix at kth moment, FkExpression state
The Jacobin matrix of equation, HkIndicate the Jacobin matrix of observational equation, ηiFor weighting coefficient,System after expression weighted average
Covariance matrix,Indicate the system state estimation after weighted average；
Wherein, step (7) map rejuvenation；For already present road sign point in global map, after corresponding filtering
Road sign point global map is directly added it to the road sign point of non-successful match in step (5) as newest road sign point
In；
The present invention compared with prior art the advantages of be：
The polarised light SLAM methods based on Extended Kalman filter that the invention discloses a kind of, belong to unmanned plane independent navigation
Scope.Measurement of this method by establishing the state model of unmanned plane and based on laser radar sensor, polarized light sensor
Model realizes the structure of the determination and ambient enviroment map of unmanned plane self-position using distributed EKF algorithms, effective to utilize
Polarization information and laser radar information it is complementary, not by the characteristic of other external interferences, improve the essence of unmanned plane location estimation
Degree.In the case where keeping laser radar measurement information constant, the advantages of accumulation at any time using polarised light error in measurement, drop
The accumulation of the error of yaw angle in the case of low long-time, the accuracy for improving global pose.Make unmanned plane in location circumstances
In positioning and pattern accuracy higher, the adaptability of circumstances not known is enhanced.
Description of the drawings
Fig. 1 is a kind of polarised light SLAM method flow diagrams based on Extended Kalman filter；
Fig. 2 is a kind of polarised light SLAM method the simulation experiment result figures based on Extended Kalman filter.
Specific implementation mode
Below in conjunction with the accompanying drawings and specific implementation mode further illustrates the present invention.
As shown in Figure 1, a kind of polarised light SLAM methods based on Extended Kalman filter of the present invention, include the following steps：
(1) posture, speed, position and the road sign point for choosing unmanned plane are system mode, establish the kinetic simulation of unmanned plane
Type；
Posture, speed, position and the road sign point that the step (1) chooses unmanned plane are system mode, establish unmanned plane
Kinetic model；Using unmanned plane initial position as the world coordinate system of origin, i.e. w systems, using direct north as the x of world coordinate system
The positive direction of axis determines the z of world coordinate system according to right hand rule using due west direction as the positive direction of the y-axis of world coordinate system
Axis positive direction；It establishes using unmanned plane body center as the body coordinate system of origin, i.e. b systems, machine is directed toward to be parallel to fuselage datum
Head direction is the positive direction of body coordinate system x-axis, and left is directed toward as the pros of body coordinate system y-axis to be parallel to fuselage horizontal axis
To determining body coordinate system z-axis positive direction according to right hand rule；Choose the position of unmanned plane, the position of speed, posture and road sign point
It sets as system mode；Then systematic state transfer equation is as follows：
Wherein
Indicate the system mode at k-1 moment,
Indicate coordinate of the k-1 moment unmanned plane under world coordinate system,Indicate that k-1 moment unmanned planes are alive respectively
X under boundary's coordinate system, y, the coordinate of tri- axis directions of z,Indicate k-1 moment unmanned planes in the world
Speed under coordinate system,Indicate that k-1 moment unmanned plane is directed toward x, y, z tri- under world coordinate system respectively
The speed of axis direction,The posture for indicating unmanned plane, is indicated in the form of Eulerian angles,Respectively roll angle, pitch angle and yaw angle,Indicate k-1 moment world coordinates
The coordinate of m road sign point under system,Indicate i-th, i=1,2 ..., m road sign point is alive
Coordinate under boundary's coordinate system,For the specific force acceleration under k-1 moment body coordinate systems,For k-1 moment body coordinate systems
Under angular speed,For the heavy acceleration under k-1 moment world coordinates, △ t are sampling time interval,
For k-1 moment body coordinate system to the transformation matrix of world coordinate system, nk-1For system noise, noise is set as white Gaussian noise,
Qk-1For system noise covariance matrix；
(2) measurement model of laser radar is established；
The step (2) establishes the measurement model of laser radar；Choose the distance of lidar measurement, angle is used as and measures
Value, the measurement model of road sign point is provided for the data of laser radar：
Wherein
I=1,2 ..., m indicate the nonlinear function in the measurement equation of i-th of road sign point of k moment pair,
yk,i,lidar=[dk,i θk,i λk,i]TIndicate the measuring value of etching system when k, dk,iIndicate the unmanned plane of k moment lidar measurements
Barycenter is at a distance from i-th of road sign point, θk,iIndicate k moment lidar measurements unmanned plane barycenter and i-th road sign point away from
From pitch angle, λk,iIndicate the azimuth of the unmanned plane barycenter and i-th of road sign point of k moment lidar measurements, uk,lidarIt indicates
Laser radar measures noise, and noise is set as white Gaussian noise, Rk,lidarTo measure noise covariance matrix；
(3) measurement model of polarized light sensor is established；
The step (3) establishes the measurement model of polarized light sensor：Choose the body longitudinal axis that polarized light sensor measures
With the meridianal angle amount of the sun as measured value, the observation model of the metric data provided for polarised light is as follows：
WhereinThe body long axis and the meridianal angle of the sun obtained for polarized light sensor,For solar azimuth,For altitude of the sun
Angle,For solar declination,For the latitude of observation point, t is solar hour angle, uk,polarIt indicates that polarized light sensor measures noise, makes an uproar
Sound is set as white Gaussian noise, Rk,polarTo measure noise covariance matrix；
(4) system initialization, map initialization；
Step (4) system initialization；Unmanned plane coordinate system is transformed into world coordinate system and by unmanned plane, is given just
Begin the moment system mode be, initial covariance matrix P0, and establish using unmanned plane initial position as the global map of origin；
(5) road sign point matches；
Step (5) the road sign point matching；To the data that k moment laser radars measure, and stored in global map
Road sign point matched using iteration closest approach (ICP) algorithm, for the road sign point of successful match, then by the road sign point be sent into
Corresponding subfilter carries out subsequent operation and then establishes corresponding subfilter for the road sign point that it fails to match, to carry out
Next iteration is prepared, for road sign point deletion its corresponding subfilter for failing to observe；
(6) laser radar data and polarized light sensor data for utilizing road sign point, are filtered by distributed spreading kalman
Wave device, estimation unmanned plane position, speed, posture and road sign point position；
The step (6) utilizes the laser radar data and polarized light sensor data of road sign point, is extended by distribution
Kalman filter estimates the position of unmanned plane, the position of speed, posture and road sign point；Every height at k-1 moment is filtered first
The estimated state and covariance matrix of wave deviceIn conjunction with the output data of the IMU at k-1 moment, carry out a step recursion obtain by
Laser sensor information and polarised light is added then according to predicted value in the robot location that IMU and state-transition matrix are extrapolated
Information carries out measuring newer calculating, exports updated value of each subfilter to correction value and the road sign point of unmanned plane position,
And corresponding covariance matrix is exported simultaneously, the estimated result of each subfilter is input in senior filter, according to each sub- filter
The covariance of wave device determines the proportion that each subfilter accounts in senior filter and is normalized, to export the position of unmanned plane
It sets, the position of speed, posture and road sign point.It is as follows：
Time updates：
Measure update：
Senior filter weighted average：
WhereinIndicate the system state estimation at kth moment,Indicate the covariance matrix of the system at kth moment, FkTable
Show the Jacobin matrix of state equation, HkIndicate the Jacobin matrix of observational equation, ηiFor weighting coefficient,Indicate weighted average
The covariance matrix of system afterwards,Indicate the system state estimation after weighted average；
(7) map rejuvenation；
Step (7) map rejuvenation；For already present road sign point in global map, by corresponding filtered road sign
Point directly adds it in global map the road sign point of non-successful match in step (5) as newest road sign point；
It is illustrated in figure 2 the simulation experiment result, * points are true UAV position and orientation, and triangulation point is estimated for proposed method
UAV position and orientation, five asterisms be setting road sign point.By test of many times, unmanned plane is 0.1812 in the mean error of x-axis
Rice, y-axis mean error are 0.2081 meter, and z-axis mean error is 0.2532 meter, and position mean error is 0.4323 meter.
Claims (8)
1. a kind of polarised light SLAM methods based on Extended Kalman filter, it is characterised in that：Include the following steps：
(1) posture, speed, position and the road sign point for choosing unmanned plane are system mode, establish the kinetic model of unmanned plane；
(2) measurement model of laser radar is established；
(3) measurement model of polarized light sensor is established；
(4) system initialization, map initialization；
(5) road sign point matches；
(6) laser radar data and polarized light sensor data for utilizing road sign point, by distributed extended Kalman filter,
Estimate unmanned plane position, speed, posture and road sign point position；
(7) map rejuvenation.
2. a kind of polarised light SLAM methods based on Extended Kalman filter according to claim 1, it is characterised in that：Institute
It is system mode to state step (1) and choose posture, speed, position and the road sign point of unmanned plane, establishes the kinetic model of unmanned plane；
Using unmanned plane initial position as the world coordinate system of origin, i.e. w systems, using direct north as the positive direction of the x-axis of world coordinate system,
Using due west direction as the positive direction of the y-axis of world coordinate system, the z-axis positive direction of world coordinate system is determined according to right hand rule；It builds
It stands using unmanned plane body center as the body coordinate system of origin, i.e. b systems, heading is directed toward as body to be parallel to fuselage datum
The positive direction of coordinate system x-axis is directed toward left as the positive direction of body coordinate system y-axis, according to right hand standard to be parallel to fuselage horizontal axis
Then determine body coordinate system z-axis positive direction；The position of the position of unmanned plane, speed, posture and road sign point is chosen as system shape
State；Then systematic state transfer equation is as follows：
Wherein
Indicate the system mode at k-1 moment,It indicates
Coordinate of the k-1 moment unmanned plane under world coordinate system,Indicate that k-1 moment unmanned plane is sat in the world respectively
The lower x of mark system, the coordinate of tri- axis directions of y, z,Indicate k-1 moment unmanned planes in world coordinates
Speed under system,Indicate that k-1 moment unmanned plane is directed toward x, the tri- axis sides y, z under world coordinate system respectively
To speed,The posture for showing unmanned plane, is indicated in the form of Eulerian angles,Point
Not Wei roll angle, pitch angle and yaw angle,Indicate m road under k-1 moment world coordinate systems
The coordinate of punctuate,Indicate i-th,i=1,2 ..., m road sign point is under world coordinate system
Coordinate,For the specific force acceleration under k-1 moment body coordinate systems,For the angular speed under k-1 moment body coordinate systems,For the heavy acceleration under k-1 moment world coordinates, △ t are sampling time interval,For the k-1 moment
Body coordinate system is to the transformation matrix of world coordinate system, nk-1For system noise, noise is set as white Gaussian noise, Qk-1For system
Noise covariance matrix.
3. a kind of polarised light SLAM methods based on Extended Kalman filter according to claim 1, it is characterised in that：Institute
State the measurement model that step (2) establishes laser radar；Choose the distance of lidar measurement, angle is used as measured value, for sharp
The data of optical radar provide the measurement model of road sign point：
Wherein
Indicate the nonlinear function in the measurement equation of i-th of road sign point of k moment pair, yk,i,lidar=
[dk,i θk,i λk,i]TIndicate the measuring value of etching system when k, dk,iIndicate the unmanned plane barycenter and i-th of k moment lidar measurements
The distance of a road sign point, θk,iThe unmanned plane barycenter of expression k moment lidar measurements pitch angle at a distance from i-th of road sign point,
λk,iIndicate the azimuth of the unmanned plane barycenter and i-th of road sign point of k moment lidar measurements, uk,lidarIndicate laser radar
Noise is measured, noise is set as white Gaussian noise, Rk,lidarTo measure noise covariance matrix.
4. a kind of polarised light SLAM methods based on Extended Kalman filter according to claim 1, it is characterised in that：Institute
State the measurement model that step (3) establishes polarized light sensor：Choose the body longitudinal axis and sun meridian that polarized light sensor measures
The observation model for the metric data that the angle amount of line is provided as measured value, for polarised light is as follows：
WhereinThe body long axis and the meridianal angle of the sun obtained for polarized light sensor,
For solar azimuth,For sun altitude,For solar declination,For observation point
Latitude,For solar hour angle, uk,polarIndicate that polarized light sensor measures noise, noise is set as white Gaussian noise, Rk,polarFor
Measure noise covariance matrix.
5. a kind of polarised light SLAM methods based on Extended Kalman filter according to claim 1, it is characterised in that：Institute
State step (4) system initialization；Unmanned plane coordinate system is transformed into world coordinate system and by unmanned plane, given initial time is
System state isInitial covariance matrix is P0, and establish using unmanned plane initial position as the global map of origin.
6. a kind of polarised light SLAM methods based on Extended Kalman filter according to claim 1, it is characterised in that：Institute
State the matching of step (5) road sign point；To the data that k moment laser radars measure, and the road sign point stored in global map is sharp
It is matched with iteration closest approach (ICP) algorithm, for the road sign point of successful match, then the road sign point is sent into corresponding son filter
Wave device carries out subsequent operation and then establishes corresponding subfilter for the road sign point that it fails to match, is done to carry out next iteration
Prepare, for road sign point deletion its corresponding subfilter for failing to observe.
7. a kind of polarised light SLAM methods based on Extended Kalman filter according to claim 1, it is characterised in that：Institute
Laser radar data and polarized light sensor data that step (6) utilizes road sign point are stated, distributed Extended Kalman filter is passed through
Device estimates the position of unmanned plane, the position of speed, posture and road sign point；First by the estimation of each subfilter at k-1 moment
State and covariance matrixIn conjunction with the output data of the IMU at k-1 moment, carries out a step recursion and obtain by IMU and state
The robot location that transfer matrix is extrapolated is added laser sensor information and polarization optical information carries out then according to predicted value
Newer calculating is measured, exports updated value of each subfilter to correction value and the road sign point of unmanned plane position, and defeated simultaneously
Go out corresponding covariance matrix, the estimated result of each subfilter is input in senior filter, according to the association of each subfilter
Variance determines the proportion that each subfilter accounts in senior filter and is normalized, to export the position of unmanned plane, speed,
The position of posture and road sign point, is as follows：
Time updates：
Measure update：
Senior filter weighted average：
WhereinIndicate the system state estimation at kth moment,Indicate the covariance matrix of the system at kth moment, FkIndicate shape
The Jacobin matrix of state equation, HkIndicate the Jacobin matrix of observational equation, ηiFor weighting coefficient,It is after indicating weighted average
The covariance matrix of system,Indicate the system state estimation after weighted average.
8. a kind of polarised light SLAM methods based on Extended Kalman filter according to claim 1, it is characterised in that：Institute
State step (7) map rejuvenation；For already present road sign point in global map, using corresponding filtered road sign point as newest
Road sign point directly adds it in global map the road sign point of non-successful match in step (5).
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810128657.4A CN108387236B (en) | 2018-02-08 | 2018-02-08 | Polarized light SLAM method based on extended Kalman filtering |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810128657.4A CN108387236B (en) | 2018-02-08 | 2018-02-08 | Polarized light SLAM method based on extended Kalman filtering |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108387236A true CN108387236A (en) | 2018-08-10 |
| CN108387236B CN108387236B (en) | 2021-05-07 |
Family
ID=63075502
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810128657.4A Active CN108387236B (en) | 2018-02-08 | 2018-02-08 | Polarized light SLAM method based on extended Kalman filtering |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108387236B (en) |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110779514A (en) * | 2019-10-28 | 2020-02-11 | 北京信息科技大学 | Hierarchical Kalman fusion method and device for auxiliary attitude determination of bionic polarization navigation |
| CN112697138A (en) * | 2020-12-07 | 2021-04-23 | 北方工业大学 | Factor graph optimization-based bionic polarization synchronous positioning and composition method |
| CN113739795A (en) * | 2021-06-03 | 2021-12-03 | 东北电力大学 | Underwater synchronous positioning and mapping method based on polarized light/inertia/vision combined navigation |
| WO2022036792A1 (en) * | 2020-08-21 | 2022-02-24 | 苏州三六零机器人科技有限公司 | Multi-data source slam method, device, and computer readable storage medium |
| US11366480B2 (en) * | 2018-09-04 | 2022-06-21 | Mazdak Hooshang | Navigating a vehicle through a predefined path |
| CN115235475A (en) * | 2022-09-23 | 2022-10-25 | 成都凯天电子股份有限公司 | MCC-based EKF-SLAM back-end navigation path optimization method |
| CN115574816A (en) * | 2022-11-24 | 2023-01-06 | 东南大学 | Bionic vision multi-source information intelligent perception unmanned platform |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR2981448A1 (en) * | 2011-10-18 | 2013-04-19 | Floch Albert Le | DEVICE FOR DETECTING THE DIRECTION OF A CACHE SUN, BASED ON A BIREFRINGENT DEPOLARIZER |
| CN105606104A (en) * | 2016-03-17 | 2016-05-25 | 北京工业大学 | Robot autonomous navigation method based on heading-assisting distributed type SLAM (Simultaneous Localization and Mapping) |
| CN105737832A (en) * | 2016-03-22 | 2016-07-06 | 北京工业大学 | Distributed SLAM (simultaneous localization and mapping) method on basis of global optimal data fusion |
| CN106197432A (en) * | 2016-08-30 | 2016-12-07 | 北京航空航天大学 | A kind of UAV Landing method based on FastSLAM algorithm |
| CN106886030A (en) * | 2017-03-24 | 2017-06-23 | 黑龙江硅智机器人有限公司 | It is applied to the synchronous mode map structuring and alignment system and method for service robot |
-
2018
- 2018-02-08 CN CN201810128657.4A patent/CN108387236B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR2981448A1 (en) * | 2011-10-18 | 2013-04-19 | Floch Albert Le | DEVICE FOR DETECTING THE DIRECTION OF A CACHE SUN, BASED ON A BIREFRINGENT DEPOLARIZER |
| CN105606104A (en) * | 2016-03-17 | 2016-05-25 | 北京工业大学 | Robot autonomous navigation method based on heading-assisting distributed type SLAM (Simultaneous Localization and Mapping) |
| CN105737832A (en) * | 2016-03-22 | 2016-07-06 | 北京工业大学 | Distributed SLAM (simultaneous localization and mapping) method on basis of global optimal data fusion |
| CN106197432A (en) * | 2016-08-30 | 2016-12-07 | 北京航空航天大学 | A kind of UAV Landing method based on FastSLAM algorithm |
| CN106886030A (en) * | 2017-03-24 | 2017-06-23 | 黑龙江硅智机器人有限公司 | It is applied to the synchronous mode map structuring and alignment system and method for service robot |
Non-Patent Citations (1)
| Title |
|---|
| 王道斌: "基于天空偏振光的SLAM方法的研究", 《中国博士学位论文全文数据库信息科技辑》 * |
Cited By (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11366480B2 (en) * | 2018-09-04 | 2022-06-21 | Mazdak Hooshang | Navigating a vehicle through a predefined path |
| CN110779514A (en) * | 2019-10-28 | 2020-02-11 | 北京信息科技大学 | Hierarchical Kalman fusion method and device for auxiliary attitude determination of bionic polarization navigation |
| CN110779514B (en) * | 2019-10-28 | 2021-04-06 | 北京信息科技大学 | Hierarchical Kalman fusion method and device for auxiliary attitude determination of bionic polarization navigation |
| WO2022036792A1 (en) * | 2020-08-21 | 2022-02-24 | 苏州三六零机器人科技有限公司 | Multi-data source slam method, device, and computer readable storage medium |
| CN112697138A (en) * | 2020-12-07 | 2021-04-23 | 北方工业大学 | Factor graph optimization-based bionic polarization synchronous positioning and composition method |
| CN113739795A (en) * | 2021-06-03 | 2021-12-03 | 东北电力大学 | Underwater synchronous positioning and mapping method based on polarized light/inertia/vision combined navigation |
| CN113739795B (en) * | 2021-06-03 | 2023-10-20 | 东北电力大学 | Underwater synchronous positioning and mapping method based on polarized light/inertia/vision integrated navigation |
| CN115235475A (en) * | 2022-09-23 | 2022-10-25 | 成都凯天电子股份有限公司 | MCC-based EKF-SLAM back-end navigation path optimization method |
| CN115235475B (en) * | 2022-09-23 | 2023-01-03 | 成都凯天电子股份有限公司 | MCC-based EKF-SLAM back-end navigation path optimization method |
| CN115574816A (en) * | 2022-11-24 | 2023-01-06 | 东南大学 | Bionic vision multi-source information intelligent perception unmanned platform |
| CN115574816B (en) * | 2022-11-24 | 2023-03-14 | 东南大学 | Bionic vision multi-source information intelligent perception unmanned platform |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108387236B (en) | 2021-05-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108387236A (en) | Polarized light S L AM method based on extended Kalman filtering | |
| CN110243358B (en) | Multi-source fusion unmanned vehicle indoor and outdoor positioning method and system | |
| CN108362288A (en) | Polarized light S L AM method based on unscented Kalman filtering | |
| CN113781582B (en) | Synchronous positioning and map creation method based on laser radar and inertial navigation combined calibration | |
| CN108871336B (en) | A kind of vehicle location estimating system and method | |
| CN110146909A (en) | A kind of location data processing method | |
| CN102538781B (en) | Machine vision and inertial navigation fusion-based mobile robot motion attitude estimation method | |
| CN104075715B (en) | A kind of underwater navigation localization method of Combining with terrain and environmental characteristic | |
| CN109991636A (en) | Map constructing method and system based on GPS, IMU and binocular vision | |
| CN103776446B (en) | A kind of pedestrian's independent navigation computation based on double MEMS-IMU | |
| CN108759838A (en) | Mobile robot multiple sensor information amalgamation method based on order Kalman filter | |
| CN107015238A (en) | Unmanned vehicle autonomic positioning method based on three-dimensional laser radar | |
| CN112697138B (en) | Bionic polarization synchronous positioning and composition method based on factor graph optimization | |
| CN105589064A (en) | Rapid establishing and dynamic updating system and method for WLAN position fingerprint database | |
| CN106780699A (en) | A kind of vision SLAM methods aided in based on SINS/GPS and odometer | |
| CN102322858B (en) | Geomagnetic matching navigation method for geomagnetic-strapdown inertial navigation integrated navigation system | |
| CN111288989B (en) | Visual positioning method for small unmanned aerial vehicle | |
| CN109752725A (en) | A kind of low speed business machine people, positioning navigation method and Position Fixing Navigation System | |
| CN105044668A (en) | Wifi fingerprint database construction method based on multi-sensor device | |
| CN104236548A (en) | Indoor autonomous navigation method for micro unmanned aerial vehicle | |
| CN104880191A (en) | Polarization aided navigation method based on solar vectors | |
| CN105333869A (en) | Unmanned reconnaissance aerial vehicle synchronous positioning and picture compositing method based on self-adaption EKF | |
| CN103822633A (en) | Low-cost attitude estimation method based on second-order measurement update | |
| CN110187375A (en) | A kind of method and device improving positioning accuracy based on SLAM positioning result | |
| CN103712621B (en) | Polarised light and infrared sensor are assisted inertial navigation system method for determining posture |
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 |