newpoint3.rar_多机器人_多机器人轨迹_多轨迹_机器人轨迹_这是一个多机器人路径规划程序

% %功能：网络信息采集过程， %时间：20171123 % 为了得到代理和目标的后验采样 % 这是一个核心函数，没有改动 function [agentPosteriors, targetPosteriors, movementAgents] = netMessagePassing2( measurements, adjacencyMatrix,... priorAgents, priorTargets, movementAgents, parameter) % 初始化参数 anchorPositions = parameter.anchorPositions; % 锚节点 messagePassingIterations = parameter.messagePassingIterations; %消息迭代次数 numParticles = parameter.numParticles*parameter.oversampling; % 采样粒子数 [~, numTargets] = size(parameter.targetStartStates); [~, numAgents] = size(parameter.agentStartPositions); measurementVariance = parameter.measurementVariance; velocityPriorUncertaintyTargets = parameter.velocityPriorUncertaintyTargets; targetStartStates = parameter.targetStartStates; % 目的是什么，目标状态已经改变了 numConsensusIterations = parameter.numConsensusIterations; % 初始化 belief 和外在信息向量和数据存储 agentPosteriors = zeros(2,numParticles,numAgents,messagePassingIterations); % 代理后验信息 没有速度 targetPosteriors = zeros(4,numParticles,numTargets,messagePassingIterations); targetPosteriorsExtrinsic = cell(numTargets,1); % 目标外在信息 agentPosteriorsExtrinsic = cell(numAgents,1); % 代理的外在信息 posteriorsAgentsLastIteration = priorAgents; agentPosteriorsExtrinsicLastIteration = netinitializeExtrinsic( priorAgents, measurements.withAgents2 ); targetPosteriorsExtrinsicLastIteration = netinitializeExtrinsic( priorTargets, measurements.withTargets ); % 信息只迭代一次mPI = 1; for iteration = 1:messagePassingIterations for agent = 1:numAgents %初始化协作代理连接和测量节点距离 % connectionsAnchors = measurements.withAnchors{agent}; % AGP 连接 ACP measurementsAnchors = measurements.distAnchors{agent}; % AGP测量 ACP connectionsAgents = measurements.withAgents{agent}; % 代理相互连接代理节点 measurementsAgents = measurements.distAgents{agent}; %代理测量的代理节点 connectionsTargets = measurements.withTargets{agent}; %代理连接的目标节点 measurementsTargets = measurements.distTargets{agent}; %代理测量的目标节点 priorAgent = []; % 建议性分布， 用于重要性采样 if(~isnan(movementAgents(agent))) % proposal是归一化预测消息 %有协作时：直接采样用先前的信息 proposal = priorAgents(:,:,agent); % 由于协作估计准确，直接按先验来采样 else %proposal 是来自合作代理附近第一个锚节点传入的归一化消息 anchorPositionTmp = permute(repmat(anchorPositions(:,connectionsAnchors(1)),[1,1,numParticles]),[1 3 2]); % 获得一个锚节点 用于定位即可 % 获得建议性分布节点 % 注意：没有协作时才要进行采样 添加功能 初始时网络信息采样 proposal = getNetMessages(measurementsAnchors, anchorPositionTmp, parameter); %添加： 新函数 measurementsAnchors = measurementsAnchors(2:end); % 这里是使用了第一个锚节点之后，剩下的连接 connectionsAnchors = connectionsAnchors(2:end); % 这里也非常好 if(~isempty(priorAgents)) priorAgent = priorAgents(:,:,agent); end end %将信念，外在信息和对应于相邻（非锚定）协作代理的测量结果对应于向量中的相邻目标 beliefsAndExtrinsic = []; measurementsPartner = []; % 寻找代理节点中的（协作节点） if(~isempty(posteriorsAgentsLastIteration)) beliefsAndExtrinsic = posteriorsAgentsLastIteration(1:2,:,connectionsAgents); measurementsPartner = measurementsAgents; end % 目标中的（协作节点） if(~isempty(targetPosteriorsExtrinsicLastIteration)) beliefsAndExtrinsic = cat(3,beliefsAndExtrinsic, targetPosteriorsExtrinsicLastIteration{agent}); measurementsPartner = cat(1,measurementsPartner,measurementsTargets); end %提取测量锚点的位置，如果没有锚节点了就没有信息协作了 anchorPositionsTmp = anchorPositions(:,connectionsAnchors); %获得目标索引 numObservedTargets = length(connectionsTargets); numObservedAgents = length(connectionsAgents); indexes = numObservedAgents+1:(numObservedTargets+numObservedAgents); % 计算不太清楚 %实行信息乘法（堆叠方法）以获得每个目标的协作代理置信度belief和外在信息 % 2017.09.11：17.03 调试到这里 这是一个关键的函数，通过计算权值得到最优的采样结果 % 添加功能 11 [agentPosteriors(:,:,agent,iteration), agentPosteriorsExtrinsic{agent}] = netmessageMultiplication2(proposal, parameter, anchorPositionsTmp,... measurementsAnchors, beliefsAndExtrinsic, ... measurementsPartner, indexes, priorAgent, (~isempty(targetPosteriorsExtrinsicLastIteration) && iteration ~= messagePassingIterations)); % 20171123 到这里了 end %% 处理所有的目标节点 % 调试到这里了，2017.09.12.11:33 for target = 1:numTargets %初始化与特定目标相对应的连接和测量 connectionsAnchors = measurements.withAnchors2{target}; % 连接的锚节点 measurementsAnchors = measurements.distAnchors2{target}; % 测量的锚节点 connectionsAgents = measurements.withAgents2{target}; % 与目标连接的代理节点 measurementsAgents = measurements.distAgents2{target}; % 代理节点对目标的测量 %从邻近的合作代理获取收到的消息 messages = zeros(2,numParticles,0); if(~isempty(agentPosteriorsExtrinsicLastIteration)) % 当代理后验不为0时 messages = getNetMessages( measurementsAgents, agentPosteriorsExtrinsicLastIteration{target}, parameter ); end %决定proposal 的分布使用重要性采样 if(~isempty(priorTargets)) %proposal 分布是归一化预测信息 proposal = priorTargets(:,:,target); else %proposal 是信息使用的是目标连接的第一个锚节点 % 获得第一个锚节点 anchorPositionTmp = permute(repmat(anchorPositions(:,connectionsAnchors(1)),[1,1,numParticles]),[1 3 2]); proposal = getNetMessages( measurementsAnchors, anchorPositionTmp, parameter); % 获得位置的采样信息 % 添加新的采样函数 measurementsAnchors = measurementsAnchors(2:end); % 剩下用于协作的定位的锚节点 connectionsAnchors = connectionsAnchors(2:end); % 获得速度，采样扩展加入误差 目标是具有速度的 velocityPrior = repmat(targetStartStates(3:4,target),[1 numParticles])+sqrt(velocityPriorUncertaintyTargets)*randn(2,numParticles); proposal = cat(1, proposal, velocityPrior); end %第一种是：分布式方式执行信息乘法（一致权重）来决定belief 每个目标相邻代理的外在信息 % 因为 numConsensusIterations = 0; if(numConsensusIterations > 0) % 该函数没有使用，怎么回事 [targetPosteriors(:,:,target,iteration), targetPosteriorsExtrinsic{target}] = netmessageMultiplicationConsensus(messages, measurementsAnchors,... connectionsAnchors, connectionsAgents, measurementVariance,... proposal, adjacencyMatrix, (~isempty(agentPosteriorsExtrinsicLastIteration) && iteration ~= messagePassingIterations), parameter); else %方法二：信息乘法（采样核密度方法）获得目标的belief 和目标邻域中每个协作代理的外在信息 indexes = 1:length(connectionsAgents); % 通过权值，得到最优的定位坐标 [targetPosteriors(:,:,target,iteration), targetPosteriorsExtrinsic{target}] = netmessageMultiplication1(messages, measurementsAnchors,... anchorPositions(:,connectionsAnchors), measurementVariance, proposal,... indexes,(~isempty(agentPosteriorsExtrinsicLastIteration) && iteration ~= messagePassingIterations)); end end % 存储外部信息和信念以在下一轮消�