CN106950976A - Indoor airship 3 D locating device and method based on Kalman and particle filter - Google Patents
Indoor airship 3 D locating device and method based on Kalman and particle filter Download PDFInfo
- Publication number
- CN106950976A CN106950976A CN201710113867.1A CN201710113867A CN106950976A CN 106950976 A CN106950976 A CN 106950976A CN 201710113867 A CN201710113867 A CN 201710113867A CN 106950976 A CN106950976 A CN 106950976A
- Authority
- CN
- China
- Prior art keywords
- mea
- airship
- posterior
- platform
- particle filter
- 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
- 239000002245 particle Substances 0.000 title claims abstract description 85
- 238000000034 method Methods 0.000 title claims abstract description 20
- 238000005259 measurement Methods 0.000 claims abstract description 50
- 238000001914 filtration Methods 0.000 claims abstract description 30
- 210000004209 hair Anatomy 0.000 claims description 3
- 230000009286 beneficial effect Effects 0.000 claims 1
- 238000005516 engineering process Methods 0.000 description 10
- 239000011159 matrix material Substances 0.000 description 10
- 238000010586 diagram Methods 0.000 description 6
- 230000008901 benefit Effects 0.000 description 5
- 230000000694 effects Effects 0.000 description 4
- 239000013598 vector Substances 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 238000012544 monitoring process Methods 0.000 description 3
- 238000005070 sampling Methods 0.000 description 3
- 239000007787 solid Substances 0.000 description 3
- 238000005538 encapsulation Methods 0.000 description 2
- YBMRDBCBODYGJE-UHFFFAOYSA-N germanium dioxide Chemical compound O=[Ge]=O YBMRDBCBODYGJE-UHFFFAOYSA-N 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- 238000012952 Resampling Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 238000007667 floating Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000002156 mixing Methods 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000008685 targeting Effects 0.000 description 1
- 230000002123 temporal effect Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
-
- 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
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
- G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
- G01S15/06—Systems determining the position data of a target
- G01S15/08—Systems for measuring distance only
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
- G01S17/08—Systems determining position data of a target for measuring distance only
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Aviation & Aerospace Engineering (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Acoustics & Sound (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
- Navigation (AREA)
Abstract
The present invention proposes indoor airship 3 D locating device and method based on Kalman and particle filter, the device includes airship platform, flight and estimates module, laser-measured height module, ultrasonic distance measuring module, flight management and control computer, wherein, flight management includes the Kalman filter and particle filter of interconnection with control computer.This method includes carrying out Kalman filtering to the height measurements of airship platform using Kalman filter and exports Kalman filtering Height Estimation value, is based on space length, Kalman filtering Height Estimation value, velocity measurement and attitude measurement value using particle filter and obtains particle filter horizontal position coordinate value using particle filter algorithm.By apparatus and method of the present invention, more accurate indoor positioning result is obtained.
Description
Technical field
The present invention relates to unmanned plane indoor navigation field of locating technology, and in particular to one kind is based on Kalman and particle filter
Indoor airship 3 D locating device and method.
Background technology
Stereoscopic monitoring indoors, especially in bulk storage plant management, use can fly at low speed people more and more at present
And the aircraft that independently hovers completes task.However, there is system similar to conventional low unmanned planes such as helicopter, many rotors
The shortcomings of framework of uniting is complicated, cruising time is poor, it is impossible to which executive chairman's Time Continuous supervises task.And such as CN201120573237.0
Disclosed small indoor dirigible, based on its floating characteristic, can solve the above problems well.
At present main indoor positioning technologies have infrared technique, REID, super-broadband tech, light tracking technique with
And ultrasonic technology.Due to directly influencing the level of hardware and navigation algorithm of alignment system using different location technology
Build, thus people have studied many different unmanned plane indoor navigation targeting schemes.More ripe positioning product is applied at present,
Its design uses ultrasonic technology, its advantage one is low cost, easy to install, and two be not influenceed by light, smog, and
And temporal information is directly perceived.For bulk storage plant management, the technology has obvious advantage.
At present, how many scholars combine ultrasonic technology (for example, please join in exploration with indoor positioning technologies
See CN201410330642.8), a kind of indoor Navigation of Pilotless Aircraft alignment system based on ultrasonic ranging is made, but be limited by super
Sound wave precision is low, the limitation of measurement noise greatly, and it is designed in specific algorithm, device layout and technology lack in terms of realizing
Effectively unified, locating effect is unsatisfactory.
Moreover, traditional Kalman filtering algorithm is simple in construction, amount of calculation is small, but is only applicable to Gaussian noise environment, right
In the high occasion of non-Gaussian noise environment, non-linearization degree, its state estimation deviation is big, easily produces diverging.Although particle
Filtering algorithm is not assumed to be limited by nonlinear degree and Gaussian noise, but its is computationally intensive, for a wide range of three dimensions ring
Border is, it is necessary to which a large amount of particle estimations could realize that system mode restrains, thus its computational efficiency is low, and system real time is poor.
Therefore, this non-linear strong for the bulk storage plant positioning based on ultrasonic wave, the big occasion of solid space size is passed
Single filtering algorithm of uniting is difficult to the demand for meeting people, and reality needs that performance is more preferable, the more efficient interior based on ultrasonic wave
Location algorithm (for example, referring to CN201410191921.0).In order to obtain more accurate interior under big indoor environment
Positioning result is, it is necessary to propose new technical scheme.
The content of the invention
The purpose of the present invention is achieved through the following technical solutions.
The present invention proposes a kind of indoor airship 3 D locating device based on Kalman and particle filter, including：Dirigible
Module, laser-measured height module, ultrasonic distance measuring module, flight management and control computer are estimated in platform, flight.
According to the indoor airship 3 D locating device of the present invention, flight therein estimates module and is installed in airship platform
Top, and including three axis accelerometer, three-axis gyroscope, three axle magnetic compasses, it is respectively used to gather the tachometric survey of airship platform
It is worth (umea,vmea,wmea), attitude measurement value (φmea,θmea,ψmea) and heading measure value；Laser-measured height module therein is mounted
In the underface of airship platform, and including laser radar and head, measured for the principle according to laser ranging and export dirigible
The height measurements h of platformmea；Ultrasonic distance measuring module therein includes connecing installed in the head position on the lower side of airship platform
Label and airship platform gain antenna are received, is mounted on below airship platform and position in four fixed chambers of neighbouring clear
The transmitting base station put and four base station gain antennas, the transmitting for obtaining and exporting airship platform and four fixed indoor locations
Space length l between base station1、l2、l3、l4；Flight management therein is installed in the top of airship platform with control computer
Portion, and Kalman filter and particle filter including being connected with each other, height of the Kalman filter therein to airship platform
Spend measured value hmeaCarry out Kalman filtering and export Kalman filtering Height Estimation value hposterior, particle filter base therein
In space length l1、l2、l3、l4, Kalman filtering Height Estimation value hposterior, velocity measurement (umea,vmea,wmea) and appearance
State measured value (φmea,θmea,ψmea) and particle filter horizontal position coordinate value (x is obtained using particle filter algorithmposterior,
yposterior), so as to obtain the fine estimation (x of the position positioning of airship platformposterior,yposterior,hposterior)。
The invention allows for a kind of indoor airship 3-D positioning method based on Kalman and particle filter, it is to use
What indoor airship 3 D locating device described above was realized, this method includes：Flight estimates that module gather and to export dirigible flat
Velocity measurement (the u of platformmea,vmea,wmea), attitude measurement value (φmea,θmea,ψmea) and heading measure value；Laser-measured height module
Principle according to laser ranging measures and exports the height measurements h of airship platformmea；Ultrasonic distance measuring module is obtained and exported
Space length l between the transmitting base station of airship platform and four fixed indoor locations1、l2、l3、l4；And, flight management with
The height measurements h of Kalman filter in control computer to airship platformmeaCarry out Kalman filtering and export Kalman
Filter Height Estimation value hposterior, particle filter therein is based on space length l1、l2、l3、l4, Kalman filtering highly estimates
Evaluation hposterior, velocity measurement (umea,vmea,wmea) and attitude measurement value (φmea,θmea,ψmea) and use particle filter
Algorithm obtains particle filter horizontal position coordinate value (xposterior,yposterior), so that obtain the position positioning of airship platform
Fine estimation (xposterior,yposterior,hposterior)。
According to the indoor airship 3-D positioning method of the present invention, wherein, ultrasonic distance measuring module obtains and exports dirigible and puts down
Space length l between the transmitting base station of platform and four fixed indoor locations1、l2、l3、l4Comprise the following steps：
Four transmitting base stations of ultrasonic distance measuring module launch ultrasonic waves by four base station gain antennas, receive label and
Airship platform gain antenna receives the ultrasonic signal of four base station gain antenna transmittings respectively, obtains airship platform and four solid
Determine the space length l between the transmitting base station of indoor location1、l2、l3、l4；And
Export the space length l between airship platform and the transmitting base station of four fixed indoor locations1、l2、l3、l4。
The advantage of the invention is that：Traditional card is combined in the indoor airship 3 D locating device and method proposed
Kalman Filtering algorithm and particle filter algorithm, realize the structure of (1) indoor airship three-dimensional navigation positioning architecture；(2) card is passed through
Kalman Filtering algorithm completes the more accurate estimation of dirigible height；(3) the accurate card based on ultrasonic ranging result and more
Germania Height Estimation, further completes the more accurate estimation of the horizontal level of indoor airship by particle filter algorithm；(4)
Height Estimation and location estimation result are integrated, the three-dimensional localization task of indoor airship is more accurately completed.
Brief description of the drawings
By reading the detailed description of following detailed description, various other advantages and benefit is common for this area
Technical staff will be clear understanding.Accompanying drawing is only used for showing the purpose of embodiment, and is not considered as to the present invention
Limitation.And in whole accompanying drawing, identical part is denoted by the same reference numerals.In the accompanying drawings：
Fig. 1 is the schematic diagram of the indoor airship 3 D locating device based on Kalman and particle filter according to the present invention.
Fig. 2 is that module and flight management and control are estimated in the flight in the indoor airship 3 D locating device according to the present invention
The close-up schematic view of computer.
Fig. 3 is the partial enlargement signal of the laser-measured height module in indoor airship 3 D locating device of the invention
Figure.
Fig. 4 is the system operation schematic diagram in the indoor airship 3 D locating device according to the present invention.
Fig. 5 is the knot of the multi-sensor information collection emerging system in indoor airship 3 D locating device of the invention
Structure block diagram.
Embodiment
The illustrative embodiments of the present invention are more fully described below with reference to accompanying drawings.Although showing this hair in accompanying drawing
Bright illustrative embodiments, it being understood, however, that may be realized in various forms the reality of the invention without that should be illustrated here
The mode of applying is limited.Conversely it is able to be best understood from the present invention there is provided these embodiments, and this can be sent out
Bright scope completely convey to those skilled in the art.
Fig. 1 is the schematic diagram of the indoor airship 3 D locating device based on Kalman and particle filter according to the present invention.
Estimate module 2 as shown in figure 1, the indoor airship 3 D locating device proposed includes airship platform 1, flight, swash
The high module 3 of flash ranging, ultrasonic distance measuring module 4 and flight management and control computer 5.
Airship platform 1 is the task platform of whole indoor airship, and corresponding airborne task mould can be carried according to mission requirements
Block, such as manipulator, detector, video camera.Connecing in module 2, laser-measured height module 3, ultrasonic distance measuring module 4 is estimated in flight
Label 405 and airship platform gain antenna 410, flight management and control computer 5 is received all to be arranged on airship platform 1.Flight
Estimate module 2 and flight management and the top of airship platform 1 is together arranged on control computer 5, laser-measured height module 3, which is arranged on, to fly
Immediately below ship platform 1, the reception label 405 and airship platform gain antenna 410 in ultrasonic distance measuring module 4 are flat installed in dirigible
The head of platform 1 position on the lower side.And four transmitting base stations 401,402,403,404 and gain antenna in ultrasonic distance measuring module 4
406th, 407,408,409 four fixing points indoors are installed, and ensure its neighbouring clear, and under airship platform 1
Side.
Fig. 2 is that module and flight management and control are estimated in the flight in the indoor airship 3 D locating device according to the present invention
The close-up schematic view of computer.
As shown in Fig. 2 module 2 is estimated in flight, it is arranged on the top of airship platform 1, and including three axis accelerometer
201st, three-axis gyroscope 202, three axle magnetic compasses 203, are respectively used to gather the velocity measurement (u of airship platform 1mea,vmea,
wmea), attitude measurement value (φmea,θmea,ψmea) and heading measure value, and pass to particle filter 502.Because interior does not have
The calibration of high-precision GPS signal, the airship platform measured value of state that the sensor is obtained is that noise is high, drift about greatly inaccurate
Data are, it is necessary to which the Kalman introduced below-particle combinations filtering algorithm carries out posteriority calibration.As shown in Fig. 2 flight management and control
Computer 5 processed includes Kalman filter 501 and particle filter 502.
Fig. 3 is the partial enlargement signal of the laser-measured height module in indoor airship 3 D locating device of the invention
Figure.
As shown in figure 3, laser-measured height module 3, it is arranged on the underface of airship platform 1, and including the He of laser radar 301
Head 302, ensures laser radar 301 in whole flight course all the time just to ground, and foundation by Three Degree Of Freedom head 302
The principle of laser ranging measures and exports the height measurements h of airship platform 1mea, and pass to Kalman filter 501.Due to
The constant error of the self vibration of airship platform 1 and sensor, the data cannot be used directly for the positioning of airship platform 1, karr
Graceful wave filter 501 completes the priori prediction and Posterior estimator to the height of airship platform 1, draws airship platform 1 by KF algorithms
Height fine estimation hposterior。
As shown in figure 3, the reception label 405 and airship platform gain antenna 410 in ultrasonic distance measuring module 4 are received respectively
The ultrasonic signal that four base station gain antennas 406,407,408,409 are launched, so as to obtain airship platform 1 and four fixed chambers
Space length l between the transmitting base station 401,402,403,404 of interior position1、l2、l3、l4。
Fig. 4 be according to the present invention indoor airship 3 D locating device in system operation schematic diagram, including particle filter
The principle of ripple.
As shown in figure 4, flight management and the Kalman filter 501 and particle filter 502 included by control computer 5
It is connected with each other.Height measurements h of the Kalman filter 501 to airship platform 1meaCarry out Kalman filtering and to particle filter
The output Kalman filtering Height Estimation value of device 502 hposterior.Particle filter 502 is based on space length l1、l2、l3、l4, karr
Graceful filtering Height Estimation value hposterior, velocity measurement (umea,vmea,wmea) (it is used as the priori prediction value (u in Fig. 4prior,
vprior, wprior)) and attitude measurement value (φmea,θmea,ψmea) (it is used as the priori prediction value (φ in Fig. 4prior, θprior,
Ψprior)) and particle filter horizontal position coordinate value (x is obtained using particle filter algorithmposterior,yposterior), so that
Fine estimation (the x positioned to the position of airship platform 1posterior,yposterior,hposterior) (equivalent in Fig. 4
xposterior, yposterior, zposterior))。
Selectively, Kalman filter 501 is by the fine estimation h of the dirigible height calculatedposteriorPass to grain
Subfilter 502.When the measuring state value (u of the airship platform 1 of the acquisition of module 2 is estimated in flightmea,vmea,wmea)、(φmea,
θmea,ψmea) pass to after particle filter 502, it is necessary to by other data messages to data progress posteriority calibration, draw winged
Fine estimation (the x of the position of ship platform 1posterior,yposterior,zposterior).Concrete operations principle is as follows：Ultrasonic ranging
Four transmitting base stations 401,402,403,404 of module 4 launch ultrasonic wave by gain antenna 406,407,408,409, install
On the head of airship platform 1 reception label 405 on the lower side is received respectively with reference to gain antenna 410 come from four transmitting base stations 401,
402nd, 403,404 ultrasonic signal sent, obtains airship platform 1 and the space length l of four base stations1、l2、l3、l4, concurrently
Particle filter 502 is given, particle filter 502 passes through l1、l2、l3、l4And hposteriorTo (umea,vmea,wmea)、
(φmea,θmea,ψmea) posteriority calibration is carried out, the accurate estimated location (x of airship platform 1 is calculated by PF algorithmsposterior,
yposterior,zposterior)。
Fig. 5 is the knot of the multi-sensor information collection emerging system in indoor airship 3 D locating device of the invention
Structure block diagram.
As shown in figure 5, the multisensor receiving portion in the indoor airship 3 D locating device of the present invention uses number
Word signal processor DSP is as flight management and control computer 5, using Texas Instrument's TMS320C6713 chips, multichannel string
Mouth expansion board is made up of two panels four-way extended chip TL16C554A and some additional devices.Processor is embedded with sensor drive
Dynamic model block, ultrasonic signal processing module, Data Fusion of Sensor module, blending algorithm module, by interface on piece with leading to more
Road serial ports expansion plate serial ports is connected with airborne laser radar 301.Wherein, three axis accelerometer 201, three-axis gyroscope 202 pass through
I2C buses are connected with DSP；Three axle magnetic compasses 203 are connected by A/D translation interfaces with DSP；Laser radar 301 connects with ultrasonic wave
Label 405 is received to be connected with Multi-channel extension plate by serial ports.Various kinds of sensors information passes through corresponding interface and multichannel serial
The serial ports of expansion board is transferred to multi-sensor information collection emerging system, and on the one hand data message is sampled and encapsulated, the opposing party
Face is sent to intelligence by Kalman-particle combinations filtering algorithm, the significant data of generation dirigible positioning, data by CAN
Can decision system.
The invention allows for a kind of indoor airship 3-D positioning method based on Kalman and particle filter, this method is adopted
With indoor airship 3 D locating device described above, it is described in detail below.Provide related symbol explanation first herein
(note：This algorithm is based on international inertial coodinate system and body axis system)：
(umea,vmea,wmea)：The velocity measurement of the collection of module 2 is estimated in flight；
(φmea,θmea,ψmea)：The attitude measurement value of the collection of module 2 is estimated in flight；
hmea：The height measurements that laser radar 301 is gathered；
dt：Double sampling time interval；
(xbi,ybi,zbi) (i=1,2,3,4)：Respectively in ultrasonic distance measuring module 4 four transmitting base stations (401,
402nd, 403, the 404) position in space indoors；
li(i=1,2,3,4)：Dirigible and four hairs that label 405 is gathered are received respectively in ultrasonic distance measuring module 4
Penetrate base station (401,402,403,404) air line distance；
KF state vectors；
Z=hpiror：KF measurement vectors；
P：KF state covariance matrix；
A：KF state-transition matrixes；
Q：KF system noise diagonal matrixs；
H：KF calculation matrix；
R：Measurement noise diagonal matrix；
K：Kalman gain；
hprior：Dirigible height priori prediction value；
hposterior：Dirigible height posterior estimate；
(xprior,yprior)：Dirigible horizontal level priori prediction value；
(xposterior,yposterior)：Dirigible horizontal level posterior estimate；
N：PF populations；
na：PF system Gaussian noises；
ba：PF system driftings；
P particle priori observations；
ωn：PF particle weights values.
Comprised the following steps according to the indoor airship 3-D positioning method based on Kalman and particle filter of the present invention：
Velocity measurement (the u that module 2 gathers and exports airship platform 1 is estimated in flightmea,vmea,wmea) and attitude measurement value
(φmea,θmea,ψmea)；Laser-measured height module 3 measures according to the principle of laser ranging and exports the height measurements of airship platform 1
hmea；The space that ultrasonic distance measuring module 4 obtains and exports airship platform 1 between the transmitting base station of four fixed indoor locations
Apart from l1、l2、l3、l4；And, flight management is surveyed with the Kalman filter in control computer 5 to the height of airship platform 1
Value hmeaCarry out Kalman filtering and export Kalman filtering Height Estimation value hposterior, particle filter therein is based on empty
Between apart from l1、l2、l3、l4, Kalman filtering Height Estimation value hposterior, velocity measurement (umea,vmea,wmea) and attitude survey
Value (φmea,θmea,ψmea) and particle filter horizontal position coordinate value (x is obtained using particle filter algorithmposterior,
yposterior), so as to obtain the fine estimation (x of the position positioning of airship platform 1posterior,yposterior,hposterior)。
Wherein, ultrasonic distance measuring module 4 obtain and export airship platform 1 and the transmitting base station of four fixed indoor locations it
Between space length l1、l2、l3、l4Comprise the following steps：
Four transmitting base stations of ultrasonic distance measuring module 4 launch ultrasonic wave by four base station gain antennas, receive label
Receive the ultrasonic signal of four base station gain antenna transmittings respectively with airship platform gain antenna, obtain airship platform 1 and four
Space length l between the transmitting base station of individual fixed indoor location1、l2、l3、l4；And
Export the space length l between airship platform 1 and the transmitting base station of four fixed indoor locations1、l2、l3、l4。
Wherein, particle filter is based on space length l1、l2、l3、l4, Kalman filtering Height Estimation value hposterior, speed
Spend measured value (umea,vmea,wmea) and attitude measurement value (φmea,θmea,ψmea) and particle filter is obtained using particle filter algorithm
Ripple horizontal position coordinate value (xposterior,yposterior) comprise the following steps：
Generate N number of discrete particle (x at random in whole indoor water plane size range1,y1), (x2,y2) ..., (xN,yN),
Position coordinate value (the x of each discrete particlei,yi) (i=1~N) be the random value of indoor range, for estimating airship platform 1
Horizontal position coordinate value；
The following state input needed for particle filter algorithm is collected, and completes sampling encapsulation：Flight is estimated module 2 and gathered simultaneously
Velocity measurement (the u of the airship platform 1 of outputmea,vmea,wmea) and attitude measurement value (φmea,θmea,ψmea), ultrasonic ranging
The space length l that module 4 is obtained and exported1、l2、l3、l4And Kalman filtering Height Estimation value hposterior；
The fantasy sport amount of each discrete particle is calculated, the airship platform 1 that module 2 is gathered and exported is estimated using flying
Velocity measurement (umea,vmea,wmea) and attitude measurement value (φmea,θmea,ψmea) according to the priori of following formula calculating airship platform 1
Speed, wherein, s () represents that sin (), c () represent cos (),
The discrete particle of subsequent time is predicted by following state equation, discrete particle priori prediction value is obtainedWherein na、baIt is the system Gaussian noise and drift set at random respectively,
And with reference to hposterior, the priori of each discrete particle is obtained apart from observation using following equation
The space length l for obtaining and exporting with reference to ultrasonic distance measuring module 41、l2、l3、l4Calculate each discrete using following formula
The weighted value of particle,
And following formula normalized weight value is used,
Particle filter horizontal position coordinate value (x is obtained using following formulaposterior,yposterior),
It can be assumed that total discrete particle number N is certain, each discrete particle is weighed in the probability that subsequent time occurs for normalization
Weight values ωn, according to normalized weight value ωnSize reselect discrete particle, obtain the new position coordinate of all discrete particles
Repeated after value since second step.In order that those skilled in the art more fully understands the present invention, it is specific real at one
Apply in example, give the illustrative steps that whole Kalman-particle combinations filter location algorithm：
Step 1：The accurate Height Estimation value h of dirigible is drawn by Kalman filtering algorithmposterior.The process passes through card
Thalmann filter 501 is completed.
1) state vector is builtAnd measurement vector Z=hmea(it is used as the dirigible height priori prediction in Fig. 4
Value hprior), state covariance matrix P, and give X initial values X according to actual conditions0, P initial values P0。
2) state priori prediction, builds state-transition matrix A and system noise diagonal matrix Q,
WhereinNoise profile covariance is represented, depending on dirigible displacement state.
Calculating state priori prediction value XpriorAnd the priori prediction value P of error co-variance matrixprior：Xprior=
AXposterior, Pprior=APposteriorAT+Q.During first time iteration, Xposterior=X0And Pposterior=P0。
3) posteriority measurement calibration
Calculation matrix H and measurement noise diagonal matrix R is built,
H=[1,0],WhereinNoise profile covariance is represented, is transported according to the characteristic of laser radar 301 and dirigible
Depending on dynamic state.
Calculate kalman gain K：K=PpriorHT(HPpriorHT+R)-1。
Complete the Posterior estimator of system mode
Xposterior=Xprior+K(Z-HXprior)。
Complete the renewal of error co-variance matrix
Pposterior=(I-KH) Pprior。
4) often complete after a posteriority calibration calculating, wave filter draws the dirigible height posterior estimate close to true value
hposterior(XposteriorSection 1) and altitude rate(XposteriorSection 2).By XposteriorPass to particle
Wave filter 502, in next step PF algorithms.Meanwhile, whenever laser radar 301 obtains new height measurements hmeaWhen, again
Perform it is above-mentioned 2).
Step 2：The accurate horizontal level of dirigible is calculated by particle filter algorithm, the process is in particle filter 502
It is middle to complete.
1) N number of discrete particle is generated at random in whole indoor water plane size range
(x1,y1), (x2,y2) ..., (xN,yN), the position coordinate value (x of each discrete particlei,yi) it is indoor range
Random value, for estimating dirigible horizontal level.
2) the stateful input of institute needed for PF algorithms is collected, and completes sampling encapsulation, is specifically included：Module 2 is estimated in flight
Velocity measurement (the u of collectionmea,vmea,wmea) and attitude measurement value (φmea,θmea,ψmea), ultrasonic distance measuring module 4 is obtained
Range data l1、l2、l3、l4And the dirigible height posterior estimate h that step 1 is calculatedposterior。
3) the fantasy sport amount of each discrete particle is calculated, estimating the measurement data of module 2 according to flight calculates airship platform 1
Priori speed, wherein, s () represent sin (), c () represent cos ().
4) subsequent time particle is predicted by state equation, obtains particle priori prediction value
Wherein na、baIt is the system Gaussian noise and drift set at random respectively.
With reference to hposterior, obtain the priori observation of each discrete particle
5) weighted value for each discrete particle of data calculating that ultrasonic range finder is gathered is combined,
Normalized weight value,
6) according to the weighted value ω of each discrete particlenCarry out resampling.Total population is certain, according to the size of weighted value
Particle is reselected, each discrete particle is ω in the probability that subsequent time occursn.Obtain particle position posteriority updated valueAnd ωn。
7) Posterior estimator is carried out to dirigible horizontal level, obtains particle filter result (xposterior,yposterior),
8) h drawn with reference to step 1posterior, draw fine estimation (x of the indoor airship in solid spaceposterior,
yposterior,hposterior), and then the location tasks of indoor airship are completed, when particle filter 502 receives next group of collection number
According to when, iteration 2 again) to 8), realize the real-time navigation capability to dirigible position.
Compared with prior art, the present invention can significantly improve dirigible to completing indoor airship three-dimensional localization with a wide range of precise
Indoor navigation scope, strengthen stereoscopic monitoring effect, greatly increase the continuous unmanned monitoring time, while simplify guider, drop
Low equipment cost, improve positioning precision, simplify the calculating and programming during regulation and control so that the effect of positioning not by bulk,
The influence of warehouse operation, navigation effect is protruded, algorithm is simple efficiently, interference rejection ability is powerful.
The above, is only the exemplary embodiment of the present invention, but protection scope of the present invention is not limited to
This, any one skilled in the art the invention discloses technical scope in, the change that can readily occur in or replace
Change, should all be included within the scope of the present invention.Therefore, protection scope of the present invention should be with the protection of the claim
Scope is defined.
Claims (4)
1. a kind of indoor airship 3 D locating device based on Kalman and particle filter, it is characterised in that including：Airship platform
(1), module (2), laser-measured height module (3), ultrasonic distance measuring module (4), flight management and control computer are estimated in flight
(5)。
2. device according to claim 1, it is characterised in that
Module (2) is estimated in flight, and it is arranged on the top of airship platform (1), and including three axis accelerometer (201), three axle tops
Spiral shell instrument (202), three axle magnetic compasses (203), are respectively used to gather the velocity measurement (u of airship platform (1)mea,vmea,wmea), appearance
State measured value (φmea,θmea,ψmea)；
Laser-measured height module (3), it is arranged on the underface of airship platform (1), and including laser radar (301) and head
(302), measured for the principle according to laser ranging and export the height measurements h of airship platform (1)mea；
Ultrasonic distance measuring module (4), it include installed in airship platform (1) head position on the lower side reception label (405) and
Airship platform gain antenna (410), it is mounted on below airship platform (1) and neighbouring position in four fixed chambers of clear
The transmitting base station (401,402,403,404) put and four base station gain antennas (406,407,408,409), for obtain and it is defeated
The space length l gone out between the transmitting base station (401,402,403,404) of airship platform (1) and four fixed indoor locations1、l2、
l3、l4；
Flight management and control computer (5), it is arranged on the top of airship platform (1), and the Kalman including being connected with each other
Wave filter (501) and particle filter (502), height measurements of the Kalman filter (501) therein to airship platform (1)
hmeaCarry out Kalman filtering and export Kalman filtering Height Estimation value hposterior, particle filter (502) therein is based on
Space length l1、l2、l3、l4, Kalman filtering Height Estimation value hposterior, velocity measurement (umea,vmea,wmea) and attitude
Measured value (φmea,θmea,ψmea) and particle filter horizontal position coordinate value (x is obtained using particle filter algorithmposterior,
yposterior), so as to obtain the fine estimation (x of the position positioning of airship platform (1)posterior,yposterior,hposterior)。
3. a kind of indoor airship 3-D positioning method based on Kalman and particle filter, it is using described in claim 1 or 2
Indoor airship 3 D locating device realize, methods described includes：
Velocity measurement (the u that module (2) gathers and exports airship platform (1) is estimated in flightmea,vmea,wmea), attitude measurement value
(φmea,θmea,ψmea) and heading measure value；
Laser-measured height module (3) measures according to the principle of laser ranging and exports the height measurements h of airship platform (1)mea；
Ultrasonic distance measuring module (4) obtain and export airship platform (1) and four fixed indoor locations transmitting base station (401,
402nd, 403,404) between space length l1、l2、l3、l4；And
Flight management and height measurements h of the Kalman filter (501) in control computer (5) to airship platform (1)mea
Carry out Kalman filtering and export Kalman filtering Height Estimation value hposterior, particle filter (502) therein is based on space
Apart from l1、l2、l3、l4, Kalman filtering Height Estimation value hposterior, velocity measurement (umea,vmea,wmea) and attitude measurement
It is worth (φmea,θmea,ψmea) and particle filter horizontal position coordinate value (x is obtained using particle filter algorithmposterior,
yposterior), so as to obtain the fine estimation (x of the position positioning of airship platform (1)posterior,yposterior,hposterior)。
4. indoor airship 3-D positioning method according to claim 3, it is characterised in that ultrasonic distance measuring module (4) is obtained
The space for taking and exporting airship platform (1) between the transmitting base station (401,402,403,404) of four fixed indoor locations away from
From l1、l2、l3、l4Comprise the following steps：
Four transmitting base stations (401,402,403,404) of ultrasonic distance measuring module (4) by four base station gain antennas (406,
407th, 408,409) launch ultrasonic wave, receive label (405) and airship platform gain antenna (410) receives four base stations and increased respectively
The ultrasonic signal of beneficial antenna (406,407,408,409) transmitting, obtains the hair of airship platform (1) and four fixed indoor locations
The space length l penetrated between base station (401,402,403,404)1、l2、l3、l4；And
Export airship platform (1) and four fixed indoor locations transmitting base station (401,402,403,404) between space away from
From l1、l2、l3、l4。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710113867.1A CN106950976B (en) | 2017-02-28 | 2017-02-28 | Indoor airship three-dimensional positioning device and method based on Kalman and particle filtering |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710113867.1A CN106950976B (en) | 2017-02-28 | 2017-02-28 | Indoor airship three-dimensional positioning device and method based on Kalman and particle filtering |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN106950976A true CN106950976A (en) | 2017-07-14 |
| CN106950976B CN106950976B (en) | 2020-04-03 |
Family
ID=59467070
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710113867.1A Active CN106950976B (en) | 2017-02-28 | 2017-02-28 | Indoor airship three-dimensional positioning device and method based on Kalman and particle filtering |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN106950976B (en) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107976683A (en) * | 2017-11-24 | 2018-05-01 | 中国人民解放军63653部队 | A kind of method for measuring terrain clearance at the top of dirigible in boathouse in real time with laser radar |
| CN108427431A (en) * | 2018-04-24 | 2018-08-21 | 东莞理工学院 | A kind of four-axle aircraft and its method based on laser scanning map structuring system |
| CN109154830A (en) * | 2017-12-18 | 2019-01-04 | 深圳市大疆创新科技有限公司 | Unmanned aerial vehicle (UAV) control method and unmanned plane |
| CN110673627A (en) * | 2019-09-16 | 2020-01-10 | 广东工业大学 | Forest unmanned aerial vehicle searching method |
| CN113029158A (en) * | 2021-04-26 | 2021-06-25 | 常州大学 | Rotor craft based on laser and sound fusion positioning and positioning method thereof |
| CN114814727A (en) * | 2021-12-29 | 2022-07-29 | 山东科技大学 | Ultra-wideband three-dimensional positioning algorithm with high positioning precision and anti-interference capability |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102818567A (en) * | 2012-08-08 | 2012-12-12 | 浙江大学 | AUV (autonomous underwater vehicle) integrated navigation method integrating Kalman filtering and particle filtering |
| JP2015060431A (en) * | 2013-09-19 | 2015-03-30 | 株式会社Ihi | Directional control method for sensor, and apparatus |
| CN105023277A (en) * | 2014-04-15 | 2015-11-04 | 南京理工大学 | Kalman particle filter tracking method based on complex dynamic scene |
| CN105572638A (en) * | 2016-02-06 | 2016-05-11 | 向杰 | Inertial attitude and ultrasonic ranging-based three-dimensional positioning method and device |
| EP3018447A1 (en) * | 2014-11-07 | 2016-05-11 | Thales | Method for determining a radius of protection associated with a navigation parameter of a hybrid inertial navigation system, and associated system |
| CN105573339A (en) * | 2016-01-16 | 2016-05-11 | 深圳先进技术研究院 | Navigation flight system based on rotor airship |
| CN105704071A (en) * | 2015-07-07 | 2016-06-22 | 大连大学 | Information-sequence-based adaptive fading extended kalman particle filter (AFEKPF) doppler frequency shift estimation method |
| CN105718549A (en) * | 2016-01-16 | 2016-06-29 | 深圳先进技术研究院 | Airship based three-dimensional WiFi (Wireless Fidelity) fingerprint drawing system and method |
| CN106017463A (en) * | 2016-05-26 | 2016-10-12 | 浙江大学 | Aircraft positioning method based on positioning and sensing device |
| CN205971851U (en) * | 2016-08-09 | 2017-02-22 | 上海禾赛光电科技有限公司 | Unmanned aerial vehicle and gaseous remote supervising system with self -align function |
-
2017
- 2017-02-28 CN CN201710113867.1A patent/CN106950976B/en active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102818567A (en) * | 2012-08-08 | 2012-12-12 | 浙江大学 | AUV (autonomous underwater vehicle) integrated navigation method integrating Kalman filtering and particle filtering |
| JP2015060431A (en) * | 2013-09-19 | 2015-03-30 | 株式会社Ihi | Directional control method for sensor, and apparatus |
| CN105023277A (en) * | 2014-04-15 | 2015-11-04 | 南京理工大学 | Kalman particle filter tracking method based on complex dynamic scene |
| EP3018447A1 (en) * | 2014-11-07 | 2016-05-11 | Thales | Method for determining a radius of protection associated with a navigation parameter of a hybrid inertial navigation system, and associated system |
| CN105704071A (en) * | 2015-07-07 | 2016-06-22 | 大连大学 | Information-sequence-based adaptive fading extended kalman particle filter (AFEKPF) doppler frequency shift estimation method |
| CN105573339A (en) * | 2016-01-16 | 2016-05-11 | 深圳先进技术研究院 | Navigation flight system based on rotor airship |
| CN105718549A (en) * | 2016-01-16 | 2016-06-29 | 深圳先进技术研究院 | Airship based three-dimensional WiFi (Wireless Fidelity) fingerprint drawing system and method |
| CN105572638A (en) * | 2016-02-06 | 2016-05-11 | 向杰 | Inertial attitude and ultrasonic ranging-based three-dimensional positioning method and device |
| CN106017463A (en) * | 2016-05-26 | 2016-10-12 | 浙江大学 | Aircraft positioning method based on positioning and sensing device |
| CN205971851U (en) * | 2016-08-09 | 2017-02-22 | 上海禾赛光电科技有限公司 | Unmanned aerial vehicle and gaseous remote supervising system with self -align function |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107976683A (en) * | 2017-11-24 | 2018-05-01 | 中国人民解放军63653部队 | A kind of method for measuring terrain clearance at the top of dirigible in boathouse in real time with laser radar |
| CN107976683B (en) * | 2017-11-24 | 2021-09-03 | 中国人民解放军63653部队 | Method for measuring ground clearance of top of airship in airship warehouse in real time by using laser radar |
| CN109154830A (en) * | 2017-12-18 | 2019-01-04 | 深圳市大疆创新科技有限公司 | Unmanned aerial vehicle (UAV) control method and unmanned plane |
| CN108427431A (en) * | 2018-04-24 | 2018-08-21 | 东莞理工学院 | A kind of four-axle aircraft and its method based on laser scanning map structuring system |
| CN110673627A (en) * | 2019-09-16 | 2020-01-10 | 广东工业大学 | Forest unmanned aerial vehicle searching method |
| CN113029158A (en) * | 2021-04-26 | 2021-06-25 | 常州大学 | Rotor craft based on laser and sound fusion positioning and positioning method thereof |
| CN113029158B (en) * | 2021-04-26 | 2023-09-22 | 常州大学 | Rotary wing aircraft based on laser and sound fusion positioning and positioning method thereof |
| CN114814727A (en) * | 2021-12-29 | 2022-07-29 | 山东科技大学 | Ultra-wideband three-dimensional positioning algorithm with high positioning precision and anti-interference capability |
| CN114814727B (en) * | 2021-12-29 | 2024-05-10 | 山东科技大学 | Ultra-wideband three-dimensional positioning method with high positioning precision and anti-interference capability |
Also Published As
| Publication number | Publication date |
|---|---|
| CN106950976B (en) | 2020-04-03 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106950976A (en) | Indoor airship 3 D locating device and method based on Kalman and particle filter | |
| CN112347840B (en) | Vision sensor laser radar integrated unmanned aerial vehicle positioning and image building device and method | |
| CN108375370B (en) | A kind of complex navigation system towards intelligent patrol unmanned plane | |
| CN110446159A (en) | A kind of system and method for interior unmanned plane accurate positioning and independent navigation | |
| US10534068B2 (en) | Localization system, vehicle control system, and methods thereof | |
| Zhao et al. | Motion measurement using inertial sensors, ultrasonic sensors, and magnetometers with extended kalman filter for data fusion | |
| CN206709853U (en) | Drawing system is synchronously positioned and builds in a kind of multi-rotor unmanned aerial vehicle room | |
| CN112987065B (en) | Multi-sensor-integrated handheld SLAM device and control method thereof | |
| CN107504969A (en) | Four rotor-wing indoor air navigation aids of view-based access control model and inertia combination | |
| CA2988326C (en) | Automated mobile geotechnical mapping | |
| CN110779496B (en) | Three-dimensional map construction system, method, device and storage medium | |
| CN110764531B (en) | Unmanned aerial vehicle formation flying obstacle avoidance method based on laser radar and artificial potential field method | |
| CN107289910A (en) | A kind of light stream alignment system based on TOF | |
| CN111983660A (en) | System and method for positioning quad-rotor unmanned aerial vehicle in GNSS rejection environment | |
| CN110286390A (en) | A kind of specified path wind measurement method, device and windfinding radar scaling method | |
| US20220189318A1 (en) | Aircraft sensor system synchronization | |
| CN109508040A (en) | A kind of unmanned plane precise positioning landing system and method | |
| CN113063401A (en) | Unmanned aerial vehicle aerial survey system | |
| KR102169512B1 (en) | Localization system and method using uav and rfid platform | |
| Huang et al. | Research on UAV flight performance test method based on dual antenna GPS/ins integrated system | |
| Guo et al. | The usefulness of sensor fusion for unmanned aerial vehicle indoor positioning | |
| CN108369086A (en) | Method and system for servicing object | |
| CN114325666A (en) | Coherent heterodyne wind measuring radar correction method under high-speed motion | |
| Liu et al. | A tightly-coupled method of lidar-inertial based on complementary filtering | |
| CN113721188B (en) | Multi-unmanned aerial vehicle self-positioning and target positioning method under refusing environment |
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 | ||
| CB03 | Change of inventor or designer information |
Inventor after: Zhao Lei Inventor after: Guo Ta Inventor before: Zhao Lei |
|
| CB03 | Change of inventor or designer information |