【路径规划】模糊逻辑算法路径规划【含Matlab源码 3801期】

Matlab领域

于 2024-01-19 10:34:38 发布

阅读量1k

点赞数 18

分类专栏： Matlab路径规划（高阶版）文章标签： matlab

本文链接：https://blog.csdn.net/weixin_63266434/article/details/135682451

版权

Matlab路径规划（高阶版）专栏收录该内容

200 篇文章 36 订阅

订阅专栏

在这里插入图片描述
✅博主简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，Matlab项目合作可私信。
🍎个人主页：海神之光
🏆代码获取方式：
海神之光Matlab王者学习之路—代码获取方式
⛳️座右铭：行百里者，半于九十。

⛄一、模糊控制机器人路径规划简介

在未知环境下，针对传统模糊控制算法规划路径在某些复杂的障碍物环境中出现的死锁问题，设计了障碍逃脱策略，即当机器人进入陷阱区并在目标点方向不可行时，寻找可行方向并设置方向点，由方向点暂代目标点继续前行，沿方向点走出障碍物陷阱区后，则恢复原目标点。对于障碍逃脱策略无法走出的障碍物环境，进一步设计了转向策略，使机器人能成功走出陷阱区域，到达目标点。基于 MATLAB 仿真平台对所设计算法在不同环境下进行了测试和比较。结果验证了所设计算法的可行性和有效性。

⛄二、部分源代码

map=int16(im2bw(imread(‘map1.bmp’))); % input map read from a bmp file. for new maps write the file name here
source=[20 20]; % source position in Y, X format
goal=[480 480]; % goal position in Y, X format
robotDirection=pi/4; % initial heading direction
robotSize=[10 10]; %length and breadth
robotSpeed=10; % arbitrary units
maxRobotSpeed=10; % arbitrary units
S=10; % safety distance
distanceThreshold=30; % a threshold distace. points within this threshold can be taken as same.
maxAcceleration=10; % maximum speed change per unit time
directionScaling=60*pi/180; % fuzzy outputs to turn are restriect to -1 and 1. these are magnified here. maximum turn can be 60 degrees

%%%%% parameters end here %%%%%

fuz=readfis(‘fuzzyBase.fis’); % fuzzy inference system used. to read/edit use fuzzy(readfis(‘fuzzyBase.fis’)) at the command line
distanceScaling=(size(map,1)^{2+size(map,2)}2)^0.5; % all inputs are scaled by this number so that all distance inputs are between 0 and 1. maximum distance can be distanceScaling
currentPosition=source; % position of the centre of the robot
currentDirection=robotDirection; % direction of orientation of the robot
robotHalfDiagonalDistance=((robotSize(1)/2)^{2+(robotSize(2)/2)}2)^0.5; % used for distance calculations
pathFound=false; % has goal been reached
prevTurn=0; % preffered turn at the previous time step, used for turning heuristic, see variable turn being set below.
prevDistanceLeftDiagonal=distanceScaling; % diagonal distance at the previous time step, used for tracking obstacles, used for turning heuristic, see variable turn being set below.
prevDistanceRightDiagonal=distanceScaling; % diagonal distance at the previous time step, used for tracking obstacles, used for turning heuristic, see variable turn being set below.
pathCost=0;
t=1;
imshow(map==1);
rectangle(‘position’,[1 1 size(map)-1],‘edgecolor’,‘k’);
pathLength=0;
if ~plotRobot(currentPosition,currentDirection,map,robotHalfDiagonalDistance)
error(‘source lies on an obstacle or outside map’);
end
M(t)=getframe;
t=t+1;

if ~feasiblePoint(goal,map), error(‘goal lies on an obstacle or outside map’); end

tic;
while ~pathFound

% calculate distance from obstacle at front
for i=robotSize(1)/2+1:distanceScaling
    x=int16(currentPosition+i*[sin(currentDirection) cos(currentDirection)]);
    if ~feasiblePoint(x,map), break; end
end
distanceFront=(i-robotSize(1)/2)/distanceScaling; % robotSize(1)/2 distance included in i was inside the robot body 

% calculate distance from obstacle at front-left diagonal
for i=robotHalfDiagonalDistance+1:distanceScaling
    x=int16(currentPosition+i*[sin(currentDirection-pi/4) cos(currentDirection-pi/4)]);
    if ~feasiblePoint(x,map), break; end
end
distanceFrontLeftDiagonal=(i-robotHalfDiagonalDistance)/distanceScaling;

% calculate distance from obstacle at front-right diagonal
for i=robotHalfDiagonalDistance+1:distanceScaling
    x=int16(currentPosition+i*[sin(currentDirection+pi/4) cos(currentDirection+pi/4)]);
    if ~feasiblePoint(x,map), break; end
end
distanceFrontRightDiagonal=(i-robotHalfDiagonalDistance)/distanceScaling;

% calculate angle deviation to goal
 slopeGoal=atan2(goal(1)-currentPosition(1),goal(2)-currentPosition(2));
 angleGoal=slopeGoal-currentDirection;
 while angleGoal>pi, angleGoal=angleGoal-2*pi; end % check to get the angle between -pi and pi
 while angleGoal<-pi, angleGoal=angleGoal+2*pi; end % check to get the angle between -pi and pi
 angleGoal=angleGoal/pi; % re-scaling the angle as per fuzzy modelling

 % calculate diatnce from goal
 distanceGoal=( sqrt(sum((currentPosition-goal).^2)) )/distanceScaling;
 if distanceGoal*distanceScaling<distanceThreshold, pathFound=true; end
 
 % calculate preferred turn. 
 % this indicates, if the front obstacle is far away, turn so as to more face the goal
 % if the front obstacle is close and a new front obstacle is encountered, turn using the side of the goal is preferred
 % if the front obstacle is close and the same obstacle as encountered in the previous step is found, same turn is made
 if (prevTurn==0 || prevTurn==1) && distanceFront<0.1 && (distanceFrontLeftDiagonal-prevDistanceLeftDiagonal)*distanceScaling<maxRobotSpeed, turn=1;
 elseif prevTurn==-1 && distanceFront<0.1 && (distanceFrontRightDiagonal-prevDistanceRightDiagonal)*distanceScaling<maxRobotSpeed, turn=-1;
 else turn=(angleGoal>=0)*1+(angleGoal<0)*(-1);prevTurn=turn;
 end
 prevDistanceLeftDiagonal=distanceFrontRightDiagonal;
 prevDistanceRightDiagonal=distanceFrontLeftDiagonal;
 
 % pass all computed inputs to a fuzzy inference system
 computedSteer=evalfis([distanceFront distanceFrontLeftDiagonal distanceFrontRightDiagonal angleGoal turn distanceGoal],fuz);
 currentDirection=currentDirection+computedSteer*directionScaling;
 
 % speed is set based on the front and diagonal distance so as not to make the robot collide, but make it slow and even stop before possible collission
 % distances here include additional safety distance of S
 distanceFrontSafety=max([distanceFront*distanceScaling-S 0]); 
 distanceFrontLeftDiagonalSafety=max([distanceFrontLeftDiagonal*distanceScaling-S 0]);
 distanceFrontRightDiagonalSafety=max([distanceFrontRightDiagonal*distanceScaling-S 0]);
 
 % maximum speeds admissible as per the above safety distance
 maxSpeed1=min([sqrt(2*maxAcceleration*distanceFrontSafety) maxRobotSpeed]);
 maxSpeed2=min([sqrt(maxAcceleration*distanceFrontLeftDiagonalSafety) maxRobotSpeed]);
 maxSpeed3=min([sqrt(maxAcceleration*distanceFrontRightDiagonalSafety) maxRobotSpeed]);
 maxSpeed=min([maxSpeed1 maxSpeed2 maxSpeed3]);
 
 % setting the speed based on vehicle acceleration and speed limits. the vehicle cannot move backwards.
 preferredSpeed=min([robotSpeed+maxAcceleration maxSpeed]);
 robotSpeed=max([robotSpeed-maxAcceleration preferredSpeed]);
 robotSpeed=min([robotSpeed maxRobotSpeed]);
 robotSpeed=max([robotSpeed 0]);
 
 if robotSpeed==0, error('robot had to stop to avoid collission'); end
 
 % calculating new position based on steer and speed
 newPosition=currentPosition+robotSpeed*[sin(currentDirection) cos(currentDirection)];
 pathCost=pathCost+distanceCost(newPosition,currentPosition);
 currentPosition=newPosition;
 if ~feasiblePoint(int16(currentPosition),map), error('collission recorded'); end
 
 % plotting robot
 if ~plotRobot(currentPosition,currentDirection,map,robotHalfDiagonalDistance)
    error('collission recorded');
 end
 M(t)=getframe;t=t+1;

end

⛄三、运行结果

在这里插入图片描述

⛄四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1]郭娜,李彩虹,王迪,张宁,宋莉.基于模糊控制的移动机器人局部路径规划[J].山东理工大学学报(自然科学版)

3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除

🍅 仿真咨询
1 各类智能优化算法改进及应用
生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化、公交排班优化、充电桩布局优化、车间布局优化、集装箱船配载优化、水泵组合优化、解医疗资源分配优化、设施布局优化、可视域基站和无人机选址优化

2 机器学习和深度学习方面
卷积神经网络（CNN）、LSTM、支持向量机（SVM）、最小二乘支持向量机（LSSVM）、极限学习机（ELM）、核极限学习机（KELM）、BP、RBF、宽度学习、DBN、RF、RBF、DELM、XGBOOST、TCN实现风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断

3 图像处理方面
图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知

4 路径规划方面
旅行商问题（TSP）、车辆路径问题（VRP、MVRP、CVRP、VRPTW等）、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、车辆协同无人机路径规划、天线线性阵列分布优化、车间布局优化

5 无人机应用方面
无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配

6 无线传感器定位及布局方面
传感器部署优化、通信协议优化、路由优化、目标定位优化、Dv-Hop定位优化、Leach协议优化、WSN覆盖优化、组播优化、RSSI定位优化

7 信号处理方面
信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号、信号配时优化

8 电力系统方面
微电网优化、无功优化、配电网重构、储能配置

9 元胞自动机方面
交通流人群疏散病毒扩散晶体生长

10 雷达方面
卡尔曼滤波跟踪、航迹关联、航迹融合