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

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

In-hole positioning method of positioning and orientation instrument suitable for long-distance horizontal core drilling rigs

technical field

The invention belongs to the field of geological exploration, and relates to an in-hole positioning method of a core drilling machine during exploration, in particular to a positioning and orientation instrument in-hole positioning method suitable for a long-distance horizontal core drilling machine.

Background technique

The Sichuan-Tibet Railway is a high-speed railway connecting Sichuan Province and the Tibet Autonomous Region in my country. During the construction process, it needs to face construction problems such as mountains and mountains, terrain height difference, complex geology, etc., and the construction is extremely difficult. In order to eliminate the terrain height difference, it is necessary to use more high bridges and tunnels, so that the bridge-to-tunnel ratio of the whole line of the Sichuan-Tibet Railway is over 80%. In order to ensure the construction quality, during the tunnel construction process, it is necessary to comprehensively and accurately grasp the geological information along the line. However, due to the limitation of the harsh ground conditions in the plateau and alpine regions, general vertical detection cannot be implemented, which cannot meet the geological exploration needs of the plateau and alpine regions.

Long-distance horizontal coring drilling rig is a kind of equipment suitable for rapid geological drilling in horizontal direction and long-distance horizontal coring. It is of great significance for the construction of railways in the plateau and alpine regions. In order to accurately control the drilling path and meet the needs of large buried depth and long-distance geological core drilling, it is necessary to accurately locate and orient the horizontal core drilling rig in the hole. Therefore, the long-distance horizontal core drilling rig needs to be configured with positioning and orientation at the same time. instrument. The positioning and orientation instrument includes an inertial measurement unit, which calculates and outputs the position information of the horizontal core drilling rig, and then adjusts and controls the drilling path. However, in the case of long-distance horizontal drilling, when there is no external information in the hole to assist, the positioning error of the inertial measurement unit will continue to increase with the extension of the working time, which affects the control accuracy of the drilling path.

Therefore, in view of the needs of geological exploration in the plateau and alpine regions, it is necessary to study the positioning method of the positioning directional instrument in the hole suitable for the long-distance horizontal core drilling rig. The horizontal core drilling rig can be precisely positioned in the hole, thereby improving the control accuracy of the drilling path.

SUMMARY OF THE INVENTION

The technical problem to be solved by the present invention is: combining the working characteristics of the long-distance horizontal core drilling rig, rationally plan the working mode of the core drilling rig, make full use of all the measurement data saved by the positioning and orienting instrument, and realize the long distance without external information assistance. The horizontal core drilling rig can accurately locate the hole, thereby improving the control accuracy of the drilling path and meeting the needs of geological exploration in the plateau and alpine regions.

In order to solve the above-mentioned technical problems, the solution proposed by the present invention is:

The positioning method in the hole of the positioning and orientation instrument suitable for the long-distance horizontal core drilling machine includes the following steps:

(1) Install the positioning and directional instrument on the coring device, and place it on the base of the core drilling rig vehicle, and bind the initial position information and initial speed information to the positioning and directional instrument. minutes, and perform initial self-alignment to obtain initial attitude information. The positioning and orientation instrument should save the angular increment information and velocity increment information measured by the inertial measurement unit online; after the positioning and orientation instrument completes the initial self-alignment, based on the non-holonomic constraints The Kalman filter performs forward autonomous navigation and positioning; among them, the nonholonomic constrained Kalman filter is designed according to the following steps:

定位定向仪惯性测量单元的安装误差η为系统状态x(t)，分别确定姿态误差、速度误差、陀螺漂移、加速度计零偏、安装误差的微分方程如下：(1.1) Take attitude error φ ⁿ , velocity error δv ⁿ , position error δp ⁿ , gyro drift ε ^b , accelerometer bias

The installation error η of the inertial measurement unit of the positioning and orientation instrument is the system state x(t). The differential equations for determining the attitude error, velocity error, gyro drift, accelerometer bias, and installation error are as follows:

表示导航坐标系n相对于惯性坐标系i的旋转角速度，

表示载体坐标系b与导航坐标系n之间的姿态矩阵，fⁿ表示导航坐标系下表示的比力，

表示地球自转角速度，

表示转移角速度，vⁿ表示速度，

分别表示导航坐标系旋转角速度误差量、地球自转角速度误差量、转移角速度误差量，

表示陀螺组件测量误差，

表示加速度计组件测量误差，w_g、w_a分别表示陀螺组件测量噪声、加速度计组件测量噪声，η＝[η_θ η_Ψ]^T由俯仰角安装误差η_θ及航向角安装误差η_Ψ构成；in,

represents the rotational angular velocity of the navigation coordinate system n relative to the inertial coordinate system i,

represents the attitude matrix between the carrier coordinate system b and the navigation coordinate system n, f ⁿ represents the specific force expressed in the navigation coordinate system,

represents the angular velocity of the Earth's rotation,

represents the transfer angular velocity, v ⁿ represents the velocity,

respectively represent the rotation angular velocity error of the navigation coordinate system, the earth's rotation angular velocity error, and the transfer angular velocity error,

represents the measurement error of the gyro component,

represents the measurement error of the accelerometer component, w _g and w _a represent the measurement noise of the gyro component and the measurement noise of the accelerometer component, respectively, η=[η _θ η _Ψ ] ^T is composed of the pitch angle installation error η _θ and the heading angle installation error η _Ψ ;

(1.2) According to the differential equations of attitude error, velocity error, position error, gyro drift, accelerometer bias, and installation error determined in step (1.1), the system state equation is constructed as follows:

表示系统状态矩阵；in,

Represents the system state matrix;

In the formula, v _E , v _N , and v _U represent the easting, northing, and vertical velocities, respectively, L represents the local latitude, h represents the local altitude, R _E , R _N represent the radius of the unitary circle and the radius of the meridian circle, respectively, ω _ie Represents the modulus value of the angular velocity of the Earth's rotation;

表示系统噪声矩阵；

represents the system noise matrix;

w(t)=[w _g w _a ] ^T represents system noise;

(1.3) When the corer advances along the drilling path, its lateral velocity and vertical velocity are zero, and the nonholonomic constrained observation _z ( _t )=[ δv _x δv _z ] ^T , and determine the observation equation;

(2) The corer will be pushed to the drilled hole by the core drilling rig truck and parked. The corer will stay at the drilled hole for 10 to 20 seconds, and the first zero-speed correction of the positioning and orientation instrument will be completed. Then, the corer is pushed along the drilling path by means of high-pressure gas push, and the positioning and orientation instrument saves the angular increment information and velocity increment information measured by the inertial measurement unit online;

(3) When the corer is advanced to the drilling rig at the end of the drilling path, the corer completes the coring operation, and then the corer stays at the end of the drilling path again for 10-20 seconds, and completes the positioning and orientation instrument. The second zero-speed correction; in addition, the positioning and orientation instrument should save the angular increment information and speed increment information measured by the inertial measurement unit online;

(4) After the second zero-speed correction is completed, the core remover is pulled out of the hole by the hoist. When the core remover is dragged to the drill hole, it stays for 10 to 20 seconds again, and the positioning and orientation instrument is completed. The third zero-speed correction; after the third zero-speed correction is completed, drag the coring device to the base of the core drilling rig vehicle, and stand still for 15 to 25 minutes again; in addition, the positioning and orientation instrument should save the inertial measurement online Angular increment information and velocity increment information obtained by unit measurement;

加速度计组件测量比力f^b及地球自转角速度

取反，并按照从后至前的时间顺序反转步骤(1)-(4)中保存的角增量信息、速度增量信息，构成逆向测量数据序列，并基于非完整约束卡尔曼滤波器进行逆向自主导航定位，其中，逆向导航解算如下式所述：(5) Measure the angular velocity of the gyro component

The accelerometer assembly measures the specific force f ^b and the angular velocity of the earth's rotation

Invert, and reverse the angular increment information and velocity increment information saved in steps (1)-(4) in the time sequence from back to front to form a reverse measurement data sequence, and based on the nonholonomic constrained Kalman filter Perform reverse autonomous navigation and positioning, where the reverse navigation solution is described as follows:

in,

分别表示逆向时序d、d-1时刻的姿态矩阵，

分别表示逆向时序d、d-1时刻的速度，

分别表示逆向时序d、d-1时刻的位置，gⁿ、

分别表示正向解算、逆向解算当地重力加速度，I₃表示三阶单位矩阵，△T表示采样间隔；

Represent the attitude matrices at the reverse time series d and d-1, respectively,

respectively represent the speed at the time of reverse sequence d and d-1,

Represent the positions of the reverse time series d and d-1, respectively, g ⁿ ,

Respectively represent the forward solution and reverse solution of the local gravitational acceleration, I ₃ represents the third-order unit matrix, and △T represents the sampling interval;

的均值

作为平均定位，将取芯器推进段的定位结果作为钻进路径轨迹，并根据钻进路径轨迹计算其相对于设计路径的偏差，进而对钻进路径进行调整。(6) Use the time-aligned forward autonomous navigation and positioning results p ⁿ and reverse autonomous navigation and positioning results

mean of

As the average positioning, the positioning result of the advancing section of the corer is used as the drilling path trajectory, and the deviation from the design path is calculated according to the drilling path trajectory, and then the drilling path is adjusted.

Further, the determination of the observation equation in the step (1.3) is achieved by the following steps:

(1.3.1) Project the speed output of the positioning directional instrument to the inertial measurement unit coordinate system m, as follows:

表示定位定向仪速度输出在惯性测量单元坐标系m下的投影，v^m表示真实的m坐标系下表示的定位定向仪速度，

表示载体坐标系b与惯性测量单元坐标系m之间的安装关系矩阵，ζ＝[η_θ η_γ η_Ψ]^T表示安装误差角，

表示导航坐标系n与载体坐标系b之间的姿态矩阵；由于横滚角安装误差η_γ不会影响前向速度投影，将其赋值为0，即η_γ＝0；in,

Represents the projection of the speed output of the positioning and orientation instrument in the inertial measurement unit coordinate system m, v ^m represents the positioning and orientation instrument speed expressed in the real m coordinate system,

represents the installation relationship matrix between the carrier coordinate system b and the inertial measurement unit coordinate system m, ζ=[η _θ η _γ η _Ψ ] ^T represents the installation error angle,

Represents the attitude matrix between the navigation coordinate system n and the carrier coordinate system b; since the roll angle installation error η _γ does not affect the forward velocity projection, it is assigned to 0, that is, η _γ =0;

的x、z分量作为非完整约束观测量z(t)，构建观测方程如下：(1.3.2) with

The x and z components of , are used as the nonholonomic constraint observation z(t), and the observation equation is constructed as follows:

z(t)=H(t)x(t)+υ(t) (4)

且M₁＝[1 0 0]，M₃＝[0 0 1]，

υ(t)表示观测噪声。in,

and M ₁ =[1 0 0], M ₃ =[0 0 1],

υ(t) represents observation noise.

Further, the zero-speed correction in the step (2) is carried out according to the following steps:

(2.1) The positioning and orientation instrument automatically detects its zero-speed state according to the angular velocity and speed information, and expands the forward speed error δv _y into an observational quantity. The observational quantity in the zero-speed state is z _ZUPT (t)=[δv _x δv _y δv _z ] ^T ;

的x、y、z分量作为零速状态下的观测量z_ZUPT(t)，构建观测方程如下：(2.2) with

The x, y, and z components of z are taken as the observed quantity z _ZUPT (t) in the zero-speed state, and the observation equation is constructed as follows:

z _ZUPT (t)=H _ZUPT (t)x(t)+μ(t) (5)

μ(t)表示观测噪声；in,

μ(t) represents observation noise;

(2.3) When the positioning and directional instrument is in the zero-speed state, the Kalman filter observation equation is switched from equation (4) to (5), and the measurement update is completed by sequential processing.

Further, the reverse autonomous navigation and positioning in the step (5) includes the following steps:

(5.1) Use the static phase of the first 15-25 minutes of the reverse measurement data to complete the initial alignment and obtain the initial attitude information;

(5.2) Perform reverse filtering according to the nonholonomic constrained Kalman filter described in steps (1.1)-(1.3), attitude error equation, velocity error equation, position error equation, gyro drift, acceleration bias and inertial measurement of positioning and orientation instrument The differential equation of the installation error of the unit remains unchanged, and the observation equation also remains unchanged;

(5.3) During the process of reverse autonomous navigation and positioning, when the corer is in the resident state, perform error correction according to the zero-speed correction method described in steps (2.1)-(2.3).

Further, in the steps (1) and (4), the stationary time of the corer on the car base of the core drill is 15 minutes respectively.

Further, in the steps (1) and (4), the stationary time of the corer on the car base of the core drill is 25 minutes respectively.

Further, in the steps (2), (3) and (4), the dwell time of the corer at the drilled hole and at the end of the drilling path is respectively 10 seconds.

Further, in the steps (2), (3) and (4), the dwell time of the corer at the borehole and at the end of the drilling path is respectively 20 seconds.

Further, in the step (2), a high-pressure hydraulic push method is used to push the corer along the drilling path.

Further, the gyro drift and the accelerometer bias state in the steps (1.1) and (1.2) are corrected by feedback.

Further, the installation error state in the steps (1.1) and (1.2) adopts open-loop correction.

Further, in the step (1), when the inertial measurement unit measures the angular increment information and the velocity increment information, the sampling interval is not greater than 0.01s.

Compared with the prior art, the advantages of the present invention are:

(1) The present invention combines the working characteristics of the long-distance horizontal core drilling rig, rationally plans the working mode of the corer, makes full use of all the measurement data saved by the positioning and orientation instrument, improves the data utilization rate, and realizes long-distance without external information assistance. Precise positioning in the hole of the horizontal core drilling machine;

(2) The present invention makes full use of the difference complementarity of the forward and reverse autonomous navigation positioning error characteristics, and uses the average positioning result as the drilling path trajectory to improve the positioning accuracy.

Description of drawings

Fig. 1 is the schematic flow chart of the method of the present invention;

Fig. 2 is the schematic diagram of the implementation process of the present invention;

Fig. 3 is the forward and reverse solution schematic diagram of the present invention;

FIG. 4 is a schematic diagram of forward and reverse autonomous navigation and positioning according to the present invention.

Detailed ways

The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

As shown in Figure 1, the in-hole positioning method of the positioning and orientation instrument suitable for long-distance horizontal core drilling rigs, by combining the working characteristics of long-distance horizontal core drilling rigs, rationally plan the working mode of the corer, and make full use of the positioning and orientation instrument to save All the measurement data, construct the reverse measurement data sequence, carry out forward and reverse autonomous navigation and positioning based on the nonholonomic constrained Kalman filter, suppress and correct the positioning error combined with the zero-speed correction, and use the forward and reverse autonomous navigation and positioning error characteristics The difference complementarity, the average positioning result is used as the drilling path trajectory, and the positioning accuracy is improved.

As shown in Figure 2, in conjunction with specific application examples, the specific flow of the present invention is described:

(1) Install the positioning and directional instrument on the coring device, and place it on the base of the core drilling rig vehicle, and bind the initial position information and initial speed information to the positioning and directional instrument. minutes, and perform initial self-alignment to obtain initial attitude information. The positioning and orientation instrument should save the angular increment information and velocity increment information measured by the inertial measurement unit online; after the positioning and orientation instrument completes the initial self-alignment, based on the non-holonomic constraints The Kalman filter performs forward autonomous navigation and positioning; among them, the nonholonomic constrained Kalman filter is designed according to the following steps:

定位定向仪惯性测量单元的安装误差η为系统状态x(t)，分别确定姿态误差、速度误差、陀螺漂移、加速度计零偏、安装误差的微分方程如下：(1.1) Take attitude error φ ⁿ , velocity error δv ⁿ , position error δp ⁿ , gyro drift ε ^b , accelerometer bias

The installation error η of the inertial measurement unit of the positioning and orientation instrument is the system state x(t). The differential equations for determining the attitude error, velocity error, gyro drift, accelerometer bias, and installation error are as follows:

表示导航坐标系n相对于惯性坐标系i的旋转角速度，

表示载体坐标系b与导航坐标系n之间的姿态矩阵，fⁿ表示导航坐标系下表示的比力，

表示地球自转角速度，

表示转移角速度，vⁿ表示速度，

分别表示导航坐标系旋转角速度误差量、地球自转角速度误差量、转移角速度误差量，

表示陀螺组件测量误差，

表示加速度计组件测量误差，w_g、w_a分别表示陀螺组件测量噪声、加速度计组件测量噪声，η＝[η_θ η_Ψ]^T由俯仰角安装误差η_θ及航向角安装误差η_Ψ构成；in,

represents the rotational angular velocity of the navigation coordinate system n relative to the inertial coordinate system i,

represents the attitude matrix between the carrier coordinate system b and the navigation coordinate system n, f ⁿ represents the specific force expressed in the navigation coordinate system,

represents the angular velocity of the Earth's rotation,

represents the transfer angular velocity, v ⁿ represents the velocity,

respectively represent the rotation angular velocity error of the navigation coordinate system, the earth's rotation angular velocity error, and the transfer angular velocity error,

represents the measurement error of the gyro component,

represents the measurement error of the accelerometer component, w _g and w _a represent the measurement noise of the gyro component and the measurement noise of the accelerometer component, respectively, η=[η _θ η _Ψ ] ^T is composed of the pitch angle installation error η _θ and the heading angle installation error η _Ψ ;

(1.2) According to the differential equations of attitude error, velocity error, position error, gyro drift, accelerometer bias, and installation error determined in step (1.1), the system state equation is constructed as follows:

表示系统状态矩阵；in,

Represents the system state matrix;

In the formula, v _E , v _N , and v _U represent the easting, northing, and vertical velocities, respectively, L represents the local latitude, h represents the local altitude, R _E , R _N represent the radius of the unitary circle and the radius of the meridian circle, respectively, ω _ie Represents the modulus value of the angular velocity of the Earth's rotation;

表示系统噪声矩阵；

represents the system noise matrix;

w(t)=[w _g w _a ] ^T represents system noise;

(1.3) When the corer advances along the drilling path, its lateral velocity and vertical velocity are zero, and the nonholonomic constrained observation _z ( _t )=[ δv _x δv _z ] ^T , and determine the observation equation;

(2) The corer will be pushed to the drilled hole by the core drilling rig truck and parked. The corer will stay at the drilled hole for 10 to 20 seconds, and the first zero-speed correction of the positioning and orientation instrument will be completed. Then, the corer is pushed along the drilling path by means of high-pressure gas push, and the positioning and orientation instrument saves the angular increment information and velocity increment information measured by the inertial measurement unit online;

(3) When the corer is advanced to the drilling rig at the end of the drilling path, the corer completes the coring operation, and then the corer stays at the end of the drilling path again for 10-20 seconds, and completes the positioning and orientation instrument. The second zero-speed correction; in addition, the positioning and orientation instrument should save the angular increment information and speed increment information measured by the inertial measurement unit online;

(4) After the second zero-speed correction is completed, the core remover is pulled out of the hole by the hoist. When the core remover is dragged to the drill hole, it stays for 10 to 20 seconds again, and the positioning and orientation instrument is completed. The third zero-speed correction; after the third zero-speed correction is completed, drag the coring device to the base of the core drilling rig vehicle, and stand still for 15 to 25 minutes again; in addition, the positioning and orientation instrument should save the inertial measurement online Angular increment information and velocity increment information obtained by unit measurement;

加速度计组件测量比力f^b及地球自转角速度

取反，并按照从后至前的时间顺序反转步骤(1)-(4)中保存的角增量信息、速度增量信息，构成逆向测量数据序列，并基于非完整约束卡尔曼滤波器进行逆向自主导航定位，其中，逆向导航解算如下式所述：(5) As shown in Figure 3 and Figure 4, measure the angular velocity of the gyro assembly

The accelerometer assembly measures the specific force f ^b and the angular velocity of the earth's rotation

Invert, and reverse the angular increment information and velocity increment information saved in steps (1)-(4) in the time sequence from back to front to form a reverse measurement data sequence, and based on the nonholonomic constrained Kalman filter Perform reverse autonomous navigation and positioning, where the reverse navigation solution is described as follows:

in,

分别表示逆向时序d、d-1时刻的姿态矩阵，

分别表示逆向时序d、d-1时刻的速度，

分别表示逆向时序d、d-1时刻的位置，gⁿ、

分别表示正向解算、逆向解算当地重力加速度，I₃表示三阶单位矩阵，△T表示采样间隔；

Represent the attitude matrices at the reverse time series d and d-1, respectively,

respectively represent the speed at the time of reverse sequence d and d-1,

Represent the positions of the reverse time series d and d-1, respectively, g ⁿ ,

Respectively represent the forward solution and reverse solution of the local gravitational acceleration, I ₃ represents the third-order unit matrix, and △T represents the sampling interval;

的均值

作为平均定位，将取芯器推进段的定位结果作为钻进路径轨迹，并根据钻进路径轨迹计算其相对于设计路径的偏差，进而对钻进路径进行调整。(6) Use the time-aligned forward autonomous navigation and positioning results p ⁿ and reverse autonomous navigation and positioning results

mean of

As the average positioning, the positioning result of the advancing section of the corer is used as the drilling path trajectory, and the deviation from the design path is calculated according to the drilling path trajectory, and then the drilling path is adjusted.

Further, the determination of the observation equation in the step (1.3) is achieved by the following steps:

(1.3.1) Project the speed output of the positioning directional instrument to the inertial measurement unit coordinate system m, as follows:

表示定位定向仪速度输出在惯性测量单元坐标系m下的投影，v^m表示真实的m坐标系下表示的定位定向仪速度，

表示载体坐标系b与惯性测量单元坐标系m之间的安装关系矩阵，ζ＝[η_θ η_γ η_Ψ]^T表示安装误差角，

表示导航坐标系n与载体坐标系b之间的姿态矩阵；由于横滚角安装误差η_γ不会影响前向速度投影，将其赋值为0，即η_γ＝0；in,

Represents the projection of the speed output of the positioning and orientation instrument in the inertial measurement unit coordinate system m, v ^m represents the positioning and orientation instrument speed expressed in the real m coordinate system,

represents the installation relationship matrix between the carrier coordinate system b and the inertial measurement unit coordinate system m, ζ=[η _θ η _γ η _Ψ ] ^T represents the installation error angle,

Represents the attitude matrix between the navigation coordinate system n and the carrier coordinate system b; since the roll angle installation error η _γ does not affect the forward velocity projection, it is assigned to 0, that is, η _γ =0;

的x、z分量作为非完整约束观测量z(t)，构建观测方程如下：(1.3.2) with

The x and z components of , are used as the nonholonomic constraint observation z(t), and the observation equation is constructed as follows:

z(t)=H(t)x(t)+υ(t) (4)

且M₁＝[1 0 0]，M₃＝[0 0 1]，

υ(t)表示观测噪声。in,

and M ₁ =[1 0 0], M ₃ =[0 0 1],

υ(t) represents observation noise.

Further, the zero-speed correction in the step (2) is carried out according to the following steps:

(2.1) The positioning and orientation instrument automatically detects its zero-speed state according to the angular velocity and speed information, and expands the forward speed error δv _y into an observational quantity. The observational quantity in the zero-speed state is z _ZUPT (t)=[δv _x δv _y δv _z ] ^T ;

的x、y、z分量作为零速状态下的观测量z_ZUPT(t)，构建观测方程如下：(2.2) with

The x, y, and z components of z are taken as the observed quantity z _ZUPT (t) in the zero-speed state, and the observation equation is constructed as follows:

z _ZUPT (t)=H _ZUPT (t)x(t)+μ(t) (5)

μ(t)表示观测噪声；in,

μ(t) represents observation noise;

(2.3) When the positioning and directional instrument is in the zero-speed state, the Kalman filter observation equation is switched from equation (4) to (5), and the measurement update is completed by sequential processing.

Further, the reverse autonomous navigation and positioning in the step (5) includes the following steps:

(5.1) Use the static phase of the first 15-25 minutes of the reverse measurement data to complete the initial alignment and obtain the initial attitude information;

(5.2) Perform reverse filtering according to the nonholonomic constrained Kalman filter described in steps (1.1)-(1.3), attitude error equation, velocity error equation, position error equation, gyro drift, acceleration bias and inertial measurement of positioning and orientation instrument The differential equation of the installation error of the unit remains unchanged, and the observation equation also remains unchanged;

(5.3) During the process of reverse autonomous navigation and positioning, when the corer is in the resident state, perform error correction according to the zero-speed correction method described in steps (2.1)-(2.3).

Further, in the steps (1) and (4), the stationary time of the corer on the car base of the core drill is 15 minutes respectively.

Further, in the steps (1) and (4), the stationary time of the corer on the car base of the core drill is 25 minutes respectively.

Further, in the steps (2), (3) and (4), the dwell time of the corer at the drilled hole and at the end of the drilling path is respectively 10 seconds.

Further, in the steps (2), (3) and (4), the dwell time of the corer at the borehole and at the end of the drilling path is respectively 20 seconds.

Further, in the step (2), a high-pressure hydraulic push method is used to push the corer along the drilling path.

Further, the gyro drift and the accelerometer bias state in the steps (1.1) and (1.2) are corrected by feedback.

Further, the installation error state in the steps (1.1) and (1.2) adopts open-loop correction.

Further, in the step (1), when the inertial measurement unit measures the angular increment information and the velocity increment information, the sampling interval is not greater than 0.01s.

The above are only the preferred embodiments of the present invention, and the protection scope of the present invention is not limited to the above-mentioned embodiments, and all technical solutions under the idea of the present invention belong to the protection scope of the present invention. It should be pointed out that for those skilled in the art, some improvements and modifications without departing from the principle of the present invention should also be regarded as the protection scope of the present invention.

Claims (12)

1. The positioning method in the hole of the positioning orientator that is applicable to the long-distance horizontal core drilling rig, is characterized in that, comprises the following steps: (1) Install the positioning and directional instrument on the coring device, and place it on the base of the core drilling rig vehicle, and bind the initial position information and initial speed information to the positioning and directional instrument. minutes, and perform initial self-alignment to obtain initial attitude information. The positioning and orientation instrument should save the angular increment information and velocity increment information measured by the inertial measurement unit online; after the positioning and orientation instrument completes the initial self-alignment, based on the non-holonomic constraints The Kalman filter performs forward autonomous navigation and positioning; among them, the nonholonomic constrained Kalman filter is designed according to the following steps: (1.1) Take attitude error φ ⁿ , velocity error δv ⁿ , position error δp ⁿ , gyro drift ε ^b , accelerometer bias

The installation error η of the inertial measurement unit of the positioning and orientation instrument is the system state x(t). The differential equations for determining the attitude error, velocity error, gyro drift, accelerometer bias, and installation error are as follows:

in,

represents the rotational angular velocity of the navigation coordinate system n relative to the inertial coordinate system i,

represents the attitude matrix between the carrier coordinate system b and the navigation coordinate system n, f ⁿ represents the specific force expressed in the navigation coordinate system,

represents the angular velocity of the Earth's rotation,

represents the transfer angular velocity, v ⁿ represents the velocity,

respectively represent the rotation angular velocity error of the navigation coordinate system, the earth's rotation angular velocity error, and the transfer angular velocity error,

represents the measurement error of the gyro component,

represents the measurement error of the accelerometer component, w _g and w _a represent the measurement noise of the gyro component and the measurement noise of the accelerometer component, respectively, η=[η _θ η _Ψ ] ^T is composed of the pitch angle installation error η _θ and the heading angle installation error η _Ψ ; (1.2) According to the differential equations of attitude error, velocity error, position error, gyro drift, accelerometer bias, and installation error determined in step (1.1), the system state equation is constructed as follows:

in,

Represents the system state matrix;

In the formula, v _E , v _N , and v _U represent the easting, northing, and vertical velocities, respectively, L represents the local latitude, h represents the local altitude, R _E , R _N represent the radius of the unitary circle and the radius of the meridian circle, respectively, ω _ie Represents the modulus value of the angular velocity of the Earth's rotation;

represents the system noise matrix; w(t)=[w _g w _a ] ^T represents system noise; (1.3) When the corer advances along the drilling path, its lateral velocity and vertical velocity are zero, and the nonholonomic constrained observation _z ( _t )=[ δv _x δv _z ] ^T , and determine the observation equation; (2) The corer will be pushed to the drilled hole by the core drilling rig truck and parked. The corer will stay at the drilled hole for 10 to 20 seconds, and the first zero-speed correction of the positioning and orientation instrument will be completed. Then, the corer is pushed along the drilling path by means of high-pressure gas push, and the positioning and orientation instrument saves the angular increment information and velocity increment information measured by the inertial measurement unit online; (3) When the corer is advanced to the drilling rig at the end of the drilling path, the corer completes the coring operation, and then the corer stays at the end of the drilling path again for 10-20 seconds, and completes the positioning and orientation instrument. The second zero-speed correction; in addition, the positioning and orientation instrument should save the angular increment information and speed increment information measured by the inertial measurement unit online; (4) After the second zero-speed correction is completed, the core remover is pulled out of the hole by the hoist. When the core remover is dragged to the drill hole, it stays for 10 to 20 seconds again, and the positioning and orientation instrument is completed. The third zero-speed correction; after the third zero-speed correction is completed, drag the coring device to the base of the core drilling rig vehicle, and stand still for 15 to 25 minutes again; in addition, the positioning and orientation instrument should save the inertial measurement online Angular increment information and velocity increment information obtained by unit measurement; (5) Measure the angular velocity of the gyro component

The accelerometer assembly measures the specific force f ^b and the angular velocity of the earth's rotation

Invert, and reverse the angular increment information and velocity increment information saved in steps (1)-(4) in the time sequence from back to front to form a reverse measurement data sequence, and based on the nonholonomic constrained Kalman filter Perform reverse autonomous navigation and positioning, where the reverse navigation solution is described as follows:

in,

Represent the attitude matrices at the reverse time series d and d-1, respectively,

respectively represent the speed at the time of reverse sequence d and d-1,

Represent the positions of the reverse time series d and d-1, respectively, g ⁿ ,

Respectively represent the forward solution and reverse solution of the local gravitational acceleration, I ₃ represents the third-order unit matrix, and △T represents the sampling interval; (6) Use the time-aligned forward autonomous navigation and positioning results p ⁿ and reverse autonomous navigation and positioning results

mean of

As the average positioning, the positioning result of the advancing section of the corer is used as the drilling path trajectory, and the deviation from the design path is calculated according to the drilling path trajectory, and then the drilling path is adjusted. 2. the positioning method in the hole of the positioning orientator that is applicable to the long-distance horizontal core drilling rig as claimed in claim 1, is characterized in that, in described step (1.3), the determination of observation equation is realized by the following steps: (1.3.1) Project the speed output of the positioning directional instrument to the inertial measurement unit coordinate system m, as follows:

in,

Represents the projection of the speed output of the positioning and orientation instrument in the inertial measurement unit coordinate system m, v ^m represents the positioning and orientation instrument speed expressed in the real m coordinate system,

represents the installation relationship matrix between the carrier coordinate system b and the inertial measurement unit coordinate system m, ζ=[η _θ η _γ η _Ψ ] ^T represents the installation error angle,

Represents the attitude matrix between the navigation coordinate system n and the carrier coordinate system b; since the roll angle installation error η _γ does not affect the forward velocity projection, it is assigned to 0, that is, η _γ =0; (1.3.2) with

The x and z components of , are used as the nonholonomic constraint observation z(t), and the observation equation is constructed as follows: z(t)=H(t)x(t)+υ(t) (4) in,

and M ₁ =[1 0 0], M ₃ =[0 0 1],

υ(t) represents observation noise. 3. the positioning method in the hole of the positioning orientator that is applicable to the long-distance horizontal core drilling rig as claimed in claim 1, is characterized in that, the zero-speed correction in described step (2) is carried out according to the following steps: (2.1) The positioning and orientation instrument automatically detects its zero-speed state according to the angular velocity and speed information, and expands the forward speed error δv _y into an observational quantity. The observational quantity in the zero-speed state is z _ZUPT (t)=[δv _x δv _y δv _z ] ^T ; (2.2) with

The x, y, and z components of z are taken as the observed quantity z _ZUPT (t) in the zero-speed state, and the observation equation is constructed as follows: z _ZUPT (t)=H _ZUPT (t)x(t)+μ(t) (5) in,

μ(t) represents observation noise; (2.3) When the positioning and directional instrument is in the zero-speed state, the Kalman filter observation equation is switched from equation (4) to (5), and the measurement update is completed by sequential processing. 4. The method for positioning in the hole of the positioning orientator as claimed in claim 1, wherein the reverse autonomous navigation positioning in the step (5) comprises the following steps: (5.1) Use the static phase of the first 15-25 minutes of the reverse measurement data to complete the initial alignment and obtain the initial attitude information; (5.2) Perform reverse filtering according to the nonholonomic constrained Kalman filter described in steps (1.1)-(1.3), attitude error equation, velocity error equation, position error equation, gyro drift, acceleration bias and inertial measurement of positioning and orientation instrument The differential equation of the installation error of the unit remains unchanged, and the observation equation also remains unchanged; (5.3) During the process of reverse autonomous navigation and positioning, when the corer is in the resident state, perform error correction according to the zero-speed correction method described in steps (2.1)-(2.3). 5. The method for positioning in the hole of a positioning orientator suitable for a long-distance horizontal coring drill as claimed in claim 1, characterized in that, in the steps (1) and (4), the coring device is mounted on the coring drill carrier The periods of rest on the pedestals are 15 minutes each. 6. The method for positioning in the hole of a positioning orientator suitable for a long-distance horizontal coring drill as claimed in claim 1, characterized in that, in the steps (1) and (4), the coring device is mounted on the coring drill carrier The periods of stillness on the pedestal were 25 minutes each. 7. The in-hole positioning method of a positioning and orientation instrument suitable for a long-distance horizontal core drilling machine as claimed in claim 1, characterized in that, in the steps (2), (3) and (4), the corer is in the drilling process. The dwell time at the hole and at the end of the drilling path was 10 seconds, respectively. 8. The in-hole positioning method of a positioning and orientation instrument suitable for a long-distance horizontal core drilling machine as claimed in claim 1, characterized in that, in the steps (2), (3) and (4), the corer is in the drilling process. The dwell time at the hole and at the end of the drilling path was 20 seconds, respectively. 9. The in-hole positioning method of the positioning and orientation instrument suitable for a long-distance horizontal coring drill as claimed in claim 1, characterized in that, in the step (2), a high-pressure hydraulic push method is used to push the coring device along the drill. Advance the path. 10. The in-hole positioning method for a positioning and orientation instrument suitable for a long-distance horizontal core drilling rig as claimed in claim 1, wherein the gyro drift and accelerometer zero-bias state in the steps (1.1) and (1.2) Use feedback correction. 11 . The in-hole positioning method for a positioning and orientation instrument suitable for a long-distance horizontal core drilling rig according to claim 1 , wherein the installation error state in the steps (1.1) and (1.2) adopts open-loop correction. 12 . 12. The method for positioning in-hole positioning of a positioning and orientation instrument suitable for a long-distance horizontal core drilling rig as claimed in claim 1, wherein in the step (1), the inertial measurement unit measures the angular increment information and the velocity increment information When the sampling interval is not more than 0.01s.

Images