JP6677576B2 - Method and apparatus for estimating position / posture of suspended moving object using acceleration signal - Google Patents

Description

  The present invention accurately estimates the position and orientation when an object such as a measuring instrument or a camera is suspended using a wire or a string and vertically moved up and down using a signal of an acceleration sensor attached to the object itself. And a method and apparatus for performing the method.

  2. Description of the Related Art Conventionally, various techniques for measuring the shape of an object or the inner surface of a space have been proposed (for example, see Patent Literature 1, Patent Literature 2, and Patent Literature 3).

  The technology described in Patent Literature 1 is based on an optical motion capture system including a plurality of cameras and a motion measurement including a sensor module including a nine-axis sensor. Is provided, and the positional relationship of the optical marker with respect to the sensor module is stored. Then, the coordinate value of the hidden optical marker in the world coordinate system is represented by the positional relationship of the optical marker with respect to the sensor module, the coordinate value of the sensor module in the world coordinate system, and the rotation matrix acquired by the 9-axis sensor of the sensor module. Is estimated using the sensor module coordinate system calculated using the above and a transformation matrix between the world coordinate system.

  The technique described in Patent Literature 2 estimates a rotation axis direction of a stick unit during a playing operation based on an angular velocity, and determines a tip end of the stick unit based on an angular velocity excluding a longitudinal axis component of the stick unit. Is calculated, and the position of the stick unit in the world coordinate system after a predetermined time is calculated based on the estimated closest rotational axis direction and the calculated closest angular speed of the tip of the stick unit. Then, the moving distance is calculated based on the acceleration, and the position of the stick unit in the world coordinate system after a predetermined time is corrected based on the calculated position and the calculated moving distance.

  The technique described in Patent Literature 3 changes the viewpoint of an imaging unit, inputs an image of a measurement target, measures a first viewpoint, extracts a plurality of feature points of the input image, and measures a second viewpoint. Then, corresponding points corresponding to the feature points of the input image are extracted. Then, the gravitational direction and the angle around gravity at the two viewpoints are input, the posture of the imaging means at each viewpoint is calculated from the gravitational direction and the angle at the two viewpoints, and the first viewpoint and the second viewpoint are calculated from the posture values, the feature points, and the corresponding points. A gaze vector that passes through each feature point and corresponding point at the viewpoint is calculated, a translational motion vector from the first viewpoint to the second viewpoint is calculated based on the gaze vector, the feature point, and the corresponding point, and measured by the translational motion vector and the gaze vector. The three-dimensional shape of the object is calculated.

JP-A-2006-6415 JP 2013-213947 A JP-A-11-37736

  By the way, when calculating the position from the acceleration of the object, it is necessary to integrate the acceleration twice. For this reason, in the conventional method of estimating the position of the object using the acceleration signal, when integrating the obtained acceleration, the effects of signal noise, posture / speed errors, and the like are amplified, so that the position and the position are accurately estimated. It was difficult to estimate the posture.

  The techniques described in Patent Literatures 1 to 3 estimate the position of an object using various methods, but consideration is given to eliminating the effects of signal noise, attitude / speed errors, and the like. It was difficult to say that the position and orientation could be estimated with high accuracy.

  The present invention has been proposed in view of the above circumstances, and provides a method and an apparatus for estimating the position and orientation of a suspended moving object using an acceleration signal capable of estimating the position and orientation with high accuracy. The purpose is to:

  A method and apparatus for estimating the position and orientation of a suspended moving object using an acceleration signal according to the present invention have the following features in order to achieve the above object. That is, the position / posture estimation method of a suspended moving object using the acceleration signal according to the present invention is configured to move an object equipped with an acceleration sensor for measuring accelerations in three axial directions perpendicular to each other in an up-down direction, thereby obtaining an acceleration signal. Obtaining noise, removing noise from the obtained acceleration signal, performing speed correction, shaking correction, and rotation correction on the noise-removed acceleration signal, and detecting an object based on the corrected acceleration signal. And a step of estimating the position and orientation of the object.

  An apparatus for estimating the position and orientation of a suspended moving object using an acceleration signal according to the present invention includes: a measuring unit having an acceleration sensor for measuring accelerations in three axial directions perpendicular to each other; And estimating means for estimating the position and orientation of the measuring means based on the measurement signal received from the measuring means. The estimating means includes a filter unit for removing noise from the received measurement signal, and a correction unit for performing speed correction, shake correction, and rotation correction on the measurement signal filtered by the filter unit. It is assumed that.

  In the method and apparatus for estimating the position and orientation of a suspended moving object using an acceleration signal according to the present invention, the movable object may be a measuring device or a camera.

  According to the method and apparatus for estimating the position and orientation of a suspended moving object using the acceleration signal according to the present invention, the position and orientation of the suspended moving object are estimated using a three-axis acceleration sensor. By performing a noise removal process and a speed, swing, and rotation correction process of the acceleration signal received from the acceleration sensor, it is possible to estimate the position and orientation with high accuracy.

4 is a schematic flowchart of a method for estimating the position and orientation of a suspended moving object using an acceleration signal according to an embodiment of the present invention. 1 is a schematic block diagram of a position / posture estimation device for a suspended moving object using an acceleration signal according to an embodiment of the present invention. FIG. 4 is an explanatory diagram of local coordinates and global coordinates. Explanatory drawing which shows an example of the speed before correction and the speed after correction. FIG. 4 is an explanatory diagram showing a comparison between an estimation result of a vertical movement amount and an actual measurement result. FIG. 4 is an explanatory diagram illustrating an example of correcting a shake of an object. FIG. 3 is an explanatory diagram illustrating an example of correcting rotation of an object.

  Hereinafter, a method and an apparatus for estimating the position and orientation of a suspended moving object using an acceleration signal according to an embodiment of the present invention will be described with reference to the drawings. 1 to 7 illustrate a position / posture estimation method and an estimation device of a suspended moving object using an acceleration signal according to an embodiment of the present invention. FIG. 1 is a schematic flowchart of the position / posture estimation method. 2 is a schematic block diagram of a position / posture estimating device, FIG. 3 is an explanatory diagram of local coordinates and global coordinates, FIG. 4 is an explanatory diagram showing an example of a pre-correction speed and a post-correction speed, and FIG. FIG. 6 is an explanatory diagram showing a comparison between a result and an actual measurement result, FIG. 6 is an explanatory diagram showing an example of correction of shaking of an object, and FIG. 7 is an explanatory diagram showing an example of correction of rotation of the object.

<Outline of position / posture estimation method and estimation device of suspended moving object>
A method and apparatus for estimating the position and orientation of a suspended moving object according to an embodiment of the present invention perform a filtering process such as a low-pass filter or a Kalman filter on acceleration signal noise, and an error of the moving speed of the object. Make corrections. When an object is suspended using a wire, a string, or the like, the object is shaken (pendulum motion) or rotated (yawing). . This makes it possible to accurately estimate the position and orientation of the object.

  Note that the signal output from the acceleration sensor contains a gravitational component, a translational acceleration component, and a rotational motion component (centrifugal acceleration component, tangential acceleration component), and these signals must be separated by only one acceleration sensor signal. Can not. Therefore, in the present invention, it is assumed that a three-axis acceleration sensor (such as a 3D motion sensor) is used.

<Details of position / posture estimation method for suspended moving object>
The position / posture estimation method of a suspended moving object according to the embodiment of the present invention is to move an object equipped with an acceleration sensor that measures acceleration in three axes perpendicular to each other vertically as shown in FIG. Receiving (acquiring) the acceleration signal (S1), removing noise from the received (acquired) acceleration signal (S2), and performing speed correction, vibration correction, rotation on the acceleration signal from which the noise has been removed. It includes a step of performing correction (S3, S4, S5) and a step of estimating the position and orientation of the object based on the corrected acceleration signal (S6).

<Outline of position and orientation estimation device for suspended moving object>
As shown in FIG. 2, the position / posture estimating apparatus used in the above-described method for estimating the position / posture of a suspended moving object includes a measuring unit 11, a moving unit 12, and an estimating unit 13; 13 includes a filter unit 13a and a correction unit 13b.

  The measuring means 11 includes an acceleration sensor for measuring accelerations in three axial directions perpendicular to each other, and is attached to a suspended moving object. The moving unit 12 is a device for suspending and moving the measuring unit 11 in the vertical direction. For example, a device for hanging and unwinding a wire or a string suspending the measuring unit 11 and a wire or a string. Consists of

  The estimating unit 13 performs a filtering process and a correcting process based on the measurement signal received from the measuring unit 11 to estimate the position and the posture of the measuring unit 11. The estimating means 13 includes a filter unit 13a for removing noise from the received measurement signal, and a correction unit 13b for performing speed correction, shaking correction, and rotation correction on the measurement signal filtered by the filter unit 13a. I have. The filter unit 13a performs filter processing such as a low-pass filter or a Kalman filter on the acceleration signal noise. The correction unit 13b includes a computer and a program installed in the computer, and performs a predetermined calculation to perform speed correction, shake correction, and rotation correction.

<Method of estimating the amount of vertical movement>
As shown in FIG. 3, the local coordinate system of a certain object is xyz, and the global coordinate system is XYZ. When an acceleration sensor (measurement means 11) is attached to this object and moved, acceleration accompanying the movement is generated. Since the acceleration sensor is attached to the object itself, the measured acceleration is the acceleration α _x (t), α _y (t), α _z (t) in each axis direction of the local coordinate system xyz of the object. It is. Assuming that each axis of the local coordinate system of this object is rotated by θ _x , θ _y , θ _{z with} respect to the global coordinates, accelerations α _x (t), α _y (t), α _z ( t) is obtained as in the following equation (1) in consideration of the rotation of the object.

Since the measured acceleration signals α _x (t), α _y (t), and α _z (t) include noise, filter processing such as a low-pass filter or a Kalman filter is performed. Here, for the case where the object is moved up and down, attention is paid to the acceleration α _z (t) in the vertical Z direction. Also, it is assumed that the object is stopped before and after the movement starts. In this case, the moving velocity ν _z (t) of the object in the vertical Z direction (global coordinate system) is obtained by integrating the acceleration α _z (t) in the vertical Z direction shown in the above equation (1), by the following equation (2). ).

In the above equation (2), Δt is an acceleration measurement interval time, and g is a gravitational acceleration. Since the object moving speed ν _z (t) is considered to be in the stopped state before and after the start of the movement, the moving speed becomes zero at the end of the measurement (here, it is assumed that t = t _L ). Must. However, when the movement speed ν _z (t) is calculated from the acceleration actually measured by stopping the object before and after the start of movement using the above equations (1) and (2), the measurement end time (t = t _L) the moving speed [nu _z of (t _L) is not zero (see FIG. 4). This is because measurement errors accumulate in the process of accumulating the acceleration shown in the above equation (2).

Therefore, assuming that the measurement error follows a linear function, the following correction function H is obtained using the moving speed ν _z (t _L ) at the end of measurement (t = t _L ) obtained by the above equation (2). (T) is created (Equation (3) below). The following equation (3) is an equation relating to speed correction.

  Using this correction function H (t), the moving speed is corrected as in the following equation (4) so that the moving speed at the end of the measurement becomes zero. FIG. 4 shows an example of the moving speed before correction and the moving speed after correction.

  The vertical movement amount Z (t) of the object is obtained by integrating the corrected movement speed as in the following equation (5).

  FIG. 5 is an example in which the estimation result of the vertical movement amount obtained from the acceleration measured by the acceleration sensor (three-dimensional motion sensor; 3DMS) is compared with the result of measuring the vertical movement amount of the object by the total station. The vertical movement amount estimated from the 3DMS is almost the same as the measurement result of the total station, and the error is less than 1% for the lifting amount (movement amount) of about 2 m.

<Method of estimating the amount of horizontal movement>
When an object suspended using a wire, a string, or the like is moved up and down, the movement of the object can be regarded as a simple pendulum movement. Assuming that the horizontal movement amount is small relative to the length L (t) from the fulcrum of the pendulum to the object, the horizontal movement amounts x (t) and y (t) of the local coordinate system are each It is expressed by the following equation (6) using the rotation angles θ _x and θ _y of the shaft.

Further, the length L (t) of the pendulum changes by moving the object in the vertical direction. If the length from the fulcrum to the object before the start of measurement is L _ini , the length is expressed as the following equation (7). It is.

In the above equation (7), Z (t) is the amount of vertical movement shown in equation (4). When the object is hung by a wire, a string, or the like, the object before the start of movement slightly shakes due to, for example, the influence of wind or Coriolis force. Therefore, if the period T _ini of the pendulum before the movement is measured from the rotation angles θ _x , θ _y , θ _{z of} each axis, the initial length L _ini can be obtained from the period T _ini by the following equation (8). it can. Expressions (6), (7), and (8) are expressions relating to shake correction.

When an object is suspended using a wire, a string, or the like, the object rotates due to the kinking of the wire, the string, or the like. Therefore, the amount of rotation needs to be corrected. Assuming that the rotation (yaw) angle around the z-axis at this time is θ _z , the horizontal movement amounts X (t) and Y (t) in the global coordinate system are obtained as in the following equation (9). Equation (9) is an equation relating to yawing correction.

  FIGS. 6 and 7 show examples in which an acceleration sensor is mounted on the measurement device and the horizontal movement amount and the rotation (yaw) are respectively corrected. As is clear from FIGS. 6 and 7, according to the present invention, the swing and rotation (rotation) of the object can be appropriately corrected.

<Measurement means>
As described above, the measuring unit 11 includes an acceleration sensor (three-dimensional motion sensor; 3DMSS) portion attached to the moving object, a personal computer for processing the measured acceleration, and the like.

<Application example>
A method and an apparatus for estimating the position and orientation of a suspended moving object using an acceleration signal according to the present invention include, for example, an underground cavity of a mine abandoned mine abandoned after mining underground resources, and a space of a discarded underground facility. By measuring the shape quickly and accurately, the present invention can be applied to an apparatus and a method for smoothly performing a filler compounding design, a filler construction, and the like when performing a cavity filling work.

  Such an apparatus and method are described, for example, in JP-A-2015-230301. The measuring device for measuring the inner surface shape of a space described in Japanese Patent Application Laid-Open No. 2015-230301 is provided with a measuring unit, a position detecting unit, a storing unit, a moving unit, and a shape calculating unit. This measuring device is a device for measuring the inner shape of a space in various modes, and can measure the inner shape of a space such as a cavity, a tunnel, a water storage tank, and a sewer pipe existing in the ground.

  A method for measuring the inner surface shape of a space described in Japanese Patent Application Laid-Open No. 2015-230301 discloses a method for irradiating a laser beam into a space to be measured using a measuring device for the inner surface shape of a space having such a configuration. And a housing means in which the wide-angle lens, the image sensor, and the position detecting means are housed integrally. To measure the shape of the inner surface of the space, the housing means is moved in the space to be measured, and while detecting its position, the inner surface of the space is irradiated with laser light in an annular shape to form the inner surface of the space. The formed laser light image is formed on an imaging surface by a wide-angle lens and output as image data. Then, based on the output image data and the detected position data of the image data, the shape of the inner surface of the space is obtained by performing an arithmetic process.

<Advantageous effects of the present invention>
In the present invention, since only the signal of the acceleration sensor attached to the object itself is used, a place where a person cannot enter (for example, a narrow space or a dangerous place) or a place where radio waves such as GNSS cannot be received (for example, Even if it is indoors or underground, it is possible to remotely estimate the position and orientation of the object. In addition, since the position and orientation can be estimated with high accuracy, measurement can be performed with high accuracy by applying the present invention to a measuring device, a camera, or the like.

DESCRIPTION OF SYMBOLS 10 Position / posture estimation apparatus 11 Measuring means 12 Moving means 13 Estimating means 13a Filter section 13b Correction section

Claims (4)

A step of moving an object equipped with an acceleration sensor for measuring accelerations in three axes perpendicular to each other in the vertical direction to obtain an acceleration signal,
Removing noise from the obtained acceleration signal;
For the acceleration signal from which the noise has been removed, a step of performing velocity correction, vibration correction, and rotation correction,
Estimating the position and orientation of the object based on the corrected acceleration signal,
A position / posture estimation method of a suspended moving object using an acceleration signal.   The method according to claim 1, wherein the object is a measuring device or a camera. Measuring means having an acceleration sensor for measuring accelerations in three axes perpendicular to each other,
A moving unit that suspends the measuring unit and moves the measuring unit up and down,
Estimating means for estimating the position and orientation of the measuring means based on the measurement signal received from the measuring means,
The estimating means includes:
A filter unit for removing noise of the received measurement signal,
A position / posture estimating apparatus for a suspended moving object using an acceleration signal, comprising: a correction unit configured to perform speed correction, shaking correction, and rotation correction on the measurement signal filtered by the filter unit. .   The position / posture estimating device of a suspended moving object according to claim 3, wherein the object is a measuring device or a camera.

Images