工业机器人——工件定位问题

需求分析：在工业机器人中进行工件的定位的时候，需要知道工件相对于机器人来说，在什么位置。

首先，我们需要得到的就是通过雷达激光扫描或者其他方式得到的点云数据，然后对点云数据进行分析。
下面是我们目前得到的点云数据：如图

数据百度云链接：链接：http://pan.baidu.com/s/1cdHJOy 密码：5dof

思路：可以看到点云数据是一个三维的，所以至少需要三个点才能定位一个工件在空间中的位置。现在把这个工件分成两部分看，一个是规整的圆锥，一个是圆锥上面的圆筒。
首先，针对圆锥，求出圆锥的圆心和半径，那么就有很多个圆心和半径，就可以确定圆锥的中心线。根据圆心和半径的关系，可以把圆锥以外的点找到，这时候桶桶的点云数据就单独取出来了，然后通过法向量，取相同的法向量最多的那个向量，就是面的向量，再来确定桶桶的朝向，就可以确定整个工件的位置了。

先贴出找到工件圆心和半径的代码：

clear all;close all;clc;
%导入数据
%data = importdata('D:\data\point5.txt');
data =  load('my_bucket.mat');
profile on ;

%对数据进行排序，取精确度
[~,I]=sort(data(:,3));%假如对第3列进行升序排列
small_big=data(I,:);%这样排列就完成了       
small_big = roundn(small_big,-4);

%定义变量
i = 1;
j = 0;
k= 1;
empty = 1
num = 100;
w = 1;

sum  = 0;
[m1  n1 ] = size(small_big);






 while ~isempty(small_big)  
 %while empty <27
      %fprintf('11111111111111')
      [m n ] = size(small_big);

      %判断是否为剩下的数的Z是否都相同


        %  while num >20

if  length(unique(small_big(:,3))) == 1
    i = length(small_big(:,3))
else

while   (abs(small_big(1,3) - small_big(i+1,3)))<0.001
        %  fprintf('22222222222222')
     i = i + 1;


end
     while i<=3
        small_big=[small_big(1, :); small_big(1,:); small_big(2:end, :)];
        i = i+1;
     end


end








%把相同Z的点暂存到 tmp中，然后从原矩阵中删除
tmp = small_big(1:1:i,:);
small_big(1:1:i,:) = [];

%拿出相同Z的每一列数据
x = tmp(:,1)';
y = tmp(:,2)';
z = tmp(:,3)';
%plot3(x,y,z,'*');
%hold on ;

number = length(x);
if  number<4
    number =4;
end

 set_3P=nchoosek(1: number,3); 



 AI=set_3P(1,1);

BI=set_3P(1,2);

CI=set_3P(1,3);

A=[x(AI) y(AI)];

B=[x(BI) y(BI)];

C=[x(CI) y(CI)];

while 1

    R=minCirclePoints3(A,B,C);

    cr=[R(1),R(2)];

    r=zeros(1,length(x));

    for i=1:length(x)

       r(i)=sqrt((x(i)-cr(1))^2+(y(i)-cr(2))^2);

    end;

    maxValue=max(max(r));    %或者N=max(r(:))

    [mc]=find(maxValue==r);
   % mc = mc(1,1)

    c=randperm(numel(mc));
    mc=mc(c(1:1));


    %没有点在圆外，或者最远点包含在A、B、C三点之中（精度误差导致），结束

    if maxValue <= R(3) || ~isempty( intersect(mc, [AI BI CI]) )

        alpha=0:pi/20:2*pi;%角度[0,2*pi]

       plot(cr(1)+R(3)*cos(alpha),cr(2)+R(3)*sin(alpha),'--r');%中心点在（R(1),R(2)）半径为R(3)的圆
        hold on;

        axis equal;

        %把数据保存起来

        if k == 1
            result = R;
    fprintf('+++++++++++++++')        
        else
            result =[result;R];
            [result_row,result_column] = size(result);
            if w == 1
               % if sqrt(result(result_row,1).^2 +result(result_row,2).^2) - sqrt(result(result_row - 1,1).^2 +result(result_row - 1,2).^2)
               if result(result_row,1)-result(result_row-1,1) > 0   ||  result(result_row,2)-result(result_row-1,2)>0
                   tmp_z = tmp;
                   tmp_z_high = tmp_z(1,3);
                   w = w+1;
                   pre_center = [result(result_row-1,1),result(result_row-1,2)];
               end
            elseif w ==2
                if (result(result_row-1,3)-result(result_row,3)) >0
                    tmp_z_2 = tmp;
                    tmp_z_2_high =tmp_z_2(1,3);
                    w = w+1;
                    pre_center_2 = [result(result_row-1,1),result(result_row-1,2)];
                    fprintf('1111')        
                end

                elseif w ==3
                    fprintf('2222')        

                    if result(result_row,1) ==result(1,1) &&result(result_row,2) ==result(1,2);
                    tmp_z_3 = tmp;
                    tmp_z_3_high = tmp_z_3(1,3);
                   w =w+1;
                    %pre_center_3 = [result(result_row-1,1),result(result_row-1,2)];
                    pre_center_3 = [10,0];