Introduction to Self-Driving Cars Final Project-W7_PID纵向控制+纯跟踪横向控制
Introduction to Self-Driving Cars Final Project-W7_PID+纯跟踪实现车辆横向和纵向控制
- Pure Pursuit简介
- Final项目中纯跟踪横向控制器
- contorller2d.py
本文是《Introduction to Self-Driving Cars Final Project-W7_PID实现车辆横向和纵向控制》的延续，旨在将车辆横向控制器改为纯跟踪（Pure pursuit）模式。（是Coursera自动驾驶系列课程第一部分的最后一个项目作业）
Pure Pursuit简介
纯跟踪及其变体是移动机器人路径跟踪问题常使用的方法，也常被用到自动驾驶汽车上，它的主要思路是计算车辆后轴中心和车辆期望跟踪点（目标点）间轨迹的曲率半径，进而得出跟踪路径所需的方向盘转角或前轮转角。
目标点由从当前后轴位置到所需路径的前视距离ld确定。目标点 (gx, gy) 如图所示。
使用目标点位置和车辆航向矢量与前视矢量之间的角度 α 来确定即可车辆的转向角 δ 。根据正弦定理，得到以下公式：
第一个式子为什么是2α？因为这个数学模型中，车辆轨迹近似成圆弧，在后轴点位置，后轴朝向与圆弧相切，又因为圆心、后轴中心、目标点构成的是一个等腰三角形，可以得出那个角是2α。最后一个式子中的κ是圆弧的曲率。根据阿克曼转向车辆的自行车模型，转向角可以写为：
根据上边两个式子，得出转角公式：
通过定义一个新变量，可以更好地理解该控制律，eld 代表航向矢量和目标点之间的横向距离，从而得到公式：
这时曲率公式可以写成：
这个公式说明了纯追踪其实是一个根据车辆前方一定前视距离ld的跟踪误差来计算转角的的比例控制器，控制器的增益为 2/ld^2。
在实践中，会根据不同的速度来调整稳定的增益（前视距离），这一点很好理解，参考人类驾驶员，当车速变化时，需要观察的远近是不同的，这也导致 ld 被指定为车辆速度的函数。重写控制律为：
这里用车辆的纵向速度来比例控制前视距离，这种方式是一种常见的做法。此外，前视距离通常在最小值和最大值处饱和。
以上就是纯跟踪控制的大部分公式。
Final项目中纯跟踪横向控制器
同样和上一篇文章类似，将纯跟踪控制器独立成一个类。
具体实现：
#pure_pursuit.py
import numpy as np
class PP(object):
def __init__(self,L, k, k_Ld):
self.L = L
self.k = k
self.k_Ld = k_Ld
def update(self, coeffs, velocity):
'''
coeffs：拟合跟踪曲线的系数，用来计算eld
'''
eps = 0.001
if velocity < eps:
steering_angle = 0.0
else:
# For Pure Pursuit if we look ahead distance changes, then the cte changes
Ld_x = self.k_Ld * velocity
cte = np.polyval(coeffs, Ld_x)
alpha = np.arctan(cte/Ld_x)
sin_alpha = np.sin(alpha)
steering_angle = np.arctan((2*self.L* sin_alpha) / (self.k * velocity))
return steering_angle
contorller2d.py
同样对controlle2d进行简单修改，主要是初始化PP控制器，然后用PP控制器更新方向盘转角。
#controller2d.py
#!/usr/bin/env python3
"""
2D Controller Class to be used for the CARLA waypoint follower demo.
"""
import cutils
import numpy as np
from pid import PID
from pure_pursuit import PP
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._conv_rad_to_steer = 180.0 / 70.0 / np.pi
self._pi = np.pi
self._2pi = 2.0 * np.pi
## PID
self.throttle_brake_pid = PID(P=7.0, I=1.0, D=1.026185)
self.throttle_brake_pid.setSampleTime = 0.033
## Pure Pursuit
self.pp = PP(L=4.5, k=1.00, k_Ld=1.3)
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_desired_speed(self):
min_idx = 0
min_dist = float("inf")
desired_speed = 0
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
if min_idx < len(self._waypoints) - 1:
desired_speed = self._waypoints[min_idx][2]
self.target_wp = self._waypoints[min_idx]
else:
desired_speed = self._waypoints[-1][2]
self.target_wp = self._waypoints[-1]
self._desired_speed = desired_speed
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 map_coord_2_Car_coord(self, x, y, yaw, waypoints):
wps = np.squeeze(waypoints)
wps_x = wps[:, 0]
wps_y = wps[:, 1]
num_wp = wps.shape[0]
## create the Matrix with 3 vectors for the waypoint x and y coordinates w.r.t. car
wp_vehRef = np.zeros(shape=(3, num_wp))
cos_yaw = np.cos(-yaw)
sin_yaw = np.sin(-yaw)
wp_vehRef[0, :] = cos_yaw * (wps_x - x) - sin_yaw * (wps_y - y)
wp_vehRef[1, :] = sin_yaw * (wps_x - x) + cos_yaw * (wps_y - y)
return wp_vehRef
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(, )'
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('v_previous', 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)
"""
wps_vehRef = self.map_coord_2_Car_coord(x, y, yaw, waypoints)
wps_vehRef_x = wps_vehRef[0, :]
wps_vehRef_y = wps_vehRef[1, :]
## fit a 3rd order polynomial to the waypoints
coeffs = np.polyfit(wps_vehRef_x, wps_vehRef_y, 7)
#vel_poly = np.polyfit(wps_vehRef_x, wps_vehRef_y, 3)
## get cross-track error from fit
# In vehicle coordinates the cross-track error is the intercept at x = 0, That means that since we have a fit of the form:
# Y = C0 + C1*X + C2*X^2 + C3X^3 + ....
# when we evaluate using x=0 we just get C0.
# But to understand where this is coming from I like to keep the whole evaluation, even though this is exactly C0
CarRef_x = CarRef_y = CarRef_yaw = 0.0
# For Pure Pursuit if we look ahead a distance Ld = nnnn then the cte changes
cte = np.polyval(coeffs, CarRef_x) - CarRef_y
# get orientation error from fit ( Since we are trying a 3rd order poly, then, f' = a1 + 2*a2*x + 3*a3*x2)
# in this case and since we moved our reference sys to the Car, x = 0 and also yaw = 0
yaw_err = CarRef_yaw - np.arctan(coeffs[1])
speed_err = v_desired - v
######################################################
######################################################
# MODULE 7
# LONGITUDINAL CONTROLLER
# AND
# LATERAL 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".
"""
"""
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".
"""
#### PID ####
#纵向控制PID
self.throttle_brake_pid.update(speed_err,
output_limits=[-1.0, 1.00])
if self.throttle_brake_pid.output < 0.0:
throttle_output = 0
brake_output = -self.throttle_brake_pid.output
else:
throttle_output = self.throttle_brake_pid.output
brake_output = 0
#### PURE PURSUIT ####
#横向控制PP
steer_output = self.pp.update(coeffs, v)
print("speed Err: ", speed_err)
print("cte : ", cte)
######################################################
# 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