✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进。
🍎个人主页：Matlab科研工作室
🍊个人信条：格物致知。
更多Matlab仿真内容点击👇
⛄ 内容介绍
我们使用了本文[2]中描述的匈牙利赋值算法，并改编了[1]中的matlab实现。为了估计机器人团队的运行时间，我们计算了10条路径的平均值。然后我们将C-CAPT算法扩展到去中心化D-CAPT算法。我们在覆盖起始位置和目标位置的凸包中施加相同的无障碍消耗。在分散的假设所有机器人彼此相距足够远，以便机器人能够计算而不考虑与其他机器人的碰撞和交互。最初，所有机器人都是随机分配一个目标，然后我们假设机器人的数量与目标数量。假设机器人有一个可以发送和接收信息的视野来自其他机器人的消息。因此，当两个机器人进入彼此的领域时，它们会局部优化通过改变它们的目标位置，使它们在本地移动最小化平方距离损失函数。
⛄ 部分代码
function [desired_state,fret] = dcaptTrajGenerator(var,tc,state, qn)
%% The function which performs the dcapt computation and simulation
% var is a structure containing start goal f(assignment of goal) nbots
% bound vmax bound
%% Assign params
persistent tprev C_prev U_prev f coeff G
N = var.nbots;
tf = var.tf;
if(tc>=tf)
desired_state.pos = G(f(qn),:)';
desired_state.vel = zeros(3,1);
desired_state.acc = zeros(3,1);
desired_state.yaw = 0;
desired_state.yawdot = 0;
fret =f;
return;
end
if isempty(tprev)
G = var.goal;
f = var.f;
C_prev = computeNeighb(var.start, var.h);
U_prev = C_prev;
tprev = var.t0;
%% TODO set the des
X_V_0 = extractPosVel(state);
coeff = computeCoeff(X_V_0, G(f,:), tc, var.tf, true(N,1));
desired_state.pos = X_V_0(qn,1:3)';
desired_state.vel = zeros(3,1);
desired_state.acc = zeros(3,1);
desired_state.yaw = 0;
desired_state.yawdot = 0;
fret = f;
return;
end
%% Actual algorithm
X_V_prev = extractPosVel(state);
X_prev = X_V_prev(:,1:3);
isFC = false(N,1);
isFC(qn) = true;
C_t = computeNeighb(X_prev, var.h);
U_t = U_prev | (C_t & (~C_prev));
U_t = U_t & C_t;
while(any(isFC))
%robots to check
rtc = find(isFC);
for i = 1:numel(rtc)
ri = rtc(i);
isFC(ri) = false;
% neighb
n_ind = find(U_t(ri,:));
for j = 1:numel(n_ind)
% neighb
rj = n_ind(j);
% any has to be swapped
if isSwap(X_prev(ri,:), G(f(ri),:), X_prev(rj,:), G(f(rj),:))
% swap fi fj
tmp = f(ri);
f(ri) = f(rj);
f(rj) = tmp';
% new neighb
U_t(ri,:) = C_t(ri,:);
U_t(ri,rj) = false;
U_t(rj,:) = C_t(rj,:);
U_t(rj,ri) = false;
% recompute trajs
r_inds = [ri;rj];
coeff_ij = computeCoeff(X_V_prev, G(f,:), tprev, tf , r_inds );
coeff.x([ri;rj],:) = coeff_ij.x;
coeff.y([ri;rj],:) = coeff_ij.y;
coeff.z([ri;rj],:) = coeff_ij.z;
isFC(ri) = true;
isFC(rj) = true;
end
U_t(ri,rj) = false;
end
end
end
%plotSim(X_c, var, var.start, var.goal, var.scale)
tprev = tc;
C_prev = C_t;
U_prev = U_t;
fret=f;
[Xc,Vc,Ac] = computeState(tc, coeff, qn);
desired_state.pos = Xc;
desired_state.vel = Vc;
desired_state.acc = Ac;
desired_state.yaw = 0;
desired_state.yawdot = 0;
end
function C = computeNeighb(X, h )
%% X = N by ndim position
% C = N by N sparse matrix 1 for points which are less than h dist
% btw them
N = size(X,1);
C = pdist2(X,X)<=h;
% make diag zeros
C(logical(eye(N))) = 0;
end
function [Xnext,Vnext,Anext] = computeState(tc, coeff, qn)
%% Given the current state and current goals and the
% current time, the time to estimate the next state
% and the end time compute Xnext
tpos = [ 1; tc; tc^2; tc^3; tc^4; tc^5; tc^6; tc^7];
tvel = [ 0; 1; 2*tc; 3*tc^2; 4*tc^3; 5*tc^4; 6*tc^5; 7*tc^6];
tacc = [ 0; 0; 2; 6*tc; 12*tc^2; 20*tc^3; 30*tc^4; 42*tc^5];
Xnext = [coeff.x(qn,:)*tpos; coeff.y(qn,:)*tpos; coeff.z(qn,:)*tpos];
Vnext = [coeff.x(qn,:)*tvel; coeff.y(qn,:)*tvel; coeff.z(qn,:)*tvel];
Anext = [coeff.x(qn,:)*tacc; coeff.y(qn,:)*tacc; coeff.z(qn,:)*tacc];
end
function toSwap = isSwap(x_i, g_i, x_j, g_j)
%% checks if u'*w <0; where u = xj_c - xj_c
% checks if u'*w <0; where w = xj_tf - xj_tf
U = x_j - x_i; % 1 by n
W = g_j - g_i; % 1 by n
toSwap = U*W' < 0; % 1 by n x n by 1
end
function X = extractPosVel(state)
%% extract pos and vel
X = cell2mat(state);
X = X(1:6,:)';
end
function coeff = computeCoeff(X_V, G, tprev, tf , rob_ind)
%% Compute the coefficients of the septic for all the
% robots if rob_ind is not passed
X_V_int = X_V(rob_ind,:)';
G_int = G(rob_ind,:)';
zrs = zeros(size(X_V_int(1,:)));
x0= X_V_int(1,:);
vx0= X_V_int(4,:);
y0= X_V_int(2,:);
vy0= X_V_int(5,:);
z0= X_V_int(3,:);
vz0= X_V_int(6,:);
xf = G_int(1,:);
yf = G_int(2,:);
zf = G_int(3,:);
% computeSeptic(x0,v0,a0,j0,xf,vf,af,jf, t0,tf)
coeff.x = computeSeptic(x0,vx0,zrs,zrs,xf,zrs,zrs,zrs, tprev,tf)';
coeff.y = computeSeptic(y0,vy0,zrs,zrs,yf,zrs,zrs,zrs, tprev,tf)';
coeff.z = computeSeptic(z0,vz0,zrs,zrs,zf,zrs,zrs,zrs, tprev,tf)';
end
⛄ 运行结果
⛄ 参考文献
[1] James Munkres. Algorithms for the Assignment and Transportation Problems. Journal of the Society for Industrial and Applied Mathematics, 5(1):32–38, 1957.
[2] Matthew Turpin, Nathan Michael, and Vijay Kumar. Capt: Concurrent assignment and planning of trajectories for multiple robots. Int. J. Rob. Res., 33(1):98–112, January 2014.
