动态窗口算法（DWA）

2023-12-16 06:09| 来源: 网络整理| 查看: 265

路径规划算法中动态窗口算法十分灵活好用，但发现在自动驾驶中几乎没有应用，原因大概就是其难于复杂的环境，日后将对其进行详细的调研，今天先记录一下其原理及过程。一、DWA原理仔细想想，机器人的运动状态，包括其不停变换的位置及运动方向，实际上是由其当前的运动速度及角速度（转向速度）决定的。那么，动态窗口法核心的动态窗口，其实就是将机器人当前状态（即当前速度，航向角）及机器人运动模型（机器人所能达到的最大速度，最大角速度，加速度，旋转加速度）计算出当前机器人的最大最小速度及角速度，以此作为一个限定范围，这个范围就是窗口。在此范围中计算每个速度及角速度下所能到达到的位置，在对每个位置进行测评（测评内容包括据障碍物的距离，朝向终点的角度等），由此选出当前的最佳位置，然后再由这个最佳位置继续重复以上过程建立新的窗口，这样窗口就动起来了，即所谓的动态窗口。二、实现过程之后将我找到的动态窗口法的一个用matlab实现的demo贴出来，可以跟据这个小历程来更清晰明了的了解一下动态窗口法，其中很多地方做了解释，然后再将具体流程写于下面。

% ------------------------------------------------------------------------- % % File : DynamicWindowApproachSample.m % % Discription : Mobile Robot Motion Planning with Dynamic Window Approach % % Environment : Matlab % % Author : Atsushi Sakai % % Copyright (c): 2014 Atsushi Sakai % % License : Modified BSD Software License Agreement % log: 20181031 增加详细的注释信息下标的定义 % 20181101 ：增加画出障碍物的大小，更直观的看到障碍物和机器人之间的位置关系 % ------------------------------------------------------------------------- function [] = DynamicWindowApproachSample() close all; clear all; disp('Dynamic Window Approach sample program start!!') %显示数组 %% 机器人的初期状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)] % x=[0 0 pi/2 0 0]'; % 5x1矩阵列矩阵位置 0，0 航向 pi/2 ,速度、角速度均为0 x = [0 0 pi/10 0 0]'; % 下标宏定义状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)] POSE_X = 1; %坐标 X POSE_Y = 2; %坐标 Y YAW_ANGLE = 3; %机器人航向角 V_SPD = 4; %机器人速度 W_ANGLE_SPD = 5; %机器人角速度 goal = [10,10]; % 目标点位置 [x(m),y(m)] % 障碍物位置列表 [x(m) y(m)] obstacle=[0 2; 2 4; 2 5; 4 2; % 4 4; 5 4; % 5 5; 5 6; 5 9 8 8 8 9 7 9]; % obstacle=[0 2; % 4 2; % 4 4; % 5 4; % 5 5; % 5 6; % 5 9 % 8 8 % 8 9 % 7 9 % 6 5 % 6 3 % 6 8 % 6 7 % 7 4 % 9 8 % 9 11 % 9 6]; obstacleR = 0.5;% 冲突判定用的障碍物半径 global dt; dt = 0.1;% 时间[s] % 机器人运动学模型参数 % 最高速度[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)];%toRadian角度转弧度，radian = degree/180*pi; %定义Kinematic的下标含义 MD_MAX_V = 1; % 最高速度[m/s] MD_MAX_W = 2; % 最高旋转速度[rad/s] MD_ACC = 3; % 加速度[m/ss] MD_VW = 4; % 旋转加速度[rad/ss] MD_V_RESOLUTION = 5; % 速度分辨率[m/s] MD_W_RESOLUTION = 6; % 转速分辨率[rad/s]] % 评价函数参数 [heading,dist,velocity,predictDT] % 航向得分的比重、距离得分的比重、速度得分的比重、向前模拟轨迹的时间 evalParam = [0.05, 0.2 ,0.1, 3.0]; area = [-1 11 -1 11];% 模拟区域范围 [xmin xmax ymin ymax] % 模拟实验的结果 result.x=[]; %定义一个数组，累积存储走过的轨迹点的状态值 tic; % 估算程序运行时间开始 % movcount=0; %% Main loop 循环运行 5000次指导达到目的地或者 5000次运行结束 for i = 1:5000 %最多5000个循环到达目标点，每个循环作为一个计算阶段，得到一小段路 % 返回控制量 u = [v(m/s),w(rad/s)] 和 traj 该阶段的一小段轨迹 [u,traj] = DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR); x = f(x,u);% 机器人移动到下一个时刻的状态量根据当前速度和角速度推导下一刻的位置和角度 % 历史轨迹的保存 result.x = [result.x; x']; %最新结果以列的形式添加到result.x%？？？？？x中只有列？？？x开始到底是行还是列？？？？？ % 是否到达目的地 if norm(x(POSE_X:POSE_Y)-goal') stopDist % 如果可能撞到最近的障碍物则舍弃此路径（到最近障碍物的距离大于刹车距离才取用） evalDB = [evalDB;[vt ot heading dist vel]]; trajDB = [trajDB;traj]; % 每5行一条轨迹 end end end %% 归一化处理 % 每一条轨迹的单项得分除以本项所有分数和 function EvalDB=NormalizeEval(EvalDB) % 评价函数正则化 if sum(EvalDB(:,3))~= 0 EvalDB(:,3) = EvalDB(:,3)/sum(EvalDB(:,3)); %矩阵的数除单列矩阵的每元素分别除以本列所有数据的和 end if sum(EvalDB(:,4))~= 0 EvalDB(:,4) = EvalDB(:,4)/sum(EvalDB(:,4)); end if sum(EvalDB(:,5))~= 0 EvalDB(:,5) = EvalDB(:,5)/sum(EvalDB(:,5)); end %% 单条轨迹生成、轨迹推演函数 % 输入参数：当前状态、vt当前速度、ot角速度、evaldt 前向模拟时间、机器人模型参数（没用到） % 返回参数; % x : 机器人模拟时间内预测的终点状态; % traj: 当前时刻到预测时刻之间过程中的位姿记录（状态记录）当前模拟的轨迹 % 轨迹点的个数为 evaldt / dt + 1 = 3.0 / 0.1 + 1 = 31 % function [x,traj] = GenerateTrajectory(x,vt,ot,evaldt,model) global dt; time = 0; u = [vt;ot];% 输入值 traj = x; % 机器人轨迹 while time 0 % 给定加速度的条件下速度减到0所走的距离 stopDist = stopDist + vel*dt; % 制动距离的计算 vel = vel - model(MD_ACC)*dt; % end %% 障碍物距离评价函数（机器人在当前轨迹上与最近的障碍物之间的距离，如果没有障碍物则设定一个常数） % 输入参数：位姿、所有障碍物位置、障碍物半径 % 输出参数：当前预测的轨迹终点的位姿距离所有障碍物中最近的障碍物的距离如果大于设定的最大值则等于最大值 % 距离障碍物距离越近分数越低 function dist = CalcDistEval(x,ob,R) dist=100; for io = 1:length(ob(:,1)) disttmp = norm(ob(io,:)-x(1:2)')-R; % 距离 = 到第io个障碍物的距离 - 障碍物半径！！！有可能出现负值吗？？ if dist > disttmp % 大于最小值则选择最小值 dist = disttmp; end end % 障碍物距离评价限定一个最大值，如果不设定，一旦一条轨迹没有障碍物，将太占比重 if dist >= 2*R %最大分数限制 dist = 2*R; end %% heading的评价函数计算 % 输入参数：当前位置、目标位置 % 输出参数：航向参数得分当前车的航向和相对于目标点的航向偏离程度越小分数越高最大180分 function heading = CalcHeadingEval(x,goal) theta = toDegree(x(3)); % 机器人朝向 goalTheta = toDegree(atan2(goal(2)-x(2),goal(1)-x(1))); % 目标点相对于机器人本身的方位 if goalTheta > theta targetTheta = goalTheta-theta;% [deg] else targetTheta = theta-goalTheta;% [deg] end heading = 180 - targetTheta; %% 计算动态窗口 % 返回最小速度最大速度最小角速度最大角速度速度 function Vr = CalcDynamicWindow(x,model) V_SPD = 4;%机器人速度 W_ANGLE_SPD = 5;%机器人角速度 MD_MAX_V = 1;% MD_MAX_W = 2;% MD_ACC = 3;% MD_VW = 4;% global dt; % 机器人速度的最大最小范围依次为：最小速度最大速度最小角速度最大角速度速度 Vs=[0 model(MD_MAX_V) -model(MD_MAX_W) model(MD_MAX_W)]; % 根据当前速度以及加速度限制计算的动态窗口依次为：最小速度最大速度最小角速度，最大角速度速度，此处应为速度=model(MD_ACC)/dt Vd = [x(V_SPD)-model(MD_ACC)*dt x(V_SPD)+model(MD_ACC)*dt x(W_ANGLE_SPD)-model(MD_VW)*dt x(W_ANGLE_SPD)+model(MD_VW)*dt]; % 最终的Dynamic Window Vtmp = [Vs;Vd]; %2 X 4 每一列依次为：最小速度最大速度最小角速度最大角速度速度 Vr = [max(Vtmp(:,1)) min(Vtmp(:,2)) max(Vtmp(:,3)) min(Vtmp(:,4))]; %% Motion Model 根据当前状态推算下一个控制周期（dt）的状态 % 输入u = [vt; wt];当前时刻的速度、角速度 x = 状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)] %输出：下一位置点，x=[[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)] function x = f(x, u) global dt; F = [1 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0]; B = [dt*cos(x(3)) 0 dt*sin(x(3)) 0 0 dt 1 0 0 1]; x= F*x+B*u; %根据航向角计算下一点的坐标， %% degree to radian function radian = toRadian(degree) radian = degree/180*pi; %% radian to degree function degree = toDegree(radian) degree = radian/pi*180; %% END

三、DWA具体过程（程序直接在Main loop主循环处开始） 1、输入参数：x,model,goal,evalParam,ob,R： function [u,trajDB] =DynamicWindowApproach(x,model,goal,evalParam,ob,R) 六个参数分别对应

输入参数对应函数xx=[0 0 pi/2 0 0]’%[机器人初始状态。x轴坐标，y轴坐标,航向角 ,速度、角速度]modelKinematic = [1.0,toRadian(20.0),0.2,toRadian(50.0),0.01,toRadian(1)];%机器人运动学模型=[最高速度[m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss],速度分辨率[m/s],转速分辨率[rad/s]]goal目标位置坐标evalparamevalParam = [0.05, 0.2 ,0.1, 3.0]%[航向得分的比重、距离得分的比重、速度得分的比重、向前模拟轨迹的时间]ob所有障碍物坐标R障碍物半径，障碍物在图中显示为圆形

2、输出参数 u：机器人当前阶段窗口结束时的最佳速度与角速度，作为下一阶段窗口选择的初始速度与角速度。 trajDB：机器人该阶段的路点坐标集。 3、过程：

过程1.计算动态窗口:Vr = CalcDynamicWindow(x,model)1.1提取动态模型状态范围：速度、角速度范围Vs=[0 model(MD_MAX_V) -model(MD_MAX_W) model(MD_MAX_W)]%[最小速度最大速度最小角速度最大角速度]1.2根据当前状态计算速度角速度的限定范围Vd = [x(V_SPD)-model(MD_ACC)*dt x(V_SPD)+model(MD_ACC)*dt x(W_ANGLE_SPD)-model(MD_VW)*dt x(W_ANGLE_SPD)+model(MD_VW)*dt]%最小速度(Vmin=Vinit-a/▲t) 最大速度最小角速度(Wmin=Winit-W/▲t)，最大角速度速度1.3选取当前动态窗口Vr = [max(Vtmp(:,1)) min(Vtmp(:,2)) max(Vtmp(:,3)) min(Vtmp(:,4))]%[max(Vmin),min(Vmax),max(Wmin),min(wmax)]2. 计算生成轨迹，并就该段轨迹中的最后一个位姿进行综合测评[evalDB,trajDB]= Evaluation(x,Vr,goal,ob,R,model,evalParam)2.1生成轨迹[xt,traj] = GenerateTrajectory(x,vt,ot,evalParam(4),model);%生成由3个（time模拟时间）路点组成的一小段轨迹。每个点具体由三角函数计算得出。2.2综合评估评估该小段轨迹的最后一点的位姿（1）heading = CalcHeadingEval(xt,goal); % 预测与终点的航向得分，偏差越小分数越高。（2）dist = CalcDistEval(xt,ob,R); % 预测该点距离最近障碍物的距离得分，距离越远分数越高。（3）vel = abs(vt);%速度得分，速度越快分越高。（4）stopDist = CalcBreakingDist(vel,model); % 返回制动距离3.将范围内所有的轨迹进行评估，归一化处理evalDB = NormalizeEval(evalDB)4.选取分数最高的路径5. 继续循环

4、剩下的为matlab的作图过程，很有意思的小箭头的生成，之后在进行细化。

【本文地址】

动态窗口算法（DWA）

动态窗口算法（DWA）

今日新闻

推荐新闻