JP2013117392A - Positioning system, positioning method and program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a positioning system that can measure on a real-time basis the position and direction of a moving body by using a simpler device than a total station.SOLUTION: A positioning system 10 that measures the position and direction of a pile driver 1 comprises: a plurality of targets 30 installed on the pile driver 1 with preset positional relationships to a prescribed reference point installed on the pile driver 1 and having preset positional relationships to one another; and an LRF 20 that acquires positional information on a plurality of measurement points on the surface of each of the targets 30 by optically scanning the surface of each of the targets 30. On the basis of the positional information on the plurality of measurement points on the surface of each of the targets 30 acquired by the LRF 20, the central position of each of the targets 30 is estimated, and the position and direction of the pile driver 1 are estimated on the basis of the estimated central positions of the plurality of targets 30 and the preset positional relationships.

Description

  The present invention relates to a positioning system, a positioning method, and a program for measuring the position and orientation of a moving body.

  A system using a total station and a GPS having a tracking function is known as a system for performing pile placement work while measuring the placement position in real time (see, for example, Patent Document 1). In the system described in Patent Document 1, a plurality of reflectors are installed on a pile suspended by an offshore crane, and the center position of the pile is calculated by collimating with a plurality of total stations floating on the ocean. And measure the center position of the pile based on the measurement result.

  Further, there is known a laser range finder (optical scanning distance measuring device) that measures distances to a plurality of measurement points on a measurement object by scanning laser light (see, for example, Patent Document 2).

JP 2008-14877 A JP 2007-225434 A

  In the system described in Patent Document 1, although the measurement accuracy of the total station is sufficiently higher than the required accuracy of the pile placement position, the measurement accuracy of the GPS is not sufficient, so the required accuracy of the pile placement position is reduced. You may not be satisfied. Moreover, since the total station and GPS are expensive, it becomes a high-cost system.

  In addition, the laser range finder described above is used for measuring the distance to a plurality of measurement points on the surface of the measurement object or obtaining profile data of the measurement object, but the central axis of the columnar pile It is not used for the purpose of measuring the position of the feature point with respect to the contour of the measured object.

  In view of the above circumstances, the present invention provides a positioning system, a positioning method, and a program that can measure the position and orientation of a moving object in real time using a simpler measuring device than a total station or GPS.

  In order to solve the above problems, a positioning system according to the present invention is a positioning system that measures the position and orientation of a moving object, and has a predetermined positional relationship with respect to a predetermined reference point set on the moving object. In addition, a plurality of measurement objects installed on the movable body with a predetermined positional relationship with each other, and position information of a plurality of measurement points on the surface of each measurement object by scanning the surface of each measurement object with light An optical scanning distance measuring device for obtaining the contour of each measured object based on positional information of a plurality of measurement points on the surface of each measured object acquired by the optical scanning distance measuring device. A first positioning unit that estimates a position of a predetermined point in a predetermined positional relationship; positions of the predetermined points of the plurality of measured objects estimated by the first positioning unit; the plurality of measured objects; A default positional relationship with a given reference point; The position of the reference point and the position of the moving body in the rotation direction around the reference point are calculated based on the predetermined positional relationship between the plurality of measured objects, and the position of the reference point And a second positioning unit that estimates the position of the moving body based on the position of the moving body and estimates the direction of the moving body based on the position in the rotational direction.

  In the positioning system, four or more objects to be measured may be installed on the moving body, and at least one object to be measured may be installed in a blind spot area of the optical scanning rangefinder. The second positioning unit may include a predetermined position of the predetermined points of the two measured objects estimated by the first positioning unit, and the predetermined positions of the two measured objects and the predetermined reference point. Based on the positional relationship and the predetermined positional relationship between the two objects to be measured, the position of the reference point and the position of the moving body in the rotation direction around the reference point are calculated. Also good.

  In the positioning system, three or more objects to be measured are installed on the moving body, and all the objects to be measured are installed so that they are not always located in the blind spot area of the optical scanning rangefinder. The second positioning unit may be a predetermined position of the predetermined points of the three measured objects estimated by the first positioning unit, and the predetermined positions of the three measured objects and the predetermined reference point. And the position of the reference point and the position of the movable body in the rotational direction centered on the reference point are calculated based on the positional relationship of the three measured objects and the predetermined positional relationship of the three measured objects. May be.

  In the positioning system, the plurality of objects to be measured may be installed on the moving body so that their mutual distances are all different.

  In the positioning system, the measured object may be a cylindrical body, and the predetermined point may be a point on a central axis of the cylindrical body, and the first positioning unit uses a least square method or a maximum likelihood method. Then, an equation of a circle that approximates the positions of the plurality of measurement points may be obtained, and the position of the point on the central axis may be estimated based on the obtained equation of the circle.

  Further, the positioning system according to the present invention is a positioning method for measuring the position and orientation of a moving body, wherein a plurality of measured bodies have a predetermined positional relationship with respect to a predetermined reference point set on the moving body. And having a predetermined positional relationship with each other on the surface of each measured object by scanning the surface of each measured object with light by an optical scanning distance measuring device. A step of acquiring position information of a plurality of measurement points and a contour of each measurement object based on the position information of the plurality of measurement points on the surface of each measurement object acquired by the optical scanning distance measuring device. Estimating a position of a predetermined point having a predetermined positional relationship, a predetermined position of the predetermined point of the plurality of measured objects, and a predetermined number of the plurality of measured objects and the predetermined reference point The positional relationship and each other of the plurality of measured objects The position of the reference point and the position of the moving body in the rotation direction around the reference point are calculated based on a fixed positional relationship, and the position of the moving body is calculated based on the position of the reference point. Estimating, and estimating the direction of the moving body based on the position in the rotation direction.

  Further, the program according to the present invention includes a plurality of measured objects installed on the moving body with a predetermined positional relationship with respect to a predetermined reference point set on the moving body, and each with a predetermined positional relationship with each other, An optical scanning distance measuring device that acquires position information of a plurality of measurement points on the surface of each measurement object by scanning the surface of the measurement object with light, and measures the position and orientation of the moving object A program to be executed by a computer included in the positioning system, the contour of each measurement object based on positional information of a plurality of measurement points on the surface of each measurement object acquired by the optical scanning distance measuring device A position of a predetermined point in a predetermined positional relationship, a position of the estimated predetermined points of the plurality of measured objects, and a plurality of measured objects and the predetermined reference point With the default position Based on the predetermined positional relationship between the plurality of objects to be measured, the position of the reference point and the position of the moving body in the rotation direction around the reference point are calculated, and the position of the reference point And estimating the position of the moving body based on the position of the moving body and estimating the direction of the moving body based on the position in the rotational direction.

  According to the present invention, it is possible to measure the position and orientation of a moving body in real time using a simple measuring device as compared with a total station or GPS.

It is a perspective view which shows the structure of the positioning system which concerns on one Embodiment. It is a functional block diagram of a positioning system. It is a flowchart for demonstrating the process which calculates the center position of each target. (A) is a graph which shows the detection result of the profile of the one side of the circumferential surface in case the noise removal by a Kalman filter is not performed. (B) is a graph which shows the detection result of the profile of the one side of the circumferential surface at the time of performing the noise removal by a Kalman filter. It is a figure which shows the principle of the constant distance method. It is a figure which shows the principle of the method of estimating the center position of the circumferential surface from all the data of a cluster using the least squares method and the maximum likelihood method. It is a graph which shows the result of having estimated the center position of the target from the data acquired by LRF. It is a graph which shows the result of having estimated the center position of the target from the data which scanned and acquired the laser beam by LRF 10 times, and removed the noise component. It is a figure for demonstrating the principle which measures the position and direction of a pile driver. It is a figure for demonstrating the principle which measures the position and direction of a pile driver. It is a figure for demonstrating the principle which measures the position and direction of a pile driver. It is a figure for demonstrating the principle which measures the position and direction of a pile driver. It is a flowchart for demonstrating the process which measures the position and direction of a pile driver. It is a figure for demonstrating the method to convert XY coordinate to a polar coordinate. It is a figure which shows the structure of the positioning system which concerns on other embodiment. It is a flowchart for demonstrating the process which measures the position and direction of a pile driver. (A) is a side view which shows the positioning system which concerns on other embodiment, (B) is a BB arrow line view of (A). It is a perspective view which shows the state which is performing the initial setting and calibration of the position of a vertical axis using a laser pointer. (A) is a figure which shows the measurement point on the circumferential surface of a target in case LRF scans a laser beam once in the state where the turntable stopped. Moreover, (B) is a figure which shows the measurement point on the circumferential surface of the target at the time of implementing the high precision measurement which concerns on this embodiment. (A) is a graph which shows the result of having estimated the center position of the target from the data of the measurement point on the circumferential surface acquired by making LRF scan a laser beam 10 times in the state which stopped the turntable. . Further, (B) is a graph showing the result of estimating the center position of the target from the data of the measurement points on the circumferential surface acquired by scanning the LRF with the laser beam 10 times while rotating the turntable. .

  Hereinafter, an embodiment of the present invention will be described with reference to the drawings. FIG. 1 is a diagram illustrating a configuration of a positioning system 10 according to an embodiment. As shown in this figure, the positioning system 10 is a system for measuring the position and orientation of the pile driving machine 1 (position in the rotational direction around the vertical axis), and a laser range finder (hereinafter referred to as LRF) 20 and The four targets 30 installed in the pile driving machine 1 are provided.

  The LRF 20 scans the laser beam in the horizontal direction by rotating the laser distance meter around the vertical axis 20A, and measures the distance from the vertical axis 20A to a plurality of measurement points on the contour of the measured object. This is a line scan type single-axis optical scanning rangefinder. Here, the scanning of the laser beam is to sequentially trace the light receiving surface with the laser beam in a predetermined direction by sequentially changing the emission angle of the laser beam. The LRF 20 emits laser light from the light source every time it is rotated by a predetermined angle (for example, 0.25 °), and the light receiving unit receives the reflected laser light, and the contour of the measurement object is received from the vertical axis 20A. Measure the distance to the measurement point.

  Three targets 30 are installed on the body of the pile driving machine 1. The three targets 30 are cylindrical bodies having a radius r, and are supported by one end of a shaft body 34 to which a level 36 is attached. The target 30 and the shaft body 34 are arranged so that the respective central axes 30E and 34A are aligned on the same straight line. The level 36 is a device for confirming whether or not the posture of the shaft body 34 is vertical. The remaining one target 30 is a cylindrical body having a radius r and is arranged coaxially with the pile.

  Further, the circumferential surface 30F of the target 30 is finished with a low luminance so as to suppress irregular reflection of laser light. Thereby, the noise at the time of measuring the distance from the vertical axis 20A to the measurement point on the circumferential surface 30F can be reduced. Further, the color of the circumferential surface 30F is not a dark color such as black but a light color such as white in consideration of the reflectance of the laser beam.

  FIG. 2 is a functional block diagram of the positioning system 10. As shown in this figure, the positioning system 10 is installed in the cockpit of the pile driver 1 and a computer 12 for calculation processing connected to the LRF 20 by a wireless LAN or a communication cable. And a monitor 18 on which the direction and the driving position P of the pile are displayed. The computer 12 includes a storage unit 14 such as a non-volatile memory that stores a calculation processing program, an information processing unit (CPU) 40 that reads the program into a volatile memory and executes the calculation process, and an LRF 20 And an input unit (input interface) 16 for inputting distance information.

  The information processing unit 40 calculates the center position of each target 30 and the position and orientation of the pile driving machine 1 based on the center positions of the plurality of targets 30 (position in the rotational direction around the vertical axis). And a display control unit 402 that displays on the monitor 18 the position and orientation of the pile driving machine 1. The above program realizes the function of each unit of the information processing unit 40. The first positioning unit 41 includes a classification unit 42, a clustering unit 44, a noise removal unit 46, an estimation unit 48, and a determination unit 49.

  FIG. 3 is a flowchart for explaining the process of calculating the center position of each target 30. As shown in this flowchart, when the input unit 16 acquires distance information from the LRF 20 (step 1), the classification unit 42 has a circumferential surface 30F as a foreground region among the measurement points to which the distance information is transmitted from the LRF 20. The upper measurement points and the measurement points in the peripheral region of the circumferential surface 30F as the background region are sorted by the background difference method (step 2).

  In addition, the clustering unit 44 performs clustering based on the Eugrid distance using the shortest distance method on the measurement points determined to belong to the foreground area by the classification unit 42 (step 3).

  Moreover, the noise removal part 46 removes the noise component contained in the data of many measurement points clustered by the clustering part 44 using a Kalman filter (step 4). The details of the noise component removal method will be described later.

  Further, the estimation unit 48 uses an estimation method such as a constant distance method, a minimum two-row method, a maximum likelihood method, or the like, from a large number of measurement points clustered by the clustering unit 44 and noise components removed by the noise removal unit 46. The center position of 30 is estimated (step 5). The estimation of the center position by the estimation unit 48 is performed using an estimation method such as the least square method based on the profile data of the circumferential surface 30F of the target 30 input in advance, and is repeatedly calculated by the Newton method. Is used to obtain a convergence value. The estimation of the center position by the estimation unit 48 will be described later in detail.

  Further, the determination unit 49 determines whether the value calculated by the iterative calculation by the Newton method by the estimation unit 48 is less than the convergence value. If the value is less than the convergence value, the center point is determined. If so, the calculation is repeated again (steps 6 and 7).

  FIG. 4A is a graph showing a detection result of a profile on one side of the circumferential surface 30F when noise removal by the Kalman filter is not performed. FIG. 4B is a graph showing the detection result on one side of the profile of the circumferential surface 30F when noise removal by the Kalman filter is performed.

  Here, the light of the specular component reflected at both ends of the arc portion on one side of the circumferential surface 30F does not return to the LRF 20, but only the light of the irregular reflection component reflected at both ends of the arc portion returns to the LRF 20. For this reason, the signal noise ratio (S / N ratio) of the reflected light detected by the LRF 20 is increased. Therefore, when noise removal by the Kalman filter is not performed, the detection accuracy of distances to a large number of measurement points on the circumferential surface 30F deteriorates, and as shown in FIG. 4A, the circular arc of the circumferential surface 30F. The shape of both ends of the part collapses.

  Therefore, the reflected light intensity filter is used as a Kalman filter, and data having a reflected light intensity lower than a predetermined value is removed as a noise component. Thereby, the measurement accuracy of the distance to a large number of measurement points on the circumferential surface 30F can be improved, and as shown in FIG. 4 (B), the collapse of the shape of both ends of the arc portion of the circumferential surface 30F can be suppressed, More accurate profile data of the circumferential surface 30F can be obtained.

Next, details of the estimation method of the center position of the target 30 by the estimation unit 48 will be described.
First, the constant distance method will be described. FIG. 5 is a diagram illustrating the principle of the constant distance method. As shown in this graph, in the constant distance method, the positions (x _cl , y _cl ) of the centers (cluster centers) of many measurement points (indicated by black circles in the figure) clustered by the clustering unit 44 are obtained. A position shifted by a distance d from the cluster center to the opposite side of the vertical axis 20A along the straight line L connecting the vertical axis 20A and the cluster center is regarded as the center point (x _obj , y _obj ) of the circumferential surface 30F. .

Here, because of the measurement accuracy of the LRF 20, the cluster center point (x _cl , y _cl ) derived based on the measurement data of the LRF 20 does not coincide with the center of the circumferential surface 30F. Therefore, the above-mentioned parameter d is determined based on the radius r of the circumferential surface 30F, and the position shifted by the distance d from the cluster center to the opposite side of the vertical axis 20A along the straight line L connecting the vertical axis 20A and the cluster center. , The center point (x _obj , y _obj ) of the circumferential surface 30F.

The center point (x _obj , y _obj ) of the circumferential surface 30F is represented by the following (formula 1). Β is an angle between the straight line L connecting the vertical axis 20A and the cluster center and the X axis (where x ≧ 0).

  Here, in the constant distance method, a parameter d is set in accordance with a known contour profile of the measured object, and a predetermined point (hereinafter referred to as a feature point) having a predetermined positional relationship with the contour of the measured object. ), And has the convenience of being able to estimate feature points of various shapes of objects to be measured. However, the constant distance method is a method of estimating the approximate position of the feature point of the measured object based on the cluster center data and the parameter d. For this reason, the fixed distance method is inferior to the methods such as the least square method and the maximum likelihood method in which the feature points of the measurement object are estimated using all data of the cluster in terms of estimation accuracy.

Therefore, as shown in FIG. 6, the center position of the circumferential surface 30F is estimated from all the cluster data (x _a , y _a ), α = 1,..., N using the least square method and the maximum likelihood method. To do. Here, when the profile of the circular arc part, which is the measurement region of the circumferential surface 30F, is mathematically expressed, a circular equation represented by the following (Expression 2) is obtained. Therefore, the parameters (a, b, r) of the circle equation are estimated from all the data (x _a , y _a ), α = 1,. Here, a and b are the X coordinate and Y coordinate in the two-dimensional plane of the center point of the target 30, respectively, and r is the radius of the target 30.

First, the least square method will be described.
The circle least square method is a method for estimating parameters a, b, and r that minimize the square sum of errors J _LS expressed by the following (Equation 3).

In this embodiment, since the radius r of the target 30 is known, there are two parameters to be estimated, a and b, and the above (Equation 3) is a nonlinear equation represented by the following (Equation 4). .

Therefore, the parameters a and b are estimated using the Newton-Raphson method as an iterative solution of the nonlinear equation. At this time, the setting of the initial value u ₀ of the Newton-Raphson method is important. In this embodiment, the constant distance method estimated values (x _obj , y _obj ) represented by (Equation 1) are used as Newton · Set to the initial value u ₀ of the Raphson method.

As a procedure of the Newton-Raphson method, first, a point u ₁ = (a ₁ , b ₁ ) that gives a minimum value of a convex surface under quadratic approximation of a function J _LS (u ₀ ) substituted with an initial value u ₀ Ask for. In the next step, the minimum value u ₂ of the curved surface obtained by quadratic approximation of the function J _LS (u ₁ ) substituted with this point u ₁ is obtained. Finally, this is repeated until u _i converges. If u _i converges, that u _i gives the minimum value of J _LS (u).

The minimum value u _{i + 1} = (a _{i + 1} , b _{i + 1} ) of the downwardly convex curved surface obtained by quadratic approximation of the function J _LS (a _i , b _i ) substituted with u _i = (a _i , b _i ) 5).

Here, the gradient g _{LS, i} including the first-order derivative on the right side of (Expression 5) is expressed by the following (Expression 6) and (Expression 7).

_{The Hessian matrix HLS, i including the second derivative on the right side of (Expression 5) is expressed by the following (Expression 8), (Expression 9), and (Expression 10).}

That is, in the procedure of the least square method of a circle having a known radius r using the Newton-Raphson method, in the first step, the initial value u ₀ = (a ₀ , b ₀ ) is set to the above (formula 5) to (formula 10). ), The initial value H _{LS, 0 of} the Hessian matrix and the initial value g _{LS, 0 of the} gradient are calculated, and the minimum value of the convex curved surface is given by quadratic approximation of the function J _LS (u ₀ ). The point u ₁ = (a ₁ , b ₁ ) is calculated. In the next step, u ₁ = (a ₁ , b ₁ ) is substituted into the above (formula 5) to (formula 10), the Hessian matrix H _{LS, 1} and the gradient g _{LS, 1} are calculated, and the function A point u ₂ = (a ₂ , b ₂ ) that gives the minimum value of the convexly curved surface obtained by quadratic approximation of J _LS (u ₁ ) is calculated. This step is repeated until u _i finally converges.

Next, the maximum likelihood method will be described.
The maximum likelihood method of a circle is a method for estimating parameters a, b, and r that minimize J _ML represented by the following (formula 11).

Here, in this embodiment, since the radius r of the target 30 is known, the parameters to be estimated are two parameters a and b, and the above (formula 11) is a nonlinear equation expressed by the following (formula 12). .

Therefore, as in the case of the least square method, the parameters a and b are estimated using the Newton-Raphson method as an iterative solution of the nonlinear equation. At that time, the estimated value (x _obj , y _obj ) of the constant distance method expressed by the above (formula 1) is set to the initial value u ₀ of the Newton-Raphson method.

As a procedure of the Newton-Raphson method, first, a point u ₁ = (a ₁ , b ₁ ) that gives a minimum value of a lower convex curved surface obtained by quadratic approximation of a function J _ML (u ₀ ) substituted with an initial value u ₀ Ask for. In the next step, the minimum value u ₂ of the curved surface obtained by quadratic approximation of the function J _ML (u ₁ ) substituted with this point u ₁ is obtained. Finally, this is repeated until u _i converges. If u _i converges, that u _i gives the minimum value of J _LS (u).

Here, the (formula 12), the molecule is the sum of squares, since the denominator is also ^{_{(x α -a) 2 + (}} y α -b) 2, which is a function of the convex downward. Therefore, the minimum value u _{i + 1} = (a _{i + 1} , b _{i + 1} ) of the curved surface obtained by quadratic approximation of the function J _ML (a _i , b _i ) substituted with u _i = (a _i , b _i ) is expressed by the following (formula 13). It is represented by

Here, the gradient g _{ML, i} in the above (Expression 13) is expressed by the following (Expression 14) and (Expression 15).

However, A (a _i , b _i ) is expressed by the following (formula 16).

Further, the Hessian matrix H _{ML, i} in the above (Expression 13) is expressed by the following (Expression 17), (Expression 18), and (Expression 19).

That is, in the maximum likelihood method of a circle having a known radius r using the Newton-Raphson method, in the first step, the initial value u ₀ = (a ₀ , b ₀ ) is changed to the above (formula 13) to (formula 19). ), The initial value H _{LS, 0 of} the Hessian matrix and the initial value g _{LS, 0 of the} gradient are calculated, and the minimum value of the convex curved surface is given by quadratic approximation of the function J _LS (u ₀ ). The point u ₁ = (a ₁ , b ₁ ) is calculated. In the next step, u ₁ = (a ₁ , b ₁ ) is substituted into the above (formula 13) to (formula 19), the Hessian matrix H _{ML, 1} and the gradient g _{ML, 1} are calculated, and the function A point u ₂ = (a ₂ , b ₂ ) that gives the minimum value of the convexly curved surface obtained by quadratic approximation of J _LS (u ₁ ) is calculated. This step is repeated until u _i finally converges.

  FIG. 7 is a graph showing the result of estimating the center position of the target 30 from the data acquired by the LRF 20 using the three analysis methods described above. In this graph, the estimation result using the constant distance method is indicated by a solid line, the estimation result using the least square method is indicated by a broken line, and the estimation result using the maximum likelihood method is indicated by a chain line. The horizontal axis represents the distance (mm) from the vertical axis 20A to the center axis 30E of the target 30, and the vertical axis represents the estimated center position error (mm).

In this experiment, the laser beam is scanned once with respect to the circumferential surface 30F of the target 30 having a radius r = 250 mm using the LRF 20, and data (x _a , y) on the circumferential surface 30F is scanned. _a ), α = 1,..., N were acquired, and the center position of the target 30 was estimated from all the acquired data. In this experiment, white drawing paper having a high reflectance with respect to infrared light was attached to the circumferential surface 30F. In this experiment, the Kalman filter of the noise removing unit 46 is turned off, and the data of many measurement points includes a noise component whose reflected light intensity is smaller than a predetermined value.

  As shown in the graph of FIG. 7, the result of estimating the center position of the target 30 using the least square method and the maximum likelihood method is compared with the result of estimating the center position of the target 30 using the constant distance method. It was good. Further, when the center position of the target 30 is located 30 m away from the vertical axis 20A, the error of the estimated value by the least square method is about 3.5 cm, and the error of the estimated value by the maximum likelihood method is about 2.5 cm. .

  Here, in the present embodiment, since the radius r of the target 30 is known, the parameters estimated using the least square method and the maximum likelihood method are reduced to two, a and b. Thereby, the precision which estimates the center position of the target 30 is improved.

  FIG. 8 is a graph showing the results of an experiment conducted under conditions different from the above experiment. This experiment differs from the above-described experiment in that the laser beam is scanned 10 times with respect to the circumferential surface 30F of the target 30 by the LRF 20 and the data of a large number of measurement points by turning on the Kalman filter of the noise removing unit 46. This is a point where a noise component having a reflected light intensity smaller than a predetermined value is removed.

  As shown in the graph of FIG. 8, the value obtained by estimating the center position of the target 30 using the least square method and the maximum likelihood method is proportional to the distance from the vertical axis 20A to the measurement point. It can be seen that it is more stable than the estimated value shown in.

  This is because, when the center position of the target 30 is estimated from data obtained by scanning the laser beam only once, the influence of noise on the estimation result is increased, whereas the laser beam is scanned a plurality of times. This is because when the center position of the target 30 is estimated from the acquired data, robust estimation against noise is possible. Further, by using the Kalman filter as the reflection intensity filter, as shown in FIG. 4B, it is possible to suppress the collapse of the shape of both ends of the circular arc portion of the circumferential surface 30F, and more accurate profile of the circumferential surface 30F. This is probably because the data can be obtained.

  As described above, according to the positioning system 10 according to the present embodiment, the center position of the target 30 can be measured using the LRF 20 that is less expensive than the total station. Further, according to the positioning system 10 according to the present embodiment, the center position of the target 30 can be measured if it is within the scanning range of the laser beam of the LRF 20. As a result, the center position of the target 30 can be measured in real time, the center position of the moving target 30 can be measured, and a plurality of targets can be measured during a series of laser beam scans by the LRF 20. It is also possible to measure 30 center positions.

  9-12 is a figure for demonstrating the principle which measures the position and direction of the pile driving machine 1. FIG. As shown in these drawings, four targets 30 </ b> A to 30 </ b> D are installed at predetermined positions of the pile driving machine 1. The target 30 </ b> A is installed coaxially with the pile in front of the pile driving machine 1, and the target 30 </ b> B is installed on the left side of the pile driving machine 1. The target 30 </ b> C is installed on the right side of the pile driving machine 1, and the target 30 </ b> D is installed on the right rear part of the pile driving machine 1.

  Here, the four targets 30 </ b> A to 30 </ b> D are installed with a relative positional relationship determined in advance. That is, a predetermined reference point O (in this embodiment, a pentagonal centroid composed of the four points of the body of the substantially rectangular pile driving machine 1 in plan view and the center point of the target 30A) is the origin. The position coordinates of the targets 30A to 30D and the distance between the targets 30A to 30D are set in advance. And these data are memorize | stored in the memory | storage part 14 (refer FIG. 2) of the computer 12, and the 2nd positioning part 401 processes based on these data.

  Further, the targets 30A to 30D are installed at a height lower than the total height of the pile driving machine 1, and any one of the targets 30A to 30D has the laser beam L of the LRF 20 blocked by the pile driving machine 1. Located in a blind spot area where the position cannot be detected. For example, as shown by a solid line in the figure, in the state where the front portion of the pile driving machine 1 faces the LRF 20 and the target 30A is placed in the middle and the targets 30B and 30D are positioned on both sides thereof, the target 30C is positioned in the blind spot region. To do. Further, as shown by a chain line in the figure, when the target 30D is placed in the middle with the right side of the pile driving machine 1 facing the LRF 20 and the targets 30A and 30D are positioned on both sides thereof, the target 30B is positioned in the blind spot region. To do.

  When measuring the position and orientation of the pile driving machine 1, the laser beam L of the LRF 20 is scanned in the counterclockwise direction in the figure. For this reason, when the pile driving machine 1 is in the state shown in FIG. 9, the center positions of the targets 30B, 30A, and 30D are detected in the order of 30B → 30A → 30D. When the pile driving machine 1 is in the state shown in FIG. 10, the center positions of the targets 30A, 30D, and 30C are detected in the order of 30A → 30D → 30C.

Further, the distances L _AB , L _AC , L _AD , L _BC , L _BD , and L _CD between the targets 30A to 30D are set to different sizes. Accordingly, if the distances L _AB , L _AC , L _AD , L _BC , L _BD , and L _CD between the targets 30A to 30D are calculated from the position coordinates of the center points of the targets 30A to 30D, the distances correspond to the distances. Two targets 30 can be discriminated.

  Here, when the positions of the two targets 30 in the counterclockwise direction around the LRF 20 are interchanged, one of the targets 30 is located in the blind spot region of the LRF 20. For example, as shown in FIG. 9, when the target 30 </ b> B and the target 30 </ b> A are arranged in the counterclockwise direction around the LRF 20, as shown in FIG. 10, the order is reversed. The target 30B is located in the blind spot area of the LRF 20. That is, the positional relationship of the two targets 30 detected by the LRF 20 in the counterclockwise direction around the LRF 20 is determined. For this reason, two targets 30 detected by the LRF 20 can be discriminated based on the positional relationship.

The distance _{L AB, L AC, L AD} , L BC, L BD, where the error occurs in the measured value of _{L CD,} the distance _{L AB,} L _AC this _{error, L AD, L BC, L} BD, L _{In order} not to change the relative magnitude relationship of _CD , each of these distances L _AB , L _AC , L _AD , L _BC , L _BD , and L _CD are larger than this error. The distance is set.

Further, as shown in FIG. 11, when measuring the position and orientation of the pile driving machine 1, the position coordinates of the reference point O in the coordinate system (origin O ₀ (0, 0)) set in the measurement region ( X _P1 , Y _P1 ) is measured, and the angle (hereinafter referred to as declination) θ of the center point A (X _A1 , Y _A1 ) of the target 30A in the polar coordinate system with the reference point O (X _P1 , Y _P1 ) as the origin Measure. Here, the deflection angle θ is an angle with respect to the X axis of a straight line connecting the reference point O and the center point A of the target 30A.

  Here, as shown in FIG. 12, the two targets 30 may overlap with the LRF 20 in the front-rear direction. In this case, the LRF 20 is the front one of the two overlapping targets 30. The target 30 on the left side is detected.

  FIG. 13 is a flowchart for explaining the process of measuring the position and orientation of the pile driving machine 1. As shown in this flowchart, first, the laser beam is scanned by the LRF 20, and the position coordinates of any two center points of the targets 30A to 30D are detected (step 11). At this time, the first positioning unit 41 calculates the position coordinates of the center point of the target 30 detected first and second by the LRF 20 that scans the laser beam counterclockwise. For example, in the state shown in FIG. 9, the position coordinates of the center points of the targets 30B and 30A are calculated, and in the state shown in FIG. 10, the position coordinates of the center points of the targets 30A and 30D are calculated.

Next, the second positioning unit 401 calculates the distance between the center points of the two targets 30 calculated by the first positioning unit 41 (step 12), and the known data L _AB stored in the storage unit 14 , L _AC , L _AD , L _BC , L _BD , and L _CD are compared to determine the corresponding two targets 30 (step 13). For example, in the state shown in FIG. 9, _LAB is extracted as known data, and the targets 30B and 30A are determined. In the state shown in FIG. 10, _LAD is extracted as known data, and the targets 30A and 30D are determined.

  Here, as described above, the positional relationship of the two targets 30 detected by the LRF 20 in the counterclockwise direction around the LRF 20 is determined, and this positional relationship is stored in the storage unit 14. And the 2nd positioning part 401 discriminate | determines the two targets 30 detected by LRF20 based on the positional relationship information of the targets 30A-30D memorize | stored in the memory | storage part 14. FIG.

Next, the second positioning unit 401 calculates the position coordinates (X _P1 , Y _P1 ) of the reference point O from the positions of the center points of the two targets 30 (step 14). Here, the positional relationship between each target 30 and the reference point O is determined in advance and the information is stored in the storage unit 14, and the second positioning unit 401 determines the positional relationship between each target 30 and the reference point O. Based on the information, the position coordinates (X _P1 , Y _P1 ) of the reference point O are calculated.

Next, as shown in FIG. 14, the polar second positioning part 401, the X-Y coordinates origin and _O 0 (0,0), the reference point O is coincident with the origin _O 0 (0,0) (Step 15). Then, the second positioning unit 401 calculates the position coordinates (X _A1 −X _P1 , Y _A1 −Y _P1 ) of the center point A of the target 30A from the positions of the center points of the two targets 30, and Based on this, the deflection angle θ of the center point A of the target 30A is calculated (step 16).

Here, when the center point A of the target 30A is located in the second to fourth quadrants of the polar coordinates, the second positioning unit 401 converts θ in the equation 20 into −θ, π−θ, and π + θ, respectively. To calculate the deflection angle θ. The second positioning unit 401 determines that the center point A is located in the first quadrant when X _A1 −X _P1 ≧ 0 and Y _A1 −Y _P1 > 0, and X _A1 −X _P1 <0, Y _A1. When −Y _P1 ≧ 0, it is determined that the center point A is located in the second quadrant. In addition, the second positioning unit 401 determines that the center point A is located in the third quadrant when X _A1 −X _P1 ≦ 0 and Y _A1 −Y _P1 <0, and X _A1 −X _P1 > 0, Y _A1. When −Y _P1 ≦ 0, it is determined that the center point A is located in the fourth quadrant.

  Next, the display control unit 402 displays the position and orientation of the pile driving machine 1 on the monitor 18 based on the position coordinates of the reference point O calculated by the second positioning unit 401 and the deflection angle θ of the target 30A ( Step 17). Here, information on the driving position P of the pile is stored in advance in the storage unit 14 (see FIG. 2) of the computer 12, and the display control unit 402 sets the driving of the pile together with the position and orientation of the pile driving machine 1. The position P is displayed on the monitor 18 (see FIG. 2). In addition, you may make it notify with a sound or a voice that the pile driving machine 1 approached the placement position P. FIG. For example, the frequency of the sound may increase as the pile driving machine 1 approaches the placement position P, or the distance may be notified by voice.

  By repeatedly executing the above steps 11 to 17, the position and orientation of the pile driving machine 1 displayed on the monitor 18 are updated in real time, so that the operator of the pile driving machine 1 can execute the pile driving machine in real time. While confirming the position and orientation of 1, the pile can be moved to a predetermined placement position.

  FIG. 15 is a diagram illustrating a configuration of a positioning system 100 according to another embodiment. The positioning system 100 includes an LRF 20 and four targets 30 installed in the pile driving machine 1. Here, the LRF 20 and the four targets 30 are installed at a position higher than the overall height of the pile driving machine 1, and the four targets 30 are not located in the blind spot area of the LRF 20.

  Here, in the present embodiment, the positional relationship of the two targets 30 in the counterclockwise direction around the LRF 20 may be switched, and in this case, the specific two targets 30 out of the four targets 30 are replaced. Although it can be determined that it has been detected, it cannot be determined which target 30 each of the two targets 30 is. For example, in the state shown in FIG. 9 and the state shown in FIG. 10, since the positional relationship between the targets 30A and 30B is switched, it can be determined that the two detected targets are the targets 30A and 30B. It cannot be determined whether the target is the target 30A.

  Therefore, since the positional relationship is determined for the three targets 30, the position of the center point of the three targets 30 is detected, the distance between the center points of the three targets 30 is calculated, Three targets 30 are determined from the distance between the center points of the targets 30.

  FIG. 16 is a flowchart for explaining processing for measuring the position and orientation of the pile driving machine 1 in the present embodiment. As shown in this flowchart, first, laser light is scanned by the LRF 20, and the first positioning unit 41 calculates any three central positions of the targets 30A to 30D (step 101). Here, the first positioning unit 41 calculates the center position of the target 30 that is first to third detected by the LRF 20 that scans the laser light counterclockwise. For example, in the state shown in FIG. 9, the center positions of the targets 30B, 30A, and 30C are calculated, and in the state shown in FIG. 10, the center positions of the targets 30A, 30D, and 30B are calculated.

Next, the second positioning unit 401 calculates the distance between the center points of the three targets 30 calculated by the first positioning unit 41 (step 102), and the known data L _AB stored in the storage unit 14 , L _AC , L _AD , L _BC , L _BD , and L _CD are compared to determine the corresponding three targets 30 (step 103). For example, in the state shown in FIG. 9, L _AB , L _BC , and L _AC are extracted as known data, and the targets 30B, 30A, and 30C are determined. In the state shown in FIG. 10, L _AD , L _AB , and L _BD are extracted as known data, and the targets 30A, 30D, and 30B are determined.

Next, the second positioning unit 401 calculates the position coordinates (X _P1 , Y _P1 ) of the reference point O from the positions of the center points of the three targets 30 (step 104). Here, the positional relationship between each target 30 and the reference point O is determined in advance and the information is stored in the storage unit 14, and the second positioning unit 401 determines the positional relationship between each target 30 and the reference point O. Based on the information, the position coordinates (X _P1 , Y _P1 ) of the reference point O are calculated.

Next, as shown in FIG. 14, the second positioning unit 401 converts the reference point O into polar coordinates that coincide with the origin O ₀ (0, 0) (step 105). Then, the second positioning unit 401 calculates the position coordinates (X _A1 -X _P1 , Y _A1 -Y _P1 ) of the center point A of the target 30A from the positions of the center points of the three targets 30, and Based on this, the deflection angle θ of the center point A of the target 30A is calculated (step 106). As in the above embodiment, the second positioning unit 401 converts θ according to the quadrant where the center point A is located.

  Next, the display control unit 402 displays the position and orientation of the pile driving machine 1 on the monitor 18 based on the position coordinates of the reference point O calculated by the second positioning unit 401 and the deflection angle θ of the target 30A ( Step 107).

  By repeatedly executing the above steps 101 to 107, the position and orientation of the pile driving machine 1 displayed on the monitor 18 are updated in real time, so that the operator of the pile driving machine 1 can execute the pile driving machine in real time. While confirming the position and orientation of 1, the pile can be moved to a predetermined placement position.

  FIG. 17 (A) is a side view showing a positioning system 100 according to another embodiment, and FIG. 17 (B) is a view taken along the line BB in FIG. 17 (A). As shown in these drawings, the positioning system 100 includes an LRF 20, a turntable 50 that rotatably supports the LRF 20 around the vertical axis 20A, and a rotary base 50 that rotates and rotates the turntable 50 around the vertical axis 20A. An adjustment base 60 that is rotatably supported around the shaft, a tripod 70 that supports the adjustment base 60, an awning cover 72 that is supported by the tripod 70, and a laser pointer 80 that is attached to the top of the LRF 20 are provided. .

  The awning cover 72 is integrally formed with a bottom plate 72A sandwiched between the rotary table 50 and the adjustment table 60, a side plate 72B disposed on the back side of the LRF 20, and a top plate 72C disposed on the upper side of the LRF 20. Being done. The top plate 72 </ b> C of the sunshade cover 72 prevents direct sunlight from being applied to the LRF 20 and the laser pointer 80.

  The turntable 50 includes a drive unit 52 installed on the bottom plate 72A, and a turntable 54 that is rotatably supported by the drive unit 52 around the vertical axis 20A and is driven to rotate. The LRF 20 is installed on the rotary table 54, and the rotary table 54 and the LRF 20 are integrally rotated around the vertical axis 20A.

  The adjustment table 60 is a pan / tilt table, can rotate 360 ° around the vertical axis 20A, can also rotate around the horizontal axis, and the angle of the rotary table 50, the LRF 20 and the laser pointer 80 around the vertical axis 20A, And the angle around the horizontal axis is adjustable. The laser pointer 80 emits a laser beam Lp for detecting a reference position in parallel with the laser beam Lf emitted from the LRF 20 to the central portion in the scanning direction. That is, by rotating the adjustment base 60 around the vertical axis 20A, the optical axis of the laser light Lp emitted by the laser pointer 80 and the scanning range of the laser light Lf by the LRF 20 can be adjusted around the vertical axis 20A. . Further, by rotating the adjustment base 60 around the horizontal axis, the laser light Lp emitted from the laser pointer 80 and the laser light Lf emitted from the LRF 20 can be adjusted around the horizontal axis.

  FIG. 18 is a perspective view showing a state in which the initial setting and calibration of the installation position and installation angle of the vertical axis 20 </ b> A are performed using the laser pointer 80. As shown in this figure, a target 84 having a retroreflecting material 82 attached to the front surface is set at a predetermined reference position, and laser light Lp is emitted from a laser pointer 80 toward the retroreflecting material. The retroreflecting material 82 has a property of reflecting incident light in the direction of the light source. The relative position of the LRF 20 with respect to the target 84 is detected according to the luminance of the laser light Lp reflected by the retroreflecting material 82, the position of the tripod 70 is adjusted, and the pan angle and tilt angle of the LRF 20 are adjusted by the adjustment table 60. To adjust.

FIG. 19A is a diagram illustrating measurement points on the circumferential surface 30F of the target 30 when the LRF 20 scans the laser beam once with the rotary table 50 stopped. As plotted with “+” in this figure, the measurement points on the circumferential surface 30F are discrete with an interval corresponding to the angular resolution θ _f ° of the LRF 20.

FIG. 19B is a diagram illustrating measurement points on the circumferential surface 30F of the target 30 when the LRF 20 scans the laser beam a plurality of times while rotating the turntable 50. FIG. In this measurement, the LRF 20 scans the laser beam a plurality of times, and during that time, the drive unit 52 is smaller than the angular resolution θ _f ° (for example, 0.25 °) of the LRF 20 every time the laser beam is scanned once. The rotary table 54 is rotated by an angle θ _t ° (for example, 0.015 °). Thereby, as shown in FIG. 19B, data having a higher resolution than the angular resolution θ _f ° of the LRF 20 can be acquired, and more accurate profile data of the circumferential surface 30F can be acquired. Therefore, the center position of the target 30 can be estimated with higher accuracy.

  FIG. 20A shows the result of estimating the center position of the target 30 from the data of the measurement points on the circumferential surface 30F acquired by causing the LRF 20 to scan the laser beam 10 times while the turntable 50 is stopped. It is a graph which shows. FIG. 20B shows the result of estimating the center position of the target 30 from the data of the measurement points on the circumferential surface 30F acquired by causing the LRF 20 to scan the laser beam 10 times while rotating the turntable 50. It is a graph which shows. In these graphs, estimation results using the constant distance method are indicated by solid lines, estimation results using the least square method are indicated by broken lines, and estimation results using the maximum likelihood method are indicated by chain lines. The horizontal axis is the distance (mm) from the vertical axis 20A to the center axis 30E of the target 30, and the vertical axis is the estimated error (mm) of the center position of the target 30.

  As shown in the graph of FIG. 20A, from the data of the measurement points on the circumferential surface 30F obtained by causing the LRF 20 to scan the laser beam 10 times while the turntable 50 is stopped, The average error when the center position of the target 30 was estimated by the maximum likelihood method was 15.3 mm. On the other hand, as shown in the graph of FIG. 20B, from the data of the measurement points on the circumferential surface 30F obtained by causing the LRF 20 to scan the laser beam 10 times while rotating the turntable 50, the least square method and The average value of errors when the center position of the target 30 was estimated by the maximum likelihood method was 8.3 mm. Thus, it can be seen that the center position of the target 30 can be estimated with higher accuracy when the method for acquiring data of measurement points on the circumferential surface 30F according to the present embodiment is used.

  In the embodiment described above, the measurement target is the target 30 that is a cylindrical body and the feature point is the central axis 30E. However, for example, a polygonal prism or pyramid with a cross-section and a feature point as its central axis, a sphere and a feature point as its center point, a cuboid or cube, and a feature point as its center point As long as the feature point can be estimated from the measurement point on the contour, such as the above, it can be adopted as the measurement object.

  In addition, the cross section of the cylindrical body as the measurement target is not limited to a perfect circle, but may be an ellipse. The equation of a circle that approximates the positions of a plurality of measurement points using the least square method or the maximum likelihood method is the radius described above. The equation for a perfect circle of r is not limited to an elliptic equation. The analysis method is not limited to the constant distance method, the least square method, and the maximum likelihood method described in the above embodiment.

  Moreover, although the present invention has been described by taking the positioning systems 10 and 100 that measure the position and orientation of the pile driving machine 1 as an example, the positioning system that measures the position and orientation of other moving bodies such as an excavator and a crane can be used. The present invention can also be applied.

DESCRIPTION OF SYMBOLS 1 Pile driver (moving body), 10 positioning system, 12 computer, 14 memory | storage part, 16 input part, 18 monitor, 20 LRF, 20A vertical axis, 30 (30A-30D) Target (measuring object), 30E Central axis , 30F circumferential surface, 34 shaft body, 34A central axis, 36 level, 40 information processing section, 41 first positioning section, 42 classification section, 44 clustering section, 46 noise removal section, 48 estimation section, 49 determination section, 50 rotary table, 52 drive unit, 54 rotary table, 60 adjustment table, 70 tripod, 72 sun protection cover, 72A bottom plate, 72B side plate, 72C top plate, 80 laser pointer, 82 retroreflective material, 84 target, 100 positioning system, 401 2nd positioning part, 402 Display control part

Claims (7)

A positioning system that measures the position and orientation of a moving object,
A plurality of measured objects installed in the movable body with a predetermined positional relationship with respect to a predetermined reference point set in the movable body, and with a predetermined positional relationship with each other;
An optical scanning distance measuring device that acquires position information of a plurality of measurement points on the surface of each measurement object by scanning the surface of each measurement object with light;
Based on the position information of a plurality of measurement points on the surface of each measurement object acquired by the optical scanning distance measuring device, the position of a predetermined point having a predetermined positional relationship with the contour of each measurement object is determined. A first positioning unit to be estimated;
The positions of the predetermined points of the plurality of measured objects estimated by the first positioning unit, a predetermined positional relationship between the plurality of measured objects and the predetermined reference points, and the plurality of measured objects Based on each other's predetermined positional relationship, the position of the reference point and the position of the moving body in the rotation direction around the reference point are calculated, and the position of the moving body is calculated based on the position of the reference point. A second positioning unit that estimates a position and estimates a direction of the moving body based on a position in the rotation direction;
Positioning system with Four or more objects to be measured are installed on the moving body, and one object to be measured is installed so that it is always located in a blind spot area of the optical scanning distance measuring device,
The second positioning unit has a predetermined positional relationship between the positions of the predetermined points of the two measured objects estimated by the first positioning unit and the two measured objects and the predetermined reference point. The position of the reference point and the position of the moving body in the rotation direction around the reference point are calculated based on the predetermined positional relationship between the two measured objects. The positioning system described in. Three or more objects to be measured are installed on the movable body, and all the objects to be measured are installed so that they are not always located in the blind spot area of the optical scanning rangefinder,
The second positioning unit has a predetermined positional relationship between the positions of the predetermined points of the three measured objects estimated by the first positioning unit and the three measured objects and the predetermined reference point. The position of the reference point and the position of the moving body in the rotation direction around the reference point are calculated based on the predetermined positional relationship between the three measured objects. The positioning system described in.   The positioning system according to any one of claims 1 to 3, wherein the plurality of objects to be measured are installed on the moving body so that their mutual distances are all different. The object to be measured is a cylindrical body, and the predetermined point is a point on a central axis of the cylindrical body,
The first positioning unit uses a least square method or a maximum likelihood method to obtain a circle equation that approximates the positions of the plurality of measurement points, and based on the obtained circle equation, calculates the point on the central axis. The positioning system according to any one of claims 1 to 4, wherein the position is estimated. A positioning method for measuring the position and orientation of a moving object,
Installing a plurality of objects to be measured on the moving body so as to have a predetermined positional relationship with respect to a predetermined reference point set on the moving body and have a predetermined positional relationship with each other;
Acquiring the positional information of a plurality of measurement points on the surface of each measured object by scanning the surface of each measured object with light by an optical scanning distance measuring device;
Based on position information of a plurality of measurement points on the surface of each measurement object acquired by the optical scanning distance measuring device, the position of a predetermined point having a predetermined positional relationship with the contour of each measurement object is estimated. And steps to
The estimated position of the predetermined points of the plurality of measured objects, the predetermined positional relationship between the plurality of measured objects and the predetermined reference point, and the predetermined positional relationship between the plurality of measured objects. And calculating the position of the reference point and the position of the moving body in the rotation direction around the reference point, estimating the position of the mobile object based on the position of the reference point, Estimating the direction of the moving body based on the position in the rotational direction;
A positioning method comprising: A plurality of measured objects installed on the movable body with a predetermined positional relationship with respect to a predetermined reference point set on the movable body and with a predetermined positional relationship with each other, and the surface of each measured body are scanned with light. And an optical scanning distance measuring device that acquires position information of a plurality of measurement points on the surface of each measured object, and causes a computer included in the positioning system to measure the position and orientation of the moving object to execute the computer. A program,
Based on the position information of a plurality of measurement points on the surface of each measurement object acquired by the optical scanning distance measuring device, the position of a predetermined point having a predetermined positional relationship with the contour of each measurement object is determined. The estimation procedure;
The estimated position of the predetermined points of the plurality of measured objects, the predetermined positional relationship between the plurality of measured objects and the predetermined reference point, and the predetermined positional relationship between the plurality of measured objects. And calculating the position of the reference point and the position of the moving body in the rotation direction around the reference point, estimating the position of the mobile object based on the position of the reference point, A procedure for estimating the direction of the moving body based on the position in the rotation direction;
A program for causing the computer to execute.