1 简介
移动机器人的路径规划是移动机器人研究领域中的一个热点问题。
2 部分代码
% Rahul Kala, IIIT Allahabad, Creative Commons Attribution-ShareAlike 4.0 International License.
% The use of this code, its parts and all the materials in the text; creation of derivatives and their publication; and sharing the code publically is permitted without permission.
urrentDirection-pi/4) cos(currentDirection-pi/4)] + ...
(1.0/distanceFrontRightDiagonal)^k*[sin(currentDirection+pi/4) cos(currentDirection+pi/4)];
attractivePotential=max([(1.0/distanceGoal)^k*attractivePotentialScaling minAttractivePotential])*[sin(angleGoal) cos(angleGoal)];
totalPotential=attractivePotential-repulsivePotentialScaling*repulsivePotential;
% perform steer
preferredSteer=atan2(robotSpeed*sin(currentDirection)+totalPotential(1),robotSpeed*cos(currentDirection)+totalPotential(2))-currentDirection;
while preferredSteer>pi, preferredSteer=preferredSteer-2*pi; end % check to get the angle between -pi and pi
while preferredSteer<-pi, preferredSteer=preferredSteer+2*pi; end % check to get the angle between -pi and pi
preferredSteer=min([maxTurn preferredSteer]);
preferredSteer=max([-maxTurn preferredSteer]);
currentDirection=currentDirection+preferredSteer;
% setting the speed based on vehicle acceleration and speed limits. the vehicle cannot move backwards.
preferredSpeed=sqrt(sum((robotSpeed*[sin(currentDirection) cos(currentDirection)] + totalPotential).^2));
preferredSpeed=min([robotSpeed+maxAcceleration preferredSpeed]);
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
fprintf('processing time=%d \nPath Length=%d \n\n', toc,pathCost);
3 仿真结果
4 参考文献
[1]杨一波, 王朝立. 基于改进的人工势场法的机器人避障控制及其MATLAB实现[J]. 上海理工大学学报, 2013, 35(5):5.
部分理论引用网络文献，若有侵权联系博主删除。
5 MATLAB代码与数据下载地址
见博客主页
