第5章 传感器数据预处理免责声明本文档为学术研究与技术学习目的而编写基于Autoware开源项目Apache 2.0许可证的源码分析。文档内容力求准确但不保证完全无误仅供参考。读者在实际应用时应以官方文档和源码为准。本文档不涉及任何商业用途所有代码示例均来自开源项目。如有侵权请联系删除。摘要本章介绍Autoware中传感器数据预处理模块的实现与原理。传感器数据预处理是自动驾驶感知流程的第一步负责将原始传感器数据转换为感知算法可用的标准格式。主要内容包括点云预处理地面分割、滤波、下采样、图像预处理去畸变、增强以及传感器融合预处理坐标变换、时间对齐。这些预处理操作直接影响后续感知算法的精度和效率是整个感知系统的基础。目录5.1 点云预处理5.1.1 点云滤波5.1.2 点云投影与坐标变换5.1.3 点云下采样5.1.4 pointcloud_to_laserscan转换5.2 图像预处理5.2.1 图像去畸变5.2.2 图像增强5.2.3 图像压缩与传输5.3 传感器融合预处理5.3.1 坐标系变换5.3.2 多传感器数据对齐5.3.3 数据质量评估参考资料5.1 点云预处理点云预处理是LiDAR数据处理流程中的关键环节负责将原始点云数据转换为适合后续算法使用的格式。预处理包括噪声去除、地面分割、坐标变换、下采样等操作这些操作可以显著提高感知算法的效率和准确性。5.1.1 点云滤波点云滤波的主要目的是去除噪声点、离群点和不相关的点云数据保留对感知任务有价值的信息。地面分割Ground Segmentation源码路径:universe/autoware_universe/perception/ground_segmentation地面分割是点云预处理中最重要的步骤之一将点云分为地面点和非地面点。地面点通常不包含障碍物信息可以被过滤掉以减少计算量。核心算法RANSAC平面拟合通过随机采样一致性算法拟合地面平面Ray Ground Filter基于射线的地面分割适用于结构化道路Scan Ground Filter基于扫描线的地面分割配置示例# 参数文件: universe/autoware_universe/perception/ground_segmentation/config/ground_segmentation.param.yaml/**:ros__parameters:# 地面高度阈值米global_slope_max_angle_deg:8.0# 最大地面坡度角度local_slope_max_angle_deg:6.0# 局部最大坡度radial_divider_angle_deg:1.0# 径向分割角度split_points_distance_tolerance:0.2# 点距离容差米split_height_distance:0.2# 高度分割阈值米代码示例// 地面分割核心逻辑简化版voidGroundSegmentation::segment(constsensor_msgs::msg::PointCloud2input,sensor_msgs::msg::PointCloud2ground_cloud,sensor_msgs::msg::PointCloud2obstacle_cloud){// 1. 将点云按径向分组std::vectorPointCloudSegmentsegments;radialDividePointCloud(input,segments);// 2. 对每个segment进行地面检测for(autosegment:segments){// 计算局部地面平面Plane ground_planeestimateGroundPlane(segment);// 分类点云for(constautopoint:segment.points){doubledist_to_planedistanceToPlane(point,ground_plane);if(dist_to_planeground_threshold_){ground_cloud.points.push_back(point);}else{obstacle_cloud.points.push_back(point);}}}}离群点去除Outlier Removal离群点通常由传感器噪声、多路径反射、空气中的灰尘等因素产生。常用方法统计滤波器Statistical Outlier Removal计算每个点与邻近点的平均距离去除距离超过标准差阈值的点半径滤波器Radius Outlier Removal在指定半径内搜索邻近点去除邻近点数量少于阈值的点// PCL统计滤波器示例#includepcl/filters/statistical_outlier_removal.hpcl::StatisticalOutlierRemovalpcl::PointXYZsor;sor.setInputCloud(cloud);sor.setMeanK(50);// 邻近点数量sor.setStddevMulThresh(1.0);// 标准差倍数sor.filter(*cloud_filtered);ROI感兴趣区域滤波ROI滤波用于去除超出感兴趣区域的点云数据减少无关数据的处理。# ROI滤波参数roi_filter:min_x:-50.0# 前方50米max_x:50.0min_y:-20.0# 左右各20米max_y:20.0min_z:-2.0# 地面以下2米max_z:5.0# 地面以上5米5.1.2 点云投影与坐标变换点云投影和坐标变换是多传感器融合的基础确保不同传感器数据在统一坐标系下进行处理。坐标系定义Autoware使用ROS标准的坐标系定义base_link: 车辆中心坐标系前-左-上map: 全局地图坐标系lidar_top: LiDAR传感器坐标系camera_front: 摄像头坐标系TF变换管理源码路径:universe/autoware_universe/common/tier4_autoware_utils// 使用tf2进行坐标变换#includetf2_ros/transform_listener.h#includetf2_sensor_msgs/tf2_sensor_msgs.h// 将点云从sensor坐标系变换到base_link坐标系geometry_msgs::msg::TransformStamped transform;try{transformtf_buffer_-lookupTransform(base_link,// 目标坐标系cloud_msg-header.frame_id,// 源坐标系cloud_msg-header.stamp,// 时间戳rclcpp::Duration::from_seconds(0.5));// 应用变换sensor_msgs::msg::PointCloud2 transformed_cloud;tf2::doTransform(*cloud_msg,transformed_cloud,transform);}catch(tf2::TransformExceptionex){RCLCPP_WARN(get_logger(),TF lookup failed: %s,ex.what());}点云投影到图像平面将3D点云投影到2D图像平面用于LiDAR-Camera融合。// 点云投影到图像cv::Point2dprojectPointToImage(constEigen::Vector3dpoint_3d,constEigen::Matrix3dcamera_intrinsic,constEigen::Matrix4dlidar_to_camera_transform){// 1. 变换到相机坐标系Eigen::Vector4dpoint_homogeneous(point_3d.x(),point_3d.y(),point_3d.z(),1.0);Eigen::Vector4d point_cameralidar_to_camera_transform*point_homogeneous;// 2. 投影到图像平面Eigen::Vector3d point_2dcamera_intrinsic*point_camera.head3();// 3. 归一化doubleupoint_2d.x()/point_2d.z();doublevpoint_2d.y()/point_2d.z();returncv::Point2d(u,v);}5.1.3 点云下采样点云下采样用于减少点云数据量提高处理速度同时保持点云的几何特征。体素网格滤波Voxel Grid Filter最常用的下采样方法将空间划分为体素网格每个体素内的点用中心点代替。源码路径:universe/autoware_universe/perception/pointcloud_preprocessor#includepcl/filters/voxel_grid.h// 体素下采样pcl::VoxelGridpcl::PointXYZvoxel_filter;voxel_filter.setInputCloud(cloud);voxel_filter.setLeafSize(0.2f,0.2f,0.2f);// 20cm体素voxel_filter.filter(*cloud_downsampled);参数配置voxel_grid_filter:voxel_size_x:0.2# 体素大小米voxel_size_y:0.2voxel_size_z:0.2随机采样Random Sampling随机选择固定数量的点适用于需要固定点云数量的场景。#includepcl/filters/random_sample.hpcl::RandomSamplepcl::PointXYZrandom_filter;random_filter.setInputCloud(cloud);random_filter.setSample(10000);// 采样10000个点random_filter.filter(*cloud_sampled);5.1.4 pointcloud_to_laserscan转换将3D点云转换为2D激光扫描数据用于兼容基于2D LiDAR的算法。源码路径:universe/autoware_universe/perception/pointcloud_to_laserscan# LaserScan转换参数pointcloud_to_laserscan:target_frame:base_linkmin_height:-0.5# 提取点云的高度范围max_height:2.0angle_min:-3.14159# 扫描角度范围弧度angle_max:3.14159angle_increment:0.0087# 角度分辨率约0.5度scan_time:0.1range_min:0.5# 有效距离范围米range_max:100.05.2 图像预处理图像预处理负责将原始摄像头图像转换为适合感知算法使用的格式包括畸变校正、增强和压缩等操作。5.2.1 图像去畸变摄像头镜头会产生径向和切向畸变需要通过标定参数进行校正。相机标定模型Autoware支持多种相机模型针孔模型Pinhole: 标准透视投影鱼眼模型Fisheye: 广角镜头全景模型Omnidirectional: 360度全景标定参数示例# camera_info (sensor_msgs/CameraInfo)image_width:1920image_height:1080camera_name:front_cameradistortion_model:plumb_bob# 畸变模型# 内参矩阵 [fx, 0, cx, 0, fy, cy, 0, 0, 1]K:[1200.0,0.0,960.0,0.0,1200.0,540.0,0.0,0.0,1.0]# 畸变系数 [k1, k2, p1, p2, k3]D:[-0.2,0.05,0.001,0.001,0.0]# 投影矩阵P:[1200.0,0.0,960.0,0.0,0.0,1200.0,540.0,0.0,0.0,0.0,1.0,0.0]去畸变实现#includeopencv2/opencv.hpp#includesensor_msgs/msg/camera_info.hppclassImageRectifier{public:voidinitUndistortRectifyMap(constsensor_msgs::msg::CameraInfocamera_info){// 构造内参矩阵cv::Mat K(cv::Mat_double(3,3)camera_info.k[0],camera_info.k[1],camera_info.k[2],camera_info.k[3],camera_info.k[4],camera_info.k[5],camera_info.k[6],camera_info.k[7],camera_info.k[8]);// 畸变系数cv::Mat Dcv::Mat(camera_info.d);// 生成去畸变映射表cv::initUndistortRectifyMap(K,D,cv::Mat(),K,cv::Size(camera_info.width,camera_info.height),CV_32FC1,map_x_,map_y_);}voidundistortImage(constcv::Matinput,cv::Matoutput){// 应用去畸变映射cv::remap(input,output,map_x_,map_y_,cv::INTER_LINEAR);}private:cv::Mat map_x_,map_y_;};5.2.2 图像增强图像增强用于改善图像质量提高后续算法的鲁棒性。亮度与对比度调整// 自适应直方图均衡化CLAHEcv::Ptrcv::CLAHEclahecv::createCLAHE(2.0,cv::Size(8,8));cv::Mat enhanced;clahe-apply(gray_image,enhanced);去噪处理// 双边滤波保边去噪cv::Mat denoised;cv::bilateralFilter(input_image,denoised,9,// 滤波核大小75.0,// 颜色空间标准差75.0);// 坐标空间标准差色彩空间转换// RGB转HSV用于颜色识别cv::Mat hsv_image;cv::cvtColor(rgb_image,hsv_image,cv::COLOR_RGB2HSV);// RGB转灰度cv::Mat gray_image;cv::cvtColor(rgb_image,gray_image,cv::COLOR_RGB2GRAY);5.2.3 图像压缩与传输图像数据量大需要压缩以降低网络传输和存储负担。压缩格式选择# image_transport配置image_transport:compressed:format:jpeg# 或pngjpeg_quality:80# 0-100png_level:3# 0-9压缩率ROS 2图像传输#includeimage_transport/image_transport.hpp// 发布压缩图像image_transport::ImageTransportit(node);autopubit.advertise(camera/image,1);sensor_msgs::msg::Image::SharedPtr image_msg;// ... 填充image_msgpub.publish(image_msg);// 自动选择compressed传输5.3 传感器融合预处理传感器融合预处理确保来自不同传感器的数据可以在统一的时空坐标系下进行处理。5.3.1 坐标系变换坐标系树管理源码路径:universe/autoware_universe/localization/pose_initializerAutoware使用TF2库管理复杂的坐标系变换关系map (全局坐标系) └─ base_link (车辆坐标系) ├─ lidar_top (顶部LiDAR) ├─ lidar_front (前方LiDAR) ├─ camera_front (前置摄像头) ├─ camera_left (左侧摄像头) ├─ camera_right (右侧摄像头) ├─ radar_front (前置毫米波雷达) └─ gnss (GNSS天线)静态变换配置# sensors.calibration.yamlbase_link:lidar_top:x:0.0y:0.0z:2.0roll:0.0pitch:0.0yaw:0.0camera_front:x:1.5y:0.0z:1.2roll:0.0pitch:-0.1yaw:0.0动态变换查询#includetf2_ros/buffer.h#includetf2_ros/transform_listener.hclassTransformManager{public:TransformManager(rclcpp::Node::SharedPtr node):tf_buffer_(node-get_clock()),tf_listener_(tf_buffer_){}std::optionalgeometry_msgs::msg::TransformStampedgetTransform(conststd::stringtarget_frame,conststd::stringsource_frame,constrclcpp::Timetime){try{returntf_buffer_.lookupTransform(target_frame,source_frame,time,rclcpp::Duration::from_seconds(0.1));}catch(tf2::TransformExceptionex){RCLCPP_WARN(rclcpp::get_logger(transform_manager),Transform lookup failed: %s,ex.what());returnstd::nullopt;}}private:tf2_ros::Buffer tf_buffer_;tf2_ros::TransformListener tf_listener_;};5.3.2 多传感器数据对齐时间同步策略源码路径:universe/autoware_universe/common/tier4_autoware_utils消息同步器示例#includemessage_filters/subscriber.h#includemessage_filters/sync_policies/approximate_time.h#includemessage_filters/synchronizer.hclassSensorFusion{public:SensorFusion(rclcpp::Node::SharedPtr node){// 创建订阅器lidar_sub_.subscribe(node,/lidar/points);camera_sub_.subscribe(node,/camera/image);// 时间同步策略允许10ms误差usingSyncPolicymessage_filters::sync_policies::ApproximateTimesensor_msgs::msg::PointCloud2,sensor_msgs::msg::Image;sync_std::make_sharedmessage_filters::SynchronizerSyncPolicy(SyncPolicy(10),lidar_sub_,camera_sub_);// 注册回调函数sync_-registerCallback(std::bind(SensorFusion::fusionCallback,this,std::placeholders::_1,std::placeholders::_2));}private:voidfusionCallback(constsensor_msgs::msg::PointCloud2::ConstSharedPtrlidar_msg,constsensor_msgs::msg::Image::ConstSharedPtrcamera_msg){// 融合处理逻辑RCLCPP_INFO(rclcpp::get_logger(sensor_fusion),Synchronized data - LiDAR: %ld, Camera: %ld,lidar_msg-header.stamp.nanosec,camera_msg-header.stamp.nanosec);}message_filters::Subscribersensor_msgs::msg::PointCloud2lidar_sub_;message_filters::Subscribersensor_msgs::msg::Imagecamera_sub_;std::shared_ptrmessage_filters::Synchronizer...sync_;};时间戳对齐配置# 时间同步参数message_filters:approximate_sync:queue_size:10max_interval_duration:0.01# 最大允许时间差秒5.3.3 数据质量评估点云质量检查structPointCloudQuality{size_t point_count;// 点数量doubleaverage_intensity;// 平均强度doubledensity;// 点云密度boolhas_nan;// 是否包含NaN值boolis_valid;// 是否有效};PointCloudQualityevaluatePointCloud(constsensor_msgs::msg::PointCloud2cloud){PointCloudQuality quality;quality.point_countcloud.width*cloud.height;// 检查点云数量if(quality.point_count1000){quality.is_validfalse;returnquality;}// 检查NaN值pcl::PointCloudpcl::PointXYZ::Ptrpcl_cloud(newpcl::PointCloudpcl::PointXYZ);pcl::fromROSMsg(cloud,*pcl_cloud);quality.has_nanfalse;for(constautopoint:pcl_cloud-points){if(std::isnan(point.x)||std::isnan(point.y)||std::isnan(point.z)){quality.has_nantrue;break;}}quality.is_valid!quality.has_nan;returnquality;}图像质量检查structImageQuality{doublebrightness;// 平均亮度doublecontrast;// 对比度doublesharpness;// 清晰度拉普拉斯方差boolis_valid;};ImageQualityevaluateImage(constcv::Matimage){ImageQuality quality;// 计算平均亮度cv::Scalar mean_scalarcv::mean(image);quality.brightnessmean_scalar[0];// 计算清晰度拉普拉斯算子cv::Mat laplacian;cv::Laplacian(image,laplacian,CV_64F);cv::Scalar mean,stddev;cv::meanStdDev(laplacian,mean,stddev);quality.sharpnessstddev[0]*stddev[0];// 判断有效性quality.is_valid(quality.brightness20quality.brightness235)(quality.sharpness100);returnquality;}数据质量监控# 数据质量阈值配置data_quality:pointcloud:min_points:1000max_nan_ratio:0.01image:min_brightness:20max_brightness:235min_sharpness:100.0参考资料官方文档Autoware Documentation - SensingROS 2 Sensor MessagesPCL (Point Cloud Library) Tutorials源码仓库autoware_universe - Perceptionpointcloud_preprocessorimage_projection_based_fusion学术论文Fast Segmentation of 3D Point Clouds: A Paradigm on LiDAR Data for Autonomous Vehicle Applications, IEEE ICRA 2017Ground Segmentation for Navigation in Urban Environments, IEEE ITSC 2018Real-time Camera Pose Estimation for Autonomous Vehicles, IEEE Intelligent Vehicles Symposium 2019技术博客PCL Filtering TutorialsOpenCV Camera CalibrationROS 2 Message Filters