def compute_steering_constraints( v, v_dot, psi_dot, delta, a_l_min, a_l_max ):
"""
Compute constraints for the steering angle and its rate so that the acceleration is bounded
delta - the steering angle state of the vehicle (i.e., not the unsaturated control command)
"""
delta_min = dy.float64(-1.0)
delta_max = dy.float64(1.0)
# note: check this proper min/max
delta_dot_min = ( a_l_min - v_dot * dy.sin(delta) ) / ( v * dy.cos(delta) ) - psi_dot
delta_dot_max = ( a_l_max - v_dot * dy.sin(delta) ) / ( v * dy.cos(delta) ) - psi_dot
return delta_min, delta_max, delta_dot_min, delta_dot_max
def compute_steering_constraints(v, v_dot, psi_dot, delta, a_l_min, a_l_max):
"""
delta - steering state of the vehicle (i.e., not the unsaturated control command)
"""
delta_min = dy.float64(-1.0)
delta_max = dy.float64(1.0)
# note: check this proper min/max
delta_dot_min = (a_l_min -
v_dot * dy.sin(delta)) / (v * dy.cos(delta)) - psi_dot
delta_dot_max = (a_l_max -
v_dot * dy.sin(delta)) / (v * dy.cos(delta)) - psi_dot
return delta_min, delta_max, delta_dot_min, delta_dot_max
def bicycle_model(delta, v):
x = dy.signal()
y = dy.signal()
psi = dy.signal()
# bicycle model
tmp = delta + psi
tmp.set_name('tmp')
print()
# x_dot = v * dy.cos( delta + psi )
# y_dot = v * dy.sin( delta + psi )
x_dot = v * dy.cos(tmp)
y_dot = v * dy.sin(tmp)
psi_dot = v / dy.float64(wheelbase) * dy.sin(delta)
x_dot.set_name('x_dot')
y_dot.set_name('y_dot')
psi_dot.set_name('psi_dot')
# integrators
sampling_rate = 0.01
x << dy.euler_integrator(x_dot, sampling_rate, 0.0)
y << dy.euler_integrator(y_dot, sampling_rate, 0.0)
psi << dy.euler_integrator(psi_dot, sampling_rate, 0.0)
return x, y, psi
def discrete_time_bicycle_model(delta, v, Ts, wheelbase, x0=0.0, y0=0.0, psi0=0.0):
"""
Implement an ODE solver (Euler) for the kinematic bicycle model equations
x, y - describes the position of the front axle,
delta - the steering angle
v - the velocity measured on the front axle
wheelbase - the distance between front- and rear axle
Ts - the sampling time for the Euler integration
psi - the orientation of the carbody
(x0, y0, psi0) - the initial states of the ODEs
"""
x = dy.signal()
y = dy.signal()
psi = dy.signal()
# bicycle model
x_dot = v * dy.cos( delta + psi )
y_dot = v * dy.sin( delta + psi )
psi_dot = v / dy.float64(wheelbase) * dy.sin( delta )
# integrators
x << dy.euler_integrator(x_dot, Ts, x0)
y << dy.euler_integrator(y_dot, Ts, y0)
psi << dy.euler_integrator(psi_dot, Ts, psi0)
return x, y, psi, x_dot, y_dot, psi_dot
def compute_acceleration( v, v_dot, delta, delta_dot, psi_dot ):
"""
Compute the acceleration at the front axle
"""
a_lat = v_dot * dy.sin( delta ) + v * ( delta_dot + psi_dot ) * dy.cos( delta )
a_long = None
return a_lat, a_long
def project_velocity_on_path(velocity, Delta_u, Delta_l, K_r):
"""
Compute the velocity of the closest point on the path
"""
#
# This evaluates the formula
#
# v_star = d d_star / dt = v * cos( Delta_u ) / ( 1 - Delta_l * K(d_star) )
#
v_star = velocity * dy.cos( Delta_u ) / ( dy.float64(1) - Delta_l * K_r )
return v_star
def discrete_time_bicycle_model(delta,
v,
Ts,
wheelbase,
x0=0.0,
y0=0.0,
psi0=0.0):
x = dy.signal()
y = dy.signal()
psi = dy.signal()
# bicycle model
x_dot = v * dy.cos(delta + psi)
y_dot = v * dy.sin(delta + psi)
psi_dot = v / dy.float64(wheelbase) * dy.sin(delta)
# integrators
x << dy.euler_integrator(x_dot, Ts, x0)
y << dy.euler_integrator(y_dot, Ts, y0)
psi << dy.euler_integrator(psi_dot, Ts, psi0)
return x, y, psi, x_dot, y_dot, psi_dot
def distance_to_line(x_s, y_s, x_e, y_e, x_test, y_test):
"""
compute the shortest distance to a line
returns a negative distance in case (x_test, y_test) is to the left of the vector pointing from
(x_s, y_s) to (x_e, y_e)
"""
Delta_x = x_e - x_s
Delta_y = y_e - y_s
x_test_ = x_test - x_s
y_test_ = y_test - y_s
psi = dy.atan2(Delta_y, Delta_x)
test_ang = dy.atan2(y_test_, x_test_)
delta_angle = dy.unwrap_angle(test_ang - psi, normalize_around_zero=True)
length_s_to_test = dy.sqrt(x_test_ * x_test_ + y_test_ * y_test_)
distance = dy.sin(delta_angle) * length_s_to_test
distance_s_to_projection = dy.cos(delta_angle) * length_s_to_test
return distance, distance_s_to_projection
def path_following(
controller,
par,
path,
x, y, psi, velocity,
Delta_l_r = 0.0,
Delta_l_r_dot = None,
psi_dot = None,
velocity_dot = None,
Delta_l_r_dotdot = None,
Ts=0.01
):
"""
Basic steering control for path tracking and user-defined lateral error compensation
Implements steering control for exact path following.
Assumed is a kinematic bicycle model.
Herein, the steering angle (delta) is the control variable. The variables
x, y, psi, and velocity are measurements taken from the controlled system.
The lateral offset Delta_l_r to the path is the reference for control.
The optional signal Delta_l_r_dot describes the time derivative of Delta_l_r.
controller - callback function that defines the error compensating controller
par - parameters that are passed to the callback
Ts - the sampling time
Return values
-------------
results = {}
results['x_r'] # the current x-position of the closest point on the reference path
results['y_r'] # the current y-position of the closest point on the reference path
results['v_star'] # the current velocity of the closest point on the reference path
results['d_star'] # the current distance parameter of the closest point on the reference path
results['psi_r'] # the current path-tangent orientation angle in the closest point on the reference path
results['psi_r_dot'] # the time derivative of psi_r
results['Delta_l_r'] # the reference to the distance to the path
results['Delta_l_r_dot'] # optional: the time derivative of Delta_l_r
results['Delta_l'] # the distance to the closest point on the reference path
results['Delta_u'] # small steering delta
results['delta'] # the requested steering angle / the control variable
in case Delta_l_r_dot, psi_dot, velocity_dot, and Delta_l_r_dotdot are given
the steering derivatives can be computed.
results['Delta_u_dot'] # the derivative of Delta_u
results['delta_dot'] # the derivative of delta_dot
"""
index_head, _ = path_horizon_head_index(path)
# structure for output signals
results = dy.structure()
# track the evolution of the closest point on the path to the vehicles position
minimal_number_of_path_samples_to_start = 5 # depends on tracker(); should be at least 2
with dy.sub_if( index_head > minimal_number_of_path_samples_to_start, subsystem_name='tracker' ) as system:
tracking_results = tracker(path, x, y)
system.set_outputs( tracking_results.to_list() )
tracking_results.replace_signals( system.outputs )
output_valid = tracking_results['minimal_distance_reached']
need_more_path_input_data = tracking_results['reached_the_end_of_currently_available_path_data']
# in case the lookup was successful, run further operations on the path
# to generate references and run the controller.
with dy.sub_if( output_valid, prevent_output_computation=False, subsystem_name='controller') as system:
# ps - path sample
ps = track_projection_on_path(path, x, y, tracking_results = tracking_results)
#
# project the vehicle velocity onto the path yielding v_star
#
# Used formula inside project_velocity_on_path:
# v_star = d d_star / dt = v * cos( Delta_u ) / ( 1 - Delta_l * K(d_star) )
#
Delta_u = dy.signal() # feedback from control
v_star = project_velocity_on_path(velocity, Delta_u, ps['Delta_l'], ps['K_r'])
#
# compute an enhanced (less noise) signal for the path orientation psi_r by integrating the
# curvature profile and fusing the result with ps['psi_r'] to mitigate the integration drift.
#
psi_r, psi_r_dot = compute_path_orientation_from_curvature( Ts, v_star, ps['psi_r'], ps['K_r'], L=1.0 )
#
# controller callback
#
references = {
'Delta_l_r' : Delta_l_r,
'Delta_l_r_dot' : Delta_l_r_dot,
'Delta_l_r_dotdot' : Delta_l_r_dotdot
}
# Delta_l_dot might be further computed which improves the accuracy of the derivatives
# in case of strong feedback control activity.
Delta_l_dot = None # TODO: implement
measurements = {
'velocity' : velocity,
'velocity_dot' : velocity_dot,
'psi' : psi,
'psi_dot' : psi_dot,
'Delta_l' : ps['Delta_l'],
'Delta_l_dot' : Delta_l_dot
}
u, u_dot = controller( references, measurements, par )
#
# path tracking
#
# resulting lateral model u --> Delta_l : 1/s
#
Delta_u << dy.asin( dy.saturate(u / velocity, -0.99, 0.99) )
delta = dy.unwrap_angle(angle=psi_r - psi + Delta_u, normalize_around_zero = True)
# compute the derivatives of the steering angle (steering rate)
if psi_dot is not None and Delta_l_r_dot is not None and velocity_dot is not None and Delta_l_r_dotdot is not None:
Delta_u_dot = dy.cos( u / velocity ) * ( velocity * u_dot - velocity_dot * u ) / ( velocity*velocity )
delta_dot = psi_r_dot - psi_dot + Delta_u_dot
results['Delta_u_dot'] = Delta_u_dot
results['delta_dot'] = delta_dot
# collect resulting signals
results['x_r'] = ps['x_r'] # the current x-position of the closest point on the reference path
results['y_r'] = ps['y_r'] # the current y-position of the closest point on the reference path
results['v_star'] = v_star # the current velocity of the closest point on the reference path
results['d_star'] = ps['d_star'] # the current distance parameter of the closest point on the reference path
results['psi_r'] = psi_r # the current path-tangent orientation angle in the closest point on the reference path
results['K_r'] = ps['K_r'] # the curvature
results['psi_r_dot'] = psi_r_dot # the time derivative of psi_r
results['Delta_u'] = Delta_u # small steering delta
results['delta'] = delta # the requested steering angle / the control variable
results['Delta_l'] = ps['Delta_l'] # the distance to the closest point on the reference path
results['Delta_l_dot'] = dy.float64(math.nan) # d/dt Delta_l TODO: implement
# results['line_tracking_internals'] = ps['internals']
# return
system.set_outputs( results.to_list() )
results.replace_signals( system.outputs )
results['tracked_index'] = tracking_results['tracked_index']
results['Delta_l_r'] = Delta_l_r # the reference to the distance to the path
results['need_more_path_input_data'] = need_more_path_input_data
results['output_valid'] = output_valid
results['read_position'] = results['tracked_index'] + 1
results['minimal_read_position'] = results['read_position'] - 100
return results
def compute_accelearation(v, v_dot, delta, delta_dot, psi_dot):
a_lat = v_dot * dy.sin(delta) + v * (delta_dot + psi_dot) * dy.cos(delta)
a_long = None
return a_lat, a_long
x = dy.signal().set_name('x')
y = dy.signal().set_name('y')
psi = dy.signal().set_name('psi')
# controller error
error = reference - y
error.set_name('error')
steering = dy.float64(0.0) + k_p * error - psi
steering.set_name('steering')
sw = False
if sw:
x_dot = velocity * dy.cos(steering + psi)
y_dot = velocity * dy.sin(steering + psi)
psi_dot = velocity / dy.float64(wheelbase) * dy.sin(steering)
# integrators
sampling_rate = 0.01
x << euler_integrator(x_dot, sampling_rate, 0.0)
y << euler_integrator(y_dot, sampling_rate, 0.0)
psi << euler_integrator(psi_dot, sampling_rate, 0.0)
else:
def bicycle_model(delta, v):
x = dy.signal()
y = dy.signal()
psi = dy.signal()
def path_following_controller_P( path, x, y, psi, velocity, Delta_l_r = 0.0, Delta_l_r_dot = None, k_p=2.0, Ts=0.01, psi_dot = None, velocity_dot = None, Delta_l_r_dotdot = None, Delta_l_dot = None ):
"""
Basic steering control for path tracking using proportional lateral error compensation
Path following steering control for exact path following and P-control to control the lateral
distance to the path are combined.
Controlls a kinematic bicycle model (assumption) to follow the given path.
Herein, the steering angle delta is the control variable. The variables
x, y, psi, and velocity are measurements taken from the controlled system.
The lateral offset Delta_l_r to the path is the reference. The optional
signal Delta_l_r_dot describes the time derivative of Delta_l_r.
Ts - the sampling time
Return values
-------------
results = {}
results['x_r'] # the current x-position of the closest point on the reference path
results['y_r'] # the current y-position of the closest point on the reference path
results['v_star'] # the current velocity of the closest point on the reference path
results['d_star'] # the current distance parameter of the closest point on the reference path
results['psi_r'] # the current path-tangent orientation angle in the closest point on the reference path
results['psi_r_dot'] # the time derivative of psi_r
results['Delta_l_r'] # the reference to the distance to the path
results['Delta_l_r_dot'] # optional: the time derivative of Delta_l_r
results['Delta_l'] # the distance to the closest point on the reference path
results['Delta_u'] # small steering delta
results['delta'] # the requested steering angle / the control variable
in case Delta_l_r_dot, psi_dot, velocity_dot, and Delta_l_r_dotdot are given
the steering derivatives can be computed.
results['Delta_u_dot'] # the derivative of Delta_u
results['delta_dot'] # the derivative of delta_dot
Optionally, Delta_l_dot might be further given which improves the accuracy of the derivatives
in case of strong feedback control activity.
"""
index_head, _ = path_horizon_head_index(path)
# structure for output signals
results = dy.structure()
# track the evolution of the closest point on the path to the vehicles position
with dy.sub_if( index_head > 10 ) as system:
tracking_results = tracker(path, x, y)
system.set_outputs( tracking_results.to_list() )
tracking_results.replace_signals( system.outputs )
output_valid = tracking_results['minimal_distance_reached']
need_more_path_input_data = tracking_results['reached_the_end_of_currently_available_path_data']
# position_on_path_found = dy.boolean(True)
with dy.sub_if( output_valid, prevent_output_computation=False, subsystem_name='controller') as system:
# ps - path sample
ps = track_projection_on_path(path, x, y, tracking_results = tracking_results)
#
# project the vehicle velocity onto the path yielding v_star
#
# Used formula inside project_velocity_on_path:
# v_star = d d_star / dt = v * cos( Delta_u ) / ( 1 - Delta_l * K(d_star) )
#
Delta_u = dy.signal() # feedback from control
v_star = project_velocity_on_path(velocity, Delta_u, ps['Delta_l'], ps['K_r'])
#
# compute an enhanced (less noise) signal for the path orientation psi_r by integrating the
# curvature profile and fusing the result with ps['psi_r'] to mitigate the integration drift.
#
psi_r, psi_r_dot = compute_path_orientation_from_curvature( Ts, v_star, ps['psi_r'], ps['K_r'], L=1.0 )
# feedback control
u_fb = k_p * (Delta_l_r - ps['Delta_l'])
if Delta_l_r_dot is not None:
u = Delta_l_r_dot + u_fb
else:
u = u_fb
# path tracking
# resulting lateral model u --> Delta_l : 1/s
Delta_u << dy.asin( dy.saturate(u / velocity, -0.99, 0.99) )
delta = dy.unwrap_angle(angle=psi_r - psi + Delta_u, normalize_around_zero = True)
# compute the derivatives of the steering angle (steering rate)
if psi_dot is not None and Delta_l_r_dot is not None and velocity_dot is not None and Delta_l_r_dotdot is not None:
if Delta_l_dot is None:
u_dot = Delta_l_r_dotdot # + 0 neglect numerical random walk error compensation
else:
u_dot = Delta_l_r_dotdot + Delta_l_dot
Delta_u_dot = dy.cos( u / velocity ) * ( velocity * u_dot - velocity_dot * u ) / ( velocity*velocity )
delta_dot = psi_r_dot - psi_dot + Delta_u_dot
results['Delta_u_dot'] = Delta_u_dot
results['delta_dot'] = delta_dot
# collect resulting signals
results['x_r'] = ps['x_r'] # the current x-position of the closest point on the reference path
results['y_r'] = ps['y_r'] # the current y-position of the closest point on the reference path
results['v_star'] = v_star # the current velocity of the closest point on the reference path
results['d_star'] = ps['d_star'] # the current distance parameter of the closest point on the reference path
results['psi_r'] = psi_r # the current path-tangent orientation angle in the closest point on the reference path
results['psi_r_dot'] = psi_r_dot # the time derivative of psi_r
results['Delta_l'] = ps['Delta_l'] # the distance to the closest point on the reference path
results['Delta_u'] = Delta_u # small steering delta
results['delta'] = delta # the requested steering angle / the control variable
# results['line_tracking_internals'] = ps['internals']
# return
system.set_outputs( results.to_list() )
results.replace_signals( system.outputs )
results['tracked_index'] = tracking_results['tracked_index']
results['Delta_l_r'] = Delta_l_r # the reference to the distance to the path
results['need_more_path_input_data'] = need_more_path_input_data
results['output_valid'] = output_valid
results['read_position'] = results['tracked_index'] + 1
results['minimal_read_position'] = results['read_position'] - 100
return results