Low-Cost MEMS Sensors and Vision System for Motion and Position Estimation of a Scooter

The possibility to identify with significant accuracy the position of a vehicle in a mapping reference frame for driving directions and best-route analysis is a topic which is attracting a lot of interest from the research and development sector. To reach the objective of accurate vehicle positioning and integrate response events, it is necessary to estimate position, orientation and velocity of the system with high measurement rates. In this work we test a system which uses low-cost sensors, based on Micro Electro-Mechanical Systems (MEMS) technology, coupled with information derived from a video camera placed on a two-wheel motor vehicle (scooter). In comparison to a four-wheel vehicle; the dynamics of a two-wheel vehicle feature a higher level of complexity given that more degrees of freedom must be taken into account. For example a motorcycle can twist sideways; thus generating a roll angle. A slight pitch angle has to be considered as well; since wheel suspensions have a higher degree of motion compared to four-wheel motor vehicles. In this paper we present a method for the accurate reconstruction of the trajectory of a “Vespa” scooter; which can be used as alternative to the “classical” approach based on GPS/INS sensor integration. Position and orientation of the scooter are obtained by integrating MEMS-based orientation sensor data with digital images through a cascade of a Kalman filter and a Bayesian particle filter.


Introduction
The development of electronic systems for determining the position and orientation of moving objects in real-time has been a critical research topic for the last decade. Applications vary in many fields and range from rigid frames-aerial and land-based vehicles-as well as dynamic and complex frames like a human body [1,2]. The objective of measuring such parameters can also vary a lot; in remote sensing it is a crucial aspect for correct georeferencing of data acquired from optical sensors. Navigation and road safety purposes have also become common applications. Two main results of the technological progress in this field are represented by the Electronic Stability Program (ESP), an evolution of the Anti-Blocking System (ABS), and satellite positioning of vehicles. In the automotive sector, due to limited budgets and sizes, navigation sensors rely on the integration between a low cost GPS receiver and an Inertial Measurement Unit (IMU) based on Micro Electro-Mechanical System (MEMS) technology. Such integration is commonly realized through an extended Kalman filter [3][4][5][6][7], which provides optimal results for offsets, drifts and scale factors of employed sensors. However the application of this filter to motorcycle dynamics does not perform similarly. Unlike cars, motorcycles are able to rotate around their own longitudinal axis (roll), bending to the left and the right, therefore the yaw angular velocity is not measured by just one sensor, rather it is the result of the measurements of all three angular sensors, which contribute differently in time according to the current tilting of the motorcycle. Consequently, an error on the estimate of the roll angle at time t will affect the estimate of the pitch and yaw angles at next time t+1 as well. In this paper we consider the problem of detecting the position and orientation of a "Vespa", a popular Italian scooter brand, using a low cost Positioning and Orientation System (POS) and images acquired by an on-board digital video camera. The estimate of the parameters (position in space and orientation angles) of the dynamic model of the scooter is achieved by integrating in a Bayesian particle filter the measurements acquired with a MEMS-based navigation sensor and a double frequency GPS receiver. In order to further improve the accuracy of orientation data, roll and pitch angles provided by the MEMS sensor are pre-filtered in a Kalman filter with those computed with the application of the cumulated Hough transform to the digital images captured by a video-camera.
In the next sections, after an overview of the system components, the method adopted for trajectory reconstruction is described in detail. Specifically, in Section 3 we present the "Whipple model" [8,9], which constitutes the mathematical basis of the dynamic model of the motorcycle, and in Section 4 we focus on the estimate of the roll angle from the images recorded by the video-camera using the cumulated Hough transform [10][11][12]. Then in Section 5 we discuss the use of the Bayesian particle filter to integrate MEMS sensor data with GPS measurements. Results achieved with the proposed method are reported in Section 6, while final conclusions are discussed in Section 7.

System Components
The method for the motion estimation of a motorcycle proposed in this work has been tested on a "Vespa", a common Italian scooter, which was equipped with a set of navigation sensors as shown in  Table 1. Data acquisition and sensor synchronization were handled by a Notebook PC (Acer Travelmate) provided with 1,024 MB of RAM and a CPU processing speed of 1.66 GHz. A Novatel DL-4 double frequency GPS receiver was also fixed on the scooter and used to collect data for reference trajectory determination.

The Whipple Model
The "Whipple model" [8,9] consists in an inverse pendulum fixed in a frame moving along a line with the wheels which are considered to be discs with no width (Figure 2). The vehicle's entire mass m is assumed to be concentrated at its mass center, which is located at height h above the ground and distance b from the rear wheel, along the x axis. The parameter ψ represents the yaw angle, ϕ the roll angle, δ the steering angle and w is the distance between the two wheels. In this model the motorcycle motion is assumed to be constrained so that no lateral motion of the tires is allowed (non-holonomic constraint). The mathematical model does not take into account the possible oscillation of the scooter's wheel suspensions neither the movement of a driver. The motion equations are therefore described by: (1) where x, y and z represent the real-time vehicle positions in the spatial frame, v is the forward velocity, and θ is the pitch angle (not shown in Figure 2).
From the geometry of the system the rate of change (i.e., first derivative) of the yaw angle is defined as follows: where σ is the instantaneous curvature of the path followed by the motorcycle in the xy plane and R is the instantaneous curvature ray (σ = R −1 ).

Figure 2.
The inverted pendulum motorcycle model.
According to the inverted pendulum dynamics; the roll angle satisfies the following equation: where g is the acceleration due to gravity. The term hσ sinϕ can be rewritten as a function of the steering angle δ and the roll angle ϕ: (4) and given that angles δ and ϕ do not simultaneously assume high values, the term hδ sinϕ can be neglected. Therefore, taking into account also Equation (2), Equation (3) becomes: (5) Assuming that we can measure the roll angle ϕ(t), the pitch angle θ(t) and the velocity v(t), Equation (5) could be used to estimate the curvature σ. Indeed, by integrating Equation (5) we can cos cos sin cos sin compute the instantaneous curvature σ(t), provided that an initial condition σ(0) is given. Similarly, knowing the profile σ, if we integrate the non-holonomic kinematic model (1) from an initial position [x(0), y(0), z(0)] the path followed by the motorcycle can be fully reconstructed. In next sections we will discuss how we estimate the parameters φ, θ and v, whose knowledge is crucial for the application of the proposed method.

Roll and Pitch Angle Estimation
Roll and pitch angles can be estimated by using the frames recorded from the videocamera, which is rigidly fixed to the motorcycle, and detecting the position in the image of the horizon line estimating slope and distance of this line from the image origin. Using the perspective projection camera model, the horizon line projected onto the image plane can be described in terms of roll and pitch angles as follows (see [10,11] for details): (6) where (U,V) denote the image plane coordinates of a point P with coordinates [x,y,z] in the camera frame Σ c . Therefore, the pitch and roll angles θ and ϕ can be determined knowing the position of the horizon line in the image. Despite the horizon cannot be easily determined due to occlusions frequently occurring in the scene, roll and pitch rates can be robustly estimated by comparing two consecutive images. Indeed, given the horizon line in the frame at time t i , I(t i ), in the next frame at time t i +1, I(t i +1), the horizon is described by the following relationship: Linearizing Equation (7) about θ(t) and ϕ(t), neglecting terms of order higher than one in Δ and assuming small pitch angles (θ ≅ 0), we obtain: (8) Equation (8) shows that in two successive frames, the horizon rotates by Δϕ and translates by −Δθ cosϕ. These two quantities (Δϕ, Δθ) can be measured by computing the Hough transform on a region of interest centered around a neighborhood of the current estimation of the horizon line.
The Hough transform [12] is a feature extraction technique used in image analysis, computer vision, and digital image processing, whose purpose is to find imperfect instances of objects within a certain class of shapes by a voting procedure. This voting procedure is carried out in a parameter space, from which object candidates are obtained as local maxima in a so-called accumulator space that is explicitly constructed by the algorithm for computing the Hough transform. In this case this transform is used to determine the horizon line in the images acquired by the scooter's on-board videocamera. To this aim polar coordinates (ρ,α) are used as space parameters and are related to the image coordinates (U,V) as follows: An example of such image space parametrization is shown in Figure 3.      a he (13) Similarly, the estimate of the increment of the roll angle θ is computed as follows: After these steps, the estimates of the roll and pitch angles are computed by time integration of the rates Δϕ and Δθ.

The Bayesian Particle Filter
The key point of all navigation and tracking applications is the motion model to which bayesian recursive filters (as the particle filter [13]) can be applied. Models which are linear in the state dynamics and non-linear in the measurements can be described as follows: (15) where x t is the state vector at time t, u t the input, f t the error model, y t the measurements and e t the measurement error. In this model, indipendent distributions are assumed for f t , e t and the initial state x 0 , with known probability densities p et , p ft and p x0 , respectively, but not necessarily Gaussian.
We denote the set of available observations at time t as: The Bayesian solution to equations (15) deals with the computing of the a prior distribution p(x t+1 |Y t ), given past distribution p(x t |Y t ). In case the noise can be modeled by indipendent, white and gaussian with zero mean probability density functions, and h(x t ) is a linear function, then the optimal solution is provided by the Kalman filter. Should be this condition not satisfied, an approximation of the a prior distribution p(x t+1 |Y t ) can be still provided using a Bayesian particle filter [13]. This filter is an iterative process by which a collection of particles, each one representing a possible target state, approximates the a prior probability distribution, which describes the possible states of the target. Each particle is assigned a weight w t i , whose value will increase as closer to true value the related sample will be. When a new observation arrives, the particles are time updated to reflect the time of the observation. Then, a likelihood function is used to updated the weights of the particles based on the new information contained in the observation. Finally, resampling is performed to replace low weight particles with randomly perturbed copies of high weight particles. A block diagram of the particle filter is presented in Figure 8. Since the computational cost of a particle filter is quite high, only an adequate minimum number of variables has been included in the dynamic model of the scooter. It was therefore chosen to neglect any movement along the z axis (e.g., "bouncing" of suspensions), and to account for position variables x and y, speed v, the three angles needed for modelling the orientation (φ, θ, ψ) and the filtered version of the curvature σ. In order to further improve the accuracy of orientation data, roll and pitch angles provided by the MEMS sensor have been combined and pre-filtered in a Kalman filter with those computed using the cumulated Hough transform applied to the digital images captured by a video-camera. Assuming that the system is now represented as a collection of N particles, the dynamics of the generic particle s i (i.e., a possible system state) is described by the following model: where: − ΔT is the sampling interval; − N(0,∆x i2 t ) represents the measurement noise of the X coordinate, modeled as a Gaussian function with a zero mean and standard deviation ∆x i t . Similar assumption holds for measurement noises N(0, ∆y i2 t ), N(0, ∆v i2 t ) and N(0,∆ψ i2 t ); − is the weighted combination of the curvature estimated at previous time t ( ) and the current input , being γ s the weighting term (γ s = 1/10); is weight of the i-th particle; − is the importance function, i.e., the likelihood function through which the weights are updated according to the following relationship: is a coefficient which dynamically changes in order to give more weight to minimal curvatures and roll angular velocities as denoted by: where σ l and are the thresholds for the maximum curvature and roll angular velocity respectively. We set γ m = 1/500, σ l = 1/100 m −1 and = 30 °/s.
In the model Equation (17) we used different formulas for the derivatives of the orientation angles , and . This is due to the fact that the angular velocities (ω x , ω y , ω z ) measured by the MEMS sensor are related to the body frame (i.e., the coordinate system fixed with the scooter) while orientation angles (φ, θ, ψ) are determined in a world reference frame (e.g., the GPS coordinate system, WGS-84). A frame transformation from the body to the world frame is therefore needed, which leads to different equations for the orientation angles.
The components of the state vector at time t are then computed as weighted average of the variables estimated by the filter, using the weights w i of all particles s i : (20) In order to limit the computational effort of the filter, the update of the particle weights w i is not performed at every step of the algorithm, but rather when the GPS data are available from the receiver.

Results and Discussion
Three drive tests were carried out on the same track in order to evaluate the measurement repeatability, whose results for the roll angle are shown in Figure 9. A slight difference can be observed for test No. 3 where the speed was slower than for the other two tests.
An example of the track reconstructed from the data received during one of the three tests is shown in Figure 10. Here the trajectory (dotted line) estimated with the Bayesian particle filter is compared with the reference trajectory derived from differentially corrected GPS measurements (solid line). Beyond vibrations, offsets and scale factors, further interesting sources of error to be tested are wrong initial conditions and noises of the roll and pitch angles. During the test the roll angle was brought to more than 20° to evaluate the performance of the filter; no GPS update was used by the Bayesian particle filter to estimate the trajectory covered by the scooter. The algorithm was able to converge, albeit slowly, towards the real angle. Some statistics highlighting the residual distances between the reference trajectory shown in Figure 10 and that estimated with the particle filter are summarized in Table 2.
ϕ θ ψ σ ϕ θ ψ σ = = ∑ Figure 9. Roll angle profiles resulting from the performed tests.  Table 2. Statistics of the residual displacements btw. the GPS reference trajectory and that estimated by the particle filter shown in Figure 10. Developments of the proposed method will deal with the encoding of the Bayesian filter inside an integrated system which can be used to equip the scooter. This can lead in the future to provide even motorcycles with traction control systems. Further developments will be the inclusion in the dynamic model of the suspensions' motion along the Z axis, and also the study of the influence of the steering angle (δ) on the estimation of the roll angle. These two parameters are indeed related by the following relationship, which can be easily derived from Equation (2)

Conclusions
In this paper we have presented an alternative method for the reconstruction of the trajectory of a motorcycle ("Vespa" scooter) with respect to the "classical" approach based on GPS/INS sensor integration. In our implementation position and orientation of the scooter are obtained by integrating MEMS-based orientation sensor data with digital images through a cascade of a Kalman filter and a Bayesian particle filter. As shown, the proposed method provides quite acceptable results though its application is affected by environment conditions. Indeed the roll angle estimation based on the Hough transform requires a minimal amount of linear elements in the scene, whose absence can degrade the results achievable for the roll angle. For example a complex skyline and low contrast between the road segment and neighboring object can be problematic, even if not common.