.. ******************************************************************************* 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_driver_: AlgorithmLateralDriver ###################### This module is responsible for the control of the vehicle's lateral dynamics. - :ref:`algorithm_lateral_driver_overview_section` - :ref:`algorithm_lateral_driver_detailed_description` It receives the command variables from a driver behaviour model like :ref:`implementation_architecture_` and generates the steering wheel angle of the driver to match these command variables. The steering wheel angle has than to be send to a vehicle dynamics module like :ref:`dynamics_regular_driving_`. .. _algorithm_lateral_driver_overview_section: Overview of the module's functionalities ======================================== AlgorithmLateralDriver models human lateral guidance according to the two-level-model of vehicle guidance from Donges. It is the original form of his later three-level-model of vehicle guidance, which is expressing driver behaviour in a more phenomenological context, and was developed as part of Donges' Ph.D. thesis. The model takes two overlapping mechanisms of human behaviour in human-machine-interactions into account: **Anticipatory open-loop control on guidance level:** Every driver has knowledge about how his vehicle works, which is based on driving experience and familiarity with the vehicle. This can be expressed as a mental model representation about the vehicle. If the driver approaches a curve on the road, he visually perceives the road's curvature and can use his knowledge about the vehicle to estimate, how much he has to turn the steering wheel, so the vehicle describes a curve of the same curvature. This is a typically open-loop control process, where the mental representation of the vehicle represents a feedforward control, which generates a steering wheel angle according to a needed curvature, but the actual curvature, which is produced by the vehicle, is not monitored. The better the mental model of the vehicle is, the better is this feedforward control, but even the best mental model cannot take into account external disturbances like cross slope or side wind. For influences like this, a second underlying control mechanism is needed. **Compensatory closed-loop control on stabilization level:** As already mentioned above, the guidance level cannot take into account any external disturbance, because an open-loop control does not monitor the difference between command variable and its corresponding system state. For this reason (and to compensate the driver's necessarily imperfect mental model about his vehicle), an underlying closed-loop control is needed. For instance, a driver is driving on a straight road, which means that the required curvature is equal zero. In this case, the guidance level will also produce a steering wheel angle equal zero. If an external disturbance like side wind occurs, the states of the anticipatory level will not change, because the curvature of the road is still zero. The vehicle will of cause start to deviate from its lane, which must be counter-steered by a closed-loop control that monitors the actual guidance errors towards the wished trajectory. In case of lateral guidance, the error in heading angle and the lateral deviation from the trajectory are addressed in this model. :ref:`image_PlaneDrivingKinematics` illustrates the above mentioned lateral guidance values of the vehicle in the road coordinate system for lane keeping. For a specific trajectory (i.e. a lane change), these values has to be expressed according to this trajectory, which means that the command variables are no longer equal zero. .. _image_PlaneDrivingKinematics: .. figure:: _static/images/PlaneDrivingKinematics.png :alt: Lateral guidance values of the vehicle in the road coordinate system Lateral guidance values of the vehicle in the road coordinate system :ref:`image_LateralGuidanceErrorsVisually` is taken from Donges Ph.D. thesis and illustrates, how a human driver is able to visually perceive the guidance errors. .. _image_LateralGuidanceErrorsVisually: .. figure:: _static/images/LateralGuidanceErrorsVisually.png :alt: Visual perception of lateral guidance errors according to Donges Visual perception of lateral guidance errors according to Donges .. _algorithm_lateral_driver_detailed_description: Detailed description of the module's features ============================================= AlgorithmLateralDriver receives its command variables from a driver behaviour model like :ref:`implementation_architecture_` 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 behaviour model. This overall control loop of vehicle lateral guidance involving AlgorithmLateralDriver is illustrated in :numref:`image_LateralControllerSignalFlow` 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 To enable small turn radiuses e.g. for cornering in city scenarios the driver has to anticipate the road geometry. This is achieved using a mechanism akin to the Salvucci controler. 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 When the driver is actively trying not to follow its lanes geometry e.g. when changing lanes another component: * :math:`\kappa_{manoeuvre}` containing the curvature of the planned trajectory relative to the road Other variables shown in :numref:`image_LateralControllerSignalFlow`: * :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_driver_detailed_description_anticipation: Anticipatory control on guidance level -------------------------------------- As described in the :ref:`overview section`, the driver's controls on guidance level are characterized by anticipatory mechanisms regarding the expected vehicle's behaviour. In terms of control theory and lateral guidance, this is modelled as an open-loop feedforward controller, which generates an estimated steering wheel angle :math:`\delta_\kappa` that fits a required curvature :math:`\kappa_{target}` from the driver behaviour model. This model-predictive controller is modelled as an inverted Ackermann model. The Ackermann model 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 :numref:`image_AckermannModel` 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 axes * :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.png :target: AckermannModel.png :alt: Illustration of the Ackermann model Illustration of the Ackermann model In accordance to the current definitions of :ref:`coordinate systems `, the coordinate reference point of the vehicle is considered to be the rear axle. Therefore, the curvature of the vehicle is also expressed towards the rear axle and not to the centre of gravity, which is why the centre of gravity is not depicted in :numref:`image_AckermannModel`. The equation derived from the Ackermann model in :numref:`image_AckermannModel` 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_agent_`. .. _algorithm_lateral_driver_detailed_description_control: Compensatory control on stabilization level ------------------------------------------- As described in the :ref:`overview section`, the driver's controls on stabilization level are characterized by compensatory mechanisms regarding :ref:`imperfect anticipation of the vehicle's behaviour` and external disturbances on the real vehicle. In terms of control theory and lateral guidance, this is modelled as two closed-loop controllers, which generate steering wheel angles :math:`\delta_{h,\phi}` and :math:`\delta_{h,w}` that compensate the lateral guidance errors :math:`\Delta\phi` (heading error) and :math:`\Delta` w (lateral deviation). These controllers are derived from the kinematical relations between the lateral guidance values curvature :math:`\kappa`, heading angle :math:`\phi`, and lateral position w, according to the plane driving kinematics of Kramer, and are described below. .. _algorithm_lateral_driver_detailed_description_control_heading: Heading controller ~~~~~~~~~~~~~~~~~~ Similar to Donges' two-level-model of vehicle guidance, 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` 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 from the simulation step size. Because of this, the absolute change 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 behaviour 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_agent_`, 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 behaviour model like :ref:`implementation_architecture_`. .. _algorithm_lateral_driver_detailed_description_control_deviation: Lateral deviation controller ~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Similar to Donges' two-level-model of vehicle guidance, 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_driver_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` 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 ds in this equation is problematic, because the controller amplification becomes dependent from 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 behaviour 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_agent_`, 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 behaviour model like :ref:`implementation_architecture_`.