CN112797978A

CN112797978A - Guiding method and system of heading machine and storage medium

Info

Publication number: CN112797978A
Application number: CN202110397541.2A
Authority: CN
Inventors: 伦华江; 张晓日; 黄喆; 戴璐璐
Original assignee: Lixin Measurement Shanghai Co ltd; Shanghai Lixin Measurement System Co ltd
Current assignee: Lixin Measurement Shanghai Co ltd; Shanghai Lixin Measurement System Co ltd
Priority date: 2021-04-14
Filing date: 2021-04-14
Publication date: 2021-05-14
Anticipated expiration: 2041-04-14
Also published as: CN112797978B

Abstract

The invention provides a guiding method, a guiding system and a storage medium of a heading machine, wherein the method comprises the following steps: acquiring an attitude angle of the development machine through a gyroscope; acquiring the number of output pulses through a milemeter; calculating to obtain position coordinates of a machine head central point and a machine tail central point of the heading machine under a navigation coordinate system according to the output pulse number, the attitude angle and a pre-acquired initial calibration relation matrix; and calculating to obtain the attitude deviation of the heading machine according to the position coordinates and the plan line. The invention realizes the full-autonomous and high-precision navigation positioning, effectively inhibits the positioning error and effectively improves the accuracy of the heading machine in the advancing direction.

Description

Guiding method and system of heading machine and storage medium

Technical Field

The invention relates to the technical field of inertial measurement, in particular to a guiding method, a guiding system and a storage medium of a heading machine.

Background

Coal is used as main energy and important industrial raw material in China and is an important support for the economic and healthy development of China. However, because coal is buried deep in the stratum, and has the characteristics of complex mining conditions, severe operation environment, high labor intensity, high risk coefficient and the like, coal mining is always regarded as a high-risk industry by people, and the demand on intelligent technology is particularly urgent.

Due to the sealing property of the working environment of the heading machine, positioning methods which are similar to GPS and the like and are commonly used on land cannot be used, and in addition, conventional positioning methods which are similar to infrared correlation methods and the like cannot simultaneously measure the position and the posture of the heading machine. Moreover, due to the shielding of dust, a driver of the heading machine cannot correctly judge the posture and the heading direction of the heading machine in real time, and the phenomenon of deviation and wrong heading occurs, so that the construction quality of the roadway is influenced, and the normal and efficient heading of the roadway is also influenced.

Disclosure of Invention

The invention aims to provide a guiding method, a guiding system and a storage medium of a heading machine, which are used for guiding analysis by combining the motion characteristics of a coal mining machine on the basis of an inertial navigation theory and have the characteristics of high guiding precision, high data updating rate, low maintenance cost, small size and the like.

The technical scheme provided by the invention is as follows:

the invention provides a guiding system of a heading machine, which comprises:

measuring devices including gyroscopes and odometers;

the gyroscope is arranged in an explosion-proof control box, and the explosion-proof control box is arranged at the top of the development machine;

the gyroscope is used for acquiring the attitude angle of the heading machine;

the odometer is arranged on a walking part of the development machine and is used for acquiring the output pulse number;

and the processor is respectively connected with the gyroscope and the odometer and is used for calculating position coordinates of a machine head central point and a machine tail central point of the heading machine under a navigation coordinate system according to the output pulse number, the attitude angle and a pre-acquired initial calibration relation matrix, and calculating the attitude deviation of the heading machine according to the position coordinates and a plan line.

The invention also provides a guiding method of the heading machine, which comprises the following steps:

acquiring an attitude angle of the development machine through a gyroscope;

acquiring the number of output pulses through a milemeter;

calculating to obtain position coordinates of a machine head central point and a machine tail central point of the heading machine under a navigation coordinate system according to the output pulse number, the attitude angle and a pre-acquired initial calibration relation matrix;

and calculating to obtain the attitude deviation of the heading machine according to the position coordinates and the plan line.

The invention also provides a storage medium, wherein at least one instruction is stored in the storage medium, and the instruction is loaded and executed by a processor to realize the operation executed by the guiding method of the heading machine.

The guiding method, the guiding system and the storage medium of the heading machine provided by the invention can realize full-autonomous and high-precision navigation positioning, effectively inhibit positioning errors and effectively improve the accuracy of the heading machine in the advancing direction.

Drawings

The above features, technical features, advantages and implementation of a method, system and storage medium for guiding a heading machine will be further described in the following detailed description of preferred embodiments in a clearly understandable manner in connection with the accompanying drawings.

Figure 1 is a flow chart of one embodiment of a method of guiding a heading machine of the present invention;

FIG. 2 is a schematic structural diagram of one embodiment of a guidance system of a heading machine and its corresponding coordinate system definition according to the present invention;

figure 3 is a schematic structural view of another embodiment of a guidance system of a heading machine of the present invention;

FIG. 4 is a schematic view of attitude deviation of one embodiment of a heading machine guidance method of the present invention;

figure 5 is a schematic view of attitude deviation of another embodiment of a method of guiding a heading machine according to the present invention.

Detailed Description

In the following description, for purposes of explanation and not limitation, specific details are set forth, such as particular system structures, techniques, etc. in order to provide a thorough understanding of the embodiments of the present application. However, it will be apparent to one skilled in the art that the present application may be practiced in other embodiments that depart from these specific details. In other instances, detailed descriptions of well-known systems, devices, circuits, and methods are omitted so as not to obscure the description of the present application with unnecessary detail.

It will be understood that the terms "comprises" and/or "comprising," when used in this specification and the appended claims, specify the presence of stated features, integers, steps, operations, elements, and/or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and/or groups thereof.

For the sake of simplicity, the drawings only schematically show the parts relevant to the present invention, and they do not represent the actual structure as a product. In addition, in order to make the drawings concise and understandable, components having the same structure or function in some of the drawings are only schematically illustrated or only labeled. In this document, "one" means not only "only one" but also a case of "more than one".

It should be further understood that the term "and/or" as used in this specification and the appended claims refers to and includes any and all possible combinations of one or more of the associated listed items.

In addition, in the description of the present application, the terms "first", "second", and the like are used only for distinguishing the description, and are not intended to indicate or imply relative importance.

In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following description will be made with reference to the accompanying drawings. It is obvious that the drawings in the following description are only some examples of the invention, and that for a person skilled in the art, other drawings and embodiments can be derived from them without inventive effort.

In one embodiment of the present invention, a guidance system 102 for a heading machine 101 includes:

measurement devices including a gyroscope 104 and an odometer 106;

the gyroscope 104 is arranged in an explosion-proof control box which is arranged at the top of the heading machine 101;

the gyroscope 104 is used for acquiring the attitude angle of the heading machine 101;

an odometer 106 mounted on a traveling part of the heading machine 101 and configured to acquire the number of output pulses of the heading machine 101;

specifically, the measuring device includes a gyroscope 104 and an odometer 106, wherein the gyroscope 104 is a three-axis fiber optic gyroscope 104. The odometer 106 operates on the principle of detecting the degree of arc of the traveling track shaft of the traveling unit that has rotated for a certain period of time by a photoelectric encoder attached to the traveling unit of the heading machine 101, and then estimating the change in the relative position of the heading machine 101. Preferably, the gyroscope 104 and the odometer 106 are mounted on the top and the traveling part of the heading machine 101, respectively.

And the processor is respectively connected with the gyroscope 104 and the odometer 106 and is used for calculating to obtain space coordinates of a head central point B and a tail central point A of the heading machine 101 under a navigation coordinate system according to the output pulse number, the attitude angle and a pre-acquired initial calibration relation matrix, and then calculating to obtain the attitude deviation of the heading machine 101 according to the position coordinates and the plan line.

Specifically, as shown in fig. 2 and 3, the guiding system is composed of a heading machine 101 and a guiding system 102, and measuring devices included in the guiding system 102 are all mounted on the heading machine 101. The measuring device mainly comprises a gyroscope 104 and an odometer 106. The gyroscope 104 is arranged on the plane of the development machine 101 through an explosion-proof shell and is used for providing attitude angles such as a course angle, a roll angle, a pitch angle and the like; the odometer 106 is mounted on a traveling track shaft 107 of the heading machine 101 and is used for providing the number of output pulses by which the heading machine 101 advances.

The processor is installed in the explosion-proof control box and is responsible for fusing the data acquired by the measuring unit to obtain real-time vector mileage, resolving real-time pose through a combined navigation algorithm and other error elimination algorithms, and providing attitude data and position data of the heading machine 101 for dynamic display on the display screen 109.

As shown in fig. 2, when the heading machine 101 guiding method based on the strapdown inertial navigation system provided by the present invention is used for resolving the pose of the heading machine 101, a relative relationship with a relevant coordinate system required to be used for positioning the heading machine needs to be established, wherein the coordinate system is defined as shown in fig. 3:

the navigation coordinate system OnXnYnZn (hereinafter referred to as navigation coordinate system n or n system) is selected according to the actual construction site, where each coordinate axis direction is the same as the local northeast coordinate system (also referred to as the station center coordinate system).

A carrier coordinate system omxmymzmm (hereinafter, simply referred to as a carrier coordinate system m or m system), in which the m system takes the centroid of the gyroscope 104 as the origin of coordinates, and the horizontal axis, the vertical axis, and the vertical axis of the gyroscope 104 as Xm, Ym, and Zm axes.

A heading machine 101 coordinate system ObXbYbZb (hereinafter referred to as heading machine 101 coordinate system b or b system) takes the characteristic points of the heading machine 101 as the origin of coordinates, the longitudinal axis of the heading machine 101 coordinate system b is a Yb axis, and the direction of motion of the heading machine 101 is positive; the horizontal axis of the coordinate system b of the heading machine 101 is Xb, and the right is positive; according to the right-hand rule, the vertical axis of the b system of the coordinate system of the heading machine 101 is Zb axis, and the vertical direction is positive.

The processor is respectively connected with the gyroscope 104 and the odometer 106 in a communication mode, and can acquire the attitude angle of the heading machine 101 detected and acquired by the gyroscope 104 and acquire the output pulse number of the heading machine 101 from the odometer 106. After the processor obtains the output pulse number and the attitude angle, inertial navigation settlement is carried out according to the attitude angle to obtain the attitude deviation of the heading machine 101.

The method of the invention combines the automatic guidance of the gyroscope 104 and the manual positioning laser pointing of the total station to guide the tunneling roadway 2, can provide attitude angles (including pitch angles, roll angles and azimuth angles), five-dimensional measurement information of up-down deviation and five-dimensional measurement information of left-right deviation in real time, further assists the tunneling machine 101 to run according to a preset track, has great practical significance for efficiently and rapidly tunneling the roadway 2 (including a shaft or a mine tunnel), and is an important component part for improving the mechanization and automation level of the electromechanical equipment of the mine.

Based on the foregoing embodiments, a processor includes:

the calibration module is used for acquiring rigid body coordinates of a head central point B and a tail central point A of the heading machine 101, which are acquired from a total station in an initial state, in a navigation coordinate system respectively, and acquiring an initial calibration relation matrix of the heading machine 101 in the initial state;

the initial calibration relation matrix is a matrix of the carrier coordinate system relative to the heading machine 101 coordinate system in an initial state.

Specifically, the heading machine 101 system is used as a carrier of a guiding measurement system and is used for bearing the guiding measurement system, and the guiding measurement system comprises a measurement device and a processor; the measuring device can be divided into a high-precision optical fiber gyroscope 104 and a speedometer 106, and certainly, the guiding measuring system further comprises system accessories which comprise an explosion-proof control box, a display screen, a data storage board, a flame-retardant cable and the like. The processor analyzes and calculates the data measured by the measuring device to obtain real-time attitude data of the heading machine 101 and displays the real-time attitude data. Before the guiding step of the guiding system, firstly, zero position configuration is performed on the heading machine 101, namely, zero position configuration is performed by using the relative relationship between the guiding system calibration gyroscope 104 and the central axis of the heading machine 101. Because the gyroscope 104 is fixedly connected to the heading and anchoring machine (or the heading machine 101), the rigid relationship (i.e., the initial calibration relationship matrix) between the carrier coordinate system m and the heading machine 101 coordinate system b is kept unchanged during the forward operation of the heading and anchoring machine.

The calibration module comprises:

the first acquisition submodule is used for acquiring initial coordinates of a tail central point A and a head central point B in a navigation coordinate system by using a total station in an initial state and acquiring an initial attitude angle from the gyroscope 104; the initial attitude angle comprises an initial pitch angle, an initial roll angle and an initial course angle;

specifically, the first obtaining submodule obtains initial coordinates of a tail central point a, a head central point B and a gyroscope 104 central point in a navigation coordinate system n by using a total station at an initial state

、

The gyroscope 104 calculates three attitude angles of the gyroscope, namely an initial pitch angle

Initial roll angle

And initial course angle

。

And the first calculation submodule is used for calculating to obtain an initial rotation matrix according to the initial coordinate and the plan line, calculating to obtain a zero rotation matrix according to the initial attitude angle, and calculating to obtain an initial calibration relation matrix according to the initial rotation matrix and the zero rotation matrix.

Specifically, because the marking line is given, a first linear direction vector ls corresponding to the marking line is given, and according to an initial state, an initial coordinate of a tail center point A and an initial coordinate of a head center point B under a navigation coordinate system n are obtained by using a total station

And

and further using the spatial coordinate point

And

calculating a second linear direction vector lt in the navigation coordinate system n, and calculating an initial rotation matrix of the heading and anchoring machine (also can be a heading machine 101) in an initial state according to the first linear direction vector ls and the second linear direction vector lt

The calculation formula is shown as the following formula (1):

（1）

wherein l_sFor counting the corresponding first linear direction vector, l_tThe initial coordinates of the machine tail central point A and the machine head central point B in a navigation coordinate system are utilized

And

and calculating to obtain a second linear direction vector under the navigation coordinate system.

Substituting the initial attitude angle into the following formula (2) by the first calculation submodule to calculate to obtain a zero rotation matrix

：

(2)

Wherein,

in the form of a zero rotation matrix,

for the initial pitch angle in the carrier coordinate system acquired by the gyroscope 104 at the initial state,

for the initial roll angle in the carrier coordinate system acquired by the gyroscope 104 at the initial state,

the initial heading angle in the carrier coordinate system acquired by the gyroscope 104 at the initial state.

Calculating by the formula to obtain an initial rotation matrix and a zero rotation matrix of the driving and anchoring machine (also can be the heading machine 101) in an initial state, substituting the initial rotation matrix and the zero rotation matrix into the following formula (3) to calculate an initial calibration relation matrix from the carrier coordinate system m to the heading machine 101 coordinate system b:

（3）

wherein,

is an initial calibration relation matrix from the carrier coordinate system to the heading machine 101 coordinate system,

is the inverse of the zero rotation matrix,

is an initial rotation matrix in the navigation coordinate system in the initial state.

The first obtaining sub-module is further used for obtaining an initial coordinate of the central point of the gyroscope 104 in the navigation coordinate system by using the total station in an initial state;

and the first calculation submodule is also used for calculating rigid body coordinates of the head central point B and the tail central point A respectively in a carrier coordinate system according to initial coordinates of the tail central point A, the head central point B and the central point of the gyroscope 104 respectively in a navigation coordinate system and an inverse matrix of the zero rotation matrix.

Specifically, the first obtaining sub-module further obtains an initial coordinate of the center point of the gyroscope 104 in the navigation coordinate system n by using the total station in the initial state

. The first calculation submodule can calculate to obtain rigid body coordinates of a tail central point A and a head central point B under a carrier coordinate system m according to the following formula (4)

And

：

（4）

wherein,

the initial coordinate of the tail center point A under a navigation coordinate system n is obtained from the total station in the initial state,

the initial coordinate of the head center point B under the navigation coordinate system n is obtained from the total station in the initial state,

initial coordinates of the center point of the gyroscope 104 in the navigation coordinate system n are obtained from the total station in an initial state,

is a rigid body coordinate of the central point A of the tail of the initial state under a carrier coordinate system m,

is a rigid body coordinate of a machine head central point B under a carrier coordinate system m in an initial state,

for the zero rotation matrix from the initial state carrier coordinate system m to the navigation coordinate system n,

the inverse of the zero rotation matrix.

Once the positions of the gyroscope 104 and the odometer 106 are fixed, the accuracy of the initial calibration relation matrix and the initial attitude angle directly affects the accuracy of the guidance system in calculating the attitude deviation of the heading machine 101, so that before the guidance system performs guidance, zero position configuration, namely initial calibration, is made to be an important prerequisite for subsequent guidance work. According to the method, the zero position configuration is carried out on the heading machine 101 by the principle that the rigid body relation between the carrier coordinate system m and the coordinate system b of the heading machine 101 is kept unchanged in the advancing operation process of the heading and anchoring machine, so that the measurement accuracy of attitude deviation is improved, the accuracy of guiding advancing of the heading machine 101 in a mine tunnel is further improved, and the tunneling efficiency is improved due to the reduction of the error rate of the guiding direction.

Based on the foregoing embodiment, the processor further includes:

the initial alignment module is used for calculating an initial attitude transformation matrix of the navigation coordinate system relative to the carrier coordinate system according to the attitude angle acquired from the gyroscope 104;

specifically, the carrier coordinate system m has three angle changes relative to the navigation coordinate system n, namely a heading angle psi, a pitch angle theta and a roll angle phi, and according to the Euler transformation formula, the attitude transformation matrix of the navigation coordinate system n relative to the carrier coordinate system m can be calculated and obtained through the following formula (5)

Comprises the following steps:

(5)

wherein,

psi is a heading angle acquired by the gyroscope 104 under the carrier coordinate system, phi is a roll angle acquired by the gyroscope 104 under the carrier coordinate system, and theta is a pitch angle acquired by the gyroscope 104 under the carrier coordinate system.

Due to initial attitude transformation matrix

I.e. the initial value of the attitude transformation matrix of the carrier coordinate system m relative to the navigation coordinate system n, therefore, the initial attitude transformation matrix of the navigation coordinate system relative to the carrier coordinate system can be calculated according to the following formula (6)

：

（6）

Wherein,

transforming matrices for initial poses

The inverse of the matrix of (a) is,

is the transpose of the attitude transformation matrix.

The guiding planning module is used for fitting and generating a marking line of the heading machine 101 in the navigation coordinate system according to the initial space coordinate and the preset target space coordinate of the heading machine 101 in the navigation coordinate system;

specifically, a movement route of the heading machine 101 under a navigation coordinate system n is fitted according to the roadway environment through an initial space coordinate of the heading machine 101 under the navigation coordinate system and a target space coordinate under the navigation coordinate system, and the movement route is used as a planning line.

The real-time measurement module is used for acquiring a real-time attitude angle measured by the gyroscope 104 and acquiring the output pulse number measured by the odometer 106; the real-time attitude angle comprises a real-time course angle, a real-time roll angle and a real-time pitch angle;

the mileage counting module 106 is used for calculating the mileage increment of the heading machine 101 according to the output pulse number and the factory parameters of the mileage counting module 106;

the mileage calculation module 106 includes:

the second calculation submodule is used for calculating displacement increments respectively corresponding to the last sampling period and the current sampling period of the heading machine 101 in the heading machine 101 coordinate system according to the output pulse number and the delivery parameters;

and the second calculation submodule is also used for calculating and obtaining the mileage increment from the last sampling period to the current sampling period according to the displacement matrixes corresponding to the displacement increments respectively corresponding to the last sampling period and the current sampling period.

Specifically, the odometer 106 located on the walking track shaft 107 (i.e., the walking part of the present invention) measures in real time to obtain the output pulse number, the factory parameters of the odometer 106 include unit motion displacement, resolution and frequency multiplication coefficient, and then the displacement increment s of the heading machine 101 in the initial sampling period of the heading machine 101 in the coordinate system b of the heading machine 101 is obtained by substituting the output pulse number and the factory parameters into the following formula (6)₀：

（6）

Similarly, the displacement increment s of the heading machine 101 in the current sampling period t under the heading machine 101 coordinate system b is calculated according to the following formula_t：

（7）

Wherein a is a unit movement displacement corresponding to a displacement per 1 ° rotation of the traveling part of the heading machine 101 during movement, Z is a resolution (number of pulses/revolution) of the odometer 106, and M is a resolution (number of pulses/revolution) of the odometer₀Is the number of output pulses, M, of the odometer 106 during the initial sampling period_tIs the output pulse number, s, of the odometer 106 in the current sampling period t₀Is a displacement increment corresponding to the initial sampling period, s_tFor the displacement increment corresponding to the current sampling period, K_DThe displacement increment of the initial sampling period is set to s for the odometer 106 to output the frequency multiplication factor of the pulse₀。

Suppose that the odometer 106 measures a displacement matrix of

The displacement matrix measured in the current sampling period t is

. Wherein,

. The mileage increment in the coordinate system b of the heading machine 101 between the initial sampling period and the current sampling period t is expressed as

. The mileage increment is a vector representation mode of displacement increment under a coordinate system b of the tunneling machine 101, and the mileage increment with the current sampling period t as a terminal point and any one sampling period before as a starting point can be calculated

And k is any sampling period before the current sampling period t, k is more than or equal to 0 and less than t, and k belongs to a positive integer. Thus, the mileage increment of the odometer 106 between the initial sampling period and the current sampling period in the coordinate system b of the heading machine 101 may be calculated

。

In addition, the gyroscope 104 on the heading machine 101 measures and obtains the real-time attitude angle ω_bThe expression formula of the real-time attitude angle is shown in the following formula (8):

（8）

wherein,

is the real-time pitch angle of the gyroscope 104 in the carrier coordinate system m at the current sampling period t,

is the real-time roll angle of the gyroscope 104 in the carrier coordinate system m,

is the real-time heading angle of the gyroscope 104 in the carrier coordinate system m,

are measured values of the gyroscope 104 in the carrier coordinate system m.

The real-time alignment module is used for calculating a real-time attitude transformation matrix of the navigation coordinate system relative to the carrier coordinate system according to the real-time attitude angle;

specifically, the real-time attitude angle measured by the gyroscope 104 in the current sampling period t is substituted into the following formula (11), and the real-time attitude transformation matrix of the navigation coordinate system relative to the carrier coordinate system can be calculated

：

(12)

Wherein,

and converting a matrix for the real-time posture of the navigation coordinate system relative to the carrier coordinate system.

And the deviation calculation module is used for calculating to obtain position coordinates according to the initial calibration relation matrix, the real-time attitude conversion matrix and the mileage increment and rigid body coordinates of the gyroscope 104 in the navigation coordinate system, and calculating to obtain attitude deviation of the heading machine 101 according to the position coordinates and the plan line.

The deviation calculation module includes:

the third calculation operator module is used for calculating to obtain the real-time coordinate of the central point of the gyroscope 104 in the navigation coordinate system according to the initial calibration relation matrix, the real-time attitude conversion matrix, the mileage increment and the initial coordinate of the central point of the gyroscope 104 in the navigation coordinate system;

specifically, coordinate calculation is performed by using a dead reckoning algorithm in combination with data of the odometer 106 and the gyroscope 104. I.e., a real-time attitude transformation matrix formed from real-time attitude angles measured by the gyroscope 104 is

According to the dead reckoning principle, the real-time coordinates of the center point of the gyroscope 104 can be calculated according to the following equation (12):

（12）

wherein,

is the real-time coordinates of the center point of the gyroscope 104 in the navigational coordinate system,

is a real-time attitude transformation matrix of the navigation coordinate system relative to the carrier coordinate system in the current sampling period,

is the mileage increment between the initial sampling period and the current sampling period of the odometer 106 under the heading machine 101 coordinate system b.

The coordinate solving submodule is also used for calculating to obtain the space coordinates of the tail central point and the head central point of the current sampling period under the navigation coordinate system according to the real-time coordinate, the rigid body coordinate, the real-time attitude transformation matrix, and the initial coordinates of the tail central point and the head central point under the navigation coordinate system;

specifically, the spatial coordinates of the tail center point a and the head center point B in the current sampling period in the navigation coordinate system can be calculated according to the following formula (13):

（13）

wherein, P_atIs the space coordinate P of the tail center point A of the current sampling period in the navigation coordinate system n system_btThe spatial coordinates of the handpiece center point B in the navigation coordinate system n system in the current sampling period are shown.

The projection submodule is used for projecting the center point of the machine head on a vertical axis and a horizontal axis respectively to obtain corresponding machine head projection points, projecting the center point of the machine tail on the vertical axis and the horizontal axis respectively to obtain corresponding machine tail projection points, projecting the starting point of the marking line on the vertical axis and the horizontal axis respectively to obtain corresponding starting point projection points, and projecting the end point of the marking line on the vertical axis and the horizontal axis respectively to obtain corresponding end point projection points;

specifically, deviation calculation is carried out according to space coordinates of a tail central point A and a head central point B of the current sampling period under a navigation coordinate system, namely horizontal and vertical deviation values are calculated according to the principle that the shortest distance from the tail central point A and the head central point B to a straight line.

The principle of solving the deviation of the center point a of the tail and the center point B of the nose is shown in fig. 4 and 5, and the center point a of the tail and the center point B of the nose of the excavator (or the excavator 101) are located on a horizontal axis, i.e., X_nO_nY_nA plane (i.e., a plane composed of an X-axis, a Y-axis, and an origin in a navigation coordinate system) and a vertical axis (i.e., Z)_nO_nY_nRespectively projecting at planes (namely planes composed of a Y axis, a Z axis and an origin in a navigation coordinate system), and further obtaining corresponding projection points A ', B ', and D ', and respectively locating a planning line starting point C and a planning line end point D at an X position_nO_nY_nPlane and Z_nO_nY_nAnd respectively projecting the planes to obtain corresponding projection points C ', D ' and D '. As can be seen from fig. 5, plane a 'AA' intersects with segment CD, segment C 'D' and segment C 'D' respectively for the points E, E 'and E', and furthermore plane B 'BB' intersects with segment CD, segment C 'D' and segment C 'D' respectively for the points F, F 'and F'.

According to the relationship between the machine tail central point A, the machine head central point B and the straight line, the deviation formulas are shown as the following formula (14):

（14）

and the deviation calculation submodule is used for calculating the horizontal deviation and the vertical deviation of the center point of the machine head relative to the marking line and the horizontal deviation and the vertical deviation of the center point of the machine tail relative to the marking line through a space distance solving formula according to the coordinate values of the projection points of the machine head and the machine tail and the coordinate values of the projection points of the starting point and the end point.

Specifically, since the plan line is generated from the initial space coordinates and the preset target space coordinates, the coordinates of the plan line start point and plan line end point in the navigation coordinate system n are known and are respectively denoted as C (x)_C，y_C，z_C) And D (x)_D，y_D，z_D) The coordinates of each projection point are C' respectively by equation (14) above_C，y_C，0）、C´´（0，y_C，z_C）、D´（x_D，y_D，0）、D´´（0，y_D，z_D). The spatial coordinates corresponding to the head center point B and the tail center point A can be solved by the formula (13), and the head center point B and the tail center point A are respectively marked as P_at（x_a，y_a，z_a) And P_bt（x_b，y_b，z_b) Then, A' can be calculated_a，y_a，z_a）、A´´（x_a，y_a，z_a）、B´（x_b，y_b，z_b）、B´´（x_b，y_b，z_b). The coordinates corresponding to the projection points are substituted into the following formula (15) to be calculated:

(15)

in the formula,

and

the central point A of the tail of the digging and anchoring machine (or the digging machine 101) is respectively relative to the meterThe amount of horizontal and vertical deviation of the scribe line,

and

the horizontal deviation amount and the vertical deviation amount of the head center point B of the heading and anchoring machine (or the heading machine 101) relative to the score line respectively, and by this time, attitude deviation calculation is completed, the attitude deviation comprises the horizontal deviation amount and the vertical deviation amount of the tail center point a relative to the score line and the horizontal deviation amount and the vertical deviation amount of the head center point B relative to the score line.

Based on the foregoing embodiment, further comprising: a display;

and the display is connected with the processor and is used for graphically displaying the attitude deviation of the heading machine 101 so as to guide a driver of the heading machine 101 to heading towards the correct direction.

Specifically, dead reckoning is carried out according to information of the gyroscope 104 and the odometer 106 to obtain real-time space coordinates corresponding to the center points of the machine head and the machine tail, deviation calculation of the heading machine 101 is carried out according to the space coordinates corresponding to the center points of the machine head and the machine tail, and finally the attitude of the coal machine is adjusted by a driver of the heading machine 101 according to attitude deviation prompted by software and parameters displayed by a display screen. The display screen displays the attitude of the heading machine 101 and various deviation information (namely the attitude deviation of the invention) in a graphical manner through the guidance software, so as to guide a driver of the heading machine 101 to heading in the correct direction.

In summary, the invention includes at least one of the following beneficial effects:

1. the invention adopts the combination of the odometer 106 and the inertial navigation system for guidance, can realize full-autonomous and high-precision navigation positioning, effectively inhibits positioning errors, and lays a foundation for obtaining the attitude data of the heading machine 101 accurately.

2. According to the invention, the strapdown inertial navigation system and the dead reckoning system are combined for reckoning, so that the positioning precision of the system is effectively improved, and the accuracy of coal machine construction is enhanced. The automatic tunneling of the tunneling machine 101 provides a technical foundation, so that a tunneling driver can be far away from a tunneling working face and is liberated from a severe working environment; the problem of irregular tunnel forming caused by human factors of a tunneling driver can be effectively avoided, the tunnel tunneling efficiency is improved, the tunnel forming quality is improved, and the maintenance of a large-section coal tunnel is facilitated.

The invention also provides a guiding method of the heading machine 101, as shown in fig. 1, comprising the following steps:

s100, acquiring an attitude angle of the heading machine 101 through a gyroscope 104;

s200, acquiring the output pulse number of the heading machine 101 through the odometer 106;

s300, calculating to obtain position coordinates of a head central point and a tail central point of the heading machine 101 under a navigation coordinate system according to the output pulse number, the attitude angle and a pre-acquired initial calibration relation matrix;

s400, calculating to obtain the attitude deviation of the heading machine 101 according to the position coordinates and the planning line.

The invention also provides a guiding method of the heading machine 101, which comprises the following steps:

s010 obtains rigid body coordinates of a head central point and a tail central point of the heading machine 101 in a navigation coordinate system from a total station in an initial state, and obtains an initial calibration relation matrix of the heading machine 101 in the initial state;

s010 specifically includes the steps:

s011, acquiring initial coordinates of a tail central point and a head central point under a navigation coordinate system by using a total station in an initial state, and acquiring an initial attitude angle from a gyroscope 104; the initial attitude angle comprises an initial pitch angle, an initial roll angle and an initial course angle;

s012, calculating according to the initial coordinate and the plan line to obtain an initial rotation matrix, calculating according to the initial attitude angle to obtain a zero rotation matrix, and calculating according to the initial rotation matrix and the zero rotation matrix to obtain an initial calibration relation matrix;

s013, acquiring initial coordinates of the central point of the gyroscope 104 in a navigation coordinate system by using a total station in an initial state;

s014 respectively obtaining rigid body coordinates of the machine head central point and the machine tail central point under a carrier coordinate system according to initial coordinates of the machine tail central point, the machine head central point and the central point of the gyroscope 104 under a navigation coordinate system and inverse matrix calculation of a zero rotation matrix;

s310, calculating to obtain an initial attitude transformation matrix of the navigation coordinate system relative to the carrier coordinate system according to the attitude angle acquired from the gyroscope 104;

s320, fitting and generating a marking line of the heading machine 101 in the navigation coordinate system according to the initial space coordinate and the preset target space coordinate of the heading machine 101 in the navigation coordinate system;

s330, acquiring a real-time attitude angle measured by the gyroscope 104, and acquiring the number of output pulses measured by the odometer 106; the real-time attitude angle comprises a real-time course angle, a real-time roll angle and a real-time pitch angle;

s340, calculating to obtain the mileage increment of the heading machine 101 according to the output pulse number and the factory parameters of the odometer 106;

s340 specifically includes the steps of:

s341, calculating displacement increments respectively corresponding to the last sampling period and the current sampling period of the heading machine 101 in the heading machine 101 coordinate system according to the output pulse number and the delivery parameters;

and S342, calculating the mileage increment from the last sampling period to the current sampling period according to the displacement matrixes corresponding to the displacement increments respectively corresponding to the last sampling period and the current sampling period.

S350, calculating to obtain a real-time attitude transformation matrix of the navigation coordinate system relative to the carrier coordinate system according to the real-time attitude angle;

s360, calculating to obtain position coordinates according to the initial calibration relation matrix, the real-time attitude transformation matrix, the mileage increment and rigid body coordinates of the gyroscope 104 in the navigation coordinate system;

S400 specifically comprises the following steps:

s410, calculating to obtain a real-time coordinate of the central point of the gyroscope 104 in the navigation coordinate system according to the initial calibration relation matrix, the real-time attitude transformation matrix, the mileage increment and the initial coordinate of the central point of the gyroscope 104 in the navigation coordinate system;

s420, according to the real-time coordinates, the rigid body coordinates, the real-time posture conversion matrix, the initial coordinates of the machine tail central point and the machine head central point under the navigation coordinate system, respectively, calculating to obtain the space coordinates of the machine tail central point and the machine head central point under the navigation coordinate system in the current sampling period;

s430, respectively projecting the center point of the machine head on a vertical axis and a horizontal axis to obtain corresponding machine head projection points, respectively projecting the center point of the machine tail on the vertical axis and the horizontal axis to obtain corresponding machine tail projection points, respectively projecting the starting point of the marking line on the vertical axis and the horizontal axis to obtain corresponding starting point projection points, and respectively projecting the end point of the marking line on the vertical axis and the horizontal axis to obtain corresponding end point projection points; the vertical axis is a plane formed by an X axis, a Y axis and an origin in the navigation coordinate system, and the horizontal axis is a plane formed by the Y axis, the Z axis and the origin in the navigation coordinate system;

s440, according to the coordinate values of the projection points of the head and the tail and the coordinate values of the projection points of the starting point and the end point, the horizontal offset and the vertical offset of the center point of the head relative to the marking line and the horizontal offset and the vertical offset of the center point of the tail relative to the marking line are obtained through calculation of a space distance solving formula.

The S500 display receives the attitude deviation and displays the attitude deviation of the heading machine 101 in a graphical mode so as to guide a driver of the heading machine 101 to heading towards the correct direction.

The initial calibration relation matrix is a matrix of the carrier coordinate system relative to the coordinate system of the heading machine 101 in an initial state, and the initial attitude angle comprises an initial pitch angle, an initial roll angle and an initial course angle.

Specifically, this embodiment is a method embodiment corresponding to the system embodiment, and specific effects are referred to the system embodiment, which is not described in detail herein.

It will be apparent to those skilled in the art that, for convenience and brevity of description, only the above-described division of program modules is illustrated, and in practical applications, the above-described distribution of functions may be performed by different program modules, that is, the internal structure of the apparatus may be divided into different program units or modules to perform all or part of the above-described functions. Each program module in the embodiments may be integrated in one processing unit, or each unit may exist alone physically, or two or more units are integrated in one processing unit, and the integrated unit may be implemented in a form of hardware, or may be implemented in a form of software program unit. In addition, the specific names of the program modules are only used for distinguishing the program modules from one another, and are not used for limiting the protection scope of the application.

They may be implemented in program code that is executable by a computing device such that it is executed by the computing device, or separately, or as individual integrated circuit modules, or as a plurality or steps of individual integrated circuit modules. Thus, the present invention is not limited to any specific combination of hardware and software.

In the above embodiments, the descriptions of the respective embodiments have respective emphasis, and reference may be made to the related descriptions of other embodiments for parts that are not described or recited in detail in a certain embodiment.

Those of ordinary skill in the art will appreciate that the various illustrative elements and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware or combinations of computer software and electronic hardware. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the implementation. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present application.

In the embodiments provided in the present application, it should be understood that the disclosed apparatus/terminal device and method may be implemented in other ways. For example, the above-described embodiments of the apparatus/terminal device are merely illustrative, and for example, the division of the modules or units is only one logical division, and there may be other divisions when actually implemented, for example, a plurality of units or components may be combined or integrated into another system, or some features may be omitted, or not executed. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be an indirect coupling or communication connection of some interfaces, devices or units, and may be in an electrical, mechanical or other form.

The units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiment.

In addition, functional units in the embodiments of the present application may be integrated into one processing unit, or each unit may exist alone physically, or two or more units are integrated into one unit. The integrated unit can be realized in a form of hardware, and can also be realized in a form of a software functional unit.

It should be noted that the above embodiments can be freely combined as necessary. The foregoing is only a preferred embodiment of the present invention, and it should be noted that, for those skilled in the art, various modifications and decorations can be made without departing from the principle of the present invention, and these modifications and decorations should also be regarded as the protection scope of the present invention.

Claims

1. A guide system for a heading machine, comprising:

measuring devices including gyroscopes and odometers;

the gyroscope is used for acquiring the attitude angle of the heading machine;

2. The guidance system of a heading machine according to claim 1, wherein the processor comprises:

the calibration module is used for acquiring rigid body coordinates of a head central point and a tail central point of the tunneling machine under a navigation coordinate system from a total station in an initial state and acquiring an initial calibration relation matrix of the tunneling machine in the initial state;

and the initial calibration relation matrix is a matrix of the carrier coordinate system relative to the heading machine coordinate system in the initial state.

3. The guidance system of a heading machine of claim 2, wherein the calibration module comprises:

the first acquisition submodule is used for acquiring initial coordinates of a tail central point and a head central point under a navigation coordinate system by using a total station in an initial state and acquiring an initial attitude angle from the gyroscope; the initial attitude angle comprises an initial pitch angle, an initial roll angle and an initial course angle;

the first calculation submodule is used for calculating according to the initial coordinate and the plan line to obtain an initial rotation matrix, calculating according to the initial attitude angle to obtain a zero rotation matrix, and calculating according to the initial rotation matrix and the zero rotation matrix to obtain the initial calibration relation matrix;

；

And

calculating to obtain a second linear direction vector under the navigation coordinate system,

in the form of a zero rotation matrix,

the initial pitch angle of the gyroscope in the carrier coordinate system is acquired in the initial state,

the initial roll angle of the gyroscope in the carrier coordinate system is obtained in the initial state,

the initial course angle of the gyroscope in the carrier coordinate system is obtained in the initial state,

is an initial calibration relation matrix from a carrier coordinate system to a heading machine coordinate system,

is the inverse of the zero rotation matrix,

4. The guidance system of a heading machine of claim 3, wherein the processor further comprises:

the initial alignment module is used for calculating an initial attitude transformation matrix of the navigation coordinate system relative to the carrier coordinate system according to the attitude angle acquired from the gyroscope;

the guiding planning module is used for generating a marking line of the heading machine under a navigation coordinate system in a fitting mode according to an initial space coordinate and a preset target space coordinate of the heading machine under the navigation coordinate system;

the real-time measuring module is used for acquiring a real-time attitude angle measured by the gyroscope and acquiring the output pulse number measured by the odometer; the real-time attitude angle comprises a real-time course angle, a real-time roll angle and a real-time pitch angle;

the mileage calculation module is used for calculating the mileage increment of the heading machine according to the output pulse number and the factory parameters of the mileage meter;

the real-time alignment module is used for calculating a real-time attitude transformation matrix of the navigation coordinate system relative to the carrier coordinate system according to a real-time attitude angle;

the deviation calculation module is used for calculating to obtain the position coordinate according to the initial calibration relation matrix, the real-time attitude conversion matrix, the mileage increment and the rigid body coordinate of the gyroscope in the navigation coordinate system, and then calculating to obtain the attitude deviation of the heading machine according to the position coordinate and the plan line;

wherein,

in order to be the initial attitude transformation matrix,

is the inverse of the initial attitude transformation matrix,

is an attitude transformation matrix of the navigation coordinate system relative to the carrier coordinate system,

is a transposed matrix of the attitude transformation matrix, psi is a heading angle of the carrier coordinate system relative to the navigation coordinate system, theta is a pitch angle of the carrier coordinate system relative to the navigation coordinate system, phi is a roll angle of the carrier coordinate system relative to the navigation coordinate system,

is the real-time pitch angle of the gyroscope in the current sampling period t under the carrier coordinate system,

is the real-time roll angle of the gyroscope in the current sampling period t under the carrier coordinate system,

is the gyroscope at the current sampling cycleThe real-time course angle of the period t under the carrier coordinate system,

5. The guidance system of a heading machine according to claim 4, wherein the mileage calculating module comprises:

the second calculation submodule is used for calculating displacement increments respectively corresponding to the last sampling period and the current sampling period of the heading machine in a heading machine coordinate system according to the output pulse number and the delivery parameters;

the second calculation submodule is further configured to calculate, according to the displacement matrices corresponding to the displacement increments respectively corresponding to the previous sampling period and the current sampling period, a mileage increment from the previous sampling period to the current sampling period;

wherein the odometer measures a displacement matrix of

The displacement matrix measured in the current sampling period t is

，s₀Is a corresponding displacement of an initial sampling periodIncrement, s_tIs the displacement increment corresponding to the current sampling period,

，

，

the mileage increment between the initial sampling period and the current sampling period t in the coordinate system of the heading machine is shown.

6. The guidance system of a heading machine of claim 5, wherein:

the first acquisition submodule is also used for acquiring an initial coordinate of a gyroscope center point in a navigation coordinate system by using a total station in an initial state;

the first calculation submodule is also used for calculating to obtain rigid body coordinates of the machine head central point and the machine tail central point under a carrier coordinate system according to initial coordinates of the machine tail central point, the machine head central point and the gyroscope central point under a navigation coordinate system and an inverse matrix of the zero rotation matrix;

wherein,

the initial coordinate of the tail center point A under a navigation coordinate system is obtained from the total station in an initial state,

the initial coordinate of the head center point B under a navigation coordinate system is acquired from the total station in an initial state,

the initial coordinate of the central point of the gyroscope in a navigation coordinate system is obtained from the total station in an initial state,

is a rigid body coordinate of the central point A of the tail of the initial state under a carrier coordinate system,

is a rigid body coordinate of a machine head central point B under a carrier coordinate system in an initial state,

for the zero rotation matrix from the carrier coordinate system to the navigation coordinate system in the initial state,

the inverse of the zero rotation matrix.

7. The guidance system of a heading machine of claim 6, wherein the deviation calculation module comprises:

the third calculation operator module is used for calculating to obtain the real-time coordinate of the central point of the gyroscope in the navigation coordinate system according to the initial calibration relation matrix, the real-time attitude conversion matrix, the mileage increment and the initial coordinate of the central point of the gyroscope in the navigation coordinate system;

the coordinate solving submodule is also used for calculating to obtain the space coordinates of the tail central point and the head central point of the current sampling period under the navigation coordinate system according to the real-time coordinate, the rigid body coordinate, the real-time attitude conversion matrix, and the initial coordinates of the tail central point and the head central point under the navigation coordinate system;

the projection submodule is used for projecting the center point of the machine head on a vertical axis and a horizontal axis respectively to obtain corresponding machine head projection points, projecting the center point of the machine tail on the vertical axis and the horizontal axis respectively to obtain corresponding machine tail projection points, projecting the starting point of the marking line on the vertical axis and the horizontal axis respectively to obtain corresponding starting point projection points, and projecting the end point of the marking line on the vertical axis and the horizontal axis respectively to obtain corresponding end point projection points; the vertical axis is a plane formed by an X axis, a Y axis and an origin in a navigation coordinate system, and the horizontal axis is a plane formed by the Y axis, the Z axis and the origin in the navigation coordinate system;

the deviation calculation submodule is used for calculating the horizontal deviation and the vertical deviation of the center point of the machine head relative to the marking line and the horizontal deviation and the vertical deviation of the center point of the machine tail relative to the marking line through a space distance solving formula according to the coordinate values of the projection points of the machine head and the machine tail and the coordinate values of the projection points of the starting point and the end point;

wherein,

is a real-time coordinate of the central point of the gyroscope in a navigation coordinate system,

is to navigate the coordinate system in the current sampling periodA real-time attitude transformation matrix relative to the carrier coordinate system,

the mileage increment from the initial sampling period to the current sampling period of the odometer under the coordinate system of the heading machine is obtained; p_atIs the space coordinate P of the tail central point A of the current sampling period in the navigation coordinate system_btThe spatial coordinate of the handpiece central point B in the navigation coordinate system in the current sampling period is shown;

A. b is a machine tail central point and a machine head central point, a ' and a ' are respectively machine tail projection points at which the machine tail central point is projected on a horizontal axis and a vertical axis, B ' and B ' are machine head projection points at which the machine head central point is projected on a horizontal axis and a vertical axis, C, D is a scoring starting point and a planning line end point, C ' and C ' are respectively starting point projection points at which the scoring starting point is projected on a horizontal axis and a vertical axis, D ', d 'is an end-point projection point at which a scoring end-point is projected on a horizontal axis and a vertical axis, and E, E' and E 'are respectively intersection points at which plane a' and segment CD, segment C 'D' and segment C 'D' intersect, F, F 'and F' are intersection points at which plane B 'BB' and segment CD, segment C 'D' and segment C 'D' intersect, respectively;

and

respectively the horizontal deviation amount and the vertical deviation amount of the center point of the tail relative to the marking line,

and

the horizontal and vertical deviations of the handpiece center point from the gauge line, respectively.

8. The guide system of a heading machine according to any one of claims 1 to 7, further comprising: a display;

and the display is connected with the processor and is used for graphically displaying the attitude deviation of the heading machine so as to guide a driver of the heading machine to heading towards the correct direction.

9. A method of guiding a heading machine, which is applied to the guidance system of the heading machine according to any one of claims 1 to 8, comprising the steps of:

acquiring an attitude angle of the development machine through a gyroscope;

acquiring the number of output pulses through a milemeter;

10. The method of guiding a heading machine according to claim 9, further comprising the steps of:

acquiring rigid body coordinates of a machine head central point and a machine tail central point of the tunneling machine in a navigation coordinate system from a total station in an initial state, and acquiring an initial calibration relation matrix of the tunneling machine in the initial state;

11. The method of guiding a heading machine according to claim 10, wherein said obtaining an initial calibration relationship matrix of the heading machine at the initial state comprises:

acquiring initial coordinates of a machine tail central point and a machine head central point under a navigation coordinate system by using a total station in an initial state, and acquiring an initial attitude angle from a gyroscope; the initial attitude angle comprises an initial pitch angle, an initial roll angle and an initial course angle;

calculating according to the initial coordinate and the plan line to obtain an initial rotation matrix, calculating according to the initial attitude angle to obtain a zero rotation matrix, and calculating according to the initial rotation matrix and the zero rotation matrix to obtain the initial calibration relation matrix;

；

And

in the form of a zero rotation matrix,

is the inverse of the zero rotation matrix,

12. The method for guiding the heading machine according to claim 11, wherein the step of obtaining the position coordinates of the center point of the nose and the center point of the tail of the heading machine in a navigation coordinate system by calculation according to the output pulse number, the attitude angle and a pre-obtained initial calibration relation matrix comprises the following steps:

calculating to obtain an initial attitude transformation matrix of the navigation coordinate system relative to the carrier coordinate system according to the attitude angle obtained from the gyroscope;

according to the initial space coordinate of the heading machine in a navigation coordinate system and a preset target space coordinate, fitting to generate a marking line of the heading machine in the navigation coordinate system;

acquiring a real-time attitude angle measured by the gyroscope, and acquiring the number of output pulses measured by the odometer; the real-time attitude angle comprises a real-time course angle, a real-time roll angle and a real-time pitch angle;

calculating the mileage increment of the heading machine according to the output pulse number and the factory parameters of the odometer;

calculating to obtain a real-time attitude transformation matrix of the navigation coordinate system relative to the carrier coordinate system according to the real-time attitude angle;

calculating to obtain the position coordinate according to the initial calibration relation matrix, the real-time attitude transformation matrix, the mileage increment and the rigid body coordinate of the gyroscope in the navigation coordinate system;

wherein,

in order to be the initial attitude transformation matrix,

is the inverse of the initial attitude transformation matrix,

is a transposed matrix of the attitude transformation matrix, psi is a course angle of the carrier coordinate system relative to the navigation coordinate system, theta is a pitch angle of the carrier coordinate system relative to the navigation coordinate system, and phi is a relative angle of the carrier coordinate systemIn the roll angle of the navigation coordinate system,

is the real-time course angle of the gyroscope in the current sampling period t under the carrier coordinate system,

13. The method according to claim 12, wherein the step of calculating the mileage increment of the heading machine according to the output pulse number and the factory parameters of the odometer comprises the steps of:

calculating displacement increments respectively corresponding to the last sampling period and the current sampling period of the heading machine in a heading machine coordinate system according to the output pulse number and the delivery parameters;

calculating the mileage increment from the last sampling period to the current sampling period according to the displacement matrixes corresponding to the displacement increments respectively corresponding to the last sampling period and the current sampling period;

wherein the odometer measures a displacement matrix of

The displacement matrix measured in the current sampling period t is

，s₀Is a displacement increment corresponding to the initial sampling period, s_tIs the displacement increment corresponding to the current sampling period,

，

，

14. The method of guiding a heading machine according to claim 13, wherein said obtaining rigid body coordinates of a nose center point and a tail center point of the heading machine in a navigation coordinate system from a total station at an initial state comprises:

acquiring an initial coordinate of a gyroscope center point in a navigation coordinate system by using a total station in an initial state;

calculating to obtain rigid body coordinates of the machine head central point and the machine tail central point under a carrier coordinate system according to initial coordinates of the machine tail central point, the machine head central point and the gyroscope central point under a navigation coordinate system and an inverse matrix of the zero rotation matrix;

wherein,

is a rigid coordinate of the tail central point A of the initial state machine under a carrier coordinate system, is a rigid coordinate of the head central point B of the initial state machine under the carrier coordinate system,

the inverse of the zero rotation matrix.

15. The method of guiding a heading machine according to claim 14, wherein said calculating an attitude deviation of the heading machine from the position coordinates and the plan line comprises:

calculating to obtain a real-time coordinate of the central point of the gyroscope in a navigation coordinate system according to the initial calibration relation matrix, the real-time attitude conversion matrix, the mileage increment and the initial coordinate of the central point of the gyroscope in the navigation coordinate system;

according to the real-time coordinates, rigid body coordinates, a real-time posture conversion matrix, initial coordinates of a machine tail central point and a machine head central point under a navigation coordinate system, and spatial coordinates of the machine tail central point and the machine head central point under the navigation coordinate system in the current sampling period are obtained through calculation;

respectively projecting the center point of the machine head on a vertical axis and a horizontal axis to obtain corresponding machine head projection points, respectively projecting the center point of the machine tail on the vertical axis and the horizontal axis to obtain corresponding machine tail projection points, respectively projecting the starting point of the marking line on the vertical axis and the horizontal axis to obtain corresponding starting point projection points, and respectively projecting the end point of the marking line on the vertical axis and the horizontal axis to obtain corresponding end point projection points; the vertical axis is a plane formed by an X axis, a Y axis and an origin in a navigation coordinate system, and the horizontal axis is a plane formed by the Y axis, the Z axis and the origin in the navigation coordinate system;

according to the coordinate values of the projection points of the machine head and the machine tail and the coordinate values of the projection points of the starting point and the end point, calculating by a space distance-solving formula to obtain the horizontal offset and the vertical offset of the central point of the machine head relative to the marking line and the horizontal offset and the vertical offset of the central point of the machine tail relative to the marking line;

wherein,

is a real-time attitude transformation matrix of the navigation coordinate system relative to the carrier coordinate system at the current sampling period,

A. b is a machine tail central point and a machine head central point, a ' and a ' are respectively machine tail projection points at which the machine tail central point is projected on a horizontal axis and a vertical axis, B ' and B ' are machine head projection points at which the machine head central point is projected on a horizontal axis and a vertical axis, C, D is a scoring starting point and a planning line end point D, C ' and C ' are respectively starting point projection points at which the scoring starting point is projected on a horizontal axis and a vertical axis, D ', d 'is an end-point projection point at which a scoring end-point is projected on a horizontal axis and a vertical axis, and E, E' and E 'are respectively intersection points at which plane a' and segment CD, segment C 'D' and segment C 'D' intersect, F, F 'and F' are intersection points at which plane B 'BB' and segment CD, segment C 'D' and segment C 'D' intersect, respectively;

and

and

16. The method of guiding a heading machine according to any one of claims 9 to 15, further comprising:

and the display receives the attitude deviation and displays the attitude deviation of the heading machine in a graphical mode so as to guide a driver of the heading machine to heading towards the correct direction.

17. A storage medium having stored therein at least one instruction which is loaded and executed by a processor to carry out the operations carried out by the method of guiding a heading machine according to any one of claims 9 to 16.