【协同任务】基于matlab VFH算法多无人机协同控制技术【含Matlab源码 1999期】

68 阅读 0 评论 45 点赞

我是靠谱客的博主幽默老师，最近开发中收集的这篇文章主要介绍【协同任务】基于matlab VFH算法多无人机协同控制技术【含Matlab源码 1999期】，觉得挺不错的，现在分享给大家，希望可以做个参考。

概述

⛄一、VFH*算法简介

在机器人的每个位置,建立相应的向量场直方图,得到若干个初始候选方向,VFH将沿每个候选方向前进的后果考虑进去。对每个候选方向,首先估算出机器人沿该方向前进一段距离ds后的新位置,然后以该位置为中心,再建立新的向量场,对新的向量场继续分析得到若干候选方向,如此继续下去,重复ng次,就建立了一个深度为ng的搜索树。最后使用A算法,找出一条路径,使根结点到某一个叶子结点的代价最低,则这条路径上的初始候选方向即为我们选定的下一步前进方向。

实验证实,ds取为机器人的直径,ng取为INT(传感器量程/ds)时效果最好。由分析可知,当ds取为活动窗口大小,ng取为1的时候,VFH算法即退化为VFH+算法。VFH+算法是VFH算法的一种特殊情况。

搜索树上的每个结点都有一个代价值。代价值的定义为f©=g©+h©。相应的每个符号的定义如下:

对于初始候选方向c0
在这里插入图片描述
对于第i层结点的候选方向ci

启发函数h©定义如下:

搜索树构造完成以后,剩下的工作就是A算法找代价最小的路径了。图3为VFH算法示意图。

图3 VFH*算法示意图
其中大圆表示超声传感器的测量范围,小圆是机器人在位置O时候的活动窗口。假设OA是其中一个初始候选方向(候选方向的选取请参照VFH+算法),对其进行扩展。估算出机器人沿OA前进距离ds后到达A点,然后以A点为新的圆心创建新的向量场直方图,可以得到一些候选方向,假设AB是其中之一,然后以B为圆心继续创建新的向量场直方图,得到新的候选方向,并继续扩展下去。这个过程继续下去,直到扩展ng次。则相应的初始候选方向OA的代价函数即为:

f(A)=g(A)+g(B)+g©+g(D)+g(E)+f(E)

其中g(B)表示从A运动到B的实际代价,其余的依此类推。对O点的每一个候选方向求出相应的f,求出f最小的那个初始候选方向,假设为ON,则ON的方向就是机器人的下一步运动方向,将ON的方向作为指令发送给机器人的运动控制机构。

⛄二、部分源代码

clc
clear
close all
load obstacle ‘obstacle’;
startpoint = [0 0];
endpoint = [6.5 9.5];
% endpoint = [6 9];
%障碍路径图%
subplot(2, 2, 1);
plot(obstacle(:, 1), obstacle(:, 2), ‘.k’);
hold on
plot(startpoint(:, 1), startpoint(:, 2), ‘.b’);
hold on
plot(endpoint(:, 1), endpoint(:, 2), ‘.r’);
hold on
title(‘Implementation of algorithm’);
%VFH 算法变量定义%
step_rec = 0;
step = 0.1; %机器人步进值
f = 5; %角分辨率，单位°
dmax = 1; %激光雷达检测长度
smax = 18; %宽波谷窄波谷阈值
b = 2.5; %常量
a = 1 + b.(dmax.^2); %常量
C = 15; %cv初始值
alpha = deg2rad(f); %角分辨率，单位弧度
n = 360 / f; %扇区数量
thresholdhigh = 3000; %二值化直方图高阈值
thresholdlow = 2000; %二值化直方图低阈值
rsafe = 0.5; %机器人安全距离
current_point = startpoint; %机器人实时位置
%定义机器人目标方向%
kt = round(caculate_beta(current_point, endpoint) / alpha);
if kt == 0
kt = n;
end
%定义机器人初始避障方向%
forward_direction = kt;
cim1_point = [0 0];
%定义二元直方图上次值%
binary_hisvalue = zeros(1, 72);
%算法实现，步骤为H值-》安全角-》机器人下一坐标%
%首先计算每一个扇区H值%
while norm(current_point - endpoint) ~= 0
if norm(current_point - endpoint) > step
i = 1;
obstacle_amplitude = zeros(n, 1);
obstacle_density = zeros(n, 1);
while i<= length(obstacle)
obstacle_distance = norm(obstacle(i, : ) - current_point);
if obstacle_distance < dmax
beta = caculate_beta(current_point, obstacle(i, : ));
enlarged_ange = asin(rsafe / obstacle_distance); % 安全角
k = round(beta / alpha); % 当前方向
if k == 0
k = n;
end
if((5k>rad2deg(beta)-rad2deg(enlarged_ange))&&(5k<rad2deg(beta)+rad2deg(enlarged_ange)))
% h(k) = 1; % (VFH+, 5,6)
h(k) = 1 * caculate_abs(k, caculate_beta(current_point,endpoint)/alpha) + …
1 * caculate_abs(k, cim1_point); % (VFH, 8)
else
h(k) = 0;
end
m = C^2 * (a-b*(obstacle_distance.^2)); % (VFH+, 2)
obstacle_amplitude(k) = obstacle_amplitude(k) + m.*h(k);
i = i + 1;
else
i = i + 1;
end
end
%得到扇区密度值%
obstacle_density = obstacle_amplitude;
%VFH+：下面函数计算出目标向量kt，最佳前进方向angle，机器人下一坐标robot%
% [kt, current_point, forward_direction] = primary_dir(obstacle_density, …
% kt, current_point, forward_direction, current_point, endpoint, …
% thresholdhigh, smax, n, alpha, step);

    %VFH*：下面投影候选方向函数计算出投影点和最佳前进方向angle%
    %VFH*：下面主要候选方向函数计算出目标向量kt，机器人下一坐标robot，
    %      并更新前进方向%
    [~, projected_point, forward_direction] = projected_dir(obstacle_density, ...
        kt, current_point, cim1_point, endpoint, ...
        thresholdhigh, smax, n, alpha, step, 5);  % ng       
    [kt, current_point, forward_direction] = primary_dir(obstacle_density, ...
        kt, current_point, forward_direction, projected_point, endpoint, ...
        thresholdhigh, smax, n, alpha, step);
    cim1_point = current_point;  % c_{i-1}
    
    scatter(current_point(1), current_point(2), '.m');
    drawnow;
    %画极坐标直方图%
    plotHistogram(obstacle_density,kt,forward_direction,thresholdhigh,thresholdlow);
    binary_hisvalue = plotBinHistogram(obstacle_density,kt,forward_direction,thresholdhigh,thresholdlow,binary_hisvalue);
    step_rec = step + step_rec;
else
    disp(['路径长度：' num2str(step_rec)])
    break
end

end