An MPEC Formulation for Parameter Identification of Complementarity - PowerPoint PPT Presentation

An MPEC Formulation for Parameter Identification of Complementarity Systems An MPEC Formulation for Parameter Identification of Complementarity Systems S. Berard J.C. Trinkle Department of Computer Science Rensselaer Polytechnic Institute May 30, 2008

An MPEC Formulation for Parameter Identification of Complementarity Systems Outline 1 Introduction 2 Complementarity Problem 3 Dynamics Model 4 Estimation Problem 5 Identification as an Optimization Problem 6 MPEC 7 Examples 2D Particle Falling and Sliding Multiple particles Experimental Sliding Block Results 8 Conclusion

An MPEC Formulation for Parameter Identification of Complementarity Systems Introduction Problem Statement Using noisy observations of a dynamical multi-rigid-body system, determine the parameters of a given mixed complementarity problem dynamics model that best predicts the observation.

An MPEC Formulation for Parameter Identification of Complementarity Systems Introduction Problem Statement Using noisy observations of a dynamical multi-rigid-body system, determine the parameters of a given mixed complementarity problem dynamics model that best predicts the observation.

An MPEC Formulation for Parameter Identification of Complementarity Systems Introduction Problem Statement Using noisy observations of a dynamical multi-rigid-body system, determine the parameters of a given mixed complementarity problem dynamics model that best predicts the observation.

An MPEC Formulation for Parameter Identification of Complementarity Systems Introduction Problem Statement Using noisy observations of a dynamical multi-rigid-body system, determine the parameters of a given mixed complementarity problem dynamics model that best predicts the observation.

An MPEC Formulation for Parameter Identification of Complementarity Systems Introduction Problem Statement Using noisy observations of a dynamical multi-rigid-body system, determine the parameters of a given mixed complementarity problem Coefficient of friction = 0.2 dynamics model that best predicts the observation.

An MPEC Formulation for Parameter Identification of Complementarity Systems Complementarity Problem Complementarity Problem Let u ∈ R n 1 , v ∈ R n 2 and let g : R n 1 × R n 2 → R n 1 , f : R n 1 × R n 2 → R n 2 be two vector functions and the notation 0 ≤ x ⊥ y ≥ 0 imply that x is orthogonal to y and each component of each vector is non-negative. Definition Orthogonality The mixed complementarity problem is to     0 + find u and v satisfying 0 +         x =  = y + 0 g ( u , v ) = 0 , u free        . . . . 0 ≤ v ⊥ f ( u , v ) ≥ 0 . .

An MPEC Formulation for Parameter Identification of Complementarity Systems Dynamics Model Instantaneous Dynamics Model M : Inertia matrix W n : Maps normal forces to body λ vp : Velocity product forces frame λ app : Applied forces W f : Maps friction forces to body λ n : Magnitude of normal forces frame λ n : Magnitude of frictional q : Configuration forces ν : Velocity Newton-Euler Equations: M ( q ) ˙ ν = W n λ n + W f λ f + λ vp + λ app

An MPEC Formulation for Parameter Identification of Complementarity Systems Dynamics Model Instantaneous Dynamics Model G = Jacobian of kinematic velocity map Newton-Euler Equations: M ( q ) ˙ ν = W n λ n + W f λ f + λ vp + λ app Kinematic Map: ˙ q = G ν

An MPEC Formulation for Parameter Identification of Complementarity Systems Dynamics Model Instantaneous Dynamics Model ψ i n = signed distance function at contact i Newton-Euler Equations: M ( q ) ˙ ν = W n λ n + W f λ f + λ vp + λ app Kinematic Map: ˙ q = G ν Normal Complementarity Constraint: ψ n λ W 0 ≤ λ i n ⊥ ψ i n ( q , t ) ≥ 0 n n ����������������� ����������������� ����������������� ����������������� ����������������� ����������������� ����������������� �����������������

An MPEC Formulation for Parameter Identification of Complementarity Systems Dynamics Model Instantaneous Dynamics Model v i = relative velocity at contact i F i ( λ i n , µ ) = friction cone at contact i Newton-Euler Equations: M ( q ) ˙ ν = W n λ n + W f λ f + λ vp + λ app Kinematic Map: ˙ q = G ν v λ Normal Complementarity Constraint: W t t ������������������ ������������������ ������������������ ������������������ 0 ≤ λ i n ⊥ ψ i n ( q , t ) ≥ 0 ������������������ ������������������ Max. Power Dissipation: λ i f ∈ argmax {− v i f λ ′ i f : λ ′ i f ∈ F i ( λ i n , µ ) }

An MPEC Formulation for Parameter Identification of Complementarity Systems Dynamics Model Discrete Time Dynamics Model ν ≈ ( ν ℓ +1 − ν ℓ ) / h q ≈ ( q ℓ +1 − q ℓ ) / h ˙ ˙ M ν ℓ +1 = M ν ℓ + h ( W n λ ℓ +1 + W f λ ℓ +1 + λ ℓ vp + λ ℓ app ) n f q ℓ +1 = q ℓ + hG ν ℓ +1 0 ≤ h λ ℓ +1 ⊥ ψ n ( q ℓ + 1 ) ≥ 0 n λ ℓ +1 ∈ argmax {− v ℓ +1 i f : λ ′ ℓ +1 ∈ F i ( λ ℓ +1 λ ′ i n , µ ) } i f i f i f Where h is the length of the time step and superscripts ℓ and ℓ + 1 denote values at the beginning and end of the ℓ th time step.

An MPEC Formulation for Parameter Identification of Complementarity Systems Estimation Problem State Estimation The dynamic system is modeled with two equations: State Transition Equation x ℓ +1 = F ( x ℓ , u ℓ , ζ ℓ ) x = [ x p y p ˙ x p ˙ y p λ n λ f σ ] 0 x 1 0 x = MCP(x ) F ( · ) : Dynamic model x 1 x ℓ : Unobserved state at time ℓ u ℓ : known input at time ℓ ζ ℓ : process noise at time ℓ

An MPEC Formulation for Parameter Identification of Complementarity Systems Estimation Problem State Estimation Measurement Equation y ℓ = H ( x ℓ , n ℓ ) y = [ x p y p ] ⇔ H ( x ℓ , n ℓ ) = [ x ℓ 1 x ℓ 2 ] + n ℓ H ( · ) : Measurement Function 0 x 1 0 x = MCP(x ) x ℓ : Unobserved state at time ℓ x 1 y ℓ : Observed state at time ℓ n ℓ : Observation noise at time ℓ

An MPEC Formulation for Parameter Identification of Complementarity Systems Estimation Problem Parameter Estimation 4 3 x = MCP(x ) Determine the nonlinear mapping: 3 4 x x y ℓ = G ( x ℓ , p ) Coulomb’s law: λ f ≤ µλ n −1 2tan µ G ( · ) : Nonlinear Map W n λ n p : Parameters of the mapping

An MPEC Formulation for Parameter Identification of Complementarity Systems Estimation Problem Dual Estimation Applied Force Special case where both the state Initial Pos and parameters must be learned simultaneously. x ℓ +1 = F ( x ℓ , u ℓ , ζ ℓ , p ) x ℓ = [ x ℓ p y ℓ x ℓ y ℓ p λ ℓ n λ ℓ f σ ℓ ] p ˙ p ˙ y ℓ = H ( x ℓ , u ℓ , n ℓ , p ) y ℓ = [ x ℓ p y ℓ p ] Both x ℓ , ℓ = 1 , 2 , . . . N and p p = [ µ ] must be simultaneously F ( · ) = Discrete time MCP estimated. H ( · ) = [ x ℓ p y ℓ p ] + n ℓ

An MPEC Formulation for Parameter Identification of Complementarity Systems Estimation Problem Difficulties of Current Approaches Difficulties of Particle Filtering Difficulties of Kalman Filtering Difficult to apply physical Not possible to apply physical constraints to parameters or constraints to parameters or state state (e.g. µ > 0) With small process noise, all Noise is assumed to be Gaussian particles can collapse into a single point within a few Fails with multimodal pdfs iterations

An MPEC Formulation for Parameter Identification of Complementarity Systems Identification as an Optimization Problem Problem Formulation Optimization Problem for Dual Estimation of Rigid Body Dynamics T n ℓ T n ℓ � n 0 ,..., n N , x 0 ,..., x N , p ( x 0 − ¯ x 0 ) T ( x 0 − ¯ x 0 ) + min (1) ℓ =0 p ∈ P , n ∈ N subject to: (2) x ℓ +1 ∈ SOL (MCP( x ℓ , p )) (3) y ℓ = [ I 0 ] x ℓ + n ℓ (4) x 0 is the initial state estimate, n is a slack variable where ¯ representing the error between observation and prediction, I is an identity matrix of appropriate size, MCP is the mixed complementarity problem arising from the discrete time dynamics model, and P and N are the sets of possible parameter values and max observations error respectively.

An MPEC Formulation for Parameter Identification of Complementarity Systems MPEC MPEC Definition Definition u ∈ R n 1 , v ∈ R n 2 f ( u , v ) min (5) subject to: ( u , v ) ∈ Z , and (6) v solves MCP( g ( u , · ) , B ) , (7) where f is a desired objective function, Z ⊆ R n 1 + n 2 is a nonempty closed set (equation (6) represents standard nonlinear programming constraints), and equation (7) signifies v is a solution to the MCP defined by the function g and the bound set B . For the special case when f and the MCP are linear, the problem is known as a linear program with equilibrium constraints (LPEC).