【路径规划】人工势场算法多机器人避障路径规划【含Matlab源码 3639期】

最新推荐文章于 2024-03-18 22:58:45 发布

Matlab领域

最新推荐文章于 2024-03-18 22:58:45 发布

阅读量1.4k

点赞数 22

分类专栏： Matlab路径规划（高阶版）文章标签： matlab

本文链接：https://blog.csdn.net/weixin_63266434/article/details/135003652

版权

Matlab路径规划（高阶版）专栏收录该内容

210 篇文章 36 订阅

订阅专栏

在这里插入图片描述

⛄一、人工势场算法车辆避障路径规划简介

1 人工势场算法
人工势场法是由Khatib于1985年在论文《Real-Time Obstacle Avoidance for Manipulators and Mobile Robots》中提出的一种虚拟力法。它的基本思想是将机器人在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求合力来控制移动机器人的运动。应用势场法规划出来的路径一般是比较平滑并且安全，但是这种方法存在局部最优点问题。

算法思想
（1）人工势场法的基本思想是在障碍物周围构建障碍物斥力势场，在目标点周围构建引力势场，类似于物理学中的电磁场。

（2）被控对象在这两种势场组成的复合场中受到斥力作用和引力作用，斥力和引力的合力指引着被控对象的运动，搜索无碰的避障路径。

（3）更直观而言，势场法是将障碍物比作是平原上具有高势能值的山峰，而目标点则是具有低势能值的低谷。

2 人工势场算法车辆避障路径规划
人工势场算法是一种基于势能场的路径规划方法，它通过构建势能场来实现车辆的避障和路径规划。具体来说，它将车辆看作一个带电粒子，将障碍物看作一个带同性电荷的粒子，通过计算粒子间的相互作用力，来实现车辆的避障和路径规划。以下是人工势场算法车辆避障路径规划的步骤：
（1）构建势能场
首先，需要构建势能场，将车辆和障碍物看作带电粒子，通过计算粒子间的相互作用力，来构建势能场。其中，车辆的势能为负，障碍物的势能为正。

（2）计算合力
根据势能场，可以计算出车辆所受到的合力，即车辆所处位置的梯度方向。合力的方向指向势能下降最快的方向。

（3）更新车辆位置
根据车辆所受到的合力，可以更新车辆的位置，使车辆向势能下降最快的方向移动。

（4）重复上述步骤
重复上述步骤，直到车辆到达目标点或者无法到达目标点。

⛄三、部分源代码

clc
clear all;
% X1=[-unidrnd(8) -unidrnd(8)];
% X2=[-unidrnd(8) -unidrnd(8)];
% X3=[-unidrnd(8) -unidrnd(8)];
% X4=[-unidrnd(8) -unidrnd(8)];
% X5=[-unidrnd(8) -unidrnd(8)];
% X6=[-unidrnd(8) -unidrnd(8)];
X1=[3 2];
X2=[3 1];
X3=[2 1];
X4=[2 -1];
X5=[4.5 -1];
X6=[1.5 -3];
% plot(x,y,‘.’,‘Markersize’,50,‘color’,[0 0 0]);
plot(X1(1),X1(2),‘o’,‘Markersize’,10,‘color’,‘r’);
text(X1(1),X1(2)-0.5,‘leader’);
hold on
plot(X2(1),X2(2),‘o’,‘Markersize’,10,‘color’,‘b’);
text(X2(1),X2(2)-0.5,‘robot_1’);
hold on
plot(X3(1),X3(2),‘o’,‘Markersize’,10,‘color’,‘g’);
text(X3(1),X3(2)-0.5,‘robot_2’);
hold on
plot(X4(1),X4(2),‘o’,‘Markersize’,10,‘color’,‘y’);
text(X4(1),X4(2)-0.5,‘robot_3’);
hold on
plot(X5(1),X5(2),‘o’,‘Markersize’,10,‘color’,‘k’);
text(X5(1),X5(2)-0.5,‘robot_4’);
hold on
plot(X6(1),X6(2),‘o’,‘Markersize’,10,‘color’,‘m’);
text(X6(1),X6(2)-0.5,‘robot_5’);
hold on
k=12;%引力增益
m=5;%斥力增益
Po=2.5;%识别半径
n=6;%障碍数目
a=0.1;%斥力半径系数
l=1.5;%队形距离
v=pi/4;%队形夹角
l1=0.1;%leader步长
% l2=0.15;%follower步长
J=300;%步数
Xsum1(1,1)=12;
Xsum1(1,2)=12;
Xsum1=[10,10;4 4;5.1 2;6 -1;6 6;7 4;7.5 1.5];
plot(Xsum1(1),Xsum1(2));
hold on
for i=2:n+1%…………………………………………………………………………………………………………………………障碍物初始化
% Xsum1(i,1)=unidrnd(10);
% Xsum1(i,2)=-2+unidrnd(10);
Xsum2(i,1)=Xsum1(i,1);
Xsum2(i,2)=Xsum1(i,2);
Xsum3(i,1)=Xsum1(i,1);
Xsum3(i,2)=Xsum1(i,2);
Xsum4(i,1)=Xsum1(i,1);
Xsum4(i,2)=Xsum1(i,2);
Xsum5(i,1)=Xsum1(i,1);
Xsum5(i,2)=Xsum1(i,2);
Xsum6(i,1)=Xsum1(i,1);
Xsum6(i,2)=Xsum1(i,2);
plot(Xsum1(i,1),Xsum1(i,2),‘.’,‘Markersize’,150,‘color’,‘k’);
hold on
end