CN111504313B - Drilling positioning and orientating instrument hole positioning method based on speed information assistance - Google Patents
Drilling positioning and orientating instrument hole positioning method based on speed information assistance Download PDFInfo
- Publication number
- CN111504313B CN111504313B CN202010348945.8A CN202010348945A CN111504313B CN 111504313 B CN111504313 B CN 111504313B CN 202010348945 A CN202010348945 A CN 202010348945A CN 111504313 B CN111504313 B CN 111504313B
- Authority
- CN
- China
- Prior art keywords
- positioning
- error
- speed
- information
- representing
- 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.)
- Active
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
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01N—INVESTIGATING OR ANALYSING MATERIALS BY DETERMINING THEIR CHEMICAL OR PHYSICAL PROPERTIES
- G01N1/00—Sampling; Preparing specimens for investigation
- G01N1/02—Devices for withdrawing samples
- G01N1/04—Devices for withdrawing samples in the solid state, e.g. by cutting
- G01N1/08—Devices for withdrawing samples in the solid state, e.g. by cutting involving an extracting tool, e.g. core bit
Abstract
The invention belongs to the field of geological exploration and discloses a speed information-assisted positioning and orientating instrument hole positioning method for drilling. 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 and the position increment information of a photoelectric encoder on a winch to construct a reverse measurement data sequence, carries out forward and reverse autonomous navigation positioning based on a complete constraint Kalman filter assisted by speed information, combines zero-speed correction to inhibit and correct positioning errors, and utilizes the difference complementarity of the characteristics of the forward and reverse autonomous navigation positioning errors to weight and position output as a drilling path track so as 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
Technical Field
The invention belongs to the field of geological exploration, and relates to a method for positioning a core drill in a hole in an exploration process, in particular to a method for positioning a positioning and orienting instrument in the hole for drilling based on the assistance of speed information.
Background
In the railway tunnel construction process in the plateau alpine region, 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 influence of the error of the inertial device, in the case of long-distance horizontal drilling, the positioning error of the inertial measurement unit will increase continuously with the increase of the working time, and the control precision of the drilling path is influenced. Therefore, it is necessary to suppress or correct the positioning error of the inertial measurement unit by means of the auxiliary information, thereby achieving precise control of the drilling path.
Under the actual operation environment, limited by working conditions and supporting equipment of horizontal core drilling rigs of various models, the positioning and orientation instrument has very limited usable auxiliary positioning means or auxiliary information and is different, and how to fully excavate the usable information in the actual working condition environment so as to improve the positioning and orientation precision becomes a problem which is very concerned by geological exploration technicians. Aiming at the geological exploration requirement of plateau alpine regions, the invention researches the speed information-assisted positioning and orientating instrument hole positioning method for drilling, combines the working characteristics of the long-distance horizontal coring drilling machine, fully utilizes the available auxiliary information of the matching equipment of the coring drilling machine, realizes the accurate positioning in the hole of the long-distance horizontal coring drilling machine under the speed information-assisted condition, and further improves the control precision of the drilling path.
Disclosure of Invention
The technical problem to be solved by the invention is as follows: the working characteristics of a long-distance horizontal core drilling machine are combined, the working mode of the core taking device is reasonably planned, available auxiliary information of the core drilling machine matching equipment and all measurement data stored by the positioning and orienting instrument are fully utilized, the positioning in the positioning and orienting instrument hole for drilling based on the assistance of speed information 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 method for positioning the inside of the hole of the positioning and orienting instrument for drilling based on the assistance of speed information 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, additionally installing a photoelectric encoder on a winch, further 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 the binding is finished, performing initial self-alignment to obtain initial posture information, storing angle increment information and speed increment information obtained by measurement of an inertial measurement unit on line by the positioning and orienting instrument, and storing position increment information of the photoelectric encoder on the winch at the same time; after the positioning and orientation instrument completes the initial self-alignment, a complete constraint Kalman filter based on the assistance of speed information carries out forward autonomous navigation and positioning; the complete constraint Kalman filter based on the speed information assistance is designed according to the following steps:
(1.1) with an attitude error phinVelocity error vnPosition error pnGyro driftbAccelerometer zero offset
The installation error eta of the inertial measurement unit of the positioning and orientation instrument is a system state x (t), and differential equations respectively determining attitude error, speed error, gyro drift, accelerometer zero offset and installation error are as follows:
wherein the content of the first and second substances,
representing the angular velocity of rotation of the navigation coordinate system n relative to the inertial coordinate system i,
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,
the angular velocity of rotation of the earth is represented,
indicating the transfer angular velocity, vnThe speed is indicated in the form of a speed,
respectively representing the rotational angular velocity error, the earth rotational angular velocity error and the transfer angular velocity error of the navigation coordinate system,
the measurement error of the gyro component is shown,
representing the measurement error of the accelerometer assembly, wg、waRespectively representing the measurement noise of a gyro component and the measurement noise of an accelerometer component, eta ═ etaθ ηΨ]TInstallation error eta from pitch angleθAnd course angle mounting error etaΨ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:
wherein the content of the first and second substances,
representing a system state matrix;
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;
w(t)＝[wg wa]Trepresenting system noise;
(1.3) the lateral and vertical velocities of the coring tool are zero as it advances along the drilling path, proceeding in the forward directionVelocity error vyLateral velocity error vxAnd vertical velocity error vzConstruction of a complete constraint observation z (t) ═ vx vy vz]TAnd determining an observation equation, wherein the determination of the observation equation 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:
wherein the content of the first and second substances,
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,
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,
representing an attitude matrix between a navigation coordinate system n and a carrier coordinate system b; due to the installation error eta of the transverse roll angleγThe forward velocity projection is assigned a value of 0, i.e. η, without affecting the forward velocity projectionγ＝0；
(1.3.2) by
As a complete constraint observation z (t), the observation equation is constructed as follows:
z(t)＝H(t)x(t)+υ(t) (4)
wherein the content of the first and second substances,
and v isfNumerical value ofEqual to the position increment information output by the photoelectric encoder divided by the sampling time,
and is
V (t) represents the observation noise;
(2) the coring method comprises the following steps that a coring drill carries a vehicle to push a coring device to a drill hole and stay the coring device, the coring device stays at the drill hole for 10-20 seconds, the first zero speed correction of a positioning and orienting instrument is completed, then the coring device is pushed along a drilling path in a high-pressure gas pushing mode, the positioning and orienting instrument stores angle increment information and speed increment information obtained by measurement of an inertia measurement unit on line, and simultaneously stores position increment information of an optical encoder on a winch, wherein the zero speed correction is carried out 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 observed quantity in the zero-speed state is
(2.2) by
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 the content of the first and second substances,
μ (t) represents observation noise;
(2.3) completing measurement updating of the positioning and orientation instrument in the zero-speed state according to the steps (2.1) and (2.2);
(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 an inertia measurement unit on line and stores position increment information of a photoelectric encoder on the winch;
(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 an inertia measurement unit on line and stores position increment information of a photoelectric encoder on the winch;
(5) measuring angular velocity of gyro assembly
Accelerometer assembly measuring specific force fbAnd the rotational angular velocity of the earth
And (3) negation is performed, the angle increment information, the speed increment information and the position increment information of the photoelectric encoder which are stored in the steps (1) to (4) are reversed according to the time sequence from the back to the front to form a reverse measurement data sequence, the position increment information of the photoelectric encoder is only subjected to reverse order processing in time and is not negated, and finally, a reverse autonomous navigation positioning is performed on the reverse measurement data sequence by a complete constraint Kalman filter assisted by the speed information, wherein the reverse navigation calculation is as follows:
wherein the content of the first and second substances,
the reverse autonomous navigation positioning 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 complete constraint Kalman filter based on the velocity information assistance in the steps (1.1) - (1.3), wherein a differential equation of an attitude error equation, a velocity error equation, a position error equation, gyro drift, acceleration zero offset and installation errors of the inertial measurement unit of the positioning and orientation instrument is 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).
(6) Positioning result p with forward autonomous navigation after time alignmentnAnd reverse autonomous navigation positioning results
The weighting result of (2) is used as positioning output, and the weighting mode is described by the following formula:
wherein p issAs a result of weighting, Ps、Pf、PbRespectively a smooth variance, a forward variance and a reverse variance;
and finally, taking the positioning result of the propelling section of the coring device as a drilling path track, and calculating the deviation of the propelling section of the coring device relative to the designed path according to the drilling path track so as to adjust the drilling path.
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, in the step (2.3), measurement update is completed by adopting a sequential update 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 and the position increment information of the photoelectric encoder on the winch, improves the data utilization rate, and realizes the accurate positioning in the hole of the long-distance horizontal coring drilling machine under the condition of speed information assistance;
(2) the method fully utilizes the difference complementarity of the forward and reverse autonomous navigation positioning error characteristics, takes the weighted positioning output 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 speed information-assisted positioning and orientating instrument hole positioning method for drilling reasonably plans the working mode of a coring device by combining the working characteristics of a long-distance horizontal coring drilling machine, fully utilizes all measurement data stored by a positioning and orientating instrument and position increment information of an optoelectronic encoder on a winch to construct a reverse measurement data sequence, carries out forward and reverse autonomous navigation positioning by a complete constraint Kalman filter based on speed information assistance, inhibits and corrects positioning errors by combining zero-speed correction, and takes a weighted positioning result as a drilling path track by utilizing the difference complementarity of the characteristics of forward and reverse autonomous navigation positioning errors to improve the positioning accuracy.
As shown in fig. 2, the specific process of the present invention is described with reference to a specific application example:
the method for positioning the inside of the hole of the positioning and orienting instrument for drilling based on the assistance of speed information 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, additionally installing a photoelectric encoder on a winch, further 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 the binding is finished, performing initial self-alignment to obtain initial posture information, storing angle increment information and speed increment information obtained by measurement of an inertial measurement unit on line by the positioning and orienting instrument, and storing position increment information of the photoelectric encoder on the winch at the same time; after the positioning and orientation instrument completes the initial self-alignment, a complete constraint Kalman filter based on the assistance of speed information carries out forward autonomous navigation and positioning; the complete constraint Kalman filter based on the speed information assistance is designed according to the following steps:
(1.1) with an attitude error phinVelocity error vnPosition error pnGyro driftbAccelerometer zero offset
The installation error eta of the inertial measurement unit of the positioning and orientation instrument is a system state x (t), and differential equations respectively determining attitude error, speed error, gyro drift, accelerometer zero offset and installation error are as follows:
wherein the content of the first and second substances,
representing the angular velocity of rotation of the navigation coordinate system n relative to the inertial coordinate system i,
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,
the angular velocity of rotation of the earth is represented,
indicating the transfer angular velocity, vnThe speed is indicated in the form of a speed,
respectively representing the rotational angular velocity error, the earth rotational angular velocity error and the transfer angular velocity error of the navigation coordinate system,
the measurement error of the gyro component is shown,
representing the measurement error of the accelerometer assembly, wg、waRespectively representing the measurement noise of a gyro component and the measurement noise of an accelerometer component, eta ═ etaθ ηΨ]TInstallation error eta from pitch angleθAnd course angle mounting error etaΨ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:
wherein the content of the first and second substances,
representing a system state matrix;
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;
w(t)＝[wg wa]Trepresenting system noise;
(1.3) the lateral and vertical velocities of the corer are zero as it advances along the drilling path, with a forward velocity error vyLateral velocity error vxAnd vertical velocity error vzConstruction of a complete constraint observation z (t) ═ vx vy vz]TAnd determining an observation equation, wherein the determination of the observation equation 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:
wherein the content of the first and second substances,
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,
representing a carrier coordinate systemb and the coordinate system m of the inertial measurement unit, zeta ═ ηθ ηγ ηΨ]TThe angle of the installation error is indicated,
representing an attitude matrix between a navigation coordinate system n and a carrier coordinate system b; due to the installation error eta of the transverse roll angleγThe forward velocity projection is assigned a value of 0, i.e. η, without affecting the forward velocity projectionγ＝0；
(1.3.2) by
As a complete constraint observation z (t), the observation equation is constructed as follows:
z(t)＝H(t)x(t)+υ(t) (4)
wherein the content of the first and second substances,
and v isfIs equal to the position increment information output by the photoelectric encoder divided by the sampling time,
and is
V (t) represents the observation noise;
(2) the coring method comprises the following steps that a coring drill carries a vehicle to push a coring device to a drill hole and stay the coring device, the coring device stays at the drill hole for 10-20 seconds, the first zero speed correction of a positioning and orienting instrument is completed, then the coring device is pushed along a drilling path in a high-pressure gas pushing mode, the positioning and orienting instrument stores angle increment information and speed increment information obtained by measurement of an inertia measurement unit on line, and simultaneously stores position increment information of an optical encoder on a winch, wherein the zero speed correction is carried out 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 observed quantity in the zero-speed state is
(2.2) by
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 the content of the first and second substances,
μ (t) represents observation noise;
(2.3) completing measurement updating of the positioning and orientation instrument in the zero-speed state according to the steps (2.1) and (2.2);
(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 an inertia measurement unit on line and stores position increment information of a photoelectric encoder on the winch;
(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 an inertia measurement unit on line and stores position increment information of a photoelectric encoder on the winch;
(5) as shown in fig. 3 and 4, the gyro assembly is used to measure the angular velocity
Accelerometer assembly measuring specific force fbAnd the rotational angular velocity of the earth
And (3) negation is performed, the angle increment information, the speed increment information and the position increment information of the photoelectric encoder which are stored in the steps (1) to (4) are reversed according to the time sequence from the back to the front to form a reverse measurement data sequence, the position increment information of the photoelectric encoder is only subjected to reverse order processing in time and is not negated, and finally, a reverse autonomous navigation positioning is performed on the reverse measurement data sequence by a complete constraint Kalman filter assisted by the speed information, wherein the reverse navigation calculation is as follows:
wherein the content of the first and second substances,
the reverse autonomous navigation positioning 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 complete constraint Kalman filter based on the velocity information assistance in the steps (1.1) - (1.3), wherein a differential equation of an attitude error equation, a velocity error equation, a position error equation, gyro drift, acceleration zero offset and installation errors of the inertial measurement unit of the positioning and orientation instrument is 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).
(6) Positioning result p with forward autonomous navigation after time alignmentnAnd reverse autonomous navigation positioning results
The weighting result of (2) is used as positioning output, and the weighting mode is described by the following formula:
wherein p issAs a result of weighting, Ps、Pf、PbRespectively a smooth variance, a forward variance and a reverse variance;
and finally, taking the positioning result of the propelling section of the coring device as a drilling path track, and calculating the deviation of the propelling section of the coring device relative to the designed path according to the drilling path track so as to adjust the drilling path.
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, in the step (2.3), measurement update is completed by adopting a sequential update 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 (10)
1. The method for positioning the inside of the hole of the positioning and orienting instrument for drilling based on the assistance of speed information 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, additionally installing a photoelectric encoder on a winch, further 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 the binding is finished, performing initial self-alignment to obtain initial posture information, storing angle increment information and speed increment information obtained by measurement of an inertial measurement unit on line by the positioning and orienting instrument, and storing position increment information of the photoelectric encoder on the winch at the same time; after the positioning and orientation instrument completes the initial self-alignment, a complete constraint Kalman filter based on the assistance of speed information carries out forward autonomous navigation and positioning; the complete constraint Kalman filter based on the speed information assistance is designed according to the following steps:
(1.1) with an attitude error phinVelocity error vnPosition error pnGyro driftbAccelerometer zero offset
The installation error eta of the inertial measurement unit of the positioning and orientation instrument is a system state x (t), and differential equations respectively determining attitude error, speed error, position error, gyro drift, accelerometer zero offset and installation error are as follows:
wherein the content of the first and second substances,
representing the angular velocity of rotation of the navigation coordinate system n relative to the inertial coordinate system i,
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,
the angular velocity of rotation of the earth is represented,
indicating the transfer angular velocity, vnThe speed is indicated in the form of a speed,
respectively representing the rotational angular velocity error, the earth rotational angular velocity error and the transfer angular velocity error of the navigation coordinate system,
the measurement error of the gyro component is shown,
representing the measurement error of the accelerometer assembly, wg、waRespectively representing the measurement noise of a gyro component and the measurement noise of an accelerometer component, eta ═ etaθ ηΨ]TInstallation error eta from pitch angleθAnd course angle mounting error etaΨ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:
wherein the content of the first and second substances,
representing a system state matrix;
in the formula, vE、vN、vURespectively represents east, north and vertical speeds, L represents local latitude, h represents local height,RE、RNrespectively represent the radius of the prime circle, the radius of the meridian circle, omegaieRepresenting the earth rotation angular velocity module value;
w(t)＝[wg wa]Trepresenting system noise;
(1.3) the lateral and vertical velocities of the corer are zero as it advances along the drilling path, with a forward velocity error vyLateral velocity error vxAnd vertical velocity error vzConstruction of a complete constraint observation z (t) ═ vx vy vz]TAnd determining an observation equation, wherein the determination of the observation equation 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:
wherein the content of the first and second substances,
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,
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,
representing an attitude matrix between a navigation coordinate system n and a carrier coordinate system b; due to the transverse rollingAngular setting error etaγThe forward velocity projection is assigned a value of 0, i.e. η, without affecting the forward velocity projectionγ＝0；
(1.3.2) by
As a complete constraint observation z (t), the observation equation is constructed as follows:
z(t)＝H(t)x(t)+υ(t) (4)
wherein the content of the first and second substances,
and v isfIs equal to the position increment information output by the photoelectric encoder divided by the sampling time,
and is
V (t) represents the observation noise;
(2) the coring method comprises the following steps that a coring drill carries a vehicle to push a coring device to a drill hole and stay the coring device, the coring device stays at the drill hole for 10-20 seconds, the first zero speed correction of a positioning and orienting instrument is completed, then the coring device is pushed along a drilling path in a high-pressure gas pushing mode, the positioning and orienting instrument stores angle increment information and speed increment information obtained by measurement of an inertia measurement unit on line, and simultaneously stores position increment information of an optical encoder on a winch, wherein the zero speed correction is carried out 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 observed quantity in the zero-speed state is
(2.2) by
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 the content of the first and second substances,
μ (t) represents observation noise;
(2.3) completing measurement updating of the positioning and orientation instrument in the zero-speed state according to the steps (2.1) and (2.2);
(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 an inertia measurement unit on line and stores position increment information of a photoelectric encoder on the winch;
(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 an inertia measurement unit on line and stores position increment information of a photoelectric encoder on the winch;
(5) measuring angular velocity of gyro assembly
Accelerometer assembly measuring specific force fbAnd the rotational angular velocity of the earth
Inverting the angle increment information, the speed increment information and the position increment information of the photoelectric encoder stored in the steps (1) to (4) according to the time sequence from the back to the front to form a reverse measurement data sequenceIncremental information is only processed in a reverse order in time and is not inverted, and finally, a reverse measurement data sequence is subjected to reverse autonomous navigation positioning based on a complete constraint Kalman filter assisted by speed information, wherein the reverse navigation is calculated according to the following formula:
wherein the content of the first and second substances,
the reverse autonomous navigation positioning 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 complete constraint Kalman filter based on the velocity information assistance in the steps (1.1) - (1.3), wherein a differential equation of an attitude error equation, a velocity error equation, a position error equation, gyro drift, acceleration zero offset and installation errors of the inertial measurement unit of the positioning and orientation instrument is kept unchanged, and an observation equation is also kept unchanged;
(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 in the steps (2.1) - (2.3);
(6) positioning result p with forward autonomous navigation after time alignmentnAnd reverse autonomous navigation positioning results
The weighting result of (2) is used as positioning output, and the weighting mode is described by the following formula:
wherein p issAs a result of weighting, Ps、Pf、PbRespectively a smooth variance, a forward variance and a reverse variance;
and finally, taking the positioning result of the propelling section of the coring device as a drilling path track, and calculating the deviation of the propelling section of the coring device relative to the designed path according to the drilling path track so as to adjust the drilling path.
2. The method for positioning the inside of the hole of the positioning and orienting instrument for drilling based on the assistance of the speed information as claimed in claim 1, wherein the rest time of the coring device on the vehicle-carrying base of the coring drilling machine in the steps (1) and (4) is respectively 15 minutes.
3. The method for positioning the inside of the hole of the positioning and orienting instrument for drilling based on the assistance of the speed information as claimed in claim 1, wherein the rest time of the coring device on the vehicle-carrying base of the coring drilling machine in the steps (1) and (4) is respectively 25 minutes.
4. The method as recited in claim 1, wherein the dwell time of the coring tool at the borehole and at the end of the drilling path in steps (2), (3) and (4) is 10 seconds.
5. The method as recited in claim 1, wherein the dwell time of the coring tool at the borehole and at the end of the drilling path in steps (2), (3) and (4) is 20 seconds.
6. The method as recited in claim 1, wherein said step (2) comprises advancing the corer along the drilling path by means of high pressure hydraulic thrust.
7. The method as claimed in claim 1, wherein the step (2.3) is performed by sequential updating.
8. The method of claim 1, wherein the gyro drift and accelerometer zero offset states in steps (1.1) and (1.2) are corrected by feedback.
9. The method of claim 1, wherein the installation error status in steps (1.1) and (1.2) is corrected by open loop.
10. The method of claim 1 wherein the inertial measurement unit in step (1) measures angular delta information and velocity delta information at a sampling interval of no more than 0.01 s.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010348945.8A CN111504313B (en) | 2020-04-28 | 2020-04-28 | Drilling positioning and orientating instrument hole positioning method based on speed information assistance |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010348945.8A CN111504313B (en) | 2020-04-28 | 2020-04-28 | Drilling positioning and orientating instrument hole positioning method based on speed information assistance |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111504313A CN111504313A (en) | 2020-08-07 |
| CN111504313B true CN111504313B (en) | 2021-01-05 |
Family
ID=71869543
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010348945.8A Active CN111504313B (en) | 2020-04-28 | 2020-04-28 | Drilling positioning and orientating instrument hole positioning method based on speed information assistance |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111504313B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114894222B (en) * | 2022-07-12 | 2022-10-28 | 深圳元戎启行科技有限公司 | External parameter calibration method of IMU-GNSS antenna and related method and equipment |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101413800A (en) * | 2008-01-18 | 2009-04-22 | 南京航空航天大学 | Navigating and steady aiming method of navigation / steady aiming integrated system |
| CN106482749A (en) * | 2016-12-07 | 2017-03-08 | 西北工业大学 | Alignment methods are combined with tachometer based on the inertial navigation of reverse navigation algorithm |
| CN109891191A (en) * | 2016-11-29 | 2019-06-14 | 赫尔实验室有限公司 | Chance sensor fusion algorithm for independently being guided with brill |
| CN110886606A (en) * | 2019-11-20 | 2020-03-17 | 中国地质大学（北京） | Characteristic quantity-while-drilling assisted inertial inclinometry method and device |
-
2020
- 2020-04-28 CN CN202010348945.8A patent/CN111504313B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101413800A (en) * | 2008-01-18 | 2009-04-22 | 南京航空航天大学 | Navigating and steady aiming method of navigation / steady aiming integrated system |
| CN109891191A (en) * | 2016-11-29 | 2019-06-14 | 赫尔实验室有限公司 | Chance sensor fusion algorithm for independently being guided with brill |
| CN106482749A (en) * | 2016-12-07 | 2017-03-08 | 西北工业大学 | Alignment methods are combined with tachometer based on the inertial navigation of reverse navigation algorithm |
| CN110886606A (en) * | 2019-11-20 | 2020-03-17 | 中国地质大学（北京） | Characteristic quantity-while-drilling assisted inertial inclinometry method and device |
Non-Patent Citations (2)
| Title |
|---|
| 基于光纤陀螺的煤矿钻机定向仪设计;张春熹等;《煤炭学报》;20131130;第38卷(第11期);第2061-2066页 * |
| 煤矿井下开孔定向仪寻北系统误差分析及补偿;燕斌等;《工矿自动化》;20190831;第45卷(第8期);第92-96页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111504313A (en) | 2020-08-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN101514900B (en) | Method for initial alignment of a single-axis rotation strap-down inertial navigation system (SINS) | |
| CN107588769B (en) | Vehicle-mounted strapdown inertial navigation, odometer and altimeter integrated navigation method | |
| CN103235328B (en) | GNSS (global navigation satellite system) and MEMS (micro-electromechanical systems) integrated navigation method | |
| Han et al. | A novel initial alignment scheme for low-cost INS aided by GPS for land vehicle applications | |
| CN109974697A (en) | A kind of high-precision mapping method based on inertia system | |
| CN104515527B (en) | A kind of anti-rough error Combinated navigation method under no gps signal environment | |
| CN111207744B (en) | Pipeline geographical position information measuring method based on thick tail robust filtering | |
| CN111156994A (en) | INS/DR & GNSS loose integrated navigation method based on MEMS inertial component | |
| CN109540130A (en) | A kind of continuous milling machine inertial navigation positioning and orienting method | |
| CN104181572A (en) | Missile-borne inertia/ satellite tight combination navigation method | |
| WO1988005113A1 (en) | Apparatus and method for gravity correction in borehole survey systems | |
| CN105068102A (en) | DSP+FPGA-based ultra-tight combined navigation method | |
| CN111102993A (en) | Initial alignment method for shaking base of rotary modulation type strapdown inertial navigation system | |
| CN105698822A (en) | Autonomous inertial navigation action initial alignment method based on reverse attitude tracking | |
| CN103727941A (en) | Volume kalman nonlinear integrated navigation method based on carrier system speed matching | |
| CN107289942B (en) | Relative navigation system and method for formation flight | |
| CN111337025B (en) | Positioning and orientating instrument hole positioning method suitable for long-distance horizontal core drilling machine | |
| CN114739425A (en) | Coal mining machine positioning calibration system based on RTK-GNSS and total station and application method | |
| CN109959374A (en) | A kind of full-time reverse smooth filtering method of whole process of pedestrian's inertial navigation | |
| CN102645223A (en) | Serial inertial navigation vacuum filtering correction method based on specific force observation | |
| CN111504313B (en) | Drilling positioning and orientating instrument hole positioning method based on speed information assistance | |
| CN111521179B (en) | Drilling positioning director hole positioning method based on self-detection of traveling speed | |
| CN110095135A (en) | A kind of method and device for development machine positioning and directing | |
| CN111521178B (en) | Drilling positioning director hole positioning method based on pipe length constraint | |
| CN113236363A (en) | Mining equipment navigation positioning method, system, equipment and readable storage medium |
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 |