标签：Xj angle 势场 路径 算法 Obscale 斥力 Po Xsum
人工势场法是由Khatib于1986年提出，其方法是将移动机器人所处的环境用势场来定义，通过位置信息来控制机器人的避障行驶，基本思想是构造目标位姿引力场和障碍物周围斥力场共同作用的人工势场，搜索势函数的下降方向来寻找无碰撞路径。人工势场法避障技术使得机器人的移动能很好的适应机器人周围环境的变化，实时性高。
1 人工势场法的原理
人工势场法原理是：首先构建一个人工虚拟势场，该势场由两部分组成，一部分是目标点对移动机器人产生的引力场，方向由机器人指向目标点，另一部分是障碍物对移动机器人产生的斥力场，方向为由障碍物指向机器人。运行空间的总势场为斥力场和引力场共同叠加作用，从而通过引力和斥力的合力来控制移动机器人的移动。
2 引力函数
引力函数受移动机器人与目标点的距离影响，当目标点与移动机器人的距离越远时，其所受的势能越大；当目标点与机器人的距离越近时，其所受的势能越小。当机器人势能为零时，则表明机器人到达目标点位置，引力势函数表示为：
3 斥力函数
斥力函数受移动机器人与障碍物的距离影响，当障碍物与移动机器人的距离越远时，其所受的势能越小；当障碍物与机器人的距离越近时，其所受的势能越大。当机器人势能为零时，则表明机器人已经脱离障碍物的影响范围，斥力势函数表示为：
4 全局势场函数
根据上述定义的引力场函数和斥力场函数，可以得到整个运行空间的全局势场函数，机器人的全局势场大小为机器人所受的斥力势场和引力势场之和，故全局势场总函数为：
5 Matlab代码实现及仿真结果
我们设置随机生成20个障碍点，将起点设置为(0,0)，终点设置为(5,8)，步长设置为0.05，其他设置参数详见后附的matlab代码，仿真结果如下图所示。
杨馨怡，重庆大学无线通信技术实验室硕士研究生，主研方向为无线通信中的预编码技术。
主函数
%% 初始化
k = 15;%计算引力的增益系数
K = 0;
m = 5; %计算斥力的增益系数。
Po =2.5; %障碍影响距离，当障碍和车的距离大于这个距离时，斥力为0
a =0.5;
l =0.05; %步长
J =200;
start =[0 0];
target= [5 8];
Obscale= ceil(rand(2,20)*10);
x_Obscale= Obscale(1,:)';
y_Obscale= Obscale(2,:)';
Xsum =[target;x_Obscale y_Obscale];
Xj =start;
%% 主体程序
for j =1:J
Goal(j,1) = Xj(1);
Goal(j,2) = Xj(2);
%% 调用计算角度模块
Theta = compute_angle(Xj,Xsum,length(x_Obscale));
%% 调用计算引力模块
Angle = Theta(1);
angle_at = Theta(1);
[Fatx,Faty] =compute_Attract(Xj,Xsum,k,Angle,0,Po,length(x_Obscale));
for i = 1:length(x_Obscale)
angle_re(i) = Theta(i+1);
end
%% 调用计算斥力模块
[Frerxx,Freryy,Fataxx,Fatayy] =compute_repulsion(Xj,Xsum,m,angle_at,angle_re,length(x_Obscale),Po,a);
%% 计算合力和方向，
Fsumyj = Faty+Freryy+Fatayy;
Fsumxj = Fatx+Frerxx+Fataxx;
Position_angle(j) = atan(Fsumyj/Fsumxj);
%% 计算下一步位置
Xnext(1) = Xj(1)+l*cos(Position_angle(j));
Xnext(2) = Xj(2)+l*sin(Position_angle(j));
Xj = Xnext;
if ((Xj(1)-Xsum(1,1))>0) &&((Xj(2)-Xsum(1,2))>0)
K = j;
break;
end
end
K = j;
Goal(K,1)= Xsum(1,1);
Goal(K,2)= Xsum(1,2);
%% 画出路径和障碍
plot(x_Obscale,y_Obscale,'bo');
holdon;
plot(start(1),start(2),'ro',target(1),target(2),'ro');
text(start(1)+0.1,start(2),'start');
text(target(1)+0.1,target(2),'target');
holdon;
plot(Goal(:,1),Goal(:,2),'-k');
axis([010 0 10]);
引力函数
function[Yatx,Yaty] = compute_Attract(X,Xsum,k,angle,b,Po,n)
R =(X(1)-Xsum(1,1))^2+(X(2)-Xsum(1,2))^2;
r =sqrt(R);
Yatx =k*r*cos(angle);
Yaty =k*r*sin(angle);
End
斥力函数
function[Yrerxx,Yreryy,Yataxx,Yatayy] = compute_repulsion(X,Xsum,m,angle_at,angle_re,n,Po,a)
Rat =(X(1)-Xsum(1,1))^2+(X(2)-Xsum(1,2))^2;
rat =sqrt(Rat);
for i =1:n
Rrei(i) =(X(1)-Xsum(i+1,1))^2+(X(2)-Xsum(i+1,2))^2;
rre(i) = sqrt(Rrei(i));
R0 =(Xsum(1,1)-Xsum(i+1,1))^2+(Xsum(1,2)-Xsum(i+1,2))^2;
r0 = sqrt(R0);
if rre(i) > Po
Yrerx(i) = 0;
Yrery(i) = 0;
Yatax(i) = 0;
Yatay(i) = 0;
else
if rre(i)< Po/2
Yrer(i) =m*(1/rre(i)-1/Po)*(1/Rrei(i))*(rat^a);
Yata(i) = a*m*((1/rre(i)-1/Po)^2)*(rat^(1-a))/2;
Yrerx(i) =(1+0.1)*Yrer(i)*cos(angle_re(i));
Yrery(i) =-(1-0.1)*Yrer(i)*sin(angle_re(i));
Yatax(i) = Yata(i)*cos(angle_at);
Yatay(i) = Yata(i)*sin(angle_at);
else
Yrer(i) =m*(1/rre(i)-1/Po)*1/Rrei(i)*Rat;
Yata(i) =m*((1/rre(i)-1/Po)^2)*rat;
Yrerx(i) =Yrer(i)*cos(angle_re(i));
Yrery(i) =Yrer(i)*sin(angle_re(i));
Yatax(i) = Yata(i)*cos(angle_at);
Yatay(i) = Yata(i)*sin(angle_at);
end
end
end
Yrerxx = sum(Yrerx);
Yreryy = sum(Yrery);
Yataxx = sum(Yatax);
Yatayy = sum(Yatay);
end
角度计算函数
functionY = compute_angle(X,Xsum,n)
for i =1:n+1
deltaX(i) = Xsum(i,1)-X(1);
deltaY(i) = Xsum(i,2)-X(2);
r(i) = sqrt(deltaX(i)^2+deltaY(i)^2);
if deltaX(i) > 0
theta = acos(deltaX(i)/r(i));
else
theta = pi-acos(deltaX(i)/r(i));
end
if i == 1
angle = theta;
else
angle = theta;
end
Y(i) =angle;
end
end
标签：Xj,angle,势场,路径,算法,Obscale,斥力,Po,Xsum 来源： https://blog.51cto.com/15127585/2670711
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。