【控制】人工势场法及人工势场函数「建议收藏」

【控制】人工势场法及人工势场函数「建议收藏」人工势场法是由Khatib提出的一种机器人路径规划算法。该算法将目标和障碍物分别看做对机器人有引力和斥力的物体,机器人沿引力与斥力的合力来进行运动。该法结构简单,便于低层的实时控制,在实时避障和平滑的轨迹控制方面,得到了广泛应用,其不足在于存在局部最优解,容易产生死锁现象,因而可能使移动机器人在到达目标点之前就停留在局部最优点。From:人工势场法1.概述我们打两个比方来说明人工势场法的作用机理。首先,我们把构型空间比作一个电势场平面,机器人(的当前构型)比作空间中一点。如果让机器人的起点和障碍物

大家好,又见面了,我是你们的朋友全栈君。


人工势场法-维基百科

人工势场法是由Khatib提出的一种机器人路径规划算法。该算法将目标和障碍物分别看做对机器人有引力和斥力的物体,机器人沿引力与斥力的合力来进行运动。

该法结构简单,便于低层的实时控制,在实时避障和平滑的轨迹控制方面,得到了广泛应用,其不足在于存在局部最优解,容易产生死锁现象,因而可能使移动机器人在到达目标点之前就停留在局部最优点。

From: 人工势场法-维基百科


路径规划-人工势场法(Artifical Potential Field)

引力场 (attractive/gravitation field)

常用的引力函数:
U a t t ( q ) = 1 2 ξ ρ 2 ( q , q g o a l ) U_{att}(q) = \frac{1}{2}\xi\rho^2(q,q_{goal}) Uatt(q)=21ξρ2(q,qgoal)

这里的 ξ \xi ξ 是尺度因子, ρ ( q , q g o a l ) \rho(q,q_{goal}) ρ(q,qgoal) 表示物体当前状态与目标的距离。引力场有了,那么引力就是引力场对距离的导数(类比物理里面 W = F X W=FX W=FX):
F a t t ( q ) = − ∇ U a t t ( q ) = ξ ( q g o a l − q ) F_{att}(q) = -\nabla U_{att}(q) = \xi(q_{goal}-q) Fatt(q)=Uatt(q)=ξ(qgoalq)


斥力场 (repulsive field)

传统的斥力场公式
U r e p ( q ) = { 1 2 η ( 1 ρ ( q , q o b s ) − 1 ρ 0 ) 2 , if  ρ ( q , q o b s ) ≤ ρ 0 0 , if  ρ ( q , q o b s ) > ρ 0 U_{rep}(q) = \left\{\begin{aligned} \frac{1}{2} \eta (\frac{1}{\rho(q, q_{obs})} – \frac{1}{\rho_0})^2, \quad \text{if}\ \rho(q, q_{obs}) \le \rho_0 \\ 0,\quad \text{if}\ \rho(q, q_{obs}) > \rho_0 \end{aligned}\right. Urep(q)=21η(ρ(q,qobs)1ρ01)2,if ρ(q,qobs)ρ00,if ρ(q,qobs)>ρ0

其中,
η \eta η 是斥力尺度因子,
ρ ( q , q o b s ) \rho(q, q_{obs}) ρ(q,qobs) 代表物体和障碍物之间的距离。
ρ 0 \rho_0 ρ0 代表每个障碍物的影响半径。
换言之,离开一定的距离,障碍物就对物体没有斥力影响。

斥力就是斥力场的梯度
F r e p ( q ) = − ∇ U r e p ( q ) = { η ( 1 ρ ( q , q o b s ) − 1 ρ 0 ) ⋅ 1 ρ 2 ( q , q o b s ) ∇ ρ ( q , q o b s ) , if  ρ ( q , q o b s ) ≤ ρ 0 0 , if  ρ ( q , q o b s ) > ρ 0 \begin{aligned}F_{rep}(q) &= -\nabla U_{rep}(q)\\ &= \left\{\begin{aligned} &\eta (\frac{1}{\rho(q, q_{obs})} – \frac{1}{\rho_0}) \\ & \cdot \frac{1}{\rho^2(q,q_{obs})}\nabla\rho(q,q_{obs}),\quad & \text{if}\ \rho(q, q_{obs}) \le \rho_0 \\ & 0,\quad & \text{if}\ \rho(q, q_{obs}) > \rho_0 \end{aligned}\right.\end{aligned} Frep(q)=Urep(q)=η(ρ(q,qobs)1ρ01)ρ2(q,qobs)1ρ(q,qobs),0,if ρ(q,qobs)ρ0if ρ(q,qobs)>ρ0


总场

总的场就是斥力场合引力场的叠加,也就是 U = U a t t + U r e p U=U_{att}+U_{rep} U=Uatt+Urep,总的力也是对对应的分力的叠加,如下图所示:

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

From: 路径规划-人工势场法(Artifical Potential Field)


【机器人路径规划】人工势场法

1.概述

我们打两个比方来说明人工势场法的作用机理。首先,我们把构型空间比作一个电势场平面,机器人(的当前构型)比作空间中一点。如果让机器人的起点和障碍物带正电荷,终点带负电荷,机器人带正电荷。由于同性电荷相斥,异性电荷相吸的原理,机器人将会在电场力的作用下沿着某条路径向终点移动 ,并避开带正电荷的障碍物,如图1所示。

在这里插入图片描述
类似的,我们也可以把构型空间比作一个有起伏地形的区域。其中,起点和障碍物位于较高的区域,终点位于较低的区域,机器人视作一个球体。那么在重力的作用下,机器人将沿着某条轨迹从较高的起点滑落到较低的终点,并避开较高的障碍物。如图2[2]所示。

在这里插入图片描述

以上的两个例子其实就是电势场与重力势场的作用机制,电势场和重力势场都是自然势场。而人工势场法就是在已知起点、终点和障碍物位置的情况下,构建一个人工势场来模仿这种作用机制。人工势场法的优点在于,它其实是一种反馈控制策略,对控制和传感误差有一定的鲁棒性;缺点在于存在局部极小值问题,因此不能保证一定能找到问题的解。

From: 【机器人路径规划】人工势场法


Paper

M. Zhang, Y. Shen, Q. Wang and Y. Wang, “Dynamic artificial potential field based multi-robot formation control,” 2010 IEEE Instrumentation & Measurement Technology Conference Proceedings, 2010, pp. 1530-1534, doi: 10.1109/IMTC.2010.5488238.
在这里插入图片描述

在这里插入图片描述

在这里插入图片描述


在这里插入图片描述

close all;clear

density = 0.2;
Grid_X = 0:density:10;
Grid_Y = 0:density:10;
Basic_Z = ones(length(Grid_X), length(Grid_Y));

P0 = 20;
a = 20; %斥力影响因素
b = 10; %引力影响因素


Goal = [10,10]; %目标
Obs = [ 3, 2;
        3, 3;
        5, 7;
      5.3, 6;
        6, 6;
        2, 4;
        3, 8;
        4, 7;
        8, 9]; %障碍物坐标
for k1 = 1: length(Grid_X)
    for k2 =1:length(Grid_Y)
        X_c = Grid_X(k1);
        Y_c = Grid_Y(k2);
        rre =[];
        
        rat = sqrt((Goal(1)-X_c)^2 +(Goal(2)-Y_c)^2);
        
        Y_rre = [];
        Y_ata = [];
        
        for k3 = 1:length(Obs)
            rre(k3) =    sqrt((Obs(k3,1)-X_c)^2 +(Obs(k3,2)-Y_c)^2);
            Y_rre(k3) = a*(1/rre(k3)  - 1/P0 ) *1/(rre(k3)^2); %基本斥力场公式
            if  isinf(Y_rre(k3))==1|| Y_rre(k3)>150 %为显示效果做的限制处理
                Y_rre = 150;
            end
        end
        Y_ata = b*rat; %基本引力场公式,注意这里引力只有一个值
        Field_rre(k1,k2) = sum(Y_rre);
        Field_ata(k1,k2) =  Y_ata;
    end
end

SUM = Field_rre  + Field_ata;
surf(Grid_X,Grid_Y,SUM) %总力场


Matlab 代码

人工势场算法 Matlab版源码


自己编写的 Matlab

1. 仅考虑引力的情况

% 自写人工势场法测试函数
% 说明:
% X 轴:时间 time
% Y 轴:小车横坐标
% Z 轴:小车纵坐标
% 初始位置:[0 0]'
% 目标位置:[10 10]'

clear

P0 = [0 0]';
Pt(:,1) = P0;
PGoal = [10 10]';
Ut(:,1) = [0 0]';

% 时间参数
tBegin = 0;
tEnd = 10;
dT = 0.01;
T(1,1) = tBegin;
times = (tEnd - tBegin)/dT;

% 其他参数
xi = 0.8;

for time=1:times
    T(1,time+1) = T(1,time) + dT;
    % 引力
    FGravitation = xi * (PGoal - Pt(:,time));
    % 斥力
    FRepulsive = 0;
    % 合力
    Ut(:,time+1) = FGravitation + FRepulsive;
    % 更新位置
    Pt(:,time+1) = Pt(:,time) + dT * Ut(:,time);
end

figure(1)
plot3(T,Pt(1,:),Pt(2,:))
xlabel('Time');
ylabel('XPosition');
zlabel('YPosition');
grid on

figure(2)
plot3(T,Ut(1,:),Ut(2,:))
xlabel('Time');
ylabel('XSpeed');
zlabel('YSpeed');
grid on

在这里插入图片描述

在这里插入图片描述
在这里插入图片描述

在这里插入图片描述

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

发布者:全栈程序员-用户IM,转载请注明出处:https://javaforall.cn/147909.html原文链接:https://javaforall.cn

【正版授权,激活自己账号】: Jetbrains全家桶Ide使用,1年售后保障,每天仅需1毛

【官方授权 正版激活】: 官方授权 正版激活 支持Jetbrains家族下所有IDE 使用个人JB账号...

(0)


相关推荐

  • CAS 认证原理[通俗易懂]

    CAS 认证原理[通俗易懂]一CAS原理简介CAS官方网站上的介绍图:主要原理:用户第一次访问一个CAS服务的客户web应用时(访问URL:http://192.168.7.90:8081/web1),部署在客户web应用的casAuthenticationFilter,会截获此请求,生成s…

  • 用递归函数求斐波那契数列_利用递归求斐波那契数列

    用递归函数求斐波那契数列_利用递归求斐波那契数列函数递归求斐波那契数列//函数递归求斐波那契数列//编写程序,求数列1,1,2,3,5,8,13,21,……//思路://第一步:找出表示数列第N项的递归公式:F(N)=F(N-1)+F(N-2)//第二步:递归的结束条件,当N=1或N=2时,F(N)=1;longintFib(intn){ if(n<=2) return1; else returnFib(n-1)+Fib(n-2); //拿n=3带入一下,第一个返回值为1第二个返回值1

  • Matlab中sqrt函数的用法「建议收藏」

    Matlab中sqrt函数的用法「建议收藏」目录语法说明示例向量元素的平方根输入参数详细信息IEEE合规性提示sqrt求解平方根语法B=sqrt(X)说明B=sqrt(X)返回数组X的每个元素的平方根。对于X的负元素或复数元素,sqrt(X)生成复数结果。sqrt函数的域包含负数和复数,如果使用不当,可能会导致意外结果。对于负的复数z=u+i*w,复数方根sqrt(z)返回sqrt(r)*(cos(phi/2)+1i*sin(phi/2))…

  • 获取本机IP地址(自动获取不了ip地址怎么解决)

    获取本机IP地址的方法总结1.命令行获取第一步:在任意位置同时按下windows键+r键,第二步:回车键或者点击确定第三步:输入ipconfig然后回车箭头所指的位置就是你本机的IP咯~2.使用网络状态查看IP地址第一步:进入“网络和共享中心”(在控制面板可以进入,右单击左下角那个网络图标也可以进入),然后再点击已链接的网络,如图第二步:完成第一步后,…

  • Maven私库安装与配置

    Maven私库安装与配置前言项目开发时,一般采用Maven来进行项目构建,管理,jar包下载,项目组开发人员开发时,只要在项目的pom.xml文件中,添加如下pom.xml信息从公共的库中下载相关的Jar包以及依赖Jar包,但有时候公共的库不是那么稳定而且下载速度往往很慢,一个团队一起开发项目时,如果每个组员都去下载一遍所有依赖的Jar包,其实是一件很麻烦的事情,所以这时候就有必要搭建一个Maven的私库了,这样

  • igmp协议分析_协议分析

    igmp协议分析_协议分析IGMP协议。通过抓包详细分析了IGMP成员报告报文和IGMP离组报文。类型1字节最大响应时间1字节检验和2字节多播地址4字节类型11成员查询12成员报告(v1)16成员报告(v2)22成员报告(v3)17成员退出IGMP协议分析…

发表回复

您的电子邮箱地址不会被公开。

关注全栈程序员社区公众号