局部路径规划算法——人工势场法

内容导读

互联网集市收集整理的这篇技术教程文章主要介绍了局部路径规划算法——人工势场法，小编现在分享给大家，供广大互联网技能从业者学习和参考。文章包含4625字，纯文字阅读大概需要7分钟。

内容图文

人工势场法是由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

内容总结

以上是互联网集市为您收集整理的局部路径规划算法——人工势场法全部内容，希望文章能够帮你解决局部路径规划算法——人工势场法所遇到的程序开发问题。 如果觉得互联网集市技术教程内容还不错，欢迎将互联网集市网站推荐给程序员好友。

内容备注

版权声明：本文内容由互联网用户自发贡献，该文观点与技术仅代表作者本人。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容， 请发送邮件至 gblab@vip.qq.com 举报，一经查实，本站将立刻删除。

内容手机端

---