def lateral_vehicle_model(u_delta, v, v_dot, Ts, wheelbase, x0=0.0, y0=0.0, psi0=0.0, delta0=0.0, delta_disturbance=None): # add disturbance to steering if delta_disturbance is not None: delta_ = u_delta + delta_disturbance else: delta_ = u_delta # saturate steering delta = dy.saturate(u=delta_, lower_limit=-math.pi / 2.0, upper_limit=math.pi / 2.0) # bicycle model x, y, psi, x_dot, y_dot, psi_dot = discrete_time_bicycle_model( delta, v, Ts, wheelbase, x0, y0, psi0) # delta_dot = dy.diff(delta, initial_state=delta) / dy.float64(Ts) delta = dy.delay(delta, delta0) # compute acceleration in the point (x,y) espessed in the vehicle frame a_lat, a_long = compute_accelearation(v, v_dot, delta, delta_dot, psi_dot) return x, y, psi, delta, delta_dot, a_lat, a_long, x_dot, y_dot, psi_dot
def lateral_vehicle_model(u_delta, v, v_dot, Ts, wheelbase, x0=0.0, y0=0.0, psi0=0.0, delta0=0.0, delta_disturbance=None, delta_factor=None): """ Bicycle model and the acceleration at the front axle """ # add malicious factor to steering if delta_factor is not None: delta_ = u_delta * delta_factor else: delta_ = u_delta # add disturbance to steering if delta_disturbance is not None: delta_ = delta_ + delta_disturbance # saturate steering delta = dy.saturate(u=delta_, lower_limit=-math.pi/2.0, upper_limit=math.pi/2.0) # bicycle model x, y, psi, x_dot, y_dot, psi_dot = discrete_time_bicycle_model(delta, v, Ts, wheelbase, x0, y0, psi0) # delta_dot = dy.diff( delta, initial_state=delta ) / dy.float64(Ts) delta = dy.delay( delta, delta0 ) # compute acceleration in the point (x,y) in the vehicle frame a_lat, a_long = compute_acceleration( v, v_dot, delta, delta_dot, psi_dot ) return x, y, psi, delta, delta_dot, a_lat, a_long, x_dot, y_dot, psi_dot
# track the evolution of the closest point on the path to the vehicles position d_star, x_r, y_r, psi_r, K_r, Delta_l, tracked_index, Delta_index = track_projection_on_path( path, x, y) # reference for the lateral distance Delta_l_r = dy.float64(0.0) # zero in this example dy.append_output(Delta_l_r, 'Delta_l_r') # feedback control u = dy.PID_controller(r=Delta_l_r, y=Delta_l, Ts=0.01, kp=k_p) # path tracking # resulting lateral model u --> Delta_l : 1/s Delta_u = dy.asin(dy.saturate(u / velocity, -0.99, 0.99)) delta_star = psi_r - psi steering = delta_star + Delta_u steering = dy.unwrap_angle(angle=steering, normalize_around_zero=True) dy.append_output(Delta_u, 'Delta_u') dy.append_output(delta_star, 'delta_star') # # The model of the vehicle including a disturbance # # model the disturbance disturbance_transient = np.concatenate( (cosra(50, 0, 1.0), co(10, 1.0), cosra(50, 1.0, 0))) steering_disturbance, i = dy.play(
steering_rate = dy.system_input( dy.DataTypeFloat64(1), name='steering_rate', default_value=1.0, value_range=[-10, 10], title="steering_rate [degrees/s]") * dy.float64(math.pi / 180.0) initial_steering = dy.system_input( dy.DataTypeFloat64(1), name='initial_steering', default_value=-10.0, value_range=[-40, 40], title="initial steering angle [degrees]") * dy.float64(math.pi / 180.0) initial_orientation = dy.system_input( dy.DataTypeFloat64(1), name='initial_orientation', default_value=0.0, value_range=[-360, 360], title="initial orientation angle [degrees]") * dy.float64(math.pi / 180.0) # parameters wheelbase = 3.0 # sampling time Ts = 0.01 # create storage for the reference path: path = import_path_data(track_data) # linearly increasing steering angle delta = dy.euler_integrator( steering_rate, Ts, initial_state=initial_steering ) delta = dy.saturate(u=delta, lower_limit=-math.pi/2.0, upper_limit=math.pi/2.0) # the model of the vehicle x, y, psi, x_dot, y_dot, psi_dot = discrete_time_bicycle_model(delta, velocity, Ts, wheelbase, psi0=initial_orientation) # # outputs: these are available for visualization in the html set-up # dy.append_output(x, 'x') dy.append_output(y, 'y') dy.append_output(psi, 'psi') dy.append_output(delta, 'steering') # generate code for Web Assembly (wasm), requires emcc (emscripten) to build
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 path_lateral_modification2( input_path, par, Ts, wheelbase, velocity, Delta_l_r, Delta_l_r_dot, Delta_l_r_dotdot, d0, x0, y0, psi0, delta0, delta_dot0 ): """ Take an input path, modify it according to a given lateral distance profile, and generate a new path. Technically this combines a controller that causes an simulated vehicle to follows the input path with defined lateral modifications. Note: this implementation is meant as a callback routine for async_path_data_handler() """ # create placeholders for the plant output signals x = dy.signal() y = dy.signal() psi = dy.signal() psi_dot = dy.signal() if 'lateral_controller' not in par: # controller results = path_following_controller_P( input_path, x, y, psi, velocity, Delta_l_r = Delta_l_r, Delta_l_r_dot = Delta_l_r_dot, Delta_l_r_dotdot = Delta_l_r_dotdot, psi_dot = dy.delay(psi_dot), velocity_dot = dy.float64(0), Ts = Ts, k_p = 1 ) else: # path following and a user-defined linearising controller results = path_following( par['lateral_controller'], # callback to the implementation of P-control par['lateral_controller_par'], # parameters to the callback input_path, x, y, psi, velocity, Delta_l_r = Delta_l_r, Delta_l_r_dot = Delta_l_r_dot, Delta_l_r_dotdot = Delta_l_r_dotdot, psi_dot = dy.delay(psi_dot), velocity_dot = dy.float64(0), # velocity_dot Ts = Ts ) # # The model of the vehicle # with dy.sub_if( results['output_valid'], prevent_output_computation=False, subsystem_name='simulation_model') as system: results['output_valid'].set_name('output_valid') results['delta'].set_name('delta') # steering angle limit limited_steering = dy.saturate(u=results['delta'], lower_limit=-math.pi/2.0, upper_limit=math.pi/2.0) # the model of the vehicle x_, y_, psi_, x_dot, y_dot, psi_dot_ = discrete_time_bicycle_model( limited_steering, velocity, Ts, wheelbase, x0, y0, psi0 ) # driven distance d = dy.euler_integrator(velocity, Ts, initial_state=d0) # outputs model_outputs = dy.structure( d = d, x = x_, y = y_, psi = psi_, psi_dot = dy.delay(psi_dot_) # NOTE: delay introduced to avoid algebraic loops, wait for improved ORTD ) system.set_outputs(model_outputs.to_list()) model_outputs.replace_signals( system.outputs ) # close the feedback loops x << model_outputs['x'] y << model_outputs['y'] psi << model_outputs['psi'] psi_dot << model_outputs['psi_dot'] # output_path = dy.structure({ 'd' : model_outputs['d'], 'x' : model_outputs['x'], 'y' : model_outputs['y'], 'psi' : psi, 'psi_dot' : psi_dot, 'psi_r' : psi + results['delta'], # orientation angle of the path the vehicle is drawing 'K' : ( psi_dot + results['delta_dot'] ) / velocity , # curvature of the path the vehicle is drawing 'delta' : results['delta'], 'delta_dot' : results['delta_dot'], 'd_star' : results['d_star'], 'tracked_index' : results['tracked_index'], 'output_valid' : results['output_valid'], 'need_more_path_input_data' : results['need_more_path_input_data'], 'read_position' : results['read_position'], 'minimal_read_position' : results['minimal_read_position'] }) return output_path
path_index_start_open_loop_control = dy.sample_and_hold( tracked_index, event=dy.initial_event()) path_distance_start_open_loop_control = dy.sample_and_hold( d_star, event=dy.initial_event()) dy.append_output(path_index_start_open_loop_control, 'path_index_start_open_loop_control') dy.append_output(Delta_l, 'Delta_l') # # model vehicle braking # velocity = dy.euler_integrator( acceleration, Ts, initial_state=initial_velocity) * velocity_factor velocity = dy.saturate(velocity, lower_limit=0) dy.append_output(velocity, 'velocity') # # open-loop control # # estimate the travelled distance by integration of the vehicle velocity d_hat = dy.euler_integrator( velocity, Ts, initial_state=0) + path_distance_start_open_loop_control # estimated travelled distance (d_hat) to path-index open_loop_index, _, _ = tracker_distance_ahead( path, current_index=path_index_start_open_loop_control,
L_Delta_l = Ts/(z-1) Delta_l_model = z_tf( u, L_Delta_l ) dy.append_output(Delta_l_model, 'Delta_l_model') # path tracking # resulting lateral model u --> Delta_l : 1/s Delta_u = dy.asin( dy.saturate(u / velocity, -0.99, 0.99) ) steering = psi_r - psi + Delta_u steering = dy.unwrap_angle(angle=steering, normalize_around_zero = True) dy.append_output(Delta_u, 'Delta_u') dy.append_output(u, 'l_dot_r') # # safety function # # velocity: should be the projection of the vehicle velocity on the path? # compute nominal steering and steering angle from curvature
dy.append_output(Delta_l_mdl, 'Delta_l_mdl') # # path tracking control # # Control of the lateral distance to the path can be performed via the augmented control # variable u. # # Herein, a linearization yielding the resulting lateral dynamics u --> Delta_l : 1/s is applied. # Delta_u << dy.asin( dy.saturate(u / velocity, -0.99, 0.99) ) delta_star = psi_r - psi delta = delta_star + Delta_u delta = dy.unwrap_angle(angle=delta, normalize_around_zero = True) dy.append_output(Delta_u, 'Delta_u') dy.append_output(delta_star, 'delta_star') # # The model of the vehicle including a disturbance # # steering angle limit delta = dy.saturate(u=delta, lower_limit=-math.pi/2.0, upper_limit=math.pi/2.0)
def path_lateral_modification2(Ts, wheelbase, input_path, velocity, Delta_l_r, Delta_l_r_dot, Delta_l_r_dotdot): """ Take an input path, modify it according to a given lateral distance profile, and generate a new path. """ # create placeholders for the plant output signals x = dy.signal() y = dy.signal() psi = dy.signal() psi_dot = dy.signal() # controller results = path_following_controller_P( input_path, x, y, psi, velocity, Delta_l_r = Delta_l_r, Delta_l_r_dot = Delta_l_r_dot, Delta_l_r_dotdot = Delta_l_r_dotdot, psi_dot = dy.delay(psi_dot), velocity_dot = dy.float64(0), Ts = Ts, k_p = 1 ) # # The model of the vehicle including a disturbance # with dy.sub_if( results['output_valid'], prevent_output_computation=False, subsystem_name='simulation_model') as system: results['output_valid'].set_name('output_valid') results['delta'].set_name('delta') # steering angle limit limited_steering = dy.saturate(u=results['delta'], lower_limit=-math.pi/2.0, upper_limit=math.pi/2.0) # the model of the vehicle x_, y_, psi_, x_dot, y_dot, psi_dot_ = discrete_time_bicycle_model(limited_steering, velocity, Ts, wheelbase) # driven distance d = dy.euler_integrator(velocity, Ts) # outputs model_outputs = dy.structure( d = d, x = x_, y = y_, psi = psi_, psi_dot = dy.delay(psi_dot_) # NOTE: delay introduced to avoid algebraic loops, wait for improved ORTD ) system.set_outputs(model_outputs.to_list()) model_outputs.replace_signals( system.outputs ) # close the feedback loops x << model_outputs['x'] y << model_outputs['y'] psi << model_outputs['psi'] psi_dot << model_outputs['psi_dot'] # output_path = dy.structure({ 'd' : model_outputs['d'], 'x' : model_outputs['x'], 'y' : model_outputs['y'], 'psi' : psi, 'psi_dot' : psi_dot, 'psi_r' : psi + results['delta'], 'K' : ( psi_dot + results['delta_dot'] ) / velocity , 'delta' : results['delta'], 'delta_dot' : results['delta_dot'], 'd_star' : results['d_star'], 'tracked_index' : results['tracked_index'], 'output_valid' : results['output_valid'], 'need_more_path_input_data' : results['need_more_path_input_data'], 'read_position' : results['read_position'], 'minimal_read_position' : results['minimal_read_position'] }) return output_path
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