Shield pose high-precision and high-real-time measurement method and device

Abstract

The invention belongs to the technical field of shield tunneling machine construction, and discloses a high-precision and high-real-time measurement method and device for a shield pose, and the method comprises the steps: respectively collecting data of an accelerometer, a gyroscope, an oil cylinder stroke sensor and a laser target, and calculating the pose of a shield tunneling machine through employing the acceleration and angular velocity measured by a speedometer and the gyroscope, according to the method, the propulsion speed of the shield tunneling machine is obtained after data differentiation and lever arm error compensation of an oil cylinder stroke sensor, the position posture of the shield tunneling machine is obtained after time synchronization and delay compensation of laser target measurement data, and the error of an inertial navigation unit is estimated through extended Kalman filtering based on the error state of the inertial navigation unit. And finally obtaining the corrected position and posture of the shield tunneling machine. According to the shield tunneling machine pose measurement method based on multi-sensor fusion, the measurement precision, the measurement frequency and the measurement real-time performance can be remarkably improved. According to the invention, the problems of poor measurement precision, long period and low real-time performance of an existing shield tunneling machine pose measurement mode are solved.

Description

Technical Field

[0001] The present invention belongs to, but is not limited to, the technical field of shield machine construction, and particularly relates to a method and device for high-precision and high-real-time measurement of shield pose. Background Art

[0002] With the development of the urbanization process, the demand for shield construction of long tunnels such as cross-river, cross-sea, and subway is increasing. However, the traditional shield construction method has a very long cycle of tunneling - segment assembly - step change, and it is difficult to meet the current rapid economic development needs. The new synchronous pushing and assembling method integrates part or all of the segment assembly operation time into the construction stage, which can effectively shorten the construction cycle and has received extensive attention and research. The synchronous pushing and assembling method puts forward higher requirements for the pose measurement accuracy and real-time performance of the shield guidance system. The attitude measurement accuracy is ≤0.5 mrad, and the measurement period is ≤500 ms. The existing shield pose measurement methods such as the laser target method, the multi-prism method, and the inertial navigation method cannot meet the requirements.

[0003] Currently, the most widely used laser target method for shield pose measurement is an optical pose measurement method based on ATR laser. It uses a total station and a laser target to cooperate to measure the pose of the shield, and has the advantage of high measurement accuracy. However, the alignment time of the total station is long, and the calculation of the laser target is complex, resulting in poor real-time performance of the laser target method, and the measurement period reaches 30 s. The multi-prism method is a method that uses a total station to sequentially measure the positions of multiple prisms on the shield and then calculates the shield pose. It has poor real-time performance and is easily blocked, affecting the shield pose measurement. The gyroscope measurement method has random drift errors and cannot work for a long time. A single pose measurement method is difficult to meet the pose measurement requirements of synchronous pushing and assembling.

[0004] In order to achieve high-precision and high-real-time measurement of shield pose, Patent CN201010532191[1] uses a multi-sensor fusion method to calculate the shield pose when the target fails, but it cannot obtain higher precision and high-real-time shield pose. Guo Qingyao[2] et al. proposed a combined pose measurement method based on a laser target and an inertial navigation system, which solved the problem of the failure of the laser target to measure the attitude angle and improved the anti-interference ability of the system, but did not consider the problem that the measurement period of the laser target is not fixed and there is a delay in measurement.

[0005] Through the above analysis, the problems and defects existing in the prior art are as follows:

[0006] (1) The measurement accuracy of the existing single or combined pose measurement methods is low, the measurement period is long, and they cannot meet the measurement requirements of the synchronous pushing and assembling for the guidance system.

[0007] (2) The existing pose measurement methods do not consider the time delay of processing the spot image by the laser target, and the real-time performance is poor.

[0008] (3) Existing single measurement methods may all fail under special working conditions such as occlusion, dust, and strong vibration, making it difficult to meet the reliability requirements of the intelligent shield's pose measurement system.

[0009] [1] Huazhong University of Science and Technology. A real-time guidance system for shield machines based on multi-sensor data fusion: CN201010532191.8[P]. 2011-05-11.

[0010] [2] Guo Qingyao, Lin Jiarui, Ren Yongjie, etc. A combined pose measurement method based on a laser target and a strapdown inertial navigation system [J]. Laser & Optoelectronics Progress, 2018, 55(01): 322-329. Summary of the Invention

[0011] Aiming at the problems existing in the prior art, the present invention provides a shield pose high-precision and high-real-time measurement method and device.

[0012] The present invention is implemented as follows. A high-precision and high-real-time pose measurement device for a shield machine, the hardware of the shield machine pose includes: an inertial navigation unit; an optical measurement system composed of a laser target, an ATR total station, and a backsight prism; an oil cylinder stroke sensor and a PLC.

[0013] The high-precision and high-real-time pose measurement method for the shield machine includes the following steps:

[0014] Step 1, collect the measurement data of the accelerometer, gyroscope, oil cylinder stroke sensor, laser target, and the measurement timestamp of the laser target;

[0015] Step 2, use the accelerations and angular velocities measured by the speedometer and gyroscope to calculate and obtain the position, attitude, and speed P INS , A INS , V INS of the shield machine obtained by inertial navigation solution, and update the error state of the inertial navigation unit; perform fault diagnosis on the position and attitude data obtained by the laser target to determine whether it is available. If it is not available, it is excluded. Differentiate the data of the oil cylinder stroke sensor to obtain the oil cylinder propulsion speed V cyl-i ;

[0016] Step 3, perform time synchronization and delay compensation on the laser target measurement data, and perform lever arm error compensation on the oil cylinder propulsion speed;

[0017] Step 4, respectively use the differences between the position and attitude of the inertial navigation unit and the laser target, and the differences between the speed of the inertial navigation unit and the oil cylinder propulsion speed as the observation quantities, use the extended error state of the inertial navigation unit as the state quantity, and use the adaptive extended Kalman filter to fuse the observation values and the state values to estimate and correct the errors of the inertial navigation unit;

[0018] Step 5: Subtract the attitude, position, and velocity obtained from INS solution from the attitude, position, and velocity errors after filtering and correction, and output the results to perform closed-loop correction on the attitude, position, and velocity of the INS.

[0019] Further, for the discrete time t of the system k At any time t t The measured laser target data at time t t Between discrete times t s-1 And t s The laser target measurement data delay compensation in step 3 is divided into two steps:

[0020] Step 1: Through linear interpolation, make the measured value at time t t Correspond to the state value at time t s To synchronize the measurement time with the Kalman filter time. The specific method is as follows:

[0021]

[0022] In the formula, Is the system state estimate value at the laser target measurement time t t ; Is the system state estimate value at the previous time point t s-1 Before the measurement time of the filtering system; Is the system state estimate value at the next time point t s After the measurement time of the filtering system;

[0023]

[0024] In the formula, Is the observed value after time synchronization; Is the system state extrapolation value at the laser target measurement time t t ; Is the system state estimate value at the next time point t s After the measurement time of the filtering system, Z i,t Is the laser target measurement value at the laser target measurement time t t ; Is the laser target observation matrix at time t s ; Is the laser target observation matrix at time t t ;

[0025] Step 2: Fuse the delayed measurements after synchronization through the Larsen method, and use the observed value to correct the state value at time t k And correct the state noise matrix. The specific method is as follows:

[0026] The observed value obtained by measurement at the moment is:

[0027]

[0028] In the formula is the observed value at time t after delay compensation, k and is the observed value at time t after time synchronization, k and is the laser target observation matrix and the system state prediction value at time t, k and and is the laser target observation matrix and the system state prediction value at time t; s

[0029] The Kalman gain is:

[0030]

[0031] The covariance matrix is:

[0032]

[0033] where P s pre is the predicted value of the state covariance matrix at time t, s and is the observation noise matrix at time t, M k and T is the time delay compensation matrix, and its specific form is as follows:

[0034]

[0035] where j = s, s + 1,..., k - 1, then the state estimate value after updating with the laser target measurement data is:

[0036]

[0037] Furthermore, the rod arm error compensation for the cylinder propulsion speed in the third step is as follows:

[0038]

[0039] In the formula, is the attitude transformation matrix for converting the vehicle coordinate system to the navigation coordinate system, is the propulsion speed of the cylinder in the vehicle coordinate system, v cyl is the result of differentiating the cylinder stroke, is the angular rate of the vehicle relative to the navigation coordinate system, δl b is the position vector from the inertial navigation unit to the ball joint connecting the propulsion cylinder and the shield body.

[0040] ​Further, the state variables and state transition equation of the adaptive extended Kalman filter based on the inertial navigation error state in Step 4 are as follows:

[0041] The inertial navigation error state vector is δx IMU =[δφ IMU δv IMU δp IMU ε gb ε ab ε gr ε ar T , and the inertial navigation error state transfer equation is as follows:

[0042]

[0043] That is:

[0044]

[0045] After discretization using Taylor expansion, we get

[0046]

[0047] That is:

[0048] δx IMU =Φ IMU δx IMU +Γ IMU W IMU (14)

[0049] The error state vector of the laser target and the oil cylinder stroke sensor is δx mea =[δφ Tar δp Tar δv cyl δk cyl T , where δφ Tar is the laser target attitude error, δv cyl is the oil cylinder speed error, δp Tar is the laser target position error, and δk cyl is the oil cylinder misalignment angle error; the error transfer equation of the oil cylinder stroke sensor is:

[0050] where which represents the influence of the oil cylinder misalignment angle error on the speed error.

[0051] Then the error state transition equation of the entire system is:

[0052]

[0053] That is:

[0054] X k+1 ​​= ΦX k + ΓW k (16).

[0055] Further, the adaptive extended Kalman filter observation equation based on the inertial navigation error state in the fourth step is as follows:

[0056]

[0057] Z cyl = [0 3×3 I 3×3 0 3×3 0 3×3 0 3×3 0 3×3 0 3×3 0 3×3 0 3×3 -I 3×3 0 3×3 X + W cyl (18)

[0058] Furthermore, the measurement values can be adaptively fused; at any discrete moment, if no observation value arrives at the fusion center within this time interval, only time update is performed without measurement update; if a laser target or a cylinder stroke sensor arrives at the fusion center, after time update, measurement update is performed using the corresponding observation equation; if both a laser target and a cylinder stroke sensor arrive at the fusion center simultaneously, sequential filtering is adopted, that is, after one time update, two measurement updates are performed respectively using the laser target observation value and the cylinder stroke sensor measurement value.

[0059] Another object of the present invention is to provide a shield machine pose high-precision and high-real-time pose measurement system applying the shield machine pose high-precision and high-real-time pose measurement method described above. This system includes:

[0060] A data acquisition module, which is used to acquire the angular velocity and acceleration collected by the inertial navigation unit, the position and attitude data collected by the laser target, and the cylinder stroke data obtained by the cylinder stroke sensor;

[0061] An information preprocessing module, which is used to integrate the angular velocity and acceleration to obtain the inertial navigation attitude, velocity, and position data, perform fault diagnosis on the laser target data to judge whether it is available and perform time synchronization delay compensation, differentiate the cylinder stroke sensor data to obtain the cylinder propulsion speed and perform lever arm error compensation;

[0062] An information fusion module, which is used to perform adaptive Kalman filtering on the laser target position, attitude data and the cylinder propulsion speed of the cylinder stroke sensor to estimate the inertial navigation error state;

[0063] An angular velocity compensation module, which is used to correct the inertial navigation attitude, velocity, and position and output the corrected attitude and position.

[0064] Another object of the present invention is to provide a computer device, which includes a memory and a processor. The memory stores a computer program. When the computer program is executed by the processor, the processor executes the steps of the high-precision and high-real-time pose measurement method for the shield machine position and attitude.

[0065] Another object of the present invention is to provide a computer-readable storage medium storing a computer program. When the computer program is executed by a processor, the processor executes the steps of the high-precision and high-real-time pose measurement method for the shield machine position and attitude.

[0066] Another object of the present invention is to provide an information data processing terminal for implementing the high-precision and high-real-time pose measurement system for the shield machine position and attitude.

[0067] Combined with the above technical solutions and solved technical problems, the advantages and positive effects of the technical solution to be protected by the present invention are as follows:

[0068] First, the present invention provides a high-precision and high-real-time pose measurement method for a shield machine based on the fusion of an inertial navigation unit, a laser target, and an oil cylinder stroke sensor, which accurately estimates and compensates the errors of the inertial navigation unit in real time according to the high-precision data of the laser target and the propulsion speed of the propulsion oil cylinder, and obtains high-precision and high-real-time shield machine position and attitude information.

[0069] The present invention proposes an adaptive delay compensation Kalman filter fusion method, which improves the real-time performance and accuracy of the fusion system by compensating the measurement delay of the laser target, and can adaptively fuse the measurement information to make the asynchronous and delayed measurement information fuse unbiased.

[0070] The shield machine position and attitude measurement method provided by the present invention improves the accuracy and environmental adaptability of the measurement system by using the high seismic resistance performance of the inertial navigation unit and the high precision of the laser target.

[0071] Compared with the prior art, the shield tunneling attitude measurement method of the present invention improves the real-time performance and measurement accuracy of the shield machine position and attitude, and provides high-real-time and high-reliability position and attitude data for the attitude control of unattended intelligent shields.

[0072] Second, by improving the measurement accuracy and real-time performance of the shield machine position and attitude, the present invention improves the measurement cycle from 30 s to 0.5 s and the measurement accuracy by 35%, provides a measurement basis for the push-assembly synchronous construction method, thereby improving the tunneling efficiency of the shield machine, and demonstrating significant engineering application value and commercial value.

[0073] Through domestic and foreign patent literature and technical research, traditional shield machine pose measurement methods mainly rely on laser targets. Some researchers have only fused the inertial navigation unit and the laser target in a loose coupling form to solve the real-time measurement of attitude angles, but the position error is too large. On this basis, the present invention first proposes a tight coupling of the inertial navigation unit and the oil cylinder stroke sensor, making full use of the original sensor information of the shield machine, improving the real-time measurement of the shield machine pose, and providing a measurement basis for the intelligent and autonomous tunneling of the shield machine.

[0074] For a long time, shield pose measurement has been troubled by poor real-time measurement. Existing technologies have tried to improve it through the combination of inertial navigation units and laser targets, but the effect of position measurement accuracy is limited, and high-precision and high-real-time measurement has never been achieved. The present invention has successfully realized high-precision real-time measurement of the shield machine pose by fusing the inertial navigation unit, the laser target and the oil cylinder stroke, solving this long-standing unsolved technical problem. Brief Description of the Drawings

[0075] Figure 1 It is a multi-sensor layout diagram of the shield tunneling pose measurement system provided by the embodiment of the present invention;

[0076] Figure 2 It is a step diagram of the high-precision and high-real-time pose measurement method of the shield machine provided by the embodiment of the present invention;

[0077] Figure 3 It is a schematic diagram of the high-precision and high-real-time pose measurement method of the shield machine provided by the embodiment of the present invention;

[0078] Figure 4 It is a time synchronization and delay compensation method diagram of the laser target measurement information provided by the embodiment of the present invention

[0079] Figure 5 It is a comparison diagram of the attitude measurement accuracy between the optical measurement system provided by the embodiment of the present invention and the solution of the present invention;

[0080] Figure 6 It is a comparison diagram of the attitude measurement frequency between the optical measurement system provided by the embodiment of the present invention and the solution of the present invention;

[0081] In the figure: 1. Cutter head; 2. Front shield; 3. Ball joint of the propulsion cylinder; 4. Arm error; 5. Propulsion cylinder; 6. Inertial navigation unit; 7. Laser target; 8. Tail shield; 9. Oil cylinder piston; 10. Ball joint of the support shoe; 11. Segment; 12. Rear shield; 13. Total station instrument. Detailed Embodiment

[0082] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention will be further described in detail below in conjunction with embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0083] The shield tunneling attitude measurement provided by the embodiment of the present invention includes an optical measurement system composed of a total station and a laser target, an inertial navigation unit, an oil cylinder stroke sensor, a data acquisition module and a fusion calculation module, and its layout is as Figure 1 shown.

[0084] The high-precision and high-real-time measurement device for the shield machine pose of the present invention is based on the collaborative fusion measurement principle of multi-source heterogeneous sensors, and combines the structural assembly relationship and functional coupling relationship of each module to realize the high-precision acquisition and real-time dynamic correction of the shield machine pose information. The system obtains the initial pose information of the shield machine in the three-dimensional space through the inertial navigation unit (IMU). The IMU is fixedly assembled with the shield machine body through a rigid connection. Its output data provides the system with continuous high-frequency dynamic response ability and is the main information source for pose calculation. The accelerometer and gyroscope inside the IMU measure the three-axis acceleration and angular velocity, and the initial solutions of position, attitude and velocity are obtained through integral operation, providing a priori states for subsequent error estimation and fusion.

[0085] To compensate for the drift error generated during the long-term operation of the inertial navigation system, the optical measurement subsystem composed of a laser target - ATR total station - rear view prism is designed and introduced into this system to form a spatial optical positioning constraint. The laser target is fixedly installed at the tail of the shield machine or an appropriate position, forming a stable spatial measurement baseline relationship with the total station. The laser reflection signal is captured in real time through ATR technology to measure the accurate position and attitude of the shield machine in the global coordinate system. This data has the characteristics of high precision and anti-drift, and participates in subsequent fusion calculations as external observation data. At the same time, considering the risk of low frequency and unavailability of optical data, the system is equipped with a fault diagnosis module to identify abnormal conditions of the total station and realize the rejection processing of abnormal observation values.

[0086] In order to further improve the response speed and accuracy of the pose measurement system to the shield propulsion condition, the system also integrates an oil cylinder stroke sensor. The magnetostrictive oil cylinder sensor is assembled inside the propulsion oil cylinder to measure the telescopic displacement of the propulsion oil cylinder in real time, and the forward speed information of the shield is obtained through differential calculation. Due to the geometric offset (lever arm) between the installation position of the oil cylinder and the inertial navigation centroid, the system introduces a lever arm error compensation module in the data processing process. According to the structural connection relationship and kinematic model, the measurement deviation is eliminated to ensure that the propulsion speed can be accurately mapped to the linear velocity observation of the machine body. This speed quantity and the IMU speed quantity together constitute an observation vector for subsequent state estimation.

[0087] At the data fusion level, the system uses the Adaptive Extended Kalman Filter algorithm (AEKF) to achieve the joint estimation of multi-source observations and IMU states. Taking the IMU solution output, the pose measured by the laser, and the cylinder speed error as the observation quantities, an error state equation and an observation model are established. Through the filtering algorithm, the IMU error state is recursively estimated, and the attitude, position, and speed outputs of the IMU are corrected through closed-loop feedback, realizing real-time error compensation and system self-calibration. This fusion method effectively combines the accuracy, update rate, and robustness characteristics of each sensor, achieving the organic unity of high precision and high real-time performance, and meeting the engineering requirements of shield machine attitude monitoring under complex working conditions.

[0088] As Figure 2 shown, the high-precision and high-real-time pose measurement method for the shield machine provided by the embodiment of the present invention includes the following steps:

[0089] S101, collect the data of the laser target, cylinder stroke sensor, inertial navigation accelerometer, and gyroscope;

[0090] S102, perform fault diagnosis on the laser target data, differentiate the cylinder stroke data to obtain the cylinder propulsion speed, and integrate the inertial navigation data to obtain the inertial navigation position, attitude, and speed;

[0091] S103, perform time synchronization and delay compensation on the laser target data, and perform lever arm error compensation on the cylinder propulsion speed;

[0092] S104: Based on the Extended Kalman Filter of the inertial navigation error state, fuse the laser target and cylinder stroke sensor data to correct the inertial navigation error;

[0093] S105: Output the fused and corrected shield machine pose and correct the inertial navigation position, attitude, and speed.

[0094] As Figure 3 shown, the high-precision and high-real-time pose measurement method for the shield machine provided by the embodiment of the present invention specifically includes the following steps:

[0095] Step S1, collect the measurement data of the accelerometer, gyroscope, cylinder stroke sensor, laser target, and the time stamp of the laser target measurement.

[0096] Step S2, use the acceleration and angular velocity measured by the speedometer and gyroscope to calculate the position, attitude, and speed P INS , A INS , V INS of the shield machine solved by inertial navigation, and update the error state of the inertial navigation unit. Perform fault diagnosis on the position and attitude data obtained from the laser target to determine whether it is available. If there is a fault, it is excluded. Differentiate the cylinder stroke sensor data to obtain the cylinder propulsion speed V cyl-i .

[0097] Step S3: Perform time synchronization and delay compensation on the laser target measurement data, and perform lever-arm error compensation on the cylinder propulsion speed.

[0098] Step S4: Respectively, take the differences between the position and attitude of the inertial navigation unit and the position and attitude of the laser target, and the differences between the speed of the inertial navigation unit and the cylinder propulsion speed as observation quantities, take the extended error state of the inertial navigation unit as the state quantity, and use the adaptive extended Kalman filter to fuse the observation values and state values to estimate and correct the errors of the inertial navigation unit.

[0099] Step S5: Subtract the attitude, position, and speed errors after filtering and correction from the attitude, position, and speed solved by the inertial navigation to output the results, and perform closed-loop correction on the attitude, position, and speed of the inertial navigation.

[0100] In one embodiment, step S1 provided by the embodiment of the present invention includes:

[0101] The inertial navigation outputs the acceleration and angular velocity of the shield machine, the laser target outputs the position and attitude of the shield machine, and the cylinder stroke sensor outputs the stroke of the propulsion cylinder.

[0102] In one embodiment, step S2 provided by the embodiment of the present invention includes:

[0103] Diagnose the pose data output by the target based on the measurement residual check:

[0104] The system residual is:

[0105] r k =Z k -H Tar X k|k-1 (55)

[0106] The variance of the residual of the Kalman filter is

[0107]

[0108] Construct a fault detection function with a data window length of 10 for every 10 samples

[0109]

[0110] where λ k follows a chi-square distribution with 45 degrees of freedom, that is, λ k ~χ 2 (45).

[0111] Taking the significance level as 0.995, the judgment condition for whether the system fault occurs is:

[0112]

[0113] If λ k≤T D , it is determined that there is no fault; if λ k >T D , it is determined that there is a fault, indicating that the subsystem observation is abnormal and isolation is required.

[0114] In one embodiment, step S2 provided by the embodiment of the present invention includes:

[0115] Differentiate the cylinder stroke data to obtain the cylinder propulsion speed:

[0116]

[0117] In one embodiment, step S3 provided by the embodiment of the present invention includes:

[0118] Time synchronization and delay compensation of the laser target measurement information.

[0119] As Figure 4 shown, at the discrete time t of the system k obtain the laser target data measured at any time t t , t t is between the discrete time period t k before t s-1 and t s .

[0120] First, perform time synchronization on it. This process first obtains the system state at time t t through linear interpolation;

[0121]

[0122] Then extrapolate the measurement information to estimate the measurement information at time t s ;

[0123]

[0124] Then perform delay compensation based on the Larsen method. First, use the observation value at time t t to extrapolate to obtain the measurement value at time t k , estimate and correct the system state, and compensate the Kalman gain and state covariance matrix:

[0125] The extrapolated measurement is:

[0126]

[0127] The Kalman gain is:

[0128]

[0129] The covariance matrix is:

[0130]

[0131] Among them, P s pre is the predicted value of the state covariance matrix at time t s and for t k is the observation noise matrix at time t, M T is the time delay compensation matrix, which is specifically as follows in the dry case:

[0132]

[0133] where j = s, s + 1, …, k - 1, then the state after updating with the laser target measurement data is:

[0134]

[0135] In one embodiment, step S3 provided by the embodiment of the present invention includes:

[0136] Such as Figure 1 shown compensation for the lever arm error:

[0137] The vector of the lever arm is δl, and the linear velocity of the shield machine at this moment is the angular velocity is Therefore, the cylinder speed after compensating for the lever arm error is:

[0138]

[0139] In one embodiment, step S4 provided by the embodiment of the present invention includes: The state vector based on the extended inertial navigation error state Kalman filter is:

[0140] δx = [δφ IMU δv IMU δp IMU ε gb ε ab ε gr ε ar δφ Tar δp Tar δv cyl δk cyl T (69)

[0141] The meanings of its respective components are shown in Table 1:

[0142] Table 2 Meanings of the components of the system state vector

[0143] State variable Description <![CDATA[δφ IMU > INS attitude angle error <![CDATA[δv IMU > INS velocity error <![CDATA[δp IMU > INS position error <![CDATA[ε gb > Gyroscope constant drift <![CDATA[ε ab > Accelerometer constant drift <![CDATA[ε gr > Gyroscope zero bias instability <![CDATA[ε ar > Accelerometer zero bias instability <![CDATA[δφ Tar > Optical measurement system attitude measurement error <![CDATA[δp Tar > Optical measurement system position measurement error <![CDATA[δv cyl > Cylinder stroke sensor velocity measurement error <![CDATA[δk cyl > Cylinder stroke sensor slip and misalignment angle error

[0144] ​In one of the embodiments, step S4 provided by the embodiment of the present invention includes: a state transition equation based on an extended inertial navigation error state Kalman filter.

[0145] It is composed of two parts, namely an inertial navigation error transition equation and a measurement error transition equation. The IMU error state equation is:

[0146]

[0147] Where:

[0148]

[0149]

[0150] F pv = I3(77)

[0151] After discretizing it using Taylor expansion, we get

[0152]

[0153] That is:

[0154] δx IMU = Φ IMU δx IMU + Γ IMU W IMU (80)

[0155] The measurement error state equation is:

[0156]

[0157] k cyl = k cyl + G cyl W cyl (82)

[0158]

[0159] In summary, the state equation of the measurement system is:

[0160]

[0161] In one of the embodiments, step S4 provided by the embodiment of the present invention includes:

[0162] The observed values based on the extended inertial navigation error state Kalman filter are the differences between the inertial navigation pose and the target pose, and between the inertial navigation speed and the cylinder speed, that is:

[0163]

[0164] Zcyl = v IMU -v cyl (86)

[0165] In one embodiment, step S4 provided by the embodiment of the present invention includes:

[0166] The state observation equation based on the extended inertial navigation error state Kalman filter is:

[0167]

[0168] Z cyl = [0 3×3 I 3×3 0 3×3 0 3×3 0 3×3 0 3×3 0 3×3 0 3×3 0 3×3 -I 3×3 0 3×3 X + W cyl (88)

[0169] In one embodiment, step S4 provided by the embodiment of the present invention includes:

[0170] Adaptive extended Kalman filter fusion depends on whether the observation data reaches the fusion center. As Figure 4 shown, at time t t , the laser target data measured by the laser target, at time t s , t s+1 when no measurement data reaches the fusion center, the filter only performs time update, that is:

[0171]

[0172] Then, let

[0173] At time t s+2 when the oil cylinder stroke sensor data reaches the fusion center, after preprocessing the oil cylinder stroke, first perform time update on the filter, that is:

[0174]

[0175] Then, use the oil cylinder propulsion speed to perform measurement update on the filter, that is:

[0176]

[0177] At time t s+6 the time t tThe pose data of the laser target measured at a certain moment, after time synchronization and delay compensation, first update the time of the filter, that is:

[0178]

[0179] Then, use the attitude and position of the laser target to update the measurement of the filter, that is:

[0180]

[0181] Where:

[0182]

[0183] At a certain moment, the laser target data and the oil cylinder stroke sensor data are obtained simultaneously. First, perform a time update, that is:

[0184]

[0185] Then perform sequential filtering and two measurement updates, that is:

[0186]

[0187] In one embodiment, step S5 provided by the embodiment of the present invention includes: subtracting the attitude and position after filter correction from the attitude and position obtained by inertial navigation solution as the output result, that is:

[0188]

[0189] In one embodiment, step S5 provided by the embodiment of the present invention includes: closed-loop correcting the attitude, position and speed of the inertial navigation, taking the output position, attitude speed as the initial value for the next inertial navigation pose calculation, and setting the attitude angle error, position error, and speed error of the inertial navigation error state, that is:

[0190]

[0191] I. The specific application fields or related products of the present invention.

[0192] A high-precision and high-real-time pose measurement device for the pose of a shield machine, the hardware of the shield machine pose includes: an inertial navigation unit 6; an optical measurement system composed of a laser target 7, an ATR total station and a rear-view prism; an oil cylinder stroke sensor and a PLC.

[0193] II. Evidence related to the technical effects obtained by the embodiments of the present invention.

[0194] To verify the effectiveness of the present invention, the following simulation experiment was carried out: with the initial attitude angles being: pitch angle 0°, pitch angle 0°, pitch angle 0°, and a propulsion speed of 90 mm / min, the angular rate changes were pitch angle ±0.075° / h, roll angle ±0.15° / h, and horizontal angle 0.4 - 0.6° / h to generate shield pose data containing time. Gyro zero bias 0.01° / h (the same for three axes), gyro zero bias instability 0.01° / h (the same for three axes), accelerometer zero bias 10 μg (the same for three axes), accelerometer zero bias instability 13 μg (the same for three axes), and a measurement period of 0.1 s to generate inertial navigation data. With a measurement period of 30 - 40 s, adding random noise of N(0, 0.0096 2 ) to the attitude angles and adding random noise of N(0, 0.002 2 ) to the positions to generate optical measurement system attitude data, and recording the time as the measurement time. Adding a time delay of τ ~ N(1, 0.01) to the measurement time to obtain the time for the fusion center to acquire the pose of the optical measurement system. The results were compared using the scheme with only the optical measurement system and the scheme of the present invention respectively, and the results are as shown in Figure 5 、 6 .

[0195] It can be seen that the attitude error of the shield machine is relatively large in the case of only optical measurement. Except for a relatively large error in the initial stage of filtering, the peak-to-peak value of the attitude angle error is less than 0.4 mrad after convergence in the present invention, which is better than the optical measurement method; the measurement frequency of the optical measurement method is low, and the measurement frequency of this scheme is 2 Hz (period 500 ms), which is better than the optical measurement method.

[0196] It should be noted that the embodiments of the present invention can be implemented through hardware, software, or a combination of software and hardware. The hardware part can be implemented using dedicated logic; the software part can be stored in a memory and executed by an appropriate instruction execution system, such as a microprocessor or dedicated designed hardware. Those of ordinary skill in the art can understand that the above devices and methods can be implemented using computer-executable instructions and / or included in processor control code, for example, such code is provided on a carrier medium such as a disk, CD, or DVD-ROM, a programmable memory such as a read-only memory (firmware), or a data carrier such as an optical or electronic signal carrier. The devices and their modules of the present invention can be implemented by hardware circuits of programmable hardware devices such as very large scale integrated circuits or gate arrays, semiconductors such as logic chips, transistors, etc., or field programmable gate arrays, programmable logic devices, etc., can also be implemented by software executed by various types of processors, or can be implemented by a combination of the above hardware circuits and software such as firmware.

[0197] As described above, it is only the specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention, any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be covered by the protection scope of the present invention.

Claims

1. A high-precision and high-real-time pose measurement device for a shield machine, characterized in that, Including: An inertial navigation unit, which is used to obtain the initial position, attitude and speed information of the shield machine; An optical measurement system, including a laser target, an automatic target recognition (ATR) total station and a backsight prism. The laser target is fixedly arranged on the shield machine and is used to obtain the position and attitude information of the shield machine in the total station coordinate system; An oil cylinder stroke sensor, which is used to measure the stroke of the shield propulsion oil cylinder and calculate the propulsion speed; A programmable logic controller (PLC), which is used to collect the data information of the inertial navigation unit, the optical measurement system and the oil cylinder stroke sensor; A data fusion processing module, which is used to receive the measurement data of the above-mentioned modules, estimate, correct and output the pose state of the shield machine based on the adaptive extended Kalman filter algorithm, and realize the closed-loop correction of the output error of the inertial navigation unit.

2. The pose measurement device according to claim 1, wherein: The data fusion processing module includes: A time synchronization and delay compensation unit, which is used to calibrate the timestamp of the laser target measurement data and compensate for the measurement delay; A lever arm error compensation unit, which is used to correct the propulsion speed obtained by differentiating the oil cylinder stroke according to the structural connection relationship between the oil cylinder and the inertial navigation installation position; A state estimation and correction unit, which uses the difference between the output of the inertial navigation unit and the laser measurement and the oil cylinder speed observation quantity as the observation vector, constructs an extended Kalman filter model to estimate and correct the error state of the inertial navigation unit, and realizes high-precision pose output.

3. A high-precision and high-real-time pose measurement method for a shield machine, characterized in that, The method includes the following steps: Step 1, collect the measurement data of the accelerometer, gyroscope, oil cylinder stroke sensor, laser target and the laser target measurement timestamp; Step 2: Calculate the position, attitude, and speed P of the shield machine obtained by inertial navigation solution by inferring the acceleration and angular velocity measured by the speedometer and gyroscope INS ,A INS ,V INS , update the error state of the inertial navigation unit; perform fault diagnosis on the position and attitude data obtained by the laser target to determine whether it is available. If it is not available, eliminate it. Differentiate the data of the oil cylinder stroke sensor to obtain the oil cylinder propulsion speed V cyl-i ; Step 3, perform time synchronization and delay compensation on the laser target measurement data, and perform lever arm error compensation on the oil cylinder propulsion speed; Step 4, respectively use the difference between the inertial navigation unit position and attitude and the laser target position and attitude, and the difference between the inertial navigation unit speed and the oil cylinder propulsion speed as the observation quantity, use the extended inertial navigation unit error state as the state quantity, and use the adaptive extended Kalman filter to fuse the observation value and the state value to estimate and correct the error of the inertial navigation unit; Step 5, subtract the attitude, position and speed errors after filtering and correction from the attitude, position and speed solved by the inertial navigation, and perform closed-loop correction on the attitude, position and speed of the inertial navigation.

4. The high-precision and high-real-time pose measurement method for the shield machine as described in claim 3, wherein, System discrete time t k Obtain at any time t t Measured laser target data, t t Between discrete time t s-1 And t s During this period, the laser target measurement data delay compensation in the third step is divided into two steps: Step 1, through linear interpolation, make the measurement value at time t t correspond to the state value at time t s to synchronize the measurement time with the Kalman filter time. The specific method is as follows: Wherein, is the system state at the laser target measurement time t t , is the system state of the filtering system at the previous time point t s-1 before the measurement time, is the system state of the filtering system at the next time point t s after the measurement time; Wherein, is the observed value after time synchronization, is the system state at the laser target measurement time t t ; is the system state at a time point t s after the measurement time of the filtering system, Z i,t is the laser target measurement value at the laser target measurement time t t ; is the laser target observation matrix at time t s ; is the laser target observation matrix at time t t ; Step 2: By using the Larsen method to fuse the synchronized delay measurements, use the observed values to correct the state value at time t k and correct the state noise matrix. The specific method is as follows: The measurement obtains the observation value at the moment as: In the formula is the observed value at time t after delay compensation k , is the observed value at time t after time synchronization k ; is the laser target observation matrix and system state at time t k , and are the laser target observation matrix and system state at time t s . The Kalman gain is: The covariance matrix is: Among them, is the predicted value of the state covariance matrix at time t, s is the observation noise matrix at time t, k and M T is the time delay compensation matrix, which is specifically as follows:​ Where j = s, s + 1,..., k - 1, then the state after updating with the laser target measurement data is:

5. The high-precision and high-real-time pose measurement method for the shield machine according to claim 2, characterized in that The lever arm error compensation for the three oil cylinder propulsion speeds in the above Step 3 is as follows: In the formula, is the attitude transformation matrix for converting the carrier coordinate system to the navigation coordinate system, is the propulsion speed of the oil cylinder in the carrier coordinate system, v cyl is the result after differentiating the oil cylinder stroke, is the angular rate of the carrier relative to the navigation coordinate system, δl b is the position vector from the inertial navigation unit to the ball joint connecting the propulsion oil cylinder and the shield body.

6. The high-precision and high-real-time pose measurement method for the shield machine according to claim 3, characterized in that The state quantity and state transition equation of the adaptive extended Kalman filter based on the inertial navigation error state in the above Step 4 are as follows: The inertial navigation error state vector is δx IMU = [δφ IMU δv IMU δp IMU ε gb ε ab ε gr ε ar T , and the inertial navigation error state transfer equation is as follows:​ That is: After using Taylor expansion for discretization, it is obtained That is: δx IMU = Φ IMU δx IMU + Γ IMU W IMU (14) The error state vector of the laser target and the oil cylinder stroke sensor is δx mea = [δφ Tar δp Tar δv cyl δk cyl T , where δφ Tar is the attitude error of the laser target, δv cyl is the speed error of the oil cylinder, δp Tar is the position error of the laser target, δk cyl is the misalignment angle error of the oil cylinder; the measurement method number and its error transfer equation are as follows:​ Among them Then the error state transition equation of the entire system is: That is: X k+1 = ΦX k + ΓW k (16).

7. The shield machine pose measurement method with high precision and high real-time performance as described in claim 2, characterized in that, The observation equation of the adaptive extended Kalman filter based on the inertial navigation error state in the above Step 4 is as follows: Z cyl = [0 3×3 I 3×3 0 3×3 0 3×3 0 3×3 0 3×3 0 3×3 0 3×3 0 3×3 -I 3×3 0 3×3 X + W cyl (18).

8. The high-precision and high-real-time pose measurement method for the shield machine according to claim 3, characterized in that It can adaptively fuse measurement values; at any discrete moment, if no observation value arrives at the fusion center within this time interval, only time update is performed without measurement update; if a laser target or a cylinder stroke sensor arrives at the fusion center, after time update, measurement update is performed in the corresponding observation equation; if both a laser target and a cylinder stroke sensor arrive at the fusion center simultaneously, sequential filtering is adopted, that is, after one time update, two measurement updates are respectively performed using the laser target observation value and the cylinder stroke sensor measurement value.

9. A shield machine pose high-precision and high-real-time pose measurement system applying the shield machine pose high-precision and high-real-time pose measurement method according to any one of claims 1 to 8, characterized in that, The system includes: A data acquisition module, configured to acquire the angular velocity and acceleration collected by an inertial navigation unit, the position and attitude data collected by a laser target, and the cylinder stroke data obtained by a cylinder stroke sensor; An information preprocessing module, configured to integrate the angular velocity and acceleration to obtain inertial navigation attitude, velocity, and position data, perform fault diagnosis on the laser target data to determine its availability and perform time synchronization delay compensation, differentiate the cylinder stroke sensor data to obtain the cylinder propulsion speed and perform lever arm error compensation; An information fusion module, configured to use the laser target position and attitude data and the cylinder propulsion speed of the cylinder stroke sensor for adaptive Kalman filtering to estimate the inertial navigation error state; An angular velocity compensation module, configured to correct the inertial navigation attitude, velocity, and position, and output the corrected attitude and position.

10. A computer-readable storage medium stores a computer program. When the computer program is executed by a processor, the processor is caused to execute the steps of the high-precision and high-real-time pose measurement method for a shield machine position and pose according to any one of claims 1 to 8.

Images