SUMO - Simulation of Urban MObility
MSCFModel_CACC.cpp
Go to the documentation of this file.
1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2001-2018 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
15 // CACC car-following model based on [1], [2].
16 // [1] Milanes, V., and S. E. Shladover. Handling Cut-In Vehicles in Strings
17 // of Cooperative Adaptive Cruise Control Vehicles. Journal of Intelligent
18 // Transportation Systems, Vol. 20, No. 2, 2015, pp. 178-191.
19 // [2] Xiao, L., M. Wang and B. van Arem. Realistic Car-Following Models for
20 // Microscopic Simulation of Adaptive and Cooperative Adaptive Cruise
21 // Control Vehicles. Transportation Research Record: Journal of the
22 // Transportation Research Board, No. 2623, 2017. (DOI: 10.3141/2623-01).
23 /****************************************************************************/
24 
25 // ===========================================================================
26 // included modules
27 // ===========================================================================
28 #include <config.h>
29 
30 #include <stdio.h>
31 #include <iostream>
32 
33 #include "MSCFModel_CACC.h"
34 #include <microsim/MSVehicle.h>
35 #include <microsim/MSLane.h>
37 #include <utils/common/SUMOTime.h>
39 #include <math.h>
40 #include <microsim/MSNet.h>
41 
42 // ===========================================================================
43 // debug flags
44 // ===========================================================================
45 #define DEBUG_CACC 0
46 #define DEBUG_COND (veh->isSelected())
47 
48 
49 // ===========================================================================
50 // defaults
51 // ===========================================================================
52 #define DEFAULT_SC_GAIN_CACC -0.4
53 #define DEFAULT_GCC_GAIN_GAP_CACC 0.005
54 #define DEFAULT_GCC_GAIN_GAP_DOT_CACC 0.05
55 #define DEFAULT_GC_GAIN_GAP_CACC 0.45
56 #define DEFAULT_GC_GAIN_GAP_DOT_CACC 0.0125
57 #define DEFAULT_CA_GAIN_GAP_CACC 0.45
58 #define DEFAULT_CA_GAIN_GAP_DOT_CACC 0.05
59 
60 // override followSpeed when deemed unsafe by the given margin (the value was selected to reduce the number of necessary interventions)
61 #define DEFAULT_EMERGENCY_OVERRIDE_THRESHOLD 2.0
62 
63 
64 // ===========================================================================
65 // method definitions
66 // ===========================================================================
68  MSCFModel(vtype), acc_CFM(MSCFModel_ACC(vtype)),
69  mySpeedControlGain(vtype->getParameter().getCFParam(SUMO_ATTR_SC_GAIN_CACC, DEFAULT_SC_GAIN_CACC)),
70  myGapClosingControlGainGap(vtype->getParameter().getCFParam(SUMO_ATTR_GCC_GAIN_GAP_CACC, DEFAULT_GCC_GAIN_GAP_CACC)),
71  myGapClosingControlGainGapDot(vtype->getParameter().getCFParam(SUMO_ATTR_GCC_GAIN_GAP_DOT_CACC, DEFAULT_GCC_GAIN_GAP_DOT_CACC)),
72  myGapControlGainGap(vtype->getParameter().getCFParam(SUMO_ATTR_GC_GAIN_GAP_CACC, DEFAULT_GC_GAIN_GAP_CACC)),
73  myGapControlGainGapDot(vtype->getParameter().getCFParam(SUMO_ATTR_GC_GAIN_GAP_DOT_CACC, DEFAULT_GC_GAIN_GAP_DOT_CACC)),
74  myCollisionAvoidanceGainGap(vtype->getParameter().getCFParam(SUMO_ATTR_CA_GAIN_GAP_CACC, DEFAULT_CA_GAIN_GAP_CACC)),
75  myCollisionAvoidanceGainGapDot(vtype->getParameter().getCFParam(SUMO_ATTR_CA_GAIN_GAP_DOT_CACC, DEFAULT_CA_GAIN_GAP_DOT_CACC)) {
77 }
78 
80 
81 
82 double
83 MSCFModel_CACC::followSpeed(const MSVehicle* const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle* const /*pred*/) const {
84 
85  const double desSpeed = MIN2(veh->getLane()->getSpeedLimit(), veh->getMaxSpeed());
86  const double vCACC = _v(veh, gap2pred, speed, predSpeed, desSpeed, true);
87  const double vSafe = maximumSafeFollowSpeed(gap2pred, speed, predSpeed, predMaxDecel);
88  if (vSafe + DEFAULT_EMERGENCY_OVERRIDE_THRESHOLD < vCACC) {
89  if DEBUG_COND {
91  std::cout << "\n";
92  std::cout << "Apply Safe speed"<< "\n";
93  std::cout << SIMTIME << " veh=" << veh->getID() << " v=" << speed << " vL=" << predSpeed << " gap=" << gap2pred << " vCACC=" << vCACC << " vSafe=" << vSafe << " cm=" << vars->CACC_ControlMode << "\n";
94  }
96  }
97  return vCACC;
98 }
99 
100 double
101 MSCFModel_CACC::stopSpeed(const MSVehicle* const veh, const double speed, double gap) const {
102 
103  // NOTE: This allows return of smaller values than minNextSpeed().
104  // Only relevant for the ballistic update: We give the argument headway=TS, to assure that
105  // the stopping position is approached with a uniform deceleration also for tau!=TS.
106  return MIN2(maximumSafeStopSpeed(gap, speed, false, veh->getActionStepLengthSecs()), maxNextSpeed(speed, veh));
107 }
108 
109 
111 double
112 MSCFModel_CACC::interactionGap(const MSVehicle* const /* veh */, double /* vL */) const {
113  /*maximum radar range is CACC is enabled*/
114  return 250;
115 }
116 
117 double MSCFModel_CACC::speedSpeedContol(const double speed, double vErr) const {
118  // Speed control law
119  double sclAccel = mySpeedControlGain*vErr;
120  double newSpeed = speed + ACCEL2SPEED(sclAccel);
121  return newSpeed;
122 }
123 
124 double MSCFModel_CACC::speedGapControl(const MSVehicle* const veh, const double gap2pred,
125  const double speed, const double predSpeed, const double desSpeed, double vErr) const {
126  // Gap control law
127  double newSpeed = 0.0;
128 
129  std::pair<const MSVehicle* const, double> leaderInfo = veh->getLeader(100);
130  if (leaderInfo.first ) {
131  if (leaderInfo.first->getCarFollowModel().getModelID() != SUMO_TAG_CF_CACC) {
132  //ACC control mode
133  newSpeed = acc_CFM._v(veh, gap2pred, speed, predSpeed, desSpeed, true);
134 
135  } else {
136  //CACC control mode
137  double desSpacing = myHeadwayTime * speed;
138  double gap = gap2pred - veh->getVehicleType().getMinGap();
139  double spacingErr = gap - desSpacing;
140  double accel = veh->getAcceleration();
141  double spacingErr1 = predSpeed - speed + myHeadwayTime * accel;
142 
143  if ((spacingErr > 0 && spacingErr < 0.2) && (vErr < 0.1)) {
144  // gap mode
145  //newSpeed = speed + 0.45 * spacingErr + 0.0125 *spacingErr1;
146  #if DEBUG_CACC == 1
147  if DEBUG_COND {
148  std::cout << " applying gap control" << std::endl;
149  }
150  #endif
151  newSpeed = speed + myGapControlGainGap * spacingErr + myGapControlGainGapDot *spacingErr1;
152  } else if (spacingErr < 0) {
153  // collision avoidance mode
154  //newSpeed = speed + 0.45 * spacingErr + 0.05 *spacingErr1;
155  #if DEBUG_CACC == 1
156  if DEBUG_COND {
157  std::cout << " applying collision avoidance" << std::endl;
158  }
159  #endif
160  newSpeed = speed + myCollisionAvoidanceGainGap * spacingErr + myCollisionAvoidanceGainGapDot *spacingErr1;
161  } else {
162  // gap closing mode
163  #if DEBUG_CACC == 1
164  if DEBUG_COND {
165  std::cout << " applying gap closing" << std::endl;
166  }
167  #endif
168  newSpeed = speed + myGapClosingControlGainGap * spacingErr + myGapClosingControlGainGapDot *spacingErr1;
169  }
170  }
171 
172  } else { /* no leader */
173  newSpeed = speedSpeedContol(speed, vErr);
174  }
175 
176  return newSpeed;
177 
178 }
179 
180 double
181 MSCFModel_CACC::_v(const MSVehicle* const veh, const double gap2pred, const double speed,
182  const double predSpeed, const double desSpeed, const bool /* respectMinGap */) const {
183 
184  double newSpeed = 0.0;
185 
186 #if DEBUG_CACC == 1
187  if DEBUG_COND {
188  std::cout << SIMTIME << " MSCFModel_CACC::_v() for veh '" << veh->getID() << "'\n"
189  << " gap=" << gap2pred << " speed=" << speed << " predSpeed=" << predSpeed
190  << " desSpeed=" << desSpeed << std::endl;
191  }
192 #endif
193 
194  /* Velocity error */
195  double vErr = speed - desSpeed;
196  int setControlMode = 0;
200  setControlMode = 1;
201  }
202 
203  double time_gap = gap2pred / speed;
204  if (time_gap > 2) {
205 #if DEBUG_CACC == 1
206  if DEBUG_COND {
207  std::cout << " applying speedControl" << std::endl;
208  }
209 #endif
210  // Find acceleration - Speed control law
211  newSpeed = speedSpeedContol(speed, vErr);
212  // Set cl to vehicle parameters
213  if (setControlMode) {
214  vars->CACC_ControlMode = 0;
215  }
216  } else if (time_gap < 1.5 ){
217  // Find acceleration - Gap control law
218  newSpeed = speedGapControl(veh, gap2pred, speed, predSpeed, desSpeed, vErr);
219  // Set cl to vehicle parameters
220  if (setControlMode) {
221  vars->CACC_ControlMode = 1;
222  }
223  } else {
224  // Follow previous applied law
225  int cm = vars->CACC_ControlMode;
226  if (!cm) {
227 
228 #if DEBUG_CACC == 1
229  if DEBUG_COND {
230  std::cout << " applying speedControl" << std::endl;
231  }
232 #endif
233  newSpeed = speedSpeedContol(speed, vErr);
234  } else {
235  newSpeed = speedGapControl(veh, gap2pred, speed, predSpeed, desSpeed, vErr);
236  }
237  }
238 
239 #if DEBUG_CACC == 1
240  if DEBUG_COND {
241  std::cout << " result: accel=" <<SPEED2ACCEL(newSpeed-speed) << " newSpeed=" << newSpeed << std::endl;
242 }
243 #endif
244 
245  return MAX2(0., newSpeed);
246 }
247 
248 
249 
250 MSCFModel*
252  return new MSCFModel_CACC(vtype);
253 }
254 
#define DEFAULT_EMERGENCY_OVERRIDE_THRESHOLD
double maximumSafeFollowSpeed(double gap, double egoSpeed, double predSpeed, double predMaxDecel, bool onInsertion=false) const
Returns the maximum safe velocity for following the given leader.
Definition: MSCFModel.cpp:854
#define DEBUG_COND
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:79
MSCFModel::VehicleVariables * getCarFollowVariables() const
Returns the vehicle&#39;s car following model variables.
Definition: MSVehicle.h:909
#define ACCEL2SPEED(x)
Definition: SUMOTime.h:54
MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:565
std::pair< const MSVehicle *const, double > getLeader(double dist=0) const
Returns the leader of the vehicle looking for a fixed distance.
Definition: MSVehicle.cpp:4740
The car-following model abstraction.
Definition: MSCFModel.h:57
virtual double maxNextSpeed(double speed, const MSVehicle *const veh) const
Returns the maximum speed given the current speed.
Definition: MSCFModel.cpp:239
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:165
T MAX2(T a, T b)
Definition: StdDefs.h:76
int CACC_ControlMode
The vehicle&#39;s CACC precious time step gap error.
#define DEFAULT_GC_GAIN_GAP_CACC
#define SPEED2ACCEL(x)
Definition: SUMOTime.h:56
The car-following model and parameter.
Definition: MSVehicleType.h:66
MSCFModel * duplicate(const MSVehicleType *vtype) const
Duplicates the car-following model.
#define SIMTIME
Definition: SUMOTime.h:65
double myCollisionAvoidanceGainGapDot
double getMaxSpeed() const
Returns the maximum speed.
double myGapClosingControlGainGapDot
double speedSpeedContol(const double speed, double vErr) const
#define DEFAULT_GCC_GAIN_GAP_CACC
double getActionStepLengthSecs() const
Returns the vehicle&#39;s action step length in secs, i.e. the interval between two action points...
Definition: MSVehicle.h:517
double interactionGap(const MSVehicle *const, double vL) const
Returns the maximum gap at which an interaction between both vehicles occurs.
The ACC car-following model.
Definition: MSCFModel_ACC.h:49
double getSpeedLimit() const
Returns the lane&#39;s maximum allowed speed.
Definition: MSLane.h:506
#define DEFAULT_SC_GAIN_CACC
MSCFModel_ACC acc_CFM
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
Definition: MSNet.h:263
double myCollisionAvoidanceGainGap
T MIN2(T a, T b)
Definition: StdDefs.h:70
double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0) const
Computes the vehicle&#39;s safe speed (no dawdling)
double getMinGap() const
Get the free space in front of vehicles of this class.
MSCFModel_CACC(const MSVehicleType *vtype)
Constructor.
double maximumSafeStopSpeed(double gap, double currentSpeed, bool onInsertion=false, double headway=-1) const
Returns the maximum next velocity for stopping within gap.
Definition: MSCFModel.cpp:709
const SUMOVTypeParameter & getParameter() const
double getCFParam(const SumoXMLAttr attr, const double defaultValue) const
Returns the named value from the map, or the default if it is not contained there.
#define DEFAULT_GC_GAIN_GAP_DOT_CACC
const MSVehicleType & getVehicleType() const
Returns the vehicle&#39;s type definition.
~MSCFModel_CACC()
Destructor.
double getAcceleration() const
Returns the vehicle&#39;s acceleration in m/s (this is computed as the last step&#39;s mean acceleration in c...
Definition: MSVehicle.h:500
double _v(const MSVehicle *const veh, const double gap2pred, const double mySpeed, const double predSpeed, const double desSpeed, const bool respectMinGap=true) const
#define DEFAULT_CA_GAIN_GAP_DOT_CACC
double myGapControlGainGap
#define DEFAULT_CA_GAIN_GAP_CACC
double myCollisionMinGapFactor
The factor of minGap that must be maintained to avoid a collision event.
Definition: MSCFModel.h:599
double _v(const MSVehicle *const veh, const double gap2pred, const double mySpeed, const double predSpeed, const double desSpeed, const bool respectMinGap=true) const
double myGapClosingControlGainGap
double myHeadwayTime
The driver&#39;s desired time headway (aka reaction time tau) [s].
Definition: MSCFModel.h:602
double speedGapControl(const MSVehicle *const veh, const double gap2pred, const double speed, const double predSpeed, const double desSpeed, double vErr) const
double mySpeedControlGain
const std::string & getID() const
Returns the name of the vehicle.
double myGapControlGainGapDot
double stopSpeed(const MSVehicle *const veh, const double speed, double gap2pred) const
Computes the vehicle&#39;s safe speed for approaching a non-moving obstacle (no dawdling) ...
#define DEFAULT_GCC_GAIN_GAP_DOT_CACC