人工势场法是由Khatib于1986年提出,其方法是将移动机器人所处的环境用势场来定义,通过位置信息来控制机器人的避障行驶,基本思想是构造目标位姿引力场和障碍物周围斥力场共同作用的人工势场,搜索势函数的下降方向来寻找无碰撞路径。人工势场法避障技术使得机器人的移动能很好的适应机器人周围环境的变化,实时性高。

人工势场法的原理

人工势场法原理是:首先构建一个人工虚拟势场,该势场由两部分组成,一部分是目标点对移动机器人产生的引力场,方向由机器人指向目标点,另一部分是障碍物对移动机器人产生的斥力场,方向为由障碍物指向机器人。运行空间的总势场为斥力场和引力场共同叠加作用,从而通过引力和斥力的合力来控制移动机器人的移动。

引力函数

引力函数受移动机器人与目标点的距离影响,当目标点与移动机器人的距离越远时,其所受的势能越大;当目标点与机器人的距离越近时,其所受的势能越小。当机器人势能为零时,则表明机器人到达目标点位置,引力势函数表示为:

斥力函数

斥力函数受移动机器人与障碍物的距离影响,当障碍物与移动机器人的距离越远时,其所受的势能越小;当障碍物与机器人的距离越近时,其所受的势能越大。当机器人势能为零时,则表明机器人已经脱离障碍物的影响范围,斥力势函数表示为:

全局势场函数

根据上述定义的引力场函数和斥力场函数,可以得到整个运行空间的全局势场函数,机器人的全局势场大小为机器人所受的斥力势场和引力势场之和,故全局势场总函数为:

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


©著作权归作者所有:来自51CTO博客作者mob604756f3c518的原创作品,如需转载,请注明出处,否则将追究法律责任

更多相关文章

  1. 基于RBF神经网络的函数曲线拟合
  2. Matlab基本语法7
  3. 遗传算法与函数优化
  4. 3-23(类与对象终结)
  5. “手撕”BP算法——使用MATLAB搭建简单的神经网络(附代码)
  6. 练习2-10 计算分段函数[1] (10分)
  7. 启动延时缩短 50%-80%,函数计算发布镜像加速功能
  8. VS经常提示函数不安全(报错、报警告),该怎么解决?
  9. 14 个 JavaScript 代码优化技巧

随机推荐

  1. Android学习笔记——入门框
  2. android 简易任务管理器源码
  3. android:screenOrientation的说明
  4. Android Studio导入项目常出现的问题
  5. android超炫的图片浏览器
  6. android常用函数参数补充
  7. TextView跑马灯效果
  8. android 滚动条
  9. Android 4.0 SDK下载
  10. Android - 自定义SeekBar