手把手教你学simulink（26.3）--simulink多模融合场景实例：基于Simulink的多传感器融合实现自动驾驶汽车的车道保持辅助系统（LKA）

 % 定义传感器参数

    
 camera_fov = 90; % 摄像头视场角（度）
    
 lidar_range = 100; % 激光雷达最大探测距离（米）
    
 radar_range = 200; % 雷达最大探测距离（米）
    
  
    
 % 定义工作环境
    
 environment = 'highway'; % 高速公路环境
    
 weather_conditions = 'clear'; % 天气条件（晴天）
    
    
    
    

 % 创建摄像头传感器模型

    
 camera = vision.RoadRunner.Sensor.Camera(...
    
     'FieldOfView', camera_fov, ...
    
     'MountingLocation', [0, 0, 1.5], ... % 摄像头安装位置（x, y, z）
    
     'MountingAngles', [0, 0, 0], ... % 摄像头安装角度（俯仰角, 横滚角, 方位角）
    
     'IntrinsicMatrix', cameraIntrinsics([640, 480], 700, 320, 240), ... % 内参矩阵
    
     'DetectionProbability', 0.9); % 物体检测概率
    
    
    
    

 % 创建激光雷达传感器模型

    
 lidar = fusion.LidarPointCloudGenerator(...
    
     'MaxRange', lidar_range, ...
    
     'MinElevation', -15, ... % 最小俯仰角（度）
    
     'MaxElevation', 15, ... % 最大俯仰角（度）
    
     'AzimuthResolution', 0.2, ... % 方位角分辨率（度）
    
     'ElevationResolution', 0.4, ... % 俯仰角分辨率（度）
    
     'PositionAccuracy', 0.1, ... % 位置精度（米）
    
     'NoiseDensity', 0.01); % 噪声密度
    
    
    
    

 % 创建前向雷达传感器模型

    
 front_radar = fusion.RadarDataGenerator(...
    
     'MaxRange', radar_range, ...
    
     'FieldOfView', [45, 10], ... % 视场角（方位角, 俯仰角）
    
     'AzimuthResolution', 1, ... % 方位角分辨率（度）
    
     'UpdateRate', 10, ... % 更新频率（Hz）
    
     'MountingLocation', [1.5, 0, 0.5], ... % 雷达安装位置（x, y, z）
    
     'MountingAngles', [0, 0, 0], ... % 雷达安装角度（俯仰角, 横滚角, 方位角）
    
     'DetectionProbability', 0.8); % 目标检测概率
    
    
    
    

 % 创建IMU传感器模型

    
 imu = imuSensor('SampleRate', 100, ... % 更新频率（Hz）
    
     'AccelerometerBiasNoise', 0.001, ... % 加速度计偏置噪声
    
     'GyroscopeBiasNoise', 0.001); % 陀螺仪偏置噪声
    
    
    
    

 % 创建车道线检测器

    
 lane_detector = vision.LaneDetector;
    
  
    
 % 检测车道线
    
 [lane_points, lane_types] = lane_detector(camera_image);
    
  
    
 % 可视化车道线检测结果
    
 if ~isempty(lane_points)
    
     figure;
    
     imshow(camera_image);
    
     hold on;
    
     for i = 1:size(lane_points, 3)
    
     plot(lane_points(:, 1, i), lane_points(:, 2, i), ...
    
         'Color', 'r', 'LineWidth', 2);
    
     end
    
     hold off;
    
 end
    
    
    
    

 % 读取激光雷达点云数据

    
 ptCloud = lidar(); % 假设lidar()返回一个pointCloud对象
    
  
    
 % 降采样点云
    
 ptCloudDownsampled = pcdownsample(ptCloud, 'gridAverage', 0.2);
    
  
    
 % 分离地面点
    
 groundPtsIdx = segmentGroundFromLidarData(ptCloudDownsampled, 'ElevationAngleDifference', 5);
    
 nonGroundPtCloud = select(ptCloudDownsampled, ~groundPtsIdx, 'OutputSize', 'full');
    
  
    
 % 提取车道边界点
    
 lane_boundary_points = extractLaneBoundaryPoints(nonGroundPtCloud);
    
  
    
 % 可视化点云
    
 figure;
    
 pcshow(nonGroundPtCloud.Location, nonGroundPtCloud.Color);
    
 title('Non-Ground Point Cloud with Lane Boundaries');
    
 hold on;
    
 plot3(lane_boundary_points(:, 1), lane_boundary_points(:, 2), lane_boundary_points(:, 3), 'r', 'LineWidth', 2);
    
 hold off;
    
    
    
    

 % 获取雷达检测结果

    
 radar_data = front_radar();
    
  
    
 % 过滤低置信度的检测
    
 valid_detections = radar_data{1}.ObjectProperties.DetectionConfidence > 0.5;
    
  
    
 % 提取有效检测的目标位置和速度
    
 positions = radar_data{1}.ObjectProperties.Position(valid_detections, :);
    
 velocities = radar_data{1}.ObjectProperties.Velocity(valid_detections, :);
    
  
    
 % 将雷达检测结果转换到全局坐标系
    
 global_positions = transformPointsForward(front_radar.MountingLocation, positions);
    
    
    
    

 % 获取IMU数据

    
 [acceleration, angular_velocity] = imu();
    
  
    
 % 使用互补滤波器融合IMU数据
    
 filter = complementaryFilter;
    
 [pitch, roll, yaw] = filter(acceleration, angular_velocity);
    
    
    
    

 % 定义状态转移函数

    
 stateTransitionFcn = @(x, u) stateTransitionFunction(x, u);
    
  
    
 % 定义观测函数
    
 measurementFcn = @(x) measurementFunction(x);
    
  
    
 % 初始化状态估计
    
 initialState = [0; 0; 0]; % 例如：位置(x,y)和航向角(yaw)
    
 initialCovariance = diag([1, 1, 1]);
    
  
    
 % 过程噪声协方差矩阵
    
 processNoise = diag([0.1, 0.1, 0.1]);
    
  
    
 % 测量噪声协方差矩阵
    
 measurementNoise = diag([0.1, 0.1, 0.1]);
    
  
    
 % 创建扩展卡尔曼滤波器对象
    
 ekf = extendedKalmanFilter(stateTransitionFcn, measurementFcn, ...
    
     'State', initialState, ...
    
     'StateCovariance', initialCovariance, ...
    
     'ProcessNoise', processNoise, ...
    
     'MeasurementNoise', measurementNoise);
    
    
    
    

 % 提取车道线点

    
 left_lane_points = lane_points(:,:,1);
    
 right_lane_points = lane_points(:,:,2);
    
  
    
 % 拟合车道线
    
 p_left = polyfit(left_lane_points(:, 1), left_lane_points(:, 2), 2);
    
 p_right = polyfit(right_lane_points(:, 1), right_lane_points(:, 2), 2);
    
  
    
 % 计算车道中心线
    
 x_center = linspace(min(left_lane_points(:, 1)), max(right_lane_points(:, 1)), 100);
    
 y_center = (polyval(p_left, x_center) + polyval(p_right, x_center)) / 2;
    
  
    
 % 可视化车道中心线
    
 figure;
    
 imshow(camera_image);
    
 hold on;
    
 plot(x_center, y_center, 'g', 'LineWidth', 2);
    
 hold off;
    
    
    
    

 % 计算车辆当前位置与车道中心线的偏差

    
 vehicle_position = [0, 0]; % 假设车辆位置为原点
    
 deviation = interp1(x_center, y_center, vehicle_position(1)) - vehicle_position(2);
    
  
    
 % 设定控制器增益
    
 Kp = 0.5; % 比例增益
    
  
    
 % 计算转向角度
    
 steering_angle = Kp * deviation;
    
  
    
 % 应用转向控制
    
 applySteering(steering_angle);