CN108680167A - Indoor dead reckoning localization method and system based on UWB and laser ranging - Google Patents
Indoor dead reckoning localization method and system based on UWB and laser ranging Download PDFInfo
- Publication number
- CN108680167A CN108680167A CN201810466502.1A CN201810466502A CN108680167A CN 108680167 A CN108680167 A CN 108680167A CN 201810466502 A CN201810466502 A CN 201810466502A CN 108680167 A CN108680167 A CN 108680167A
- Authority
- CN
- China
- Prior art keywords
- dead reckoning
- information
- uwb
- moment
- formula
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- G—PHYSICS
- 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
- 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
-
- 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
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
-
- 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
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
- G01S5/0257—Hybrid positioning
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
- H04W4/024—Guidance services
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/30—Services specially adapted for particular environments, situations or purposes
- H04W4/33—Services specially adapted for particular environments, situations or purposes for indoor environments, e.g. buildings
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Signal Processing (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
The present invention provides a kind of, and indoor dead reckoning localization method and system based on UWB and laser ranging carry out data fusion when UWB location informations are effective using UWB location informations and dead reckoning information；When UWB location informations are invalid, data fusion is carried out using laser ranging information and dead reckoning information.The indoor dead reckoning localization method and system of the present invention can effectively avoid the problem that the location information of pure DR dissipates at any time, and when UWB signal is stopped or positioning carrier exceeds the effective position regions UWB, the negative effect that UWB position errors increased dramatically, so as to greatly improve the positioning accuracy of indoor navigation positioning.
Description
Technical field
The present invention relates to indoor navigation field of locating technology more particularly to a kind of indoor boats based on UWB and laser ranging
Position reckoning positioning method and system.
Background technology
With the progress of informationization technology, the development of location technology greatly changes the live and work side of modern society
Formula is to depend on popularizing for GPS positioning technology such as the electronic navigation generally used in going on a journey or travelling.To daily
Demand in life to based on positioning is continuously increased, and how to accurately determine user location then is to realize to be based on location-based service
The basis of (LBS, Location based service) and key.Although the application of GPS positioning technology is more universal, and
Its positioning accuracy is also higher, but with the development of the city with construction, the activity of people can be concentrated in some indoor areas mostly
Domain, such as subway, Business Building, market, dining room etc., and these places are huge or relative closure, often can not
GPS signal is received, demand for indoor positioning can not be fully achieved by only relying on GPS.
Main indoor positioning technologies include at present：The SLAM technologies of view-based access control model sensor or radar laser, are based on
WiFi, bluetooth, the location technology of ZigBee, dead reckoning (Dead Reckoning, DR) location technology and ultra wide band
(Ultra Wide Band, UWB) location technology etc..
SLAM technologies do not depend on oracle, have stronger independence, change minimum to environment, but in densely populated place
Dynamic environment under SLAM technologies it is hard to work.
UWB positioning is a kind of positioning method being based on TOF (Time of Flight) principle, it passes through to jumpy
Shock pulse is directly modulated, and so that signal is had the bandwidth of GHz magnitudes, is needed to detect compared to WiFi, bluetooth, ZigBee etc.
The method of signal strength, UWB positioning is more reliable, with insensitive to signal fadeout, transmitting power spectrum density is low, anti-
The features such as interception capability is strong can provide decimeter grade positioning accuracy.Location navigation application aspect is paid close attention to indoors in recent years.But
It is that when UWB signal is stopped or when positioning carrier exceeds the effective position regions UWB, UWB position errors can increased dramatically.
DR systems are not necessarily to the actual beacon node in building, utilize inertial sensor (such as accelerometer, gyroscope)
Displacement distance and the direction of positioning carrier are calculated with velocity sensor, you can speculate its movement locus.But the position of DR and boat
It can be dissipated to information with the time.
Invention content
The current prior art, it is only difficult that continuous accurately provide is led by single navigation system to be directed to indoor positioning
Boat location information, in view of this, the present invention provides a kind of indoor dead reckoning localization methods, on the basis of dead reckoning
It is accompanied by UWB location informations and laser ranging information；When UWB location informations are effective, believed using UWB location informations and dead reckoning
Breath carries out data fusion；When UWB location informations are invalid, data are carried out with dead reckoning information using laser ranging information and are melted
It closes.
Further, when needing to initialize dead reckoning information, using UWB location informations to dead reckoning
Location information and course angle are initialized.
Further, the position and course of dead reckoning, which initialize, includes：
It before the course angle of dead reckoning is not initialised, updates without calculating, is directly come using the location information of UWB
The location information of assignment dead reckoning；
When positioning carrier setting in motion, the speed of positioning carrier is calculated using UWB location informations, calculation formula is as follows
It is shown；
Vue (Ku+1)=(Pue (Ku+1)-Pue (Ku))/Tku (1)
Vun (Ku+1)=(Pun (Ku+1)-Pun (Ku))/Tku (2)
Wherein Vue (Ku+1) and Vun (Ku+1) is respectively UWB in the coordinate system of the northeast east orientation speed at Ku+1 moment and north
To speed, the east orientation position at Pue (Ku+1) and Pue (Ku) are respectively UWB in the coordinate system of northeast Ku+1 and Ku moment, Pun (Ku
+ 1) and the north orientation position at Pun (Ku) Ku+1 and Ku moment that is respectively UWB in the coordinate system of northeast, Tku is that UWB is arrived at the Ku moment
The time interval at Ku+1 moment；
Course angle ψ 0 is calculated using formula (1) and the velocity information of (2) acquisition, formula is as follows：
ψ 0=(Vue/Vun) * 180/PI (3)
Wherein Vue and Vun is UWB east orientation speeds and north orientation speed current in the coordinate system of northeast respectively, and PI is circumference
Rate, the unit degree of being of ψ 0；
It, will be by ψ 0 that formula (3) are calculated as dead reckoning when detecting positioning carrier after moving in a straight line
Course angle carrys out the location information of assignment dead reckoning using the location information of current UWB, initial to complete dead reckoning information
Change.
Further, detect whether positioning carrier is moving in a straight line by gyroscope.
In one embodiment of the invention, it sets the course angular speed to take the air line and is less than 3~5 degree per seconds.
Further, using the information of gyroscope and velocity sensor (such as wheel type encoder) come the position to dead reckoning
Information and course angle carry out recursion update.
Further, using the information of gyroscope and velocity sensor come to dead reckoning location information and course angle into
Row recursion updates, including：
The course angle of Kg+1 moment dead reckonings updates, and more new formula is：
ψ (Kg+1)=ψ (Kg)+St* (Wt-Wt0) * Tkg (4)
Wherein ψ (Kg+1) and ψ (Kg) is respectively course angle of the dead reckoning at Kg+1 the and Kg moment, and St is the quarter of gyroscope
Coefficient is spent, Wt is that the angular speed of gyroscope exports, and Wt0 is the angular speed zero bias of gyroscope, and Tkg is gyro data at the Kg moment
To the time interval at Kg+1 moment；
The updating location information of Kv+1 moment dead reckonings, more new formula are：
Pe (Kv+1)=Pe (Kv)+sin (ψ) * Sv*Vp*Tkv (5)
Pn (Kv+1)=Pn (Kv)+cos (ψ) * Sv*Vp*Tkv (6)
Lat (Kv+1)=Lat (Kv)+Pn (Kv+1)/Er (7)
Lon (Kv+1)=Lon (Kv)+Pe (Kv+1)/Er/cos (Lat) (8)
Wherein, Pe (Kv+1) and Pn (Kv+1) is east orientation and north orientation of the Kv+1 moment dead reckoning in northeast coordinate system respectively
Position, Pe (Kv) and Pn (Kv) are the dead reckoning of Kv moment respectively in the east orientation of northeast coordinate system and the position of north orientation, and ψ is boat
The course angle that position calculates, ψ=(ψ (Kv+1)+ψ (Kv))/2, Sv is the speed calibration factor of velocity sensor, and Vp is velocity pick-up
The speed of device exports, and Tkv is velocity sensor data at the Kv moment to the time interval at Kv+1 moment, and Er is earth radius, Lat
(Kv+1) and Lon (Kv+1) be respectively Kv+1 moment dead reckonings longitude and latitude, Lat (Kv) and Lon (Kv) are respectively Kv
The longitude and latitude of moment dead reckoning, Lat=(Lat (Kv+1)+Lat (Kv))/2.
Further, UWB location informations and the data fusion of dead reckoning information use EKF filter algorithm.
Further, the data fusion of UWB location informations and dead reckoning information, including：
The location information of the location information of UWB and dead reckoning is made the difference, in this, as the observation of EKF filter
Information；
The quantity of state of selecting system error equation, respectively east orientation error delta Pe, north orientation error delta Pn, speed calibration factor
Error delta Sv, course angle error delta ψ, totally 5 quantity of states, Wt are that the angular speed of gyroscope is defeated to gyro scale coefficient error Δ St
Go out, shown in error equation such as formula (9)~(13)：
The discrete system error equation of Kalman filter will can be obtained after formula (9)~(13) discretization, such as formula (14) institute
Show：
Xk+1=Φk+1,kXk (14)
Wherein, Xk=[Δ Pe Δ Pn Δ ψ Δ Sv Δs St]T；
It carries out Kalman filter and the status information amendment that is estimated using Kalman filter and updates in dead reckoning
Pe, Pn, ψ, Sv, St information.
Further, laser ranging information and the data fusion of dead reckoning information use EKF filter algorithm.
Further, the data fusion of laser ranging information and dead reckoning information, including：
The forward direction distance Sf (Kl) and Sf (Kl+1) and Kl at Kl moment and Kl+1 moment are measured using laser ranging system
The lateral distance Ss (Kl) and Ss (Kl+1) at moment and Kl+1 moment, the east orientation in the coordinate system of northeast is calculated using following formula
With the projection of north orientation：
Δ Sf=Sf (Kl+1)-Sf (Kl) (15)
Δ Ss=Ss (Kl+1)-Ss (Kl) (16)
Ple (Kl+1)=Ple (Kl)+sin (ψ) * Δs Sf+cos (ψ) * Δs Ss (17)
Pln (Kl+1)=Pln (Kl)+cos (ψ) * Δs Sf-sin (ψ) * Δs Ss (18)
Wherein, Ple and Pln is the projection of Sf and Ss in northeast coordinate system respectively, and ψ is the course angle of positioning carrier；
The location information of Ple and Pln and dead reckoning are made the difference, in this, as the observation information of EKF filter；
The quantity of state of selecting system error equation, respectively east orientation error delta Pe, north orientation error delta Pn, speed calibration factor
Error delta Sv, course angle error delta ψ, totally 5 quantity of states, Wt are that the angular speed of gyroscope is defeated to gyro scale coefficient error Δ St
Go out, shown in error equation such as formula (19)~(23)：
The discrete system error equation of Kalman filter will can be obtained after formula (19)~(23) discretization, such as formula (24) institute
Show：
Xk+1=Φk+1,kXk (24)
Wherein, Xk=[Δ Pe Δ Pn Δ ψ Δ Sv Δs St]T；
It carries out Kalman filter and the status information amendment that is estimated using Kalman filter and updates in dead reckoning
Pe, Pn, ψ, Sv, St information.
The present invention also provides a kind of indoor dead reckoning positioning system, positioning carrier is provided with UWB device, gyro
Instrument, velocity sensor and laser ranging system, and use above-mentioned indoor dead reckoning localization method.
Formal notation explanation：
" * " indicates multiplication sign；
"/" indicates the division sign；
Indicate the first derivative of X.
Technique effect：
Indoor navigation because of environment complexity, single navigation system be difficult it is continuous accurately provide navigator fix information, and incite somebody to action
Multiple navigation system combine application, then the shortcomings that capable of evading single navigation system, accomplish continuous accurately positioning.DR systems
External information can not depended on, the data for relying solely on itself sensor carry out continuous navigator fix calculating, but the positioning of DR with
And course information can be dissipated with the time.UWB can provide accurate indoor navigation location information, and the positioning accuracy of UWB not with
It time factor to change, but due to the complexity of interior architecture object so that the UWB signal in indoor many places is blocked, no
The continuous navigator fix of energy realization, and the navigation positioning system that DR, UWB and laser ranging are combined, then can realize and work as UWB
When effective, UWB continuously can accurately export navigator fix information with DR data fusions；When UWB is invalid, make
With laser ranging information with DR data fusions, it can effectively avoid the problem that the location information of pure DR dissipates at any time, and work as UWB
When signal is stopped or positioning carrier exceeds the effective position regions UWB, the negative effect that UWB position errors increased dramatically, from
And the positioning accuracy of indoor navigation positioning can be greatly improved.
The technique effect of the design of the present invention, concrete structure and generation is described further below with reference to attached drawing, with
It is fully understood from the purpose of the present invention, feature and effect.
Description of the drawings
Fig. 1 is the system block diagram of the preferred embodiment of the present invention；
Coordinate system in Fig. 2 employed in a preferred embodiment of the invention；
Fig. 3 is the flow chart of the preferred embodiment of the present invention.
Specific implementation mode
In the description of embodiments of the present invention, it is to be understood that term "upper", "lower", "front", "rear", " left side ",
The orientation of the instructions such as " right side ", " vertical ", "horizontal", "top", "bottom", "inner", "outside", " clockwise ", " counterclockwise " or position are closed
System is merely for convenience of description of the present invention and simplification of the description to be based on the orientation or positional relationship shown in the drawings, rather than indicates
Or imply that signified device or element must have a particular orientation, with specific azimuth configuration and operation, therefore cannot understand
For the limitation to invention.Attached drawing is schematic diagram or concept map, relationship and each section between each section thickness and width it
Between proportionate relationship etc., it is not completely the same with its actual value.
Fig. 1 shows that the system block diagram of the preferred embodiment of the present invention, system include MEMS gyroscope, speed biography
Sensor (such as wheel type encoder), UWB device and laser ranging system.MEMS gyroscope and velocity sensor are respectively configured to provide fixed
The angular speed and speed of position carrier are used for the position of dead reckoning and passing for course information by the angular speed and speed that monitor
Push away update.Meanwhile UWB reception devices and laser ranging system are also equipped on positioning carrier, when UWB location informations are effective,
Itself and dead reckoning information are subjected to data fusion；When UWB location informations are invalid, using laser ranging information and dead reckoning
Information carries out data fusion.Above-mentioned data fusion using EKF filter (Extended Kalman Filter,
EKF), and by the status information amendment estimated through EKF and update into dead reckoning, so that it is determined that the positioning of positioning carrier
Information.
In the present embodiment, the coordinate system used is northeast coordinate system (abbreviation N systems) and carrier coordinate system (abbreviation B systems), N
System and B systems are defined as follows：
Northeast coordinate system (abbreviation N systems)：Xn axis is directed toward east, and Yn axis is directed toward north perpendicular to Xn axis；
Carrier coordinate system (abbreviation B systems)：Xb axis is directed toward side, and Yb is directing forwardly perpendicular to Xb axis axis, as shown in Fig. 2, its
Middle ψ is course angle, clockwise for just.
As shown in figure 3, elaborating below for the localization method of Fig. 1 systems.
1, when positioning carrier moves indoors, UWB location informations are to dead reckoning (Dead Reckoning, below letter
Claim DR) location information and course angle initialized, which specifically includes following steps：
1.1, it before the course angle of DR is not initialised, is updated without calculating, directly UWB is used to be obtained based on N systems
Location information is assigned to the location information of DR；
1.2, when positioning carrier setting in motion, the speed of positioning carrier, calculation formula are calculated using UWB location informations
As follows；
Vue (Ku+1)=(Pue (Ku+1)-Pue (Ku))/Tku (1)
Vun (Ku+1)=(Pun (Ku+1)-Pun (Ku))/Tku (2)
The east orientation speed and north orientation speed at wherein Vue (Ku+1) and Vun (Ku+1) is respectively UWB in N systems Ku+1 moment,
The east orientation position at Pue (Ku+1) and Pue (Ku) is respectively UWB in N systems Ku+1 and Ku moment, Pun (Ku+1) and Pun (Ku) points
Not Wei UWB Ku+1 and Ku moment in N systems north orientation position, Tku is time intervals of the UWB at the Ku moment to the Ku+1 moment；
1.3, course angle ψ 0 is calculated using formula (1) and the velocity information of (2) acquisition, formula is as follows：
ψ 0=(Vue/Vun) * 180/PI (3)
Wherein Vue and Vun is UWB east orientation speeds and north orientation speed current in N systems respectively, and PI is pi, ψ's 0
Unit degree of being；
1.4, whether taken the air line using the course angle rate determination positioning carrier obtained from MEMS gyroscope, the present embodiment
The middle course angular speed to take the air line that sets will be calculated less than 3~5 degree per seconds if carrier takes the air line by formula (3)
ψ 0 be assigned to the course angle of DR, the location information of assignment DR is carried out using location informations of the UWB this moment in N systems, to complete
DR information initializings.
2, after the location information of DR and course angle are initialised, believed using the monitoring of MEMS gyroscope and velocity sensor
Breath to carry out recursion update to the location information and course angle of DR, and specific following steps include：
2.1, the course angle update of Kg+1 moment DR, more new formula is：
ψ (Kg+1)=ψ (Kg)+St* (Wt-Wt0) * Tkg (4)
Wherein ψ (Kg+1) and ψ (Kg) is respectively course angles of the DR at Kg+1 the and Kg moment, and St is the scale system of gyroscope
Number, Wt are that the angular speed of gyroscope exports, and Wt0 is the angular speed zero bias of gyroscope, and Tkg is gyro data at the Kg moment to Kg
The time interval at+1 moment；
2.2, the updating location information of Kv+1 moment DR, more new formula are：
Pe (Kv+1)=Pe (Kv)+sin (ψ) * Sv*Vp*Tkv (5)
Pn (Kv+1)=Pn (Kv)+cos (ψ) * Sv*Vp*Tkv (6)
Lat (Kv+1)=Lat (Kv)+Pn (Kv+1)/Er (7)
Lon (Kv+1)=Lon (Kv)+Pe (Kv+1)/Er/cos (Lat) (8)
Wherein, Pe (Kv+1) and Pn (Kv+1) is Kv+1 moment DR respectively in the east orientation of N systems and the position of north orientation, Pe (Kv)
It is Kv moment DR respectively in the east orientation of N systems and the position of north orientation with Pn (Kv), ψ is the course angle of DR, ψ=(ψ (Kv+1)+ψ
(Kv))/2, Sv is the speed calibration factor of velocity sensor, and Vp is the speed output of velocity sensor, and Tkv is velocity sensor
Data are at the Kv moment to the time interval at Kv+1 moment, and Er is earth radius, and Lat (Kv+1) and Lon (Kv+1) are respectively Kv+1
The longitude and latitude of moment DR, Lat (Kv) and Lon (Kv) are respectively the longitude and latitude of Kv moment DR, Lat=(Lat (Kv+1)
+Lat(Kv))/2。
3, the validity for detecting UWB and the data information of laser ranging, specifically includes following steps：
3.1, whether the data information of detection UWB is effective, is directly entered data fusions of the UWB with DR if effectively, i.e.,
Step 4；
If 3.2, UWB data informations are invalid, start the data information for detecting laser ranging, if laser ranging data is believed
Breath is effectively then directly entered laser ranging with the data fusion of DR, i.e. step 5.
4, Kalman data fusions are extended using the location information of the location information of UWB and DR, specifically included following
Step：
4.1, the location information of the location information of UWB and DR is made the difference, the observation in this, as EKF filter is believed
Breath；
4.2, the quantity of state of selecting system error equation, respectively east orientation error delta Pe, north orientation error delta Pn, speed scale
System errors Δ Sv, course angle error delta ψ, totally 5 quantity of states, Wt are the angular speeds of gyroscope to gyro scale coefficient error Δ St
Output, shown in error equation such as formula (9)~(13)：
The discrete system error equation of Kalman filter will can be obtained after formula (9)~(13) discretization, such as formula (14) institute
Show：
Xk+1=Φk+1,kXk (14)
Wherein, Xk=[Δ Pe Δ Pn Δ ψ Δ Sv Δs St]T；
4.3, it carries out Kalman filter and the status information amendment that is estimated using Kalman filter and updates in DR
Pe, Pn, ψ, Sv, St information.
5, Kalman data fusions are extended using the location information of the data information of laser ranging and DR, specifically included
Following steps：
5.1, the forward direction distance Sf (Kl) and Sf (Kl+1) that Kl moment and Kl+1 moment are measured using laser ranging system, with
And the lateral distance Ss (Kl) and Ss (Kl+1) at Kl moment and Kl+1 moment, using following formula calculate in N systems east orientation and
The projection of north orientation：
Δ Sf=Sf (Kl+1)-Sf (Kl) (15)
Δ Ss=Ss (Kl+1)-Ss (Kl) (16)
Ple (Kl+1)=Ple (Kl)+sin (ψ) * Δs Sf+cos (ψ) * Δs Ss (17)
Pln (Kl+1)=Pln (Kl)+cos (ψ) * Δs Sf-sin (ψ) * Δs Ss (18)
Wherein, Ple and Pln is the projection of Sf and Ss in N systems respectively, and ψ is the course angle of positioning carrier；
5.2, the location information of Ple and Pln and DR is made the difference, in this, as the observation information of EKF filter；
5.3, the quantity of state of selecting system error equation, respectively east orientation error delta Pe, north orientation error delta Pn, speed scale
System errors Δ Sv, course angle error delta ψ, totally 5 quantity of states, Wt are the angular speeds of gyroscope to gyro scale coefficient error Δ St
Output, shown in error equation such as formula (19)~(23)：
The discrete system error equation of Kalman filter will can be obtained after formula (19)~(23) discretization, such as formula (24) institute
Show：
Xk+1=Φk+1,kXk (24)
Wherein, Xk=[Δ Pe Δ Pn Δ ψ Δ Sv Δs St]T；
5.4, it carries out Kalman filter and the status information amendment that is estimated using Kalman filter and updates in DR
Pe, Pn, ψ, Sv, St information.
The indoor DR localization methods and system of the present embodiment can effectively avoid the location information of pure DR from dissipating at any time
Problem, and when UWB signal is stopped or positioning carrier exceeds the effective position regions UWB, UWB position errors increased dramatically
Negative effect, so as to greatly improve indoor navigation positioning positioning accuracy.
The preferred embodiment of the present invention has been described in detail above.It should be appreciated that those skilled in the art without
It needs creative work according to the present invention can conceive and makes many modifications and variations.Therefore, all technologies in the art
Personnel are available by logical analysis, reasoning, or a limited experiment on the basis of existing technology under this invention's idea
Technical solution, all should be in the protection domain being defined in the patent claims.
Claims (11)
1. a kind of interior dead reckoning localization method, which is characterized in that be aided with UWB location informations and laser ranging information；Work as UWB
When location information is effective, data fusion is carried out using UWB location informations and dead reckoning information；When UWB location informations are invalid,
Data fusion is carried out using laser ranging information and dead reckoning information.
2. interior dead reckoning localization method as described in claim 1, which is characterized in that when need to dead reckoning information into
When row initialization, the location information and course angle of dead reckoning are initialized using UWB location informations.
3. dead reckoning localization method in interior as claimed in claim 2, which is characterized in that at the beginning of the position and course of dead reckoning
Beginningization includes：
Before the course angle of dead reckoning is not initialised, is updated without calculating, directly carry out assignment using the location information of UWB
The location information of dead reckoning；
When positioning carrier setting in motion, the speed of positioning carrier, the following institute of calculation formula are calculated using UWB location informations
Show；
Vue (Ku+1)=(Pue (Ku+1)-Pue (Ku))/Tku (1)
Vun (Ku+1)=(Pun (Ku+1)-Pun (Ku))/Tku (2)
The east orientation speed at Ku+1 moment that wherein Vue (Ku+1) and Vun (Ku+1) is respectively UWB in the coordinate system of northeast and north orientation speed
Degree, the east orientation position at Pue (Ku+1) and Pue (Ku) are respectively UWB in the coordinate system of northeast Ku+1 and Ku moment, Pun (Ku+1)
With the north orientation position at Pun (Ku) Ku+1 and Ku moment that is respectively UWB in the coordinate system of northeast, Tku is UWB at the Ku moment to Ku+1
The time interval at moment；
Course angle ψ 0 is calculated using formula (1) and the velocity information of (2) acquisition, formula is as follows：
ψ 0=(Vue/Vun) * 180/PI (3)
Wherein Vue and Vun is UWB east orientation speeds and north orientation speed current in the coordinate system of northeast respectively, and PI is pi, ψ 0
Unit degree of being；
It, will be by the course of ψ 0 that formula (3) are calculated as dead reckoning when detecting positioning carrier after moving in a straight line
The location information of assignment dead reckoning is carried out at angle using the location information of current UWB, to complete dead reckoning information initializing.
4. dead reckoning localization method in interior as claimed in claim 3, which is characterized in that detect positioning carrier by gyroscope
Whether moving in a straight line.
5. dead reckoning localization method in interior as described in claim 1, which is characterized in that use gyroscope and velocity sensor
Information to carry out recursion update to the location information and course angle of dead reckoning.
6. dead reckoning localization method in interior as claimed in claim 5, which is characterized in that use gyroscope and velocity sensor
Information to carry out recursion update to the location information and course angle of dead reckoning, including：
The course angle of Kg+1 moment dead reckonings updates, and more new formula is：
ψ (Kg+1)=ψ (Kg)+St* (Wt-Wt0) * Tkg (4)
Wherein ψ (Kg+1) and ψ (Kg) is respectively course angle of the dead reckoning at Kg+1 the and Kg moment, and St is the scale system of gyroscope
Number, Wt are that the angular speed of gyroscope exports, and Wt0 is the angular speed zero bias of gyroscope, and Tkg is gyro data at the Kg moment to Kg
The time interval at+1 moment；
The updating location information of Kv+1 moment dead reckonings, more new formula are：
Pe (Kv+1)=Pe (Kv)+sin (ψ) * Sv*Vp*Tkv (5)
Pn (Kv+1)=Pn (Kv)+cos (ψ) * Sv*Vp*Tkv (6)
Lat (Kv+1)=Lat (Kv)+Pn (Kv+1)/Er (7)
Lon (Kv+1)=Lon (Kv)+Pe (Kv+1)/Er/cos (Lat) (8)
Wherein, Pe (Kv+1) and Pn (Kv+1) is the dead reckoning of Kv+1 moment respectively in the east orientation of northeast coordinate system and the position of north orientation
It sets, Pe (Kv) and Pn (Kv) are the dead reckoning of Kv moment respectively in the east orientation of northeast coordinate system and the position of north orientation, and ψ is that boat position pushes away
The course angle of calculation, ψ=(ψ (Kv+1)+ψ (Kv))/2, Sv are the speed calibration factors of velocity sensor, and Vp is velocity sensor
Speed export, Tkv be velocity sensor data at the Kv moment to the time interval at Kv+1 moment, Er is earth radius, Lat (Kv+
1) and Lon (Kv+1) be respectively Kv+1 moment dead reckonings longitude and latitude, Lat (Kv) and Lon (Kv) are respectively the Kv moment
The longitude and latitude of dead reckoning, Lat=(Lat (Kv+1)+Lat (Kv))/2.
7. dead reckoning localization method in interior as described in claim 1, which is characterized in that UWB location informations and dead reckoning
The data fusion of information uses EKF filter algorithm.
8. dead reckoning localization method in interior as claimed in claim 6, which is characterized in that UWB location informations and dead reckoning
The data fusion of information, including：
The location information of the location information of UWB and dead reckoning is made the difference, in this, as the observation information of EKF filter；
The quantity of state of selecting system error equation, respectively east orientation error delta Pe, north orientation error delta Pn, speed scale coefficient error
Δ Sv, course angle error delta ψ, gyro scale coefficient error Δ St totally 5 quantity of states,
Wt is the angular speed output of gyroscope, shown in error equation such as formula (9)~(13)：
The discrete system error equation of Kalman filter will can be obtained after formula (9)~(13) discretization, as shown in formula (14)：
Xk+1=Φk+1,kXk (14)
Wherein, Xk=[Δ Pe Δ Pn Δ ψ Δ Sv Δs St]T；
Carry out Kalman filter and the status information amendment that is estimated using Kalman filter and update the Pe in dead reckoning,
Pn, ψ, Sv, St information.
9. dead reckoning localization method in interior as described in claim 1, which is characterized in that laser ranging information and dead reckoning
The data fusion of information uses EKF filter algorithm.
10. dead reckoning localization method in interior as claimed in claim 6, which is characterized in that laser ranging information is pushed away with boat position
The data fusion of information is calculated, including：
Forward direction distance Sf (Kl) and Sf (Kl+1) and the Kl moment at Kl moment and Kl+1 moment are measured using laser ranging system
With the lateral distance Ss (Kl) and Ss (Kl+1) at Kl+1 moment, east orientation and north in the coordinate system of northeast are calculated using following formula
To projection：
Δ Sf=Sf (Kl+1)-Sf (Kl) (15)
Δ Ss=Ss (Kl+1)-Ss (Kl) (16)
Ple (Kl+1)=Ple (Kl)+sin (ψ) * Δs Sf+cos (ψ) * Δs Ss (17)
Pln (Kl+1)=Pln (Kl)+cos (ψ) * Δs Sf-sin (ψ) * Δs Ss (18)
Wherein, Ple and Pln is the projection of Sf and Ss in northeast coordinate system respectively, and ψ is the course angle of positioning carrier；
The location information of Ple and Pln and dead reckoning are made the difference, in this, as the observation information of EKF filter；
The quantity of state of selecting system error equation, respectively east orientation error delta Pe, north orientation error delta Pn, speed scale coefficient error
Δ Sv, course angle error delta ψ, totally 5 quantity of states, Wt are the angular speed output of gyroscope to gyro scale coefficient error Δ St, accidentally
Shown in eikonal equation such as formula (19)~(23)：
The discrete system error equation of Kalman filter will can be obtained after formula (19)~(23) discretization, as shown in formula (24)：
Xk+1=Φk+1,kXk (24)
Wherein, Xk=[Δ Pe Δ Pn Δ ψ Δ Sv Δs St]T；
Carry out Kalman filter and the status information amendment that is estimated using Kalman filter and update the Pe in dead reckoning,
Pn, ψ, Sv, St information.
11. a kind of interior dead reckoning positioning system, which is characterized in that positioning carrier is provided with UWB device, gyroscope, speed
Sensor and laser ranging system, and using the indoor dead reckoning localization method as described in claim 1-10 is any.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810466502.1A CN108680167B (en) | 2018-05-16 | 2018-05-16 | Indoor dead reckoning positioning method and system based on UWB and laser ranging |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810466502.1A CN108680167B (en) | 2018-05-16 | 2018-05-16 | Indoor dead reckoning positioning method and system based on UWB and laser ranging |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108680167A true CN108680167A (en) | 2018-10-19 |
| CN108680167B CN108680167B (en) | 2020-10-16 |
Family
ID=63806541
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810466502.1A Active CN108680167B (en) | 2018-05-16 | 2018-05-16 | Indoor dead reckoning positioning method and system based on UWB and laser ranging |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108680167B (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109788550A (en) * | 2018-12-25 | 2019-05-21 | 广东云立方互动科技有限公司 | A kind of indoor track positioning system and method based on super-broadband tech |
| CN111417069A (en) * | 2020-03-27 | 2020-07-14 | 安徽理工大学 | Positioning device and method for coal mine underground transportation robot with wheel type odometer and UWB integrated |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104076382A (en) * | 2014-07-22 | 2014-10-01 | 中国石油大学(华东) | Seamless vehicle positioning method based on multi-source information fusion |
| CN104596504A (en) * | 2015-01-30 | 2015-05-06 | 中国科学院上海高等研究院 | Method and system for quickly setting up map to assist indoor positioning under emergency rescue scene |
| CN104864865A (en) * | 2015-06-01 | 2015-08-26 | 济南大学 | Indoor pedestrian navigation-oriented AHRS/UWB (attitude and heading reference system/ultra-wideband) seamless integrated navigation method |
| CN106643694A (en) * | 2016-11-04 | 2017-05-10 | 航天科工智能机器人有限责任公司 | Method for indoor positioning of robot |
| CN106871893A (en) * | 2017-03-03 | 2017-06-20 | 济南大学 | Distributed INS/UWB tight integrations navigation system and method |
| CN106908759A (en) * | 2017-01-23 | 2017-06-30 | 南京航空航天大学 | A kind of indoor pedestrian navigation method based on UWB technology |
| CN107402375A (en) * | 2017-08-08 | 2017-11-28 | 济南大学 | A kind of indoor pedestrian of band observation time lag positions EFIR data fusion systems and method |
| KR20180042546A (en) * | 2016-10-18 | 2018-04-26 | 조선대학교산학협력단 | Hybrid indoor positioning system and method thereof |
-
2018
- 2018-05-16 CN CN201810466502.1A patent/CN108680167B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104076382A (en) * | 2014-07-22 | 2014-10-01 | 中国石油大学(华东) | Seamless vehicle positioning method based on multi-source information fusion |
| CN104596504A (en) * | 2015-01-30 | 2015-05-06 | 中国科学院上海高等研究院 | Method and system for quickly setting up map to assist indoor positioning under emergency rescue scene |
| CN104864865A (en) * | 2015-06-01 | 2015-08-26 | 济南大学 | Indoor pedestrian navigation-oriented AHRS/UWB (attitude and heading reference system/ultra-wideband) seamless integrated navigation method |
| KR20180042546A (en) * | 2016-10-18 | 2018-04-26 | 조선대학교산학협력단 | Hybrid indoor positioning system and method thereof |
| CN106643694A (en) * | 2016-11-04 | 2017-05-10 | 航天科工智能机器人有限责任公司 | Method for indoor positioning of robot |
| CN106908759A (en) * | 2017-01-23 | 2017-06-30 | 南京航空航天大学 | A kind of indoor pedestrian navigation method based on UWB technology |
| CN106871893A (en) * | 2017-03-03 | 2017-06-20 | 济南大学 | Distributed INS/UWB tight integrations navigation system and method |
| CN107402375A (en) * | 2017-08-08 | 2017-11-28 | 济南大学 | A kind of indoor pedestrian of band observation time lag positions EFIR data fusion systems and method |
Non-Patent Citations (5)
| Title |
|---|
| FRANCISCO ZAMPELLA等: "A constraint approach for UWB and PDR fusion", 《2012 INTERNATIONAL CONFERENCE ON INDOOR POSITIONING AND INDOOR NAVIGATION (IPIN)》 * |
| 刘韬等: "超宽带与惯性导航组合的室内导航定位", 《测绘科学》 * |
| 曹学伟等: "基于EKF的DR/激光测距组合导航算法研究", 《山东科技大学学报(自然科学版)》 * |
| 杨洲: "基于UWB/MEMS的高精度室内定位技术研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
| 闫林波等: "基于扩展卡尔曼滤波的动中通低成本姿态估计", 《电光与控制》 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109788550A (en) * | 2018-12-25 | 2019-05-21 | 广东云立方互动科技有限公司 | A kind of indoor track positioning system and method based on super-broadband tech |
| CN111417069A (en) * | 2020-03-27 | 2020-07-14 | 安徽理工大学 | Positioning device and method for coal mine underground transportation robot with wheel type odometer and UWB integrated |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108680167B (en) | 2020-10-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN103777220B (en) | Based on the accurate position and orientation estimation method in real time of optical fibre gyro, speed pickup and GPS | |
| CN109682375B (en) | UWB (ultra-wideband) auxiliary inertial positioning method based on fault-tolerant decision tree | |
| CN106908759A (en) | A kind of indoor pedestrian navigation method based on UWB technology | |
| CN105783923A (en) | Personnel positioning method based on RFID and MEMS inertial technologies | |
| Abosekeen et al. | A novel multi-level integrated navigation system for challenging GNSS environments | |
| US20170219351A1 (en) | Navigational and location determination system | |
| TWI522258B (en) | Based on electronic map, global navigation satellite system and vehicle motion detection technology Lane identification method | |
| Abdulrahim et al. | Integrating low cost IMU with building heading in indoor pedestrian navigation | |
| CN109059909A (en) | Satellite based on neural network aiding/inertial navigation train locating method and system | |
| Armstrong et al. | AUV navigation in the presence of a magnetic disturbance with an extended Kalman filter | |
| CN107274721A (en) | Many vehicle cooperative localization methods in a kind of intelligent transportation system | |
| Chen et al. | IMU/GPS based pedestrian localization | |
| JP2003503717A (en) | Terrain navigation device | |
| CN106017460B (en) | A kind of underwater hiding-machine navigation locating method of terrain aided inertial navigation tight integration | |
| CN106093992A (en) | A kind of sub-meter grade combined positioning and navigating system based on CORS and air navigation aid | |
| CN114018273A (en) | Accurate positioning system and method for automatic driving vehicle in underground coal mine | |
| CN108562289A (en) | Quadrotor laser radar air navigation aid in continuous polygon geometry environment | |
| US20140249750A1 (en) | Navigational and location determination system | |
| Gao et al. | An integrated land vehicle navigation system based on context awareness | |
| CN108680167A (en) | Indoor dead reckoning localization method and system based on UWB and laser ranging | |
| Rashed et al. | Leveraging FMCW-radar for autonomous positioning systems: Methodology and application in downtown Toronto | |
| CN103308047B (en) | A kind of method for positioning mobile robot with artificial on-line amending function | |
| Zhong et al. | A particle filter for vehicle tracking with lane level accuracy under GNSS-denied environments | |
| US10006770B2 (en) | Remote location determination system | |
| Dezert | Improvement of strapdown inertial navigation using PDAF |
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 | ||
| CP03 | Change of name, title or address | ||
| CP03 | Change of name, title or address |
Address after: 312030 Building 5, intelligent innovation center, 487 Kebei Avenue, Keqiao Economic and Technological Development Zone, Keqiao District, Shaoxing City, Zhejiang Province Patentee after: Shendi semiconductor (Shaoxing) Co.,Ltd. Address before: Room 208, podium building, No.1 building, 3000 Longdong Avenue, Pudong New Area pilot Free Trade Zone, Shanghai, 201203 Patentee before: Senodia Technologies (Shanghai) Co.,Ltd. |