From 4625d69e6a952d6b728c9993c37f10c91c4289cd Mon Sep 17 00:00:00 2001
From: Steven Lange <langestx@informatik.hu-berlin.de>
Date: Fri, 15 Dec 2017 15:54:40 +0100
Subject: [PATCH] CACC Code Clean Up
---
modules/CACC/ACC-module_v0.01_Flowchart.txt | 108 --------------------
modules/CACC/CACC-Module-Test.py | 40 +++++---
modules/CACC/car.py | 61 ++++-------
modules/CACC/platoonSimulation.py | 1 -
modules/CACC/simlifiedSpeedVisualization.py | 62 +++--------
5 files changed, 59 insertions(+), 213 deletions(-)
delete mode 100644 modules/CACC/ACC-module_v0.01_Flowchart.txt
diff --git a/modules/CACC/ACC-module_v0.01_Flowchart.txt b/modules/CACC/ACC-module_v0.01_Flowchart.txt
deleted file mode 100644
index 98ff8104..00000000
--- a/modules/CACC/ACC-module_v0.01_Flowchart.txt
+++ /dev/null
@@ -1,108 +0,0 @@
-state(n) = {
- IPD(n),
- PS(n),
- v_z(n),
- d_z(n),
- v_c(n),
- d_c(n),
- b(n) = { }
- }
-
-state(n+1) = {
- IPD(n+1),
- PS(n+1),
- v_z(n+1),
- d_z(n+1),
- v_c(n+1),
- d_c(n+1),
- b(n+1) = { }
- }
-
-ustate(n+1) = {
- u(IPD(n+1)),
- u(PS(n+1)),
- v_z(n+1),
- d_z(n+1),
- u(v_c(n+1)),
- u(d_c(n+1)),
- b(n+1) = { }
- }
-
-while(true):
- # 1. update
- update()
- # 2. (neue) Ziele für das eigene Auto definieren)
- define()
- # 3. (neue) Ziele umsetzen
- set()
-end
-
-update():
- #updates von Sensor holen und nach ustate(n+1) schreiben
-end
-
-define():
- # aktueller Abstand zum Vorgänger entspricht der IPD (aka der Kolonenabstand wir mit Toleranz gehalten)
- if u(IPD(n+1) -t <= u(d_c(n+1)) <= u(IPD(n+1)) +t:
- # Das Zielfahrzeug ist da, wo es erwartet wurde, und das eigene FZ hält die IPD
- if d_c(n+1) -t <= u(d_c(n+1)) <= d_c(n+1) +t:
- # Von der Distanz her perfekt
- # d_z(n+2) - mit IPS vergleichen und pot. halten
- # v_z(n+2) - mit PS vergleichen und pot. halten
-
- # Das Zielfahrzeug ist dichter als es erwartet wurde und das eigene FZ hält die richtige IPD
- elif d_c(n+1) -t > u(d_c(n+1)):
- #
-
- # Das Zielfahrzeug ist weiter entfernt als es erwartet wurde und das eigene FZ hält die richtige IPD
- else:
- #
-
-
- # aktueller Abstand zum Vorgänger unterschreitet die IPD
- elif u(IPD(n+1)) -t > u(d_c(n+1)):
- # Das Zielfahrzeug ist da, wo es erwartet wurde, und das eigene FZ unterschreitet die IPD
- if d_c(n+1) -t <= u(d_c(n+1)) <= d_c(n+1) +t
- # IPD wurde geändert oder Fall wird bereits behandelt
- # d_z(n+2) - mit IPD vergleichen
- # v_z(n+2) - mit PS vergleichen
-
- # Das Zielfahrzueg ist dichter als es erwartet wurde und das eigene FZ unterscheitet die IPD
- elif d_c(n+1) -t > u(d_c(n+1)):
- # Zu dicht am Vorgänger der zu dem noch langsamer wird
- # d_z(n+2) - mit IPD vergleich und pot. vergrössern
- # v_z(n+2) - mit v_z(n+1), v_c(n) vergleichen und pot. senken
-
- # Das Zielfharzeug ist weiter entfernt als erwartet wurde und das eigene FZ unterschreitet die IPD
- else:
- # Das ZF beschleunigt und vergössert den Abstand
- # d_z(n+2) erhöhen und zusammen mit aktueller Änderung anpassen
- # v_z(n+2) halten (pot. mit v_c(n) und v_z(n+1) anpassen)
-
-
- # aktueller Abstand zum Vorgänger überschreitet die IPD
- else:
- # Das Zielfahrzeug ist da, wo es erwartet wurde, und das eigene FZ überschreitet die IPD
- if d_c(n+1) -t <= u(d_c(n+1)) <= d_c(n+1) +t:
- # IPD wurde geändert oder der Fall wird bereits behandelt
- # d_z(n+2) - mit IPD vergleichen
- # v_z(n+2) - mit PS vergleichen
-
- # Das Zielfahrzeug ist dichter als erwartet wurde und das eigene FZ überschreitet die IPD
- elif d_c(n+1) -t > u(d_c(n+1)):
- # Das ZFZ ist langsamer geworden
- # d_z(n+2) - ist zu gross, wenn das FZ ausserhalb des IPDs fährt (Vgl. mit d_c(n), da die Entfernung kleiner geworden ist
- # v_z(n+2) - mit PS vergleichen, aber erstmal beibehalten
-
- # Das Zielfahrzeug ist weiter entfernt als erwartet wurde und das eigene FZ überschreitet die IPD
- else:
- # Das ZFZ ist schneller geworden
- # d_z(n+2) - ist zu gross, wenn FZ ausserhalb des IPDs fährt
- # v_z(n+2) - zu klein
- end
-end
-
-set():
- # Aus Ziel-Geschwindigkeit, Ziel-Entfernung, aktuelle Geschwindigkeit und aktueller Entfernung eine Beschleunigung finden
-end
-
diff --git a/modules/CACC/CACC-Module-Test.py b/modules/CACC/CACC-Module-Test.py
index 3de0a30d..ffcb87b4 100644
--- a/modules/CACC/CACC-Module-Test.py
+++ b/modules/CACC/CACC-Module-Test.py
@@ -7,7 +7,8 @@ from car import Car
class UnderCIPDerror(Exception):
pass
-def checkInbound(argument, target, delta): #Ueberall ist ein delta von 0.01 hinterlegt
+def checkInbound(argument, target, delta):
+ # Checking, if target - delta <= agrument <= target + delta is true
if argument > target + delta:
return False
elif argument < target - delta:
@@ -21,6 +22,7 @@ def broadcastPlattonSettings(carList, newIPD, newPS):
item.IPD = newIPD
def update(carList):
+ # updates the simulation. E.g. new car postion, new distance, ...
for item in carList:
item.updatePos()
for item in reversed(carList):
@@ -31,29 +33,30 @@ if __name__ == "__main__":
setIPD = [40, 40, 40, 60, 45, 30, 15] # event of IPD (activ until time event)
setPS = [20, 30, 10, 10, 10, 10, 10] # event of PS (activ until time event)
- v = [52.0, 50.0, 10.0] # startvektor (Startgeschwindikeit)
- pos = [90, 30, 0] # startvektor (Startposition)
- tps = 60
+ v = [52.0, 50.0, 10.0] # start vector (Velocity)
+ pos = [90, 30, 0] # start vector (Position)
+ tps = 60 # ticks per second
cars = []
cars.append(Car(None, v[0], pos[0], setIPD[0], setPS[0],tps))
cars.append(Car(cars[0], v[1], pos[1], setIPD[0], setPS[0],tps))
cars.append(Car(cars[1], v[2], pos[2], setIPD[0], setPS[0],tps))
- t = []
- IPD= []
- PS = []
+ t = [] # logging plot argument
+ IPD= [] # logging Inner Platoon Distance
+ PS = [] # logging Platoon Speed
- y_pos = [[],[],[]]
- y_speed = [[],[],[]]
- y_cSpeed = [[],[],[]]
- y_dist = [[],[],[]]
- y_cDist = [[],[],[]]
- y_cIPD = [[],[],[]]
- y_v_v = [[],[],[]]
+ y_pos = [[],[],[]] # real position
+ y_speed = [[],[],[]] # est. velocity
+ y_cSpeed = [[],[],[]] # real velocity
+ y_dist = [[],[],[]] # est. distance
+ y_cDist = [[],[],[]] # real distance
+ y_cIPD = [[],[],[]] # crit distance
+ y_v_v = [[],[],[]] # est. velocity of prev. platoon mamber
i = 0.0; end = False
for j in range(0, len(maxT)):
+ # simulates changes broadcasted by the LV
newIPD = setIPD[j]
newPS = setPS[j]
broadcastPlattonSettings(cars, newIPD, newPS)
@@ -75,6 +78,8 @@ if __name__ == "__main__":
y_cIPD[k].append(cars[k].getCIPD())
i = i+1/tps
+
+ # if any car ist below cIPD, the simulation stops.
try:
update(cars)
except UnderCIPDerror:
@@ -85,13 +90,14 @@ if __name__ == "__main__":
plt.subplot(221)
+ plt.title("Position")
plt.plot(t, y_pos[0], 'r', label='pos LV')
plt.plot(t, y_pos[1], 'b', label='pos FV1')
plt.plot(t, y_pos[2], 'g', label='pos FV2')
- plt.title("Position")
plt.legend()
plt.subplot(222)
+ plt.title("Distance")
plt.plot(t, IPD, 'r--', label='IPD')
plt.plot(t, y_dist[1], 'b-', label='assumed dist FV1')
plt.plot(t, y_cDist[1], 'b--', label='real dist FV1')
@@ -99,20 +105,20 @@ if __name__ == "__main__":
plt.plot(t, y_dist[2], 'g-', label='assumed dist FV1')
plt.plot(t, y_cDist[2], 'g--', label='real dist FV1')
plt.plot(t, y_cIPD[2], 'g-.', label='cIPD')
- plt.title("Distance")
plt.legend()
plt.subplot(223)
+ plt.title("Speed - LV-FV1")
plt.plot(t, PS, 'm--', label='PS')
# plt.plot(t, y_speed[0], 'r-', label='assumed speed LV')
plt.plot(t, y_cSpeed[0], 'r--', label='real speed LV')
plt.plot(t, y_v_v[1], 'b:', label='speed estiamtion FV1 for LV')
# plt.plot(t, y_speed[1], 'b-', label='assumed speed FV1')
# plt.plot(t, y_cSpeed[1], 'b--', label='real speed FV1')
- plt.title("Speed")
plt.legend()
plt.subplot(224)
+ plt.title("Speed - FV1-FV2")
plt.plot(t, PS, 'm--', label='PS')
# plt.plot(t, y_speed[1], 'b-', label='assumed speed FV1')
plt.plot(t, y_cSpeed[1], 'b--', label='real speed FV1')
diff --git a/modules/CACC/car.py b/modules/CACC/car.py
index 38ed2212..62ce856b 100644
--- a/modules/CACC/car.py
+++ b/modules/CACC/car.py
@@ -6,23 +6,29 @@ from kalmanfilter import KalmanFilter
class Car(object):
def __init__(self, prev, speed, position, IPD, PS, tps):
- self.platoonPrev = prev
- self.cPos = position
+ self.platoonPrev = prev # conatining the reference
+ self.cPos = position # inner car values
self.IPD = IPD
self.PS = PS
self.tps = tps
- self.bcIPD = False
+ self.bcIPD = False # below cIPD. If true, sets speed to 0 and skips all further postions and speed updates
# initialise calman filter
- if not prev == None:
+ if not prev == None:
dt = 1/tps
+ # x: current car state x = (distance, velocity)^T. distance means the distance to it's prev platoon member and velocity its relative speed.
x = np.array([[0,0]]).T
+ # P: covariance matrix
P = np.array([[10,0],[0,10]])
+ # F: dynamic model
F = np.array([[1,dt],[0,1]])
+ # Q: process error
Q = np.array([[1,0],[0,1]])
+ # H: messuare matrix, references the distance
H = np.array([[1,0]])
+ # R: messuarement error
R = np.array([[5]])
self.KF = KalmanFilter(x, P, F, Q, H, R, tps)
@@ -33,17 +39,20 @@ class Car(object):
self.distHistLength = 15
self.ds = np.empty(self.distHistLength)
+ # Create randomness for sensors
RAND = 0.5
self.SonarSystemUnc = random.uniform(-RAND,RAND) # in cm
- self.SonarStatisticalUnc = lambda : random.gauss(0,RAND) # in cm
+ self.SonarStatisticalUnc = lambda : random.gauss(0,RAND) # in cm
- self.SpeedSystemUnc = random.uniform(-RAND,RAND) # in meter per seconds
- self.SpeedStatisticalUnc = lambda : random.gauss(0,RAND) # in meter per seconds
+ self.SpeedSystemUnc = random.uniform(-RAND,RAND) # in meter per seconds
+ self.SpeedStatisticalUnc = lambda : random.gauss(0,RAND) # in meter per seconds
- self.EngineSystemUnc = random.uniform(-RAND,RAND) # in meter per seconds
+ self.EngineSystemUnc = random.uniform(-RAND,RAND) # in meter per seconds
+ # Fahrzeuginterne Daten setzen
self.setSpeed(speed)
self.getDistance()
+ # set previous car speed to own correct speed (to prevent a zero speed)
self.v_v = self.cSpeed
return None
@@ -51,6 +60,7 @@ class Car(object):
return max(0.1*self.IPD,1)
def getSpeed(self):
+ # returns the own speed with uncertainty and median filter
if self.cSpeed <= 1e-8:
return 0
self.dIdx = (self.dIdx + 1) % self.distHistLength
@@ -66,6 +76,7 @@ class Car(object):
self.speed = 0
def getDistance(self):
+ # returns the distance between the car and its previously driving car
setLater = False
try:
self.oldDist = self.dist
@@ -99,7 +110,7 @@ class Car(object):
d = x[0,0]
self.v_v = v_v = max(v + x[1,0], 0)
- v_new = self.computeNewSpeed_2(d, v, v_v, IPD, PS)
+ v_new = self.computeNewSpeed(d, v, v_v, IPD, PS)
inertia = 0.8 # in [0,1] und gibt die Traegheit des Fahrzeuges an
self.setSpeed(inertia * v + (1- inertia) * v_new)
@@ -113,37 +124,7 @@ class Car(object):
self.v_v = 0
self.setSpeed(c * v + (1-c) * PS)
-
- def computeNewSpeed_1(self, d, v, v_v, IPD, PS):
- if checkInbound(d, IPD, 0.01*IPD):
- if checkInbound (v, v_v, 0.01*v_v):
- if checkInbound(v, PS, 0.01*PS):
- v_new = PS
- else:
- if v > PS:
- v_new = v_v - abs(PS-v_v)/4
- else:
- v_new = v_v + abs(PS-v_v)/4
- else:
- if v > v_v:
- v_new = v_v - abs(PS-v_v)/4
- else:
- v_new = v_v + abs(PS-v_v)/4
- else:
- v_new = (v_v * (d/IPD)**2) #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
- return v_new
-
- def computeNewSpeed_2(self, d, v, v_v, IPD, PS):
- # Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
- DIST_POW = 2
- SPEED_POW = 0.5
- if abs(v_v) > 1e-8:
- v_new = (v_v * (d/(IPD))**DIST_POW * (PS/v_v)**SPEED_POW)
- else:
- v_new = (d - IPD)/IPD
- return v_new
-
- def computeNewSpeed_3(self, d, v, v_v, IPD, PS):
+ def computeNewSpeed(self, d, v, v_v, IPD, PS):
# Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
DIST_POW = 1.5; DIST_FAC = 1
SPEED_POW = 0.5; SPEED_FAC = 1
diff --git a/modules/CACC/platoonSimulation.py b/modules/CACC/platoonSimulation.py
index 367bca46..caa31505 100644
--- a/modules/CACC/platoonSimulation.py
+++ b/modules/CACC/platoonSimulation.py
@@ -1,5 +1,4 @@
import sys
-import IPython
import bisect
import time
import math
diff --git a/modules/CACC/simlifiedSpeedVisualization.py b/modules/CACC/simlifiedSpeedVisualization.py
index 73a0b704..857cccda 100644
--- a/modules/CACC/simlifiedSpeedVisualization.py
+++ b/modules/CACC/simlifiedSpeedVisualization.py
@@ -11,11 +11,12 @@ IPD = 20
IPD_TOL = 0.05*IPD
-DIST_POW = 1
+DIST_POW = 2
SPEED_POW = 0.5
-ds = np.linspace(0.*IPD, 1.5*IPD, num = 1000)
-v_news = []
+ds = np.linspace(0.*IPD, 1.7*IPD, num = 1000)
+v_newsAdv = []
+v_newsLin = []
def checkInbound(argument, target, delta): #Ueberall ist ein delta von 0.01 hinterlegt
if argument > target + delta:
@@ -25,58 +26,25 @@ def checkInbound(argument, target, delta): #Ueberall ist ein delta von 0.01 h
else:
return True
-def variante_1(d):
- v_new = None
- if checkInbound(d, IPD, IPD_TOL):
- if checkInbound (v, v_v, 0.01*v_v):
- if checkInbound(v, PS, 0.01*PS):
- v_new = PS
- else:
- if v > PS:
- v_new = v_v - abs(PS-v_v)/4
- else:
- v_new = v_v + abs(PS-v_v)/4
- else:
- if v > v_v:
- v_new = v_v - abs(PS-v_v)/4
- else:
- v_new = v_v + abs(PS-v_v)/4
- else:
- v_new = (v_v * (d/IPD)**2) #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
+def variante_adv(d):
+ v_new = (v_v * (d/(IPD))**DIST_POW * (PS/v_v)**SPEED_POW)
return v_new
-def variante_2(d):
- v_new = None
- if d < IPD - IPD_TOL:
- v_new = (v_v * (d/(IPD - IPD_TOL))**2) #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
- elif d > IPD + IPD_TOL:
- v_new = (v_v * (d/(IPD + IPD_TOL))**2) #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
- else:
- v_new = v_v
- return v_new
-
-def variante_3(d):
- v_new = (v_v * (d/(IPD))**DIST_POW) #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
- return v_new
-
-def variante_4(d):
- v_new = (v_v * (d/(IPD))**DIST_POW * (PS/v_v)**SPEED_POW) #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
- return v_new
-
-def variante_5(d):
- v_new = (v_v * (d/(IPD))**DIST_POW * (PS/v)**SPEED_POW) #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
+def variante_lin(d):
+ v_new = (v_v * (d/(IPD))**1 * (PS/v_v)**0.5)
return v_new
for d in ds:
- v_new = variante_5(d)
- v_news.append(v_new)
+ v_newsAdv.append(variante_adv(d))
+ v_newsLin.append(variante_lin(d))
-plt.plot(ds, v_news,'b', label='v_new')
+plt.plot(ds, v_newsAdv,'b-', label='v_new (exp)')
+plt.plot(ds, v_newsLin,'b:', label='v_new (lin)')
plt.plot(ds, [v_v for d in ds],'tab:orange', label='v_v')
plt.plot(ds, [PS for d in ds], 'g', label='PS')
plt.plot(ds, [v for d in ds], 'b--', label='v')
-plt.plot([IPD, IPD], [0, 1.1*max(max(v_news),PS,v_v,v)], 'r', label='IPD')
-plt.plot([IPD - IPD_TOL, IPD - IPD_TOL], [0, 1.1*max(max(v_news),PS,v_v,v)], 'r--', label='IPD-')
-plt.plot([IPD + IPD_TOL, IPD + IPD_TOL], [0, 1.1*max(max(v_news),PS,v_v,v)], 'r--', label='IPD+')
+plt.plot([IPD, IPD], [0, 1.1*max(max(v_newsAdv),PS,v_v,v)], 'r', label='IPD')
+plt.plot([IPD - IPD_TOL, IPD - IPD_TOL], [0, 1.1*max(max(v_newsAdv),PS,v_v,v)], 'r--', label='IPD-')
+plt.plot([IPD + IPD_TOL, IPD + IPD_TOL], [0, 1.1*max(max(v_newsAdv),PS,v_v,v)], 'r--', label='IPD+')
plt.legend()
plt.show()
--
GitLab