A coal mine roadheader posture measurement method based on dual vectors

Abstract

The present invention discloses a method for measuring the attitude of a coal mine roadheader based on double vectors. The attitude measurement method is based on an inclinometer, a binocular camera, and a laser. The inclinometer and the binocular camera are arranged on the roadheader; the laser is along the designed axis of the tunnel. Step 1: In the inclinometer coordinate system s, a gravity vector model g is established according to the x and y axes output by the inclinometer. s ; Step 2: Through the binocular camera and the laser, the optical vector is calculated in the left camera coordinate system c, and through coordinate transformation, the optical vector model in the inclinometer coordinate system s is calculated and established; Step 3: According to the gravity vector model and the optical vector model, the attitude transformation matrix between the construction coordinate system n and the inclinometer coordinate system s is calculated by the double-vector attitude determination algorithm. The present invention has no data drift and error accumulation in the attitude measurement of the roadheader, does not require initial alignment, and has the advantages of high measurement accuracy, low cost, and small interference from the site.

Description

Technical field:

[0001] The invention belongs to the field of coal mine roadheader automation technology, and in particular relates to a coal mine roadheader posture measurement method based on double vectors. Background technology:

[0002] At present, in the process of coal mining, due to the uncertainty of posture sensing and measurement methods, it is still difficult for the roadheader to fully realize autonomous navigation. How to effectively obtain sensor information and solve posture in the closed, harsh and unknown underground coal mine tunnels is the core issue to realize autonomous navigation of the roadheader and free the underground construction workers from the high-risk and high-intensity mining labor as soon as possible. In most navigation algorithms, posture is the premise for solving the position. Compared with the position, the accurate solution of posture information is usually more difficult.

[0003] The existing methods for solving the problem of tunnel boring machine posture settlement can be divided into two categories. The first category is the "track guessing" method, represented by the inertial navigation system. Its characteristics are independence, autonomy, stability, reliability and high short-term accuracy. The disadvantage is that the error accumulates over time and the cost is high. The second category is the "absolute positioning" method, represented by measurement methods based on UWB, total station, vision, etc. that rely on the deployment of base stations. The advantage is high accuracy, and the disadvantage is that the large amount of dust generated during coal mining will reduce the imaging quality, and the "active" sensor system requires manual station setting and calibration, which is difficult to achieve in the complex and harsh coal mine construction environment.

[0004] In the current tunnel excavation construction, the quality of tunnel excavation is mainly guaranteed by manual work. The tunnel boring machine driver controls the excavation of the machine body while observing the light spot projected by the high-energy laser pointer on the front working surface, and adjusts the posture of the tunnel boring machine based on the position of the light spot. Summary of the invention:

[0005] The existing methods for measuring the attitude of coal mine tunneling machines all have shortcomings and defects to varying degrees. High-energy laser pointers are indispensable equipment in construction scenarios. If a pointing laser can be used to successfully complete the measurement task of the attitude of the coal mine tunneling machine body, it has been a long-term research project for those skilled in the art. The present invention proposes a method for measuring the attitude of a coal mine tunneling machine based on dual vectors. The attitude measurement algorithm has no data drift and error accumulation, does not require initial alignment, has the advantages of high measurement accuracy, low cost, and little interference from the site, and can replace the inertial navigation system in tunnel excavation, providing a theoretical basis and technical support for the future tunneling machine to achieve automatic navigation. The present invention draws on the measurement principle of the star sensor, the core sensor device in satellite attitude measurement, and uses a dual-vector attitude determination algorithm to achieve high-precision measurement of the spatial attitude of the tunneling machine coordinate system relative to the on-site construction coordinate system by fusing the image information obtained by the industrial camera and the dual-axis attitude information obtained by the inclinometer.

[0006] In order to solve the problems of the prior art, the present invention adopts the following technical solutions:

[0007] A coal mine roadheader posture measurement method based on dual vectors, the posture measurement method is based on an inclinometer, a binocular camera and a laser, the inclinometer and the binocular camera are arranged on the roadheader; the laser is along the tunnel design axis, and the posture measurement method comprises the following steps:

[0008] Step 1: In the inclinometer coordinate system s, establish the gravity vector model g according to the x and y axis outputs of the inclinometer s :

[0009]

[0010] Among them: the pitch angle β” and roll angle γ” of the gravity vector;

[0011] Step 2: Calculate the light vector in the left camera coordinate system c in the binocular camera, and calculate and establish the light vector model in the inclinometer coordinate system s through coordinate system transformation:

[0012]

[0013] in: is the rotation matrix, l c is the light plane intersection line in the left camera coordinate system;

[0014] Step 3: Establish a reference coordinate system d according to the gravity vector model and the light vector model, and solve the attitude transformation matrix between the inclinometer coordinate system and the construction coordinate system by using the dual-vector attitude determination algorithm, so as to obtain the attitude angle of the roadheader in the construction coordinate system n, including the azimuth angle α, the pitch angle β and the roll angle γ:

[0015]

[0016] in: is the attitude transformation matrix between the construction coordinate system n and the inclinometer coordinate system s.

[0017] Further, in step 1, the gravity vector model g is established by the x-axis and y-axis outputs of the inclinometer. s process:

[0018] 201. Assume that the outputs of the inclinometer x and y axes are γ' and β' respectively. According to the definition of the coordinate system, the pitch angle β' and roll angle γ' of the gravity vector are:

[0019]

[0020] 202. A gravity vector model is established according to the pitch angle β'' and the roll angle γ'' of the gravity vector.

[0021] Furthermore, in step 2, the light vector model is established by transforming and calculating between the binocular cameras in the left camera coordinate system c:

[0022] 301. Obtaining indicating laser parameters according to the optical center of the binocular camera and the laser line segment projected by the laser according to the Hough transform algorithm; the indicating laser parameters are discretized (ρ, θ) curves, where: ρ is the shortest distance from the origin of the coordinate system to the straight line, and θ is the angle between the x-axis and the straight line connecting the origin and the nearest point;

[0023] 302. Convert the indicating laser parameters to the left and right camera coordinate systems using the camera projection model, and establish the plane equations of the left optical plane in the left camera coordinate system and the right optical plane in the right camera coordinate system in combination with the left and right camera optical centers, respectively, to obtain the normal vector N of the left optical plane in the left camera coordinate system. lc and the normal vector N of the right light plane in the right camera coordinate system lr ;

[0024] 303. The normal vector of the right light plane is unified with the left camera coordinate system c according to the following formula:

[0025]

[0026] Among them, N rc is the normal vector of the right light plane in the left camera coordinate system, is the attitude transformation matrix between the left camera coordinate system and the right camera coordinate system, (N lr ) T N lr The transpose of

[0027] 304. According to the normal vectors of the left and right light planes in the left camera coordinate system c, the intersection line is solved according to the following formula:

[0028] l c =N lc ×N rc

[0029] Among them, l c is the light vector in the left camera coordinate system;

[0030] 305. Combine the rotation matrix between the inclinometer coordinate system s and the left camera coordinate system c Build a light vector model.

[0031] Further, the process of obtaining the attitude angle of the tunnel boring machine in the construction coordinate system n in step 3 includes the following steps:

[0032] 401. A reference coordinate system d is established according to the gravity vector model and the light vector model, and its orthogonal coordinate basis in the inclinometer coordinate system s is:

[0033]

[0034] 402. Attitude transformation matrix from inclinometer coordinate system s to reference coordinate system d for:

[0035]

[0036] Among them, a T is the transpose of a, b T is the transpose of b, c T is the transpose of c, (a T ,b T ,c T ) T for (a T ,b T ,c T )

[0037] 403. A reference coordinate system d is established according to the gravity vector model and the light vector model, and its orthogonal coordinate basis in the construction coordinate system n is:

[0038]

[0039] 404. Attitude transformation matrix from construction coordinate system n to reference coordinate system d for:

[0040]

[0041] Among them, A T is the transpose of A, B T is the transpose of B, C T is the transpose of C, (A T ,B T ,C T ) T For (A T ,B T ,C T )

[0042] 405. Obtain the attitude transformation matrix of the inclinometer coordinate system s relative to the construction coordinate system n according to the attitude transformation matrix from the inclinometer coordinate system s to the reference coordinate system d and the attitude transformation matrix from the construction coordinate system n to the reference coordinate system d.

[0043]

[0044] in, for Inverse of a matrix;

[0045] 406. The attitude angle of the tunnel boring machine in the construction coordinate system n is obtained through the attitude transformation matrix of the inclinometer coordinate system s relative to the construction coordinate system n, including the azimuth angle α, the pitch angle β and the roll angle γ.

[0046] Beneficial effects:

[0047] 1. The present invention proposes a method for realizing the posture calculation of the tunnel boring machine body by using inertial inclination measurement and binocular vision measurement technology. Compared with the inertial navigation system, this method does not produce data drift and error accumulation, does not require initial calibration, and has high measurement accuracy;

[0048] 2. The present invention is composed of a binocular camera and an inclinometer, which is small in size, low in cost, has the characteristics of long range and high precision, and has strong on-site application and promotion value, and can be applied to tunnel excavation sites;

[0049] 3. The present invention utilizes a binocular camera to identify laser line segments. When there is a lot of smoke and dust at the excavation site, due to the Tyndall effect, the light strip imaging effect is better and easier to identify. Therefore, this measurement scheme is more robust in a construction environment with high dust and low illumination. Description of the drawings:

[0050] Figure 1 It is a principle diagram of a coal mine roadheader posture measurement method based on dual vectors of the present invention;

[0051] Figure 2 It is a gravity vector model in the inclinometer coordinate system in a coal mine roadheader posture measurement method based on dual vectors of the present invention;

[0052] Figure 3 A schematic diagram of the positional relationship between the inclinometer and the horizontal plane according to the present invention;

[0053] Figure 4 It is a light vector model in a coal mine roadheader posture measurement method based on dual vectors of the present invention;

[0054] Figure 5 The present invention provides a coordinate system definition diagram for a coal mine roadheader posture measurement method based on dual vectors. Specific implementation method:

[0055] The present invention proposes a coal mine roadheader posture measurement method based on dual vectors. Figure 1 The implementation process of the patent of this invention is further described in detail.

[0056] Step 1: Calculate the gravity vector model using the inclinometer:

[0057] The mathematical expression of the gravity vector in the inclinometer coordinate system s is calculated based on the dual-axis output of the inclinometer. The coordinate system is established with the x and y axes of the dual-axis inclinometer, such as Figure 2 As shown, the position relationship between the inclinometer and the horizontal plane is as follows Figure 3 As shown. Assume that the outputs of the inclinometer x and y axes are γ' and β' respectively. According to the definition of the coordinate system, the pitch angle β' and roll angle γ' of the gravity vector are

[0058]

[0059] The gravity vector model can be expressed as

[0060]

[0061] Step 2: Calculate the light vector model using the binocular camera:

[0062] The calculation principle of light vector is as follows Figure 4 As shown, the optical centers of the left and right cameras are O c , O r The binocular camera shoots the indicator laser. Let the laser line segment captured by the left camera be AB and the laser line segment captured by the right camera be CD. c The left optical plane is established with the laser line segment AB, and the right camera optical center O is established r The right light plane is established with the laser line segment CD, and the indicator laser can be represented by the intersection line of the left and right light planes.

[0063] The present invention only needs to ensure that the binocular camera captures the indicating laser, and does not need to capture the same laser line segment. Therefore, the method is simple and easy to implement, and no theoretical error is generated in the process of capturing the target.

[0064] After the left and right cameras capture the laser line segments, the Hough transform algorithm is used to convert each pixel point (x, y) on the image into a discretized (ρ, θ) curve, where ρ is the shortest distance from the origin of the coordinate system to the straight line, and θ is the angle between the x-axis and the straight line connecting the origin and the nearest point. The number of pixels with the same (ρ, θ) curve is accumulated, and by setting a threshold, the pixel points with accumulated values ​​greater than the threshold are found to detect the laser line segments and obtain the mathematical expression of the indicator laser in each camera image coordinate system. According to the camera intrinsic parameters, it is converted to the camera coordinate system to obtain the plane equations of the left and right light planes. In this way, the normal vector N of the left light plane in the left camera coordinate system c can be obtained. lc and the normal vector N of the right light plane in the right camera coordinate system r lr .

[0065] To find the intersection line, the two normal vectors must be unified in the same coordinate system. Now they are unified in the left camera coordinate system c. According to the calibrated attitude transformation matrix from the right camera coordinate system r to the left camera coordinate system c The normal vector of the right light plane in the left camera coordinate system c is

[0066]

[0067] The intersection of the two light planes in the left camera coordinate system c is

[0068] l c =N lc ×N rc .

[0069] Combine the rotation matrix between the calibrated inclinometer coordinate system s and the left camera coordinate system c The intersection line of the inclinometer coordinate system s is

[0070]

[0071] Step 3: Double vector pose determination:

[0072] Combining the understanding of tunneling construction technology with existing guidance technology, on-site construction personnel currently use a high-energy laser with known azimuth information to guide the tunnel boring machine, that is, the light vector l of this laser is known in the construction coordinate system n. n X n Y n The surface is parallel to the geoid, so the gravity vector g is also a known quantity in the n system.

[0073] like Figure 5 As shown in the figure, during the coal mining process, the construction coordinate system is defined as (n system), and the tunnel boring machine carrier coordinate system is defined as O s -X s Y s Z s (s system), what needs to be done is to calculate the attitude transformation matrix of the inclinometer coordinate system s relative to the construction coordinate system n. That is, the data representation of the construction personnel's attitude control of the tunnel boring machine.

[0074] According to the existence of two vector models in space, namely the gravity vector model and the light vector model, which are expressed as g and l, they are expressed as g in the inclinometer coordinate system s and the construction coordinate system n respectively. s , g n , l s , l n (The subscript indicates the name of the coordinate system). The reference coordinate system d is established with these two light vector models, and the orthogonal coordinate basis is constructed in the inclinometer coordinate system s as follows:

[0075]

[0076] The attitude transformation matrix from the inclinometer coordinate system s to the reference coordinate system d is:

[0077]

[0078] Similarly, the coordinate orthogonal basis of the reference coordinate system d in the construction coordinate system n is:

[0079]

[0080] The attitude transformation matrix from the construction coordinate system n to the reference coordinate system d is:

[0081]

[0082] According to the attitude transformation matrix from the inclinometer coordinate system s to the reference coordinate system d and the attitude transformation matrix from the construction coordinate system n to the reference coordinate system d, the attitude transformation matrix of the inclinometer coordinate system s relative to the construction coordinate system n is obtained:

[0083]

[0084] The attitude angle of the tunnel boring machine in the construction coordinate system n is obtained through the attitude transformation matrix of the inclinometer coordinate system s relative to the construction coordinate system n, including the azimuth angle α, pitch angle β and roll angle γ.

[0085]

[0086] The present invention is not limited to the embodiments described above. The above description of the specific embodiments is intended to describe and illustrate the technical solution of the present invention. The above specific embodiments are merely illustrative and not restrictive. Without departing from the scope of the present invention and the scope of protection of the claims, a person of ordinary skill in the art can also make many forms of specific changes under the guidance of the present invention, which all fall within the scope of protection of the present invention.

Claims

1. A coal mine roadheader posture measurement method based on dual vectors, the posture measurement method is based on an inclinometer, a binocular camera and a laser, the inclinometer and the binocular camera are arranged on the roadheader; the laser is along the tunnel design axis, Features: The posture measurement method comprises the following steps: Step 1: In the inclinometer coordinate system s, establish the gravity vector model g according to the x and y axis outputs of the inclinometer s : Among them: the pitch angle β” and roll angle γ” of the gravity vector; Step 2: Calculate the light vector in the left camera coordinate system c in the binocular camera, and calculate and establish the light vector model in the inclinometer coordinate system s through coordinate system transformation: in: is the rotation matrix, l c is the light plane intersection line in the left camera coordinate system; Step 3: Establish a reference coordinate system d according to the gravity vector model and the light vector model, and solve the attitude transformation matrix between the inclinometer coordinate system and the construction coordinate system by using the dual-vector attitude determination algorithm, so as to obtain the attitude angle of the roadheader in the construction coordinate system n, including the azimuth angle α, the pitch angle β and the roll angle γ: in: is the attitude transformation matrix between the construction coordinate system n and the inclinometer coordinate system s.

2. A coal mine boring machine posture measurement method based on dual vectors according to claim 1, Features: In step 1, the gravity vector model g is established by the x and y axis outputs of the inclinometer. s Process:

201. Assume that the outputs of the inclinometer x and y axes are γ' and β' respectively; according to the definition of the coordinate system, the pitch angle β" and roll angle γ" of the gravity vector are respectively:

202. A gravity vector model is established according to the pitch angle β'' and the roll angle γ'' of the gravity vector.

3. A coal mine roadheader posture measurement method based on dual vectors according to claim 1, Features: In step 2, the light vector model is established by transforming and calculating between the binocular cameras in the left camera coordinate system c:

301. Obtaining indicating laser parameters according to the optical center of the binocular camera and the laser line segment projected by the laser according to the Hough transform algorithm; the indicating laser parameters are discretized (ρ, θ) curves, where: ρ is the shortest distance from the origin of the coordinate system to the straight line, and θ is the angle between the x-axis and the straight line connecting the origin and the nearest point; 302. Convert the indicating laser parameters to the left and right camera coordinate systems using the camera projection model, and establish the plane equations of the left optical plane in the left camera coordinate system and the right optical plane in the right camera coordinate system in combination with the left and right camera optical centers, respectively, to obtain the normal vector N of the left optical plane in the left camera coordinate system. lc and the normal vector N of the right light plane in the right camera coordinate system lr ; 303. The normal vector of the right light plane is unified with the left camera coordinate system c according to the following formula: Among them, N rc is the normal vector of the right light plane in the left camera coordinate system, is the attitude transformation matrix between the left camera coordinate system and the right camera coordinate system, (N lr ) T N lr The transpose of 304. According to the normal vectors of the left and right light planes in the left camera coordinate system c, the intersection line is solved according to the following formula: L c =N lc ×N rc Among them, l c is the light vector in the left camera coordinate system; 305. Combine the rotation matrix between the inclinometer coordinate system s and the left camera coordinate system c Build a light vector model.

4. A coal mine boring machine posture measurement method based on dual vectors according to claim 1, Features: The process of obtaining the attitude angle of the tunnel boring machine in the construction coordinate system n described in step 3 includes the following steps:

401. A reference coordinate system d is established according to the gravity vector model and the light vector model, and its orthogonal coordinate basis in the inclinometer coordinate system s is:

402. Attitude transformation matrix from inclinometer coordinate system s to reference coordinate system d for: Among them, a T is the transpose of a, b T is the transpose of b, c T is the transpose of c, (a T ,b T ,c T ) T for (a T ,b T ,c T ) 403. A reference coordinate system d is established according to the gravity vector model and the light vector model, and its orthogonal coordinate basis in the construction coordinate system n is:

404. Attitude transformation matrix from construction coordinate system n to reference coordinate system d for: Among them, A T is the transpose of A, B T is the transpose of B, C T is the transpose of C, (A T ,B T ,C T ) T For (A T ,B T ,C T ) 405. Obtain the attitude transformation matrix of the inclinometer coordinate system s relative to the construction coordinate system n according to the attitude transformation matrix from the inclinometer coordinate system s to the reference coordinate system d and the attitude transformation matrix from the construction coordinate system n to the reference coordinate system d. in, for Inverse of a matrix; 406. The attitude angle of the tunnel boring machine in the construction coordinate system n is obtained through the attitude transformation matrix of the inclinometer coordinate system s relative to the construction coordinate system n, including the azimuth angle α, the pitch angle β and the roll angle γ.

Images