CN115950420A - Combined pose detection system and detection method for development machine - Google Patents

Abstract

The invention discloses a combined position and posture detection system of a roadheader, which includes: a laser perception system, an inertial navigation system, a host computer and an electric control system for the roadheader. The laser perception system includes: a plane laser transmitter fixed on the roadway roof, a A laser sensing target and a second laser sensing target; the first laser sensing target is set at the rear end of the body of the roadheader; the second laser sensing target is set in front of the first laser sensing target, connected through a bus, and the laser Multiple photosensitive elements are arranged horizontally on the sensing target; the inertial navigation system is set above the body of the roadheader, and the laser perception system is connected to the inertial navigation system; the host computer is connected to the inertial navigation system; the electric control system of the roadheader is set on the roadheader , the present invention completes the real-time detection of the position and posture of the roadheader through the method of combined navigation, and overcomes the cumulative error of the inertial navigation system, so that the inertial navigation system can be applied to the detection of the position and posture of each mobile device in the roadway.

Description

A combination position and posture detection system and detection method of roadheader

technical field

The invention relates to the technical field of pose detection and navigation, in particular to a combined pose detection system of a roadheader and a detection method thereof.

Background technique

With the development of multi-sensor fusion technology, roadheader operations are no longer satisfied with the traditional pointing laser navigation mode and gradually turn to real-time pose detection. For the position and orientation detection of roadheaders, various researchers have proposed different methods, such as using inclination sensors, odometers, machine vision, etc. for combined measurement. Affected by the magnetic field, the odometer will produce measurement errors due to track slippage and it is difficult to correct, and the machine vision is more difficult to adapt to the smoke and dust environment, so many sensor combination detection methods are not practical.

Considering the actual working environment of the roadway, the laser penetration ability is strong. Compared with other sensors, it can better adapt to harsh environmental factors such as smoke, water mist, etc. on the working surface. The inertial navigation system has the characteristics of not being affected by the external environment. It has been proposed A roadheader pose detection method based on the combination of laser perception and inertial navigation has been proposed, such as the strapdown inertial navigation and laser perception composite roadheader pose detection system proposed by Wu Miao's team at China University of Mining and Technology. However, the actual excavation process is slow, the working time is long, and it is accompanied by mechanical vibration, and the inertial navigation system calculates the angle and displacement in the form of integral, so the measurement results will have a large cumulative error, and the measurement error on the displacement is very large , cannot be used for displacement detection of roadheaders. Therefore, when the inertial navigation system is used to measure the attitude angle of the roadheader, its measured value needs to be corrected, and other detection equipment is required to separately measure the two vectors of the displacement and lateral offset of the roadheader.

Contents of the invention

The object of the present invention is to provide a roadheader combination posture detection system and detection method thereof, so as to solve the above-mentioned problems.

Technical scheme of the present invention is:

A combined position and posture detection system for a roadheader, comprising: a laser sensing system, the laser sensing system comprising: a planar laser emitter, a first laser sensing target and a second laser sensing target, the planar laser emitter is fixed on Roadway roof; the first laser sensing target is arranged at the rear end of the body of the roadheader; the second laser sensing target is arranged in front of the first laser sensing target, and the first laser sensing target and the second laser The sensing targets are connected through the RS485 bus, and multiple photosensitive elements are horizontally arranged on the first laser sensing target and the second laser sensing target; an inertial navigation system, the inertial navigation system is arranged on the body of the roadheader Above the midpoint, the laser sensing system is connected to the inertial navigation system through the CAN bus; the upper computer is connected to the inertial navigation system through a wireless network, and the upper computer is arranged in the control center on the well, and the base station and the industrial environment arranged in the underground are connected to each other. network for data communication; the electric control system of the roadheader is set in the protective shell of the electric control system, and the protective shell of the electric control system is set on the roadheader and connected with the hydraulic valves of each hydraulic mechanism on the roadheader, and the electric control system of the roadheader The system has a built-in wireless communication module, communicates with the host computer, and controls the hydraulic valves to complete the control of the roadheader by receiving the control instructions sent by the host computer.

Further, the centerlines of the first laser sensory target and the second laser sensory target in the laser sensory system coincide with the centerline of the body of the roadheader.

Further, the center line of the inertial navigation system coincides with the body center line of the roadheader.

Further, the distance between the second laser sensing target and the first laser sensing target is 50 cm.

A method for detecting a roadheader combined with a position and posture detection system, comprising the following steps:

The planar laser emitter of the S1 laser sensing system emits laser light on the photosensitive elements on the first laser sensing target and the second laser sensing target, and calculates relative Based on the lateral offset distance of the center line of the roadway and the heading angle of the roadheader, the lateral offset, heading angle and displacement measured by the laser sensing system are transmitted to the inertial navigation system. When calculating the lateral offset distance of the roadheader, the position information of the photosensitive element scanned by the laser plane on the two targets, the attitude angle information of the roadheader and the position information of the target installed on the roadheader are needed. That is, the lateral offset of the fuselage can only be calculated by contacting 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;

The attitude angle information of the roadheader fuselage obtained by the inertial navigation system described in S2 obtains the heading angle and lateral offset of the roadheader, and establishes the time and displacement function of the laser scanning photosensitive element. Time to calculate the displacement information of the roadheader;

After S3 obtains the lateral offset and displacement of the roadheader, it will be sent back to the inertial navigation system together with the heading angle information of the body, and the output of the inertial navigation system will be corrected by using the displacement and heading angle. The heading angle detected by the inertial navigation system is weighted and then used in the subsequent attitude angle correction process;

S4 After measuring all the body pose parameters of the roadheader, the specific pose parameters are transmitted to the host computer, and after being processed by the host computer, they are sent to the electronic control system of the roadheader. Adjust the posture of the roadheader to continue the excavation operation, and at the same time continue to detect the posture of the roadheader.

Further, in S1, when the laser of the planar laser emitter sweeps the photosensitive element in the middle position between the first laser sensing target and the second laser sensing target, it means that the heading angle of the roadheader has no deviation, such as sweeping away from the middle position When the photosensitive element is turned off, it means that the heading angle of the roadheader has deviated.

Further, in S2, since the laser emitted by the planar laser emitter has a time difference when scanning the photosensitive element, it is assumed that the time when the laser just touches the photosensitive element is t ₁ , and the time when it leaves the photosensitive element is t ₂ , t ₁ -t ₂ It is the total time that the laser stays on the photosensitive element, which is related to the rotation speed ω of the laser scanning, and compensates the influence caused by the pitch angle change of the roadheader, and the angle α between the two lasers before and after is obtained. After the function, the time when the laser sweeps across the photosensitive element can be converted into the displacement information of the excavation.

Further, the displacement is judged by the time when the laser scans the photosensitive element. When the heading angle and roll angle of the roadheader change, it has little effect on the distance measurement results. When the pitch angle changes, the laser scanning distance and Normally, the swept distance is different, and the difference is L. Use simple trigonometric functions to find L, set the pitch angle to -A, and add the actual time to sweep the photosensitive element and the additional time to sweep L·(-cosA), That is to get the time needed when the pitch angle does not change, and then judge the displacement information of the roadheader according to the time; the time when the roadheader sweeps the photosensitive element collects multiple sets of data, and directly fits the function curve to obtain it. In order to reduce the difficulty of calculation, it is fitted into two The secondary function is y=a ₁ x ² +a ₂ x+a ₃ .

Compared with prior art, the beneficial effect of the present invention is:

1. The present invention completes the real-time detection of the position and posture of the roadheader through the method of combined navigation, and overcomes the cumulative error of the inertial navigation system, so that the inertial navigation system can be applied to the detection of the position and posture of each mobile device in the roadway.

2. The laser sensing system of the present invention has strong laser penetration and is less disturbed. The strapdown inertial navigation system is not affected by the external environment. The combined position and posture detection system proposed by the present invention is suitable for the harsh working environment on the tunneling face.

3. The attitude angle detected by the inertial navigation system after the alignment of the present invention is used as the initial value for the first offset distance calculation and displacement calculation, and then the calculation result is fed back to the inertial navigation system for attitude angle correction. The initial setting is accurate, and the subsequent process It can be continuously corrected and the measurement accuracy is high.

4. The present invention combines two types of measurement systems. The entire measurement system has a simple structure, and all 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 of the laser perception system are taken. information without complex algorithm support.

5. In the detection process of the present invention, the measurement is only related to the position of the body of the roadheader, and no additional errors will be generated due to slippage of the crawler belt and the like.

Description of drawings

Fig. 1 is the front view structural diagram that roadheader of the present invention advances in roadway;

Fig. 2 is the top view of the structural representation of the roadheader of the present invention;

Fig. 3 is the working schematic diagram of the laser target of the present invention;

Fig. 4 is a schematic diagram of the data interaction framework of the pose detection system of the present invention;

Fig. 5 is a schematic diagram of the overall system framework of the present invention;

Fig. 6 is a schematic diagram of the ranging principle of the present invention.

1. Planar laser transmitter; 2. First laser sensing target; 3. Second laser sensing target; 4. Inertial navigation system; 5. Electronic control system protective shell.

Detailed ways

The specific implementation manner of the present invention will be described in detail below in conjunction with accompanying drawings 1 to 6 . In describing the present invention, it is to be understood that the terms "center", "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", The orientations or positional relationships indicated by "top", "bottom", "inner", "outer", etc. are based on the orientations or positional relationships shown in the drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying References to devices or elements must have a particular orientation, be constructed, and operate in a particular orientation and therefore should not be construed as limiting the invention.

The terms "first" and "second" are used for descriptive purposes only, and cannot be understood as indicating or implying relative importance or implicitly specifying the quantity of indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include one or more of these features; in the description of the present invention, unless otherwise specified, the meaning of "plurality" is two or more.

It should be noted that the circuit connections involved in the present invention all adopt conventional circuit connection methods and do not involve any innovation.

Example

A roadheader combined pose detection system, comprising: a laser sensing system, an inertial navigation system 4, a host computer and an electric control system of the roadheading machine, the laser sensing system comprising: a plane laser transmitter 1, a first laser sensing target 2 And the second laser sensing target 3, the plane laser transmitter is fixed on the roadway roof; the first laser sensing target 2 is arranged on the rear end of the body of the roadheader; the second laser sensing target 3 is arranged on the first In front of a laser sensing target, the first laser sensing target and the second laser sensing target are connected through the RS485 bus, and the first laser sensing target 2 and the second laser sensing target 3 are horizontally arranged A plurality of photosensitive elements are distributed; the inertial navigation system 4 is arranged above the midpoint of the roadheader body, and the laser sensing system is connected to the inertial navigation system 4 through a CAN bus; the upper computer is connected to the inertial navigation system through a wireless network, The upper computer is arranged in the control center on the well, and data communication is carried out through the base station arranged in the underground and the industrial ring network; the electric control system of the roadheader is set in the protective shell 5 of the electric control system, and the protective shell 5 of the electric control system is set on the roadheader. The hydraulic valves of the hydraulic mechanisms on the roadheader are connected, such as the throttle valve of the hydraulic motor in the crawler belt, and the hydraulic valves at all levels of the hydraulic device driving the cutting arm. The electric control system of the roadheader has a built-in wireless communication module, which communicates with the upper Machine communication, by receiving the control instructions sent by the host computer, controlling the hydraulic valves to complete the control of the roadheader. Both the laser target and the inertial navigation system have data processing modules, and the host computer is only used to display the detection data and download control commands. , wherein the control command is transmitted to the electronic control system of the roadheader through the wireless network.

The target has a built-in data processing chip. The inertial navigation system itself has data calculation capabilities. The inertial navigation system returns the measured attitude angle parameters to the second laser sensing target. The target is based on the position of the photosensitive element illuminated by the plane laser and the inertial navigation. The transmitted attitude angle information calculates the lateral offset of the body, and judges the displacement according to the time when the laser sweeps across the photosensitive element.

The second laser marker sends the calculated displacement and lateral offset back to the inertial navigation system through the CAN bus. After the inertial navigation system corrects its own attitude angle measurement data, it packs the attitude data and sends them to the host computer through the wireless module. That is: the upper computer is connected with the inertial navigation system through the wireless network, and the upper computer is not arranged in the body of the roadheader, but arranged in the well control center. Data communication is carried out through the base station arranged underground and the industrial ring network.

Preferably, the center line of the first laser sensing target 2 and the second laser sensing target 3 in the laser sensing system coincides with the center line of the roadheader fuselage, and the centerline of the inertial navigation system 4 coincides with the body of the roadheader. The midline coincides, and the distance between the second laser sensing target 3 and the first laser sensing target 2 is 50 cm.

A method for detecting a roadheader combined with a position and posture detection system, comprising the following steps:

The planar laser emitter 1 of the S1 laser sensing system emits laser light on the photosensitive elements on the first laser sensing target 2 and the second laser sensing target 3, and the calculation is based on the installation position of the target and the specific pose parameters of the roadheader. The lateral offset distance of the roadheader relative to the center line of the roadway and the heading angle of the roadheading machine are transmitted to the inertial navigation system 4 by the lateral offset, heading angle and displacement measured by the laser sensing system. With dense photosensitive elements, the position information of the photosensitive elements that the laser plane sweeps across the two targets, the attitude angle information of the roadheader and the position of the target installed on the roadheader are needed to calculate the lateral offset distance of the roadheader. information. That is, the lateral offset of the fuselage can only be calculated by contacting 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;

The attitude angle information of the roadheader fuselage obtained by the inertial navigation system 4 in S2 obtains the heading angle and lateral offset of the roadheader, establishes the time and displacement function of the laser scanning photosensitive element, and scans the photosensitive element through the laser after pitch angle compensation time to calculate the displacement information of the roadheader;

After S3 obtains the lateral offset and displacement of the roadheader, it is sent back to the inertial navigation system 4 together with the heading angle information of the body, and the output of the inertial navigation system is corrected by using the displacement and heading angle. The heading angle obtained by the laser sensing system is The weighted calculation is carried out with the heading angle detected by the inertial navigation system and then used in the subsequent attitude angle correction process;

The target itself has a built-in data processing chip, the formula of the lateral offset is written into the chip in the form of code, and the installation position information of the target on the roadheader is converted into a constant and written into the formula. After data interaction with the inertial navigation system, the lateral 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 it leaves the photosensitive element, according to the time directly match displacement data;

The heading angle of the roadheader is calculated according to the positions where the laser sweeps across the photosensitive elements on the two targets. That is, the target obtains a course angle information, and the inertial navigation system can also provide a course angle information to the target. The measurement accuracy of the heading angle is further improved by means of weighted average. The specific weighting coefficient depends on the actual working conditions. When the dust is relatively large and the laser is affected, the heading angle measured by the inertial navigation system takes up more weight, and when the dust is less, The heading angle obtained through the laser target takes up more weight;

S4 After measuring all the body pose parameters of the roadheader, the specific pose parameters are transmitted to the host computer, and after being processed by the host computer, they are sent to the electronic control system of the roadheader. Adjust the posture of the roadheader to continue the excavation operation, and at the same time continue to detect the posture of the roadheader.

In S1, when the laser of the planar laser transmitter 1 sweeps the photosensitive element in the middle position between the first laser sensing target 2 and the second laser sensing target 3, it means that the heading angle of the roadheader does not shift When the photosensitive element is turned off, it means that the heading angle of the roadheader has deviated.

In S2, since the laser emitted by the planar laser emitter 1 has a time difference when scanning the photosensitive element, assuming that the time when the laser just touches the photosensitive element is t ₁ , and the time when it leaves the photosensitive element is t ₂ , t ₂ -t ₁ is The total time that the laser stays on the photosensitive element is related to the rotation speed ω of the laser scanning, and the influence caused by the pitch angle change of the roadheader is compensated to obtain the angle α between the front and rear lasers, which corresponds to different distances according to different angles. After constructing the function The time when the laser sweeps across the photosensitive element can be converted into the displacement information of the excavation.

The displacement is judged by the time when the laser scans the photosensitive element. When the heading angle and roll angle of the roadheader change, it has little effect on the distance measurement results. When the pitch angle changes, the distance scanned by the laser is the same as the normal scanning distance. The distance passed is different, and the difference is L. Use a simple trigonometric function to find L, set the pitch angle to -A, add the actual time for sweeping the photosensitive element and the additional time for sweeping L·(1-cosA), and you can get The time required when the pitch angle does not change, and then judge the displacement information of the roadheader according to the time;

The time when the roadheader sweeps the photosensitive element is obtained by collecting multiple sets of data and directly fitting the function curve. In order to reduce the difficulty of calculation, it is fitted into a quadratic function as y=a ₁ x ² +a ₂ x+a ₃ .

The above disclosures are only some preferred specific embodiments of the present invention, however, the embodiments of the present invention are not limited thereto, and any changes conceivable by those skilled in the art shall fall within the protection scope of the present invention.

Claims (8)

1. The utility model provides a entry driving machine combination position appearance detecting system which characterized in that includes:

a laser perception system, the laser perception system comprising: the plane laser transmitter comprises a plane laser transmitter (1), a first laser perception target (2) and a second laser perception target (3), wherein the plane laser transmitter (1) is fixed on a roadway top plate; the first laser perception target (2) is arranged at the rear end of the machine body of the heading machine; the second laser perception target (3) is arranged in front of the first laser perception target (2), the first laser perception target (2) is connected with the second laser perception target (3) through a bus, and a plurality of photosensitive elements are horizontally arranged on the first laser perception target (2) and the second laser perception target (3);

the laser sensing system is arranged above the midpoint of the machine body of the heading machine, and is communicated with the inertial navigation system (4);

the upper computer is connected with the inertial navigation system through a wireless network, is arranged in an aboveground control center and is in data communication with the industrial ring network through a base station arranged underground;

the electric control system of the heading machine is arranged in an electric control system protective shell (5), the electric control system protective shell (5) is arranged on the heading machine and connected with hydraulic valves of hydraulic mechanisms on the heading machine, a wireless communication module is arranged in the electric control system of the heading machine and communicated with the upper computer, and the electric control system of the heading machine controls the hydraulic valves to complete the control of the heading machine by receiving control instructions sent by the upper computer.

2. The combined pose detection system of the heading machine according to claim 1, wherein the center lines of the first laser sensing target (2) and the second laser sensing target (3) in the laser sensing system coincide with the center line of the body of the heading machine.

3. A combined posture detecting system of a heading machine according to claim 2, characterized in that the center line of the inertial navigation system (4) coincides with the center line of the body of the heading machine.

4. The combined position and posture detection system of the heading machine as claimed in claim 2, characterized in that the distance between the second laser sensing target (3) and the first laser sensing target (2) is 50cm.

5. The method for detecting the pose of the combined pose detection system of the heading machine according to claim 1, comprising the following steps of:

s1, a plane laser transmitter (1) of a laser sensing system transmits laser to irradiate photosensitive elements on a first laser sensing target (2) and a second laser sensing target (3), the mounting position of the targets and specific pose parameters of a heading machine are related, the lateral offset distance of the heading machine relative to the center line of a roadway and the heading angle of the heading machine are calculated, and the lateral offset, the heading angle and the displacement measured by the laser sensing system are transmitted to an inertial navigation system (4);

s2, acquiring heading angle and transverse offset of the heading machine according to the attitude angle information of the machine body of the heading machine, which is acquired by the inertial navigation system (4), establishing a time and displacement function of laser scanning of a photosensitive element, and calculating the displacement information of the heading machine through the time of laser scanning of the photosensitive element after pitch angle compensation;

s3, after the transverse offset and the transverse displacement of the development machine are obtained, the transverse offset and the transverse displacement of the development machine are transmitted back to the inertial navigation system (4) together with the information of the heading angle of the machine body, the output of the inertial navigation system is corrected by utilizing the displacement and the heading angle in the inertial navigation system, and the heading angle obtained by the laser sensing system and the heading angle detected by the inertial navigation system are subjected to weighted calculation and then used in the subsequent attitude angle correction process;

and S4, after all the body pose parameters of the heading machine are measured, the specific pose parameters are transmitted to an upper computer, the specific pose parameters are transmitted to an electric control system of the heading machine after being processed by the upper computer, and after one round of heading operation is finished, the pose of the heading machine is adjusted by the electric control system to continue heading operation, and meanwhile, the pose of the heading machine is continuously detected.

6. The method for detecting the combined pose of the heading machine according to claim 5, wherein in S1, when the laser of the planar laser transmitter (1) sweeps over a photosensitive element at the middle position of the first laser sensing target (2) and the second laser sensing target (3), no deviation of the heading angle of the heading machine is indicated.

7. The method for detecting the combined pose of the heading machine according to claim 5, wherein in S2, the time when the laser emitted by the planar laser emitter (1) just contacts the photosensitive element is set as t ₁ Leaving the light-sensitive element for a time t ₂ ，(t ₂ -t ₁ ) The total time of the laser staying on the photosensitive element is related to the rotating speed omega during laser scanning, the influence caused by the change of the pitching angle of the heading machine is compensated, the included angle alpha of the front laser and the back laser is obtained, different distances are corresponding to different angles, and the time of scanning the photosensitive element by the laser after the function is constructed is converted into the tunneling displacement information.

8. The method for detecting the combined pose of the heading machine according to claim 5, wherein the displacement is judged by the time of the laser sweeping the photosensitive element, the heading angle and the roll angle of the heading machine are changed without considering the distance measurement result, when the pitch angle is changed, the distance swept by the laser is different from the distance swept normally, the difference is L, the L is obtained by using a simple trigonometric function, the pitch angle is set as-A, the time actually sweeping the photosensitive element is added with the time additionally required to sweep L (1-cosA), the time required when the pitch angle is not changed is obtained, and the displacement information of the heading machine is further judged according to the time;

the time of the tunneling machine for sweeping the photosensitive element is obtained by collecting a plurality of groups of data and directly fitting a function curve, and in order to reduce the calculation difficulty, the time is fit into a quadratic function with y = a ₁ x ² +a ₂ x+a ₃ 。

Images