CN105424041A - Pedestrian positioning algorithm based on BD/INS (Beidou/Inertial Navigation System) tight coupling - Google Patents

Abstract

The invention discloses a pedestrian positioning algorithm based on BD/INS (Beidou/Inertial Navigation System) tight coupling. The pedestrian positioning algorithm comprises the following steps: 1) acquiring pseudo-range, pseudo-range rate and satellite ephemeris information data, accelerator data, magnetometer data and gyroscope data; 2) synchronizing a BD satellite navigation subsystem and an INS subsystem; 3) acquiring a pedestrian position by using accelerator data, a pedestrian posture and a PDR (Pedestrian Dead Reckoning) algorithm; 4) predicting a pseudo-range and a pseudo-range rate of a BD signal according to the pedestrian position, speed and BD satellite ephemeris; subtracting the predicted pseudo-range and pseudo-range rate of the BD signal from an actual measured value obtained from the BD satellite navigation system to form a residual and obtain a pedestrian position and speed correction amount; 5) finally adding the pedestrian position and speed correction amount and the pedestrian position and speed previously output from the INS module to obtain the corrected pedestrian position information. According to the pedestrian position algorithm disclosed by the invention, a carrier posture can be obtained by using a triaxial accelerator, a triaxial magnetometer and an auxiliary gyroscope on the basis of a pedestrian track plotting PDR algorithm, and the problem of long-time INS error accumulation is overcome.

Description

A kind of based on the tightly coupled pedestrian's location algorithm of BD/INS

Technical field

The present invention is the technical research to pedestrian navigation technological system scheme, belong to integrated navigation and location technical field, special for low cost inertial navigation system (hereinafter referred to as INS, InertialNavigationSystem) in easily there is drift and can not locate for a long time and be easily blocked with signal in the Big Dipper (hereinafter referred to as BD, BeiDou) satellite navigation system and locate inaccurate situation in gyroscope; Be specifically related to a kind of based on the tightly coupled pedestrian's location algorithm of BD/INS.

Background technology

Along with the appearance being the Intelligent mobile equipment of representative with smart mobile phone, notebook computer, panel computer etc. and development, mobile Internet obtains to be applied more and more widely, the tool software made based on mobile platform and Information Mobile Service emerge in large numbers like the mushrooms after rain, for all kinds of industry user and public users provide abundant service.In mobile Internet, location Based service is that frequency of utilization is the highest, one of the service that is most widely used, and what other Mobile solution many were all direct or indirect has used LBS (LocationBasedService).By means of various navigation positioning system, in the open place such as outdoor indoor, LBS can provide the location-based service of high precision, high stability for user, and its application has penetrated into industry-by-industry field and general marketplace.But in the more indoor place of mankind's activity, because buildings blocks and the problems such as the defect of positioning system itself, and then the accurate location at user place cannot be measured exactly.Therefore, study stable location technology, to meet one of the study hotspot in the positioning system mobile computing field of industry user and popular application demand.

INS navigational system is a kind of system not relying on external information, has to be convenient to pedestrian and to carry and the advantage such as anti-interference, and shortcoming is that gyro error can be accumulated in time, therefore cannot work independently for a long time.BD satellite navigation system can provide round-the-clock for pedestrian, the information such as continuous print three-dimensional position, three-dimensional velocity and time, shortcoming be dynamic poor, be subject to electromagnetic interference (EMI), in city, Satellite signal is easily blocked.In addition, also have the multiple pedestrian navigation locator meams such as GLONASS, WIFI and Cellular Networks, but all there are respective relative merits, can only as a part for integrated navigation.In various pedestrian's integrated navigation system, the complementarity of BD/INS integrated navigation system is the strongest, and best performance, may be used for public security fire-fighting, rescue and relief work, intelligent transportation etc., has wide application prospect.

The array mode of current BD/INS has three kinds, is respectively loose coupling, tight coupling and hypercompact coupling.In loosely coupled system, BD satellite navigation system and INS navigational system independent operating separately, when BD satellite navigation system and INS navigational system all can normally work, the position and speed information exported separately is carried out data fusion by junction filter, improve positioning precision; In tightly coupled system, mainly utilize the pseudorange that BD satellite navigation system provides, the raw informations such as pseudorange rates, the raw information that the sensor in conjunction with INS navigational system exports is processed initial parameter information by Navigation Filter, provide optimum positioning result, hereafter will tell about in detail; In hypercompact coupled system, BD/INS combination occurs in tracking loop, INS measurement information is directly utilized to assist BD code tracking loop and carrier tracking loop, and in BD/INS combined system, replace traditional tracking loop loop by an integrated kalman filter device backfeed loop, thus improve the precision that Beidou receiver exports data.

In these three kinds of array modes, loosely coupled system is the simplest, be convenient to Project Realization, but BD and INS two navigation subsystem works alone, and navigation information can be caused to have certain redundance, poor stability; Simultaneously loose combined system is high to BD system dependence, at least will there is the navigation information that 4 satellites just can obtain BD system and upgrade wave filter; And the position that BD system provides and velocity error are all time correlation usually.Loose coupling at home and abroad investigative technique is all very ripe, plays the militaries such as American and Britain and civilian company is just interested in GPS/INS combined system as far back as the eighties in 20th century, in 1986 just in the combining and take a flight test of the companies such as Boeing; The Wang Yandong of domestic Tsing-Hua University etc. just have studied GPS/INS pine combination (list of references: Wang Yandong based on hardware-in-the-loop simulation in 1999, Huang Jixun, model jumps the research of ancestral .GPS/INS integrated navigation system hardware-in-the-loop simulation. BJ University of Aeronautics & Astronautics's journal, 1999,03:299-301.) etc.

Compared with loose coupling, tight coupling can obtain higher positioning precision, and can locate when the number of visible star is less than 4, improves dynamic and the antijamming capability of receiver.The Li Ruijiang etc. of domestic Northwestern Polytechnical University devises auto adapted filtering tight coupling scheme; The Sun Dao province etc. of Beijing Automation Control Equipment Research Institute have employed the Federated filter method etc. of GPS/INS tight coupling navigational system, improves the precision of GPS/INS combined system all to some extent, enhances the stability of system, overcome the shortcoming of loose coupling.

The people such as the DonaldGustafson in U.S. Draper laboratory in 2000 specify that hypercompact coupling scheme, its concept that the GautierJD of Stanford Univ USA is perfect further; The Di Manmin etc. of the domestic National University of Defense technology is studied the anti-interference of GPS and the hypercompact coupling technique of inertia; The incoherent searching algorithm of time domain that Huang new life of the National University of Defense technology etc. then utilizes MIMU to catch improves the capture ability etc. to gps signal, but all rests on simulation stage, does not obtain concrete physical engineering verification experimental verification.Hypercompact coupling not only at height excellent performance dynamically and under strong jamming condition, and has good rejection ability and corrective action to multipath effect.But this array mode needs to be deep into receiver inside, even relate to the layout again of internal code ring carrier wave loop circuit, too complicated, can be subject to the restriction from BD satellite chip manufacturer, opening interface is less simultaneously, is unfavorable for the popularization of BD location technology.

Current BD/INS integrated navigation system is generally based on onboard system, less to the research of pedestrian's application.Compare onboard system, have following several difficult point in pedestrian's application aspect: the more difficult judgement of pedestrian's attitude; The long-time accumulation of error causes location difficulty; Pedestrian activity's circumstance complication, BD signal is easily blocked.

Summary of the invention

In view of this, the object of this invention is to provide one based on the tightly coupled pedestrian's location algorithm of BD/INS.

The object of the invention is to be achieved through the following technical solutions, a kind of based on the tightly coupled pedestrian's location algorithm of BD/INS, comprise the following steps:

Step 1) utilize BD satellite navigation subsystem to obtain pseudorange, pseudorange rates and satellite ephemeris information; The MEMS Inertial Measurement Unit in INS subsystem is utilized to obtain accelerometer, magnetometer and gyro data;

Step 2) carry out synchronously, comprising spatial synchronization and time synchronized to BD satellite navigation subsystem and INS subsystem;

Step 3) fusion of INS inertial reference calculation module accelerometer, magnetometer and gyro data, calculate pedestrian's attitude; Utilize accelerometer data, pedestrian's attitude and PDR algorithm, obtain pedestrian position;

Step 4) by step 2) after, according to pedestrian position, speed and BD satellite ephemeris that INS inertial reference calculation module exports, dope the pseudorange of BD signal, pseudorange rates; Subtract each other formation residual error by doping the actual measured value that the pseudorange of BD signal, pseudorange rates and BD satellite navigation system obtain, this residual error, after filtering after device, obtains pedestrian position, velocity correction amount;

Step 5) finally will by step 4) pedestrian position, speed addition that the pedestrian position that obtains, velocity correction amount export with INS module before, the pedestrian position information after correction can be obtained.

Further, described spatial synchronization comprises and the BD information under Chinese earth coordinates and the INS pedestrian information under carrier coordinate system are transformed into east northeast sky navigational coordinate system carry out data fusion.

Further, described method BD information under Chinese earth coordinates being transformed into east northeast sky navigational coordinate system comprises following sub-step:

Step 31) BD information under Chinese earth coordinates is transformed into rectangular coordinate system in space;

Step 32) BD information under rectangular coordinate system in space is transformed into east northeast sky inertial coordinates system.

Further, the method for described time synchronized is:

Step 41) detect the high level pps pulse per second signal produced when each almanac data of Beidou receiver comes critical;

Step 42) open serial data passage corresponding to Beidou receiver and almanac data be written in the data file of specifying;

Step 43) when almanac data reads in complete, serial ports is switched to the serial data passage that MEMS Inertial Measurement Unit is corresponding, then write MEMS nine axle sensor data until next pulse per second (PPS) arrives;

Step 44) when next pulse per second (PPS) arrives, data are read in passage and are then switched to serial data passage corresponding to Beidou receiver; Switch so back and forth until stop receiving data, finally complete BD data and MEMS sensor data in time synchronous.

Further, the pedestrian of described INS inertial reference calculation module output is at t _kthe position P in moment _k(N _k, E _k) be:

Wherein, N _k, t _krepresent E respectively _ktime be engraved in the position in positive north, due east, N ₀, E ₀be illustrated respectively in t ₀time be engraved in the position in positive north, due east; d _ktime interval t _k-1to t _kthe size of motion vector, θ _kit is the direction of motion vector.

Owing to have employed technique scheme, the present invention has following advantage:

The present invention is on the basis of pedestrian's reckoning PDR (PedestrianDeadReckoning) algorithm, utilize three axis accelerometer, three axis magnetometer secondary gyroscope, application extension Kalman filter realizes each Data Fusion of Sensor, not only can obtain attitude of carrier, and overcome the problem of long-time INS error accumulation; When BD signal deletion, establish the tightly coupled integrated navigation wave filter of BD/INS, decrease BD positioning error; Meanwhile, BD/INS integrated navigation system, by integrated navigation wave filter, makes its precision all higher than the precision of independent navigational system.

Accompanying drawing explanation

In order to make the object, technical solutions and advantages of the present invention clearly, below in conjunction with accompanying drawing, the present invention is described in further detail, wherein:

Fig. 1 is the Big Dipper in the present invention/INS tightly coupled system block diagram;

Fig. 2 is PDR schematic diagram;

Fig. 3 is that INS resolves schematic diagram.

Embodiment

Below with reference to accompanying drawing, the preferred embodiments of the present invention are described in detail; Should be appreciated that preferred embodiment only in order to the present invention is described, instead of in order to limit the scope of the invention.

INS navigational system is a kind of system not relying on external information, has to be convenient to pedestrian and to carry and the advantage such as anti-interference, and shortcoming is that gyro error can be accumulated in time.The present invention will solve the problem that INS navigational system cannot work independently for a long time.BD satellite navigation system can provide round-the-clock for pedestrian, the information such as continuous print three-dimensional position, three-dimensional velocity and time, shortcoming be dynamic poor, be subject to electromagnetic interference (EMI), in city, Satellite signal is easily blocked.

The present invention is applicable to INS and works long hours separately error accumulation and cause the situation of dispersing; Beidou satellite navigation system is because trees, and building etc. block and occur the situation compared with big error; There is unstable and that error is larger situation in the location of independent positioning system.

A kind of based on the tightly coupled pedestrian's location algorithm of BD/INS, comprise the following steps:

Step 1) utilize BD satellite navigation subsystem to obtain pseudorange, pseudorange rates and satellite ephemeris information; The MEMS Inertial Measurement Unit in INS subsystem is utilized to obtain accelerometer, magnetometer and gyro data;

Step 2) carry out synchronously, comprising spatial synchronization and time synchronized to BD satellite navigation subsystem and INS subsystem;

Spatial synchronization: the coordinate system that the present invention uses comprises carrier coordinate system (b system), east northeast sky navigational coordinate system (NEU), Chinese earth coordinates (CGCS2000) and rectangular coordinate system in space.The low cost gyroscope precision comparison adopted due to the present invention is low, INS subsystem does not seek the function in north, so when carrying out real-time resolving be on the right side of initial position, the coordinate system of front, three axis in sky, namely under carrier coordinate system, carry out resolving of pedestrian's attitude.Now need that the BD information under Chinese earth coordinates (CGCS2000) and the INS pedestrian information under carrier coordinate system are transformed into east northeast sky (NEU) navigational coordinate system and carry out data fusion.

BD satellite navigation system coordinate conversion

BD information under China earth coordinates (CGCS2000) is transformed into east northeast sky navigational coordinate system conversion needs 2 steps: the BD information 1), under CGCS2000 coordinate system is transformed into rectangular coordinate system in space; 2) the BD information, under rectangular coordinate system in space is transformed into east northeast sky inertial coordinates system.

The position of known target K under CGCS2000 earth coordinates (L, B, H), the formula sitting (X, Y, Z) under asking rectangular coordinate system is:

Wherein L, B, H are respectively longitude, latitude and the height under earth coordinates: X, Y, Z are respectively vector under the rectangular coordinate system after conversion; N is the prime vertical radius of ellipsoid e is the first excentricity of ellipsoid wherein a is the oval major semi-axis of the earth, and Big Dipper file of eating dishes without rice or wine is set to a=6378.137km, and b is the oval minor semi-axis of the earth, b=6356.7523142km.

Rectangular coordinate system in space to the formula of east northeast sky navigational coordinate system as shown in (6) formula:

In formula, N _qP, E _qP, U _qPthe navigation coordinate of a Q relative to P; X _p, Y _p, Z _p, X _q, Y _q, Z _qp, the Q coordinate under O-XYZ respectively.

INS navigational system coordinate conversion

The conversion that INS pedestrian information under carrier coordinate system is transformed into east northeast sky navigational coordinate system only needs once to rotate:

Wherein be called attitude (rotation) matrix, it is orthogonal matrix, meets it reflects the position relation between navigational coordinate system and carrier coordinate system.Wherein the form of rotation matrix is relevant with rotation order.

The present embodiment rotation sequence is followed successively by ψ, θ, γ.Wherein, ψ is the angle (course angle) rotated around U axle, and θ is the angle (angle of pitch) rotated around E axle, and γ is the angle (roll angle) rotated around N axle.

Time synchronized:

Beidou receiver exports an almanac data p.s., can produce the pps pulse per second signal of high level when every secondary data is arrived.Utilizing the external interrupt function of ARM embedded device to detect pps pulse per second signal based on this present embodiment, when signal being detected to meet, serial data passage corresponding for Beidou receiver being opened, BD data are write in the data file of specifying simultaneously.When almanac data reads in complete, RAM embedded device is switched to serial data passage corresponding to MEMS, then writes MEMS nine axle sensor data, until next pulse per second (PPS) arrives.When next pulse per second (PPS) arrives, data are read in passage and are then switched to serial data passage corresponding to Beidou receiver.Switch so back and forth until stop receiving data, finally complete BD data and MEMS sensor data in time synchronous.

Step 3) fusion of INS inertial reference calculation module accelerometer, magnetometer and gyro data, calculate pedestrian's attitude; Utilize accelerometer data, pedestrian's attitude and PDR algorithm, obtain pedestrian position;

INS resolves block diagram as shown in Figure 3.INS subsystem first degree of will speed up meter three axle exports data by carrying out gait detection and step number estimation after low-pass filter smoothing processing, as location updating point; Then adopt empirical model, estimate the step-length of each step of pedestrian, as the size of point-to-point displacement; Then adopt the form of three axis accelerometer and magnetometer secondary gyroscope, after realizing each Data Fusion of Sensor with EKF wave filter, obtain attitude quaternion, calculate pedestrian's attitude; Finally extrapolate pedestrian's flight path with PDR algorithm.

INS inertial reference calculation module adopts PDR algorithm to carry out pedestrian location.

The ultimate principle of PDR algorithm is under the condition of known current time position, by measuring distance d and the orientation θ of pedestrian's movement, can calculate subsequent time position in real-time continuous ground.Pedestrian is regarded as the motion on two dimensional surface, as shown in Figure 2, E, N represent due east, orientation, Zheng Bei to its schematic diagram respectively.Suppose that pedestrian is at t ₀the position in moment is P ₀(N ₀, E ₀), so subsequent time t ₁the position in moment is P ₁(N ₁, E ₁), from P ₀to P ₁during this period of time in course angle be θ ₀, then corresponding relation can be expressed as formula (1):

Then pedestrian is at t ₂the position P in moment ₂(N ₂, E ₂) be:

Calculate thus, then have pedestrian at t _kthe position P in moment _k(N _k, E _k) be:

D _ktime interval t _k-1to t _ithe size of motion vector, θ _kit is the direction of motion vector.

Step 4) by step 2) after, according to pedestrian position, speed and BD satellite ephemeris that INS inertial reference calculation module exports, dope the pseudorange of BD signal, pseudorange rates; Subtract each other formation residual error by doping the actual measured value that the pseudorange of BD signal, pseudorange rates and BD satellite navigation system obtain, this residual error, after Navigation Filter, obtains pedestrian position, velocity correction amount;

In the present invention, Navigation Filter is PF wave filter, not only solves nonlinear problem, turn reduces the calculated amount of high dimension, improves engineering efficiency, effectively can estimate state, makes the square error of estimation minimum simultaneously.By the error analysis to whole integrated navigation system, choose the east orientation in geographic coordinate system and north orientation site error δ E, δ N, vertical error δ h, east orientation and north orientation velocity error δ V _e, δ V _n, height velocity error delta V _h, the corresponding distance δ t of equivalent clocking error _u, equivalent clocking error corresponding distance rate δ t _ru, heading angle deviation margin of error B _efor quantity of state.In following formula, X represents the quantity of state of system.

X＝[δEδNδhδV _EδV _NδV _hδt _uδt _ruB _e] ^T

Observed quantity is that pseudorange that is that BD exports and that obtained by INS navigation system computes is poor, pseudorange rate variance and course angular difference, namely

The pseudorange recording i-th (i=1,2,3,4) satellite is ρ _ci, pseudorange rates is course angle is θ _bD, the pseudorange obtained by INS information is ρ _ii, pseudorange rates is course angle is θ _iNS.

Step 5) finally will by step 4) pedestrian position, speed addition that the pedestrian position that obtains, velocity correction amount export with INS module before, the pedestrian position information after correction can be obtained.

The foregoing is only the preferred embodiments of the present invention, be not limited to the present invention, obviously, those skilled in the art can carry out various change and modification to the present invention and not depart from the spirit and scope of the present invention.Like this, if these amendments of the present invention and modification belong within the scope of the claims in the present invention and equivalent technologies thereof, then the present invention is also intended to comprise these change and modification.

Claims (5)

1., based on the tightly coupled pedestrian's location algorithm of BD/INS, it is characterized in that: comprise the following steps:

Step 1) utilize BD satellite navigation subsystem to obtain pseudorange, pseudorange rates and satellite ephemeris information; The MEMS Inertial Measurement Unit in INS subsystem is utilized to obtain accelerometer, magnetometer and gyro data;

Step 2) carry out synchronously, comprising spatial synchronization and time synchronized to BD satellite navigation subsystem and INS subsystem;

Step 3) fusion of INS inertial reference calculation module accelerometer, magnetometer and gyro data, calculate pedestrian's attitude; Utilize accelerometer data, pedestrian's attitude and PDR algorithm, obtain pedestrian position;

Step 4) by step 2) after, according to pedestrian position, speed and BD satellite ephemeris that INS inertial reference calculation module exports, dope the pseudorange of BD signal, pseudorange rates; Subtract each other formation residual error by doping the actual measured value that the pseudorange of BD signal, pseudorange rates and BD satellite navigation system obtain, this residual error, after filtering after device, obtains pedestrian position, velocity correction amount;

Step 5) finally will by step 4) pedestrian position, speed addition that the pedestrian position that obtains, velocity correction amount export with INS module before, the pedestrian position information after correction can be obtained.

2. according to claim 1 based on the tightly coupled pedestrian's location algorithm of BD/INS, it is characterized in that: described spatial synchronization comprises and the BD information under Chinese earth coordinates and the INS pedestrian information under carrier coordinate system are transformed into east northeast sky navigational coordinate system carry out data fusion.

3. according to claim 2 based on the tightly coupled pedestrian's location algorithm of BD/INS, it is characterized in that: described method BD information under Chinese earth coordinates being transformed into east northeast sky navigational coordinate system comprises following sub-step:

Step 31) BD information under Chinese earth coordinates is transformed into rectangular coordinate system in space;

Step 32) BD information under rectangular coordinate system in space is transformed into east northeast sky inertial coordinates system.

4. according to claim 3 based on the tightly coupled pedestrian's location algorithm of BD/INS, it is characterized in that: the method for described time synchronized is:

Step 41) detect the high level pps pulse per second signal produced when each almanac data of Beidou receiver comes critical;

Step 42) open serial data passage corresponding to Beidou receiver and almanac data be written in the data file of specifying;

Step 43) when almanac data reads in complete, serial ports is switched to the serial data passage that MEMS Inertial Measurement Unit is corresponding, then write MEMS nine axle sensor data until next pulse per second (PPS) arrives;

Step 44) when next pulse per second (PPS) arrives, data are read in passage and are then switched to serial data passage corresponding to Beidou receiver; Switch so back and forth until stop receiving data, finally complete BD data and MEMS sensor data in time synchronous.

5. according to claim 1 based on the tightly coupled pedestrian's location algorithm of BD/INS, it is characterized in that: the pedestrian that described INS inertial reference calculation module exports is at t _kthe position P in moment _k(N _k, E _k) be:

$N_k=N_0+\underset{k=0}{\overset{k-1}{\Σ}}{d}_k{\mathrm{cos\θ}}_k---\left(3\right)$

$E_k=E_0+\underset{k=0}{\overset{k-1}{\Σ}}{d}_k{\mathrm{sin\θ}}_k---\left(4\right)$

Wherein, N _k, t _krepresent E respectively _ktime be engraved in the position in positive north, due east, N ₀, E ₀be illustrated respectively in t ₀time be engraved in the position in positive north, due east; d _ktime interval t _k-1to t _kthe size of motion vector, θ _kit is the direction of motion vector.