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 generate_one_dimensional_motion_ORTD(acceleration_input, Ts): # Delta_l_dotdot; the motion jerk is limited to +-1.0 m/s^3 acceleration = dy.rate_limit(acceleration_input, Ts, lower_limit = -1.0, upper_limit = 1.0) # Delta_l_dot velocity = dy.euler_integrator(acceleration, Ts, 0) # Delta_l lateral_distance = dy.euler_integrator(velocity, Ts, 0) return lateral_distance, velocity, acceleration
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 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 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
velocity = dy.system_input( dy.DataTypeFloat64(1), name='velocity', default_value=25.0, value_range=[0, 25], title="vehicle velocity [m/s]") 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')
# 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 if testname == 'oscillator_modulation_example': baseDatatype = dy.DataTypeFloat64(1) #loop_iterations = dy.system_input( baseDatatype ).set_name('loop_iterations').set_properties({ "range" : [0, 100], "default_value" : 20 })
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, 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()) 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,
# # meaning u is the lateral velocity to which is used to control the lateral # distance to the path. # # generate a velocity profile u_move_left = dy.signal_step( dy.int32(50) ) - dy.signal_step( dy.int32(200) ) u_move_right = dy.signal_step( dy.int32(500) ) - dy.signal_step( dy.int32(350) ) # apply a rate limiter to limit the acceleration u = dy.rate_limit( max_lateral_velocity * (u_move_left + u_move_right), Ts, dy.float64(-1) * max_lateral_accleration, max_lateral_accleration) dy.append_output(u, 'u') # internal lateral model (to verify the lateral dynamics of the simulated vehicle) Delta_l_mdl = dy.euler_integrator(u, Ts) 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. #
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