✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。
🍎个人主页：Matlab科研工作室
🍊个人信条：格物致知。
更多Matlab仿真内容点击👇
⛄ 内容介绍
对未知环境下移动机器人的路径规划进行了研究,提出了一种基于模糊控制的路径规划算法.运用模糊推理,构造出一张实践效果较好的控制响应表.针对局部路径规划算法存在的死锁问题,该算法通过建立预防死锁机制,能够使机器人探测到危险区域即沿障碍物的边缘行走绕出障碍物,从而有效避免了死锁现象的发生,为解决死锁问题提供了一种新的思路.在多种环境中进行了仿真实验,仿真结果表明了该算法的有效性和可行性.
⛄ 部分代码
function [Distance,Flag]=GetMinDistance(DectLineK, DectLineB, DectLineF,PgoalXnew,PgoalYnew)
%Distance 为最短距离，Flag表征其有效性 0 无 1有 2异常
%依据测量线斜率情况及走向 选择出交涉区域的障碍，及夹角取向
global CirX;
global CirY;
global CirR;
global ProbotX;
global ProbotY;
if DectLineF ==1
if ProbotY < PgoalYnew %竖直向上运动
Ydown=PgoalYnew;
Yup =PgoalY;
else
Ydown=PgoalYnew;
Yup =ProbotY;
end
IndexX= ((ProbotX-CirR) < CirX) & (CirX < (ProbotX+CirR));
IndexY= (Ydown < CirY) & (CirY < Yup);
IndexALL=IndexX & IndexY;
elseif DectLineF == 2
if ProbotX < PgoalXnew %水平向右运动
Xleft = ProbotX;
Xright= PgoalXnew;
else
Xleft = PgoalXnew;
Xright= ProbotX;
end
IndexX= (Xleft < CirX) & (CirX < Xright);
IndexY= ((ProbotY-CirR) < CirX) & (CirX < (ProbotY+CirR));
IndexALL=IndexX & IndexY;
else
AngelDectLine=atan(DectLineK);
if DectLineK > 0
if ProbotX < PgoalXnew %正斜率向上运动
Xleft = ProbotX;
Yleft = ProbotY;
Xright= PgoalXnew;
Yright= PgoalYnew;
else %正斜率向下运动
Xleft = PgoalXnew;
Yleft = PgoalYnew;
Xright= ProbotX;
Yright= ProbotY;
end
LineBleft = Yleft + Xleft/DectLineK;
LineBright= Yright + Xright/DectLineK;
Btemp=CirR/cos(AngelDectLine);
Xtemp=CirR*sin(AngelDectLine);
LineBup = DectLineB +Btemp;
LineBdown= DectLineB -Btemp;
X1=Xleft-Xtemp;
X2=Xleft+Xtemp;
X3=Xright-Xtemp;
X4=Xright+Xtemp;
IndexX1= (X1 < CirX) & (CirX < X2);
IndexX2= (X2 < CirX) & (CirX < X3);
IndexX3= (X3 < CirX) & (CirX < X4);
Ytemp=CirX/DectLineK;
Y1=LineBleft-Ytemp; %与斜率垂直的左右两条直线
Y2=LineBright-Ytemp;
Ytemp=CirX*DectLineK;
Y3=LineBup +Ytemp; %与斜率平行的上下两条直线
Y4=LineBdown+Ytemp;
IndexY1= (Y1 < CirY) & (CirY < Y3);
IndexY2= (Y4 < CirY) & (CirY < Y3);
IndexY3= (Y4 < CirY) & (CirY < Y2);
IndexALL=(IndexX1 & IndexY1)|(IndexX2 & IndexY2)|(IndexX3 & IndexY3);
else
if ProbotX < PgoalXnew %负斜率向 下运动
Xleft = ProbotX;
Yleft = ProbotY;
Xright= PgoalXnew;
Yright= PgoalYnew;
else %负斜率向 上运动
Xleft = PgoalXnew;
Yleft = PgoalYnew;
Xright= ProbotX;
Yright= ProbotY;
end
LineBleft = Yleft + Xleft/DectLineK;
LineBright= Yright + Xright/DectLineK;
Btemp=CirR/cos(AngelDectLine); %平行线的截矩
Xtemp=CirR*sin(-AngelDectLine); %注意加负号
LineBup = DectLineB +Btemp;
LineBdown= DectLineB -Btemp;
X1=Xleft-Xtemp;
X2=Xleft+Xtemp;
X3=Xright-Xtemp;
X4=Xright+Xtemp;
IndexX1= (X1 < CirX) & (CirX < X2);
IndexX2= (X2 < CirX) & (CirX < X3);
IndexX3= (X3 < CirX) & (CirX < X4);
Ytemp=CirX/DectLineK;
Y1=LineBleft-Ytemp; %与斜率垂直的左右两条直线
Y2=LineBright-Ytemp;
Ytemp=CirX*DectLineK;
Y3=LineBup +Ytemp; %与斜率平行的上下两条直线
Y4=LineBdown+Ytemp;
IndexY1= (Y4 < CirY) & (CirY < Y1);
IndexY2= (Y4 < CirY) & (CirY < Y3);
IndexY3= (Y2 < CirY) & (CirY < Y3);
IndexALL=(IndexX1 & IndexY1)|(IndexX2 & IndexY2)|(IndexX3 & IndexY3);
end
end
%如果没有找出，机器可直达目标，穿越障碍成功，结束探测
CheckX=CirX(IndexALL);
if isempty(CheckX)
Distance =0;
Flag=0;
return;
end
CirXt=CirX(IndexALL); %此时可能有多个
CirYt=CirY(IndexALL);
CirRt=CirR(IndexALL);
%计算测量线与障碍物的远近情况
DCirToLine=DotLineDist(CirXt,CirYt,DectLineK,DectLineB,DectLineF);
%圆至机器人之间的距离
DCirToRobot=TwoDotDist(ProbotX,ProbotY,CirXt,CirYt);
%找出与测量线相交的处到 机器人的距离
Dtemp=DCirToLine.^2;
DCrossToRobot=sqrt(DCirToRobot.^2 -Dtemp)-sqrt(CirRt.^2 - Dtemp);
DProbotToBar=min(DCrossToRobot);
if DProbotToBar < 0 %出现严重异常
Distance =0;
Flag=2;
else
Distance =DProbotToBar;
Flag=1;
end
⛄ 运行结果
⛄ 参考文献
[1] 任伟建王飞吕微. 分层模糊控制的移动机器人路径规划[J]. 科学技术与工程, 2010, 010(010):2317-2321.
[2] 王菁华, 张翠敏. 智能机器人综合路径规划算法在Matlab中的实现[J]. 天津工程师范学院学报, 2006, 16(3):4.
[3] 宋海燕. 基于MATLAB的机器人协助系统中路径规划的蚁群算法研究[J]. 金山, 2010, 02(6).
[4] 李鹏, 温素芳. 基于模糊控制的路径规划算法的实现[J]. 杭州电子科技大学学报：自然科学版, 2007(6):5.
