Simulink开发项1000例实战专栏--实例210：Simulink智能驾驶辅助系统：行人检测与避障仿真

 % 创建新的Simulink模型

    
 modelName = 'PedestrianDetectionAndAvoidance';
    
 new_system(modelName);
    
 open_system(modelName);

 % 添加车辆底盘模块

    
 add_block('automatedDriving/Rigid Body', [modelName '/Vehicle']);
    
 set_param([modelName '/Vehicle'], 'Mass', '1500'); % 设置车辆质量为1500kg
    
 set_param([modelName '/Vehicle'], 'Inertia', '[1000, 0, 0; 0, 1500, 0; 0, 0, 1000]'); % 设置惯性矩矩阵

 % 添加纵向控制模块

    
 add_block('automatedDriving/Longitudinal Driver', [modelName '/Longitudinal_Driver']);
    
 set_param([modelName '/Longitudinal_Driver'], 'MaxAcceleration', '3'); % 设置最大加速度为3m/s^2
    
  
    
 % 添加横向控制模块
    
 add_block('automatedDriving/Lateral Driver', [modelName '/Lateral_Driver']);
    
 set_param([modelName '/Lateral_Driver'], 'MaxSteeringRate', '0.5'); % 设置最大转向角速率为0.5rad/s

 % 连接纵向控制模块到车辆底盘模块

    
 add_line(modelName, [modelName '/Longitudinal_Driver'], [modelName '/Vehicle'], 'autorouting', 'on');
    
  
    
 % 连接横向控制模块到车辆底盘模块
    
 add_line(modelName, [modelName '/Lateral_Driver'], [modelName '/Vehicle'], 'autorouting', 'on');

 % 添加摄像头传感器模块

    
 add_block('automatedDriving/Camera Sensor', [modelName '/Camera_Sensor']);
    
 set_param([modelName '/Camera_Sensor'], 'FOV', '90'); % 设置视场角为90度
    
 set_param([modelName '/Camera_Sensor'], 'Resolution', '[640, 480]'); % 设置分辨率为640x480

 % 添加行人检测模块

    
 add_block('automatedDriving/Pedestrian Detector', [modelName '/Pedestrian_Detector']);
    
 set_param([modelName '/Pedestrian_Detector'], 'DetectionThreshold', '0.5'); % 设置检测阈值为0.5
    
 set_param([modelName '/Pedestrian_Detector'], 'MinTargetSize', '[20, 20]'); % 设置最小目标尺寸为20x20像素

 % 连接摄像头传感器模块到行人检测模块

    
 add_line(modelName, [modelName '/Camera_Sensor'], [modelName '/Pedestrian_Detector'], 'autorouting', 'on');

 % 添加目标跟踪模块

    
 add_block('sensorFusion/Multi-Object Tracker', [modelName '/Tracker']);
    
  
    
 % 设置参数
    
 set_param([modelName '/Tracker'], 'TrackerType', 'Global Nearest Neighbor (GNN)'); % 设置跟踪器类型为全局最近邻
    
 set_param([modelName '/Tracker'], 'InitializationMethod', 'Constant Velocity'); % 设置初始化方法为恒定速度

 % 连接行人检测模块到目标跟踪模块

    
 add_line(modelName, [modelName '/Pedestrian_Detector'], [modelName '/Tracker'], 'autorouting', 'on');

 % 添加决策逻辑模块

    
 add_block('simulink/User-Defined Functions/MATLAB Function', [modelName '/Decision_Logic']);
    
  
    
 % 编写MATLAB Function代码
    
 function action = fcn(pedestrianPosition, vehiclePosition)
    
     % pedestrianPosition 是从目标跟踪模块获取的行人位置信息
    
     % vehiclePosition 是当前车辆的位置信息
    
     distance = norm(pedestrianPosition - vehiclePosition); % 计算行人与车辆的距离
    
     
    
     if distance < 10 % 如果距离小于10米
    
     action = 'Brake'; % 减速
    
     elseif distance < 20 % 如果距离小于20米
    
     action = 'SteerLeft'; % 向左转向
    
     else
    
     action = 'Continue'; % 继续行驶
    
     end
    
 end

 % 连接目标跟踪模块到决策逻辑模块

    
 add_line(modelName, [modelName '/Tracker'], [modelName '/Decision_Logic'], 'autorouting', 'on');

 % 添加PID控制器模块

    
 add_block('simulink/Continuous/PID Controller', [modelName '/PID_Controller']);
    
 set_param([modelName '/PID_Controller'], 'P', '1'); % 设置比例系数为1
    
 set_param([modelName '/PID_Controller'], 'I', '0.1'); % 设置积分时间为0.1秒
    
 set_param([modelName '/PID_Controller'], 'D', '0.01'); % 设置微分时间为0.01秒

 % 连接决策逻辑模块到控制器模块

    
 add_line(modelName, [modelName '/Decision_Logic'], [modelName '/PID_Controller'], 'autorouting', 'on');

 % 连接控制器模块到车辆底盘模块

    
 add_line(modelName, [modelName '/PID_Controller'], [modelName '/Vehicle'], 'autorouting', 'on');

 % 设置仿真参数

    
 set_param(modelName, 'StopTime', '60'); % 模拟运行时间为60秒
    
 set_param(modelName, 'Solver', 'ode45');

 % 获取检测统计数据

    
 detectedCount = getDetectedCount(); // 自定义函数获取检测到的行人数量
    
 actualCount = getActualCount(); // 自定义函数获取实际存在的行人数量
    
  
    
 % 计算检测准确率
    
 accuracy = detectedCount / actualCount;
    
  
    
 % 显示结果
    
 disp(['Detection Accuracy: ', num2str(accuracy * 100), '%']);

 % 获取避障统计数据

    
 avoidedCount = getAvoidedCount(); // 自定义函数获取成功避开的行人数量
    
 totalCount = getTotalCount(); // 自定义函数获取总出现的行人数量
    
  
    
 % 计算避障成功率
    
 successRate = avoidedCount / totalCount;
    
  
    
 % 显示结果
    
 disp(['Obstacle Avoidance Success Rate: ', num2str(successRate * 100), '%']);

 % 计算超调量

    
 overshoot = calculateOvershoot(vehicleSpeed); // 自定义函数计算车辆速度的超调量
    
  
    
 // 显示结果
    
 disp(['Overshoot: ', num2str(overshoot), '%']);

 % 获取车辆轨迹数据

    
 trajectory = getTrajectory();
    
  
    
 % 检查是否发生碰撞
    
 if any(isCollision(trajectory)) % 自定义函数检查是否发生碰撞
    
     disp('Warning: Collision detected.');
    
 else
    
     disp('Vehicle successfully avoided pedestrians without collision.');
    
 end

 % 修改行人检测模块中的检测阈值

    
 set_param([modelName '/Pedestrian_Detector'], 'DetectionThreshold', '0.6'); % 更改为0.6

 % 添加激光雷达传感器模块

    
 add_block('automatedDriving/Lidar Sensor', [modelName '/Lidar_Sensor']);
    
 set_param([modelName '/Lidar_Sensor'], 'Range', '100'); % 设置激光雷达的最大探测范围为100米
    
  
    
 % 将激光雷达传感器模块的输出连接到行人检测模块
    
 add_line(modelName, [modelName '/Lidar_Sensor'], [modelName '/Pedestrian_Detector'], 'autorouting', 'on');

 % 使用深度学习行人检测模型

    
 add_block('deepLearning/Pedestrian Detection Using Deep Learning', [modelName '/Deep_Learning_Pedestrian_Detector']);

 % 创建测试用例

    
 testCase = sltest.testmanager.TestFile('PedestrianDetection_TestCases');
    
 testCase.addTestSuite('PedestrianDetection_TestSuite');
    
 testCase.addTestCase('Scenario_1', 'Initial_Pedestrian_Counts_5');
    
  
    
 % 运行测试
    
 sltest.testmanager.run(testCase);