【路径规划】基于matlab正余弦算法栅格地图机器人最短路径规划【含Matlab源码 3066期】

💥💥💥💥💞💞💞💞💞💞欢迎来到麒麟科研社博客之家💞💞💞💞💞💞💥💥💥💥

✅博主简介：985研究生，热爱科研的Matlab仿真开发者，完整代码 论文复现 程序定制 期刊写作 科研合作 扫描文章底部QQ二维码。
🍎个人主页：麒麟科研社
🏆代码获取方式：扫描文章底部QQ二维码

⛳️座右铭：行百里者，半于九十。
更多Matlab路径规划仿真内容点击👇
①Matlab路径规划（麒麟科研社版）

⛳️关注微信公众号Matlab王者助手或Matlab海神之光，更多资源等你来！！

⛄一、正余弦算法及栅格地图简介

1 正余弦算法
对于栅格地图机器人最短路径规划，正余弦算法（Cosine Algorithm）是一种常用的方法之一。该算法基于栅格地图中的直线距离（欧氏距离），并使用余弦公式来计算路径的权重。

以下是使用正余弦算法进行栅格地图机器人最短路径规划的基本步骤：

1、确定起点和终点：在栅格地图上确定机器人的起点和终点位置。
2、构建栅格地图：将栅格地图抽象为一个二维矩阵，其中障碍物或不可通行区域用障碍物标记表示。
3、初始化路径和权重：为每个栅格单元格设置初始路径和权重值。起点的路径值为0，终点的路径值设为无穷大。
4、利用余弦公式计算权重：对于起点周围的邻居单元格，使用余弦公式计算路径的权重。余弦公式如下：
权重 = 当前路径值 + 直线距离 * 余弦方向角度
其中，直线距离是起点到当前单元格的欧氏距离，余弦方向角度是起点到当前单元格的直线方向与水平方向之间的夹角。
5、更新路径和权重：对于每个邻居单元格，如果通过当前单元格的路径可以获得更小的权重，则更新邻居单元格的路径和权重值。
6、重复步骤4和步骤5，直到终点被标记为已访问，或者所有可通行的单元格都已被访问。
7、从终点开始回溯：从终点开始，根据每个单元格的最小路径值，沿着路径回溯到起点，得到最短路径。

2 栅格地图
2.1 栅格法应用背景
路径规划时首先要获取环境信息， 建立环境地图， 合理的环境表示有利于建立规划方法和选择合适的搜索算法，最终实现较少的时间开销而规划出较为满意的路径。一般使用栅格法在静态环境下建立环境地图。
2.2 栅格法实质
将AGV的工作环境进行单元分割， 将其用大小相等的方块表示出来，这样栅格大小的选取是影响规划算法性能的一个很重要的因素。栅格较小的话，由栅格地图所表示的环境信息将会非常清晰，但由于需要存储较多的信息，会增大存储开销，同时干扰信号也会随之增加，规划速度会相应降低，实时性得不到保证；反之，由于信息存储量少，抗干扰能力有所增强，规划速随之增快，但环境信息划分会变得较为模糊，不利于有效路径的规划。在描述环境信息时障碍物所在区域在栅格地图中呈现为黑色，地图矩阵中标为1，可自由通行区域在栅格地图中呈现为白色，地图矩阵中标为0。路径规划的目的就是在建立好的环境地图中找到一条最优的可通行路径，所以使用栅格法建立环境地图时，栅格大小的合理设定非常关键。
2.3 10乘10的静态环境地图

10乘10的静态环境地图代码

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%建立环境地图%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function DrawMap(map)
n = size(map);
step = 1;
a = 0 : step :n(1);
b = 0 : step :n(2);
figure(1)
axis([0 n(2) 0 n(1)]); %设置地图横纵尺寸
set(gca,'xtick',b,'ytick',a,'GridLineStyle','-',...
'xGrid','on','yGrid','on');
hold on
r = 1;
for(i=1:n(1))         %设置障碍物的左下角点的x,y坐标
    for(j=1:n(2))
        if(map(i,j)==1)
            p(r,1)=j-1;
            p(r,2)=i-1;
            fill([p(r,1) p(r,1) + step p(r,1) + step p(r,1)],...
                 [p(r,2) p(r,2) p(r,2) + step p(r,2) + step ],'k');
            r=r+1;
            hold on
        end
    end
end
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%栅格数字标识%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x_text = 1:1:n(1)*n(2)<