Kinematic/Dynamic SLAM for Autonomous Vehicles Using the Linear Parameter Varying Approach

Abstract

Most existing algorithms in mobile robotics consider a kinematic robot model for the the Simultaneous Localization and Mapping (SLAM) problem. However, in the case of autonomous vehicles, because of the increase in the mass and velocities, a kinematic model is not enough to characterize some physical effects as, e.g., the slip angle. For this reason, when applying SLAM to autonomous vehicles, the model used should be augmented considering both kinematic and dynamic behaviours. The inclusion of dynamic behaviour implies that nonlinearities of the vehicle model are most important. For this reason, classical observation techniques based on the the linearization of the system model around the operation point, such as the well known Extended Kalman Filter (EKF), should be improved. Consequently, new techniques of advanced control must be introduced to more efficiently treat the nonlinearities of the involved models. The Linear Parameter Varying (LPV) technique allows working with nonlinear models, making a pseudolinear representation, and establishing systematic methodologies to design state estimation schemes applying several specifications. In recent years, it has been proved in many applications that this advanced technique is very useful in real applications, and it has been already implemented in a wide variety of application fields. In this article, we present a SLAM-based localization system for an autonomous vehicle considering the dynamic behaviour using LPV techniques. Comparison results are provided to show how our proposal outperforms classical observation techniques based on model linearization.

1. Introduction

The European Parliament Research Service (EPRS) considers autonomous driving as one of the top ten technologies that will change people’s lives during this century [1]. The reason is that benefits provided by the autonomous driving are many and affect many social spheres. The most relevant ones are the significant reduction in the number of traffic accidents, by the elimination of the human error; the social inclusion of people with reduced mobility, by the generation of economic door-to-door services; the reduction in the traffic congestion, through the communication between vehicles and the use of an intelligent agent of the transport; and the reduction in the energetic consumption and the pollution, by the use of electric actuators and sophisticated control techniques.

To develop an autonomous vehicle, it is necessary that the vehicle software architecture includes a localization system that provides the vehicle location to the controller and the surrounding map to the trajectory planner. Then, the planner generates references and provides them to the controller to execute the vehicle control, and, finally, the localization system has to make new estimations for the carried actions. Therefore, this three modules are intimately joined, and none of them can be designed without taking the characteristics of the others into account. The authors have already largely explored the motion control [2] and the trajectory planner [3] for vehicles with evident dynamic response. Now, we are interested in extending this knowledge to build a localization node based on the solution of a Simultaneous Localization and Mapping (SLAM) problem. This problem, in the context of autonomous vehicles, has recently been reviewed in [4,5] with special emphasis on visual SLAM methods.

An important aspect to bear in mind when designing a localization system for an autonomous vehicle is its dynamic behaviour. Traditionally, SLAM algorithms have been developed for mobile platforms that move at low velocities and consequently can be modelled by only applying kinematic models [6,7]. However, for velocities larger than 5 m/s (18 km/h), kinematic models cannot describe the behaviour of a vehicle correctly enough [8]. An autonomous vehicle can very easily exceed the 5 m/s threshold during its usual operation. Therefore, the modelling of its dynamic response becomes a fundamental issue for autonomous driving. Consequently, the dynamic behaviour of the vehicle must be considered for both the control and the localization system of the autonomous vehicle.

Methods considering the dynamic model for autonomous driving of vehicles have just recently started to appear. In [9], a dynamic predictive control for the vehicle is proposed, while the localization system based on the SLAM problem is still based on a kinematic model. Although the dynamic states of the vehicle are observed, dynamics are only modelled by the integration of the accelerations of the vehicle without using a dynamic model of the vehicle, which takes into account the forces acting on it. To date, literature in this field has not yet explored SLAM for autonomous vehicles considering dynamic models. During the last decade, the development of SLAM-based techniques for autonomous driving has been focused on perception techniques and on the descriptors, especially on map management and on collaborative solutions [4]. However, all the proposed solutions still use a kinematic model for the vehicle. Therefore, it is interesting to investigate SLAM approaches for autonomous vehicles considering dynamic models.

Another relevant aspect to take into account is that vehicles are systems with an important nonlinear behaviour, especially when its dynamic response is considered [8]. Therefore, both for the observation and for the control of the vehicle, it is necessary to use nonlinear techniques. In the SLAM literature, EKF is the predominant approach (see previous referenced reviews [4,5]). UKF and CKF have also been considered as a manner to deal with the nonlinearity problem related to the SLAM for autonomous vehicles (see, e.g., [10,11]), but the the increased computational complexity is not justified by the improvement in the accuracy achieved. In terms of control, there are a few nonlinear methodologies which allow systematic tuning. The Linear Parameter Varying (LPV) technique allows working with nonlinear models and establishing systematic methodologies to tune the controller and the observer by applying various specifications using Linear Matrix Inequalities (LMI). In recent years, it has been proved that this technique of advanced control is very useful in real applications and has been implemented in a huge variety of fields [12]. There are tuning methodologies based on the LPV technique for Model Predictive Control (MPC), an optimal control methodology which is very interesting when combining the vehicle controller with the trajectory planner module. In addition, in terms of observation, LPV techniques avoid the linearization of the nonlinear problem around the operation point and allow guaranteeing the global stability of the system by keeping the Lyapunov conditions for nonlinear systems. This is the opposite of classical observation techniques, such as the Extended Kalman Filter (EKF), which only guarantee stability around the point where the linearization is calculated. Considering the coupling between the software modules, which allow the autonomous driving and the benefits provided by the LPV techniques in systems with a huge nonlinear response, it is logical to think that if the LPV technique has been applied to the vehicle controller [2], it can also be applied to the localization system. However, the LPV technique has not yet been applied to the SLAM problem. The most similar approach found in the literature is an EKF-SLAM problem solved by means fuzzy models using the Takagi–Sugeno technique and applying a purely kinematic conception [13]. It is already known that the Takagi–Sugeno technique has strong connections with LPV techniques [14]. Hence, this approach is a good reference to rewrite the SLAM problem to solve it applying LPV techniques and to incorporate the dynamic response of the vehicle, which is needed for the autonomous driving of vehicles.

In this article, a SLAM-based localization system for an autonomous vehicle in the LPV framework is proposed. The proposed approach takes into account the dynamic behaviour of the vehicle and it is adapted to the kinematic/dynamic controller architecture for an autonomous vehicle previously presented in [2]. This controller is also implemented using LPV techniques and executed into two separate layers that work with different time periods. The fastest layer is in charge of the dynamic response of the vehicle, while the slowest layer is in charge of its kinematic response. Therefore, the proposed vehicle state estimation scheme should also be organized into two separate layers. The dynamic observation must serve the dynamic controller, being part of the fastest layer. In the slowest layer, the solution to a SLAM problem should allow executing the localization task so as to serve the kinematic controller. At the same time, it should allow the map generation of the surroundings of the vehicle so as to serve as the trajectory planner module.

The main contributions with respect to the SLAM state of the art are the following:

• A SLAM LPV KF scheme is proposed that does not require the linearization of the nonlinear vehicle model but instead embeds the nonlinearities in the varying parameters.
• The proposed SLAM approach considers both the kinematic and dynamic model of the vehicle, allowing it to operate at higher speeds compared to the pure kinematic schemes available in the literature.
• A design procedure based on the LMI framework allows the offline design of the LPV KF, reducing the online computational complexity.
• The LMI framework is rooted in the Lyapunov stability theory guaranteeing the quadratic stability of the LPV KF scheme.

This article is structured as follows. Section 2 presents the kinematic and dynamic models for the autonomous vehicle considered as a case study. Section 3 describes the proposed SLAM approach formulated in the LPV framework. Section 4 shows the design procedure for the kinematic and the dynamic state estimation scheme. Section 5 presents the simulation results using the considered case study. Finally, Section 6 shows the conclusions of this work.

2. Problem Statement

To formulate a SLAM problem, firstly, it is necessary to define a motion model for the vehicle and an observation model for the map. The motion model defines how the vehicle moves inside the map, and the observation model allows determining how the landmarks of the map are seen from the vehicle in movement. Therefore, the motion model is a state transition function and the observation model is a kinematic transform inside the same state representation.

In this article, a two dimensional environment is considered, and, therefore, the vehicle can only do a planar movement (x and y translations and yaw rotation). Hence, ground unevenness and vehicle instabilities are not taken into account.

2.1. Vehicle Modelling

As already stated, mobile robotics addresses the SLAM problem using a purely kinematic model formulation. However, in the case of autonomous vehicles, because of the increase in the mass and velocities, a kinematic model is not enough to characterize some physical effects as, e.g., the slip angle. Experimental results show that during the turns of a vehicle, the linear velocity vector $\overrightarrow{v}$ detaches from the longitudinal direction of the vehicle and lateral forces appear at the wheels, which can result in drifting situations. Therefore, the motion model of the vehicle must take its kinematic and dynamic response into account.

The bicycle model proposed at [8] (see Figure 1) is taken to model the dynamic behaviour of the vehicle. Alcalá et al. [2] show that this simplified model is sufficient for dynamically controlling the vehicle and providing a good balance between representativeness and simplicity. The control actions are the traction force of the rear wheel $F_{xR}$ (accelerator and breaker) and the steering angle of the front wheel $\delta$ (steering wheel). In addition, the opposition forces acting on the vehicle are the aerodynamic force ($F_{drag}=\frac{1}{2}{C}_d\rho A{(v+v_{wind})}^2$), the friction force between the vehicle and the ground ($F_{friction}=\mu mg$), and the lateral forces that appear on the wheels during the turns ($F_{yF}$ i $F_{yR}$). On the one hand, a windless environment is considered $(v_{wind}=0)$, and, therefore, the aerodynamic force is known. On the other hand, a variable friction is considered, and, therefore, the friction force acts as an unknown perturbation to the system. The dynamic states are the linear velocity v, the angular velocity $\omega$, and the slip angle $\alpha$; the latter appear between the linear velocity vector and the longitudinal direction of the vehicle. Applying Newton’s second law, considering a polar frame formed by the velocity vectors of the vehicle, leads to the following dynamic vehicle model

$$\left\{\begin{array}{ccc}\hfill \dot{v}& =& {\displaystyle \frac{F_{xR}cos\alpha +F_{yF}sin(\alpha -\delta )+F_{yR}sin\alpha -C_D{v}^2}{m}}-\mu g,\hfill \\ \hfill \dot{\alpha }& =& {\displaystyle \frac{-F_{xR}sin\alpha +F_{yF}cos(\alpha -\delta )+F_{yR}cos\alpha }{mv}}-\omega ,\hfill \\ \hfill \dot{\omega }& =& {\displaystyle \frac{F_{yF}acos\delta -F_{yR}b}{I}}\hfill \end{array}\right.$$

(1)

where

$$\begin{array}{ccc}\hfill F_{yF}& =& C_x(\delta -\alpha -{\displaystyle \frac{a\omega }{v}}),\hfill \\ \hfill F_{yR}& =& C_x(-\alpha +{\displaystyle \frac{b\omega }{v}}),\hfill \\ \hfill C_D& =& \frac{1}{2}{C}_d\rho A\hfill \end{array}$$

To exemplify the results in this paper, an autonomous vehicle based on a Tazzari Zero used in the Elektra research project (http://adas.cvc.uab.es/elektra/, accessed on 24 October 2022) will be used as a case study. For this vehicle, the parameters of the dynamic model (1) are summarized in

Table 1. Dynamic model parameters of Tazzari Zero vehicle, which belongs to the Elektra research project [15].

Dynamic Model Parameters of Tazzari Zero Vehicle
Par.	Description	Value
a	Distance from CoG to front shaft	0.758 m
b	Distance from CoG to rear shaft	1.036 m
m	Vehicle mass	683 kg
I	Vehicle inertia	561 kg·${\mathrm{m}}^2$
$C_x$	Tire stiffness coefficient	15,000 $\frac{\mathrm{N}}{\mathrm{rad}}$
$C_d$	Vehicle drag coefficient	0.5
A	Vehicle frontal area	4 m${}^2$
$\rho$	Air density at 25 °C	1.2 $\frac{\mathrm{kg}}{{\mathrm{m}}^3}$
$\mu$	Friction coefficient tire-ground	variable

.

The kinematic model of the vehicle describes, to a fixed observer placed at the origin of the world frame $\left\{W\right\}$, how the centre of gravity of the vehicle $\left\{B\right\}$ moves, giving the kinematic states x and y, corresponding to the vehicle position, and $\theta$, corresponding to its orientation (see Figure 1). Considering the dynamic effects, the linear velocity v is rotated by the slip angle $\alpha$, leading to the following kinematic model for the vehicle

$$\left\{\begin{array}{ccc}\hfill \dot{x}& =& vs.cos(\theta +\alpha ),\hfill \\ \hfill \dot{y}& =& vs.sin(\theta +\alpha ),\hfill \\ \hfill \dot{\theta }& =& \omega .\hfill \end{array}\right.$$

(2)

Note that the slip angle $\alpha$ is the link between both dynamic and kinematic models.

2.2. Map Modelling

The observation model of the map allows performing the kinematic transformation between the observations of the surroundings of the vehicle measured from the absolute world frame $\left\{W\right\}$ and the same observations measured from the mobile frame of the vehicle $\left\{V\right\}$. In this section, the observation model is derived for an exteroceptive sensor with range and bearing measures, such as a laser sensor. To do so, firstly, some considerations must be made:

• The sensor is not placed at the centre of gravity of the vehicle $\left\{V\right\}$ but at a fixed point of the vehicle frame described by the pose $(s,t)$ and the orientation $\beta$. The new frame that can be defined from this point is called the sensor frame $\left\{S\right\}$.
• To reduce the nonlinearity of the observation model, a transformation block of polar coordinates to Cartesian coordinates is installed at the sensor output:

  $$\left[\begin{array}{c}{x}_l^s(k)\\ y_l^s(k)\end{array}\right]=\overrightarrow{j}(\overrightarrow{y}(k))=\left[\begin{array}{cc}cos(\alpha (k))& 0\\ sin(\alpha (k))& 0\end{array}\right]\left[\begin{array}{c}d(k)\\ \alpha (k)\end{array}\right],$$

  where $(x_l^s,y_l^s)$ is the sensor measurement Cartesian representation and $(d,\alpha )$ is its polar representation given by the original sensor. Using this block, it is considered that the sensor provides a Cartesian measurement to the SLAM system, as is suggested in [13].

According to the diagram presented in Figure 2, the observation model of the map corresponds to the geometric transform $T_w^s(k)$, which goes from the world frame $\left\{W\right\}$ to the sensor frame $\left\{S\right\}$. Nevertheless, there is not enough information available to determine it completely, and it is necessary to pass by the vehicle frame $\left\{V\right\}$. Therefore, if $T_w^v(k)$ is the homogeneous transform which goes from the world frame to the vehicle frame (which is variable depending on the vehicle position) and if $T_v^s$ is the homogeneous transform which goes from the vehicle frame to the sensor frame (which is constant), then:

$$T_w^s(k)=T_w^v(k)T_v^s.$$

Considering that the vehicle is placed at the position $(x_v^w,y_v^w)$ described from the world with an orientation $\theta$ and the sensor is placed at the position $(s,t)$ described from the vehicle with an orientation $\beta$, when the pertinent transforms are applied, the analytic observation model is

$$\left\{\begin{array}{ccc}\hfill x_l^s(k)& =& -x_v^w(k)cos(\theta (k)+\beta )-y_v^w(k)sin(\theta (k)+\beta )\hfill \\ & +& x_l^w(k)cos(\theta (k)+\beta )+y_l^w(k)sin(\theta (k)+\beta )+N_1,\hfill \\ \hfill y_l^s(k)& =& x_v^w(k)sin(\theta (k)+\beta )-y_v^w(k)cos(\theta (k)+\beta )\hfill \\ & -& x_l^w(k)sin(\theta (k)+\beta )+y_l^w(k)cos(\theta (k)+\beta )+N_2,\hfill \end{array}\right.$$

(3)

where

$$\begin{array}{ccc}\hfill N_1& =& -scos\beta -tsin\beta ,\hfill \\ \hfill N_2& =& ssin\beta -tcos\beta ,\hfill \end{array}$$

and the installation parameters of the exteroceptive sensor are described in

Table 2. Installation parameters of the exteroceptive sensor on the Tazzari Zero vehicle.

Installation Parameters of LIDAR Sensor
Par.	Description	Value
s	Distance from CoG to sensor on the vehicle longitudinal direction	0.3 m
t	Distance from CoG to sensor on the vehicle cross direction	0.1 m
$\beta$	Sensor orientation respect to the vehicle longitudinal direction	90°

.

Apart from the observation model of the map, a dynamic model of the map is needed to describe the dynamic evolution of its landmarks. Since fixed landmarks at the world frame $\left\{W\right\}$ are considered, modelling its performance from this frame is very easy:

$$\left\{\begin{array}{ccc}\hfill {\dot{x}}_l^w& =& 0,\hfill \\ \hfill {\dot{y}}_l^w& =& 0.\hfill \end{array}\right.$$

(4)

where $(x_l^w,y_l^w)$ is the position of a landmark represented in the world frame $\left\{W\right\}$.

2.3. Motion Model for the SLAM System

To develop a SLAM scheme using the proposed LPV approach, a linear observation model for the system is necessary. Otherwise, the complexity of the interpolation of the vertices of the polytopic LPV model grows notably. To accomplish this restriction, the map observation model must be introduced with the system motion model; thus, all system sensor measurements also become states and the observation model becomes trivial. Hence, the kinematic transform of Equation (3) must be converted into a state transition function. To do so, the time derivative of the kinematic transform is taken, and it is forced by the dynamic model of the map (Equation (4)). The following kinematic motion model for the map is obtained:

$$\left\{\begin{array}{ccc}\hfill {\dot{x}}_l^s& =& -vs.cos(\alpha -\beta )+\omega (y_l^s-N_2),\hfill \\ \hfill {\dot{y}}_l^s& =& -vs.sin(\alpha -\beta )-\omega (x_l^s-N_1),\hfill \end{array}\right.$$

(5)

where $N_1$ and $N_2$ are defined in Equation (3), the sensor parameters are in Table 2, and $(x_l^s,y_l^s)$ is the position of a landmark represented in the sensor frame $\left\{S\right\}$. This model describes how the static landmarks in the world frame $\left\{W\right\}$ are moving with respect to a static observer placed at the sensor frame $\left\{S\right\}$, which introduces a robocentric point of view to the SLAM problem.

With the benefits of this derived model, now, the problem state vector includes all sensor measurements, and it is possible to write a linear observation model for the SLAM problem. Additionally, a conceptual reorganization of the problem models could be conducted as follows:

$$\begin{array}{ccc}\hfill & \mathrm{Vehicle}\hspace{0.17em}\mathrm{dynamic}\hspace{0.17em}\mathrm{model}\hspace{0.17em}(\mathrm{Equation}\hspace{0.17em}(1))& \\ \hfill & \left(\begin{array}{ccc}vs.& \alpha & \omega \end{array}\right)& \\ \hfill & \downarrow & \\ \hfill & \mathrm{Direct}\hspace{0.17em}\mathrm{observation}\hspace{0.17em}\mathrm{model}\hspace{0.17em}(\mathrm{Equation}\hspace{0.17em}(3))& \\ \hfill \mathrm{Vehicle}\hspace{0.17em}\mathrm{kinematic}\hfill & ⟶& \hfill \mathrm{Map}\hspace{0.17em}\mathrm{kinematic}\hfill \\ \hfill \mathrm{model}\hspace{0.17em}(\mathrm{Equation}\hspace{0.17em}(2))& & \mathrm{model}\hspace{0.17em}(\mathrm{Equation}\hspace{0.17em}(5))\hfill \\ \hfill \left(\begin{array}{ccc}{x}_v^w& y_v^w& \theta \end{array}\right)\hfill & ⟵& \hfill \left(\begin{array}{cc}{x}_l^s& y_l^s\end{array}\right)\hfill \\ \hfill & \mathrm{Inverse}\hspace{0.17em}\mathrm{observation}\hspace{0.17em}\mathrm{model}\hspace{0.17em}(\mathrm{Equation}\hspace{0.17em}(6))& \\ \hfill & \uparrow & \\ \hfill & \mathrm{Map}\hspace{0.17em}\mathrm{dynamic}\hspace{0.17em}\mathrm{model}\hspace{0.17em}(\mathrm{Equation}\hspace{0.17em}(4))& \\ \hfill & \left(\begin{array}{cc}{x}_l^w& y_l^w\end{array}\right)& \end{array}$$

Note that the dynamic model defines the vehicle behaviour and provides the necessary data to the two kinematic models. These two models allow one to perform the two kinematic interpretations needed for the SLAM problem: vehicle displacement respect to the static observer at the world frame and world displacement with respect to the static observer at the vehicle frame. The direct and inverse observation models are the transforms between these two kinematic conceptions. Finally, a dynamic model for the landmarks is needed to define its evolution in the world map. However, as static landmarks at the world frame are considered, the map dynamic model is very simple, and, with the purpose of simplicity, it has been forced into the map kinematic model. Consequently, this model is not explicit in the SLAM system motion model. Hence, the static position of each landmark in the world frame is not explicit in the problem state vector and, if it is needed, it has to be obtained at each iteration applying the inverse map observation model to the state vector:

$$\left\{\begin{array}{ccc}\hfill x_l^w(k)& =& x_v^w(k)+x_l^s(k)cos(\theta (k)+\beta )-y_l^s(k)sin(\theta (k)+\beta )+M_1,\hfill \\ \hfill y_l^w(k)& =& y_v^w(k)+x_l^s(k)sin(\theta (k)+\beta )+y_l^s(k)cos(\theta (k)+\beta )+M_2,\hfill \end{array}\right.$$

(6)

where

$$\begin{array}{ccc}\hfill M_1& =& scos\beta -tsin\beta ,\hfill \\ \hfill M_2& =& ssin\beta +tcos\beta ;\hfill \end{array}$$

which results from inverting Equation (3).

To summarize, the resulting motion model for the dynamic SLAM problem is:

$$\left\{\begin{array}{ccc}\hfill \dot{v}& =& {\displaystyle \frac{F_{xR}cos\alpha +F_{yF}sin(\alpha -\delta )+F_{yR}sin\alpha -C_D{v}^2}{m}}-\mu g,\hfill \\ \hfill \dot{\alpha }& =& {\displaystyle \frac{-F_{xR}sin\alpha +F_{yF}cos(\alpha -\delta )+F_{yR}cos\alpha }{mv}}-\omega ,\hfill \\ \hfill \dot{\omega }& =& {\displaystyle \frac{F_{yF}acos\delta -F_{yR}b}{I}},\hfill \\ \hfill \dot{x_v^w}& =& vs.cos(\theta +\alpha ),\hfill \\ \hfill \dot{y_v^w}& =& vs.sin(\theta +\alpha ),\hfill \\ \hfill \dot{\theta }& =& \omega ,\hfill \\ \hfill {\dot{x}}_l^s& =& \omega y_l^s-cos(\alpha -\beta )vs.+(tcos\beta -ssin\beta )\omega ,\hfill \\ \hfill {\dot{y}}_l^s& =& -\omega x_l^s-sin(\alpha -\beta )vs.-(tsin\beta +scos\beta )\omega ,\hfill \end{array}\right.$$

(7)

where:

$$\begin{array}{ccc}\hfill F_{yF}& =& C_x(\delta -\alpha -{\displaystyle \frac{a\omega }{v}}),\hfill \\ \hfill F_{yR}& =& C_x(-\alpha +{\displaystyle \frac{b\omega }{v}}),\hfill \\ \hfill C_D& =& \frac{1}{2}{C}_d\rho A\hfill \end{array}$$

and its parameters are presented in Table 1 and Table 2.

3. Proposed Solution

Before we introduce in detail the proposed kinematic/dynamic SLAM approach, firstly, an overall overview is presented. To develop a SLAM system that suits the kinematic/dynamic controller proposed in [2], it is necessary to structure it in layers. The fastest layer is in charge of the vehicle dynamics estimation, while the slowest layer addresses the SLAM problem using the results provided by the fastest layer (see Figure 3). Hence, the motion model (7) is divided into two submodels, one per layer. On the one hand, the state estimator of the fastest layer has to estimate the vehicle dynamic state ${\overrightarrow{x}}_d={\left[\begin{array}{ccc}vs.& \alpha & \omega \end{array}\right]}^T$ for the dynamic controller of the autonomous driving system. As inputs, it has the dynamic control signals ${\overrightarrow{u}}_d={\left[\begin{array}{cc}{F}_{xR}& \delta \end{array}\right]}^T$ and the unknown input $\overrightarrow{n}=\mu$, using the sensor measurements ${\overrightarrow{y}}_d={\left[\begin{array}{cc}vs.& \omega \end{array}\right]}^T$ to correct the estimation. On the other hand, the kinematic state estimator must estimate the vehicle kinematic state ${\overrightarrow{x}}_k={\left[\begin{array}{ccc}{x}_v^w& y_v^w& \theta \end{array}\right]}^T$ and the states of the incremental map built by the SLAM, to serve the kinematic controller and the trajectory planner system of the autonomous driving system. As input, it has the state estimation provided by the dynamic state estimator ${\overrightarrow{x}}_d={\left[\begin{array}{ccc}vs.& \alpha & \omega \end{array}\right]}^T$ that uses the sensor measurements ${\overrightarrow{y}}_k={\left[\begin{array}{cccccccc}{x}_v^w& y_v^w& \theta & x_{l1}^s& y_{l1}^s& \cdots & x_{lm}^s& y_{lm}^s\end{array}\right]}^T$ to correct the estimation.

Once an overview of the proposed two-layer estimation scheme is provided, in the following subsections, the detail description of each estimator is provided.

3.1. Dynamic State Estimation

To estimate the dynamic behaviour of the vehicle, the vehicle dynamic model (1) is considered as the motion model. This model is temporally discretized by applying the Euler approximation using the dynamic sampling time ${\tau }_d$. This allows one to obtain a pseudolinear representation applying the LPV state-space formulation by embedding the nonlinearities in the varying parameters as follows:

$$\begin{array}{cc}\hfill {\overrightarrow{x}}_d(k)& ={\overrightarrow{f}}_d({\overrightarrow{x}}_d(k-1),{\overrightarrow{u}}_d(k-1),\overrightarrow{n}(k-1))\hfill \\ & =\mathsf{\Phi }{({\overrightarrow{\psi }}_d)}_{k-1}{\overrightarrow{x}}_d(k-1)+\Gamma {({\overrightarrow{\psi }}_d)}_{k-1}{\overrightarrow{u}}_d(k-1)+\eta \overrightarrow{n}(k-1)\hfill \end{array}$$

(8)

where

$$\mathsf{\Phi }{({\overrightarrow{\psi }}_d)}_k=\left[\begin{array}{ccc}{\mathsf{\Phi }}_{11}& {\mathsf{\Phi }}_{12}& {\mathsf{\Phi }}_{13}\\ 0& {\mathsf{\Phi }}_{22}& {\mathsf{\Phi }}_{23}\\ 0& {\mathsf{\Phi }}_{32}& {\mathsf{\Phi }}_{33}\end{array}\right],$$

$$\begin{array}{ccc}\hfill {\mathsf{\Phi }}_{11}& =& 1-{\tau }_d\frac{1}{2}{C}_d\rho A{\displaystyle \frac{v}{m}},\hfill \\ \hfill {\mathsf{\Phi }}_{12}& =& -{\tau }_d{C}_x{\displaystyle \frac{sin(\alpha -\delta )+sin\alpha }{m}},\hfill \\ \hfill {\mathsf{\Phi }}_{13}& =& {\tau }_d{C}_x{\displaystyle \frac{bsin\alpha -asin(\alpha -\delta )}{mv}},\hfill \\ \hfill {\mathsf{\Phi }}_{22}& =& 1-{\tau }_d{C}_x{\displaystyle \frac{cos(\alpha -\delta )+cos\alpha }{mv}},\hfill \\ \hfill {\mathsf{\Phi }}_{23}& =& {\tau }_d{C}_x{\displaystyle \frac{bcos\alpha -acos(\alpha -\delta )}{m{v}^2}}-{\tau }_d,\hfill \\ \hfill {\mathsf{\Phi }}_{32}& =& {\tau }_d{C}_x{\displaystyle \frac{b-acos\delta }{I}},\hfill \\ \hfill {\mathsf{\Phi }}_{33}& =& 1-{\tau }_d{C}_x{\displaystyle \frac{b^2+a^{2}cos\delta }{Iv}},\hfill \end{array}$$

$$\Gamma {({\overrightarrow{\psi }}_d)}_k=\left[\begin{array}{cc}{\Gamma }_{11}& {\Gamma }_{12}\\ {\Gamma }_{21}& {\Gamma }_{22}\\ 0& {\Gamma }_{32}\end{array}\right],$$

$$\begin{array}{ccc}\hfill {\Gamma }_{11}& =& {\tau }_d{\displaystyle \frac{cos\alpha }{m}},\hfill \\ \hfill {\Gamma }_{12}& =& {\tau }_d{C}_x{\displaystyle \frac{sin(\alpha -\delta )}{m}},\hfill \\ \hfill {\Gamma }_{21}& =& -{\tau }_d{\displaystyle \frac{sin\alpha }{mv}},\hfill \\ \hfill {\Gamma }_{22}& =& {\tau }_d{C}_x{\displaystyle \frac{cos(\alpha -\delta )}{mv}},\hfill \\ \hfill {\Gamma }_{32}& =& {\tau }_d{C}_x{\displaystyle \frac{acos\delta }{m}},\hfill \end{array}$$

$$\eta =\left[\begin{array}{c}-{\tau }_{d}g\\ 0\\ 0\end{array}\right],$$

the model parameters are in Table 1 and the dynamic scheduling vector is ${\overrightarrow{\psi }}_d(k)={\left[\begin{array}{ccc}\delta (k)& v(k)& \alpha (k)\end{array}\right]}^T$.

If velocities v and $\omega$ are measured, the output equation for the dynamic system using the LPV formulation is:

$${\overrightarrow{y}}_d(k)={\overrightarrow{h}}_d({\overrightarrow{x}}_d(k))=C_d{\overrightarrow{x}}_d(k)$$

(9)

where

$$C_d=\left[\begin{array}{ccc}1& 0& 0\\ 0& 0& 1\end{array}\right].$$

Using these sensor measurements, the dynamic LPV system (8) is observable for all the values of the varying parameters.

The dynamic motion model (8) is additionally affected by an unknown input $\overrightarrow{n}$ that acts as a perturbation and is due to the friction force that is a function of the friction coefficient $\mu (k)$ that varies and in general is not known exactly. To be able to estimate the dynamic states without having this force, an Unknown Input Observer (UIO) scheme is proposed [16]. According to this reference, to apply the UIO, firstly, the condition $rank\left\{C\eta \right\}=rank\left\{\eta \right\}$ must be verified. As matrix C is constant and given by (9), it is easy to see that the condition is satisfied. Therefore, the UIO could be implemented. According to [16], a possible implementation is based on the transformation matrix $\Omega$:

$$\Omega =I-\eta \left[{(C\eta )}^{T}C\eta \right]{}^{-1}{(C\eta )}^{T}C=\left[\begin{array}{ccc}0& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right].$$

which allows one to produce a new motion model decoupled from the perturbation and can be estimated using the input/output measurements:

$$\begin{array}{cc}\hfill {\overrightarrow{x}}_d(k)& ={\overrightarrow{f}}_d({\overrightarrow{x}}_d(k-1),{\overrightarrow{u}}_d(k-1),{\overrightarrow{y}}_d(k))\hfill \\ & =\overline{\mathsf{\Phi }}{({\overrightarrow{\psi }}_d)}_{k-1}{\overrightarrow{x}}_d(k-1)+\overline{\Gamma }{({\overrightarrow{\psi }}_d)}_{k-1}{\overrightarrow{u}}_d(k-1)+\Sigma {\overrightarrow{y}}_d(k)\hfill \end{array}$$

(10)

where

$$\begin{array}{ccc}\hfill \overline{\mathsf{\Phi }}{(\overrightarrow{{\psi }_d})}_k& =& \Omega \mathsf{\Phi }{({\overrightarrow{\psi }}_d)}_k,\hfill \\ \hfill \overline{\Gamma }{(\overrightarrow{{\psi }_d})}_k& =& \Omega \Gamma {({\overrightarrow{\psi }}_d)}_k,\hfill \\ \hfill \Sigma & =& \eta \left[{(C\eta )}^{T}C\eta \right]{}^{-1}{(C\eta )}^T,\hfill \end{array}$$

Note that all the involved matrices are defined in Equation (8).

3.2. Kinematic State Estimation and SLAM

To solve the SLAM problem, the motion and observation models of the kinematic layer must be defined. The motion model includes the vehicle kinematic model (2), the map kinematic model, and the implicit map dynamic model. As in the case of the dynamic model, they are temporally discretized by applying the Euler approximation using the kinematic sampling time ${\tau }_k$ and expressed in pseudolinear representation using the state-space LPV representation based on nonlinear embedding the nonlinearities in the varying parameters. In the case of considering n landmarks, the obtained LPV representation is:

$${\overrightarrow{x}}_k(k)={\overrightarrow{f}}_k({\overrightarrow{x}}_k(k-1),{\overrightarrow{x}}_d(k-1))=\mathsf{\Phi }{({\overrightarrow{\psi }}_k)}_{k-1}{\overrightarrow{x}}_k(k-1)+\Gamma {({\overrightarrow{\psi }}_k)}_{k-1}{\overrightarrow{x}}_d(k-1),$$

(11)

where

$$\mathsf{\Phi }{({\overrightarrow{\psi }}_k)}_k=\left[\begin{array}{cccccccc}1& 0& 0& 0& 0& \cdots & 0& 0\\ 0& 1& 0& 0& 0& \cdots & 0& 0\\ 0& 0& 1& 0& 0& \cdots & 0& 0\\ 0& 0& 0& 1& \varphi & \cdots & 0& 0\\ 0& 0& 0& -\varphi & 1& \cdots & 0& 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& \ddots & ⋮& ⋮\\ 0& 0& 0& 0& 0& \cdots & 1& \varphi \\ 0& 0& 0& 0& 0& \cdots & -\varphi & 1\end{array}\right],$$

$$\begin{array}{ccc}\hfill \varphi & =& {\tau }_k\omega ,\hfill \end{array}$$

$$\Gamma {({\overrightarrow{\psi }}_k)}_k=\left[\begin{array}{ccc}{\Gamma }_{11}& 0& 0\\ {\Gamma }_{21}& 0& 0\\ 0& 0& {\Gamma }_{33}\\ {\Gamma }_{41}& 0& {\Gamma }_{43}\\ {\Gamma }_{51}& 0& {\Gamma }_{53}\\ ⋮& ⋮& ⋮\\ {\Gamma }_{41}& 0& {\Gamma }_{43}\\ {\Gamma }_{51}& 0& {\Gamma }_{53}\end{array}\right],$$

$$\begin{array}{ccc}\hfill {\Gamma }_{11}& =& {\tau }_{k}cos(\theta +\alpha ),\hfill \\ \hfill {\Gamma }_{21}& =& {\tau }_{k}sin(\theta +\alpha ),\hfill \\ \hfill {\Gamma }_{33}& =& {\tau }_k,\hfill \\ \hfill {\Gamma }_{41}& =& -{\tau }_{k}cos(\alpha -\beta ),\hfill \\ \hfill {\Gamma }_{43}& =& {\tau }_k(tcos\beta -ssin\beta ),\hfill \\ \hfill {\Gamma }_{51}& =& -{\tau }_{k}sin(\alpha -\beta ),\hfill \\ \hfill {\Gamma }_{53}& =& -{\tau }_k(tsin\beta +scos\beta ),\hfill \end{array}$$

where the model parameters are summarized in Table 2 and the kinematic scheduling vector is ${\overrightarrow{\psi }}_k(k)=\left[\begin{array}{ccc}\alpha (k)& \omega (k)& \theta (k)\end{array}\right]$. Matrix $\mathsf{\Phi }{({\overrightarrow{\psi }}_k)}_k$ is square with dimension $3+2n$ and the dimensions of matrix $\Gamma {({\overrightarrow{\psi }}_k)}_k$ are $(3+2n)\times 3$.

To solve the SLAM problem, it is necessary to bear in mind that the kinematic state vector is formed by the n discovered landmarks which form the incremental map. Note that technically it is only possible to consider a subset of the discovered landmarks, called the active landmarks. From that subset of active landmarks, a group of measurements are correlated with the known landmarks of the map, which are used to perform the kinematic observation task. The rest of active landmarks are new discoveries and will be included in the map once the kinematic observation is performed. Hence, the measurements of the kinematic system are the pose of the vehicle in the world frame and the position of the m correlated active landmarks measured in the sensor frame are

$${\overrightarrow{y}}_k={\left[\begin{array}{cccccccc}{x}_v^w(k)& y_v^w(k)& \theta & x_{l1}^s(k)& y_{l1}^s& \cdots & x_{lm}^s(k)& y_{lm}^s(k)\end{array}\right]}^T.$$

The resulting observation model is

$${\overrightarrow{y}}_k(k)={\overrightarrow{h}}_k({\overrightarrow{x}}_k(k))=C_k{\overrightarrow{x}}_k(k)$$

(12)

where

$$C_k=\left[\begin{array}{cccccccc}1& 0& 0& 0& 0& \cdots & 0& 0\\ 0& 1& 0& 0& 0& \cdots & 0& 0\\ 0& 0& 1& 0& 0& \cdots & 0& 0\\ 0& 0& 0& 1& 0& \cdots & 0& 0\\ 0& 0& 0& 0& 1& \cdots & 0& 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& \ddots & ⋮& ⋮\\ 0& 0& 0& 0& 0& \cdots & 1& 0\\ 0& 0& 0& 0& 0& \cdots & 0& 1\end{array}\right]$$

with dimensions $(3+2m)\times (3+2n)$.

$${\overrightarrow{\widehat{x}}}_k(k)={\overrightarrow{\widehat{x}}}_k^{+}(k)+L_k(k)\left[{\overrightarrow{y}}_k(k)-C_k{\overrightarrow{\widehat{x}}}_k^{+}(k)\right]$$

3.3. State Estimation Scheme

Once the models used for state estimation and SLAM have been introduced, they are used through the following estimation structure,

$${\overrightarrow{\widehat{x}}}_d(k)={\overrightarrow{\widehat{x}}}_d^{+}(k)+L_d(k)\left[{\overrightarrow{y}}_d(k)-C_d(k){\overrightarrow{\widehat{x}}}_d^{+}(k)\right]$$

(13)

where $C_d$ is the output matrix defined in Equation (9) in the case of the dynamic estimation or in Equation (12) in the case of SLAM. The difference between the different proposed estimators is the considered model described in previous subsections and the way the correcting gain $L_d(k)$ is obtained using the design procedures presented in the next section.

Using (13) at every time iteration k, the vehicle motion model must be evaluated using the control signals ${\overrightarrow{u}}_d(k-1)$ and previous state estimations ${\overrightarrow{\widehat{x}}}_d^{+}(k)$. The sensor measurements ${\overrightarrow{y}}_d(k)$ are used to correct the state estimations provided by the model through the correcting factor $L_d(k)$.

4. Design Procedures

In this section, two different procedures are proposed to determine the correction gain $L_d(k)$ required for the state estimation scheme (13). The first approach is based on solving online the Ricatti equation as in EKF [17]. However, the proposed approach avoids the linearization of the model required in EKF by using the pseudolinearization offered by the LPV formulation of the model presented in the previous section for the different models considered. The second approach is based on using the polytopic representation of the LPV model. This approach allows solving the Ricatti equations formulated as LMIs [17] offline only in the vertices of the polytopic model. During the online operation, the value of correction gain $L_d(k)$ at each operation point is calculated online by interpolation of the solution found offline at the vertices of the polytopic LPV model [12].

4.1. Online Approach

To apply the online approach, the matrices of the pseudolinear system model representation are evaluated at the current operating point. The calculation of the gain $L_d(k)$ is based on solving the Ricatti Equation of the Kalman Filter using the current value of system matrices. This implies performing the following calculations at each time iteration k to determine the gain $L_d(k)$ and the covariance matrix $P_d(k)$

$$\begin{array}{ccc}\hfill P_d^{+}(k)& =& \overline{\mathsf{\Phi }}{({\overrightarrow{\psi }}_d)}_{k-1}{P}_d(k-1)\overline{\mathsf{\Phi }}{({\overrightarrow{\psi }}_d)}_{k-1}^T+\overline{\Gamma }{({\overrightarrow{\psi }}_d)}_{k-1}{Q}_d\overline{\Gamma }{({\overrightarrow{\psi }}_d)}_{k-1}^T+\Sigma R_d\Sigma ,\hfill \\ \hfill Z(k)& =& C_d{P}_d^{+}(k)C_d^T+R_d,\hfill \\ \hfill L_d(k)& =& P_d^{+}(k)C_d^{T}Z{(k)}^{-1},\hfill \\ \hfill P_d(k)& =& P_d^{+}(k)-L_d(k)C_d^T{P}_d^{+}(k).\hfill \end{array}$$

Matrices $\overline{\mathsf{\Phi }}{({\overrightarrow{\psi }}_d)}_{k-1}$, $\overline{\mathsf{\Phi }}{({\overrightarrow{\psi }}_d)}_{k-1}$ and $\Sigma$ are defined in Equation (10), matrix $C_d$ is defined in Equation (9), and $Q_d$ and $R_d$ are, respectively, the covariance matrices of the disturbances and sensor measurements.

In the case of SLAM, the following calculation must be performed at each iteration to determine the gain $L_k(k)$ and the covariance matrix $P_k(k)$:

$$\begin{array}{ccc}\hfill P_k^{+}(k)& =& \mathsf{\Phi }{({\overrightarrow{\psi }}_k)}_{k-1}{P}_k(k-1)\mathsf{\Phi }{({\overrightarrow{\psi }}_k)}_{k-1}^T+\Gamma {({\overrightarrow{\psi }}_k)}_{k-1}{P}_d(k-1)\Gamma {({\overrightarrow{\psi }}_k)}_{k-1}^T,\hfill \\ \hfill Z(k)& =& C_k{P}_k^{+}[rl,rl](k)C_k^T+R_k,\hfill \\ \hfill L_k(k)& =& P_k^{+}[rm,rl](k)C_k^{T}Z{(k)}^{-1},\hfill \\ \hfill P_k(k)& =& P_k^{+}[rm,rm](k)-L_k(k)C_k^T{P}_k^{+}[rl,rm](k).\hfill \end{array}$$

Matrices $\mathsf{\Phi }{({\overrightarrow{\psi }}_k)}_{k-1}$ and $\Gamma {({\overrightarrow{\psi }}_k)}_{k-1}$ are defined in Equation (11), and matrix $C_k$ is defined in Equation (12). $P_d(k-1)$ is the covariance matrix of the dynamic estimation applied to the kinematic estimation. In contrast, $P_k(k-1)$ is the variance matrix of the kinematic estimation carried out on the previous iteration. As seen, both layers are linked by the variance matrix $P_d$. Finally, $rl$ refers to the dimensions of matrix $P_k^{+}(k)$ corresponding to the vehicle kinematics and to the active landmarks, and $rm$ refers to all dimensions of matrix $P_k^{+}(k)$.

4.2. Offline Approach

The offline approach is based on the use of the polytopic LPV vehicle model.

To define the polytopic LPV vehicle model, firstly, it must be noted that the dimension of the dynamic scheduling vector ${\overrightarrow{\psi }}_d$ is $n_{\psi }=3$. Hence, the dynamic polytopic model has $n_{\mathsf{\Psi }}=8$ vertices ${\overrightarrow{\psi }}_{di}$, which are:

$${\mathsf{\Psi }}_d=\left[\begin{array}{ccc}\underline{\delta }& \underline{v}& \underline{\alpha }\\ \underline{\delta }& \underline{v}& \overline{\alpha }\\ \underline{\delta }& \overline{v}& \underline{\alpha }\\ \underline{\delta }& \overline{v}& \overline{\alpha }\\ \overline{\delta }& \underline{v}& \underline{\alpha }\\ \overline{\delta }& \underline{v}& \overline{\alpha }\\ \overline{\delta }& \overline{v}& \underline{\alpha }\\ \overline{\delta }& \overline{v}& \overline{\alpha }\end{array}\right],$$

where the underlined and the overlined represent, respectively, the lowest and the highest limit for each scheduling variable.

Then, the following Kalman Filter Ricatti equation expressed in LMI form [17] must be solved for each vertex of the polytopic LPV model:

$$\begin{array}{c}\hfill \left[\begin{array}{cccc}-Y_d& Y_d\overline{\mathsf{\Phi }}({\overrightarrow{\psi }}_{di})-W({\overrightarrow{\psi }}_{di})C_d& Y_d\overline{Q}{({\overrightarrow{\psi }}_{di})}^T& W({\overrightarrow{\psi }}_{di})\\ \overline{\mathsf{\Phi }}{({\overrightarrow{\psi }}_{di})}^T{Y}_d-C_d^{T}W{({\overrightarrow{\psi }}_{di})}^T& -Y_d& 0& 0\\ \overline{Q}({\overrightarrow{\psi }}_{di})Y_d& 0& -I& 0\\ W{({\overrightarrow{\psi }}_{di})}^T& 0& 0& -R^{-1}\end{array}\right]\le 0fori=1\cdots n_{\mathsf{\Psi }}\end{array}$$

(14)

where

$$\begin{array}{ccc}\hfill \overline{Q}({\overrightarrow{\psi }}_{di})& =& \sqrt{\overline{\Gamma }({\overrightarrow{\psi }}_{di})Q_d\overline{\Gamma }{({\overrightarrow{\psi }}_{di})}^T+\Sigma R_d{\Sigma }^T},\hfill \\ \hfill W({\overrightarrow{\psi }}_{di})& =& Y_d^{-1}{L}_d({\overrightarrow{\psi }}_{di}).\hfill \end{array}$$

$\overline{\mathsf{\Phi }}({\overrightarrow{\psi }}_{di})$, $\overline{\Gamma }({\overrightarrow{\psi }}_{di})$ and $\Sigma$ are defined in Equation (10), and $C_d$ is the output matrix defined in Equation (9). $Q_d$ and $R_d$ are the variance matrices of the control signals and sensor measurements. An additional LMI has to be included to guarantee the stability in the Lyapunov sense of the resulting polytopic LPV estimation scheme:

$$\left[\begin{array}{cc}\gamma I& I\\ I& Y_d\end{array}\right]\ge 0,$$

(15)

where I is a three-dimensional identity matrix.

Finally, an optimization problem considering the previous LMIs (14) and (15) as constraints must be solved, where the minimization goal is the scalar $\gamma$ and the decision variables are matrix $Y_d=P_d^{-1}$ and matrices $W({\overrightarrow{\psi }}_{di})$. The problem solution provides the gain $L_d({\overrightarrow{\psi }}_{di})$ for each vertex of the dynamic polytopic LPV model.

Once the gains $L_d({\overrightarrow{\psi }}_{di})$ for each vertex of the polytopic model are obtained offline, the gain $L_d({\overrightarrow{\psi }}_d)(k)$ is adapted in function of the operation point as follows

$$L_d({\overrightarrow{\psi }}_d)(k)=\sum _{i=1}^{n_{\mathsf{\Psi }}}{\mu }_{di}(k)L_d({\overrightarrow{\psi }}_{di}).$$

(16)

where the interpolation factors ${\mu }_{di}(k)$ for each vertex of the polytopic model are obtained as follows:

$${\mu }_{di}(k)=\prod _{j=1}^{n_{\psi }}{\epsilon }_{ij}({\eta }_0^j(k),{\eta }_1^j(k))fori=1\dots n_{\mathsf{\Psi }}$$

(17)

with

$$\begin{array}{cc}\begin{array}{ccc}\hfill {\eta }_0^j(k)& =& {\displaystyle \frac{\overline{{\psi }_j}-{\psi }_j(k)}{\overline{{\psi }_j}-\underline{{\psi }_j}}},\hfill \\ \hfill {\eta }_1^j(k)& =& 1-{\eta }_0^j(k),\hfill \end{array}& forj=1\dots n_{\psi },\end{array}$$

(18)

taking into account the limits for each scheduling variable ${\psi }_j(k)$.

Using the LPV polytopic approach to solve a SLAM problem requires limiting the dimension of the map state vector of the kinematic system to only the m active landmarks correlated with the known map. In this way, the dimension of the state vector does not grow indefinitely as the problem advances and, by fixing a limit $\rho$ to the maximum number of correlated active landmarks at each iteration, the possible $\rho +1$ dimensions for the state vector can be forecast in advance. As the kinematic scheduling vector is not affected by the position of any landmark, the polytopic model generated for the subset of m correlated active landmarks is valid for any subset of active landmarks, while the dimension of the subset remains at m. Following this approach, it is possible to tune $\rho +1$ different gains offline. Furthermore, during the observation algorithm, it is possible to choose the convenient set of gains depending on the m correlated active landmarks available at each operation point. Unlike with the online Ricatti methodology, the SLAM problem solved by means of the polytopic approach uses a limited memory considering only the landmarks close to the surroundings of the vehicle to perform the localization task, without doing corrections to the entirety of the incremental map built during the whole execution. Nevertheless, this problem relaxation allows the observability condition of the kinematic layer to be accomplished.

To define the polytopic model of the kinematic system, firstly, it must be noted that the dimension of the kinematic scheduling vector ${\overrightarrow{\psi }}_k$ is $n_{\psi }=3$. Hence, the kinematic polytopic model has $n_{\mathsf{\Psi }}=8$ vertices ${\overrightarrow{\psi }}_{ki}$ defined as follows:

$${\mathsf{\Psi }}_k=\left[\begin{array}{ccc}\underline{\alpha }& \underline{\omega }& \underline{\theta }\\ \underline{\alpha }& \underline{\omega }& \overline{\theta }\\ \underline{\alpha }& \overline{\omega }& \underline{\theta }\\ \underline{\alpha }& \overline{\omega }& \overline{\theta }\\ \overline{\alpha }& \underline{\omega }& \underline{\theta }\\ \overline{\alpha }& \underline{\omega }& \overline{\theta }\\ \overline{\alpha }& \overline{\omega }& \underline{\theta }\\ \overline{\alpha }& \overline{\omega }& \overline{\theta }\end{array}\right],$$

where the underlined and the overlined represent, respectively, the lowest and the highest limit for each scheduling variable.

The dimension of the kinematic system is variable because of the number of landmarks considered in the SLAM problem. Therefore, if $ϵ\in [0,\rho ]$ is the number of active landmarks correlated with the known map for an operation point of the observer system, then a tuning process must be performed for each possible dimension $ϵ$ for the correlated active landmarks subset. Hence, $\rho +1$ different sets of gains must be calculated, because the case without any measured known landmarks should be taken into account. To tune the set of gains for $ϵ$ landmarks, the LMI for the optimum gain [17] has to be considered for each vertex of the polytopic model:

$$\begin{array}{c}\hfill \left[\begin{array}{cccc}-Y_{kϵ}& Y_{kϵ}{\mathsf{\Phi }}_{ϵ}({\overrightarrow{\psi }}_{ki})-W_{ϵ}({\overrightarrow{\psi }}_{ki})C_{kϵ}& Y_{kϵ}\overline{Q}{({\overrightarrow{\psi }}_{ki})}^T& W_{ϵ}({\overrightarrow{\psi }}_{ki})\\ {\mathsf{\Phi }}_{ϵ}{({\overrightarrow{\psi }}_{ki})}^T{Y}_{kϵ}-C_{kϵ}^T{W}_{ϵ}{({\overrightarrow{\psi }}_{ki})}^T& -Y_{kϵ}& 0& 0\\ \overline{Q}({\overrightarrow{\psi }}_{ki})Y_{kϵ}& 0& -I& 0\\ W_{ϵ}{({\overrightarrow{\psi }}_{ki})}^T& 0& 0& -R_{kϵ}^{-1}\end{array}\right]\le 0fori=1\dots n_{\mathsf{\Psi }}\end{array}$$

where

$$\begin{array}{ccc}\hfill \overline{Q}({\overrightarrow{\psi }}_{ki})& =& \sqrt{{\Gamma }_{ϵ}({\overrightarrow{\psi }}_{ki})P_d{\Gamma }_{ϵ}{({\overrightarrow{\psi }}_{ki})}^T},\hfill \\ \hfill W_{ϵ}({\overrightarrow{\psi }}_{ki})& =& Y_{kϵ}^{-1}{L}_{ϵ}({\overrightarrow{\psi }}_{ki}).\hfill \end{array}$$

${\mathsf{\Phi }}_{ϵ}({\overrightarrow{\psi }}_{ki})$ and ${\Gamma }_{ϵ}({\overrightarrow{\psi }}_{ki})$ are defined in Equation (11), $C_{kϵ}$ is the output matrix defined in Equation (12), and $R_k$ and $P_d$ are, respectively, the covariance matrices of the sensor measurements and the dynamic estimations used. As can be seen, all matrices are defined according to the fixed $ϵ$ active landmarks, and the covariance matrix $P_d$ is the link between the kinematic/dynamic estimation layers. Next, an additional LMI must be included to guarantee the global stability of the polytopic estimation scheme to accomplish Lyapunov conditions:

$$H=\left[\begin{array}{cc}\gamma I& I\\ I& Y_{kϵ}\end{array}\right]\ge 0,$$

where I is an $n+2ϵ$ dimensional identity matrix.

Finally, the optimization problem defined by the resulting LMIs must be solved, where the minimization goal is the scalar $\gamma$ and the decision variables are the matrix $Y_{kϵ}$ and the matrices $W_{ϵ}({\overrightarrow{\psi }}_{ki})$. The problem solution provides a gain $L_{ϵ}({\overrightarrow{\psi }}_{ki})$ for each vertex of the kinematic polytopic model. This set of gains for the subset of $ϵ$ active landmarks must be saved in the vehicle estimator memory because it must be used during the estimation process every time $ϵ$ correlated active landmarks are available. The described process must be repeated for each $\rho +1$ possible subset of active landmarks.

To determine the polytopic gain $L_{ϵ}({\overrightarrow{\psi }}_{ki})(k)$ for each operation point inside the estimation loop, firstly, the $ϵ$ active landmarks correlated with the map must be determined, that is, for which map landmarks are the vehicle sensors able to obtain measurements at the current iteration. This information allows fixing the system state ${\overrightarrow{x}}_k\left[rl\right](k)$ for the operation point. Next, the set of gains $L_{ϵ}({\overrightarrow{\psi }}_{ki})$ for $ϵ$ active landmarks has to be searched in the vehicle estimator memory. Afterwards, the direct and inverse unit scaling of each scheduling variable must be determined according to the previous kinematic estimations ${\overrightarrow{x}}_k(k-1)$, applying Equation (18) for each scheduling variable. Then, the interpolation factors ${\mu }_{ki}(k)$ for each vertex of the polytopic estimator must be determined by applying Equation (17) and following the logic used to define ${\mathsf{\Psi }}_k$. Lastly, the interpolated gain must be calculated by applying Equation (16) and the interpolation factors ${\mu }_{ki}(k)$ previously found.

5. Simulation Results

In this section, simulation results are provided to illustrate the performance of the proposed approaches and compare the differences between the two design procedures presented in Section 4. Furthermore, a comparison with the classical EKF is provided to prove the interest of the introduced techniques.

5.1. Simulation Set-Up

To conduct these simulations, a handmade Matlab environment inspired by the SLAM Toolbox for Matlab [18] is used, and the control data are taken from the experimentation carried in a previous work [2]. As a world map, a synthetic map is taken, formed by a mesh of $12\times 40$ elements uniformly distributed between coordinates $-50$ m and 1050 m in the x direction and between $-50$ m and 450 m in the y direction. Hence, the vehicle has 480 available landmarks along its path, although a limit of $\rho =10$ active landmarks at each iteration is set. Simulations are conducted considering the Tazzari Zero autonomous vehicle, whose parameters are shown in Table 1, as well as the exteroceptive sensor installation parameters shown in Table 2. The limits for the scheduling variables of the kinematic/dynamic LPV models are set as follows:

$$\begin{array}{cccccc}\hfill \underline{\delta }& =& -{25}^{\circ },\hfill & \hfill \overline{\delta }& =& {25}^{\circ },\hfill \\ \hfill \underline{v}& =& 2\mathrm{m}/\mathrm{s},\hfill & \hfill \overline{v}& =& 18\mathrm{m}/\mathrm{s},\hfill \\ \hfill \underline{\alpha }& =& -0.1\mathrm{rad},\hfill & \hfill \overline{\alpha }& =& 0.1\mathrm{rad},\hfill \\ \hfill \underline{\theta }& =& -{90}^{\circ },\hfill & \hfill \overline{\theta }& =& {160}^{\circ },\hfill \\ \hfill \underline{\omega }& =& -0.2\mathrm{rad}/\mathrm{s},\hfill & \hfill \overline{\omega }& =& 0.2\mathrm{rad}/\mathrm{s}.\hfill \end{array}$$

A sampling time of ${\tau }_d=1$ ms is used for the dynamic state estimation layer, while a sampling time of ${\tau }_k=100$ ms is considered for the SLAM layer. For the design of the dynamic state estimator, diagonal covariance matrices $Q_d=\left[\begin{array}{cc}1,& 1\times {10}^{-4}\end{array}\right]$, and $R_d=\left[\begin{array}{cc}1\times {10}^{-2},& 1\times {10}^{-4}\end{array}\right]$ are considered. For the SLAM layer, diagonal covariance matrices $R_k=\left[\begin{array}{ccc}1\times {10}^{-2},& 1\times {10}^{-2},& 1\times {10}^{-6}\end{array}\right]$, and $R_m=\left[\begin{array}{cc}1\times {10}^{-2},& 1\times {10}^{-2}\end{array}\right]$ are used, where $R_k$ is related to the kinematic sensors of the vehicle and $R_m$ is related to the measure of a single landmark. To design the polytopic LPV estimators following the offline approach presented in Section 4, Mosek solver [19] is used to solve all the involved optimization problems.

5.2. Simulation Scenarios

Two different experiments are conducted. In both cases, a variable friction between the vehicle and the ground is considered, as well as additive perturbations and sensor noise. All perturbations and noises are assumed to be Gaussian and are generated according to their corresponding covariance matrices Q and R, which were previously defined. In Figure 4, top right, the variable friction applied to the system is shown, and in Figure 4 left, the dynamic control signals ${\overrightarrow{u}}_d$ with additive Gaussian disturbances are presented. Using these data as inputs to the vehicle simulator and the sensor measurements ${\overrightarrow{y}}_d$ obtained, the dynamic response of the vehicle could be estimated. In both simulation scenarios, these inputs are common, and consequently, the dynamic estimation results are the same. These results are shown in Figure 5, where it can be observed that both estimation techniques provide good results with a very similar performance. Both techniques filter the system disturbances, although the polytopic LPV approach presents less estimation error accumulation. As will be shown next, this result will be amplified in the kinematic SLAM layer. Furthermore, in Figure 5 it is shown that for state $\alpha$, which is not measured, a good estimation is achieved. In contrast to all of that, in Figure 4, right, the friction estimation performed by the UIO is shown. It can be seen that this estimation is equivalent for both estimation techniques, although the UIO associates most of the disturbances received with the friction, complicating the distinction between process disturbances and friction force. Furthermore, the application of an UIO to this specific problem implies that the measurement of the state v is analytically assigned directly to the estimation, blocking any correction or filtering process to that state. This phenomenon is shown in Figure 5, where it is seen how the estimation of the state v could not be filtered. In conclusion, although the UIO is necessary to compensate the variable perturbation introduced by the vehicle friction, it cannot completely eliminate the effect of additive Gaussian disturbances.

Using the dynamic estimations, the kinematic SLAM estimation can be performed. Two experiments are proposed to evaluate the robustness of the proposed SLAM approach. In the first experiment, initialization of landmarks is corrupted with an additive Gaussian noise with a standard deviation of 10 m, which allows modelling an exteroceptive sensor with a very bad state initialization. The results in this case are shown in Figure 6, where significant differences are observed between the online and offline design approaches. For this reason, the classical solution using an EKF, based on first-order linearization, is also plotted and used as a reference for comparison. On the one hand, it can be observed that the online approach presents significant biases, following the same trend as the EKF but accumulating more error. On the other hand, the offline design approach based on the polytopic LPV estimator accumulates less error than the EKF and provides a vehicle localization with an order accuracy of a few decimeters for the position, one order of magnitude less than the estimation performed by the Ricatti observer. These error accumulations appear during the vehicle turns, when the $\overrightarrow{v}$ direction is detached from the longitudinal direction of the vehicle, dynamic effects become evident and drifts can appear at the vehicle wheels. As the dynamic behaviour is affected by important nonlinearities, these results show how the polytopic observer is able to deal with the nonlinearities problem, and therefore, it becomes the most dynamically sensitive, improving the classical EKF. Despite this, note that the estimation for the polytopic observer is a bit more noisy than the EKF, especially for the vehicle orientation. However, the global accuracy improves with respect to the EKF, and it can be concluded that the polytopic observer stands out for the localization task.

Figure 7, left, shows the results of the mapping task for this experiment. It can be seen how, unless using a sensor with bad accuracy for the initialization of landmarks, both proposed techniques are robust enough to converge close to the real landmark’s position. Furthermore, note how the vehicle discovers its closest surroundings and accurately places the landmarks in the incremental map. At the same time, the vehicle trajectory on the map can also be determined correctly. Therefore, the mapping task is carried out properly for both techniques, and no significant differences are observed.

The second experiment belongs to a critical case in which no state initialization mechanism is available. The EKF-SLAM technique is very sensitive if new discovered landmarks are not properly initialized. In [20], the initialization process for the SLAM system state vector $\overrightarrow{x}$ and the SLAM system variance matrix P is described. This process was applied in the previous experiment, but now it is removed and all landmarks initialization are set to nearly zero. This will allow us to more deeply analyse the robustness of the proposed SLAM solution and check if this initialization is needed for the proposed methodologies. Figure 8 shows the kinematic results for this experiment. It is seen how, as expected, the EKF is not able to deal with this high uncertainty. Similarly, the Riccati observer also cannot assume it, but, on the contrary, the polytopic approach is able to perform a good localization with an error of decimetres for the vehicle position. These results show how this technique outperforms the robustness of the classical EKF when no accuracy on landmark initialization is able. Furthermore, Figure 7, right, shows the results of the mapping task for this experiment. It can be observed how only the polytopic observer is able to converge and accurately place the discovered landmarks in the incremental map. For the other techniques, a significant drift on the vehicle trajectory is appreciated, and in particular for the EKF, significant oscillations appear in every landmark discovery.

In conclusion, the presented results show how the the dynamic estimation tasks are developed correctly for both proposed techniques, apparently without significant differences at this level. However, achieving accuracy in the vehicle location becomes more complex, and the best estimation is provided by the polytopic observer. The Ricatti observer is conceived as the direct evolution of the EKF-SLAM methodology to include the dynamic response of the vehicle and use LPV techniques. It is seen how this approximation fails, and the results become worse with respect to the classical EKF. For this reason, it is necessary to go in depth into the LPV techniques and find a new approximation of the SLAM problem based on the polytopic models and tuning methodologies based on the optimization of a set of LMIs. This approximation turns out to be more accurate because it avoids updating the system model at the operation point, performing an interpolation for the whole problem domain. This weighing allows Lyapunov stability conditions to be accomplished for the global system and allows one to reach a better observation of the nonlinear problem. This is the improvement to the SLAM problem introduced in this article.

6. Conclusions

In this article, the SLAM problem is solved by applying a methodology based on kinematic/dynamic models using the LPV framework. The proposed SLAM approach is designed for mobile platforms with a considerable mass and not operated at low velocities, such as autonomous vehicles. For this type of mobile platform, its dynamic response becomes evident and contrasts with the mobile robots considered in classical SLAM problems, where only kinematic effects are modelled. A difference from those classical approaches is that in this proposal, an estimation mechanism of the dynamic response of the vehicle is provided, which is combined with the solution to a SLAM problem. Furthermore, this approach proves that to solve a problem with important nonlinear behaviour, it is necessary to go in depth into advanced control techniques, avoiding the classical EKF, which linearizes the system model around the operation point. The approach introduced in this paper is based on the use of the LPV technique, which considers the whole problem domain and avoids any linearization, and it has been verified in simulation, showing the advantages of the polytopic stationary observer with respect to the Ricatti iterative observer. To implement the polytopic estimator, a reformulation of the SLAM problem has been necessary, developing the map kinematic model presented in Equation (5), which allows a linear observation model for the SLAM problem and introduces a robocentric point of view to it. Furthermore, the polytopic technique only requires considering the measurable surroundings of the vehicle, avoiding the maintenance of the whole discovered map. It is verified in simulation that these relaxations do not have adverse effects to the localization task and allows it outperform the classical EKF-based methods.

Moreover, the proposed approach considers an observation system with an architecture that couples with the kinematic/dynamic controller structured in layers for an autonomous vehicle proposed at [2]. Both systems are built using the same vehicle models and are implemented with gain-scheduling LPV techniques. Both systems allow showing that it is possible to separate the kinematic and dynamic responses, permitting perform the dynamic treatment independently to the kinematic treatment. At the same time, in this article is shown how the vehicle kinematic estiamted layer can be combined with a SLAM problem, in the same way as in [3] the vehicle kinematic control layer is combined with the trajectory planner. Therefore, the presented kinematic/dynamic localization system allows to advance in the development of a layered software which permits the autonomous driving of a vehicle.

As future work, the following aspects will be further researched:

• The proposed approach will be applied and tested in the real vehicle in the context of current ongoing research project.
• The extension of the proposed observation approach to take into account 3D scene factors (such as ground unevenness and vehicle instability) will be explored.
• The inclusion of range sensors instead of state sensors will also be considered.