CN114777749A - Position and attitude measurement method, system, medium, equipment and terminal of development machine - Google Patents
Position and attitude measurement method, system, medium, equipment and terminal of development machine Download PDFInfo
- Publication number
- CN114777749A CN114777749A CN202210446784.5A CN202210446784A CN114777749A CN 114777749 A CN114777749 A CN 114777749A CN 202210446784 A CN202210446784 A CN 202210446784A CN 114777749 A CN114777749 A CN 114777749A
- Authority
- CN
- China
- Prior art keywords
- prisms
- prism
- cos
- heading machine
- measured
- 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
- 238000000691 measurement method Methods 0.000 title abstract description 8
- 238000011161 development Methods 0.000 title abstract description 5
- 238000000034 method Methods 0.000 claims abstract description 55
- 238000005259 measurement Methods 0.000 claims abstract description 54
- 238000010276 construction Methods 0.000 claims abstract description 16
- 238000001914 filtration Methods 0.000 claims description 23
- 238000004422 calculation algorithm Methods 0.000 claims description 15
- 101000802640 Homo sapiens Lactosylceramide 4-alpha-galactosyltransferase Proteins 0.000 claims description 6
- 102100035838 Lactosylceramide 4-alpha-galactosyltransferase Human genes 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 6
- 238000013461 design Methods 0.000 claims description 5
- 230000001133 acceleration Effects 0.000 claims description 4
- 238000004590 computer program Methods 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 claims description 3
- 238000012937 correction Methods 0.000 claims description 3
- 230000006870 function Effects 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 230000008569 process Effects 0.000 description 7
- 238000010586 diagram Methods 0.000 description 5
- 230000000694 effects Effects 0.000 description 4
- 230000005641 tunneling Effects 0.000 description 4
- 238000004088 simulation Methods 0.000 description 3
- 238000003491 array Methods 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 230000009191 jumping Effects 0.000 description 2
- 230000008092 positive effect Effects 0.000 description 2
- 238000012827 research and development Methods 0.000 description 2
- 238000005096 rolling process Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 239000006185 dispersion Substances 0.000 description 1
- 239000000428 dust Substances 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000011435 rock Substances 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C15/00—Surveying instruments or accessories not provided for in groups G01C1/00 - G01C13/00
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Length Measuring Devices With Unspecified Measuring Means (AREA)
Abstract
The invention belongs to the technical field of construction of development machines, and discloses a method, a system, a medium, equipment and a terminal for measuring position and attitude of a development machine, wherein after prisms 1 to n-1 are measured, coordinates of the prisms 1 to n-1 when the prisms n are measured are predicted when the prisms n are measured; reading 3 attitude angles output by the angle sensor at the moment, and solving the position and posture of the heading machine by using a simultaneous equation set of the predicted coordinates of the prisms 1-n-1 and the measured coordinate of the prism n. The invention provides a position and attitude measurement method of a heading machine based on multi-prism track prediction, which aims to solve the problem of low multi-prism attitude measurement precision in heading machine construction. Compared with an unpredicted model, the method for measuring the position and the attitude of the heading machine based on the multi-prism track prediction can obviously improve the measurement precision and the measurement frequency. The invention solves the problem of position error caused by asynchronous prism measurement and improves the accuracy of pose measurement.
Description
Technical Field
The invention belongs to the technical field of measurement and control, and particularly relates to a method, a system, a medium, equipment and a terminal for measuring the position and the attitude of a heading machine.
Background
At present, a polygon prism method is a common method for measuring the pose of a tunnel boring machine. Before tunnel construction begins, a tunnel boring machine manufacturer installs a plurality of prisms on a boring machine, and measures the position relation between the prisms and the center of the boring machine in the manufacturer. The position relation between the prisms and the center of the heading machine cannot be changed in the construction process. In the tunneling process, the tunneling machine moves along the direction of a design axis, and a measuring responsible person of a tunnel construction project firstly installs a total station and a rearview prism on a hanging basket with a known coordinate position; then, measuring the plurality of prisms in sequence by using a total station, and calculating to obtain the three-dimensional coordinate of each prism; the coordinates of the center of the heading machine under the designed line can be calculated by combining the prism coordinates obtained by the in-plant measurement and the construction measurement with the head and tail coordinates of the heading machine.
However, the following problems exist in practical use: in the construction process of the heading machine, the heading speed fluctuates within a range due to different hardness of surrounding rocks, and the heading speed is generally within the range of 10 mm/min-120 mm/min. The total station measuring prism comprises the steps of searching, aiming, measuring and the like, is influenced by ambient light, dust, the rotating speed of a motor of the total station and the like, and the time spent on measuring one prism is generally 10-30 s. The total station is required to measure each prism in turn during the whole measurement process, which causes three problems. The first problem is that the coordinates of three prisms are observed only by a general multi-prism method in a tunnel, redundant observed quantity is lacked, and once a certain coordinate measurement error is large, great influence is brought to final pose resolving precision. The second problem is that the first prism is displaced when the first prism is measured and the second prism is measured, which can cause the measured prism coordinates to have time-space mismatch. The third problem is that the overall measurement period is long, generally 1-2 min, and the posture is not updated timely.
Through the above analysis, the problems and defects of the prior art are as follows:
(1) in a tunnel, a general multi-prism method only observes coordinates of three prisms, redundant observation quantity is lacked, and once a certain coordinate measurement error is large, the final pose resolving precision is greatly influenced.
(2) When the first prism is measured and the second prism is measured, the first prism is displaced, and the measured prism coordinates have time-space mismatching. Serious errors are caused when pose solution is performed by these unmatched measurement data.
(3) The whole measurement period is long, generally 1-2 min, and the posture is not updated timely.
Disclosure of Invention
The invention provides a method, a system, a medium, equipment and a terminal for measuring the position and the attitude of a heading machine, and particularly relates to a method, a system, a medium, equipment and a terminal for measuring the position and the attitude of the heading machine based on multi-prism track prediction.
The invention is realized in such a way that the position and posture measuring method of the development machine comprises the following steps:
after the prisms 1 to n-1 are measured, predicting coordinates of the prisms 1 to n-1 when the prisms n are measured; reading 3 attitude angles output by the angle sensor at the moment, and solving the position and posture of the heading machine by using a simultaneous equation set of the predicted coordinates of the prisms 1-n-1 and the measured coordinate of the prism n.
Further, the heading machine position and posture measuring method further comprises the following steps:
installing n prisms and m attitude angle sensors on the heading machine; the total station measures the coordinates of 1-n prisms in sequence, predicts the coordinates of the 1-n-1 prisms at the moment of measuring the n prisms after the nth prism measurement is completed, and unifies the coordinates to the same coordinate system at the same moment; and the prediction algorithm adopts a segmented Kalman filtering algorithm for prediction, and the prediction algorithm is used for sequentially matching the linearity of a design line where the heading machine is located, establishing a state equation, fusing the state equation with an observed value and performing Kalman filtering prediction.
Further, the heading machine position and attitude measuring method comprises the following steps:
the method comprises the following steps of firstly, sequentially measuring coordinates of a plurality of prisms on the tunneling machine by using a total station, and measuring a plurality of attitude angles by using an angle sensor arranged on the tunneling machine;
predicting coordinates of the prisms at the same moment by using a segmented Kalman filtering algorithm;
and step three, establishing a measurement equation set by adopting a coordinate conversion model, and solving the pose by using a least square method.
Further, the plurality of prisms may be 2 prisms or more, and preferably the number of prisms is 3.
The angle sensor adopts a sensor such as an inclination angle sensor or a gyroscope, and the attitude angle uses 1, 2 or 3 attitude angles, preferably 3 attitude angles.
And the state equation is determined according to the line type of the design line section where the heading machine is located at present, and is determined according to the line section, the relaxation curve section and the circular curve section respectively.
Further, the heading machine position and attitude measuring method further comprises the following steps:
(1) constructing equations of state
1) Equation of state of straight line
X(k+1)＝ΦX(k)+ΓW(k)；
X＝[N E H dl v a]T；
2) Equation of state of the easement curve
Wherein,
X＝[N E H l dl v a θ dθ β]T；
M11＝cos(θk)·cos(βk)·cos(α)-sin(θk)·cos(βk)·sin(α)；
M12＝-dlk·sin(θk)·cos(βk)·cos(α)-dlk·cos(θk)·cos(βk)·sin(α)；
M12＝-dlk·cos(θk)·sin(βk)·cos(α)+dlk·cos(θk)·sin(βk)·sin(α)；
M21＝cos(θk)·cos(βk)·sin(α)+sin(θk)·cos(βk)·cos(α)；
M22＝-dlk·sin(θk)·cos(βk)·cos(α)+dlk·cos(θk)·cos(βk)·cos(α)；
M23＝-dlk·cos(θk)·sin(βk)·cos(α)-dlk·sin(θk)·sin(βk)·cos(α)；
3) equation of state of circular curve
The circular curve is similar in structure to the easement curve, and only the angular increment calculation differs, so there is:
M31＝0；
n is a north coordinate, E is an east coordinate, H is an elevation, dL is an oil cylinder increment, alpha is an azimuth angle of a designed line, T is a measurement period, beta is a pitch angle, alpha is the azimuth angle, and L is an advancing distance; the velocity v and the acceleration a are obtained by measurement.
(2) Construction of an Observation equation
The heading machine advancing distance is measured through an oil cylinder stroke sensor, the prism coordinate is measured through a total station, and the stroke of the oil cylinder stroke sensor and the prism coordinate measured through the total station are used as observed quantities.
And (3) taking the prism coordinate measured by the total station and the oil cylinder stroke measured by the oil cylinder stroke sensor as observed values, and listing an observation equation:
Z＝[N E H L]T；
(3) predictive update
1) The straight line segment is directly predicted by kalman filtering:
P(k+1|k)＝φ(k+1|k)P(k)φT(k+1|k)+Q(k+1)；
K(k+1)＝P(k+1|k)HT(k+1)[H(k+1)P(k+1|k)HT(k+1)+R(k+1)]-1；
P(k+1)＝[I-K(k+1)H(k+1)]P(k+1|k)；
2) for the relaxation curve and the circular curve segment, because a nonlinear function exists, the linear model is predicted by adopting an extended kalman, and then:
P(k+1|k)＝Φ(k+1|k)P(k)ΦT(k+1|k)+Q(k+1)；
K(k+1)＝P(k+1|k)HT(k+1)[H(k+1)P(k+1|k)HT(k+1)+R(k+1)]-1；
P(k+1)＝[I-K(k+1)H(k+1)]P(k+1|k)。
further, the heading machine position and attitude measuring method further comprises the following steps:
measuring m attitude angles of the heading machine by an angle sensor, and establishing a simultaneous equation set according to the predicted prism set coordinates and attitude angles; and solving an optimal solution of error correction by taking a least square criterion as an estimation condition.
Another object of the present invention is to provide a position and orientation measuring system of a heading machine using the method of measuring position and orientation of a heading machine, the system comprising:
the attitude angle measuring module is used for sequentially measuring the coordinates of a plurality of prisms on the heading machine by using a total station and measuring a plurality of attitude angles by using an angle sensor arranged on the heading machine;
the prism coordinate prediction module is used for predicting the coordinates of the prisms at the same moment by using a segmented Kalman filtering algorithm;
and the pose measurement module is used for establishing a measurement equation set by adopting a coordinate conversion model and solving the pose by utilizing a least square method.
It is a further object of the invention to provide a computer device comprising a memory and a processor, the memory storing a computer program which, when executed by the processor, causes the processor to perform the steps of:
after the prisms 1 to n-1 are measured, predicting coordinates of the prisms 1 to n-1 when the prisms n are measured; reading 3 attitude angles output by the angle sensor at the moment, and solving the position and posture of the heading machine by using a simultaneous equation set of the predicted coordinates of the prisms 1-n-1 and the measured coordinate of the prism n.
It is another object of the present invention to provide a computer-readable storage medium storing a computer program which, when executed by a processor, causes the processor to perform the steps of:
after the prisms 1 to n-1 are measured, predicting coordinates of the prisms 1 to n-1 when the prisms n are measured; reading 3 attitude angles output by the angle sensor at the moment, and solving the position and attitude of the heading machine by using a simultaneous equation set of the predicted coordinates of the prisms 1-n-1 and the measured coordinate of the prism n.
Another object of the present invention is to provide an information data processing terminal for implementing the heading machine pose measurement system.
In combination with the technical solutions and the technical problems to be solved, please analyze the advantages and positive effects of the technical solutions to be protected in the present invention from the following aspects:
first, aiming at the technical problems existing in the prior art and the difficulty in solving the problems, the technical problems to be solved by the technical scheme of the present invention are closely combined with the technical scheme to be protected and the results and data in the research and development process, and some creative technical effects brought after the problems are solved are analyzed in detail and deeply. The specific description is as follows:
the invention provides a position and attitude measurement method of a heading machine based on multi-prism track prediction, which aims to solve the problem of low multi-prism attitude measurement precision in heading machine construction. According to the position of a designed line where the heading machine is located, state equations of a straight line section, a easement curve section and a circular curve section are respectively established, Kalman filtering is carried out on the state equations and measured prism coordinates, and the prism coordinates at the same moment are predicted. And then, with the minimum prism error as a target, performing least square pose solution on the plurality of prism coordinates and the plurality of attitude angles. Compared with the existing polygon prism method, the method for measuring the position and the attitude of the heading machine based on the polygon prism track prediction can obviously improve the measurement precision and the measurement frequency.
Secondly, considering the technical scheme as a whole or from the perspective of products, the technical effect and advantages of the technical scheme to be protected by the invention are specifically described as follows:
the invention solves the position error caused by the asynchronous prism measurement and improves the pose measurement precision.
Third, as inventive supplementary proof of the claims of the present invention, there are several important aspects as follows:
(1) the technical scheme of the invention fills the technical blank in the industry at home and abroad:
the existing heading machine at home and abroad is provided with a multi-prism guide system, the measurement error is large during the construction of the heading machine, and accurate pose measurement can be carried out only when the heading machine is stopped. The invention can realize dynamic and accurate measurement of the position and posture of the heading machine during heading construction.
(2) The technical scheme of the invention solves the technical problem that people are eagerly to solve but can not be successfully solved all the time: in order to measure the accurate pose during the construction of the heading machine adopting the multi-prism method guide system, the heading machine is usually stopped to measure the pose so as to ensure the measurement precision, which seriously influences the construction efficiency. The dynamic, real-time and accurate measurement of the position and posture of the heading machine is an important problem in the multi-prism heading machine position and posture measurement. The invention solves the problem of measurement error caused by movement of the heading machine during the measurement by a polygon prism method, can realize real-time dynamic measurement of the pose of the polygon prism heading machine, and can realize the improvement of the construction efficiency.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required to be used in the embodiments of the present invention will be briefly described below, and it is obvious that the drawings described below are only some embodiments of the present invention, and it is obvious for those skilled in the art that other drawings can be obtained according to the drawings without creative efforts.
Fig. 1 is a flow chart of a heading machine position and attitude measurement method provided by an embodiment of the invention;
FIG. 2 is a schematic diagram of a heading machine position and attitude measurement method provided by an embodiment of the invention;
fig. 3 is a structural block diagram of a heading machine position and attitude measurement system provided by an embodiment of the invention;
FIG. 4 is a flow diagram of a piecewise Kalman filter prediction provided by an embodiment of the present invention;
FIG. 5 is a simulation result of a prior art scheme provided by an embodiment of the present invention;
fig. 6 is a simulation result of the present invention provided by the embodiment of the present invention.
In the figure: 1. an attitude angle measurement module; 2. a prism coordinate prediction module; 3. and a pose measurement module.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is further described in detail with reference to the following embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
Aiming at the problems in the prior art, the invention provides a method, a system, a medium, equipment and a terminal for measuring the position and the attitude of a heading machine, and the invention is described in detail by combining the attached drawings.
First, an embodiment is explained. This section is an illustrative example developed to explain the claims in order to enable those skilled in the art to fully understand how to implement the present invention.
As shown in fig. 1, the heading machine position and attitude measurement method provided by the embodiment of the invention comprises the following steps:
s101, sequentially measuring coordinates of a plurality of prisms on the heading machine by using a total station, and measuring a plurality of attitude angles by using an angle sensor arranged on the heading machine;
s102, predicting coordinates of a plurality of prisms at the same moment by using a piecewise Kalman filtering algorithm;
and S103, establishing a measurement equation set by adopting a coordinate conversion model, and solving the pose by using a least square method.
A schematic diagram of a heading machine attitude and position measuring method provided by the embodiment of the invention is shown in fig. 2.
As shown in fig. 3, the heading machine position and posture measuring system provided by the embodiment of the present invention includes:
the attitude angle measuring module 1 is used for sequentially measuring the coordinates of a plurality of prisms on the heading machine by using a total station and measuring a plurality of attitude angles by using an angle sensor arranged on the heading machine;
the prism coordinate prediction module 2 is used for predicting the coordinates of a plurality of prisms at the same moment by using a segmented Kalman filtering algorithm;
and the pose measurement module 3 is used for establishing a measurement equation set by adopting a coordinate conversion model and solving the pose by using a least square method.
The flow chart of the piecewise kalman filtering prediction provided by the embodiment of the present invention is shown in fig. 4.
The technical solution of the present invention is further described below with reference to specific examples.
The invention provides a method for measuring the position and the attitude of a heading machine based on multi-prism track prediction, which is used for solving the technical problem of low multi-prism measurement precision of the existing heading machine.
The technical scheme for solving the technical problems is as follows:
the heading machine is provided with 3 prisms and 2 attitude angle sensors. And the total station measures the coordinates of 1-3 prisms in sequence, predicts the coordinates of the prisms 1-n-1 at the moment of measuring the prism n after the measurement of the prism n is completed, and unifies the coordinates of the prisms into the same coordinate system at the same moment. The prediction algorithm can adopt a segmented Kalman filtering algorithm for prediction, the prediction algorithm is to sequentially match the linearity of a design line where the heading machine is located, establish a state equation, and then fuse the state equation with an observed value to perform Kalman filtering prediction. Wherein,
1. equation of state
1) Equation of state of straight line
X(k+1)＝ΦX(k)+ΓW(k) (1)
X＝[N E H dl v a] T (2)
2) Equation of state of the relaxation curve
Wherein,
X＝[N E H l dl v a θ dθ β] T (5)
M11＝cos(θk)·cos(βk)·cos(α)-sin(θk)·cos(βk)·sin(α) (7)
M12＝-dlk·sin(θk)·cos(βk)·cos(α)-dlk·cos(θk)·cos(βk)·sin(α) (8)
M12＝-dlk·cos(θk)·sin(βk)·cos(α)+dlk·cos(θk)·sin(βk)·sin(α) (9)
M21＝cos(θk)·cos(βk)·sin(α)+sin(θk)·cos(βk)·cos(α) (10)
M22＝-dlk·sin(θk)·cos(βk)·cos(α)+dlk·cos(θk)·cos(βk)·cos(α) (11)
M23＝-dlk·cos(θk)·sin(βk)·cos(α)-dlk·sin(θk)·sin(βk)·cos(α) (12)
3) equation of state of circular curve
The circular curve is similar in structure to the easement curve, and only the angle increment calculation differs, so there is a difference for equation 4:
M31＝0 (15)
wherein N is a north coordinate, E is an east coordinate, H is an elevation, dL is an oil cylinder increment, alpha is an azimuth angle of a designed line, T is a measurement period, beta is a pitch angle, alpha is an azimuth angle, and L is an advancing distance. The velocity v and the acceleration a can be obtained by measurement.
2. Equation of observation
The advancing distance of the heading machine can be measured by an oil cylinder stroke sensor, and the coordinates of the prism can be measured by a total station, so that the stroke of the oil cylinder stroke sensor and the coordinates of the prism measured by the total station are used as observed quantities.
And (4) taking the prism coordinates measured by the total station and the oil cylinder stroke measured by the oil cylinder stroke sensor as observed values. The observation equation can be listed:
Z＝[N E H L]T (17)
3. predictive update
1) The straight line segment can be directly predicted by adopting Kalman filtering
P(k+1|k)＝φ(k+1|k)P(k)φT(k+1|k)+Q(k+1) (21)
K(k+1)＝P(k+1|k)HT(k+1)[H(k+1)P(k+1|k)HT(k+1)+R(k+1)]-1 (22)
P(k+1)＝[I-K(k+1)H(k+1)]P(k+1|k)
(24)
2) For the relaxation curve and the circular curve segment, because a nonlinear function exists, the linearized model is predicted by adopting an extended kalman method, and the method comprises the following steps:
P(k+1|k)＝Φ(k+1|k)P(k)ΦT(k+1|k)+Q(k+1) (26)
K(k+1)＝P(k+1|k)HT(k+1)[H(k+1)P(k+1|k)HT(k+1)+R(k+1)]-1 (27)
P(k+1)＝[I-K(k+1)H(k+1)]P(k+1|k) (29)
the method considers the change of the speed and the acceleration of the track, and can accurately predict the coordinates of the prism.
Meanwhile, the inclination angle sensor measures 2 attitude angles of the heading machine. And establishing a simultaneous equation set according to the predicted prism set coordinates and attitude angles. And solving an error correction optimal solution by taking a least square criterion as an estimation condition.
The invention provides a position and attitude measurement method of a heading machine based on multi-prism track prediction, aiming at solving the problem of low multi-prism attitude measurement precision in heading machine construction.
Example 2 measuring prism coordinates and attitude angle with a Total station and a Gyroscope
In this example, as shown in fig. 2, a total station is used to measure 3 prisms and a gyroscope to measure 3 attitude angles.
1. The total station first measures prism No. 1 at position 1, then prism No. 2 at position 2, and then the coordinates of prism No. 3 at position 3. And 3 attitude angles output by the gyroscope are read when the coordinate of the No. 3 prism is measured.
And (3) predicting the coordinates of the prism No. 1 and the prism No. 2 when the prism No. 3 is measured by adopting a piecewise Kalman filtering prediction method. And then, simultaneously establishing an equation set by the predicted coordinates of the prism 1 and the prism 2 and the measured coordinate of the prism 3, and solving the position and the pose of the heading machine.
2. And the total station measures the coordinates of the prism 1 at the position 4 again, and reads 3 attitude angles output by the gyroscope. And (3) predicting the coordinates of the No. 2 prism and the No. 3 prism when the No. 1 prism is measured by adopting a piecewise Kalman filtering prediction method. And then, the predicted coordinates of the prism 2 and the prism 3 and the measured coordinate of the prism 1 are combined together to form an equation set, and the position and the attitude of the heading machine are solved.
3. And the total station measures the coordinates of the prism 2 at the position 5 again, and reads 3 attitude angles output by the gyroscope. And (3) predicting the coordinates of the prism No. 1 and the prism No. 3 when the prism No. 2 is measured by adopting a piecewise Kalman filtering prediction method. And then, simultaneously establishing an equation set by the predicted coordinates of the prism 1 and the prism 3 and the measured coordinate of the prism 2, and solving the position and the pose of the heading machine.
4. And correcting the error of the dispersion of the gyroscope along with the time. And the position 6 is a state that the heading machine stops heading, at the moment, coordinates of the prism 1, the prism 2 and the prism 3 are measured in sequence, the position and the posture of the heading machine are solved, and the calculated attitude angle is used for calibrating the angle of the gyroscope again.
5. The next measurement cycle is continued.
And II, application embodiment. In order to prove the creativity and the technical value of the technical scheme of the invention, the part is the application example of the technical scheme of the claims on specific products or related technologies.
Application example
Measuring prism coordinates using total station only
This example uses a total station to measure 3 prism coordinates.
1. The total station first measures prism No. 1 at position 1, then prism No. 2 at position 2, and then the coordinates of prism No. 3 at position 3. And (3) predicting the coordinates of the prism No. 1 and the prism No. 2 when the prism No. 3 is measured by adopting a piecewise Kalman filtering prediction method. And then, simultaneously establishing an equation set by the predicted coordinates of the prism 1 and the prism 2 and the measured coordinate of the prism 3, and solving the position and the pose of the heading machine.
2. The total station then measures prism coordinates No. 1 at position 4. And (3) predicting the coordinates of the No. 2 prism and the No. 3 prism when the No. 1 prism is measured by adopting a piecewise Kalman filtering prediction method. And then, the predicted coordinates of the prism 2 and the prism 3 and the measured coordinate of the prism 1 are combined together to form an equation set, and the position and the attitude of the heading machine are solved.
3. The total station then measures prism coordinates No. 2 at position 5. And (3) predicting the coordinates of the prism No. 1 and the prism No. 3 when the prism No. 2 is measured by adopting a piecewise Kalman filtering prediction method. And then, the predicted coordinates of the prism 1 and the prism 3 and the measured coordinate of the prism 2 are combined together to form an equation set, and the position and the attitude of the heading machine are solved.
4. The next measurement cycle is continued.
And thirdly, evidence of relevant effects of the embodiment. The embodiment of the invention achieves some positive effects in the process of research and development or use, and has great advantages compared with the prior art, and the following contents are described by combining data, diagrams and the like in the test process.
To verify the effect of the present invention, the simulation experiment was performed as follows:
the sample data for simulating and generating a section of three prisms and three attitude angles comprises a straight line section, a gentle curve section and a circular curve section. Wherein the straight line section is 29m long, the gentle curve section is 50m long, the circular curve section is 40m long, and the turning radius is 200 m. A comparison of the results using the prior art scheme and the present invention, respectively, is shown in fig. 5 and fig. 6.
It can be seen that the error of the existing scheme is always large, especially the error is gradually amplified in the curve segment. The scheme of the invention has small errors as a whole.
(1) In the existing scheme, the initial speed of the N coordinate deviation is about 0mm, but the initial speed is increased to 20-30 mm quickly; e, the coordinate deviation is about 5mm in the straight line section, and is increased to 8-12 mm after reaching the curve section; the deviation of the H coordinate is continuously jumping at-4-5 mm. The azimuth angle and the pitch angle jump within-0.2 degrees, and the rolling angle jumps within-0.15 degrees.
(2) In the scheme of the invention, the deviation of the N coordinate, the E coordinate and the H coordinate is less than 2 mm. The deviation of the azimuth angle and the pitch angle is about 0.05 degrees of jumping, and the deviation of the rolling angle is less than 0.1 degree.
It should be noted that the embodiments of the present invention can be realized by hardware, software, or a combination of software and hardware. The hardware portions may be implemented using dedicated logic; the software portions may be stored in a memory and executed by a suitable instruction execution system, such as a microprocessor or specially designed hardware. Those skilled in the art will appreciate that the apparatus and methods described above may be implemented using computer executable instructions and/or embodied in processor control code, such code being provided on a carrier medium such as a disk, CD-or DVD-ROM, programmable memory such as read only memory (firmware), or a data carrier such as an optical or electronic signal carrier, for example. The apparatus of the present invention and its modules may be implemented by hardware circuits such as very large scale integrated circuits or gate arrays, semiconductors such as logic chips, transistors, or programmable hardware devices such as field programmable gate arrays, programmable logic devices, or software executed by various types of processors, or a combination of hardware circuits and software, e.g., firmware.
The above description is only for the purpose of illustrating the present invention and the appended claims are not to be construed as limiting the scope of the invention, which is intended to cover all modifications, equivalents and improvements that are within the spirit and scope of the invention as defined by the appended claims.
Claims (10)
1. The method for measuring the position and the posture of the heading machine is characterized by comprising the following steps of:
after the prisms 1 to n-1 are measured, predicting coordinates of the prisms 1 to n-1 when the prisms n are measured; reading 3 attitude angles output by the angle sensor at the moment, and solving the position and posture of the heading machine by using a simultaneous equation set of the predicted coordinates of the prisms 1-n-1 and the measured coordinate of the prism n.
2. The method of measuring the heading machine attitude of claim 1, further comprising:
installing n prisms and m attitude angle sensors on the heading machine; the total station sequentially measures the coordinates of 1-n prisms, predicts the coordinates of the prism 1-n-1 at the moment of measuring the prism n after the measurement of the prism n is completed, and unifies the coordinates to the same coordinate system at the same moment; and the prediction algorithm adopts a segmented Kalman filtering algorithm for prediction, and the prediction algorithm is used for sequentially matching the linearity of a design line where the heading machine is located, establishing a state equation, fusing the state equation with an observed value and performing Kalman filtering prediction.
3. The heading machine position and attitude measuring method according to claim 1, wherein the heading machine position and attitude measuring method comprises the following steps:
the method comprises the following steps that firstly, coordinates of a plurality of prisms on the heading machine are measured in sequence by using a total station, and a plurality of attitude angles are measured by using an angle sensor arranged on the heading machine;
predicting the coordinates of the prisms at the same moment by using a piecewise Kalman filtering algorithm;
and step three, establishing a measurement equation set by adopting a coordinate conversion model, and solving the pose by using a least square method.
4. The heading machine attitude and orientation measuring method according to claim 1, wherein 2 or more prisms are used for the plurality of prisms, preferably 3 prisms;
the angle sensor adopts sensors such as an inclination angle sensor or a gyroscope, and the attitude angles use 1, 2 or 3 attitude angles, preferably 3 attitude angles;
the state equation is determined according to the line type of the designed line section where the heading machine is located at present, and is determined according to the line section, the easement curve section and the circular curve section respectively.
5. The method for measuring the position and the attitude of the heading machine as claimed in claim 1, further comprising:
(1) building an equation of state
1) Equation of state of straight line
X(k+1)＝ΦX(k)+ΓW(k)；
X＝[N E H dl v a]T；
2) Equation of state of the relaxation curve
Wherein,
X＝[N E H l dl v a θ dθ β]T；
M11＝cos(θk)·cos(βk)·cos(α)-sin(θk)·cos(βk)·sin(α)；
M12＝-dlk·sin(θk)·cos(βk)·cos(α)-dlk·cos(θk)·cos(βk)·sin(α)；
M12＝-dlk·cos(θk)·sin(βk)·cos(α)+dlk·cos(θk)·sin(βk)·sin(α)；
M21＝cos(θk)·cos(βk)·sin(α)+sin(θk)·cos(βk)·cos(α)；
M22＝-dlk·sin(θk)·cos(βk)·cos(α)+dlk·cos(θk)·cos(βk)·cos(α)；
M23＝-dlk·cos(θk)·sin(βk)·cos(α)-dlk·sin(θk)·sin(βk)·cos(α)；
3) equation of state of circular curve
The circular curve is similar in structure to the easement curve, and only the angular increment calculation differs, so there is:
M31＝0；
n is a north coordinate, E is an east coordinate, H is an elevation, dL is an oil cylinder increment, alpha is an azimuth angle of a designed line, T is a measurement period, beta is a pitch angle, alpha is an azimuth angle, and L is a forward distance; the speed v and the acceleration a are obtained through measurement;
(2) construction of an Observation equation
The heading machine advancing distance is measured through an oil cylinder stroke sensor, the prism coordinate is measured through a total station, and the stroke of the oil cylinder stroke sensor and the prism coordinate measured through the total station are used as observed quantities;
and (3) taking the prism coordinate measured by the total station and the oil cylinder stroke measured by the oil cylinder stroke sensor as observed values, and listing an observation equation:
Z＝[N E H L]T；
(3) predictive update
1) The straight line segment is directly predicted by kalman filtering:
P(k+1|k)＝φ(k+1|k)P(k)φT(k+1|k)+Q(k+1)；
K(k+1)＝P(k+1|k)HT(k+1)[H(k+1)P(k+1|k)HT(k+1)+R(k+1)]-1；
P(k+1)＝[I-K(k+1)H(k+1)]P(k+1|k)；
2) for the relaxation curve and the circular curve segment, because a nonlinear function exists, the linear model is predicted by adopting an extended kalman, and then:
P(k+1|k)＝Φ(k+1|k)P(k)ΦT(k+1|k)+Q(k+1)；
K(k+1)＝P(k+1|k)HT(k+1)[H(k+1)P(k+1|k)HT(k+1)+R(k+1)]-1；
P(k+1)＝[I-K(k+1)H(k+1)]P(k+1|k)。
6. the method of measuring the heading machine attitude of claim 1, further comprising:
measuring m attitude angles of the heading machine by an angle sensor, and establishing a simultaneous equation set according to the predicted prism set coordinates and attitude angles; and solving an optimal solution of error correction by taking a least square criterion as an estimation condition.
7. A heading machine position and posture measuring system to which the heading machine position and posture measuring method according to any one of claims 1 to 6 is applied, characterized by comprising:
the attitude angle measuring module is used for sequentially measuring the coordinates of a plurality of prisms on the heading machine by using the total station and measuring a plurality of attitude angles by using an angle sensor arranged on the heading machine;
the prism coordinate prediction module is used for predicting the coordinates of the prisms at the same moment by using a segmented Kalman filtering algorithm;
and the pose measurement module is used for establishing a measurement equation set by adopting a coordinate conversion model and solving the pose by utilizing a least square method.
8. A computer device, characterized in that the computer device comprises a memory and a processor, the memory storing a computer program which, when executed by the processor, causes the processor to carry out the steps of:
after the prisms 1 to n-1 are measured, predicting coordinates of the prisms 1 to n-1 when the prisms n are measured; reading 3 attitude angles output by the angle sensor at the moment, and solving the position and posture of the heading machine by using a simultaneous equation set of the predicted coordinates of the prisms 1-n-1 and the measured coordinate of the prism n.
9. A computer-readable storage medium storing a computer program which, when executed by a processor, causes the processor to perform the steps of:
after the prisms 1 to n-1 are measured, predicting coordinates of the prisms 1 to n-1 when the prisms n are measured; reading 3 attitude angles output by the angle sensor at the moment, and solving the position and posture of the heading machine by using a simultaneous equation set of the predicted coordinates of the prisms 1-n-1 and the measured coordinate of the prism n.
10. An information data processing terminal, characterized in that the information data processing terminal is used for realizing the heading machine position and attitude measuring system according to claim 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210446784.5A CN114777749A (en) | 2022-04-26 | 2022-04-26 | Position and attitude measurement method, system, medium, equipment and terminal of development machine |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210446784.5A CN114777749A (en) | 2022-04-26 | 2022-04-26 | Position and attitude measurement method, system, medium, equipment and terminal of development machine |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114777749A true CN114777749A (en) | 2022-07-22 |
Family
ID=82433862
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210446784.5A Pending CN114777749A (en) | 2022-04-26 | 2022-04-26 | Position and attitude measurement method, system, medium, equipment and terminal of development machine |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114777749A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116026322A (en) * | 2022-12-09 | 2023-04-28 | 华中科技大学 | Method and system for measuring shield tunneling posture |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101251367A (en) * | 2008-04-02 | 2008-08-27 | 上海隧道工程股份有限公司 | Real-time measurement system for shield excavation attitude |
| CN104764434A (en) * | 2015-03-31 | 2015-07-08 | 徐州市市政设计院有限公司 | Quick solution system and method for shield attitude |
| CN112066955A (en) * | 2020-08-24 | 2020-12-11 | 西安科技大学 | Method and system for measuring pose parameters of body of underground dynamic heading machine |
| CN112683268A (en) * | 2020-12-08 | 2021-04-20 | 中国铁建重工集团股份有限公司 | Roadway real-time positioning navigation method and system based on extended Kalman filtering |
-
2022
- 2022-04-26 CN CN202210446784.5A patent/CN114777749A/en active Pending
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101251367A (en) * | 2008-04-02 | 2008-08-27 | 上海隧道工程股份有限公司 | Real-time measurement system for shield excavation attitude |
| CN104764434A (en) * | 2015-03-31 | 2015-07-08 | 徐州市市政设计院有限公司 | Quick solution system and method for shield attitude |
| CN112066955A (en) * | 2020-08-24 | 2020-12-11 | 西安科技大学 | Method and system for measuring pose parameters of body of underground dynamic heading machine |
| CN112683268A (en) * | 2020-12-08 | 2021-04-20 | 中国铁建重工集团股份有限公司 | Roadway real-time positioning navigation method and system based on extended Kalman filtering |
Non-Patent Citations (1)
| Title |
|---|
| 黄俊杰: "卡尔曼滤波在盾构姿态测量中的应用", 华东理工大学学报自然科学版, vol. 37, no. 6, pages 782 - 786 * |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116026322A (en) * | 2022-12-09 | 2023-04-28 | 华中科技大学 | Method and system for measuring shield tunneling posture |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111596613B (en) | Welding deviation determination method, welding deviation determination device, electronic equipment and storage medium | |
| CN111537002B (en) | Calibration method and orientation method for laser strapdown inertial measurement unit installation error | |
| CN114777749A (en) | Position and attitude measurement method, system, medium, equipment and terminal of development machine | |
| CN111174791A (en) | Positioning correction method based on bidirectional long-short term memory network | |
| CN111089576A (en) | Method for determining actual output value of fiber-optic gyroscope and method for testing threshold value of fiber-optic gyroscope | |
| CN113074753A (en) | Star sensor and gyroscope combined attitude determination method, combined attitude determination system and application | |
| CN101566466B (en) | Profile analysis system and method | |
| US11620846B2 (en) | Data processing method for multi-sensor fusion, positioning apparatus and virtual reality device | |
| CN110109165A (en) | The detection method and device of abnormal point in driving trace | |
| CN110940336B (en) | Strapdown inertial navigation simulation positioning resolving method and device and terminal equipment | |
| CN112197771A (en) | Vehicle failure track reconstruction method, device and storage medium | |
| CN109116845B (en) | Automatic guided transport vehicle positioning method, positioning system and automatic guided transport system | |
| CN111693051A (en) | Multi-target data association method based on photoelectric sensor | |
| JPH04231813A (en) | Method for measuring angle and angular characteristic curve | |
| CN114623832B (en) | Method and system for characterization, analysis and judgment of observable ability dimension reduction of autonomous navigation system | |
| CN116608859A (en) | Navigation method, storage medium and device of self-adaptive unscented Kalman filtering based on threshold processing | |
| CN110793549B (en) | Quick offline data analysis system of inertial measurement unit | |
| CN113219973B (en) | Local path control method of mobile robot | |
| CN112987054B (en) | Method and device for calibrating SINS/DVL combined navigation system error | |
| CN113029200B (en) | Method, system and medium for testing course angle and accuracy based on robot sensor | |
| CN111965662B (en) | Speed measuring and positioning method for indoor trolley | |
| CN114279395A (en) | Deformation detection method and system for pipeline | |
| CN118305810B (en) | AMR laser combined calibration method based on iterative optimization algorithm | |
| CN116878477B (en) | Hemispherical resonator gyro damping non-uniformity parameter identification method, equipment and storage medium | |
| Zhang et al. | Mounting Misalignment and Time Offset Self-Calibration Online Optimization Method for Vehicular Visual-Inertial-Wheel Odometer System |
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 |