..
  *******************************************************************************
  Copyright (c) 2021-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)

  This program and the accompanying materials are made available under the
  terms of the Eclipse Public License 2.0 which is available at
  http://www.eclipse.org/legal/epl-2.0.

  SPDX-License-Identifier: EPL-2.0
  *******************************************************************************

.. _algorithm_lateral:

Algorithm_Lateral
#################


This module is responsible for the control of the vehicle's lateral behavior.
It converts the lateral input of the driver module into a steering wheel angle.
The steering wheel angle can then be forwarded to a vehicle dynamics
module like :ref:`dynamics_regular_driving`.

.. _algorithm_lateral_detailed_description:

Detailed description of the module's features
=============================================

Algorithm_Lateral receives its command variables from a driver behavior model (or an ADAS) 
and generates the steering wheel angle of the driver to match these command
variables. The steering wheel angle parts of all controllers are summed up to
the overall steering wheel angle of the driver, which is then sent to a vehicle
dynamics module like :ref:`dynamics_regular_driving`.
The control loop is closed by the movement of the vehicle in the world, which is
again monitored by the driver behavior model. This overall control loop of
vehicle lateral guidance involving Algorithm_Lateral is illustrated in the following image with the following variables:

* :math:`\kappa_{act}` is the actual curvature of the vehicle

Four variables containing the information needed for the open-loop controller:


* :math:`\kappa_{road}` is the curvature at the front center (i.e. center of the front of the bounding box) of the ego vehicle. It uses the vectors:
* :math:`\kappa_{segment,near}` containing the curvatures of several segments starting at the front center up to 2m in front of the vehicles leading edge 
* :math:`\kappa_{segment,far}` containing the curvatures of several segments starting 2m in front of the vehicle and ending 8m in front of it
* :math:`\kappa_{manoeuvre}` containing the curvature of the planned trajectory relative to the road

Other variables:


* :math:`w_{act}` is the actual lateral position of the vehicle in the road coordinate system
* :math:`\Delta` w is the lateral deviation
* :math:`\phi_{act}` is the actual heading angle of the vehicle in the road coordinate system
* :math:`\Delta`:math:`\phi` is the heading error
* :math:`\delta_{h,\kappa}` is the steering wheel angle from the curvature controller
* :math:`\delta_{h,w}` is the steering wheel angle from the lateral deviation controller
* :math:`\delta_{h,\phi}` is the steering wheel angle from the heading error controller
* :math:`\delta_h` is the overall steering wheel angle of the driver

.. _image_LateralControllerSignalFlow:

.. figure:: _static/images/LateralControllerSignalFlow.png
   :alt: Components and signal flow of the lateral guidance control loop

   Components and signal flow of the lateral guidance control loop

The following subsections describe the theoretical background and the transfer
functions of the different controllers.


.. _algorithm_lateral_detailed_description_lateral_dynamics:

Lateral dynamics
----------------

The lateral dynamics model is based on the Ackermann model which is a simple geometric 
expression for the relationship between the steering angle at the front wheels and 
the curvature the vehicle produces from it. It has several simplifications, where 
the most notable is the reduction of the wheels of one axle to a single surrogate 
wheel. This is suitable under the consideration that the steering angles of the front
wheels are rather small and do not differ much between the left and the right
front wheel. The first simplification may not hold up in city traffic with high
curvatures. Therefore the closed-loop-controller must have increased gains in
these situations. :math:`\kappa_{road}`, :math:`\kappa_{segment,near}` and :math:`\kappa_{segment,far}` are smoothed and weighted before adding :math:`\kappa_{manoeuvre}` to form :math:`\kappa_{target}`.

The Ackermann model is illustrated in the following image with the following variables:


* :math:`l` is the wheelbase of the vehicle
* :math:`\delta` is the surrogate steering angle at the front wheels towards the vehicle's longitudinal axis
* :math:`M` is the instantaneous centre of rotation, around which the vehicle is driving on a curve
* :math:`r` is the radius of the curve, which the vehicle is driving around M
* :math:`\kappa` is the curvature of this curve, which is simply the inverse of r

.. _image_AckermannModel:

.. figure:: _static/images/AckermannModel.svg
   :alt: Illustration of the Ackermann model

   Illustration of the Ackermann model

In accordance to the current definitions of :ref:`coordinate systems <coordinatesystems>`,
the coordinate reference point of the vehicle is considered to be the rear axle center.
Therefore, the curvature of the vehicle is also expressed towards the rear axle center
and not to the centre of gravity, which is why the centre of gravity is not
depicted in illustration of the Ackermann model.

The equation derived from the Ackermann model states the following
relation between the surrogate steering angle :math:`\delta` of the front wheels and
the curvature :math:`\kappa` described by the rear axle around the instantaneous
centre of rotation :math:`M`:

.. math::
   \kappa = \frac{\tan(\delta)}{l}


This equation can be inverted to express a required steering angle :math:`\delta` to
adjust a specific curvature :math:`\kappa`:

.. math::
   \delta = \arctan(\kappa \cdot l)

To convert this into the required steering wheel angle :math:`\delta_{h,\kappa}`,
the ratio :math:`i_s` of the steering gear must be applied:

.. math::
   \delta_{h, \kappa} = i_{s} \cdot \arctan(\kappa \cdot l)


The vehicle parameters :math:`l` and :math:`i_s` are received from the module :ref:`parameters_vehicle`.

.. _algorithm_lateral_detailed_description_control_heading:

Heading controller
------------------

The heading controller is designed as a simple proportional controller. 
The general transfer function of a P-controller with the input signal heading 
error :math:`\Delta\phi` and the output signal steering wheel angle :math:`\delta_{h,\phi}` 
is described as:

.. math::
   \delta_{h, \phi} = P_{\phi} \cdot \Delta\phi


The controller amplification :math:`P_\phi` is therefore described as the
ratio between input signal and output signal:

.. math::
   P_{\phi} = \frac{\delta_{h,\phi}}{\Delta\phi}

The controller amplification :math:`P_\phi` therefore expresses, how many
degrees of steering wheel angle are generated by one degree heading error.
The design of this controller amplification :math:`P_\phi` uses some
considerations about plane driving kinematics of Kramer. First of all,
the heading error :math:`\Delta\phi` is derived from the current curvature :math:`\kappa`
of the vehicle over the change of distance :math:`ds` along the road's longitudinal
coordinate :math:`s` in one time step:

.. math::
   \Delta\phi = \arctan(ds \cdot \kappa)

Under the consideration of small angular changes in one time step, this equation
can be linearized to:

.. math::
   \Delta\phi = ds \cdot \kappa

The curvature :math:`\kappa` of the vehicle can be substituted by an Ackermann model
(see :ref:`here<algorithm_lateral_detailed_description_lateral_dynamics>` for further information about that):


.. math::
   \Delta\phi = \frac{ds \cdot \tan(\delta)}{l}

This equation can also be linearized under the consideration of small angles:

.. math::
   \Delta\phi = \frac{ds \cdot \delta} {l}

The connection to the steering wheel angle :math:`\delta_{h,\phi}` can be
applied by the ratio :math:`i_s` of the vehicle's steering gear:

.. math::
   \Delta\phi = \frac{ds \cdot \delta_{h,\phi}}{i_s \cdot l}

This equation can be transformed to a similar form as the controller's transfer
function above:

.. math::
   \frac{i_s \cdot l}{ds} = \frac{\delta_{h,\phi}}{\Delta\phi}

The usage of the incremental difference :math:`ds` in this equation is problematic,
because the controller amplification becomes dependent on the simulation step
size. Because of this, the absolute change :math:`ds` of the longitudinal road
coordinate :math:`s` is substituted by the vehicle's absolute velocity :math:`v` as a
simplification, which is the change of travelled distance with respect to time:

.. math::
   \frac{i_s \cdot l}{v} = \frac{\delta_{h,\phi}}{\Delta\phi}

To tune the absolute influence of the heading controller in the overall control
loop, an additional gain factor :math:`\text{Gain}_{\Delta\phi}` is applied to this
transfer function, which allows a situation dependent amplification of the
heading controller by the driver behavior model:

.. math::
   \delta_{h,\phi} = \frac{\text{Gain}_{\Delta\phi} \cdot i_s \cdot l \cdot \Delta\phi}{v}

The vehicle parameters :math:`l` and :math:`i_s` are received from the module :ref:`parameters_vehicle`, the vehicle's
absolute velocity :math:`v` is received from the module :ref:`sensor_driver`,
and the additional controller gain :math:`\text{Gain}_{\Delta\phi}` is received from
a driver behavior model.


.. _algorithm_lateral_detailed_description_control_deviation:

Lateral deviation controller
----------------------------

The lateral deviation controller is designed as a simple proportional controller. The general transfer
function of a P-controller with the input signal lateral deviation :math:`\Delta` w and
the output signal steering wheel angle :math:`\delta_{h,w}` is described as:

.. math::
   \delta_{h,w} = P_w \cdot \Delta w

The controller amplification :math:`P_w` is therefore described as the ratio
between input signal and output signal:

.. math::
   P_w = \frac{\delta_{h,w}}{\Delta w}

The controller amplification :math:`P_w` therefore expresses, how many degrees
of steering wheel angle are generated by one metre of lateral deviation. The
design of this controller amplification :math:`P_w` uses some considerations
about plane driving kinematics of Kramer. First of all, the lateral deviation
:math:`\Delta` w is derived from the current heading error :math:`\Delta\phi` over the change
of distance :math:`ds` along the road's longitudinal coordinate :math:`s` in one time step:

.. math::
   \Delta w = ds \cdot \sin(\Delta\phi)

The current heading error :math:`\Delta\phi` can further be substituted by an
expression of :math:`ds` and the current curvature :math:`\kappa` of the vehicle
(see :ref:`algorithm_lateral_detailed_description_control_heading` for that matter):

.. math::
   \Delta w = ds \cdot \sin(\arctan(ds \cdot \kappa))

Under the consideration of small angular changes in one time step, this equation
can be linearized to:

.. math::
   \Delta w = ds^2 \kappa

The curvature :math:`\kappa` of the vehicle can be substituted by an Ackermann model
(see :ref:`here<algorithm_lateral_detailed_description_lateral_dynamics>` for further information about that):

.. math::
   \Delta w = \frac{ds^2 \tan(\delta)}{l}

This equation can also be linearized under the consideration of small angles:

.. math::
   \Delta w = \frac{ds^2 \cdot \delta}{l}

The connection to the steering wheel angle :math:`\delta_{h,w}` can be applied
by the ratio :math:`i_s` of the vehicle's steering gear:

.. math::
   \Delta w = \frac{ds^2 \cdot \delta_{h,w}}{i_s \cdot l}

This equation can be transformed to a similar form as the controller's transfer
function above:

.. math:.
   \frac{i_s \cdot l}{ds^2} = \frac{\delta_{h,w}}{\Delta w}

The usage of the incremental difference :math:`ds` in this equation is problematic,
because the controller amplification becomes dependent on the simulation step
size. Because of this, the absolute change :math:`ds` of the longitudinal road
coordinate :math:`s` is substituted by the vehicle's absolute velocity :math:`v` as a
simplification, which is the change of travelled distance with respect to time:

.. math:.
   \frac{i_s \cdot l}{v^2} = \frac{\delta_{h,w}}{\Delta w}

To tune the absolute influence of the lateral deviation controller in the
overall control loop, an additional gain factor :math:`\text{Gain}_{\Delta w}` is
applied to this transfer function, which allows a situation dependent
amplification of the lateral deviation controller by the driver behavior model:

.. math::
   \delta_{h,w} = \frac{\text{Gain}_{\Delta w} \cdot i_s \cdot l \cdot \Delta w}{v^2}

The vehicle parameters :math:`l` and :math:`i_s` are received from the module :ref:`parameters_vehicle`, the vehicle's
absolute velocity :math:`v` is received from the module :ref:`sensor_driver`,
and the additional controller gain :math:`\text{Gain}_{\Delta w}` is received from a
driver behavior model.
