JP6035671B2 - Device state identification method and apparatus - Google Patents

Description

  The present invention relates to a device state identification method and apparatus for identifying states of a plurality of devices (for example, mobile robots).

  In order to control a plurality of mobile devices (for example, mobile robots) or to guide a person, there is known a technique for identifying states such as the positions and postures of mobile devices and targets. In general position identification means, various information relating to the position and orientation of the mobile device are measured as follows.

Means A: The position, speed, acceleration, and the like of the mobile device are measured by a GPS, encoder, and gyro sensor installed on the mobile device.
Means B: The direction and distance of surrounding objects are measured by an external sensor (such as a camera or a laser sensor) on the moving device. Alternatively, the direction and distance of the moving device are measured by an external sensor outside the moving device.
These pieces of information are integrated using a Bayes filter (Kalman filter, particle filter, etc.) to identify the position and orientation of the mobile device.

  The means B measures a relative relationship such as the direction and distance of the measurement object viewed from the measurement side. Therefore, in order to identify the position of the moving device in the means B, information such as the positions of surrounding objects and the positions and orientations of external sensors are also required. Therefore, there are identification means 1 to 3 for identifying with the Bayes filter including these pieces of information.

The identification unit 1 is SLAM technology that simultaneously estimates the position of surrounding objects (extracted landmarks, markers, etc.) and the position of the moving device using an external sensor on the moving device. Has been.
The identification unit 2 performs the SLAM technique of the identification unit 1 simultaneously with a plurality of mobile devices, and shares landmark position information (map information) between the mobile devices.
The identification unit 3 is a device in which a plurality of mobile devices measure relative relationships with each other and estimate both positions and postures.

  Here, the identification means 2 and the identification means 3 arrange a plurality of movement sensors and fixed sensors in the area. Thus, when integrating the information of the sensors arranged in a distributed manner using a Bayes filter, the following integration means 1 to 3 are conceivable.

The integration unit 1 collects and processes the information of each sensor in one processing device.
The integration unit 2 processes each sensor using a Bayes filter, and transmits the processing result (estimated probability distribution) to an upper integrated node. The integration node integrates the estimation results by overlapping probability distributions. This integration means is disclosed in Patent Document 2, for example.
The integration unit 3 performs processing by an information type Kalman filter (information filter) on each sensor, and communicates and integrates the correction amount calculated at the time of processing between peripheral devices. The information filter in the integration unit is a unit that simplifies the correction process among the prediction process and the correction process in the Bayes filter. That is, there is a feature that correction processing can be performed only by adding the correction amount calculated on each sensor. This integration means is disclosed in Non-Patent Document 1, for example.

JP 2008-304268 A JP 2011-197964 A

H. Durrant-Whyte, M.M. Stevens. “Data Fusion in Decentralized Sensing Networks”, Proceedings of the International Conference on Information Fusion, 2001, pp. 199 WeA3-19-24.

When integrating the sensor information in one place as in the integration unit 1, there are the following problems.
(1) When the number of mobile devices for position identification increases, the amount of calculation of observation update per time in Bayesian filter processing increases.
(2) As the number of mobile devices increases, the integrated observation values increase. As a result, the number of observation updates increases and the calculation load increases.
(3) When the number of mobile devices increases, the communication load for collecting the measurement results increases. In particular, in the case of wireless communication, in order to communicate between devices where radio waves do not reach directly, transfer processing (ad hoc communication) by a relay device is required, which further increases the communication load.

  In the integration unit 2, the amount of calculation in the integration node is reduced as much as each sensor performs Bayes filter processing. However, the number of observation updates listed in (2) is not solved. Moreover, in patent document 2, the communication which collects information on an integration node is required. Therefore, when the number of sensors is large or when transfer processing is necessary for communication between the sensors and the integration node, the communication load of (3) may be a problem.

  The integration unit 3 performs Bayesian filter processing on each sensor and integrates information only by communication with nearby devices (communication that does not require transfer processing). However, in the case of an information filter, the prediction process is complicated. When only map information is shared as in the identification unit 2, each sensor may only predict its own state (position, speed, etc.). On the other hand, the identification means 3 needs to predict the states of other sensors. Therefore, as the number of mobile devices increases, the amount of calculation listed in (1) increases.

  As described above, for the case of the identification means 3, there is only a Bayesian filter framework that simultaneously predicts and corrects the state of all the sensors, and the device (for example, mobile robot) in terms of calculation amount and communication amount. ) Could not cope with the increase.

  The present invention has been developed to solve the above-described problems. That is, an object of the present invention is to provide a device state identification method and apparatus that can significantly suppress an increase in calculation load and communication load even if the number of devices (for example, mobile robots) for position identification increases. is there.

According to the present invention, it comprises a plurality of devices whose state quantity is position, posture, speed, angular velocity or acceleration,
Each device has an external sensor that observes the direction or distance of the other device, and a state estimation unit that estimates and manages its own state quantity.
(A) managing its own state quantity and its variance and covariance;
(B) Only the correlation information having a high correlation between the state quantity of another device and its own state quantity is managed,
(C1) Based on the observation value by the external sensor, communicate with other devices, predict the state quantity of itself and other devices at the observation time, their variance and covariance, and the covariance between them,
(C2) Correct the state quantity of itself and other devices, its variance and covariance, and the covariance between them from the predicted value and the observation value of the external sensor,
(C3) A device state identification method is provided, in which the corrected result is shared by itself and other devices.

According to an embodiment of the present invention, the device consists of a mobile device movable, fixed or portable fixing devices,
The mobile device or fixed device has an internal sensor for observing its own position, velocity or acceleration, and each state estimation unit
(D1) Predicting the internal state, variance and covariance of the own time at the observation time when the observed value by the internal sensor was obtained,
(D2) It modifies its own state quantity and its variance and covariance from the predicted value predicted and the observed value of the internal sensor.

  The correlation information having a high correlation is a plurality of correlation information having a high absolute value of the correlation coefficient.

Each state estimating unit has a plurality of memories for storing correlation information,
Storing the correlation coefficient of the correlation information in the memory in the descending order of absolute value;
In (B), only the correlation information in the one or more memories having the higher rank is managed.

The correlation information having a high correlation is
(A) Correlation information with other devices that have been observed by external sensors, or
(B) It includes correlation information with other devices that have observed themselves with external sensors.

The correlation information having a high correlation is
(A) a covariance with a small absolute value,
(B) Covariance with devices that are not measured by the external sensor 16 among the devices that are not measured by the external sensor 16 for a certain period of time,
(C) When the internal state of the device is modified, covariance with the device that is not modified at the same time, or
(D) In the case of communication failure or ad hoc communication, covariance with a device having a communication hop count greater than a preset number is excluded.

In addition, according to the present invention, it comprises a plurality of devices having a state quantity of position, posture, speed, angular velocity or acceleration,
Each device has an external sensor that observes the direction or distance of the other device, and a state estimation unit that estimates and manages its own state quantity.
(A) managing its own state quantity and its variance and covariance;
(B) Only the correlation information having a high correlation between the state quantity of another device and its own state quantity is managed,
(C1) Based on the observation value by the external sensor, communicate with other devices, predict the state quantity of itself and other devices at the observation time, their variance and covariance, and the covariance between them,
(C2) Correct the state quantity of itself and other devices, its variance and covariance, and the covariance between them from the predicted value and the observation value of the external sensor,
(C3) A device state identification apparatus is provided, in which the corrected result is shared by itself and other devices.

  According to the above-described method and apparatus of the present invention, during the update process of (C1) and (C2), only the state of the device (self and other) involved in the observation is predicted and corrected. Even if the total number increases, the calculation amount does not increase.

  In addition, since the devices measured by the external sensor are each updated, the frequency at which each device performs the update processing does not increase even if the frequency at which the observation value is obtained in the entire system increases.

  In addition, since communication is performed only with (self and other) devices involved in the observation in (C1), the communication load is not concentrated in one place. Moreover, the physical distance between the device observed by the external sensor and the observed device is close, and there is no shield between them. For this reason, there is a high possibility that both of them will receive radio waves directly, and communication that does not require transfer processing is likely to occur. Therefore, in the case of ad hoc communication, the communication load is small.

  Each device can refer to its internal state estimation result without communication. For example, when used as a device for controlling a mobile robot, the latest estimation result without communication delay can be used for control, so that the control performance is improved.

Moreover, since each device performs prediction processing individually, it is not necessary to grasp the motion model and internal state of other devices. Therefore, a motion model and a state quantity to be estimated can be freely set for each device.

It is a figure which shows the usage example of this invention. It is a figure which shows the structural example of a mobile device. It is a figure which shows the structural example of a fixed device. It is a flowchart which shows the state identification method by a state estimation part.

  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.
As shown in this figure, the state identification device of the present invention includes a plurality of devices A, B, and C whose state quantities are position, posture, speed, or acceleration.

In this example, devices A and B are devices (for example, mobile robots) that can move within a preset area. The device C is a device (e.g., one or more fixed devices) located in a fixed or portable to preset area.
Hereinafter, when distinguishing device is required, "mobile device" movable devices, fixed or portable devices is referred to as a "fixation device".

The state identification apparatus of the present invention obtains the state quantities (position, posture, speed, or acceleration) of these devices A, B, and C. The obtained estimation result of the state quantity is used for controlling the devices A, B, and C, creating an area map, monitoring, and the like via the host control device 1.
The communication between the host control device 1 and each of the devices A, B, and C is longer than the measurement cycle of the inner world sensor 10 and the outer world sensor 16 described later, and with a low communication load frequency (for example, at intervals of 1 to 10 minutes). It is good to carry out.

FIG. 2 is a diagram illustrating a configuration example of the mobile devices A and B.
In this figure, each mobile device A, B (for example, a mobile robot) includes an internal sensor 10 and an external sensor 16.

  In this example, the internal sensor 10 is provided in the gyro sensor 11 that measures the angular velocity or acceleration of the mobile devices A and B, the GPS compass 12 that measures the position and orientation of the mobile devices A and B, and the mobile devices A and B. A wheel encoder 13 for measuring the amount of rotation of the wheel and a steering angle encoder 14 for measuring the steering angle of the wheel provided in each of the moving devices A and B, and the position of itself (that is, the moving devices A and B). Observe the speed or acceleration.

  In this example, the external sensor 16 includes a camera 17 and a laser sensor 18 that measures a surrounding shape, and observes the direction or distance of another device A, B, or C.

  In FIG. 2, each of the moving devices A and B further includes a robot drive unit 21 for movement, a vehicle control unit 22 that controls the robot drive unit 21, and an action plan unit 23 that calculates a target trajectory of each of the mobile devices A and B. The measuring unit 24 that performs sensor processing of the inner world sensor 10 and the outer world sensor 16 is provided.

  The action planning unit 23 is a real-time processing system that operates at a constant control cycle, and predicts the current or future position or posture of each of the mobile devices A and B using the on-robot prediction unit 23a, and the trajectory generation unit 23b. Is used to calculate the target trajectories of the mobile devices A and B. In addition, the current or future position or posture of each mobile device A or B is predicted as necessary, such as in the case of collision avoidance.

  The on-robot prediction unit 23a uses the state transition equation f (X) to specify a state quantity related to device control among the internal states X recorded in the state estimation unit 30 (described later) at a specified time. Calculate to make a transition. The on-robot prediction unit 23a is designed to perform prediction processing under the time constraint of real-time control.

  The vehicle control unit 22 is a real-time processing system that operates at a constant control cycle. The command value calculation unit 22a calculates a command value so that the mobile devices A and B move along the target trajectory, and the robot drive unit 21 To control.

  The communication unit 25 has a data communication function with another device A, B, or C. Specifically, a device that performs wired communication such as Ethernet (registered trademark) or USB, or wireless communication such as Bluetooth (registered trademark) or wireless LAN is used.

FIG. 3 is a diagram illustrating a configuration example of the fixed device C.
In this figure, the fixed device C includes an inner world sensor 10 and an outer world sensor 16 as in the case of the moving devices A and B.
The internal sensor 10 in this example is a GPS compass 12. In addition, the external sensor 16 includes a camera 17 and a laser sensor 18 in this example, and observes the direction or distance of another device A, B, or C.

The fixed device C includes a display unit 26 including a prediction unit 26a and a display 26b.
The prediction unit 26a uses the state transition equation f (X) to transition a state quantity related to device control from the internal state X recorded in the state estimation unit 30 (described later) to a specified time. Perform a calculation. Like the on-robot prediction unit 23a, the prediction unit 26a is designed to perform prediction processing under the time constraint of real-time control.
The display 26b displays the state quantity predicted by the prediction unit 26a.
Other configurations are the same as those in FIG.

  2 and 3, the devices A, B, and C further include a state estimation unit 30 that estimates and manages its own state quantity.

FIG. 4 is a flowchart showing a state identification method by the state estimation unit 30.
As shown in this figure, the state identification method by the state estimation unit 30 includes steps (steps) (S1) to (S7).
Hereinafter, the state estimation unit 30 of the device A will be described. In this case, “self” means device A.
The same applies to the devices B and C.

In (S1), device A manages its own (device A) state quantity and its variance and covariance.
That is, the device A manages the state quantity such as its own position, posture, velocity, acceleration, and its variance and covariance as a probability distribution.

In (S2), management is limited to correlation information in which the correlation between the state quantity of the other device B or C and the state quantity of itself (device A) is high.
That is, the device A manages only necessary information (for example, covariance) indicating the correlation between the state quantity of the other device B or C and its own state quantity. There may be a plurality of other devices B or C.

The “correlation information with high correlation” in (S2) is a plurality of correlation information having a high absolute value of the correlation coefficient, for example.
That is, each state estimation unit 30 of the device A has a plurality of memories (not shown) for storing correlation information, and the correlation coefficient of the correlation information with another device B or C has a high absolute value. Store in memory in order of rank.
Further, in (S2), only the correlation information in one or a plurality of memories having a higher stored order is managed.

The “correlation information with high correlation” in (S2) preferably includes the following (A) or (B).
(A) Correlation information with other devices that have been observed by external sensors
(B) Correlation information with other devices that have observed themselves (device A) by an external sensor.

Moreover, it is preferable to exclude the following (A) (B) (C) or (D) from "correlation information with high correlation" in (S2).
(A) Covariance with a small absolute value. This can be determined, for example, by comparing the sum or average of the absolute values of the components of the covariance matrix.
(B) Covariance with devices that have not been measured by the external sensor 16 among the devices that have not been measured by the external sensor 16 for a certain period of time.
(C) Covariance with devices that have not been modified at the same time when their internal state is modified. For example, when device A observes device B, all covariances with devices other than device B are excluded from management targets.
(D) Covariance with a device having a communication hop count greater than a preset number in the case of communication failure or ad hoc communication.

(S1) and (S2) described above are preferably performed in this order, but may be performed in parallel.
Further, (S1) and (S2) are preferably performed in real time at all times.

In (S3), the internal state, dispersion, and covariance of itself (device A) at the observation time when the observation value obtained by the internal sensor 10 is obtained are predicted.
In (S4), the state quantity, its variance and covariance of itself (device A) are corrected from the predicted value predicted and the observed value of the internal sensor 10.
(S3) (S4) is repeatedly performed every relatively short observation cycle (for example, 1 to 10 msec) by the internal sensor 10.

  In (S5), it communicates with another device A, B, or C, and the device (device A) and the other device A, B, or C at the observation time when the observation value by the external sensor 16 is obtained. Predict the state quantity, its variance and covariance, and the covariance between them.

For example, it communicates with “only the observed partner (one of B or C)” and predicts the state quantity, dispersion / covariance, and covariance between the two of the observed devices.
Furthermore, it is preferable to communicate with other devices having a high correlation with the device to predict the state quantity of the device, the variance / covariance, and the covariance between the two.
Next, the predicted value is corrected and shared as described below (S6) (S7).

The data to be communicated is, for example, the following (1), (2), or (3).
(1) A state quantity of another device predicted on another device.
(2) Variance and covariance of other devices predicted on other devices.
(3) Covariance information predicted between other devices.

The above-mentioned “covariance information between the two” in (3) is precisely a covariance × state transition matrix as follows.
(3-1) The covariance between devices A and B is represented by a covariance matrix Cov _{AB, t} .
(3-2) The predicted value Cov _{AB, t + Δt} of the covariance matrix after _Δt is expressed by the following equation (1).
Cov _{AB, t + Δt} = A _At · Cov _{AB, t} · A _Bt ^T (1)
Here, A _At is a state transition matrix of the internal state of the device A (managed by the device A), and A _Bt ^T is a state transition matrix of the internal state of the other device B (managed by the device B).
In the present invention, Cov _{AB, t} · A _Bt ^T in the formula (1) is acquired from another device by communication, and the covariance between the two is predicted.

In (S6), the state quantity of itself and other devices A, B, or C, its variance and covariance, and the covariance between them are corrected from the predicted value and the observed value of the external sensor 16.
In (S7), the corrected result is shared by itself and other devices A, B, or C.
(S5) to (S7) are repeatedly performed every relatively long observation cycle (for example, 0.05 to 1.0 sec) by the external sensor 16.

The operation of the above-described state identification device of the present invention will be described below.
Each time the devices A, B, and C obtain observation values from the inner world sensor 10 and the outer world sensor 16, they are updated in the following procedure. The timing for obtaining the observation value is determined by the measurement period of the inner sensor 10 and the outer sensor 16 and the like.

(1) Acquire information related to observation (observation time, observation value, observation noise). The observation value is defined for each type of the inner sensor 10 and the outer sensor 16. The observation noise is the magnitude of noise expected when measured by the internal sensor 10 and the external sensor 16, and is defined in the form of a dispersion matrix.
(2) Predict the internal state, dispersion, and covariance of devices A, B, and C involved in the observation at the observation time. Here, when the relative relationship between the devices A, B, and C is measured, the predicted value is acquired by communicating with the devices A, B, and C to be observed.
(3) The predicted value of the observed value is calculated from the calculated predicted value of the internal state and the observation equation defined for each of the inner sensor 10 and the outer sensor 16.
(4) The predicted value of the observed value is compared with the actual observed value to correct the internal state.

  According to the above-described method and apparatus of the present invention, in the update process of (S5) and (S6), only the state of the device A, B, or C (self and other) involved in the observation is predicted and corrected. Even if the total number of devices A and B (for example, mobile robots) increases, the amount of calculation does not increase.

  In addition, since the devices A, B, and C measured by the external sensor 16 are updated, the frequency at which the devices A, B, and C are updated does not increase even if the frequency at which observation values are obtained in the entire system increases.

  In addition, since communication is performed only with the devices A, B, or C (self and other) involved in the observation in (S5), the communication load is not concentrated in one place. Further, the physical distance between the device observed by the external sensor 16 (for example, A) and the observed device (for example, B) is short, and there is no shielding object between them. For this reason, there is a high possibility that both of them will receive radio waves directly, and communication that does not require transfer processing is likely to occur. Therefore, in the case of ad hoc communication, the communication load is small.

  Each device A, B, C can refer to its own internal state estimation result without communication. For example, when the devices A and B are used for controlling a mobile robot, the latest estimation result without communication delay can be used for control, so that the control performance is improved.

  Moreover, since each device A, B, and C performs prediction processing individually, it is not necessary to grasp the motion model and internal state of other devices A, B, or C. Therefore, the motion model and the state quantity to be estimated can be freely set for each of the devices A, B, and C.

  In the example described above, the “observed device A” performs update processing. However, the predicted values may be aggregated and updated on the “observed device B” side. However, in this case, it is necessary to transmit information related to observation (information such as observation values, observation noise, and observation equations) to the observed device B.

  Even in the conventional method in which the information of all the sensors 10 and 16 is collected in one place, it is possible to reduce the amount of calculation by predicting and correcting only the internal state of the observed device A and the observed device B.

The present invention can be applied to a Bayes filter that satisfies the following conditions.
Condition 1: Time transition (prediction process) of state quantities related to each device can be predicted separately for each device.
Condition 2: Time transition of correlation information between two devices can be predicted from information on the two devices.
Examples of satisfying the conditions 1 and 2 include a Kalman filter, an extended Kalman filter, and an unscented Kalman filter. In any case, the probability distribution is represented by mean, variance, and covariance.

However, in the case of the unscented Kalman filter, the prediction process is partially different from the example described above as follows.
(1) Summarize the current estimation results (estimated value, variance and covariance) for the devices involved in the observation.
(2) Several points (sigma points) representing the probability distribution are calculated from the estimated value, variance, and covariance of (1).
(3) Predict the sigma point at the observation time. The state transition model of both devices is used.
(4) From the predicted sigma points, the internal state, dispersion and covariance of both devices at the observation time, and covariance between both devices are obtained.

On the other hand, unlike the example described above, the following Bayes filter requires processing different from that of the present invention.
Since the particle filter represents the probability distribution as a point group and does not manage the variance and covariance, the processing is different.
The information filter represents a probability distribution by an information vector and an information matrix (an inverse matrix of a dispersion matrix). In the prediction process, since inverse matrix calculation of the information matrix is required, each device cannot perform distributed processing independently. Therefore, it is necessary to predict the internal state of all devices at the same time.

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

A and B devices (mobile devices, mobile robots),
C device (fixed device),
DESCRIPTION OF SYMBOLS 1 High-order control apparatus 10 Inner world sensor, 11 Gyro sensor, 12 GPS compass,
13 Wheel encoder, 14 Steer angle encoder,
16 external sensor, 17 camera, 18 laser sensor,
21 robot drive unit, 22 vehicle control unit, 22a command value calculation unit,
23 Action planning unit, 23a Prediction unit on robot, 23b Trajectory generation unit,
24 measurement unit, 25 communication unit, 26 display unit,
26a prediction unit, 26b display,
30 State estimation unit

Claims (8)

A plurality of devices having state, position, posture, velocity, angular velocity or acceleration as state quantities,
Each said device, the position of its own, and internal sensor for observing the velocity or acceleration, and the outside world sensor for observing the direction or distance of other said devices, and a state estimation unit for estimating its own state amount management has, by each said state estimation unit,
(A) managing own state quantity and dispersion and covariance of the state quantity ;
(B) Targeting the relatively high correlation among the correlation between the state quantity of the device itself and the state quantities of the other devices, the state quantity of the device corresponding to the target and the other Manage only the correlation information with the device state quantity ,
(C1) The external sensor receives the state quantity of the device, the distribution and covariance of the state quantity, or the covariance information between the device and the device itself from the device by communication with the other device. state quantity of its own and other of the devices in the observation time at which the observed value is obtained by, the state quantity of variances and covariances, and predicts the covariance between itself and said other of said devices,
(C2) based on a comparison of the predicted value and the observed value of the external sensor, the state of its own and other of the devices, the state quantity of variances and covariances, and themselves and between said other of said device The covariance of
(C3) The results of the modified, shared by itself and another one of the devices by transmitting to the other of said devices, state identification method of the device, characterized in that. The device comprises a movable mobile device and a fixed device in a fixed position ;
Said mobile device or the stationary device, by each said state estimation unit,
(D1) and the internal state of its own in the observation time at which the observed value is obtained by the internal sensor, and a variance and covariance of the internal state prediction,
(D2) based on the comparison of the predicted prediction value and the observed value of the internal sensor, and its own state quantity, to modify the dispersion and covariance of the state quantity, it in claim 1, wherein The device state identification method described. The device state identification method according to claim 1 , wherein the correlation has a relatively high absolute value of a correlation coefficient among the correlations . Each said state estimation unit has a plurality of memory for storing the correlation information,
Wherein stored in the memory in the order of higher rank of the correlation coefficient absolute value of each correlation,
In the (B), the state identification method of the device according to claim 3, wherein only the correlation information of the higher order one or the plurality of memory managing, characterized in that. The correlation information managed in a limited manner is:
(A) correlation information with some other of the devices be observed by the external sensor, or,
(B) including the correlation information with some other of the devices be observed itself by the external sensor, the state identification method of the device according to claim 1, characterized in that. The correlation information managed in a limited manner is:
(A) a covariance with a small absolute value,
(B) a certain time, of the device that is not measured by the external sensor, the covariance of the device itself by the external sensor of the opponent is not measured,
(C) when modifying the internal state of its own, the covariance of the device did not modify simultaneously, or,
2. The device status identification method according to claim 1, wherein (D) communication is disabled or covariance with the device having a communication hop count greater than a preset number in the case of ad hoc communication is excluded. A plurality of devices having state, position, posture, velocity, angular velocity or acceleration as state quantities,
Each said device, the position of its own, and internal sensor for observing the velocity or acceleration, and the outside world sensor for observing the direction or distance of other said devices, and a state estimation unit for estimating its own state amount management has, by each said state estimation unit,
(A) managing own state quantity and dispersion and covariance of the state quantity ;
(B) Targeting the relatively high correlation among the correlation between the state quantity of the device itself and the state quantities of the other devices, the state quantity of the device corresponding to the target and the other Manage only the correlation information with the device state quantity ,
(C1) The external sensor receives the state quantity of the device, the distribution and covariance of the state quantity, or the covariance information between the device and the device itself from the device by communication with the other device. state quantity of its own and other of the devices in the observation time at which the observed value is obtained by, the state of the dispersion and covariance, and predicts the covariance between itself and said other of said devices,
(C2) based on a comparison of the predicted value and the observed value of the external sensor, the state of its own and other of the devices, the state quantity of variances and covariances, and themselves and between said other of said device The covariance of
(C3) modified the results shared by itself and another one of the devices by transmitting to the other of the devices, that the state identification apparatus of a device according to claim. A state identification device for a plurality of devices having a position, posture, speed, angular velocity or acceleration as a state quantity,
With any one of the devices as its own device, the device includes an internal sensor, an external sensor, and a state estimation unit provided in the device,
The inner world sensor observes the position, velocity or acceleration of the device itself, the outer world sensor observes the direction or distance of the other device, and the state estimation unit is a state quantity of the device. Estimate and manage
The state estimation unit
(A) managing own state quantity and dispersion and covariance of the state quantity;
(B) Targeting the relatively high correlation among the correlation between the state quantity of the device itself and the state quantities of the other devices, the state quantity of the device corresponding to the target and the other Manage only the correlation information with the device state quantity,
(C1) The external sensor receives the state quantity of the device, the distribution and covariance of the state quantity, or the covariance information between the device and the device itself from the device by communication with the other device. Predicting the state quantity of the device itself and other devices at the observation time when the observation value is obtained, the variance and covariance of the state quantity, and the covariance between itself and the other device,
(C2) Based on the comparison between the predicted value and the observed value of the external sensor, the state quantity of itself and other devices, the variance and covariance of the state quantity, and between itself and the other devices The covariance of
(C3) A device state identification apparatus that transmits the corrected result to another device.

Images