Vehicle Motion Estimation Using an Infrared Camera an Industrial Paper

Transcription

1 Vehicle Motion Estimation Using an Infrared Camera an Industrial Paper Emil Nilsson, Christian Lundquist +, Thomas B. Schön +, David Forslund and Jacob Roll * Autoliv Electronics AB, Linköping, Sweden. + Division of Automatic Control, Linköping University, Linköping, Sweden. Supported by the Australian Research Council, the Swedish foundation for strategic research in the center MOVIII and the Swedish Research Council in the Linnaeus center CADICS.

2 Problem formulation 2(14) Goal: Compute an off-line estimate of the vehicle motion using the information from the following sensors, Infrared camera Inertial sensors (acceleration and yaw rate) Wheel speed sensors

3 Key sensor - the far infrared (FIR) camera 3(14) (a) Standard camera (b) FIR camera Currently in production in BMW 5- and 7-series and Audi A8.

4 Model coordinate frames 4(14) x c z y c b x b y z p w x y w z World (w): Considered to be an inertial frame. Vehicle body (b): Fixed to the vehicle (coincides with the world frame at time t = 0). Camera (c): Origin in the optical center of the camera and fixed w.r.t. the body frame.

5 Model state-space model 5(14) The state vector x t consists of two parts, 1. The vehicle states x v t, 2. The landmark states (or rather parameters) x l t. The vehicle states x v t consists of p - Vehicle position (expressed in w) v x - Vehicle (longitudinal) velocity ψ - Yaw angle (expressed in w) δ - Front wheel angle α - Road pitch angle ϕ - Vehicle pitch angle x v t+1 = f (x v 1, u t) + w t.

6 Sensor model FIR camera 6(14)

7 Model landmark parameterisation 7(14) Inverse depth parameterisation l α d = 1 ρ m y x c w t c w 0 c w 0 z c w t x y W z

8 Model landmark parameterisation 8(14) The landmark states (parameters) x l t consists of x l 1,t x l x l 2,t ( ) T t =., x l j,t = (k w j,t ) T θj,t w φj,t w ρ j,t, x l M t,t

9 Model landmark parameterisation 9(14) The landmark parameters c w 0 - Camera position (in the w frame) when the landmark was first seen. θ - Azimuth angle of the landmark relative to c w 0. ψ - Elevation angle of the landmark relative to c w 0. ρ - Inverse depth (i.e. inverse distance) from c w 0 to the landmark. Basic trigonometry provides the relation between the landmark and the camera, resulting in a sensor measurement. y t = h(x t ) + e t

10 Model landmark parameterisation 9(14) The landmark parameters c w 0 - Camera position (in the w frame) when the landmark was first seen. θ - Azimuth angle of the landmark relative to c w 0. ψ - Elevation angle of the landmark relative to c w 0. ρ - Inverse depth (i.e. inverse distance) from c w 0 to the landmark. Basic trigonometry provides the relation between the landmark and the camera, resulting in a sensor measurement. y t = h(x t ) + e t

11 Estimation as an optimisation problem (I/II) 10(14) To summarise, we now have a model on this form, x t+1 = f (x t, u t ) + w t, w t N (0, Q), (1a) y t = h(x t ) + e t, e t N (0, R). (1b) Let us compute the MAP estimate ˆx 1:t = arg max x 1:t p(x 1:t y 1:t ), Assume that we have a linear model, then ˆx t computed according to (1) is the same as the Kalman filter, the Kalman filter is just the sequential solution to a weighted least-squares problem. For example, straightforward to include constraints.

12 Estimation as an optimisation problem (I/II) 10(14) To summarise, we now have a model on this form, x t+1 = f (x t, u t ) + w t, w t N (0, Q), (1a) y t = h(x t ) + e t, e t N (0, R). (1b) Let us compute the MAP estimate ˆx 1:t = arg max x 1:t p(x 1:t y 1:t ), Assume that we have a linear model, then ˆx t computed according to (1) is the same as the Kalman filter, the Kalman filter is just the sequential solution to a weighted least-squares problem. For example, straightforward to include constraints.

13 Estimation as an optimization problem (II/II) 11(14) Our problem ends up being the following nonlinear least squares problem ˆx 1:t = arg min x 1:t x 1 x P 1 + t i=2 x i f (x i 1 ) 2 Q 1 + t i=1 Initialise using an extended Kalman filter and solve the above problem using GN or LM for example. y i h(x i ) 2 R 1 Given the computational power and the solvers currently available, this way of solving estimation problems in dynamical systems can (and should) be used much more. Thomas Schön, Fredrik Gustafsson, and Anders Hansson. A Note on State Estimation as a Convex Optimization Problem. Proceedings of the IEEE International Conference on Acoustics, Speech, and Signal Processing (ICASSP), Hong Kong, Apr

14 Estimation as an optimization problem (II/II) 11(14) Our problem ends up being the following nonlinear least squares problem ˆx 1:t = arg min x 1:t x 1 x P 1 + t i=2 x i f (x i 1 ) 2 Q 1 + t i=1 Initialise using an extended Kalman filter and solve the above problem using GN or LM for example. y i h(x i ) 2 R 1 Given the computational power and the solvers currently available, this way of solving estimation problems in dynamical systems can (and should) be used much more. Thomas Schön, Fredrik Gustafsson, and Anders Hansson. A Note on State Estimation as a Convex Optimization Problem. Proceedings of the IEEE International Conference on Acoustics, Speech, and Signal Processing (ICASSP), Hong Kong, Apr

15 Experimental results (I/II) 12(14) 12 measurement sequences recorded during nighttime driving on rural roads in Sweden were used. No ground truth available for evaluation purposes. However, will show the estimates reprojected onto the road. For more detailed evaluations, see the paper.

16 Experimental results (II/II) 13(14) Filtered estimate Smoothed estimate Show movie

17 Conclusions 14(14) Showed how an infrared camera can be used to compute estimates of the vehicle motion. Two fairly obvious, but to some extent overlooked observations: Using vision as a sensor is powerful. Solve the estimation problem as an optimization problem!

18 Conclusions 14(14) Showed how an infrared camera can be used to compute estimates of the vehicle motion. Two fairly obvious, but to some extent overlooked observations: Using vision as a sensor is powerful. Solve the estimation problem as an optimization problem!