CN112683268A - Roadway real-time positioning navigation method and system based on extended Kalman filtering - Google Patents
Roadway real-time positioning navigation method and system based on extended Kalman filtering Download PDFInfo
- Publication number
- CN112683268A CN112683268A CN202011424015.2A CN202011424015A CN112683268A CN 112683268 A CN112683268 A CN 112683268A CN 202011424015 A CN202011424015 A CN 202011424015A CN 112683268 A CN112683268 A CN 112683268A
- Authority
- CN
- China
- Prior art keywords
- state
- roadway
- covariance
- current state
- tunneling equipment
- 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.)
- Pending
Links
- 238000001914 filtration Methods 0.000 title claims abstract description 50
- 238000000034 method Methods 0.000 title claims abstract description 32
- 230000005641 tunneling Effects 0.000 claims abstract description 114
- 238000006243 chemical reaction Methods 0.000 claims abstract description 23
- 238000005259 measurement Methods 0.000 claims description 70
- 239000011159 matrix material Substances 0.000 claims description 25
- 238000012937 correction Methods 0.000 claims description 8
- 239000000126 substance Substances 0.000 claims description 6
- 230000007704 transition Effects 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 6
- 238000009434 installation Methods 0.000 description 3
- 230000008901 benefit Effects 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 238000009412 basement excavation Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 239000000428 dust Substances 0.000 description 1
- 230000005611 electricity Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 239000007788 liquid Substances 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 230000017105 transposition Effects 0.000 description 1
Images
Abstract
The invention discloses a roadway real-time positioning navigation method and a system based on extended Kalman filtering, which comprises the following steps: acquiring initial state parameters and a coordinate conversion relation of the tunneling equipment; establishing a odometer walking model by a model linear approximation method, and predicting and updating the odometer walking model by using an extended Kalman filtering algorithm and taking related parameters as filtering state variables to obtain a state one-step predicted value and a state one-step predicted covariance of the tunneling equipment; acquiring current state parameters of the tunneling equipment, and measuring and updating the state one-step predicted value and the state one-step predicted covariance by using an extended Kalman filtering algorithm to obtain a current state optimal estimated value and a current state optimal estimated covariance of the tunneling equipment; and controlling the motion state of the tunneling equipment according to the optimal estimation value of the current state and the optimal estimation covariance of the current state. The invention can improve the real-time positioning precision and positioning reliability of the tunneling equipment in the roadway and improve the working efficiency and quality of tunneling.
Description
Technical Field
The invention relates to the technical field of roadway positioning, in particular to a roadway real-time positioning navigation method based on extended Kalman filtering. The invention also relates to a roadway real-time positioning navigation system based on the extended Kalman filtering.
Background
With the development of the Chinese machinery industry, more and more mechanical devices are widely used.
The types of mechanical equipment are many, and large-scale engineering equipment such as a heading machine, an excavator, a gantry crane and the like is generally used in civil engineering and building engineering. Taking a shield machine as an example, the modern shield machine is large-scale high-end tunneling equipment integrating light, machine, electricity and liquid.
During the operation of the tunneling equipment (hereinafter referred to as "tunneling equipment"), the precise tunneling direction and movement state need to be kept constantly to prevent the accumulation of system errors. In order to meet the requirements of automatic positioning, guiding and posture adjustment of tunneling equipment in the walking, tunneling and supporting processes, the premise of accurate tunnel construction is to study the positioning and guiding technology suitable for the complex and severe environment of tunnel tunneling.
At present, the common positioning modes mainly include ultra-wideband positioning, optical positioning, ultrasonic positioning, inertial navigation positioning and the like. The ultra-wideband positioning is a common underground engineering positioning technology, has the advantages of low power consumption, difficulty in interference, high positioning accuracy and the like, but due to the fact that underground space is limited, a base station system required by three-dimensional positioning cannot be arranged at a sufficient distance, and positioning errors in the left, right and height directions are large. In the measurement method based on optics, such as laser, machine vision and the like, because the dust in front of the tunnel face is large, the visibility is low, the optical channel is blocked, and the problems exist in the actual application. Based on the positioning mode of supersound, the system fuses through ultrasonic positioning system and digital magnetic compass, and the realization is simple, but the condition of crosstalking easily appears between the sensor, is difficult to guarantee the precision. The maximum advantage of inertial navigation positioning is that the method does not depend on the external environment, can provide detailed attitude angle and speed information, but has large accumulated error of inertial navigation positioning, and cannot keep long-time high-precision work. Therefore, in the prior art, the requirements of roadway excavation on real-time positioning are difficult to meet by various single roadway positioning modes.
Therefore, how to improve the real-time positioning accuracy and positioning reliability of the tunneling equipment in the roadway and improve the working efficiency and quality of tunneling is a technical problem faced by technical personnel in the field.
Disclosure of Invention
The invention aims to provide a roadway real-time positioning navigation method based on extended Kalman filtering, which can improve the real-time positioning precision and positioning reliability of tunneling equipment in a roadway and improve the working efficiency and quality of tunneling. The invention further aims to provide a roadway real-time positioning navigation system based on the extended Kalman filtering.
In order to solve the technical problem, the invention provides a roadway real-time positioning navigation method based on extended Kalman filtering, which comprises the following steps:
acquiring initial state parameters of the tunneling equipment in a roadway and a conversion relation of the roadway relative to a geographic coordinate system;
establishing an odometer walking model by using the initial state parameters and the conversion relation through a model-based linear approximation method, and predicting and updating the odometer walking model by using an extended Kalman filtering algorithm and using related parameters in the odometer walking model as filtering state variables to obtain a state one-step predicted value and a state one-step predicted covariance of the tunneling equipment;
acquiring current state parameters of the tunneling equipment in a roadway, and measuring and updating the state one-step predicted value and the state one-step predicted covariance according to each current state parameter by using an extended Kalman filtering algorithm to obtain a current state optimal estimated value and a current state optimal estimated covariance of the tunneling equipment;
and controlling the motion state of the tunneling equipment according to the optimal estimation value of the current state and the optimal estimation covariance of the current state.
Preferably, the acquiring of the initial state parameters of the heading equipment in the roadway specifically includes:
and acquiring the space coordinate, the attitude angle, the walking coefficient, the steering coefficient, the pitching correction angle of the walking track relative to the vehicle body and the course correction angle of the walking track relative to the vehicle body of the tunneling device.
Preferably, the obtaining of the conversion relationship of the roadway with respect to the geographic coordinate system specifically includes:
and acquiring the conversion relation of the roadway relative to a geographic coordinate system through a total station erected at the road junction and a prism installed on the tunneling equipment.
Preferably, the acquiring the space coordinate of the heading device specifically includes:
acquiring the longitudinal position of the tunneling equipment in a roadway through an ultra-wideband positioning assembly arranged on a roadway opening and the tunneling equipment;
and acquiring the position of the tunneling equipment in the roadway through the total station and the prism.
Preferably, the acquiring the attitude angle of the heading device specifically includes:
and acquiring the attitude angle of the tunneling equipment through an inertial navigator arranged on the tunneling equipment.
Preferably, the predicting and updating the odometer walking model by using the extended kalman filtering algorithm and taking the relevant parameters in the odometer walking model as filtering state variables specifically comprises the following steps:
by the state transition equation:
calculating a state one-step predicted value;
wherein the content of the first and second substances,
for a one-step prediction of state, mutIs the state value u of the tunneling equipment at the moment tt+1＝[θ1,θ2]T，Wt+1For system noise, T is the transpose of the matrix, θ1For left-hand walking encoder increments, θ2Is the increment of the right-side walking encoder, and g is a functional relation;
and by the formula:
calculating the one-step prediction covariance of the state;
wherein the content of the first and second substances,
one-step prediction of covariance for state, Gt+1Is g (u)t+1,μt) At mutPartial derivatives of (E), sigmatAs an initial covariance, Gt+1 TIs Gt+1Transpose of Rt+1Is a preset constant.
Preferably, the measuring and updating the state one-step predicted value and the state one-step predicted covariance according to each current state parameter by using an extended kalman filter algorithm specifically includes:
by the formula:
calculating Kalman filtering gain;
wherein, Kt+1As a Kalman filter gain, Ht+1For measuring the matrix, Ht+1 TIs Ht+1Transpose of (Q)t+1Is a measurement error vector;
and by the formula:
calculating the optimal estimation value of the current state of the tunneling equipment;
wherein, mut+1For optimal estimation of the current state of the tunnelling apparatus, Dt+1Predicting an error for the measurement;
and then through the formula:
calculating the optimal estimation covariance of the current state of the tunneling equipment;
wherein, sigmat+1And (3) optimally estimating the covariance for the current state of the tunneling equipment, wherein I is a unit matrix.
Preferably, the measuring and updating the state one-step predicted value and the state one-step predicted covariance according to each current state parameter by using an extended kalman filter algorithm specifically includes:
and measuring and updating the state one-step prediction value and the state one-step prediction covariance through the measurement matrix, the measurement prediction error and the measurement error vector of the inertial navigator, the ultra-wideband positioning assembly and the total station respectively.
The invention also provides a roadway real-time positioning navigation system based on the extended Kalman filtering, which comprises:
the initial acquisition module is used for acquiring initial state parameters of the tunneling equipment in a roadway and a conversion relation of the roadway relative to a geographic coordinate system;
the model establishing module is used for establishing an odometer walking model by using the initial state parameters and the conversion relation through a model-based linear approximation method;
the prediction updating module is used for predicting and updating the odometer walking model by using an extended Kalman filtering algorithm and taking related parameters in the odometer walking model as filtering state variables so as to obtain a state one-step prediction value and a state one-step prediction covariance of the tunneling equipment;
the measurement updating module is used for acquiring current state parameters of the tunneling equipment in a roadway, and measuring and updating the state one-step predicted value and the state one-step predicted covariance according to the current state parameters by using an extended Kalman filtering algorithm so as to obtain a current state optimal estimated value and a current state optimal estimated covariance of the tunneling equipment;
and the control output module is used for controlling the motion state of the tunneling equipment according to the optimal estimation value of the current state and the optimal estimation covariance of the current state.
The invention provides a roadway real-time positioning navigation method based on extended Kalman filtering, which mainly comprises four steps. In the first step, the main content is to obtain the initial state parameters of the tunneling equipment in the tunnel (or tunnel) at the initial time (t) and the conversion relation of the tunnel relative to the geographic coordinate system. In the second step, firstly, a plurality of initial state parameters and a conversion relation of the tunneling equipment acquired in the first step are utilized, an odometer walking model related to odometer parameters (changed when the tunneling equipment walks) is established by a model-based linear approximation method, then a plurality of initial state parameters of the tunneling equipment are used as filtering state variables, the filtering state variables are utilized to calculate through an extended Kalman filtering algorithm, and the established odometer walking model is predicted and updated according to the calculation result, so that a state one-step predicted value and a state one-step prediction covariance of the tunneling equipment are obtained. In the third step, when the tunneling equipment is in the running state, the current state parameters (observed values) of the tunneling equipment in the roadway are obtained, the extended Kalman filter algorithm is also used for calculating according to all the current state parameters, and the calculation results are used for measuring, updating and correcting the state one-step predicted value and the state one-step predicted covariance, so that the current state optimal estimation value and the current state optimal estimation covariance of the tunneling equipment are obtained. In the fourth step, the movement states of the tunneling equipment, such as walking and steering, can be accurately controlled according to the obtained optimal estimation value of the current state and the optimal estimation covariance of the current state. Therefore, the roadway real-time positioning navigation method based on the extended Kalman filtering determines the recursion prediction quantity of the odometer walking model by using Kalman filtering gain and integrates the weight of the observed value acquired by a multi-channel detection mode, outputs the optimal estimated value of the current state, realizes the real-time high-precision accurate positioning of the tunneling equipment, can improve the real-time positioning precision and the positioning reliability of the tunneling equipment in the roadway, and improves the working efficiency and the quality of tunnel tunneling.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the provided drawings without creative efforts.
Fig. 1 is a flow chart of an embodiment of the present invention.
Fig. 2 is a schematic block diagram of a roadway real-time positioning and navigation method.
Fig. 3 is a block diagram of an embodiment of the present invention.
Fig. 4 is a hardware structure diagram of an embodiment of the present invention.
Among them, in fig. 3 to 4:
the system comprises an initial acquisition module-1, a model building module-2, a prediction updating module-3, a measurement updating module-4, a control output module-5, a vehicle body-6, a total station-7, a prism-8, an ultra wide band positioning component-9, an inertial navigator-10 and a walking encoder-11.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Referring to fig. 1 and fig. 2, fig. 1 is a flowchart illustrating an embodiment of the present invention, and fig. 2 is a schematic block diagram illustrating a method for real-time positioning and navigating a roadway.
In a specific embodiment provided by the invention, the roadway real-time positioning navigation method based on the extended kalman filter mainly comprises four steps, which are respectively:
s1, acquiring initial state parameters of the tunneling equipment in the roadway and a conversion relation of the roadway relative to a geographic coordinate system;
s2, establishing an odometer walking model by using the initial state parameters and the conversion relation through a model-based linear approximation method, and predicting and updating the odometer walking model by using an extended Kalman filtering algorithm and using related parameters in the odometer walking model as filtering state variables to obtain a state one-step predicted value and a state one-step prediction covariance of the tunneling equipment;
s3, obtaining current state parameters of the tunneling equipment in a roadway, and measuring and updating the state one-step predicted value and the state one-step predicted covariance according to the current state parameters by using an extended Kalman filtering algorithm to obtain a current state optimal estimated value and a current state optimal estimated covariance of the tunneling equipment;
and S4, controlling the motion state of the tunneling equipment according to the optimal estimation value of the current state and the optimal estimation covariance of the current state.
In step S1, the main content is to obtain the initial state parameters of the heading device in the roadway (or tunnel) at the initial time (t), and the conversion relationship of the roadway with respect to the geographic coordinate system.
In step S2, a plurality of initial state parameters and a conversion relationship of the tunneling apparatus obtained in the first step are used to establish an odometer travel model related to odometer parameters (which change when the tunneling apparatus travels) by a model-based linear approximation method, then a plurality of initial state parameters of the tunneling apparatus are used as filter state variables, the filter state variables are used to calculate by an extended kalman filter algorithm, and the established odometer travel model is predicted and updated according to the calculation result, so as to obtain a state one-step predicted value and a state one-step predicted covariance of the tunneling apparatus.
In step S3, when the tunneling apparatus is in the operating state, current state parameters (observed values) of the tunneling apparatus in the roadway are obtained, calculation is performed according to each current state parameter by using the extended kalman filter algorithm, and the state one-step predicted value and the state one-step predicted covariance are measured, updated and corrected by using the calculation result, so as to obtain the current state optimal estimated value and the current state optimal estimated covariance of the tunneling apparatus.
In step S4, the movement states of the tunneling apparatus, such as walking and steering, can be accurately controlled according to the obtained optimal estimation value of the current state and the optimal estimation covariance of the current state.
Therefore, according to the extended kalman filter-based roadway real-time positioning and navigation method provided by the embodiment, by determining the recursive prediction quantity of the odometer travel model by using the kalman filter gain and fusing the weight of the observed value acquired by the multi-channel detection mode, the optimal estimated value of the current state is output, the real-time high-precision accurate positioning of the tunneling equipment is realized, the real-time positioning precision and the positioning reliability of the tunneling equipment in the roadway can be improved, and the working efficiency and the quality of tunneling of the tunnel are improved.
In step S1, a total station is set to measure and an inertial navigator is used to find north, and system parameters at an initial time (t time) and an initial state of the tunneling apparatus in a tunnel are obtained, where the system parameters mainly include a transformation matrix of the tunnel relative to a geographic coordinate system, an installation error of the inertial navigator, an installation position of an ultra-wideband positioning component (transceiver), a prism coordinate on a vehicle body, an initial covariance matrix, and the like, and the initial state of the tunneling apparatus in the tunnel mainly includes 10 parameters of a spatial coordinate of the tunneling apparatus, an attitude angle, a walking coefficient, a steering coefficient, a pitch correction angle of a walking track relative to the vehicle body, and a heading correction angle of the walking track relative to the vehicle body, and the 10 parameters are used as filter state variables:
μ＝[x,y,z,α,β,γ,ω,κ,Δα,Δγ]T
the space coordinates are respectively a longitudinal position x (mileage or walking distance) of the vehicle body relative to the roadway, a transverse position y (left deviation) of the vehicle body in the roadway and a vertical position z (upper deviation) of the vehicle body in the roadway; and the attitude angles of the tunneling equipment are a pitch angle alpha, a roll angle beta and a heading angle gamma respectively. The longitudinal position x is divided by the increment of the walking encoder to form a walking coefficient omega, the steering angle is divided by the increment difference of the walking encoder (the two sides of the vehicle body are respectively provided with a walking encoder, namely a left walking encoder and a right walking encoder) to form a steering coefficient kappa, the pitch correction angle delta alpha of the walking track (such as a track) relative to the vehicle body is included in the filtering state variable, and the course correction angle delta gamma of the walking track relative to the vehicle body is also included in the filtering state variable. Meanwhile, ω, κ, Δ α, Δ γ may also be collectively referred to as walking characteristic parameters.
When the conversion relation of the roadway relative to the geographic coordinate system is obtained, the conversion relation can be obtained through a total station erected at the road junction and a prism installed on tunneling equipment; when the spatial coordinates of the tunneling equipment are obtained, specifically, the longitudinal position of the tunneling equipment in a roadway can be obtained through an ultra-wideband positioning assembly arranged on a roadway opening and the tunneling equipment, and meanwhile, the position (including the longitudinal position, the transverse position and the vertical position) of the tunneling equipment in the roadway is obtained through a total station and a prism; when the attitude angle of the heading equipment is obtained, specifically, the attitude angle of the heading equipment can be obtained through an inertial navigator installed on the heading equipment.
In addition, the extended kalman filter algorithm is to predict the state value at the previous time to obtain the predicted state value at the current time, correct the predicted value by using the measured value at the current time to obtain the state estimated value of the minimum mean square error, and divide the state estimated value into two links, namely prediction updating and measurement updating, wherein step S2 mainly implements prediction updating, and step S3 mainly implements measurement updating.
In step S2, the prediction update is mainly to obtain a state one-step prediction value
Sum state one-step prediction covariance matrix
First, there is a state transition equation
Wherein u ist+1＝[θ1,θ2]T，θ1In increments of left-hand walking encoders, θ2In increments of the right-hand walking encoder,
for a one-step prediction of state, Wt+1Is systematic noise, which is often taken as the mean 0 and covariance Rt+1White gaussian noise, mutAnd g is a function relation, and T is the transposition of the matrix.
Establishing an odometer walking model based on a model linear approximation method, namely the detailed description of a state transition equation:
where ε is the uncertainty error of the system prediction, i.e., the error introduced by the system noise, over the time t to t + 1.
By the above state transition equation
And state transition matrix Gt+1Wherein G ist+1Is g (u)t+1,μt) At mutThe partial derivative of (c).
Further, state further prediction covariance matrix
The following is obtained:
wherein the content of the first and second substances,
one-step prediction of covariance for state, Gt+1 TIs Gt+1Transposition of (5), initial covariance matrix sigmatValue of 0.1Unit array, Rt+1The value of (a) is determined according to the actual situation, and is generally a preset constant.
In step S3, there is first a measurement error equation:
wherein Z ist+1Represents an observed quantity (or measured value),
for measurement one-step prediction, Vt+1Is the measurement noise, usually takes the mean value of 0 and the covariance of Qt+1The function h is used to transfer the predicted state quantity to the measurement one-step prediction.
To define the weight, the kalman filter gain needs to be calculated by formula (i):
then, the optimal estimation value of the current state of the tunneling equipment can be calculated through a formula II:
meanwhile, the optimal estimation covariance of the current state of the tunneling equipment can be calculated through a formula III:
wherein, Kt+1As a Kalman filter gain, Ht+1For measuring the matrix (function h is in
First partial derivative of (d), Ht+1 TIs Ht+1Transpose of (Q)t+1To measure errorsDifference vector, mut+1For optimal estimation of the current state of the tunnelling apparatus, Dt+1For measuring prediction errors, sigmat+1And (3) optimally estimating the covariance for the current state of the tunneling equipment, wherein I is a unit matrix.
In addition, in order to meet the real-time high-precision requirement of the tunneling equipment on the roadway positioning, the measurement updating link in the embodiment is divided into four steps of inertial navigation measurement updating, ultra-wideband ranging measurement updating, total station measurement updating and manual measurement updating, and the acquired observed quantity is subjected to fusion calculation by using an extended kalman filtering algorithm.
For updating by inertial navigation measurement, an inertial navigation measurement error model may be established first:
wherein e isαError of measurement prediction for pitch angle (calculated by subtracting instrument measurement from prediction output of previous step), eβError prediction for roll angle measurement, eγThe measured prediction error of the course angle is,
for the pitch angle predicted by one step (predicted value output by the odometer travel model),
for the roll angle to be predicted in one step,
the predicted heading angle for the one step.
The measurement matrix is:
the measurement prediction error is as follows:
the measurement error vector is:
wherein, deltaα,δβ,δγThe measurement error introduced for the inertial navigator itself is often taken as the precision value of the instrument. During calculation, the measurement matrix, the measurement prediction error and the measurement error vector are respectively substituted into the formula I, the formula II and the formula III, so that the Kalman filtering gain, the optimal estimation value of the current state and the optimal estimation covariance of the current state which are updated through inertial navigation measurement can be respectively calculated.
When measurement and updating are carried out through ultra-wideband ranging, in order to solve the problem of positioning errors of ultra-wideband ranging in the left, right and height directions caused by limited roadway space, a distance model of the tunneling equipment relative to a roadway is established based on the existing known working conditions of the tunneling equipment, and the advancing distance of the tunneling equipment in the roadway is obtained.
The distance model of ultra-wideband ranging is as follows:
wherein L is the ultra-wideband ranging result,
the installation position of a receiver in the ultra-wideband positioning component in the roadway is that the receiver is a rotating matrix of the carrier relative to the roadway
The position of a transmitter arranged on the body of the tunneling equipment is
Position of tunneling equipment relative to roadwayPut PB＝[x,y,z]T，eLFor the measurement prediction error of ultra-wideband ranging,
is the predicted ranging value.
The measurement matrix is:
the measurement prediction error is as follows:
D＝eL
the measurement error vector is:
Qt+1＝[δL 2]
wherein, deltaLThe measurement error introduced for the ultra-wideband positioning component itself is often taken as the precision value of the instrument.
The measurement matrix, the measurement prediction error and the measurement error vector are respectively substituted into the formula I, the formula II and the formula III, so that the Kalman filtering gain, the current state optimal estimation value and the current state optimal estimation covariance updated through ultra-wideband ranging measurement can be respectively calculated.
For measurement update by a total station, a total station measurement error model may be established first:
wherein the content of the first and second substances,
for measuring the coordinates of the vehicle body predicted in one step, x, y, z are measured values of the vehicle body relative to the roadway, exError prediction for measurement of longitudinal position of vehicle body, eyError prediction for the measurement of the transverse position of the vehicle body, ezAnd predicting the error of the measurement of the vertical position of the vehicle body.
The measurement matrix is:
the measurement prediction error is as follows:
the measurement error vector is:
wherein, deltax,δy,δzThat is, the measurement error introduced by the total station itself is often taken as the precision value of the instrument.
The measurement matrix, the measurement prediction error and the measurement error vector are respectively substituted into the formula I, the formula II and the formula III, so that the Kalman filtering gain, the current state optimal estimation value and the current state optimal estimation covariance updated through ultra-wideband ranging measurement can be respectively calculated.
When the data are updated through manual measurement, a field operator can estimate and give x, y and Z values of the tunneling equipment relative to roadway driving through rough measurement to serve as observed quantity Zt+1The value-taking principles of the artificial measurement error model, the measurement matrix, the measurement prediction error and the measurement error vector are the same as those described above, and the values are respectively substituted into the formula I, the formula II and the formula III, so that the Kalman filter gain, the current state optimal estimation value and the current state optimal estimation covariance updated through artificial measurement ultra-wideband distance measurement can be respectively calculated.
As shown in fig. 3 and 4, fig. 3 is a block diagram of an embodiment of the present invention, and fig. 4 is a hardware diagram of an embodiment of the present invention.
The embodiment also provides a roadway real-time positioning navigation system based on extended kalman filtering, which mainly comprises an initial acquisition module 1, a model establishing module 2, a prediction updating module 3, a measurement updating module 4 and a control output module 5. The initial acquisition module 1 is mainly used for acquiring initial state parameters of the tunneling equipment in a roadway and a conversion relation of the roadway relative to a geographic coordinate system. The model establishing module 2 is mainly used for establishing an odometer walking model by using initial state parameters and a conversion relation through a model-based linear approximation method. The prediction updating module 3 is mainly used for performing prediction updating on the odometer walking model by using the extended Kalman filtering algorithm and taking the related parameters in the odometer walking model as filtering state variables so as to obtain a state one-step prediction value and a state one-step prediction covariance of the tunneling equipment. The measurement updating module 4 is mainly used for obtaining current state parameters of the tunneling equipment in a roadway, and measuring and updating the state one-step predicted value and the state one-step predicted covariance according to the current state parameters by using an extended Kalman filtering algorithm so as to obtain the current state optimal estimated value and the current state optimal estimated covariance of the tunneling equipment. The control output module 5 is mainly used for controlling the motion state of the tunneling equipment according to the optimal estimation value of the current state and the optimal estimation covariance of the current state.
For a hardware structure, the total station 7 and the receiving end of the ultra-wideband positioning component 9 are arranged at the position of the road junction, and the vehicle body 6 is provided with the transmitting end of the ultra-wideband positioning component 9, the prism 8 used in cooperation with the total station 7, the inertial navigator 10 and the walking encoders 11 distributed on the left side and the right side of the vehicle body.
The previous description of the disclosed embodiments is provided to enable any person skilled in the art to make or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the invention. Thus, the present invention is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.
Claims (9)
1. A roadway real-time positioning navigation method based on extended Kalman filtering is characterized by comprising the following steps:
acquiring initial state parameters of the tunneling equipment in a roadway and a conversion relation of the roadway relative to a geographic coordinate system;
establishing an odometer walking model by using the initial state parameters and the conversion relation through a model-based linear approximation method, and predicting and updating the odometer walking model by using an extended Kalman filtering algorithm and using related parameters in the odometer walking model as filtering state variables to obtain a state one-step predicted value and a state one-step predicted covariance of the tunneling equipment;
acquiring current state parameters of the tunneling equipment in a roadway, and measuring and updating the state one-step predicted value and the state one-step predicted covariance according to each current state parameter by using an extended Kalman filtering algorithm to obtain a current state optimal estimated value and a current state optimal estimated covariance of the tunneling equipment;
and controlling the motion state of the tunneling equipment according to the optimal estimation value of the current state and the optimal estimation covariance of the current state.
2. The extended kalman filter-based roadway real-time positioning and navigation method according to claim 1, wherein the acquiring of the initial state parameters of the tunneling device in the roadway specifically comprises:
and acquiring the space coordinate, the attitude angle, the walking coefficient, the steering coefficient, the pitching correction angle of the walking track relative to the vehicle body and the course correction angle of the walking track relative to the vehicle body of the tunneling device.
3. The extended kalman filter-based roadway real-time positioning and navigation method according to claim 2, wherein obtaining a conversion relationship of the roadway with respect to a geographic coordinate system specifically comprises:
and acquiring the conversion relation of the roadway relative to a geographic coordinate system through a total station erected at the road junction and a prism installed on the tunneling equipment.
4. The extended kalman filter-based roadway real-time positioning and navigation method according to claim 3, wherein the obtaining of the space coordinates of the heading equipment specifically comprises:
acquiring the longitudinal position of the tunneling equipment in a roadway through an ultra-wideband positioning assembly arranged on a roadway opening and the tunneling equipment;
and acquiring the position of the tunneling equipment in the roadway through the total station and the prism.
5. The extended Kalman filtering-based roadway real-time positioning navigation method according to claim 4, characterized in that the obtaining of the attitude angle of the tunneling device specifically comprises:
and acquiring the attitude angle of the tunneling equipment through an inertial navigator arranged on the tunneling equipment.
6. The extended kalman filter-based roadway real-time positioning and navigation method according to claim 5, wherein the predicting and updating of the odometer travel model by using the extended kalman filter algorithm with the relevant parameters in the odometer travel model as filter state variables specifically comprises:
by the state transition equation:
calculating a state one-step predicted value;
wherein the content of the first and second substances,
for a one-step prediction of state, mutIs the state value u of the tunneling equipment at the moment tt+1＝[θ1,θ2]T，Wt+1For system noise, T is the transpose of the matrix, θ1For left-hand walking encoder increments, θ2Is the increment of the right-side walking encoder, and g is a functional relation;
and by the formula:
calculating the one-step prediction covariance of the state;
wherein the content of the first and second substances,
one-step prediction of covariance for state, Gt+1Is g (u)t+1,μt) At mutPartial derivatives of (E), sigmatAs an initial covariance, Gt+1 TIs Gt+1Transpose of Rt+1Is a preset constant.
7. The extended kalman filter-based roadway real-time positioning navigation method of claim 6, wherein the step-by-step prediction value of the state and the step-by-step prediction covariance of the state are measured and updated by using an extended kalman filter algorithm according to each current state parameter, specifically comprising:
by the formula:
calculating Kalman filtering gain;
wherein, Kt+1As a Kalman filter gain, Ht+1For measuring the matrix, Ht+1 TIs Ht+1Transpose of (Q)t+1Is a measurement error vector;
and by the formula:
calculating the optimal estimation value of the current state of the tunneling equipment;
wherein, mut+1For optimal estimation of the current state of the tunnelling apparatus, Dt+1Predicting an error for the measurement;
and then through the formula:
calculating the optimal estimation covariance of the current state of the tunneling equipment;
wherein, sigmat+1And (3) optimally estimating the covariance for the current state of the tunneling equipment, wherein I is a unit matrix.
8. The extended kalman filter-based roadway real-time positioning navigation method according to claim 7, wherein the step-by-step prediction value of the state and the step-by-step prediction covariance of the state are measured and updated by using an extended kalman filter algorithm according to each current state parameter, specifically comprising:
and measuring and updating the state one-step prediction value and the state one-step prediction covariance through the measurement matrix, the measurement prediction error and the measurement error vector of the inertial navigator, the ultra-wideband positioning assembly and the total station respectively.
9. The utility model provides a tunnel real time positioning navigation based on extended Kalman filtering which characterized in that includes:
the initial acquisition module is used for acquiring initial state parameters of the tunneling equipment in a roadway and a conversion relation of the roadway relative to a geographic coordinate system;
the model establishing module is used for establishing an odometer walking model by using the initial state parameters and the conversion relation through a model-based linear approximation method;
the prediction updating module is used for predicting and updating the odometer walking model by using an extended Kalman filtering algorithm and taking related parameters in the odometer walking model as filtering state variables so as to obtain a state one-step prediction value and a state one-step prediction covariance of the tunneling equipment;
the measurement updating module is used for acquiring current state parameters of the tunneling equipment in a roadway, and measuring and updating the state one-step predicted value and the state one-step predicted covariance according to the current state parameters by using an extended Kalman filtering algorithm so as to obtain a current state optimal estimated value and a current state optimal estimated covariance of the tunneling equipment;
and the control output module is used for controlling the motion state of the tunneling equipment according to the optimal estimation value of the current state and the optimal estimation covariance of the current state.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011424015.2A CN112683268A (en) | 2020-12-08 | 2020-12-08 | Roadway real-time positioning navigation method and system based on extended Kalman filtering |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011424015.2A CN112683268A (en) | 2020-12-08 | 2020-12-08 | Roadway real-time positioning navigation method and system based on extended Kalman filtering |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN112683268A true CN112683268A (en) | 2021-04-20 |
Family
ID=75446339
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011424015.2A Pending CN112683268A (en) | 2020-12-08 | 2020-12-08 | Roadway real-time positioning navigation method and system based on extended Kalman filtering |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112683268A (en) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113252044A (en) * | 2021-05-25 | 2021-08-13 | 中国煤炭科工集团太原研究院有限公司 | Method for calculating deviation of tunneling equipment body |
| CN114166508A (en) * | 2021-12-09 | 2022-03-11 | 中国铁建重工集团股份有限公司 | Method for acquiring load distribution state of multi-row roller turntable bearing in real time |
| CN114323003A (en) * | 2021-12-27 | 2022-04-12 | 青岛慧拓智能机器有限公司 | Underground mine fusion positioning method based on UMB, IMU and laser radar |
| CN114543795A (en) * | 2021-12-31 | 2022-05-27 | 文远苏行(江苏)科技有限公司 | Installation error estimation method and adjustment method for double-antenna course angle and related equipment |
| CN114777749A (en) * | 2022-04-26 | 2022-07-22 | 华中科技大学 | Position and attitude measurement method, system, medium, equipment and terminal of development machine |
| CN116222578A (en) * | 2023-05-04 | 2023-06-06 | 山东大学 | Underwater integrated navigation method and system based on self-adaptive filtering and optimal smoothing |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105588566A (en) * | 2016-01-08 | 2016-05-18 | 重庆邮电大学 | Indoor positioning system and method based on Bluetooth and MEMS (Micro-Electro-Mechanical Systems) fusion |
| CN108180925A (en) * | 2017-12-15 | 2018-06-19 | 中国船舶重工集团公司第七0七研究所 | A kind of odometer assists vehicle-mounted dynamic alignment method |
| CN108871325A (en) * | 2018-06-26 | 2018-11-23 | 北京航空航天大学 | A kind of WiFi/MEMS combination indoor orientation method based on two layers of Extended Kalman filter |
| CN110296709A (en) * | 2019-07-23 | 2019-10-01 | 南京邮电大学 | Vehicle mounted positioning navigation method based on adaptive odometer model |
| CN110375730A (en) * | 2019-06-12 | 2019-10-25 | 深圳大学 | The indoor positioning navigation system merged based on IMU and UWB |
| CN110849374A (en) * | 2019-12-03 | 2020-02-28 | 中南大学 | Underground environment positioning method, device, equipment and storage medium |
| US10578743B1 (en) * | 2018-12-27 | 2020-03-03 | Intel Corporation | System and method of location determination using multiple location inputs |
| CN111256727A (en) * | 2020-02-19 | 2020-06-09 | 广州蓝胖子机器人有限公司 | Method for improving accuracy of odometer based on Augmented EKF |
-
2020
- 2020-12-08 CN CN202011424015.2A patent/CN112683268A/en active Pending
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105588566A (en) * | 2016-01-08 | 2016-05-18 | 重庆邮电大学 | Indoor positioning system and method based on Bluetooth and MEMS (Micro-Electro-Mechanical Systems) fusion |
| CN108180925A (en) * | 2017-12-15 | 2018-06-19 | 中国船舶重工集团公司第七0七研究所 | A kind of odometer assists vehicle-mounted dynamic alignment method |
| CN108871325A (en) * | 2018-06-26 | 2018-11-23 | 北京航空航天大学 | A kind of WiFi/MEMS combination indoor orientation method based on two layers of Extended Kalman filter |
| US10578743B1 (en) * | 2018-12-27 | 2020-03-03 | Intel Corporation | System and method of location determination using multiple location inputs |
| CN110375730A (en) * | 2019-06-12 | 2019-10-25 | 深圳大学 | The indoor positioning navigation system merged based on IMU and UWB |
| CN110296709A (en) * | 2019-07-23 | 2019-10-01 | 南京邮电大学 | Vehicle mounted positioning navigation method based on adaptive odometer model |
| CN110849374A (en) * | 2019-12-03 | 2020-02-28 | 中南大学 | Underground environment positioning method, device, equipment and storage medium |
| CN111256727A (en) * | 2020-02-19 | 2020-06-09 | 广州蓝胖子机器人有限公司 | Method for improving accuracy of odometer based on Augmented EKF |
Non-Patent Citations (3)
| Title |
|---|
| 孟祥萍等: "自适应扩展卡尔曼滤波器在移动机器人定位中的应用", 《计算机系统应用》 * |
| 张胜宾等: "基于卡尔曼滤波的轮式移动机器人定位实验研究", 《机电工程技术》 * |
| 谢海天: "基于捷联惯导的电缆隧道巡检机器人定位技术研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113252044A (en) * | 2021-05-25 | 2021-08-13 | 中国煤炭科工集团太原研究院有限公司 | Method for calculating deviation of tunneling equipment body |
| CN114166508A (en) * | 2021-12-09 | 2022-03-11 | 中国铁建重工集团股份有限公司 | Method for acquiring load distribution state of multi-row roller turntable bearing in real time |
| CN114323003A (en) * | 2021-12-27 | 2022-04-12 | 青岛慧拓智能机器有限公司 | Underground mine fusion positioning method based on UMB, IMU and laser radar |
| CN114543795A (en) * | 2021-12-31 | 2022-05-27 | 文远苏行(江苏)科技有限公司 | Installation error estimation method and adjustment method for double-antenna course angle and related equipment |
| CN114543795B (en) * | 2021-12-31 | 2024-01-02 | 文远苏行(江苏)科技有限公司 | Installation error estimation method and adjustment method for dual-antenna course angle and related equipment |
| CN114777749A (en) * | 2022-04-26 | 2022-07-22 | 华中科技大学 | Position and attitude measurement method, system, medium, equipment and terminal of development machine |
| CN116222578A (en) * | 2023-05-04 | 2023-06-06 | 山东大学 | Underwater integrated navigation method and system based on self-adaptive filtering and optimal smoothing |
| CN116222578B (en) * | 2023-05-04 | 2023-08-29 | 山东大学 | Underwater integrated navigation method and system based on self-adaptive filtering and optimal smoothing |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112683268A (en) | Roadway real-time positioning navigation method and system based on extended Kalman filtering | |
| Milanés et al. | Autonomous vehicle based in cooperative GPS and inertial systems | |
| CN105509738B (en) | Vehicle positioning orientation method based on inertial navigation/Doppler radar combination | |
| CN110702091B (en) | High-precision positioning method for moving robot along subway rail | |
| US5983166A (en) | Structure measurement system | |
| CN107132563B (en) | Combined navigation method combining odometer and dual-antenna differential GNSS | |
| EP0544403A1 (en) | Vehicle navigation system | |
| CN108931244B (en) | Inertial navigation error suppression method and system based on train motion constraint | |
| CN103335647B (en) | A kind of attitude of shield machine measuring system and measuring method thereof | |
| CN111637888B (en) | Tunneling machine positioning method and system based on inertial navigation and laser radar single-point distance measurement | |
| CN110542417A (en) | gyroscope linear measurement method and system based on static and dynamic inclinometer correction | |
| CN112628524B (en) | High-precision positioning method for small-diameter pipeline robot based on turning angle | |
| CN114166221B (en) | Auxiliary transportation robot positioning method and system in dynamic complex mine environment | |
| CN112014849A (en) | Unmanned vehicle positioning correction method based on sensor information fusion | |
| CN114838732B (en) | Collaborative navigation method based on graph optimization under communication limited environment | |
| Bouvet et al. | A real-time localization system for compactors | |
| CN112229422A (en) | Speedometer quick calibration method and system based on FPGA time synchronization | |
| CN114136275A (en) | Track line state detection device and roadbed settlement detection method | |
| CN113236363A (en) | Mining equipment navigation positioning method, system, equipment and readable storage medium | |
| CN112229392B (en) | High-redundancy indoor coal yard navigation method and system | |
| CN111811467B (en) | Dynamic detection method and device for long-wave differential settlement of high-speed railway rail surface | |
| CN110333523B (en) | Track line three-dimensional data generation method for RTG automatic walking system | |
| CN115655268A (en) | Underground mine heading machine navigation positioning method based on graph optimization algorithm | |
| CN110864688A (en) | Attitude heading method for vehicle-mounted azimuth open-loop horizontal attitude angle closed loop | |
| CN114485614B (en) | Navigation positioning system and method of mining equipment based on double total stations |
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 | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20210420 |