CN104280740A - Device for jointly positioning blast hole based on camera and laser distance measuring sensor and positioning method - Google Patents

Abstract

The invention relates to a device for jointly positioning a blast hole based on a camera and a laser distance measuring sensor and a positioning method. The device comprises a mechanical arm, the camera and the laser distance measuring sensor are installed on the mechanical arm, and the camera is connected with a computer through an image collecting card and a vision module; the laser distance measuring sensor and the mechanical arm are connected with the computer through control modules. By means of the device, specific position data of the cliff blast hole are obtained through the steps of blast hole searching, space straight line obtaining, target hole distance measurement, three-dimensional coordinate obtaining and the like, the aim of conducting three-dimensional positioning on the blast hole is achieved, and the explosive implantation work can be completed through the mechanical arm. The device has the advantages of being high in explosive implantation accuracy and low in neglected packaging rate, and the labor intensity of an operator is reduced.

Description

A kind of device based on video camera and laser range sensor co-located blast hole and localization method

Technical field

The present invention relates to observation and control technology field, be specifically related to a kind of device based on video camera and laser range sensor co-located blast hole and utilize this device to complete blast hole the method for locating.

Background technology

Usually need to carry out explosion to massif in large scale civil engineering, generally first hole on cliff, then load explosive.But, because cliff in field environment is very high, large and the surface topography irregular of surface area, not only labour intensity is large to adopt Artificial Intervention explosive, efficiency is low, and danger is very high, explosive is implanted according to automation equipment, again because the operator on most of hole and equipment is apart from too far away, aperture pattern irregular easily and finger stone the reason such as to obscure and make the frequent neglected loading of equipment operator or the many holes of misloading, thus the automatic detection & localization device in exploitation blasting hole is needed, for operator provides the spatial positional information of blast hole, improve the accuracy that explosive is implanted, reduce the labour intensity of neglected loading rate and operator.

Summary of the invention

The invention provides a kind of device based on video camera and laser range sensor co-located blast hole and localization method, this device and method can provide the spatial positional information of blast hole for operator, improve the accuracy that explosive is implanted, reduce the labour intensity of neglected loading rate and operator.

The object of the present invention is achieved like this: a kind of device based on video camera and laser range sensor co-located blast hole, comprise mechanical arm, be provided with video camera and laser range sensor on a robotic arm, video camera is connected with computing machine by image pick-up card and vision module; Laser range sensor and mechanical arm are connected with computing machine by control module.

Described mechanical arm is also provided with displacement and speed pickup.

Described displacement and the signal transmission of speed pickup are to computing machine.

Utilize said apparatus to locate the method for blast hole, its positioning step is as follows:

1) blast hole search: utilize video camera to search for the hole on cliff, if vision module judges in shooting area porose in search procedure, then obtain the image in hole, and segmentation in the ken, to lock each hole be current goal hole;

2) space line obtains: processed by the image of the image processing software in vision module to hole, obtain the two-dimensional image coordinate in hole, again in conjunction with the camera system parameter matrix of on-site proving before life's work, can in the hope of being pointed to the space line at target hole center by camera coordinates initial point;

3) target hole range finding: control module sends instruction adjustment laser range sensor alignment procedures 2) in the direction of space line that obtains find range, the data obtained are the distance between target hole and laser range sensor;

4) three-dimensional coordinate obtains: by step 2) in the space line that obtains and step 3) in distance parameter synthesis after be converted in mechanical arm base reference frame further, resulting in the 3 d space coordinate of target hole, complete the location of cliff blast hole, machinery can be guided to plant personally into explosive.

Step 1)-4) coordinate data of moving in process all with mechanical arm base for reference coordinate system, need before life's work to demarcate video camera at the scene; By installation displacement on a robotic arm, speed pickup Real-time Obtaining video camera, laser range sensor and the arm end topworks locus relative to this reference frame in the course of work, and then other desired relative coordinate date can be obtained, obtain the relative distance between arm end effector and target hole in all directions simultaneously.

When utilizing video camera to search for the hole on cliff, if occur with the situation photographing multiple hole in piece image, a locking wherein hole is current goal hole.

Step 2)-4) in the method that obtains of space line, three-dimensional coordinate be:

Hole in the image in the hole obtained is split, and to lock a hole be current goal hole, to Given Graph picture, known target hole two-dimensional coordinate I (u, v) in the picture, being first translated into video camera is the coordinate system C (X of reference, Y, Z) in, result is:

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=k\left[\begin{array}{cccc}{h}_{11}& h_{12}& h_{13}& h_{14}\\ h_{21}& h_{22}& h_{23}& h_{24}\\ h_{31}& h_{32}& h_{33}& h_{34}\end{array}\right]\left[\begin{array}{c}X\\ Y\\ \text{Z}\\ 1\end{array}\right]---\left(1\right)$

Wherein h ₁₁~ h ₃₄for known camera chain parameter, (1) formula is launched to obtain:

$\left\{\begin{array}{c}({\mathrm{uh}}_{31}-h_{11})X+({\mathrm{uh}}_{32}-h_{12})Y+({\mathrm{uh}}_{33}-h_{13})Z=h_{14}-{\mathrm{uh}}_{34}\\ ({\mathrm{vh}}_{31}-h_{21})X+({\mathrm{vh}}_{32}-h_{22})Y+({\mathrm{vh}}_{33}-h_{23})Z=h_{21}-{\mathrm{vh}}_{31}\end{array}\right.---\left(2\right)$

Through type (2) can determine the space line being pointed to target hole center by camera coordinates initial point, but straight length and the unknown of the distance between camera and hole, this distance can be measured by laser sensor and obtain, and namely obtains the three-dimensional coordinate of target hole in camera coordinate system C thus;

Further, camera coordinate system C (X, Y, Z) and mechanical arm base coordinate system O (X is supposed ₀, Y ₀, Z ₀) between space conversion matrix be P, each parameter wherein in P is recorded in real time by the displacement transducer being arranged on manipulator joint position, then camera coordinate system C can be converted in robot coordinate system O:

After this, after obtaining arbitrary target hole, system can automatically by above calculation procedure, and by target hole space coordinate conversion in mechanical arm base coordinate system, realize target hole is relative to the space orientation of mechanical arm base coordinate system.

Device based on video camera and laser range sensor co-located blast hole provided by the invention and localization method, by video camera, blast hole is located, range information is obtained again by laser range sensor, reach the object of blast hole being carried out three-dimensional localization, utilize mechanical arm can complete explosive and implant operation.The accuracy that the present invention has explosive implantation is high, and the feature that neglected loading rate is low, reduces the labour intensity of operator.

Accompanying drawing explanation

Below in conjunction with drawings and Examples, the invention will be further described.

Fig. 1 is structural representation of the present invention.

Fig. 2 is systematic schematic diagram of the present invention.

Fig. 3 is the blast hole image that the present invention obtains.

Embodiment

Structure of the present invention is as shown in Figure 1: a kind of device based on video camera and laser range sensor co-located cliff blast hole, comprise mechanical arm 7, mechanical arm 7 is provided with video camera 1 and laser range sensor 8, and video camera 1 is connected with computing machine 3 by image pick-up card 2 and vision module 4; Laser range sensor 8 and mechanical arm 7 are connected with computing machine 3 by control module 5.

Principle of work of the present invention is as follows: utilize the hole on video camera 1 pair of cliff to search for, if vision module 4 judges in shooting area porose in search procedure, then splits in the ken, locks each hole; If occur with when photographing multiple hole in piece image, then locking a certain hole is that explosive is implanted in order to mechanical arm in current goal hole.

Processed by the image of the image processing software in vision module 4 to hole, obtain the two-dimensional image coordinate in hole, again in conjunction with the camera system parameter matrix of on-site proving before life's work, can in the hope of being pointed to the space line at target hole center by camera coordinates initial point; Image processing software can complete Iamge Segmentation, morphological image process, image rectification, hole identification etc., utilizes existing program and algorithm to complete.

The control module 5 adjustment laser range sensor 8 that sends instructions is aimed at space line direction and is found range, the data obtained are the distance between target hole and laser range sensor 8, be converted in mechanical arm fixed reference frame further, resulting in the 3 d space coordinate of target hole, machinery can be guided to plant personally into explosive.

Move in above process move and coordinate data all with mechanical arm base for reference coordinate system, need before life's work to demarcate video camera at the scene; By installation displacement on a robotic arm, speed pickup Real-time Obtaining video camera, laser range sensor and the arm end topworks locus relative to this reference frame in the course of work, and then calculate the other desired relative coordinate date of acquisition by control software design, obtain the relative distance between arm end effector and target hole in all directions simultaneously.

The process obtaining blast hole two dimension and three-dimensional coordinate in the present invention is as follows:

Hole in the image in the hole obtained is split, and to lock a hole be current goal hole, to Given Graph picture, known target hole two-dimensional coordinate I (u, v) in the picture, being first translated into video camera is the coordinate system C (X of reference, Y, Z) in, result is:

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=k\left[\begin{array}{cccc}{h}_{11}& h_{12}& h_{13}& h_{14}\\ h_{21}& h_{22}& h_{23}& h_{24}\\ h_{31}& h_{32}& h_{33}& h_{34}\end{array}\right]\left[\begin{array}{c}X\\ Y\\ \text{Z}\\ 1\end{array}\right]---\left(1\right)$

Wherein h ₁₁~ h ₃₄for known camera chain parameter, (1) formula is launched to obtain:

$\left\{\begin{array}{c}({\mathrm{uh}}_{31}-h_{11})X+({\mathrm{uh}}_{32}-h_{12})Y+({\mathrm{uh}}_{33}-h_{13})Z=h_{14}-{\mathrm{uh}}_{34}\\ ({\mathrm{vh}}_{31}-h_{21})X+({\mathrm{vh}}_{32}-h_{22})Y+({\mathrm{vh}}_{33}-h_{23})Z=h_{21}-{\mathrm{vh}}_{31}\end{array}\right.---\left(2\right)$

Through type (2) can determine the space line being pointed to target hole center by camera coordinates initial point, but straight length and the unknown of the distance between camera and hole, this distance can be measured by laser sensor and obtain, and namely obtains the three-dimensional coordinate of target hole in camera coordinate system C thus;

Further, camera coordinate system C (X, Y, Z) and mechanical arm base coordinate system O (X is supposed ₀, Y ₀, Z ₀) between space conversion matrix be P, each parameter wherein in P is recorded in real time by the displacement transducer being arranged on manipulator joint position, then camera coordinate system C can be converted in robot coordinate system O:

After this, after obtaining arbitrary target hole, system can automatically by above calculation procedure, and by target hole space coordinate conversion in mechanical arm base coordinate system, realize target hole is relative to the space orientation of mechanical arm base coordinate system.

Be below an application example of the present invention:

As shown in Figure 3: target hole 1 diameter in shown in known Fig. 3 is 100mm, and its central point is (-180.32,720.60,1584.50) at the coordinate of mechanical arm base coordinate system, and unit is mm.

The two-dimensional coordinate of central point in image coordinate system recording blast hole 1 through detection system of the present invention is I (297.67,1552.45), can be calculated the space line of hole 1 central point in camera coordinate system according to formula (2) is X=-0.4227Z, Y=-0.1005Z; Further, the distance L=856.17mm between video camera and center, hole 1 is obtained, according to Z by laser sensor measurement ²=L ²-X ²-Y ²can Z=785.32mm be obtained, and then known X=-331.98mm, Y=-78.95mm, namely obtaining the coordinate of blast hole 1 central point in camera coordinate system is C (-331.98 ,-78.95,785.32).

Now, camera coordinates is P (150.00,800.00 relative to robot base transition matrix, 800.00), the coordinate of center in mechanical arm base coordinate system that (3) formula of substitution can obtain blast hole 1 is O (-181.98,721.05,1585.32).

To when result:

Contrasted from the actual coordinate at above blast hole 1 center and surving coordinate, maximum error between Two coordinate is 1.66mm (X-direction), therefore the present invention has higher measuring accuracy (especially for larger-diameter hole) and good realizability.

Claims (7)

1. the device based on video camera and laser range sensor co-located blast hole, it is characterized in that: comprise mechanical arm (7), mechanical arm (7) is provided with video camera (1) and laser range sensor (8), and video camera (1) is connected with computing machine (3) by image pick-up card (2) and vision module (4); Laser range sensor (8) and mechanical arm (7) are connected with computing machine (3) by control module (5).

2. a kind of device based on video camera and laser range sensor co-located blast hole according to claim 1, is characterized in that: described mechanical arm (7) is also provided with displacement and speed pickup.

3. a kind of device based on video camera and laser range sensor co-located blast hole according to claim 2, is characterized in that: described displacement and the signal transmission of speed pickup are to computing machine (3).

4. utilize the method for the device location blast hole in claim 1-3 described in arbitrary claim, it is characterized in that: its positioning step is as follows:

1) blast hole search: utilize video camera to search for the hole on cliff, if vision module (4) judges in shooting area porose in search procedure, then obtain the image in hole, and segmentation in the ken, to lock each hole be current goal hole;

2) space line obtains: processed by the image of image processing software to hole in vision module (4), obtain the two-dimensional image coordinate in hole, again in conjunction with the camera system parameter matrix of on-site proving before life's work, can in the hope of being pointed to the space line at target hole center by camera coordinates initial point;

3) target hole range finding: control module (5) sends instruction adjustment laser range sensor alignment procedures 2) in the direction of space line that obtains find range, the data obtained are the distance between target hole and laser range sensor;

4) three-dimensional coordinate obtains: by step 2) in the space line that obtains and step 3) in distance parameter synthesis after be converted in mechanical arm base reference frame further, resulting in the 3 d space coordinate of target hole, complete the location of cliff blast hole, machinery can be guided to plant personally into explosive.

5. method according to claim 4, is characterized in that: step 1)-4) coordinate data of moving in process all with mechanical arm base for reference coordinate system, need before life's work to demarcate video camera at the scene; By installation displacement on a robotic arm, speed pickup Real-time Obtaining video camera, laser range sensor and the arm end topworks locus relative to this reference frame in the course of work, and then other desired relative coordinate date can be obtained, obtain the relative distance between arm end effector and target hole in all directions simultaneously.

6. method according to claim 4, is characterized in that: when utilizing video camera to search for the hole on cliff, if occur with the situation photographing multiple hole in piece image, a locking wherein hole is current goal hole.

7. method according to claim 4, is characterized in that: step 2)-4) in the method that obtains of space line, three-dimensional coordinate be:

Hole in the image in the hole obtained is split, and to lock a hole be current goal hole, to Given Graph picture, known target hole two-dimensional coordinate I (u, v) in the picture, being first translated into video camera is the coordinate system C (X of reference, Y, Z) in, result is:

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=k\left[\begin{array}{cccc}{h}_{11}& h_{12}& h_{13}& h_{14}\\ h_{21}& h_{22}& h_{23}& h_{24}\\ h_{31}& h_{32}& h_{33}& h_{34}\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]---\left(1\right)$

Wherein h ₁₁~ h ₃₄for known camera chain parameter, (1) formula is launched to obtain:

$\left\{\begin{array}{c}({\mathrm{uh}}_{31}-h_{11})X+({\mathrm{uh}}_{32}-h_{12})Y+({\mathrm{uh}}_{33}-h_{13})Z=h_{14}-{\mathrm{uh}}_{34}\\ ({\mathrm{vh}}_{31}-h_{21})X+({\mathrm{vh}}_{32}-h_{22})Y+({\mathrm{vh}}_{33}-h_{23})Z=h_{21}-{\mathrm{vh}}_{31}\end{array}\right.---\left(2\right)$

Through type (2) can determine the space line being pointed to target hole center by camera coordinates initial point, but straight length and the unknown of the distance between camera and hole, this distance can be measured by laser sensor and obtain, and namely obtains the three-dimensional coordinate of target hole in camera coordinate system C thus;

Further, camera coordinate system C (X, Y, Z) and mechanical arm base coordinate system O (X is supposed ₀, Y ₀, Z ₀) between space conversion matrix be P, each parameter wherein in P is recorded in real time by the displacement transducer being arranged on manipulator joint position, then camera coordinate system C can be converted in robot coordinate system O:

After this, after obtaining arbitrary target hole, system can automatically by above calculation procedure, and by target hole space coordinate conversion in mechanical arm base coordinate system, realize target hole is relative to the space orientation of mechanical arm base coordinate system.