【路径规划】基于状态空间采样实现机器人二维路径规划附matlab完整代码

1 内容介绍

路径规划是移动机器人实现自主导航的关键技术.移动机器人路径规划的任务是:给定起始位置和目标位置,在工作环境中搜索一条从初始位置到目标位置的无碰撞路径.

2 完整代码

% 基于状态空间的采样
%% preProcess
clear;
clc;
figure(1);
clf;
figName = './roadmap.bmp';
road = imread(figName);
[yDirLim, xDirLim] = size(road);
xDirStep = 20;
yDirStep = 25;
xPos = 0;
xPosSet = 0: 1: xDirStep;
yPos = yDirLim / 2;
yPosSet = 0:yDirStep:yDirLim;
yPosSet(1) = 1;
[~, yPosCnt] = size(yPosSet);
imshow(road);
%% mainProgress
while xPos < xDirLim
    % 添加有效终止状态点到集合中
    effectiveY =[];
    for i = 1:yPosCnt
        if noColi(xPos + xDirStep, yPosSet(i), road)
            effectiveY = [effectiveY,yPosSet(i)];
        end
    end
    [~, effectCnt] = size(effectiveY);
    yEval = [];
    xLoca = xPosSet &#