Method and system for calibrating posture of roadheader

Abstract

The present invention relates to the technical field of attitude measurement of a roadheader, and provides an attitude calibration method and system for a roadheader, the method comprising: obtaining a first displacement vector of a first image acquisition device relative to a light spot on a laser target surface and a second displacement vector of a second image acquisition device relative to the light spot, wherein the first displacement vector and the second displacement vector are not parallel in space, the laser target is suspended in front of a laser pointer, and the laser pointer is used to form a light spot on the laser target surface; according to the first displacement vector, the second displacement vector and the first projection relationship between the image acquisition device coordinate system and the strapdown inertial navigation housing coordinate system, determining the installation matrix of the image acquisition device coordinate system relative to the strapdown inertial navigation; determining the installation angle of the image acquisition device coordinate system relative to the strapdown inertial navigation through the installation matrix, so as to calibrate the attitude information of the strapdown inertial navigation in the tunnel. Through the present invention, the technical problems of complicated operation and high cost of attitude calibration of a roadheader in the related art are solved.

Description

Technical Field

[0001] The present invention relates to the technical field of posture measurement of a roadheader, and in particular to a posture calibration method and system for a roadheader. Background Art

[0002] In the related art, the common method for calibrating the camera coordinate system and the strapdown inertial navigation coordinate system is to use the optical transfer method. Specifically, a standard hexahedron is installed on the camera housing, and the hexahedron is adjusted by professional equipment so that each face of the hexahedron is perpendicular to each axis of the camera coordinate system. At this time, the normal lines of each face of the hexahedron on the camera housing can represent each axis of the camera coordinate system. On the other hand, a standard hexahedron is also installed on the strapdown inertial navigation housing, and the hexahedron is adjusted by professional equipment so that each face of the hexahedron is perpendicular to each axis of the inertial navigation coordinate system. At this time, the normal lines of each face of the hexahedron on the inertial navigation housing can represent each axis of the inertial navigation coordinate system. After completing the above work, the inertial navigation is installed in the user system, and the angle relationship between the two hexahedrons can be measured by the theodolite, and the installation angle calibration is completed.

[0003] The above solution is very intuitive, but there are the following problems: the hexahedron on the camera housing must be installed and calibrated in advance in a professional optical laboratory, which is complicated and the test cost is also high. The hexahedron on the inertial navigation housing requires the inertial navigation manufacturer to install and calibrate in advance in a professional inertial technology laboratory, which is complicated, requires high professionalism, and the test cost is also high; in addition, the cost of the hexahedron is also high, and it is generally used in the military field with extremely high requirements for high precision.

[0004] Currently, no effective solution has been proposed for the technical problems existing in the above-mentioned related technologies. Summary of the invention

[0005] In view of the above problems, the present invention proposes a posture calibration method and system for a roadheader, so as to at least solve the technical problems of complicated posture calibration operation and high cost of a roadheader in the related art. The technical solution is as follows:

[0006] In a first aspect, a method for calibrating a posture of a roadheader is provided, comprising: obtaining a first displacement vector of a first image acquisition device relative to a light spot on a laser target surface, and a second displacement vector of a second image acquisition device relative to the light spot, wherein the first displacement vector and the second displacement vector are not parallel in space, and the laser target is suspended in front of a laser pointer, and the laser pointer is used to form the light spot on the laser target surface; determining an installation matrix of the image acquisition device coordinate system relative to the strapdown inertial navigation system according to the first displacement vector, the second displacement vector, and a first projection relationship of the image acquisition device coordinate system relative to the strapdown inertial navigation system housing coordinate system, wherein the strapdown inertial navigation system is arranged on the roadheader; and determining an installation angle of the image acquisition device coordinate system relative to the strapdown inertial navigation system through the installation matrix to calibrate the posture information of the strapdown inertial navigation system in the tunnel.

[0007] In a possible implementation, determining the installation matrix of the image acquisition device coordinate system relative to the strapdown inertial navigation system according to the first displacement vector, the second displacement vector, and a first projection relationship between the image acquisition device coordinate system and the strapdown inertial navigation system housing coordinate system includes: determining a first projection vector of the first displacement vector in the image acquisition device coordinate system and a second projection vector of the second displacement vector in the image acquisition device coordinate system based on a binocular vision algorithm, wherein the image acquisition device coordinate system is based on the center position of the first image acquisition device or the second image acquisition device as the origin and the optical axis direction as the z-axis. , a spatial rectangular coordinate system constructed with the focal plane and the z direction perpendicular to the x-axis; determining a third projection vector of the first displacement vector in the strapdown inertial navigation housing coordinate system, and a fourth projection vector of the second displacement vector in the strapdown inertial navigation housing coordinate system, wherein the strapdown inertial navigation housing coordinate system is a spatial rectangular coordinate system constructed with the housing of the strapdown inertial navigation as the origin, the forward direction of the tunnel boring machine as the y-axis, and the vertical plane of the tunnel boring machine and the vertical direction to the y-axis as the z-axis; calculating the installation matrix according to the first projection vector, the second projection vector, the third projection vector, the fourth projection vector and the first projection relationship.

[0008] In another possible implementation, determining a third projection vector of the first displacement vector in the strapdown inertial navigation housing coordinate system and a fourth projection vector of the second displacement vector in the strapdown inertial navigation housing coordinate system includes: obtaining a third displacement vector of the first image acquisition device relative to a laser ranging device; and obtaining a fourth displacement vector of the second image acquisition device relative to the laser ranging device; and obtaining a fifth displacement vector of the laser ranging device relative to the light spot, wherein the laser ranging device is disposed in the lane, between the first image acquisition device, the second image acquisition device and the laser target. ; Respectively measure the fifth projection vector, the sixth projection vector and the seventh projection vector corresponding to the third displacement vector, the fourth displacement vector and the fifth displacement vector in the tunnel coordinate system; calculate the third projection vector and the fourth projection vector according to the fifth projection vector, the sixth projection vector, the seventh projection vector, the second projection relationship of the geographic coordinate system relative to the tunnel coordinate system, and the third projection relationship of the strapdown inertial navigation housing coordinate system relative to the geographic coordinate system, wherein the tunnel coordinate system is a spatial rectangular coordinate system constructed with the laser pointer as the origin, the laser emission direction in the horizontal plane as the y-axis, and the vertical direction as the z-axis.

[0009] In another possible implementation, the third projection vector and the fourth projection vector are calculated according to the fifth projection vector, the sixth projection vector, the seventh projection vector, the second projection relationship of the geographic coordinate system relative to the roadway coordinate system, and the third projection relationship of the strapdown inertial navigation housing coordinate system relative to the geographic coordinate system, including: based on the vector synthesis law, calculating the eighth projection vector of the first image acquisition device in the roadway coordinate system and the ninth projection vector of the second image acquisition device in the roadway coordinate system according to the fifth projection vector, the sixth projection vector and the seventh projection vector; converting the eighth projection vector and the ninth projection vector to the strapdown inertial navigation housing coordinate system according to the second projection relationship and the third projection relationship to obtain the third projection vector and the fourth projection vector, wherein the second projection relationship and the third projection relationship are known quantities.

[0010] In another possible implementation, after calculating the installation matrix according to the first projection vector, the second projection vector, the third projection vector, the fourth projection vector and the first projection relationship, the method further includes: normalizing the first projection vector, the second projection vector, the third projection vector and the fourth projection vector, respectively; constructing a first normalized vector of the first image acquisition device and a second normalized vector of the second image acquisition device according to the normalization results, wherein the normalization results are both unit orthogonal matrices; and calibrating the installation matrix according to the first normalized vector and the second normalized vector.

[0011] In the second aspect, a posture calibration system for a roadheader is also provided, the system comprising at least a roadheader, a strapdown inertial navigation system, a first image acquisition device, a second image acquisition device, a laser target and a laser pointer, wherein the laser target and the laser pointer are both arranged in a tunnel, the laser target is suspended in front of the laser pointer, and the laser pointer is used to form a light spot on the laser target surface; the first image acquisition device and the second image acquisition device are both arranged on the roadheader, and are used to obtain a first displacement vector of the first image acquisition device relative to the light spot by acquiring the light spot image on the laser target. and a second displacement vector of the second image acquisition device relative to the light spot, wherein the first displacement vector and the second displacement vector are not parallel in space; the strapdown inertial navigation is arranged on the roadheader, and is used to determine the installation matrix of the image acquisition device coordinate system relative to the strapdown inertial navigation according to a first projection relationship between the image acquisition device coordinate system and the strapdown inertial navigation housing coordinate system, the first displacement vector and the second displacement vector, wherein the installation matrix is ​​used to characterize the installation angle of the image acquisition device coordinate system relative to the strapdown inertial navigation, so as to calibrate the posture information of the strapdown inertial navigation in the tunnel.

[0012] In a possible implementation, the system also includes a laser ranging device, wherein the laser ranging device is disposed in the tunnel and between the first image acquisition device, the second image acquisition device and the laser target, and is used to measure the displacement vector of the first image acquisition device, the second image acquisition device and the light spot on the laser target surface relative to the laser ranging device.

[0013] In another possible implementation, the laser ranging device is a total station.

[0014] In another possible implementation, the light emitted by the laser pointer is parallel to the horizontal plane direction set for the lane.

[0015] In another possible implementation, the laser target is installed on the top surface or side surface of the tunnel behind the tunnel boring machine.

[0016] By means of the above technical scheme, the posture calibration method of the roadheader provided by the embodiment of the present invention is provided with two image acquisition devices on the roadheader, a laser target and a laser pointer are provided in the tunnel, a light spot is formed on the laser target surface by the laser pointer, a strapdown inertial navigation is provided on the roadheader, and the roadheader is located in the tunnel; firstly, two displacement vectors of the two image acquisition devices relative to the light spot on the laser target surface are obtained, and the two displacement vectors are not parallel in space; then, according to the two displacement vectors and the projection relationship of the image acquisition device coordinate system relative to the strapdown inertial navigation housing coordinate system, the installation matrix of the image acquisition device coordinate system relative to the strapdown inertial navigation is determined, that is, the installation angle of the image acquisition device coordinate system relative to the strapdown inertial navigation is obtained, and the posture information of the strapdown inertial navigation in the tunnel is calibrated, the operation is simple, and the cost is low, and the technical problems of complex operation and high cost of posture calibration of the roadheader in the related art are solved, thereby improving the accuracy of the posture information of the strapdown inertial navigation in the tunnel. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for describing the embodiments of the present invention are briefly introduced below.

[0018] Figure 1 is a flow chart of a calibration method for a tunnel boring machine according to an embodiment of the present invention;

[0019] Figure 2 Schematic diagram of a posture measurement system for a roadheader based on binocular vision and strapdown inertial navigation according to an embodiment of the present invention;

[0020] Figure 3 It is a schematic diagram of a coordinate system calibration involved in the posture calibration of a roadheader provided according to an embodiment of the present invention;

[0021] Figure 4 It is a schematic diagram of the principle of the installation angle calibration method provided according to an embodiment of the present invention. DETAILED DESCRIPTION

[0022] The exemplary embodiments of the present invention will be described in more detail below with reference to the accompanying drawings. Although the exemplary embodiments of the present invention are shown in the accompanying drawings, it should be understood that the present invention can be implemented in various forms and should not be limited by the embodiments set forth herein. On the contrary, these embodiments are provided in order to enable a more thorough understanding of the present invention and to enable the scope of the present invention to be fully communicated to those skilled in the art.

[0023] It should be noted that the terms "first", "second", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that such usage is interchangeable where appropriate, so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the term "including" and its variants are to be interpreted as open-ended terms meaning "including but not limited to".

[0024] In order to solve the technical problems existing in the related art, a posture calibration method of a tunnel boring machine is provided in this embodiment. The technical scheme of the present invention and how the technical scheme of the present invention solves the above-mentioned technical problems are described in detail with specific embodiments below. The following specific embodiments can be combined with each other, and the same or similar concepts or processes may not be repeated in some embodiments. The embodiments of the present invention will be described below in conjunction with the accompanying drawings.

[0025] In one application scenario of this case, when a tunnel boring machine is used to dig a tunnel, it is required that the excavated tunnel is along the direction given by the laser pointer, and the distance between the upper, lower, left and right boundaries and the laser pointer light meets the predetermined requirements. By installing two cameras on the vehicle, a laser target is made of translucent material and hung in front of the laser pointer to ensure that the laser pointer can illuminate the laser target surface to form a light spot. Then, the light spot image is collected by two cameras at the same time, and the displacement of the vehicle relative to the light spot can be determined based on binocular vision technology. The displacement of the vehicle relative to the light spot determined by binocular vision is expressed in the camera coordinate system, and it must be converted to the tunnel coordinate system to be added to the light spot coordinates to obtain the coordinates of the vehicle in the tunnel coordinate system. If the installation angle between the camera coordinate system and the strapdown inertial navigation system that provides the heading information is known, it can be transformed into the tunnel coordinate system through the heading information, thereby determining the position information of the vehicle body in the tunnel coordinate system. How to accurately calibrate the installation angle of the camera coordinate system relative to the strapdown inertial navigation system is the key to the success of the above scheme.

[0026] The present invention provides a method for calibrating the posture of a roadheader. Figure 1 is a flow chart of a calibration method for a tunnel boring machine according to an embodiment of the present invention. Figure 1 As shown, the process includes the following steps:

[0027] Step S102, obtaining a first displacement vector of the first image acquisition device relative to the light spot on the laser target surface, and a second displacement vector of the second image acquisition device relative to the light spot;

[0028] In this embodiment, the first image acquisition device and the second image acquisition device are both arranged on the tunneling machine, the first displacement vector and the second displacement vector are not parallel in space, the laser target and the laser pointer are both arranged in the tunnel, the laser target is suspended in front of the laser pointer, and the laser pointer is used to form a light spot on the laser target surface; optionally, the laser target is installed on the top or side of the tunnel behind the tunneling machine.

[0029] Optionally, the image acquisition device in this embodiment is a camera.

[0030] In this embodiment, the non-parallel characteristics of the first displacement vector and the second displacement vector in space are used to complete the calibration of the image acquisition device coordinate system relative to the installation matrix of the strapdown inertial navigation. The laser pointer position information is introduced through this embodiment, replacing manual point alignment, which not only reduces manpower but also improves accuracy.

[0031] Step S104, determining an installation matrix of the image acquisition device coordinate system relative to the strapdown inertial navigation system according to the first displacement vector, the second displacement vector, and a first projection relationship between the image acquisition device coordinate system and the strapdown inertial navigation system housing coordinate system;

[0032] Among them, the strapdown inertial navigation is installed on the tunnel boring machine.

[0033] Step S106, determining the installation angle of the image acquisition device coordinate system relative to the strapdown inertial navigation system through the installation matrix, so as to calibrate the position and posture information of the strapdown inertial navigation system in the tunnel.

[0034] The method for calibrating the posture of a roadheader provided by an embodiment of the present invention comprises the following steps: arranging two image acquisition devices on the roadheader, arranging a laser target and a laser pointer in a tunnel, forming a light spot on the laser target surface by the laser pointer, and the strapdown inertial navigation is arranged on the roadheader, and the roadheader is located in the tunnel; firstly, obtaining two displacement vectors of the two image acquisition devices relative to the light spot on the laser target surface, and the two displacement vectors are not parallel in space; then, determining the installation matrix of the image acquisition device coordinate system relative to the strapdown inertial navigation according to the two displacement vectors and the projection relationship of the image acquisition device coordinate system relative to the strapdown inertial navigation housing coordinate system, that is, obtaining the installation angle of the image acquisition device coordinate system relative to the strapdown inertial navigation, realizing the calibration of the posture information of the strapdown inertial navigation in the tunnel, and having simple operation and low cost, solving the technical problems of complex operation and high cost of posture calibration of the roadheader in the related art, and thus improving the accuracy of the posture information of the strapdown inertial navigation in the tunnel.

[0035] In a possible implementation, determining the installation matrix of the image acquisition device coordinate system relative to the strapdown inertial navigation system according to the first displacement vector, the second displacement vector and the first projection relationship between the image acquisition device coordinate system and the strapdown inertial navigation system housing coordinate system includes: determining the first projection vector of the first displacement vector in the image acquisition device coordinate system and the second projection vector of the second displacement vector in the image acquisition device coordinate system based on a binocular vision algorithm, wherein the image acquisition device coordinate system is a spatial rectangular coordinate system constructed based on the center position of the first image acquisition device or the second image acquisition device as the origin, the optical axis direction as the z-axis, the focal plane and the direction perpendicular to the z-axis as the x-axis; determining the third projection vector of the first displacement vector in the strapdown inertial navigation system housing coordinate system and the fourth projection vector of the second displacement vector in the strapdown inertial navigation system housing coordinate system, wherein the strapdown inertial navigation system housing coordinate system is a spatial rectangular coordinate system constructed based on the housing of the strapdown inertial navigation system as the origin, the forward direction of the tunnel boring machine as the y-axis, and the vertical plane of the tunnel boring machine and the direction perpendicular to the y-axis as the z-axis; and calculating the installation matrix according to the first projection vector, the second projection vector, the third projection vector, the fourth projection vector and the first projection relationship.

[0036] In this embodiment, the use of binocular vision to calibrate the installation angle of the camera relative to the strapdown inertial navigation is the key to introducing the position information of the laser pointer. Among them, the principle of binocular detection is: by calculating the parallax of the two images, the distance of the scene in front (the range captured by the image) is directly measured without judging what type of obstacle appears in front. Therefore, for any type of obstacle, necessary warnings or braking can be carried out according to the change of distance information; the principle of the binocular camera is similar to that of the human eye. The human eye can perceive the distance of an object because the two eyes present different images of the same object, also known as "parallax". The farther the object is, the smaller the parallax; conversely, the larger the parallax. The principle of binocular vision generally involves two cameras (i.e., the above-mentioned image acquisition device) simultaneously obtaining two digital images of the object to be measured from different angles; and based on the parallax principle, the three-dimensional geometric information of the object is restored to reconstruct the three-dimensional contour and position of the object.

[0037] In one embodiment of the present case, the system Figure 2 As shown, Figure 2 It is a schematic diagram of a tunnel boring machine posture measurement system based on binocular vision and strapdown inertial navigation provided according to an embodiment of the present invention, the system includes a strapdown inertial navigation 2 installed on a tunnel boring machine body 1, a camera 3 (i.e., the above-mentioned first image acquisition device) and a camera 4 (i.e., the above-mentioned second image acquisition device) installed on the tunnel boring machine body 1, a laser target 5 installed on the tunnel, a laser pointer 6 installed on the tunnel, and a light spot 7 formed on the laser target surface.

[0038] In this embodiment, camera 3 and camera 4 measure the projection of vector r3 (i.e., the first displacement vector) and r4 (i.e., the second displacement vector) in the coordinate system c of camera 4 (i.e., the image acquisition device coordinate system) according to the binocular vision principle. (i.e. the first projection vector mentioned above) and (ie the second projection vector mentioned above). According to the projection relationship between r3 and r4 in the c and b systems (ie the first projection relationship mentioned above), formula (1) is obtained.

[0039]

[0040] In an optional embodiment of the present case, determining a third projection vector of the first displacement vector in the strapdown inertial navigation housing coordinate system and a fourth projection vector of the second displacement vector in the strapdown inertial navigation housing coordinate system includes: obtaining a third displacement vector of the first image acquisition device relative to the laser ranging device; and obtaining a fourth displacement vector of the second image acquisition device relative to the laser ranging device; and obtaining a fifth displacement vector of the laser ranging device relative to the light spot, wherein the laser ranging device is arranged in the tunnel and is located between the first image acquisition device, the second image acquisition device and the laser target; respectively measuring a fifth projection vector, a sixth projection vector and a seventh projection vector corresponding to the third displacement vector, the fourth displacement vector and the fifth displacement vector in the tunnel coordinate system; calculating the third projection vector and the fourth projection vector according to the fifth projection vector, the sixth projection vector, the seventh projection vector, the second projection relationship of the geographic coordinate system relative to the tunnel coordinate system, and the third projection relationship of the strapdown inertial navigation housing coordinate system relative to the geographic coordinate system, wherein the tunnel coordinate system is a spatial rectangular coordinate system constructed with the laser pointer as the origin, the laser emission direction in the horizontal plane as the y-axis, and the vertical direction as the z-axis.

[0041] In this embodiment, based on the above formula (1), if it can be measured (i.e. the third projection vector mentioned above) and (i.e. the fourth projection vector mentioned above), we can get the installation matrix, i.e. formula (2).

[0042]

[0043] In an optional embodiment of the present case, the third projection vector and the fourth projection vector are calculated according to the fifth projection vector, the sixth projection vector, the seventh projection vector, the second projection relationship of the geographic coordinate system relative to the tunnel coordinate system, and the third projection relationship of the strapdown inertial navigation housing coordinate system relative to the geographic coordinate system, including: based on the vector synthesis law, according to the fifth projection vector, the sixth projection vector and the seventh projection vector, calculating the eighth projection vector of the first image acquisition device in the tunnel coordinate system and the ninth projection vector of the second image acquisition device in the tunnel coordinate system; according to the second projection relationship and the third projection relationship, converting the eighth projection vector and the ninth projection vector to the strapdown inertial navigation housing coordinate system to obtain the third projection vector and the fourth projection vector, wherein the second projection relationship and the third projection relationship are known quantities.

[0044] The laser distance measuring device in this embodiment is preferably a total station. The projection of the displacement vector of the camera 3 relative to the light spot on the laser target surface (i.e., the second displacement vector) and the displacement vector of the camera 4 relative to the light spot on the laser target surface (i.e., the third displacement vector) in the lane coordinate system is indirectly measured by the total station.

[0045] Based on the above embodiment, in order to calibrate the installation matrix, a total station is used to calibrate the installation matrix. and Indirect measurement is performed by placing a total station between cameras 3 and 4 and the laser target surface. The total station can be used to calibrate the installation angle of the camera relative to the strapdown inertial navigation system. The operation is simple and convenient for on-site operation.

[0046] In an optional embodiment of the present case, after calculating the installation matrix according to the first projection vector, the second projection vector, the third projection vector, the fourth projection vector and the first projection relationship, it also includes: normalizing the first projection vector, the second projection vector, the third projection vector and the fourth projection vector respectively; constructing a first normalized vector of the first image acquisition device and a second normalized vector of the second image acquisition device according to the normalization results, wherein the normalization results are both unit orthogonal matrices; calibrating the installation matrix according to the first normalized vector and the second normalized vector.

[0047] By normalizing the first projection vector, the second projection vector, the third projection vector and the fourth projection vector in this embodiment, that is, orthogonalizing the matrix of each projection vector to obtain an orthogonal matrix to calculate the installation angle, the influence of the measurement error on the angle calibration result can be reduced.

[0048] The solution provided by the present invention is further described below in conjunction with a specific embodiment:

[0049] Figure 3 is a schematic diagram of a coordinate system calibration involved in the posture calibration of a tunnel boring machine provided by the present invention, for Figure 2 The system provided calibrates the coordinate system of each device in the system.

[0050] The origin of the lane coordinate system t is located on the laser pointer, and y t In the horizontal plane along the laser emission direction, z t The axis is vertically upward, x t ,y t and z t The coordinate system c of the camera 4 (i.e. the image acquisition device coordinate system) is referred to as the camera coordinate system, and its origin is located at the optical center of the camera. c Along the camera optical axis, x c To the right in the focal plane and with z c Vertical, x c ,y c and z c The strapdown inertial navigation housing coordinate system b is fixed to the strapdown inertial navigation housing, x b The y axis points to the right side of the vehicle body. b The axis points to the front of the vehicle body, z b The axis points upwards, x b ,y b and z b The geographic coordinate system n is Figure 3 It is not shown in the figure. The origin of this coordinate system coincides with the origin of the b system. n Pointing due east, y n Pointing to due north, z n Point to the sky, x n ,y n and z n Construct a right-handed rectangular coordinate system.

[0051] The tunnel boring machine needs the strapdown inertial navigation to provide its displacement information relative to the tunnel coordinate system (i.e. the above-mentioned posture information), as shown in formula (3):

[0052]

[0053] in: is the rotation matrix from the navigation system lane coordinate system n to the lane coordinate system t, which is the lane design value and is a known quantity; It is given by strapdown inertial navigation and is a known quantity; is the projection of the displacement vector of the strapdown inertial navigation relative to the camera 4 on the inertial navigation shell system b, which is a known quantity; is the projection of the displacement vector r4 of camera 4 relative to the light spot on the laser target surface in the camera coordinate system, measured by the binocular vision system composed of camera 3 and camera 4; It is the installation matrix of the camera coordinate system relative to the strapdown inertial navigation system, which must be determined by calibration method before the system is put into use.

[0054] The displacement vector r3 (first displacement vector) of the camera 3 relative to the light spot on the laser target surface and the displacement vector r4 (second displacement vector) of the camera 4 relative to the light spot on the laser target surface are not parallel. According to this characteristic, the present invention performs the installation matrix of the camera coordinate system c relative to the strapdown inertial navigation shell system b. Calibration.

[0055] Camera 3 and camera 4 measure the projection of vectors r3 and r4 in the coordinate system c (image acquisition device coordinate system) of camera 4 according to the binocular vision principle. (first projection vector) and (Second projection vector). According to the projection relationship between r3 and r4 in the c and b systems, the above formula (1) is obtained. and Afterwards, the above formula (2) can be calculated.

[0056] To calibrate the installation matrix using formula (2): Using total station (i.e. the third projection vector mentioned above) and (i.e. the fourth projection vector mentioned above) for indirect measurement. The measurement principle is shown in Figure 4 , Figure 4 It is a schematic diagram of the principle of the installation angle calibration method provided according to an embodiment of the present invention.

[0057] Figure 4 o is the location of the total station, and the output of the total station is (i.e. the fifth projection vector mentioned above), (i.e. the sixth projection vector mentioned above) and (i.e. the seventh projection vector mentioned above), that is Figure 3 Medium A (i.e. the third displacement vector mentioned above), r B (i.e. the fourth displacement vector mentioned above) and r C (i.e. the fifth displacement vector) is projected on the roadway coordinate system. According to the vector synthesis rule, we can get formula (4) and formula (5):

[0058]

[0059]

[0060] Further use and The two known quantities will be (i.e. the eighth projection vector mentioned above) and (i.e. the ninth projection vector) is transformed into the b system into formula (6) and formula (7):

[0061]

[0062]

[0063] Will and Substituting into formula (1) we can get The calibration results.

[0064] Considering and There are measurement errors, so the actual calibration algorithm first normalizes the above measurement results:

[0065]

[0066] Then construct a normalized vector orthogonal to v3 and v4 and

[0067]

[0068] but and The following relationship exists:

[0069]

[0070] From this we get:

[0071]

[0072] In summary, the working process of the present invention comprises the following steps:

[0073] Step 1: Based on the binocular vision principle, measure the projection of the displacement vector r3 of camera 3 relative to the light spot on the laser target surface in the coordinate system c of camera 4 See attached Figure 2 ;

[0074] Step 2: Based on the binocular vision principle, the projection of the displacement vector r4 of the camera 4 relative to the light spot on the laser target surface in the coordinate system c of the camera 4 is measured. See attached Figure 2 ;

[0075] Step 3: Place the total station between cameras 3 and 4 and the laser target surface, and measure the vector of the light spots on cameras 3, camera 4 and the laser target surface relative to the total station. and

[0076] Step 4: Calculate the projection of r3 and r4 in the roadway coordinate system and

[0077]

[0078]

[0079] Step 5: Calculate the projection of r3 and r4 in the inertial navigation shell coordinate system b and

[0080]

[0081]

[0082] Step 6: Calculation and The normalized vector of gets the unit orthogonal matrix:

[0083]

[0084] Step 7: Construct a normalized vector orthogonal to v3 and v4 and

[0085]

[0086] Step 8: Calculate the final calibration result

[0087]

[0088] The embodiments of the present invention have at least the following beneficial effects:

[0089] (1) Realize the introduction of laser pointer position information to replace manual point-to-point;

[0090] (2) Calibrating the installation angle of the binocular vision camera relative to the strapdown inertial navigation system is the key to introducing the position information of the laser pointing instrument;

[0091] (3) The camera installation angle relative to the strapdown inertial navigation system can be calibrated using a total station, which is simple to operate and convenient for on-site operation;

[0092] (4) The present invention does not require professional equipment such as a gyro north finder and a collimator, and thus has low cost.

[0093] Based on the posture calibration method of the tunnel boring machine provided in each of the above embodiments, based on the same inventive concept, in this embodiment, a posture calibration system of the tunnel boring machine is also provided, the system at least comprises a tunnel boring machine, a strapdown inertial navigation, a first image acquisition device, a second image acquisition device, a laser target and a laser pointer, wherein the laser target and the laser pointer are both arranged in the tunnel, the laser target is suspended in front of the laser pointer, and the laser pointer is used to form a light spot on the laser target surface; the first image acquisition device and the second image acquisition device are both arranged on the tunnel boring machine, and are used to obtain a light spot image on the laser target by acquiring the light spot image on the laser target. A first displacement vector of the first image acquisition device relative to the light spot and a second displacement vector of the second image acquisition device relative to the light spot are taken, wherein the first displacement vector and the second displacement vector are not parallel in space; a strapdown inertial navigation system is arranged on the roadheader, and is used to determine the installation matrix of the image acquisition device coordinate system relative to the strapdown inertial navigation system according to the first projection relationship of the image acquisition device coordinate system relative to the strapdown inertial navigation system housing coordinate system, the first displacement vector and the second displacement vector, wherein the installation matrix is ​​used to characterize the installation angle of the image acquisition device coordinate system relative to the strapdown inertial navigation system, so as to calibrate the position and posture information of the strapdown inertial navigation system in the tunnel.

[0094] In an optional embodiment of the present case, the system also includes a laser ranging device, wherein the laser ranging device is arranged in the tunnel and between the first image acquisition device, the second image acquisition device and the laser target, and is used to measure the displacement vector of the light spot on the first image acquisition device, the second image acquisition device and the laser target surface relative to the laser ranging device.

[0095] In an optional embodiment of the present case, the laser ranging device is a total station.

[0096] In an optional embodiment of the present case, the light emitted by the laser pointer is parallel to the horizontal plane direction set in the tunnel.

[0097] In an optional embodiment of the present case, the laser target is installed on the top or side of the tunnel behind the tunnel boring machine.

[0098] The system is used to implement the above method embodiments and preferred implementation modes, and those that have been described will not be repeated here.

[0099] Obviously, those skilled in the art should understand that the above modules or steps of the present invention can be implemented by a general computing device, they can be concentrated on a single computing device, or distributed on a network composed of multiple computing devices, and optionally, they can be implemented by a program code executable by a computing device, so that they can be stored in a storage device and executed by the computing device, and in some cases, the steps shown or described can be executed in a different order than here, or they can be made into individual integrated circuit modules, or multiple modules or steps therein can be made into a single integrated circuit module for implementation. Thus, the present invention is not limited to any specific combination of hardware and software.

[0100] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A method for calibrating the posture of a roadheader, characterized in that: include: Acquire a first displacement vector of the first image acquisition device relative to the light spot on the laser target surface, and a second displacement vector of the second image acquisition device relative to the light spot, wherein the first displacement vector and the second displacement vector are not parallel in space, the laser target is suspended in front of a laser pointing instrument, and the laser pointing instrument is used to form the light spot on the laser target surface; According to the first displacement vector, the second displacement vector and a first projection relationship between the image acquisition device coordinate system and the strapdown inertial navigation housing coordinate system, determining the installation matrix of the image acquisition device coordinate system relative to the strapdown inertial navigation, wherein the strapdown inertial navigation is arranged on the roadheader; according to the first displacement vector, the second displacement vector and a first projection relationship between the image acquisition device coordinate system and the strapdown inertial navigation housing coordinate system, determining the installation matrix of the image acquisition device coordinate system relative to the strapdown inertial navigation, comprising: Determine a first projection vector of the first displacement vector in the image acquisition device coordinate system and a second projection vector of the second displacement vector in the image acquisition device coordinate system based on a binocular vision algorithm, wherein the image acquisition device coordinate system is a spatial rectangular coordinate system constructed based on the center position of the first image acquisition device or the second image acquisition device as the origin, the optical axis direction as the z-axis, and the focal plane and the direction perpendicular to the z-axis as the x-axis; Determine a third projection vector of the first displacement vector in the strapdown inertial navigation housing coordinate system, and a fourth projection vector of the second displacement vector in the strapdown inertial navigation housing coordinate system, wherein the strapdown inertial navigation housing coordinate system is a spatial rectangular coordinate system constructed with the housing of the strapdown inertial navigation as the origin, the forward direction of the roadheader as the y-axis, and the vertical plane of the roadheader and the direction vertical to the y-axis as the z-axis; Calculate the installation matrix according to the first projection vector, the second projection vector, the third projection vector, the fourth projection vector and the first projection relationship; The installation angle of the image acquisition device coordinate system relative to the strapdown inertial navigation is determined by the installation matrix to calibrate the position and posture information of the strapdown inertial navigation in the lane.

2. The method according to claim 1, characterized in that Determining a third projection vector of the first displacement vector in the strapdown inertial navigation housing coordinate system and a fourth projection vector of the second displacement vector in the strapdown inertial navigation housing coordinate system comprises: Acquire a third displacement vector of the first image acquisition device relative to the laser distance measuring device; and, acquire a fourth displacement vector of the second image acquisition device relative to the laser distance measuring device; and, acquire a fifth displacement vector of the laser distance measuring device relative to the light spot, wherein the laser distance measuring device is disposed in the lane, between the first image acquisition device, the second image acquisition device and the laser target; Respectively measuring a fifth projection vector, a sixth projection vector and a seventh projection vector corresponding to the third displacement vector, the fourth displacement vector and the fifth displacement vector in the lane coordinate system; The third projection vector and the fourth projection vector are calculated according to the fifth projection vector, the sixth projection vector, the seventh projection vector, the second projection relationship of the geographic coordinate system relative to the tunnel coordinate system, and the third projection relationship of the strapdown inertial navigation housing coordinate system relative to the geographic coordinate system, wherein the tunnel coordinate system is a spatial rectangular coordinate system constructed with the laser pointer as the origin, the laser emission direction in the horizontal plane as the y-axis, and the vertical direction as the z-axis.

3. The method according to claim 2, characterized in that Calculating the third projection vector and the fourth projection vector according to the fifth projection vector, the sixth projection vector, the seventh projection vector, a second projection relationship of the geographic coordinate system relative to the roadway coordinate system, and a third projection relationship of the strapdown inertial navigation housing coordinate system relative to the geographic coordinate system includes: Based on the vector synthesis rule, calculate an eighth projection vector of the first image acquisition device in the lane coordinate system and a ninth projection vector of the second image acquisition device in the lane coordinate system according to the fifth projection vector, the sixth projection vector and the seventh projection vector; The eighth projection vector and the ninth projection vector are converted to the strapdown inertial navigation housing coordinate system according to the second projection relationship and the third projection relationship to obtain the third projection vector and the fourth projection vector, wherein the second projection relationship and the third projection relationship are known quantities.

4. The method according to claim 1, characterized in that After calculating the installation matrix according to the first projection vector, the second projection vector, the third projection vector, the fourth projection vector and the first projection relationship, the method further includes: Normalizing the first projection vector, the second projection vector, the third projection vector, and the fourth projection vector respectively; constructing a first normalized vector of the first image acquisition device and a second normalized vector of the second image acquisition device according to the normalization processing results, wherein the normalization processing results are both unit orthogonal matrices; The installation matrix is ​​calibrated according to the first normalized vector and the second normalized vector.

5. A posture calibration system for a roadheader, characterized in that: The system at least includes a roadheader, a strapdown inertial navigation system, a first image acquisition device, a second image acquisition device, a laser target and a laser pointing device, wherein: The laser target and the laser pointer are both arranged in the lane, the laser target is suspended in front of the laser pointer, and the laser pointer is used to form a light spot on the laser target surface; The first image acquisition device and the second image acquisition device are both arranged on the roadheader, and are used to acquire a first displacement vector of the first image acquisition device relative to the light spot and a second displacement vector of the second image acquisition device relative to the light spot by acquiring a light spot image on the laser target, wherein the first displacement vector and the second displacement vector are not parallel in space; The strapdown inertial navigation system is provided on the tunnel boring machine and is used to determine the installation matrix of the image acquisition device coordinate system relative to the strapdown inertial navigation system according to the first projection relationship of the image acquisition device coordinate system relative to the strapdown inertial navigation system housing coordinate system, the first displacement vector and the second displacement vector, wherein the installation matrix is ​​used to characterize the installation angle of the image acquisition device coordinate system relative to the strapdown inertial navigation system to calibrate the position information of the strapdown inertial navigation system in the tunnel; the method of determining the installation matrix of the image acquisition device coordinate system relative to the strapdown inertial navigation system according to the first projection relationship of the image acquisition device coordinate system relative to the strapdown inertial navigation system housing coordinate system, the first displacement vector and the second displacement vector comprises: Determine a first projection vector of the first displacement vector in the image acquisition device coordinate system and a second projection vector of the second displacement vector in the image acquisition device coordinate system based on a binocular vision algorithm, wherein the image acquisition device coordinate system is a spatial rectangular coordinate system constructed based on the center position of the first image acquisition device or the second image acquisition device as the origin, the optical axis direction as the z-axis, and the focal plane and the direction perpendicular to the z-axis as the x-axis; Determine a third projection vector of the first displacement vector in the strapdown inertial navigation housing coordinate system, and a fourth projection vector of the second displacement vector in the strapdown inertial navigation housing coordinate system, wherein the strapdown inertial navigation housing coordinate system is a spatial rectangular coordinate system constructed with the housing of the strapdown inertial navigation as the origin, the forward direction of the roadheader as the y-axis, and the vertical plane of the roadheader and the direction vertical to the y-axis as the z-axis; The installation matrix is ​​calculated according to the first projection vector, the second projection vector, the third projection vector, the fourth projection vector and the first projection relationship.

6. The system according to claim 5, characterized in that The system also includes a laser distance measuring device, wherein: The laser ranging device is arranged in the lane and between the first image acquisition device, the second image acquisition device and the laser target, and is used to measure the displacement vector of the light spot on the first image acquisition device, the second image acquisition device and the laser target surface relative to the laser ranging device.

7. The system according to claim 6, characterized in that The laser distance measuring device is a total station.

8. The system according to claim 5, characterized in that The light emitted by the laser pointer is parallel to the horizontal plane direction set in the lane.

9. The system according to claim 5, characterized in that The laser target is installed on the top surface or side surface of the tunnel behind the tunnel boring machine.

Images