return lat
This commit is contained in:
Comma Device
@@ -75,14 +75,6 @@ class LanePlanner:
|
||||
self.params = Params()
|
||||
self.camera_offset = self.params.get_int("CameraOffset") * 0.01
|
||||
|
||||
# 障碍物绕行参数
|
||||
# 绕行要“明显”，需要更快的响应；时间常数过大时偏移会被抹平
|
||||
self.obstacle_avoidance_offset = FirstOrderFilter(0.0, 0.4, DT_MDL)
|
||||
self.obstacle_offset_left = 0.0
|
||||
self.obstacle_offset_right = 0.0
|
||||
self.last_avoidance_time = 0.0 # 记录最后一次绕行时间
|
||||
self.avoidance_cooldown = 2.0 # 绕行结束后的冷却时间（秒）
|
||||
|
||||
def parse_model(self, md):
|
||||
|
||||
lane_lines = md.laneLines
|
||||
@@ -111,137 +103,7 @@ class LanePlanner:
|
||||
self.l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
|
||||
self.r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
|
||||
|
||||
def calculate_obstacle_avoidance_offset(self, leads_left, leads_right, v_ego, lead_one=None):
|
||||
"""计算障碍物绕行偏移量，包含对向来车的避让优化"""
|
||||
|
||||
def _lead_field(lead, name, default=0.0):
|
||||
# 既兼容 dict，又兼容 capnp/对象属性
|
||||
if isinstance(lead, dict):
|
||||
return lead.get(name, default)
|
||||
return getattr(lead, name, default)
|
||||
|
||||
offset_left = 0.0
|
||||
offset_right = 0.0
|
||||
oncoming_detected = False
|
||||
|
||||
# -----------------------
|
||||
# 1. 处理前方主目标（本车车道内障碍物）
|
||||
# -----------------------
|
||||
if lead_one is not None:
|
||||
try:
|
||||
d_rel = float(_lead_field(lead_one, 'dRel', 100.0))
|
||||
v_lead = float(_lead_field(lead_one, 'vLead', 0.0))
|
||||
d_path = float(_lead_field(lead_one, 'dPath', 10.0))
|
||||
|
||||
if 5.0 < d_rel < 50.0 and abs(d_path) < 1.5:
|
||||
v_lead_kph = v_lead * 3.6
|
||||
|
||||
# 识别障碍物类型（行人/电动车/一般车辆）
|
||||
if v_lead_kph < 6.0:
|
||||
vulnerable_factor = 2.5
|
||||
elif v_lead_kph < 25.0:
|
||||
vulnerable_factor = 2.0
|
||||
elif v_lead_kph < 40.0:
|
||||
vulnerable_factor = 1.5
|
||||
else:
|
||||
vulnerable_factor = 1.0
|
||||
|
||||
distance_factor = np.interp(d_rel, [5.0, 30.0], [1.0, 0.3])
|
||||
|
||||
# 左侧障碍物 → 向右偏；右侧障碍物 → 向左偏
|
||||
if d_path < -0.3:
|
||||
offset_right = max(offset_right, 0.8 * distance_factor * vulnerable_factor)
|
||||
elif d_path > 0.3:
|
||||
offset_left = max(offset_left, 0.8 * distance_factor * vulnerable_factor)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# -----------------------
|
||||
# 2. 侧向障碍物 + 对向来车处理
|
||||
# left 列表代表车辆左侧一带，right 代表车辆右侧一带
|
||||
# -----------------------
|
||||
|
||||
def _process_side_leads(leads, is_left_side):
|
||||
nonlocal offset_left, offset_right, oncoming_detected
|
||||
|
||||
for lead in leads:
|
||||
status = bool(_lead_field(lead, 'status', False))
|
||||
if not status:
|
||||
continue
|
||||
|
||||
d_rel = float(_lead_field(lead, 'dRel', 100.0))
|
||||
v_lead = float(_lead_field(lead, 'vLead', 0.0))
|
||||
v_rel = float(_lead_field(lead, 'vRel', 0.0))
|
||||
d_path = float(abs(_lead_field(lead, 'dPath', 10.0)))
|
||||
|
||||
if d_rel >= 80.0 or d_path >= 4.0:
|
||||
continue
|
||||
|
||||
v_lead_kph = v_lead * 3.6
|
||||
|
||||
# 基础类型权重：行人/电动车/普通车
|
||||
if v_lead_kph < 6.0:
|
||||
vulnerable_factor = 2.5
|
||||
min_safe_distance = 2.0
|
||||
elif v_lead_kph < 25.0:
|
||||
vulnerable_factor = 2.0
|
||||
min_safe_distance = 1.5
|
||||
elif v_lead_kph < 40.0:
|
||||
vulnerable_factor = 1.5
|
||||
min_safe_distance = 1.2
|
||||
else:
|
||||
vulnerable_factor = 1.0
|
||||
min_safe_distance = 1.0
|
||||
|
||||
# 静止/缓慢目标增强
|
||||
if abs(v_rel) < 2.0 and v_lead_kph < 10.0:
|
||||
vulnerable_factor *= 1.4
|
||||
|
||||
# 对向来车判定：相对速度为负且较大（更早触发），并且目标自身速度不低
|
||||
is_oncoming = (v_rel < -3.0 and v_lead_kph > 20.0)
|
||||
if is_oncoming:
|
||||
# 对向车优先级再提升一些，并允许在更远距离就开始偏移
|
||||
oncoming_detected = True
|
||||
vulnerable_factor *= 1.8
|
||||
distance_factor = np.interp(d_rel, [12.0, 90.0], [1.6, 0.35])
|
||||
else:
|
||||
distance_factor = np.interp(d_rel, [3.0, 40.0], [1.3, 0.2])
|
||||
|
||||
lateral_factor = np.interp(d_path, [0.3, 3.8], [1.2, 0.2])
|
||||
if d_path < min_safe_distance:
|
||||
lateral_factor *= 1.8
|
||||
|
||||
# 左侧列表 → 向右偏；右侧列表 → 向左偏
|
||||
base_gain = 1.35 if is_oncoming else 1.0
|
||||
avoidance_strength = base_gain * 0.95 * distance_factor * lateral_factor * vulnerable_factor
|
||||
if is_left_side:
|
||||
offset_right = max(offset_right, avoidance_strength)
|
||||
else:
|
||||
offset_left = max(offset_left, avoidance_strength)
|
||||
|
||||
if leads_left is not None:
|
||||
_process_side_leads(leads_left, is_left_side=True)
|
||||
if leads_right is not None:
|
||||
_process_side_leads(leads_right, is_left_side=False)
|
||||
|
||||
# 计算最终偏移
|
||||
final_offset = offset_left - offset_right
|
||||
|
||||
# 速度自适应
|
||||
# 对向场景下不要在高速被过度削弱，否则体感“不明显”
|
||||
if oncoming_detected:
|
||||
speed_factor = np.interp(v_ego * 3.6, [5, 30, 100], [1.8, 1.55, 1.0])
|
||||
else:
|
||||
speed_factor = np.interp(v_ego * 3.6, [5, 30, 80], [1.6, 1.3, 0.8])
|
||||
final_offset *= speed_factor
|
||||
|
||||
# 限制最大偏移量
|
||||
max_offset = np.interp(v_ego * 3.6, [10, 60], [1.35, 0.9])
|
||||
if oncoming_detected:
|
||||
max_offset *= 1.35
|
||||
return np.clip(final_offset, -max_offset, max_offset)
|
||||
|
||||
def get_d_path(self, CS, v_ego, path_t, path_xyz, curve_speed, leads_left=None, leads_right=None, lead_one=None):
|
||||
def get_d_path(self, CS, v_ego, path_t, path_xyz, curve_speed):
|
||||
#if v_ego > 0.1:
|
||||
# self.lane_width_updated_count = max(0, self.lane_width_updated_count - 1)
|
||||
# Reduce reliance on lanelines that are too far apart or
|
||||
@@ -370,31 +232,6 @@ class LanePlanner:
|
||||
# self.lane_width_left_filtered.x, self.lane_width, self.lane_width_right_filtered.x)
|
||||
|
||||
adjustLaneTime = self.params.get_float("LatMpcInputOffset") * 0.01 # 0.06
|
||||
|
||||
# 计算障碍物绕行偏移（在车道线处理之前）
|
||||
obstacle_offset = 0.0
|
||||
has_obstacle = False
|
||||
try:
|
||||
if leads_left is not None and leads_right is not None:
|
||||
obstacle_offset = self.calculate_obstacle_avoidance_offset(leads_left, leads_right, v_ego, lead_one)
|
||||
|
||||
# 检测是否有需要绕行的障碍物
|
||||
if abs(obstacle_offset) > 0.05:
|
||||
has_obstacle = True
|
||||
self.last_avoidance_time = 0.0 # 重置计时器
|
||||
else:
|
||||
self.last_avoidance_time += DT_MDL
|
||||
|
||||
# 如果障碍物消失，平滑回归原车道
|
||||
if not has_obstacle and self.last_avoidance_time < self.avoidance_cooldown:
|
||||
# 在冷却期内，逐渐减小绕行偏移
|
||||
decay_factor = 1.0 - (self.last_avoidance_time / self.avoidance_cooldown)
|
||||
obstacle_offset = self.obstacle_avoidance_offset.x * decay_factor
|
||||
|
||||
self.obstacle_avoidance_offset.update(obstacle_offset)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
laneline_active = False
|
||||
self.d_prob_count = self.d_prob_count + 1 if self.d_prob > 0.3 else 0
|
||||
if self.lanefull_mode and self.d_prob_count > int(1 / DT_MDL):
|
||||
@@ -409,13 +246,10 @@ class LanePlanner:
|
||||
lane_path_y_interp = np.interp(path_t * (1.0 + adjustLaneTime), self.ll_t[safe_idxs], lane_path_y[safe_idxs])
|
||||
path_xyz[:,1] = self.d_prob * lane_path_y_interp + (1.0 - self.d_prob) * path_xyz[:,1]
|
||||
|
||||
# 应用障碍物绕行偏移（优先级高于车道线）
|
||||
if abs(self.obstacle_avoidance_offset.x) > 0.03:
|
||||
path_xyz[:,1] += self.obstacle_avoidance_offset.x
|
||||
|
||||
path_xyz[:, 1] += (self.camera_offset + self.lane_offset_filtered.x)
|
||||
|
||||
self.offset_total = self.lane_offset_filtered.x + self.obstacle_avoidance_offset.x
|
||||
self.offset_total = self.lane_offset_filtered.x
|
||||
|
||||
return path_xyz, laneline_active
|
||||
|
||||
|
||||
@@ -60,7 +60,6 @@ class LateralPlanner:
|
||||
|
||||
self.useLaneLineSpeedApply = self.params.get_int("UseLaneLineSpeed")
|
||||
self.pathOffset = float(self.params.get_int("PathOffset")) * 0.01
|
||||
self.bypass_lat_offset = 0.0
|
||||
self.useLaneLineMode = False
|
||||
self.plan_a = np.zeros((TRAJECTORY_SIZE, ))
|
||||
self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
|
||||
@@ -161,29 +160,7 @@ class LateralPlanner:
|
||||
self.latDebugText = self.LP.debugText
|
||||
#self.lanelines_active = True if self.LP.d_prob > 0.3 and self.LP.lanefull_mode else False
|
||||
|
||||
# Bypass lateral assist (no new model): when a close slow lead exists and
|
||||
# lane-change intent is active, add a small temporary lateral offset to help
|
||||
# the vehicle commit to bypass trajectory earlier.
|
||||
lead = sm['radarState'].leadOne
|
||||
lane_change_active = md.meta.desire != log.Desire.none or carrot.desireState > 0.7
|
||||
lead_slow_close = lead.status and lead.dRel < 45.0 and (self.v_ego - lead.vLead) > 1.0 and self.v_ego < (50.0 / 3.6)
|
||||
|
||||
if lane_change_active and lead_slow_close:
|
||||
# choose offset direction from current model desire state (left/right)
|
||||
if md.meta.desire == log.Desire.laneChangeLeft:
|
||||
target_bypass_offset = 0.28
|
||||
elif md.meta.desire == log.Desire.laneChangeRight:
|
||||
target_bypass_offset = -0.28
|
||||
else:
|
||||
target_bypass_offset = 0.0
|
||||
else:
|
||||
target_bypass_offset = 0.0
|
||||
|
||||
# smooth offset transitions to avoid lateral jerk
|
||||
alpha = np.clip(DT_MDL / 0.5, 0.0, 1.0)
|
||||
self.bypass_lat_offset += alpha * (target_bypass_offset - self.bypass_lat_offset)
|
||||
|
||||
self.path_xyz[:, 1] += (self.pathOffset + self.bypass_lat_offset)
|
||||
self.path_xyz[:, 1] += self.pathOffset
|
||||
|
||||
self.lat_mpc.set_weights(self.lateralPathCost, self.lateralMotionCost,
|
||||
LATERAL_ACCEL_COST, LATERAL_JERK_COST,
|
||||
|
||||
Reference in New Issue
Block a user