JP6319736B2 - Sensor orientation control method and apparatus - Google Patents

Description

  The present invention relates to a method and apparatus for controlling the directivity direction of an external sensor such as a camera or laser sensor of a measuring object.

When multiple objects (for example, moving objects) move within the area independently of each other, control for preventing the plurality of moving objects from colliding with each other, control for causing other moving objects to follow the preceding moving object, etc. May be performed.
Note that the plurality of moving bodies are, for example, mobile robots that transport articles, and can freely move within the work area.

Patent documents 1 and 2 have already been disclosed as means for estimating the position of a mobile robot.
Further, Patent Documents 3 to 5 have already been disclosed as pointing control means for robot sensors.

In Patent Document 1, an external sensor such as a camera or a laser sensor is mounted on a mobile robot. A feature point is extracted from the result of measuring the surroundings by an external sensor, and the position of the feature point and the self position are estimated simultaneously.
Patent Document 2 estimates the positions and orientations of all measuring bodies by using a plurality of measuring bodies equipped with external sensors and integrating the results obtained by measuring each other's azimuth and distance. .
Patent Document 3 uses a predetermined mapping function to map the deviation of the position before and after the operation to the actuator control coordinates to form a command for the actuator means.
Patent Document 4 includes at least two sound wave sensors that receive sound waves, and calculates a sound wave incident angle to the robot by using a sound wave reception time difference between the two sound wave sensors.
Japanese Patent Application Laid-Open No. 2004-228561 sequentially controls the direction of the sensor of the robot path following apparatus around the center axis of the end effector, and maintains the direction of the sensor appropriately.

JP 2010-288112 A JP 2013-61307 A JP 2005-305633 A JP 2004-212400 A JP-A-7-141008

In either method, the positions of a plurality of objects are estimated simultaneously using an external sensor. The estimation is calculated by the following method.
Step 1: Appropriate initial values are given in advance as estimated values of the position and orientation of each object. In addition, an error variance matrix representing the magnitude of the initial value error is also set.
Step 2: Using an external sensor, measure the relative relationship (azimuth and distance) between objects.
Step 3: The estimated value of each object is corrected based on the measurement result, and the error variance matrix is reduced. At this time, an estimated value having a larger error variance has a larger correction amount.
If the positions and orientations of some objects are known in advance, the positions and orientations of the respective objects are estimated on the basis thereof, and the error variance is reduced as a whole.

In the above-described position estimation, in order to accurately extract the characteristics of the object, it is necessary to measure the object with high resolution using an external sensor. However, in general, external sensors have a trade-off relationship between resolution and field of view. For example, in the case of a camera, the smaller the angle of view of the lens, the higher the resolution. In the case of a laser sensor, the space can be measured with a higher density by narrowing the laser scanning range.
Therefore, the external sensor used for position estimation has a configuration in which a camera or a laser sensor with a viewing angle of 90 degrees or less is mounted on a pan head that can control the pan angle and the tilt angle.

However, with a general external sensor configuration, it is not possible to put all objects within the field of view. If there is no object whose estimated value has converged in the field of view and its own estimated value has not converged, the estimation will not converge even if steps 2 and 3 are repeated.
Patent Document 1 describes a method for controlling the orientation of a sensor so that at least a part of previously measured feature points remains in the field of view. However, this method is effective only when the surroundings of the moving body are all stationary and it can be said that “the previously measured points have a small error variance”.

In the case where there are moving objects in the surroundings, there are cases where the error variance increases due to movement even if the measurement has been performed in the past. Even if such a moving body is measured, it is not effective in estimating the self-position. On the other hand, since the position error of the moving body gradually increases, if the observation is not made periodically, the position becomes unknown and the control operation using the estimated value is affected.
In addition, when there are a plurality of measuring bodies, the error dispersion of surrounding objects may be reduced by observing other measuring bodies regardless of the self. As described above, there has been a demand for effectively controlling the orientation of the sensor in a situation where the error variance of the self and the surrounding objects dynamically increases and decreases.

  Also, for example, if each object is equipped with a GPS, the exact position of each object can be acquired, but the position of each object can be obtained even in an environment (such as indoors, urban areas, and forests) where the accurate position cannot be acquired with GPS. Need to be estimated.

  The present invention has been developed to solve the above-described problems. That is, according to the present invention, the distance to an object positioned around a measurement body including an external sensor such as a camera or a laser sensor is long, and the measurement body or the object is a moving body. It is assumed that the error variance increases or decreases dynamically. Under such circumstances, an object of the present invention is to provide a sensor directivity control method and apparatus capable of effectively controlling the directivity direction of an external sensor of a measuring body.

ここで、ＣＸｉｉは状態量Ｘｉの誤差分散行列であり、ＣＸｉｊは共分散行列であり、Ｖｉｊは、観測誤差分散であり、Ｒは観測誤差分散値であり、Ｂｉ，Ｂｊは観測行列であり、
（Ｃ）前記カルマンゲインが最も大きい物体を注視物体として選択し、
（Ｄ）選択した注視物体の方向に前記雲台のパン角又はチルト角を制御する、ことを特徴とするセンサの指向制御方法が提供される。
According to the present invention, an external sensor capable of measuring the azimuth or distance of an object located in the vicinity, a camera platform on which the external sensor is mounted, and a camera platform control device that controls the pan angle or tilt angle of the camera platform. , A sensor directivity control method in a measuring body,
The measurement body includes a communication unit capable of communicating with surrounding objects, and a state estimation unit that outputs a command value to the pan head control device.
(A) Obtain an estimate of the position or orientation of the object and its surrounding objects,
The estimated value is a state quantity indicating a position and orientation, and an error variance matrix of the state quantity,
(B) calculating a Kalman gain of the estimated value of surrounding objects;
The Kalman gain is a first correction gain related to the position of the observation target, a second correction gain related to the position of the observation source, a third correction gain related to the attitude of the observation source, or an integrated correction gain obtained by integrating these.
The first correction gain is a sum of squares of position components of the Kalman gain Kj of the observation target j,
The second correction gain is a sum of squares of position components of the Kalman gain Ki of the observation source i,
The third correction gain is a sum of squares of attitude components of the Kalman gain Ki of the observation source i,
The integrated correction gain is the sum of squares of each component of the Kalman gains Ki and Kj.
The Kalman gains Ki and Kj are calculated by Equations (10) and (11) of Equation 9,

Here, CXii is an error variance matrix of the state quantity Xi, CXij is a covariance matrix, Vij is an observation error variance, R is an observation error variance value, Bi and Bj are observation matrices,
(C) selecting an object having the largest Kalman gain as a gaze object,
(D) A sensor orientation control method is provided, wherein the pan angle or tilt angle of the camera platform is controlled in the direction of the selected gaze object.

  The integrated correction gain is a value obtained by adding the product of the distance between the measurement object and the surrounding object, the third correction gain, and the weight coefficient β of the posture component to the sum of the first correction gain and the second correction gain.

  In (C), the first correction gain, the second correction gain, and the third correction gain are alternately switched as the Kalman gain.

ここで、ＣＸｉｉは状態量Ｘｉの誤差分散行列であり、ＣＸｉｊは共分散行列であり、Ｖｉｊは、観測誤差分散であり、Ｒは観測誤差分散値であり、Ｂｉ，Ｂｊは観測行列であり、
（Ｃ）前記カルマンゲインが最も大きい物体を注視物体として選択し、
（Ｄ）選択した注視物体の方向に前記雲台のパン角又はチルト角を制御する、ことを特徴とするセンサの指向制御装置が提供される。
In addition, according to the present invention, an external sensor capable of measuring the azimuth or distance of an object located in the vicinity, a pan head on which the external sensor is mounted, and a pan head control for controlling the pan angle or tilt angle of the pan head A sensor directivity control device provided on a measuring body having a device,
A communication unit capable of communicating with surrounding objects, and a state estimation unit that outputs a command value to the pan head control device, the state estimation unit,
(A) Obtain an estimate of the position or orientation of the object and its surrounding objects,
The estimated value is a state quantity indicating a position and orientation, and an error variance matrix of the state quantity,
(B) calculating a Kalman gain of the estimated value of surrounding objects;
The Kalman gain is a first correction gain related to the position of the observation target, a second correction gain related to the position of the observation source, a third correction gain related to the attitude of the observation source, or an integrated correction gain obtained by integrating these.
The first correction gain is a sum of squares of position components of the Kalman gain Kj of the observation target j,
The second correction gain is a sum of squares of position components of the Kalman gain Ki of the observation source i,
The third correction gain is a sum of squares of attitude components of the Kalman gain Ki of the observation source i,
The integrated correction gain is the sum of squares of each component of the Kalman gains Ki and Kj.
The Kalman gains Ki and Kj are calculated by Equations (10) and (11) of Equation 9,

Here, CXii is an error variance matrix of the state quantity Xi, CXij is a covariance matrix, Vij is an observation error variance, R is an observation error variance value, Bi and Bj are observation matrices,
(C) selecting an object having the largest Kalman gain as a gaze object,
(D) A sensor directivity control device is provided, which controls a pan angle or a tilt angle of the pan head in a direction of a selected gaze object.

According to the method and apparatus of the present invention described above, the Kalman gain of the estimated value of the position or orientation of the surrounding object is calculated by the state estimation unit. Next, an object having the largest Kalman gain (maximum gain object) is selected as the gaze object, and the pan angle or tilt angle of the camera platform is controlled in the direction of the selected gaze object.
By observing the gaze object with this control, the position or posture of the self or the gaze object is corrected.

  The Kalman gain is a coefficient proportional to the amount of correction of the estimated value, and the larger the value, the larger the amount of correction of the estimated value. Therefore, the estimated value can be converged efficiently by selecting the object having the highest Kalman gain (maximum gain object) as the gaze object.

  Therefore, the distance to the object located around the measurement body having an external sensor such as a camera or a laser sensor is large, and the measurement body or the object is a moving body, and the error dispersion between the measurement body and the surrounding object is reduced. It is possible to effectively control the directivity direction of the external sensor of the measuring body under the condition of dynamically increasing / decreasing.

It is a figure which shows the usage example of this invention. It is a block diagram of a measurement body (moving body). It is a block diagram of a mobile body (for example, mobile robot). 1 is an overall flowchart of a sensor directivity control method according to the present invention. It is an image figure of an estimation error value. It is a schematic diagram which shows the method of this invention.

  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 diagram showing an example of use of the present invention.
In this figure, the measuring body 12 is a movable body 13 that can move or a fixed body 14 that cannot move.
In the example of FIG. 1, a moving body 13 (for example, a mobile robot) and a fixed body 14 (for example, a fixed device) are arranged in a predetermined area, and the positions of the moving body 13 and the fixed body 14 are obtained. Hereinafter, the measurement body 12 (the moving body 13 or the fixed body 14) and the fixed body (for example, the landmark 15) other than the measurement body 12 are simply referred to as the object 10 unless distinction is necessary. The obtained position of the object 10 is used for control of the moving body 13, area mapping, monitoring, and the like.

FIG. 2 is a configuration diagram of the measuring body 12 (the moving body 13 in this example).
The measuring body 12 includes an external sensor 16, a pan head 18, and a pan head control device 20.

The external sensor 16 is a sensor that can measure the orientation or distance of another object 10.
In this example, the external sensor 16 includes a camera 16a and an LRF 16b (laser sensor). In addition, although it is preferable to mount both the camera 16a and LRF16b in the measurement body 12, only one may be sufficient.

The camera 16a can measure the azimuth of the surrounding object 10 as an azimuth angle and an elevation angle. The azimuth angle and elevation angle have the same definition as in general polar coordinate expression. The object 10 is preferably provided with a marker 10a for detection by the camera 16a.
The distance to the surrounding object 10 can be measured by the LRF 16b. Further, not only the distance but also the orientation of another object 10 can be measured by the LRF 16b.

  The camera platform 18 carries the external sensor 16 and can adjust two axes of a horizontal pan angle and a vertical tilt angle.

  The camera platform controller 20 controls the pan angle or tilt angle of the camera platform 18.

  As shown in this figure, the measuring body 12 (moving body 13 or fixed body 14) has a pan head 18 on which an external sensor 16 is placed.

  With the configuration described above, the orientation of the external sensor 16 (camera 16a and LRF 16b) can be changed to the horizontal direction by moving the pan angle. Further, by moving the tilt angle, the direction of the camera 16a and the LRF 16b can be changed to the vertical direction. The camera 16a and the LRF 16b may be mounted on different pan heads 18 and moved independently.

  In FIG. 2, the measuring body 12 is a moving body 13 (for example, a mobile robot), but may be a fixed body 14 that is not movable.

FIG. 3 is a configuration diagram of the moving body 13 (for example, a mobile robot).
In this figure, the above-described pan head control device 20 includes a pan head drive unit 20a and a pan head control unit 20b. In addition to the external sensor 16 and the pan head control device 20 described above, the mobile body 13 includes an internal sensor 22, a robot drive unit 24, and a vehicle control unit 26.

In this example, the internal sensor 22 is a gyro sensor 22a (a sensor that measures the acceleration and angular velocity of the moving body 13) and a wheel encoder 22b (a sensor that measures the speed and angular velocity of the moving body 13).
The detection data of the external sensor 16 and the internal sensor 22 are input to a state estimation unit 34 (described later) via each measurement unit.

The sensor directivity control device 30 according to the present invention is provided in the measurement body 12 (the moving body 13 in this example), and includes a communication unit 32 and a state estimation unit 34.
The communication unit 32 can communicate with the surrounding object 10 and is preferably configured to be able to communicate with the wireless LAN in real time.
The state estimation unit 34 uses the sensor information measured by the external sensor 16 and the internal sensor 22 to acquire an estimated value A (described later) of itself (the measurement body 12) and the surrounding object 10.
The directivity control device 30 is configured by, for example, a computer (PC). Moreover, you may comprise by the same computer (PC) including the pan head control part 20b mentioned above and the vehicle control part 26. FIG.

The state estimation unit 34 outputs control signals to the vehicle control unit 26 and the pan head control unit 20b. The vehicle control unit 26 calculates a command value and outputs it to the robot drive unit 24, and the robot drive unit 24 controls the wheel encoder 22b. Similarly, the pan head control unit 20b calculates a command value and outputs it to the pan head drive unit 20a, and the pan head drive unit 20a controls the pan angle or tilt angle of the pan head 18.
Further, the state estimation unit 34 executes a sensor directivity control method described later.

FIG. 4 is an overall flowchart of the sensor directivity control method according to the present invention.
As shown in this figure, the directivity control method of the present invention comprises steps (steps) S1 to S6.
Hereinafter, unless otherwise necessary, the self (observation source measurement body 12) is referred to as “device i” or “observation source i”, and the surrounding object 10 (observation target) is referred to as “device j” or “observation target j”. "
If necessary, another object 10 to be observed is referred to as “device k”. i, j, k are 1, 2, 3,... positive numbers, and device i, device j, and device k mean different objects 10.

In step S1, an estimated value A of the position or orientation of the self and the surrounding object 10 is acquired. This means is based on Patent Documents 1 and 2 and the like.
Usually, the approximate position or posture is known when each object 10 is placed in the area. Therefore, the initial values of the self and the surrounding object 10 are set with an accuracy of, for example, a position error of 2 m and a posture error of about 2 degrees.
The estimated value A of the position or orientation of the self (observation source measuring body 12) is calculated from the immediately preceding data stored in the state estimation unit 34 and the output of the internal sensor 22. The estimated value A of the position or orientation of the surrounding object 10 (measurement body 12) acquires the immediately preceding data stored in the state estimation unit 34 of the object via the communication unit 32. Further, in the case where the surrounding object 10 is a fixed body 14 that cannot move or a fixed body other than the measuring body 12 (for example, a landmark 15), the position or orientation thereof is stored in the state estimation unit 34 in advance.

The surrounding object 10 is an object 10 that is predicted to be within a distance range that can be measured by the external sensor 16 among the estimation results in the state estimation unit 34.
The estimated value A is a state quantity Xi indicating the position and orientation, and an error variance matrix CXii of the state quantity Xi. The estimated value A is constantly updated over time.
In the case of the moving body 13, motion components such as speed and angular velocity may be included in the state quantity Xi.

The current estimated value A (state quantity Xi and error variance matrix CXii) of the device i is obtained by Equations (1) to (3) of Equation 1. Further, the covariance matrix CXij between the state quantity Xi of the device i and the state quantity Xj of the device j can also be acquired from the state estimation unit 34.
In Expression (1), (x, y, z) represents a position, and (rx, ry, rz) represents a posture.

Each time the measuring object observes the object, the state quantity Xi and the variance are updated.
The equation for correcting the state quantity Xi can be expressed by (4) in [Equation 2]. Also, the distribution update formula can be expressed by (5) in [Equation 2].

  The definition of X or CovX can be expressed by Equations (6) to (9) in Equation 3. Therefore, the error variance matrix CXii and the covariance matrix CXij of each object are updated by Expression (5). Note that the order of Equation 3 changes depending on the number of objects.

  In step S2, the Kalman gain K of the estimated value A of the surrounding object 10 is calculated.

FIG. 5 is an image diagram of the estimated error value B.
In this figure, device i, device j, and device k are three different objects 10. At least one of the three different objects 10 is a measuring body 12. The measuring body 12 may be a moving body 13 or a fixed body 14.
Ellipses (indicated by broken lines) in the figure indicate the position error ranges of device i, device j, and device k. Also, the angle α in the figure indicates the observation error range.

The Kalman gain Ki of the observation source i and the Kalman gain Kj of the observation object j are calculated by the equations (10) and (11) of Equation 4.
Here, R is an observation error variance value assumed as a measurement error of distance, azimuth angle, and elevation angle.
Further, each component of the Kalman gain Ki is expressed as in Expression (12) (the same applies to the Kalman gain Kj).
Hereinafter, “Kalman gain K” means Kalman gain Ki or Kalman gain Kj.

For example, when the difference between the distance (L) and direction (θ, φ) observed at the observation source i and the current estimated value A is ΔL, Δθ, Δφ, the correction amount of the state quantity Xi in the Kalman filter theory Is calculated by the equation (13) of Formula 5.
From equation (13), it can be seen that as each component of the Kalman gain Ki increases, the correction amount of the state quantity Xi increases and the state quantity Xi is more likely to be corrected.

In the present invention, the Kalman gain K of the estimated value A of the surrounding object 10 is a first correction gain E1 related to the position of the observation target j, a second correction gain E2 related to the position of the observation source i, and a third correction related to the attitude of the observation source i. The correction gain E3 or the integrated correction gain E _all obtained by integrating them.
The integrated correction gain E _all is obtained by adding the product of the distance between the measuring body 12 and the surrounding object 10, the third correction gain E3, and the weight coefficient β of the posture component to the sum of the first correction gain E1 and the second correction gain E2. It is the value.

The first correction gain E1, the second correction gain E2, and the third correction gain E3 are given by Expression (14) of Equation 6.
The integrated correction gain E _all is given by equation (15).
However, there is a relationship of Expression (16).
Similarly, yj to rzi are the sum of corresponding one row of the Kalman gain matrix (formula (12)).
In the E _all equation, more precisely, C indicates C _n ^bi .
P _ij means “position vector of observation object j viewed from observation source i”.

As is clear from the equations (10) and (11), the Kalman gain K (Ki or Kj) is a value obtained by dividing the error variance CX of the estimated value A by the sum of the observation error variance Vij and the observation error variance R. .
The observation error variance Vij means “the sum of error variances of the observation source i and the observation target j”.
Accordingly, the Kalman gain K can be approximated as in the equations (17) and (18) of Equation 7. This equation is an approximation that ignores the observation matrix Bn and the covariance matrix CXij, but the tendency of the approximate value to increase or decrease is the same as the Kalman gain K.

Therefore, the above-described correction gains E1 to E3 and E _all increase / decrease as follows according to the size of the error variance matrix CXii of the estimated value A.

(1) 1st correction gain E1 regarding the position of the observation object j
From Expression (14), the first correction gain E1 is the sum of squares of the position (x, y, z) components of the Kalman gain Kj (similar to Expression (12)) of the observation target j. Therefore, the value is large when the error variance matrix CXii of the observation source i is small and the error variance matrix CXjj of the observation target j is large.
Conversely, when the error variance matrix CXii of the observation source i is large and the error variance matrix CXjj of the observation target j is small, the value is small.

(2) The second correction gain E2 related to the position of the observation source i and the third correction gain E3 related to the attitude of the observation source i
From Equation (14), the second correction gain E2 is the sum of squares of the position (x, y, z) components of the Kalman gain Ki of the observation source i (similar to Equation (12)).
Similarly, from equation (14), the third correction gain E3 is the sum of squares of the posture (rx, ry, rz) components of the Kalman gain Ki of the observation source i.
Therefore, the value is large when the error variance matrix CXii of the observation source i is large and the error variance matrix CXjj of the observation target j is small.
Conversely, when the error variance matrix CXii of the observation source i is small and the error variance matrix CXjj of the observation target j is large, the value is small.

(3) Integrated correction gain E _all integrating these
The integrated correction gain E _all is a weighted sum of squares of all Kalman gains K (E1, E2, E3) of the observation source i and the observation target j. That is, the integrated correction gain E _all is the sum of squares of the components of Ki and Kj. Therefore, the tendency is as shown in Table 1.

Therefore, by selecting the gaze object 11 having the largest integrated correction gain E _all , the target corresponding to cases 1 and 2 is preferentially selected. The effect of combining the first correction gain E1 and the second correction gain E2 can be expected.

Returning to FIG. 4, in step S <b> 3, the object 10 having the largest Kalman gain K is selected as the gaze object 11. The gaze object 11 is selected from the surrounding objects 10.
The Kalman gain K may be the first correction gain E1, the second correction gain E2, the third correction gain E3, or the integrated correction gain E _all .

  Further, the first correction gain E1, the second correction gain E2, and the third correction gain E3 may be switched as the Kalman gain K at regular intervals.

In step S4, a pan angle command value θ _ope and a tilt angle command value φ _ope are calculated.
The direction (θ, φ) of the gaze object 11 viewed from the self is obtained in the second and third rows of the observation equation g , and these are set as the pan angle command value θ _ope and the tilt angle command value φ _ope .

In step S5, the pan angle or tilt angle of the pan head 18 is controlled.
Based on the pan angle command value θ _ope and the tilt angle command value φ _ope , the pan head 18 is controlled as follows.
The θ-axis is moved within the range of Equation (19) in Equation 8.
The φ axis is moved within the range of equation (20).
Here, Vth and Vph are diagonal terms of the observation error variance Vij represented by Expression (21). Expression (21) defines symbols in the diagonal terms of the observation error variance Vij.
N is a coefficient that determines the search range of the observation target j. When n is set to 0, it points to one point in the predicted direction.

In the equation (21), VL means “distance observation error range”.
The observation error variance Vij is a 3 × 3 matrix. Of these, Vth and Vph are used to determine the width of the reciprocating motion of the camera platform.

In step S6, the process returns to step S1 after the gaze object 11 selected in step S3 is observed or after a predetermined time has elapsed.
It is preferable to return to step S1 after the gaze object 11 selected in step S3 is observed. By observing the gaze object 11, the position or posture of the gaze object 11 or itself is corrected.
However, in a situation where the gaze object 11 changes every time as a result of the selection in step S3, the target command values θ _ope and φ _ope may change from loop to loop and the control may become unstable. In this case, after the control in step S5 is continued for a certain time, the process returns to step S1.
Note that the control in step S5 may be continued until the Kalman gain K related to the gaze object 11 decreases from the time selected in step S3, instead of the lapse of a fixed time.

  Further, if the gaze object 11 is not observed even after the control in step S5 is continued for a certain time, there is a possibility that the target does not exist in the predicted direction due to the large estimation error. In such a case, an operation of turning the line-of-sight direction over the entire periphery may be added.

FIG. 6 is a schematic diagram showing the method of the present invention.
In this figure, the device i detects the device j as the maximum gain object of the first correction gain E1, and detects the landmark 15 as the maximum gain object of the second correction gain E2.

  The Kalman gain K is a coefficient proportional to the correction amount of the estimated value A, and the correction amount of the estimated value A increases as this value increases. Therefore, by selecting the target so that the Kalman gain K is increased, the estimated value A can be efficiently converged. In the present invention, verifying a specific component of the Kalman gain K has the following effects.

As shown in the prior art, the estimated error value B (position error variance B1) of the object 10 that is not observed (particularly the moving body 13) gradually increases. Such an object 10 has a large Kalman gain K (first correction gain E1) at the position when it becomes the observation target j.
When the first correction gain E1 related to the position of the observation target j is selected as the gaze object 11 in step S3, in step S4 to S6, the measurement body 12 causes the object 10 (observation target) whose position error variance B1 is increasing. j) is preferentially measured. For this reason, an increase in the position error variance B1 can be prevented.

In step S3, when the second correction gain E2 related to the position of the observation source i or the third correction gain E3 related to the attitude of the observation source i is selected as the gaze object 11, the estimated value A of the measuring body 12 (observation source i) An object (observation object j) that increases the correction amount is selected. For this reason, the position and orientation of the measuring body 12 itself (observation source i) can be efficiently estimated.
In patent document 1, it controls so that the point measured previously remains in a visual field. On the other hand, in the present invention, by selecting the second correction gain E2 or the third correction gain E3 as the gaze object 11, it is most suitable in a situation where the error variance of the surrounding object 10 is dynamically changed. The object 10 can be selected.

  In step S3, the first correction gain E1, the second correction gain E2, and the third correction gain E3 are alternately switched as the Kalman gain K. Thus, it is possible to efficiently estimate the position and orientation of each measurement body 12 (observation source i) while preventing a state where the position error of the specific object 10 (observation target j) is excessive.

The integrated correction gain E _all is a sum of index values used in the first correction gain E1 and the second correction gain E2. If there is an object 10 (observation target j) in which the position error variance B1 is increasing, the first correction gain E1 is increased, and the object 10 is selected. Other than that, an object (observation object j) that can efficiently estimate the position and orientation of itself (observation source i) is selected. Therefore, it is possible to select the gaze object 11 that comprehensively considers the error variance of the surrounding object 10 and the error variance of the measuring body 12 itself.

  According to the method of the present invention described above, the state estimation unit 34 calculates the estimation error value B and the Kalman gain K of the estimated value A of the position or orientation of the surrounding object 10. Next, the object 10 with the largest Kalman gain K (maximum gain object) is selected as the gaze object 11, and the pan angle or tilt angle of the pan head 18 is controlled in the direction of the selected gaze object 11.

  The Kalman gain K is a coefficient proportional to the correction amount of the estimated value A, and the correction amount of the estimated value A increases as this value increases. Therefore, the estimated value A can be efficiently converged by selecting the object 10 having the largest Kalman gain K (maximum gain object) as the gaze object 11.

In the present invention, the distance to the object 10 located around the measuring body 12 including the external sensor 16 such as the camera 16a and the laser sensor 16b is long, and the measuring body 12 or the object 10 is the moving body 13. It is assumed that the error variance between the measurement body 12 and the surrounding object 10 is dynamically increased or decreased.
According to the present invention, the directivity direction of the external sensor 16 such as the camera 16a and the laser sensor 16b provided in the measuring body 12 can be effectively controlled under this situation.

  When one measuring body 12 has two pan heads 18 that move two sets of external sensors 16 independently, one pan head 18 is controlled with respect to the first correction gain E1. The other head 18 may be controlled with respect to the second correction gain E2. In this way, by sharing the roles of the two external sensors 16 and the pan head 18, observation of the surrounding object 10 and estimation of its own position and orientation can be achieved at the same time.

  In addition, this invention is not limited to embodiment mentioned above, Of course, a various change can be added in the range which does not deviate from the summary of this invention.

A estimated value, B estimated error value, B1 position error variance,
g Observation equation, K (Ki, Kj) Kalman gain,
E1 first correction gain, E2 second correction gain, E3 third correction gain,
Eall integrated correction gain, R observation error variance, Xi, Xj state quantity,
CXii, CXjj error variance matrix, CXij covariance matrix,
Vij observation error variance, θ _ope pan angle command value, φ _ope tilt angle command value,
10 object, 10a marker, 11 gaze object, 12 measuring object, 13 moving object,
14 fixed body, 15 landmark, 16 external sensor, 16a camera,
16b LRF (laser sensor), 18 pan head, 20 pan head control device,
20a pan head drive unit, 20b pan head control unit, 22 internal sensor,
22a gyro sensor, 22b wheel encoder, 24 robot drive unit,
26 vehicle control unit, 30 directivity control device, 32 communication unit, 34 state estimation unit

Claims (4)

In a measuring body having an external sensor capable of measuring the azimuth or distance of an object located around, a camera platform on which the external sensor is mounted, and a camera platform controller for controlling the pan angle or tilt angle of the camera platform A sensor orientation control method,
The measurement body includes a communication unit capable of communicating with surrounding objects, and a state estimation unit that outputs a command value to the pan head control device.
(A) Obtain an estimate of the position or orientation of the object and its surrounding objects,
The estimated value is a state quantity indicating a position and orientation, and an error variance matrix of the state quantity,
(B) calculating a Kalman gain of the estimated value of surrounding objects;
The Kalman gain is a first correction gain related to the position of the observation target, a second correction gain related to the position of the observation source, a third correction gain related to the attitude of the observation source, or an integrated correction gain obtained by integrating these.
The first correction gain is a sum of squares of position components of the Kalman gain Kj of the observation target j,
The second correction gain is a sum of squares of position components of the Kalman gain Ki of the observation source i,
The third correction gain is a sum of squares of attitude components of the Kalman gain Ki of the observation source i,
The integrated correction gain is the sum of squares of each component of the Kalman gains Ki and Kj.
The Kalman gains Ki and Kj are calculated by Equations (10) and (11) of Equation 9,

Here, CXii is an error variance matrix of the state quantity Xi, CXij is a covariance matrix, Vij is an observation error variance, R is an observation error variance value, Bi and Bj are observation matrices,
(C) selecting an object having the largest Kalman gain as a gaze object,
(D) A sensor orientation control method, wherein the pan angle or tilt angle of the camera platform is controlled in the direction of the selected gaze object. The integrated correction gain is a value obtained by adding the product of the distance between the measurement object and the surrounding object, the third correction gain, and the weight coefficient β of the posture component to the sum of the first correction gain and the second correction gain. The sensor directivity control method according to claim 1 . The sensor directivity control method according to claim 1 , wherein in (C), the first correction gain, the second correction gain, and the third correction gain are alternately switched as the Kalman gain. An external body sensor capable of measuring the azimuth or distance of an object located around, a pan head on which the external sensor is mounted, and a pan head control device that controls the pan angle or tilt angle of the pan head. A sensor directivity control device provided,
A communication unit capable of communicating with surrounding objects, and a state estimation unit that outputs a command value to the pan head control device, the state estimation unit,
(A) Obtain an estimate of the position or orientation of the object and its surrounding objects,
The estimated value is a state quantity indicating a position and orientation, and an error variance matrix of the state quantity,
(B) calculating a Kalman gain of the estimated value of surrounding objects;
The Kalman gain is a first correction gain related to the position of the observation target, a second correction gain related to the position of the observation source, a third correction gain related to the attitude of the observation source, or an integrated correction gain obtained by integrating these.
The first correction gain is a sum of squares of position components of the Kalman gain Kj of the observation target j,
The second correction gain is a sum of squares of position components of the Kalman gain Ki of the observation source i,
The third correction gain is a sum of squares of attitude components of the Kalman gain Ki of the observation source i,
The integrated correction gain is the sum of squares of each component of the Kalman gains Ki and Kj.
The Kalman gains Ki and Kj are calculated by Equations (10) and (11) of Equation 9,

Here, CXii is an error variance matrix of the state quantity Xi, CXij is a covariance matrix, Vij is an observation error variance, R is an observation error variance value, Bi and Bj are observation matrices,
(C) selecting an object having the largest Kalman gain as a gaze object,
(D) A sensor orientation control device that controls a pan angle or a tilt angle of the pan head in a direction of a selected gaze object.

Images