JP6541026B2 - Apparatus and method for updating state data - Google Patents

Description

  The present invention relates to a state data updating apparatus and method for determining the state of a system including a plurality of target objects (for example, a mobile robot).

  In the related art, in order to obtain state data such as position, attitude, and velocity of a target object, observation values obtained by observing the target object with a sensor provided on the target object or a sensor provided at a known position State data representing the state of the target object is estimated. The target object is, for example, a mobile robot or an article. Such techniques are described in the following Patent Document 1, Non-Patent Document 1, Non-Patent Document 2 and the like.

  In patent document 1, the state data (position) regarding several articles | goods is acquired as follows as object of several articles to move. The cameras arranged so as to be able to photograph a plurality of articles observe the positions of the plurality of articles, and estimate and update state data indicating the positions of the plurality of articles based on the obtained observation values.

  In Non-Patent Document 1, state data (position and posture) related to a mobile robot is acquired for a single mobile robot as follows. From the mobile robot, observe the position of a landmark (for example, an artificial object as a mark) in the surrounding environment, and estimate and update state data indicating the position and attitude of the mobile robot based on the obtained observation values. ing. At this time, position estimation of the mobile robot and creation of a map near the mobile robot are simultaneously performed. Simultaneous localization and mapping is called SLAM (simultaneous localization and mapping). The obtained state data and map data are used to control the operation of a single mobile robot.

  In Non-Patent Document 2, state data (position and posture) regarding a plurality of mobile robots are acquired as described below for a plurality of mobile robots. The positions of landmarks (for example, landmarks as landmarks) in the surrounding environment and the positions of other mobile robots are observed from each mobile robot, and these observation values are collected into one central processing unit. One central processing unit estimates and updates state data indicating the position and orientation of each mobile robot based on the collected observation values. At this time, the above-described SLAM is performed.

  The following Non-Patent Document 3 describes prior art (information vector and information matrix) related to an embodiment of the present invention described later.

Patent No. 4875228

A. Costa, G. Kantor, H., et al. Choset, "Bearing-only Landmark Initialization with Unknown Data Association", in 2004 IEEE Intl. Conf. on Robotics and Automation, April 2004, pp. 1764-1770 Otani and Shukawa, “Multi-Robot Coordinated Self-Positioning and Environment Recognition Problem and Its Convergence Analysis”, 54th Joint Conference on Automatic Control, November 19 and 20, 2011 Slan et al., "Probability Robotics", Daily Communications, 2007

  However, in Patent Document 1 and Non-patent Document 1, since the surrounding environment is observed from a single camera or a mobile robot, there is a limit to the accuracy of the calculated state data.

  Further, in Non-Patent Document 2, since one central processing unit processes the collected observation values, if the number of target objects (mobile robots) is large, the computational load on the central processing unit becomes excessive.

  Therefore, it is an object of the present invention to improve the estimation accuracy of state data when obtaining state data of a system including a plurality of target objects, and to avoid concentration of calculation load on one place. It provides an apparatus and method.

In order to achieve the above-mentioned object, according to the present invention, there is provided a state data updating apparatus for determining the state of a system including a plurality of target objects,
The target object is a stationary body stationary on the ground, or a moving body moving relative to the ground,
A plurality of arithmetic communication devices respectively provided to the plurality of target objects;
Each arithmetic communication device is
An observation unit that acquires one or both of the observed values by observing one or both of an internal state of a target object provided with the arithmetic communication device and an external state relative to the target object;
According to an estimation formula for transitioning state data of the system at a past time point to state data at a new time point, state data of the system at the observation time when the observed value is acquired and error data representing an error of this state data are estimated State estimation unit,
A state update unit that updates state data of the system and updates error data based on the state data estimated by the state estimation unit and the observation value;
The update data related to the update by the state update unit is transmitted to a target object different from the target object provided with the arithmetic communication device, and updated data from the target object different from the target object provided with the arithmetic communication device The communication unit to receive
A state integration unit for obtaining new latest state data by integrating the latest state data updated by the state update unit and the update data received by the communication unit. A data updating device is provided.

  The status data update device of the present invention is configured, for example, as follows.

In the case of using a Kalman filter, the state data is a state vector directly representing the state of the system, the error data is an error covariance matrix of the state vector, and each operation communication device is a Kalman filter. Configured to use it to update the state vector and the error covariance matrix,
The state estimation unit of each arithmetic communication device estimates a state vector at the observation time when the observation value is acquired and its error covariance matrix according to the estimation formula,
The state update unit of each arithmetic communication device updates a state vector of the system and its error covariance matrix using a Kalman filter,
The communication unit of each arithmetic communication device transmits, as the update data, the state vector updated by the state update unit to a target object other than the target object provided with the arithmetic communication device.
The state integration unit of each arithmetic communication device adds together vectors obtained by multiplying a state vector updated by the arithmetic communication device and a state vector received by the communication unit by a predetermined weighting factor. Thus, the new latest state vector integrated is obtained.

  When a Kalman filter is used, weighting factors are determined in advance, and these weighting factors are respectively multiplied by the updated state vector and the state vector received by the communication unit, and the vectors obtained thereby are added together, The updated state vector and the state vector received by the communication unit can be integrated.

When using an information filter, the state data is an information vector transformed from a state vector directly representing the state of the system, and the error data is transformed from an error covariance matrix of the state vector Is an information matrix,
The state estimation unit of each arithmetic communication device estimates an information vector and an information matrix at an observation time point at which the observation value is acquired, according to the estimation formula.
The state update unit of each arithmetic communication device updates the information vector and the information matrix using the information filter based on the information vector estimated by the state estimation unit and the information matrix, and estimates the updated information vector and the information vector. Obtain an information vector difference amount regarding the difference between the information vector and the information vector, and obtain an information matrix difference amount regarding the difference between the updated information matrix and the estimated information matrix,
The communication unit of each computing communication device transmits the information vector difference amount and the information matrix difference amount obtained by the state updating unit of the computing communication device to a target object different from the target object provided with the computing communication device. ,
The state integration unit of each arithmetic communication device
An integrated new latest information vector is obtained by adding up the information vector updated by the arithmetic communication device and the information vector difference amount received by the communication unit,
The integrated new latest information matrix is obtained by adding up the information matrix updated by the arithmetic communication device and the information matrix difference amount received by the communication unit.

  In the case of using an information filter, the simple process of receiving the information matrix difference amount obtained at the time of updating in another target object, and adding the information matrix difference amount to the information matrix updated in its own target object The information matrix difference amount from another target object can be integrated with the information matrix updated in its own target object. The same applies to the information vector.

Each arithmetic communication device is
(A) acquiring the observation value by the observation unit;
(B) The state estimation unit estimates an information vector and an information matrix at an observation time point when the observation value is acquired;
(C) The state updating unit updates the information vector and the information matrix, and obtains the information vector difference amount and the information matrix difference amount,
Repeat steps (A) to (C),
When (C) is performed, the (C) is the current update process, and the previous (C) for the current is the previous update process, from the time when the previous update process is performed to the current update process The communication unit of the computing communication device adds up all of the plurality of information vector difference amounts received from another computing communication device and the information vector updated in the current update process in the period up to the time at which To obtain the updated new latest information vector, and in the period, the communication unit of the arithmetic communication device updates the plurality of information matrix difference amounts received from another arithmetic communication device and the current update processing The updated new latest information matrix is obtained by adding up the selected information matrix.

As described above, in the period from the time when the previous update process was performed to the time when the current update process was performed, the arithmetic communication device calculates the difference between all the information vector differences received from another arithmetic communication device and the current time. In one process of adding up the information vector obtained in the update process, it is possible to integrate a plurality of information vector difference amounts received from another arithmetic communication device into the information vector updated by itself.
Similarly, with regard to the information matrix, a plurality of information matrix difference amounts received from another arithmetic communication device can be integrated into the information matrix updated by itself in one process.

  The arithmetic communication device designates a transmission timing for transmitting the update data related to the update by the state update unit of its own, or selects another target object as a transmission destination of the update data.

The plurality of arithmetic communication devices include at least first, second, and third arithmetic communication devices,
When the communication unit of the first arithmetic communication device and the communication unit of the second arithmetic communication device can not directly communicate with each other:
The communication unit of the first arithmetic communication device communicates updated data concerning the update by the state update unit of the first arithmetic communication device with the communication unit of the second arithmetic communication device via the communication unit of the third arithmetic communication device. It can be sent to.

As described above, even if the communication unit of the first arithmetic communication device and the communication unit of the second arithmetic communication device can not directly communicate with each other, the first communication unit of the first arithmetic communication device may not communicate with the communication unit of the third arithmetic communication device. The update data can be transmitted from the arithmetic communication device to the second arithmetic communication device.
Therefore, even if each of the plurality of arithmetic communication devices can not communicate with all the other arithmetic communication devices, the update data obtained by each arithmetic communication device is used to integrate state data in all the other arithmetic communication devices. It becomes possible.

Further, to achieve the above-mentioned object, according to the present invention, there is provided a state data updating method for obtaining a state of a system including a plurality of target objects,
The target object is a stationary body stationary on the ground, or a moving body moving relative to the ground,
Providing a plurality of arithmetic communication devices for each of the plurality of target objects;
In each arithmetic communication device,
(A) The observation unit observes one or both of an internal state of a target object provided with the arithmetic communication device and an external state with respect to the target object, and acquires one or both observed values;
(B) State data of the system at the observation time when the observed value is acquired, and the state data according to an estimation formula that causes the state estimation unit to transition state data of the system at a past time to state data at a new time Estimate error data representing the error of
(C) The state updating unit updates the state data of the system and the error data, based on the state data estimated in (B) and the observed value.
(D) The communication unit transmits update data regarding update by the state update unit to a target object other than the target object provided with the arithmetic communication device,
(E) When update data is received by the communication unit from a target object other than the target object provided with the arithmetic communication device, the latest state data updated in (D) by the state integration unit A state data updating method is provided, characterized by obtaining new latest state data by integrating the update data received by the communication unit.

According to the present invention described above, a plurality of arithmetic communication devices are respectively provided on separate stationary bodies or mobile bodies, and each arithmetic communication device observes one or both of the internal state and the external state to obtain observation values. In the case where state data representing the state of the system including a plurality of target objects is updated based on the observed value, each arithmetic communication device transmits update data regarding the update to the other arithmetic communication devices, and itself The updated state data and update data received from another arithmetic communication device are integrated to obtain new state data.
As described above, each arithmetic communication device integrates new state data updated by itself with update data received from other arithmetic communication devices to obtain new state data. Since the new state data reflect the observation values obtained from the viewpoints of the plurality of target objects, the accuracy of the integrated new state data is improved. Therefore, the state of the system including a plurality of target objects can be determined with higher accuracy.

  Since each arithmetic communication device performs data processing for obtaining integrated new state data, concentration of data processing in one device is avoided. That is, the calculation load for calculating the state data of the system including a plurality of target objects with high accuracy can be distributed among the plurality of arithmetic communication devices.

1 shows a system that is the subject of the present invention. Fig. 3 shows a status data update device according to an embodiment of the invention, which comprises a plurality of arithmetic communication devices. It is a flowchart of the state data update method at the time of using a Kalman filter. It is a flowchart of the state data update method at the time of using an information filter. The state data update apparatus at the time of providing an observed value integration part is shown.

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

  FIG. 1 shows a system 20 that is the subject of the present invention. The system 20 includes a plurality of target objects 3. FIG. 1 is a view of a plurality of target objects 3 viewed from above in the vertical direction. Further, although the example of FIG. 1 shows the case where three target objects 3 exist, according to the present invention, the number of target objects 3 constituting the system 20 is four or more even if the number is two. It may be

  Each target object 3 is a stationary body stationary on the ground, or a moving body moving relative to the ground. The plurality of target objects 3 may include both a stationary body and a moving body. In this example, each target object 3 is a mobile robot that moves relative to the ground surface. Each mobile robot 3 may have wheels and travel on the ground surface by being rotationally driven while contacting the ground. Alternatively, each mobile robot 3 may have a walking leg and move relative to the ground surface by the action of the walking leg.

  FIG. 2 shows a state data updating device 10 according to an embodiment of the present invention, which comprises a plurality of arithmetic communication devices 5.

  The state data update device 10 is a group of devices for determining the state of the system 20 including a plurality of target objects 3. The device group is a plurality of arithmetic communication devices 5 provided to the plurality of target objects 3 respectively. That is, the state data update device 10 includes a plurality of arithmetic communication devices 5 as a device group. The status of the system 20 is determined by this group of devices.

  Although the arithmetic communication device 5 provided for one target object 3 will be described based on FIG. 2, the same configuration as that described below is also applied to the arithmetic communication device 5 provided for each of the other target objects 3. And perform the same processing as the processing described below. In the following, the target object 3 of its own means the target object 3 provided with the arithmetic communication device 5 described below, and another target object 3 is the arithmetic communication described below A target object 3 different from the target object 3 provided with the device 5 is meant.

  The arithmetic communication device 5 includes an observation unit 7, a state estimation unit 9, a storage unit 11, a state update unit 13, a communication unit 15, and a state integration unit 17.

The observation unit 7 observes one or both of the internal state of the target object 3 of its own and the external state of the target object 3 of its own. Thereby, the observation unit 7 acquires observation values of one or both of the internal state of the target object 3 of its own and the external state of the target object 3 of its own. The observation unit 7 acquires such observation values at constant cycle times.
When the observation unit 7 includes a camera as an external sensor 7b described later, the cycle time described above is, in one example, a value of 1 ms (milliseconds) or more and a value of 300 ms or less, and in another example, 10 ms or more The value is 200 ms or less, and in yet another example, it is 50 ms or more and 200 ms or less (e.g., 100 ms).
When the observation unit 7 includes a laser sensor as an external sensor 7b described later, the cycle time described above is, in one example, a value of 1 ms or more and 100 ms or less, and in another example, 5 ms or more and 100 ms or less In another example, the value is 10 ms or more and 50 ms or less (e.g., 20 ms).

  The internal state is a value indicating a state directly related to the target object 3 of its own. In the present embodiment, the internal state includes the position, velocity, acceleration, and attitude of the target object 3 (for example, a mobile robot) relative to the ground. However, the internal state may be any one of these position, velocity, acceleration, and posture (for example, position), or any combination selected from these position, velocity, acceleration, and posture (for example, , Position, velocity and attitude). Also, according to the present invention, the internal state may be another value.

  When the arithmetic communication device 5 observes an external state (for example, the state of another target object 3) by the observation unit 7 of the target object 3 of its own, this external state is observed from the outside of the other target object 3. It becomes the value which shows the state of the said another target object 3 measured by. This external state includes, for example, one (for example, position) or both of the position and velocity of another target object 3 (for example, a mobile robot) with respect to its own target object 3 or the ground on which the observation unit 7 is provided. However, according to the present invention, the external state that the arithmetic communication device 5 observes by the observation unit 7 of the target object 3 of its own may be another value indicating the state of the external of the target object 3 of its own.

  The observation unit 7 includes an internal sensor 7a that observes an internal state, and an external sensor 7b that observes an external state. In the present embodiment, the internal sensor 7a measures the position, velocity, acceleration, and posture of the target object 3 (mobile robot) relative to the ground as observed values, an acceleration measuring unit, and a posture measuring unit. including. In the present embodiment, the external sensor 7b measures the position and velocity of another mobile robot 3 as an observation value. In one example, the external sensor 7b processes the measurement data obtained by the laser sensor (for example, LRF) that measures the shape of the surroundings and the laser sensor, and one of the position and velocity of another target object 3 (for example, position) or And a data processing unit for obtaining both. In another example, the outside sensor 7b is a camera that acquires an image of the surroundings, and data processing that processes image data obtained by the camera to obtain one or both (for example, position) or both of another target object 3 Including the department.

  The observation unit 7 repeatedly performs observation at time intervals, and acquires observation values of the internal state described above and observation values of the external state described above each time observation is performed.

  The state estimation unit 9 estimates state data representing the state of the system 20 at the observation time and error data representing an error of the state data each time the observation unit 7 performs observation. This estimation is performed according to the estimation formula. The estimation equation is a calculation equation for transitioning the state data of the system 20 at a past observation time point to the state data at a new observation time point. The estimation formula will be described in detail later. Every time the observation unit 7 performs observation, the state estimation unit 9 calculates the state data and its error data at the observation time point according to the estimation formula, the latest state data and the latest error data stored in the storage unit 11. presume.

  The storage unit 11 stores the state data estimated by the state estimation unit 9. In the present embodiment, the storage unit 11 stores the latest state data updated by the state update unit 13 as described later, separately from the estimated state data. At the initial time, the storage unit 11 may store the latest state data known in advance. Further, in the present embodiment, the storage unit 11 also stores error data representing an error of the state data. At an initial time, error data of the latest state data known in advance may be stored in the storage unit 11 as the latest error data.

  The state update unit 13 updates the state data of the system 20 based on the state data estimated by the state estimation unit 9 and the observation value obtained by the observation unit 7 each time the observation unit 7 performs observation. The error data of this state data is updated. As a result, the updated state data and error data are stored in the storage unit 11 as the latest state data and error data. That is, in the storage unit 11, these latest state data and error data become new latest state data and error data. At this time, the latest state data and error data stored previously may be deleted from the storage unit 11.

  The communication unit 15 transmits update data (to be described in detail later) regarding the update by the state update unit 13 to the communication unit 15 of the arithmetic communication device 5 of another target object 3, and the arithmetic communication device of the other target object 3 The update data is received from the communication unit 15 that the communication 5 has.

  The state integration unit 17 integrates the latest state data updated by the state update unit 13 in the target object 3 of itself and the update data from the communication unit 15 of another target object 3 to obtain a new latest state. Determine state data. The new latest state data is stored in the storage unit 11. That is, in the storage unit 11, this latest state data becomes the new latest state data. At this time, the latest state data previously stored may be deleted from the storage unit 11. The integration method by the state integration unit 17 will be described in detail later.

  According to this embodiment, update data can also be transferred as follows. In this example, the plurality of arithmetic communication devices 5 constituting the state data updating device 10 include at least the first, second and third arithmetic communication devices 5. When the communication unit 15 of the first arithmetic communication device 5 and the communication unit 15 of the second arithmetic communication device 5 can not directly communicate with each other due to an obstacle or another factor, the communication of the first arithmetic communication device 5 The unit 15 transmits update data regarding update by the state update unit 13 of the first arithmetic communication device 5 to the communication unit 15 of the second arithmetic communication device 5 via the communication unit 15 of the third arithmetic communication device 5. In this case, the state integration unit 17 of the second arithmetic communication device 5 can transmit data, and the latest data updated by the state update unit 13 in the target object 3 of its own and the update data from the first arithmetic communication device 5 By integrating with the state data, new latest state data is obtained.

  Further, according to the present embodiment, update data can be transmitted as follows. In this example, the plurality of arithmetic communication devices 5 constituting the state data updating device 10 include at least the first, second and third arithmetic communication devices 5. It is assumed that the observation unit 7 (external sensor 7b) of the first arithmetic communication device 5 observes the target object 3 provided with the second arithmetic communication device 5 and obtains the observation value of the target object 3 Do. In this case, the state update unit 13 of the first arithmetic communication device 5 updates the state data and the error data as described above based on this observation value (and other observation values obtained by the observation unit 7). Then, the communication unit 15 of the first arithmetic communication device 5 transmits the update data regarding this update to the communication unit 15 of the third arithmetic communication device 5 different from the first and second arithmetic communication devices 5. . In this case, the state integration unit 17 of the third arithmetic communication device 5 updates the update data from the first arithmetic communication device 5 and the latest state data updated by the state update unit 13 in the target object 3 of its own. By integrating, new new state data is obtained.

  Arithmetic communication device 5 described above may be configured to use a Kalman filter, or may be configured to use an information filter. The contents described above are applied to both the case where a Kalman filter is used and the case where an information filter is used.

(Operational communication device using Kalman filter)
A case where the arithmetic communication device 5 uses a Kalman filter will be described.

  In this case, the above-mentioned state data is a state vector that directly represents the state of the system 20. The state vector is composed of a plurality of elements indicating the respective states of the plurality of target objects 3 constituting the system 20. The state vector u is expressed by the following equation (1).

  In equation (1), the symbol T indicates that u is a column vector (hereinafter, the same applies to other T attached to the vector), and the indices 1, 2,. It is a continuous identification number (positive integer) for identifying the target objects 3 from one another. N is equal to the number of target objects present in the system 20 (and so forth).

Further, in the present embodiment, the state vector u _i (i is an integer from 1 to N) of each target object 3 is expressed by the following equation (2). In the following, the suffix i of each symbol or code means the above-described identification number for identifying the target object 3. For example, the target object 3i indicates a target object identified by i.

In Formula (2), each symbol is as follows.
Each of x _i , y _i and z _i indicates the position of the target object 3i. That is, x _i , y _i and z _i are the x coordinate, y coordinate and z coordinate of the target object 3i, respectively. The x coordinate, the y coordinate, and the z coordinate are coordinates on the x coordinate axis, the y coordinate axis, and the z coordinate axis of the stationary coordinate system fixed on the ground, respectively.
θ _i , φ _i and α _i are angles representing the attitude of the target object 3i with respect to the above-mentioned stationary coordinate system.
One black circle (dot) on the top of x _i , y _i , z _i , θ _i , φ _i , α _i means a one-time derivative of time (ie velocity and angular velocity, respectively), x _{i, y} i, 2 two black circles attached to the top of the _{z i} (dot dot ..) means second derivative of time (i.e., acceleration).

  Further, the above-mentioned error data is the error covariance matrix P of the above-mentioned state vector u. This error covariance matrix P is expressed by the following equation (3).

In equation (3), P _i (i is an integer from 1 to N) is an error covariance matrix of the state vector u _i of the target object 3i. Also, P _{i, j} is a partial covariance matrix between the target object 3i and the target object 3j (j is an integer from 1 to N excluding i).

  Hereinafter, in the case where a Kalman filter is used, the arithmetic communication device 5 provided for one target object 3 will be described, but the arithmetic communication device 5 provided for each of the other target objects 3 has the same configuration as that described below. It has a configuration and performs the same processing as the processing described below.

The state estimation unit 9 estimates a state vector u representing a state of the system 20 at the observation time k and an error covariance matrix P of the state vector u at the observation time k each time the observation unit 7 performs observation. The estimation of the state vector u is performed according to the estimation equation represented by the following equation (4). The estimation of the error covariance matrix P is performed according to the estimation equation represented by the following equation (5).

u _k ′ = f (u _k−1 , β _k ) (4)

P _k '= F _k P _k F _k ^T + Q _k (5)

In this formula (4), subscript k of each symbol and (k-1) are natural numbers and indicate time points (the same applies hereinafter).
u _k-1 is the latest state vector u stored in the storage unit 11, and the subscript (k-1) of u is based on the observed value obtained at the past time point (k-1) , Indicates that it is an updated state vector u.
Further, β _k is an input value obtained at time k, which is the above-mentioned internal state. For example, β _k is the velocity and acceleration of the target object 3 and is measured by the acceleration measurement unit and the velocity measurement unit of the target object 3. However, β _k may be another value.

In equation (5), Q _k is an error covariance matrix of process noise, and F _k is a coefficient matrix represented by the following equation (6).

Furthermore, the state estimation unit 9 performs matching between the state vector u _k ′ estimated as described above and the observation value z acquired by the observation unit 7. For example, when the observation value z acquired by the observation unit 7 includes the positions x _i , y _i and z _{i of} another target object 3 _i , these positions x _i , y _i and z _i are states Identify which value in the vector u _k 'corresponds to. Such matching can be performed by chi-square test or other methods.

After that, the state update unit 13 associates the state vector u _k ′ and the error covariance matrix P _k ′ estimated by the state estimation unit 9 with the observed values (the values included in u _k ′ by the above-described matching). The state vector u and the error covariance matrix P are updated based on the observation values. This update is performed by the following equations (7) and (8).

u _k = u _k '+ K _k (z _k -H _k u _k ') (7)

P _k = (I−K _k H _k ) P _k ′ (8)

In equation (7), u _k is the updated state vector u at time k (observation time k). In equation (8), P _k is the updated error covariance matrix P of time k.
Further, in Equations (7) and (8), H _k is a coefficient matrix represented by the following Equation (9).

In equation (9), h is an observation equation represented by the following equation (10).

z _k = h (u _k ) (10)

In this equation (10), h is a function of the state vector u _k of the system 20, z _k is an observation value obtained by the observation unit 7 at the observation time point k, and the observation provided in its own target object 3 It is the above-mentioned external state which the part 7 observed.

In Equations (7) and (8), K _k is a Kalman gain and is expressed by the following Equation (11).

K _k = P _k ′ H _k ^T S _k ⁻¹ (11)

Further, in the equation (11), S _k is expressed by the following equation (12).

S _k = H _k P _k ′ ⁻¹ H _k ^T (12)

The state vector u _k and the error covariance matrix P _k of the observation time point k updated as described above are stored in the storage unit 11 as a new latest state vector u _k and an error covariance matrix P _k , respectively.

In addition, the arithmetic communication device 5 sets the state vector u _k at the observation time point k updated by the state update unit 13 of itself or the state vector u _k and the error covariance matrix P _k thereof as the update data described above. The communication unit 15 of its own transmits to the communication unit 15 of another target object 3.

  On the other hand, when the arithmetic communication device 5 receives the updated state vector u from the communication unit 15 of another target object 3 by the communication unit 15 of its own, this state vector u is as follows: The latest state vector u stored in its own storage unit 11 is integrated.

Regarding this, according to the present embodiment, the state integration unit 17i of the arithmetic communication device 5i (here, i is the identification number of the target object 3 provided with the arithmetic communication device 5. The same applies to the following). Received from the communication unit 15j of another target object 3j (where i ≠ j), and the latest state vector u (ie, the latest state vector u stored in the storage unit 11i) obtained by the communication device 5i A new integrated state vector u is determined by adding together vectors obtained by multiplying the state vector u by a predetermined weighting factor. That is, the state integration unit 17i obtains the integrated new latest state vector u _C according to the following equation (13a).

u _C = w _i × u _S + w _j × u _D (13a)

In this equation (13a), u _C is the integrated new latest state vector u, and u _S is the latest state vector u obtained by the arithmetic communication device 5i and stored in the storage unit 11i. , U _D are state vectors u received by the arithmetic communication device 5 i from the communication unit 15 of another target object 3 j. Also, w _i and w _j are weighting factors, and w _i + w _j = 1. w _i and w _j may be preset, and are, for example, 1/2, respectively. Alternatively, w _i may be larger than w _j and smaller than one.

The latest state vector u _C integrated as described above is stored in the storage unit 11 i as a new latest state vector u.

The state integration unit 17 of the arithmetic communication device 5 may further perform integration of the error covariance matrix in addition to the integration of the state vector. That is, the state integration unit 17i of the arithmetic communication device 5i is different from the latest error covariance matrix P (that is, the latest error covariance matrix P stored in the storage unit 11i) obtained by the arithmetic communication device 5i. New error covariance matrix obtained by multiplying each of the error covariance matrices P received from the communication unit 15j of the target object 3j (where i.noteq.j) with a predetermined weighting factor, thereby adding a new integrated Find the most recent error covariance matrix P. That is, the state integration unit 17i obtains a new integrated latest error covariance matrix P _C according to the following equation (13b).

P _C = W _i × P _S + W _j × P _D (13 b)

In this equation (13b), P _C is a new integrated error covariance matrix P, and P _S is the latest error covariance matrix P obtained by the arithmetic communication device 5i and stored in the storage unit 11i. P _D is the error covariance matrix P that the arithmetic communication device 5 i receives from the communication unit 15 of another target object 3 j. W _i and W _j are weighting coefficients, and W _i + W _j = 1. W _i and W _j may be preset, and are, for example, 1/2. Alternatively, W _i may be larger than W _j and smaller than one.

The latest error covariance matrix P _C integrated as described above is stored in the storage unit 11 i as a new latest error covariance matrix P.

  FIG. 3 is a flowchart of a state data update method according to an embodiment of the present invention. FIG. 3 shows the case where a Kalman filter is used. This state data update method is performed by the above-described arithmetic communication device 5.

  In step S1, the observation unit 7 acquires the above-described observed value.

In step S2, the state estimation unit 9 estimates a state vector u representing the state of the system 20 at the observation time point k at which the observation value is obtained in step S1 and an error covariance matrix P of the state vector u at the observation time point k. . Thereby, the estimated state vector u _k 'and the estimated error covariance matrix P _k ' are obtained.

In step S3, the matching between the state vector u _k ′ obtained in step S2 and the observed value z _k obtained in step S1 is performed. This specifies which value in the state vector u _k ′ corresponds to the observation value z _k .

In step S4, as described above, the state updating unit 13 calculates the state vector u _k ′ and the error covariance matrix P _k ′ estimated in step S2, and the observed value z _k obtained in step S1. Update the state vector u and the error covariance matrix P. The updated state vector u and error covariance matrix P are stored in the storage unit 11 as the latest state vector u and error covariance matrix P.

In step S5, the arithmetic communication device 5 transmits the state vector u _k at the observation time k updated by the state update unit 13 of itself to the communication unit 15 of another target object 3 by the communication unit 15 of itself.

In step S6, the arithmetic communication device 5 determines whether the updated state vector u _k has been received from the communication unit 15 of another target object 3 by the communication unit 15 of its own target object 3 by the present time Do. If the result of this determination is affirmative, the process proceeds to step S7, and if not, step S6 is performed again after a predetermined time, or the process returns to step S1.

In step S7, the state integration unit 17 integrates the received state vector u and the latest state vector u stored in its own storage unit 11 as described above. The integrated state vector u _C is stored in the storage unit 11 as a new latest state vector u. After step S7, the process returns to step S1 to repeat the above-described processing.

  When the operation of the state data update device 10 is stopped while repeating the above-described process, the flow of FIG. 3 is ended.

(Operational communication device using information filter)
A case where the arithmetic communication device 5 uses an information filter will be described. In addition, the point which is not described below is the same as the case where the above-mentioned Kalman filter is used. In particular, the same symbols as described above indicate the same as described above.

  In this case, the above-mentioned state data is an information vector ζ converted from the state vector u represented by the above-mentioned equation (1). The error data is an information matrix Λ transformed from the error covariance matrix P of the state vector u.

Specifically, the information matrix Λ and the information vector ζ are expressed by the following equations (14) and (15), respectively.

Λ = P ^-1 (14)

ζ = Λ u ... (15)

  Hereinafter, in the case where an information filter is used, the arithmetic communication device 5 provided for one target object 3 will be described, but the arithmetic communication device 5 provided for each of the other target objects 3 also has the configuration described below. It has the same configuration and performs the same processing as the processing described below.

The state estimation unit 9 estimates the information matrix Λ _k ′ and the information vector ζ _k ′ at the observation time point k at which the observation unit 7 acquires the observation value according to the estimation formula. The estimation formula is expressed by the following formulas (16) and (17).

Λ _k ′ = (F _k Λ _k ^{−1 −1} F _k ^T + Q _k ) ⁻¹ (16)

ζ _k '= Λ _k ' u _k '(17)

In this equation (16), u _k ′ is the state vector u estimated by the equation (4) described above. Further, Λ _k-1 is the latest information matrix stored in the storage unit 11. Λ _k ′ and ζ _k ′ are an information matrix and an information vector of the estimated observation time point k, respectively. Other symbols indicate the same as the same symbols described in the case of the Kalman filter.

Furthermore, the state estimation unit 9 matches the information vector ζ _k ′ (corresponding to the state vector u _k ′) estimated as described above with the observation value acquired by the observation unit 7. For example, when the observation value acquired by the observation unit 7 includes the positions x _i , y _i and z _{i of} another target object 3 _i , these positions x _i , y _i and z _i are information vectors Identify which value in ζ _k '(corresponding to the state vector u _k ') corresponds to. Such matching can be performed by chi-square test or other methods.

  For example, with the chi-square test, matching can be performed as follows.

In the state estimation unit 9, the observed value z _i of another target object 3 (for example, the position and velocity of the target object 3i) obtained by observation of the external sensor 7b corresponds to the value related to another known target object 3j. Determine whether or not (i = j). Note that the value related to another known target object 3 _j is a value (u _j ) included in the latest state vector u corresponding to the latest information vector ζ stored in the storage unit 11. The latest state vector u is also stored in the storage unit 11.

State estimation unit 9, for observations z _i to determine whether it corresponds to values for known another object 3j (i = either a j), whether the following inequality (18) is satisfied To judge. If the state estimation unit 9 determines that the inequality (18) is satisfied, it determines that the observation value z _i corresponds to the value of another known target object 3 _j . That is, the state estimation unit 9 determines that the observation value z _i is the value of another known target object 3 _j .

A ² μ μ _ji ^T S _k ^-1 μ _ji (18)

In this equation (18), A is a preset value (chi value). μ _ji is expressed by the following equation (19).

μ _ji = p _j −z _i (19)

Here, p _j is a vector (x _j , y _j , z _j ) ^T indicating the position of another known target object 3 _j , and z _i is an observation value z _i obtained at the current observation time point k This is a vector indicating (the position of the target object 3i).

After the matching, the state updating unit 13 associates the information matrix Λ _k ′ and the information vector ζ _k ′ estimated by the state estimating unit 9 with the observed values (the values included in u _k ′ by the above-mentioned matching) The information matrix Λ and the information vector ζ are updated based on the observation values. This update is performed by the following equations (20) and (21).

Λ _k = Λ _k '+ gΛ _{k, d} (20)

ζ _k = ζ _k '+ g ζ _{k , d} (21)

In this equation (20), Λ _{k, d} is an information matrix difference amount relating to the difference between the updated information matrix Λ _k and the estimated information matrix Λ _k ′. In equation (21), ζ _{k, d} is an information vector difference amount relating to the difference between the updated information vector ζ _k and the estimated information vector ζ _k ′. Λ _{k, d} and ζ _{k, d} are expressed by the following equations (22) and (23), respectively.

Λ _{k, d} = H _k ^T R ^-1 H _k (22)

ζ _{k, d} = H _k ^T R ^-1 (z _k- h (u _k ') + H _k u _k ') (23)

In Equations (22) and (23), R is an error covariance matrix of observation noise regarding the observation unit 7.

When updating the information matrix Λ and the information vector ζ, the state updating unit 13 also obtains the information matrix difference amount Λ _{k, d} and the information vector difference amount ζ _{k, d} described above.

The information matrix Λ _k and information vector ζ _k at the observation time point k updated as described above are stored in the storage unit 11 as new latest information matrix Λ and information vector ζ, respectively.

In addition, the arithmetic communication device 5 separates the information matrix difference amount Λ _{k, d} and the information vector difference amount ζ _{k, d} obtained by the state updating unit 13 of its own by the communication unit 15 of its own as the above-mentioned update data. To the communication unit 15 of the target object 3 of FIG.

On the other hand, when the arithmetic communication device 5 receives the information matrix difference amount Λ _{k, d} and the information vector difference amount ζ _{k, d} from the communication unit 15 of another target object 3 by its own communication unit 15, Perform integration processing. That is, the state integration unit 17 of the arithmetic communication device 5 adds up the received information matrix difference amount Λ _{k, d} and the latest information matrix 記憶 stored in its own storage unit 11 to integrate the latest By obtaining the information matrix Λ _C and adding the received information vector difference amount ζ _{k, d} to the latest information vector 記憶 stored in the storage unit 11 of itself, the integrated latest information vector ζ _Ask for _C.

That is, the state integration unit 17 obtains a new integrated latest information matrix Λ _C and an information vector ζ _C according to the following equations (24) and (25).

Λ _C = Λ _S + Λ _{k, d} (24)

ζ _C = ζ _S + ζ _{k, d} (25)

In this equation (24), Λ _C is a new integrated latest information matrix, and Λ _S is the latest information matrix obtained by the arithmetic communication device 5 and stored in the storage unit 11. In Expression (25), ζ _C is a new integrated latest information vector, and ζ _S is the latest information vector calculated by the arithmetic communication device 5 and stored in the storage unit 11.

  FIG. 4 is a flowchart of a state data update method according to an embodiment of the present invention. FIG. 4 shows the case where an information filter is used. This state data update method is performed by the above-described arithmetic communication device 5.

  In step S11, the observation unit 7 acquires the above-described observed value.

In step S12, the state estimation unit 9 estimates the information matrix Λ and the information vector ζ at the observation time point k at which the observation value is acquired in step S11. Thereby, an estimated information matrix Λ _k ′ and an estimated information vector ζ _k ′ are obtained.

In step S13, the state vector u _k 'corresponding to the information vector ζ _k ' obtained in step S12 is matched with the observation value z _k obtained in step S1. This identifies which value in the state vector u _k '(ie, information vector 情報_k ') corresponds to the observed value z _k .

In step S14, as described above, the state updating unit 13 based on the information matrix Λ _k 'and the information vector ζ _k ' estimated in step S3 and the observed value z _k obtained in step S11, as described above. Update firewood and information vector firewood. The updated information matrix Λ and information vector ζ are stored in the storage unit 11 as the latest information matrix Λ and information vector ζ.

In step S14, the state updating unit 13 also obtains the information matrix difference amount Λ _{k, d} and the information vector difference amount ζ _{k, d} described above.

In step S15, the arithmetic communication device 5 uses the communication unit 15 of its own to calculate another target object 3 using the information matrix difference amount Λ _{k, d} and the information vector difference amount ζ _{k, d} obtained by the state updating unit 13 of its own. To the communication unit 15 of

In step S16, from the communication unit 15 of another target object 3, the arithmetic communication device 5 makes the information matrix difference amount Λ _d and the information vector difference amount ζ _d up to the present time by the communication unit 15 of its own target object 3 Determine if it has been received. If the result of the determination is affirmative, the process proceeds to step S17. If not, step S16 is performed again after a predetermined time, or the process returns to step S11.

In step S17, the state integrating unit 17 integrates the received information matrix difference amount Λ _d and the latest information matrix Λ stored in the storage unit 11 of the own unit as described above. Further, in step S17, the state integrating unit 17 integrates the received information vector difference amount と_d with the latest information vector ζ stored in the storage unit 11 of the own unit as described above. The information matrix Λ _C and information vector ζ _C obtained by integration are stored as new latest information matrix Λ and information vector ζ. After step S17, the process returns to step S11, and the above-described process is repeated.

  When the operation of the state data update device 10 is stopped while repeating the above-described process, the flow of FIG. 4 is ended.

A case where the above integration is performed collectively will be described. Here, when step S14 is performed, this step S14 is set as the update process of this time, and the previous step S14 of this time is set as the update process of the previous time. When subjected to this step S14, a plurality of times, when the communication unit 15 of the arithmetic communication device 5 receives the information matrix differential amount lambda _d and information vector differential amount zeta _d from another computing communication device 5, the current In step S17 immediately after step S14, the state integration unit 17 integrates together as follows.

In the state integration unit 17, the communication unit 15 of the arithmetic communication device 5 is another in the period D from the time when the previous update process (step S 14) was performed to the time when the current update process (step S 14) is performed. A new integrated latest by combining all the information matrix difference amounts Λ _d received from the arithmetic communication device 5 with the new latest information matrix で obtained by the update processing according to the following equation (26) Find the information matrix Λ _C of. In addition, the state integration unit 17 determines, during the period D, all the information vector difference amounts ζ _d received from the other arithmetic communication devices 5 by the communication unit 15 of the arithmetic communication device 5 and the new latest obtained by the current update processing. Is added according to the following equation (27) to obtain the integrated new latest information vector ζ _C.

Λ _C = Λ + Λ _{d 1} + Λ _{d 2} + · · · + Λ _dm (26)

ζ _C = ζ + ζ _{d 1} + ζ _{d 2} + · · · + _{dm dm} (27)

In equation (26), Λ _{d 1} , Λ _{d 2} ,... _{Dm d m} are the information matrix difference amount Λ _d received for the first time after the previous step S 14, and the information matrix difference amount Λ received for the second time _{d 1} ,..., the information matrix difference amount Λ _d received at the m th time (m is an integer of 2 or more). However, in the case of m = 2, the formula (26) becomes _{Λ C = Λ + Λ d1 +} Λ d2.
Similarly, in equation (27), ζ _{d 1} , ζ _{d 2} ,... Ζ _{d m} are the first received information vector difference amount ζ _d after the previous step S 14, and the second received information vector The difference amount ζ _d ... Is the information vector difference amount ζ _d received at the m-th time (m is an integer of 2 or more). However, in the case of m = 2, the formula (27) becomes _{ζ C = ζ + ζ d1 +} ζ d2.

According to the embodiment of the present invention described above, the plurality of arithmetic communication devices 5 are respectively provided on separate stationary bodies or mobile bodies, and each arithmetic communication device 5 observes one or both of the internal state and the external state. In the case where state data representing the state of the system 20 including the plurality of target objects 3 is updated based on the observation values, each operation communication device 5 performs another operation on the update data related to the update. It transmits to the communication device 5 and integrates the state data updated by itself and the update data received from other arithmetic communication devices 5 to obtain new state data.
As described above, each arithmetic communication device 5 integrates state data updated by itself and update data received from other arithmetic communication devices 5 to obtain new state data. Since the observation values obtained from the viewpoints of the plurality of target objects 3 are reflected in the new state data, the accuracy of the integrated new state data is improved. Therefore, the state of the system 20 including the plurality of target objects 3 can be determined with higher accuracy.

  Since each arithmetic communication device 5 performs data processing for obtaining integrated new state data, concentration of data processing in one device is avoided. That is, the calculation load for calculating the state data of the system 20 including the plurality of target objects 3 with high accuracy can be distributed among the plurality of arithmetic communication devices 5. Therefore, in each arithmetic communication device 5, the latest state data (and error data) can be calculated in a short time.

  The present invention is not limited to the above-described embodiment, and it goes without saying that various modifications can be made without departing from the scope of the present invention. For example, any of the following other embodiments 1 to 3 may be adopted, and a plurality of other embodiments 1 to 3 may be arbitrarily combined and adopted. In this case, points not described below are the same as described above.

(Other embodiment 1)
In the above description, each time the arithmetic communication device 5 updates the state data of the system 20 by the state updating unit 13 of its own, the update data is a target different from the target object 3 on which the arithmetic communication device 5 is provided. It was sending to Object 3. However, according to the present invention, each arithmetic communication device 5 may designate a transmission timing for transmitting update data regarding the update by the state update unit 13 of itself. That is, each arithmetic communication device 5 designates a transmission timing for transmitting the update data updated by its own state update unit 13 to the target object 3 different from the target object 3 provided with the arithmetic communication device 5 It is also good. In this case, each arithmetic communication device 5 is different from the update data (latest update data) updated by its own state update unit 13 from the target object 3 provided with the arithmetic communication device 5 at the designated transmission timing. To the target object 3 of.

  For example, each arithmetic communication device 5 is based on the position or velocity (external state) of another object 3 observed by the observation unit 7 of the object 3 provided with the arithmetic communication device 5. Determine if 3 is stationary. Here, when the arithmetic communication device 5 determines that another target object 3 is stationary, the time of the cycle in which the state updating unit 13 of itself updates the state data of the system 20 (that is, the observation unit 7 observes Designate a time longer than the above-mentioned cycle time) to perform a transmission cycle (transmission timing). In this way, when another target object 3 is at rest, the need for measuring the position or velocity of another target object 3 with high accuracy is reduced, so the position or velocity of the other target object 3 is reduced. The frequency of transmitting the observation value of the target object 3 to the other target object 3 or another target object 3 is reduced.

(Other embodiment 2)
Each arithmetic communication device 5 may select another target object 3 as a transmission destination of update data related to the update by the state update unit 13 of itself. For example, each computing communication device 5 may select another target object 3 located within a set distance from its own target object 3 provided with the computing communication device 5 as a transmission destination. In this case, next, the arithmetic communication device 5 transmits the update data to one or more other target objects 3 selected as the transmission destination.

When each arithmetic communication device 5 uses a Kalman filter, in step S5 described above, the arithmetic communication device 5 is another target object located within a set distance from its own target object 3 on which the arithmetic communication device 5 is provided. Select 3 as the destination. Next, in step S5 described above, the arithmetic communication device 5 selects another state object u 3 that has selected the state vector u _k (that is, updated data) at the observation time k updated by its state update unit 13 as the transmission destination. Is transmitted to the communication unit 15 by the communication unit 15 of its own.

  The arithmetic communication device 5 is within a set distance from the target object 3 of itself based on the latest state data (data including the position of another target object 2) stored in the storage unit 11 of the target object 3 of its own. Another target object 3 located at may be selected as the destination. This point is the same as in the case of using an information filter as described later.

When each arithmetic communication device 5 uses an information filter, in step S15 described above, the arithmetic communication device 5 is another object located within the set distance from its own target object 3 on which the arithmetic communication device 5 is provided. Select an object as the destination. Next, in step S15 described above, the arithmetic communication device 5 transmits the information matrix difference amount Λ _{k, d} and the information vector difference amount ζ _{k, d} (that is, the update data) obtained by the state updating unit 13 of itself. It transmits to the communication part 15 of another target object 3 selected as previous by the communication part 15 of self.

  Thus, each arithmetic communication device 5 selects another object 3 located within the set distance from the object 3 provided with the arithmetic communication device 5 as a transmission destination. On the other hand, each computing communication device 5 can be configured not to select another target object 3 located away from the target object 3 provided with the computing communication device 5 by more than the set distance as a transmission destination. Thus, each arithmetic communication device 5 sets another target object 3 that can be observed directly from the target object 3 provided with the arithmetic communication device 5 as a transmission destination, and directly from the object object 3 provided with the arithmetic communication device 5 It does not use update data as a transmission destination to another target object 3 in a distant place that can not be observed. As a result, the transmission destination of the update data can be reduced, and the update data can be efficiently reflected on the latest state data of the system 20.

(Other embodiment 3)
For example, as shown in FIG. 5, each arithmetic communication device 5 may further include the observation value integration unit 19.

  Hereinafter, although the arithmetic communication device 5 provided to one target object 3 will be described, the arithmetic communication devices 5 provided to the other target objects 3 also have the same configuration as the configuration described below, The same processing as the processing described below is performed. In the following, the target object 3 of its own means the target object 3 provided with the arithmetic communication device 5 described below, and another target object 3 is the arithmetic communication described below A target object 3 different from the target object 3 provided with the device 5 is meant.

  When the communication unit 15 acquires observation values of the external state acquired by the observation unit 7 (here, one or both of the position and velocity of another target object 3 with respect to its own target object 3 or the ground), the observation values Are transmitted to the communication unit 15 provided in the arithmetic communication device 5 of this another target object 3.

  Furthermore, when one or both observed values of the position and velocity of the target object 3 of its own are acquired as observation values of the external state by the observation unit 7 in another target object 3, communication of the target object 3 of its own The unit 15 receives the observed value of the external state from the communication unit 15 in the other target object 3, and the observed value is stored in the storage unit 11. Thereafter, when the observation unit 7 in the target object 3 of its own acquires the observation value of the internal state (one or both of the position and the velocity of the target object 3 of its own), the observation value integration unit 19 The observation values (one or both of the position and velocity of the target object 3 of its own) transmitted from the communication unit 15 of another target object 3 and stored in the storage unit 11 are integrated. This integration may be performed, for example, as follows.

The observation value integration unit 19i of the arithmetic communication device 5i (here, i is an identification number of the target object 3 provided with the arithmetic communication device 5. The same applies to the following) is the observation unit 7i of the arithmetic communication device 5i. The obtained observation value z and the observation value z received from the communication unit 15j of another target object 3j (where i ≠ j) and stored in the storage unit 11i are respectively multiplied by a predetermined weighting factor New integrated observation value z is obtained by adding together these values. That is, the observation value integration unit 19i obtains the integrated observation value z _C according to the following equation (28).

z _C = M _i × z _S + M _j × z _D (28)

In this equation (28), z _C is the integrated observation value z, z _S is the observation value z acquired by the observation unit 7 i, and z _D is another target object 3 j (here, i These are observation values z acquired by the observation unit 7j of ≠ j), transmitted from the communication unit 15j, and stored in the storage unit 11i. Further, M _i and M _j are weighting coefficients, and M _i + M _j = 1. M _i and M _j may be preset, and are, for example, 1/2. Alternatively, M _i may be larger than M _j and smaller than one.

The above integration is performed for one or both of the positions of the target object 3i and the speeds. Also, for this integration, z _S and z _D are represented by coordinate values of the same coordinate system. For example, z _S is represented by coordinate values of a stationary coordinate system fixed on the ground, and z _D is also transmitted from the communication unit 15 j to the communication unit 15 i in a state represented by coordinate values of this stationary coordinate system. Be done.

  When observation values are integrated in this way, the state update unit 13 updates the state data of the system 20 based on the state data estimated by the state estimation unit 9 and the integrated observation values, and this state is updated. Update data error data. That is, in the above, “observed values obtained by the observation unit 7” and other similar descriptions can be read as “integrated observed values”.

Reference Signs List 3 target object (mobile robot), 5 arithmetic communication device, 7 observation unit, 7a internal sensor, 7b external sensor, 9 state estimation unit, 10 state data update device, 11 storage unit, 13 state update unit, 15 communication unit, 17 state integration unit, 19 observation value integration unit, 20 system

Claims (9)

A state data updating apparatus for determining the state of a system including a plurality of target objects, comprising:
The target object is a stationary body stationary on the ground, or a moving body moving relative to the ground,
A plurality of arithmetic communication devices respectively provided to the plurality of target objects;
Each arithmetic communication device is
An observation unit that acquires one or both of the observed values by observing one or both of an internal state of a target object provided with the arithmetic communication device and an external state relative to the target object;
According to an estimation formula for transitioning state data of the system at a past time point to state data at a new time point, state data of the system at the observation time when the observed value is acquired and error data representing an error of this state data are estimated State estimation unit,
A state update unit that updates state data of the system and updates error data based on the state data estimated by the state estimation unit and the observation value;
The update data related to the update by the state update unit is transmitted to a target object different from the target object provided with the arithmetic communication device, and updated data from the target object different from the target object provided with the arithmetic communication device The communication unit to receive
And the latest state data updated by the status update section, by integrating the update data by the communication unit receives, Bei example and a state integration unit for obtaining a new date data,
In each arithmetic communication device, acquisition of the observed value by the observation unit, estimation of the state data and the error data by the state estimation unit, update of the state data and the error data by the state update unit, and The transmission of the update data by the communication unit is repeated,
The state data is an information vector converted from a state vector directly representing the state of the system, and the update data is an information vector difference amount relating to a difference between the updated information vector and the estimated information vector. Including
When each arithmetic communication device performs the current update, a plurality of the information vector difference amounts are received from the communication unit of another arithmetic communication device from the time of the previous update to the time of the current update A state data updating apparatus , wherein a new updated information vector updated is obtained by adding together the plurality of information vector difference amounts and the information vector obtained by the current update. A state data updating apparatus for determining the state of a system including a plurality of target objects, comprising:
The target object is a stationary body stationary on the ground, or a moving body moving relative to the ground,
A plurality of arithmetic communication devices respectively provided to the plurality of target objects;
Each arithmetic communication device is
An observation unit that acquires one or both of the observed values by observing one or both of an internal state of a target object provided with the arithmetic communication device and an external state relative to the target object;
According to an estimation formula for transitioning state data of the system at a past time point to state data at a new time point, state data of the system at the observation time when the observed value is acquired and error data representing an error of this state data are estimated State estimation unit,
A state update unit that updates state data of the system and updates error data based on the state data estimated by the state estimation unit and the observation value;
The update data related to the update by the state update unit is transmitted to a target object different from the target object provided with the arithmetic communication device, and updated data from the target object different from the target object provided with the arithmetic communication device The communication unit to receive
And the latest state data updated by the status update section, by integrating the update data by the communication unit receives, Bei example and a state integration unit for obtaining a new date data,
When each arithmetic communication device determines that the other target object is stationary based on the position or velocity of another target object observed by the observation unit of the target object provided with the arithmetic communication device, An apparatus for updating state data , wherein the frequency of transmitting the observation value of the position or velocity of the other target object to the other target object or the other target object is reduced . The state data is a state vector directly representing the state of the system, the error data is an error covariance matrix of the state vector, and each arithmetic communication device uses a Kalman filter to calculate the state vector and the error covariance. Configured to update the variance matrix,
The state estimation unit of each arithmetic communication device estimates a state vector at the observation time when the observation value is acquired and its error covariance matrix according to the estimation formula,
The state update unit of each arithmetic communication device updates a state vector of the system and its error covariance matrix using a Kalman filter,
The communication unit of each arithmetic communication device transmits, as the update data, the state vector updated by the state update unit to a target object other than the target object provided with the arithmetic communication device.
The state integration unit of each arithmetic communication device adds together vectors obtained by multiplying a state vector updated by the arithmetic communication device and a state vector received by the communication unit by a predetermined weighting factor. The state data updating device according to claim 2 , wherein the integrated new latest state vector is determined by Before Symbol error data is converted information matrix from the error covariance matrix of the state vector,
The state estimation unit of each arithmetic communication device estimates an information vector and an information matrix at an observation time point at which the observation value is acquired, according to the estimation formula.
The state update unit of each arithmetic communication device updates the information vector and the information matrix using the information filter based on the information vector estimated by the state estimation unit and the information matrix, and estimates the updated information vector and the information vector. Obtain an information vector difference amount regarding the difference between the information vector and the information vector, and obtain an information matrix difference amount regarding the difference between the updated information matrix and the estimated information matrix,
The communication unit of each arithmetic communication device is different from the target object on which the arithmetic communication device is provided, as the update data, the information vector differential amount and the information matrix differential amount obtained by the state updating unit of the arithmetic communication device. Send to target object,
The state integration unit of each arithmetic communication device
The integrated new latest information vector is determined by adding the information vector updated by the arithmetic communication device and the information vector difference amount received by the communication unit, and the information matrix updated by the arithmetic communication device The state data updating apparatus according to claim 1, wherein the integrated new latest information matrix is obtained by adding up the information matrix difference amount received by the communication unit. Each arithmetic communication device is
(A) acquiring the observation value by the observation unit;
(B) The state estimation unit estimates an information vector and an information matrix at an observation time point when the observation value is acquired;
(C) The state updating unit updates the information vector and the information matrix, and obtains the information vector difference amount and the information matrix difference amount,
Repeat steps (A) to (C),
When (C) is performed, the (C) is the current update process, and the previous (C) for the current is the previous update process, from the time when the previous update process is performed to the current update process The communication unit of the computing communication device adds up all of the plurality of information vector difference amounts received from another computing communication device and the information vector updated in the current update process in the period up to the time at which To obtain the updated new latest information vector, and in the period, the communication unit of the arithmetic communication device updates the plurality of information matrix difference amounts received from another arithmetic communication device and the current update processing 5. The state data updating device according to claim 4 , wherein the updated new latest information matrix is obtained by adding up the selected information matrix. The arithmetic communication device is characterized in that it designates a transmission timing for transmitting the update data related to the update by the state update unit of its own, or selects another target object as a transmission destination of the update data. The state data update device according to claim 1, 4 or 5 . The plurality of arithmetic communication devices include at least first, second and third arithmetic communication devices,
When the communication unit of the first arithmetic communication device and the communication unit of the second arithmetic communication device can not directly communicate with each other:
The communication unit of the first arithmetic communication device communicates updated data concerning the update by the state update unit of the first arithmetic communication device with the communication unit of the second arithmetic communication device via the communication unit of the third arithmetic communication device. The status data update device according to any one of claims 1 to 6 , which can be transmitted to. A state data updating method for determining the state of a system including a plurality of target objects, comprising:
The target object is a stationary body stationary on the ground, or a moving body moving relative to the ground,
Providing a plurality of arithmetic communication devices for each of the plurality of target objects;
In each arithmetic communication device,
(A) The observation unit observes one or both of an internal state of a target object provided with the arithmetic communication device and an external state with respect to the target object, and acquires one or both observed values;
(B) State data of the system at the observation time when the observed value is acquired, and the state data according to an estimation formula that causes the state estimation unit to transition state data of the system at a past time to state data at a new time Estimate error data representing the error of
(C) The state updating unit updates the state data of the system and the error data, based on the state data estimated in (B) and the observed value.
(D) The communication unit transmits update data regarding update by the state update unit to a target object other than the target object provided with the arithmetic communication device,
(E) When update data is received by the communication unit from a target object other than the target object provided with the arithmetic communication device, the latest state data updated in ( C ) by the state integration unit Obtaining new latest status data by integrating the update data received by the communication unit,
The state data is an information vector converted from a state vector directly representing the state of the system, and the update data is an information vector difference amount relating to a difference between the updated information vector and the estimated information vector. Including
In each arithmetic communication device,
Repeat steps (A) to (C),
When (C) is performed, the (C) is the current update process, and the previous (C) for the current is the previous update process, from the time when the previous update process is performed to the current update process When a plurality of the information vector difference amounts are received from the communication unit of another arithmetic communication device in a period up to the time at which the second step is performed, the plurality of information vector difference amounts and the information obtained by the current update A state data updating method characterized in that an updated new latest information vector is obtained by adding up the vectors . A state data updating method for determining the state of a system including a plurality of target objects, comprising:
The target object is a stationary body stationary on the ground, or a moving body moving relative to the ground,
Providing a plurality of arithmetic communication devices for each of the plurality of target objects;
In each arithmetic communication device,
(A) The observation unit observes one or both of an internal state of a target object provided with the arithmetic communication device and an external state with respect to the target object, and acquires one or both observed values;
(B) State data of the system at the observation time when the observed value is acquired, and the state data according to an estimation formula that causes the state estimation unit to transition state data of the system at a past time to state data at a new time Estimate error data representing the error of
(C) The state updating unit updates the state data of the system and the error data, based on the state data estimated in (B) and the observed value.
(D) The communication unit transmits update data regarding update by the state update unit to a target object other than the target object provided with the arithmetic communication device,
(E) When update data is received by the communication unit from a target object other than the target object provided with the arithmetic communication device, the latest state data updated in ( C ) by the state integration unit Obtaining new latest status data by integrating the update data received by the communication unit,
When it is determined that the other target object is stationary based on the position or the velocity of the other target object observed by the observation unit of the target object provided with the arithmetic communication device, the other target object A method of updating state data, characterized in that the frequency of transmitting position or velocity observations to said another object or another object is reduced .

Images