自适应蚁群算法优化的攀爬机器人的路径规划

作者：我家自动化 | 2024-07-02 04:26:53

踩

大家好，我是带我去滑雪！

攀爬机器人是一种能够在复杂环境中自主移动和攀爬的具有广阔应用前景的智能机器人，具有较强的应用潜力和广泛的研究价值。随着科技的不断发展，攀爬机器人在许多领域中的应用越来越广泛，例如建筑物维护、救援任务、环境监测等，其在复杂环境下的路径规划问题一直是学术界和工业界的关注焦点。在现实应用中，攀爬机器人的自主移动和路径规划仍然面临很多挑战。因为攀爬机器人工作环境复杂、移动范围高度不确定，传统的路径规划方法存在着计算复杂度高、搜索效率低等问题，往往难以适应攀爬机器人的需求。所以，研究如何有效地规划攀爬机器人的行驶路径已经成为了当前研究的热点问题。

自适应蚁群算法作为一种常用的优化算法，具有并行计算能力、适应性强以及全局搜索的特性，因而在解决路径规划的优化问题等方面广泛应用。本项目旨在基于自适应蚁群算法优化，探索解决攀爬机器人路径规划问题的新方法。通过利用自适应蚁群算法的全局搜索能力和路径优化特性，来提高全局路径规划的效率和质量，为攀爬机器人的实际应用提供有效的路径规划解决方案。使用matlab软件进行模拟，下面开始代码实战。

（1）地图建模

（2）机器人建模

(3)自适应蚁群算法代码实现

（1）地图建模

栅格法建模是一种广泛应用于地理信息系统中的空间数据建模方法。由于采用其方法建模的分辨率高，安全系数高，信息清晰，规划空间表达一致，灵活性强，易于存储和更新，在编程中容易实现，所以采用栅格法建立攀爬机器人的工作环境模型。下面，绘制栅格划分图：


function [map1] = map1(type)
 
% type=1 简单栅格 =2 复杂栅格
map_easy = [1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
    1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
    1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
    1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
    1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
    1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
    1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
    1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
    1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
    1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
    1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 
    1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
    1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
    1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
    1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
    1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
    1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
    1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
    1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
    1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1];
map_comp = [1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
    1 0 1 1 0 1 1 1 1 1 0 1 1 1 1 0 0 0 1 1
    1 0 0 1 0 1 0 1 0 1 1 0 1 1 1 0 1 0 1 1
    1 0 1 1 1 0 0 1 0 0 1 1 0 1 1 0 1 0 1 1
    1 1 1 0 1 1 0 0 0 1 1 1 1 1 1 0 1 0 1 1
    1 0 0 0 1 1 0 0 0 1 1 0 1 1 1 1 0 0 1 1
    1 0 1 0 0 1 0 0 0 1 1 1 1 0 1 0 0 1 1 1
    1 0 1 0 1 1 0 0 0 1 0 1 0 0 1 1 0 0 1 1
    1 0 0 0 1 1 1 1 1 1 0 1 0 0 1 1 1 1 1 1
    1 1 1 1 0 1 1 1 1 1 0 1 0 0 1 1 0 1 1 1
    1 1 1 0 0 0 1 0 0 1 0 0 0 0 0 1 1 1 1 1
    1 1 1 0 1 1 1 0 0 1 1 1 1 1 1 1 1 1 1 1
    1 1 1 0 1 1 1 1 1 0 1 0 0 0 1 0 0 1 0 1
    1 1 1 1 1 1 1 1 1 0 1 0 0 0 1 0 0 1 0 1
    1 1 0 0 1 1 1 0 1 1 1 0 0 0 1 0 0 1 0 1
    1 1 0 0 1 1 0 1 0 1 1 1 1 0 0 1 1 1 1 1
    1 1 1 1 1 1 0 0 0 1 0 0 1 1 1 1 0 0 0 1
    1 1 1 1 1 1 1 1 1 1 0 1 1 1 0 1 1 0 0 1
    1 1 1 1 1 1 1 1 1 1 1 1 0 1 0 0 1 1 1 1
    1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 1 1 0 0 1];
 
if type==1
    map1 = map_easy;
else
    map1 = map_comp;
end

输出结果：

（2）机器人建模

攀爬机器人利用视觉传感器对工作环境的信息进行采集整理，进行降维处理，将三维空间的信息转化为二进制方阵，进而在一个二维平面空间中，找到一条从起始位置到目标位置的无重复无碰撞的最优解路径。出于对建模的有效性考虑，假设攀爬机器人在二维空间中移动，将攀爬机器人视作一个红色质点在二维空间内移动，因此机器人的体积可忽略不计。工作空间内的障碍物可用黑体正方形进行表示。

(3)自适应蚁群算法代码实现

采用自适应蚁群算法在20×20的平面空间中寻找一条从起始位置S到目标位置T的最优路径，在该机器人二维空间模型中存在着多个不移动的不规则障碍物。其中，起始位置O的坐标为（10,16），目标位置E的坐标为（4,2）。算法的执行流程如下图所示：


clc,clear
close all
rng(2)
 
%% 构建简单/复杂环境栅格图
type = 1; % 1 简单栅格 2 复杂栅格
map = map(type);
map1 = map;
 
%% 画出栅格
[m,n] = size(map);
figure(1)
for i=1:m
    for j=1:n
        if map(i,j)==0
            x1=j;   y1=n-i;
            x2=j;   y2=n-i+1;
%             if j-1~=0
%                 map1(i,j-1) = 0;
%             end
            x3=j-1; y3=n-i+1;
            if i+1<=m
                map1(i+1,j) = 0;
            end
            x4=j-1; y4=n-i;
            fill([x1,x2,x3,x4],[y1,y2,y3,y4],[0,0,0]);
            hold on
        else
            x1=j;   y1=n-i;
            x2=j;   y2=n-i+1;
            x3=j-1; y3=n-i+1;
            x4=j-1; y4=n-i;
            fill([x1,x2,x3,x4],[y1,y2,y3,y4],[1,1,1]);
            hold on
        end
        
    end
end
axis([0,m,0,n])
grid on
grid minor
box on
set(gca,'LineWidth',1)
map(:,:) = map1(end:-1:1,:);
 
%% 起始点/终点
Spoint = [2,4];
Epoint = [16,10];
Start = sub2ind([m,n],Spoint(1),Spoint(2));% 下标转索引
End = sub2ind([m,n],Epoint(1),Epoint(2));% 下标转索引
 
%% 参数的设置
popsize = 50;           % 蚂蚁数目
maxiter = 800;          % 最大迭代次数
Rho = 0.1;              % 信息素挥发系数
Beta = 5;               % 启发式因子重要程度参数
Alpha = 2;              % 信息素重要程度参数
q0 = 0.9;               % 偏向选择的阈值
Q=1;
X = size(map,1);      % 问题的状态空间矩阵的行数
Y = size(map,2);      % 问题的状态空间矩阵的列数
 
tauInit = 1/((X+Y)*(X*Y));        % 初始化信息素的量
tauInfo = tauInit .* ones(X*Y,8); % 8个节点 对于某个节点 其可选的下一个节点是其邻接节点 共8个
 
upPheromone = 500;              % 信息素上限
lowPheromone = tauInit;         % 信息素下限
 
bestSoFar_path_length = inf;    % 当前最优路径长度
bestSoFar_pathNode = [];
 
bestSoFar_PathNode = cell(1,maxiter);
bestSoFar_PathLength = zeros(1,maxiter);
best_dist = [];
 
%% 迭代
tic
for NC=1:maxiter
    for k=1:popsize
        currentNode = Start;
        pathInfo = currentNode; % 记录一只蚂蚁的行驶路径的节点信息
        path_length = 0;        % 记录蚂蚁走过的路径长度
        distan = []; % 距离矩阵
        availableNode = findNextNodeSet(map,currentNode); %% 未经过禁忌表处理的可选节点
        nodeNumber = size(availableNode,1);
        while(nodeNumber>0&&currentNode~=End)
            % 计算启发信息
            heuristicInfo = compute_HeuristicInfo(End,map,availableNode);
            tauInfo_ = tauInfo(currentNode,:)';% 提取信息素
            % 启发信息和信息素的综合
            [info,availableNode_] = CCP(...
                tauInfo_,heuristicInfo,pathInfo,availableNode,Alpha,Beta); % 经过禁忌表处理的可选节点
            
            nodeNumber = size(availableNode_,1);
            % 如果无可选节点 就跳出循环
            if nodeNumber==0
                break
            end
            nextNode = selectNextNode(info,availableNode_,q0);
            [xi,yi] = ind2sub([m,n],currentNode);
            [x,y] = ind2sub([m,n],nextNode);
            distanceCurrentAndNext = sqrt( (x-xi)^2 + (y-yi)^2 );
            distan = [distan distanceCurrentAndNext];
            path_length = path_length + distanceCurrentAndNext;
            
            pathInfo = [pathInfo,nextNode];
            
            %%% 判断下一节点集是否中存在目标点
            currentNode = nextNode;
            availableNode = findNextNodeSet(map,currentNode);
            nodeNumber = size(availableNode,1);
            
            flag = ismember(End,availableNode);
            if flag==1
                nextNode = End;
                pathInfo = [pathInfo,nextNode];
                [xi,yi] = ind2sub([m,n],currentNode);
                [x,y] = ind2sub([m,n],nextNode);
                distanceCurrentAndNext = sqrt( (x-xi)^2 + (y-yi)^2 );
                distan = [distan distanceCurrentAndNext];
                path_length = path_length + distanceCurrentAndNext;
                break
            end
        end %end while，
        
        % 更新路径
        if pathInfo(end) == End && bestSoFar_path_length>path_length
            bestSoFar_path_length = path_length;
            bestSoFar_pathNode = pathInfo;
            best_dist = distan;
        end
        
        % 更新信息素
        for i=1:size(pathInfo,2)-1
            currentCity = pathInfo(1,i);
            nextCity = pathInfo(1,i+1);
            [xi,yi] = ind2sub([m,n],currentCity);
            [x,y] = ind2sub([m,n],nextCity);
            
            relativeIndex = CCTRI(xi,yi,x,y);%第二点相对于第一个点的位置
            
            tau_1 = tauInfo(currentCity,:);
            
            tau_1(1,relativeIndex) = (1-Rho)*tau_1(1,relativeIndex) + Q/distan(i);
            
            if tau_1(1,relativeIndex)<lowPheromone
                tau_1(1,relativeIndex) = lowPheromone;
            end
            
            tauInfo(currentCity,:) = tau_1;
            
        end
    end 
    
    bestSoFar_PathNode{1,NC} = bestSoFar_pathNode;
    bestSoFar_PathLength(1,NC) = bestSoFar_path_length;
    % 更新信息素
    for i=1:size(bestSoFar_pathNode,2)-1
        currentCity = bestSoFar_pathNode(1,i);
        nextCity = bestSoFar_pathNode(1,i+1);
        [xi,yi] = ind2sub([m,n],currentCity);
        [x,y] = ind2sub([m,n],nextCity);
        
        relativeIndex = CCTRI(xi,yi,x,y);%第二点相对于第一个点的位置
        
        tau_1 = tauInfo(currentCity,:);
        
        tau_1(1,relativeIndex) = (1-Rho)*tau_1(1,relativeIndex) + Q/best_dist(i);
        
        if tau_1(1,relativeIndex)<lowPheromone
            tau_1(1,relativeIndex) = lowPheromone;
        end
        
        tauInfo(currentCity,:) = tau_1;
        
    end
end %end NCmax
 
toc
clear currentNode distanceCurrentAndNext bad_pathNode flag heuristicInfo NC NCmax nextNode ...
    nodeNumber num_point path_length pathInfo info availableNode availableNode_ ...
    bestSoFar_path_length bestSoFar_pathLength bestSoFar_PathNode
 
%% 绘制全局最优路径节点信息
the_end_path = bestSoFar_pathNode;
the_end = length(bestSoFar_PathLength);
length_shortest = bestSoFar_PathLength(1,the_end);
len_ROUTS = length(the_end_path);
Rx = the_end_path;
Ry = the_end_path;
for i=1:len_ROUTS
    [Rx(i),Ry(i)] = ind2sub([m,n],the_end_path(i));
end
plot(Ry,Rx,'r-','lineWidth',2);% 画出最优路径图
title(['Length of the global shortest road is ',num2str(length_shortest)])
 
%% 绘制迭代图
figure(2)
plot(bestSoFar_PathLength,'r-','lineWidth',2)
xlabel('迭代次数')
ylabel('适应度值')
title(['进化过程 '])

输出轨迹路径图：

输出算法迭代图：

需要数据集的家人们可以去百度网盘（永久有效）获取：

链接：https://pan.baidu.com/s/173deLlgLYUz789M3KHYw-Q?pwd=0ly6
提取码：2138

更多优质内容持续发布中，请移步主页查看。

博主的WeChat:TCB1736732074

点赞+关注,下次不迷路！

声明：本文内容由网友自发贡献，转载请注明出处：【wpsshop】