1 简介
移动机器人可以在某些环境尤其是恶劣环境下代替人类完成相应的工作，这使得移动机器人的研究在国内外受到广泛关注，而路径规划作为移动机器人完成某项工作的基础功能显得尤为重要，如何快速、准确地规划路径成为移动机器人领域的一大研究热点。移动机器人的路径规划包含全局路径规划和局部路径规划。全局路径规划是在机器人位姿、环境和目标都已知的情形下，寻找一条从起始位置到达目标位置的无碰撞最优路径。局部路径规划是在运行环境未知的情况下实时地规划路径，移动机器人由于传感器探测距离有限以及周围环境的不确定性，很难从整体最优上进行路径规划，因此机器人从局部最优进行路径规划.
在未知环境下,针对传统模糊控制算法规划路径在某些复杂的障碍物环境中出现的死锁问题,设计了障碍逃脱策略,即当机器人进入陷阱区并在目标点方向不可行时,寻找可行方向并设置方向点,由方向点暂代目标点继续前行,沿方向点走出障碍物陷阱区后,则恢复原目标点.对于障碍逃脱策略无法走出的障碍物环境,进一步设计了转向策略,使机器人能成功走出陷阱区域,到达目标点.基于MATLAB仿真平台对所设计算法在不同环境下进行了测试和比较.结果验证了所设计算法的可行性和有效性.
2 部分代码
clear;clc;close all;
%约束条件设定区
global PgoalX;
global PgoalY;
global ProbotX;
global ProbotY;
ProbotX=0;ProbotY=0; %机器人所处位置，起始位置固定值为[0,0]，方便后续计算
PgoalX=500;PgoalY=550;%目标地点位置坐标，建议均为正值，即位于第一象限
NumberOfStep=300;%机器人直线到达目标位置计划所用步数
global DistanceOfStep;
global MaxDistDetect;
DistanceOfStep=ceil(sqrt(PgoalX^2 + PgoalY^2)/NumberOfStep); %每一步最大长度
MaxDistDetect=10*DistanceOfStep;%机器人的最大有效探测距离，10倍已经关联地设入模糊控制器
MaxTurnAngelPerTime = pi/15; %每次最大角度
DetectAngelPerTime =pi/10; %每次探测角度增量
%在固定区随机设定障碍物的位置和大小
global CirX;
global CirY;
global CirR;
CirN=50; %圆式障碍物数量;
CirRmax=30;
CirR=zeros(1,CirN);
CirX=MaxDistDetect+randi(PgoalX-MaxDistDetect,1,CirN); %防止起终点在障碍物里面
CirY=MaxDistDetect+randi(PgoalY-MaxDistDetect,1,CirN);
%障碍圆的半径，防止交叉, 方便计算
for kz=1:CirN
Distance=TwoDotDist(CirX(kz),CirY(kz),CirX,CirY);
%Distance=Distance(find(Distance > 0));
Distance=Distance(Distance > 0);
CirR(kz)=min(0.45*min(Distance),CirRmax);
end
Debug =0;
%标出障碍物---蓝色
if Debug
for p=1:CirN
PlotCir(CirX(p),CirY(p),CirR(p));
end
hold on;grid on;
plot([0,PgoalX],[0,PgoalY],'g-.');
end
%建立理想航线直线方程
IdealgK=PgoalY/PgoalX;
IdealgB=0;
IdealgF=0;
%fismat=readfis('lillterB'); %fismat=readfis('more');
fismat=readfis('lillterC');
AlreadyInBlockedState =0; %预设当前没有处于受阻状态, 向左突围为正 右负
Nstep=NumberOfStep * 30; %控制步数，以免死循环
Rpx=zeros(1,Nstep); %记录机器人轨迹
Rpy=zeros(1,Nstep);
for kz=1:Nstep
%计算出最大偏转角度 / 固定
% MaxDistance=max(LeftDistance,RightDistance);
% MaxAngelCanTurn =pi/3-asin(1/MaxDistance);
MaxAngelCanTurn = MaxTurnAngelPerTime;
%测算前方是否受阻
InBlockedFlag=CheckIsBlocked(LeftDistance, MiddleDistance, RightDistance, 2*DistanceOfStep);
%首先要保证突围方向持续性，防止反复来回沿着障碍边走
if AlreadyInBlockedState %已经在突围状态中
AngelDir =sign(AlreadyInBlockedState);
if InBlockedFlag
[DistanceLMR, AngelNum, Flag]=GetOutBlocked(AngelDir * DetectAngelPerTime);
if Flag
MaxAngelCanTurn = AngelDir * AngelNum * DetectAngelPerTime;
LeftDistance =DistanceLMR(1);
MiddleDistance=DistanceLMR(2);
RightDistance =DistanceLMR(3);
else
warning('计算中可能存在错误，中断前进!!');
break;
end
if abs(AlreadyInBlockedState) < 3 %要不要添加历史记录 再看运行情况
AlreadyInBlockedState =AlreadyInBlockedState +AngelDir;
end
else %换向移动没有受阻说明已经离开之前的死锁区
AlreadyInBlockedState =AlreadyInBlockedState -AngelDir;
end
else %之前没堵
if InBlockedFlag
[DistanceLMR1, AngelNum1, Flag1]=GetOutBlocked( DetectAngelPerTime);
[DistanceLMR2, AngelNum2, Flag2]=GetOutBlocked(-DetectAngelPerTime);
if Flag1 && Flag2
if AngelNum1 < AngelNum2
AlreadyInBlockedState =1;
MaxAngelCanTurn = AngelNum1 * DetectAngelPerTime;
LeftDistance =DistanceLMR1(1);
MiddleDistance=DistanceLMR1(2);
RightDistance =DistanceLMR1(3);
else
AlreadyInBlockedState =-1;
MaxAngelCanTurn = -AngelNum2 * DetectAngelPerTime;
LeftDistance =DistanceLMR2(1);
MiddleDistance=DistanceLMR2(2);
RightDistance =DistanceLMR2(3);
end
else
warning('计算中可能存在错误，中断前进!!');
break;
end
end
end
%对距离进行进行模糊化处理
MiddleDistance=min(MiddleDistance/DistanceOfStep,10);
LeftDistance =min(LeftDistance/DistanceOfStep,10);
RightDistance =min(RightDistance/DistanceOfStep,10);
Dt=min([LeftDistance,MiddleDistance,RightDistance]);
if kz < Nstep
Rpx=Rpx(1:kz); %没有到达规定步数之前就退出，所以需要去除后续的零值
Rpy=Rpy(1:kz); %Rpy(1:kz-1); kz-1
Rpx(kz)=PgoalX; %将目标点位置放入
Rpy(kz)=PgoalY;
else
Rpx(kz+1)=PgoalX; %将目标点位置放入
Rpy(kz+1)=PgoalY;
end
%仿真结果图形化展示
close all;
%机器人运动轨迹---红色
plot(Rpx,Rpy,'r');
hold on;
%plot([Rpx(nt),PgoalX],[Rpy(nt),PgoalY],'r-.');
plot([0,PgoalX],[0,PgoalY],'g-.');
plot(PgoalX,PgoalY,'g*');
plot(0,0,'g*');
xlabel(strcat('红色线条为机器人运动轨迹，本次所用步数为',int2str(kz)));
title('基于模糊控制的路径规划算法的仿真实现');
%标出障碍物---蓝色
for p=1:CirN
PlotCir(CirX(p),CirY(p),CirR(p));
end
3 仿真结果
4 参考文献
[1]郭娜, 李彩虹, 王迪, 张宁, & 宋莉. (2020). 基于模糊控制的移动机器人局部路径规划. 山东理工大学学报：自然科学版, 34(4), 6.
部分理论引用网络文献，若有侵权联系博主删除。
