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)