2015/04/20: Biologically Inspired Bearing-Only Navigation and Tracking

Post date: Apr 20, 2015 1:52:3 AM

File MATLAB này được viết dựa trên thuật toán trong bài báo "Biologically Inspired Bearing-Only Navigation and Tracking" - Tác giả: Savvas G. Loizou và Vijay Kumar.

Trên mặt phẳng tọa độ 2D, giả thuyết rằng ta có ba vật mốc cố định tại A, B, C, cùng một mobile agent. Mobile agent này được trang bị sensors có khả năng đo góc lệch (bearing angle) giữa trục tọa độ x và vị trí của ba vật mốc (landmark). S. Loizou và V. Kumar đề xuất thuật toán điều khiển chỉ sử dụng các góc lệch này để mobile agent di chuyển tới bất cứ vị trí nào trên mặt phẳng. Ý tưởng của thuật toán điều khiển này bắt nguồn từ việc nhiều loài côn trùng với khả năng thu nhận thông tin về khoảng cách (range) kém, có thể di chuyển chỉ dựa trên thông tin về góc lệch (bearing-only navigation).

% Simulation of Savvas G. Loizou and Vijay Kumar's paper

% "Biologically Inspired Bearing-Only Navigation and Tracking"

% 2014.02.25

clear all

clc

%

% Simulation Setup

%

% Parameters

t_end = 1000; % simulation time

T_step = 0.1; % simulation step

J = [0 -1; 1 0]; % 90 deg rotation matrix

a = [0;0]; b = a; c = a;

% Three landmarks A, B, C positions in the plane

pA = [0;0];

pB = [2;0];

pC = [0;2];

% Initial position of vehicle

p = zeros(2, t_end); % initialize the position vector

p0 = [1;0];

p(:,1) = p0;

% Calculate the desired position and the desired bearing vectors of mobile agent

p_d = [3;3]; % desired position

a_d = (pA - p_d)/norm(pA - p_d,2); % desired bearing vectors

b_d = (pB - p_d)/norm(pB - p_d,2);

c_d = (pC - p_d)/norm(pC - p_d,2);

disp('Desired bearing angles:')

angle1d = findAngle2vector(a_d(1,1),a_d(2,1),b_d(1,1),b_d(2,1));

angle2d = findAngle2vector(b_d(1,1),b_d(2,1),c_d(1,1),c_d(2,1));

angle3d = findAngle2vector(c_d(1,1),c_d(2,1),a_d(1,1),a_d(2,1));

rad2deg([angle1d angle2d angle3d])

%

% System's Dynamics

%

for i=1:t_end-1

p_t = p(:,i);

a = (pA - p_t)/norm(pA - p_t, 2);

b = (pB - p_t)/norm(pB - p_t, 2);

c = (pC - p_t)/norm(pC - p_t, 2);

u = -((a_d'*(J*a))*(J*a) + (b_d'*(J*b))*(J*b) + (c_d'*(J*c))*(J*c));

p(:,i+1) = p_t + T_step * u;

end

%

% Simulation Results

%

hold on

% System's Trajectory

plot(p(1,1), p(2,1), '*k', p(1,1:t_end-1), p(2,1:t_end-1), 'r', p(1,t_end), p(2,t_end), 'or', pA(1,1), pA(2,1), '*g', pB(1,1), pB(2,1), '*y', pC(1,1), pC(2,1), '*m')

grid on

axis square

% Final bearing angles

disp('Final bearing angles:')

angle1 = findAngle2vector(a(1,1),a(2,1),b(1,1),b(2,1));

angle2 = findAngle2vector(b(1,1),b(2,1),c(1,1),c(2,1));

angle3 = findAngle2vector(c(1,1),c(2,1),a(1,1),a(2,1));

rad2deg([angle1 angle2 angle3])

% Mark final positions

line([p(1,t_end),pA(1,1)],[p(2,t_end),pA(2,1)],'LineStyle','--','Color','b');

line([p(1,t_end),pB(1,1)],[p(2,t_end),pB(2,1)],'LineStyle','--','Color','b');

line([p(1,t_end),pC(1,1)],[p(2,t_end),pC(2,1)],'LineStyle','--','Color','b');

Kết quả mô phỏng như sau

Trong cả hai mô phỏng này, ba vật mốc được đặt ở vị trí có tọa độ A(0;0), B(2;0), C(0;2). Vị trí ban đầu của mobile agent được đánh dấu bởi dấu "*", trong khi vị trí cuối được đánh dấu bằng dấu tròn "o".

Simulation 1: vị trí ban đầu của mobile agent: p_0 = (1;0), vị trí mong muốn di chuyển tới: p_d = (3;3)

Simulation 2: vị trí ban đầu của mobile agent: p_0 = (1;0), vị trí mong muốn di chuyển tới: p_d = (0.5;1)

Để tìm hiểu chi tiết hơn, các bạn tham khảo bài báo và file MATLAB đính kèm.