CN106950976B - Indoor airship three-dimensional positioning device and method based on Kalman and particle filtering - Google Patents

Indoor airship three-dimensional positioning device and method based on Kalman and particle filtering Download PDF

Info

Publication number
CN106950976B
CN106950976B CN201710113867.1A CN201710113867A CN106950976B CN 106950976 B CN106950976 B CN 106950976B CN 201710113867 A CN201710113867 A CN 201710113867A CN 106950976 B CN106950976 B CN 106950976B
Authority
CN
China
Prior art keywords
mea
airship
posterior
platform
indoor
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.)
Active
Application number
CN201710113867.1A
Other languages
Chinese (zh)
Other versions
CN106950976A (en
Inventor
赵磊
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Infinite Space Technology Co ltd
Original Assignee
Beijing Infinite Space Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing Infinite Space Technology Co ltd filed Critical Beijing Infinite Space Technology Co ltd
Priority to CN201710113867.1A priority Critical patent/CN106950976B/en
Publication of CN106950976A publication Critical patent/CN106950976A/en
Application granted granted Critical
Publication of CN106950976B publication Critical patent/CN106950976B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (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 invention provides an indoor airship three-dimensional positioning device and method based on Kalman and particle filtering. The method comprises the steps of carrying out Kalman filtering on a height measurement value of an airship platform by adopting a Kalman filter and outputting a Kalman filtering height estimation value, and obtaining a particle filtering horizontal position coordinate value by adopting a particle filter based on a space distance, the Kalman filtering height estimation value, a speed measurement value and an attitude measurement value and adopting a particle filtering algorithm. By the device and the method, more accurate indoor positioning results are obtained.

Description

Indoor airship three-dimensional positioning device and method based on Kalman and particle filtering
Technical Field
The invention relates to the technical field of indoor navigation and positioning of unmanned aerial vehicles, in particular to a three-dimensional positioning device and method of an indoor airship based on Kalman and particle filtering.
Background
In indoor stereo monitoring, especially in large warehouse management, people are currently increasingly using aircrafts that can fly at low speed and hover autonomously to accomplish tasks. However, similar to the traditional low-speed unmanned aerial vehicle such as a helicopter and a multi-rotor, the system architecture is complex, the endurance time is poor, and the like, and the long-time continuous monitoring task cannot be executed. And a small indoor airship such as that disclosed in CN201120573237.0 can solve the above problems well based on its floating characteristic.
The current main indoor positioning technologies include an infrared technology, a radio frequency identification technology, an ultra wide band technology, an optical tracking technology and an ultrasonic technology. Because different positioning technologies are adopted to directly influence the hardware level of a positioning system and the construction of a navigation algorithm, people research a plurality of different indoor navigation and positioning schemes of the unmanned aerial vehicle. The existing positioning product with mature application mainly adopts an ultrasonic technology, and has the advantages of low cost, convenient installation and use, no influence of light and smoke and visual time information. For large warehouse management, the technology has obvious advantages.
At present, many scholars are exploring how to combine the ultrasonic technology with the indoor positioning technology (for example, please refer to CN201410330642.8), and make an indoor unmanned aerial vehicle navigation positioning system based on ultrasonic ranging, but due to the limitations of low ultrasonic precision and large measurement noise, the positioning effect is not ideal because of lack of effective unification in specific algorithm design, device layout, and technology implementation.
Moreover, the traditional Kalman filtering algorithm is simple in structure and small in calculation amount, but is only suitable for Gaussian noise environment, and the state estimation deviation is large and divergence is easy to generate on occasions with non-Gaussian noise environment and high non-linearity degree. Although the particle filter algorithm is not limited by the nonlinearity degree and the Gaussian noise assumption, the calculation amount is large, and for a large-range three-dimensional space environment, a large amount of particle estimation is needed to realize the system state convergence, so the calculation efficiency is low, and the system real-time performance is poor.
Therefore, for the ultrasonic-based large warehouse positioning, which is an occasion with strong nonlinearity and large stereo space size, the traditional single filtering algorithm is difficult to meet the requirements of people, and an ultrasonic-based indoor positioning algorithm with better performance and higher efficiency is needed (for example, please refer to CN 201410191921.0). In order to obtain more accurate indoor positioning results in a large indoor environment, a new technical scheme needs to be provided.
Disclosure of Invention
The purpose of the invention is realized by the following technical scheme.
The invention provides an indoor airship three-dimensional positioning device based on Kalman and particle filtering, which comprises: the device comprises an airship platform, a flight estimation module, a laser height measurement module, an ultrasonic distance measurement module and a flight management and control computer.
According to the indoor airship three-dimensional positioning device, the flight estimation module is installed at the top of the airship platform and comprises a three-axis accelerometer, a three-axis gyroscope and a three-axis magnetic compass which are respectively used for acquiring the speed measurement value (u) of the airship platformmea,vmea,wmea) Attitude measurement value (phi)mea,θmea,ψmea) And a course measurement; wherein the laser height measurement module is arranged under the airship platform, comprises a laser radar and a holder and is used for measuring and outputting a height measurement value h of the airship platform according to the laser ranging principlemea(ii) a The ultrasonic ranging module comprises a receiving tag and an airship platform gain antenna which are arranged at the lower position of the head of the airship platform, and transmitting base stations and four base station gain antennas which are arranged at four fixed indoor positions which are positioned below the airship platform and are free of obstacles nearby, and is used for acquiring and outputting the spatial distances l between the airship platform and the transmitting base stations at the four fixed indoor positions1、l2、l3、l4(ii) a Wherein the flight management and control computer is mounted on top of the airship platform and comprises a Kalman filter and a particle filter connected to each other, wherein the Kalman filter measures the altitude h of the airship platformmeaPerforming Kalman filtering and outputting a Kalman filtering height estimation value hposteriorWherein the particle filter is based on the spatial distance l1、l2、l3、l4Kalman filtering height estimation value hposteriorVelocity measurement (u)mea,vmea,wmea) And attitude measurement value (phi)mea,θmea,ψmea) And obtaining a horizontal position coordinate value (x) of the particle filter by adopting a particle filter algorithmposterior,yposterior) To obtain an accurate estimate (x) of the position location of the airship platformposterior,yposterior,hposterior)。
The invention also provides an indoor airship three-dimensional positioning method based on Kalman and particle filtering, which is realized by adopting the indoor airship three-dimensional positioning device, and the method comprises the following steps: the flight estimation module collects and outputs a speed measurement (u) of the airship platformmea,vmea,wmea) Attitude measurement value (phi)mea,θmea,ψmea) And a course measurement; the laser height measurement module measures and outputs a height measurement value h of the airship platform according to the principle of laser distance measurementmea(ii) a Airship platform acquired and output by ultrasonic ranging moduleSpatial distances l from four transmitting base stations fixed indoor positions1、l2、l3、l4(ii) a And, a measurement h of the altitude of the airship platform by a Kalman filter in the flight management and control computermeaPerforming Kalman filtering and outputting a Kalman filtering height estimation value hposteriorWherein the particle filter is based on the spatial distance l1、l2、l3、l4Kalman filtering height estimation value hposteriorVelocity measurement (u)mea,vmea,wmea) And attitude measurement value (phi)mea,θmea,ψmea) And obtaining a horizontal position coordinate value (x) of the particle filter by adopting a particle filter algorithmposterior,yposterior) To obtain an accurate estimate (x) of the position location of the airship platformposterior,yposterior,hposterior)。
According to the three-dimensional positioning method of the indoor airship, the ultrasonic ranging module obtains and outputs the spatial distances l between the airship platform and the four transmitting base stations for fixing indoor positions1、l2、l3、l4The method comprises the following steps:
the four transmitting base stations of the ultrasonic ranging module transmit ultrasonic waves through the four base station gain antennas, the receiving tag and the airship platform gain antenna respectively receive ultrasonic signals transmitted by the four base station gain antennas, and the spatial distances l between the airship platform and the four transmitting base stations at fixed indoor positions are obtained1、l2、l3、l4(ii) a And
outputting spatial distances l between airship platform and four transmitting base stations fixing indoor positions1、l2、l3、l4。
The invention has the advantages that: the three-dimensional positioning device and the method for the indoor airship combine the traditional Kalman filtering algorithm and the particle filtering algorithm to realize (1) construction of a three-dimensional navigation and positioning framework of the indoor airship; (2) the more accurate estimation of the height of the airship is completed through a Kalman filtering algorithm; (3) based on the ultrasonic ranging result and more accurate Kalman height estimation, more accurate estimation of the horizontal position of the indoor airship is further completed through a particle filter algorithm; (4) and integrating the height estimation result and the position estimation result to more accurately complete the three-dimensional positioning task of the indoor airship.
Drawings
Various other advantages and benefits will become apparent to those of ordinary skill in the art upon reading the following detailed description of the specific embodiments. The drawings are only for purposes of illustrating the particular embodiments and are not to be construed as limiting the invention. Also, like reference numerals are used to refer to like parts throughout the drawings. In the drawings:
fig. 1 is a schematic diagram of an indoor airship three-dimensional positioning device based on kalman and particle filtering according to the invention.
Fig. 2 is a partially enlarged schematic view of a flight estimation module and a flight management and control computer in the three-dimensional positioning device of the indoor airship according to the invention.
Fig. 3 is a partially enlarged schematic view of a laser height measuring module in the three-dimensional positioning device of the indoor airship according to the invention.
Fig. 4 is a schematic diagram of the operation of the system in the three-dimensional positioning device of the indoor airship according to the invention.
Fig. 5 is a block diagram of a multi-sensor information acquisition and fusion system in the three-dimensional positioning device of the indoor airship according to the invention.
Detailed Description
Exemplary embodiments of the present invention will be described in more detail below with reference to the accompanying drawings. While exemplary embodiments of the invention are shown in the drawings, it should be understood that the invention can be embodied in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the scope of the invention to those skilled in the art.
Fig. 1 is a schematic diagram of an indoor airship three-dimensional positioning device based on kalman and particle filtering according to the invention.
As shown in fig. 1, the proposed indoor airship three-dimensional positioning device comprises an airship platform 1, a flight estimation module 2, a laser altimetry module 3, an ultrasonic ranging module 4 and a flight management and control computer 5.
The airship platform 1 is a task platform of the whole indoor airship and can carry corresponding airborne task modules such as a manipulator, a detector, a camera and the like according to task requirements. The receiving tag 405, the airship platform gain antenna 410 and the flight management and control computer 5 in the flight estimation module 2, the laser height measurement module 3 and the ultrasonic ranging module 4 are all installed on the airship platform 1. The flight estimation module 2 and the flight management and control computer 5 are installed on the top of the airship platform 1 together, the laser height measurement module 3 is installed under the airship platform 1, and the receiving tag 405 and the airship platform gain antenna 410 in the ultrasonic ranging module 4 are installed at the position of the head of the airship platform 1, which is lower. And the four transmitting base stations 401, 402, 403 and 404 and the gain antennas 406, 407, 408 and 409 in the ultrasonic ranging module 4 are installed at four fixed points in the room, and ensure that no obstacles exist nearby and are located below the airship platform 1.
Fig. 2 is a partially enlarged schematic view of a flight estimation module and a flight management and control computer in the three-dimensional positioning device of the indoor airship according to the invention.
As shown in fig. 2, the flight estimation module 2 is installed on top of the airship platform 1, and includes a three-axis accelerometer 201, a three-axis gyroscope 202, and a three-axis magnetic compass 203, which are respectively used for acquiring a speed measurement value (u) of the airship platform 1mea,vmea,wmea) Attitude measurement value (phi)mea,θmea,ψmea) And a heading measurement, and passed to the particle filter 502. Because there is no high-precision GPS signal calibration indoors, the airship platform state measurement value acquired by the sensor is inaccurate data with high noise and large drift, and the later-described Kalman-particle combined filter algorithm is required to perform posterior calibration. As shown in fig. 2, the flight management and control computer 5 includes a kalman filter 501 and a particle filter 502.
Fig. 3 is a partially enlarged schematic view of a laser height measuring module in the three-dimensional positioning device of the indoor airship according to the invention.
As shown in fig. 3, the laser altimeter module 3 is installed right below the airship platform 1, and includes a laser radar 301 and a pan-tilt 302, the three-degree-of-freedom pan-tilt 302 ensures that the laser radar 301 always faces the ground in the whole flight process, and measures and outputs the altitude measurement value h of the airship platform 1 according to the principle of laser rangingmeaAnd passed to the kalman filter 501. Because the airship platform 1 vibrates and the intrinsic error of the sensor is caused, the data can not be directly used for positioning the airship platform 1, the Kalman filter 501 completes the prior prediction and the posterior estimation of the height of the airship platform 1 through a KF algorithm, and the accurate estimated value h of the height of the airship platform 1 is obtainedposterior。
As shown in fig. 3, the receiving tag 405 and the airship platform gain antenna 410 in the ultrasonic ranging module 4 respectively receive ultrasonic signals transmitted by four base station gain antennas 406, 407, 408, 409, so as to obtain spatial distances l between the airship platform 1 and four fixed indoor position transmitting base stations 401, 402, 403, 4041、l2、l3、l4。
Fig. 4 is a schematic diagram of the operation of the system in the indoor airship three-dimensional positioning device according to the invention, including the principle of particle filtering.
As shown in fig. 4, the flight management and control computer 5 includes a kalman filter 501 and a particle filter 502 connected to each other. Kalman filter 501 measures the altitude h of the airship platform 1meaPerforms Kalman filtering and outputs a Kalman filtering height estimate h to the particle filter 502posterior. Particle filter 502 is based on spatial distance l1、l2、l3、l4Kalman filtering height estimation value hposteriorVelocity measurement (u)mea,vmea,wmea) (as a priori predictor (u) in FIG. 4prior，vprior，wprior) And attitude measurement value (phi)mea,θmea,ψmea) (as a priori predictor (phi) in FIG. 4prior，θprior，Ψprior) And obtain grains using a particle filtering algorithmSub-filter horizontal position coordinate value (x)posterior,yposterior) To obtain an accurate estimate (x) of the position location of the airship platform 1posterior,yposterior,hposterior) (corresponding to x in FIG. 4)posterior，yposterior，zposterior))。
Alternatively, the Kalman filter 501 will calculate an accurate estimate h of the airship's heightposteriorTo the particle filter 502. When the flight estimation module 2 obtains the measured state value (u) of the airship platform 1mea,vmea,wmea)、(φmea,θmea,ψmea) After being transferred to the particle filter 502, the data needs to be calibrated a posteriori by other data information to obtain an accurate estimation value (x) of the position of the airship platform 1posterior,yposterior,zposterior). The specific operating principle is as follows: four transmitting base stations 401, 402, 403 and 404 of the ultrasonic ranging module 4 transmit ultrasonic waves through gain antennas 406, 407, 408 and 409, a receiving tag 405 installed under the head of the airship platform 1 is combined with the gain antenna 410 to receive ultrasonic signals transmitted by the four transmitting base stations 401, 402, 403 and 404 respectively, and the spatial distance l between the airship platform 1 and the four base stations is obtained1、l2、l3、l4And sent to the particle filter 502, the particle filter 502 passes l1、l2、l3、l4And hposteriorTo (u)mea,vmea,wmea)、(φmea,θmea,ψmea) Carrying out posterior calibration, and calculating the accurate estimation position (x) of the airship platform 1 by a PF algorithmposterior,yposterior,zposterior)。
Fig. 5 is a block diagram of a multi-sensor information acquisition and fusion system in the three-dimensional positioning device of the indoor airship according to the invention.
As shown in fig. 5, the multi-sensor receiving part in the three-dimensional positioning device for the indoor airship according to the present invention uses a digital signal processor DSP as the flight management and control computer 5, and uses a texas instruments TMS320C6713 chip, and the multi-channel serial port expansion board is composed of two four-channel expansion chips TL16C554A and some auxiliary devices. The processor is embedded with a sensor driving module, an ultrasonic signal processing module, a sensor data fusion module and a fusion algorithm module, and is connected with the airborne laser radar 301 through an on-chip interface and a serial port of the multi-channel serial port expansion board. Wherein, the three-axis accelerometer 201 and the three-axis gyroscope 202 are connected with the DSP through an I2C bus; the three-axis magnetic compass 203 is connected with the DSP through an A/D conversion interface; the laser radar 301 and the ultrasonic receiving tag 405 are connected with the multichannel expansion board through serial ports. Various sensor information is transmitted to the multi-sensor information acquisition and fusion system through corresponding interfaces and serial ports of the multi-channel serial port expansion board, on one hand, data information is sampled and packaged, on the other hand, important data for positioning the airship are generated through a Kalman-particle combined filtering algorithm, and the data are transmitted to the intelligent decision-making system through a CAN bus.
The invention also provides an indoor airship three-dimensional positioning method based on Kalman and particle filtering, which adopts the indoor airship three-dimensional positioning device and is described in detail below. First, a description of relevant symbols is given here (note: the algorithm is based on the international universal inertial coordinate system and the body coordinate system):
(umea,vmea,wmea): the flight estimation module 2 acquires a speed measurement value;
(φmea,θmea,ψmea): the flight estimation module 2 acquires an attitude measurement value;
hmea: height measurements acquired by the lidar 301;
dt: two sampling time intervals;
(xbi,ybi,zbi) (i ═ 1,2,3, 4): the positions of four transmitting base stations (401, 402, 403, 404) in the ultrasonic ranging module 4 in the indoor space respectively;
li(i ═ 1,2,3, 4): the linear distances between the airship acquired by the receiving tag 405 in the ultrasonic ranging module 4 and the four transmitting base stations (401, 402, 403 and 404) are respectively set;
Figure BDA0001235126930000071
a KF state vector;
Z＝hpiror: KF measures a vector;
p: a KF state covariance matrix;
a: a KF state transition matrix;
q: KF system noise diagonal array;
h: a KF measurement matrix;
r: measuring a noise diagonal matrix;
k: a Kalman gain;
hprior: the airship height prior predicted value;
hposterior: an airship height posterior estimation value;
(xprior,yprior): the airship horizontal position priori predicted value;
(xposterior,yposterior): a posteriori estimated value of the horizontal position of the airship;
n: the number of PF particles;
na: PF system Gaussian noise;
ba: PF system drift;
Figure BDA0001235126930000081
p particle prior observed values;
ωn: PF particle weight value.
The invention discloses a Kalman and particle filter based indoor airship three-dimensional positioning method, which comprises the following steps:
the flight estimation module 2 collects and outputs the speed measurement (u) of the airship platform 1mea,vmea,wmea) And attitude measurement value (phi)mea,θmea,ψmea) (ii) a The laser height measurement module 3 measures and outputs the height measurement value h of the airship platform 1 according to the principle of laser distance measurementmea(ii) a The ultrasonic ranging module 4 acquires and outputs the spatial distances l between the airship platform 1 and the four transmitting base stations which fix indoor positions1、l2、l3、l4(ii) a And the height measurement h of the airship platform 1 by the kalman filter in the flight management and control computer 5meaPerforming Kalman filtering and outputting a Kalman filtering height estimation value hposteriorWherein the particle filter is based on the spatial distance l1、l2、l3、l4Kalman filtering height estimation value hposteriorVelocity measurement (u)mea,vmea,wmea) And attitude measurement value (phi)mea,θmea,ψmea) And obtaining a horizontal position coordinate value (x) of the particle filter by adopting a particle filter algorithmposterior,yposterior) To obtain an accurate estimate (x) of the position location of the airship platform 1posterior,yposterior,hposterior)。
Wherein, the ultrasonic ranging module 4 acquires and outputs the space distances l between the airship platform 1 and the four transmitting base stations which fix the indoor positions1、l2、l3、l4The method comprises the following steps:
the four transmitting base stations of the ultrasonic ranging module 4 transmit ultrasonic waves through the four base station gain antennas, the receiving tag and the airship platform gain antenna respectively receive ultrasonic signals transmitted by the four base station gain antennas, and the spatial distances l between the airship platform 1 and the four transmitting base stations at fixed indoor positions are obtained1、l2、l3、l4(ii) a And
outputting spatial distances l between the airship platform 1 and the four transmitting base stations which fix the indoor positions1、l2、l3、l4。
Wherein the particle filter is based on the spatial distance l1、l2、l3、l4Kalman filtering height estimation value hposteriorVelocity measurement (u)mea,vmea,wmea) And attitude measurement value (phi)mea,θmea,ψmea) And obtaining a horizontal position coordinate value (x) of the particle filter by adopting a particle filter algorithmposterior,yposterior) The method comprises the following steps:
randomly generating N discrete particles (x) in the whole indoor horizontal plane size range1,y1)，(x2,y2)…，(xN,yN) Position coordinate value (x) of each discrete particlei,yi) (i is 1 to N) are all random values of an indoor range, and are used for estimating a horizontal position coordinate value of the airship platform 1;
collecting the following state inputs required by a particle filtering algorithm, and completing sampling and packaging: the flight estimation module 2 collects and outputs the speed measurement value (u) of the airship platform 1mea,vmea,wmea) And attitude measurement value (phi)mea,θmea,ψmea) And the spatial distance l acquired and output by the ultrasonic ranging module 41、l2、l3、l4And a Kalman filter height estimate hposterior；
Calculating the virtual motion amount of each discrete particle, and acquiring and outputting a speed measurement value (u) of the airship platform 1 by using a flight estimation module 2mea,vmea,wmea) And attitude measurement value (phi)mea,θmea,ψmea) The a priori velocity of the airship platform 1 is calculated according to the following formula, where s () denotes sin (), c () denotes cos (),
Figure BDA0001235126930000091
predicting the discrete particles at the next moment by the following state equation to obtain a discrete particle priori predicted value
Figure BDA0001235126930000092
Wherein n isa、baRespectively system gaussian noise and drift that are set randomly,
Figure BDA0001235126930000093
and in combination with hposteriorThe prior range observations for each discrete particle are determined using the following formula
Figure BDA0001235126930000094
Spatial distance l acquired and output by combining ultrasonic ranging module 41、l2、l3、l4The weight value for each discrete particle is calculated using the following equation,
Figure BDA0001235126930000101
and the weight values are normalized using the following equation,
Figure BDA0001235126930000102
the particle filter horizontal position coordinate value (x) is obtained using the following formulaposterior,yposterior)，
Figure BDA0001235126930000103
It can be assumed that the total number N of discrete particles is constant, and the probability of each discrete particle appearing at the next time is the normalized weight value ωnAccording to the normalized weight value omeganAnd (3) reselecting the discrete particles, and repeating from the second step after obtaining new position coordinate values of all the discrete particles. In order to enable a person skilled in the art to better understand the present invention, in a specific embodiment, exemplary steps of the entire kalman-particle combination filter positioning algorithm are given:
step 1: obtaining accurate height estimation value h of airship through Kalman filtering algorithmposterior. This process is accomplished by a kalman filter 501.
1) Constructing state vectors
Figure BDA0001235126930000104
And the measurement vector Z ═ hmea(as airship altitude a priori predicted value h in FIG. 4prior) State covariance momentArray P, and setting X initial value X according to actual conditions0P initial value P0。
2) State prior prediction, constructing a state transition matrix A and a system noise diagonal matrix Q,
Figure BDA0001235126930000105
wherein
Figure BDA0001235126930000106
Representing the covariance of the noise distribution, depending on the state of motion of the airship itself.
Calculating state prior predicted value XpriorAnd the prior prediction value P of the error covariance matrixprior：Xprior＝AXposterior，Pprior＝APposteriorAT+ Q. On the first iteration, Xposterior＝X0And P isposterior＝P0。
3) Posterior measurement calibration
Constructing a measurement matrix H and a measurement noise diagonal matrix R,
H＝[1,0]，
Figure BDA0001235126930000111
wherein
Figure BDA0001235126930000112
Represents the noise distribution covariance, depending on the characteristics of the lidar 301 and the state of motion of the airship.
Calculating a Kalman gain K: k is PpriorHT(HPpriorHT+R)-1。
Performing a posteriori estimation of the state of the system
Xposterior＝Xprior+K(Z-HXprior)。
Completing the update of the error covariance matrix
Pposterior＝(I-KH)Pprior。
4) A posteriori calibration at each completionAfter calculation, the filter obtains an airship height posterior estimated value h close to a true valueposterior(XposteriorFirst term) and rate of change of height
Figure BDA0001235126930000113
(XposteriorThe second term). Mixing XposteriorPassed to the particle filter 502 for use in the next PF algorithm. At the same time, each time lidar 301 obtains a new height measurement hmeaWhen the above 2) is executed again.
Step 2: the exact horizontal position of the airship is calculated by a particle filter algorithm, which is done in a particle filter 502.
1) Randomly generating N discrete particles in the whole indoor horizontal plane size range
(x1,y1)，(x2,y2)…，(xN,yN) Position coordinate value (x) of each discrete particlei,yi) Are all random values of the indoor range and are used for estimating the horizontal position of the airship.
2) Collecting all state inputs required by a PF algorithm, and completing sampling encapsulation, specifically comprising: the speed measurement (u) collected by the flight estimation module 2mea,vmea,wmea) And attitude measurement (phi)mea,θmea,ψmea) Distance data l acquired by ultrasonic ranging module 41、l2、l3、l4And the calculated airship height posterior estimated value h in the step 1posterior。
3) And calculating the virtual motion amount of each discrete particle, and calculating the prior speed of the airship platform 1 according to the measurement data of the flight estimation module 2, wherein s (eta)) represents sin (eta), and c (eta)) represents cos (eta).
Figure BDA0001235126930000121
4) Predicting the particles at the next moment through a state equation to obtain a particle prior predicted value
Figure BDA0001235126930000122
Wherein n isa、baRespectively, system gaussian noise and drift, which are set randomly.
Figure BDA0001235126930000123
Binding hposteriorDetermining a priori observations of each discrete particle
Figure BDA0001235126930000124
5) Calculating the weight value of each discrete particle by combining the data collected by the ultrasonic range finder,
Figure BDA0001235126930000125
the weight values are normalized by the weight values,
Figure BDA0001235126930000126
6) according to the weight value omega of each discrete particlenResampling is performed. The total number of particles is fixed, the particles are reselected according to the weight value, and the probability of each discrete particle appearing at the next moment is omegan. Obtaining the posterior updated value of the particle position
Figure BDA0001235126930000127
And ωn。
7) Carrying out posterior estimation on the horizontal position of the airship to obtain a particle filtering result (x)posterior,yposterior)，
Figure BDA0001235126930000128
8) Combining h obtained in step 1posteriorObtaining the accurate estimated value (x) of the indoor airship in the three-dimensional spaceposterior,yposterior,hposterior)，And then completing the positioning task of the indoor airship, and iterating 2) to 8) again when the particle filter 502 receives the next set of acquired data, thereby realizing the real-time positioning function of the position of the airship.
Compared with the prior art, the invention can complete large-scale accurate three-dimensional positioning of the indoor airship, remarkably improve the indoor navigation range of the airship, enhance the three-dimensional monitoring effect, greatly increase the continuous unmanned monitoring time, simplify the navigation device, reduce the equipment cost, improve the positioning accuracy, simplify the calculation and programming during regulation and control, ensure that the positioning effect is not influenced by the space size and warehouse operation, and has outstanding navigation effect, simple and efficient algorithm and strong anti-interference capability.
The above description is only an exemplary embodiment of the present invention, but the scope of the present invention is not limited thereto, and any changes or substitutions that can be easily conceived by those skilled in the art within the technical scope of the present invention are included in the scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the appended claims.

Claims (3)

1. The utility model provides an indoor airship three-dimensional positioning device based on kalman and particle filter, its characterized in that includes: an airship platform (1), a flight estimation module (2), a laser height measurement module (3), an ultrasonic ranging module (4) and a flight management and control computer (5), which is characterized in that,
the flight prediction module (2) is arranged at the top of the airship platform (1), comprises a three-axis accelerometer (201), a three-axis gyroscope (202) and a three-axis magnetic compass (203), and is respectively used for acquiring a speed measurement value (u) of the airship platform (1)mea,vmea,wmea) Attitude measurement value (phi)mea,θmea,ψmea)；
The laser height measurement module (3) is arranged right below the airship platform (1), comprises a laser radar (301) and a cloud platform (302) and is used for measuring and outputting a height measurement value h of the airship platform (1) according to the principle of laser rangingmea；
An ultrasonic ranging module (4) which is installed on the airshipA receiving tag (405) and an airship platform gain antenna (410) at a head-down position of the station (1), four base station gain antennas (406, 407, 408, 409) and transmitting base stations (401, 403, 404) installed at four fixed indoor positions under the airship platform (1) and without obstacles nearby, for acquiring and outputting spatial distances l between the airship platform (1) and the transmitting base stations (401, 402, 403, 404) at the four fixed indoor positions1、l2、l3、l4；
A flight management and control computer (5) mounted on top of the airship platform (1) and comprising a Kalman filter (501) and a particle filter (502) connected to each other, wherein the Kalman filter (501) measures the altitude h of the airship platform (1)meaPerforming Kalman filtering and outputting a Kalman filtering height estimation value hposteriorWherein the particle filter (502) is based on the spatial distance l1、l2、l3、l4Kalman filtering height estimation value hposteriorVelocity measurement (u)mea,vmea,wmea) And attitude measurement value (phi)mea,θmea,ψmea) And obtaining a horizontal position coordinate value (x) of the particle filter by adopting a particle filter algorithmposterior,yposterior) To obtain an accurate estimate (x) of the position location of the airship platform (1)posterior,yposterior,hposterior)。
2. An indoor airship three-dimensional positioning method based on Kalman and particle filtering, which is realized by adopting the indoor airship three-dimensional positioning device of claim 1, and the method comprises the following steps:
the flight estimation module (2) collects and outputs a speed measurement value (u) of the airship platform (1)mea,vmea,wmea) Attitude measurement value (phi)mea,θmea,ψmea) And a course measurement;
the laser height measurement module (3) measures and outputs the height measurement value h of the airship platform (1) according to the principle of laser rangingmea；
Ultrasonic ranging module (4) acquisitionAnd outputting the spatial distances l between the airship platform (1) and the four fixed indoor-position transmitting base stations (401, 402, 403, 404)1、l2、l3、l4(ii) a And
altitude measurement h of airship platform (1) by Kalman filter (501) in flight management and control computer (5)meaPerforming Kalman filtering and outputting a Kalman filtering height estimation value hposteriorWherein the particle filter (502) is based on the spatial distance l1、l2、l3、l4Kalman filtering height estimation value hposteriorVelocity measurement (u)mea,vmea,wmea) And attitude measurement value (phi)mea,θmea,ψmea) And obtaining a horizontal position coordinate value (x) of the particle filter by adopting a particle filter algorithmposterior,yposterior) To obtain an accurate estimate (x) of the position location of the airship platform (1)posterior,yposterior,hposterior)。
3. An indoor airship three-dimensional positioning method as claimed in claim 2, characterized in that the ultrasonic ranging module (4) acquires and outputs the spatial distances l between the airship platform (1) and the four transmitting base stations (401, 402, 403, 404) fixing the indoor position1、l2、l3、l4The method comprises the following steps:
four transmitting base stations (401, 402, 403 and 404) of an ultrasonic ranging module (4) transmit ultrasonic waves through four base station gain antennas (406, 407, 408 and 409), a receiving tag (405) and an airship platform gain antenna (410) respectively receive ultrasonic signals transmitted by the four base station gain antennas (406, 407, 408 and 409), and spatial distances l between an airship platform (1) and the four transmitting base stations (401, 402, 403 and 404) at fixed indoor positions are obtained1、l2、l3、l4(ii) a And
outputting spatial distances l between an airship platform (1) and four fixed indoor-position transmitting base stations (401, 402, 403, 404)1、l2、l3、l4。
CN201710113867.1A 2017-02-28 2017-02-28 Indoor airship three-dimensional positioning device and method based on Kalman and particle filtering Active CN106950976B (en)

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 CN106950976A (en) 2017-07-14
CN106950976B true 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)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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
WO2019119240A1 (en) * 2017-12-18 2019-06-27 深圳市大疆创新科技有限公司 Unmanned aerial vehicle control method and unmanned aerial vehicle
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
CN113029158B (en) * 2021-04-26 2023-09-22 常州大学 Rotary wing aircraft based on laser and sound fusion positioning and positioning method thereof
CN114814727B (en) * 2021-12-29 2024-05-10 山东科技大学 Ultra-wideband three-dimensional positioning method with high positioning precision and anti-interference capability

Citations (5)

* Cited by examiner, † Cited by third party
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

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105718549A (en) * 2016-01-16 2016-06-29 深圳先进技术研究院 Airship based three-dimensional WiFi (Wireless Fidelity) fingerprint drawing system and method
CN105573339B (en) * 2016-01-16 2019-01-11 深圳先进技术研究院 A kind of navigation flight system based on rotor dirigible
CN105572638A (en) * 2016-02-06 2016-05-11 向杰 Inertial attitude and ultrasonic ranging-based three-dimensional positioning method and device
CN106017463B (en) * 2016-05-26 2019-02-26 浙江大学 A kind of Aerial vehicle position method based on orientation sensing device
CN205971851U (en) * 2016-08-09 2017-02-22 上海禾赛光电科技有限公司 Unmanned aerial vehicle and gaseous remote supervising system with self -align function

Patent Citations (5)

* Cited by examiner, † Cited by third party
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

Also Published As

Publication number Publication date
CN106950976A (en) 2017-07-14

Similar Documents

Publication Publication Date Title
CN106950976B (en) Indoor airship three-dimensional positioning device and method based on Kalman and particle filtering
US20210293977A1 (en) Systems and methods for positioning of uav
US10914590B2 (en) Methods and systems for determining a state of an unmanned aerial vehicle
RU2704614C1 (en) Flight control system of an unmanned aerial vehicle with differential positioning based on the network cors
US10534068B2 (en) Localization system, vehicle control system, and methods thereof
CN108521670B (en) UWB communication and positioning based method for multi-machine-oriented close formation flight and integrated system
CN110446159A (en) A kind of system and method for interior unmanned plane accurate positioning and independent navigation
CN206709853U (en) Drawing system is synchronously positioned and builds in a kind of multi-rotor unmanned aerial vehicle room
CN108520640B (en) Ultra-wideband-based unmanned aerial vehicle navigation method, navigation equipment and unmanned aerial vehicle
CN111983660A (en) System and method for positioning quad-rotor unmanned aerial vehicle in GNSS rejection environment
CN110779496B (en) Three-dimensional map construction system, method, device and storage medium
CN113419235A (en) Unmanned aerial vehicle positioning method based on millimeter wave radar
CN110658828A (en) Autonomous landform detection method and unmanned aerial vehicle
CN111694001A (en) Real-time distance measurement positioning system for unmanned aerial vehicle
CN115902930A (en) Unmanned aerial vehicle room built-in map and positioning method for ship detection
JP2019007964A (en) System and method for estimating position of uav
CN105928512A (en) Geomagnetic field-based indoor positioning method
CN114035150A (en) Radio frequency source direction finding device and positioning method based on unmanned aerial vehicle lift-off platform
CN111694006A (en) Navigation sensing system for indoor unmanned system
CN113063401A (en) Unmanned aerial vehicle aerial survey system
CN116859332A (en) System and method for positioning civil aviation radio interference source by multiple unmanned aerial vehicles
CN110388917B (en) Aircraft monocular vision scale estimation method and device, aircraft navigation system and aircraft
CN110764525A (en) Unmanned aerial vehicle flight control system based on skynet number one
WO2022094962A1 (en) Hovering method for unmanned aerial vehicle, unmanned aerial vehicle and storage medium
CN110531397B (en) Outdoor inspection robot positioning system and method based on GPS and microwave

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