Matlab controls tricopter

Autonomous indoor waypoint navigation with a tricopter from W. Thielicke on Vimeo.

Tracking IR LEDs with Matlab

by W. Thielicke

Programmed for my tricopter (micro air vehicle), http://shrediquette.blogspot.com/



My tricopter tracking process uses several functions: There is one function (StartCam.m) that sets up everything, starts a GUI and another function (independentTrack_timer.m). The latter is the main function, which grabs the images from the camera and executes a tracking algorithm. A GUI is used to enable "on the fly" parameter editing. I will not include the code of the GUI because it is quite a lot of code and in essence, it doesn't do anything important. It simply makes PID parameter optimization more comfortable. Another function (figureclosingTrack.m) is used as the close request function of the GUI. It will be executed when the user wants to stop tracking. This function closes all connections (camera, serialport, timer function).

I tried to comment everything in the source code. Comments are always on top of the commands that they describe.


--------------------------------------------------------------------------------

This is the code of the .m file "StartCam.m". This function connects to a USB webcam, starts a timer and opens a serial port connection.

function StartCam (s,events)

%This block deletes all variables that were stored as application data

%(via "setappdata") in previous sessions. Restarting Matlab would have the

%same effect. The variables have to be deleted in order to enable a "clean" start.

try

    app=getappdata(0);

    appdatas = fieldnames(app);

    for kA = 1:length(appdatas)

        rmappdata(0,appdatas{kA});

    end

catch ME

    disp('rmappdata unsuccessful')

    disp(ME.message)

end



%Clear the variables in the workspace

clear all

clc



%Find available video devices

info = imaqhwinfo('winvideo');

%Create a video object (camera device nr.2, resolution setting 5 = 640x480)

vid = videoinput('winvideo', 2, info.DeviceInfo(1,2).SupportedFormats{5});

%If we send a trigger signal, we want to receive 1 frame from the camera

set(vid,'FramesPerTrigger',1);

%Don't stop automatically after one frame was triggered

set(vid,'TriggerRepeat',inf);

%Manual means: The camera only records a frame if we send a trigger signal

triggerconfig(vid, 'Manual');

%Specific setting for my USB Camera

srcObj1 = get(vid, 'Source');

set(srcObj1(1), 'FrameRate', '30');

set(srcObj1(1), 'sharpness', 4);

set(srcObj1(1), 'BacklightCompensation', 'off');

set(srcObj1(1), 'ColorEnable', 'off');

set(srcObj1(1), 'ExposureMode', 'manual');

set(srcObj1(1), 'Exposure', 5);



%Connect to the camera...

start(vid);

%...and trigger a frame

trigger(vid)

%get the frame from the camera (the frame will not be used, just for "fun")

A = (getdata(vid,1,'uint8')); %get a frame from cam

%specific settings for my GUI application

setappdata(0,'timecount',0);

setappdata(0,'creep',0);

%I am calling "tic" here, because I will measure the speed of the analysis

%in a different function. "toc" without "tic" won't work of course...

tic

%start the GUI where you can set all parameters for the PID control loop

sensGUI;

%Get the "address" of the GUI

HsensGUI=getappdata(0,'sensGUI');

%Get a list of all elements in the GUI

handles=guihandles(HsensGUI);

%Some changes of the appearance of the GUI

set(HsensGUI, 'DockControls', 'off', 'menubar', 'none', 'name', 'Tritrack');

%When the user closes the GUI, some specfic actions have to be performed

%(e.g. closing the serialport, shutting down camera etc.) These actions will

%be found in the function "figureclosingTrack.m"

set(HsensGUI,'CloseRequestFcn',@figureclosingTrack)

%save the video object as appdata, so that other functions know which

%camera was selected.

setappdata(0,'vid', vid);

%save the list of elements of the GUI

setappdata(0,'handles',handles);

%Create a serialport with specific settings

s = serial('COM5','BaudRate',38400);

%After each command that was send via the serial port,

%a "carriage return" is added

set(s, 'Terminator', 'CR');

%open the serial port

fopen(s);

%save the serialport object

setappdata(0,'s',s);



%Create a timer object that will execute the function in

%"independentTrack_timer.m" as fast as possible with a pause of 0.01

%seconds between each execution. This little pause is necessary for matlab

%to perform other tasks. If we don't pause, Matlab will not accept any

%commands from the user anymore (that means you can't end the execution of

%the function independentTrack_timer anymore, nor can you exit Matlab.

t = timer('TimerFcn',@independentTrack_timer, 'Period', 0.01,'ExecutionMode', 'fixedSpacing');

%Start the timer

start(t)

%Open a window that shows a live preview of the camera image. First I

%wanted to have this preview inside my GUI. But it is much faster if you

%use the preview(vid) function. I don't know why.

preview(vid);

This is the code of the .m file "independentTrack_timer.m". This function is executed repeatedly as fast as possible. It gets images from the camera and performs motion tracking. Subsequently, it transfers data to the copter via the serial port.



%This function is a called in a loop as fast as possible (leaving 0.01

%seconds for Matlab to perform different tasks).

function independentTrack_timer(s, events)

%Measure the speed of calculations (only important for optimizing the

%analysis)

try

    timecount=getappdata(0,'timecount');

    if timecount < 20

        setappdata(0,'timecount',timecount+1);

        runningat=getappdata(0,'runningat');

    else

        runningat=round(1/(toc/20));

        setappdata(0,'timecount',0);

        tic

        setappdata(0,'runningat',runningat);

    end

catch ME

    runningat=0;

    disp (ME.message)

end

%Get the videoobject from the appdata

vid=getappdata(0,'vid');

%trigger a frame

trigger(vid);

%and read the frame from the camera

img1=getdata(vid,1,'uint8');

%empty the image buffer for the camera

flushdata(vid);

%in my case, images are transferred as YUV, that means that the grayscale

%information (that's the only thing I am interested in) can be found in

%(:,:,1)

img1=img1(:,:,1);

%we want to track 4 points

nrpoints=4;

%the threshold for the image binarization is set in the GUI

thresh=getappdata(0,'thresh');

try

    %Binarize the image

    A = im2bw(img1, (thresh/255));

    %Find connected pixels in the image and give them a label

    A = bwlabel(A,8);

    %"Compress" the image

    XMLABELS=sparse(A);

    maxXM=max(A(:));

    %Centroids: Finds the coordinates and the area of the connected regions. The following

    %code is much faster than "regionprops".

    for i=1:maxXM

        [ii,jj]=find(XMLABELS==i);

        numberofpixels(i,1)=length(ii);

        iind(i,1)=mean(ii);

        jind(i,1)=mean(jj);

    end

    %Ainfo now contains a list with the positions and the size of all

    %bright spots in the image

    Ainfo=[jind iind numberofpixels];

    %Sort this list, so that the biggest regions are on top of the list

    Ainfo = sortrows(Ainfo,-3);

    %How many bright spots did we find?

    amountpeaks=size(Ainfo,1);

    %Keep only the 4 biggest bright spots

    Ainfo(nrpoints+1:end,:)=[];

    %Calculate a matrix that contains the distances between individual

    %bright spots.

    distances=zeros(nrpoints,nrpoints);

    for l=1:nrpoints

        for q=1:nrpoints

            distances(l,q)=sqrt((Ainfo(q,1)-Ainfo(l,1))^2+(Ainfo(q,2)-Ainfo(l,2))^2);

        end

    end

   

    %Now you can do all sorts of calculations with the coordinates of the

    %bright spots. In my case, I calculate the mean x&y displacement of all

    %bright spots to get the position of the copter. By calculating the mean

    %distance of the bright spots, you can find out the distance of the

    %markers to the camera. I also calculate the yaw angle of the copter.

    %When you have the information you need, you can calculate velocities and

    %accelerations as well, in order to create a PID control loop.



    %Here I am putting all parameters together....

    senden_roll=((xsoll_send+xsolldiff_send+xsoll_a_send)*rollxinfluence + (ysoll_send+ysolldiff_send+ysoll_a_send)*-rollyinfluence)*startupgain*sensdist;

    senden_nick=((xsoll_send+xsolldiff_send+xsoll_a_send)*nickxinfluence + (ysoll_send+ysolldiff_send+ysoll_a_send)*nickyinfluence)*startupgain*sensdist;

    senden_pitch=-(psoll_send+psolldiff_send+psoll_a_send+creep)*startupgain*sensdist;

    senden_yaw=(gsoll_send+gsolldiff_send+gsoll_a_send)*startupgain;

    %The data is sent out via the serialport. Inside the copter, these values

    %directly control the RPM of the motors. That means I am not modifying

    %or filtering these values anymore.

    fprintf(s,['A' int2str(senden_pitch)]);

    fprintf(s,['B' int2str(senden_yaw)]);

    fprintf(s,['C' int2str(senden_roll)]);

    fprintf(s,['D' int2str(senden_nick)]);

catch

end

Finally, the code that is executed when the GUI is closed.

function figureclosingTrack (s,event)

disp('Figure was closed...')

%get information about the serialport, the GUI and the video connection

fighandle=getappdata(0,'sensGUI');

s=getappdata(0,'s');

vid=getappdata(0,'vid');

%first close the serial port

try

    fclose(s)

    disp('  --> serialport closed')

catch

    disp('Error: Serial close')

end

%then shut down the preview window

try

    stoppreview(vid)

    closepreview(vid)

    disp('  --> preview stopped')

catch

    disp('Error: Preview stop')

end

%then stop and clear the timer function

try

    stop(timerfind)

    delete(timerfind)

    disp('  --> timer stopped')

catch ME

    disp('Error: Timer delete')

end

%Stop the camera object

try

    stop(vid)

    disp('  --> video stopped')

catch

    disp('Error: Video stop')

end

%Clear the serialport

try

    delete(s)

catch

    disp('Error: Serial delete')

end

%Close the GUI

delete(fighandle)

clear all

what's this? Could you give us some photos?

HDreamcatcher 发表于 2011-9-20 15:08
**** 作者被禁止或删除 内容自动屏蔽 ****

你要什么图片啊，这本来就是用MATBLAB写的CODE，可以实时跟踪物体的运动。