用系留机器人解决mTSP问题的算法（销售人员）matlab代码.rar

function [soln, robotDistances, totTime, objectiveMinMax, flagIsFeasible, routeMatrix] = ... runNaiveMethod(nodecoords,numOfCities, flagTether, ... tetherLength,flagNorm, flagIterativeNew, timeLimit) % Background %{ We want to take the mTSP formulation and incorporate constraints that represent tethers We've got two routes here. ROUTE 1: Brute force Make a toy example (2 robots, 3 or 4 cities) and add a bunch of tether constraints like we talked about in our meeting. Run it and observe the results RESULT 2: Also Brute force, but different Run the whole simulation and then check to see if a tether constraint was violated. If a constraint was violated, exclude that solution, add a constraint that prevents that solution from happening, and resolve. The constraint would involve just saying x(i,j,1) and x(i,j,2) can't both be 1 at the same time. In this case, we'd also make a toy problem to speed things up. 2 robots and 5 cities. To change between norms, just change the distance function in the % cost matrix. %} %% Setup % TO RUN: Press "Run" in the MATLAB editor % clc; clearvars -except nodecoords numOfCities ... flagTether tetherLength flagNorm flagIterativeNew timeLimit; close all; yalmip('clear'); flagIsFeasible = 1; % default this flag to 1 % If flagIterative == 1, the iterative method will run. Otherwise, only the % original simulation will run. % flagIterativeNew = 1; flagIterative = 0; flagSolveBaseProblem = 0; % If flagTether == 1, the plotting tool will include tethers. % flagTether = 1; % Note that if you turn on flagIterative, you should probably turn on % flagTether, as the Iterative Method is the version of the sim that % implements the tether constraints. % flagNorm = "2_norm"; % options: "2_norm", "1_norm" numOfRobots = 3; % numOfCities = 5; % % tetherLength = 70; % Distance value used for dummy node creation. Ensures that our 'zeroth' % node has zero cost from 0 to node 1 but doesn't go to other nodes % during optimization. M = 10e4; % Tried M = inf, but YALMIP poops itself. % Set that represents all nodes but the first. Used for generating a set of % all possible subtours with Dantzig-Fulkerson-Johsnon (DFJ) subtour % eliminiation constraints (SECs). VminusFirst = num2cell(2:numOfCities); V0 = num2cell(1:numOfCities); % set of nodes that includes dummy node % Initialize bin for SECs before solving problem % So first get all possible subtours: S = PowerSet(VminusFirst); % Ensure the cardinality of the subsets lays between 2 and (numOfCities-1) % S = S(2:end-1); S = S(2:end); bin = binvar(numel(S),numOfRobots,'full'); assign(bin,ones(numel(S),numOfRobots)) % Initialize sdpvar object for use in constraints 3,4. Will be % included in objective function. Traversal: any entrance or exit numNodeTraversals = sdpvar(numOfCities, numOfRobots, 'full'); x = intvar(numOfCities, numOfCities, numOfRobots, 'full'); u = sdpvar(numOfCities, 1, 'full'); p = numOfCities - numOfRobots; % Truncated eil51 node coords further to just 5 cities % nodecoords = load('ToyProblemNodeCoords.txt'); % nodecoords = load('TruncatedEil51NodeCoords.txt'); % nodecoords = nodecoords(1:numOfCities,:); C = zeros(numOfCities); for i = 1:numOfCities for j = 1:numOfCities if( (strcmp(flagNorm,"1_norm") == 1) ) C(i,j) = distance1(nodecoords(i,2), nodecoords(i,3), ... nodecoords(j,2), nodecoords(j,3)); end if ( (strcmp(flagNorm,"2_norm") == 1) ) C(i,j) = distance(nodecoords(i,2), nodecoords(i,3), ... nodecoords(j,2), nodecoords(j,3)); end end end C = real(C); %% DEBUG % % x = zeros(numOfCities,numOfCities,numOfRobots); % % x(:,:,1) = ... % [0 1 0 0 0 0; % 1 0 0 0 1 0; % 0 0 0 0 0 0; % 0 1 0 0 0 0; % 0 0 0 1 0 0; % 0 0 0 0 0 0]; % % x(:,:,2) = ... % [0 1 0 0 0 0; % 1 1 0 0 0 1; % 0 0 0 0 0 0; % 0 0 0 0 0 0; % 0 0 0 0 0 0; % 0 1 0 0 0 1;]; % % x(:,:,3) = ... % [0 1 0 0 0 0; % 1 1 1 0 0 0; % 0 1 0 0 0 0; % 0 0 0 0 0 0; % 0 0 0 0 0 0; % 0 0 0 0 0 0;]; %% mTSP constraints % When setting j = 1 with no x(1,1,k) == 0 for all k constraint: % % 1 2 2 5 4 2 1 % 1 2 2 4 3 2 1 % 1 2 6 2 2 2 1 % When setting j=2 with x(1,1,k) == 0 for all k % % value(objective3) % % ans = % % 187.6801 % % value(objective2) % % ans = % % 187.6621 % routeMatrix = % % 1 2 4 2 5 2 1 % 1 2 2 6 3 2 1 % 1 2 4 2 4 2 1 % ConstraintS 1 and 2 (eqns 10 and 11 in the ICTAI paper) % Ensure robots start and end at the first (dummy node) constraint1 = []; constraint2 = []; x1jkTotal = 0; xj1kTotal = 0; for k = 1:numOfRobots x1jkTotal = 0; xj1kTotal = 0; for j = 1:numOfCities x1jkTotal = x1jkTotal + x(1,j,k); xj1kTotal = xj1kTotal + x(j,1,k); end constraint1 = [constraint1, (x1jkTotal >= 1)]; constraint2 = [constraint2, (xj1kTotal >= 1)]; end % constraint1 = []; % constraint2 = []; % Constraints 3 and 4 (eqns 12 and 13) % Ensures robots, as a whole, enter and exit each node once except for the % first node. We want all robots to go to the first node, so we want the % robots to enter and exit that node 3 times, not just once. We implement % that change in the following constraints after these 2. constraint3 = []; % constraint 3 start % Then, build a vector to hold the sum of all of % the values of each numOfCities x numOfRobots matrix. % See handwritten work for a more intuitive visualiztion. planeTotal_ik = 0; % for j = 1:numOfCities % % reset the value of planeTotal_ik to 0 for the next constraint: % planeTotal_ik = 0; % for i = 1:numOfCities % for k = 1:numOfRobots % planeTotal_ik = planeTotal_ik + x(i,j,k); % end % end % % Build the constraint here % constraint3 = [constraint3, (planeTotal_ik == numNodeTraversals(j)) ]; % end for k = 1:numOfRobots for j = 1:numOfCities planeTotal_ik = 0; for i = 1:numOfCities planeTotal_ik = planeTotal_ik + x(i,j,k); end constraint3 = [constraint3, (planeTotal_ik == numNodeTraversals(j,k))]; end end constraint4 = []; % constraint 4 start planeTotal_jk = 0; % for i = 1:numOfCities % planeTotal_jk = 0; % for j = 1:numOfCities % for k = 1:numOfRobots % planeTotal_jk = planeTotal_jk + x(i,j,k); % end % end % constraint4 = [constraint4, (planeTotal_jk == numNodeTraversals(i)) ]; % end for k = 1:numOfRobots for i = 1:numOfCities planeTotal_jk = 0; for j = 1:numOfCities planeTotal_jk = planeTotal_jk + x(i,j,k); end constraint4 = [constraint4, (planeTotal_jk == numNodeTraversals(i,k))]; end end % Constraint 5 (eqn 14) % Prevent robots from entering and exiting more than one city at a time lhsSum = 0; rhsSum = 0; constraint5 = []; for j = 2:numOfCities for k = 1:numOfRobots for i = 1:numOfCities if i ~= j lhsSum = lhsSum + x(i,j,k); rhsSum = rhsSum + x(j,i,k); end end constraint5 = [constraint5, (lhsSum == rhsSum)]; % 0 out sums for next constraint lhsSum = 0; rhsSum = 0; end end % constraint5 = []; % Constraint 6: subtour elimination constraints (SECs) xS = 0; constraint6 = []; for j = 2:numOfCities for i = 2:numOfCities