A combined pose detection system for a heading machine and a detection method thereof

Abstract

The application discloses a combined position and posture detection system of a heading machine, which comprises a laser sensing system, an inertial navigation system, an upper computer and an electric control system of the heading machine, wherein the laser sensing system comprises a plane laser emitter fixed on a roof of a tunnel, a first laser sensing target and a second laser sensing target; the first laser sensing target is arranged at a rear end of a body of the heading machine; the second laser sensing target is arranged in front of the first laser sensing target and is connected with the first laser sensing target through a bus; a plurality of photosensitive elements are horizontally arranged on the first laser sensing target and the second laser sensing target; the inertial navigation system is arranged above the body of the heading machine; the laser sensing system is connected with the inertial navigation system; the upper computer is connected with the inertial navigation system; and the electric control system of the heading machine is arranged on the heading machine. The application realizes real-time detection of the position and posture of the heading machine by means of a combined navigation method, overcomes cumulative errors of the inertial navigation system and makes the inertial navigation system applicable to position and posture detection of various mobile devices in the tunnel.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of pose detection and navigation, and particularly relates to a combined pose detection system for a heading machine and a detection method thereof. BACKGROUND

[0002] With the development of multi-sensor fusion technology, the heading machine operation is no longer satisfied with the traditional pointing laser navigation mode and gradually turns to real-time pose detection. For the pose detection of the heading machine, different methods are proposed by researchers, such as using an inclination sensor, an odometer, and machine vision for combined measurement. However, considering the actual situation of the heading face, the above sensors have disadvantages, such as the inclination sensor is affected by the magnetic field, the odometer will generate measurement errors due to the slipping of the track and is difficult to correct, and the machine vision is more difficult to adapt to the smoke environment, so many sensor combined detection methods are not practical.

[0003] Considering the actual working environment of the roadway, the laser has strong penetration ability, and compared with other sensors, it can better adapt to the adverse environmental factors such as smoke and water mist on the working face. The inertial navigation system has the characteristics of not being affected by the external environment. A heading machine pose detection method based on the combination of laser perception and inertial navigation has been proposed. However, the actual heading work process is slow, the working time is long, and is accompanied by mechanical vibration. The inertial navigation system calculates the angle and displacement in the form of integration, so the measurement result will have a large cumulative error, and the measurement error of displacement is very large, which cannot be used for displacement detection of the heading machine. Therefore, when the inertial navigation system is used to measure the pose angle of the heading machine, the measurement value needs to be corrected, and other detection equipment is needed to separately measure the displacement and transverse offset vectors of the heading machine. SUMMARY

[0004] The purpose of the present application is to provide a combined pose detection system for a heading machine and a detection method thereof to solve the above problems.

[0005] The technical scheme of the present application is:

[0006] The combined pose detection system of the heading machine comprises a laser sensing system, the laser sensing system comprising: a planar laser transmitter, a first laser sensing target and a second laser sensing target, the planar laser transmitter being fixed on a roadway roof; the first laser sensing target being arranged at the rear end of the body of the heading machine; the second laser sensing target being arranged in front of the first laser sensing target, the first laser sensing target and the second laser sensing target being connected through an RS485 bus, and a plurality of photosensitive elements being horizontally arranged on the first laser sensing target and the second laser sensing target; an inertial navigation system, the inertial navigation system being arranged above the midpoint of the body of the heading machine, and the laser sensing system being connected with the inertial navigation system through a CAN bus; a host computer, the host computer being connected with the inertial navigation system through a wireless network, the host computer being arranged in an overground control center and performing data communication through a base station and an industrial ring network arranged underground; and an electric control system of the heading machine, the electric control system being arranged in a control system protection shell, the control system protection shell being arranged on the heading machine and being connected with hydraulic valves of various hydraulic mechanisms on the heading machine, and the electric control system of the heading machine being provided with a wireless communication module and being connected with the host computer to receive control instructions sent by the host computer and control the hydraulic valves to control the heading machine.

[0007] Further, the middle lines of the first laser sensing target and the second laser sensing target in the laser sensing system coincide with the middle line of the body of the heading machine.

[0008] Further, the middle line of the inertial navigation system coincides with the middle line of the body of the heading machine.

[0009] Further, the second laser sensing target is 50 cm away from the first laser sensing target.

[0010] A method for detecting by a combined pose detection system of a heading machine, comprising the following steps:

[0011] S1 the planar laser transmitter of the laser sensing system emits laser on the photosensitive elements of the first laser sensing target and the second laser sensing target, the installation position of the target and the specific pose parameters of the heading machine are contacted, the lateral offset distance of the heading machine relative to the middle line of the roadway and the heading direction angle of the heading machine are calculated, the lateral offset, the heading direction angle and the displacement measured by the laser sensing system are transmitted to the inertial navigation system, a row of close photosensitive elements are arranged on the first and second targets, the position information of the photosensitive elements swept by the laser plane on the two targets, the attitude angle information of the heading machine and the installation position information of the target on the heading machine are required when the lateral offset distance of the heading machine is calculated, that is, the installation position of the target on the heading machine, the attitude angle parameters of the heading machine and the position information of the photosensitive elements irradiated by the two targets are contacted to calculate the lateral offset of the body;

[0012] The attitude angle information of the machine body of the heading machine obtained by the inertial navigation system obtains the heading angle and the lateral deviation of the heading machine, establishes a time and displacement function of the laser scanning photosensitive element, and after the pitch angle compensation, the displacement information of the heading machine is calculated through the time of the laser scanning the photosensitive element;

[0013] After the lateral deviation and the displacement of the heading machine are obtained, the heading angle information of the machine body is returned to the inertial navigation system together, the displacement and the heading angle are utilized to correct the output of the inertial navigation system, the heading angle obtained by the laser sensing system and the heading angle detected by the inertial navigation system are weighted and calculated, and then are used in the subsequent attitude angle correction process;

[0014] After all the machine body posture parameters of the heading machine are measured, the specific posture parameters are transmitted to the upper computer, and after the processing of the upper computer, the specific posture parameters are transmitted to the electric control system of the heading machine, when a round of heading operation is completed, the electric control system is utilized to adjust the posture of the heading machine to continue the heading operation, and the posture of the heading machine is continuously detected.

[0015] Further, in S1, when the laser of the plane laser transmitter scans the photosensitive element at the position between the first laser sensing target and the second laser sensing target, it indicates that the heading angle of the heading machine is not deviated, and when the laser scans the photosensitive element deviated from the position, it indicates that the heading angle of the heading machine is deviated.

[0016] Further, in S2, since the laser emitted by the plane laser transmitter has a time difference when scanning the photosensitive element, it is assumed that the time when the laser contacts the photosensitive element is t1, the time when the laser leaves the photosensitive element is t2, t2-t1 is the total time of the laser staying on the photosensitive element, the rotation speed ω of the laser scanning is contacted, the influence of the change of the pitch angle of the heading machine is compensated, the included angle α of the front and rear two lasers is obtained, according to the different angles corresponding to the different distances, the function is constructed, and then the displacement information of the heading can be calculated by using the time of the laser scanning the photosensitive element.

[0017] Further, the time of the laser scanning the photosensitive element is utilized to judge the displacement, the heading angle and the roll angle of the heading machine have little influence on the distance measuring result, and are not considered, when the pitch angle changes, the distance scanned by the laser is different from the distance scanned in the normal time, and the difference is L·(1-cosA), wherein L is the length of the photosensitive element without the pitch angle, L is obtained by using a simple trigonometric function, the actual time of scanning the photosensitive element is added to the time of scanning L·(-cosA) additionally, that is, the time required when the pitch angle is not changed is obtained, and then the displacement information of the heading machine is judged according to the time; a plurality of groups of data of the time of the heading machine scanning the photosensitive element are collected, and the function curve is directly fitted to obtain .

[0018] Compared with the prior art, the beneficial effects of the present application are:

[0019] The application realizes real-time detection of the pose of the tunneling machine by the method of combined navigation, overcomes the cumulative error of the inertial navigation system, and makes the inertial navigation system applicable to the pose detection of each mobile device in the tunnel.

[0020] The laser sensing system of the application has strong laser penetration and small interference degree, the strapdown inertial navigation system is not affected by the external environment, and the combined pose detection system of the application is suitable for the harsh working environment on the tunneling face.

[0021] The application uses the attitude angle detected by the aligned inertial navigation system as the initial value to calculate the first offset distance and displacement, and then feeds back the calculation results to the inertial navigation system for attitude angle correction, so that the initial value is accurate, the subsequent process can be continuously corrected, and the measurement accuracy is high.

[0022] The application combines two types of measurement systems, the whole measurement system has simple structure, the required physical quantities can be obtained, and the output is simple, only the angle information of the strapdown inertial navigation system and the offset and position information of the laser sensing system are taken, and no complex algorithm is needed.

[0023] In the detection process, the measurement is only related to the position of the tunneling machine body, and no additional error is generated due to the slipping of the track and the like. DETAILED DESCRIPTION

[0024] Figure 1 It is a front view structural diagram of the tunneling machine running in the tunnel of the application;

[0025] Figure 2 It is a top view of the structural diagram of the tunneling machine of the application;

[0026] Figure 3 It is a laser target working diagram of the application;

[0027] Figure 4 It is a data interaction framework diagram of the pose detection system of the application;

[0028] Figure 5 It is a general system framework diagram of the application;

[0029] Figure 6 It is a distance measurement principle diagram of the application.

[0030] 1, plane laser emitter; 2, first laser sensing target; 3, second laser sensing target; 4, inertial navigation system, 5, electric control system protection shell. DETAILED DESCRIPTION

[0031] The following will be combined with the drawings of the application Figure 1 to the drawings of the application Figure 6The specific embodiments of the present application are described in detail. In the description of the present application, it should be understood that the terms "center", "upper", "lower", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer" and the like indicate the orientation or positional relationship based on the orientation or positional relationship shown in the drawings, and are only for the convenience of describing the present application and simplifying the description, and therefore cannot be understood as indicating or implying that the devices or elements referred to must have a particular orientation, be constructed and operated in a particular orientation, and therefore cannot be understood as limiting the present application.

[0032] The terms "first", "second" are only for descriptive purposes, and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the technical features indicated. Therefore, the features defined with "first", "second" can explicitly or implicitly include one or more of the features; in the description of the present application, unless otherwise stated, the meaning of "multiple" is two or more.

[0033] It should be noted that the circuit connections involved in the present application all adopt conventional circuit connection methods and do not involve any innovation. Embodiment

[0034] A combined position and posture detection system of a heading machine, comprising: a laser sensing system, an inertial navigation system 4, an upper computer and a heading machine electric control system, the laser sensing system comprising: a plane laser emitter 1, a first laser sensing target 2 and a second laser sensing target 3, the plane laser emitter being fixed on a roadway roof; the first laser sensing target 2 being arranged at the rear end of the body of the heading machine; the second laser sensing target 3 being arranged in front of the first laser sensing target, the first laser sensing target and the second laser sensing target being connected through an RS485 bus, a plurality of photosensitive elements being horizontally arranged on the first laser sensing target 2 and the second laser sensing target 3; the inertial navigation system 4 being arranged above the midpoint of the body of the heading machine, the laser sensing system being connected with the inertial navigation system 4 through a CAN bus; the upper computer being connected with the inertial navigation system through a wireless network, the upper computer being arranged in an uphole control center and performing data communication through a base station arranged underground and an industrial ring network; the heading machine electric control system being arranged in an electric control system protection shell 5, the electric control system protection shell 5 being arranged on the heading machine and being connected with the hydraulic valves of various hydraulic mechanisms on the heading machine, such as the throttle valve of the hydraulic motor in the track and the various stage hydraulic valves of the hydraulic device driving the cutting arm, the heading machine electric control system being internally provided with a wireless communication module and being in communication with the upper computer, receiving the control instructions sent by the upper computer to control the various hydraulic valves to complete the control of the heading machine, there being data processing modules in the laser target and the inertial navigation system, and the upper computer being only used for displaying detection data and transmitting control commands downward, wherein the control commands are transmitted to the heading machine electric control system through a wireless network.

[0035] The target built-in data processing chip, the inertial navigation system itself has data calculation capability, the inertial navigation system returns the measured attitude angle parameter to the second laser sensing target, the target calculates the lateral offset of the body according to the position of the photosensitive element irradiated by the plane laser and the attitude angle information from the inertial navigation, and judges the displacement according to the time when the laser scans the photosensitive element.

[0036] The second laser target returns the calculated displacement and lateral offset to the inertial navigation system through the CAN bus, and the inertial navigation system corrects the attitude angle measurement data, packs the position data, and sends it to the upper computer through the wireless module. That is, the upper computer is connected to the inertial navigation system through a wireless network, and the upper computer is not arranged on the machine body of the roadheader, but is arranged in the control center on the ground. The data communication is carried out through the base station arranged underground and the industrial ring network.

[0037] Preferably, the middle line of the first laser sensing target 2 and the second laser sensing target 3 in the laser sensing system coincides with the middle line of the machine body of the roadheader, the middle line of the inertial navigation system 4 coincides with the middle line of the machine body of the roadheader, and the second laser sensing target 3 is 50 cm away from the first laser sensing target 2.

[0038] A method for detecting a combined position and attitude detection system of a roadheader, comprising the following steps:

[0039] S1 The plane laser emitter 1 of the laser sensing system emits laser on the photosensitive elements of the first laser sensing target 2 and the second laser sensing target 3, contacts the installation position of the target and the specific position parameters of the roadheader, calculates the lateral offset distance of the roadheader relative to the center line of the roadway and the heading angle of the roadheader, and transmits the lateral offset, the heading angle and the displacement measured by the laser sensing system to the inertial navigation system 4. A row of closely arranged photosensitive elements are arranged on the first and second targets, and the position information of the photosensitive elements scanned by the laser plane on the two targets is needed when calculating the lateral offset distance of the roadheader, the attitude angle information of the roadheader and the installation position information of the target on the roadheader. That is, the installation position of the target on the roadheader, the attitude angle parameters of the roadheader and the position information of the photosensitive elements irradiated by the two targets are contacted to calculate the lateral offset of the machine body.

[0040] S2 The attitude angle information of the machine body obtained by the inertial navigation system 4 obtains the heading angle and the lateral offset of the roadheader, establishes the time and displacement function of the laser scanning photosensitive element, and calculates the displacement information of the roadheader through the time when the laser scans the photosensitive element after the pitch angle compensation;

[0041] S3 gets the transverse offset and displacement of the heading machine, and returns them to the inertial navigation system 4 together with the heading angle information of the machine body. The displacement and heading angle are used to correct the output of the inertial navigation system. The heading angle obtained by the laser sensing system is weighted with the heading angle detected by the inertial navigation system, and then used in the subsequent attitude angle correction process.

[0042] The target itself has a data processing chip. The formula of the transverse offset is written in the chip in the form of code. The installation position information of the target on the heading machine is converted into a constant and written into the formula. Through data interaction with the inertial navigation system, the transverse offset can be directly calculated in the data processing chip built in the target. Similarly, the target itself can record the time when the laser just scans the photosensitive element and the time when the laser leaves the photosensitive element. According to the time, the displacement data can be directly matched.

[0043] The heading angle of the heading machine is calculated according to the positions of the photosensitive elements on which the laser scans the two targets. That is, the target obtains a heading angle information, and the inertial navigation system also provides a heading angle information to the target. The measurement accuracy of the heading angle is further improved by weighted averaging. The specific weighting coefficient is determined according to the actual working condition. When the dust is large and the laser is affected, the heading angle measured by the inertial navigation system has a large weight. When the dust is small, the heading angle obtained by the laser target has a large weight.

[0044] S4 transmits the specific pose parameters to the host computer after measuring the pose parameters of the whole machine body of the heading machine. After processing by the host computer, the pose parameters are transmitted to the electric control system of the heading machine. When a round of heading operation is completed, the electric control system adjusts the pose of the heading machine to continue the heading operation, and the pose of the heading machine is continuously detected.

[0045] In S1, when the laser of the planar laser emitter 1 scans the photosensitive element at the position between the first laser sensing target 2 and the second laser sensing target 3, it indicates that the heading angle of the heading machine is not offset. If the photosensitive element deviates from the middle position, it indicates that the heading angle of the heading machine is offset.

[0046] In S2, since there is a time difference when the laser emitted by the planar laser emitter 1 scans the photosensitive element, it is assumed that the time when the laser just contacts the photosensitive element is t1, and the time when the laser leaves the photosensitive element is t2. The total time that the laser stays on the photosensitive element is t2-t1. In combination with the rotation speed ω of the laser scanning, the influence of the change of the pitch angle of the heading machine is compensated, and the included angle α of the front and rear two lasers is obtained. According to the fact that different angles correspond to different distances, a function is constructed, and then the time when the laser scans the photosensitive element is converted into displacement information of the heading.

[0047] The displacement is determined by the time of laser scanning the photosensitive element. The heading angle and roll angle of the roadheader have little effect on the ranging result, and are not considered. When the pitch angle changes, the distance of laser scanning is different from the distance of normal scanning, and the difference is L*(1-cosA), wherein L is the length of the photosensitive element without the pitch angle. The length L is obtained by using simple trigonometric functions. The actual time of scanning the photosensitive element and the additional time of scanning L*(1-cosA) are added, and the time required when the pitch angle does not change is obtained. Then, the displacement information of the roadheader is determined according to the time.

[0048] The time of the roadheader scanning the photosensitive element is obtained by collecting multiple groups of data and directly fitting a function curve. In order to reduce the calculation difficulty, the function curve is fitted into a quadratic function. .

[0049] The above disclosure is only a few preferred specific embodiments of the present application, but the embodiments of the present application are not limited thereto. Any changes that can be thought of by those skilled in the art shall fall within the protection scope of the present application.

Claims

1. A method for detecting the combined pose of a tunneling machine, characterized in that, A tunneling machine assembly pose detection system is used to perform pose detection. This tunneling machine assembly pose detection system includes: A laser sensing system, comprising: a planar laser emitter (1), a first laser sensing target (2), and a second laser sensing target (3), wherein the planar laser emitter (1) is fixed on the roof of the tunnel; the first laser sensing target (2) is located at the rear end of the tunneling machine; the second laser sensing target (3) is located in front of the first laser sensing target (2), and the first laser sensing target (2) and the second laser sensing target (3) are connected by a bus, and multiple photosensitive elements are horizontally arranged on both the first laser sensing target (2) and the second laser sensing target (3); An inertial navigation system (4) is installed above the midpoint of the tunneling machine body, and the laser sensing system communicates with the inertial navigation system (4). The host computer is connected to the inertial navigation system via a wireless network. The host computer is located in the control center above ground and communicates with the base station and industrial ring network located underground. The tunneling machine electrical control system is set in the electrical control system protective shell (5). The electrical control system protective shell (5) is set on the tunneling machine and connected to the hydraulic valves of each hydraulic mechanism on the tunneling machine. The tunneling machine electrical control system has a built-in wireless communication module to communicate with the host computer. By receiving the control commands sent by the host computer, it controls each hydraulic valve to complete the control of the tunneling machine. The method for detecting the combined position and posture of a tunneling machine includes the following steps: The planar laser emitter (1) of the S1 laser sensing system emits a laser to illuminate the photosensitive elements on the first laser sensing target (2) and the second laser sensing target (3). Based on the installation position of the target and the specific posture parameters of the tunneling machine, the lateral offset distance and heading angle of the tunneling machine relative to the center line of the roadway are calculated. The lateral offset, heading angle and displacement measured by the laser sensing system are transmitted to the inertial navigation system (4). The inertial navigation system (4) obtained by S2 obtains the heading angle and lateral offset of the tunneling machine from the attitude angle information of the tunneling machine body, establishes the time and displacement function of the laser scanning photosensitive element, performs pitch angle compensation, and calculates the displacement information of the tunneling machine by the time the laser scans the photosensitive element. After S3 obtains the lateral offset and displacement of the tunneling machine, it sends the information along with the heading angle of the machine body back to the inertial navigation system (4). The displacement and heading angle are used to correct the output of the inertial navigation system. The heading angle obtained by the laser sensing system and the heading angle detected by the inertial navigation system are weighted and then used for subsequent attitude angle correction. After S4 measures all the body posture parameters of the tunneling machine, it transmits the specific posture parameters to the host computer. After processing by the host computer, the parameters are transmitted to the tunneling machine's electrical control system. When a round of tunneling operation is completed, the tunneling machine's posture is adjusted through the electrical control system to continue tunneling operations, while the posture of the tunneling machine is continuously monitored.

2. The method for detecting the combined pose of a tunneling machine according to claim 1, characterized in that, In the laser sensing system, the centerline of the first laser sensing target (2) and the second laser sensing target (3) coincides with the centerline of the tunneling machine body.

3. The method for detecting the combined pose of a tunneling machine according to claim 2, characterized in that, The centerline of the inertial navigation system (4) coincides with the centerline of the tunneling machine.

4. The method for detecting the combined pose of a tunneling machine according to claim 2, characterized in that, The second laser sensing target (3) is 50cm away from the first laser sensing target (2).

5. The method for detecting the combined pose of a tunneling machine according to claim 1, characterized in that, In S1, when the laser of the planar laser emitter (1) sweeps across the photosensitive element at the middle position between the first laser sensing target (2) and the second laser sensing target (3), it indicates that the heading angle of the tunneling machine has no deviation.

6. The method for detecting the combined pose of a tunneling machine according to claim 1, characterized in that, In S2, since there is a time difference when the laser emitted by the planar laser emitter (1) sweeps the photosensitive element, let the time when the laser just touches the photosensitive element be t1 and the time when it leaves the photosensitive element be t2. t2-t1 is the total time when the laser stays on the photosensitive element. By considering the rotation speed ω when the laser is scanning, the influence of the pitch angle change of the tunneling machine is compensated, and the angle α between the two lasers is obtained. According to the different distances corresponding to different angles, after constructing the function, the time when the laser sweeps the photosensitive element is converted into the displacement information of the tunneling.

7. The method for detecting the combined pose of a tunneling machine according to claim 1, characterized in that, The displacement is determined by the time it takes for a laser to scan the photosensitive element. Changes in the heading angle and roll angle of the tunneling machine have little impact on the distance measurement results and are therefore not considered. When the pitch angle changes, the distance scanned by the laser is different from the distance scanned under normal conditions, by a difference of L·(1-cosA), where L is the length of the photosensitive element when there is no pitch angle. Let the pitch angle be -A. The actual time it takes to scan the photosensitive element is added to the additional time required to scan L·(1-cosA), which gives the time required when the pitch angle is unchanged. The displacement information of the tunneling machine is then determined based on the time.

Images