Dynamic Window Approach_机器人局部避障的动态窗口法

2024-02-10 03:58

本文主要是介绍Dynamic Window Approach_机器人局部避障的动态窗口法,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

  • 最近正在学习DWA,按照DynamicWindowApproachSample.m 提供的matlab仿真代码简单梳理下,但是这并不是ros里用的,只是一个简单的仿真。
  • DWA的原理:假设一个机器人要从当前点通过一定的速度和方向到达目标点,在速度(v, w)空间中采样多组轨迹,为这些轨迹使用评价函数进行评价,选取最优的轨迹对应的速度(v_best, w_best)来驱动机器人,最终到达目标点。
代码分析:
  1. 首先定义一些初始状态

    % 机器人的初始状态,包括初始位置,朝向(这里可以理解为yaw角)速度,角速度
    % [x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
    x=[0 0 pi/2 0 0]';% 目标点位置 [x(m),y(m)]
    goal=[10,10];% 障碍物位置列表 [x(m) y(m)]
    obstacle=[0 2;4 2;4 4;5 4;5 5;5 6; 6 7;7 4;9 8;9 11;9 6];% 冲突判定用的障碍物半径
    obstacleR=0.5;% 时间间隔[s],主要用于运动模型
    global dt; dt=0.1;%% 机器人运动学模型参数
    % 最高速度[m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss],
    % 速度分辨率[m/s],转速分辨率[rad/s]]
    Kinematic=[1.0,toRadian(20.0),0.2,toRadian(50.0),0.01,toRadian(1)];% 评价函数参数 [heading,dist,velocity,predictDT]
    evalParam=[0.05,0.2,0.1,3.0];% 模拟区域范围 [xmin xmax ymin ymax]
    area=[-1 11 -1 11];% 模拟实验的结果
    result.x=[];
  2. 进入仿真主循环
    循环主要函数为DynamicWindowApproach,输入机器人当前的状态x,运动模型参数,评价函数参数,障碍物,障碍-物半径,输出最优的控制量u(v, w)以及模拟轨迹,控制量传递给运动模型后机器人开始运动,最终到达目标点,后面绘图部分不重要,不予解释了。

    for i=1:5000% DWA参数输入[u,traj]=DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR);% 机器人运动模型x=f(x,u);% 模拟结果的保存result.x=[result.x; x'];% 是否到达目的地if norm(x(1:2)-goal')<0.5disp('Arrive Goal!!');break;end%% ====绘图部分===hold off;ArrowLength=0.5;%% 机器人quiver(x(1),x(2),ArrowLength*cos(x(3)),ArrowLength*sin(x(3)),'ok');hold on;plot(result.x(:,1),result.x(:,2),'-b');hold on;plot(goal(1),goal(2),'*r');hold on;plot(obstacle(:,1),obstacle(:,2),'*k');hold on;% 探索轨迹if ~isempty(traj)for it=1:length(traj(:,1))/5ind=1+(it-1)*5;plot(traj(ind,:),traj(ind+1,:),'-g');hold on;endendaxis(area);grid on;drawnow; 
    end

    运动模型 x=f(x,u)指的是在世界坐标系下,机器人从a点运动到b点遵循的运动规则,w指的是世界坐标系
    世界坐标系下
    机器人从a点到b点的运动模型
    用矩阵的形式表示这个模型如下:

function x = f(x, u)
% u = [vt; wt];当前时刻的速度、角速度
global dt;F = [1 0 0 0 00 1 0 0 00 0 1 0 00 0 0 0 00 0 0 0 0];B = [dt*cos(x(3)) 0dt*sin(x(3)) 00 dt1 00 1];x= F*x+B*u;
  1. DynamicWindowApproach函数,这个是DWA函数的模型,输出最优的线速度和角速度
    首先会根据机器人模型参数中的最高速度,最高旋转速度,加速度,旋转加速度等参数计算出机器人在dt时间内速度和角速度可以到达的最大最小范围,这个范围可以根据情况调整;
    之后将这个窗口范围内的速度(包含角速度)进行采样,输入到评价函数Evaluation中,计算出每个采样速度对应的评价值evalDB,在对这些评价值进行归一化操作,分别乘以评价值对应的评价参数(cost = a*heading+b*dist+c*velocity),得到最优的评价值后找到其对应的u(v, w),输出这个最佳的速度和角速度

    function [u,trajDB]=DynamicWindowApproach(x,model,goal,evalParam,ob,R)% 动态窗口 [vmin,vmax,wmin,wmax]
    Vr=CalcDynamicWindow(x,model);% 评价函数的计算
    [evalDB,trajDB]=Evaluation(x,Vr,goal,ob,R,model,evalParam);if isempty(evalDB)disp('no path to goal!!');u=[0;0];return;
    end% 各评价函数正则化
    evalDB=NormalizeEval(evalDB);% 最终评价函数的计算
    feval=[];
    for id=1:length(evalDB(:,1))feval=[feval;evalParam(1:3)*evalDB(id,3:5)'];
    end
    evalDB=[evalDB feval];
    % 最优评价函数
    [maxv,ind]=max(feval);
    % 最优的速度和角速度
    u=evalDB(ind,1:2)';
  2. CalcDynamicWindow

    function Vr=CalcDynamicWindow(x,model)
    %
    global dt;
    % 车子速度的最大最小范围
    Vs=[0 model(1) -model(2) model(2)];
    % 根据当前速度以及加速度限制计算的动态窗口
    Vd=[x(4)-model(3)*dt x(4)+model(3)*dt x(5)-model(4)*dt x(5)+model(4)*dt];
    % 最终的Dynamic Window
    Vtmp=[Vs;Vd];
    Vr=[max(Vtmp(:,1)) min(Vtmp(:,2)) max(Vtmp(:,3)) min(Vtmp(:,4))];
  3. Evaluation函数,评价函数的计算,输入初始x的状态,动态窗口,障碍物,冲突判定用的障碍物半径,机器人运动模型参数以及评价函数参数,通过两个for循环计算出需要评价的参数heading dist vel

    function [evalDB,trajDB]=Evaluation(x,Vr,goal,ob,R,model,evalParam)
    % 评价函数值
    evalDB=[];
    % 预测轨迹
    trajDB=[];
    for vt=Vr(1):model(5):Vr(2)for ot=Vr(3):model(6):Vr(4)% GenerateTrajectory轨迹推测% 得到 xt: 机器人向前运动后的预测位姿; traj: 当前时刻到预测时刻之间的轨迹% evalParam(4),前向模拟时间;[xt,traj]=GenerateTrajectory(x,vt,ot,evalParam(4),model);%% 各评价函数的计算% 当前机器人和目标点的朝向heading=CalcHeadingEval(xt,goal);% 当前机器人和障碍物的距离dist=CalcDistEval(xt,ob,R);vel=abs(vt);% 制动距离的计算stopDist=CalcBreakingDist(vel,model);if dist>stopDist %evalDB=[evalDB;[vt ot heading dist vel]];trajDB=[trajDB;traj];endend
    end
  4. GenerateTrajectory函数

    function [x,traj]=GenerateTrajectory(x,vt,ot,evaldt,model)
    % 轨迹生成函数
    % evaldt:前向模拟时间; vt、ot当前速度和角速度;
    global dt;
    time=0;
    % 输入值
    u=[vt;ot];
    % 机器人轨迹
    traj=x;
    while time<=evaldt
    % 时间更新time=time+dt;
    % 运动更新x=f(x,u);traj=[traj x];
    end
  5. CalcHeadingEval函数,heading的评价函数计算

    function heading=CalcHeadingEval(x,goal)% 机器人朝向
    theta = toDegree(x(3));
    % 目标点的方位
    goalTheta = toDegree(atan2(goal(2)-x(2),goal(1)-x(1)));if goalTheta > thetatargetTheta = goalTheta - theta;% [deg]
    elsetargetTheta = theta - goalTheta;% [deg]
    end% theta越小,评价越高
    heading = 180 - targetTheta;
  6. CalcDistEval函数,障碍物距离评价函数

    function dist=CalcDistEval(x,ob,R)dist=100;for io=1:length(ob(:,1))disttmp=norm(ob(io,:)-x(1:2)')-R;% 离障碍物最小的距离if dist>disttmpdist=disttmp;endend% 障碍物距离评价限定一个最大值,如果不设定,一旦一条轨迹没有障碍物,将太占比重if dist>=2*Rdist=2*R;end

这篇关于Dynamic Window Approach_机器人局部避障的动态窗口法的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/696059

相关文章

Windows的CMD窗口如何查看并杀死nginx进程

《Windows的CMD窗口如何查看并杀死nginx进程》:本文主要介绍Windows的CMD窗口如何查看并杀死nginx进程问题,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地... 目录Windows的CMD窗口查看并杀死nginx进程开启nginx查看nginx进程停止nginx服务

Java调用C#动态库的三种方法详解

《Java调用C#动态库的三种方法详解》在这个多语言编程的时代,Java和C#就像两位才华横溢的舞者,各自在不同的舞台上展现着独特的魅力,然而,当它们携手合作时,又会碰撞出怎样绚丽的火花呢?今天,我们... 目录方法1:C++/CLI搭建桥梁——Java ↔ C# 的“翻译官”步骤1:创建C#类库(.NET

MyBatis编写嵌套子查询的动态SQL实践详解

《MyBatis编写嵌套子查询的动态SQL实践详解》在Java生态中,MyBatis作为一款优秀的ORM框架,广泛应用于数据库操作,本文将深入探讨如何在MyBatis中编写嵌套子查询的动态SQL,并结... 目录一、Myhttp://www.chinasem.cnBATis动态SQL的核心优势1. 灵活性与可

Mybatis嵌套子查询动态SQL编写实践

《Mybatis嵌套子查询动态SQL编写实践》:本文主要介绍Mybatis嵌套子查询动态SQL编写方式,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录前言一、实体类1、主类2、子类二、Mapper三、XML四、详解总结前言MyBATis的xml文件编写动态SQL

SpringBoot实现Kafka动态反序列化的完整代码

《SpringBoot实现Kafka动态反序列化的完整代码》在分布式系统中,Kafka作为高吞吐量的消息队列,常常需要处理来自不同主题(Topic)的异构数据,不同的业务场景可能要求对同一消费者组内的... 目录引言一、问题背景1.1 动态反序列化的需求1.2 常见问题二、动态反序列化的核心方案2.1 ht

golang实现动态路由的项目实践

《golang实现动态路由的项目实践》本文主要介绍了golang实现动态路由项目实践,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的朋友们下面随着小编来一起学习学习... 目录一、动态路由1.结构体(数据库的定义)2.预加载preload3.添加关联的方法一、动态路由1

使用WPF实现窗口抖动动画效果

《使用WPF实现窗口抖动动画效果》在用户界面设计中,适当的动画反馈可以提升用户体验,尤其是在错误提示、操作失败等场景下,窗口抖动作为一种常见且直观的视觉反馈方式,常用于提醒用户注意当前状态,本文将详细... 目录前言实现思路概述核心代码实现1、 获取目标窗口2、初始化基础位置值3、创建抖动动画4、动画完成后

Python Selenium动态渲染页面和抓取的使用指南

《PythonSelenium动态渲染页面和抓取的使用指南》在Web数据采集领域,动态渲染页面已成为现代网站的主流形式,本文将从技术原理,环境配置,核心功能系统讲解Selenium在Python动态... 目录一、Selenium技术架构解析二、环境搭建与基础配置1. 组件安装2. 驱动配置3. 基础操作模

慢sql提前分析预警和动态sql替换-Mybatis-SQL

《慢sql提前分析预警和动态sql替换-Mybatis-SQL》为防止慢SQL问题而开发的MyBatis组件,该组件能够在开发、测试阶段自动分析SQL语句,并在出现慢SQL问题时通过Ducc配置实现动... 目录背景解决思路开源方案调研设计方案详细设计使用方法1、引入依赖jar包2、配置组件XML3、核心配

Python中pywin32 常用窗口操作的实现

《Python中pywin32常用窗口操作的实现》本文主要介绍了Python中pywin32常用窗口操作的实现,pywin32主要的作用是供Python开发者快速调用WindowsAPI的一个... 目录获取窗口句柄获取最前端窗口句柄获取指定坐标处的窗口根据窗口的完整标题匹配获取句柄根据窗口的类别匹配获取句