State Estimation for Continuous-Time Systems with Perspective Outputs from Discrete Noisy Time-Delayed Measurements

Transcription

1 State Estimation for Continuous-Time Systems with Perspective Outputs from Discrete Noisy Time-Delayed Measurements António Pedro Aguiar João Pedro Hespanha Dept. Electrical and Computer Engineering University of California Santa Barbara, CA 936, USA CDC 4 43rd IEEE Conference on Decision and Control December 4-7, 24 Paradise Island, Bahamas Activmedia Pioneer 2 Motivation indoors robot navigation CCD camera (sensor) provides image coordinates of visual landmarks one image every 33ms (time-stamped) variable delay 2-ms (image processing) some images without landmarks Wheel motors (actuator) independent wheel velocities delay of 5ms Wheel encoders (sensor) provide position/orientation with respect to initial configuration significant drift (low frequency error) one measurement every ms with 68ms (known) delay (from embedded µcontroller)

2 Motivation landing on vision acquire site CCD IR camera (sensor) provides image coordinates of landing strip one image every 33ms (time-stamped) variable delay landing strip not on all images Frog UAV (I. Kaminer, NPS) Inertial sensor & GPS GPS provides position with respect to earth coordinate system (low sampling rate) INS provides data at high sampling rate-frequency data with significant drift (low frequency error) Outline Systems with perspective outputs State estimation The observer equations Estimator convergence Pose estimation of autonomous vehicles using CCD cameras Simulation and Experimental results Experimental setup Motion estimation Output feedback control (parking experiment)

3 Process model State-affine system with multiple perspective outputs disturbance input (cannot be measured) jth perspective measurement output noise Normalization constraint (specifies α j ) constant vector inspired by the (single output) perspective systems introduced by Ghosh et al JMSEC 94 This system can describe the kinematic model of a rigid-body whose outputs are the (homogeneous) image coordinates of N fixed points provided by an on-board camera. Process model State-affine system with multiple perspective outputs disturbance input (cannot be measured) jth perspective measurement output noise Normalization constraint (specifies α j ) Discrete & time-delayed measurements discrete measurement delay generally a strict subset because not all measurements are available simultaneously

4 Process Model State-affine system with multiple discrete & delayed perspective outputs jth discrete & delayed perspective output state-transition matrix of Problem statement State-affine system with multiple discrete & delayed perspective outputs jth discrete & delayed perspective output Goal: Design an optimal observer to estimate the continuous time state vector x(t), given the discrete time-delay measurements y j (t i ).

5 Minimum-energy state-estimation State-affine system with multiple discrete & delayed perspective outputs jth discrete & delayed perspective output Minimum-energy state estimator [Mortensen 68] state value for which the measured outputs can be made compatible with the system dynamics for the smallest noise and disturbance in purely continuous or discrete-time gives rise to Kalman-like filter The observer equations The exact estimate of the state is obtained as the solution to the impulse system Proof: Show that the cost to be minimized can be written as follows piecewise constant

6 Estimator convergence Problem: Under what conditions the state estimate converges to the true state x Assuming that P remains uniformly bounded, there exist positive constants c, r <, γ d, γ,... γ N such that state estimation error ISS-like bound P remains uniformly bounded provided that there exist positive constants N, ε such that state transition matrix of trajectory-dependent grammian Vehicle motion estimation using vision Goal: estimate the position & orientation of autonomous vehicle using onboard CCD camera that observes the apparent image motion of stationary points. kinematics homogeneous image coordinates of the point Q j (q b body, q i inertial) configuration of the body frame with respect to the camera frame camera intrinsic parameters The position can be estimated using

7 Experimental results Pioneer 2-DXE mobile robot from ActivMedia two front wheels powered by Visual landmark independent reversible-dc motors one passive rear caster Sony EVI D3 pan-tilt-zoom (PTZ) color video camera Q Q 4 Q 2 Q 3 Following a circular path experiment Simulation results x error [m] y error [m] x estimation error y estimation error σ min (P) σ min [P] z error [m] z estimation error σ max (P) 3 2 σ max [P] measurements available θ error [degree] 2 θ estimation error σ no measurements The vision sampling interval is T =.4s and the time-delay is τ =.2s. The estimation errors only reduce when the visual landmarks are in the camera's field of view.

8 Experimental results Following a circular path experiment pan angle σ (P) σ (P) max min γ x hat [m] θ hat z hat [m] y hat [m] x estimate σ min [P] σ max [P] y estimate z estimate θ estimate Point 4 Point 3 Point 2 Point 4 measurements Output feedback control (experimental results). Parking experiment x [m] y [m] z [m] x estimate y estimate z estimate y [m] θ [rad] θ estimate u [m/s] left wheel velocity x [m] Minimum-energy state estimator & pan controller & point stabilization controller (Aicardi et al, IEEE Rob. & Autom. Mag., 95). u 2 [rad/s] pan [degree] right wheel velocity camera pan angle

9 Conclusions We addressed the position/orientation estimation for autonomous vehicles using onboard cameras that observe the apparent motion of stationary points (landmarks). We formulated the problem in the framework of state estimation of a system with perspective outputs with measurements are noisy, arrive at discrete-time instants, suffer delays, and may not be complete. We designed a dynamical impulsive system that produces an estimate of the state that is most compatible with the dynamics, in the sense that it requires the least amount of noise energy to explain the measured output. The convergence of the proposed estimator system was analyzed and illustrated through computer simulation and experimentally.