CN111337025A - Positioning and orientating instrument hole positioning method suitable for long-distance horizontal core drilling machine - Google Patents

Positioning and orientating instrument hole positioning method suitable for long-distance horizontal core drilling machine Download PDF

Info

Publication number
CN111337025A
CN111337025A CN202010349576.4A CN202010349576A CN111337025A CN 111337025 A CN111337025 A CN 111337025A CN 202010349576 A CN202010349576 A CN 202010349576A CN 111337025 A CN111337025 A CN 111337025A
Authority
CN
China
Prior art keywords
positioning
error
representing
instrument
speed
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202010349576.4A
Other languages
Chinese (zh)
Other versions
CN111337025B (en
Inventor
王林
张永健
高春峰
罗晖
袁保伦
魏国
李耿
樊振方
熊振宇
周盟孟
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
National University of Defense Technology
Original Assignee
National University of Defense Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by National University of Defense Technology filed Critical National University of Defense Technology
Priority to CN202010349576.4A priority Critical patent/CN111337025B/en
Publication of CN111337025A publication Critical patent/CN111337025A/en
Application granted granted Critical
Publication of CN111337025B publication Critical patent/CN111337025B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • E—FIXED CONSTRUCTIONS
    • E21—EARTH OR ROCK DRILLING; MINING
    • E21B—EARTH OR ROCK DRILLING; OBTAINING OIL, GAS, WATER, SOLUBLE OR MELTABLE MATERIALS OR A SLURRY OF MINERALS FROM WELLS
    • E21B47/00—Survey of boreholes or wells
    • E21B47/09—Locating or determining the position of objects in boreholes or wells, e.g. the position of an extending arm; Identifying the free or blocked portions of pipes
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • Geology (AREA)
  • Automation & Control Theory (AREA)
  • General Physics & Mathematics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Mining & Mineral Resources (AREA)
  • Geophysics (AREA)
  • Fluid Mechanics (AREA)
  • Environmental & Geological Engineering (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Geochemistry & Mineralogy (AREA)
  • Navigation (AREA)

Abstract

The invention belongs to the field of geological exploration and discloses a positioning and orientating instrument hole positioning method suitable for a long-distance horizontal coring drilling machine. The invention combines the working characteristics of a long-distance horizontal coring drilling machine, reasonably plans the working mode of a coring device, fully utilizes all the measurement data stored by a positioning orientation instrument to construct a reverse measurement data sequence, carries out forward and reverse autonomous navigation positioning based on an incomplete constraint Kalman filter, combines zero-speed correction to inhibit and correct positioning errors, utilizes the difference complementarity of the forward and reverse autonomous navigation positioning error characteristics, and takes an average positioning result as a drilling path track to improve the positioning precision. The method can meet the requirement of geological exploration in the plateau alpine region, and provides powerful support for comprehensively and accurately mastering the geological information along the railway in the plateau alpine region.

Description

Positioning and orientating instrument hole positioning method suitable for long-distance horizontal core drilling machine
Technical Field
The invention belongs to the field of geological exploration, relates to an in-hole positioning method of a core drilling machine in an exploration process, and particularly relates to an in-hole positioning method of a positioning and orienting instrument suitable for a long-distance horizontal core drilling machine.
Background
The Sichuan-Tibet railway is a rapid railway connecting Sichuan provinces and Tibet autonomous regions in China, and the construction process needs to face the construction problems of great mountains and mountains, terrain height difference, complex geology and the like, so that the construction difficulty is great. In order to eliminate the height difference of the terrain, more high bridges and tunnels are needed, so that the tunnel ratio of the full-line bridge of the Sichuan-Tibet railway reaches more than 80 percent. In order to ensure the construction quality, in the tunnel construction process, geological information along the line needs to be comprehensively and accurately mastered, and general vertical detection cannot be implemented and cannot meet the geological detection requirement in the plateau alpine region due to the limitation of severe ground conditions in the plateau alpine region.
The long-distance horizontal core drilling machine is equipment suitable for horizontal direction rapid geological drilling and long-distance horizontal core drilling, can meet the requirement of long-distance geological core drilling of various rock tunnel constructions in the fields of railways, roads, water conservancy, mines and the like, and has very important significance for the construction of railways in plateau alpine regions. For accurate control drilling path, satisfy the demand of big buried depth, long distance geological core drilling, need carry out accurate location orientation to horizontal coring rig downthehole, for this reason, long distance horizontal coring rig needs dispose the location orientation appearance simultaneously. The positioning and orienting instrument comprises an inertia measuring unit, and the inertia measuring unit calculates and outputs the position information of the horizontal coring drilling machine so as to adjust and control the drilling path. However, under the condition of long-distance horizontal drilling, when no external information is assisted in a hole, the positioning error of the inertial measurement unit is continuously increased along with the extension of the working time, and the control precision of a drilling path is influenced.
Therefore, aiming at the demand of geological exploration in plateau alpine regions, a positioning and orientation instrument hole positioning method suitable for the long-distance horizontal core drilling machine needs to be researched, the working characteristics of the long-distance horizontal core drilling machine are combined, the accurate positioning in the hole of the long-distance horizontal core drilling machine under the condition of no external information assistance is realized, and the control precision of a drilling path is further improved.
Disclosure of Invention
The technical problem to be solved by the invention is as follows: the working characteristics of the long-distance horizontal core drilling machine are combined, the working mode of the core drilling machine is reasonably planned, all measurement data stored by the positioning orientation instrument are fully utilized, the accurate positioning in the hole of the long-distance horizontal core drilling machine under the condition of no external information assistance is realized, the control precision of a drilling path is further improved, and the requirement of geological exploration in plateau alpine regions is met.
In order to solve the technical problems, the solution proposed by the invention is as follows:
the positioning and orientating instrument hole positioning method suitable for the long-distance horizontal coring drilling machine comprises the following steps:
(1) installing a positioning and orienting instrument on a core taking device, placing the positioning and orienting instrument on a base of a carrier vehicle of a core taking drilling machine, binding initial position information and initial speed information to the positioning and orienting instrument, standing the positioning and orienting instrument for 15-25 minutes after binding is completed, performing initial self-alignment to obtain initial posture information, and storing angle increment information and speed increment information obtained by measurement of an inertial measurement unit by the positioning and orienting instrument on line; after the initial self-alignment is finished by the positioning and orientation instrument, forward autonomous navigation positioning is carried out based on the incomplete constraint Kalman filter; the method comprises the following steps of:
(1.1) with an attitude error phinVelocity error δ vnPosition, positionPosition error deltapnGyro drift epsilonbAccelerometer zero offset
Figure BDA00024713722900000213
The installation error η of the inertial measurement unit of the positioning and orientation device is the system state x (t), and the differential equations respectively determining the attitude error, the velocity error, the gyro drift, the accelerometer zero offset and the installation error are as follows:
Figure BDA0002471372290000021
wherein,
Figure BDA0002471372290000022
representing the angular velocity of rotation of the navigation coordinate system n relative to the inertial coordinate system i,
Figure BDA0002471372290000023
representing the attitude matrix between the carrier coordinate system b and the navigation coordinate system n, fnIndicating the specific force represented in the navigational coordinate system,
Figure BDA0002471372290000024
the angular velocity of rotation of the earth is represented,
Figure BDA0002471372290000025
indicating the transfer angular velocity, vnThe speed is indicated in the form of a speed,
Figure BDA0002471372290000026
respectively representing the rotational angular velocity error, the earth rotational angular velocity error and the transfer angular velocity error of the navigation coordinate system,
Figure BDA0002471372290000027
the measurement error of the gyro component is shown,
Figure BDA0002471372290000028
representing the measurement error of the accelerometer assembly, wg、waRespectively representing the measurement noise and acceleration of the gyro assemblyThe assembly measures the noise, η ═ ηθηΨ]TInstallation error by pitch angle ηθAnd heading angle installation error ηΨForming;
(1.2) according to the attitude error, the speed error, the position error, the gyro drift, the accelerometer zero offset and the installation error differential equation determined in the step (1.1), constructing a system state equation as follows:
Figure BDA0002471372290000029
wherein,
Figure BDA00024713722900000210
representing a system state matrix;
Figure BDA00024713722900000211
Figure BDA00024713722900000212
Figure BDA0002471372290000031
Figure BDA0002471372290000032
in the formula, vE、vN、vURespectively representing east, north and vertical speeds, L representing local latitude, h representing local height, R representing local altitudeE、RNRespectively represent the radius of the prime circle, the radius of the meridian circle, omegaieRepresenting the earth rotation angular velocity module value;
Figure BDA0002471372290000033
representing a system noise matrix;
w(t)＝[wgwa]Trepresenting system noise;
(1.3) the lateral and vertical velocities of the corer are zero as it advances along the drilling path, with a lateral velocity error δ vxAnd vertical velocity error δ vzConstruction of an incomplete constraint observation z (t) ═ δ vxδvz]TAnd determining an observation equation;
(2) pushing the core taking device to a drill hole by a core taking drilling machine carrier vehicle and staying, staying the core taking device at the drill hole for 10-20 seconds, completing the first zero speed correction of the positioning and orienting instrument, then pushing the core taking device along the drilling path in a high-pressure air pushing mode, and storing angle increment information and speed increment information obtained by measurement of an inertia measurement unit on line by the positioning and orienting instrument;
(3) when the coring device is pushed to a drilling machine at the tail end of the drilling path, the coring device finishes coring operation, then the coring device stays at the tail end of the drilling path for 10-20 seconds again, and the second zero-speed correction of the positioning and orienting instrument is finished; in addition, the positioning and orientation instrument stores angle increment information and speed increment information measured by the inertial measurement unit on line;
(4) after the second zero-speed correction is finished, pulling the coring device out of the hole through the winch, and when the coring device is pulled to the drill hole, staying for 10-20 seconds again, and finishing the third zero-speed correction of the positioning and orienting instrument; after the third zero-speed correction is completed, dragging the coring device to a base of a coring drilling machine vehicle, and standing for 15-25 minutes again; in addition, the positioning and orientation instrument stores angle increment information and speed increment information measured by the inertial measurement unit on line;
(5) measuring angular velocity of gyro assembly
Figure BDA0002471372290000034
Accelerometer assembly measuring specific force fbAnd the rotational angular velocity of the earth
Figure BDA0002471372290000035
Taking the inverse, reversing the angle increment information and the speed increment information stored in the steps (1) to (4) according to the time sequence from the back to the front to form a reverse measurement data sequence, and performing reverse autonomous measurement based on the incomplete constraint Kalman filterNavigation positioning, wherein the reverse navigation solution is described as follows:
Figure BDA0002471372290000041
Figure BDA0002471372290000042
Figure BDA0002471372290000043
wherein,
Figure BDA0002471372290000044
Figure BDA0002471372290000045
respectively representing the attitude matrices at the time of reverse time sequence d and d-1,
Figure BDA0002471372290000046
respectively representing the speeds at the time points of the reverse sequences d and d-1,
Figure BDA0002471372290000047
respectively representing the positions of the moments d, d-1 in reverse sequence, gn、
Figure BDA0002471372290000048
Respectively represents forward solving and backward solving of the local gravitational acceleration, I3Representing a third order identity matrix, △ T representing a sampling interval;
(6) positioning result p with forward autonomous navigation after time alignmentnAnd reverse autonomous navigation positioning results
Figure BDA0002471372290000049
Mean value of
Figure BDA00024713722900000410
As an averageAnd positioning, namely taking the positioning result of the advancing section of the coring device as a drilling path track, calculating the deviation of the advancing section of the coring device relative to the designed path according to the drilling path track, and further adjusting the drilling path.
Further, the determination of the observation equation in the step (1.3) is realized by the following steps:
(1.3.1) projecting the position and orientation machine velocity output to the inertial measurement unit coordinate system m as follows:
Figure BDA00024713722900000411
wherein,
Figure BDA00024713722900000412
representing the projection, v, of the velocity output of the position and orientation finder in the inertial measurement unit coordinate system mmRepresenting the speed of the position and orientation device expressed in a real m coordinate system,
Figure BDA00024713722900000413
a matrix representing the installation relationship between the carrier coordinate system b and the inertial measurement unit coordinate system m, ζ ═ ηθηγηΨ]TThe angle of the installation error is indicated,
Figure BDA00024713722900000414
representing the attitude matrix between the navigation coordinate system n and the carrier coordinate system b, due to roll angle mounting error ηγThe forward velocity projection is assigned a value of 0, η, without affecting the forward velocity projectionγ＝0；
(1.3.2) by
Figure BDA00024713722900000415
As the incomplete constraint observation quantity z (t), the observation equation is constructed as follows:
z(t)＝H(t)x(t)+υ(t) (4)
wherein,
Figure BDA00024713722900000416
and M1＝[1 0 0]，M3＝[0 0 1]，
Figure BDA00024713722900000417
V (t) represents the observation noise.
Further, the zero-speed correction in the step (2) is performed according to the following steps:
(2.1) the positioning and orientation instrument automatically detects the zero speed state according to the angular speed and speed information, and the forward speed error delta vyThe amplification is observed quantity, and the observed quantity in the zero-speed state is zZUPT(t)＝[δvxδvyδvz]T；
(2.2) by
Figure BDA0002471372290000052
As an observed quantity z in a zero velocity stateZUPT(t), constructing an observation equation as follows:
zZUPT(t)＝HZUPT(t)x(t)+μ(t) (5)
wherein,
Figure BDA0002471372290000051
μ (t) represents observation noise;
and (2.3) when the positioning and orientation instrument is in a zero-speed state, switching the Kalman filter observation equation from the equation (4) to the equation (5), and finishing measurement updating by adopting a sequential processing mode.
Further, the reverse autonomous navigation positioning in the step (5) comprises the following steps:
(5.1) completing initial alignment by using the first static stage of 15-25 minutes of reverse measurement data to obtain initial attitude information;
(5.2) performing reverse filtering according to the incomplete constraint Kalman filter in the steps (1.1) - (1.3), wherein an attitude error equation, a speed error equation, a position error equation, gyro drift, acceleration zero offset and a differential equation of installation errors of the inertial measurement unit of the positioning and orientation instrument are kept unchanged, and an observation equation is also kept unchanged;
and (5.3) in the reverse autonomous navigation positioning process, when the coring device is in a resident state, carrying out error correction according to the zero speed correction mode of the steps (2.1) to (2.3).
Further, the rest time of the coring device on the vehicle-mounted base of the coring drilling machine in the steps (1) and (4) is 15 minutes respectively.
Further, the rest time of the coring device on the vehicle-mounted base of the coring drilling machine in the steps (1) and (4) is 25 minutes respectively.
Further, the residence time of the coring device at the drilling hole and at the end of the drilling path in the steps (2), (3) and (4) is 10 seconds respectively.
Further, the residence time of the coring device at the drilling hole and at the end of the drilling path in the steps (2), (3) and (4) is 20 seconds respectively.
Further, in the step (2), the coring device is pushed along the drilling path in a high-pressure hydraulic pushing mode.
Further, feedback correction is adopted for gyro drift and accelerometer zero-offset states in the steps (1.1) and (1.2).
Further, the installation error state in the steps (1.1) and (1.2) adopts open loop correction.
Further, the sampling interval of the inertial measurement unit in step (1) when measuring the angular increment information and the velocity increment information is not more than 0.01 s.
Compared with the prior art, the invention has the advantages that:
(1) the invention combines the working characteristics of the long-distance horizontal coring drilling machine, reasonably plans the working mode of the coring device, fully utilizes all the measurement data stored by the positioning orientation instrument, improves the data utilization rate, and realizes the accurate positioning in the hole of the long-distance horizontal coring drilling machine without the assistance of external information;
(2) the method fully utilizes the difference complementarity of the forward and reverse autonomous navigation positioning error characteristics, takes the average positioning result as the drilling path track, and improves the positioning accuracy.
Drawings
FIG. 1 is a schematic flow diagram of the process of the present invention;
FIG. 2 is a schematic diagram of an embodiment of the present invention;
FIG. 3 is a schematic diagram of forward and reverse solution of the present invention;
FIG. 4 is a schematic diagram of forward and reverse autonomous navigation positioning according to the present invention.
Detailed Description
The invention will be described in further detail below with reference to the drawings and specific examples.
As shown in figure 1, the positioning and orientating instrument hole positioning method suitable for the long-distance horizontal core drilling machine reasonably plans the working mode of the core taking machine by combining the working characteristics of the long-distance horizontal core drilling machine, fully utilizes all the measurement data stored by the positioning and orientating instrument to construct a reverse measurement data sequence, carries out forward and reverse autonomous navigation positioning based on an incomplete constraint Kalman filter, combines zero-speed correction to restrain and correct positioning errors, utilizes the difference complementarity of the forward and reverse autonomous navigation positioning error characteristics, and takes an average positioning result as a drilling path track to improve the positioning precision.
As shown in fig. 2, the specific process of the present invention is described with reference to a specific application example:
(1) installing a positioning and orienting instrument on a core taking device, placing the positioning and orienting instrument on a base of a carrier vehicle of a core taking drilling machine, binding initial position information and initial speed information to the positioning and orienting instrument, standing the positioning and orienting instrument for 15-25 minutes after binding is completed, performing initial self-alignment to obtain initial posture information, and storing angle increment information and speed increment information obtained by measurement of an inertial measurement unit by the positioning and orienting instrument on line; after the initial self-alignment is finished by the positioning and orientation instrument, forward autonomous navigation positioning is carried out based on the incomplete constraint Kalman filter; the method comprises the following steps of:
(1.1) with an attitude error phinVelocity error δ vnPosition error δ pnGyro drift epsilonbAccelerometer zero offset
Figure BDA0002471372290000061
Error in installation of inertial measurement unit of positioning and orientation instrumentThe difference η is a system state x (t), and differential equations respectively determining attitude error, velocity error, gyro drift, accelerometer zero offset, and mounting error are as follows:
Figure BDA0002471372290000062
wherein,
Figure BDA0002471372290000063
representing the angular velocity of rotation of the navigation coordinate system n relative to the inertial coordinate system i,
Figure BDA0002471372290000064
representing the attitude matrix between the carrier coordinate system b and the navigation coordinate system n, fnIndicating the specific force represented in the navigational coordinate system,
Figure BDA0002471372290000065
the angular velocity of rotation of the earth is represented,
Figure BDA0002471372290000066
indicating the transfer angular velocity, vnThe speed is indicated in the form of a speed,
Figure BDA0002471372290000067
respectively representing the rotational angular velocity error, the earth rotational angular velocity error and the transfer angular velocity error of the navigation coordinate system,
Figure BDA0002471372290000068
the measurement error of the gyro component is shown,
Figure BDA0002471372290000069
representing the measurement error of the accelerometer assembly, wg、waRepresenting gyro component measurement noise and accelerometer component measurement noise, respectively, η ═ ηθηΨ]TInstallation error by pitch angle ηθAnd heading angle installation error ηΨForming;
(1.2) according to the attitude error, the speed error, the position error, the gyro drift, the accelerometer zero offset and the installation error differential equation determined in the step (1.1), constructing a system state equation as follows:
Figure BDA00024713722900000610
wherein,
Figure BDA0002471372290000071
representing a system state matrix;
Figure BDA0002471372290000072
Figure BDA0002471372290000073
Figure BDA0002471372290000074
Figure BDA0002471372290000075
in the formula, vE、vN、vURespectively representing east, north and vertical speeds, L representing local latitude, h representing local height, R representing local altitudeE、RNRespectively represent the radius of the prime circle, the radius of the meridian circle, omegaieRepresenting the earth rotation angular velocity module value;
Figure BDA0002471372290000076
representing a system noise matrix;
w(t)＝[wgwa]Trepresenting system noise;
(1.3) the lateral and vertical velocities of the corer are zero as it advances along the drilling path, with a lateral velocity error δ vxAnd vertical velocity error δ vzConstruction of an incomplete constraint observation z (t) ═ δ vxδvz]TAnd is combined withDetermining an observation equation;
(2) pushing the core taking device to a drill hole by a core taking drilling machine carrier vehicle and staying, staying the core taking device at the drill hole for 10-20 seconds, completing the first zero speed correction of the positioning and orienting instrument, then pushing the core taking device along the drilling path in a high-pressure air pushing mode, and storing angle increment information and speed increment information obtained by measurement of an inertia measurement unit on line by the positioning and orienting instrument;
(3) when the coring device is pushed to a drilling machine at the tail end of the drilling path, the coring device finishes coring operation, then the coring device stays at the tail end of the drilling path for 10-20 seconds again, and the second zero-speed correction of the positioning and orienting instrument is finished; in addition, the positioning and orientation instrument stores angle increment information and speed increment information measured by the inertial measurement unit on line;
(4) after the second zero-speed correction is finished, pulling the coring device out of the hole through the winch, and when the coring device is pulled to the drill hole, staying for 10-20 seconds again, and finishing the third zero-speed correction of the positioning and orienting instrument; after the third zero-speed correction is completed, dragging the coring device to a base of a coring drilling machine vehicle, and standing for 15-25 minutes again; in addition, the positioning and orientation instrument stores angle increment information and speed increment information measured by the inertial measurement unit on line;
(5) as shown in fig. 3 and 4, the gyro assembly is used to measure the angular velocity
Figure BDA0002471372290000081
Accelerometer assembly measuring specific force fbAnd the rotational angular velocity of the earth
Figure BDA0002471372290000082
And (3) taking the inverse, reversing the angle increment information and the speed increment information stored in the steps (1) to (4) according to the time sequence from back to front to form a reverse measurement data sequence, and performing reverse autonomous navigation positioning based on an incomplete constraint Kalman filter, wherein the reverse navigation solution is as follows:
Figure BDA0002471372290000083
wherein,
Figure BDA0002471372290000084
Figure BDA0002471372290000085
respectively representing the attitude matrices at the time of reverse time sequence d and d-1,
Figure BDA0002471372290000086
respectively representing the speeds at the time points of the reverse sequences d and d-1,
Figure BDA0002471372290000087
respectively representing the positions of the moments d, d-1 in reverse sequence, gn、
Figure BDA0002471372290000088
Respectively represents forward solving and backward solving of the local gravitational acceleration, I3Representing a third order identity matrix, △ T representing a sampling interval;
(6) positioning result p with forward autonomous navigation after time alignmentnAnd reverse autonomous navigation positioning results
Figure BDA0002471372290000089
Mean value of
Figure BDA00024713722900000810
And as the average positioning, taking the positioning result of the advancing section of the coring device as a drilling path track, calculating the deviation of the drilling path track relative to the designed path according to the drilling path track, and further adjusting the drilling path.
Further, the determination of the observation equation in the step (1.3) is realized by the following steps:
(1.3.1) projecting the position and orientation machine velocity output to the inertial measurement unit coordinate system m as follows:
Figure BDA00024713722900000811
wherein,
Figure BDA00024713722900000812
representing the projection, v, of the velocity output of the position and orientation finder in the inertial measurement unit coordinate system mmRepresenting the speed of the position and orientation device expressed in a real m coordinate system,
Figure BDA00024713722900000813
a matrix representing the installation relationship between the carrier coordinate system b and the inertial measurement unit coordinate system m, ζ ═ ηθηγηΨ]TThe angle of the installation error is indicated,
Figure BDA0002471372290000091
representing the attitude matrix between the navigation coordinate system n and the carrier coordinate system b, due to roll angle mounting error ηγThe forward velocity projection is assigned a value of 0, η, without affecting the forward velocity projectionγ＝0；
(1.3.2) by
Figure BDA0002471372290000092
As the incomplete constraint observation quantity z (t), the observation equation is constructed as follows:
z(t)＝H(t)x(t)+υ(t) (4)
wherein,
Figure BDA0002471372290000093
and M1＝[1 0 0]，M3＝[0 0 1]，
Figure BDA0002471372290000094
V (t) represents the observation noise.
Further, the zero-speed correction in the step (2) is performed according to the following steps:
(2.1) the positioning and orientation instrument automatically detects the zero speed state according to the angular speed and speed information, and the forward speed error delta vyThe amplification is observed quantity, and the observed quantity in the zero-speed state is zZUPT(t)＝[δvxδvyδvz]T；
(2.2) by
Figure BDA0002471372290000095
As an observed quantity z in a zero velocity stateZUPT(t), constructing an observation equation as follows:
zZUPT(t)＝HZUPT(t)x(t)+μ(t) (5)
wherein,
Figure BDA0002471372290000096
μ (t) represents observation noise;
and (2.3) when the positioning and orientation instrument is in a zero-speed state, switching the Kalman filter observation equation from the equation (4) to the equation (5), and finishing measurement updating by adopting a sequential processing mode.
Further, the reverse autonomous navigation positioning in the step (5) comprises the following steps:
(5.1) completing initial alignment by using the first static stage of 15-25 minutes of reverse measurement data to obtain initial attitude information;
(5.2) performing reverse filtering according to the incomplete constraint Kalman filter in the steps (1.1) - (1.3), wherein an attitude error equation, a speed error equation, a position error equation, gyro drift, acceleration zero offset and a differential equation of installation errors of the inertial measurement unit of the positioning and orientation instrument are kept unchanged, and an observation equation is also kept unchanged;
and (5.3) in the reverse autonomous navigation positioning process, when the coring device is in a resident state, carrying out error correction according to the zero speed correction mode of the steps (2.1) to (2.3).
Further, the rest time of the coring device on the vehicle-mounted base of the coring drilling machine in the steps (1) and (4) is 15 minutes respectively.
Further, the rest time of the coring device on the vehicle-mounted base of the coring drilling machine in the steps (1) and (4) is 25 minutes respectively.
Further, the residence time of the coring device at the drilling hole and at the end of the drilling path in the steps (2), (3) and (4) is 10 seconds respectively.
Further, the residence time of the coring device at the drilling hole and at the end of the drilling path in the steps (2), (3) and (4) is 20 seconds respectively.
Further, in the step (2), the coring device is pushed along the drilling path in a high-pressure hydraulic pushing mode.
Further, feedback correction is adopted for gyro drift and accelerometer zero-offset states in the steps (1.1) and (1.2).
Further, the installation error state in the steps (1.1) and (1.2) adopts open loop correction.
Further, the sampling interval of the inertial measurement unit in step (1) when measuring the angular increment information and the velocity increment information is not more than 0.01 s.
The above description is only a preferred embodiment of the present invention, and the protection scope of the present invention is not limited to the above embodiments, and all technical solutions belonging to the idea of the present invention belong to the protection scope of the present invention. It should be noted that modifications and embellishments within the scope of the invention may occur to those skilled in the art without departing from the principle of the invention, and are considered to be within the scope of the invention.

Claims (12)

1. The positioning and orientating instrument hole positioning method suitable for the long-distance horizontal coring drilling machine is characterized by comprising the following steps of:
(1) installing a positioning and orienting instrument on a core taking device, placing the positioning and orienting instrument on a base of a carrier vehicle of a core taking drilling machine, binding initial position information and initial speed information to the positioning and orienting instrument, standing the positioning and orienting instrument for 15-25 minutes after binding is completed, performing initial self-alignment to obtain initial posture information, and storing angle increment information and speed increment information obtained by measurement of an inertial measurement unit by the positioning and orienting instrument on line; after the initial self-alignment is finished by the positioning and orientation instrument, forward autonomous navigation positioning is carried out based on the incomplete constraint Kalman filter; the method comprises the following steps of:
(1.1) with an attitude error phinVelocity error δ vnPosition error δ pnGyro drift epsilonbAccelerometer zero offset
Figure FDA00024713722800000112
The installation error η of the inertial measurement unit of the positioning and orientation device is the system state x (t), and the differential equations respectively determining the attitude error, the velocity error, the gyro drift, the accelerometer zero offset and the installation error are as follows:
Figure FDA0002471372280000011
wherein,
Figure FDA0002471372280000012
representing the angular velocity of rotation of the navigation coordinate system n relative to the inertial coordinate system i,
Figure FDA0002471372280000013
representing the attitude matrix between the carrier coordinate system b and the navigation coordinate system n, fnIndicating the specific force represented in the navigational coordinate system,
Figure FDA0002471372280000014
the angular velocity of rotation of the earth is represented,
Figure FDA0002471372280000015
indicating the transfer angular velocity, vnThe speed is indicated in the form of a speed,
Figure FDA0002471372280000016
respectively representing the rotational angular velocity error, the earth rotational angular velocity error and the transfer angular velocity error of the navigation coordinate system,
Figure FDA0002471372280000017
the measurement error of the gyro component is shown,
Figure FDA0002471372280000018
representing the measurement error of the accelerometer assembly, wg、waRepresenting gyro component measurement noise and accelerometer component measurement noise, respectively, η ═ ηθηΨ]TInstallation error by pitch angle ηθAnd heading angle installation error ηΨForming;
(1.2) according to the attitude error, the speed error, the position error, the gyro drift, the accelerometer zero offset and the installation error differential equation determined in the step (1.1), constructing a system state equation as follows:
Figure FDA0002471372280000019
wherein,
Figure FDA00024713722800000110
representing a system state matrix;
Figure FDA00024713722800000111
Figure FDA0002471372280000021
Figure FDA0002471372280000022
Figure FDA0002471372280000023
in the formula, vE、vN、vURespectively representing east, north and vertical speeds, L representing local latitude, h representing local height, R representing local altitudeE、RNRespectively represent the radius of the prime circle, the radius of the meridian circle, omegaieRepresenting the earth rotation angular velocity module value;
Figure FDA0002471372280000024
representing a system noise matrix;
w(t)＝[wgwa]Trepresenting system noise;
(1.3) the lateral and vertical velocities of the corer are zero as it advances along the drilling path, with a lateral velocity error δ vxAnd vertical velocity error δ vzConstruction of an incomplete constraint observation z (t) ═ δ vxδvz]TAnd determining an observation equation;
(2) pushing the core taking device to a drill hole by a core taking drilling machine carrier vehicle and staying, staying the core taking device at the drill hole for 10-20 seconds, completing the first zero speed correction of the positioning and orienting instrument, then pushing the core taking device along the drilling path in a high-pressure air pushing mode, and storing angle increment information and speed increment information obtained by measurement of an inertia measurement unit on line by the positioning and orienting instrument;
(3) when the coring device is pushed to a drilling machine at the tail end of the drilling path, the coring device finishes coring operation, then the coring device stays at the tail end of the drilling path for 10-20 seconds again, and the second zero-speed correction of the positioning and orienting instrument is finished; in addition, the positioning and orientation instrument stores angle increment information and speed increment information measured by the inertial measurement unit on line;
(4) after the second zero-speed correction is finished, pulling the coring device out of the hole through the winch, and when the coring device is pulled to the drill hole, staying for 10-20 seconds again, and finishing the third zero-speed correction of the positioning and orienting instrument; after the third zero-speed correction is completed, dragging the coring device to a base of a coring drilling machine vehicle, and standing for 15-25 minutes again; in addition, the positioning and orientation instrument stores angle increment information and speed increment information measured by the inertial measurement unit on line;
(5) measuring angular velocity of gyro assembly
Figure FDA0002471372280000031
Accelerometer assembly measuring specific force fbAnd the rotational angular velocity of the earth
Figure FDA0002471372280000032
And (3) taking the inverse, reversing the angle increment information and the speed increment information stored in the steps (1) to (4) according to the time sequence from back to front to form a reverse measurement data sequence, and performing reverse autonomous navigation positioning based on an incomplete constraint Kalman filter, wherein the reverse navigation solution is as follows:
Figure FDA0002471372280000033
Figure FDA0002471372280000034
Figure FDA0002471372280000035
wherein,
Figure FDA0002471372280000036
Figure FDA0002471372280000037
respectively representing the attitude matrices at the time of reverse time sequence d and d-1,
Figure FDA0002471372280000038
respectively representing the speeds at the time points of the reverse sequences d and d-1,
Figure FDA0002471372280000039
respectively representing the positions of the moments d, d-1 in reverse sequence, gn、
Figure FDA00024713722800000310
Respectively represents forward solving and backward solving of the local gravitational acceleration, I3Representing a third order identity matrix, △ T representing a sampling interval;
(6) positioning result p with forward autonomous navigation after time alignmentnAnd reverse autonomous navigation positioning results
Figure FDA00024713722800000311
Mean value of
Figure FDA00024713722800000312
And as the average positioning, taking the positioning result of the advancing section of the coring device as a drilling path track, calculating the deviation of the drilling path track relative to the designed path according to the drilling path track, and further adjusting the drilling path.
2. The method for positioning inside a bore of an orienter suitable for use with a long-reach horizontal coring drill as recited in claim 1, wherein the determination of the observation equation in step (1.3) is accomplished by:
(1.3.1) projecting the position and orientation machine velocity output to the inertial measurement unit coordinate system m as follows:
Figure FDA00024713722800000313
wherein,
Figure FDA00024713722800000314
representing the projection, v, of the velocity output of the position and orientation finder in the inertial measurement unit coordinate system mmRepresenting the speed of the position and orientation device expressed in a real m coordinate system,
Figure FDA00024713722800000315
a matrix representing the installation relationship between the carrier coordinate system b and the inertial measurement unit coordinate system m, ζ ═ ηθηγηΨ]TThe angle of the installation error is indicated,
Figure FDA00024713722800000316
representing the attitude matrix between the navigation coordinate system n and the carrier coordinate system b, due to roll angle mounting error ηγWill not affect the forward velocity projectionIts value is assigned 0, ηγ＝0；
(1.3.2) by
Figure FDA00024713722800000317
As the incomplete constraint observation quantity z (t), the observation equation is constructed as follows:
z(t)＝H(t)x(t)+υ(t) (4)
wherein,
Figure FDA00024713722800000318
and M1＝[1 0 0]，M3＝[0 0 1]，
Figure FDA0002471372280000041
V (t) represents the observation noise.
3. The method for positioning and orientating the bore of a hole in a long-reach horizontal coring drill as set forth in claim 1, wherein the zero speed correction in step (2) is performed by:
(2.1) the positioning and orientation instrument automatically detects the zero speed state according to the angular speed and speed information, and the forward speed error delta vyThe amplification is observed quantity, and the observed quantity in the zero-speed state is zZUPT(t)＝[δvxδvyδvz]T；
(2.2) by
Figure FDA0002471372280000042
As an observed quantity z in a zero velocity stateZUPT(t), constructing an observation equation as follows:
zZUPT(t)＝HZUPT(t)x(t)+μ(t) (5)
wherein,
Figure FDA0002471372280000043
μ (t) represents observation noise;
and (2.3) when the positioning and orientation instrument is in a zero-speed state, switching the Kalman filter observation equation from the equation (4) to the equation (5), and finishing measurement updating by adopting a sequential processing mode.
4. The method for positioning inside a bore of an orienter suitable for use with a long-reach horizontal coring drill as recited in claim 1, wherein the reverse autonomous navigational positioning in step (5) comprises the steps of:
(5.1) completing initial alignment by using the first static stage of 15-25 minutes of reverse measurement data to obtain initial attitude information;
(5.2) performing reverse filtering according to the incomplete constraint Kalman filter in the steps (1.1) - (1.3), wherein an attitude error equation, a speed error equation, a position error equation, gyro drift, acceleration zero offset and a differential equation of installation errors of the inertial measurement unit of the positioning and orientation instrument are kept unchanged, and an observation equation is also kept unchanged;
and (5.3) in the reverse autonomous navigation positioning process, when the coring device is in a resident state, carrying out error correction according to the zero speed correction mode of the steps (2.1) to (2.3).
5. The method for positioning and orientating instrument hole inside of a long distance horizontal coring drill as set forth in claim 1, wherein the rest time of the coring device on the vehicle carrying base of the coring drill in the steps (1) and (4) is 15 minutes respectively.
6. The method for positioning and orientating instrument hole inside of a long distance horizontal coring drill as set forth in claim 1, wherein the rest time of the coring device on the vehicle carrying base of the coring drill in the steps (1) and (4) is 25 minutes respectively.
7. The method for positioning and orienting the hole of the instrument for long-distance horizontal core drilling machine as claimed in claim 1, wherein the residence time of the coring device at the drilling hole and the end of the drilling path in the steps (2), (3) and (4) is 10 seconds respectively.
8. The method for positioning and orienting the hole of the instrument for long-distance horizontal core drilling machine as claimed in claim 1, wherein the residence time of the coring device at the drilling hole and the end of the drilling path in the steps (2), (3) and (4) is 20 seconds respectively.
9. A method for downhole positioning of a positioning and orienting instrument suitable for use in a long reach horizontal coring drill as set forth in claim 1 wherein step (2) comprises advancing the coring apparatus along the drilling path by means of high pressure hydraulic thrust.
10. The method for locating the position and orientation instrument hole in the long-distance horizontal core drilling machine according to claim 1, wherein the gyro drift and the accelerometer zero offset state in the steps (1.1) and (1.2) are corrected by feedback.
11. The method for positioning inside a hole of a positioning and orienting instrument suitable for a long-distance horizontal coring drill as set forth in claim 1, wherein the installation error state in the steps (1.1) and (1.2) is corrected by open loop.
12. The method for positioning and orienting the inside of the hole of the positioning and orienting instrument for the long-distance horizontal core drilling machine as claimed in claim 1, wherein the sampling interval of the inertial measurement unit for measuring the angular increment information and the speed increment information in the step (1) is not more than 0.01 s.
CN202010349576.4A 2020-04-28 2020-04-28 Positioning and orientating instrument hole positioning method suitable for long-distance horizontal core drilling machine Active CN111337025B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010349576.4A CN111337025B (en) 2020-04-28 2020-04-28 Positioning and orientating instrument hole positioning method suitable for long-distance horizontal core drilling machine

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010349576.4A CN111337025B (en) 2020-04-28 2020-04-28 Positioning and orientating instrument hole positioning method suitable for long-distance horizontal core drilling machine

Publications (2)

Publication Number Publication Date
CN111337025A true CN111337025A (en) 2020-06-26
CN111337025B CN111337025B (en) 2021-02-26

Family

ID=71184925

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010349576.4A Active CN111337025B (en) 2020-04-28 2020-04-28 Positioning and orientating instrument hole positioning method suitable for long-distance horizontal core drilling machine

Country Status (1)

Country Link
CN (1) CN111337025B (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113153150A (en) * 2021-04-23 2021-07-23 中国铁建重工集团股份有限公司 Horizontal drilling machine drilling track measuring method based on zero-speed correction
CN114658379A (en) * 2022-05-09 2022-06-24 中国铁建重工集团股份有限公司 Directional core drill and using method thereof

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103591962A (en) * 2013-11-11 2014-02-19 北京航空航天大学 Positioning and orienting instrument optical fiber strap-down inertial measurement unit for coal industry
CN104949687A (en) * 2014-03-31 2015-09-30 北京自动化控制设备研究所 Whole parameter precision evaluation method for long-time navigation system
US9182517B1 (en) * 2010-09-10 2015-11-10 Selman and Associates, Ltd. Drilling rig for horizontal, lateral, and directional drilling that is adjustable in real time
CN106595652A (en) * 2016-11-30 2017-04-26 西北工业大学 Vehicle MCA (motion constraints aided) backtracking type aligning-on-the-move method
CN208534410U (en) * 2018-07-22 2019-02-22 泉州市利器金刚石工具有限公司 A kind of positioning device of horizontal core drilling rig
CN110296701A (en) * 2019-07-09 2019-10-01 哈尔滨工程大学 Inertia and satellite combined guidance system gradation type failure recall fault-tolerance approach
CN110806220A (en) * 2019-11-23 2020-02-18 中国船舶重工集团公司第七一七研究所 Inertial navigation system initial alignment method and device

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9182517B1 (en) * 2010-09-10 2015-11-10 Selman and Associates, Ltd. Drilling rig for horizontal, lateral, and directional drilling that is adjustable in real time
CN103591962A (en) * 2013-11-11 2014-02-19 北京航空航天大学 Positioning and orienting instrument optical fiber strap-down inertial measurement unit for coal industry
CN104949687A (en) * 2014-03-31 2015-09-30 北京自动化控制设备研究所 Whole parameter precision evaluation method for long-time navigation system
CN106595652A (en) * 2016-11-30 2017-04-26 西北工业大学 Vehicle MCA (motion constraints aided) backtracking type aligning-on-the-move method
CN208534410U (en) * 2018-07-22 2019-02-22 泉州市利器金刚石工具有限公司 A kind of positioning device of horizontal core drilling rig
CN110296701A (en) * 2019-07-09 2019-10-01 哈尔滨工程大学 Inertia and satellite combined guidance system gradation type failure recall fault-tolerance approach
CN110806220A (en) * 2019-11-23 2020-02-18 中国船舶重工集团公司第七一七研究所 Inertial navigation system initial alignment method and device

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
YANG, H. 等，: ""A Fault-Tolerant Integrated Borehole Trajectory Location Method Based on Geomagnetism/IMU of MWD"", 《IEEE ACCESS》 *
邓继权 等，: ""一种基于实时再处理技术的SINS初始对准算法"", 《导航定位与授时》 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113153150A (en) * 2021-04-23 2021-07-23 中国铁建重工集团股份有限公司 Horizontal drilling machine drilling track measuring method based on zero-speed correction
CN114658379A (en) * 2022-05-09 2022-06-24 中国铁建重工集团股份有限公司 Directional core drill and using method thereof
CN114658379B (en) * 2022-05-09 2024-03-12 中国铁建重工集团股份有限公司 Directional core drill and use method thereof

Also Published As

Publication number Publication date
CN111337025B (en) 2021-02-26

Similar Documents

Publication Publication Date Title
CN101514900B (en) Method for initial alignment of a single-axis rotation strap-down inertial navigation system (SINS)
CN103335647B (en) A kind of attitude of shield machine measuring system and measuring method thereof
US20020133958A1 (en) Continuous measurement-while-drilling surveying
CN102536207B (en) Gyro inclinometer attitude measurement solving method applicable to measurement of low well deviation angle
CN111207744B (en) Pipeline geographical position information measuring method based on thick tail robust filtering
US8305230B2 (en) Method and apparatus for initialization of a wellbore survey tool
US6212476B1 (en) Apparatus to measure the earth's local gravity and magnetic field in conjunction with global positioning attitude determining
CN111337025B (en) Positioning and orientating instrument hole positioning method suitable for long-distance horizontal core drilling machine
CN106507913B (en) Combined positioning method for pipeline mapping
US10948607B2 (en) Dead reckoning-augmented GPS for tracked vehicles
CN104515527B (en) A kind of anti-rough error Combinated navigation method under no gps signal environment
CN109540130A (en) A kind of continuous milling machine inertial navigation positioning and orienting method
US20040089474A1 (en) Continuous measurement-while-drilling surveying
CN106050223A (en) Omnibearing magnetostatic-field while-drilling positioning method and positioning system for relief well
CN111504313B (en) Drilling positioning and orientating instrument hole positioning method based on speed information assistance
CN102052921B (en) Method for determining initial heading of single-axis rotating strapdown inertial navigation system
CN113236363B (en) Mining equipment navigation positioning method, system, equipment and readable storage medium
CN104864874B (en) A kind of inexpensive single gyro dead reckoning navigation method and system
CN107677292B (en) Vertical line deviation compensation method based on gravity field model
CN103697878A (en) Rotation-modulation north-seeking method utilizing single gyroscope and single accelerometer
CN101012745B (en) Method for measurement of oil gas well bore track
CN111521179B (en) Drilling positioning director hole positioning method based on self-detection of traveling speed
CN111521178B (en) Drilling positioning director hole positioning method based on pipe length constraint
CN112683270B (en) GNSS/SINS/magnetometer combination method based on smooth variable structure filtering
CN116202516B (en) Track three-dimensional reconstruction method for track BIM multidimensional parameter auxiliary IMU

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant