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_path_orientation_from_curvature( Ts : float, velocity, psi_rr, K_r, L ): """ Compute the noise-reduced path orientation Psi_r from curvature Ts - the sampling time velocity - the driving velocity projected onto the path (d/dt d_star) psi_rr - noisy (e.g., due to sampling) path orientation K_r - path curvature L - gain for fusion using internal observer returns psi_r_reconst - the noise-reduced path orientation (reconstructed) psi_r_reconst_dot - the time derivative (reconstructed) """ eps = dy.signal() psi_r_reconst_dot = velocity * (K_r + eps) psi_r_reconst = dy.euler_integrator( u=psi_r_reconst_dot, Ts=Ts, initial_state=psi_rr ) # observer to compensate for integration error eps << (psi_rr - psi_r_reconst) * dy.float64(L) return psi_r_reconst, psi_r_reconst_dot
def euler_integrator( u : dy.Signal, Ts : float, name : str, initial_state = None): yFb = dy.signal() i = dy.add( [ yFb, u ], [ 1, Ts ] ).set_name(name + '_i') y = dy.delay( i, initial_state ).set_name(name + '_y') yFb << y return y
def dInt( u , name : str = ''): yFb = dy.signal() i = dy.add( [ yFb, u ], [ 1, 1 ] ).set_name(name + '_i') y = dy.delay( i).set_name(name + '_y') yFb << y return y
def firstOrder( u, z_inf, name : str = ''): yFb = dy.signal() i = dy.add( [ yFb, u ], [ z_inf, 1 ] ).set_name(name + '_i') y = dy.delay( i).set_name(name + '_y') yFb << y return y
def euler_integrator(u: dy.Signal, sampling_rate: float, initial_state=0.0): yFb = dy.signal() i = dy.add([yFb, u], [1, sampling_rate]) y = dy.delay(i, initial_state) yFb << y return y
def compute_nominal_steering_from_path_heading(Ts: float, l_r: float, v, psi_r): psi = dy.signal() delta = psi_r - psi psi_dot = v / dy.float64(l_r) * dy.sin(delta) psi << dy.euler_integrator(psi_dot, Ts) return delta, psi, psi_dot
def firstOrderAndGain( u, z_inf, gain, name : str = ''): dFb = dy.signal() s = dy.add( [ dFb, u ], [ z_inf, 1 ] ).set_name('s'+name+'') d = dy.delay( s).set_name('d'+name+'') dFb << d y = dy.gain( d, gain).set_name('y'+name+'') return y
def compute_nominal_steering_from_curvature( Ts : float, l_r : float, v, K_r ): """ compute the nominal steering angle and rate from given path heading and curvature signals. """ psi_dot = dy.signal() delta_dot = v * K_r - psi_dot delta = dy.euler_integrator( delta_dot, Ts ) psi_dot << (v / dy.float64(l_r) * dy.sin(delta)) return delta, delta_dot, psi_dot
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 compute_nominal_steering_from_path_heading( Ts : float, l_r : float, v, psi_r ): """ Compute the steering angle to follow a path given the path tangent angle Internally uses a (partial) model of the bicycle-vehicle to comput the optimal steering angle given the path orientation-angle. Internally, the orientation of the vehicle body (psi) is computed to determine the optimal steering angle. """ psi = dy.signal() delta = psi_r - psi psi_dot = v / dy.float64(l_r) * dy.sin(delta) psi << dy.euler_integrator( psi_dot, Ts ) return delta, psi, psi_dot
def tracker(path, x, y): index_track = dy.signal() with dy.sub_loop(max_iterations=1000) as system: search_index_increment = dy.int32( 1) # positive increment assuming positive velocity Delta_index = dy.sum(search_index_increment, initial_state=-1) Delta_index_previous_step = Delta_index - search_index_increment x_test = dy.memory_read(memory=path['X'], index=index_track + Delta_index) y_test = dy.memory_read(memory=path['Y'], index=index_track + Delta_index) distance = distance_between(x_test, y_test, x, y) distance_previous_step = dy.delay(distance, initial_state=100000) minimal_distance_reached = distance_previous_step < distance # introduce signal names distance.set_name('tracker_distance') minimal_distance_reached.set_name('minimal_distance_reached') # break condition system.loop_until(minimal_distance_reached) # return system.set_outputs([Delta_index_previous_step, distance_previous_step]) Delta_index = system.outputs[0].set_name('tracker_Delta_index') distance = system.outputs[1].set_name('distance') index_track_next = index_track + Delta_index index_track << dy.delay(index_track_next, initial_state=1) return index_track_next, Delta_index, distance
y6 = dInt( y5, name="int6") # define the outputs of the simulation output_signals = [ y6 ] # specify what the input signals shall be in the runtime input_signals_mapping = {} input_signals_mapping[ U ] = 1.0 if testname == 'test_oscillator': baseDatatype = dy.DataTypeFloat64(1) U = dy.system_input( baseDatatype ).set_name('extU') x = dy.signal() v = dy.signal() acc = dy.add( [ U, v, x ], [ 1, -0.1, -0.1 ] ).set_blockname('acc').set_name('acc') v << dy.euler_integrator( acc, Ts=0.1) x << dy.euler_integrator( v, Ts=0.1) # define the outputs of the simulation output_signals = [ x, v ] # specify what the input signals shall be in the runtime input_signals_mapping = {} input_signals_mapping[ U ] = 1.0
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(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_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
name='sample_disturbance', default_value=50, value_range=[0, 300], title="disturbance position") # parameters wheelbase = 3.0 # sampling time Ts = 0.01 # create storage for the reference path: path = import_path_data(track_data) # create placeholders for the plant output signals x = dy.signal() y = dy.signal() psi = dy.signal() # track the evolution of the closest point on the path to the vehicles position d_star, x_r, y_r, psi_rr, K_r, Delta_l, tracked_index, Delta_index, _ = track_projection_on_path( path, x, y) # # 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
def continuous_optimization_along_path(path, current_index, J, par): """ Minimize the given cost function by varying the index of the path array <----- Delta_index_track -----> array: X X X X X X X X X X X X X X X ^ current_index """ if 'Delta_d' in path: # constant sampling interval in distance # computation can be simplified pass # get the highest available array index in the horizon index_head, _ = path_horizon_head_index(path) # # # Delta_index_track = dy.signal() # initialize J_star J_star_0 = J(path, current_index + Delta_index_track, par) # # compute the direction (gradient) in which J has its decent # if true: with increasing index J increases --> decrease search index # if false: with increasing index J decreases --> increase search index # J_prev_index = J(path, current_index + Delta_index_track - 1, par) J_Delta_to_next_index = J_star_0 - J_prev_index search_index_increment = dy.conditional_overwrite( dy.int32(1), J_Delta_to_next_index > 0, dy.int32(-1)) # loop to find the minimum of J with dy.sub_loop(max_iterations=1000, subsystem_name='optim_loop') as system: # J_star(k) - the smallest J found so far J_star = dy.signal() # inc- / decrease the search index Delta_index_previous_step, Delta_index = dy.sum2( search_index_increment, initial_state=0) index_to_investigate = current_index + Delta_index_track + Delta_index # sample the cost function and check if it got smaller in this step J_to_verify = J(path, index_to_investigate, par) step_caused_improvement = J_to_verify < J_star # in case the step yielded a lower cost, replace the prev. minimal cost J_star_next = dy.conditional_overwrite(J_star, step_caused_improvement, J_to_verify) # state for J_star J_star << dy.delay(J_star_next, initial_state=J_star_0) # # loop break conditions # # when reaching the end of the available data, stop the loop and indicate the need for extending the horizon reached_the_end_of_currently_available_path_data = index_to_investigate >= index_head # reached the end of the input data? # similarly check for the begin ... reached_the_begin_of_currently_available_path_data = index_to_investigate - 1 <= path_horizon_tail_index( path)[0] # in case the iteration did not reduce the cost, assume that the minimum was reached in the prev. iteration reached_minimum = dy.logic_not(step_caused_improvement) system.loop_until( dy.logic_or( dy.logic_or(reached_minimum, reached_the_end_of_currently_available_path_data), reached_the_begin_of_currently_available_path_data).set_name( 'loop_until')) # assign signals names to appear in the generated source code J_star_0.set_name('J_star_0') search_index_increment.set_name('search_index_increment') J_star.set_name('J_star') Delta_index.set_name('Delta_index') index_head.set_name('index_head') index_to_investigate.set_name('index_to_investigate') J_to_verify.set_name('J_to_verify') step_caused_improvement.set_name('step_caused_improvement') # return outputs = dy.structure() outputs['Delta_index'] = Delta_index_previous_step outputs['J_star'] = J_star_next outputs['reached_minimum'] = reached_minimum outputs[ 'reached_the_end_of_currently_available_path_data'] = reached_the_end_of_currently_available_path_data outputs['index_head'] = index_head * 1 outputs['index_to_investigate'] = index_to_investigate outputs['J_to_verify'] = J_to_verify system.set_outputs(outputs.to_list()) outputs.replace_signals(system.outputs) Delta_index = outputs['Delta_index'] J_star = outputs['J_star'] reached_minimum = outputs['reached_minimum'] reached_the_end_of_currently_available_path_data = outputs[ 'reached_the_end_of_currently_available_path_data'] # Introduce dy.sink(signal) in ORTD to ensure the given signals is not optimized out and becomes visible in the debugging traces dummy = 0 * outputs['index_head'] + 0 * outputs[ 'index_to_investigate'] + 0 * outputs['J_to_verify'] Delta_index_track_next = Delta_index_track + Delta_index Delta_index_track << dy.delay( Delta_index_track_next, initial_state=1 ) # start at 1 so that the backwards gradient can be computed at index=1 Delta_index_track.set_name('Delta_index_track') # optimal index optimal_index = current_index + Delta_index_track_next results = dy.structure() results['optimal_index'] = optimal_index results['J_star'] = J_star + 0 * dummy results['Delta_index'] = Delta_index results['Delta_index_track_next'] = Delta_index_track_next results['reached_minimum'] = reached_minimum results[ 'reached_the_end_of_currently_available_path_data'] = reached_the_end_of_currently_available_path_data return results
def tracker(path, x, y): """ Continuously project the point (x, y) onto the given path (closest distance) This is an internal function. C.f. track_projection_on_path for details and assumptions. returns in structure tracking_results: tracked_index - the index in the path array for the closest distance to (x, y) Delta_index - the change of the index to the previous lookup distance - the absolute value of the closest distance of (x, y) to the path reached_the_end_of_currently_available_path_data - reached the end of the path """ index_head, _ = path_horizon_head_index(path) index_head.set_name('index_head') # the index that currently describes the index on the path with the closest distance to (x,y) index_track = dy.signal().set_name('index_track') with dy.sub_loop( max_iterations=200, subsystem_name='tracker_loop' ) as system: search_index_increment = dy.int32(1) # positive increment assuming positive velocity Delta_index = dy.sum(search_index_increment, initial_state=-1 ) Delta_index_previous_step = Delta_index - search_index_increment # the index at which to compute the distance to and see if it is the minimum index_to_investigate = index_track + Delta_index x_test, y_test = sample_path_xy(path, index_to_investigate) distance = distance_between( x_test, y_test, x, y ) distance_previous_step = dy.delay(distance, initial_state=100000) minimal_distance_reached = distance_previous_step < distance # introduce signal names distance.set_name('distance') minimal_distance_reached.set_name('minimal_distance_reached') # break condition reached_the_end_of_currently_available_path_data = index_to_investigate >= index_head # reached the end of the input data? system.loop_until( dy.logic_or( minimal_distance_reached, reached_the_end_of_currently_available_path_data ) ) # return system.set_outputs([ Delta_index_previous_step.set_name('Delta_index_previous_step'), distance_previous_step.set_name('distance_previous_step'), minimal_distance_reached.set_name('minimal_distance_reached'), reached_the_end_of_currently_available_path_data.set_name('reached_the_end_of_currently_available_path_data') ]) Delta_index = system.outputs[0].set_name('tracker_Delta_index') distance = system.outputs[1].set_name('distance') minimal_distance_reached = system.outputs[2].set_name('minimal_distance_reached') reached_the_end_of_currently_available_path_data = system.outputs[3].set_name('reached_the_end_of_currently_available_path_data') # update current state of index_track index_track_next = index_track + Delta_index index_track << dy.delay(index_track_next, initial_state=1) # tracking_results = dy.structure() tracking_results['tracked_index'] = index_track_next tracking_results['Delta_index'] = Delta_index tracking_results['distance'] = distance tracking_results['minimal_distance_reached'] = minimal_distance_reached tracking_results['reached_the_end_of_currently_available_path_data'] = reached_the_end_of_currently_available_path_data # return index_track_next, Delta_index, distance, minimal_distance_reached, reached_the_end_of_currently_available_path_data return tracking_results
k_p = dy.system_input(baseDatatype).set_name('k_p').set_properties({ "range": [0, 1.0], "default_value": 0.33 }) wheelbase = 3.0 # generate a step-wise reference signal pwm_signal, state_control = generate_signal_PWM(period=dy.float64(200), modulator=dy.float64(0.5)) reference = (pwm_signal - dy.float64(0.5)) * dy.float64(1.0) reference.set_name('reference') # create placeholder for the plant output signal 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)
v = dy.float64(0.0).set_name('v_def') counter = dy.counter().set_name('counter') timeout = ( counter > number_of_samples_to_stay_in_A ).set_name('timeout') next_state = dy.conditional_overwrite(signal=dy.int32(-1), condition=timeout, new_value=1 ).set_name('next_state') system.set_switched_outputs([ x, v, counter ], next_state) with switch.new_subsystem('state_B') as system: # implement a simple spring-mass oscillator: # x is the position, v is the velocity, acc is the acceleration # create placeholder symbols for x and v (as they are used before being defined) x = dy.signal() v = dy.signal() acc = dy.add( [ U, v, x ], [ 1, -0.1, -0.1 ] ).set_blockname('acc').set_name('acc') # close the feedback loops for x and v v << euler_integrator( acc, Ts=0.1, name="intV", initial_state=-1.0 ) x << euler_integrator( v, Ts=0.1, name="intX" ) leave_this_state = (x > threshold_for_x_to_leave_B).set_name("leave_this_state") next_state = dy.conditional_overwrite(signal=dy.int32(-1), condition=leave_this_state, new_value=0 ).set_name('next_state') counter = dy.counter().set_name('counter') system.set_switched_outputs([ x, v, counter ], next_state)
def tracker_distance_ahead(path, current_index, distance_ahead): """ <----- Delta_index_track -----> array: X X X X X X X X X X X X X X X ^ current_index """ if 'Delta_d' in path: # constant sampling interval in distance # computation can be simplified pass target_distance = dy.float64(distance_ahead) + dy.memory_read( memory=path['D'], index=current_index) def J(index): d_test = dy.memory_read(memory=path['D'], index=index) distance = dy.abs(d_test - target_distance) return distance Delta_index_track = dy.signal() # initialize J_star J_star_0 = J(current_index + Delta_index_track) J_star_0.set_name('J_star_0') # # compute the direction in which J has its decent # if true: with increasing index J increases --> decrease search index # if false: with increasing index J decreases --> increase search index # J_next_index = J(current_index + Delta_index_track + dy.int32(1)) J_Delta_to_next_index = J_next_index - J_star_0 direction_flag = J_Delta_to_next_index > dy.float64(0) search_index_increment = dy.int32(1) search_index_increment = dy.conditional_overwrite(search_index_increment, direction_flag, dy.int32(-1)) search_index_increment.set_name('search_index_increment') # loop to find the minimum of J with dy.sub_loop(max_iterations=1000) as system: # J_star(k) - the smallest J found so far J_star = dy.signal() # inc- / decrease the search index Delta_index_prev_it, Delta_index = dy.sum2(search_index_increment, initial_state=0) Delta_index.set_name('Delta_index') # sample the cost function and check if it got smaller in this step J_to_verify = J(current_index + Delta_index_track + Delta_index) J_to_verify.set_name('J_to_verify') step_caused_improvment = J_to_verify < J_star # replace the J_star_next = dy.conditional_overwrite(J_star, step_caused_improvment, J_to_verify) # state for J_star J_star << dy.delay(J_star_next, initial_state=J_star_0).set_name('J_star') # loop break condition system.loop_until(dy.logic_not(step_caused_improvment)) # return the results computed in the loop system.set_outputs([Delta_index_prev_it, J_to_verify, J_star]) Delta_index = system.outputs[0] Delta_index_track_next = Delta_index_track + Delta_index Delta_index_track << dy.delay(Delta_index_track_next, initial_state=0) Delta_index_track.set_name('Delta_index_track') # compute the residual distance optimal_distance = dy.memory_read(memory=path['D'], index=current_index + Delta_index_track_next) distance_residual = target_distance - optimal_distance return Delta_index_track_next, distance_residual, Delta_index
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
name='activate_IMU', default_value=0, value_range=[0, 1], title="mode: activate IMU") # parameters wheelbase = 3.0 # sampling time Ts = 0.01 # create storage for the reference path: path = import_path_data(track_data) # create placeholders for the plant output signals x_real = dy.signal() y_real = dy.signal() psi_measurement = dy.signal() # # track the evolution of the closest point on the path to the vehicles position # note: this is only used to initialize the open-loop control with the currect vehicle position on the path # d_star, x_r, y_r, psi_r, K_r, Delta_l, tracked_index, Delta_index = track_projection_on_path( path, x_real, y_real) 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())
ofs = dy.float64(0.1).set_name('ofs') # define system inputs friction = dy.system_input(baseDatatype).set_name('friction') * dy.float64( 0.05) + ofs mass = dy.system_input(baseDatatype).set_name('mass') * dy.float64(0.05) + ofs length = dy.system_input(baseDatatype).set_name('length') * dy.float64( 0.05) + ofs # length = dy.float64(0.3) g = dy.float64(9.81).set_name('g') # create placeholder for the plant output signal angle = dy.signal() angular_velocity = dy.signal() angular_acceleration = dy.float64( 0) - g / length * dy.sin(angle) - (friction / (mass * length)) * angular_velocity angular_acceleration.set_name('angular_acceleration') sampling_rate = 0.01 angular_velocity_ = euler_integrator(angular_acceleration, sampling_rate, 0.0).set_name('ang_vel') angle_ = euler_integrator(angular_velocity_, sampling_rate, 30.0 / 180.0 * math.pi).set_name('ang') angle << angle_
name='sample_disturbance', default_value=50, value_range=[0, 300], title="disturbance position") # parameters wheelbase = 3.0 # sampling time Ts = 0.01 # create storage for the reference path: path = import_path_data(track_data) # create placeholders for the plant output signals x = dy.signal() y = dy.signal() psi = dy.signal() # 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)
# parameters wheelbase = 3.0 Ts=0.01 # driving with constant velocity v_dot=dy.float64(0) # create storage for the reference path: path = import_path_data(track_data) # create placeholders for the plant output signals x = dy.signal() y = dy.signal() psi = dy.signal() psi_dot = dy.signal() delta = dy.signal() a_lat = dy.signal() # track the evolution of the closest point on the path to the vehicles position tracked_index, Delta_index, closest_distance = tracker(path, x, y) second_closest_distance, index_second_star = find_second_closest( path, x, y, index_star=tracked_index ) interpolated_closest_distance = compute_distance_from_linear_interpolation( second_closest_distance, closest_distance ) dy.append_output(interpolated_closest_distance, 'interpolated_closest_distance') dy.append_output(second_closest_distance, 'second_closest_distance')