matlab画势场图,【matlab国外编程代写】多智能图-势场法程序matlab

%障碍物和目标，起始位置都已知的路径规划，实现从起点到终点的壁障路径

%初始化车的参数

Xo=[0 0];%起点位置

k=2;%计算引力需要的增益系数

K=0;%初始化

m=5;%计算斥力增益系数，自己设定

Po=2;%障碍影响距离，当障碍和机器人的距离大于这个距离时，斥力为0，不受障碍物的影响，自己设定

n=7;%障碍个数

l=0.5;%步长

J=200;%循环迭代次数

%如果不能实现目标，可能与初始的增益系数，Po设置的不合理有关

%end

%给出障碍物和目标信息

Xsum=[13,12;1 1.2;3 2.5;4 4.5;3 6;6 2;5.5,5.5;8,8.5];%这个向量是(n+1)*2维，其中[10,10]是目标位置，剩下的都是障碍物位置

Xj=Xo;%j=1循环初始，将机器人的起始坐标赋给Xj

%***************初始化结束，开始整体循环******************

for j=1:J%循环开始

Goal(j,1)=Xj(1)%Goal是保存机器人走过的每个点的坐标，刚开始先将起点放进该向量Goal(j,2)=Xj(2)

%调用计算角度模块

Theta=compute_angle(Xj,Xsum,n)%Theta是计算出来的机器人和障碍物，和目标之间的与X轴之间的夹角，统一规定角度为逆时针方向，用这个模块可以计算出来

%调用计算引力模块

Angle=Theta(1)%Theta(1)是机器人和目标之间的角度，目标对车是引力

angle_at=Theta(1)%为了后续计算斥力在引力方向的分量赋值给angle_at

[Fatx,Faty]=compute_Attract(Xj,Xsum,k,Angle)%计算出目标对机器人的引力在x，y方向的两个分量值

%for i=1:n

%if i==1

% Fatxx(i)=Fatx

% Fatyy(i)=Faty

%else

% Fatxx(i)=0

% Fatyy(i)=0

%end

%end%这段循环是为了将引力的两个分量扩展成向量，和后面的斥力叠加时，可以维数相同

%end

for i=1:n

angle_re(i)=Theta(i+1)%计算斥力用的角度是个向量，因为有n个向量，就有n个角度