WO2024071155A1

WO2024071155A1 - Information processing device, information processing method, and computer program

Info

Publication number: WO2024071155A1
Application number: PCT/JP2023/035050
Authority: WO
Inventors: 陽滋山田; 唯眞金
Original assignee: 国立大学法人東海国立大学機構
Priority date: 2022-09-27
Filing date: 2023-09-27
Publication date: 2024-04-04

Abstract

A position estimation unit in this information processing device for estimating the position of an object executes a stochastic filtering process in which a prediction step for calculating a preliminary estimation value and a preliminary error covariance using a dynamic model, and a filtering step for calculating a postliminary estimation value and a postliminary error covariance on the basis of observation data obtained by a sensor as well as the preliminary estimation value and the preliminary error covariance, are executed in sequence. The position estimation unit includes an outlier detection unit that assesses whether the observation data is an outlier, and an outlier identification unit that identifies whether the outlier is a negative outlier or a positive outlier. When the observation data is a negative outlier, the position estimation unit executes one of a compensation process for carrying out compensation using the observation data and an exclusion process for excluding the observation data. When the observation data is a positive outlier, the position estimation unit executes the other of the compensation process and the exclusion process.

Description

Information processing device, information processing method, and computer program

The technology disclosed in this specification relates to an information processing device for estimating the position of an object.

In order to estimate the state (e.g., position) of an object, a probabilistic filtering process using a stochastic filter such as a Kalman filter is used. In the stochastic filtering process, a prediction step and a filtering step are executed sequentially at discrete times. In the prediction step, a priori estimate and a priori error covariance of the object's state are calculated using a dynamic model of the object. In the filtering step, a posterior estimate and a posterior error covariance of the object's state are calculated based on the observation data of the object's state obtained using a sensor and the priori estimate and a priori error covariance calculated in the prediction step. The stochastic filtering process reduces the effect of noise contained in the observation data and allows the state of the object to be estimated with high accuracy.

Sensor observation data on states can contain outliers that deviate significantly from true values. In the past, in state estimation using a Kalman filter, a compensation process has been proposed that uses outliers as inaccurate measured values in order to suppress the deterioration of estimation accuracy caused by outliers (see, for example, Non-Patent Document 1).

When estimating the position of an object using a probabilistic filtering process, the position observation data from the sensor may include outliers, such as negative outliers that deviate closer to the sensor than the pre-prediction value calculated in the prediction step, and positive outliers that deviate farther from the sensor than the pre-prediction value.

In the above conventional technology, compensation processing is performed uniformly without considering the direction of the outlier, so that the application is limited. For example, when estimating the position of a human using observation data from a sensor installed on a robot to avoid contact between the robot and a human, if the observation data is a negative outlier that is closer to the sensor (robot), the outlier is an outlier on the side where the possibility of contact with the robot is low (safety side) for the human, so there is no problem in performing the compensation processing using the above-mentioned outlier. On the other hand, if the observation data is a positive outlier that is farther from the sensor (robot), the outlier is an outlier on the side where the possibility of contact with the robot is high (danger side) for the human, so performing the compensation processing using the above-mentioned outlier may cause contact between the human and the robot. Therefore, in such a situation, it is not appropriate to apply the above conventional technology that performs compensation processing uniformly without considering the direction of the outlier. In contrast to the above example, there may be a situation where problems occur when performing compensation processing when the observation data is a negative outlier.

In this way, there is room for improvement in conventional probabilistic filtering processing technology in terms of reducing the deterioration of estimation accuracy caused by outliers in a wider range of application scenarios.

This specification discloses technology that can solve the problems mentioned above.

The technology disclosed in this specification can be realized, for example, in the following forms:

(1) The information processing device disclosed in this specification is an information processing device for estimating the position of an object, and includes a position estimation unit. The position estimation unit executes a probabilistic filtering process that sequentially performs, for each discrete time, a prediction step of calculating a prior estimate and a prior error covariance of the position of the object using a dynamic model of the object, and a filtering step of calculating a posterior estimate and a posterior error covariance of the position of the object based on observation data of the position of the object using a sensor, the prior estimate, and the prior error covariance. The position estimation unit includes an outlier detection unit that determines whether the observation data is an outlier, and an outlier identification unit that identifies whether the detected outlier is a negative outlier that is closer to the sensor than the prior estimate, or a positive outlier that is farther from the sensor than the prior estimate. When the observation data is determined to be the negative outlier, the position estimation unit performs one of a compensation process that uses the observation data determined to be the outlier and an exclusion process that excludes the observation data determined to be the outlier in the filtering step, and when the observation data is determined to be the positive outlier, the position estimation unit performs the other of the compensation process and the exclusion process in the filtering step.

In this information processing device, if the observed data is determined to be a negative outlier, a compensation process is executed in the filtering step, and if the observed data is determined to be a positive outlier, an exclusion process is executed in the filtering step. Therefore, according to this information processing device, it is possible to suppress the decrease in estimation accuracy caused by outliers in a wider range of application scenarios compared to conventional configurations in which compensation process is executed uniformly without considering the direction in which the outliers deviate.

(2) In the above information processing device, the position estimation unit may be configured to execute the compensation process when the observation data is determined to be the negative outlier, and execute the exclusion process when the observation data is determined to be the positive outlier. This information processing device can effectively suppress a decrease in estimation accuracy caused by outliers in a wider range of application scenarios. For example, when this information processing device is applied to a space in which a robot and a human exist, a negative outlier is an outlier on the side (safety side) where the possibility of contact between a human and a robot is low, so by executing a compensation process that uses the negative outlier as an inaccurate measurement value, a decrease in the accuracy of human position estimation can be suppressed. In addition, a positive outlier is an outlier on the side (danger side) where the possibility of contact between a human and a robot is high, so by executing an exclusion process that excludes the positive outlier, a decrease in the estimation accuracy of the human position caused by the outlier and an increase in the possibility of contact between a human and a robot can be suppressed.

(3) In the information processing device, the compensation process may include a process of calculating the posterior estimated value and the posterior error covariance based on the observation data with increased error covariance, the a priori estimated value, and the a priori error covariance. By adopting this configuration, a decrease in accuracy of the target position estimation can be effectively suppressed by the compensation process that uses the increased error covariance of observation data that is a negative outlier as an inaccurate measurement value.

(4) In the information processing device, the exclusion process may include a process of setting the posterior estimated value to be equal to the a priori estimated value at the current discrete time. By adopting this configuration, it is possible to effectively prevent a decrease in the estimation accuracy of the object position due to positive outliers.

(5) In the information processing device, the outlier identification unit may identify whether the detected positive outlier is a temporary outlier that occurs continuously across multiple discrete times or an additive outlier that occurs in a single discrete time, and the position estimation unit may set the posterior error covariance to be the same as the posterior error covariance in the previous discrete time as the exclusion process when the observation data is determined to be the temporary outlier, and set the posterior error covariance to be the same as the prior error covariance in the current discrete time as the exclusion process when the observation data is determined to be the additive outlier. By adopting this configuration, it is possible to prevent errors from accumulating and becoming excessively large when the positive outlier is a temporary outlier, thereby preventing a decrease in the estimation accuracy of the target position.

(6) In the information processing device, the outlier detection unit may be configured to determine whether the observation data is an outlier using Mahalanobis distance. By adopting this configuration, outliers can be appropriately detected even when two-dimensional or three-dimensional position information is the target.

(7) In the information processing device, the probabilistic filtering process may be a filtering process using a Kalman filter. By adopting this configuration, it is possible to estimate the position of an object efficiently and with high accuracy.

(8) In the above information processing device, the sensor may be a radar sensor that measures the position of the object by transmitting and receiving radio waves. Radar sensors have a wider detection range (radio wave irradiation range) than other types of sensors, such as laser sensors, and can therefore detect relatively small objects such as a human hand. On the other hand, because radar sensors have a wider detection range, observation data from the radar sensor may contain a relatively large amount of noise. However, according to the information processing device, it is possible to suppress the decrease in position estimation accuracy caused by such noise (outliers), and therefore it is possible to estimate the position of a relatively small object with high accuracy.

In addition, this information processing device can also obtain speed information based on the time difference of estimated position (distance) information.

(9) The information processing device may further include a robot control unit that controls the operation of the robot based on the result of estimation of the position of the human as the target object by the position estimation unit. By adopting this configuration, the position of the human can be estimated with high accuracy in a space in which the robot and the human exist, and contact between the human and the robot can be avoided more reliably.

The technology disclosed in this specification can be realized in various forms, such as an information processing device, an information processing method, a computer program that realizes the method, or a non-transitory recording medium on which the computer program is recorded.

FIG. 1 is an explanatory diagram showing a configuration of a safety monitoring system 10 according to an embodiment of the present invention. A block diagram showing the configuration of a safety monitoring device 100. A diagram conceptually illustrating the procedure of a typical extended Kalman filtering process. An explanatory diagram conceptually illustrating the basic operation of a normal extended Kalman filtering process. FIG. 1 is an explanatory diagram showing an example of an algorithm for a typical extended Kalman filtering process. A flowchart showing an object position estimation process executed by the safety monitoring device 100 of the present embodiment. FIG. 1 is an explanatory diagram showing an example of a negative outlier and a positive outlier. FIG. 1 is an explanatory diagram conceptually illustrating compensation processing executed when observed data z _k is an outlier. FIG. 1 is an explanatory diagram conceptually illustrating a result of performing a normal extended Kalman filtering process when observed data z _k is an outlier. FIG. 1 is an explanatory diagram conceptually illustrating the exclusion process that is executed when observed data z _k is an outlier. FIG. 1 is an explanatory diagram showing an example of a temporary outlier and an additional outlier. FIG. 1 is an explanatory diagram showing an example of an algorithm for an extended Kalman filtering process executed in this embodiment. FIG. 1 is an explanatory diagram showing the configuration of an apparatus according to the present embodiment. An illustration showing observed data at one epoch during a fast movement trial. An illustration showing observed data at one epoch during a slow movement trial. FIG. 1 is an explanatory diagram showing data after applying the asymmetric extended Kalman filtering process of the present embodiment to observed data at a certain epoch during a high-speed movement trial. FIG. 1 is an explanatory diagram showing data after applying the asymmetric extended Kalman filtering process of the present embodiment to observed data at a certain epoch during a slow movement trial. Diagram summarizing test results FIG. 1 is an explanatory diagram showing a comparison result between an estimated value using the asymmetric extended Kalman filtering process of the present embodiment and a value calculated by generating temporary heavy-tailed Gaussian noise. FIG. 11 is an explanatory diagram summarizing the results of a comparison between the asymmetric extended Kalman filtering process of the present embodiment and the extended Kalman filtering process of a comparative example. FIG. 11 is an explanatory diagram summarizing the ratio of erroneously adopting outliers on the dangerous side for the asymmetric extended Kalman filtering process of this embodiment and the extended Kalman filtering process of the comparative example; FIG. 11 is an explanatory diagram showing the results of comparing the ratio of outliers on the dangerous side when α=5 and λ=0.15. FIG. 10 is an explanatory diagram showing the results of comparing the ratio of outliers on the dangerous side when α=7 and λ=0.3.

A. Embodiments:
A-1. Configuration of safety monitoring system 10:
1 is an explanatory diagram showing the configuration of a safety monitoring system 10 according to the present embodiment. The safety monitoring system 10 according to the present embodiment is a system that is introduced into a space in which a robot 200 and a human HU exist (for example, a production site such as a factory or a logistics site such as a distribution center). The safety monitoring system 10 includes the robot 200 and a safety monitoring device 100.

The robot 200 is a machine that performs various operations. In the example of FIG. 1, the robot 200 is a vertical articulated robot, but the type of the robot 200 is not limited to this.

The safety monitoring device 100 is a device that detects a human HU and controls the operation of the robot 200 to avoid contact between the human HU and the robot 200. The safety monitoring device 100 is installed, for example, on a non-moving part of the robot 200.

FIG. 2 is a block diagram showing the configuration of the safety monitoring device 100. The safety monitoring device 100 comprises a control unit 110, a memory unit 120, a radar sensor 130, an operation input unit 140, and an interface unit 150. These units are connected to each other via a bus 190 so that they can communicate with each other. The safety monitoring device 100 is an example of an information processing device within the scope of the claims.

The radar sensor 130 of the safety monitoring device 100 is a device that measures the position of an object (in this embodiment, the hand of a human HU) by emitting radio waves RW (e.g., millimeter waves) and receiving reflected waves, which are radio waves RW reflected by the object. In this embodiment, the radar sensor 130 is a MIMO (Multi-Input Multi-Output) type sensor that uses multiple antennas for both emitting and receiving radio waves RW. Compared to other types of sensors such as laser sensors, the radar sensor 130 has a wide detection range (radiation range of the radio waves RW), so it can detect relatively small objects such as the hand of a human HU. On the other hand, because the radar sensor 130 has a wide detection range, the observation data obtained by the radar sensor 130 may contain a relatively large amount of noise (e.g., noise caused by reflected waves from walls and ceilings).

The operation input unit 140 of the safety monitoring device 100 is configured, for example, with buttons, a touch panel, etc., and accepts operations and instructions from the administrator. The interface unit 150 is configured, for example, with a network interface, etc., and communicates with other devices (for example, the robot 200) via wired or wireless communication.

The storage unit 120 of the safety monitoring device 100 is composed of, for example, ROM, RAM, a hard disk drive (HDD), a solid state drive (SSD), etc., and is used to store various programs and data, and as a working area when executing various programs, and as a temporary storage area for data. For example, the storage unit 120 stores a safety monitoring program CP. The safety monitoring program CP is provided in a state stored in a computer-readable recording medium (not shown), such as a CD-ROM, DVD-ROM, or USB memory, or is provided in a state that can be obtained from an external device (for example, a server on the cloud or other terminal device) via the interface unit 150, and is stored in the storage unit 120 in a state that can be operated on the safety monitoring device 100.

In addition, the storage unit 120 of the safety monitoring device 100 stores various types of data such as a dynamic model DM in the object position estimation process described below. These types of data will be explained in conjunction with the explanation of the object position estimation process described below.

The control unit 110 of the safety monitoring device 100 is configured with, for example, a CPU, and controls the operation of the safety monitoring device 100 by executing a computer program read from the storage unit 120. For example, the control unit 110 functions as a position estimation unit 111 that estimates the position of an object (the hand of a human HU) by reading a safety monitoring program CP from the storage unit 120 and executing it. The position estimation unit 111 includes an observation data acquisition unit 112, a model acquisition unit 113, an outlier detection unit 114, and an outlier identification unit 115. The functions of each of these units will be explained in conjunction with the explanation of the object position estimation process described below.

The control unit 110 also functions as a robot control unit 119 that controls the operation of the robot 200 based on the estimation result of the hand position of the human HU by the position estimation unit 111, by reading and executing the safety monitoring program CP from the memory unit 120. For example, when the hand position of the human HU estimated by the position estimation unit 111 is in a position where there is a risk of contact with the robot 200, the robot control unit 119 stops (or slows down) the movement of the robot 200 or changes the direction of the movement of the robot 200 to avoid contact between the human HU and the robot 200.

A-2. Object position estimation process:
Next, an object position estimation process executed by the safety monitoring device 100 of this embodiment will be described. The object position estimation process of this embodiment is a process for accurately estimating the position of the hand of the human being HU as an object by executing a probabilistic filtering process. In this embodiment, an extended Kalman filter (abbreviated as EKF), which is one of the Kalman filters, is used as the probabilistic filter. Hereinafter, the probabilistic filtering process using the extended Kalman filter will be referred to as the extended Kalman filtering process.

As described below, in the probabilistic filtering process of this embodiment, when the observation data from the radar sensor 130 is an outlier, different processing is performed depending on the type of outlier. That is, in this embodiment, an asymmetric extended Kalman filtering process (hereinafter also referred to as "AS-EKF") is performed in which different processing is performed depending on the type of outlier. Below, we will explain the normal extended Kalman filtering process that is performed when the observation data is not an outlier, and then we will explain the asymmetric extended Kalman filtering process AS-EKF that is performed in this embodiment.

In this specification and drawings, the symbols ~ (tilde) and ^ (hat) attached to each symbol mean the measured value and estimated value, respectively, and the symbols (-) and (+) attached to each symbol mean the prior value (before the latest observation data is used) and the post-event value (after the latest observation data is used), respectively. However, for convenience, these symbols may be omitted.

(Normal extended Kalman filtering process)
First, a typical extended Kalman filtering process will be described. First, a nonlinear dynamic model DM is set for an object (in this embodiment, the hand of a human HU) as shown in formula (1). In formula (1), x _k is an n-th order state space vector at time k, f is a transition function, and w _k is a process noise assumed to follow a Gaussian distribution.

Next, an extrapolation process for state estimation is performed after observation according to equation (2), where z _k is the m-th observation vector at time k, h is the observation function, and v _k is the measurement noise assumed to follow a Gaussian distribution.

FIG. 3 is an explanatory diagram conceptually showing the procedure of a normal extended Kalman filtering process. The upper part of FIG. 3 shows the procedure of estimating the state, and the lower part of FIG. 3 shows the procedure of estimating the error covariance. FIG. 4 is an explanatory diagram conceptually showing the basic operation of a normal extended Kalman filtering process. As shown in FIG. 3 and FIG. 4, in the extended Kalman filtering process, a prediction step and a filtering step are executed sequentially for each discrete time. In the prediction step, a prior estimate x _k (-) of the position of the object at time k and a prior error covariance P _k (-) are calculated as shown in Equation (3) and Equation (4) using the dynamic model DM of the object described above. In Equation (3) and Equation (4), x _k-1 (+) is a posterior estimate at the previous discrete time (k-1), and Q _k is the process noise covariance at time k according to Equation (5). Furthermore, F _k is the Jacobian (function determinant) of the nonlinear transition function f expressed by Equation (6). In the prediction step, for example, estimation is performed based on a constant velocity rectilinear motion model.

Next, the Kalman gain K _k is calculated according to equation (7): In equation (7), H _k is the observation matrix expressed by equation (8), and R _k is the measurement noise covariance according to equation (9).

Next, in a filtering step, a posterior estimate x _k (+) and a posterior error covariance P _k (+) of the object's position at time k are calculated based on the observation data z _k of the object's position using the radar sensor 130 and the a priori estimate x _k (-) and a priori error covariance P _k (-) calculated in the prediction step, as shown in equations (10) and (11). In equation (11), I is a unit matrix.

In the extended Kalman filtering process, the prediction step and filtering step described above are repeatedly executed at discrete times. The extended Kalman filtering process can reduce the effect of noise contained in the observation data of the position of the object (the hand of the human HU) observed by the radar sensor 130, and can estimate the position of the object with high accuracy. Figure 5 is an explanatory diagram showing an example of an algorithm (Algorithm 1) for a typical extended Kalman filtering process.

(Object Position Estimation Process of the Present Embodiment)
Next, a description will be given of the object position estimation process executed by the safety monitoring device 100 of this embodiment. Fig. 6 is a flowchart showing the object position estimation process executed by the safety monitoring device 100 of this embodiment. The object position estimation process is started in response to, for example, an administrator operating the operation input unit 140 of the safety monitoring device 100 to input a start instruction.

First, the model acquisition unit 113 (FIG. 2) of the safety monitoring device 100 acquires a dynamic model DM of the target object (the hand of the human HU) (S110). The dynamic model DM is, for example, a model previously set by a supervisor, acquired via the interface unit 150 and stored in the memory unit 120.

Next, as a prediction step, the position estimation unit 111 (FIG. 2) of the safety monitoring device 100 uses the dynamic model DM to calculate the a priori estimate x _k (-) and a priori error covariance P _k (-) of the object's position at time k according to the above-mentioned equations (3) and (4) (S120).

Next, the observation data acquisition unit 112 (FIG. 2) of the safety monitoring device 100 acquires observation data _zk of the position of the object at time k measured by the radar sensor 130 (S130).

Next, the outlier detection unit 114 (FIG. 2) of the safety monitoring device 100 calculates an outlier determination index γ _k according to equation (12) to determine whether the acquired observation data z _k is an outlier or not (S140). In equation (12), M _k is the Mahalanobis distance, and γ _k is an outlier determination index (gamma determination index) that follows a chi-square distribution with m degrees of freedom from a state space vector. In the following, "z _k -h(x _k (-))" in equation (12) is represented as n _k .

In order to determine the threshold for outlier determination using the Mahalanobis distance, the concept of inclusion distance expressed by formula (13) is taken into consideration. In formula (13), C _p is the inclusion probability, DR is the required rate of the safety-related system, and PFH _u is the upper limit of the failure probability in one hour. As shown in formula (14), the probability threshold α is selected based on the inclusion distance. In formula (14), χ _α is the α quantile of a predetermined chi-square distribution. In this embodiment, α is set to a value greater than 1-2.5×10 ⁻⁷ , for example, based on the required performance level. Therefore, a chi-square distribution with two degrees of freedom having a significance level is, for example, 1-2.5×10 ⁻⁷ , and χ _α is set to, for example, 30.41.

The outlier detection unit 114 of the safety monitoring device 100 judges whether or not the observation data z _k is an outlier based on the calculated outlier judgment index γ _k (S150). Specifically, when the outlier judgment index γ _k is equal to or smaller than the above-mentioned χ _α , the outlier detection unit 114 judges that the observation data z _k is not an outlier (S150: NO). In this case, the position estimation unit 111 executes the filtering step of the above-mentioned normal extended Kalman filtering process (see FIG. 3 and FIG. 4) (S160).

On the other hand, when the outlier determination index γ _k is greater than the above-mentioned χ _α , the outlier detection unit 114 determines that the observed data z _k is an outlier (S150: YES).

Here, the outliers include negative and positive outliers. FIG. 7 is an explanatory diagram showing an example of a negative and positive outlier. FIG. 7 shows the estimated value PD (pre-estimated value x _k (−)) of the hand position of the human HU calculated in the prediction step, and an ellipse E1 showing the estimation error covariance. As shown on the left side of FIG. 7, the negative outlier NO is an outlier in which the observation data (the hand position of the human HU observed by the radar sensor 130) deviates from the estimated value PD in the prediction step to the side closer to the radar sensor 130 of the safety monitoring device 100. The negative outlier NO is an outlier in which the hand of the human HU is mistakenly recognized as being closer to the radar sensor 130 (robot 200) than it actually is, and can be said to be an outlier on the side (safety side) where the possibility of contacting the robot 200 is low for the human HU. On the other hand, the positive outlier PO is an outlier in which the observation data deviates from the estimated value PD in the prediction step to the side farther from the radar sensor 130, as shown on the right side of FIG. 7. The positive outlier PO is an outlier in which the hand of the human being HU is mistakenly recognized as being farther from the radar sensor 130 (robot 200) than it actually is, and can be said to be an outlier on the side (danger side) where the possibility of contact between the human being HU and the robot 200 is higher for the human being HU. In this way, negative outliers and positive outliers have different effects on the possibility of contact between the human being HU and the robot 200, so in this embodiment, as described below, the processing of the filtering step is made different depending on whether the observation data _zk is a negative outlier or a positive outlier.

The outlier identification unit 115 (FIG. 2) of the safety monitoring device 100 identifies whether the detected outlier is a negative outlier or a positive outlier (S170). Specifically, when n _k (=z _k -h(x _k (-))) in the above formula (12) is a negative value, the outlier identification unit 115 determines that the detected outlier is a negative outlier, and when n _k is a positive value, the outlier identification unit 115 determines that the detected outlier is a positive outlier.

If it is determined that the detected outlier is a negative outlier (S170: YES), the position estimation unit 111 of the safety monitoring device 100 performs compensation processing using the negative outlier in the filtering step, as described below (S180).

In the compensation process, the position estimation unit 111 increases the error covariance R _k of the observation data by using the scaling factor λ as shown in formula (15). Then, the above formula (12) for calculating the outlier determination index γ _k is updated to the following formula (16). Note that P _k (-) (actually, there is a bar above P) in formula (16) is expressed by formula (17).

The position estimation unit 111 calculates the optimal value of the scaling factor λ at time k using the Newton method. When the function g is defined as in formula (18), λ _k is optimized as shown in formula (19). In formula (19), i is the i-th innovation, and "'" represents the derivative (differential coefficient) of the function. Here, the derivative of the inverse matrix is expressed as formula (20). In formula (20), A is a random invertible matrix, and the function of time t can be rewritten as formula (21). As described above, n _k in formula (21) represents z _k -h(x _k (-)). The initial value of λ _k is set to 1, for example, and i is incremented until the calculated value of formula (16) is equal to or less than χ _α .

FIG. 8 is an explanatory diagram conceptually showing a compensation process executed when the observation data z _k is an outlier. FIG. 9 is an explanatory diagram conceptually showing a result of performing a normal extended Kalman filtering process when the observation data z _k is an outlier. As shown in FIG. 9, when a normal extended Kalman filtering process is executed when the observation data z _k is an outlier, the outlier observation data z _k is used as is in the filtering step, so that an erroneous posterior estimate value x _k (+) and a posterior error covariance P _k (+) are calculated. On the other hand, as shown in FIG. 8, in the compensation process, the posterior estimate value x _k (+) and the posterior error covariance P _k (+) are calculated based on the observation data z _k whose error covariance R _k has been increased by multiplying it by a scaling factor λ, and the a priori estimate value x _k (-) and a priori error covariance P _k (-) calculated in the prediction step. Therefore, by using the outlier observation data z _k as an inaccurate measurement value, it is possible to suppress a decrease in the estimation accuracy in the extended Kalman filtering process.

As described above, a negative outlier is an outlier on the side (safe side) where the possibility of contact between the human HU and the robot 200 is low. Therefore, when it is determined that the detected outlier is a negative outlier (S170: YES), a compensation process is executed that uses the outlier as an inaccurate measurement value, thereby suppressing a decrease in estimation accuracy in the extended Kalman filtering process without increasing the possibility of contact between the human HU and the robot 200.

On the other hand, if it is determined that the detected outlier is a positive outlier (S170: NO), the position estimation unit 111 of the safety monitoring device 100 performs an exclusion process to exclude the outlier in the filtering step, as described below (S190).

In the exclusion process, the position estimation unit 111 sets the posterior estimate _xk (+) and the posterior error covariance _Pk (+) to be equal to the a priori estimate _xk (-) and the a priori error covariance _Pk (-) calculated in the prediction step, respectively, as shown in equations (22) and (23). In other words, in the exclusion process, the filtering step is not performed, and the observation data _zk that is an outlier is not used for estimation.

10 is an explanatory diagram conceptually illustrating the exclusion process executed when the observation data z _k is an outlier. As shown in FIG. 10, in the exclusion process, the observation data z _k that is an outlier is excluded, and the posterior estimate x _k (+) and the posterior error covariance P _k (+) are set to be equal to the a priori estimate x _k (-) and the a priori error covariance P _k (-) calculated in the prediction step, respectively. This makes it possible to suppress a decrease in the estimation accuracy of the target position due to the outlier, which in turn suppresses an increase in the possibility of contact between the human HU and the robot 200.

Here, there are two types of outliers: temporary outliers and additive outliers. FIG. 11 is an explanatory diagram showing an example of a temporary outlier and an additive outlier. As shown in FIG. 11, a temporary outlier TO is an outlier that occurs continuously across multiple discrete times and forms a trend in a specified period of time. A temporary outlier TO occurs mainly due to a miss-detection by a sensor. On the other hand, an additive outlier AO is an outlier that occurs in a single discrete time and affects the observation in a single discrete time (only). An additive outlier AO occurs mainly due to heavy-tailed error distribution noise.

As described above, in this embodiment, when the detected outlier is determined to be a positive outlier, the removal process is performed. However, when the detected outlier is a temporary outlier, if the posterior error covariance P _k (+) is set to be equal to the prior error covariance P _k (-) according to the above formula (23), there is a problem that the error accumulates and becomes excessively large. Therefore, as shown in formula (24), when the detected outlier is a positive outlier and a temporary outlier, the posterior error covariance P _k (+) is set to be equal to the posterior error covariance P _k-1 (+) at the previous discrete time (k-1). In formula (24), τ is a temporary outlier determination index represented by the number of consecutive outliers. When τ is 2 or more, the outlier is a temporary outlier, and when τ is less than 2, the outlier is an additional outlier. In addition, γ ⁱ _k is the i-th repetition of formula (16).

Thus, in this embodiment, the outlier identification unit 115 of the safety monitoring device 100 identifies whether a detected positive outlier is a temporary outlier or an additional outlier. When the position estimation unit 111 determines that the observation data is a temporary outlier, it sets the posterior error covariance P _k (+) to be equal to the posterior error covariance P _k-1 (+) at the previous discrete time as an exclusion process. On the other hand, when the position estimation unit 111 determines that the observation data is an additional outlier, it sets the posterior error covariance P _k (+) to be equal to the a priori error covariance P _k (-) at the current discrete time as an exclusion process.

After the filtering step at the current discrete time k is completed, the position estimation unit 111 of the safety monitoring device 100 determines whether or not there has been an instruction to end the object position estimation process (S200). If there has been no such instruction (S200: NO), the position estimation unit 111 updates the current discrete time k (k = k + 1) (S210) and similarly executes the processes from S120 onwards described above. If there has been an instruction to end (S200: YES), the position estimation unit 111 ends the object position estimation process.

FIG. 12 is an explanatory diagram showing an example of an algorithm (Algorithm 2) for the extended Kalman filtering process (asymmetric extended Kalman filtering process AS-EKF) executed in this embodiment.

A-3. Advantages of this embodiment:
As described above, the safety monitoring device 100 of this embodiment is an information processing device for estimating the position of an object, and includes a position estimation unit 111. The position estimation unit 111 executes a probabilistic filtering process that sequentially performs, for each discrete time, a prediction step of calculating a priori estimate x _k (-) and a priori error covariance P _k (-) of the object's position using a dynamic model _DM of the object, and a filtering step of calculating a posteriori estimate x _k (+) and a posteriori error covariance P _k (+) of the object's position based on observation data z k of the object's position using the radar sensor 130, the priori estimate x _k (-), and the priori error covariance P _k (-). The position estimation unit 111 also includes an outlier detection unit 114 that determines whether the observation data z _k is an outlier, and an outlier identification unit 115 that identifies whether the detected outlier is a negative outlier that deviates from the prior estimate x _k (-) on the side closer to the radar sensor 130, or a positive outlier that deviates from the prior estimate x _k (-) on the side farther from the radar sensor 130. If the observation data z _k is determined to be a negative outlier, the position estimation unit 111 executes a compensation process in a filtering step to perform compensation using the observation data z _k determined to be an outlier, and if the observation data z _k is determined to be a positive outlier, the position estimation unit 111 executes an exclusion process in a filtering step to exclude the observation data z _k determined to be an outlier.

In this manner, in the safety monitoring device 100 of the present embodiment, when the observation data z _k is determined to be a negative outlier, a compensation process is performed in the filtering step, and when the observation data z _k is determined to be a positive outlier, an exclusion process is performed in the filtering step. Therefore, according to the safety monitoring device 100 of the present embodiment, it is possible to suppress a decrease in the estimation accuracy due to the outlier in a wider range of application scenes. For example, when the safety monitoring device 100 of the present embodiment is applied to a space in which the robot 200 and the human HU exist, the negative outlier is an outlier on the side (safety side) where the possibility of contact with the robot 200 is low for the human HU, so that a compensation process using the negative outlier as an inaccurate measurement value can be performed to suppress a decrease in the accuracy of the position estimation of the human HU. In addition, the positive outlier is an outlier on the side (danger side) where the possibility of contact with the robot 200 is high for the human HU, so that an exclusion process for excluding the positive outlier is performed to suppress a decrease in the estimation accuracy of the position of the human HU due to the outlier, and an increase in the possibility of contact between the human HU and the robot 200.

Furthermore, in the safety monitoring device 100 of this embodiment, the compensation process includes a process of calculating a posterior estimated value _xk (+) and a posterior error covariance _Pk (+) based on the observation data _zk with an increased error covariance, the a priori estimated value _xk (-) and the a priori error covariance _Pk (-). Therefore, according to the safety monitoring device 100 of this embodiment, the compensation process uses the increased error covariance of the observation data _zk , which is a negative outlier, as an inaccurate measurement value, making it possible to effectively suppress a decrease in accuracy of the object position estimation.

Furthermore, in the safety monitoring device 100 of this embodiment, the exclusion process includes a process of setting the posterior estimated value x _k (+) to be equal to the a priori estimated value x _k (−) at the current discrete time. Therefore, according to the safety monitoring device 100 of this embodiment, it is possible to effectively suppress a decrease in the estimation accuracy of the object position caused by the positive outlier.

In addition, in the safety monitoring device 100 of this embodiment, the outlier identification unit 115 identifies whether the detected positive outlier is a temporary outlier that occurs continuously across multiple discrete times or an additional outlier that occurs in a single discrete time. In addition, when the observation data z _k is determined to be a temporary outlier, the position estimation unit 111 sets the posterior error covariance P _k (+) to be equal to the posterior error covariance P _k-1 (+) in the previous discrete time as an exclusion process, and when the observation data z _k is determined to be an additional outlier, sets the posterior error covariance P _k (+) to be equal to the prior error covariance P _k (-) in the current discrete time as an exclusion process. Therefore, according to the safety monitoring device 100 of this embodiment, when the positive outlier is a temporary outlier, it is possible to suppress a decrease in the estimation accuracy of the position of the object due to an excessively large accumulated error.

Furthermore, in the safety monitoring device 100 of this embodiment, the outlier detection unit 114 uses the Mahalanobis distance to determine whether or not the observation data z _k is an outlier. Therefore, according to the safety monitoring device 100 of this embodiment, even when two-dimensional or three-dimensional position information is the target, outliers can be appropriately detected.

In addition, in the safety monitoring device 100 of this embodiment, a filtering process using a Kalman filter is executed as a probabilistic filtering process. Therefore, the safety monitoring device 100 of this embodiment can estimate the position of an object efficiently and with high accuracy.

In addition, the safety monitoring device 100 of this embodiment uses a radar sensor 130 that measures the position of an object by transmitting and receiving radio waves. The radar sensor 130 has a wide detection range (radiation range of radio waves RW) compared to other types of sensors such as laser sensors, so it can detect relatively small objects such as the hand of a human HU. On the other hand, since the radar sensor 130 has a wide detection range, the observation data by the radar sensor 130 may contain a relatively large amount of noise. However, according to the safety monitoring device 100 of this embodiment, the decrease in position estimation accuracy caused by such noise (outliers) can be suppressed, so that the position of a relatively small object can be estimated with high accuracy. For example, when the safety monitoring device 100 of this embodiment is applied to a space where a robot 200 and a human HU exist, the safety monitoring device 100 can estimate the position of the hand of the human HU with high accuracy. Therefore, compared to conventional safety monitoring methods that detect the positions of relatively large parts of the human HU, such as the torso or legs, and uniformly define the area within a specified distance (e.g., 80 cm) from the detected parts as an area where there is a risk of contact with the robot, and thus avoid contact between the robot and the human, the separation distance between the robot and the human can be reduced, improving the efficiency of space utilization and the workability of the human HU.

In addition, according to the safety monitoring device 100 of this embodiment, speed information can also be obtained based on the time difference of the estimated position (distance) information.

The safety monitoring device 100 of this embodiment further includes a robot control unit 119 that controls the operation of the robot 200 based on the result of estimation of the position of the human being HU as an object by the position estimation unit 111. Therefore, according to the safety monitoring device 100 of this embodiment, it is possible to estimate the position of the human being HU with high accuracy in the space in which the robot 200 and the human being HU exist, and it is possible to more reliably avoid contact between the human being HU and the robot 200.

A-4. Example:
Next, an example (experimental example) of the above-mentioned asymmetric extended Kalman filtering process (AS-EKF) executed by the safety monitoring device 100 of this embodiment will be described. FIG. 13 is an explanatory diagram showing the device configuration in this embodiment. In this embodiment, a test was conducted to detect the position of a human hand entering the vicinity of a robot using a radar sensor. Specifically, a linear actuator 310 was used to reciprocate a hand test piece 320 (ABS, IEC 61496-3) simulating a human hand so as to repeatedly move toward and away from the radar sensor 130, the position of the hand test piece 320 was measured by the radar sensor 130, and the asymmetric extended Kalman filtering process (AS-EKF) was applied to the observation data by the radar sensor 130. In order to approximate the actual hand intrusion environment, a mannequin 330 simulating a human (whose surface is made of a soft urethane material) was placed behind the hand test piece 320.

The duration of the test was 15 minutes according to IEC/TS 62998. The stroke of the linear actuator 310 (LEFB25S2S-1000-S2A1, SMC) was 0.85 m. The radar sensor 130 was a MIMO radar sensor (IWR68431SK, Texas Instruments, USA) with a 60 GHz standard antenna. The azimuth and elevation angles of the radar sensor 130 were ±60 degrees and ±20 degrees, respectively. The transmission power of the radar sensor 130 was 12 dBm, the maximum bandwidth was 4 GHz, and the frequency was 60-64 GHz. The frequency slope of the chirp was 71.26 MHz/μs, the sampling rate was 5279 ksps, and 222 samples were obtained for each frequency-modulated chirp. The range resolution using a 256-size FFT was 4.34 cm (measurement rate 30 Hz).

In this embodiment, the entire measurement system was managed by Melodic, a robot operating system (ROS) that runs on Linux Ubuntu 18.04 LTS (64-bit). The 3D point cloud was measured by the millimeter wave ros package from Texas Instruments, which was officially provided by the manufacturer with a calibration file and a serial driver required for communication with the PC. All point clouds and the closest distance points extracted by the Euclidean clustering method were recorded with time stamp data by the rosbag package. For comparison, a motion capture system using 12 cameras (Motion Analysis, Santa Rosa, California, USA) was used to measure the relative distance and speed between the 3D position of the sensor antenna and the tip of the hand test piece. In this example, the actual measurements taken by the motion capture system were treated as true values.

In this embodiment, a Gaussian mixture model is used to generate two or more complex noise signals. A heavy-tailed Gaussian noise distribution can be generated as shown in Equation (25). In Equation (25), v _k is the measurement error vector, R _k is the measurement error covariance, λ is the contamination ratio, and α is a scaling factor. λ contributes to the frequency of occurrence of outliers, and α is related to the magnitude of the outliers.

Considering that a temporal outlier is observed when the primary target is lost and immediately transitions to a secondary target, a temporal heavy-tailed Gaussian noise is generated as shown in Equation (26) below: In Equation (26), z _k is the observation vector of the primary target, z′ _k−1 is the observation vector of the secondary target, ρ is the temporal contamination rate contributing to the generation of the temporal outlier, and π _k is the Bernoulli variable depending on ρ.

The effective ranges for generating a suitable noise profile by changing the parameters are different from each other, specifically, as follows:
1 < α
0<λ<1
0<ρ<1

As an option for evaluating the performance tolerant to outliers, we used the root mean square error (RMSE) expressed by equation (28), where L is the number of trials in the Monte Carlo simulation, and N is the number of samples of the observation vector included in each trial.

To investigate the difference in the outlier ratio depending on the speed of the object, a test was conducted on the reciprocating motion of the hand test piece 320 at high and low speeds. To synchronize the measurement time of the rosbag recording of the motion capture camera and the radar sensor 130, an analog pulse signal with a delay of less than 1 millisecond was generated. The initial conditions of the Kalman variables were set as follows. A conventional EKF to which the robust OD-KF method was applied (hereinafter also referred to as "OD-EKF") was used as a comparative example.
Error covariance P ₀ =Q _k
Initial spatial state x ₀ , x ₀ (estimated value)=z ₀ (measured value)
Measurement noise covariance R _k = diag(0.05, 0.05)
Process noise covariance _Qk = diag(0.01, 0.01, 0.10, 5)

FIG. 14 is an explanatory diagram showing observation data at a certain epoch during a high-speed movement trial. FIG. 14 shows the position (distance from the radar sensor 130) of the hand test piece 320 (dummy hand) measured by the motion capture system, the position of the mannequin 330 (dummy chest) measured by the motion capture system, and the observation data by the radar sensor 130. This point is similar to FIG. 15 and others. In the example of FIG. 14, the observation data by the radar sensor 130 generally indicates the true value (the true position of the dummy hand), but also includes outliers that are significantly different from the true position of the dummy hand. In this high-speed movement trial, the ratios of normal values and outliers were 97.5% and 2.5%, respectively, and the ratios of additive outliers and temporary outliers among the outliers were 1.5% and 1.0%, respectively.

Figure 15 is an explanatory diagram showing the observed data at a certain epoch during a slow-speed movement trial. As shown in Figure 15, the observed data during a slow-speed movement trial contains more outliers (especially temporary outliers) than the fast-speed movement trial shown in Figure 14. In this slow-speed movement trial, the ratios of normal values and outliers were 84.8% and 15.2%, respectively, and the ratios of additive outliers and temporary outliers among the outliers were 0.2% and 15.0%, respectively. Thus, in the slow-speed movement trial, the frequency of outliers increased by about six times (2.5% to 15.2%) compared to the fast-speed movement trial, and the ratio of temporary outliers among the outliers increased from 40% (1.0/2.5) to 99% (15.0/15.2).

FIG. 16 is an explanatory diagram showing data after applying the asymmetric extended Kalman filtering process (AS-EKF) of this embodiment to observed data at a certain epoch during a high-speed movement trial. In the data after applying AS-EKF in the high-speed movement trial, the ratios of normal values and outliers were 99.6% and 0.4%, respectively, and the ratio of outliers was reduced by 84% from the value before applying AS-EKF (2.5%). In addition, in the data after applying AS-EKF in the high-speed movement trial, the ratios of additive outliers and temporary outliers among the outliers were 0.3% and 0.38%, respectively, and reduced by 80% and 62% from the values before applying AS-EKF (1.5% and 1.0%).

FIG. 17 is an explanatory diagram showing data after applying the asymmetric extended Kalman filtering process (AS-EKF) of this embodiment to observed data at a certain epoch during a slow-speed movement trial. In the data after applying AS-EKF in the slow-speed movement trial, the ratios of normal values and outliers were 99.94% and 0.06%, respectively, and the ratio of outliers was reduced by 99.6% from the value before applying AS-EKF (15.2%). In addition, in the data after applying AS-EKF in the slow-speed movement trial, the ratios of additive outliers and temporary outliers among the outliers were 0.0014% and 0.06%, respectively, and were reduced by 99.3% and 99.6% from the values before applying AS-EKF (0.2% and 15.0%).

FIG. 18 is an explanatory diagram summarizing the test results. Note that FIG. 18 also shows the test results when the comparative extended Kalman filtering process (OD-EKF) was applied. Referring to FIG. 18, it can be said that the asymmetric extended Kalman filtering process (AS-EKF) of this embodiment can effectively reduce outliers (temporary outliers and additional outliers) in both high-speed and low-speed movements, compared to the comparative extended Kalman filtering process (OD-EKF).

FIG. 19 is an explanatory diagram showing the comparison results between the estimated value using the asymmetric extended Kalman filtering process (AS-EKF) of this embodiment and the value calculated by generating temporary heavy-tailed Gaussian noise (α=5, λ=0.5, ρ=0.5). With reference to FIG. 19, it can be said that the asymmetric extended Kalman filtering process (AS-EKF) of this embodiment can tolerate both additive and temporary outliers.

FIG. 20 is an explanatory diagram summarizing the results of a comparison between the asymmetric extended Kalman filtering process (AS-EKF) of this embodiment and the extended Kalman filtering process (OD-EKF) of the comparative example. To make a fair comparison between the two, 100 rounds were performed for each simulation (L=100), and 26,000 observation vector samples generated by temporary heavy-tailed Gaussian noise were used (N=26,000). In both the AS-EKF of this embodiment and the OD-EKF of the comparative example, the RMSE increased as the above-mentioned scaling factor α and contamination rate λ increased. When the temporary contamination rate ρ was smaller than 0.2, there was no significant difference in performance between the AS-EKF and the OD-EKF. However, when the temporary contamination rate ρ was larger than 0.5, the RMSE increased significantly in the OD-EKF, while the RMSE did not increase significantly in the AS-EKF.

FIG. 21 is an explanatory diagram summarizing the ratio of erroneously adopting outliers on the dangerous side for the asymmetric extended Kalman filtering process (AS-EKF) of this embodiment and the extended Kalman filtering process (OD-EKF) of the comparative example. When the temporary contamination rate ρ was greater than 0.4, the ratio of erroneously adopting outliers on the dangerous side increased significantly in the OD-EKF of the comparative example, whereas the ratio did not increase significantly in the AS-EKF of this embodiment.

FIG. 22 is an explanatory diagram showing the comparison results of the ratio of dangerous outliers when α=5 and λ=0.15. For both the AS-EKF of this embodiment and the OD-EKF of the comparative example, the ratio of dangerous outliers increases as the temporary contamination rate ρ increases. However, regardless of the value of ρ, the ratio of dangerous outliers is always lower for the AS-EKF of this embodiment than for the OD-EKF of the comparative example. FIG. 23 is an explanatory diagram showing the comparison results of the ratio of dangerous outliers when α=7 and λ=0.3. Similarly, in the case shown in FIG. 23, the ratio of dangerous outliers is always lower for the AS-EKF of this embodiment than for the OD-EKF of the comparative example, regardless of the value of ρ. When the comparison results for other cases not shown are also taken into account, the ratio of outliers is lower for the AS-EKF of this embodiment than for the OD-EKF of the comparative example, so it can be said that the AS-EKF of this embodiment can withstand temporary outliers.

B. Variations:
The technology disclosed in this specification is not limited to the above-described embodiments, and can be modified in various forms without departing from the spirit of the invention. For example, the following modifications are also possible.

The configuration of the safety monitoring system 10 in the above embodiment is merely an example and can be modified in various ways. For example, in the above embodiment, the safety monitoring device 100 is attached to the robot 200, but the safety monitoring device 100 may be installed in a location separate from the robot 200.

In the above embodiment, the safety monitoring device 100 has the radar sensor 130, but the safety monitoring device 100 may have other types of object detection sensors, such as a laser sensor, instead of or in addition to the radar sensor 130. Also, the radar sensor 130 (or other types of sensors, the same applies below) may be provided separately from the safety monitoring device 100, and the safety monitoring device 100 may acquire observation data from the radar sensor 130 via the interface unit 150.

The contents of the object position estimation process in the above embodiment are merely examples and can be modified in various ways. For example, in the above embodiment, a probabilistic filtering process using an extended Kalman filter is executed, but other types of probabilistic filters (for example, other Kalman filters such as an Unscented Kalman Filter (abbreviated as UKF), a particle filter, an H ^∞ filter, etc.) may be used for the probabilistic filtering process.

In the above embodiment, the Mahalanobis distance is used to determine whether or not the observed data is an outlier, but other methods may be used to determine whether or not the observed data is an outlier.

In the above embodiment, when a positive outlier is detected, the method of calculating the posterior error covariance is different depending on whether the outlier is a temporary outlier or an additive outlier. However, when a positive outlier is detected, the posterior error covariance may be uniformly set to be the same as the a priori error covariance at the current discrete time.

In the above embodiment, compensation processing is performed when a negative outlier is detected, and exclusion processing is performed when a positive outlier is detected, but conversely, exclusion processing may be performed when a negative outlier is detected, and compensation processing may be performed when a positive outlier is detected.

In the above embodiment, a safety monitoring system 10 is described that is introduced to avoid contact between the robot 200 and the human HU in a space in which the robot 200 and the human HU exist, but the technology disclosed in this specification is not limited to this and can be similarly applied to cases in which the position of an object is estimated by probabilistic filtering processing.

In the above embodiment, some of the configurations realized by hardware may be replaced by software, and conversely, some of the configurations realized by software may be replaced by hardware.

10: Safety monitoring system 100: Safety monitoring device 110: Control unit 111: Position estimation unit 112: Observation data acquisition unit 113: Model acquisition unit 114: Outlier detection unit 115: Outlier identification unit 119: Robot control unit 120: Memory unit 130: Radar sensor 140: Operation input unit 150: Interface unit 190: Bus 200: Robot 310: Linear actuator 320: Hand test piece 330: Mannequin

Claims

An information processing device for estimating a position of an object,
a position estimation unit that executes a probabilistic filtering process that sequentially performs, at discrete times, a prediction step of calculating a priori estimate and a priori error covariance of the position of the object using a dynamic model of the object, and a filtering step of calculating a posterior estimate and a posterior error covariance of the position of the object based on observation data of the position of the object using a sensor, the priori estimate and the priori error covariance;
The position estimation unit is
an outlier detection unit that determines whether the observation data is an outlier;
an outlier identification unit that identifies whether the detected outlier is a negative outlier that deviates from the pre-estimated value on a side closer to the sensor, or a positive outlier that deviates from the pre-estimated value on a side farther from the sensor;
Including,
The position estimation unit is
When the observation data is determined to be the negative outlier, in the filtering step, one of a compensation process for performing compensation using the observation data determined to be the outlier and an exclusion process for excluding the observation data determined to be the outlier is executed;
When the observation data is determined to be the positive outlier, the information processing device executes the other of the compensation process and the exclusion process in the filtering step.
2. The information processing device according to claim 1,
The position estimation unit is
When the observation data is determined to be a negative outlier, the compensation process is performed;
When the observation data is determined to be the positive outlier, the information processing device executes the exclusion process.
3. The information processing device according to claim 2,
The information processing device, wherein the compensation process includes a process of calculating the posterior estimated value and the posterior error covariance based on the observation data in which error covariance has been increased, the a priori estimated value, and the a priori error covariance.
4. The information processing device according to claim 2,
The information processing apparatus, wherein the exclusion process includes a process of setting the posterior estimated value to be equal to the a priori estimated value at the current discrete time.
5. The information processing device according to claim 4,
the outlier identification unit identifies whether the detected positive outlier is a temporary outlier that occurs continuously across a plurality of discrete times or an additional outlier that occurs during a single discrete time;
The position estimation unit is
When the observation data is determined to be the temporary outlier, the exclusion process includes setting the posterior error covariance to be equal to the posterior error covariance at the previous discrete time;
When the observed data is determined to be the additional outlier, the information processing device sets the posterior error covariance to be equal to the a priori error covariance at the current discrete time as the exclusion process.
2. The information processing device according to claim 1,
The information processing device, wherein the outlier detection unit determines whether the observation data is an outlier by using a Mahalanobis distance.
2. The information processing device according to claim 1,
The information processing device, wherein the probabilistic filtering process is a filtering process using a Kalman filter.
2. The information processing device according to claim 1,
The information processing device, wherein the sensor is a radar sensor that measures the position of the object by transmitting and receiving radio waves.
The information processing device according to claim 1, further comprising:
An information processing device comprising: a robot control unit that controls an operation of a robot based on a result of estimation of the position of a human being as the target object by the position estimation unit.
An information processing method for estimating a position of an object, comprising:
a position estimation process that executes a probabilistic filtering process that sequentially performs, at discrete times, a prediction step of calculating a prior estimate and a prior error covariance of the position of the object using a dynamic model of the object, and a filtering step of calculating a posterior estimate and a posterior error covariance of the position of the object based on observation data of the position of the object using a sensor and the prior estimate and the prior error covariance,
The position estimation step includes:
determining whether the observed data is an outlier;
A step of identifying whether the detected outlier is a negative outlier that deviates from the pre-estimated value on the side closer to the sensor, or a positive outlier that deviates from the pre-estimated value on the side farther from the sensor;
Including,
The position estimation step includes:
When the observation data is determined to be the negative outlier, in the filtering step, one of a compensation process for performing compensation using the observation data determined to be the outlier and an exclusion process for excluding the observation data determined to be the outlier is executed;
an information processing method, wherein, when the observation data is determined to be the positive outlier, the filtering step is a step of executing the other of the compensation process and the exclusion process.
1. A computer program for estimating a position of an object, comprising:
On the computer,
a prediction step of calculating a priori estimate and a priori error covariance of the position of the object using a dynamic model of the object, and a filtering step of calculating a posteriori estimate and a posteriori error covariance of the position of the object based on observation data of the position of the object using a sensor and the priori estimate and the priori error covariance,
The position estimation process includes:
A process of determining whether the observed data is an outlier;
A process of identifying whether the detected outlier is a negative outlier that deviates from the pre-estimated value on the side closer to the sensor, or a positive outlier that deviates from the pre-estimated value on the side farther from the sensor;
Including,
The position estimation process includes:
When the observation data is determined to be the negative outlier, in the filtering step, one of a compensation process for performing compensation using the observation data determined to be the outlier and an exclusion process for excluding the observation data determined to be the outlier is executed;
a process of performing, in the filtering step, the other of the compensation process and the exclusion process when the observation data is determined to be the positive outlier.