JP2017083922A - System parameter identification device, system parameter identification method, and computer program therefor - Google Patents

Abstract

PROBLEM TO BE SOLVED: To disclose a technology capable of improving identification accuracy of a parameter when identifying the parameter of a system subjected to control while using a particle filter.SOLUTION: In a system parameter identification device 10, a particle filtering execution part 24 allocates a state vector of a system to each of a plurality of particles, calculates a weight of each of the particles on the basis of an observed value and an estimate of each of the particles and updates the particle on the basis of the weight. Therefore, the state vector is estimated and the parameter is identified. In the particle filtering execution part 24, the particle is updated at an interval of (n) steps ((n) is an integer equal to or greater than 2) from a first step within any period, and the weight of each of the particles is calculated on the basis of observed values and estimates that are acquired in at least two steps among the (n) steps included in one update cycle in which the particle is updated.SELECTED DRAWING: Figure 1

Description

  The technology disclosed in this specification relates to a system parameter identification device, a system parameter identification method, and a computer program therefor. Specifically, the present invention relates to a technique for identifying parameters of a system to be controlled.

  Non-Patent Document 1 discloses a technique for identifying a parameter of a system to be controlled using a particle filter. The particle filter is a known method for obtaining the value to be estimated by repeating the procedure of obtaining the weight of each particle based on a given observation value and the estimated value of each particle, and updating the particle based on the weight. is there.

Tetsuya Masuda, Toshiharu Sugie, “Parameter Identification of Linear Approximation System Using Particle Filter”, Transactions of the Institute of Systems, Control and Information Engineers, vol. 24, no. 10, p250-254, 2011

  In Non-Patent Document 1, the output of the system is observed for each step, and the particles are updated for each step. That is, the particle update period is equal to the step period of the system output. In this case, the weight of each particle is calculated based on the observed value and the estimated value in one step. In such a method, there are cases where the weight of particles increases for parameters having values different from actual parameters (hereinafter also referred to as true values). For this reason, it is possible to identify a parameter that is not a true value, and the accuracy of identifying a true value is low.

  The present specification discloses a technique capable of improving parameter identification accuracy when identifying a parameter of a system to be controlled using a particle filter.

  The present specification discloses a system parameter identification device that identifies a parameter of a system to be controlled using a particle filter. The system parameter identification device includes an input value acquisition unit, an observation value acquisition unit, a first function storage unit, a second function storage unit, a state vector calculation unit, an estimated value calculation unit, a particle filter execution unit, . The input value acquisition unit can acquire an input value to the system for each step in an arbitrary period. The observation value acquisition unit can acquire the observation value of the output from the system for each step in an arbitrary period. The first function storage unit stores a first function that calculates a system state vector at an arbitrary step within an arbitrary period based on a system state vector at a step immediately preceding the arbitrary step. The system state vector includes system state quantities, system parameters, and system noise related to input values. The second function storage unit stores a second function that calculates an estimated value of the output of the system at an arbitrary step based on the state vector of the system at the arbitrary step. The state vector calculation unit inputs the system state vector in the step immediately before the arbitrary step to the first function stored in the first function storage unit, and calculates the system state vector in the arbitrary step. . The estimated value calculating unit inputs a system state vector at an arbitrary step to the second function stored in the second function storage unit, and calculates an estimated value of the system output at the arbitrary step. The particle filter execution unit assigns a system state vector to each of a plurality of particles, and based on the observation values acquired from the observation value acquisition unit and the estimation values acquired from the estimation value calculation unit for each particle, The system state vector is estimated by updating the particles based on the weight and the system parameters are identified. In the particle filter execution unit, the particles are updated every n steps (n: an integer of 2 or more) from the first step in an arbitrary period, and the weight of each particle is the same as that of the particle being updated. It is calculated based on the observed value and the estimated value acquired in at least two of the n steps included in the update period.

  In the system parameter identification device described above, the particle update period is n times the step period (n is an integer of 2 or more). The weight of each particle is calculated based on the observed value and the estimated value acquired in at least two of the n steps included in one update cycle in which the particle is updated. . For this reason, compared with the case where the weight of each particle is calculated based on the observed value and the estimated value in one step, the particle weight may increase for a parameter different from the true value. It becomes difficult. Particle weights tend to be large for parameters closer to true values. As a result, the identification accuracy of the parameters of the system to be controlled can be improved.

  The present specification also discloses a novel system parameter identification method for identifying parameters of a system to be controlled using a particle filter. This system parameter identification method includes an input value acquisition step, an observation value acquisition step, a state vector calculation step, an estimated value calculation step, and a particle filter execution step. In the input value acquisition process, an input value to the system is acquired for each step in an arbitrary period. In the observation value acquisition step, the observation value of the output from the system is acquired for each step in an arbitrary period. In the state vector calculation process, a system state vector including elements of system state quantities, system parameters, and system noise related to input values, and the state of the system at any step within an arbitrary period The system state vector in an arbitrary step is input by inputting the system state vector in the previous step into a first function that calculates the vector based on the system state vector in the previous step of the arbitrary step. calculate. In the estimated value calculating step, the system state vector at any step is input to the second function that calculates the estimated value of the system output at the arbitrary step based on the system state vector at the arbitrary step. Compute an estimate of the system output at. In the particle filter execution step, a system state vector is assigned to each of a plurality of particles, and each particle is based on the observation value acquired in the observation value acquisition step and the estimation value acquired in the estimation value calculation step for each particle. The system state vector is estimated by updating the particles based on the weight and the system parameters are identified. In the particle filter execution step, particles are updated every n steps (n: an integer of 2 or more) from the first step in an arbitrary period, and the weight of each particle is the same as that of the particle being updated. It is calculated based on the observed value and the estimated value acquired in at least two of the n steps included in the update period. According to this system parameter identification method, the identification accuracy of the parameters of the system to be controlled can be appropriately improved.

  The present specification also discloses a novel computer program for identifying parameters of a system to be controlled using a particle filter. This computer program causes a computer to execute an input value acquisition process, an observation value acquisition process, a state vector calculation process, an estimated value calculation process, and a particle filter execution process. In the input value acquisition process, an input value to the system is acquired for each step in an arbitrary period. In the observation value acquisition process, the observation value of the output from the system is acquired for each step in an arbitrary period. In the state vector calculation process, a system state vector including elements of system state quantities, system parameters, and system noise related to input values, and the state of the system at any step within an arbitrary period The system state vector in an arbitrary step is input by inputting the system state vector in the previous step into a first function that calculates the vector based on the system state vector in the previous step of the arbitrary step. calculate. In the estimated value calculation process, the system state vector at an arbitrary step is input to a second function that calculates an estimated value of the system output at an arbitrary step based on the system state vector at an arbitrary step. Compute an estimate of the system output at. In the particle filter execution processing, a system state vector is assigned to each of a plurality of particles, and each particle is based on the observation value acquired by the observation value acquisition processing and the estimation value acquired by the estimation value calculation processing for each particle. The system state vector is estimated by updating the particles based on the weight and the system parameters are identified. In the particle filter execution processing, the particles are updated every n steps (n: an integer of 2 or more) from the first step in an arbitrary period, and the weight of each particle is the same as that of the particle being updated. It is calculated based on the observed value and the estimated value acquired in at least two of the n steps included in the update period. According to this computer program, it is possible to cause the computer to appropriately execute processing for identifying parameters.

  Details of the technology disclosed in this specification and further improvements will be described in detail in the detailed description and examples.

1 is a block diagram of a system parameter identification device and an external storage medium according to a first embodiment. The flowchart which shows the identification process of a system parameter. Time series data of the input torque to the robot arm. Time series data of the output angle from the robot arm. The graph of the dispersion | distribution formula of dispersion | distribution (beta) ₃ ² of parameter noise (zeta) ₃ of Coulomb frictional force (gamma). Dispersion equation graph of dispersion beta ₄ ² parameters noise zeta ₄ of viscous friction coefficient d. Dispersion equation graph of dispersion beta ₅ ² parameters noise zeta ₅ of inertia J. The first step and the graph showing the variance β ₁ ² ~β ₅ ² of each parameter noise ζ ₁ ~ζ ₅ at step when the particle update. The graph showing the particle distribution of each parameter in the step at the time of particle update in Experiment 1. The graph showing the distribution of the particle | grains of each parameter in the step at the time of the particle update of Experiment 2. FIG. FIG. 3 is a block diagram of a controller and system according to a second embodiment.

  The main features of the embodiments described below are listed. The technical elements described below are independent technical elements and exhibit technical usefulness alone or in various combinations, and are not limited to the combinations described in the claims at the time of filing. Absent.

(Feature 1) The system parameter identification device disclosed in the present specification may further include a noise applying unit that applies parameter noise to the system parameters. The noise adding unit may add parameter noise to the system parameters in the step of updating the particles. According to this configuration, the particles can be prevented from converging on a local solution (local minimum), and the identification accuracy can be further improved.

(Feature 2) In the system parameter identification device disclosed in this specification, the variance of the parameter noise may be variable. According to this configuration, the degree of particle diffusion can be changed.

(Characteristic 3) In the system parameter identification device disclosed in the present specification, if particles are updated m times (m is an integer equal to or greater than 2) within an arbitrary period, the j-th time (j: 1 to m−1). The variance of the parameter noise given in the step at the time of updating to (an arbitrary integer) is obtained in at least one of the n steps from the n · j + 1 step to the n · j + n step. It may be determined based on the input value or the observed value. According to this structure, it can suppress that a particle | grain spread | diffuses unnecessarily and a parameter can be identified efficiently.

(Feature 4) In the system parameter identification device disclosed in this specification, the weight of each particle may be calculated from the joint probability in the at least two steps. According to this configuration, since the weight of the particle is increased with respect to a parameter closer to the true value, the identification accuracy can be further improved.

(Feature 5) A controller for controlling the system may include the system parameter identification device described above. The control parameter of this controller may be determined based on the dispersion and average of particles updated by the particle filter execution unit. According to this configuration, the control parameter can be determined efficiently.

  The system parameter identification device 10 will be described with reference to FIGS. The identification device 10 identifies a parameter P of a system (for example, a robot arm) to be controlled using a particle filter. The identification device 10 is equipped with a computer that executes various processes for identifying the parameter P of the system. The computer includes a CPU that performs arithmetic processing, a RAM that temporarily stores arithmetic processing data, and a ROM that stores arithmetic programs executed by the CPU. When the CPU executes the arithmetic program stored in the ROM, the CPU functions as a state vector calculation unit, an estimated value calculation unit, and the like, which will be described later.

  The computer of the identification apparatus 10 includes an input value acquisition unit 12, an observation value acquisition unit 14, a first function storage unit 16, a second function storage unit 18, a state vector calculation unit 20, an estimated value calculation unit 22, , A particle filter execution unit 24, a dispersion calculation formula storage unit 26, and a noise determination unit 28.

The identification device 10 is used offline. That is, the external storage medium 30 stores in advance input value data 32 and observation value data 34 for a predetermined period (t = 0 to T). The input value data 32 is time series data indicating an input value to the system. The observation value data 34 is time-series data indicating the observation value of the output from the system with respect to the input.
The input value acquisition unit 12 acquires the input value u from the input value data 32 stored in the external storage medium 30 for each step (that is, at a constant cycle). The acquired input value is output to the noise determination unit 28 and the state vector calculation unit 20. This process is performed by the CPU. In the following, the input value u at the k-th step (k = 0 to T / T _s , T _s : step period) is represented as u _k, and the same applies to the notation of other values.
The observation value acquisition unit 14 acquires the observation value Y _k ^r for each step from the observation value data 34 stored in the external storage medium 30. The acquired observation value Y _k ^r is output to the noise determination unit 28 and the particle filter execution unit 24. This process is performed by the CPU. Note that the input value acquisition unit 12 and the observation value acquisition unit 14 may acquire the input value data 32 and the observation value data 34 in a part of a predetermined period.

  The first function storage unit 16 is provided in the ROM and stores the first function represented by Expression 3. The first function is a function that integrates the state equation of the system to be controlled and the relational expression of the parameter P between adjacent steps, and is defined in detail as follows.

The state equation of the system to be controlled is defined by Equation 1.
f is a known nonlinear function, X _k is the state quantity of the system at the k step, and η _k is the system noise at the k step. As is apparent from Equation 1, the state quantity X _{k + 1} at the ( _{k + 1) th} step is calculated based on the state quantity X _{k at the} kth step and the system noise η _k . In the present embodiment, system noise having an average input value u _k and variance α ² (constant) is used as the system noise η _k .

The relational expression of the parameter P between adjacent steps is defined by Expression 2.
ζ _k is parameter noise for the parameter P _{k at} the k-th step. The mean of ζ _k is zero and the variance is β ² . In this embodiment, the variance β ² is variable, but may be a constant. It will be described in detail later dispersion β ^2.

By integrating Equation 1 and Equation 2, a new state equation (first function) represented by Equation 3 is defined.
Z _k is a state vector of the system in the k-th step, and is defined as Z _k = (X _k ^T , P _k ^T ) ^T. f _a is a known nonlinear function derived from Equations 1 and 2. The state vector Z _k includes a state quantity X _k , a parameter P _k , a system noise η _k , and a parameter noise ζ _k as elements. As is clear from Equation 3, the state vector Z _{k + 1} at the ( _{k + 1) th} step is calculated based on the state vector Z _{k at the} kth step, the system noise η _k , and the parameter noise ζ _k .

The second function storage unit 18 is provided in the ROM and stores an output equation (hereinafter also referred to as a second function) represented by Expression 4.
Y _k is an estimated value of the output of the system at the k step, H _k is a known matrix at the k step, and σ _k is an observation noise at the k step. The average of σ _k is set in advance, and the variance is ρ ² (constant). As is apparent from Equation 4, the estimated value Y _k at the k-th step is calculated based on the state vector Z _k and the observation noise σ _k at the k-th step.

For each of S particles generated by a particle filter execution unit 24 to be described later, the state vector calculation unit 20 determines the state vector Z _k ⁱ (i = 1 to S) at the _k- ^th step and the input value acquisition unit 12. acquired the system noise eta _k which gave a predetermined dispersion alpha ² to the input values u _k and the parameter noise zeta _k obtained from the noise determining unit 28, the first function stored in the first function storage section 16 ( Then, the state vector Z (˜) _{k + 1} ⁱ at the ( _{k + 1} ) th step is calculated (refer to the tilde “˜” in the actual notation). The state vector Z (˜) _{k + 1} ⁱ calculated by the state vector calculation unit 20 is output to the estimated value calculation unit 22. This process is performed by the CPU. In the present embodiment, the state vector calculated by inputting to the first function is distinguished from the state vector estimated by the particle filter execution unit 24 by adding a tilde “˜”.

For each particle, the estimated value calculation unit 22 uses the second function (formula 4) stored in the second function storage unit 18 for the k + 1 step state vector Z (˜) _{k + 1} ⁱ acquired from the state vector calculation unit 20. And the estimated value Y (˜) _{k + 1} ⁱ of the system output of the ( _{k + 1} ) ^{th step} is calculated. At this time, the estimated value Y (˜) _{k + 1} ⁱ is calculated with the observation noise σ _k being zero. The estimated value Y (˜) _{k + 1} ⁱ calculated by the estimated value calculation unit 22 is output to the particle filter execution unit 24. This process is performed by the CPU. In the present embodiment, a tilde “˜” is added to the estimated value calculated by inputting to the second function.

The particle filter execution unit 24 generates S particles in the first step (that is, k = 0). The particles are uniformly distributed within a predetermined range. Here, the “predetermined range” is a range including the true value of the parameter P to be identified. A predetermined state vector Z ₀ ⁱ is assigned to each particle. Hereinafter, the state vector Z ₀ ⁱ in the first step is also referred to as an initial state vector.
Further, the particle filter execution unit 24 updates the particles every nth step from the first step. Specifically, the particle filter execution unit 24 updates particles according to the following procedure. (1) First, k = n · j-th step in which particles are updated (j = 1 to m, m: number of times particles are updated within a predetermined period T, n · m = T / T _s ) N observation values Y ^r (that is, Y _{n (j−1) +1} ^r to Y _{n (j−1) + n} ^r ₎ acquired from the observation value acquisition unit 14 after the j−1th particle update. N observation values Y ^r ) are obtained. Further, n estimated values Y (˜) ⁱ (that is, Y (˜) _{n (j−1) +1} ⁱ acquired after the j−1th update of the particles from the estimated value calculation unit 22 for each particle. acquires _{Y (~) n (j-} 1) + n i to the n-number of estimated values Y (~) ⁱ⁾ from. (2) Next, the n observed values Y ^r and the estimated values Y (˜) ⁱ are input to the weight calculation formula expressed by the following formula 5 to calculate the weight w ^{i of} each particle.
Note that W in Expression 5 is defined by Expression 6 below.
(3) Subsequently, the particles are updated so that the number of particles is large in the high weight region and small in the low weight region, and the state vector Z _{n · j} ⁱ at the k = n · j step is estimated. Thus, the system parameter P _{n · j} at the k = n · j step is identified. The state vector Z _{n · j} ⁱ is estimated based on the n observed values Y ^r and the estimated value Y (˜) ⁱ , and k = n · j steps calculated from the first function. It is different from the eye state vector Z (˜) _{n · j} ⁱ .
The state vector Z _{n · j} ⁱ estimated by the particle filter execution unit 24 is output to the state vector calculation unit. These processes are performed by the CPU.

The variance calculation formula storage unit 26 is provided in the ROM, and stores a calculation formula describing the variance β ² of the parameter noise ζ _k . This calculation formula is a function of the input value u or the observed value ^Yr , and is preset for each parameter (described later). Incidentally, this formula is not limited to the function of the input values u or observed value Y ^r, may be a function of the value (e.g., Y ^r) derived from the input value u or observed value Y ^r.

In the k = n · j-th step (where j ≠ m), the noise determination unit 28 receives the j + 1-th particle from the first step after the j-th particle update from the input value acquisition unit 12 or the observation value acquisition unit 14. Obtain n input values u or n observed values Y ^r up to the step at the time of update (ie, from _n input values u or Y _{n · j + 1} ^r from u _{n · j + 1} to u _{n · j + n N} observation values ^Yr up to ^Yn _{· j +} ^nr are acquired). Then, enter the one input value u or observed value Y ^r selected from n input values u or n observations Y ^r in the obtained dispersion equation from the dispersion equation storing section, a dispersion beta ² The parameter noise ζ _{n · j} at the k = n · j step is determined. On the other hand, in steps other than k = n · j, the above process is not executed, and the parameter noise ζ is uniformly set to zero. The parameter noise ζ _k determined in this way is output to the state vector calculation unit 20. This process is performed by the CPU.
As is apparent from the above description, ζ _k = 0 in the steps where the particles are not updated (steps other than k = n · j). That is, P _{k + 1} = P _k is established in the step where the particle is not updated. For this reason, the noise determination unit 28 can also determine the parameter noise only in the step of updating the particle and output it to the state vector calculation unit 20.
In the above configuration, the noise determination unit 28 determines the parameter noise only in the step of updating the particles. However, the configuration is not limited to this configuration. The noise determination unit 28 may determine the parameter noise ζ ₀ also in the first step (k = 0). In this case, the variance β ² of the parameter noise ζ ₀ is one input value u or observation value Y selected from n input values u or n observation values Y ^r from k = 1 to n steps. It is calculated by inputting ^r into the dispersion formula. The noise determining unit 28 corresponds to an example of “noise applying unit”.

Next, a process in which the computer of the identification apparatus 10 identifies the system parameter P will be described with reference to FIG. First, in the input value acquisition process of step S <b> 2, the input value acquisition unit 12 acquires the input value uk for the entire period for each step from the input value data 32 stored in the external storage medium 30. Next, in the observation value acquisition step of step S4, the observation value acquisition unit 14 acquires the observation values Y _k ^r for the entire period from the observation value data 34 stored in the external storage medium 30 for each step. Subsequently, in the initial state vector determination step in step S6, the particle filter execution unit 24 generates S particles in the first step (k = 0), and the initial state vector Z ₀ ⁱ (i = 1) is generated for each particle. ~ S). Note that the steps S2 to S6 are in no particular order.

Subsequently, in the state vector calculation step of step S8, the state vector calculation unit 20 performs the initial state vector Z ₀ ^{i for} each particle assigned in step S6 and the system noise η ₀ (that is, the average is the input value u ₀ , The noise having variance α ² ) and ζ ₀ = 0 are input to the first function, and the state vector Z (˜) ₁ ⁱ is calculated. Next, the state vector calculation unit 20 inputs Z (˜) ₁ ⁱ , η ₁ and ζ ₁ = 0 to the first function, and calculates Z (˜) ₂ ⁱ . State vector calculation unit 20, the process is repeated n times until k = n-th step, each particle n states vector Z (~) to calculate a ^{_{1 i ~Z (~) n i}} . Note that ζ _k is 0 from k = 0 to the (n−1) th step. Subsequently, the estimated value calculation step of step S10, the estimated value calculating section 22 inputs the n state vector Z calculated in step S8 (~) _{1 i} ^~Z the (~) _n ⁱ to the second function Te, n pieces of estimated values Y (~) for each particle to calculate the ^{_{1 i ~Y (~) n i}} .

Subsequently, in the particle filter execution step in step S12, the first particle update is performed. Specifically, the particle filter execution unit 24, of the observed value _Y ^{k r} of the total period obtained in step S4, k = n number of observations _Y ¹ r of 1~n until th step _{to Y} ^{n r} If, type acquired n pieces of estimated value Y in step S10 (~) ₁ ⁱ to Y a (~) _n ⁱ weight calculation formula (see equation 5), calculates the weight ^{w i} of each particle. Then, the particle filter execution unit 24 updates the particles based on the weight w ^i, estimates the k = n th step of the state vector Z _n ⁱ for each particle. The parameter P at the k = n step is identified by the particle distribution at this time.

Subsequently, in step S14, it is determined whether or not the particle update is the last (that is, j = m). If the particle update is not the last time (NO in step S14), the process proceeds to step S16. In the variance determining step of step S16, the noise determining unit 28 performs n input values u _{n + 1 to} u _2n up to k = _{n + 1 to 2n} steps or n observed values Y _{n + 1} ^{r to} Y _2n ^r (or the input values u _k or observed values Y _k 1 input values selected from ^r value derived from) u _k or observed values Y _k ^r, the calculation expression stored in the distributed computing equation storing section 26 The variance β ² is input to calculate the parameter noise ζ _n at the k = nth step.

Subsequently, in the state vector calculation step in step S18, the state vector calculation unit 20 performs the k = n-th state vector Z _n ⁱ estimated in step S12, η _n, and the parameter noise determined in step S16. ζ _n is input to the first function to calculate the state vector Z (˜) _{n + 1} ⁱ of the k = n + 1 step. When the process of step S18 ends, the computer of the identification apparatus 10 returns to the process of step S8. In step S <b> 8, the state vector calculation unit 20 calculates n−1 state vectors Z (˜) _{n + 2} ^{i to} Z (˜) _2n ^{i up} to k = _{n + 2} to _2n steps. Subsequently, in step S10, n estimated values Y (to) _{n + 1} ⁱ to Y ((n) are calculated from the n state vectors Z (to) _{n + 1} ^{i to} Z (to) _2n ⁱ calculated in steps S18 and S8. ~) _2n ⁱ is calculated, in step S12, the second particle update occurs. Thereafter, the above-described processing is repeated, and when the m-th (that is, last) particle update is performed in step S12 and the state vector Z _{n · m} ⁱ at the k = n · m step is estimated, the identification apparatus 10 In step S14, the parameter P identification processing ends. The parameter P at the k = n · m step is identified by the particle distribution at this time.

As is clear from the above description, in the step of updating the particles (k = n · j, j = 1 to m), the state vector is the state vector Z (˜) _n calculated by the state vector calculation unit 20. and ^i, 2 kinds of state vector Z _n ⁱ estimated by a particle filter execution unit 24 is present. In the process of step S18, the latter state vector Z _n ⁱ is used as the state vector input to the first function. When parameter identification is performed off-line as described above, instead of determining the parameter noise ζ _{n · j} each time as in step S16, m−1 variances β ² are calculated in advance. The parameter noise ζ _{n · j} may be determined.

In the identification device 10 described above, the particle update cycle is n times the step interval T _s . Then, the weight w ^{i of} each particle is calculated based on the observed value Y ^r and the estimated value Y (˜) ⁱ acquired in n steps included in one update cycle in which the particle is updated. The For this reason, compared with the case where the weight of each particle is calculated based on the observed value Y ^r and the estimated value Y (˜) ⁱ in one step, the particle weight w ⁱ is a parameter closer to the true value. It becomes easy to become large. As a result, the identification accuracy of the parameters of the system to be controlled can be improved.

  The identification device 10 includes a noise determination unit 28. The noise determination unit 28 determines the parameter noise ζ and outputs it to the state vector calculation unit 20 in the step (k = n · j, where j ≠ m) when the particle is updated. According to this configuration, the particles can be prevented from converging on a local solution (local minimum), and the identification accuracy can be further improved.

Further, in the identification device 10 described above, the noise determination unit 28 determines the variance β ² of the parameter noise ζ using the variance calculation formula stored in the variance calculation formula storage unit 26, so that the variance β ² is variable. Become. According to this configuration, to change the degree of diffusion of the particles according to the dispersion beta ^2.

In particular, in the identification device 10 described above, the variance β ^{2 of the} parameter noise ζ _{n · j} input to the first function in the step at the j-th update of the particle is the n · j + n step from the k = n · j + 1 step. It is determined based on the input value u or the observed value Y ^r acquired in one of the n steps up to the eye. According to this configuration, it can be determined a dispersion beta ² according to amount of information for identifying the parameters. Therefore, information due to that set a large dispersion beta ² when less can suppress the occurrence of a situation that the particles will be unnecessarily spread, can be efficiently identified parameters.

In the identification device 10 described above, the weight w ^{i of} each particle is calculated from the simultaneous probabilities in n steps. For this reason, the weight w ⁱ of the particle becomes larger with respect to the parameter closer to the true value, and the identification accuracy can be further improved.

Next, a specific example in which the parameter identification accuracy of the identification device is verified using a simulation model of the robot arm will be described. In this specific example, the identification apparatus performs Experiment 1 and Experiment 2 as a comparative example of Experiment 1. In Experiment 1, above the noise determination process is performed (i.e., variance beta ² parameters noise ζ is variable). In Experiment 2, noise determination processing is not performed (that is, the variance β ² of the parameter noise ζ is a constant). This robot arm is a single-axis motion model, and rotates about the axis of the robot arm as a rotation axis. The robot arm parameters P identified by the identification device 10 are m, φ, γ, d, and J. m is a coefficient representing the swelling of the hysteresis of rolling friction, φ is the rolling displacement area width, γ is the Coulomb friction force, d is the viscous friction coefficient, and J is the moment of inertia.

(Experiment 1)
First, the state equation of the robot arm and the relational expression of the parameter P between adjacent steps are integrated to obtain the first function of the robot arm. The equation of state of the robot arm is obtained by discretizing the equation of motion of the robot arm shown by the following equation 7 in the following procedure.
θ is a rotation angle of the robot arm, τ is a torque input to the robot arm, and v is a rolling friction force including Coulomb friction. Here, v (t) is defined by the following Expression 8.
ω is the angular velocity (= θ) of the robot arm, ν (new) is the rolling friction force after the speed reversal, δ is the moving distance after the speed reversal, and ξ = δ / φ. Here, g (ξ) is defined by the following Equation 9.

Here, as described above, in order to implement Expressions 7 to 9 in the particle filter, it is necessary to convert them into a discrete system. In order to convert to a discrete system, first, the speed reversal is determined, and the rolling friction force ν _k after the speed reversal and the moving distance δ _k after the speed reversal are determined according to the following equations 10 and 11.
When Expressions 10 and 11 are substituted into Expression 8, the rolling friction force v _k in a discrete system can be derived as the following Expression 12. However, Equation 9 is used for g (ξ), and δ _k / φ _k is used for ξ.

On the other hand, when Equation 7 is rewritten into the state equation, the following Equation 13 is obtained.
When Expression 13 is simplified, the following Expression 14 is obtained.
When Expression 14 is discretized assuming zero-order hold at the input τ (t), the following Expression 15 is obtained. However, the discretization of v (t) follows Formula 12.
When equation 15 is replaced with a general equation, the state equation of the robot arm expressed by the following equation 16 is obtained.
Here, f is a known nonlinear function representing Equation 15 including nonlinear rolling friction, X _k ′ is X _k and state quantities ω _k−1 , θ _k−1 , δ _k−1 , ν _{k of} rolling friction. ₋₁ , v _k−1 containing state matrix, η _k is an input signal of mean τ _k and variance α ² (= 2.5 × 10 ⁻³ ), and corresponds to so-called system noise.

On the other hand, the relational expression of the parameter P between adjacent steps is as shown in Expression 2. Here, P _k is a matrix composed of five parameters to be identified in the simulation model of the robot arm, and is expressed by the following Expression 17.

By integrating Expression 16 and Expression 2, a new state equation (first function) represented by Expression 3 is defined. However, it is defined as Z _k = (X _k ′ ^T , P _k ^T ) ^T. f _a is a known nonlinear function derived from Equations 16 and 2.

On the other hand, since the observation amount is the angle θ _k , the output equation (second function) is linear and is expressed by the following Expression 18.
In this model, the observation noise σ _k is set to mean zero and variance ρ ² = 4.0 × 10 ⁻⁶ .

FIG. 3 shows time-series data of torque τ (t) input to the robot arm, and FIG. 4 shows time-series data of angle θ (t) output from the robot arm. The former corresponds to the input value data 32 and the latter corresponds to the observation value data 34. The period T of the time series data is T = 45 seconds, and the step interval T _s is T _s = 0.01 seconds. The input value acquisition unit 12 and the observation value acquisition unit 14 of the identification device 10 acquire input values and observation values for all periods of time-series data. The particles are updated at the n = 250th step. That is, the particle update cycle is 2.5 seconds, and the particles are updated 18 times in total (j = 1 to 18). The total number S of particles is S = 50000. The true values of the parameters are set to m = 0.8, φ = 1.5, γ = 5.0, d = 2.0, and J = 4.0.

Next, the variance calculation formula stored in the variance calculation formula storage unit 26 will be described. In this model, since there are five parameters to be identified, the parameter noise ζ _k has five parameter noises and variances corresponding to each parameter. In the following, parameter noise and variance corresponding to the parameters m _k , φ _k , γ _k , d _k , J _k are respectively expressed as ζ _1k and β ₁ ² , ζ _2k and β ₂ ² , ζ _3k and β ₃ ² , ζ. _{Let 4k} and β ₄ ² , ζ _5k and β ₅ ² .

(Dispersion calculation formula of dispersion β ₁ ² and β ₂ ² )
Rolling friction force v _k, moving distance of the displacement region width phi _k out rolling after speed reversal, the Coulomb friction force γ equivalent. For this reason, the coefficients m _k and φ _k representing the bulge of rolling friction hysteresis are identification parameters that affect the rolling friction force v _k only after the speed reversal. Accordingly, the dispersion calculation formulas of the variances β ₁ ² and β ₂ ² of the parameter noises ζ _1k and ζ _2k of the parameters m _k and φ _k are expressed by the following formulas 19 and 20, respectively.
The presence or absence of speed reversal is determined from the observed value data θ (t) (see FIG. 3).

(Dispersion calculation formula of variance β ₃ ² )
The dispersion calculation formula of the dispersion β ₃ ² of the parameter noise ζ _3k of the parameter γ _k (Coulomb friction force) is expressed by Equation 21 (see FIG. 5). Note that β ₃ ² _max = 2.5 × 10 ⁻³ . The variance β ₃ ² of the k = n · j step is expressed by the equation 21 in which the absolute value max (| ω |) of the maximum angular velocity of the observation value data θ (t) from the k = n · j + 1 to n · j + n steps Calculated by inputting. Since the viscous friction force is dominant over the Coulomb friction force during high-speed rotation of the robot arm, the variance β ₃ ² of the parameter noise ζ _3k of the Coulomb friction force γ _k is set to be small. Further, when the robot arm rotates at a very low speed, it is determined that there is a high possibility that it is immediately after the speed reversal, and in this case, the variance β ₃ ² is set to be small.

(Dispersion calculation formula of variance β ₄ ² )
The viscous frictional force is a frictional force proportional to the angular velocity. For this reason, when the angular velocity is small, it is considered that the influence of the viscous friction force on the behavior of the robot arm becomes small and the system identification becomes difficult. Therefore, the parameter _{d k} parameter noise ζ dispersed beta ₄ ² of _4k of (viscous friction coefficient), the absolute value max of the maximum angular velocity (| omega |) in proportion to the variance calculation formula of the dispersing beta ₄ ² follows It is determined as shown in Equation 22 (see FIG. 6). Note that β ₄ ² _max = 2.5 × 10 ⁻³ .

(Dispersion equation of dispersing beta ₅ ²⁾
The parameter J _k (moment of inertia) can be identified by the system when the robot arm performs a sufficient acceleration / deceleration operation. In other words, it cannot be identified when the robot arm rotates at a constant speed or stops. Therefore, dispersion beta ₅ ² of system noise zeta _5k moment of inertia J _k is in proportion to the torque tau (input value) defines the dispersion equation of dispersing beta ₅ ² as the following equation 23 (FIG. 7 reference). The variance β ₅ ² at the k = n · j step is expressed in Equation 23 by the absolute value max (| τ |) of the maximum torque of the input value data τ (t) up to the k = n · j + 1 to n · j + n steps. Calculated by inputting. Note that β ₅ ² _max = 1.0 × 10 ⁻² .

8A to 8E show the variance of the parameter noise ζ in the first step (k = 0) and the step (k = n · j (j = 1 to 17)) at the time of particle update. The horizontal axis N represents the number of variances β ₁ ^{2 to} β ₅ ² counted from the first step, and the vertical axis β ₁ ^{2 to} β ₅ ² represents the value of the variance. In this experiment, since the variance of the parameter noise ζ is also calculated in the first step, a total of 18 variances are calculated. As is apparent from FIGS. 8A to 8E, the dispersions β ₁ ^{2 to} β ₅ ² are variable. For example, in FIG. 8A, the variance β ₁ ^{2 with} N = 1 is the variance of the parameter noise ζ _{1 · 0 at} the k = 0 step, and the variance β ₁ ^{2 with} N = 18 is k = 250 · This is the variance of the parameter noise ζ _{1, 250, 17 at} the 17th step. FIGS. 9A to 9E show the particle distribution of each parameter when the identification device 10 executes the robot arm parameter identification process under the above-described conditions. In FIG. 10, the dark color indicates an area where the number of particles is small, and the light color indicates an area where the number of particles is large. The broken line indicates the true value of the parameter, and the circle indicates the average value of the particles at the step of updating the particles.

(Experiment 2)
FIGS. 10A to 10E show the particle distribution of each parameter when the variance β ² of the parameter noise ζ _k of each parameter is a constant. In this case, the variances of the parameter noises ζ _k of the parameters m _k , φ _k , γ _k , d _k , and J _k are β ₆ ² , β ₇ ² , β ₈ ² , β ₉ ² , β ₁₀ ^{2, respectively.} Then, each variance is β ₆ ² = β _1max ² = 0.02 ² , β ₇ ² = β _2max ² = 0.05 ² , β ₈ ² = β _3max ² = 2.5 × 10 ⁻³ , β ₉ ² = β _4max ² = 2.5 × 10 ⁻³ and β ₁₀ ² = β _5max ² = 1.0 × 10 ⁻² .

Table 1 shows the average and dispersion of the particles in each step at the time of the last particle update when Experiments 1 and 2 were performed for each parameter. Table 2 shows, for each parameter, the average error rate (%) of particles and the average of dispersion at the step of updating the last particle when Experiments 1 and 2 are performed 10 times.
According to FIGS. 9 and 10 and Tables 1 and 2, the average of the particles in Experiments 1 and 2 gradually converges to a value close to the true value every time the particles are updated. However, in Experiment 1, except for the parameter J, the average of the particles is closer to the true value, and the average error rate is smaller. Regarding parameter J, the average difference between experiments 1 and 2 and the difference in average error rate are negligible. Moreover, the average of particle dispersion is much smaller in Experiment 1 except for the parameter φ. Regarding the parameter φ, the average difference between the variances of Experiments 1 and 2 is very small. From this, it can be seen that the variable parameter noise variance can improve the parameter identification accuracy while suppressing the variation of the simulation results each time, compared to the case where the variance is a constant.

Regarding Experiment 1, according to the results of Tables 1 and 2, the average error rate of the parameters φ, γ, d, and J are all less than 0.5%, whereas the average error rate of the parameter m is Is 1.829%, which is slightly larger than other parameters. This is considered to be because θ _k is insensitive to the change of the parameter m. For this reason, when the identification result is applied to control such as sensorless force control, it seems that the influence is not so great.

9 and 10, it can be seen that in Experiment 2, the average of the particles greatly deviates from the true value in the process of repeatedly updating the particles in Experiment 2. This is because even when there is not enough information for identifying a parameter, parameter noise ζ having a certain variance is given to the parameter to be identified. In particular, in Experiment 2, the average of the particles of parameter J deviates significantly from the true value at j = 13 and 14. This cause is presumed as follows. That is, according to FIGS. 3 and 4, in the step of j = 13, 14 (that is, t = 30 to 32.5 seconds), the input torque τ is almost zero from about 7.5 seconds before the output angle θ. The angular acceleration of is almost zero. In this way, when the motion of the robot arm hardly generates angular acceleration, if the parameter noise ζ having a certain variance is given to the parameter as in Experiment 2, the particles are unnecessarily diffused and are not true values. This is presumed to be caused by the aggregation of particles. As in Experiment 1, by changing the variance β ² of the parameter noise ζ depending on whether or not there is sufficient information for identifying the parameter, not only at the end of the update of the particle but also at the end of the update of the particle. It can be seen that the parameters can be identified with high accuracy even in the process of repetition.

  Next, Example 2 will be described with reference to FIG. In the present embodiment, the controller 40 is communicably connected to the system 42 to be controlled. The controller 40 includes a system parameter identification device 44 and a control unit 46. The control unit 46 determines the control input value to the system 42 by performing feedback control using the control output value from the system 42 so that the control output value from the system 42 approaches the target value. The control unit 46 has a control parameter for determining a control input value to the system 42, and the control input value is determined based on the control parameter (described later).

Unlike the identification device 10 of the first embodiment, the identification device 44 identifies system parameters online. That is, the input value acquisition unit and the observation value acquisition unit of the identification device 44 acquire the control input value and the control output value of the system 42 in real time during the operation of the system 42. The identification device 44 performs the same processing as that of the identification device 10 of the first embodiment except that the noise determination processing is not performed. That is, in the present embodiment, the variance β ² of the parameter noise ζ of the system parameter at the k = n · jth step (j = 1 to m−1) is not variable but constant. When the particle filter execution unit 24 of the identification device 44 updates the particles every k = n · j steps (j = 1 to m), the average and variance of the updated particles are output to the control unit 46. Is done.

  In the present embodiment, the control parameter calculation formula for calculating the control parameter is designed to be a function having the average of particles, the dispersion of particles, and the deviation between the control output value and the control input value as variables. . For this reason, the control unit 46 inputs the average and variance of the particles output from the identification device 44 at the k = n · j step, and the deviation between the control output value and the control input value to the control parameter calculation formula. K = n · jth step control parameters are calculated. Based on this control parameter, the control unit 46 determines the control input value of the k = n · j + 1 step. The controller 40 identifies the control parameter by repeating the above processing every k = n · j steps.

  According to this configuration, the controller 40 simultaneously performs feedback control of the system 42 and identification of system parameters. The controller 40 determines a control parameter by using an online system parameter identification result (specifically, average and dispersion of particles) by the identification device 44. According to this configuration, it is possible to efficiently determine an accurate control parameter as compared with a configuration in which the control unit 46 determines the control parameter by trial and error.

  Specific examples of the present invention have been described in detail above, but these are merely examples and do not limit the scope of the claims. The technology described in the claims includes various modifications and changes of the specific examples illustrated above.

For example, the identification apparatus sets the weight w ^{i of} each particle to the observed value Y ^r and the estimated value Y (˜) acquired in at least two of the n steps included in one update period. You may calculate based on ⁱ . Further, the weight w ^{i of} each particle may be obtained by a method other than the joint probability (that is, a weight calculation formula other than Equation 5). The variances α ² and ρ ² may be set in advance so as to change at each step. Further, the average of the parameter noise ζ and the observation noise σ may not be zero. Further, the noise determining unit 28 may not be configured to determine the parameter noise ζ in all steps in which particles are updated. For example, the noise determination unit 28 may be configured to determine the parameter noise ζ in every other step (that is, periodically) among all the steps in which particles are updated.

  The technical elements described in this specification or the drawings exhibit technical usefulness alone or in various combinations, and are not limited to the combinations described in the claims at the time of filing. In addition, the technology illustrated in the present specification or the drawings achieves a plurality of objects at the same time, and has technical utility by achieving one of the objects.

10: system parameter identification device, 12: input value acquisition unit, 14: observation value acquisition unit, 16: first function storage unit, 18: second function storage unit, 20: state vector calculation unit, 22: estimated value calculation unit , 24: particle filter execution unit, 26: dispersion calculation formula storage unit, 28: noise determination unit, 30: external storage medium, 32: input value data, 34: observation value data, 40: controller, 44: identification device

Claims (8)

A system parameter identification device that identifies a parameter of a system to be controlled using a particle filter,
An input value acquisition unit, an observation value acquisition unit, a first function storage unit, a second function storage unit, a state vector calculation unit, an estimated value calculation unit, and a particle filter execution unit,
The input value acquisition unit can acquire an input value to the system for each step in an arbitrary period,
The observation value acquisition unit can acquire the observation value of the output from the system for each step in an arbitrary period,
The first function storage unit calculates a first function that calculates a state vector of the system at an arbitrary step within the arbitrary period based on a state vector of the system at a step immediately before the arbitrary step. The system state vector includes elements of the system state quantity, the system parameters, and system noise associated with the input value;
The second function storage unit stores a second function for calculating an estimated value of the output of the system in the arbitrary step based on the state vector of the system in the arbitrary step.
The state vector calculation unit inputs the state vector of the system in the step immediately before the arbitrary step to the first function stored in the first function storage unit, and the state vector in the arbitrary step Calculate the system state vector,
The estimated value calculation unit inputs the state vector of the system in the arbitrary step to the second function stored in the second function storage unit, and estimates the output of the system in the arbitrary step To calculate
The particle filter execution unit assigns a state vector of the system to each of a plurality of particles, and is based on an observation value acquired from the observation value acquisition unit and an estimation value acquired from the estimation value calculation unit for each particle. Calculating the weight of each particle, and updating the particles based on the weight to estimate the state vector of the system and identifying the parameters of the system,
In the particle filter execution unit,
The particles are updated every nth step from the first step in the arbitrary period (n: an integer of 2 or more),
The weight of each particle is calculated based on the observed value and the estimated value acquired in at least two of the n steps included in one update cycle in which the particle is updated. Parameter identification device. It further includes a noise applying unit that adds parameter noise to the parameters of the system,
The system parameter identification device according to claim 1, wherein the noise adding unit adds the parameter noise to a parameter of the system in a step in which particles are updated.   The system parameter identification device according to claim 2, wherein the variance of the parameter noise is variable.   If the particle is updated m times (m: integer greater than or equal to 2) within the arbitrary period, it is given at the step when it is updated j times (j: any integer from 1 to m−1). The variance of the parameter noise is determined based on an input value or an observation value acquired in at least one of the n steps from the n · j + 1 step to the n · j + n step. The system parameter identification device according to claim 3.   The system parameter identification device according to any one of claims 1 to 4, wherein a weight of each particle is calculated from a joint probability in the at least two steps. A controller for controlling the system, comprising the system parameter identification device according to claim 1,
The control parameter of the controller is determined based on a dispersion and an average of particles updated by the particle filter execution unit. A system parameter identification method for identifying a parameter of a system to be controlled using a particle filter,
An input value acquisition step, an observation value acquisition step, a state vector calculation step, an estimated value calculation step, and a particle filter execution step,
In the input value acquisition step, an input value to the system is acquired for each step in an arbitrary period,
In the observation value acquisition step, the observation value of the output of the system is acquired for each step in an arbitrary period,
In the state vector calculation step, the state vector of the system, the system state vector including the system state quantity, the system parameter, and the system noise related to the input value as elements, in any step within the arbitrary period The system state vector in the previous step is input to a first function that calculates the system state vector based on the system state vector in the previous step of the arbitrary step. Calculating the state vector of the system at any step;
In the estimated value calculating step, a second function for calculating an estimated value of the output of the system at the arbitrary step based on the state vector of the system at the arbitrary step is used as a second state function of the system at the arbitrary step. To calculate an estimate of the output of the system at the arbitrary step,
In the particle filter execution step, a system state vector is assigned to each of a plurality of particles, and based on the observation value acquired in the observation value acquisition step and the estimation value acquired in the estimation value calculation step for each particle. Calculating the weight of each particle, and updating the particles based on the weight to estimate the state vector of the system and identifying the parameters of the system,
In the particle filter execution step,
The particles are updated every nth step from the first step in the arbitrary period (n: an integer of 2 or more),
The weight of each particle is calculated based on the observed value and the estimated value acquired in at least two of the n steps included in one update cycle in which the particle is updated. Parameter identification method. A computer program for identifying parameters of a system to be controlled using a particle filter,
Let the computer execute an input value acquisition process, an observation value acquisition process, a state vector calculation process, an estimated value calculation process, and a particle filter execution process,
In the input value acquisition process, an input value to the system is acquired for each step in an arbitrary period,
In the observation value acquisition process, the observation value of the output of the system is acquired for each step in an arbitrary period,
In the state vector calculation process, the state vector of the system, the system state vector including the system state quantity, the system parameter, and the system noise related to the input value as elements, and in an arbitrary period within the arbitrary period The system state vector in the previous step is input to a first function that calculates the state vector of the system in the step based on the state vector of the system in the step immediately before the arbitrary step. And calculating the state vector of the system in the arbitrary step,
In the estimated value calculating process, a second function for calculating an estimated value of the output of the system at the arbitrary step based on the state vector of the system at the arbitrary step is used as a second state function of the system at the arbitrary step. To calculate an estimate of the output of the system at the arbitrary step,
In the particle filter execution process, a state vector of the system is assigned to each of a plurality of particles, and based on the observed value acquired in the observed value acquisition process and the estimated value acquired in the estimated value calculation process for each particle. Calculating the weight of each particle, and updating the particles based on the weight to estimate the state vector of the system and identifying the parameters of the system,
In the particle filter execution process,
The particles are updated every nth step from the first step in the arbitrary period (n: an integer of 2 or more),
The weight of each particle is calculated based on the observed value and the estimated value acquired in at least two of the n steps included in one update cycle in which the particle is updated. program.