CARLA 轨道跟踪
轨道形状每个轨道点的巡航速度要求longitudinal controller 控制油门和刹车，用了 PI 控制器lateral controller 控制方向盘，用了 PD 控制器#!/usr/bin/env python3"""2D Controller Class to be used for the CARLA waypoint follower demo."""...
·
轨道形状
每个轨道点的巡航速度要求
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
开放原子开发者工作坊旨在鼓励更多人参与开源活动，与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动，如meetup、训练营等，主打技术交流，干货满满，真诚地邀请各位开发者共同参与！
更多推荐
3
0- 0
已为社区贡献8条内容
所有评论(0)