JP2013050852A - Method and apparatus for integrating sensor information - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a method and apparatus for integrating information from a plurality of sensors without increasing errors and the amount of calculation.SOLUTION: Newly received sensor information Yn is recorded in observation time order and determined on whether each sensor information satisfies a first condition (S3). The first condition is that "sensor information from all sensors has been received at the observation time of received sensor information or later" If the first condition is satisfied, each sensor information 6 that satisfies the first condition is used in observation time order to predict the internal condition of a target object, a first estimate value X1 is updated with the predicted internal condition and stored, a second estimate value X2 is overwritten by the updated first estimate value X1, and the sensor information used for the update is deleted (S4, S5, S7, S8). If the first condition is not satisfied, the internal condition is predicted using the newly received sensor information Yn, and the second estimate value X2 is updated with the predicted internal condition and stored (S9). The second estimate value X2 is output as the internal condition of the target object (S10).

Description

  The present invention relates to a sensor information integration method and apparatus in an apparatus using a plurality of sensors such as a robot and a measuring instrument.

An automatic device that handles an object with a robot, a mobile robot that moves while measuring the surroundings with a sensor, and the like often include a plurality of measuring means for measuring the position and speed of the object and the robot. For example, in the case of a mobile robot, the following sensors are generally installed.
(A) an encoder that measures the amount of rotation of the wheel of the mobile robot, and (b) a camera that measures the positions of landmarks around the robot.

In general, as a means for integrating a plurality of sensor information, there is a Bayesian filter such as a Kalman filter, an information filter, or a particle filter.
The integration method of multiple sensor information using the Bayes filter is as follows.
(1) Prediction processing for predicting the internal state at the observation time when new sensor information (the result of any sensor is acceptable) is obtained.
(2) Update processing for comparing and correcting the predicted internal state with the sensor information.
Here, the internal state is defined in advance as various information related to the measurement target. For example, when it is desired to measure the position of the robot, the position, posture, speed, angular velocity, etc. of the robot are defined as internal states.

  The internal state corrected by the above processes (1) and (2) is a value at the observation time. Therefore, when the sensor information is obtained as Y1, Y2, Y3..., The time of the internal state also changes as Y1 time, Y2 time, Y3 time.

When there are a plurality of sensors, the sensor information cannot be obtained in the order of the observation times due to the influence of different sensor processing times and communication delay times with the sensors. For example, when the sensor information is obtained as Y1, Y2, Y3,... On an apparatus that integrates information, the observation time of Y3 may be older than the observation time of Y2.
In the Bayes filter prediction process, the estimation accuracy decreases (estimation error increases) according to the prediction time width (time Y2−time Y1...). Therefore, if the sensor information is not processed in the order of the observation times, the estimation time width becomes long, and the estimation error of the internal state increases.
The Bayesian filter uses the estimation result at the previous step for the estimation process at the next step. Therefore, when the estimation error increases, it affects the subsequent estimation.

  In order to solve the above-described problems, various means have already been proposed (for example, Non-Patent Documents 1 and 2 and Patent Document 1).

The method of Non-Patent Document 1 sequentially updates the internal state every time sensor information is obtained. When sensor information (Y) older than the previously processed sensor information is obtained, the internal state at the time of Y is predicted and corrected.
The means of Patent Document 1 accumulates measurement results for each sensor, and predicts subsequent measurement values from the history of measurement data for sensors with a large delay. It integrates sensor information with small predicted values and predicted values.
The means of Non-Patent Document 2 sorts and accumulates sensor information for a certain time in order of observation time, and processes all the accumulated data for each step.

Shoichi Maeyama, Akihisa Oya, Shinichi Yuta, “Retrospective Current Position Estimation Method for Mobile Robots Utilization of External Sensor Data that Takes Processing Time” Journal of the Robotics Society of Japan, vol. 15, no. 7, pp. 1075-1081, 1997 Shingo Kagami, Masatoshi Ishikawa, “Sensor Selection Method Considering Communication Delay” IEICE Transactions Vol. J88-A, no. 5, pp. 577-587, 2005

Japanese Patent No. 3367461, "Moving Body Posture Angle Detection Device"

  Since the method of Non-Patent Document 1 does not perform estimation processing in the order of the observation times, the error of the integration result becomes large. This document shows a method for integrating sensor information at an old time into the latest time estimation result. However, this integration method is effective only for the mobile robot targeted by this document. Further, since the approximation process is performed at the time of integration, the error becomes large.

  The means of Patent Document 1 is based on the premise that a sensor with a large delay is known. Therefore, it cannot be applied when the delay time of each sensor is indeterminate.

  In the method of Non-Patent Document 2, it is necessary to set the accumulation time (allowable time) in advance, and sensor information having a delay exceeding the allowable time is not reflected in the estimation. In addition, since all the accumulated data is processed each time, the amount of calculation increases according to the allowable time. Therefore, there is a restriction on the allowable time.

  The present invention has been developed to solve the above-described problems. In other words, the object of the present invention is that there are a plurality of sensors, the delay time of each sensor is uncertain, and even if the sensor information from each sensor cannot be obtained in the order of the observation time, the error and the calculation amount It is an object of the present invention to provide a method and an apparatus capable of integrating a plurality of sensor information without increasing the number of sensors.

According to the present invention, a sensor information integration method for receiving sensor information from a plurality of sensors at random, integrating the plurality of sensor information, and predicting and outputting an internal state of the object,
(A) The newly received sensor information is recorded in order of observation time by the data storage unit,
(B) The control unit determines whether or not the first condition is satisfied for each sensor information recorded in the data storage unit,
First condition: sensor information that is the same as or newer than the observation time of the sensor information is received from all sensors,
(C) When the first condition is satisfied, each sensor information satisfying the first condition is used in the order of observation time, the internal state is predicted by the prediction unit, and the first estimation value is predicted by the first estimation value storage unit Updated and stored in the state, overwritten with the first estimated value updated by the second estimated value storage unit, delete the sensor information used for the update,
(D) When the first condition is not satisfied, the internal state is predicted by the prediction unit using the newly received sensor information, and the second estimated value is updated to the predicted internal state by the second estimated value storage unit. Remember,
(E) A sensor information integration method is provided, wherein the second estimated value stored in the second estimated value storage unit is output as the internal state of the object.

According to the present invention, there is provided a sensor information integrating device that randomly receives sensor information from a plurality of sensors, integrates the plurality of sensor information, and predicts and outputs an internal state of an object.
A data storage unit that records the received sensor information in the order of their observation times;
A prediction unit for predicting the internal state of the object from the sensor information;
An update unit for updating the internal state;
A first estimated value storage unit for storing a first estimated value of the internal state;
A second estimated value storage unit for storing a second estimated value of the internal state;
A data storage unit, a prediction unit, an update unit, a control unit that controls the first estimated value storage unit and the second estimated value storage unit,
(A) The newly received sensor information is recorded in the data storage unit in order of observation time,
(B) For each sensor information recorded in the data storage unit, determine whether the first condition is satisfied,
First condition: sensor information that is the same as or newer than the observation time of the sensor information is received from all sensors,
(C) When the first condition is satisfied, each sensor information satisfying the first condition is used in the order of observation time, the internal state is predicted, the first estimated value is updated to the predicted internal state, and the first estimated value is stored. The second estimated value is overwritten with the updated first estimated value, the sensor information used for the update is deleted,
(D) When the first condition is not satisfied, the internal state is predicted using the newly received sensor information, and the second estimated value is updated to the predicted internal state and stored in the second estimated value storage unit. ,
(E) A sensor information integration device is provided that outputs the second estimated value in the second estimated value storage unit as an internal state of the object.

  According to the above method and apparatus of the present invention, when the first condition is satisfied, sensor information that is the same as or newer than the observation time of the sensor information is received from all the sensors. Similarly to the means, the first estimated value in the first estimated value storage unit is an estimation result obtained by processing the sensor information in the order of the observation time. Therefore, by overwriting the second estimated value with the first estimated value and outputting it as the internal state of the object, a plurality of sensor information can be integrated with high accuracy.

When the first condition is not satisfied, the internal state is predicted using the newly received sensor information, the second estimated value is updated to the predicted internal state, and output as the internal state of the object. A plurality of pieces of sensor information can be integrated based on the latest sensor information.
In this case, since sensor information with an old time may be processed, the accuracy may temporarily decrease. However, this error is a highly accurate first estimated value when the first condition is satisfied. Overwriting eliminates all, and does not affect the subsequent estimation. Therefore, even when the first condition is not satisfied, the estimation result of the second estimated value is more accurate than the method of Non-Patent Document 1.

In addition, the accumulated data is not processed every time, but is processed only at the timing when the first condition is satisfied.
Also, by processing when the first condition is satisfied, sensor information can always be processed in the order of observation time even if the delay time of each sensor is uncertain without setting an allowable time in advance.

It is a block diagram of the robot system provided with the integrated apparatus by this invention. It is a block diagram of the measuring device which has the integrated apparatus by this invention. It is a block diagram of the integrated apparatus by this invention. 4 is a flowchart illustrating an integration method according to the present invention. It is explanatory drawing which shows the measurement state by three sensors. It is explanatory drawing which shows the input order from three sensors to an integrated apparatus. It is explanatory drawing which shows transition of the sensor information of a data storage part at the time of measuring with three sensors.

  Hereinafter, preferred embodiments of the present invention will be described in detail with reference to the accompanying 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 robot system provided with an integration apparatus according to the present invention.
In this figure, 1 is a mobile robot, 2 is a camera on the robot mounted on the mobile robot 1, and 3 is an external camera. In this example, the position, posture, velocity, angular velocity, etc. of the mobile robot 1 are measured using the on-robot camera 2, the external camera 3, or the laser radar.
Hereinafter, the robot upper camera 2, the external camera 3, the laser radar, and other measurement means are collectively referred to as “sensor”, and information measured by the sensor is referred to as “sensor information”.

FIG. 2 is a block diagram of a measuring device having an integrated device according to the present invention.
In this figure, an integrated device 10 of the present invention is a state estimation device, which receives sensor information 6 from a plurality of sensors 5 and outputs an estimated value of an internal state 7.
Sensor information 6 obtained by the plurality of sensors 5 is sequentially input to the state estimation device 10. The sensor information 6 is data in which information such as a measured value of the sensor 5, an observation time (observation time), a sensor device ID, and the like are collected.
The state estimation device 10 estimates the internal state 7 such as the position, posture, speed, angular velocity, etc., of the object 1 (the mobile robot 1 in the example of FIG. 1) based on the input sensor information 6.
In this case, since the measurement timing, measurement process, and transmission method differ for each sensor 5, the sensor information 6 input to the state estimation device 10 is not in the order of the observation time.

FIG. 3 is a block diagram of an integrated device according to the present invention.
The integrated device 10 according to the present invention is a state estimation device, and includes a data storage unit 12, a prediction unit 14, an update unit 16, a first estimated value storage unit 17, a second estimated value storage unit 18, and a control unit 20.

  The integration device 10 according to the present invention is a device that randomly receives sensor information 6 from a plurality of sensors 5, integrates the plurality of sensor information 6, and predicts and outputs an internal state 7 of the object 1.

The data storage unit 12 records the received sensor information 6 in the order of the observation times.
The prediction unit 14 predicts the internal state 7 of the object 1 from the sensor information 6.
The update unit 16 updates the internal state 7.
The first estimated value storage unit 17 stores the first estimated value X1 of the internal state 7.
The second estimated value storage unit 18 stores the second estimated value X2 of the internal state 7.
The control unit 20 controls the data storage unit 12, the prediction unit 14, the update unit 16, the first estimated value storage unit 17, and the second estimated value storage unit 18.

The first estimated value X1 and the second estimated value X2 are the position, posture, velocity, angular velocity, and the like of the target 1 estimated based on the sensor information 6. The breakdown of the internal state 7 is appropriately set according to the purpose of using the integration result.
The prediction unit 14 and the update unit 16 are implemented with Bayesian filter prediction processing and update processing described in the related art.
The data storage unit 12 has a function of temporarily storing the obtained sensor information 6. Hereinafter, among the accumulated sensor information 6, the oldest observation time is Yb and the newest is Yt.

  FIG. 4 is a flowchart illustrating an integration method according to the present invention. As shown in this figure, the integration method of the present invention comprises steps (steps) S1 to S10.

When new sensor information Yn is input, the sensor information Yb with the oldest observation time in the data storage unit 12 is compared with the new sensor information Yn in S1, and the observation time of the new sensor information Yn is better. If it is old (NO), the estimation process is not performed, and the second estimated value X2 stored in the second estimated value storage unit 18 in S10 is output as it is.
Note that this procedure occurs only when the second condition or the third condition is satisfied in S3 described later.

In S1, when the observation time of the new sensor information Yn is newer than the oldest sensor information Yb (YES), the new sensor information Yn is recorded in the data storage unit 12 in the order of the observation time in S2.
As shown in FIG. 3, the observation time of the new sensor information Yn is not always the newest.

In S3, the sensor information 6 is extracted from the data storage unit 12 in the order of oldest observation time, and it is determined whether any of the following first condition, second condition, or third condition is satisfied.
First condition: Sensor information 6 that is the same as or newer than the observation time of the sensor information 6 has been received from all the sensors 5.
Second condition: The observation time of the sensor information 6 is a time older than a set time set in advance with respect to the current time.
Third condition: Sensor information 6 newer than the observation time of the sensor information 6 is recorded more than a preset number.

In order to determine that “received from all the sensors 5” in the first condition, a sensor list in which the sensors 5 to be used are registered may be prepared, and the sensor list may be changed during operation. For example, at the start of measurement, the four sensors 5 of A, B, C, and D may be used and changed to only three of A, B, and C from the middle. After the change, the first condition is satisfied if the sensor information of A to C is received.
Further, a plurality of integrated devices having different sensor lists may be used. For example, although there are four sensors 5 of A, B, C, and D in the entire system, the integrated device 1 determines only A, B, and C, and the integrated device 2 determines with A, B, C, and D. It may be.

  When the oldest sensor information Yb satisfies the first condition, sensor information whose observation time is older than the oldest sensor information Yb will not be input in the future. Therefore, if only the oldest sensor information Yb that satisfies the first condition is processed, the prediction process and the update process can be performed while the time of the internal state 7 is advanced in the forward direction from the past to the future.

Under only the first condition, if the processing time or communication delay of the sensor 5 is large, the accumulated sensor information 6 may increase and the storage area may be insufficient. Therefore, the second condition and the third condition are appropriately combined.
For example, a combination of only the first condition and the second condition, or a combination of only the first condition and the third condition may be used.
When the oldest sensor information Yb satisfying the second condition and the third condition is processed, even if sensor information with a time older than the oldest sensor information Yb is input thereafter, it is not used for the estimation process (see S1 → S10). ).

  However, the new sensor information 6 is more important when the position and orientation of the object 1 change from moment to moment. Therefore, as in the second condition, sensor information at a time older than a certain time is often not a problem even if it is not used for estimation. The setting time of the second condition is preferably set in a range that is longer than the time interval received from all the sensors 5 in a normal state and that allows a delay.

  Further, by processing the oldest sensor information Yb that satisfies the third condition, the number of accumulated sensor information 6 is always equal to or less than a certain number, so that it can be guaranteed that the capacity of the data storage unit 12 will not be exceeded. The set number of the third condition is preferably set within a range not exceeding the capacity of the data storage unit 12.

If it is determined in S3 that the process is to be performed (YES), the first estimated value X1 of the internal state 7 is estimated as follows using the oldest sensor information Yb in S4.
First, the internal state 7 at the observation time of the oldest sensor information Yb is predicted using the currently recorded first estimated value X1. Next, the internal state 7 is updated by comparing the measured value of the oldest sensor information Yb with the predicted internal state 7. The updated internal state 7 is set as a new first estimated value X1.

  For the estimation process (prediction process, update process), conventional algorithms such as a Kalman filter, an information filter, and a particle filter are used.

  In S5, the oldest sensor information Yb processed in S4 is deleted from the data storage unit 12. By repeating S3 to S5, all sensor information 6 that satisfies the processing conditions is estimated.

  If the first estimated value X1 is not updated in S3 to S5 (NO in S6), the second estimated value X2 is estimated using the new sensor information Yn in S9, and the second estimated value is determined in S10. Outputs the value X2.

  When the first estimated value X1 is updated in S3 to S5 (YES in S6), the second estimated value X2 is overwritten with the first estimated value X1 in S7, and all the remaining sensor information 6 is used in S8. 2 Estimate processing is performed on the estimated value X2, and the second estimated value X2 is output in S10.

  Hereinafter, an example in which sensor information is accumulated and processed according to the first condition of the operation flow will be described.

  FIG. 5 is an explanatory diagram showing measurement states by the three sensors A, B, and C, and FIG. 6 is an explanatory diagram showing an input order from the three sensors A, B, and C to the integrated device 10.

In FIG. 5, the three sensors A, B, and C sequentially output sensor information A1, A2, A3, A4, B1, B2, B3, C1, and C2, respectively. At this time, the order of the sensor information 6 input to the state estimation device 10 is as shown in FIG. 6 depending on the processing time, communication delay, and the like of the three sensors A, B, and C.
Steps 1 to 5 in FIG. 6 indicate intermediate stages of processing in the state estimation device 10.

FIG. 7 is an explanatory diagram showing transition of sensor information 6 in the data storage unit 12 when measured by three sensors A, B, and C. FIG.
In the example of FIG. 6, the sensor information 6 in the data storage unit 12 changes as shown in FIG. 7 through a series of processes.

  Until step 1, since there is no information of the sensor C, the first estimated value X1 is not updated. Meanwhile, the state estimation device 10 performs the estimation process on the second estimated value X2 using the sensor information 6 in the order of A1 → B1 → A2.

At step 2, the sensor information A1, C1 satisfies the first condition. Therefore, the first estimated value X1 is updated using the sensor information A1 → C1, and the sensor information A1, C1 is deleted. The second estimated value X2 is overwritten with the first estimated value X1, and is estimated using all the remaining data. Therefore, the second estimated value X2 is a result of processing in the order of the observation time in the sensor information A1->C1->B1-> A2.
As a result, the sensor information 6 in the data storage unit 12 is only the sensor information B1 and A2.

  Until step 3, there is no data satisfying the first condition, and only the second estimated value X2 is sequentially processed using only new sensor information B2 and B3. In this case, since the sensor information B1 and A2 are not processed, the calculation amount is smaller than that of the method of Non-Patent Document 2.

  Also in step 4, only the second estimated value X2 is processed using only the new sensor information A3. Since the sensor information A3 newly added at this time is older than the sensor information B3, the estimation accuracy of the second estimated value X2 temporarily decreases.

  In step 5, since the sensor information B1, A2, B2, A3 satisfies the first condition, the first estimated value X1 is updated. The final output of the second estimated value X2 is the result of processing the sensor information in the order of observation time in the order of A1, C1, B1, A2, B2, A3, C2, B3. As a result, the error generated in step 4 is eliminated.

  When sensor information Yn is recorded in the data storage unit 12, if there is sensor information 6 with the same observation time, the statistical value of the measured value can be taken and integrated into one sensor information 6. For example, an average value of measured values is taken or integrated by an information filter as in the method of Non-Patent Document 2. By doing so, the sensor information 6 to be recorded in the data storage unit 12 can be reduced, and the consumption memory and the calculation amount can be reduced.

  According to the above-described method and apparatus of the present invention, when the first condition is satisfied, sensor information 6 that is the same as or newer than the observation time of the sensor information 6 is received from all the sensors. Similar to the means of Patent Document 2, the first estimated value X1 of the first estimated value storage unit 17 is an estimation result obtained by processing the sensor information 6 in the order of the observation time. Therefore, by overwriting the second estimated value X2 with the first estimated value X1 and outputting it as the internal state 7 of the object 1, a plurality of sensor information 6 can be integrated with high accuracy.

When the first condition is not satisfied, the internal state 7 is predicted using the newly received sensor information 6 and the second estimated value X2 is updated to the predicted internal state 7, so that the internal state 7 of the object 1 is updated. As a result, a plurality of sensor information 6 can be integrated based on the latest sensor information 6.
In this case, since sensor information 6 with an old time may be processed, the accuracy may be temporarily lowered. This error is a highly accurate first estimated value when the first condition is satisfied. All are canceled by being overwritten with X1, and the subsequent estimation is not affected. Therefore, even when the first condition is not satisfied, the estimation result of the second estimated value X2 is more accurate than the method of Non-Patent Document 1.

In addition, the accumulated data is not processed every time, but is processed only at the timing when the first condition is satisfied.
Further, by processing when the first condition is satisfied, the sensor information 6 can always be processed in the order of observation time even if the delay time of each sensor 5 is indeterminate without setting an allowable time in advance.

  In addition, this invention is not limited to embodiment mentioned above, is shown by description of a claim, and also includes all the changes within the meaning and range equivalent to description of a claim.

1 Mobile robot (object) 2 Camera on the robot 3 External camera
5 Sensor, 6 Sensor information, 7 Internal state,
10 Integrated device (state estimation device),
12 data storage units, 14 prediction units, 16 update units,
17 1st estimated value memory | storage part, 18 2nd estimated value memory | storage part, 20 control part

Claims (5)

A sensor information integration method for receiving sensor information from a plurality of sensors at random, integrating the plurality of sensor information, and predicting and outputting an internal state of the object,
(A) The newly received sensor information is recorded in order of observation time by the data storage unit,
(B) The control unit determines whether or not the first condition is satisfied for each sensor information recorded in the data storage unit,
First condition: sensor information that is the same as or newer than the observation time of the sensor information is received from all sensors,
(C) When the first condition is satisfied, each sensor information satisfying the first condition is used in the order of observation time, the internal state is predicted by the prediction unit, and the first estimation value is predicted by the first estimation value storage unit Updated and stored in the state, overwritten with the first estimated value updated by the second estimated value storage unit, delete the sensor information used for the update,
(D) When the first condition is not satisfied, the internal state is predicted by the prediction unit using the newly received sensor information, and the second estimated value is updated to the predicted internal state by the second estimated value storage unit. Remember,
(E) A method for integrating sensor information, wherein the second estimated value stored in the second estimated value storage unit is output as an internal state of the object. 2nd condition: The observation time of the sensor information is a time older than a set time that can be delayed with respect to the current time, or 3rd condition: sensor information newer than the observation time of the sensor information is stored in the data 2. The sensor information integration method according to claim 1, wherein the processing is performed in the same manner as in the first condition when it is satisfied that a set number of records not exceeding the capacity of the copy is satisfied.   The sensor information according to claim 1, wherein a plurality of sensor lists in which sensors to be used are registered are prepared, the sensor list is changed during operation, and it is determined whether or not the first condition is satisfied. Integration method. A sensor information integration device that randomly receives sensor information from a plurality of sensors, integrates the plurality of sensor information, and predicts and outputs an internal state of an object,
A data storage unit that records the received sensor information in the order of their observation times;
A prediction unit for predicting the internal state of the object from the sensor information;
An update unit for updating the internal state;
A first estimated value storage unit for storing a first estimated value of the internal state;
A second estimated value storage unit for storing a second estimated value of the internal state;
A data storage unit, a prediction unit, an update unit, a control unit that controls the first estimated value storage unit and the second estimated value storage unit,
(A) The newly received sensor information is recorded in the data storage unit in order of observation time,
(B) For each sensor information recorded in the data storage unit, determine whether the first condition is satisfied,
First condition: sensor information that is the same as or newer than the observation time of the sensor information is received from all sensors,
(C) When the first condition is satisfied, each sensor information satisfying the first condition is used in the order of observation time, the internal state is predicted, the first estimated value is updated to the predicted internal state, and the first estimated value is stored. The second estimated value is overwritten with the updated first estimated value, the sensor information used for the update is deleted,
(D) When the first condition is not satisfied, the internal state is predicted using the newly received sensor information, and the second estimated value is updated to the predicted internal state and stored in the second estimated value storage unit. ,
(E) A sensor information integration device, characterized in that the second estimated value stored in the second estimated value storage unit is output as the internal state of the object. Using a plurality of integrated devices that record a sensor list in which a sensor to be used is registered in the data storage unit, and determining whether or not the first condition is satisfied from the sensors registered in the corresponding sensor list by each integrated device; The sensor information integration device according to claim 4.