% main_simulation.m
% Main script to run the autonomous driving simulation.
clear; clc; close all;
% --- 1. Define Environment and Vehicle Parameters ---
% Load environment data
run(\'define_environment.m\'); % This script creates \'environment_data\' struct
% Vehicle parameters
vehicle_L = 2.8; % Wheelbase (meters)
vehicle_width = 1.8; % meters
vehicle_length = 4.5; % meters
% Sensor parameters
% LiDAR
lidar_params = struct(...
\'range\', 50, ... % meters
\'angular_resolution\', deg2rad(0.5), ... % radians
\'fov\', deg2rad(180), ... % radians
\'noise_std\', 0.1 ... % meters
);
% Camera
camera_params = struct(...
\'fov_h\', deg2rad(60), ... % Horizontal FOV
\'fov_v\', deg2rad(40), ... % Vertical FOV
\'max_detection_range\', 80, ... % meters
\'detection_prob\', 0.95 ...
);
% Radar
radar_params = struct(...
\'range\', 150, ... % meters
\'fov\', deg2rad(30), ... % radians
\'noise_std_dist\', 0.5, ... % meters
\'noise_std_vel\', 0.1 ... % m/s
);
% Sensor processing parameters
processing_params = struct(...
\'ground_plane_threshold\', 0.5, ... % Max Y-offset from ground for non-ground points
\'cluster_epsilon\', 1.5, ... % Max distance for clustering points (meters)
\'cluster_min_points\', 5 ... % Min points to form a cluster
);
% Path planning parameters
planning_params = struct(...
\'grid_resolution\', 0.5, ... % meters per grid cell
\'inflation_radius\', 1.0 ... % meters, for obstacle inflation
);
% --- 2. Simulation Setup ---
% Initial vehicle state: [x, y, yaw, v]
vehicle_state = [0, 0, 0, 0]; % Start at origin, facing +X, zero velocity
% Goal pose
goal_pose = [180, 0, 0]; % Example goal: straight ahead
% Simulation time parameters
total_sim_time = 200; % seconds
dt = 0.1; % Time step (seconds)
% Store trajectory for plotting
trajectory_x = [];
trajectory_y = [];
% --- 3. Visualization Setup ---
figure(\'Name\', \'Autonomous Driving Simulation\', \'NumberTitle\', \'off\');
ax = gca; % Get current axes handle
hold(ax, \'on\');
grid(ax, \'on\');
axis(ax, \'equal\'); % Maintain aspect ratio
xlabel(ax, \'X (m)\');
ylabel(ax, \'Y (m)\');
title(ax, \'Autonomous Driving Simulation\');
% Plot initial environment
plot(ax, environment_data.road_center_x, environment_data.road_center_y, \'k--\', \'LineWidth\', 1, \'DisplayName\', \'Road Center\');
plot(ax, environment_data.road_left_x, environment_data.road_left_y, \'k-\', \'LineWidth\', 1, \'DisplayName\', \'Road Boundary\');
plot(ax, environment_data.road_right_x, environment_data.road_right_y, \'k-\', \'LineWidth\', 1);
% Plot static obstacles
for i = 1:length(environment_data.obstacles)
obs = environment_data.obstacles{i};
if strcmp(obs.type, \'rectangle\')
rectangle(ax, \'Position\', [obs.x - obs.width/2, obs.y - obs.height/2, obs.width, obs.height], ...
\'FaceColor\', obs.color, \'EdgeColor\', \'k\', \'DisplayName\', \'Obstacle\');
end
end
% Initialize plot handles for dynamic elements
h_vehicle = plot(ax, vehicle_state(1), vehicle_state(2), \'ro\', \'MarkerSize\', 10, \'MarkerFaceColor\', \'r\', \'DisplayName\', \'Vehicle\');
h_trajectory = plot(ax, [], [], \'b-\', \'LineWidth\', 1.5, \'DisplayName\', \'Vehicle Trajectory\');
h_lidar_points = plot(ax, [], [], \'g.\', \'MarkerSize\', 5, \'DisplayName\', \'LiDAR Points\');
h_camera_bboxes = []; % Will be updated dynamically
h_radar_targets = plot(ax, [], [], \'m*\', \'MarkerSize\', 8, \'DisplayName\', \'Radar Targets\');
h_planned_path = plot(ax, [], [], \'c--\', \'LineWidth\', 2, \'DisplayName\', \'Planned Path\');
legend(ax, \'show\', \'Location\', \'best\');
% Set initial view limits (adjust as needed)
xlim(ax, [-10, 210]);
ylim(ax, [-30, 30]);
% --- 4. Main Simulation Loop ---
for t = 0:dt:total_sim_time
% Current vehicle pose
current_x = vehicle_state(1);
current_y = vehicle_state(2);
current_yaw = vehicle_state(3);
current_v = vehicle_state(4);
current_pose = [current_x, current_y, current_yaw];
% 1. Simulate Sensors
lidar_points = simulate_lidar(current_x, current_y, current_yaw, environment_data, lidar_params);
camera_detections = simulate_camera(current_x, current_y, current_yaw, environment_data, camera_params);
radar_targets = simulate_radar(current_x, current_y, current_yaw, current_v, environment_data, radar_params);
% 2. Process Sensor Data
segmented_objects_lidar = process_lidar_data(lidar_points, processing_params);
processed_camera_detections = process_camera_data(camera_detections);
processed_radar_targets = process_radar_data(radar_targets);
% 3. Data Fusion
fused_objects = fuse_sensor_data(segmented_objects_lidar, processed_camera_detections, processed_radar_targets, current_pose);
% 4. Path Planning
% Plan path from current vehicle pose to goal
[planned_path_x, planned_path_y] = a_star_planner(current_pose, goal_pose, fused_objects, environment_data, planning_params);
% 5. Decision Making & Control (Simplified)
% If a path is found, follow it. Otherwise, stop or replan.
target_delta = 0; % Steering angle
target_a = 0; % Acceleration
if ~isempty(planned_path_x) && length(planned_path_x) > 1
% Determine next target point on the path
% A simple pure pursuit or PID controller would be used here.
% For simplicity, let\'s just aim for the next point on the path.
target_point_x = planned_path_x(2); % Next point after current
target_point_y = planned_path_y(2);
% Calculate desired heading to target point
desired_yaw = atan2(target_point_y - current_y, target_point_x - current_x);
% Calculate yaw error
yaw_error = desired_yaw - current_yaw;
yaw_error = atan2(sin(yaw_error), cos(yaw_error)); % Normalize to [-pi, pi]
% Simple steering control (P-controller for steering)
Kp_delta = 0.5; % Proportional gain for steering
target_delta = Kp_delta * yaw_error;
% Simple speed control (maintain a target speed)
target_speed = 5; % m/s
Kp_a = 0.5; % Proportional gain for acceleration
target_a = Kp_a * (target_speed - current_v);
% Limit steering angle and acceleration
max_steering_angle = deg2rad(30); % radians
max_acceleration = 2; % m/s^2
target_delta = max(min(target_delta, max_steering_angle), -max_steering_angle);
target_a = max(min(target_a, max_acceleration), -max_acceleration);
else
% No path or path too short, stop the vehicle
target_a = -current_v / dt; % Decelerate to stop
target_delta = 0;
end
% 6. Update Vehicle State
[vehicle_state(1), vehicle_state(2), vehicle_state(3), vehicle_state(4)] = ...
vehicle_kinematic_model(current_x, current_y, current_yaw, current_v, target_delta, target_a, dt, vehicle_L);
% Store trajectory
trajectory_x = [trajectory_x, vehicle_state(1)];
trajectory_y = [trajectory_y, vehicle_state(2)];
% 7. Visualization Update
% Update vehicle position
set(h_vehicle, \'XData\', vehicle_state(1), \'YData\', vehicle_state(2));
% Update vehicle orientation (draw a simple triangle or rectangle for vehicle body)
% This is a simplified representation. For a more accurate one, use a patch object.
vehicle_front_x = vehicle_state(1) + vehicle_length/2 * cos(vehicle_state(3));
vehicle_front_y = vehicle_state(2) + vehicle_length/2 * sin(vehicle_state(3));
vehicle_rear_x = vehicle_state(1) - vehicle_length/2 * cos(vehicle_state(3));
vehicle_rear_y = vehicle_state(2) - vehicle_length/2 * sin(vehicle_state(3));
% Update trajectory
set(h_trajectory, \'XData\', trajectory_x, \'YData\', trajectory_y);
% Update LiDAR points (convert from local to global for plotting)
if ~isempty(lidar_points)
lidar_points_global_x = vehicle_state(1) + lidar_points(:,1) .* cos(vehicle_state(3)) - lidar_points(:,2) .* sin(vehicle_state(3));
lidar_points_global_y = vehicle_state(2) + lidar_points(:,1) .* sin(vehicle_state(3)) + lidar_points(:,2) .* cos(vehicle_state(3));
set(h_lidar_points, \'XData\', lidar_points_global_x, \'YData\', lidar_points_global_y);
else
set(h_lidar_points, \'XData\', [], \'YData\', []); % Clear if no points
end
% Update Camera Bounding Boxes (remove old, draw new)
if ~isempty(h_camera_bboxes)
delete(h_camera_bboxes); % Delete previous bounding boxes
end
h_camera_bboxes = [];
for k = 1:length(processed_camera_detections)
det = processed_camera_detections{k};
% Convert relative bbox to global coordinates for plotting
% This conversion needs to be careful with bbox definition (center vs top-left)
% Assuming det.bbox is [rel_x, rel_y, width, height] where rel_x, rel_y are center of bbox in vehicle frame
bbox_center_rel_x = det.bbox(1);
bbox_center_rel_y = det.bbox(2);
bbox_width = det.bbox(3);
bbox_height = det.bbox(4);
% Rotate and translate center to global
rotated_bbox_center_x = bbox_center_rel_x * cos(vehicle_state(3)) - bbox_center_rel_y * sin(vehicle_state(3));
rotated_bbox_center_y = bbox_center_rel_x * sin(vehicle_state(3)) + bbox_center_rel_y * cos(vehicle_state(3));
global_bbox_center_x = vehicle_state(1) + rotated_bbox_center_x;
global_bbox_center_y = vehicle_state(2) + rotated_bbox_center_y;
% Plot rectangle (need to calculate corner for \'Position\' [x,y,w,h])
% This is a simplification. For rotated rectangles, use patch.
h_camera_bboxes(end+1) = rectangle(ax, \'Position\', [global_bbox_center_x - bbox_width/2, global_bbox_center_y - bbox_height/2, bbox_width, bbox_height], ...
\'EdgeColor\', \'r\', \'LineWidth\', 1.5, \'LineStyle\', \'-\' , \'DisplayName\', \'Camera Detection\');
end
% Update Radar Targets (convert from local to global for plotting)
if ~isempty(processed_radar_targets)
radar_targets_global_x = cellfun(@(t) vehicle_state(1) + t.x * cos(vehicle_state(3)) - t.y * sin(vehicle_state(3)), processed_radar_targets);
radar_targets_global_y = cellfun(@(t) vehicle_state(2) + t.x * sin(vehicle_state(3)) + t.y * cos(vehicle_state(3)), processed_radar_targets);
set(h_radar_targets, \'XData\', radar_targets_global_x, \'YData\', radar_targets_global_y);
else
set(h_radar_targets, \'XData\', [], \'YData\', []);
end
% Update Planned Path
set(h_planned_path, \'XData\', planned_path_x, \'YData\', planned_path_y);
% Adjust plot limits to follow vehicle (optional)
% xlim(ax, [vehicle_state(1) - 50, vehicle_state(1) + 50]);
% ylim(ax, [vehicle_state(2) - 30, vehicle_state(2) + 30]);
drawnow limitrate; % Update figure with limited rate
% Check for simulation end condition (e.g., reached goal)
dist_to_goal = sqrt((vehicle_state(1) - goal_pose(1))^2 + (vehicle_state(2) - goal_pose(2))^2);
if dist_to_goal < 5 % meters
disp(\'Goal reached!\');
break;
end
end
hold(ax, \'off\');
disp(\'Simulation Finished.\');
最新发布
最低0.47元/天 解锁文章
扫一扫
1148
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
