mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-23 17:23:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

openpilot v0.3.9 tweaks

Browse Source 
old-commit-hash: 1ad9cc8c67
This commit is contained in:
Vehicle Researcher
2017-12-06 12:48:00 -08:00
parent 490307fe50
commit d914111e16
30 changed files with 470 additions and 279 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/controlsd.py
+1 -1
View File
@@ -476,7 +476,7 @@ def controlsd_thread(gctx, rate=100):
rk = Ratekeeper(rate, print_delay_threshold=2./1000)
# learned angle offset
angle_offset = 0.
angle_offset = 1.5 # Default model bias
calibration_params = params.get("CalibrationParams")
if calibration_params:
try:
selfdrive/controls/lib/drive_helpers.py
+2 -1
View File
@@ -42,7 +42,8 @@ def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, ang
min_learn_speed = 1.
# learn less at low speed or when turning
alpha_v = alpha * c_prob * (max(v_ego - min_learn_speed, 0.)) / (1. + 0.2 * abs(angle_steers))
slow_factor = 1. / (1. + 0.02 * abs(angle_steers) * v_ego)
alpha_v = alpha * c_prob * (max(v_ego - min_learn_speed, 0.)) * slow_factor
# only learn if lateral control is active and if driver is not overriding:
if lateral_control and not steer_override:
selfdrive/controls/lib/latcontrol.py
+19 -4
View File
@@ -1,11 +1,14 @@
import math
import numpy as np
from selfdrive.controls.lib.pid import PIController
from selfdrive.controls.lib.lateral_mpc import libmpc_py
from common.numpy_fast import clip, interp
from common.numpy_fast import interp
from common.realtime import sec_since_boot
from selfdrive.swaglog import cloudlog
# 100ms is a rule of thumb estimation of lag from image processing to actuator command
ACTUATORS_DELAY = 0.1
ACTUATORS_DELAY = 0.2
_DT = 0.01 # 100Hz
_DT_MPC = 0.05 # 20Hz
@@ -24,6 +27,7 @@ def get_steer_max(CP, v_ego):
class LatControl(object):
def __init__(self, VM):
self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, k_f=VM.CP.steerKf, pos_limit=1.0)
self.last_cloudlog_t = 0.0
self.setup_mpc()
def setup_mpc(self):
@@ -61,7 +65,7 @@ class LatControl(object):
p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly))
# account for actuation delay
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, VM.CP.sR)
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, VM.CP.steerRatio)
v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
@@ -71,10 +75,21 @@ class LatControl(object):
delta_desired = self.mpc_solution[0].delta[1]
self.cur_state[0].delta = delta_desired
self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.CP.sR) + angle_offset)
self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.CP.steerRatio) + angle_offset)
self.angle_steers_des_time = cur_time
self.mpc_updated = True
# Check for infeasable MPC solution
nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
t = sec_since_boot()
if nans:
self.libmpc.init()
self.cur_state[0].delta = math.radians(angle_steers) / VM.CP.steerRatio
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True")
if v_ego < 0.3 or not active:
output_steer = 0.0
self.pid.reset()
selfdrive/controls/lib/lateral_mpc/generator.cpp
+1 -1
View File
@@ -79,7 +79,7 @@ int main( )
Q(3,3) = 1.0;
Q(4,4) = 2.0;
Q(4,4) = 1.0;
// Terminal cost
Function hN;
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:f01adf6d07d2ca818d2df6a79b1c10a4806dd6a1a525438950329ff0c2791437
size 194874
oid sha256:dd891792d5bc3c780941a0e3d8c37829d6f4ceef554a4704017f6024e29fba20
size 194688
selfdrive/controls/lib/planner.py
+89 -53
View File
@@ -3,6 +3,7 @@ import zmq
import numpy as np
import math
from collections import defaultdict
from common.realtime import sec_since_boot
from common.params import Params
@@ -39,6 +40,9 @@ _A_CRUISE_MAX_BP = [0., 5., 10., 20., 40.]
_A_TOTAL_MAX_V = [1.5, 1.9, 3.2]
_A_TOTAL_MAX_BP = [0., 20., 40.]
_FCW_A_ACT_V = [-3., -2.]
_FCW_A_ACT_BP = [0., 30.]
# max acceleration allowed in acc, which happens in restart
A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING)
@@ -61,7 +65,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
deg_to_rad = np.pi / 180. # from can reading to rad
a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
a_y = v_ego**2 * angle_steers * deg_to_rad / (CP.sR * CP.l)
a_y = v_ego**2 * angle_steers * deg_to_rad / (CP.steerRatio * CP.wheelbase)
a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.))
a_target[1] = min(a_target[1], a_x_allowed)
@@ -70,36 +74,64 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
class FCWChecker(object):
def __init__(self):
self.fcw_count = 0
self.last_fcw_a = 0.0
self.v_lead_max = 0.0
self.lead_seen_t = 0.0
self.last_fcw_time = 0.0
self.reset_lead(0.0)
def reset_lead(self, cur_time):
self.last_fcw_a = 0.0
self.v_lead_max = 0.0
self.lead_seen_t = cur_time
self.last_fcw_time = 0.0
self.last_min_a = 0.0
def update(self, mpc_solution, cur_time, v_ego, v_lead, y_lead, vlat_lead, fcw_lead, blinkers):
min_a_mpc = min(list(mpc_solution[0].a_ego)[1:])
self.counters = defaultdict(lambda: 0)
@staticmethod
def calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead):
max_ttc = 5.0
v_rel = v_ego - v_lead
a_rel = a_ego - a_lead
# assuming that closing gap ARel comes from lead vehicle decel,
# then limit ARel so that v_lead will get to zero in no sooner than t_decel.
# This helps underweighting ARel when v_lead is close to zero.
t_decel = 2.
a_rel = np.minimum(a_rel, v_lead/t_decel)
# delta of the quadratic equation to solve for ttc
delta = v_rel**2 + 2 * x_lead * a_rel
# assign an arbitrary high ttc value if there is no solution to ttc
if delta < 0.1 or (np.sqrt(delta) + v_rel < 0.1):
ttc = max_ttc
else:
ttc = np.minimum(2 * x_lead / (np.sqrt(delta) + v_rel), max_ttc)
return ttc
def update(self, mpc_solution, cur_time, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
mpc_solution_a = list(mpc_solution[0].a_ego)
self.last_min_a = min(mpc_solution_a[1:])
self.v_lead_max = max(self.v_lead_max, v_lead)
if (fcw_lead > 0.99
and v_ego > 5.0
and min_a_mpc < -4.0
and self.v_lead_max > 2.5
and v_ego > v_lead
and self.lead_seen_t < cur_time - 2.0
and abs(y_lead) < 1.0
and abs(vlat_lead) < 0.3
and not blinkers):
self.fcw_count += 1
if self.fcw_count > 10 and self.last_fcw_time + 5.0 < cur_time:
if (fcw_lead > 0.99):
ttc = self.calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead)
self.counters['v_ego'] = self.counters['v_ego'] + 1 if v_ego > 5.0 else 0
self.counters['ttc'] = self.counters['ttc'] + 1 if ttc < 2.5 else 0
self.counters['v_lead_max'] = self.counters['v_lead_max'] + 1 if self.v_lead_max > 2.5 else 0
self.counters['v_ego_lead'] = self.counters['v_ego_lead'] + 1 if v_ego > v_lead else 0
self.counters['lead_seen'] = self.counters['lead_seen'] + 0.33
self.counters['y_lead'] = self.counters['y_lead'] + 1 if abs(y_lead) < 1.0 else 0
self.counters['vlat_lead'] = self.counters['vlat_lead'] + 1 if abs(vlat_lead) < 0.4 else 0
self.counters['blinkers'] = self.counters['blinkers'] + 10.0 / (20 * 3.0) if not blinkers else 0
a_thr = interp(v_lead, _FCW_A_ACT_BP, _FCW_A_ACT_V)
a_delta = min(mpc_solution_a[1:15]) - min(0.0, a_ego)
fcw_allowed = all(c >= 10 for c in self.counters.values())
if (self.last_min_a < -3.0 or a_delta < a_thr) and fcw_allowed and self.last_fcw_time + 5.0 < cur_time:
self.last_fcw_time = cur_time
self.last_fcw_a = min_a_mpc
self.last_fcw_a = self.last_min_a
return True
else:
self.fcw_count = 0
return False
@@ -214,6 +246,8 @@ class LongitudinalMpc(object):
self.libmpc.init()
self.cur_state[0].v_ego = CS.vEgo
self.cur_state[0].a_ego = 0.0
self.v_mpc = CS.vEgo
self.a_mpc = CS.aEgo
self.prev_lead_status = False
@@ -257,32 +291,33 @@ class Planner(object):
self.fcw_checker = FCWChecker()
self.fcw_enabled = fcw_enabled
def choose_solution(self, v_cruise_setpoint):
solutions = {'cruise': self.v_cruise}
if self.mpc1.prev_lead_status:
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
solutions['mpc2'] = self.mpc2.v_mpc
def choose_solution(self, v_cruise_setpoint, enabled):
if enabled:
solutions = {'cruise': self.v_cruise}
if self.mpc1.prev_lead_status:
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
solutions['mpc2'] = self.mpc2.v_mpc
slowest = min(solutions, key=solutions.get)
slowest = min(solutions, key=solutions.get)
if _DEBUG:
print "D_SOL", solutions, slowest, self.v_acc_sol, self.a_acc_sol
print "D_V", self.mpc1.v_mpc, self.mpc2.v_mpc, self.v_cruise
print "D_A", self.mpc1.a_mpc, self.mpc2.a_mpc, self.a_cruise
if _DEBUG:
print "D_SOL", solutions, slowest, self.v_acc_sol, self.a_acc_sol
print "D_V", self.mpc1.v_mpc, self.mpc2.v_mpc, self.v_cruise
print "D_A", self.mpc1.a_mpc, self.mpc2.a_mpc, self.a_cruise
self.longitudinalPlanSource = slowest
self.longitudinalPlanSource = slowest
# Choose lowest of MPC and cruise
if slowest == 'mpc1':
self.v_acc = self.mpc1.v_mpc
self.a_acc = self.mpc1.a_mpc
elif slowest == 'mpc2':
self.v_acc = self.mpc2.v_mpc
self.a_acc = self.mpc2.a_mpc
elif slowest == 'cruise':
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
# Choose lowest of MPC and cruise
if slowest == 'mpc1':
self.v_acc = self.mpc1.v_mpc
self.a_acc = self.mpc1.a_mpc
elif slowest == 'mpc2':
self.v_acc = self.mpc2.v_mpc
self.a_acc = self.mpc2.a_mpc
elif slowest == 'cruise':
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
@@ -331,19 +366,19 @@ class Planner(object):
self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
v_cruise_setpoint,
accel_limits[1], accel_limits[0],
jerk_limits[1],
jerk_limits[0],
jerk_limits[1], jerk_limits[0],
_DT_MPC)
else:
starting = LoC.long_control_state == LongCtrlState.starting
a_ego = min(CS.aEgo, 0.0)
self.v_cruise = CS.vEgo
self.a_cruise = self.CP.startAccel if starting else CS.aEgo
self.a_cruise = self.CP.startAccel if starting else a_ego
self.v_acc_start = CS.vEgo
self.a_acc_start = self.CP.startAccel if starting else CS.aEgo
self.a_acc_start = self.CP.startAccel if starting else a_ego
self.v_acc = CS.vEgo
self.a_acc = self.CP.startAccel if starting else CS.aEgo
self.a_acc = self.CP.startAccel if starting else a_ego
self.v_acc_sol = CS.vEgo
self.a_acc_sol = self.CP.startAccel if starting else CS.aEgo
self.a_acc_sol = self.CP.startAccel if starting else a_ego
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
@@ -351,15 +386,16 @@ class Planner(object):
self.mpc1.update(CS, self.lead_1, v_cruise_setpoint)
self.mpc2.update(CS, self.lead_2, v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint, enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = CS.leftBlinker or CS.rightBlinker
self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, CS.vEgo,
self.lead_1.vLead, self.lead_1.yRel, self.lead_1.vLat,
self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, CS.vEgo, CS.aEgo,
self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK,
self.lead_1.yRel, self.lead_1.vLat,
self.lead_1.fcw, blinkers) \
and not CS.brakePressed
if self.fcw:
selfdrive/controls/lib/radar_helpers.py
+51 -45
View File
@@ -5,7 +5,9 @@ import platform
import numpy as np
from common.numpy_fast import clip, interp
from common.kalman.ekf import FastEKF1D, SimpleSensor
from common.kalman.simple_kalman import KF1D
NO_FUSION_SCORE = 100 # bad default fusion score
# radar tracks
SPEED, ACCEL = 0, 1 # Kalman filter states enum
@@ -23,13 +25,22 @@ v_stationary_thr = 4. # objects moving below this speed are classified as stat
v_oncoming_thr = -3.9 # needs to be a bit lower in abs value than v_stationary_thr to not leave "holes"
v_ego_stationary = 4. # no stationary object flag below this speed
# Lead Kalman Filter params
_VLEAD_A = np.matrix([[1.0, ts], [0.0, 1.0]])
_VLEAD_C = np.matrix([1.0, 0.0])
#_VLEAD_Q = np.matrix([[10., 0.0], [0.0, 100.]])
#_VLEAD_R = 1e3
#_VLEAD_K = np.matrix([[ 0.05705578], [ 0.03073241]])
_VLEAD_K = np.matrix([[ 0.1988689 ], [ 0.28555364]])
class Track(object):
def __init__(self):
self.ekf = None
self.stationary = True
self.initted = False
def update(self, d_rel, y_rel, v_rel, d_path, v_ego_t_aligned):
def update(self, d_rel, y_rel, v_rel, d_path, v_ego_t_aligned, measured, steer_override):
if self.initted:
self.dPathPrev = self.dPath
self.vLeadPrev = self.vLead
@@ -39,6 +50,7 @@ class Track(object):
self.dRel = d_rel # LONG_DIST
self.yRel = y_rel # -LAT_DIST
self.vRel = v_rel # REL_SPEED
self.measured = measured # measured or estimate
# compute distance to path
self.dPath = d_path
@@ -47,59 +59,52 @@ class Track(object):
self.vLead = self.vRel + v_ego_t_aligned
if not self.initted:
self.initted = True
self.cnt = 1
self.vision_cnt = 0
self.vision = False
self.aRel = 0. # nidec gives no information about this
self.vLat = 0.
self.aLead = 0.
self.kf = KF1D(np.matrix([[self.vLead], [0.0]]), _VLEAD_A, _VLEAD_C, _VLEAD_K)
else:
# estimate acceleration
# TODO: use Kalman filter
a_rel_unfilt = (self.vRel - self.vRelPrev) / ts
a_rel_unfilt = clip(a_rel_unfilt, -10., 10.)
self.aRel = k_a_lead * a_rel_unfilt + (1 - k_a_lead) * self.aRel
v_lat_unfilt = (self.dPath - self.dPathPrev) / ts
# TODO: use Kalman filter
# neglect steer override cases as dPath is too noisy
v_lat_unfilt = 0. if steer_override else (self.dPath - self.dPathPrev) / ts
self.vLat = k_v_lat * v_lat_unfilt + (1 - k_v_lat) * self.vLat
a_lead_unfilt = (self.vLead - self.vLeadPrev) / ts
a_lead_unfilt = clip(a_lead_unfilt, -10., 10.)
self.aLead = k_a_lead * a_lead_unfilt + (1 - k_a_lead) * self.aLead
self.kf.update(self.vLead)
self.cnt += 1
self.vLeadK = float(self.kf.x[SPEED])
self.aLeadK = float(self.kf.x[ACCEL])
if self.stationary:
# stationary objects can become non stationary, but not the other way around
self.stationary = v_ego_t_aligned > v_ego_stationary and abs(self.vLead) < v_stationary_thr
self.oncoming = self.vLead < v_oncoming_thr
if self.ekf is None:
self.ekf = FastEKF1D(ts, 1e3, [0.1, 1])
self.ekf.state[SPEED] = self.vLead
self.ekf.state[ACCEL] = 0
self.lead_sensor = SimpleSensor(SPEED, 1, 2)
self.vision_score = NO_FUSION_SCORE
self.vLeadK = self.vLead
self.aLeadK = self.aLead
else:
self.ekf.update_scalar(self.lead_sensor.read(self.vLead))
self.ekf.predict(ts)
self.vLeadK = float(self.ekf.state[SPEED])
self.aLeadK = float(self.ekf.state[ACCEL])
if not self.initted:
self.cnt = 1
self.vision_cnt = 0
else:
self.cnt += 1
self.initted = True
self.vision = False
def mix_vision(self, dist_to_vision, rel_speed_diff):
def update_vision_score(self, dist_to_vision, rel_speed_diff):
# rel speed is very hard to estimate from vision
if dist_to_vision < 4.0 and rel_speed_diff < 10.:
# vision point is never stationary
self.vision_cnt += 1
# don't trust 1 or 2 fusions until model quality is much better
if self.vision_cnt >= 3:
self.vision = True
self.stationary = False
self.vision_score = dist_to_vision + rel_speed_diff
else:
self.vision_score = NO_FUSION_SCORE
def update_vision_fusion(self):
# vision point is never stationary
# don't trust 1 or 2 fusions until model quality is much better
if self.vision_cnt >= 3:
self.vision = True
self.stationary = False
def get_key_for_cluster(self):
# Weigh y higher since radar is inaccurate in this dimension
@@ -159,10 +164,6 @@ class Cluster(object):
def vLead(self):
return mean([t.vLead for t in self.tracks])
@property
def aLead(self):
return mean([t.aLead for t in self.tracks])
@property
def dPath(self):
return mean([t.dPath for t in self.tracks])
@@ -183,6 +184,10 @@ class Cluster(object):
def vision(self):
return any([t.vision for t in self.tracks])
@property
def measured(self):
return any([t.measured for t in self.tracks])
@property
def vision_cnt(self):
return max([t.vision_cnt for t in self.tracks])
@@ -201,7 +206,6 @@ class Cluster(object):
lead.vRel = float(self.vRel)
lead.aRel = float(self.aRel)
lead.vLead = float(self.vLead)
lead.aLead = float(self.aLead)
lead.dPath = float(self.dPath)
lead.vLat = float(self.vLat)
lead.vLeadK = float(self.vLeadK)
@@ -237,12 +241,14 @@ class Cluster(object):
# lat_corr used to be gated on enabled, now always running
t_lookahead = interp(self.dRel, t_lookahead_bp, t_lookahead_v)
# correct d_path for lookahead time, considering only cut-ins and no more than 1m impact
lat_corr = clip(t_lookahead * self.vLat, -1, 0)
d_path = max(d_path + lat_corr, 0)
# correct d_path for lookahead time, considering only cut-ins and no more than 1m impact.
lat_corr = clip(t_lookahead * self.vLat, -1., 1.) if self.measured else 0.
return d_path < 1.5 and not self.stationary and not self.oncoming
# consider only cut-ins
d_path = clip(d_path + lat_corr, min(0., d_path), max(0.,d_path))
return abs(d_path) < 1.5 and not self.stationary and not self.oncoming
def is_potential_lead2(self, lead_clusters):
if len(lead_clusters) > 0:
selfdrive/controls/lib/speed_smoother.py
+6
View File
@@ -16,6 +16,12 @@ def speed_smoother(vEgo, aEgo, vT, aMax, aMin, jMax, jMin, ts):
dV = vT - vEgo
# recover quickly if dV is positive and aEgo is negative or viceversa
if dV > 0. and aEgo < 0.:
jMax *= 3.
elif dV < 0. and aEgo > 0.:
jMin *= 3.
tDelta = get_delta_out_limits(aEgo, aMax, aMin, jMax, jMin)
if (ts <= tDelta):
selfdrive/controls/lib/vehicle_model.py
+35 -24
View File
@@ -10,36 +10,36 @@ from numpy.linalg import inv
# A depends on longitudinal speed, u, and vehicle parameters CP
def create_dyn_state_matrices(u, CP):
def create_dyn_state_matrices(u, VM):
A = np.zeros((2, 2))
B = np.zeros((2, 1))
A[0, 0] = - (CP.cF + CP.cR) / (CP.m * u)
A[0, 1] = - (CP.cF * CP.aF - CP.cR * CP.aR) / (CP.m * u) - u
A[1, 0] = - (CP.cF * CP.aF - CP.cR * CP.aR) / (CP.j * u)
A[1, 1] = - (CP.cF * CP.aF**2 + CP.cR * CP.aR**2) / (CP.j * u)
B[0, 0] = (CP.cF + CP.chi * CP.cR) / CP.m / CP.sR
B[1, 0] = (CP.cF * CP.aF - CP.chi * CP.cR * CP.aR) / CP.j / CP.sR
A[0, 0] = - (VM.cF + VM.cR) / (VM.m * u)
A[0, 1] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.m * u) - u
A[1, 0] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.j * u)
A[1, 1] = - (VM.cF * VM.aF**2 + VM.cR * VM.aR**2) / (VM.j * u)
B[0, 0] = (VM.cF + VM.chi * VM.cR) / VM.m / VM.sR
B[1, 0] = (VM.cF * VM.aF - VM.chi * VM.cR * VM.aR) / VM.j / VM.sR
return A, B
def kin_ss_sol(sa, u, CP):
def kin_ss_sol(sa, u, VM):
# kinematic solution, useful when speed ~ 0
K = np.zeros((2, 1))
K[0, 0] = CP.aR / CP.sR / CP.l * u
K[1, 0] = 1. / CP.sR / CP.l * u
K[0, 0] = VM.aR / VM.sR / VM.l * u
K[1, 0] = 1. / VM.sR / VM.l * u
return K * sa
def dyn_ss_sol(sa, u, CP):
def dyn_ss_sol(sa, u, VM):
# Dynamic solution, useful when speed > 0
A, B = create_dyn_state_matrices(u, CP)
A, B = create_dyn_state_matrices(u, VM)
return - np.matmul(inv(A), B) * sa
def calc_slip_factor(CP):
def calc_slip_factor(VM):
# the slip factor is a measure of how the curvature changes with speed
# it's positive for Oversteering vehicle, negative (usual case) otherwise
return CP.m * (CP.cF * CP.aF - CP.cR * CP.aR) / (CP.l**2 * CP.cF * CP.cR)
return VM.m * (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.l**2 * VM.cF * VM.cR)
class VehicleModel(object):
@@ -50,6 +50,16 @@ class VehicleModel(object):
self.update_state(init_state)
self.state_pred = np.zeros((self.steps, self.state.shape[0]))
self.CP = CP
# for math readability, convert long names car params into short names
self.m = CP.mass
self.j = CP.rotationalInertia
self.l = CP.wheelbase
self.aF = CP.centerToFront
self.aR = CP.wheelbase - CP.centerToFront
self.cF = CP.tireStiffnessFront
self.cR = CP.tireStiffnessRear
self.sR = CP.steerRatio
self.chi = CP.steerRatioRear
def update_state(self, state):
self.state = state
@@ -58,33 +68,34 @@ class VehicleModel(object):
# if the speed is too small we can't use the dynamic model
# (tire slip is undefined), we then use the kinematic model
if u > 0.1:
return dyn_ss_sol(sa, u, self.CP)
return dyn_ss_sol(sa, u, self)
else:
return kin_ss_sol(sa, u, self.CP)
return kin_ss_sol(sa, u, self)
def calc_curvature(self, sa, u):
# this formula can be derived from state equations in steady state conditions
return self.curvature_factor(u) * sa / self.CP.sR
return self.curvature_factor(u) * sa / self.sR
def curvature_factor(self, u):
sf = calc_slip_factor(self.CP)
return (1. - self.CP.chi)/(1. - sf * u**2) / self.CP.l
sf = calc_slip_factor(self)
return (1. - self.chi)/(1. - sf * u**2) / self.l
def get_steer_from_curvature(self, curv, u):
return curv * self.CP.sR * 1.0 / self.curvature_factor(u)
return curv * self.sR * 1.0 / self.curvature_factor(u)
def state_prediction(self, sa, u):
# U is the matrix of the controls
# u is the long speed
A, B = create_dyn_state_matrices(u, self.CP)
A, B = create_dyn_state_matrices(u, self)
return np.matmul((A * self.dt + np.identity(2)), self.state) + B * sa * self.dt
if __name__ == '__main__':
from selfdrive.car.toyota.interface import CarInterface
from selfdrive.car.honda.interface import CarInterface
# load car params
CP = CarInterface.get_params("TOYOTA PRIUS 2017", {})
#CP = CarInterface.get_params("TOYOTA PRIUS 2017", {})
CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING", {})
print CP
VM = VehicleModel(CP)
print VM.steady_state_sol(.1, 0.15)
print calc_slip_factor(CP)
print calc_slip_factor(VM)
selfdrive/controls/radard.py
+31 -16
View File
@@ -9,7 +9,8 @@ import selfdrive.messaging as messaging
from selfdrive.services import service_list
from selfdrive.controls.lib.latcontrol_helpers import calc_lookahead_offset
from selfdrive.controls.lib.pathplanner import PathPlanner
from selfdrive.controls.lib.radar_helpers import Track, Cluster, fcluster, RDR_TO_LDR
from selfdrive.controls.lib.radar_helpers import Track, Cluster, fcluster, \
RDR_TO_LDR, NO_FUSION_SCORE
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.swaglog import cloudlog
from cereal import car
@@ -17,7 +18,6 @@ from common.params import Params
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
from common.kalman.ekf import EKF, SimpleSensor
VISION_ONLY = False
DEBUG = False
#vision point
@@ -96,11 +96,12 @@ def radard_thread(gctx=None):
rk = Ratekeeper(rate, print_delay_threshold=np.inf)
while 1:
rr = RI.update()
ar_pts = {}
for pt in rr.points:
ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.aRel, None, False, None]
ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured]
# receive the live100s
l100 = messaging.recv_sock(live100)
@@ -130,7 +131,7 @@ def radard_thread(gctx=None):
ekfv.update(speedSensorV.read(PP.lead_dist, covar=PP.lead_var))
ekfv.predict(tsv)
ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(PP.d_poly, float(ekfv.state[XV])),
float(ekfv.state[SPEEDV]), np.nan, last_md_ts, np.nan, sec_since_boot())
float(ekfv.state[SPEEDV]), False)
else:
ekfv.state[XV] = PP.lead_dist
ekfv.covar = (np.diag([PP.lead_var, ekfv.var_init]))
@@ -152,9 +153,7 @@ def radard_thread(gctx=None):
# *** compute the tracks ***
for ids in ar_pts:
# ignore the vision point for now
if ids == VISION_POINT and not VISION_ONLY:
continue
elif ids != VISION_POINT and VISION_ONLY:
if ids == VISION_POINT:
continue
rpt = ar_pts[ids]
@@ -162,17 +161,30 @@ def radard_thread(gctx=None):
cur_time = float(rk.frame)/rate
v_ego_t_aligned = np.interp(cur_time - RI.delay, v_ego_array[1], v_ego_array[0])
d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2))
# add sign
d_path *= np.sign(rpt[1] - np.interp(rpt[0], path_x, path_y))
# create the track if it doesn't exist or it's a new track
if ids not in tracks or rpt[5] == 1:
if ids not in tracks:
tracks[ids] = Track()
tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, v_ego_t_aligned)
tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, v_ego_t_aligned, rpt[3], steer_override)
# allow the vision model to remove the stationary flag if distance and rel speed roughly match
if VISION_POINT in ar_pts:
fused_id = None
best_score = NO_FUSION_SCORE
for ids in tracks:
dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - tracks[ids].dRel)) ** 2 + (2*(ar_pts[VISION_POINT][1] - tracks[ids].yRel)) ** 2)
rel_speed_diff = abs(ar_pts[VISION_POINT][2] - tracks[ids].vRel)
tracks[ids].update_vision_score(dist_to_vision, rel_speed_diff)
if best_score > tracks[ids].vision_score:
fused_id = ids
best_score = tracks[ids].vision_score
if fused_id is not None:
tracks[fused_id].vision_cnt += 1
tracks[fused_id].update_vision_fusion()
# allow the vision model to remove the stationary flag if distance and rel speed roughly match
if VISION_POINT in ar_pts:
dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - rpt[0])) ** 2 + (2*(ar_pts[VISION_POINT][1] - rpt[1])) ** 2)
rel_speed_diff = abs(ar_pts[VISION_POINT][2] - rpt[2])
tracks[ids].mix_vision(dist_to_vision, rel_speed_diff)
# publish tracks (debugging)
dat = messaging.new_message()
@@ -185,9 +197,12 @@ def radard_thread(gctx=None):
for cnt, ids in enumerate(tracks.keys()):
if DEBUG:
print "id: %4.0f x: %4.1f y: %4.1f v: %4.1f d: %4.1f s: %1.0f" % \
print "id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f" % \
(ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel,
tracks[ids].dPath, tracks[ids].stationary)
tracks[ids].dPath, tracks[ids].vLat,
tracks[ids].vLead, tracks[ids].vLeadK,
tracks[ids].aLeadK,
tracks[ids].stationary)
dat.liveTracks[cnt].trackId = ids
dat.liveTracks[cnt].dRel = float(tracks[ids].dRel)
dat.liveTracks[cnt].yRel = float(tracks[ids].yRel)