轨道形状
每个轨道点的巡航速度要求
longitudinal controller 控制油门和刹车，用了 PI 控制器
lateral controller 控制方向盘，用了 PD 控制器
#!/usr/bin/env python3
"""
2D Controller Class to be used for the CARLA waypoint follower demo.
"""
import cutils
import numpy as np
from math import *
class Controller2D(object):
def __init__(self, waypoints):
self.vars = cutils.CUtils()
self._current_x = 0
self._current_y = 0
self._current_yaw = 0
self._current_speed = 0
self._desired_speed = 0
self._current_frame = 0
self._current_timestamp = 0
self._start_control_loop = False
self._set_throttle = 0
self._set_brake = 0
self._set_steer = 0
self._waypoints = waypoints
self._next_point = 1
self._conv_rad_to_steer = 180.0 / 70.0 / np.pi
self._pi = np.pi
self._2pi = 2.0 * np.pi
def update_values(self, x, y, yaw, speed, timestamp, frame):
self._current_x = x
self._current_y = y
self._current_yaw = yaw
self._current_speed = speed
self._current_timestamp = timestamp
self._current_frame = frame
if self._current_frame:
self._start_control_loop = True
def update_next_point(self):
min_idx = 0
min_dist = float("inf")
for i in range(len(self._waypoints)):
dist = np.linalg.norm(np.array([
self._waypoints[i][0] - self._current_x,
self._waypoints[i][1] - self._current_y]))
if dist < min_dist:
min_dist = dist
min_idx = i
index = min_idx # closest point
x = self._current_x
y = self._current_y
waypoints = self._waypoints
R_x, R_y = x - waypoints[index][0], y - waypoints[index][1]
delta_x, delta_y = waypoints[index+1][0] - waypoints[index][0], waypoints[index+1][1] - waypoints[index][1]
u = (R_x * delta_x + R_y * delta_y)/np.sqrt((R_x**2 + R_y**2)*(delta_x**2 + delta_y**2))
if u > 0 and index < len(self._waypoints)-1:
index += 1
self._next_point = index
def update_desired_speed(self):
desired_speed = 0.0
self.update_next_point()
self._desired_speed = self._waypoints[self._next_point][2]
def update_waypoints(self, new_waypoints):
self._waypoints = new_waypoints
def get_commands(self):
return self._set_throttle, self._set_steer, self._set_brake
def set_throttle(self, input_throttle):
# Clamp the throttle command to valid bounds
throttle = np.fmax(np.fmin(input_throttle, 1.0), 0.0)
self._set_throttle = throttle
def set_steer(self, input_steer_in_rad):
# Covnert radians to [-1, 1]
input_steer = self._conv_rad_to_steer * input_steer_in_rad
# Clamp the steering command to valid bounds
steer = np.fmax(np.fmin(input_steer, 1.0), -1.0)
self._set_steer = steer
def set_brake(self, input_brake):
# Clamp the steering command to valid bounds
brake = np.fmax(np.fmin(input_brake, 1.0), 0.0)
self._set_brake = brake
def update_controls(self):
######################################################
# RETRIEVE SIMULATOR FEEDBACK
######################################################
x = self._current_x
y = self._current_y
yaw = self._current_yaw
v = self._current_speed
self.update_desired_speed()
v_desired = self._desired_speed
t = self._current_timestamp
waypoints = self._waypoints
throttle_output = 0
steer_output = 0
brake_output = 0
######################################################
######################################################
# MODULE 7: DECLARE USAGE VARIABLES HERE
######################################################
######################################################
"""
Use 'self.vars.create_var(<variable name>, <default value>)'
to create a persistent variable (not destroyed at each iteration).
This means that the value can be stored for use in the next
iteration of the control loop.
Example: Creation of 'v_previous', default value to be 0
self.vars.create_var('v_previous', 0.0)
Example: Setting 'v_previous' to be 1.0
self.vars.v_previous = 1.0
Example: Accessing the value from 'v_previous' to be used
throttle_output = 0.5 * self.vars.v_previous
"""
self.vars.create_var('sum_err_v', 0.0)
self.vars.create_var('cte', 0.0)
# Skip the first frame to store previous values properly
if self._start_control_loop:
"""
Controller iteration code block.
Controller Feedback Variables:
x : Current X position (meters)
y : Current Y position (meters)
yaw : Current yaw pose (radians)
v : Current forward speed (meters per second)
t : Current time (seconds)
v_desired : Current desired speed (meters per second)
(Computed as the speed to track at the
closest waypoint to the vehicle.)
waypoints : Current waypoints to track
(Includes speed to track at each x,y
location.)
Format: [[x0, y0, v0],
[x1, y1, v1],
...
[xn, yn, vn]]
Example:
waypoints[2][1]:
Returns the 3rd waypoint's y position
waypoints[5]:
Returns [x5, y5, v5] (6th waypoint)
Controller Output Variables:
throttle_output : Throttle output (0 to 1)
steer_output : Steer output (-1.22 rad to 1.22 rad)
brake_output : Brake output (0 to 1)
"""
######################################################
######################################################
# MODULE 7: IMPLEMENTATION OF LONGITUDINAL CONTROLLER HERE
######################################################
######################################################
"""
Implement a longitudinal controller here. Remember that you can
access the persistent variables declared above here. For
example, can treat self.vars.v_previous like a "global variable".
"""
# Change these outputs with the longitudinal controller. Note that
# brake_output is optional and is not required to pass the
# assignment, as the car will naturally slow down over time.
sum_err_v = self.vars.sum_err_v
brake_output = 0
# diff_err_v = -self.vars.err_v
err_v = v_desired - v
sum_err_v += err_v
throttle_output = err_v + 0.01*sum_err_v
if throttle_output < 0:
throttle_output, brake_output = 0, 0
self.vars.sum_err_v = sum_err_v
######################################################
######################################################
# MODULE 7: IMPLEMENTATION OF LATERAL CONTROLLER HERE
######################################################
######################################################
"""
Implement a lateral controller here. Remember that you can
access the persistent variables declared above here. For
example, can treat self.vars.v_previous like a "global variable".
"""
# Change the steer output with the lateral controller.
steer_output = 0
cte = self.vars.cte
diff_cte = - cte
# ----------------------------------------
# compute the CTE
index = self._next_point - 1
R_x, R_y = x - waypoints[index][0], y - waypoints[index][1]
delta_x, delta_y = waypoints[index+1][0] - waypoints[index][0], waypoints[index+1][1] - waypoints[index][1]
u = (R_x * delta_x + R_y * delta_y)/np.sqrt((R_x**2 + R_y**2)*(delta_x**2 + delta_y**2))
cte = (R_y * delta_x - R_x * delta_y)/np.sqrt(delta_x**2 + delta_y**2)
self.vars.cte = cte
# ----------------------------------------
print('x: {:.2f} y: {:.2f} idx: {} u: {:.2f} yaw: {:.2f} delta: {}'.format(x,y,index,u, yaw, atan2(delta_y, delta_x)))
diff_cte += cte
K_p = 2
K_d = 6
K_yaw = 10
steer_output = - K_p * cte - K_d * diff_cte - K_yaw * (yaw-atan2(delta_y, delta_x))
######################################################
# SET CONTROLS OUTPUT
######################################################
self.set_throttle(throttle_output) # in percent (0 to 1)
self.set_steer(steer_output) # in rad (-1.22 to 1.22)
self.set_brake(brake_output) # in percent (0 to 1)
######################################################
######################################################
# MODULE 7: STORE OLD VALUES HERE (ADD MORE IF NECESSARY)
######################################################
######################################################
"""
Use this block to store old values (for example, we can store the
current x, y, and yaw values here using persistent variables for use
in the next iteration)
"""
self.vars.v_previous = v # Store forward speed to be used in next step
扫一扫
2754
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
