function_image_feature.m

粒子滤波程序

function fea_uv = Function_image_feature(sita,x_y_z);
% PURPOSE : this function gets the image feature u and v from the real world
%                       coordinate x and y(单位：m)
%sita1,sita2为机器人两个机械手臂的位姿角(单位：弧度)

%统一单位，本函数用mm进行计算
sita1=sita(1);
sita2=sita(2);
x_w=x_y_z(1)*1000;
y_w=x_y_z(2)*1000;
z_w=x_y_z(3)*1000;

%系统初始化，定义已知参数
L=500;
f=8; k_u=20;k_v=20;  k_1=k_u*f;    k_2=k_v*f;
u_0=240;v_0=240;
z_c=350;

t_x=L*[cos(sita1)+cos(sita1+sita2)];
t_y=L*[sin(sita1)+sin(sita1+sita2)];
t_z=350;
t=[t_x;t_y;t_z];
a_x=pi;
a_y=0;
a_z=sita1+sita2;     
r=[cos(a_z)*cos(a_y), -sin(a_z)*cos(a_x)+cos(a_z)*sin(a_y)*sin(a_x),-sin(a_z)*sin(a_x)+cos(a_z)*sin(a_y)*cos(a_x);
    sin(a_z)*cos(a_y),cos(a_z)*cos(a_x)+sin(a_z)*sin(a_y)*sin(a_x),-cos(a_z)*sin(a_x)+sin(a_z)*sin(a_y)*cos(a_x),
   -sin(a_y),cos(a_y)*sin(a_x),cos(a_y)*cos(a_x)];

tmp_1=[k_1,0,u_0;0,k_2,v_0;0,0,1];
tmp_2=[r',-r'*t];
tmp_3=[x_w;y_w;z_w;1];
tmp_4=tmp_1*tmp_2*tmp_3/z_c;

fea_uv(1)=tmp_4(1);
fea_uv(2)=tmp_4(2);




%==========摄像机成像模型==========
%x_w=800,y_w=500,z_w=0为目标物体在世界坐标系下的坐标
%L为机械手手臂的长度L=500;
%-------------------------摄像机外部参数-------------------------
%开始t_x,t_y,t_z即为基坐标到摄像头的距离，随着摄像头移动，t_x,t_y随着变化
        %t_x(0)=500mm,t_y(0)=500mm,
        %t_x=L*[cos(sita1)+cos(sita1+sita2)];    t_y=L*[sin(sita1)+sin(sita1+sita2)]; t_z=350mm
        %t=[t_x;t_y;t_z];
%a_x,a_y,a_z三个角度为世界坐标系变换到摄像头坐标系时:
%先绕ｚ轴转一个a_z,再绕ｙ轴转一个a_y，然后绕ｘ轴转一个角度a_x.
        %a_x=pi.   a_y=0.  a_z=sita1+sita2.       
%r=[cos(a_z)*cos(a_y), -sin(a_z)*cos(a_x)+cos(a_z)*sin(a_y)*sin(a_x),-sin(a_z)*sin(a_x)+cos(a_z)*sin(a_y)*cos(a_x);
%    sin(a_z)*cos(a_y),cos(a_z)*cos(a_x)+sin(a_z)*sin(a_y)*sin(a_x),-cos(a_z)*sin(a_x)+sin(a_z)*sin(a_y)*cos(a_x),
%   -sin(a_y),cos(a_y)*sin(a_x),cos(a_y)*cos(a_x)]
%-------------------------摄像机内部参数-------------------------
%焦距f=8mm,   k_u=k_v=1/0.05=20 (pixels/mm),   u_0=v_0=240mm
%k_1=k_u*f=20*8=160;    k_2=k_v*f=160;
%-------------------------ccd摄像机成像模型-------------------------
%tmp_1=[k_1,0,u_0;0,k_2,v_0;0,0,1];
%tmp_2=[r'-r'*t];
%tmp_3=[x_w;y_w;z_w]
%[u,v,tmp]=tmp_1*tmp2*tmp_3/z_c;