Skip to content
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
106 changes: 70 additions & 36 deletions src/Object_Tracking_Planning/object_detection.py
Original file line number Diff line number Diff line change
Expand Up @@ -1178,7 +1178,7 @@ def _pid_control(self, target_speed, current_speed):
return np.clip((self._k_p * error) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0)


class PIDLateralController():
class PIDLateralController:
"""
PIDLateralController implements lateral control using a PID.
"""
Expand All @@ -1187,64 +1187,98 @@ def __init__(self, vehicle, K_P=1.0, K_D=0.0, K_I=0.0, dt=0.03):
"""
Constructor method.

:param vehicle: actor to apply to local planner logic onto
:param K_P: Proportional term
:param K_D: Differential term
:param K_I: Integral term
:param dt: time differential in seconds
Args:
vehicle: Actor to apply local planner logic onto
K_P: Proportional term coefficient (default: 1.0)
K_D: Differential term coefficient (default: 0.0)
K_I: Integral term coefficient (default: 0.0)
dt: Time differential in seconds (default: 0.03)
"""
self._vehicle = vehicle
self._k_p = K_P
self._k_d = K_D
self._k_i = K_I
self._dt = dt
self._e_buffer = deque(maxlen=10)
self._error_buffer = deque(maxlen=10)

def run_step(self, waypoint):
"""
Execute one step of lateral control to steer
the vehicle towards a certain waypoin.
Execute one step of lateral control to steer the vehicle towards a waypoint.

:param waypoint: target waypoint
:return: steering control in the range [-1, 1] where:
-1 maximum steering to left
+1 maximum steering to right
Args:
waypoint: Target waypoint

Returns:
float: Steering control in range [-1, 1] where:
-1 = maximum steering to left
+1 = maximum steering to right
"""
return self._pid_control(waypoint, self._vehicle.get_transform())

def _pid_control(self, waypoint, vehicle_transform):
"""
Estimate the steering angle of the vehicle based on the PID equations
Estimate steering angle based on PID equations.

:param waypoint: target waypoint
:param vehicle_transform: current transform of the vehicle
:return: steering control in the range [-1, 1]
Args:
waypoint: Target waypoint
vehicle_transform: Current transform of the vehicle

Returns:
float: Steering control in range [-1, 1]
"""
# Calculate vehicle direction vector
v_begin = vehicle_transform.location
v_end = v_begin + carla.Location(x=math.cos(math.radians(vehicle_transform.rotation.yaw)),
y=math.sin(math.radians(vehicle_transform.rotation.yaw)))
yaw_rad = math.radians(vehicle_transform.rotation.yaw)
v_end = v_begin + carla.Location(
x=math.cos(yaw_rad),
y=math.sin(yaw_rad)
)

# Calculate vectors
v_vec = np.array([v_end.x - v_begin.x, v_end.y - v_begin.y, 0.0])
w_vec = np.array([waypoint.transform.location.x -
v_begin.x, waypoint.transform.location.y -
v_begin.y, 0.0])
_dot = math.acos(np.clip(np.dot(w_vec, v_vec) /
(np.linalg.norm(w_vec) * np.linalg.norm(v_vec)), -1.0, 1.0))

_cross = np.cross(v_vec, w_vec)
w_vec = np.array([
waypoint.transform.location.x - v_begin.x,
waypoint.transform.location.y - v_begin.y,
0.0
])

# Calculate angle difference
v_norm = np.linalg.norm(v_vec)
w_norm = np.linalg.norm(w_vec)
dot_product = np.dot(w_vec, v_vec) / (v_norm * w_norm)
dot_product_clipped = np.clip(dot_product, -1.0, 1.0)
angle_diff = math.acos(dot_product_clipped)

# Determine sign based on cross product
cross_product = np.cross(v_vec, w_vec)
if cross_product[2] < 0:
angle_diff *= -1.0

# Update error buffer and calculate PID terms
self._error_buffer.append(angle_diff)

# Calculate derivative and integral terms
derivative_error = self._calculate_derivative_error()
integral_error = self._calculate_integral_error()

# PID calculation with clamping
pid_output = (
self._k_p * angle_diff +
self._k_d * derivative_error +
self._k_i * integral_error
)

if _cross[2] < 0:
_dot *= -1.0
return np.clip(pid_output, -1.0, 1.0)

self._e_buffer.append(_dot)
if len(self._e_buffer) >= 2:
_de = (self._e_buffer[-1] - self._e_buffer[-2]) / self._dt
_ie = sum(self._e_buffer) * self._dt
else:
_de = 0.0
_ie = 0.0
def _calculate_derivative_error(self):
"""Calculate derivative error term."""
if len(self._error_buffer) >= 2:
return (self._error_buffer[-1] - self._error_buffer[-2]) / self._dt
return 0.0

return np.clip((self._k_p * _dot) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0)
def _calculate_integral_error(self):
"""Calculate integral error term."""
return sum(self._error_buffer) * self._dt


# ==============================================================================
Expand Down
Loading