Line data    Source code

       1             : /****************************************************************************/
       2             : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
       3             : // Copyright (C) 2001-2024 German Aerospace Center (DLR) and others.
       4             : // This program and the accompanying materials are made available under the
       5             : // terms of the Eclipse Public License 2.0 which is available at
       6             : // https://www.eclipse.org/legal/epl-2.0/
       7             : // This Source Code may also be made available under the following Secondary
       8             : // Licenses when the conditions for such availability set forth in the Eclipse
       9             : // Public License 2.0 are satisfied: GNU General Public License, version 2
      10             : // or later which is available at
      11             : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
      12             : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
      13             : /****************************************************************************/
      14             : /// @file    MSCFModel_ACC.cpp
      15             : /// @author  Kallirroi Porfyri
      16             : /// @date    Feb 2018
      17             : ///
      18             : // ACC car-following model based on [1], [2].
      19             : // [1] Milanes, V., and S. E. Shladover. Handling Cut-In Vehicles in Strings
      20             : //    of Cooperative Adaptive Cruise Control Vehicles. Journal of Intelligent
      21             : //     Transportation Systems, Vol. 20, No. 2, 2015, pp. 178-191.
      22             : // [2] Xiao, L., M. Wang and B. van Arem. Realistic Car-Following Models for
      23             : //    Microscopic Simulation of Adaptive and Cooperative Adaptive Cruise
      24             : //     Control Vehicles. Transportation Research Record: Journal of the
      25             : //     Transportation Research Board, No. 2623, 2017. (DOI: 10.3141/2623-01).
      26             : //[3]  Xiao, L., Wang, M., Schakel, W., & van Arem, B. (2018). Unravelling
      27             : //    effects of cooperative adaptive cruise control deactivation on
      28             : //    traffic flow characteristics at merging bottlenecks. Transportation
      29             : //    Research Part C: Emerging Technologies, 96, 380–397.
      30             : //    <https://doi.org/10.1016/j.trc.2018.10.008>
      31             : /****************************************************************************/
      32             : #include <config.h>
      33             : 
      34             : #include <stdio.h>
      35             : #include <iostream>
      36             : 
      37             : #include "MSCFModel_ACC.h"
      38             : #include <microsim/MSVehicle.h>
      39             : #include <microsim/MSLane.h>
      40             : #include <utils/common/RandHelper.h>
      41             : #include <utils/common/SUMOTime.h>
      42             : #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
      43             : #include <math.h>
      44             : #include <microsim/MSNet.h>
      45             : 
      46             : // ===========================================================================
      47             : // debug flags
      48             : // ===========================================================================
      49             : //#define DEBUG_ACC
      50             : //#define DEBUG_COND (true)
      51             : //#define DEBUG_COND (veh->isSelected())
      52             : 
      53             : 
      54             : // ===========================================================================
      55             : // defaults
      56             : // ===========================================================================
      57             : #define DEFAULT_SC_GAIN -0.4
      58             : #define DEFAULT_GCC_GAIN_SPEED 0.8
      59             : #define DEFAULT_GCC_GAIN_SPACE 0.04
      60             : #define DEFAULT_GC_GAIN_SPEED 0.07
      61             : #define DEFAULT_GC_GAIN_SPACE 0.23
      62             : #define DEFAULT_CA_GAIN_SPACE 0.8
      63             : #define DEFAULT_CA_GAIN_SPEED 0.23
      64             : 
      65             : // ===========================================================================
      66             : // thresholds
      67             : // ===========================================================================
      68             : #define GAP_THRESHOLD_SPEEDCTRL 120
      69             : #define GAP_THRESHOLD_GAPCTRL 100
      70             : // override followSpeed when deemed unsafe by the given margin (the value was selected to reduce the number of necessary interventions)
      71             : #define DEFAULT_EMERGENCY_OVERRIDE_THRESHOLD 2.0
      72             : 
      73             : /// @todo: add attributes for myCollisionAvoidanceGainSpeed and myCollisionAvoidanceGainSpace
      74             : 
      75             : // ===========================================================================
      76             : // method definitions
      77             : // ===========================================================================
      78         734 : MSCFModel_ACC::MSCFModel_ACC(const MSVehicleType* vtype) :
      79             :     MSCFModel(vtype),
      80         734 :     mySpeedControlGain(vtype->getParameter().getCFParam(SUMO_ATTR_SC_GAIN, DEFAULT_SC_GAIN)),
      81         734 :     myGapClosingControlGainSpeed(vtype->getParameter().getCFParam(SUMO_ATTR_GCC_GAIN_SPEED, DEFAULT_GCC_GAIN_SPEED)),
      82         734 :     myGapClosingControlGainSpace(vtype->getParameter().getCFParam(SUMO_ATTR_GCC_GAIN_SPACE, DEFAULT_GCC_GAIN_SPACE)),
      83         734 :     myGapControlGainSpeed(vtype->getParameter().getCFParam(SUMO_ATTR_GC_GAIN_SPEED, DEFAULT_GC_GAIN_SPEED)),
      84         734 :     myGapControlGainSpace(vtype->getParameter().getCFParam(SUMO_ATTR_GC_GAIN_SPACE, DEFAULT_GC_GAIN_SPACE)),
      85         734 :     myCollisionAvoidanceGainSpeed(vtype->getParameter().getCFParam(SUMO_ATTR_CA_GAIN_SPEED, DEFAULT_CA_GAIN_SPEED)),
      86         734 :     myCollisionAvoidanceGainSpace(vtype->getParameter().getCFParam(SUMO_ATTR_CA_GAIN_SPACE, DEFAULT_CA_GAIN_SPACE)),
      87        1468 :     myEmergencyThreshold(vtype->getParameter().getCFParam(SUMO_ATTR_CA_OVERRIDE, DEFAULT_EMERGENCY_OVERRIDE_THRESHOLD)) {
      88             :     // ACC does not drive very precise and often violates minGap
      89         734 :     myCollisionMinGapFactor = vtype->getParameter().getCFParam(SUMO_ATTR_COLLISION_MINGAP_FACTOR, 0.1);
      90         734 : }
      91             : 
      92        1025 : MSCFModel_ACC::~MSCFModel_ACC() {}
      93             : 
      94             : 
      95             : double
      96    32945021 : MSCFModel_ACC::followSpeed(const MSVehicle* const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle* const /*pred*/, const CalcReason /*usage*/) const {
      97    32945021 :     const double desSpeed = MIN2(veh->getLane()->getSpeedLimit(), veh->getMaxSpeed());
      98    32945021 :     const double vACC = _v(veh, gap2pred, speed, predSpeed, desSpeed, true);
      99    32945021 :     const double vSafe = maximumSafeFollowSpeed(gap2pred, speed, predSpeed, predMaxDecel);
     100    32945021 :     if (vSafe + myEmergencyThreshold < vACC) {
     101             :         //ACCVehicleVariables* vars = (ACCVehicleVariables*)veh->getCarFollowVariables();
     102             :         //std::cout << SIMTIME << " veh=" << veh->getID() << " v=" << speed << " vL=" << predSpeed << " gap=" << gap2pred << " vACC=" << vACC << " vSafe=" << vSafe << " cm=" << vars->ACC_ControlMode << "\n";
     103      321220 :         return vSafe + myEmergencyThreshold;
     104             :     }
     105             :     return vACC;
     106             : }
     107             : 
     108             : 
     109             : double
     110    11596601 : MSCFModel_ACC::stopSpeed(const MSVehicle* const veh, const double speed, double gap, double decel, const CalcReason /*usage*/) const {
     111             :     // NOTE: This allows return of smaller values than minNextSpeed().
     112             :     // Only relevant for the ballistic update: We give the argument headway=TS, to assure that
     113             :     // the stopping position is approached with a uniform deceleration also for tau!=TS.
     114    11596601 :     return MIN2(maximumSafeStopSpeed(gap, decel, speed, false, veh->getActionStepLengthSecs()), maxNextSpeed(speed, veh));
     115             : }
     116             : 
     117             : 
     118             : double
     119    27487591 : MSCFModel_ACC::getSecureGap(const MSVehicle* const /*veh*/, const MSVehicle* const /*pred*/, const double speed, const double leaderSpeed, const double /* leaderMaxDecel */) const {
     120             :     // Accel in gap mode should vanish:
     121             :     //      0 = myGapControlGainSpeed * (leaderSpeed - speed) + myGapControlGainSpace * (g - myHeadwayTime * speed);
     122             :     // <=>  myGapControlGainSpace * g = - myGapControlGainSpeed * (leaderSpeed - speed) + myGapControlGainSpace * myHeadwayTime * speed;
     123             :     // <=>  g = - myGapControlGainSpeed * (leaderSpeed - speed) / myGapControlGainSpace + myHeadwayTime * speed;
     124    27487591 :     return myGapControlGainSpeed * (speed - leaderSpeed) / myGapControlGainSpace + myHeadwayTime * speed;
     125             : }
     126             : 
     127             : 
     128             : double
     129      162457 : MSCFModel_ACC::insertionFollowSpeed(const MSVehicle* const v, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle* const /*pred*/) const {
     130             : //#ifdef DEBUG_ACC
     131             : //        std::cout << "MSCFModel_ACC::insertionFollowSpeed(), speed="<<speed<< std::endl;
     132             : //#endif
     133             :     // iterate to find a stationary value for
     134             :     //    speed = followSpeed(v, speed, gap2pred, predSpeed, predMaxDecel, nullptr, CalcReason::FUTURE)
     135             :     const int max_iter = 50;
     136             :     int n_iter = 0;
     137             :     const double tol = 0.1;
     138             :     const double damping = 0.1;
     139             : 
     140             :     double res = speed;
     141     5026947 :     while (n_iter < max_iter) {
     142             :         // proposed acceleration
     143     4991455 :         const double a = SPEED2ACCEL(followSpeed(v, res, gap2pred, predSpeed, predMaxDecel, nullptr, CalcReason::FUTURE) - res);
     144     4991455 :         res = res + damping * a;
     145             : //#ifdef DEBUG_ACC
     146             : //        std::cout << "   n_iter=" << n_iter << ", a=" << a << ", res=" << res << std::endl;
     147             : //#endif
     148     4991455 :         if (fabs(a) < tol) {
     149             :             break;
     150             :         } else {
     151     4864490 :             n_iter++;
     152             :         }
     153             :     }
     154      162457 :     return res;
     155             : }
     156             : 
     157             : 
     158             : /// @todo update interactionGap logic
     159             : double
     160           0 : MSCFModel_ACC::interactionGap(const MSVehicle* const /*veh */, double /* vL */) const {
     161             :     /*maximum radar range is ACC is enabled*/
     162           0 :     return 250;
     163             : }
     164             : 
     165    14037888 : double MSCFModel_ACC::accelSpeedControl(double vErr) const {
     166             :     // Speed control law
     167    14037888 :     return mySpeedControlGain * vErr;
     168             : }
     169             : 
     170             : double
     171    18960021 : MSCFModel_ACC::accelGapControl(const MSVehicle* const /* veh */, const double gap2pred, const double speed, const double predSpeed, double vErr) const {
     172             :     // Gap control law
     173             :     double gclAccel = 0.0;
     174    18960021 :     const double deltaVel = predSpeed - speed;
     175             : 
     176             :     // see dynamic gap margin definition from (Xiao et. al, 2018)[3], equation 5 reformulated as min/max to avoid discontinuities
     177    18960021 :     const double d0 = MAX2(0., MIN2(75. / speed - 5., 2.));
     178             :     // this is equation 4, gap2pred is the difference in vehicle positions minus the length
     179    18960021 :     const double spacingErr = gap2pred - myHeadwayTime * speed - d0;
     180             : 
     181             : 
     182    18960021 :     if (fabs(spacingErr) < 0.2 && fabs(vErr) < 0.1) {
     183             :         // gap mode
     184        6385 :         gclAccel = myGapControlGainSpeed * deltaVel + myGapControlGainSpace * spacingErr;
     185             : #ifdef DEBUG_ACC
     186             :         if (DEBUG_COND) {
     187             :             std::cout << "        applying gap control:  spacingErr=" << spacingErr << " speedErr=" << vErr << std::endl;
     188             :         }
     189             : #endif
     190    18953636 :     } else if (spacingErr < 0)  {
     191             :         // collision avoidance mode
     192     3586322 :         gclAccel = myCollisionAvoidanceGainSpeed * deltaVel + myCollisionAvoidanceGainSpace * spacingErr;
     193             : #ifdef DEBUG_ACC
     194             :         if (DEBUG_COND) {
     195             :             std::cout << "        applying collision avoidance:  spacingErr=" << spacingErr << " speedErr=" << vErr << std::endl;
     196             :         }
     197             : #endif
     198             :     } else {
     199             :         // gap closing mode
     200    15367314 :         gclAccel = myGapClosingControlGainSpeed * deltaVel + myGapClosingControlGainSpace * spacingErr;
     201             : #ifdef DEBUG_ACC
     202             :         if (DEBUG_COND) {
     203             :             std::cout << "        applying gap closing:  spacingErr=" << spacingErr << " speedErr=" << vErr << std::endl;
     204             :         }
     205             : #endif
     206             :     }
     207    18960021 :     return gclAccel;
     208             : }
     209             : 
     210             : 
     211             : double
     212    32997909 : MSCFModel_ACC::_v(const MSVehicle* const veh, const double gap2pred, const double speed,
     213             :                   const double predSpeed, const double desSpeed, const bool /* respectMinGap */) const {
     214             : 
     215             :     double accelACC = 0;
     216             :     double gapLimit_SC = GAP_THRESHOLD_SPEEDCTRL; // lower gap limit in meters to enable speed control law
     217             :     double gapLimit_GC = GAP_THRESHOLD_GAPCTRL; // upper gap limit in meters to enable gap control law
     218             : 
     219             : #ifdef DEBUG_ACC
     220             :     if (DEBUG_COND) {
     221             :         std::cout << SIMTIME << " MSCFModel_ACC::_v() for veh '" << veh->getID() << "'\n"
     222             :                   << "        gap=" << gap2pred << " speed="  << speed << " predSpeed=" << predSpeed
     223             :                   << " desSpeed=" << desSpeed << " tau=" << myHeadwayTime << std::endl;
     224             :     }
     225             : #endif
     226             : 
     227             : 
     228             :     /* Velocity error */
     229    32997909 :     double vErr = speed - desSpeed;
     230             :     int setControlMode = 0;
     231             :     ACCVehicleVariables* vars = (ACCVehicleVariables*) veh->getCarFollowVariables();
     232    32997909 :     if (vars->lastUpdateTime != MSNet::getInstance()->getCurrentTimeStep()) {
     233     4235246 :         vars->lastUpdateTime = MSNet::getInstance()->getCurrentTimeStep();
     234             :         setControlMode = 1;
     235             :     }
     236    32997909 :     if (gap2pred > gapLimit_SC) {
     237             : 
     238             : #ifdef DEBUG_ACC
     239             :         if (DEBUG_COND) {
     240             :             std::cout << "        applying speedControl" << std::endl;
     241             :         }
     242             : #endif
     243             :         // Find acceleration - Speed control law
     244    14037521 :         accelACC = accelSpeedControl(vErr);
     245             :         // Set cl to vehicle parameters
     246    14037521 :         if (setControlMode) {
     247       20333 :             vars->ACC_ControlMode = 0;
     248             :         }
     249    18960388 :     } else if (gap2pred < gapLimit_GC) {
     250             :         // Find acceleration - Gap control law
     251    17966769 :         accelACC = accelGapControl(veh, gap2pred, speed, predSpeed, vErr);
     252             :         // Set cl to vehicle parameters
     253    17966769 :         if (setControlMode) {
     254     4214330 :             vars->ACC_ControlMode = 1;
     255             :         }
     256             :     } else {
     257             :         // Follow previous applied law
     258      993619 :         int cm = vars->ACC_ControlMode;
     259      993619 :         if (!cm) {
     260             : 
     261             : #ifdef DEBUG_ACC
     262             :             if (DEBUG_COND) {
     263             :                 std::cout << "        applying speedControl" << std::endl;
     264             :             }
     265             : #endif
     266         367 :             accelACC = accelSpeedControl(vErr);
     267             :         } else {
     268      993252 :             accelACC = accelGapControl(veh, gap2pred, speed, predSpeed, vErr);
     269             :         }
     270             : 
     271             :     }
     272             : 
     273    32997909 :     double newSpeed = speed + ACCEL2SPEED(accelACC);
     274             : 
     275             : #ifdef DEBUG_ACC
     276             :     if (DEBUG_COND) {
     277             :         std::cout << "        result: accel=" << accelACC << " newSpeed="  << newSpeed << std::endl;
     278             :     }
     279             : #endif
     280             : 
     281    32997909 :     return MAX2(0., newSpeed);
     282             : }
     283             : 
     284             : 
     285             : MSCFModel*
     286           0 : MSCFModel_ACC::duplicate(const MSVehicleType* vtype) const {
     287           0 :     return new MSCFModel_ACC(vtype);
     288             : }