CN114440880B - Construction site control point positioning method and system based on self-adaptive iterative EKF - Google Patents
Construction site control point positioning method and system based on self-adaptive iterative EKF Download PDFInfo
- Publication number
- CN114440880B CN114440880B CN202210108939.4A CN202210108939A CN114440880B CN 114440880 B CN114440880 B CN 114440880B CN 202210108939 A CN202210108939 A CN 202210108939A CN 114440880 B CN114440880 B CN 114440880B
- Authority
- CN
- China
- Prior art keywords
- control point
- distance
- iteration
- adaptive
- reference node
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 40
- 238000010276 construction Methods 0.000 title claims abstract description 38
- 230000003044 adaptive effect Effects 0.000 claims abstract description 47
- 238000001914 filtration Methods 0.000 claims abstract description 8
- 239000011159 matrix material Substances 0.000 claims description 29
- 238000005259 measurement Methods 0.000 claims description 27
- 238000004422 calculation algorithm Methods 0.000 claims description 12
- 238000009499 grossing Methods 0.000 claims description 7
- 238000005070 sampling Methods 0.000 claims description 2
- 238000005516 engineering process Methods 0.000 description 13
- 230000004927 fusion Effects 0.000 description 5
- 238000004891 communication Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 230000001186 cumulative effect Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000005315 distribution function Methods 0.000 description 2
- 238000001228 spectrum Methods 0.000 description 2
- 238000004590 computer program Methods 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000007499 fusion processing Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000008520 organization Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000026676 system process Effects 0.000 description 1
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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
-
- 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
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/46—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being of a radio-wave signal type
-
- 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
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
-
- 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
- G01S5/0263—Hybrid positioning by combining or switching between positions derived from two or more separate positioning systems
- G01S5/0264—Hybrid positioning by combining or switching between positions derived from two or more separate positioning systems at least one of the systems being a non-radio wave positioning system
-
- 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/0294—Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
The invention discloses a construction site control point positioning method and system based on self-adaptive iteration EKF, comprising the following steps: acquiring position information of a control point; taking the position error and the speed error of an inertial navigation system INS in the east direction and the north direction acquired at the moment t as the state vector of the adaptive iterative extended Kalman filter; when GNSS data is available, taking the square difference between the estimated value of the distance between the control point and the GNSS and the estimated value of the distance between the control point and the INS reference node as an observation vector of the adaptive iterative extended Kalman filter; when GNSS data is not available, taking the square difference of the estimated value of the distance between the control point and the UWB reference node and the estimated value of the distance between the control point and the INS reference node as an observation vector of the adaptive iterative extended Kalman filter; and filtering by utilizing a self-adaptive iteration extended Kalman filter based on the state vector and the observation vector to obtain the distance between the control point and the navigation satellite or between the control point and the UWB positioning base station.
Description
Technical Field
The invention relates to the technical field of GNSS/INS/UWB fusion positioning under a complex environment, in particular to a construction site control point positioning method and system based on an adaptive iteration EKF (extended Kalman filter).
Background
The statements in this section merely provide background information related to the present disclosure and may not necessarily constitute prior art.
In recent years, with the development of scientific technology, research and application of complex constructional engineering technology have advanced, including accurate positioning of control points in construction sites. The control point is a measurement coordinate point used as construction control, and various measurements must be performed during the construction stage of the building engineering to ensure high-quality implementation of the building engineering. Such as measurement of known length, measurement of known angle, planar measurement of building minutiae points, position measurement of building minutiae points, etc. And accurate positioning measurement of the control points of the construction site is an important precondition for high-quality completion of the measurement work.
Among the existing positioning methods, the global satellite navigation system (Global Navigation Satellite System, GNSS) is one of the most commonly used. Although the GNSS can continuously and stably pass through the accurate position information, the application range is limited by the disadvantage that the GNSS is easily affected by external environments such as electromagnetic interference and shielding, and especially in some closed spaces such as indoor and underground roadways or in scenes with complex environments such as heavy fog, the GNSS signals are seriously shielded, and effective work cannot be performed. In recent years, ultra Wide Band (UWB) technology has been widely used in the field of short-range local positioning. UWB technology is a wireless carrier communication technology that does not use a sinusoidal carrier, but uses non-sinusoidal narrow pulses on the order of nanoseconds to transmit data, and therefore occupies a wide spectrum, so it is called ultra-wideband technology. The UWB technology has the advantages of low system complexity, low power spectrum density of the transmitted signal, insensitivity to channel weakness, high positioning accuracy and the like.
The prior art proposes to apply UWB-based target tracking to the location of a control point of a construction site in a GNSS failure environment. Although the mode can realize the positioning of the control point of the construction site, the UWB signal is very easy to be interfered to cause the positioning accuracy to be reduced or even the locking to be lost due to the complex and changeable environment of the construction site; meanwhile, because the communication technology adopted by UWB is usually a short-distance wireless communication technology, if the positioning of a large-scale construction site control point is required to be completed, a large number of network nodes are required to be completed together, and a series of problems such as network organization structure optimization design, multi-node multi-cluster network cooperative communication and the like are necessarily introduced. The positioning of the existing UWB-based job site control points still presents a number of challenges.
With the development of technology, a Kalman Filter (KF) has been widely used in the field of data fusion. It is noted that the kalman filter is only applicable to linear models. To ensure that the system still has good performance when dealing with non-linearity problems, extended Kalman Filters (EKFs) have been developed. However, although the extended kalman filter can be applied to a nonlinear system, the truncation error caused by the first-order taylor expansion still affects the final filtering accuracy.
In the aspect of navigation models, a plurality of loose combination navigation models are applied in the field of building engineering at present, so that the models have the advantage of easy realization. However, the implementation of the model requires multiple technologies participating in combined navigation to independently complete navigation positioning, and the navigation information of each technology can improve the positioning precision of the navigation positioning system. The prior art has great progress in positioning accuracy, and the situation is particularly obvious when GNSS signals are poor.
Disclosure of Invention
In order to solve the problems, the invention provides a construction site control point positioning method and a construction site control point positioning system based on self-adaptive iteration EKF, which can realize the positioning of control points by tightly fusing distance parameters of INS, GNSS and UWB based on an AIEKF algorithm of an expected maximized IEKF forward filter and an R-T-S (Rauch-tune-Striebel) backward smoother.
In some embodiments, the following technical scheme is adopted:
a construction site control point positioning method based on adaptive iteration EKF comprises the following steps:
acquiring the distance from a control point of a construction site to a satellite through a GNSS; acquiring the distance between a control point of a construction site and a UWB reference node through UWB, and acquiring the position information of the control point through INS;
taking the position error and the speed error of an inertial navigation system INS in the east direction and the north direction acquired at the moment t as the state vector of the adaptive iterative extended Kalman filter;
when GNSS data is available, taking the square difference between the estimated value of the distance between the control point and the GNSS and the estimated value of the distance between the control point and the INS reference node as an observation vector of the adaptive iterative extended Kalman filter;
when GNSS data is not available, taking the square difference of the estimated value of the distance between the control point and the UWB reference node and the estimated value of the distance between the control point and the INS reference node as an observation vector of the adaptive iterative extended Kalman filter;
and filtering by utilizing a self-adaptive iterative extended Kalman filter based on the state vector and the observation vector to obtain the distance between the control point and the navigation satellite or between the control point and the UWB positioning base station, thereby realizing the positioning of the control point.
In other embodiments, the following technical solutions are adopted:
a construction site control point positioning system based on an adaptive iterative EKF, comprising:
a GNSS for acquiring a distance from a control point of a construction site to a satellite; the INS is used for acquiring the distance between the control point of the construction site and the UWB reference node and acquiring the position information of the control point;
the device is used for taking the position error and the speed error of the inertial navigation system INS in the east direction and the north direction acquired at the moment t as the state vector of the adaptive iterative extended Kalman filter;
the method is used for taking square difference between an estimated value of the distance between the control point and the GNSS and an estimated value of the distance between the control point and an INS reference node as an adaptive iterative extended Kalman filter observation vector when GNSS data is available; when GNSS data is not available, taking the square difference between the estimated value of the distance between the control point and the UWB reference node and the estimated value of the distance between the control point and the INS reference node as a device for self-adaptive iterative extended Kalman filter observation vector;
and the device is used for filtering by utilizing the adaptive iteration extended Kalman filter based on the state vector and the observation vector to obtain the distance between the control point and the navigation satellite or between the control point and the UWB positioning base station, thereby realizing the positioning of the control point.
In other embodiments, the following technical solutions are adopted:
a terminal device comprising a processor and a memory, the processor being configured to implement instructions; the memory is used for storing a plurality of instructions adapted to be loaded by the processor and to perform the above-described construction site control point positioning method based on an adaptive iterative EKF.
In other embodiments, the following technical solutions are adopted:
a computer readable storage medium having stored therein a plurality of instructions adapted to be loaded by a processor of a terminal device and to perform the above-described adaptive iterative EKF-based job site control point positioning method.
Compared with the prior art, the invention has the beneficial effects that:
(1) The method constructs a corresponding state equation based on the position error and the speed error; constructing a corresponding observation equation according to whether the GNSS data is available or not, namely using the GNSS data as a measured value when the GNSS data is available; when GNSS data is not available, UWB data is used as a measurement value; through the fusion of the GNSS/INS/UWB, the problem of inaccurate GNSS data acquisition caused by shielding in a complex environment scene can be effectively solved, and the positioning accuracy of the system is effectively improved.
(2) The improved self-adaptive iterative extended Kalman filter algorithm provides accurate noise statistics data through self-adaptive updating of noise statistics, and can improve the accuracy of noise statistics, thereby improving the system performance.
(3) The invention can realize the self-adaptive iteration of the system by calculating the Mahalanobis distance to determine whether the iteration is terminated.
(4) According to the invention, the optimal estimation in the smooth interval is obtained by adopting the R-T-S smoothing method for parameters such as Kalman gain, system state vector and the like, so that the positioning accuracy of the system can be improved.
Additional features and advantages of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention.
Drawings
FIG. 1 is a schematic diagram of a construction site control point positioning method based on an adaptive iterative EKF in an embodiment of the present invention;
FIG. 2 is a schematic diagram of the eastern position given by GNSS+UWB, EKF, traditional IEKF, and the modified AIEKF proposed by the present embodiment;
FIG. 3 is a schematic representation of the north position of an embodiment of the present invention using GNSS+UWB, EKF, traditional IEKF, and the modified AIEKF of the present embodiment;
fig. 4 is a graph showing Cumulative Distribution Function (CDF) of EKF and conventional IEKF and the position error of the modified AIEKF proposed in the present embodiment.
Detailed Description
It should be noted that the following detailed description is illustrative and is intended to provide further explanation of the present application. Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs.
It is noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of example embodiments in accordance with the present application. As used herein, the singular is also intended to include the plural unless the context clearly indicates otherwise, and furthermore, it is to be understood that the terms "comprises" and/or "comprising" when used in this specification are taken to specify the presence of stated features, steps, operations, devices, components, and/or combinations thereof.
Example 1
In one or more embodiments, a construction site control point positioning method based on adaptive iterative EKF is disclosed, and in combination with fig. 1, the method specifically includes the following procedures:
(1) Acquiring the distance from a control point of a construction site to a satellite through a GNSS; the UWB obtains the distance between the control point of the construction site and the UWB reference node, and the INS obtains the position information of the control point; all three are fixed at the control point.
In the embodiment, the positioning of the control point is realized through the fusion of the GNSS, the INS and the UWB; the inertial navigation system INS can continuously provide the position, speed and attitude information of the carrier, and is beneficial to improving the navigation positioning performance of the global satellite navigation system GNSS; the ultra-wideband technology UWB is insensitive to the weakness of the channel, the positioning precision is high, the high-precision positioning result can also provide high-quality updated information for the INS, the drift of INS navigation parameter errors along with time is restrained, and the correction of the INS system is facilitated.
(2) Taking the position error and the speed error of an Inertial Navigation System (INS) in the east direction and the north direction acquired at the moment t as the state vector of the adaptive iterative extended Kalman filter;
specifically, the state vector of the adaptive iterative extended kalman filter is specifically:
wherein (δPx) t-1 ,δPy t-1 ) Position error of inertial navigation system for east and north direction at time t-1, (δvx) t-1 ,δVy t-1 ) East at time t-1Speed error of inertial navigation system in direction and north direction;
for the predicted value of the position error of the inertial navigation system at time t in the east and north directions,
a predicted value of a speed error of the inertial navigation system at the moment t in the east direction and the north direction; deltaT is the sampling period of the system, w t Is system noise, its covariance matrix is Q, X t-1 For the state vector at time t-1, A is the system matrix.
(3) When GNSS data is available, taking the square difference between the estimated value of the distance between the control point and the GNSS and the estimated value of the distance between the control point and the INS reference node as an observation vector of the adaptive iterative extended Kalman filter; the method comprises the following steps:
wherein ,
representing measurement noise, wherein a covariance matrix of the measurement noise is R; />
Respectively representing the estimated value of the distance from the control point to the satellite,/->
Representing an estimate of the distance between the control point and the INS reference node, g representing the number of satellites;
wherein ,
is the location information of the INS in the eastern direction, +.>
Is the position information in the north direction,/->
Representing the position of the ith satellite in the eastern direction; />
Indicating the position of the ith satellite in the north direction.
When GNSS data is not available, taking the square difference of the estimated value of the distance between the control point and the UWB reference node and the estimated value of the distance between the control point and the INS reference node as an observation vector of the adaptive iterative extended Kalman filter; the method comprises the following steps:
wherein ,
representing measurement noise, wherein a covariance matrix of the measurement noise is R; />
Representing an estimate of the distance between the control point and the UWB reference node, +.>
Representing the control point and INSAn estimated value of the distance between the reference nodes, g representing the number of satellites;
wherein ,
is the location information of the INS in the eastern direction, +.>
Is the position information in the north direction,/->
Indicating the position of the ith UWB in the eastern direction; />
Indicating the position of the ith UWB in the north direction.
(4) And filtering by utilizing a self-adaptive iterative extended Kalman filter based on the state vector and the observation vector to obtain the distance between the control point and the navigation satellite or between the control point and the UWB positioning base station, thereby realizing the positioning of the control point.
The adaptive iterative extended kalman filter algorithm in this embodiment is simply referred to as an improved modified IEKF algorithm based on an IEKF forward filter and an R-T-S (Rauch-tune-Striebel) backward smoother, which are expected to be maximized.
The self-adaptive updating of noise statistics is adopted, and specifically comprises the following steps:
wherein ,
is the predicted value of the covariance matrix of the state vector after the j+1th iteration,/->
Iterating covariance matrix of measurement noise of j+1 times for t time; p (P) t (j+1) Is the covariance matrix of the state vector after the j+1th iteration,
is a system state vector for iteration j+1 times at time t,/>
Is the predicted value of the system state vector at the moment t, y t Is the observation value adopted by the system according to the GNSS signal condition, < ->
Is a jacobian matrix iterating j+1 times at the moment t;
after the last iteration at time t is completed, time t
and />
Will be the initial value at time t +1 in order to iterate at time t + 1.
According to the Kalman gain calculated in the previous iteration, the input parameters are fused to obtain the output
A predicted value of a state vector at the time t; and calculates its covariance matrix for the next iteration; and calculating the mahalanobis distance between the output and the predicted value, and stopping iteration if the mahalanobis distance D (t) of the output and the predicted value is smaller than a preset value threshold.If the iteration number of the system reaches the set maximum iteration number, D (t) is still larger than the preset value, the system stops iteration and takes the last iteration result as output.
Carrying out smoothing operation on the Kalman gain and the system state vector obtained by each iteration by adopting an R-T-S smoothing algorithm, and obtaining the optimal estimated values at all moments in a smoothing interval by the smoothing algorithm
The specific algorithmic process is shown in table 1 below.
TABLE 1
wherein ,
is GNSS output data, ++>
Data output by UWB, +.>
System state vector->
Is->
Is the covariance matrix of (1), Q is the systemCovariance matrix of system process noise, and covariance matrix of R measurement noise.
In the algorithm from the 7 th row to the 11 th row, the system is realized to automatically switch y under the condition that GNSS data are available and unavailable t The variables employed. I.e. using the GNSS data when available
As a measurement value; when GNSS data is not available, UWB data is used +.>
As a measurement.
In the algorithm, the system can calculate the Kalman gain according to the last iteration
Fusion processing is carried out on the input parameters, and +.>
And calculates the covariance matrix P thereof t To allow for the next iteration.
Adaptive updating of noise statistics is employed on lines 18 to 19 of the algorithm, which is used to adaptively provide noise statistics, i.e. using P t (j+1) 、
y t 、/>
Equal parameters to calculate +.>
and />
Mahalanobis distance is called Mahalanobis distance for realizing system self-adaptionIterative, the invention uses the mahalanobis distance at line 17 of the algorithm. In addition, the positioning accuracy is improved by adopting an R-T-S method on parameters such as Kalman gain, system state vector and the like in the 27 th to 32 th rows of the algorithm.
In order to verify the effectiveness of the present embodiment method, GPS is employed as a solution to GNSS for the present embodiment method, while UWB is used to provide a solution to UWB. And fix the GPS receiver and UWB Blind Node (BN) on known coordinates.
Filter threshold=0.005 and n=5 are set, and then the parameters of the improved adaptive iterative extended kalman filter are set as follows:
X 0 ＝[0 0 0 0] T
the state vector of the adaptive iterative extended kalman filter is specifically:
the observation equation of the adaptive iterative extended kalman filter has the following two cases:
when GNSS data is available, the measurement equation of the system can be expressed by the following equation.
wherein ,
the measurement noise is represented, and the covariance matrix thereof is R. />
Representing the distance GNSS estimate of the control point to the satellite,>
an estimate of the distance between the control point and the INS reference node is shown, and g represents the number of satellites.
When UWB data is available, the measurement equation of the system can be expressed by the following equation.
wherein ,
the measurement noise is represented, and the covariance matrix thereof is R. />
An estimate of the distance between the control point and the UWB reference node is shown, and g represents the number of satellites.
Fig. 2 and 3 show an east-direction position diagram and a north-direction position diagram, respectively, using conventional gnss+uwb, EKF, and IEKF, and the modified AIEKF method of the present embodiment.
The results show that the improved AIEKF method of this embodiment has lower errors in the east and north directions than conventional gnss+uwb, EKF and IEKF.
FIG. 4 is a graph showing Cumulative Distribution Function (CDF) curves of EKF and conventional IEKF and the position error of the modified AIEKF according to the present embodiment; the results show that the position error of the modified AIEKF proposed by this example is about 0.05m, lower than that of the conventional gnss+uwb, EKF and IEKF.
In addition, root Mean Square Error (RMSE) generated by the conventional EKF, IEKF and the modified AIEKF proposed in this example is shown in table 2 below.
TABLE 2
Therefore, compared with the prior art, the error of the positioning accuracy of the GNSS/INS/UWB fusion positioning method based on the improved adaptive iteration extended Kalman filter is lower than that of the traditional Kalman filter and IEKF.
Example two
In one or more embodiments, a job site control point positioning system based on adaptive iterative EKF is disclosed, comprising:
a GNSS for acquiring a distance from a control point of a construction site to a satellite; the INS is used for acquiring the distance between the control point of the construction site and the UWB reference node and acquiring the position information of the control point;
the device is used for taking the position error and the speed error of the inertial navigation system INS in the east direction and the north direction acquired at the moment t as the state vector of the adaptive iterative extended Kalman filter;
the method is used for taking square difference between an estimated value of the distance between the control point and the GNSS and an estimated value of the distance between the control point and an INS reference node as an adaptive iterative extended Kalman filter observation vector when GNSS data is available; when GNSS data is not available, taking the square difference between the estimated value of the distance between the control point and the UWB reference node and the estimated value of the distance between the control point and the INS reference node as a device for self-adaptive iterative extended Kalman filter observation vector;
and the device is used for filtering by utilizing the adaptive iteration extended Kalman filter based on the state vector and the observation vector to obtain the distance between the control point and the navigation satellite or between the control point and the UWB positioning base station, thereby realizing the positioning of the control point.
Example III
In one or more embodiments, a terminal device is disclosed that includes a server including a memory, a processor, and a computer program stored on the memory and executable on the processor, the processor implementing the adaptive iterative EKF-based job site control point positioning method of embodiment one when the program is executed. For brevity, the description is omitted here.
It should be understood that in this embodiment, the processor may be a central processing unit CPU, and the processor may also be other general purpose processors, digital signal processors DSP, application specific integrated circuits ASIC, off-the-shelf programmable gate array FPGA or other programmable logic device, discrete gate or transistor logic devices, discrete hardware components, or the like. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.
The memory may include read only memory and random access memory and provide instructions and data to the processor, and a portion of the memory may also include non-volatile random access memory. For example, the memory may also store information of the device type.
In implementation, the steps of the above method may be performed by integrated logic circuits of hardware in a processor or by instructions in the form of software.
Example IV
In one or more embodiments, a computer-readable storage medium is disclosed having stored therein a plurality of instructions adapted to be loaded by a processor of a terminal device and to perform the adaptive iterative EKF-based job site control point positioning method described in example one.
While the foregoing description of the embodiments of the present invention has been presented in conjunction with the drawings, it should be understood that it is not intended to limit the scope of the invention, but rather, it is intended to cover all modifications or variations within the scope of the invention as defined by the claims of the present invention.
Claims (9)
1. The construction site control point positioning method based on the self-adaptive iterative EKF is characterized by comprising the following steps of:
acquiring the distance from a control point of a construction site to a satellite through a GNSS; acquiring the distance between a control point of a construction site and a UWB reference node through UWB, and acquiring the position information of the control point through INS;
taking the position error and the speed error of an inertial navigation system INS in the east direction and the north direction acquired at the moment t as the state vector of the adaptive iterative extended Kalman filter;
when GNSS data is available, taking the square difference between the estimated value of the distance between the control point and the GNSS and the estimated value of the distance between the control point and the INS reference node as an observation vector of the adaptive iterative extended Kalman filter;
when GNSS data is not available, taking the square difference of the estimated value of the distance between the control point and the UWB reference node and the estimated value of the distance between the control point and the INS reference node as an observation vector of the adaptive iterative extended Kalman filter;
filtering by utilizing a self-adaptive iterative extended Kalman filter based on the state vector and the observation vector to obtain the distance between a control point and a navigation satellite or between the control point and a UWB positioning base station, thereby realizing the positioning of the control point;
the self-adaptive updating of noise statistics is adopted, and specifically comprises the following steps:
wherein ,Pt (j+1)- Is the predicted value of the covariance matrix of the state vector after the j+1th iteration,
iterating covariance matrix of measurement noise of j+1 times for t time; p (P) t (j+1) Is the covariance matrix of the state vector after the j+1th iteration,/th iteration>
Is a system-like iterative j+1 times at t timeState vector (s)/(s)>
Is the predicted value of the system state vector at the moment t, y t Is the observation value adopted by the system according to the GNSS signal condition, < ->
Is a jacobian matrix iterating j+1 times at the moment t;
after the last iteration at time t is completed, P at time t t (j+1)- And
will be the initial value at time t +1 in order to iterate at time t + 1.
2. The construction site control point positioning method based on the adaptive iterative EKF according to claim 1, wherein the state vector of the adaptive iterative extended kalman filter is specifically:
wherein (δPx) t-1 ,δPy t-1 ) Position error of inertial navigation system for east and north direction at time t-1, (δvx) t-1 ,δVy t-1 ) The speed error of the inertial navigation system in the east direction and the north direction at the moment t-1;
for the prediction value of the position error of the inertial navigation system at time t in the east and north direction, +.>
A predicted value of a speed error of the inertial navigation system at the moment t in the east direction and the north direction; deltaT is the sampling period of the system, w t Is system noise, its covariance matrix is Q, X t-1 For the state vector at time t-1, A is the system matrix.
3. The method for positioning a control point on a construction site based on an adaptive iterative EKF according to claim 1, wherein when GNSS data is available, the adaptive iterative extended kalman filter observation vector is specifically:
wherein ,
representing measurement noise, wherein a covariance matrix of the measurement noise is R; />
Respectively representing the estimated value of the distance from the control point to the satellite,/->
Representing an estimate of the distance between the control point and the INS reference node, g representing the number of satellites; />
Is the square difference between the estimated value of the distance from the control point at time t to the INS reference node and the estimated value of the distance from the control point at time t to the satellite.
4. The method for positioning a control point on a construction site based on an adaptive iterative EKF according to claim 1, wherein when GNSS data is not available, the adaptive iterative extended kalman filter observation vector is specifically:
wherein ,
representing measurement noise, wherein a covariance matrix of the measurement noise is R; />
Representing an estimate of the distance between the control point and the UWB reference node, +.>
Representing an estimate of the distance between the control point and the INS reference node, g representing the number of satellites; />
Is the square difference of the estimated value of the distance between the t moment control point and the INS reference node and the estimated value of the distance between the t moment control point and the UWB reference node.
5. The construction site control point positioning method based on the self-adaptive iteration EKF of claim 1, wherein input parameters are fused according to Kalman gain calculated in the last iteration to obtain a predicted value of a state vector at the moment of output t, and a covariance matrix of the predicted value is calculated so as to carry out the next iteration;
calculating the mahalanobis distance between the output and the predicted value, and judging whether iteration is ended or not if the mahalanobis distance is smaller than a set threshold value; if the iteration number reaches the set maximum iteration number and the mahalanobis distance is larger than the set threshold, stopping iteration and taking the last iteration result as output.
6. The construction site control point positioning method based on the self-adaptive iteration EKF of claim 1, wherein the Kalman gain and the system state vector obtained by each iteration are subjected to smoothing operation by adopting an R-T-S smoothing algorithm; and obtaining the optimal state vector predicted value of all the moments in the smooth interval.
7. A construction site control point positioning system based on an adaptive iterative EKF, comprising:
a GNSS for acquiring a distance from a control point of a construction site to a satellite; the INS is used for acquiring the distance between the control point of the construction site and the UWB reference node and acquiring the position information of the control point;
the device is used for taking the position error and the speed error of the inertial navigation system INS in the east direction and the north direction acquired at the moment t as the state vector of the adaptive iterative extended Kalman filter;
the method is used for taking square difference between an estimated value of the distance between the control point and the GNSS and an estimated value of the distance between the control point and an INS reference node as an adaptive iterative extended Kalman filter observation vector when GNSS data is available; when GNSS data is not available, taking the square difference between the estimated value of the distance between the control point and the UWB reference node and the estimated value of the distance between the control point and the INS reference node as a device for self-adaptive iterative extended Kalman filter observation vector;
means for filtering with an adaptive iterative extended kalman filter based on the state vector and the observation vector to obtain a distance between the control point and the navigation satellite or between the control point and the UWB positioning base station, thereby achieving positioning of the control point;
the self-adaptive updating of noise statistics is adopted, and specifically comprises the following steps:
wherein ,Pt (j+1)- Is the predicted value of the covariance matrix of the state vector after the j+1th iteration,
iterating covariance matrix of measurement noise of j+1 times for t time; p (P) t (j+1) Is the covariance matrix of the state vector after the j+1th iteration,/th iteration>
Is a system state vector for iteration j+1 times at time t,/>
Is the predicted value of the system state vector at the moment t, y t Is the observation value adopted by the system according to the GNSS signal condition, < ->
Is a jacobian matrix iterating j+1 times at the moment t;
after the last iteration at time t is completed, P at time t t (j+1)- And
will be the initial value at time t +1 in order to iterate at time t + 1.
8. A terminal device comprising a processor and a memory, the processor being configured to implement instructions; the memory for storing a plurality of instructions, wherein the instructions are adapted to be loaded by a processor and to perform the adaptive iterative EKF-based job site control point positioning method according to any of claims 1-6.
9. A computer readable storage medium having stored therein a plurality of instructions adapted to be loaded by a processor of a terminal device and to perform the adaptive iterative EKF-based job site control point positioning method according to any of claims 1-6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210108939.4A CN114440880B (en) | 2022-01-28 | 2022-01-28 | Construction site control point positioning method and system based on self-adaptive iterative EKF |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210108939.4A CN114440880B (en) | 2022-01-28 | 2022-01-28 | Construction site control point positioning method and system based on self-adaptive iterative EKF |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114440880A CN114440880A (en) | 2022-05-06 |
| CN114440880B true CN114440880B (en) | 2023-06-13 |
Family
ID=81372537
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210108939.4A Active CN114440880B (en) | 2022-01-28 | 2022-01-28 | Construction site control point positioning method and system based on self-adaptive iterative EKF |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114440880B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114779307B (en) * | 2022-06-17 | 2022-09-13 | 武汉理工大学 | Port area-oriented UWB/INS/GNSS seamless positioning method |
| CN115856974B (en) * | 2022-11-18 | 2024-04-05 | 苏州华米导航科技有限公司 | GNSS, INS and visual tight combination navigation positioning method based on invariant filtering |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109633590A (en) * | 2019-01-08 | 2019-04-16 | 杭州电子科技大学 | Extension method for tracking target based on GP-VSMM-JPDA |
| WO2020221050A1 (en) * | 2019-04-28 | 2020-11-05 | 浙江大学 | Centralized cooperative localization system and method based on time-space domain joint processing |
| CN113566828A (en) * | 2021-07-09 | 2021-10-29 | 哈尔滨工业大学 | Impact-resistant scanning matching method and system based on multi-sensor decision fusion |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102278974B (en) * | 2010-06-09 | 2013-04-17 | 南京德朔实业有限公司 | Laser ranging apparatus |
| US10254393B2 (en) * | 2016-03-28 | 2019-04-09 | The United States Of America As Represented By The Secretary Of The Navy | Covariance matrix technique for error reduction |
| CN107966143A (en) * | 2017-11-14 | 2018-04-27 | 济南大学 | A kind of adaptive EFIR data fusion methods based on multiwindow |
| CN109269498B (en) * | 2018-08-06 | 2020-09-22 | 济南大学 | Adaptive pre-estimation EKF filtering algorithm and system for UWB pedestrian navigation with data missing |
| CN110596694B (en) * | 2019-09-20 | 2023-01-10 | 中汽研软件测评(天津)有限公司 | Complex environment radar multi-target tracking and road driving environment prediction method |
-
2022
- 2022-01-28 CN CN202210108939.4A patent/CN114440880B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109633590A (en) * | 2019-01-08 | 2019-04-16 | 杭州电子科技大学 | Extension method for tracking target based on GP-VSMM-JPDA |
| WO2020221050A1 (en) * | 2019-04-28 | 2020-11-05 | 浙江大学 | Centralized cooperative localization system and method based on time-space domain joint processing |
| CN113566828A (en) * | 2021-07-09 | 2021-10-29 | 哈尔滨工业大学 | Impact-resistant scanning matching method and system based on multi-sensor decision fusion |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114440880A (en) | 2022-05-06 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114440880B (en) | Construction site control point positioning method and system based on self-adaptive iterative EKF | |
| CN109141413B (en) | EFIR filtering algorithm and system with data missing UWB pedestrian positioning | |
| Liu et al. | Mercury: An infrastructure-free system for network localization and navigation | |
| CN106680765B (en) | Pedestrian navigation system and method based on distributed combined filtering INS/UWB | |
| CN111780755A (en) | Multisource fusion navigation method based on factor graph and observability degree analysis | |
| CN107255795A (en) | Localization Approach for Indoor Mobile and device based on EKF/EFIR mixed filterings | |
| CN109141412B (en) | UFIR filtering algorithm and system for data-missing INS/UWB combined pedestrian navigation | |
| CN105509739A (en) | Tightly coupled INS/UWB integrated navigation system and method adopting fixed-interval CRTS smoothing | |
| CN108759825B (en) | Self-adaptive pre-estimation Kalman filtering algorithm and system for INS/UWB pedestrian navigation with data missing | |
| CN111044050B (en) | Bluetooth positioning method based on particle filtering and Kalman filtering | |
| CN105792115A (en) | Multi-network positioning data fusion method and system | |
| CN115066012B (en) | Multi-user anchor-free positioning method based on UWB | |
| CN109655060B (en) | INS/UWB integrated navigation algorithm and system based on KF/FIR and LS-SVM fusion | |
| Luo et al. | A fast algorithm of slam based on combinatorial interval filters | |
| CN109269498B (en) | Adaptive pre-estimation EKF filtering algorithm and system for UWB pedestrian navigation with data missing | |
| Nagui et al. | Improved GPS/IMU loosely coupled integration scheme using two kalman filter-based cascaded stages | |
| CN105527639A (en) | Satellite positioning method based on smoothness and extrapolation | |
| CN113218388B (en) | Mobile robot positioning method and system considering variable colored measurement noise | |
| CN109916401B (en) | Distributed seamless tight combination navigation method and system adopting LS-SVM assisted EKF filtering method | |
| CN103727947B (en) | Based on the UKF deep coupling positioning methods of BDS and GIS filtered and system | |
| CN114222365B (en) | Ultra-wideband positioning method, device, computing unit and system based on single base station | |
| Wu et al. | Improved adaptive iterated extended Kalman filter for GNSS/INS/UWB-integrated fixed-point positioning | |
| CN110879069A (en) | UWB SLAM-oriented Kalman/R-T-S hybrid positioning method and system | |
| Chi et al. | Robust UWB Localization for Indoor Pedestrian Tracking Using EKF and Adaptive Power-Driven Parallel IMM | |
| Wang et al. | A Multi-Vehicle Cooperative Localization Method Based on Belief Propagation in Satellite Denied Environment |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |