CN114440880A - Construction site control point positioning method and system based on adaptive iteration EKF - Google Patents

Construction site control point positioning method and system based on adaptive iteration EKF Download PDF

Info

Publication number
CN114440880A
CN114440880A CN202210108939.4A CN202210108939A CN114440880A CN 114440880 A CN114440880 A CN 114440880A CN 202210108939 A CN202210108939 A CN 202210108939A CN 114440880 A CN114440880 A CN 114440880A
Authority
CN
China
Prior art keywords
control point
distance
estimated value
reference node
kalman filter
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202210108939.4A
Other languages
Chinese (zh)
Other versions
CN114440880B (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.)
University of Jinan
Shandong Luqiao Group Co Ltd
Original Assignee
University of Jinan
Shandong Luqiao Group 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 University of Jinan, Shandong Luqiao Group Co Ltd filed Critical University of Jinan
Priority to CN202210108939.4A priority Critical patent/CN114440880B/en
Publication of CN114440880A publication Critical patent/CN114440880A/en
Application granted granted Critical
Publication of CN114440880B publication Critical patent/CN114440880B/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/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)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

The invention discloses a construction site control point positioning method and a system based on adaptive iteration EKF, comprising the following steps: acquiring position information of a control point; using the position error and the speed error of the inertial navigation system INS in the east direction and the north direction collected at the time t as the state vector of the adaptive iterative extended Kalman filter; when GNSS data are available, taking the square difference of 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 expansion Kalman filter; when the GNSS data is unavailable, 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 expansion Kalman filter; and based on the state vector and the observation vector, filtering by using a self-adaptive iterative extended Kalman filter to obtain the distance between the control point and the navigation satellite or between the control point and the UWB positioning base station.

Description

Construction site control point positioning method and system based on adaptive iteration EKF
Technical Field
The invention relates to the technical field of GNSS/INS/UWB fusion positioning in a complex environment, in particular to a construction site control point positioning method and system based on 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 construction engineering technology have been advanced greatly, which includes accurate positioning of control points on construction sites. The control points are used as measurement coordinate points for construction control, and various measurements must be carried out in the construction stage of the building engineering to ensure high-quality implementation of the building engineering project. Such as measurements of known lengths, measurements of known angles, planar measurements of building minutiae, and position measurements of building minutiae, etc. Accurate positioning and measurement of control points on a construction site are important prerequisites for completing the measurement work with high quality.
Among the existing positioning methods, Global Navigation Satellite System (GNSS) is the most commonly used method. Although the GNSS can continuously and stably obtain the position information with high precision, the application range of the GNSS is limited by the defect that the GNSS is easily influenced by external environments such as electromagnetic interference and shielding, and particularly in some closed spaces such as indoor and underground roadways or scenes with complex environments such as heavy fog and the like, 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-distance local positioning. The UWB technology is a wireless carrier communication technology, which does not use a sinusoidal carrier, but uses nanosecond-level non-sinusoidal narrow pulses to transmit data, and thus occupies a wide frequency spectrum range, and is called an ultra wideband technology. The UWB technology has the advantages of low system complexity, low power spectrum density of a transmitting signal, insensitivity to attenuation of a channel, high positioning accuracy and the like.
The prior art proposes to apply UWB-based target tracking to the positioning of control points on a construction site in GNSS failure environments. Although the method can realize the positioning of the control point of the construction site, the UWB signal is very easy to be interfered to cause the reduction of the positioning precision and even the lock losing due to the complicated and changeable construction site environment; meanwhile, because the communication technology adopted by the UWB is generally a short-distance wireless communication technology, if positioning of control points on a large-scale construction site is to be completed, a large number of network nodes are required to complete together, which inevitably introduces a series of problems such as network organization structure optimization design, multi-node multi-cluster network cooperative communication, and the like. Therefore, UWB-based job site control point location still faces many challenges.
With the development of technology, Kalman Filters (KFs) have been widely used in the field of data fusion. It is noted that the kalman filter is only applicable to linear models. In order to ensure that the system still has good performance when dealing with the non-linearity problem, an Extended Kalman Filter (EKF) has come into force. 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, the loose combination navigation model is mostly applied in the field of building engineering at present, so that the model has the advantage of easy implementation. However, the implementation of the model requires that multiple technologies participating in the combined navigation can independently complete navigation positioning, and the navigation information of each technology can improve the positioning accuracy of the navigation positioning system. The prior art has great space for improvement in positioning accuracy, which is particularly obvious when the GNSS signals are not good.
Disclosure of Invention
In order to solve the problems, the invention provides a construction site control point positioning method and system based on adaptive iteration EKF, based on an AIEKF algorithm of an IEKF forward filter and an R-T-S (Rauch-Tung-Streebel) backward smoother which are expected to be maximized, distance parameters of INS, GNSS and UWB can be closely fused to realize the positioning of the control point.
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 on a construction site and a UWB reference node through UWB, and acquiring the position information of the control point through INS;
using the position error and the speed error of the inertial navigation system INS in the east direction and the north direction collected at the time t as the state vector of the adaptive iterative extended Kalman filter;
when GNSS data are available, taking the square difference of 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 expansion Kalman filter;
when the GNSS data is unavailable, 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 expansion Kalman filter;
and based on the state vector and the observation vector, filtering by using a self-adaptive iterative extended Kalman filter 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 adaptive iterative EKF comprises:
the GNSS is used for acquiring the distance from a control point of a construction site to a satellite; the system comprises a UWB used for acquiring the distance between a control point on a construction site and a UWB reference node and an INS used for 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 collected at the moment t as the state vector of the adaptive iterative extended Kalman filter;
the adaptive iterative extended Kalman filter observation vector generation method comprises the steps that when GNSS data are available, the square difference of an estimated value of the distance between a control point and a GNSS and an estimated value of the distance between the control point and an INS reference node is used as an adaptive iterative extended Kalman filter observation vector; when the GNSS data is unavailable, 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 is used as a device for observing a vector by using the adaptive iterative extended Kalman filter;
and the device is used for filtering by using 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 terminal device comprising a processor and a memory, the processor being arranged to implement instructions; the memory is configured to store a plurality of instructions adapted to be loaded by the processor and to perform the adaptive iterative EKF-based job site control point positioning method described above.
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 execute the adaptive iterative EKF-based job site control point positioning method described above.
Compared with the prior art, the invention has the beneficial effects that:
(1) the method constructs a corresponding state equation on the basis of the position error and the speed error; according to whether the GNSS data are available or not, constructing a corresponding observation equation, namely when the GNSS data are available, using the GNSS data as a measured value; when GNSS data is not available, using UWB data as a measurement; through the integration of the GNSS, the INS and the 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 adaptive iterative extended Kalman filter algorithm provides accurate noise statistical data through adaptive updating of noise statistics, and can improve the accuracy of the noise statistics, thereby improving the system performance.
(3) The invention determines whether the iteration is terminated by calculating the Mahalanobis distance (Mahalanobis distance), and can realize the self-adaptive iteration of the system.
(4) The invention adopts the R-T-S smoothing method to obtain the optimal estimation in the smoothing interval for parameters such as Kalman gain, system state vector and the like, and can improve the positioning accuracy of the system.
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 method for positioning control points of a construction site based on adaptive iterative EKF according to an embodiment of the present invention;
FIG. 2 is a schematic representation of a given east position using GNSS + UWB, EKF, conventional IEKF, and the improved AIEKF proposed in this embodiment;
FIG. 3 is a schematic representation of the north orientation given by the present embodiment of the invention using GNSS + UWB, EKF, conventional IEKF and the improved AIEKF proposed by the present embodiment;
FIG. 4 is a graph of the Cumulative Distribution Function (CDF) of the position error of EKF, conventional IEKF and the improved AIEKF proposed in this embodiment.
Detailed Description
It should be noted that the following detailed description is exemplary and is intended to provide further explanation of the disclosure. 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 according to the present application. As used herein, the singular forms "a", "an" and "the" are intended to include the plural forms as well, and it should be understood that when the terms "comprises" and/or "comprising" are used in this specification, they specify the presence of stated features, steps, operations, devices, components, and/or combinations thereof, unless the context clearly indicates otherwise.
Example one
In one or more embodiments, a method for positioning a control point of a construction site based on adaptive iterative EKF is disclosed, which, with reference to fig. 1, specifically includes the following steps:
(1) acquiring the distance from a control point of a construction site to a satellite through a GNSS; UWB obtains the distance between a control point of a construction site and a UWB reference node, and 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 position, speed and attitude information of the carrier, and is beneficial to improving the navigation and positioning performance of a Global Navigation Satellite System (GNSS); the UWB is insensitive to the attenuation of the channel, the positioning precision is high, the high-precision positioning result can also provide high-quality updating information for the INS, the drift of the INS navigation parameter error along with the time is inhibited, and the correction of the INS system is facilitated.
(2) Using the position error and the speed error of an Inertial Navigation System (INS) in the east direction and the north direction collected at the time 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:
Figure RE-GDA0003518482500000071
wherein (δ Px)t-1,δPyt-1) Position error of inertial navigation system for east and north directions at time t-1, (δ Vxt-1,δVyt-1) Velocity errors of the inertial navigation system for the east and north directions at time t-1;
Figure RE-GDA0003518482500000072
the predicted value of the position error of the inertial navigation system at the time t in the east direction and the north direction,
Figure RE-GDA0003518482500000073
the predicted values of the speed errors of the inertial navigation system at the time t in the east direction and the north direction are obtained; Δ T is the sampling period of the system, wtFor system noise, its covariance matrix is Q, Xt-1A is the system matrix for the state vector at time t-1.
(3) When GNSS data are available, taking the square difference of 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 expansion Kalman filter; the method specifically comprises the following steps:
Figure RE-GDA0003518482500000081
wherein ,
Figure RE-GDA0003518482500000082
representing measurement noiseThe covariance matrix is R;
Figure RE-GDA0003518482500000083
Figure RE-GDA0003518482500000084
respectively representing the control point to satellite distance estimates,
Figure RE-GDA0003518482500000085
representing an estimated value of the distance between the control point and the INS reference node, and g representing the number of satellites;
Figure RE-GDA0003518482500000086
the mean square error of the estimated value of the distance between the control point at the time t and the INS reference node and the estimated value of the distance between the control point at the time t and the satellite is obtained; the specific calculation process is as follows:
Figure RE-GDA0003518482500000087
wherein ,
Figure RE-GDA0003518482500000088
is the position information of the INS in the east direction,
Figure RE-GDA0003518482500000089
is the position information in the north direction,
Figure RE-GDA00035184825000000810
represents the position of the ith satellite in the east direction;
Figure RE-GDA00035184825000000811
indicating the position of the ith satellite in the north direction.
When the GNSS data are unavailable, 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 expansion Kalman filter; the method specifically comprises the following steps:
Figure RE-GDA00035184825000000812
wherein ,
Figure RE-GDA0003518482500000091
representing the measurement noise with a covariance matrix R;
Figure RE-GDA0003518482500000092
Figure RE-GDA0003518482500000093
representing an estimate of the distance between the control point and the UWB reference node,
Figure RE-GDA0003518482500000094
Figure RE-GDA0003518482500000095
representing an estimated value of the distance between the control point and the INS reference node, and g representing the number of satellites;
Figure RE-GDA0003518482500000096
the mean square difference between the estimated value of the distance between the control point at the time t and the INS reference node and the estimated value of the distance between the control point at the time t and the UWB reference node; the specific calculation process is as follows:
Figure RE-GDA0003518482500000097
wherein ,
Figure RE-GDA0003518482500000098
is the position information of the INS in the east direction,
Figure RE-GDA0003518482500000099
is the position information in the north direction,
Figure RE-GDA00035184825000000910
represents the position of the ith UWB in the east direction;
Figure RE-GDA00035184825000000911
indicating the position of the ith UWB in the north direction.
(4) And based on the state vector and the observation vector, filtering by using a self-adaptive iterative extended Kalman filter 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 based on an IEKF forward filter that expects maximization and an R-T-S (Rauch-Tung-Striebel) backward smoother, and this embodiment is simply referred to as an improved IEKF algorithm.
The adaptive updating adopting the noise statistics specifically comprises the following steps:
Figure RE-GDA00035184825000000912
Figure RE-GDA00035184825000000913
wherein ,
Figure RE-GDA0003518482500000101
is the predictor of the covariance matrix of the state vector after the j +1 th iteration,
Figure RE-GDA0003518482500000102
iterating the covariance matrix of the measurement noise for j +1 times at the time t; pt (j+1)Is the covariance matrix of the state vector after the j +1 th iteration,
Figure RE-GDA0003518482500000103
is iteration j +1 at time tThe state vector of the secondary system is,
Figure RE-GDA0003518482500000104
is an estimate of the system state vector at time t, ytIs the observation that the system takes based on the GNSS signal conditions,
Figure RE-GDA0003518482500000105
is a jacobian matrix iterated j +1 times at time t;
at time t after the last iteration at time t is complete
Figure RE-GDA0003518482500000106
And
Figure RE-GDA0003518482500000107
will be the initial value at time t +1 in order to perform the iteration at time t + 1.
According to the Kalman gain calculated by the last iteration, the input parameters are fused to obtain output
Figure RE-GDA0003518482500000108
The predicted value of the state vector at the time t; and calculating the covariance matrix thereof so as to carry out the next iteration; and calculating the mahalanobis distance between the output and the estimated value, and stopping iteration if the mahalanobis distance D (t) between the output and the estimated value is less than a preset value threshold. And if the iteration times of the system reach the set maximum iteration times and D (t) is still larger than the preset value, stopping the iteration of the system and outputting the last iteration result.
Performing smooth operation on Kalman gain and system state vector obtained by each iteration by adopting an R-T-S smoothing algorithm, and obtaining optimal estimated values at all moments in a smooth interval by using the smoothing algorithm
Figure RE-GDA0003518482500000109
The specific algorithm process is shown in table 1 below.
TABLE 1
Figure RE-GDA00035184825000001010
Figure RE-GDA0003518482500000111
Figure RE-GDA0003518482500000121
Figure RE-GDA0003518482500000131
wherein ,
Figure RE-GDA0003518482500000132
is the data that is output by the GNSS,
Figure RE-GDA0003518482500000133
is the data that is output by the UWB,
Figure RE-GDA0003518482500000134
the vector of the states of the system is,
Figure RE-GDA0003518482500000135
is that
Figure RE-GDA0003518482500000136
Q is the covariance matrix of the system process noise and R measures the covariance matrix of the noise.
Figure RE-GDA0003518482500000137
Is the jacobian matrix and N is the number of iterations.
In the algorithm from line 7 to line 11, the implementation system automatically switches y between the case where GNSS data is available and the case where GNSS data is not availabletThe variables used. I.e. when GNSS data is available, useGNSS data
Figure RE-GDA0003518482500000138
As a measured value; using UWB data when GNSS data is unavailable
Figure RE-GDA0003518482500000139
As measured values.
In this algorithm, the system can calculate the Kalman gain based on the last iteration
Figure RE-GDA00035184825000001310
Fusing the input parameters and outputting
Figure RE-GDA00035184825000001311
And calculates its covariance matrix PtFor the next iteration.
Adaptive updating of noise statistics is employed in lines 18 to 19 of the algorithm, which is used to adaptively provide noise statistics, i.e. using Pt (j+1)、
Figure RE-GDA00035184825000001312
yt、
Figure RE-GDA00035184825000001313
Equal parameter de-calculation
Figure RE-GDA00035184825000001314
And
Figure RE-GDA00035184825000001315
the Mahalanobis distance is abbreviated as the Mahalanobis distance, and in order to realize the self-adaptive iteration of the system, the Mahalanobis distance is adopted in 17 rows of the algorithm. In addition, the R-T-S method is adopted to improve the positioning accuracy by adopting parameters such as Kalman gain and system state vectors in lines 27 to 32 of the algorithm.
In order to verify the effectiveness of the method of the present embodiment, GPS is adopted as a solution of GNSS for the method of the present embodiment, and UWB is used to provide a solution. And fixes the GPS receiver and the UWB Blind Node (BN) at known coordinates.
The filter thresholds 0.005 and N5 are set, and then the parameters of the modified adaptive iterative extended kalman filter are set as follows:
X0＝[0 0 0 0]T
Figure RE-GDA0003518482500000141
Figure RE-GDA0003518482500000142
Figure RE-GDA0003518482500000143
the state vector of the adaptive iterative extended kalman filter is specifically:
Figure RE-GDA0003518482500000144
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 represented by the following equation.
Figure RE-GDA0003518482500000145
wherein ,
Figure RE-GDA0003518482500000151
representing the measurement noise with a covariance matrix R.
Figure RE-GDA0003518482500000152
Denoted control point to satellite range GNSS estimationThe value of the one or more of the one,
Figure RE-GDA0003518482500000153
denoted is the estimate of the distance between the control point and the INS reference node, and g denotes the number of satellites.
When UWB data is available, the measurement equation of the system can be represented by the following equation.
Figure RE-GDA0003518482500000154
wherein ,
Figure RE-GDA0003518482500000155
representing the measurement noise with a covariance matrix R.
Figure RE-GDA0003518482500000156
Denoted is an estimate of the distance between the control point and the UWB reference node and g denotes the number of satellites.
Fig. 2 and 3 show an east-oriented position diagram and a north-oriented position diagram respectively given by using conventional GNSS + UWB, EKF and IEKF and the improved AIEKF method of the present embodiment.
The results show that the improved AIEKF method of the present embodiment has lower errors in both the east and north directions than conventional GNSS + UWB, EKF and IEKF.
FIG. 4 is a graph showing the Cumulative Distribution Function (CDF) curves of the position error of EKF and conventional IEKF and the improved AIEKF proposed in this embodiment; the results show that the improved AIEKF proposed in this example has a position error of about 0.05m, which is lower than conventional GNSS + UWB, EKF and IEKF.
In addition, the 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
Figure RE-GDA0003518482500000161
Therefore, compared with the prior art, the positioning accuracy of the GNSS/INS/UWB fusion positioning method based on the improved adaptive iterative extended kalman filter provided by the embodiment has lower error than that of the conventional kalman filter and IEKF.
Example two
In one or more embodiments, disclosed is an adaptive iterative EKF-based job site control point positioning system, comprising:
the GNSS is used for acquiring the distance from a control point of a construction site to a satellite; the system comprises a UWB used for acquiring the distance between a control point on a construction site and a UWB reference node and an INS used for 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 collected at the moment t as the state vector of the adaptive iterative extended Kalman filter;
the adaptive iterative extended Kalman filter observation vector generation method comprises the steps that when GNSS data are available, the square difference of an estimated value of the distance between a control point and a GNSS and an estimated value of the distance between the control point and an INS reference node is used as an adaptive iterative extended Kalman filter observation vector; when the GNSS data is unavailable, 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 is used as a device for observing a vector by using the adaptive iterative extended Kalman filter;
and the device is used for filtering by using 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.
EXAMPLE III
In one or more embodiments, a terminal device is disclosed, which includes a server including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the adaptive iterative EKF-based job site control point positioning method of the first embodiment. For brevity, no further description is provided herein.
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 arrays FPGA or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, and so on. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.
The memory may include both read-only memory and random access memory, and may 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 device type information.
In implementation, the steps of the above method may be performed by integrated logic circuits of hardware in a processor or instructions in the form of software.
Example four
In one or more embodiments, a computer-readable storage medium is disclosed having stored thereon instructions adapted to be loaded by a processor of a terminal device and to perform the adaptive iterative EKF-based job site control point location method of example one.
Although the embodiments of the present invention have been described with reference to the accompanying drawings, it is not intended to limit the scope of the present invention, and it should be understood by those skilled in the art that various modifications and variations can be made without inventive efforts by those skilled in the art based on the technical solution of the present invention.

Claims (10)

1. A construction site control point positioning method based on adaptive iteration EKF is characterized by comprising 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 on a construction site and a UWB reference node through UWB, and acquiring the position information of the control point through INS;
using the position error and the speed error of the inertial navigation system INS in the east direction and the north direction collected at the time t as the state vector of the adaptive iterative extended Kalman filter;
when GNSS data are available, taking the square difference of 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 expansion Kalman filter;
when the GNSS data is unavailable, 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 expansion Kalman filter;
and based on the state vector and the observation vector, filtering by using a self-adaptive iterative extended Kalman filter 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.
2. The method as claimed in claim 1, wherein the state vector of the adaptive iterative extended kalman filter is specifically:
Figure FDA0003494401850000011
wherein (δ Px)t-1,δPyt-1) Position error of inertial navigation system for east and north directions at time t-1, (δ Vxt-1,δVyt-1) Velocity errors of the inertial navigation system for the east and north directions at time t-1;
Figure FDA0003494401850000012
the predicted value of the position error of the inertial navigation system at the time t in the east direction and the north direction,
Figure FDA0003494401850000021
the predicted values of the speed errors of the inertial navigation system at the t moment in the east direction and the north direction are obtained; at is the sampling period of the system,wtfor system noise, its covariance matrix is Q, Xt-1A is the system matrix for the state vector at time t-1.
3. The method as claimed in claim 1, wherein when GNSS data is available, the adaptive iterative extended kalman filter observation vector is specifically:
Figure FDA0003494401850000022
wherein ,
Figure FDA0003494401850000023
representing the measurement noise with a covariance matrix R;
Figure FDA0003494401850000024
respectively representing the control point to satellite distance estimates,
Figure FDA0003494401850000025
representing an estimated value of the distance between the control point and the INS reference node, and g representing the number of satellites;
Figure FDA0003494401850000026
is the square difference of the estimated value of the distance between the control point at the time t and the INS reference node and the estimated value of the distance between the control point at the time t and the satellite.
4. The method as claimed in claim 1, wherein when GNSS data is not available, the adaptive iterative extended kalman filter observation vector is specifically:
Figure FDA0003494401850000027
wherein ,
Figure FDA0003494401850000028
representing the measurement noise with a covariance matrix R;
Figure FDA0003494401850000029
representing an estimate of the distance between the control point and the UWB reference node,
Figure FDA0003494401850000031
representing an estimated value of the distance between the control point and the INS reference node, and g representing the number of satellites;
Figure FDA0003494401850000032
is the square difference of the estimated value of the distance between the control point at the time t and the INS reference node and the estimated value of the distance between the control point at the time t and the UWB reference node.
5. The method for locating the control point of the construction site based on the adaptive iterative EKF as claimed in claim 1, wherein the adaptive updating using the noise statistics is specifically as follows:
Figure FDA0003494401850000033
Figure FDA0003494401850000034
wherein ,
Figure FDA00034944018500000310
is the predictor of the covariance matrix of the state vector after the j +1 th iteration,
Figure FDA0003494401850000035
iterate for time tA covariance matrix of the measurement noise of j +1 times;
Figure FDA00034944018500000311
is the covariance matrix of the state vector after the j +1 th iteration,
Figure FDA0003494401850000036
is the system state vector for iteration j +1 times at time t,
Figure FDA0003494401850000037
is an estimate of the system state vector at time t, ytIs the observation that the system takes based on the GNSS signal conditions,
Figure FDA0003494401850000038
is a jacobian matrix iterated j +1 times at time t;
at time t after the last iteration at time t is complete
Figure FDA00034944018500000312
And
Figure FDA0003494401850000039
will be the initial value at time t +1 in order to perform an iteration at time t + 1.
6. The method as claimed in claim 1, wherein the input parameters are fused according to the kalman gain calculated from the previous iteration to obtain the predicted value of the state vector at the output time t, and the covariance matrix is calculated to perform the next iteration;
calculating the Mahalanobis distance between the output and the estimated value, and if the Mahalanobis distance is smaller than a set threshold, judging whether the iteration is terminated; and if the iteration times reach the set maximum iteration times and the Mahalanobis distance is any larger than the set threshold value, stopping the iteration and outputting the last iteration result.
7. The method as claimed in claim 1, wherein R-T-S smoothing algorithm is used to smooth kalman gain and system state vector obtained from each iteration; and obtaining the optimal state vector predicted values at all the moments in the smooth interval.
8. A construction site control point positioning system based on adaptive iterative EKF is characterized by comprising:
the GNSS is used for acquiring the distance from a control point to a satellite on a construction site; the system comprises a UWB used for acquiring the distance between a control point on a construction site and a UWB reference node and an INS used for 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 collected at the moment t as the state vector of the adaptive iterative extended Kalman filter;
the adaptive iterative extended Kalman filter observation vector generation method comprises the steps that when GNSS data are available, the square difference of an estimated value of the distance between a control point and a GNSS and an estimated value of the distance between the control point and an INS reference node is used as an adaptive iterative extended Kalman filter observation vector; when the GNSS data is unavailable, 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 is used as a device for observing a vector by using the adaptive iterative extended Kalman filter;
and the device is used for filtering by using 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.
9. A terminal device comprising a processor and a memory, the processor being arranged to implement instructions; the memory is configured to store a plurality of instructions adapted to be loaded by the processor and to perform the adaptive iterative EKF-based job site control point location method of any one of claims 1-7.
10. A computer readable storage medium having stored thereon a plurality of instructions adapted to be loaded by a processor of a terminal device and to perform the adaptive iterative EKF based jobsite control point location method of any one of claims 1-7.
CN202210108939.4A 2022-01-28 2022-01-28 Construction site control point positioning method and system based on self-adaptive iterative EKF Active CN114440880B (en)

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 true CN114440880A (en) 2022-05-06
CN114440880B 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)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114779307A (en) * 2022-06-17 2022-07-22 武汉理工大学 Port area-oriented UWB/INS/GNSS seamless positioning method
CN115856974A (en) * 2022-11-18 2023-03-28 苏州华米导航科技有限公司 GNSS, INS and visual tight integrated navigation positioning method based on invariant filtering

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102278974A (en) * 2010-06-09 2011-12-14 南京德朔实业有限公司 Laser ranging apparatus
US20170276783A1 (en) * 2016-03-28 2017-09-28 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
CN109269498A (en) * 2018-08-06 2019-01-25 济南大学 Towards auto-adaptive estimate EKF filtering algorithm and system with shortage of data UWB pedestrian navigation
CN109633590A (en) * 2019-01-08 2019-04-16 杭州电子科技大学 Extension method for tracking target based on GP-VSMM-JPDA
CN110596694A (en) * 2019-09-20 2019-12-20 吉林大学 Complex environment radar multi-target tracking and road running environment prediction method
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

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102278974A (en) * 2010-06-09 2011-12-14 南京德朔实业有限公司 Laser ranging apparatus
US20170276783A1 (en) * 2016-03-28 2017-09-28 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
CN109269498A (en) * 2018-08-06 2019-01-25 济南大学 Towards auto-adaptive estimate EKF filtering algorithm and system with shortage of data UWB pedestrian navigation
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
CN110596694A (en) * 2019-09-20 2019-12-20 吉林大学 Complex environment radar multi-target tracking and road running environment prediction method
CN113566828A (en) * 2021-07-09 2021-10-29 哈尔滨工业大学 Impact-resistant scanning matching method and system based on multi-sensor decision fusion

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
PIETRA V D等: "Seamless navigation using UWB-based multi-sensor system", 《IEEE SENSORS JOURNAL》, pages 1079 - 1084 *
YUAN XU等: "Performance enhancement for INS/UWB integrated indoor tracking using distributed iterated extended Kalman filter", 《2018 UBIQUITOUS POSITIONING,INDOOR NAVIGATION AND LOCATION-BASED SERVICES》, pages 1 - 5 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114779307A (en) * 2022-06-17 2022-07-22 武汉理工大学 Port area-oriented UWB/INS/GNSS seamless positioning method
CN115856974A (en) * 2022-11-18 2023-03-28 苏州华米导航科技有限公司 GNSS, INS and visual tight integrated navigation positioning method based on invariant filtering
CN115856974B (en) * 2022-11-18 2024-04-05 苏州华米导航科技有限公司 GNSS, INS and visual tight combination navigation positioning method based on invariant filtering

Also Published As

Publication number Publication date
CN114440880B (en) 2023-06-13

Similar Documents

Publication Publication Date Title
CN109141413B (en) EFIR filtering algorithm and system with data missing UWB pedestrian positioning
CN114440880A (en) Construction site control point positioning method and system based on adaptive iteration EKF
CN107677272B (en) AUV (autonomous Underwater vehicle) collaborative navigation method based on nonlinear information filtering
CN108896047B (en) Distributed sensor network collaborative fusion and sensor position correction method
CN111044050B (en) Bluetooth positioning method based on particle filtering and Kalman filtering
CN109782269B (en) Distributed multi-platform cooperative active target tracking method
CN108966120B (en) Combined trilateral positioning method and system for dynamic cluster network improvement
CN109141412B (en) UFIR filtering algorithm and system for data-missing INS/UWB combined pedestrian navigation
CN109655060B (en) INS/UWB integrated navigation algorithm and system based on KF/FIR and LS-SVM fusion
CN109269498B (en) Adaptive pre-estimation EKF filtering algorithm and system for UWB pedestrian navigation with data missing
CN113411883B (en) Distributed cooperative positioning method for determining convergence
CN114139109A (en) Target tracking method, system, equipment, medium and data processing terminal
CN106772510A (en) A kind of frequency hopping distance-finding method based on carrier phase measurement
CN112034713B (en) Method and system for estimating optimal state of moving target in non-ideal network environment
CN113218388B (en) Mobile robot positioning method and system considering variable colored measurement noise
CN103727947B (en) Based on the UKF deep coupling positioning methods of BDS and GIS filtered and system
CN107966697B (en) Moving target tracking method based on progressive unscented Kalman
CN109916401B (en) Distributed seamless tight combination navigation method and system adopting LS-SVM assisted EKF filtering method
CN113391285B (en) Target tracking smoothing method for measuring flicker noise under random delay
Buehrer et al. Cooperative indoor position location using the parallel projection method
Oshiga et al. Anchor selection for localization in large indoor venues
Wu et al. Improved adaptive iterated extended Kalman filter for GNSS/INS/UWB-integrated fixed-point positioning
CN109343013B (en) Spatial registration method and system based on restarting mechanism
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

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