CN108680167B - Indoor dead reckoning positioning method and system based on UWB and laser ranging - Google Patents

Indoor dead reckoning positioning method and system based on UWB and laser ranging Download PDF

Info

Publication number
CN108680167B
CN108680167B CN201810466502.1A CN201810466502A CN108680167B CN 108680167 B CN108680167 B CN 108680167B CN 201810466502 A CN201810466502 A CN 201810466502A CN 108680167 B CN108680167 B CN 108680167B
Authority
CN
China
Prior art keywords
dead reckoning
information
uwb
positioning
time
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
CN201810466502.1A
Other languages
Chinese (zh)
Other versions
CN108680167A (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.)
Shendi semiconductor (Shaoxing) Co.,Ltd.
Original Assignee
Senodia Technologies Shanghai 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 Senodia Technologies Shanghai Co Ltd filed Critical Senodia Technologies Shanghai Co Ltd
Priority to CN201810466502.1A priority Critical patent/CN108680167B/en
Publication of CN108680167A publication Critical patent/CN108680167A/en
Application granted granted Critical
Publication of CN108680167B publication Critical patent/CN108680167B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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 invention provides an indoor dead reckoning positioning method and system based on UWB and laser ranging, when UWB positioning information is effective, UWB positioning information and dead reckoning information are adopted for data fusion; and when the UWB positioning information is invalid, carrying out data fusion by adopting the laser ranging information and the dead reckoning information. The indoor dead reckoning positioning method and the indoor dead reckoning positioning system can effectively avoid the problem that the positioning information of pure DR is dispersed along with time, and the negative influence of the sudden increase of UWB positioning error when UWB signals are blocked or a positioning carrier exceeds an UWB effective positioning area, so that the positioning accuracy of indoor navigation positioning can be greatly improved.

Description

Indoor dead reckoning positioning method and system based on UWB and laser ranging
Technical Field
The invention relates to the technical field of indoor navigation positioning, in particular to an indoor dead reckoning positioning method and system based on UWB and laser ranging.
Background
With the progress of information technology, the development of positioning technology has greatly changed the life and working modes of modern society, such as electronic navigation commonly used in travel or travel, i.e. the popularization of GPS positioning technology. Therefore, the demand for Location-based services in daily life is increasing, and how to accurately determine the Location of a user is the basis and key for implementing Location Based Services (LBS). Although the application of GPS positioning technology is widespread and the positioning accuracy is high, with the development and construction of cities, most of people's activities are concentrated in indoor areas, such as subways, business buildings, shopping malls, restaurants, and the like, and these places are large in scale or relatively closed, and often cannot receive GPS signals, and the requirement for indoor positioning cannot be completely realized only by GPS.
Currently, the main indoor positioning technologies include: the system comprises a visual sensor or radar laser-based SLAM technology, a WiFi, Bluetooth and ZigBee-based positioning technology, a Dead Reckoning (DR) positioning technology, an Ultra Wide Band (UWB) positioning technology and the like.
The SLAM technology does not depend on an external information source, has strong autonomy and minimum change to the environment, but is difficult to work under the dynamic environment with dense personnel.
UWB positioning is a positioning mode based on TOF (time of flight) principle, and the signal has a bandwidth of GHz magnitude by directly modulating shock pulse which changes rapidly. In recent years attention has been paid to indoor positioning navigation applications. However, when UWB signals are blocked, or when the positioning carrier exceeds the UWB effective positioning area, UWB positioning errors increase dramatically.
The DR system can estimate the motion trail of the positioning carrier by calculating the moving distance and direction of the positioning carrier by using an inertial sensor (such as an accelerometer, a gyroscope and the like) and a speed sensor without presetting a beacon node in a building. However, the location and heading information of the DR may diverge over time.
Disclosure of Invention
In view of the fact that it is difficult to continuously and accurately provide navigation positioning information in indoor positioning only by a single navigation system in the prior art, the present invention provides an indoor dead reckoning positioning method, which attaches UWB positioning information and laser ranging information on the basis of dead reckoning; when the UWB positioning information is effective, carrying out data fusion by adopting the UWB positioning information and the dead reckoning information; and when the UWB positioning information is invalid, carrying out data fusion by adopting the laser ranging information and the dead reckoning information.
Further, when the dead reckoning information needs to be initialized, the UWB positioning information is adopted to initialize the dead reckoning position information and the course angle.
Further, the dead reckoning position and heading initialization includes:
before the course angle of the dead reckoning is not initialized, the dead reckoning is not updated, and the position information of the dead reckoning is assigned by directly using the position information of the UWB;
when the positioning carrier starts to move, calculating the speed of the positioning carrier by using UWB positioning information, wherein the calculation formula is shown as follows;
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) are an east-direction speed and a north-direction speed of the UWB at the time of Ku +1 in the northeast coordinate system respectively, Pue (Ku +1) and Pu (Ku) are east-direction positions of the UWB at the time of Ku +1 and Ku in the northeast coordinate system respectively, Pu (Ku +1) and Pu (Ku) are north-direction positions of the UWB at the time of Ku +1 and Ku in the northeast coordinate system respectively, and Tku is a time interval of the UWB from the time of Ku to the time of Ku + 1;
the heading angle ψ 0 is calculated using the velocity information obtained by the formulas (1) and (2), which are shown below:
ψ0＝(Vue/Vun)*180/PI (3)
wherein Vue and Vun are the current east and north velocities of UWB in the northeast coordinate system, respectively, PI is the circumferential ratio, and the unit of ψ 0 is degrees;
and when the positioning carrier is detected to do linear motion, taking psi 0 calculated by the formula (3) as a course angle of dead reckoning, and assigning dead reckoning position information by using the current UWB position information, thereby finishing the initialization of dead reckoning information.
Further, whether the positioning carrier moves linearly or not is detected through a gyroscope.
In one embodiment of the invention, the course angular speed of the straight line is set to be less than 3-5 degrees per second.
Further, information from gyroscopes and velocity sensors (e.g., wheel encoders) are used to recursively update the dead reckoning position information and heading angle.
Further, the information of the gyroscope and the speed sensor is adopted to carry out recursion updating on the dead reckoning position information and the heading angle, and the method comprises the following steps:
updating the course angle of dead reckoning at the Kg +1 moment, wherein the updating formula is as follows:
ψ(Kg+1)＝ψ(Kg)+St*(Wt-Wt0)*Tkg (4)
wherein ψ (Kg +1) and ψ (Kg) are heading angles of dead reckoning at Kg +1 and Kg time, respectively, St is a scale factor of the gyroscope, Wt is an angular rate output of the gyroscope, Wt0 is an angular rate zero offset of the gyroscope, Tkg is a time interval from Kg time to Kg +1 time of the gyroscope data;
updating position information of dead reckoning at Kv +1 moment, wherein an updating formula is as follows:
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)
where Pe (Kv +1) and Pn (Kv +1) are the easting and northing positions of the Kv +1 time dead reckoning in the northeast coordinate system, respectively, Pe (Kv) and Pn (Kv) are the easting and northing positions of the Kv time dead reckoning in the northeast coordinate system, respectively, ψ is the heading angle of the dead reckoning, ψ (Kv +1) + ψ (Kv))/2, Sv is the velocity scale coefficient of the velocity sensor, Vp is the velocity output of the velocity sensor, Tkv is the time interval of the velocity sensor data from the Kv time to the Kv +1 time, Er is the earth radius, Lat (Kv +1) and Lon (Kv +1) are the longitude and latitude of the Kv +1 time dead reckoning, respectively, Lat (Kv) and Lon (Kv) are the longitude and longitude of the Kv time dead reckoning, respectively, and Lat (Lat +1) + (Lat) and lan (Kv) are the longitude and Lat (Lat + 2).
Further, the data fusion of the UWB positioning information and the dead reckoning information adopts an extended Kalman filtering algorithm.
Further, the data fusion of the UWB positioning information and the dead reckoning information comprises:
subtracting the position information of the UWB and the position information of the dead reckoning to be used as observation information of the extended Kalman filtering;
selecting 5 state quantities of an east error delta Pe, a north error delta Pn, a speed scale coefficient error delta Sv, a course angle error delta psi and a gyro scale coefficient error delta St of a system error equation, wherein Wt is the angular rate output of the gyroscope, and the error equations are shown in formulas (9) to (13):
Figure BDA0001662198410000031
Figure BDA0001662198410000032
Figure BDA0001662198410000033
Figure BDA0001662198410000034
Figure BDA0001662198410000035
discretizing the equations (9) to (13) to obtain a discrete system error equation of Kalman filtering, as shown in equation (14):
Xk+1＝Φk+1,kXk(14)
wherein, Xk＝[ΔPe ΔPn Δψ ΔSv ΔSt]T；
And performing Kalman filtering, and correcting and updating the information of Pe, Pn, psi, Sv and St in dead reckoning by using the state information estimated by the Kalman filtering.
Furthermore, the data fusion of the laser ranging information and the dead reckoning information adopts an extended Kalman filtering algorithm.
Further, the data fusion of the laser ranging information and the dead reckoning information comprises the following steps:
forward distances Sf (Kl) and Sf (Kl +1) at the time Kl and Kl +1 and lateral distances Ss (Kl) and Ss (Kl +1) at the time Kl and Kl +1 are measured using a laser ranging device, and the projections of the east and north directions in the northeast coordinate system are calculated using the following formula:
ΔSf＝Sf(Kl+1)-Sf(Kl) (15)
ΔSs＝Ss(Kl+1)-Ss(Kl) (16)
Ple(Kl+1)＝Ple(Kl)+sin(ψ)*ΔSf+cos(ψ)*ΔSs (17)
Pln(Kl+1)＝Pln(Kl)+cos(ψ)*ΔSf-sin(ψ)*ΔSs (18)
ple and Pln are projections of Sf and Ss in a northeast coordinate system respectively, and psi is a heading angle of the positioning carrier;
making a difference between the position information of Ple and Pln and the position information of dead reckoning, and taking the difference as the observation information of extended Kalman filtering;
selecting 5 state quantities of an east error delta Pe, a north error delta Pn, a speed scale coefficient error delta Sv, a course angle error delta psi and a gyro scale coefficient error delta St of a system error equation, wherein Wt is the angular rate output of the gyroscope, and the error equations are shown in formulas (19) to (23):
Figure BDA0001662198410000041
Figure BDA0001662198410000042
Figure BDA0001662198410000043
Figure BDA0001662198410000044
Figure BDA0001662198410000045
discretizing the formulas (19) to (23) to obtain a discrete system error equation of Kalman filtering, as shown in the formula (24):
Xk+1＝Φk+1,kXk(24)
wherein, Xk＝[ΔPe ΔPn Δψ ΔSv ΔSt]T；
And performing Kalman filtering, and correcting and updating the information of Pe, Pn, psi, Sv and St in dead reckoning by using the state information estimated by the Kalman filtering.
The invention also provides an indoor dead reckoning positioning system, wherein the positioning carrier of the indoor dead reckoning positioning system is provided with a UWB device, a gyroscope, a speed sensor and a laser ranging device, and the indoor dead reckoning positioning method is adopted.
The formula notation explains:
"+" indicates a multiplication number;
"/" denotes a division number;
Figure BDA0001662198410000046
the first derivative of X is indicated.
The technical effects are as follows:
because the environment is complex, a single navigation system is difficult to continuously and accurately give navigation positioning information, and the defects of a single navigation system can be avoided by combining a plurality of navigation systems for application, so that continuous and accurate positioning is realized. The DR system can perform continuous navigation positioning calculation by only depending on data of a sensor of the DR system without depending on external information, but the positioning and heading information of the DR can diverge with time. UWB can provide accurate indoor navigation positioning information, and UWB positioning accuracy does not change with time factors, but because of the complexity of indoor buildings, UWB signals of many indoor places are shielded, and continuous navigation positioning can not be realized, and a navigation positioning system combining DR, UWB and laser ranging can realize that UWB and DR data are fused when UWB is effective, and navigation positioning information can be continuously and accurately output; when UWB is invalid, use laser rangefinder information and DR data fusion, can effectively avoid the problem that pure DR's locating information disperses along with time to and when UWB signal received to block or when the location carrier surpassed UWB effective location area, UWB positioning error sharply increases the negative effect, thereby can improve indoor navigation positioning's positioning accuracy by a wide margin.
The conception, the specific structure and the technical effects of the present invention will be further described with reference to the accompanying drawings to fully understand the objects, the features and the effects of the present invention.
Drawings
FIG. 1 is a system block diagram of a preferred embodiment of the present invention;
FIG. 2 is a coordinate system used in a preferred embodiment of the present invention;
FIG. 3 is a flow chart of a preferred embodiment of the present invention.
Detailed Description
In the description of the embodiments of the present invention, it should be understood that the terms "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", "clockwise", "counterclockwise", etc., indicate orientations or positional relationships based on those shown in the drawings, and are only for convenience of description and simplicity of description, but do not indicate or imply that the referred devices or elements must have a specific orientation, be constructed and operated in a specific orientation, and thus, should not be construed as limiting the invention. The drawings are schematic diagrams or conceptual diagrams, and the relationship between the thickness and the width of each part, the proportional relationship between the parts and the like are not completely consistent with actual values.
FIG. 1 shows a block diagram of a system including a MEMS gyroscope, a velocity sensor (e.g., a wheel encoder), a UWB device and a laser ranging device in accordance with a preferred embodiment of the present invention. The MEMS gyroscope and the speed sensor are respectively used for providing the angular rate and the speed of the positioning carrier, and the monitored angular rate and the monitored speed are used for recursion updating of position and heading information of dead reckoning. Meanwhile, a UWB receiving device and a laser ranging device are also installed on the positioning carrier, and when UWB positioning information is effective, the UWB positioning information and dead reckoning information are subjected to data fusion; and when the UWB positioning information is invalid, carrying out data fusion by adopting the laser ranging information and the dead reckoning information. The data fusion adopts Extended Kalman Filter (EKF), and corrects and updates the state information estimated by the EKF into dead reckoning, so as to determine the positioning information of the positioning carrier.
In the present embodiment, the coordinate systems used are a northeast coordinate system (abbreviated as N system) and a carrier coordinate system (abbreviated as B system), and the definition of the N system and the B system is as follows:
northeast coordinate system (N system for short): the Xn axis points to the east, and the Yn axis points to the north perpendicular to the Xn axis;
vector coordinate system (B series for short): the Xb axis points to the side and Yb points forward perpendicular to the Xb axis as shown in fig. 2, where ψ is the heading angle and clockwise is positive.
As shown in fig. 3, the following description is made in detail with respect to the positioning method of the system of fig. 1.
1. When the positioning carrier moves indoors, the UWB positioning information initializes position information and a heading angle of Dead Reckoning (DR), and the initializing specifically includes the following steps:
1.1, before the heading angle of the DR is not initialized, calculating and updating are not carried out, and UWB is directly used for assigning the position information of the DR based on the position information obtained by the N system;
1.2, when the positioning carrier starts to move, calculating the speed of the positioning carrier by using UWB positioning information, wherein the calculation formula is shown as follows;
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) are east-direction speed and north-direction speed of UWB at Ku +1 moment in N series, Pue (Ku +1) and Pu (Ku) are east-direction position of UWB at Ku +1 and Ku moment in N series, Pu (Ku +1) and Pu (Ku) are north-direction position of UWB at Ku +1 and Ku moment in N series, Tku is time interval of UWB from Ku moment to Ku +1 moment;
1.3, calculating heading angle ψ 0 using the velocity information obtained by the formulas (1) and (2), which is shown below:
ψ0＝(Vue/Vun)*180/PI(3)
wherein Vue and Vun are the current east and north velocities of UWB in the N series, PI is the circumference ratio, and the unit of ψ 0 is degrees;
1.4, judging whether the positioning carrier is in a straight line running or not by using the course angular rate acquired from the MEMS gyroscope, setting the course angular rate of the straight line running to be less than 3-5 degrees per second in the embodiment, if the carrier is in the straight line running, assigning psi 0 obtained by calculation of a formula (3) to the course angle of DR, and assigning position information of DR by using the position information of UWB in an N system at the moment, thereby finishing initialization of DR information.
2. After the position information and the course angle of the DR are initialized, monitoring information of an MEMS gyroscope and a speed sensor is adopted to carry out recursion updating on the position information and the course angle of the DR, and the method specifically comprises the following steps:
2.1, updating the heading angle DR at the moment of Kg +1, wherein the updating formula is as follows:
ψ(Kg+1)＝ψ(Kg)+St*(Wt-Wt0)*Tkg (4)
wherein ψ (Kg +1) and ψ (Kg) are heading angles of DR at Kg +1 and Kg time, respectively, St is a scale factor of the gyroscope, Wt is an angular rate output of the gyroscope, Wt0 is an angular rate zero offset of the gyroscope, Tkg is a time interval of gyroscope data from Kg time to Kg +1 time;
2.2, updating the position information of DR at Kv +1 moment, wherein the updating formula is as follows:
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)
where Pe (Kv +1) and Pn (Kv +1) are the east and north positions of the N series at the Kv +1 time DR, respectively, Pe (Kv) and Pn (Kv) are the east and north positions of the N series at the Kv time DR, respectively, ψ is the heading angle of DR, ψ (Kv +1) + ψ (Kv))/2, Sv is the velocity scale coefficient of the velocity sensor, Vp is the velocity output of the velocity sensor, Tkv is the time interval of the velocity sensor data from the Kv time to the Kv +1 time, Er is the earth radius, Lat (Kv +1) and Lon (Kv +1) are the longitude and latitude of the Kv +1 time DR, Lat (Kv) and Lon (Kv) are the longitude and latitude of the Kv time DR, respectively, Lat (Kv +1) + Lat (Kv) + Lat 2) are the latitude and Lat (Kv +1) + Lat (Lat).
3. The method for detecting the validity of the UWB and laser ranging data information specifically comprises the following steps:
3.1, detecting whether the data information of the UWB is effective, and if so, directly entering data fusion of the UWB and the DR, namely, a step 4;
3.2, if the UWB data information is invalid, starting to detect the data information of the laser ranging, and if the data information of the laser ranging is valid, directly entering the laser ranging and DR data fusion, namely, step 5.
4. The method for carrying out extended Kalman data fusion by using the UWB position information and the DR position information specifically comprises the following steps:
4.1, subtracting the position information of the UWB and the position information of the DR to be used as observation information of the extended Kalman filtering;
4.2, selecting state quantities of a system error equation, namely 5 state quantities including an east error delta Pe, a north error delta Pn, a speed scale coefficient error delta Sv, a heading angle error delta psi and a gyro scale coefficient error delta St, wherein Wt is the angular rate output of the gyroscope, and the error equation is shown in equations (9) to (13):
Figure BDA0001662198410000071
Figure BDA0001662198410000072
Figure BDA0001662198410000073
Figure BDA0001662198410000074
Figure BDA0001662198410000075
discretizing the equations (9) to (13) to obtain a discrete system error equation of Kalman filtering, as shown in equation (14):
Xk+1＝Φk+1,kXk(14)
wherein, Xk＝[ΔPe ΔPn Δψ ΔSv ΔSt]T；
And 4.3, performing Kalman filtering, and correcting and updating the information of Pe, Pn, psi, Sv and St in the DR by using the state information estimated by the Kalman filtering.
5. The method for carrying out extended Kalman data fusion by using the data information of laser ranging and the position information of DR specifically comprises the following steps:
5.1, forward distances Sf (Kl) and Sf (Kl +1) at the moment Kl and the moment Kl +1 and lateral distances Ss (Kl) and Ss (Kl +1) at the moment Kl and the moment Kl +1 are measured by using a laser ranging device, and the projections of east and north in the N system are calculated by using the following formula:
ΔSf＝Sf(Kl+1)-Sf(Kl) (15)
ΔSs＝Ss(Kl+1)-Ss(Kl) (16)
Ple(Kl+1)＝Ple(Kl)+sin(ψ)*ΔSf+cos(ψ)*ΔSs (17)
Pln(Kl+1)＝Pln(Kl)+cos(ψ)*ΔSf-sin(ψ)*ΔSs (18)
ple and Pln are projections of Sf and Ss in an N system respectively, and psi is a heading angle of the positioning carrier;
5.2, making a difference between the position information of Ple and Pln and DR to serve as observation information of the extended Kalman filtering;
5.3, selecting state quantities of a system error equation, wherein the state quantities are 5 state quantities including an east error delta Pe, a north error delta Pn, a speed scale coefficient error delta Sv, a heading angle error delta psi and a gyro scale coefficient error delta St, Wt is the angular rate output of the gyroscope, and the error equation is shown in formulas (19) to (23):
Figure BDA0001662198410000081
Figure BDA0001662198410000082
Figure BDA0001662198410000083
Figure BDA0001662198410000084
Figure BDA0001662198410000085
discretizing the formulas (19) to (23) to obtain a discrete system error equation of Kalman filtering, as shown in the formula (24):
Xk+1＝Φk+1,kXk(24)
wherein, Xk＝[ΔPe ΔPn Δψ ΔSv ΔSt]T；
And 5.4, performing Kalman filtering, and correcting and updating the information of Pe, Pn, psi, Sv and St in the DR by using the state information estimated by the Kalman filtering.
The indoor DR positioning method and system can effectively avoid the problem that the positioning information of pure DR diverges along with time, and when a UWB signal is blocked or a positioning carrier exceeds an effective UWB positioning area, the negative influence of sudden increase of UWB positioning error can be avoided, so that the positioning precision of indoor navigation positioning can be greatly improved.
The foregoing detailed description of the preferred embodiments of the invention has been presented. It should be understood that numerous modifications and variations could be devised by those skilled in the art in light of the present teachings without departing from the inventive concepts. Therefore, the technical solutions available to those skilled in the art through logic analysis, reasoning and limited experiments based on the prior art according to the concept of the present invention should be within the scope of protection defined by the claims.

Claims (7)

1. An indoor dead reckoning positioning method is characterized in that UWB positioning information and laser ranging information are used as auxiliary materials; when the UWB positioning information is effective, carrying out data fusion by adopting the UWB positioning information and the dead reckoning information; when the UWB positioning information is invalid, carrying out data fusion by adopting laser ranging information and dead reckoning information; when the dead reckoning information needs to be initialized, initializing the dead reckoning position information and the course angle by adopting UWB positioning information; the position and heading initialization of dead reckoning includes:
before the course angle of the dead reckoning is not initialized, the dead reckoning is not updated, and the position information of the dead reckoning is assigned by directly using the position information of the UWB;
when the positioning carrier starts to move, calculating the speed of the positioning carrier by using UWB positioning information, wherein the calculation formula is shown as follows;
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) are an east-direction speed and a north-direction speed of the UWB at the time of Ku +1 in the northeast coordinate system respectively, Pue (Ku +1) and Pu (Ku) are east-direction positions of the UWB at the time of Ku +1 and Ku in the northeast coordinate system respectively, Pu (Ku +1) and Pu (Ku) are north-direction positions of the UWB at the time of Ku +1 and Ku in the northeast coordinate system respectively, and Tku is a time interval of the UWB from the time of Ku to the time of Ku + 1;
the heading angle ψ 0 is calculated using the velocity information obtained by the formulas (1) and (2), which are shown below:
ψ0＝(Vue/Vun)*180/PI (3)
wherein Vue and Vun are the current east and north velocities of UWB in the northeast coordinate system, respectively, PI is the circumferential ratio, and the unit of ψ 0 is degrees;
and when the positioning carrier is detected to do linear motion, taking psi 0 calculated by the formula (3) as a course angle of dead reckoning, and assigning dead reckoning position information by using the current UWB position information, thereby finishing the initialization of dead reckoning information.
2. The indoor dead reckoning positioning method as claimed in claim 1, wherein it is detected whether the positioning carrier is moving linearly by a gyroscope.
3. An indoor dead reckoning positioning method as set forth in claim 1, characterized in that information of a gyroscope and a speed sensor is used to update the position information and the heading angle of dead reckoning recurrently.
4. An indoor dead reckoning positioning method as claimed in claim 3, wherein using information of gyroscope and speed sensor to update the position information and course angle of dead reckoning recurrently comprises:
updating the course angle of dead reckoning at the Kg +1 moment, wherein the updating formula is as follows:
ψ(Kg+1)＝ψ(Kg)+St*(Wt-Wt0)*Tkg (4)
wherein ψ (Kg +1) and ψ (Kg) are heading angles of dead reckoning at Kg +1 and Kg time, respectively, St is a scale factor of the gyroscope, Wt is an angular rate output of the gyroscope, Wt0 is an angular rate zero offset of the gyroscope, Tkg is a time interval from Kg time to Kg +1 time of the gyroscope data;
updating position information of dead reckoning at Kv +1 moment, wherein an updating formula is as follows:
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)
where Pe (Kv +1) and Pn (Kv +1) are the easting and northing positions of the Kv +1 time dead reckoning in the northeast coordinate system, respectively, Pe (Kv) and Pn (Kv) are the easting and northing positions of the Kv time dead reckoning in the northeast coordinate system, respectively, ψ is the heading angle of the dead reckoning, ψ (Kv +1) + ψ (Kv))/2, Sv is the velocity scale coefficient of the velocity sensor, Vp is the velocity output of the velocity sensor, Tkv is the time interval of the velocity sensor data from the Kv time to the Kv +1 time, Er is the earth radius, Lat (Kv +1) and Lon (Kv +1) are the longitude and latitude of the Kv +1 time dead reckoning, respectively, Lat (Kv) and Lon (Kv) are the longitude and longitude of the Kv time dead reckoning, respectively, and Lat (Lat +1) + (Lat) and lan (Kv) are the longitude and Lat (Lat + 2).
5. An indoor dead reckoning positioning method as recited in claim 1, wherein the data fusion of the UWB positioning information and the dead reckoning information employs an extended Kalman filtering algorithm.
6. An indoor dead reckoning positioning method as recited in claim 1, wherein data fusion of the laser ranging information and the dead reckoning information employs an extended Kalman filtering algorithm.
7. An indoor dead reckoning positioning system characterized in that a positioning carrier is provided with a UWB device, a gyroscope, a speed sensor and a laser ranging device, and the indoor dead reckoning positioning method as claimed in any one of claims 1 to 6 is employed.
CN201810466502.1A 2018-05-16 2018-05-16 Indoor dead reckoning positioning method and system based on UWB and laser ranging Active CN108680167B (en)

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 CN108680167A (en) 2018-10-19
CN108680167B true 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)

Families Citing this family (2)

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

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

Patent Citations (8)

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

* Cited by examiner, † Cited by third party
Title
A constraint approach for UWB and PDR fusion;Francisco Zampella等;《2012 International Conference on Indoor Positioning and Indoor Navigation (IPIN)》;20130124;全文 *
基于EKF的DR/激光测距组合导航算法研究;曹学伟等;《山东科技大学学报(自然科学版)》;20090430;第28卷(第2期);第84-87页 *
基于UWB/MEMS的高精度室内定位技术研究;杨洲;《中国优秀硕士学位论文全文数据库 信息科技辑》;20160215(第2期);第I136-583页 *
基于扩展卡尔曼滤波的动中通低成本姿态估计;闫林波等;《电光与控制》;20130731;第20卷(第7期);第53-57页 *
超宽带与惯性导航组合的室内导航定位;刘韬等;《测绘科学》;20161231;第41卷(第12期);第162-166页 *

Also Published As

Publication number Publication date
CN108680167A (en) 2018-10-19

Similar Documents

Publication Publication Date Title
US11243081B2 (en) Slam assisted INS
KR100518852B1 (en) Method for dead reckoning for backward improvement of mobile
JP4694870B2 (en) Navigation device, navigation system, navigation positioning method, and vehicle
US10309786B2 (en) Navigational and location determination system
AU2019203458B2 (en) Dead Reckoning-Augmented GPS for Tracked Vehicles
KR101473653B1 (en) Pedestrian Dead-Reckoning apparatus based on pedestrian motion recognition and method thereof
EP2585852A1 (en) Moving platform ins range corrector (mpirc)
CN111025366B (en) Grid SLAM navigation system and method based on INS and GNSS
CN107274721A (en) Many vehicle cooperative localization methods in a kind of intelligent transportation system
CN108680167B (en) Indoor dead reckoning positioning method and system based on UWB and laser ranging
CN108871325B (en) A kind of WiFi/MEMS combination indoor orientation method based on two layers of Extended Kalman filter
CN102998689A (en) Area judging method based on virtual sensor
US20140249750A1 (en) Navigational and location determination system
CN113029148A (en) Inertial navigation indoor positioning method based on course angle accurate correction
CN114111802A (en) Pedestrian dead reckoning assisted UWB positioning method
Zhou et al. Wi-Fi RTT/encoder/INS-based robot indoor localization using smartphones
Xia et al. A novel PDR aided UWB indoor positioning method
US10859379B2 (en) Systems and methods with dead-reckoning
US9250078B2 (en) Method for determining the position of moving objects
Ray et al. GPS and sonar based area mapping and navigation by mobile robots
KR101987413B1 (en) System and method for positioning
Luo et al. Crawler Robot Indoor Positioning Based on a Combination of Bluetooth and IMU
Prusaczyk et al. GPS-free navigation based on using inertial and odometry, data fusion and map matching algorithm
CN105651281A (en) Space positioning algorithm on basis of multi-direction human movement modes
Yao et al. Tightly coupled indoor positioning using UWB/mmWave radar/IMU

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

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.

CP03 Change of name, title or address