使用 Matlab + Arduino 进行 Kilobot 群体控制.zip

%% Object Manipulation Experiment With Kilobots % Uses an overhead vision system to control a swarm of kilobots to push an % object through a maze. Swarm is attracted to the brightest light in the % room. % % see video: https://youtu.be/tD1JvgRBRVM % % In these experiments a whiteboard on a table is the workspace, with four % 50W LED floodlights at the corners and four 30W LED floodlights on the % sides of a 6 m square centered on the workspace and 1.5 m above the % table. An Arduino Uno connected to an 8-relay shield controls the lights. % Above the table, an overhead machine vision system tracks the swarm. The % vision system identifies obstacles by color segmentation, determines the % corners (used to decrease variance), the object by color segmentation, % and identifies robots using color segmentation and circle detection with % a circular Hough transform. The path planning uses value iteration for % the object and potential fields for the swarm to interact with the % object. % % REQUIRES RegionCode.m, MDPgridworldFunction.m, RelayOn.m, dist2points.m, % AngleFix.m, circle.m, FlowForce.m, plot_gaussian_ellipsoid.m % and Arduino drivers: % http://www.mathworks.com/hardware-support/arduino-matlab.html % % SETUP: connect a webcam via USB and an Arduino via USB. % ------------------------- % By Shiva Shahrokhi, Lillian Lin, Mable Wan & Aaron T Becker, Summer 2016 format compact close all clear all %% Define webcam webcamShot = true; %if false, uses stored image 'KilobotTableExample.jpeg' if true, uses webcam %% Load maps from RegionCode.m RegionCode(webcamShot) load('MazeMap', 'movesX', 'movesY','corners'); load('ThresholdMapsMac','transferRegion','mainRegion'); if webcamShot cam = webcam(1); %may need to be changed between 1 and 2 depending on computer %% Using Arduino for our lights, this is how we define arduino in Matlab: if (ispc==1) % Code to run on Windows platform. Set correct port. a = arduino('Com4','uno'); else % Code to run on Mac platform a = arduino('/dev/tty.usbmodem1421','uno'); end %% Setup End Goal Position if (ispc==1) goalX = 5; goalY = 5; goalSize = 4; else goalX = 5; goalY = 5; goalSize = 3.5; end end %% Initalize Variables Relay=0; delayTime = 10; VarCont = false; flowDebug = true; success = false; again = true; counter = 1; c = 0; %Flow Around Variables rhoNot=7.5; %1 is main regions, 0 is transfer regions regionID=1; regionNum=3; %load regions currentRegionMap=mainRegion(:,:,regionNum); %init map while success == false %% Read in a webcam snapshot. if webcamShot if (again == true) RelayOn(a,0); pause (10); end rgbIm = snapshot(cam); else rgbIm = imread('KilobotTableExample.jpeg'); %#ok<UNRCH> goalX = 5; goalY = 5; goalSize = 4; end %% crop to have just the table view. if (ispc== 1) originalImage = imcrop(rgbIm,[50 10 500 400]); else originalImage = imcrop(rgbIm,[345 60 1110 850]); end s = size(originalImage); scale = floor( size(originalImage,2)/size(mainRegion,2)); epsilon = 1* scale; sizeOfMap = floor(s/scale); imshow(originalImage); %% make HSV scale for robots and object I = rgb2hsv(originalImage); % Define thresholds for channel 1 based on histogram settings channel1Min = 0.065; channel1Max = 0.567; % Define thresholds for channel 2 based on histogram settings channel2Min = 0.288; channel2Max = 1.000; % Define thresholds for channel 3 based on histogram settings channel3Min = 0.400; channel3Max = 1.000; % Create mask based on chosen histogram thresholds BW = (I(:,:,1) >= channel1Min ) & (I(:,:,1) <= channel1Max) & ... (I(:,:,2) >= channel2Min ) & (I(:,:,2) <= channel2Max) & ... (I(:,:,3) >= channel3Min ) & (I(:,:,3) <= channel3Max); [B,L] = bwboundaries(BW, 'noholes'); stat = regionprops(L,'Centroid','Area','PixelIdxList','MajorAxisLength','MinorAxisLength'); %% Find the object and put that out of the image [maxValue,index] = max([stat.Area]); centroids = cat(1, stat.Centroid); lengthsMajor = cat(1,stat.MajorAxisLength); lengthsMinor = cat(1,stat.MinorAxisLength); ObjectRadius = mean([lengthsMajor(index) lengthsMinor(index)],2)/2; ObjectCentroidX = centroids(index,1); ObjectCentroidY = centroids(index,2); BW(stat(index).PixelIdxList)=0; ObjCentX=floor(ObjectCentroidX/scale); ObjCentY=floor(ObjectCentroidY/scale); if (ObjCentX==0) ObjCentX=1; end if (ObjCentY==0) ObjCentY=1; end position = currentRegionMap(ObjCentY,ObjCentX); %% switching regions if(position==0) if (regionID==1)%check if it's in mainRegion state regionID=0; %changes to transferRegion state for i = 1:size(transferRegion,3)%find which trasnferRegion centroid is in tempMap=transferRegion(:,:,i); if(tempMap(ObjCentY,ObjCentX)==1) regionNum=i; %set to new region currentRegionMap=transferRegion(:,:,i); end end elseif (regionID==0) %if it is in transferRegion state regionID=1; %changes to mainRegion state for i = 1:size(mainRegion,3)%find which region centroid is in tempMap=mainRegion(:,:,i); if(tempMap(ObjCentY,ObjCentX)==1) regionNum=i; %set to new region currentRegionMap=mainRegion(:,:,i); end end end end %% Finding the green circles on the image which are kilobots. if ispc [centers, radii] = imfindcircles(BW,[4 6],'ObjectPolarity','bright','Sensitivity',0.97); else [centers, radii] = imfindcircles(BW,[10 19],'ObjectPolarity','bright','Sensitivity',0.92 ); end %% finding robots in cell sumX=0; sumY=0; countMean=0; %number of robots in cell [m,n]=size(centers); cenArray=[]; %dynamic array for selecting only robots within region for i=1:m cenX=floor(centers(i,1)/scale); cenY=floor(centers(i,2)/scale); if (cenX==0) cenX=1; end if (cenY==0) cenY=1; end if(currentRegionMap(cenY,cenX)==1) %if robot is in region sumX= sumX+centers(i,1); sumY= sumY+centers(i,2); countMean=countMean+1; cenArray(countMean,:) = centers(i,:); %#ok<SAGROW> %adding selected robots to array end end %% Robot Swarm Charactaristics %Mean M(1,1)= sumX/countMean; M(1,2)= sumY/countMean; %Variance V = var(cenArray); %Covariance C = cov(cenArray); % Drawing robots h = viscircles(centers,radii,'EdgeColor','b'); %s is number of detected robots [s, l] = size(centers); if s > 5 again = false; hold on % Variance Control constants minDis = 10000; corInd = 0; maxVar = 12000; %was 16000 minVar = 10000; %was 12000 %% Current region should at least have a minimum number of robots minNumRobD = 10; minNumRobU = 20; if countMean<minNumRobD NumRobotCont = true; % finding the nearest corner for i = 1:size(corners) dist = dist2points(corners(i,1),corners(i,2),ObjectCentroidX/scale,ObjectCentroidY/scale); if minDis > dist minDis = dist; corInd = i; end end currgoalX = (corners(corInd,1))*scale; currgoalY = (corners(corInd,2))*scale; elseif countMean> minNumRobU NumRobotCont = false; end %% Variance Control if (V > maxVar) VarCont = true; for i = 1:size(c