JP5748174B2 - Method and apparatus for measuring relative posture of moving object - Google Patents

Description

  The present invention relates to a relative posture measuring method and apparatus for measuring the relative posture of first and second moving bodies.

When a plurality of moving bodies move within the area independently of each other, it is possible to perform control for preventing the plurality of moving bodies from colliding with each other, control for causing the preceding moving body to follow another moving body, and the like. is there.
Note that the plurality of moving bodies are, for example, mobile robots that transport articles, and can freely move within the work area.

  In order to perform the above-described control, the position and posture of the moving body are measured by the following method A, B, or C.

  In Method A, a speed sensor, a gyro sensor, or the like is installed on the moving body. The position of the moving body is obtained by time-integrating the speed measurement value obtained by the speed sensor, and the attitude of the moving body is obtained by time-integrating the angular velocity measurement value obtained by the gyro sensor.

  In Method B, an external sensor is mounted on the moving body. By this external sensor, the direction of the index existing at a known position outside the moving body and the distance to the index are measured. This measurement result is collated with map data. Based on this collation, the self position and posture of the moving body are obtained. For example, a three-dimensional laser radar or camera is used as the external sensor. In Patent Document 1 below, as a posture of a mobile object that is a spacecraft, a relative relationship between the traveling direction of the mobile object and the earth or the sun corresponding to the index is detected.

  In Method C, an image including the moving body is acquired by a measuring device (for example, a camera) installed outside the moving body, and the position and orientation of the moving body are obtained based on the image.

  In addition, the technique which can be used by embodiment of this invention mentioned later is described in the following patent documents 2-4.

Japanese Patent No. 2514987 JP 2009-33366 A JP 2007-3233 A JP 2011-28417 A

  However, it is difficult to accurately measure the posture of the position and posture of the moving body.

  In the method A described above, measurement accuracy of the posture of the moving body may be reduced due to integration of the measurement error of the gyro sensor.

  In the method B described above, it is necessary to recognize the index by a three-dimensional laser radar or a camera. Therefore, when the index cannot be set, the posture of the moving body cannot be measured.

  In the method C described above, in order to obtain the posture of the moving body, it is necessary to measure the three positions on the moving body by distinguishing them from each other by a measuring device. Therefore, when the distance between the moving body and the measuring device increases, it becomes difficult to measure the three positions on the moving body separately from each other.

  On the other hand, in the case of performing control for preventing two moving bodies from colliding with each other and control for causing the preceding moving body to follow another moving body, the relative postures of the two moving bodies may be obtained. That is, it is not always necessary to obtain the posture of each moving body viewed from a stationary coordinate system fixed to the area in which the moving body moves.

  Accordingly, an object of the present invention is to provide a method and apparatus that can accurately determine the relative postures of two moving bodies.

In order to achieve the above object, according to the present invention, there is provided a relative attitude measurement method for measuring the relative attitudes of the first and second moving bodies,
(A) The first moving body is provided with a first azimuth sensor, the second moving body is provided with a second azimuth sensor,
(B) A direction from the base point set for the first mobile body to the azimuth point set for the first mobile body at a distance from the base point is defined as a first reference direction, and the first direction sensor is Measure the azimuth θ ₁ of the second moving body relative to the first reference direction as seen from the first moving body,
(C) A direction from the base point set for the second mobile body to the azimuth point set for the second mobile body at an interval from the base point is defined as a second reference direction, and the second direction sensor is Measure the orientation θ ₂ of the first moving body relative to the second reference direction as seen from the second moving body,
(D) based on the measured orientation theta ₁ and the azimuth theta ₂ and the relative relationship between the first reference direction and a second reference direction determined as the relative orientation, relative orientation measuring method for a mobile body, characterized in that Provided.

According to a preferred embodiment of the present invention, the relative posture when performing (B) is the same as the relative posture when performing (C),
Based on the azimuth θ ₁ and the azimuth θ ₂ measured in (B) and (C), the relative relationship between the first reference direction and the second reference direction is geometrically calculated.

According to another embodiment of the present invention, in (D), the probability distribution of the position and orientation of the moving body is updated based on the azimuth θ ₁ and the azimuth θ ₂ ,
The relative relationship is estimated based on the updated probability distribution.

Preferably, the position and posture of the moving body are in an internal state,
Set the initial value of the internal state and set the initial value of the variance-covariance matrix of the internal state,
In (D), the Kalman filter using the probability distribution is used to update the internal state and the variance-covariance matrix based on the azimuth θ ₁ obtained in (B), and obtained in (C). The internal state and the variance-covariance matrix are updated based on the azimuth θ ₂ , the relative relationship is estimated from the updated internal state, or the updated internal state is set as the relative relationship.

In another preferred example, in (D),
Using the position and posture of the first moving body and the position and posture of the second moving body as one sample, generating a large number of samples that can be taken by the first and second moving bodies,
Based on _{one of the} azimuth θ ₁ and the azimuth θ ₂ , update the probability that each sample actually exists, and extract a sample whose updated probability is equal to or greater than a threshold value from the multiple samples,
For each of the extracted samples, the relative position and relative attitude of the first and second moving bodies when the other of the azimuth θ ₁ and the azimuth θ ₂ is measured are predicted based on the motion model of each moving body,
Updating the probability that each of the extracted samples actually exists based on the other of the azimuth θ ₁ and the azimuth θ ₂ and the predicted relative position and relative posture of the first and second moving bodies; Extract a sample whose updated probability is equal to or greater than a threshold value from the extracted sample,
Based on the extracted sample, the relative relationship is estimated.

In order to achieve the above object, according to the present invention, there is provided a relative posture measuring device for measuring the relative posture of the first and second moving bodies,
A first azimuth sensor provided on the first moving body, and a second azimuth sensor provided on the second moving body,
The direction from the base point set for the first mobile body to the azimuth point set for the first mobile body at an interval from the base point is defined as a first reference direction, and the first direction sensor Measure the orientation θ ₁ of the second moving body relative to the first reference direction as seen from the moving body,
The direction from the base point set for the second mobile body to the azimuth point set for the second mobile body at an interval from the base point is taken as a second reference direction, and the second direction sensor Measure the orientation θ ₂ of the first moving body relative to the second reference direction as seen from the moving body,
Based on the measured orientation theta ₁ and the azimuth theta ₂ and further comprising, relative orientation measurement of the moving body, characterized in that the computing unit for determining the relative relationship between the first reference direction and a second reference direction as the relative orientation An apparatus is provided.

According to the present invention described above, the first moving body is provided with the first direction sensor, the second moving body is provided with the second direction sensor, and the first direction sensor is viewed from the first moving body. Then, the azimuth θ ₁ of the second moving body with respect to the first reference direction is measured, and the second azimuth sensor determines the azimuth θ ₂ of the first moving body with respect to the second reference direction as viewed from the second moving body. Based on the measured azimuth θ ₁ and azimuth θ ₂ , a relative relationship between the first reference direction and the second reference direction is obtained as the relative posture.
In this method, the first azimuth sensor only needs to be able to recognize the azimuth of the second moving body, and it is not necessary to distinguish and measure the three positions on the second moving body. The same applies to the second orientation sensor.
Therefore, even if the distance between the first moving body and the second moving body is large, the relative posture can be obtained with high accuracy.

1 shows a relative attitude measurement device according to an embodiment of the present invention. (A) shows a two-dimensional coordinate system fixed to the first moving body, and (B) shows a two-dimensional coordinate system fixed to the second moving body. It is a flowchart which shows the relative attitude | position measuring method by embodiment of this invention. A three-dimensional coordinate system A fixed to the first moving body and a three-dimensional coordinate system B fixed to the second moving body are shown. (A) shows the orientations θ _1z and θ _1xy measured by the first orientation sensor, and (B) shows the orientations θ _2z and θ _2xy measured by the second orientation sensor.

  A preferred embodiment of the present invention will be described with reference to the drawings. In addition, the same code | symbol is attached | subjected to the common part in each figure, and the overlapping description is abbreviate | omitted.

FIG. 1 is a configuration diagram of a relative posture measuring apparatus 10 according to an embodiment of the present invention. The relative posture measuring device 10 measures the relative posture of the second moving body 5 with respect to the first moving body 3.
In the example of FIG. 1, the first and second moving bodies 3 and 5 travel on the road surface, and FIG. 1 is a plan view of the moving bodies 3 and 5 as viewed from above. In FIG. 1, a position detection device 7 (three-dimensional laser radar) for detecting the position of each moving body 3, 5 is provided at a fixed position other than the moving bodies 3, 5.

  The relative orientation measurement device 10 includes a first orientation sensor 9, a second orientation sensor 11, and a calculation unit 13.

The first orientation sensor 9 is provided on the first moving body 3. First direction sensor 9, as viewed from the first movable body 3, and measures the azimuth theta ₁ of the second moving body 5 with respect to the first reference direction D1. Here, the first reference direction D1 is a direction from the base point P1 set for the first moving body 3 toward the azimuth point P2 set for the first moving body 3 with an interval from the base point P1. is there. Preferably, the first reference direction D1 is a front-rear direction of the first moving body 3 and is a direction from the rear to the front of the first moving body 3. However, the first reference direction D1 may be a direction inclined by a known angle from the front-rear direction of the first moving body 3.

For example, the first azimuth sensor 9 includes a camera and an image processing device as follows. The camera is installed on the first moving body 3 so as to face the first reference direction D1, or to face a direction inclined by a known angle with respect to the first reference direction D1. The image processing apparatus obtains the azimuth θ ₁ where the second moving body 5 is located based on the positional relationship between the center position of the image captured by the camera and the second moving body 5. At this time, the image processing apparatus may specify the index provided on the second moving body 5 as the position of the second moving body 5 in the image. Preferably, one camera having an omnidirectional field of view is installed on the first moving body 3 or different directions so that images in all directions are obtained when viewed from the first moving body 3. It is preferable to install a plurality of cameras facing toward the first moving body 3.

Here, θ ₁ may be expressed by, for example, a two-dimensional coordinate system A fixed to the first moving body 3. For example, as shown in FIG. 2A, the two-dimensional coordinate system A has an origin that is a base point P1, and has an x axis and ay axis that are orthogonal to each other. This x-axis faces the horizontal direction and corresponds to the first reference direction D1. That is, the azimuth point P2 exists on the x axis. The y axis faces the horizontal direction orthogonal to the x axis.
In FIG. 2A, θ ₁ is an angle with respect to the first reference direction D1 (x axis) in FIG. 2A, and a direction shifted counterclockwise around the base point P1 is a positive value, and the base point P1 is the center. The azimuth | direction shifted in the clockwise direction is made into a negative value, and the maximum absolute value is made into (pi) (radian).

The second orientation sensor 11 is provided on the second moving body 5. The second azimuth sensor 11 measures the azimuth θ ₂ of the first moving body 3 with respect to the second reference direction D 2 as viewed from the second moving body 5. Here, the second reference direction D2 is a direction from the base point P3 set for the second moving body 5 toward the azimuth point P4 set for the second moving body 5 with an interval from the base point P3. is there. Preferably, the second reference direction D2 is a front-rear direction of the second moving body 5 and is a direction from the rear to the front of the second moving body 5. However, the second reference direction D2 may be a direction inclined by a known angle from the front-rear direction of the second moving body 5.

For example, the second orientation sensor 11 includes a camera and an image processing device as follows. The camera is installed on the second moving body 5 so as to face the second reference direction D2 or to face a direction inclined by a known angle with respect to the second reference direction D2. The image processing apparatus obtains an orientation θ _{2 in} which the first moving body 3 is located based on the positional relationship between the center position of the image captured by the camera and the first moving body 3. At this time, the image processing apparatus may specify the index provided on the first moving body 3 as the position of the first moving body 3 in the image. Preferably, one camera having an omnidirectional field of view is installed on the second moving body 5 or different directions so that images in all directions are obtained when viewed from the second moving body 5. It is preferable to install a plurality of cameras facing toward the second moving body 5.

Here, θ ₂ may be expressed by, for example, a two-dimensional coordinate system B fixed to the second moving body 5. For example, as shown in FIG. 2B, the two-dimensional coordinate system B has an origin that is a base point P3 and has an x axis and ay axis that are orthogonal to each other. The x axis faces the horizontal direction and corresponds to the second reference direction D2. That is, the azimuth point P4 exists on the x axis. The y axis faces the horizontal direction orthogonal to the x axis.
In FIG. 2B, θ ₂ is an angle with respect to the second reference direction D2 (x-axis), and a direction shifted counterclockwise around the base point P3 is a positive value, and the base point P3 is the center. The azimuth | direction shifted in the clockwise direction is made into a negative value, and the maximum absolute value is made into (pi) (radian).

For example, the following indicators may be used as the above-described indicators provided in the moving bodies 3 and 5.
In one example, the index is a light emitting diode whose luminance changes in a specific pattern as in Patent Document 2. In this case, the azimuth sensors 9 and 11 capture a moving image over a short set time using the camera as an image, and use the image processing device to indicate the position of the pixel whose luminance changes in a specific pattern in the image as an index. Specify as the position of.
In another example, the indicator is a geometric pattern as described in US Pat. In this case, the image processing devices of the azimuth sensors 9 and 11 store the same pattern as the geometric pattern in advance, and in the image obtained using the camera, the portion of the same pattern as the previously stored pattern is used as the index. Specify as the position of.

  The above-described indicators may be attached to the outer surfaces (for example, outer peripheral surfaces) of the first and second moving bodies 3 and 5 at intervals.

Calculation unit 13, the azimuth theta ₁ which the first direction sensor 9 is measured, based on the orientation theta ₂ and the first direction sensor 9 is measured, a first reference direction D1 relative to the second reference direction D2 Seeking a relationship. The obtained relative relationship is the relative posture of the second moving body 5 with respect to the first moving body 3. The relative relationship, for example, as shown in FIG. 1, represented by the angle theta _t between the first reference direction D1 and the second reference direction D2. This angle θ _t means an angle formed between the first reference direction D1 or a direction parallel to the direction D1 and the second reference direction D2 or a direction parallel to the direction D2.

  FIG. 3 is a flowchart showing a relative posture measuring method using the relative posture measuring apparatus 10 described above.

In step S <b> 1, the first direction sensor 9 is provided on the first moving body 3, and the second direction sensor 11 is provided on the second moving body 5.
In step S2, the first orientation sensor 9, as viewed from the first movable body 3, and measures the azimuth theta ₁ of the second moving body 5 with respect to the first reference direction D1.
In step S <b> 3, the second direction sensor 11 measures the direction θ < _b > ₂ of the first moving body 3 with respect to the second reference direction D <b> 2 as viewed from the second moving body 5.
In step S4, the arithmetic unit 13, based on the orientation theta ₁ and the measured orientation theta ₂ and to determine the relative relationship between the first reference direction and a second reference direction as the relative attitude.
Steps S2 and S3 may be performed when the moving bodies 3 and 5 are moving.

  Hereinafter, the calculation methods 1 and 2 by the calculation unit 13 performed in step S4 will be described in detail.

[Calculation method 1]
Calculation method 1 is employed when the relative posture when performing step S2 is the same as the relative posture when performing step S3. As such a case, the posture and position of the first and second moving bodies 3 and 5 may not change between the time when step S2 is performed and the time when step S3 is performed. As another case of performing calculation method 1, step S2 and step S3 may be performed at the same time.

Calculation unit 13, based on the orientation theta ₁ and the azimuth theta ₂ and measured respectively at steps S2 and S3, geometric first reference direction D1 relative relationship between the second reference direction D2 (the angle theta _t) It is calculated by an arithmetic operation. Here, it is assumed that the orientations θ ₁ and θ ₂ can be expressed by the above-described two-dimensional coordinate systems A and B as shown in FIG. In FIG. 1, D1p is a straight line parallel to the first reference direction and passing through the base point P3.
Based on the azimuth θ ₁ and the azimuth θ ₂ , the calculation unit 13 obtains θ _t as an angle between the first reference direction D 1 and the second reference direction D 2 by the following equation (1). In this equation (1), the units of θ ₁ , θ ₂ and θ _t are radians.

θ _t = θ ₁ −θ ₂ −π (1)

In the above description, the case where the first and second moving bodies 3 and 5 move on the road surface which is a two-dimensional plane has been described. Here, the directions θ ₁ and θ ₂ are expressed in the two-dimensional coordinate systems A and B. Represented. However, when the first and second moving bodies 3 and 5 move in the three-dimensional space, the calculation unit 13 is based on the orientations θ ₁ and θ ₂ expressed in the three-dimensional coordinate system. The angle formed by the first reference direction D1 and the second reference direction D2 is determined as follows.

  FIG. 4 shows three-dimensional coordinate systems A and B fixed to the moving bodies 3 and 5 in a space in which the moving bodies 3 and 5 move. Each three-dimensional coordinate system A, B has an x-axis, a y-axis, and a z-axis that are orthogonal to each other. The first reference direction D1 described above matches the x axis of the three-dimensional coordinate system A, and the second reference direction D2 described above matches the x axis of the three-dimensional coordinate system B.

In FIG. 4, the first azimuth sensor 9 measures the azimuth θ _1z and θ _1xy as the azimuth θ ₁ described above, and the second azimuth sensor 11 measures the azimuth θ _2z and θ _2xy as the azimuth θ ₂ described above. To do. In FIG. 4, P1 _xy is a point obtained by orthogonally projecting the origin P1 of the three-dimensional coordinate system A onto the xy plane of the three-dimensional coordinate system B, and P3 _xy represents the origin P3 of the three-dimensional coordinate system B. This is a point orthogonally projected onto the xy plane of the three-dimensional coordinate system A.

Also, the first reference direction D1 expressed by a vector _{v 1,} represents a first reference direction D2 vector _{v 2,} as the rotation matrix <sub>R t,

v 1</sub> = _R t v ₂

Suppose that In this case, the first reference direction D1 relative relationship between the second reference direction D2 can be represented by the above R _t. Here, R _t is, for example,

R _t = R _1z (π) R _1z (−θ _1z ) R _1xy (θ _1xy ) R _2xy (θ _2xy ) R _2z (θ _2z )

It is represented by
R _2z (θ _2z ) is a matrix that rotates each point in space by θ _2z around the z-axis of the three-dimensional coordinate system B. R _2xy (θ _2xy ) is a matrix that rotates each point in space by θ _2xy around the y-axis of the coordinate system B1 obtained by rotating the three-dimensional coordinate system B by R _2z (θ _2z ). R _{1xy (θ 1xy)} is the z-axis of the three-dimensional coordinate system A, the y-axis of the coordinate system A1 of rotating the coordinate system A only theta _1z, each point in space, rotated by theta _1Xy It is a matrix. R _1z (−θ _1z ) is a matrix that rotates each point on the space by −θ _1z around the z-axis of the three-dimensional coordinate system A. R _1z (π) is a matrix that rotates each point in space by π (radian) around the z-axis of the three-dimensional coordinate system A. Each rotation angle θ represents a magnitude, θ represents counterclockwise rotation when viewed from the positive direction of each axis, and −θ is clockwise when viewed from the positive direction of each axis. Indicates rotation.

[Calculation method 2]
The calculation method 2 is employed when the first azimuth sensor 9 and the second azimuth sensor 11 measure the azimuth θ1 and the azimuth θ2 described above at different times or at the same time.
In this case, the calculation unit 13 estimates the relative relationship using a Bayes filter (Bayes's theorem). That is, the calculation unit 13 updates the probability distribution of the position and orientation of the moving body based on the azimuth θ ₁ and the azimuth θ ₂ and estimates the relative relationship based on the updated probability distribution. Note that, when a Bayes filter is used, even when the relative relationship is not uniquely determined, the probable relative relationship can be determined using a probability distribution. In order to narrow down this probability distribution, the position and orientation of the first and second moving bodies 3 and 5 may be measured by other measuring means, and these measured values may be further used.

  Hereinafter, examples in which a Kalman filter and a particle filter are used as Bayes filters will be described in order.

(Kalman filter)
When a Kalman filter is used as the Bayes filter, the following is performed.

First, the position and posture of the moving body are set to an internal state. For example, the position and posture of the second moving body 5 in the coordinate system stationary with respect to the base point P1 and the azimuth point P2 set for the first moving body 3 are set as the internal state. That is, in the example here, the internal state is the relative position and relative posture of the second moving body 5 with respect to the first moving body 3.
The calculation unit 13 updates the internal state and the variance-covariance matrix of the internal state based on the direction θ ₁ measured by the first direction sensor 9 by the Kalman filter using the probability distribution, and the second direction A specific example in which the internal state and its variance-covariance matrix are updated based on the azimuth θ ₂ measured by the sensor 11 and the relative relationship is estimated from the updated internal state (relative posture), or described in detail below. As described above, the updated internal state is set as the relative relationship. When the Kalman filter is used, the probability distribution is represented by an internal state and a variance-covariance matrix.

It is preferable to repeat the measurement of the azimuth θ ₁ and the azimuth θ ₂ until the probability of the internal state indicated by the variance-covariance matrix is equal to or greater than the threshold value. In this case, every time the azimuth θ ₁ is measured, the calculation unit 13 updates the internal state and its variance-covariance matrix based on the azimuth θ ₁ by the Kalman filter, and every time the azimuth θ ₂ is measured. by Kalman filter updates the internal state and its covariance matrix based on the orientation theta _2. When the probability of the internal state indicated by the variance-covariance matrix is equal to or greater than the threshold value, the calculation unit 13 estimates the relative relationship from the latest internal state, or updates the internal state as the relative relationship. To do.

  Hereinafter, a specific example in the case where the Kalman filter is used will be described.

First, as shown in the following [Equation 1], as well as defining the internal state X _t, it defines the initial value of the internal state X _t, the initial covariance matrix CovX _t internal state X _t.

In [Expression 1], x, y, and z indicate the relative positions of the second moving body 5 as viewed from the first moving body 3 at present. That is, x, y, and z are coordinate values of the x-axis, y-axis, and z-axis that are orthogonal to each other in the three-dimensional coordinate system A fixed to the first moving body 3. The x axis of the three-dimensional coordinate system A corresponds to the first reference direction D1. That is, the origin of the three-dimensional coordinate system A is the base point P1, and the azimuth point P2 exists on the x-axis.
In [Expression 1], r _x , r _y , and r _z are angles (radians) that indicate the posture of the second moving body 5 as viewed from the first moving body 3 at present. That is, in the three-dimensional coordinate system A, the second moving body 5 is rotated by r _x from the reference posture around an axis parallel to the x axis and passing through the second moving body 5 (for example, the base point P3), Next, it is rotated by r _y around the axis parallel to the y axis and passing through the second moving body 5 (base point P3). Finally, the second moving body 5 (base point P3) parallel to the z axis is rotated. When it is rotated by _rz around the passing axis, the posture of the second moving body 5 as seen from the first moving body 3 is obtained. The reference posture is, for example, the posture of the second moving body 5 in which the direction of the second reference direction D2 is the same (parallel) as the direction of the first reference direction D1. Note that, according to the present invention, the orientation of the second moving body 5 viewed from the first moving body 3 may be expressed by changing the order of rotation or by using another Euler angle expression or a rotation matrix. In this case, formula (20) described later is also changed.

In this example, the first azimuth sensor 9 measures θ _1z and θ _1xy as θ ₁ .
As shown in FIG. _5A , θ _1z is in the first reference direction D1 (x axis) when viewed from the first moving body 3 in the three-dimensional coordinate system A with respect to the circumferential direction around the z axis. On the other hand, it is an azimuth (radian) in which the second moving body 5 exists. That is, θ _1z is a point P3 _{xy obtained} by orthogonally projecting the base point P3 of the second moving body 5 onto the xy plane in the three-dimensional coordinate system A and the origin (the above-mentioned base point P1) of the three-dimensional coordinate system A. Is a direction with respect to the first reference direction D1 (x-axis).
As shown in FIG. _5A , θ _1xy is a line connecting the base point P3 of the second moving body 5 and the origin (the above-mentioned base point P1) of the three-dimensional coordinate system A in the three-dimensional coordinate system A. Minute is an angle (radian) formed with the xy plane.

When θ _1z is measured, the calculation unit 13 updates X _t and CovX _{t according} to the following equations (2) and (3). Note that CovX _t represents a variance-covariance matrix of X _t (the same applies hereinafter).

_{X t = X t -K 1 (} Y 1 -θ 1z) ··· (2)

CovX _t = (I−K ₁ B ₁ ) · CovX _t (3)

Here, in the formula (2), the left side of _{X t} is _{X t} which is updated by the equation, the right side of the _{X t} is _{X t} immediately before updating the _{X t} by the equation. Further, in the equation (3), the left side of CovX _t is CovX _t updated by the equation, CovX _t on the right side is a CovX _t immediately before updating CovX _t by the equation. Moreover, in Formula (3), I shows a unit matrix.

K ₁ is a Kalman gain and is represented by the following equation (4).

K ₁ = CovX _t · B ₁ ^T · V ₁ ⁻¹ (4)

B ₁ is a partial differential matrix represented by the following equation (5) of [Equation 2], and its superscript “T” indicates a transposed matrix.
V _{1 in the} above equation (4) is expressed by the following equation, and the superscript “−1” indicates an inverse matrix.


V ₁ = B ₁ · CovX _t · B ₁ ^T + R (6)

Here, R is a variance value assumed as an orientation measurement error (the same applies hereinafter).

Y _{1 in the} above formula (2) is represented by the following formula.

Y ₁ = g ₁ (X _t ) = tan ⁻¹ (y / x) (7)

Once theta _1Xy is measured, the arithmetic unit 13, by the following equation (8) (9), and updates the _{X t} and CovX <sub>t.

X t = X t -K 2 (</sub> Y 2 -θ 1xy) ··· (8)

CovX _t = (I−K ₂ B ₂ ) · CovX _t (9)

Here, in equation (8), the left side of _{X t} is _{X t} which is updated by the equation, the right side of the _{X t} is _{X t} immediately before updating the _{X t} by the equation. Further, in the equation (9), the left side of CovX _t is _{X t} which is updated by the equation, CovX _t on the right side is a CovX _t immediately before updating CovX _t by the equation. Moreover, in Formula (9), I shows a unit matrix.

K ₂ is the Kalman gain is expressed by the following equation (10).

K ₂ = CovX _t · B ₂ ^T · V ₂ ⁻¹ (10)

B ₂ is a partial differential matrix represented by the following equation (11) of [Equation 3].
V _{2 in the} above formula (10) is represented by the following formula (12).

V ₂ = B ₂ · CovX _t · B ₂ ^T + R (12)

Y _{2 in the} above equation (8) is represented by the following equation (13) of [Equation 4].

In this example, as θ ₂ , the second orientation sensor 11 measures θ _2z and θ _2xy .
As shown in FIG. 5B, θ _2z is a frequency around the z axis in the three-dimensional coordinate system B having the x axis, the y axis, and the z axis orthogonal to each other and fixed to the second moving body 5. Regarding the direction, it is the azimuth (radian) in which the first moving body 3 exists with respect to the second reference direction D2 (x axis) when viewed from the second moving body 5. That is, θ _2z is obtained by _defining a point P1 _{xy obtained} by orthogonally projecting the base point P1 of the first moving body 3 on the xy plane and the origin of the three-dimensional coordinate system B (the above-described base point P3) in the three-dimensional coordinate system B. This is the direction with respect to the second reference direction D2 (x axis) of the connecting line segment.
As shown in FIG. 5B, θ _2xy is a line connecting the base point P1 of the first moving body 3 and the origin (the above-mentioned base point P3) of the three-dimensional coordinate system B in the three-dimensional coordinate system B. Minute is an angle (radian) formed with the xy plane.

When θ _2z is measured, the calculation unit 13 updates X _t and CovX _{t according} to the following equations (14) and (15).

_{X t = X t -K 3 (} Y 3 -θ 2z) ··· (14)

CovX _t = (I−K ₃ B ₃ ) · CovX _t (15)

Here, in the formula (14), the left side of _{X t} is _{X t} which is updated by the equation, the right side of the _{X t} is _{X t} immediately before updating the _{X t} by the equation. Further, in the equation (15), the left side of CovX _t is CovX _t updated by the equation, CovX _t on the right side is a CovX _t immediately before updating CovX _t by the equation. Moreover, in Formula (15), I shows a unit matrix.

K ₃ is the Kalman gain is expressed by the following equation (16).

K ₃ = CovX _t · B ₃ ^T · V ₃ ⁻¹ (16)

B ₃ is a partial differential matrix represented by the following equation (17) of [Equation 5].
V _{3 in the} above equation (16) is represented by the following equation (18).

V ₃ = B ₃ · CovX _t · B ₃ ^T + R (18)

Y _{3 in the} above equation (14) is represented by the following equation (19).

Y ₃ = g ₃ (X _t ) = tan ⁻¹ (y _B / x _B ) (19)

Here, x _B and y _B are an x-axis coordinate value and a y-axis coordinate value in the three-dimensional coordinate system B, respectively. Therefore, x _B and y _B are represented by coordinate values x, y, and z of the three-dimensional coordinate system A fixed to the first moving body 3 by the following equation (20) of [Equation 6].
In the equation (20), c and s represent a cosine function (cos) and a sine function (sin), respectively. In this equation (20), z _B is the coordinate value of the z axis in the three-dimensional coordinate system B.

Once theta _2xy is measured, the arithmetic unit 13, by the following equation (21) (22), and updates the _{X t} and CovX <sub>t.

X t = X t -K 4 (</sub> Y 4 -θ 2xy) ··· (21)

CovX _t = (I−K ₄ B ₄ ) · CovX _t (22)

Here, in the formula (21), the left side of _{X t} is _{X t} which is updated by the equation, the right side of the _{X t} is _{X t} immediately before updating the _{X t} by the equation. Further, in the equation (22), the left side of CovX _t is CovX _t updated by the equation, CovX _t on the right side is a CovX _t immediately before updating CovX _t by the equation. Moreover, in Formula (22), I shows a unit matrix.

K ₄ is a Kalman gain and is expressed by the following equation (23).

K ₄ = CovX _t · B ₄ ^T · V ₄ ⁻¹ (23)

B ₄ is a partial differential matrix represented by the following equation (24) of [Equation 7].
V _{4 in the} above equation (23) is represented by the following equation (25).

V ₄ = B ₄ · CovX _t · B ₄ ^T + R (25)

Y _{4 in the} above equation (21) is represented by the following equation (26) in [Equation 8].
X _B , y _B, and z _B in this equation are the x-axis coordinate value, the y-axis coordinate value, and the z-axis coordinate in the three-dimensional coordinate system B, respectively, and the above equation (20) ) By the coordinate values x, y, z of the three-dimensional coordinate system A fixed to the first moving body 3.

The internal state updated as described above is defined as the relative relationship (the relative posture). That is, r _x , r _y , r _z of the updated internal state X _{t is set as} the relative relationship.

  In the specific example described above, the three-dimensional coordinate system A and the three-dimensional coordinate system B are fixed to the first and second moving bodies 3 and 5, respectively, but any coordinate system may be used. For example, a stationary coordinate system may be used. With respect to this stationary coordinate system, the first and second moving bodies 3 and 5 move and change their postures. In this case, the internal state consists of 12 variables. Twelve variables are three variables indicating the position of the first moving body 3, three variables indicating the attitude of the first moving body 3, and three variables indicating the position of the second moving body 5. It consists of three variables indicating the attitude of the second moving body 5. Therefore, preferably, in addition to the azimuth measured by the first and second azimuth sensors 9 and 11, the measured value of the distance between the first and second moving bodies 3 and 5 and the movement in the stationary coordinate system. The internal state is updated by the Kalman filter using the measured values of the positions of the bodies 3 and 5. The relative relationship is estimated from the updated internal state. However, even if one or both of the measured value of the distance between the moving bodies 3 and 5 and the measured value of the position of the moving bodies 3 and 5 are not obtained, the internal state is updated by the Kalman filter, and the updated internal state is The relative relationship can be estimated.

(Particle filter)
When a particle filter is used as the Bayes filter, the following is performed.

(A) First, using the position and orientation of the first moving body 3 and the position and orientation of the second moving body 5 as one sample, the calculation unit 13 calculates the first and second moving bodies 3 and 5. Produces a large number of samples that can be taken. These samples may be represented by a coordinate system of a space in which the first and second moving bodies 3 and 5 move. That is, the first and second moving bodies 3 and 5 move with respect to this coordinate system. These samples are those at the time when one of the azimuth θ ₁ and the azimuth θ ₂ is measured.
(B) The computing unit 13 extracts a sample that matches _{one of the} azimuth θ ₁ and the azimuth θ ₂ from a large number of samples. That is, the calculation unit 13 acquires (updates) the probability that each sample actually exists based on _{one of the} azimuth θ ₁ and the azimuth θ ₂ and updates the probability distribution from a large number of samples based on the probability. Samples whose probability is equal to or greater than the threshold are extracted. The probability may be that the probability that each sample actually exists as the degree of matching with _{one of the} azimuth θ ₁ and the azimuth θ ₂ is higher, or one of the azimuth θ ₁ and the azimuth θ ₂ . It may be 1 for a sample that fits and 0 for a sample that does not.
(C) Next, the calculation unit 13 first and second moving bodies 3 and 5 when the other of the azimuth θ ₁ and the azimuth θ ₂ is measured for each sample extracted in the process (b). Are predicted based on a motion model of each of the moving bodies 3 and 5. That is, for each sample extracted in the process (b), the position and orientation of the first and second moving bodies 3 and 5 that are the samples are measured when the other of the azimuth θ ₁ and the azimuth θ ₂ is measured. And the relative position and relative attitude of the first and second moving bodies 3 and 5 when the other is measured based on the obtained value. To do.
(D) Furthermore, the calculation unit 13 extracts a sample that matches the other of the azimuth θ ₁ and the azimuth θ ₂ from the sample extracted in the process (b). That is, the calculation unit 13 determines that each of the extracted samples is actually based on the other of the azimuth θ ₁ and the azimuth θ ₂ and the predicted relative position and relative posture of the first and second moving bodies 3 and 5. Then, based on the updated probability, a sample having the probability equal to or higher than a threshold is extracted from the extracted sample. This probability may be higher as the degree of matching between the azimuth θ ₁ and the other azimuth θ ₂ is higher, or may be 1 for a sample that matches the other of the azimuth θ ₁ and the azimuth θ _2. Yes, for samples that are not, it may be zero.
(E) Thereafter, the calculation unit 13 estimates the relative relationship based on the sample extracted in (d). All of the samples are considered to have the same relative posture of the second moving body 5 with respect to the first moving body 3.

At the time of measuring _{one of the} azimuth θ ₁ and the azimuth θ ₂ or immediately after that, the distance between the first and second moving bodies 3 and 5 is measured. A sample that matches the distance may be selected, and a sample that matches _{one of the} azimuth θ ₁ and the azimuth θ ₂ may be extracted from the selected samples.
Similarly, the distance between the first and second moving bodies 3 and 5 is measured immediately after or at the time when the other of the azimuth θ ₁ and the azimuth θ ₂ is measured, and the calculation unit 13 performs the process (d). , A sample suitable for the distance is selected based on the predicted relative positions of the first and second moving bodies 3 and 5, and the azimuth θ ₁ and the azimuth θ ₂ are selected from the selected samples. A sample that fits the other of the above may be extracted.
The distance between the first and second moving bodies 3 and 5 may be measured by, for example, a laser distance meter installed on the first or second moving body 3 or 5. This laser distance meter may measure the distance between the first and second moving bodies 3 and 5 in the direction corresponding to the direction θ ₁ and the direction θ ₂ obtained immediately before this measurement time.

In the process (b), the calculation unit 13 may newly generate a sample close to the sample extracted in (b). That is, the calculation unit 13 may newly generate a sample in which the extracted sample is close to the position and orientation of the moving bodies 3 and 5. In this case, in the process (c), the calculation unit 13 also uses the first and second moving bodies 3 when the other _{one of the} azimuth θ ₁ and the azimuth θ ₂ is measured for each of the newly generated samples. 5 relative positions and relative postures are predicted based on the motion models of the moving bodies 3 and 5. Further, in the process (d), the calculation unit 13 performs the same as described above based on the relative position and the relative attitude predicted for each newly generated sample and the sample extracted in (b). From each newly generated sample and the sample extracted in (b), a sample that matches the other of the azimuth θ ₁ and the azimuth θ ₂ is extracted.

Every time the azimuth θ ₁ and the azimuth θ ₂ are repeatedly measured, and the azimuth θ ₁ and the azimuth θ ₂ are measured, the (a) is omitted, and the azimuth θ ₁ and the azimuth θ ₂ are (b) and (c). The process (d) may be performed. In this case, when a measurement direction theta ₁ and the azimuth theta ₂ again, the samples extracted by said immediately preceding (d), the orientation theta ₁ and the first and second at the time when one of the azimuth theta ₂ was measured The relative position and relative posture of the moving bodies 3 and 5 are predicted based on the motion model of each moving body 3 and 5. Next, in (b) again, based on these predicted relative positions and relative orientations, _{one of the} azimuth θ ₁ and the azimuth θ ₂ is extracted from the sample extracted in the previous (d), as described above. Extract a sample that conforms to Thereafter, the processes (c) and (d) are performed in the same manner as described above. In the process (e), the relative relationship is obtained based on the sample extracted in the last process (d).

[Control of moving body]
Next, movement control of each moving body will be described.

A control device is provided for each of the moving bodies 3 and 5, and the control device determines the moving speed of the moving body so that the moving device moves to the target position based on the position of the corresponding moving body 3 or 5. Control the direction of movement.
The said control apparatus may be installed in the corresponding mobile body 3 or 5. The moving bodies 3 and 5 are, for example, a moving device that moves on the road surface by traveling wheels or walking legs, a flying object that moves in the air, or a spacecraft that moves in outer space.

In order to perform such control, a position detection device 7 for detecting the position of the moving bodies 3 and 5 in a stationary coordinate system fixed in a space in which the moving bodies 3 and 5 move is provided. The position detection device 7 may be configured by appropriate means, but may be a three-dimensional laser radar or camera installed in a space in which the moving bodies 3 and 5 move as in the example of FIG.
Based on the position of the moving bodies 3 and 5 detected by the position detecting device 7 and the relative posture obtained by the relative posture measuring device 10, the control device of each moving body 3 and 5 determines the corresponding moving body 3 or 5. Control the speed and direction of movement.

  As such a moving body control method, for example, the following control methods 1 and 2 can be employed.

(Control method 1)
When the first moving body 3 is caused to follow the second moving body 5, the control is performed as follows. The control device of the first moving body 3 obtains the relative position of the second moving body 5 with respect to the first moving body 3 from the position of each moving body 3, 5 detected by the position detection device 7, and this relative position. Then, based on the relative posture obtained by the relative posture measuring device 10, the moving speed and moving direction of the first moving body 3 are set so that the first moving body 3 follows the second moving body 5 and moves. Control. On the other hand, the control device of the second moving body 5 is configured so that the second moving body 5 moves to the target position based on the position of the second moving body 5 detected by the position detection device 7. The moving speed and moving direction of the second moving body 5 are controlled.
In this case, the calculation unit 13 of the relative attitude measurement device 10 may be installed on the first moving body 3 and receives the direction θ ₂ measured by the second direction sensor 11 from the second direction sensor 11 by radio. The obtained relative posture is output to the control device of the first moving body 3.

(Control method 2)
In order to avoid a collision between the first moving body 3 and the second moving body 5, control is performed as follows. The control device of the first moving body 3 obtains the relative position of the second moving body 5 with respect to the first moving body 3 from the position of each moving body 3, 5 detected by the position detection device 7, and this relative position. Then, based on the relative attitude obtained by the relative attitude measuring device 10, the movement of the first moving body 3 is controlled so that the first and second moving bodies 3, 5 do not collide with each other. For example, the control device of the first moving body 3 determines whether there is a possibility that the first and second moving devices collide with each other based on the relative position and the relative posture, and there is this possibility. Is determined, one or both of the moving speed and the moving direction of the first moving body 3 are controlled so as to avoid this collision.
In this case, the calculation unit 13 of the relative attitude measurement device 10 may be installed on the first moving body 3 and receives the direction θ ₂ measured by the second direction sensor 11 from the second direction sensor 11 by radio. The obtained relative posture is output to the control device of the first moving body 3.

  The present invention is not limited to the above-described embodiment, and various changes can be made without departing from the scope of the present invention.

  In the above-described embodiment, each of the azimuth sensors 9 and 11 is configured by a camera and an image processing device, but may be configured by other means. For example, each of the azimuth sensors 9 and 11 may be configured by a laser radar.

3 First moving body, 5 Second moving body, 7 Position detecting device, 9 First azimuth sensor, 11 Second azimuth sensor, 13 Arithmetic unit, 10 Relative posture measuring device

Claims (5)

A relative posture measuring method for measuring a relative posture of first and second moving bodies,
(A) The first moving body is provided with a first azimuth sensor, the second moving body is provided with a second azimuth sensor,
(B) A direction from the base point set for the first mobile body to the azimuth point set for the first mobile body at a distance from the base point is defined as a first reference direction, and the first direction sensor is Measure the azimuth θ ₁ of the second moving body relative to the first reference direction as seen from the first moving body,
(C) A direction from the base point set for the second mobile body to the azimuth point set for the second mobile body at an interval from the base point is defined as a second reference direction, and the second direction sensor is Measure the orientation θ ₂ of the first moving body relative to the second reference direction as seen from the second moving body,
(D) the measured based on the orientation theta ₁ and the azimuth theta ₂ and the relative relationship between the first reference direction and a second reference direction determined as the relative attitude, relative orientation measuring method for a mobile body, characterized in that. In (D), the probability distribution of the position and orientation of the moving body is updated based on the azimuth θ ₁ and the azimuth θ ₂ ,
The relative posture measurement method for a moving body according to claim 1, wherein the relative relationship is estimated based on the updated probability distribution. The position and posture of the moving body is an internal state,
Set the initial value of the internal state and set the initial value of the variance-covariance matrix of the internal state,
In (D), the Kalman filter using the probability distribution is used to update the internal state and the variance-covariance matrix based on the azimuth θ ₁ obtained in (B), and obtained in (C). Updating the internal state and the variance-covariance matrix based on the azimuth θ ₂ , estimating the relative relationship from the updated internal state, or setting the updated internal state as the relative relationship. The method for measuring the relative posture of the moving body according to claim 2. In (D) above,
Using the position and posture of the first moving body and the position and posture of the second moving body as one sample, generating a large number of samples that can be taken by the first and second moving bodies,
Based on _{one of the} azimuth θ ₁ and the azimuth θ ₂ , update the probability that each sample actually exists, and extract a sample whose updated probability is equal to or greater than a threshold value from the multiple samples,
For each of the extracted samples, the relative position and relative attitude of the first and second moving bodies when the other of the azimuth θ ₁ and the azimuth θ ₂ is measured are predicted based on the motion model of each moving body,
Updating the probability that each of the extracted samples actually exists based on the other of the azimuth θ ₁ and the azimuth θ ₂ and the predicted relative position and relative posture of the first and second moving bodies; Extract a sample whose updated probability is equal to or greater than a threshold value from the extracted sample,
The method for measuring a relative posture of a moving body according to claim 2, wherein the relative relationship is estimated based on the extracted sample. A relative posture measuring device for measuring a relative posture of first and second moving bodies,
A first azimuth sensor provided on the first moving body, and a second azimuth sensor provided on the second moving body,
The direction from the base point set for the first mobile body to the azimuth point set for the first mobile body at an interval from the base point is defined as a first reference direction, and the first direction sensor Measure the orientation θ ₁ of the second moving body relative to the first reference direction as seen from the moving body,
The direction from the base point set for the second mobile body to the azimuth point set for the second mobile body at an interval from the base point is taken as a second reference direction, and the second direction sensor Measure the orientation θ ₂ of the first moving body relative to the second reference direction as seen from the moving body,
Based on the measured orientation theta ₁ and the azimuth theta ₂ and further comprising, relative orientation measurement of the moving body, characterized in that the computing unit for determining the relative relationship between the first reference direction and a second reference direction as the relative orientation apparatus.