基于ROS的雷达和激光雷达数据融合
对记录在气囊上的驾驶场景的激光雷达传感器数据执行履带级传感器融合。此示例使用与前面相同的驾驶场景和传感器融合雷达和激光雷达数据的航迹级融合(传感器融合和跟踪工具箱)示例，但使用预先录制的rosbag代替驾驶场景模拟。
从Rosbag提取传感器数据
这提供了一个包含激光雷达、雷达和车辆数据的rosbag示例，大小大约为33MB。从MathWorks网站下载rosbag。
bagFile = matlab.internal.examples.downloadSupportFile (“活性氧”，“rosbags / simulated_lidar_radar_driving_798.bag”）;
访问rosbag并查看可用的主题。
袋= rosbag (bagFile);disp(包。AvailableTopics (:,“NumMessages”，“MessageType”)))
NumMessages MessageType ___________ ________________________________________ /clock 114 rosgraph_msgs/ clock /ego/lidar_scan 114 sensor_msgs/PointCloud2 /ego/radar_1/detections 114 cob_object_detection_msgs/DetectionArray /ego/radar_2/detections 114 cob_object_detection_msgs/DetectionArray /ego/radar_3/ detection114 cob_object_detection_msgs/DetectionArray /ego/radar_4/ detection114 cob_object_detection_msgs/DetectionArray /ego/radar_4/ detection114 cob_object_detection_msgs/DetectionArray /ego/state 114 nav_msgs/Odometry
自我车辆有一个激光雷达和四个雷达传感器，以及自己的绝对位置信息。可以将这些消息提取到单独的数组中，以便稍后进行融合。由于此rosbag被压缩以减少文件大小，读取消息可能需要几秒钟。方法将消息提取为结构DataFormat参数，它提高了ROS消息的总体性能。
lidarBagSel =选择(包,“主题”，“自我/ / lidar_scan”）;lidarMsgs = readMessages (lidarBagSel,“DataFormat”，“结构”）;stateBagSel =选择(包,“主题”，“自我/状态”）;stateMsgs = readMessages (stateBagSel,“DataFormat”，“结构”）;radarMsgs =细胞(包。AvailableTopics {“自我/ radar_1 /检测”，“NumMessages”4)};为idxRadar = 1:4 radarBagSel = select(bag，“主题”sprintf (“自我/ / radar_ % d /检测”idxRadar));radarMsgs (:, idxRadar) = readMessages (radarBagSel,“DataFormat”，“结构”）;结束
使融合时使用的时间戳相对于第一个消息时间戳。注意袋子开始时间不能使用，因为这是记录第一条消息的时间戳，比消息时间戳晚。
clockBagSel =选择(包,“主题”，“/钟”）;clockMsg = readMessages(袋1“DataFormat”，“结构”）;startTime = double(clockMsg{1}.Clock_.Sec) + double(clockMsg{1}.Clock_.Nsec)*1e-9;
设置传感器跟踪和融合
关于传感器的信息是根据车辆的设置而知道的，并且需要以跟踪算法可用的格式保存。
[lidarInfo, radarInfo, radarParameters] = helperRadarLidarInfo;
雷达和激光雷达跟踪算法是必要的，以处理高分辨率扫描和确定在扫描中看到的对象没有重复。这些算法被定义为辅助函数。关于算法的更多细节可以在雷达和激光雷达数据的航迹级融合(传感器融合和跟踪工具箱)的例子。
radarTrackingAlgorithm = helperROSRadarTracker (radarInfo);radarConfig = fuserSourceConfiguration (“SourceIndex”, 1.．.“IsInitializingCentralTracks”,真的,.．.“CentralToLocalTransformFcn”@central2radar,.．.“LocalToCentralTransformFcn”, @radar2central);lidarTrackingAlgorithm = helperLidarTrackingAlgorithm (lidarInfo);lidarConfig = fuserSourceConfiguration (“SourceIndex”，2，.．.“IsInitializingCentralTracks”,真正的);熔化炉= trackFuser (“SourceConfigurations”{radarConfig; lidarConfig},.．.“StateTransitionFcn”, lidarTrackingAlgorithm。StateTransitionFcn,.．.“ProcessNoise”诊断接头([1 3]),.．.“HasAdditiveProcessNoise”假的,.．.“AssignmentThreshold”, 250年正无穷,.．.“ConfirmationThreshold”[3 - 5],.．.“DeletionThreshold”, 5 [5],.．.“StateFusion”，“自定义”，.．.“CustomStateFusionFcn”, @helperRadarLidarFusionFcn);
可视化
使用辅助函数设置传感器数据可视化的图形和属性。
displayHelper = helperROSSensorFusionDisplay;
执行传感器消息融合
遍历消息并运行传感器融合算法。观看可视化看到rosbag回放和跟踪车辆使用传感器融合。
所有传感器同时被触发进行测量，所有雷达传感器在每次触发时都发布一条消息，即使没有探测到任何信号。因为所有的消息数组都是相同的长度，所以传感器数据的处理要简单得多。如果传感器以不同的速率触发，发生连接问题，或接收的消息顺序错误，则可能需要一个同步传感器数据的中间步骤。
为idx = 1:元素个数(lidarMsgs)第一次传感器读取时提取时间。%使时间相对于rosbag开始时间更容易跟踪。lidarTimeStamp = lidarMsgs {idx} .Header.Stamp;lidarTime = double(lidarTimeStamp.Sec) +.．.双(lidarTimeStamp.Nsec) * 1 e-9 -开始时间;radarTimeStamp = radarMsgs {idx 1} .Header.Stamp;radarTime = double(radarTimeStamp.Sec) +.．.双(radarTimeStamp.Nsec) * 1 e-9 -开始时间;提取车辆状态并修改结构进行处理。。egoPose =结构;stateMsg = stateMsgs {idx};positionMsg = stateMsg.Pose.Pose.Position;egoPose。位置= [positionMsg。X;positionMsg。Y;positionMsg.Z];%方向的程度。orientQuat = rosReadQuaternion (stateMsg.Pose.Pose.Orientation);orientEul = eulerd (orientQuat,“XYZ”，“点”）;egoPose。滚= orientEul (1);egoPose。距= orientEul (2);egoPose。偏航= orientEul (3);通过转换，nav_msgs/Odometry velocity在子函数中提供%参考系(车辆)。核聚变需要世界的速度%参考系。velMsg = stateMsg.Twist.Twist.Linear;egoPose。速度= rotatepoint (orientQuat,.．.[velMsg。X velMsg。Y velMsg.Z]);从激光雷达中提取点云进行处理这个激光雷达没有提供RGB或强度信息lidarXYZPoints = rosReadXYZ (lidarMsgs {idx});ptCloud = pointCloud (lidarXYZPoints);使用元数据将雷达探测提取到单个数组中%指定源传感器。nDetections = sum(cellfun(@(msg) numel(msg. detections)，radarMsgs(idx，:)));radarDetections =细胞(nDetections, 1);% PreallocateidxDetection = 1;为idxRadar = 1:尺寸(radarMsgs, 2)为idxRadarDetection = 1:numel(radarMsgs{idx,idxRadar}.Detections) detMsg = radarMsgs{idx,idxRadar}.Detections(idxRadarDetection);detTime = double(detMsg.Header.Stamp.Sec) +.．.双(detMsg.Header.Stamp.Nsec) * 1 e-9 -开始时间;measureMsg = detMsg.Pose.Pose.Position;测量= [measureMsg。X;measureMsg。Y;measureMsg.Z];%测量噪声存储在边界框字段中，原因是此消息类型包含Pose而不是PoseCovariance。measureNoise =诊断接头([detMsg.BoundingBoxLwh。X detMsg.BoundingBoxLwh.Y detMsg.BoundingBoxLwh.Z]);在Score字段中存储信噪比。objectAttributes =结构(“TargetIndex”, detMsg。Id,“信噪比”, detMsg.Score);radarDetections {idxDetection} = objectDetection (detTime、测量、.．.“MeasurementNoise”measureNoise,.．.“SensorIndex”idxRadar,.．.“ObjectClassID”，0，.．.“ObjectAttributes”{objectAttributes},.．.“MeasurementParameters”{radarParameters (idxRadar)});idxDetection = idxDetection + 1;结束结束生成传感器轨迹和分析信息，如包围框。%检测和点云分割信息。radarTracks = radarTrackingAlgorithm(egoPose, radarDetections, radarTime);[lidarTracks, lidarDetections, segmentationInfo] =.．.lidarTrackingAlgorithm (egoPose ptCloud lidarTime);localTracks = [radarTracks;lidarTracks];%更新fuser。第一个调用必须包含一个本地轨道。如果~(isempty(localTracks) && ~isLocked(fuser)) fusedTracks = fuser(localTracks,lidarTime);其他的fusedTracks = objectTrack.empty (0,1);结束%更新显示updateSensorData (displayHelper ptCloud radarDetections) plotTracks (displayHelper、radarTracks lidarTracks, fusedTracks, egoPose)结束
效用函数
上面的脚本中使用了以下函数定义。
radar2central
函数centralTrack = radar2central (radarTrack)初始化一个状态大小正确的轨道。centralTrack = objectTrack (“状态”1) 0(10日,.．.“StateCovariance”、眼睛(10));radarTrack除State和StateCovariance外的%同步属性参见下面定义的syncTrack。centralTrack = syncTrack (centralTrack radarTrack);xRadar = radarTrack.State;PRadar = radarTrack.StateCovariance;H = 0(10、7);雷达到中心的线性变换矩阵H (1, - 1) = 1;H (2, 2) = 1;H (3,3) = 1;H (4, 4) = 1;H (5,5) = 1;6 H(8日)= 1;7 H(9日)= 1;xCentral = H * xRadar;线性状态变换PCentral = H * PRadar * H ';%线性协方差变换PCentral([6 7 10]，[6 7 10]) =眼(3);%的国家设置中心轨迹的状态和协方差centralTrack。状态= xCentral;centralTrack。StateCovariance = PCentral;结束
central2radar
函数radarTrack = central2radar (centralTrack)初始化一个状态大小正确的轨道。radarTrack = objectTrack (“状态”1) 0(7日,.．.“StateCovariance”、眼睛(7));同步除State和StateCovariance之外的centralTrack的%属性参见下面定义的syncTrack。radarTrack = syncTrack (radarTrack centralTrack);xCentral = centralTrack.State;PCentral = centralTrack.StateCovariance;H = 0 (7, 10);%中心雷达线性变换矩阵H (1, - 1) = 1;H (2, 2) = 1;H (3,3) = 1;H (4, 4) = 1;H (5,5) = 1;H(6、8)= 1;H(7、9)= 1;xRadar = H * xCentral;线性状态变换PRadar = H * PCentral * H ';%线性协方差变换设置雷达轨迹的状态和协方差radarTrack。状态= xRadar;radarTrack。StateCovariance = PRadar;结束
syncTrack
函数tr1 = syncTrack(tr1,tr2) props = properties(tr1);notState = ~ strcmpi(道具、“状态”）;notCov = ~ strcmpi(道具、“StateCovariance”）;props = props(notState & notCov);为i = 1:元素个数(道具)tr1。(道具{我})= tr2。(道具{我});结束结束
您也可以从以下列表中选择网站: