CN106052645A - Real-time detection system and method for spatial pose of boom-type roadheader - Google Patents

Abstract

The invention provides a real-time detection system and method for the spatial pose of a boom-type roadheader. The real-time detection system comprises two cross laser transmitters, an industrial control computer, two Internet protocol cameras and two laser targets. Machine vision technology is used as an acquisition manner for pose parameters; a mathematic model for resolving the pose of the roadheader is established so as to obtain the yaw angle, pitch angle, roll angle of the roadheader in heading process and the offset of a fixed point on the body of the roadheader on the cross section of a tunnel, i.e., the spatial pose of the roadheader, and the spatial pose can be used for deviation rectification in automatic heading of the roadheader. Experimental results show that the precision of an angle detected by the system is within 0.5 DEG and the precision of offset is better than 20 mm; an interval between processing time and displaying time for each frame of an image is about 5 s; so requirements of tunnel construction for automation, precision and real-time positioning are met.

Description

A kind of boom-type roadheader spatial pose real-time detecting system and method

Technical field

The present invention relates to a kind of spatial pose real-time detecting system and the method for mining boom-type roadheader, belong to Extractive equipment application and machine vision applications field.

Background technology

Boom-type roadheader is a kind of common mining machinery, is widely used in coal mine roadway and tunnel In excavation.

Currently, the tunneling process of development machine relies primarily on laser target guiding and manually estimates realization, easily Disturbed by on-the-spot light and the factor such as artificial, thus cause and backbreak in tunnel, owe to dig phenomenon, cause enterprise Coal mining inefficiency even causes safety problem.Therefore, it is achieved the automatization of development machine tunneling process and intellectuality Just seem particularly urgent, and the most accurately driving of development machine to be solved has been necessary for its fuselage spatial pose The detection the most in real time of parameter.Machine body of boring machine pose detection technique based on stereo computer vision, its technology Essence is exactly to utilize machine vision technique to replace human eye to detect and the pose of analysis space rigid body, uses this technology Scheme has that hardware cost is low, easy and simple to handle, certainty of measurement high.

Existing patent of invention such as Publication No. CN101819036A and publication number CN102878976A, all The principle using stereo computer vision measures the fuselage pose parameter of development machine, but owing to this type of patent of invention is built During formwork erection type, employing point lasing light emitter is as pointing to unit, therefore the point source that all there is the transmitting of laser alignment unit is easy Blocked, thus cause measurement system accuracy poor, unstable, even result in and cannot detect heading machine pose Parameter.So, this type of invention has limitation, it is impossible to further genralrlization.

Summary of the invention

Patent of the present invention, for the deficiency of background technology, proposes a kind of boom-type roadheader spatial pose real-time Detecting system and method, can detect the spatial pose parameter of machine body of boring machine real-time and accurately, and have anti-dry Immunity is strong, stability high.

To achieve these goals, the present invention is achieved by the following technical solutions:

Patent of the present invention is a kind of boom-type roadheader spatial pose real-time detecting system and method, can be real-time Obtain the pose parameter of development machine, including yaw angle α, pitching angle beta, roll angle γ and fuselage central point The coordinate of D (dx, dy, dz), due in practical operation Herba Plantaginis away from L (i.e. dy) for the correction use of development machine Not quite, thus described detecting system and method only calculate on machine body of boring machine fixing a little value of dx Yu dz.

Further, described system includes two cross laser emitters, industrial computer, a Liang Tai Web camera, two laser targets.

Further, described two cross laser emitters are mine anti-explosion cross laser emitter, vertical Back is fixed in suspension, launches cross laser signal to said two laser target respectively.

Further, said two laser target be separately fixed at the left front of machine body of boring machine horizontal plane with Right back, for receiving the projection of the cross laser that described cross laser emitter is launched.

Further, described two web cameras are mine anti-explosion web camera, are respectively arranged in institute State the dead astern of two laser targets, for gathering the development machine time of running of the cross laser at described laser target Imaging.

Further, described industrial computer can process a large amount of graphics data in real time, by with described Two web cameras connect, and extract the imaging on the left and right laser target that described web camera gathers respectively also Carry out image procossing and characteristic parameter extraction.

Further, through described industrial computer, described web camera is gathered image to process and carry After taking characteristic parameter, the coordinate of the optical characteristic point of laser image can be calculated from described laser target target surface, The cross laser launched including described cross laser emitter and the coordinate of described laser target target surface intersection point, institute State the center point coordinate that the cross laser that cross laser emitter launches projects at described laser target target surface.

Further, according to tunnel coordinate system and fuselage coordinates system bi-coordinate system model with above-mentioned calculate The coordinate of characteristic point, carries out pose resolving, finally gives development machine spatial pose parameter under the coordinate system of tunnel, The i.e. value of yaw angle α, pitching angle beta, roll angle γ, left and right offset distance dx and upper and lower offset distance dz.

Compared with the prior art, the medicine have the advantages that

Machine vision technique is applied to boom-type roadheader fuselage pose detecting system by the present invention, by hardware Software implementation, has low cost, easy and simple to handle, certainty of measurement high.Additionally, the present invention also uses double ten Word generating laser, efficiently solve point source easily by blocking that the measurement system that causes is unstable, measurement result not Accurate problem, has the features such as strong interference immunity, cost be controlled, possesses certain market popularization value.

Accompanying drawing explanation

Accompanying drawing 1 is development machine of the present invention and measurement equipment mutual alignment figure；

Accompanying drawing 2 is tunnel of the present invention coordinate system and machine body of boring machine coordinate system illustraton of model；

Accompanying drawing 3 is development machine spatial pose overhaul flow chart of the present invention；

Accompanying drawing 4 is bi-coordinate system fuselage pose mathematical model of the present invention vector and angular transition schematic diagram；

Accompanying drawing 5 is that laser target imaging features point of the present invention resolves schematic diagram；

Accompanying drawing 6 is that development machine spatial pose of the present invention resolves schematic diagram；

Accompanying drawing 7 is fuselage coordinates system of the present invention and tunnel coordinate system Coordinate Conversion schematic diagram.

Detailed description of the invention

In order to be more clearly understood that the above-mentioned purpose of the present invention, feature, advantage, below in conjunction with the accompanying drawings With detailed description of the invention, the present invention is further elaborated on.

Equipment is installed and the foundation of model:

With reference to shown in accompanying drawing 1, patent of the present invention is that a kind of boom-type roadheader spatial pose detects system in real time System and method, including two cross laser emitters 1,2, two laser targets 4,6, two network shootings 5,7 and industrial computer of machine.

Back is fixed in the most vertical suspension of described two cross laser emitters 1,2, during installation, Ensure that two bundle horizontal laser light are overlapping and are parallel to development machine and preset direct of travel, described two cross laser emitters 1, the 2 cross laser planes launched are vertical with described laser target plane 4,6 respectively, and described two laser are sent out Emitter center is with axis, tunnel apart from equal, and described two generating laser spacing are l；Said two laser Target 4,6 is separately fixed at left front and the right back of machine body of boring machine horizontal plane, ensures described two during installation Individual laser target is parallel to vertical of machine body of boring machine and two bundle cross lasers respectively at said two laser mark Initial imaging on target is positioned at center；Described two web cameras 5,7 are installed on the dead astern of target, And adjust distance each other, to ensure the integrity of imaging.

Shown in reference accompanying drawing 2 and accompanying drawing 4, set up tunnel coordinate system and fuselage coordinates system bi-coordinate system fuselage Pose mathematical model, described model needs drift section coordinate system is O_hX_hY_hZ_h, described in the initial position moment Machine body of boring machine center line overlaps with laneway midline, described tunnel coordinate origin O_hIt is positioned at the geometry of drift section Center, X_hAxle points on the right side of tunnel, Y_hAxle and tunnel axis parallel also point to development machine direction of advance, Z_h Axle vertically constitutes right hand relations with upper two axles and orientation is above positive direction.

Described fuselage coordinates system is O_bX_bY_bZ_bSet up on machine body of boring machine, initial point O_bIt is positioned at fuselage axis On line, the X when initial position_b、Y_b、Z_bThree axles respectively with X_h、Y_h、Z_hThree axles are parallel.

Described model can be reduced to the determination of cuboid rigid body pose, i.e. fuselage coordinates system under the coordinate system of tunnel X_b、Y_b、Z_bVector under the coordinate system of tunnelAnd on rigid body any point at this seat Coordinate under mark system.

Wherein, the normal vector of three planes of fuselage under fuselage coordinates system with the transformational relation of α, β, γ is:

$\left(\begin{array}{c}{\stackrel{\→}{X}}_b\\ {\stackrel{\→}{Y}}_b\\ {\stackrel{\→}{Z}}_b\end{array}\right)=\left(\begin{array}{ccc}{X}_{bx}& X_{by}& X_{bz}\\ Y_{bx}& Y_{by}& Y_{bz}\\ Z_{bx}& Z_{by}& Z_{bz}\end{array}\right)---\left(1\right)$

And then can obtain,

$\left(\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\\ \mathrm{\γ}\end{array}\right)=arctan\left(\begin{array}{c}{Y}_{bx}{\left(Y_{by}\right)}^{-1}\\ Y_{bz}{\left(Y_{by}\right)}^{-1}\\ X_{bz}{\left(X_{bx}\right)}^{-1}\end{array}\right)---\left(2\right)$

Machine body of boring machine pose detection overall flow:

With reference to shown in accompanying drawing 3, carry in described two web cameras first by described industrial computer The imaging on said two laser target taken is sampled, and the video flowing collected can sample timing a frame figure Picture.

Further, use described industrial computer process image and extract characteristic parameter, first use Retinex Algorithm for image enhancement carries out adaption brightness regulation to image, and the image owing to collecting creates bigger bucket row Distortion, therefore need to carry out distortion correction to image, then by the process of color being filtered out in two target surface images not The laser rays needed, the method using Straight edge inspection the actual size installed according to target, calculate machine The cross laser that under body coordinate system, described cross laser emitter is launched respectively with described laser imaging target target The cross laser central point that the coordinate of face intersection point and described cross laser emitter are launched is at described laser target surface The coordinate of imaging.

Described solution process is with reference to accompanying drawing 4, with described laser target γ₁As a example by, calculate under polar coordinate respectively Go out straight line l₁、l₂With four reference point (P₁-P₄) intersection point A, B, E, F, M of line be at image coordinate system Under coordinate, then be transformed under fuselage coordinates system by formula (1):

$\left(\begin{array}{ccc}X& Y& Z\end{array}\right)=\left(\begin{array}{ccc}\frac{x-x_1}{x_3-x_1}w& \frac{y-y_4}{y_1-y_4}h& 0\end{array}\right)---\left(3\right)$

Wherein, (x, y) is 5 coordinates in image coordinate system, and h is P₁、P₄Between actual range, w For P₁、P₃Between actual range, (X, Y, Z) is 5 coordinates in fuselage coordinates system.

Further, according to above-mentioned two laser target γ₁、γ₁In the coordinate of 10 points, calculate lane Fix on the yaw angle of development machine, the angle of pitch, roll angle and fuselage under road cross-sectional coordinates and a little cut in tunnel The spatial pose (α, beta, gamma, dx, dz) of the side-play amount on face, i.e. development machine.

Fuselage is relative to the yaw angle α under the coordinate system of tunnel, pitching angle beta and roll angle γ solution process reference accompanying drawing 6, the overlaying horizontal face that described two cross laser emitters produce is considered as face α, left cross laser vertical plane is β₁, right cross laser vertical plane is β₂, right cross laser (α, β₂) at target surface γ₁Two of upper projection are intersected straight Line is AB, EF and meets at a M, left cross laser (α, β₁) at target surface γ₂Two of upper projection are intersected straight Line is CD, GH and meets at a N.Because of face γ₁With face γ₂Parallel, therefore horizontal laser light face α is at described laser target Two straight line AB with CD formed on target surface are parallel to each other, by the image procossing of previous step and described laser The actual size of target and installation site can obtain γ₁With γ₂The concrete coordinate of upper ten points, thus obtain plane α is in coordinate system O_bX_bY_bZ_bUnder normal vectorWith plane equation.

In particular cases, vectorIt is parallel to vectorNow the pitching angle beta of development machine is zero, roll Angle γ is straight line CD and axle X_bBetween angle, yaw angle α isWith during initial positionBetween angle； If it is vectorialIt is not parallel to vectorThenIt is plane β₁Normal vectorRespectively In tunnel cross-sectional coordinates X_h、Z_hDirection identical, Y_hDirection beEasily try to achieve, 1 N ' on line segment CD extended line meetsThe central axis of the rightest cross laser, thus will be flat Face β₁It is shifted to the right to plane β₂Position, simulate cross laser drop shadow effect ideally, for point coordinates Offer condition is accurately provided.

With reference to accompanying drawing 7, above-mentioned gainedFor fuselage coordinates system O_bX_bY_bZ_bUnder fuselage Direction vector, must carry out Coordinate Conversion again, obtain tunnel coordinate system O_hX_hY_hZ_hThe direction vector of lower fuselage. Concrete solution process is as follows: to matrix (Nx, Ny, Nz)^TDoing matrixing, being multiplied with transition matrix R is converted to Unit matrix, i.e. can get O_hX_hY_hZ_hThe unit vector of lower three axles of coordinate system.

N R=E (4)

VectorAt O_hX_hY_hZ_hIt is unit matrix E under coordinate system, unit matrix is done phase Convert together the matrix obtained, be O_hX_hY_hZ_hFuselage pose under coordinate system.

B=E R (5)

I.e. B=N^-1, available

$\left(\begin{array}{ccc}{X}_{bx}& Y_{bx}& Z_{bx}\\ X_{by}& Y_{by}& Z_{by}\\ X_{bz}& Y_{bz}& Z_{bz}\end{array}\right)={\left(\begin{array}{ccc}Nxx& Nxy& Nxz\\ Nyx& Nyy& Nyz\\ Nzx& Nzy& Nzz\end{array}\right)}^{-1}---\left(6\right)$

Fuselage is i.e. can get relative to the yaw angle α under the coordinate system of tunnel, pitching angle beta and roll again by (2) formula Angle γ.

Fuselage is as follows relative to the point coordinates solution process under the coordinate system of tunnel:

Further, for determining the pose of a rigid body in space, the side of three axles of body axis system is only known Can not be accurately positioned to angle, also need to obtain any point O on rigid body₁Concrete coordinate in space, i.e. (O_1x, O_1y, O_1z).In development machine alignment system, development machine is left and right, upper and lower offset distance on cross section, tunnel Find that the walking of development machine is inclined than the distance more efficiently help mine staff passed by along tunnel central shaft Difference is also rectified a deviation in time.And due to the method for described detecting system employing vision localization, it is impossible to definitely know development machine And the axial distance O between the coordinate system zero of tunnel_1y, therefore described detecting system is for the resolving of point coordinates Calculate (O_1x, O_1z), i.e. O on cross section, tunnel₁Point is relative to the offset distance of section central point.

With reference to accompanying drawing 6, straight line MN ' is right cross laser central axis, is parallel to tunnel cross-sectional coordinates Y_hAxle.Described detecting system selects O_bPoint is fuselage anchor point, seeks an O_bAt face Z_hO_hX_hUpper relative tunnel The skew of axis, can first be reduced to seek an O_bIt is being parallel to Z_hO_hX_hProjection in one plane is relative to MN ' The skew of subpoint, i.e. looks for 1 O on straight line MN '₀, meet

Point O_bAt coordinate plane Z_hO_hX_hCoordinate (dx, the dz) computational methods of upper subpoint are as follows,

$\left(\begin{array}{c}dx\\ dz\end{array}\right)=\left(\begin{array}{c}{O}_{bx}+l/2\\ O_{bz}\end{array}\right)=\left(\begin{array}{c}-O_{0x}+l/2\\ -O_{0z}\end{array}\right)---\left(8\right)$

The undeclared part related in the present invention is same as the prior art or uses prior art to be realized.

Claims (6)

1. a boom-type roadheader spatial pose real-time detecting system and method, it is characterised in that including: two cross laser emitters, industrial computer, two web cameras, two laser targets, wherein,

Described two cross laser emitters are mine anti-explosion cross laser emitter, and back is fixed in the most vertical suspension, launch cross laser bundle；

Said two laser target is frosting translucent PC plate, is fixed on left front and the right back of machine body of boring machine horizontal panel, is respectively used to receive the projection of the cross laser that described cross laser emitter is launched；

Described two web cameras are mine anti-explosion web camera, are respectively arranged in the dead astern of said two laser target, gather the development machine time of running of the cross laser imaging at described laser imaging target；

Described industrial computer, by being connected with described two web cameras, extracts the image on the left and right laser target that described web camera gathers respectively, and carries out processing and characteristic parameter extraction, finally give development machine spatial pose parameter under the coordinate system of tunnel.

A kind of boom-type roadheader spatial pose real-time detecting system the most according to claim 1 and method, it is characterized in that, the installation of described two cross laser devices need to ensure that two bundle horizontal laser light are overlapping and are parallel to development machine and preset direct of travel, the cross laser plane that described two cross laser emitters are launched is vertical with described laser target plane respectively, described two generating laser centers are with axis, tunnel apart from equal, and described two generating laser spacing arel。

A kind of boom-type roadheader spatial pose real-time detecting system the most according to claim 1 and method, it is characterized in that, said two laser target need to ensure to be parallel to the tail plane of machine body of boring machine when installing and the two bundle cross laser initial imagings on said two laser target respectively are positioned at center.

A kind of boom-type roadheader spatial pose real-time detecting system the most according to claim 1 and method, it is characterized in that, described two web cameras need to ensure to lay respectively at the dead astern of described laser target when installing, and adjusts the integrity of distance guarantee imaging each other.

A kind of boom-type roadheader spatial pose real-time detecting system the most according to claim 1 and method, it is characterized in that, described industrial computer can process Various types of data immediately, it is arranged on inside development machine, it is rigidly connected with development machine, is connected with described two web cameras respectively by network connection, obtain the imaging of said two laser target, and carry out image procossing and characteristic parameter extraction, finally calculate development machine spatial pose parameter under the coordinate system of tunnel.

6. according to a kind of boom-type roadheader spatial pose real-time detecting system described in claim 1 to 5 and method, it is characterized in that, described boom-type roadheader spatial pose real-time detecting system detection method is: use two described cross laser devices being fixed on tunnel to project to corresponding described laser target, then by the imaging on described laser target during two the described web camera Real-time Collection development machine work being fixed on machine body of boring machine, use Retinex algorithm for image enhancement subsequently, the video signal collected is processed by the image processing method such as image distortion correction and straight-line detection, finally utilize the pose established to resolve model and obtain the yaw angle in development machine traveling process, the angle of pitch, fixing some side-play amount on cross section, tunnel on roll angle and fuselage.