JP6219043B2 - Boom tip position detection device - Google Patents

Description

  The present invention relates to a boom tip position detection device that detects a spatial tip position of a boom tip and a posture angle of the boom tip using a target.

  Conventionally, a length detection sensor for detecting the extension amount of a telescopic boom of a crane, a tilt angle detection sensor for the telescopic boom, and a tip end portion of the telescopic boom by geometric calculation using the swing angle detection sensor for the telescopic boom. There is known a boom tip position detecting device for obtaining the spatial tip position and the posture angle of the boom tip.

  As a similar technique according to the present invention, a fixed part is provided at the intersection of the T-shaped jig, a target is provided at the tip of the T-shaped bar, a camera is installed at the fixed part, and surveying as a target acquisition part A device that uses a machine to capture a target and determine the position and orientation angle of the camera is known (see Patent Document 1).

JP 2008-107085 A

By the way, the telescopic boom of a crane bends with the extension amount and the load of a suspended load. For this reason, a technique for accurately measuring the spatial tip position of the tip of the telescopic boom is desired. Even if it is not a telescopic boom, it may bend by the load of a suspended load, a wind, etc.
The present invention has been made in view of the above circumstances, and its object is to accurately detect the spatial tip position of the tip of the boom and the attitude angle of the tip of the boom. There is a need to provide a possible crane boom tip position detector.

  The crane boom tip position detection device according to the present invention includes a target used to detect a spatial tip position of a crane boom tip with respect to a reference coordinate system and a posture angle of the boom tip, and a reference of the target. A target acquisition unit that acquires the distance from the origin of the coordinate system and the direction in which the target exists, and the spatial tip position of the boom tip and the boom position using the distance and direction acquired by the target acquisition unit And a calculation unit that calculates the attitude angle of the tip.

  ADVANTAGE OF THE INVENTION According to this invention, the spatial front-end | tip position with respect to the reference coordinate system of the front-end | tip part of the boom of a crane and the attitude | position angle of the front-end | tip part of a boom can be calculated | required correctly.

  In particular, according to the invention described in claims 2 to 5, when a surveying instrument or a stereo camera is used for the target acquisition unit, it can be used for detecting the length of the boom of the crane. This eliminates the need to correct the deflection of the boom when calculating the working radius and lift of the crane. In addition, in the case where a monitoring camera is provided at the tip of the boom, the load area line can be accurately drawn on the screen of the monitoring monitor.

According to the sixth aspect of the present invention, since the target acquisition unit is provided on the base boom, it is easy to capture the target provided at the tip of the boom. According to invention of Claim 7, a target can be tracked automatically.
According to the eighth aspect of the present invention, a stereo camera provided at the tip of the boom can be used for monitoring the work site.

FIG. 1 is an explanatory diagram showing an outline of a crane according to a first embodiment of a boom tip position detection device of the present invention. FIG. 2 is a plan view schematically showing the crane shown in FIG. FIG. 3 is an explanatory view of a mark member attached to the distal end portion of the telescopic boom shown in FIG. 1, and is an explanatory view showing a state where the boom is bent. 4 is a plan view of the target mark member shown in FIG. FIG. 5 is an explanatory diagram showing an outline of a surveying instrument as a target mark acquisition unit used to acquire the target mark shown in FIG. FIG. 6 is a block circuit diagram showing the relationship between the target mark acquisition unit and the calculation unit shown in FIG. FIG. 7 is a flowchart for explaining the outline of the target mark acquisition procedure shown in FIG. FIG. 8 is a flowchart for explaining the details of the procedure for measuring the distance from target mark recognition to the target mark in the target mark acquisition procedure shown in FIG. FIG. 9 is an explanatory diagram showing the relationship between the optical axis of the laser distance measuring device and the screen center on the monitor. FIG. 10 is an explanatory diagram showing an example of vector calculation for obtaining a vector having a reference target mark as a start point and a tip position as an end point. FIG. 11 is an explanatory diagram for obtaining a position vector to each target mark based on the distance and direction to the target mark obtained by the target obtaining unit. FIG. 12 is an explanatory diagram for obtaining unit vectors orthogonal to each other using the position vectors shown in FIG. FIG. 13 is an explanatory diagram for obtaining unit vectors orthogonal to these unit vectors by the outer product from the unit vectors orthogonal to each other shown in FIG. FIG. 14 is an explanatory diagram for obtaining a coordinate transformation matrix using three unit vectors orthogonal to each other shown in FIG. FIG. 15 is an explanatory diagram for obtaining the direction vector of the tip position with respect to the reference coordinate system of the tip of the boom using the coordinate transformation matrix shown in FIG. FIG. 16 is an explanatory diagram showing an outline of a crane according to a second embodiment of the boom tip position detection device of the present invention. FIG. 17 is an explanatory view showing an outline of a crane according to Embodiment 3 of the boom tip position detection apparatus of the present invention. FIG. 18 is an explanatory view showing an outline of a crane according to Embodiment 4 of the boom tip position detection apparatus of the present invention.

Example 1
A crane boom tip position detection device according to a first embodiment of the present invention will be described below with reference to the drawings.
FIG. 1 is a side view of a mobile crane work vehicle according to a first embodiment of the present invention. FIG. 2 is a schematic view of the mobile crane work vehicle shown in FIG. 1 viewed from above.

  In FIGS. 1 and 2, reference numeral 10 indicates a mobile crane work vehicle (hereinafter referred to as a crane). The crane 10 includes a carrier 11 serving as a crane body, outriggers 12 and 13, a swivel base 14, a cabin 20, a bracket 15, a telescopic boom 16, and other components.

  The telescopic boom 16 has a base end portion rotatably provided on the bracket 15 via a support shaft 17. The telescopic boom 16 can stand up and down using a cylinder 18 with the support shaft 17 as a pivot point.

  The telescopic boom 16 includes a base boom 16A, an intermediate boom 16B, and a tip boom 16C. The base boom 16A, the intermediate boom 16B, and the tip boom 16C are configured to be nested. The telescopic boom 16 can be expanded and contracted using a cylinder (not shown).

  The tip boom 16C is provided with a sheave (not shown). A wire W is hung on the sheave, and a hook block 19 is suspended from the tip of the wire W. A hook 21 is attached to the hook block 19. The wire W can be wound and fed out by a winch (not shown).

  A fixed rod 30 shown in an enlarged manner in FIG. 3 is provided at the distal end portion 16C 'of the distal end boom 16C so as to protrude from the side surface of the distal end portion 16C'. Three target mark members 31, 32, and 33 are attached to the fixing rod 30. In FIGS. 1 and 2, the target mark members 31 to 33 are not shown.

As shown in FIGS. 3 and 4, target marks (also referred to as targets) 31M to 33M are drawn on the target mark members 31 to 33, respectively. In this embodiment, the target mark 31M is used as a reference mark. The target mark 32M and the target mark 33M are a center line OL1 connecting the center O2 of the target mark 32M and the center O1 of the target mark 31M, and a center line connecting the center O3 of the target mark 33M and the center O1 of the target mark 31M. OL2 is drawn on the target mark members 32 and 33 so as to be orthogonal to each other.

The target marks 31M to 33M are a spatial tip position Q (Qx, Qy, Qz) with respect to a reference coordinate system of the tip portion 16C ′ of the telescopic boom 16 of the crane shown in FIG. The yaw angle Ψ around the Y axis in the coordinate system, the roll angle Φ around the Z axis in the reference coordinate system, and the pitch angle θ around the X axis in the reference coordinate system are used to calculate by vector calculation. The reference coordinate system will be described at the time of vector calculation by the calculation unit described later.

  As schematically shown in FIGS. 1 and 2, a target acquisition unit 40 that acquires the distance from the origin of the reference coordinate system of the target marks 31M to 33M and the direction in which the target marks 31M to 33M exist is provided on the ground. It has been.

  Here, the target acquisition unit 40 is configured by a surveying instrument having a distance measuring device 41 that measures the distance to the target marks 31M to 33M and a camera 42 that images the target marks 31M to 33M, as schematically shown in FIG. Has been.

  Here, the surveying instrument has a tripod base 43. The base of the tripod base 43 is provided with a base 44 for rotating the distance measuring device 41 and the camera 42 in the pan direction (horizontal direction), and a drive motor 45 ′ for driving the base 44. The rotation angle in the pan direction of the gantry 44 is detected by a pan angle detector 45 shown in FIG.

    The gantry 44 is provided with a support portion 46 that tilts the distance measuring device 41 and the camera 42. The support portion 46 is provided with a drive motor 47 ′ that tilts the distance measuring device 41 and the camera 42. The tilt angle (tilt angle) of the support 46 is detected by a tilt angle detector 48 shown in FIG. The support unit 46 is provided with a roll angle detector 49 that detects a rotation angle (roll angle) between the distance measuring device 41 and the camera 42 around the optical axis. The roll angle detector 49 is used to set a reference (leveling) for the orientation between the distance measuring device 41 and the camera 42.

The camera 42 has a function of imaging the target marks 31M to 33M. The distance measuring device 41 has a function of measuring the distance to the target marks 31M to 33M. Here, a known laser distance meter is used for the distance measuring device 41.
Thereby, the target acquisition unit 40 acquires the distance from the origin of the reference coordinate system to the target marks 31M to 33M and the direction in which the target marks 31M to 33M exist.

  The output signal from the target acquisition unit 40 is input to the calculation unit 50 as shown in FIG. This calculating part 50 may be provided in a surveying instrument, may be provided in a personal computer, and may be provided in the cabin 20 of a crane main body.

  The calculation unit 50 includes a video signal S1 from the camera 42, a distance measurement signal S2 from the distance measuring device 41, a pan angle detection signal S3 from the pan angle detector 45, and a tilt angle from the tilt angle detector 48. The detection signal S4 and the roll angle detection signal S5 from the roll angle detector 49 are input.

  The calculation unit 50 has a function of outputting a drive signal K1 for driving the drive motor 45 ′ and a drive signal K2 for driving the drive motor 47 ′, a function of outputting an image and a calculation result to the monitor monitor 51, and a target. Spatial tip position Q (Qx, Qy, Qz) and posture angle (yaw angle Ψ, roll angle Φ, pitch angle θ) of the tip 16C ′ of the telescopic boom 16 using the distance and direction acquired by the acquisition unit 40. It has a function to calculate.

(Outline of processing of the calculation unit 50)
The calculation unit 50 has a function as an image processing unit, and performs processing of recognizing the target marks 31M to 33M from the video of the target marks 31M to 33M captured using the camera 42 as shown in FIG. S.1).
Next, the calculation unit 50 causes the surveying instrument to automatically track the target marks 31M to 33M in response to the vertical tilt, extension and horizontal rotation of the telescopic boom 16 by the recognized target marks 31M to 33M. Drive signals K1 and K2 are output to drive motors 45 ′ and 47 ′ functioning as tracking drive units, respectively (S.2).

Next, the calculation unit 50 measures the distance to the target marks 31M to 33M, the pan angle, the tilt angle, and the roll angle (S.3). Next, the positions of the centers O1 to O3 of any one of the target marks 31M to 33M are calculated (S.4).
The calculation unit 50 determines whether or not the positions of all the centers O1 to O3 of the three target marks 31M to 33M have been calculated (S.5). When the positions of all the centers O1 to O3 of the three target marks 31M to 33M have not been calculated, drive signals K1 and K2 are output, and any one of the target marks 31M to 33M to be measured next is detected. The drive motors 45 'and 47' are driven so that the distance is measured (S.6). Then, the processing steps from S.1 to S.6 are repeated.

  When the positions of all the centers O1 to O3 of the three target marks 31M to 33M are calculated, the calculation unit 50 determines the tip position Q (Qx, Qy, Qz) of the telescopic boom 16 and the posture of the tip part 16C ′. The angles (θ, Ψ, Φ) are calculated (S.7).

(Details from image recognition to distance measurement of target marks 31M to 33M by calculation unit 50)
The calculation unit 50 geometrically calculates the positions of the centers O1 to O3 of the target marks 31M to 33M from the posture of the telescopic boom 16 of the crane so that the images of the target marks 31M to 33M are reflected in the image of the camera 42. Thus, the orientation of the camera 42 is roughly determined (S.11 in FIG. 8).

  The pattern of the target marks 31M to 33M stored in advance in the arithmetic unit 50 and the image of the target marks 31M to 33M captured by the camera 42 are compared by the pattern matching method, and the target marks 31M to 33M on the screen are compared. The position of the image is obtained (S.12 in FIG. 8).

  Next, the calculation unit 50 automatically sets the orientation of the camera 42 so that the positions of the centers O1 to O3 of the images of the target marks 31M to 33M are located in the vicinity of the position of the center O ″ of the screen 51a of the monitoring monitor 51, respectively. (S.13 in FIG. 8).

  Next, the size of the image of the target marks 31M to 33M is adjusted by image processing so that the images of the target marks 31M to 33M captured by the camera 42 have a predetermined size (S.14 in FIG. 8).

  Next, the direction of the surveying instrument (camera 42 and distance measuring device 41) is finely adjusted so that the position of the center O ″ of the screen 51a and the positions of the centers O1 to O3 of the target marks 31M to 33M are a predetermined distance. 8 (S.15 in Fig. 8) That is, on the screen G, as shown in Fig. 9, the distance Of between the optical axis center of the camera 42 and the optical axis center of the distance measuring device 41 corresponds to a predetermined distance. Make fine adjustments.

Next, the target marks 31M to 33M are irradiated with laser light, and the distances to the target marks 31M to 33M are measured. Next, the positions of the centers O1 to O3 of the target marks 31M to 33M are calculated from the direction of the surveying instrument (pan angle, tilt angle, roll angle) (S.16 in FIG. 8).
This procedure is then repeated for the target marks 31M to 33M to be measured. For example, measurement is performed in the order of target marks 31M, 32M, and 33M.

If a laser range finder that emits laser light with good coherency is used, the reflected light of the laser light irradiated on the target marks 31M to 33M is received, and the laser is applied to the centers O1 to O3 of the target marks 31M to 33M. By adjusting the direction of the surveying instrument so that the center of the light is positioned, the laser beams can be accurately irradiated to the centers O1 to O3 of the target marks 31M to 33M.
Further, if a distance measuring device in which the laser distance measuring device and the camera are integrated is used, collimation can be automated even more quickly (see Japanese Patent Laid-Open No. 10-47959).

(Vector calculation for determining the tip position Q (Qx, Qy, Qz) of the telescopic boom 16)
Here, it is assumed that the tip position Q (Qx, Qy, Qz) of the telescopic boom 16 is determined using vector calculation.
First, the determination of the vector ^→ PL from the center O1 of the target mark 31M to the tip position Q (Qx, Qy, Qz) of the telescopic boom 16 will be described with reference to FIG.

The vector from the origin O to the tip position Q (Qx, Qy, Qz) of the telescopic boom 16 with the center O1 of the target mark 31M as the origin ^→ PL is the target mark 31M to 33M attached to the telescopic boom 16 in this embodiment. Sometimes the direction and distance of the center O1 of the target mark 31M are measured using a surveying instrument in a state where the distal end portion 16C ′ of the telescopic boom 16 is not bent, and the direction of the distal end position Q (Qx, Qy, Qz) Although the description will be made assuming that the distance is obtained by measurement, it may be obtained from geometric data at the time of design.

For example, the surveying instrument is placed at an appropriate location, and the appropriate location is set as the coordinate origin O ′, and the position vector toward the center O1 of the target mark 31M ^→ PO1 and the tip position Q (Qx, Qy, Qz) of the telescopic boom 16 The position vector ^→ PQ is obtained by measurement.

The position vector ^→ PO1 can be obtained by detecting the distance to the center O1 of the target mark 31M, the tilt angle, the roll angle, and the pan angle. Similarly, the position vector ^→ PQ can be obtained by detecting the distance to the tip position Q of the telescopic boom 16 and the tilt angle, roll angle, and pan angle.

Accordingly, the tip position Q of the telescopic boom 16 from the origin at the center O1 of the target mark 31M as the origin of coordinates (Qx, Qy, Qz) position vector ^→ PL towards the be obtained by ^→ PL = ^→ PQ- ^→ PO1 it can.
This position vector ^→ PL is referred to as a vector for the tip coordinate system. This position vector ^→ PL is stored in a memory (not shown) of the calculation unit 50.
In general, the tip position Q of the telescopic boom 16 is bent by the load of the suspended load, the weight of the telescopic boom 16, the wind, etc., and therefore the tip position Q of the telescopic boom 16 may not be directly measured. In addition, the tip position Q may be present at a location where it cannot be visually recognized.
Therefore, the tip position Q (Qx, Qy, Qz) of the telescopic boom 16 is obtained indirectly.

(Vector calculation for indirectly obtaining the tip position Q of the telescopic boom 16)
11 to 15 show an example of the vector calculation.
In FIG. 11, for convenience of explanation, an appropriate portion of the surveying instrument is set as the origin O ′, the vertical direction with respect to the ground is the Z direction, and the directions orthogonal to each other in a plane orthogonal to the Z direction A three-dimensional orthogonal coordinate system having a direction and a Y direction will be described as a reference coordinate system.

However, if the positional relationship between the surveying instrument and the crane body 11 is known, the turning center OG (see FIG. 2) of the telescopic boom 16 of the crane body 11 can be used as the origin of the reference coordinate system.
First, the calculation unit 50 uses the distance and direction obtained by the measurement, as shown in FIG. 11, the position vector toward the respective centers O1, O2, O3 of the target mark 31M or the target mark 33M ^→ PO1, ^→ Find PO2, ^→ PO3. Next, as shown in FIG. 12, a unit vector ^→ YO from the center O1 of the target mark 31M to the center O2 of the target mark 32M is obtained.

This unit vector ^→ YO can be obtained by dividing the position vector difference ( ^→ PO2− ^→ PO1) by the absolute value (PO2−PO1) of the position vector difference.
That is, ^→ YO = ( ^→ PO2− ^→ PO1) / (PO2−PO1)
Similarly, a unit vector ^→ ZO from the center O1 of the target mark 31M to the center O3 of the target mark 33M is obtained.
This unit vector ^→ ZO can be obtained by dividing the position vector difference ( ^→ PO1− ^→ PO3) by the absolute value of the position vector difference (PO1−PO3).
In other ^{words, → ZO = (→ PO1- →} PO3) / (PO1-PO3)

Then, as shown in FIG. 13, and its unit vector ^→ YO and unit vector ^→ ZO by finding the outer product to obtain the unit vector ^→ XO perpendicular to both this unit vector ^→ YO and unit vector ^→ ZO.
That is, ^→ XO = ^→ YO × ^→ ZO
Note that the symbol “x” means an operation to cross product vectors.

Next, as shown in FIG. 14, the arithmetic unit 50 calculates a coordinate transformation matrix WTL for performing coordinate transformation from the tip coordinate system to the reference coordinate system using these unit vectors ^→ XO, ^→ YO, ^→ ZO.
As shown in FIG. 15, the position vector ^→ PQ from the origin O ′ of the reference coordinate system to the tip position Q is obtained by using the coordinate transformation matrix WTL and the position vector ^→ PL using the center O1 of the tip coordinate system as the coordinate origin. It can be obtained by calculation.
^→ PQ = WTL ・^→ PL
From this position vector ^→ PQ, the coordinate value (Qx, Qy, Qz) of the tip position Q with respect to the origin O ′ of the reference coordinate system can be obtained.
The spatial posture (θ, Φ, Ψ) of the distal end portion 16c ′ of the telescopic boom 16 is a matrix corresponding to the rotation of [Equation 1].
^[→ XO ^→ YO ^→ ZO] is general role as object pose representations of the pitch, because it is equivalent to the rotation matrix of the yaw angle can be easily obtained.

  Therefore, if the positional relationship between the origin O ′ and the turning center OG in this reference coordinate system is known, the tip position (Qx ′, Qy ′, Qz ′) with respect to the turning center OG can be obtained by coordinate conversion. Since the tip positions (Qx ′, Qy ′, Qz ′) thus determined are measured amounts including deflection, the telescopic boom 16 can be obtained by using the tip positions (Qx ′, Qy ′, Qz ′). The working radius can be obtained without correcting the deflection.

(Example 2)
In the second embodiment, a surveying instrument is provided on the carrier 11 as a crane body. Here, as shown in FIG. 16, the surveying instrument is provided on the swivel base 14 of the carrier 11. When the swivel base 14 is thus provided, the surveying instrument can be automatically tracked in the turning direction of the telescopic boom 16.

  In addition, when the surveying instrument as the target acquisition unit 40 is small, if the surveying instrument is provided in the base boom 16A, the surveying instrument is installed in the direction of acquiring the target marks 31M to 33M according to the change in the posture of the telescopic boom 16. The posture can be changed integrally. Thereby, the measurement time can be shortened.

(Example 3)
Instead of the surveying instrument, a stereo camera 41 ′ capable of measuring the distance by parallax may be used as schematically shown in FIG. The computing unit 50 obtains the distances from the target marks 31M to 33M acquired by the stereo camera 41 ′ to the target marks 31M to 33M. Therefore, according to the third embodiment, the distance to each of the target marks 31M to 33M can be obtained without using a laser distance measuring device.

  When the stereo camera 41 ′ is provided on the swivel base 14, the posture of the stereo camera 41 ′ can be integrally changed in the direction in which the target marks 31 </ b> M to 33 </ b> M are acquired in response to the change in the posture of the telescopic boom 16.

  Further, when the stereo camera 41 ′ is provided on the base boom 16A, the posture of the stereo camera 41 ′ is integrated in the direction in which the target marks 31M to 33M are acquired in accordance with the change in the posture of the telescopic boom 16, as in the second embodiment. Can be changed.

Example 4
In the fourth embodiment, the target marks 31M to 33M are provided on the turntable 14 as shown in FIG. The stereo camera 41 ′ is provided on the tip boom 16C as shown in FIG.
In the fourth embodiment, the distance from the origin of the camera coordinate system of the stereo camera 41 ′ to the target marks 31M to 33M and the direction in which the target marks 31M to 33M exist are obtained. The computing unit 50 uses the obtained distance and direction to determine the spatial posture angle (θ, Φ, Ψ) of the distal end portion 16C ′ of the telescopic boom 16 and the distal end position Q (Qx, Qy, Qz) is obtained by vector calculation.
In the fourth embodiment, the stereo camera 41 ′ is provided on the tip boom 16C. However, the present invention is not limited to this.
For example, a camera for imaging the target marks 31M to 33M and a laser distance meter are provided at the distal end portion 16C ′ of the telescopic boom 16, so that the distance from the camera coordinate system to the target marks 31M to 33M and the target marks 31M to 33M are You may comprise the target acquisition part 40 which acquires the existing direction.
In this case, the calculation unit uses the distance from the origin of the camera coordinate system to the target marks 31M to 33M and the direction in which the target marks 31M to 33M exist, and the spatial tip position Q ( Qx, Qy, Qz) and the posture angle (θ, Φ, Ψ) of the distal end portion 16C ′ of the telescopic boom 16 are calculated.

(Other)
The number of target marks may be one, and the posture of the distal end portion 16C ′ of the telescopic boom 16 may be detected by a known posture angle detection sensor (not shown).
Dedicated marks may be used for the target marks 31M to 33M, but the shape feature points of the distal end portion 16C ′ of the telescopic boom 16 may be extracted by image processing, and these feature points may be used as the target marks. .

16 ... telescopic boom 40 ... target acquisition unit 50 ... calculation unit 31M ... target mark

Claims (2)

A target used to detect a spatial tip position of the tip of the crane boom relative to a reference coordinate system and a posture angle of the tip of the boom;
A target acquisition unit for acquiring the distance from the origin of the reference coordinate system of the target and the direction in which the target exists;
A calculation unit that calculates the spatial tip position of the tip of the boom and the attitude angle of the tip of the boom using the distance and direction acquired by the target acquisition unit ;
The target acquisition unit is a surveying instrument having a distance measuring device for measuring a distance to the target and a camera for imaging the target, and the surveying instrument recognizes the target from an image acquired by the camera. And a tracking drive unit that automatically directs the distance measuring device from the target recognized by the image processing unit to the target,
A boom boom tip position detecting device, wherein the target acquisition unit is provided on a base boom that stands and falls . A target used to detect a spatial tip position of the tip of the crane boom relative to a reference coordinate system and a posture angle of the tip of the boom;
A target acquisition unit for acquiring the distance from the origin of the reference coordinate system of the target and the direction in which the target exists;
A calculation unit that calculates the spatial tip position of the tip of the boom and the attitude angle of the tip of the boom using the distance and direction acquired by the target acquisition unit ;
The target acquisition unit is a stereo camera, and the calculation unit obtains the distance and direction to the target with respect to the origin of the reference coordinate system by the target acquired by the stereo camera,
A boom boom tip position detecting device, wherein the target acquisition unit is provided on a base boom that stands and falls .