Motor device pose measurement system and measuring method
Technical field
The invention belongs to a kind of for coal mining, the measuring system under the environment such as tunnelling, particularly a kind of motor device pose measurement system and measuring method.
Background technology
The location navigation of down-hole motor device is that the underworkings motor device is advanced, a key issue during operation.The location of present domestic down-hole motor device adopts laser orientation instrument to provide predetermined path of movement, relies on the make a strategic decision action data of motor device of the relative position relation of staff's visual inspection motor device and director direction indication during the underground equipment motion.When motor device departs from desired trajectory a little, human eye can not in time be found, because subsurface environment is abominable, the workplace Dust Capacity is large, a little less than the light simultaneously, and staff's sight line is subject to other objects and blocks, various reasons causes motor device to be difficult to move along desired trajectory, and has relatively large deviation, and this will cause the motor device movement locus indefinite, the inefficiency of advancing, hand labor intensity is large etc.
For overcoming the above problems, realize motor device Long-distance Control and automatically control, must obtain the posture information of motor device.Publication number is CN201013380, and the utility model patent that name is called " full automatic tunneling machine " has proposed a kind of position of adopting the aerospace navigation orientator to measure development machine, but this patent is not measured the fuselage attitude of development machine; Publication number is CN101629807A, the patent of invention that name is called " machine body of boring machine pose parameter measurement system and method thereof " proposes to utilize fan-shaped laser beam at the linear hot spot of development machine figure, dependence comes the generation current signal with the laser target of light activated element, draw after treatment angle of deviation and the deflection displacement of development machine, but the method is very difficult obtaining of fan-shaped laser beam, and the processing technology of fan-shaped laser beam and precision also are difficult to guarantee.
Summary of the invention
Fuselage pose measurement problem when the objective of the invention is to solve motor device and advancing, and for motor device provides the data of automatically advancing along desired trajectory, and a kind of motor device pose measurement system and measuring method are provided.
To achieve these goals, the technical solution adopted in the present invention is:
A kind of motor device pose measurement system comprises: laser ranging module installation frame, laser ranging module, central processing system, the ARM microcontroller, wireless communication module, planar laser reflecting surface, taper laser reflection face, 3-axis acceleration sensor is characterized in that:
The laser ranging module: be installed on the laser ranging module installation frame, the laser ranging module is for off-line data and move distance on the vertical plane of measuring motor device;
ARM microcontroller: be installed on motor device and the laser ranging module installation frame;
Wireless communication module: be installed on motor device and the laser ranging module installation frame, be used for data transmission with the laser ranging module to central processing system;
Planar laser reflecting surface and taper laser reflection face: be installed in the motor device afterbody;
Laser ranging module installation frame: be installed in top, tunnel, motor device rear;
3-axis acceleration sensor: be installed on the motor device, utilize the measurement data of its Z-axis to assist and obtain motor device relative level face inclination angle;
Central processing system: the data that be used for to gather laser ranging module and the 3-axis acceleration module line number of going forward side by side is processed, and obtains the motor device data of walking, and is sent in the motor device control box by the CAN bus data of will walking.
Described laser ranging module installation frame longitudinal maneuver equipment distance is 1m-200m.
Described laser ranging module has four, per two laser ranging modules are one group, it is the pre-motion orientation of motor device that the laser of each laser ranging module penetrates direction, wherein the first laser ranging module and the second laser ranging module are one group, the first laser ranging module and the second laser ranging module are parallel to each other, and spacing is 0.5m, the 3rd laser ranging module and the 4th laser ranging module are one group, the 3rd laser ranging module and the 4th laser ranging module are parallel to each other, and spacing is 0.5m.
Described planar laser reflecting surface is positioned at the position that laser energy that the first laser ranging module and the second laser ranging module send projects, and the reflecting surface color is white or orange-yellow.
Described taper laser reflection face is positioned at the position that laser energy that the 3rd laser ranging module J 3 and the 4th laser ranging module J 4 send projects, and the reflecting surface color is white or orange-yellow.
The measuring method of described motor device pose measurement system is: use the first laser ranging module and the second laser ranging module to measure the distance of laser ranging module installation frame parallel reflective faces to the motor device, converse the horizontal deflection angle of motor device according to the spacing of the difference of two distances and the first laser ranging module and the second laser ranging module; Use the 3rd laser ranging module and the 4th laser ranging module to measure laser ranging module installation frame to the distance of the upper conical reflecting surface of motor device, calculate the horizontal deflection displacement of motor device according to the difference of two distances and the 3rd laser ranging module and the 4th laser ranging intermodule distance.
The pose measurement of motor device of the present invention and Position Fixing Navigation System are compared with conventional art, and its advantage and good effect are:
(1) the present invention uses the laser ranging module, acceleration transducer, and parallel and taper laser reflection face and ARM microcontroller are finished motor device pose measurement and movement navigation, can obtain motor device relatively angle and the offset distance of its desired trajectory.The measuring accuracy of this measuring system can reach angle precision in 7 °, and and there is not the error accumulation process in the offset distance precision in 5cm.
(2) pose measurement system of motor device of the present invention can be realized motor device robotization control according to the walking data of the pose parameter acquiring motor device of motor device, further can realize multi-machine interaction.
The present invention is particularly useful for Long-distance Control and automatic control of down-hole motor device, is beneficial to and realizes moving behind the control desk, and multi-machine interaction improves operation quality and operating speed, lays the foundation with robotization for final realization motor device is unmanned.
Description of drawings
Fig. 1 is theory diagram of the present invention.
Fig. 2 is motor device angle of pitch synoptic diagram of the present invention.
Fig. 3 is motor device angle of deviation of the present invention and deflection displacement diagram.
Fig. 4 is motor device side drift angle synoptic diagram of the present invention.
Fig. 5 is scheme of installation of the present invention.
Fig. 6 is angle of deviation instrumentation plan of the present invention.
Fig. 7 is deflection displacement measurement synoptic diagram of the present invention.
Among the figure: 1-central processing system, 2-3-axis acceleration sensor, 3-wireless module, 4-conical reflecting surface, 5-plane reflection face, 6-laser ranging module installation frame, 7-laser ranging module, 8-ARM microcontroller, 9-frame rectifier, 10-laser orientation instrument.
Embodiment
The present invention below specifies implementer's case of the present invention by installation method and the measuring method of describing measuring system on the basis of the above.The present invention only provides pose measurement system and navigation positioning system and the measuring method thereof of motor device, and relevant software program is not provided.
Installation method of the present invention:
A installs the laser ranging module carrier as shown in Figure 1 at the motor device rear, adjust the Laser emission direction and guarantee that it is identical with motor device motion desired trajectory;
B installs two groups at the laser ranging module carrier, and two the every group laser ranging modules that are parallel to each other are installed an ARM microcontroller and a wireless communication module simultaneously.The ARM microcontroller is crossed wireless communication module with the data communication device of four range finder modules and is sent to central processing system.Described laser ranging module is, described ARM microcontroller model is STM32F103C8, and the chip model that described wireless communication module uses is SI4432;
C installs the laser reflection face at the motor device tail end, and the laser reflection face is divided into a plane and a taper surface, guarantees during installation that one group shines on the plane in two groups of laser, and another group shines on the taper surface.Laser reflection plane materiel matter is the hard iron and steel, alloy or plastics etc., and the humiture acute variation can not produce obvious deformation.The color that laser reflection face look is white, orange isoreflectance is high;
D installs a 3-axis acceleration module in the motor device centre.Described acceleration module comprises a 3-axis acceleration sensor and an ARM microcontroller.Described 3-axis acceleration sensor model is ADXL345, and described ARM microcontroller is STM32F103C8;
E installs a central processing system system in the motor device centre.Described central processing system comprises an arm processor, a wireless communication module, 485 bus transceivers, a CAN bus transceiver.Described arm processor is STM32F103ZE, and the chip model that described wireless communication module uses is SI4432, and described 485 bus transceivers are SP3485, and described CAN transceiver is SJA1000.
Below in conjunction with accompanying drawing measuring method of the present invention and location navigation principle are described:
For convenience of description, be defined as follows: such as Fig. 2, definition motor device fore-and-aft direction axis and the angle of working direction axis, tunnel on surface level are angle of pitch α; Such as Fig. 4, the definition horizontal axis of motor device and the angle of working direction axis, tunnel on surface level are side drift angle β; Such as Fig. 3, definition motor device fore-and-aft direction axis and the angle of working direction axis, tunnel on vertical plane are angle of deviation γ; Definition motor device fore-and-aft direction axis and the offset distance of working direction axis, tunnel on surface level are deflection displacement d.
As shown in Figure 6, parallel shining is installed on the parallel reflective faces of motor device afterbody the first laser ranging module with the second laser ranging module, and it is D that the laser ranging module obtains range data
1, D
2, both range differences are Δ S, the distance between two bundle laser is S
1, can calculate so angle of deviation is γ=arctan (Δ S/S
1);
As shown in Figure 7, parallel shining is installed on the conical reflecting surface of motor device afterbody the 3rd laser ranging module with the 4th laser ranging module, and the distance between two bundle laser is S
1, the taper surface vertical view is isosceles triangle, its base angle is θ.It is D that the laser ranging module obtains ranging data
3, D
4, both sides are poor to be Δ d, by adjusting the installation site of conical reflecting surface so that motor device do not exist deflection displacement and deflection apart from the time Δ d=0, when Δ d ≠ 0, there is the deflection displacement in motor device, its computing method are d=Δ d/ (2*arctan θ).When there was angle of deviation in motor device, the data of obtaining needed to revise, and modification method is
3-axis acceleration sensor can output transducer at three of the three dimensions acceleration on axially.If the component of acceleration of acceleration of gravity on longitudinal axis on the sensor surface level is g
1, the component of acceleration on lateral shaft on the sensor surface level is g
2, the component of acceleration on Z-axis is g
3, when motor device is static, calculate the size of local acceleration of gravity
When sensor aclinal and motor device are static, g
1=g
2=0, g
3Be the gravity acceleration g size; When having the inclination angle, can be according to g
1And g
2Size is determined the angle of pitch and side drift angle, angle of pitch α=arctan (g
1/ g), β=arctan (g
2/ g); When motor device moves, need to revise the computing formula of the angle of pitch and roll angle.
Central processing system is used high-performance ARM microprocessor, obtain angle of deviation, deflection displacement and the displacement of motor device by wireless communication module, obtain the angle of pitch and the side drift angle of motor device by the CAN bus, can draw location parameter and the attitude parameter of motor device, then by the CAN bus data of advancing of motor device are sent to the control box of motor device.
The content that is not described in detail in this instructions belongs to the known prior art of professional and technical personnel in this area.