Алгоритм визуального сервообразования на основе изображений в MATLAB

Question

Я пытался реализовать алгоритм IBVS (тот, который изложено во введении here) в MATLAB сам, но я столкнулся со следующей проблемой: алгоритм работает только для случаев, когда камера не должна менять свою ориентацию по отношению к мировому кадру. Например, если я просто попытаюсь сделать одну вершину начального (почти) квадрата ближе к своей противоположной вершине, алгоритм не работает, как можно видеть в следующем изображении

красный х искомые проекции, синие круги являются исходными и зеленые являются те, которые я получаю п мой алгоритм.

Кроме того, ошибки не экспоненциально разлагаются так, как должны.

Что я делаю неправильно? Я прилагаю код MATLAB, который полностью работает. Если бы кто-нибудь мог взглянуть, я был бы очень благодарен. Я вынул код, который выполнял заговор. Надеюсь, теперь это станет более читаемым. Визуальный сервопривод должен выполняться как минимум с 4 целевыми точками, потому что иначе проблема не имеет уникального решения. Если вы хотите помочь, я бы посоветовал вам взглянуть на функцию calc_Rotation_matrix(), чтобы проверить правильность расчета матрицы поворота, а затем проверить правильность строки ds = vc; в euler_ode. Ориентация камеры выражается в углах Эйлера в соответствии с соглашением this. Наконец, можно проверить правильность расчета матрицы взаимодействия L.

function VisualServo() 

    global A3D B3D C3D D3D A B C D Ad Bd Cd Dd 

    %coordinates of the 4 points wrt camera frame 
    A3D = [-0.2633;0.27547;0.8956]; 
    B3D = [0.2863;-0.2749;0.8937]; 
    C3D = [-0.2637;-0.2746;0.8977]; 
    D3D = [0.2866;0.2751;0.8916]; 

    %initial projections (computed here only to show their relation with the desired ones) 
    A=A3D(1:2)/A3D(3); 
    B=B3D(1:2)/B3D(3); 
    C=C3D(1:2)/C3D(3); 
    D=D3D(1:2)/D3D(3); 

    %initial camera position and orientation 
    %orientation is expressed in Euler angles (X-Y-Z around the inertial frame 
    %of reference) 
    cam=[0;0;0;0;0;0]; 

    %desired projections 
    Ad=A+[0.1;0]; 
    Bd=B; 
    Cd=C+[0.1;0]; 
    Dd=D; 

    t0 = 0; 
    tf = 50; 

    s0 = cam; 

    %time step 
    dt=0.01; 
    t = euler_ode(t0, tf, dt, s0); 

end 


function ts = euler_ode(t0,tf,dt,s0) 

    global A3D B3D C3D D3D Ad Bd Cd Dd 

    s = s0; 
    ts=[]; 
    for t=t0:dt:tf 
     ts(end+1)=t; 
     cam = s; 

     % rotation matrix R_WCS_CCS 
     R = calc_Rotation_matrix(cam(4),cam(5),cam(6)); 
     r = cam(1:3); 

     % 3D coordinates of the 4 points wrt the NEW camera frame 
     A3D_cam = R'*(A3D-r); 
     B3D_cam = R'*(B3D-r); 
     C3D_cam = R'*(C3D-r); 
     D3D_cam = R'*(D3D-r); 

     % NEW projections 
     A=A3D_cam(1:2)/A3D_cam(3); 
     B=B3D_cam(1:2)/B3D_cam(3); 
     C=C3D_cam(1:2)/C3D_cam(3); 
     D=D3D_cam(1:2)/D3D_cam(3); 


     % computing the L matrices 
     L1 = L_matrix(A(1),A(2),A3D_cam(3)); 
     L2 = L_matrix(B(1),B(2),B3D_cam(3)); 
     L3 = L_matrix(C(1),C(2),C3D_cam(3)); 
     L4 = L_matrix(D(1),D(2),D3D_cam(3)); 
     L = [L1;L2;L3;L4]; 


     %updating the projection errors 
     e = [A-Ad;B-Bd;C-Cd;D-Dd]; 

     %compute camera velocity 
     vc = -0.5*pinv(L)*e; 

     %change of the camera position and orientation 
     ds = vc; 

     %update camera position and orientation 
     s = s + ds*dt; 


    end 
    ts(end+1)=tf+dt; 
end 

function R = calc_Rotation_matrix(theta_x, theta_y, theta_z) 

    Rx = [1 0 0; 0 cos(theta_x) -sin(theta_x); 0 sin(theta_x) cos(theta_x)]; 
    Ry = [cos(theta_y) 0 sin(theta_y); 0 1 0; -sin(theta_y) 0 cos(theta_y)]; 
    Rz = [cos(theta_z) -sin(theta_z) 0; sin(theta_z) cos(theta_z) 0; 0 0 1]; 

    R = Rx*Ry*Rz; 

end 

function L = L_matrix(x,y,z) 

    L = [-1/z,0,x/z,x*y,-(1+x^2),y; 
     0,-1/z,y/z,1+y^2,-x*y,-x]; 
end 

Дела, работа:

Ad=2*A; 
Bd=2*B; 
Cd=2*C; 
Dd=2*D; 

Ad=A+1; 
Bd=B+1; 
Cd=C+1; 
Dd=D+1; 

Ad=2*A+1; 
Bd=2*B+1; 
Cd=2*C+1; 
Dd=2*D+1; 

случаях, которые не работают: Поворот на 90 градусов и уменьшение (уменьшение в одиночку работает, но я делаю это здесь для лучшей визуализации)

Ad=2*D; 
Bd=2*C; 
Cd=2*A; 
Dd=2*B; 

Answer 1

Ваша проблема приходит от того, как вы перемещаете камеру из полученной визуальной скорости сервопривода. Вместо

cam = cam + vc*dt; 

вы должны вычислить новое положение камеры, используя экспоненциальное отображение

cam = cam*expm(vc*dt)