Position and attitude data measuring system and method for development machine
Technical Field
The invention discloses a heading machine position and attitude data measuring system based on a horizontal moving target and a measuring method thereof, and belongs to the technical field of heading machine position and attitude data measurement.
Background
The development machine is a combined unit capable of realizing cutting, loading and transporting functions, and a cantilever type tunnel development machine is used in the coal and rock development field at present; in the using process, the orientation of the heading machine has important significance for heading the direction and the gradient of the roadway.
In the traditional tunneling process, in order to keep the tunneling equipment always in the correct tunneling direction, a driver of the tunneling machine needs to manually observe at any time and accurately follow the laser pointing point of the laser pointing instrument to perform tunneling construction; with the improvement of the quality and efficiency requirements of the mine roadway tunneling project, the automatic tunneling work of the tunneling machine is required, and the tunneling work of the tunneling machine needs to be completed by an automatic control method urgently so as to reduce the working strength of field workers and improve the tunneling efficiency.
The automatic position and orientation measurement of the heading machine is the premise of realizing the automatic control of the heading machine, in order to realize the position and orientation measurement of the heading machine, the position coordinate of the heading equipment is required to be established, and the heading direction are controlled only by the position coordinate, so that the heading machine has the automatic heading function; the existing measuring device and measuring method are influenced by factors such as complex driving terrain, harsh environment and the like, so that the measuring device is complex in structure and low in measuring precision, the defects of accumulative errors, drifting and the like which cannot be avoided exist in the process of using an inertial navigation technology for long-time work, and the accuracy is difficult to ensure in high-vibration occasions.
Disclosure of Invention
In order to overcome the defects in the prior art, the invention aims to solve the technical problems that: the hardware structure of the heading machine position and attitude data measuring system based on the horizontal moving target and the improvement of the measuring method thereof are provided.
In order to solve the technical problems, the invention adopts the technical scheme that: a position and posture data measuring system of a heading machine based on horizontal moving targets comprises the heading machine and a laser direction indicator movably arranged on a top plate of a heading tunnel, wherein the laser direction indicator is arranged at the rear side of the heading machine, a pair of horizontal moving targets are arranged on the upper side of the machine body of the heading machine in parallel, each horizontal moving target comprises a detection target, the detection target is arranged on a slide rail driven by a stepping motor through a target seat slide block, the slide rails of the two horizontal moving targets are arranged on the upper side of the machine body of the heading machine in parallel, and the stepping motor is arranged on the side wall of the machine body of the heading machine;
the interior of the development machine is also provided with a control circuit board and an inclination angle sensor, and the control circuit board is integrated with a microcontroller;
the horizontal moving target is used for receiving laser beams emitted by a laser direction indicator, and the detection target is a laser capture circuit board formed by photosensitive diodes;
the microcontroller is connected with the signal output end of the detection target through a lead;
the microcontroller is connected with the stepping motor and the inclination angle sensor through leads;
the microcontroller is also connected with a data storage module through a lead;
and the power supply input end of the microcontroller is connected with the power supply module.
The chip used by the microcontroller is a control chip U1, and the circuit structure of the microcontroller is as follows:
pins 1, 9, 24, 36 and 48 of the control chip U1 are connected with a 3.3V input power supply;
pins 2, 3, 38, 39, 45 and 46 of the control chip U1 are connected with the control end of the stepping motor;
the 4 pins of the control chip U1 are connected in series with the resistor R61 and the diode LED1 and then grounded;
the 5 pin of the control chip U1 is connected with one end of a crystal oscillator XT1 and then is connected with one end of a capacitor C16;
the pin 6 of the control chip U1 is connected with the other end of the crystal oscillator XT1 in parallel and then is connected with the other end of the capacitor C17, and the other end of the capacitor C17 is connected with the other end of the capacitor C16 in parallel and then is grounded at one end of the capacitor C14;
the other end of the capacitor C14 is connected with one end of a resistor R59 and one end of a control switch SW1 in parallel and then is connected with a pin 7 of a control chip U1;
the other end of the resistor R59 is connected with a 3.3V input power supply, and the other end of the control switch SW1 is grounded;
pins 10 to 22 of the control chip U1 are connected with the signal output end of the detection target through a multiplexer;
pins 25 to 28 of the control chip U1 are connected with a data storage module;
pins 29, 30 and 31 of the control chip U1 are connected with the signal output end of the stepping motor through a 485 transceiver;
and pins 34 and 37 of the control chip U1 are connected with the signal output end of the tilt sensor.
The model of the control chip U1 is STM32F103CBU 6;
the type of a chip used in the multiplexer is 74HC 4051;
the model of the 485 transceiver is MAX 3485;
the chip model used in the data storage module is 74HC 595;
the chip models used in the power module are a power distribution switch TPS2552 and a voltage stabilizer RT 9013.
A heading machine position and attitude data measuring method based on a horizontal moving target comprises the following steps:
the method comprises the following steps: the method comprises the following steps of arranging a tunneling machine in a roadway to be tunneled, installing a laser direction indicator on a roadway top plate on the rear side of the tunneling machine, wherein the laser direction indicator is specifically arranged at the center line position of the roadway top plate, and the laser emission angle of the laser direction indicator is consistent with the tunneling direction of the roadway;
step two: a pair of horizontal moving targets are installed on a tunneling machine body, detection targets of the horizontal moving targets are vertically installed on a target seat sliding block, the target seat sliding block is installed on a sliding rail arranged on the upper side of the tunneling machine body, and the two horizontal moving targets are arranged in parallel;
step three: controlling the heading machine to start heading operation, manually adjusting the mounting position and the laser emission angle of a laser direction indicator to enable laser beams emitted By the laser direction indicator to be capable of being shot on detection targets of two horizontal moving targets, acquiring coordinate positions of the current laser beams on the detection targets By the detection targets through a laser capture circuit board, wherein the coordinate position data comprise left and right positions (Ax, Bx) and upper and lower positions (Ay, By), and sending the coordinate position data to a microcontroller through a lead wire for further processing By the detection targets;
step four: the tilt angle sensor simultaneously acquires the current tunneling pitch angle data (Yaw) of the tunneling machine, and the tilt angle sensor sends the pitch angle data to the microcontroller through a lead for further processing;
step five: the calculation parameters of the microcontroller comprise preset distance parameters, wherein the distance from the laser beam emitted by the laser direction indicator to the first horizontal moving target is defined as L1, the distance between the two horizontal moving targets is defined as L0, and the pose measurement data of the current heading machine can be calculated and obtained by combining the received laser coordinate data:
wherein the calculation formula of the pitch angle (Yaw) is as follows:
the heading angle (Pitch) is calculated as:
the Roll angle (Roll) data is the measurement data of the tilt sensor (6);
the calculation formula of the horizontal displacement (dx) is:
the calculation formula of the height displacement (dy) is:
the depth displacement (dz) is calculated as:
in the coordinate system of the above formula, an X axis is defined as the width of a roadway driving surface, a Y axis is defined as the height of the roadway driving surface, and a Z axis is defined as the depth of the roadway driving direction;
ax and Bx are x coordinates of the positions of the points where the laser beams strike the first and second horizontal movement targets, respectively;
ay and By are the y-coordinates of the positions of the points where the laser beams impinge on the first and second horizontal movement targets, respectively.
Compared with the prior art, the invention has the beneficial effects that: the invention provides a position and posture data measuring device of a development machine, which can be combined with the functions of the existing laser direction indicator used in coal mines to realize the real-time measurement of the position and posture data of the development machine in work; according to the invention, two horizontal moving type laser targets are additionally arranged on the heading machine body, so that the position and the posture of the current heading machine are measured in the whole time period in the working process of the heading machine, the posture state compensation of the heading machine in the working process can be realized, the posture state compensation is fed back to a monitoring computer, and the automatic heading work of the heading machine is finally realized; the invention is based on the laser direction indicator which is used at present, acquires the center line and the waist line of the tunnel in real time, has high positioning precision, can effectively improve the tunneling efficiency, is convenient for installing and debugging the whole measuring system, and can realize the requirement of accurate positioning of the tunneling machine.
Drawings
The invention is further described below with reference to the accompanying drawings:
FIG. 1 is a schematic structural view of the present invention;
FIG. 2 is a schematic structural diagram of an internal control circuit of the heading machine of the present invention;
FIG. 3 is a circuit diagram of a microcontroller according to the present invention;
FIG. 4 is a model diagram of the present invention for measuring and calculating the pitching angle of the heading machine;
FIG. 5 is a model diagram of the heading angle of the measuring and calculating heading machine of the invention;
FIG. 6 is a data processing flow chart of the position and orientation measurement of the heading machine of the present invention;
in the figure: the device comprises a laser direction indicator 1, a detection target 2, a target seat sliding block 3, a stepping motor 4, a sliding rail 5, an inclination angle sensor 6, a microcontroller 7, a data storage module 8 and a power module 9.
Detailed Description
The invention provides a position and attitude data measuring system and a position and attitude data measuring method of a heading machine based on a horizontal moving target, mainly relates to a heading machine positioning technology used for heading underground coal mine tunnels or railway and highway tunnels, comprises a corresponding position and attitude parameter measuring device and a measuring method thereof, and plays an important role in realizing automatic control of the heading machine.
The invention mainly comprises a laser direction indicator arranged on a top plate of a driving tunnel, wherein the laser direction indicator is arranged by a related measuring department and is used for indicating a central line and a waist line of the driving tunnel, and the laser direction indicator moves forwards in a manual mode at intervals and is repositioned;
as shown in fig. 1 to 3, two horizontally moving laser targets which are arranged in parallel are arranged in front of and behind the machine body of the heading machine, and when the heading machine is used, a laser beam of a laser direction indicator is required to strike the two laser targets; when a certain photosensitive diode receives illumination in a certain frequency spectrum range, the controller scans the voltage change of the diodes on ROW (ROW) and COLUMN (COLUMN) lines connected with the photosensitive diode according to different illumination intensities and different on-resistances of the diodes, and acquires the position of a laser point receiving point according to the voltage difference;
a tunneling machine controller is arranged in the tunneling machine, the controller is in bidirectional communication with the two horizontal moving laser targets through a communication wire, and the controller is also connected with the inclination angle sensor through a wire;
and the interior of the heading machine is also provided with an inclination angle sensor for measuring the roll angle of the heading machine and sending the obtained angle data to the heading machine controller through a lead.
The horizontal moving target comprises detection targets for receiving laser irradiation, each detection target consists of a photosensitive diode and a corresponding controller to form a laser capture circuit board, the detection targets are required to be arranged on a horizontal guide rail in order to ensure that laser beams are irradiated on the two detection targets in the advancing process of the heading machine, the detection targets can move on the guide rail in parallel by controlling a stepping motor, and the moving distance and the current position of the detection targets on the guide rail can be calculated and obtained by a motor encoder in the controller.
As shown in fig. 4 and 5, in order to realize the automatic cutting function of the heading machine on the roadway section and measure the heading position and the body attitude parameter of the heading machine in real time, the invention obtains the roadway center line and the waist line pointed by the laser direction indicator by additionally arranging two horizontally moving laser targets on the heading machine, analyzes and processes the acquired data by combining the body size of the heading machine and the body-mounted tilt angle sensor, and obtains the corresponding position and attitude parameter of the heading machine, thereby providing basic data for realizing the automatic control of the heading machine and improving the heading efficiency, the safety and the roadway flatness of the heading machine.
The invention provides a heading machine pose data measuring method which mainly comprises the following steps:
the method comprises the following steps: the method comprises the following steps of arranging a tunneling machine in a roadway to be tunneled, installing a laser direction indicator on a roadway top plate on the rear side of the tunneling machine, wherein the laser direction indicator is specifically arranged at the center line position of the roadway top plate, and the laser emission angle of the laser direction indicator is consistent with the tunneling direction of the roadway;
step two: a pair of horizontal moving targets are installed on a tunneling machine body, detection targets of the horizontal moving targets are vertically installed on a target seat sliding block, the target seat sliding block is installed on a sliding rail arranged on the upper side of the tunneling machine body, and the two horizontal moving targets are arranged in parallel;
step three: controlling the heading machine to start heading operation, manually adjusting the mounting position and the laser emission angle of a laser direction indicator to enable laser beams emitted By the laser direction indicator to be capable of being shot on detection targets of two horizontal moving targets, acquiring coordinate positions of the current laser beams on the detection targets By the detection targets through a laser capture circuit board, wherein the coordinate position data comprise left and right positions (Ax, Bx) and upper and lower positions (Ay, By), and sending the coordinate position data to a microcontroller through a lead wire for further processing By the detection targets;
step four: the tilt angle sensor simultaneously acquires the current tunneling pitch angle data (Yaw) of the tunneling machine, and the tilt angle sensor sends the pitch angle data to the microcontroller through a lead for further processing;
step five: the calculation parameters of the microcontroller comprise preset distance parameters, wherein the distance from the laser beam emitted by the laser direction indicator to the first horizontal moving target is defined as L1, the distance between the two horizontal moving targets is defined as L0, and the pose measurement data of the current heading machine can be calculated and obtained by combining the received laser coordinate data:
wherein the calculation formula of the pitch angle (Yaw) is as follows:
the heading angle (Pitch) is calculated as:
the Roll angle (Roll) data is the measurement data of the tilt sensor (6);
the calculation formula of the horizontal displacement (dx) is:
the calculation formula of the height displacement (dy) is:
the depth displacement (dz) is calculated as:
in the coordinate system of the above formula, an X axis is defined as the width of a roadway driving surface, a Y axis is defined as the height of the roadway driving surface, and a Z axis is defined as the depth of the roadway driving direction;
ax and Bx are x coordinates of the positions of the points where the laser beams strike the first and second horizontal movement targets, respectively;
ay and By are the y-coordinates of the positions of the points where the laser beams impinge on the first and second horizontal movement targets, respectively.
As shown in fig. 6, the microcontroller establishes data connection with each target unit, and can determine whether the two current horizontal moving targets receive irradiation of a laser beam, after confirming that the two horizontal moving targets capture laser points, the microcontroller calculates corresponding pose data according to a preset algorithm script, and sends corresponding calculation results to a data storage module for storage, and meanwhile, the calculation data can be sent to an upper computer on the ground through an external communication bus, and managers further process the data, so that corresponding management and statistical functions can be realized.
Finally, it should be noted that: the above embodiments are only used to illustrate the technical solution of the present invention, and not to limit the same; while the invention has been described in detail and with reference to the foregoing embodiments, it will be understood by those skilled in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some or all of the technical features may be equivalently replaced; and the modifications or the substitutions do not make the essence of the corresponding technical solutions depart from the scope of the technical solutions of the embodiments of the present invention.