Skip to content

Commit

Permalink
Merge pull request #33 from cathywu/stabilize
Browse files Browse the repository at this point in the history 
Stabilizes ACC and Midpoint controllers
  • Loading branch information
@mehtank
mehtank authored Sep 15, 2016
2 parents 8d5a067 + 14de45d commit 6aa4829
Show file tree
Hide file tree
Showing 2 changed files with 44 additions and 33 deletions.
52 changes: 31 additions & 21 deletions python/carfns.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,18 +47,18 @@ def carFn((idx, car), sim, step):
traci.vehicle.changeLane(car["id"], maxl, 10000)
return carFn

def ACCFnBuilder(follow_sec = 3.0, max_speed = 26.8, gain = 0.01):
def ACCFnBuilder(follow_sec = 3.0, max_speed = 26.8, gain = 0.01, beta = 0.5):
"""
Basic adaptive cruise control (ACC) controller
:param follow_sec:
:param follow_sec: number of seconds worth of following distance to keep from the front vehicle
:param max_speed: 26.8 m/s = 60 mph
:param gain:
:param gain: gain for tracking following distance
:param beta: gain for tracking speed of front vehicle
:return: ACCFn to input to a carParams
"""

def ACCFn((idx, car), sim, step):
"""
P controller
:param idx:
:param car:
:param sim:
Expand All @@ -70,7 +70,8 @@ def ACCFn((idx, car), sim, step):
# TODO(cathywu) Setting tau to any value seems to cause collisions
# traci.vehicle.setTau(vehID, 0.01)

if step < 250:
if step < sim.simSteps/2:
# changeFasterLaneBuilder()((idx, car), sim, step)
return

try:
Expand All @@ -89,24 +90,30 @@ def ACCFn((idx, car), sim, step):
# print delta, curr_speed, front_speed, curr_speed-front_speed
if follow_dist < front_dist and curr_speed < max_speed:
# speed up
new_speed = min(curr_speed + gain * delta, max_speed)
new_speed = min(curr_speed + beta * (front_speed-curr_speed) + gain * delta, max_speed)
traci.vehicle.slowDown(vehID, new_speed, 1000) # 2.5 sec
print step, "FASTER", curr_speed, new_speed, front_speed, delta, front_dist, follow_dist
print "t=%d, FASTER, %0.1f -> %0.1f (%0.1f) | d=%0.2f = %0.2f vs %0.2f" % \
(step, curr_speed, new_speed, front_speed, delta, front_dist, follow_dist)
elif follow_dist > front_dist:
# slow down
new_speed = max(curr_speed + gain * delta, 0)
new_speed = max(curr_speed + beta * (front_speed-curr_speed) + gain * delta, 0)
traci.vehicle.slowDown(vehID, new_speed, 1000) # 2.5 sec
print step, "SLOWER", curr_speed, new_speed, front_speed, delta, front_dist, follow_dist
print "t=%d, SLOWER, %0.1f -> %0.1f (%0.1f) | d=%0.2f = %0.2f vs %0.2f" % \
(step, curr_speed, new_speed, front_speed, delta, front_dist, follow_dist)

return ACCFn

def MidpointFnBuilder(max_speed = 26.8, gain = 0.01):
def MidpointFnBuilder(max_speed = 26.8, gain = 0.1, beta = 0.5, duration = 500, bias = 1.0, ratio = 0.5):
"""
Basic adaptive cruise control (ACC) controller
:param follow_sec:
:param max_speed: 26.8 m/s = 60 mph
:param gain:
:return: ACCFn to input to a carParams
:param gain: gain for tracking following distance
:param beta: gain for tracking speed of front vehicle
:param duration: duration for transitioning to new speed (ms)
:param bias: additive speed bias term (m/s)
:param ratio: ratio of distance between front and back vehicles to track
as following distance (default is 0.5=midpoint)
:return: MidpointFn as input to a carParams
"""

def MidpointFn((idx, car), sim, step):
Expand All @@ -119,7 +126,8 @@ def MidpointFn((idx, car), sim, step):
"""
vehID = car["id"]

if step < 250:
if step < sim.simSteps/2:
# changeFasterLaneBuilder()((idx, car), sim, step)
return

try:
Expand All @@ -133,18 +141,20 @@ def MidpointFn((idx, car), sim, step):

curr_speed = car["v"]
front_speed = front_car["v"]
follow_dist = (front_dist + back_dist)/2
follow_dist = (front_dist + back_dist) * ratio
delta = front_dist - follow_dist
# print delta, curr_speed, front_speed, curr_speed-front_speed
if follow_dist < front_dist and curr_speed < max_speed:
# speed up
new_speed = min(curr_speed + gain * delta, max_speed)
traci.vehicle.slowDown(vehID, new_speed, 1000) # 2.5 sec
print step, "FASTER", curr_speed, new_speed, front_speed, delta, front_dist, follow_dist
new_speed = min(curr_speed + beta * (front_speed-curr_speed) + gain * delta + bias, max_speed)
traci.vehicle.slowDown(vehID, new_speed, duration) # 2.5 sec
print "t=%d, FASTER, %0.1f -> %0.1f (%0.1f) | d=%0.2f = %0.2f vs %0.2f" % \
(step, curr_speed, new_speed, front_speed, delta, front_dist, follow_dist)
elif follow_dist > front_dist:
# slow down
new_speed = max(curr_speed + gain * delta, 0)
traci.vehicle.slowDown(vehID, new_speed, 1000) # 2.5 sec
print step, "SLOWER", curr_speed, new_speed, front_speed, delta, front_dist, follow_dist
new_speed = max(curr_speed + beta * (front_speed-curr_speed) + gain * delta + bias, 0)
traci.vehicle.slowDown(vehID, new_speed, duration) # 2.5 sec
print "t=%d, SLOWER, %0.1f -> %0.1f (%0.1f) | d=%0.2f = %0.2f vs %0.2f" % \
(step, curr_speed, new_speed, front_speed, delta, front_dist, follow_dist)

return MidpointFn
25 changes: 13 additions & 12 deletions python/loopsim.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -194,7 +194,7 @@ def simulate(self, opts):
tag = opts.get("tag", None)

paramsList = opts["paramsList"]
simSteps = opts.get("simSteps", 500)
self.simSteps = opts.get("simSteps", 500)

if self.label is None:
self.label = "-".join([x["name"] + "%03d" % x["count"]
Expand All @@ -206,7 +206,7 @@ def simulate(self, opts):
self._simInit("-" + self.label, [x["name"] for x in paramsList])
self._addTypes(paramsList)
self._addCars(paramsList)
self._run(simSteps)
self._run(self.simSteps)

def plot(self, show=True, save=False):
# Plot results
Expand All @@ -232,34 +232,35 @@ def plot(self, show=True, save=False):

humanParams = {
"name" : "human",
"count" : 30,
"count" : 25,
"maxSpeed" : 40,
"accel" : 4,
"decel" : 6,
"function" : changeFasterLaneBuilder(),
"accel" : 2.6,
"decel" : 4.5,
# "function" : randomChangeLaneFn,
# "function" : changeFasterLaneBuilder(),
"laneSpread" : 0,
"speedFactor" : 1.0,
"speedDev" : 0.1,
"sigma" : 0.5,
"tau" : 2,
"tau" : 4,
}

robotParams = {
"name" : "robot",
"count" : 10,
"count" : 25,
"maxSpeed" : 40,
"accel" : 4,
"decel" : 6,
"function" : MidpointFnBuilder(max_speed = 26.8, gain = 0.1),
# "function" : ACCFnBuilder(follow_sec = 3.0, max_speed = 26.8, gain = 0.1),
# "function" : MidpointFnBuilder(max_speed=40, gain=0.1, beta=0.9, duration=250, bias=1.0, ratio=0.25),
"function" : ACCFnBuilder(follow_sec=1.0, max_speed=40, gain=0.1, beta=0.9),
"laneSpread" : 0,
"tau" : 1,
"tau" : 0.5,
}

opts = {
"paramsList" : [humanParams, robotParams],
"simSteps" : 500,
"tag" : "Midpoint"
"tag" : "ACC"
}

defaults.SIM_STEP_LENGTH = 0.5
Expand Down

0 comments on commit 6aa4829

Please sign in to comment.