【路径规划】使用势函数法进行避障路径规划（Matlab实现）

  💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

💥1 概述

使用势函数法进行避障路径规划是一种常见的无人机路径规划方法，它利用势场模型来避免碰撞和规划安全路径。将环境中的障碍物视为对无人机施加的势场，其中障碍物周围的势能较高，而自由空间的势能较低。 势函数由吸引势和斥力势组成。吸引势使得无人机朝向目标移动，而斥力势则使得无人机避开障碍物。势函数法路径规划是一种常见且有效的无人机路径规划方法，适用于需要实时性和避障能力的场景。通过合适的势函数设计和参数调整，可以生成安全且高效的路径。

📚2 运行结果

主函数部分代码：

clear all

Ka = 2; %attractive
Kr = 0.01; %repulsive
Kv = 5; %velocity

%Input Initial Conditions
a = [0; 0;]; % [ax, ay]

%Objective and Obstacles
xa = [1; 1;];


xobs = [0.4; 0.5;];
po = 0.1;

th = 0:pi/50:2*pi;
circle(:,1) = po * cos(th) + xobs(1);
circle(:,2) = po * sin(th) + xobs(2);

% Initial conditions of all states and state derivatives
x = [0; 0; 0; 0;];      % [x, y, vx, yx]
xdot= [0; 0; 0; 0; 0;];    

x_labels = ['x', 'y', 'vx', 'yx'];

% Define parameters for the simulation
end_flag = true;
stepsize = 0.01;
i = 0;							
time = 0;

syms sx sy sKa xax xay;
pa = sqrt( (sx-xax)^2 + (sy-xay)^2 );
Ua = 0.5*sKa*pa^2;

sax = diff(Ua,sx);
say = diff(Ua,sy);

%syms sx sy sKr xobsx xobsy spo;
%pr = sqrt( (sx-xobsx)^2 + (sy-xobsy)^2 );
%Ur = 0.5*sKr*(1/pr + 1/spo)^2;

%sax = diff(Ur,sx)
%say = diff(Ur,sy)

  
while(end_flag)
    
    i = i+1;			% increment counter
    time = time + 0.01;
    t_out(i) = time;	 	% store time
    x_out(i,:) = x;		% store states
    a_out(i,:) = a;
    
    %a(1) = -vpa(subs(sax,sx,x(1)))-Kv*x(3);
    %a(2) = -vpa(subs(say,sy,x(2)))-Kv*x(4);
    p = sqrt(abs(x(1) - xobs(1))^2 + abs(x(2) - xobs(2))^2);
    if ( p < po )
        Urx = -(Kr*(1/po + 1/((x(1) - xobs(1))^2 + (x(2) - xobs(2))^2)^(1/2))*(2*x(1) - 2*xobs(1)))/(2*((x(1) - xobs(1))^2 + (x(2) - xobs(2))^2)^(3/2));
        Ury = -(Kr*(1/po + 1/((x(1) - xobs(1))^2 + (x(2) - xobs(2))^2)^(1/2))*(2*x(2) - 2*xobs(2)))/(2*((x(1) - xobs(1))^2 + (x(2) - xobs(2))^2)^(3/2));
    else
        Urx = 0;
        Ury = 0;    
    end
    
    Uax = (Ka*(2*x(1) - 2*xa(1)))/2;
    Uay = (Ka*(2*x(2) - 2*xa(2)))/2;
    
    a(1) = - Uax - Kv*x(3) - Urx;
    a(2) = - Uay - Kv*x(4) - Ury; 
    
    x = rk4int('model', stepsize, x, a);
    
    if(x(1)>0.99)
        "hey"
    end
    
    if( (abs(x(1)-xa(1))<0.01) && (abs(x(2)-xa(2))<0.01) )
        end_flag = false;
    end
    
    if(mod(time,1000))
        figure(2);
        plot(x_out(:,1),x_out(:,2));
        hold on;
        plot(xobs(1),xobs(2),'o');
        hold on;
        plot(circle(:,1),circle(:,2));
        hold on;
        plot(xa(1),xa(2),'o'); 
        hold off;
    end
end

figure(1);
    plot(t_out,x_out(:,1));
    hold on;
    plot(t_out,x_out(:,2));
    hold off;
    legend("x","y");
figure(2);
    plot(x_out(:,1),x_out(:,2));
    hold on;
    plot(xobs(1),xobs(2),'o');
    hold on;

🎉3 参考文献

文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。

[1]徐诚智,赵文政,刘银华.薄板件高精度测量线激光连续扫描路径规划[J].组合机床与自动化加工技术,2024(04):149-153.DOI:10.13462/j.cnki.mmtamt.2024.04.031.

[2]朱建军,王明森.基于改进RRT-Connect算法的全局路径规划[J].组合机床与自动化加工技术,2024(04):52-55+61.DOI:10.13462/j.cnki.mmtamt.2024.04.011.

🌈4 Matlab代码实现