A multi-mode combined positioning method and system for mining excavation

Abstract

The present invention relates to a multi-mode combined positioning method and system for mining excavation, and belongs to the technical field of mining excavation equipment positioning. First, the first posture and the second posture obtained by measuring the body of the excavator by the machine vision system and the fiber optic inertial navigation system are respectively obtained, and then the first posture and the second posture are aligned in time and space to obtain the first aligned posture and the second aligned posture, and finally the Kalman filtering method is used to fuse the first aligned posture and the second aligned posture to obtain the fused posture, and then the posture of the excavator body is accurately positioned based on the fusion of multi-sensor data, which solves the problem of data irrelevance and makes the positioning of the excavator body more accurate.

Description

Technical Field

[0001] The present invention relates to the technical field of mine excavation equipment positioning, and in particular to a mine excavation multi-mode combined positioning method and system based on multi-sensor data fusion. Background Art

[0002] With the development of unmanned and intelligent underground mining in coal mines, solving the problem of autonomous and precise positioning of the posture of the tunnel boring machine is the core and main task of realizing intelligent and precise mining of coal mines. At present, the posture measurement of the cantilever tunnel boring machine body is mainly divided into three categories: manual measurement, semi-automatic measurement and autonomous measurement. However, manual measurement is based on the naked eye observation of construction personnel and judgment with the help of manual experience, and it is impossible to obtain the posture information of the tunnel boring machine; semi-automatic measurement is based on the laser pointer as the reference, and the posture parameters of the tunnel boring machine relative to the laser pointer are obtained by visual measurement or other measurement methods. It can obtain a more accurate posture of the tunnel boring machine, but it still requires the surveyor to locate the laser pointer, which is a certain distance from fully automated, unmanned and intelligent tunneling; autonomous measurement can detect the posture parameters of the cantilever tunnel boring machine body without relying on external objects, but most of the current autonomous measurement systems have complex structures and are difficult to apply in actual production. Therefore, it is difficult to achieve autonomous and accurate measurement of the position and posture of a cantilevered tunnel boring machine by relying on a single detection method. It is a future development trend to propose a multi-mode combined positioning method for mining tunneling to achieve autonomous measurement of the position and posture of the tunnel boring machine. Summary of the invention

[0003] The purpose of the present invention is to provide a multi-mode combined positioning method and system for mining excavation, which improves the positioning accuracy by fusing multi-sensor data and realizes autonomous and accurate measurement of the body posture of the excavator.

[0004] To achieve the above object, the present invention provides the following solutions:

[0005] A multi-mode combined positioning method for mining excavation, the combined positioning method comprising:

[0006] Respectively obtain the first and second poses of the tunnel boring machine body measured by the machine vision system and the fiber optic inertial navigation system;

[0007] Performing temporal and spatial data alignment on the first posture and the second posture to obtain a first aligned posture and a second aligned posture;

[0008] The Kalman filtering method is used to perform data fusion on the first aligned posture and the second aligned posture to obtain a fused posture.

[0009] A multi-mode combined positioning system for mining excavation, the combined positioning system comprising:

[0010] The posture measurement module is used to obtain the first posture and the second posture obtained by the machine vision system and the fiber optic inertial navigation system respectively to measure the tunnel boring machine body;

[0011] A data alignment module, used for performing temporal and spatial data alignment on the first posture and the second posture to obtain a first aligned posture and a second aligned posture;

[0012] The data fusion module is used to perform data fusion on the first aligned posture and the second aligned posture by using a Kalman filtering method to obtain a fused posture.

[0013] According to the specific embodiments provided by the present invention, the present invention discloses the following technical effects:

[0014] The present invention is used to provide a multi-mode combined positioning method and system for mining tunneling. First, a first posture and a second posture obtained by measuring a tunneling machine body by a machine vision system and a fiber optic inertial navigation system are respectively obtained, and then the first posture and the second posture are aligned in time and space to obtain a first aligned posture and a second aligned posture. Finally, a Kalman filtering method is used to fuse the first aligned posture and the second aligned posture to obtain a fused posture, and then the posture of the tunneling machine body is accurately positioned based on the fusion of multi-sensor data, thereby solving the problem of data irrelevance and making the positioning of the tunneling machine body more accurate. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0016] Figure 1 A method flow chart of the combined positioning method provided by Embodiment 1 of the present invention;

[0017] Figure 2 A technical roadmap of the combined positioning method provided in Example 1 of the present invention;

[0018] Figure 3 This is a schematic diagram of the combined positioning method provided in Example 1 of the present invention;

[0019] Figure 4 The error curve diagram of the alignment process provided by Embodiment 1 of the present invention;

[0020] Figure 5 A schematic diagram of a machine vision and fiber optic inertial navigation combined positioning measurement coordinate system provided in Example 1 of the present invention;

[0021] Figure 6 A flow chart of the data fusion strategy provided in Example 1 of the present invention;

[0022] Figure 7 The motion trajectory, observation trajectory and filter estimation graph provided by Embodiment 1 of the present invention;

[0023] Figure 8 This is an error result diagram provided by Example 1 of the present invention;

[0024] Fig. 9 This is a system block diagram of the combined positioning system provided in Example 2 of the present invention. DETAILED DESCRIPTION

[0025] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0026] The purpose of the present invention is to provide a multi-mode combined positioning method and system for mining tunneling, which aligns the time and space data of the tunneling machine body posture detection data obtained by the machine vision system and the fiber optic inertial navigation system, and uses the Kalman filtering method to fuse the data, thereby solving the problem of data irrelevant, making the tunneling machine body positioning more accurate, and solving the problem that the existing mining cantilever tunneling machine positioning method cannot accurately locate.

[0027] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0028] Embodiment 1:

[0029] This embodiment is used to provide a multi-mode combined positioning method for mining tunneling, which is suitable for real-time monitoring of the operating status of underground tunneling machines in coal mines and can obtain the precise operating position of the tunneling machine body. Figure 1 , Figure 2 and Figure 3 As shown, the combined positioning method includes:

[0030] S1: respectively obtain the first pose and the second pose of the tunnel boring machine body measured by the machine vision system and the fiber optic inertial navigation system;

[0031] The machine vision system can be a machine monocular vision measurement system. The first posture includes the posture information of the tunnel boring machine body, and the second posture includes the posture angle and spatial position information of the tunnel boring machine body posture.

[0032] Specifically, in S1, obtaining the first pose obtained by measuring the body of the tunnel boring machine by the machine vision system may include: using a camera fixed on the body of the tunnel boring machine to collect a parallel laser beam image emitted by a laser pointer, and performing pose solving according to the parallel laser beam image to obtain the first pose. The camera may be an explosion-proof camera.

[0033] In S1, the fiber optic inertial navigation system includes three fiber optic gyroscopes and a three-axis accelerometer. Obtaining the second posture obtained by measuring the tunnel boring machine body by the fiber optic inertial navigation system may include: using the fiber optic inertial navigation system to measure the angular velocity and acceleration of the tunnel boring machine body in three directions, solving the posture according to the angular velocity and acceleration in the three directions, and obtaining the second posture.

[0034] More specifically, the three-sample algorithm of the rotating vector method is used to calculate the acceleration and angular velocity measured by the fiber-optic inertial navigation system. k , t k+1 ] time period, and take out three data of the obtained motion parameters at equal intervals as a group to form new continuous motion data. The quadratic curve is used to fit the angular velocity of the tunnel boring machine, that is,

[0035]

[0036] In formula (1), Indicates that the tunnel boring machine starts from t k Time to t k+1 The rotation speed; t k represents the initial time; τ represents the sampling period of sensor A (i.e., the machine vision system).

[0037]

[0038] In formula (2), Δθ i It represents the angle change in three time periods; h represents the step length.

[0039] Combined to get:

[0040]

[0041] In formula (3), φ(h) represents the coordinate system of the tunnel boring machine from t k Time to t k+1 The equivalent rotation vector corresponding to the angular position change at time.

[0042] During the movement of the tunnel boring machine, the acceleration information on the three coordinate axes x, y, and z output by the fiber optic inertial navigation system at time k is expressed as:

[0043]

[0044] In formula (4), a b (k) represents the matrix form of the acceleration information of the three coordinate axes at time k; a bx represents the acceleration on the x-axis; a by represents the acceleration on the y-axis; a bz Indicates the z-axis acceleration.

[0045] According to the solution principle, it is first converted into the navigation coordinate system, and the following is obtained:

[0046]

[0047] In formula (5), a n (k) represents the acceleration information of the three coordinate axes at time k in the matrix form of the navigation coordinate system.

[0048] The rotation matrix from the inertial coordinate system to the navigation coordinate system at time k is used to calculate and update the attitude based on the gyroscope data. In order to eliminate the influence of the earth's gravity acceleration, it is necessary to compensate for the gravity acceleration component and then perform an integral operation, and finally integrate and calculate the velocity and displacement information.

[0049]

[0050] In formula (6), g represents the acceleration due to gravity, a g (k) represents the acceleration value after removing the influence of gravity acceleration.

[0051] First, the acceleration a g (k) Integrate to obtain the speed of the tunnel boring machine in the navigation coordinate system:

[0052] v n (k+1)=v n (k)+a g (k)Δt; (7)

[0053] In formula (7), v n (k+1) represents the speed of the tunnel boring machine in the navigation coordinate system at time k+1; v n (k) represents the speed of the tunnel boring machine in the navigation coordinate system at time k; Δt represents the time from time k to time k+1.

[0054] In formula (7), Δt = t(k+1)-t(k). By integrating formula (7), the position information can be obtained:

[0055]

[0056] In formula (8), S(k+1) represents the displacement of the tunnel boring machine in the navigation coordinate system at time k+1.

[0057] The position information is decomposed in the navigation coordinate system. This method can be used to calculate the posture information of the tunnel boring machine at the current moment. Combined with the initial posture information, a continuous motion trajectory can be obtained by iterative calculation in chronological order.

[0058] S1 can be used to obtain the position and posture information obtained by the machine vision system and the fiber optic inertial navigation system. The subsequent step is to align and fuse the two positions and posture information to accurately position the tunnel boring machine body.

[0059] S2: performing temporal and spatial data alignment on the first posture and the second posture to obtain a first aligned posture and a second aligned posture;

[0060] In S2, the two sets of posture data obtained in S1 are aligned in time and space. The time alignment is used to ensure the time synchronization between the machine vision system and the fiber optic inertial navigation system. The spatial alignment is used to synchronize the posture data in different coordinate systems to the same coordinate system to ensure the unification of the coordinate system of the cantilever tunnel boring machine body.

[0061] S2 may include:

[0062] (1) performing time alignment on the first posture and the second posture to obtain a first intermediate posture and a second intermediate posture;

[0063] Specifically, this embodiment takes into account the different sampling frequencies of the machine vision system and the fiber optic inertial navigation system for time synchronization, takes the sampling time of the machine vision system with a low sampling frequency as the reference time, and uses the least square method to align the posture data of the fiber optic inertial navigation system whose sampling frequency is an integer multiple of the sampling frequency of the machine vision system to the reference time. That is, first set the sampling period of the machine vision system to an integer multiple of the sampling period of the fiber optic inertial navigation system, then take the first posture as the first intermediate posture, and take the sampling time of the machine vision system as the reference time, and use the least square method to align the second posture to the reference time to obtain the second intermediate posture.

[0064] Among them, taking the sampling time of the machine vision system as the reference time, aligning the second posture to the reference time by the least square method, and obtaining the second intermediate posture may include: for each first sampling period of the machine vision system, obtaining n second postures obtained by the fiber optic inertial navigation system in the first sampling period, the sampling time of the nth second posture is the same as the end time of the first sampling period, and n is the ratio of the sampling period of the machine vision system to the sampling period of the fiber optic inertial navigation system. Then, fitting the n second postures by the least square method to obtain a virtual value, and using the virtual value as the second intermediate posture of the fiber optic inertial navigation system in the first sampling period, so that the data period of the fiber optic inertial navigation system can be the same as the sampling period of the machine vision system, and time alignment is achieved.

[0065] More specifically, the machine vision system is denoted as sensor A, and the fiber optic inertial navigation system is denoted as sensor B. The sampling periods of sensors A and B are τ and T respectively, and n = τ / T. If the update time of the first posture of sensor A is (k-1)τ, then the next update time is kτ = [(k-1)τ + nT], that is, sensor A measures twice in succession, and sensor B will make n measurements. The n second postures of sensor B from (k-1)τ to ​​kτ = [(k-1)τ + nT] are time-aligned using the least squares method, and fitted into a virtual value to replace the second posture value of sensor B at kτ = [(k-1)τ + nT], that is, the second intermediate posture of sensor B in the period is obtained. This virtual value is fused with the measurement value of sensor A, which can eliminate the problem of deviation in the state estimation of the tunnel boring machine caused by time asynchrony. For example, assuming τ=4, T=2, then n=2, sensor A obtains a data at 1s, and sensor A obtains a data at 5s; sensor B obtains a data at 1s, sensor B obtains a data at 3s, and sensor B obtains a data at 5s. The time alignment of this embodiment is to merge the data of sensor B at 3s and 5s to obtain a virtual value to replace the real data of sensor B at 5s, so that the data periods of sensor A and sensor B are equal, both separated by 4s, and the time alignment is completed.

[0066] Use Zn=(Z1,Z2,...Z n ) T represents the n second postures of sensor B from (k-1)τ to ​​kτ, then the virtual value obtained by fusing the n second postures of sensor B is:

[0067]

[0068] In formula (9), is a virtual value;

[0069] The following is an analysis of the error of the virtual value:

[0070] If used represents the second posture Z1, Z2, ... Z of the sensor B from (k-1)τ to ​​kτ n The fused virtual value and its derivative can be used to accurately obtain the actual measurement value Z of sensor B through formula (10): i :

[0071]

[0072] In formula (10), v i is the noise equation.

[0073] Converting formula (10) into vector form, we can get:

[0074] Z=W n Uz n , i=1,2,3,...,n; (11)

[0075] In formula (11), z represents the measurement value of sensor B.

[0076] The noise variance obtained by fusing the n second postures of sensor B is:

[0077]

[0078] In formula (12), represents the noise variance.

[0079] The virtual value is equivalent to the observed value after fitting. The observed values ​​of sensors A and B are aligned using the least squares method, and the size of the error is judged by the result of the noise variance for correction. The accuracy of the virtual value is determined by judging the size of the error.

[0080] Specifically, the least squares principle is used to further reduce the error and correct the virtual value. The process may include: combining the measurement noise V n =(V1, V2, ... V n ) T , whose mean is zero, and the previous measurement noise equation is integrated. First, the virtual value of sensor B is fitted to replace the measured value, and then the virtual value is integrated with the measured value of sensor A, which is represented by U here, in order to reduce the error through noise variance and least squares method:

[0081]

[0082] In formula (13), n refers to the n measurement sets of sensor B from (k-1)τ to ​​kτ; T is the sampling period of sensor B.

[0083] According to the least squares criterion, to minimize J, the two sides of J Taking the partial derivative and setting it equal to zero gives:

[0084]

[0085] In formula (14), J is the loss definition function; W n is the calculation coefficient of the conversion process; represents the measured values ​​Z1, Z2, ...Z of sensor B from (k-1)τ to ​​kτ n The fused virtual values ​​and their derivatives.

[0086] like Figure 4As shown in, it is the error curve after alignment. Figure 4 It can be seen that the alignment method of this embodiment has a relatively small error.

[0087] (2) Performing spatial alignment on the first intermediate posture and the second intermediate posture to obtain a first aligned posture and a second aligned posture.

[0088] Specifically, (2) may include:

[0089] (2.1) obtaining a first rotation matrix and a first translation matrix between a first coordinate system of a machine vision system and a body coordinate system, and a second rotation matrix and a second translation matrix between a second coordinate system of a fiber optic inertial navigation system and a body coordinate system through external parameter calibration;

[0090] (2.2) The first coordinate system is transformed into the body coordinate system through the first rotation matrix and the first translation matrix, and the second coordinate system is transformed into the body coordinate system through the second rotation matrix and the second translation matrix, so as to spatially align the first intermediate posture and the second intermediate posture. The posture of the first intermediate posture in the body coordinate system is the first posture after alignment, and the posture of the second intermediate posture in the body coordinate system is the second posture after alignment.

[0091] Combining the cantilever roadheader machine vision model and the cantilever roadheader fiber optic inertial navigation model, the cantilever roadheader body coordinate system is unified through external parameter calibration, in which the body coordinate system and the time coordinate system completely coincide. Figure 5 As shown. Assume that the measurement coordinate system of the machine vision system is c system, and the measurement coordinate system of the fiber optic inertial navigation system is O ins X ins Y ins Z ins The coordinate system is m, and the navigation coordinate system is n. Both the machine vision system and the fiber optic inertial navigation system are rigidly connected to the cantilever tunnel boring machine body and are fixed in position. The goal of spatial alignment is to transform the coordinate systems of the two systems into a unified body coordinate system b. Specifically, the coordinate system is transformed through the following rotation matrix and translation matrix, and all are transformed into the body coordinate system.

[0092]

[0093]

[0094] In the above formula, P c is the measurement coordinate system of the machine vision system; P ins It is the measurement coordinate system of the fiber optic inertial navigation system; is the body coordinate system; It is the rotation matrix and translation matrix between the measurement coordinate system of the machine vision system and the body coordinate system; It is the rotation matrix and translation matrix between the measurement coordinate system of the fiber optic inertial navigation system and the body coordinate system.

[0095] All of the above parameters can be obtained through external parameter calibration. At this time, the two positioning systems realize the unification of coordinate systems, that is, unified to the cantilever tunnel boring machine body coordinate system.

[0096] S3: Using a Kalman filtering method, data fusion is performed on the first aligned posture and the second aligned posture to obtain a fused posture.

[0097] After the machine vision system and the fiber optic inertial navigation system are aligned in time and space to measure the posture of the tunnel boring machine body, that is, after they are unified in time and space, the posture of the tunnel boring machine body is solved to achieve data fusion of machine vision posture measurement and fiber optic inertial navigation autonomous positioning, so as to achieve the purpose of improving positioning accuracy and high reliability.

[0098] Here, the principle of the data fusion process used in this embodiment is first introduced:

[0099] The data fusion strategy is used to achieve accurate motion estimation. The fusion strategy process is as follows: Figure 6 As shown. Usually, two fusion strategies, loose coupling and tight coupling, are used to realize multi-sensor data fusion processing. Loose coupling means using the data results of the sensor as the observation value to update the predicted value. It is relatively simple and has strong scalability. It can add multiple sensor information for fusion. Therefore, this embodiment adopts a loose coupling method to fuse the posture data of machine vision and fiber optic inertial navigation to realize the combined positioning of the cantilevered roadheader.

[0100] The state variables of the combined positioning system are expressed as a 15-dimensional column vector as follows:

[0101] x=[P k Φ k V k δ k ε k ] T (17)

[0102] In formula (17), P k , Φ k 、V k are the position, attitude and velocity calculated by fiber optic inertial navigation respectively; δ k , ε k They are the zero drift of the speedometer and gyroscope in the fiber optic inertial navigation system.

[0103] The process model (i.e., the motion model of the cantilever tunneling machine in this embodiment) is established by the output data of the fiber optic inertial navigation system. The two parameters of acceleration and angular velocity selected in this step are obtained by the two parameters contained in the above state vector. Specifically, the acceleration and angular velocity can be obtained by respectively derivation of the position and attitude, and then the acceleration a is converted into b and angular velocity As input, use u k express:

[0104]

[0105] Taking into account the actual measurement model of noise and acceleration after compensating for the influence of gravity, the machine vision system can measure the position and attitude of the target carrier and use its output to construct the observation equation of the extended Kalman filter as follows:

[0106]

[0107] In formula (18), Φ c，k represents the posture of the target carrier, P C，K represents the position of the target carrier, v c is the measurement noise of the machine vision system.

[0108] The above formula can be used to establish the form of extended Kalman filtering:

[0109] State transition matrix:

[0110]

[0111] In formula (19), f is the function of the machine vision system relative to displacement and velocity at time k; u k is the measurement value of sensor A at time k; x is the displacement; x k is the displacement matrix at time k.

[0112] Control input matrix:

[0113]

[0114] In formula (20), ω represents the angular velocity.

[0115] Observation matrix:

[0116]

[0117] Observation noise:

[0118]

[0119] In formula (22), v is the observation noise matrix at time k.

[0120] The extended Kalman filter solution equation is used to achieve data fusion of monocular vision posture measurement and fiber optic inertial navigation autonomous positioning.

[0121] Based on the above fusion principle, we can know the parameters contained in the state equation and observation equation during combined positioning. In the following, the motion mode of the cantilever roadheader is approximated as variable acceleration motion for data fusion analysis. In order to more accurately characterize the various motion states of the roadheader, the variable acceleration motion model is used as the motion model of the cantilever roadheader, and Kalman data fusion is performed to achieve high-precision combined positioning.

[0122] Before starting the fusion, first establish the state equation, prediction equation, observation equation and update equation involved in the fusion process. Specifically, establish the motion equation according to the motion model of the tunnel boring machine, establish the state matrix and observation matrix according to the motion equation, establish the state equation according to the state matrix, and establish the observation equation according to the observation matrix. Use the Kalman filter time update equation as the prediction equation, and the Kalman filter state update equation as the update equation. The state equation is used to describe the operating status of the equipment. The prediction equation is to predict the next point of the equipment operation according to the Kalman filter data fusion method during the data fusion process. The observation equation is used to describe the operating status of the equipment after data fusion. The update equation is used to update parameters such as the Kalman gain to improve the reliability of the data fusion results.

[0123] After establishing the above four equations, S3 may include:

[0124] (1) using state equations to describe the first alignment posture and the second alignment posture respectively, to obtain a first state and a second state;

[0125] (2) Taking the first state and the second state as input, the predicted posture is obtained using the prediction equation;

[0126] (3) Using the predicted pose as input, the fused pose is obtained using the observation equation;

[0127] (4) The prediction equation is updated using the update equation to obtain an updated prediction equation, and the updated prediction equation is used as the prediction equation for the next cycle to perform data fusion on the first aligned posture and the second aligned posture at the next moment.

[0128] The following describes the process of fusing two data of the same type by analyzing the changes in coordinates and the movement mode:

[0129] Assume that the displacement is x(k) and the velocity is The acceleration is The jerk is The following equation of motion is established:

[0130]

[0131] In formula (23), k is the current moment, (k-1) is the previous moment, and T is time. This motion equation is used to describe the motion mode of the tunnel boring machine. The subsequent state equation and observation equation are established based on this motion equation, mainly according to the coefficients of each parameter in the motion equation.

[0132] The above formula can be simplified as:

[0133]

[0134] By combining machine vision and inertial navigation to locate the position data in the x, y, and z directions in the system, the state transfer matrix is:

[0135]

[0136] Then the state equation is:

[0137]

[0138] Using the group sensor method, all measurements are fused into a synthetic observation equation. Assuming the observation value is Z(k), the observation matrix is:

[0139]

[0140] Then the observation equation is:

[0141]

[0142] In this embodiment, the Kalman filter time update equation (i.e., prediction equation) is as follows:

[0143]

[0144] In formula (29), is the predicted system state vector at time k, which is the result of the prediction equation; is the posterior state estimate at time k-1, which is one of the filtering results; A is the system dynamic transfer matrix; B is the acquisition matrix; u k-1 is the state Gaussian white noise sequence; is the uncertainty variance matrix of the previous estimate at time k, and is the intermediate calculation result of the filter; P k-1 is the uncertainty covariance matrix of the previous estimate at the current state; Q is the process noise covariance matrix.

[0145] The prediction equation is used to calculate the predicted coordinates of the roadheader.

[0146] In this embodiment, the Kalman filter state update equation (i.e., update equation) is as follows:

[0147]

[0148] In formula (30), K k is the Kalman gain; H is the observation matrix; R is the measurement uncertainty; is the estimated system state vector at time k; z k is the measured value; P k is the estimated uncertainty covariance matrix of the current state.

[0149] The update equation is obtained by updating K k , P k Three parameters are used to reduce the error and make the data fusion result closer to the actual situation.

[0150] This embodiment also uses matlab to simulate and analyze the combined positioning. Figure 7 are motion trajectory, observation trajectory and filter estimation graph, Figure 8 This is the error result diagram. According to the results, as time or sampling process increases, the filtering effect is stable, and compared with the two observation results, the error with the actual motion trajectory is smaller and closer to the true value.

[0151] Embodiment 2:

[0152] This embodiment is used to provide a multi-mode combined positioning system for mining excavation, such as Fig. 9 As shown, the combined positioning system comprises:

[0153] The posture measurement module M1 is used to obtain the first posture and the second posture obtained by the machine vision system and the fiber optic inertial navigation system respectively to measure the tunnel boring machine body;

[0154] A data alignment module M2, used for performing temporal and spatial data alignment on the first posture and the second posture to obtain a first aligned posture and a second aligned posture;

[0155] The data fusion module M3 is used to perform data fusion on the first aligned posture and the second aligned posture by using a Kalman filtering method to obtain a fused posture.

[0156] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the system disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.

[0157] The principles and implementation methods of the present invention are described in this article using specific examples. The description of the above embodiments is only used to help understand the method and core idea of ​​the present invention. At the same time, for those skilled in the art, according to the idea of ​​the present invention, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting the present invention.

Claims

1. A multi-mode combined positioning method for mining excavation, characterized in that: The combined positioning method comprises: Respectively obtain the first and second poses of the tunnel boring machine body measured by the machine vision system and the fiber optic inertial navigation system; Performing temporal and spatial data alignment on the first posture and the second posture to obtain a first aligned posture and a second aligned posture; Using a Kalman filter method to perform data fusion on the first aligned posture and the second aligned posture to obtain a fused posture; The first posture obtained by the machine vision system measuring the tunnel boring machine body specifically includes: The parallel laser beam image emitted by the laser pointing instrument is collected by using a camera fixed on the tunnel boring machine body; Solving the position and posture according to the parallel laser beam image to obtain the first position and posture; The step of performing temporal and spatial data alignment on the first posture and the second posture to obtain a first aligned posture and a second aligned posture specifically includes: Temporally aligning the first posture and the second posture to obtain a first intermediate posture and a second intermediate posture; Performing spatial alignment on the first intermediate posture and the second intermediate posture to obtain a first aligned posture and a second aligned posture; The step of temporally aligning the first posture and the second posture to obtain a first intermediate posture and a second intermediate posture specifically includes: Setting the sampling period of the machine vision system to an integer multiple of the sampling period of the fiber optic inertial navigation system; Using the first posture as a first intermediate posture; using the sampling time of the machine vision system as a reference time, aligning the second posture to the reference time using the least square method to obtain a second intermediate posture; Taking the sampling time of the machine vision system as the reference time, aligning the second posture to the reference time by the least square method, and obtaining the second intermediate posture specifically includes: For each first sampling period of the machine vision system, n second postures obtained by the fiber optic inertial navigation system in the first sampling period are obtained, and the sampling time of the nth second posture is the same as the end time of the first sampling period; n is the ratio of the sampling period of the machine vision system to the sampling period of the fiber optic inertial navigation system; Fitting the n second postures by using the least square method to obtain a virtual value, and using the virtual value as the second intermediate posture of the fiber-optic inertial navigation system within the first sampling period; in, is a virtual value; Z i is the ith actual measurement value of the fiber-optic inertial navigation system; The least squares principle is used to further reduce the error and correct the virtual value: Among them, J is the loss definition function; W n is the calculation coefficient of the conversion process; is a virtual value and its derivative; Z represents the measured value of the fiber optic inertial navigation system; The using of the Kalman filtering method to perform data fusion on the first aligned posture and the second aligned posture to obtain the fused posture specifically includes: Describing the first alignment posture and the second alignment posture by using state equations to obtain a first state and a second state; Taking the first state and the second state as input, using a prediction equation to obtain a predicted posture; Taking the predicted posture as input, obtaining the fused posture using the observation equation; The prediction equation is updated by using an update equation to obtain an updated prediction equation, and the updated prediction equation is used as the prediction equation for the next cycle to perform data fusion on the first aligned posture and the second aligned posture at the next moment; The prediction equation is: in, is the predicted system state vector at time k, which is the result of the prediction equation; is the posterior state estimate at time k-1, which is one of the filtering results; A is the system dynamic transfer matrix; B is the acquisition matrix; u k-1 is the state Gaussian white noise sequence; is the uncertainty variance matrix of the previous estimate at time k, and is the intermediate calculation result of the filter; P k-1 is the uncertainty covariance matrix of the previous estimate under the current state; Q is the process noise covariance matrix; The update equation is: Among them, K k is the Kalman gain; H is the observation matrix; R is the measurement uncertainty; is the estimated system state vector at time k; z k is the measured value; P k is the estimated uncertainty covariance matrix of the current state.

2. The combined positioning method according to claim 1, characterized in that: The second posture obtained by measuring the tunnel boring machine body using the fiber optic inertial navigation system specifically includes: The fiber optic inertial navigation system is used to measure the angular velocity and acceleration of the tunnel boring machine in three directions. Solving the posture according to the angular velocities and accelerations in the three directions to obtain a second posture; The n second postures are fitted by using the least square method to obtain a virtual value, and the virtual value is used as the second intermediate posture of the fiber-optic inertial navigation system in the first sampling period.

3. The combined positioning method according to claim 1, characterized in that: The spatially aligning the first intermediate posture and the second intermediate posture to obtain a first aligned posture and a second aligned posture specifically comprises: Obtaining a first rotation matrix and a first translation matrix between the first coordinate system of the machine vision system and the body coordinate system, and a second rotation matrix and a second translation matrix between the second coordinate system of the fiber optic inertial navigation system and the body coordinate system through external parameter calibration; The first coordinate system is transformed into the body coordinate system through the first rotation matrix and the first translation matrix, and the second coordinate system is transformed into the body coordinate system through the second rotation matrix and the second translation matrix, so as to spatially align the first intermediate posture and the second intermediate posture; the posture of the first intermediate posture in the body coordinate system is the first posture after alignment; the posture of the second intermediate posture in the body coordinate system is the second posture after alignment.

4. The combined positioning method according to claim 1, characterized in that: Before using state equations to describe the states of the first aligned posture and the second aligned posture respectively, the combined positioning method further includes: Establish the motion equation according to the motion model of the roadheader; Establishing a state matrix and an observation matrix according to the motion equation; A state equation is established according to the state matrix, and an observation equation is established according to the observation matrix.

5. A multi-mode combined positioning system for mining excavation, characterized in that: The combined positioning system comprises: The posture measurement module is used to obtain the first posture and the second posture obtained by the machine vision system and the fiber optic inertial navigation system respectively to measure the tunnel boring machine body; A data alignment module, used for performing temporal and spatial data alignment on the first posture and the second posture to obtain a first aligned posture and a second aligned posture; A data fusion module, used for performing data fusion on the first aligned posture and the second aligned posture by using a Kalman filtering method to obtain a fused posture; The first posture obtained by the machine vision system measuring the tunnel boring machine body specifically includes: The parallel laser beam image emitted by the laser pointing instrument is collected by using a camera fixed on the tunnel boring machine body; Solving the position and posture according to the parallel laser beam image to obtain the first position and posture; The step of performing temporal and spatial data alignment on the first posture and the second posture to obtain a first aligned posture and a second aligned posture specifically includes: Temporally aligning the first posture and the second posture to obtain a first intermediate posture and a second intermediate posture; Performing spatial alignment on the first intermediate posture and the second intermediate posture to obtain a first aligned posture and a second aligned posture; The step of temporally aligning the first posture and the second posture to obtain a first intermediate posture and a second intermediate posture specifically includes: Setting the sampling period of the machine vision system to an integer multiple of the sampling period of the fiber optic inertial navigation system; Using the first posture as a first intermediate posture; using the sampling time of the machine vision system as a reference time, aligning the second posture to the reference time using the least square method to obtain a second intermediate posture; Taking the sampling time of the machine vision system as the reference time, aligning the second posture to the reference time by the least square method, and obtaining the second intermediate posture specifically includes: For each first sampling period of the machine vision system, n second postures obtained by the fiber optic inertial navigation system in the first sampling period are obtained, and the sampling time of the nth second posture is the same as the end time of the first sampling period; n is the ratio of the sampling period of the machine vision system to the sampling period of the fiber optic inertial navigation system; Fitting the n second postures by using the least square method to obtain a virtual value, and using the virtual value as the second intermediate posture of the fiber-optic inertial navigation system within the first sampling period; in, is a virtual value; Z i is the ith actual measurement value of the fiber-optic inertial navigation system; The least squares principle is used to further reduce the error and correct the virtual value: Among them, J is the loss definition function; W n is the calculation coefficient of the conversion process; is a virtual value and its derivative; Z represents the measured value of the fiber optic inertial navigation system; The using of the Kalman filtering method to perform data fusion on the first aligned posture and the second aligned posture to obtain the fused posture specifically includes: Describing the first alignment posture and the second alignment posture by using state equations to obtain a first state and a second state; Taking the first state and the second state as input, using a prediction equation to obtain a predicted posture; Taking the predicted posture as input, obtaining the fused posture using the observation equation; The prediction equation is updated by using an update equation to obtain an updated prediction equation, and the updated prediction equation is used as the prediction equation for the next cycle to perform data fusion on the first aligned posture and the second aligned posture at the next moment; The prediction equation is: in, is the predicted system state vector at time k, which is the result of the prediction equation; is the posterior state estimate at time k-1, which is one of the filtering results; A is the system dynamic transfer matrix; B is the acquisition matrix; u k-1 is the state Gaussian white noise sequence; is the uncertainty variance matrix of the previous estimate at time k, and is the intermediate calculation result of the filter; P k-1 is the uncertainty covariance matrix of the previous estimate under the current state; Q is the process noise covariance matrix; The update equation is: Among them, K k is the Kalman gain; H is the observation matrix; R is the measurement uncertainty; is the estimated system state vector at time k; z k is the measured value; P k is the estimated uncertainty covariance matrix of the current state.

Images