动态窗口法

运动模型建立

假设机器人的平移速度和旋转角速度可以独立地进行控制。如下图使用角速度\(\omega(t)\)、速度\(v(t)\)表示在\(t\)时刻的运动状态。

除此之外，还需要引入坐标\(x(t)\)、\(y(t)\)、\(\theta(t)\)分别表示\(t\)时刻的坐标(\(x,y\))以及机器人前进方向与水平方向的夹角。可以使用一组状态变量表示

\[X(t)= \left[ \begin{matrix} x(t) & y(t) & v(t) & \theta(t) & \omega(t) \end{matrix} \right]^T \]

另外，还需要设置系统输入，由于动态窗口法中主要使用运动模型预测轨迹，所以设置输入为

\[U(n)= \left[ \begin{matrix} v_p(n) & \omega_p(n) \end{matrix} \right]^T \]

假设机器人的速度、加速度、角速度、角加速度均有最大值，使用下面一组变量约束

\[\left[ \begin{matrix} v_{max} & \omega_{max} & a_{max} & \alpha_{max} & ratio_v & ratio_\omega \end{matrix} \right]^T \]

其中\(ratio_v,ratio_\omega\)分别表示速度分辨率和角速度分辨率。

假设初始时刻\(X(t_{n})\)已知，令\(dt=t_{n+1}-t_{n}\)即采样时间，输入为\(U(n)\)则

\[\begin{split} x(n)&=x(t_{n})+0\times y(t_{n})+0\times v(t_{n})+0\times \theta(t_{n})+ 0\times \omega(t_{n})+dt\times cos(\theta(t_{n}))\times v_p(n) + 0\times \omega_p(n) \\ y(n)&=0\times x(t_{n})+y(t_{n})+0\times v(t_{n})+0\times \theta(t_{n}) + 0\times \omega(t_{n})+dt\times sin(\theta(t_{n}))\times v_p(n) + 0\times \omega_p(n) \\ \theta(n)&=0\times x(t_{n})+0\times y(t_{n})+0\times v(n)+1\times \theta(t) + 0\times \omega(t)+0\times v_p(n)+dt\times \omega_p(t) \\ v(n)&=0\times x(t_{n})+0\times y(t_{n})+0\times v(n)+0\times \theta(t) + 0\times \omega(t)+1\times v_p(n)+0\times \omega_p(t) \\ \omega(n)&=0\times x(t_{n})+0\times y(t_{n})+0\times v(n)+0\times \theta(t) + 0\times \omega(t)+0\times v_p(n)+1\times \omega_p(t) \\ \end{split} \]

简化为

\[\begin{split} X(n)=F*X(t)+B*U(n) \end{split} \]

其中

\[\begin{split} F= \left[ \begin{matrix} 1 & 0 & 0 & 0 & 0\\ 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0\\ \end{matrix} \right], B= \left[ \begin{matrix} dt\times cos(\theta(t_{n})) & 0 \\ dt\times sin(\theta(t_{n})) & 0\\ 1 & 0 \\ 0 & dt\\ 0 & 1 \\ \end{matrix} \right] \end{split} \]

动态窗口设置

对于全向移动的机器人，其动态窗口应为下一采样时间\(dt\)内所有可能的\(v(t),\omega(t)\)构成的运动轨迹。采用\(v_{max},\omega_{max}\)约束机器人状态，即\(v(t)\in [0,v_{max}],\omega(t)\in [-\omega_{max},\omega_{max}]\)。

假设\(t_{n}\)时刻\(X(t_{n})\)已知,则\(t_{n+1}\)时刻

\[\begin{split} v(t_{n+1})&\in [v(t_{n})-dt*a_{max},v(t_{n})+dt*a_{max}]\\ \omega(t_{n+1})&\in [\omega(t_{n})-dt*\alpha_{max},\omega(t_{n})+dt*\alpha_{max}] \end{split} \]

再由\(v(t)\in [0,v_{max}],\omega(t)\in [-\omega_{max},\omega_{max}]\)进行约束，即可得到\(t_{n+1}\)时刻的动态窗口。由于任意两个速度之间有无数种取值，所以引入\(ratio_v,ratio_\omega\)进行有限次数的采样，不同\(ratio_v,ratio_\omega\)下采样效果如下图

圆圈为机器人位置，绿色线条为预测轨迹；ratiov影响前进方向上预测轨迹的密集程度，ratioω影响以机器人为中心扇形区域的预测轨迹的密集程度。

如果只对未来dt内进行轨迹预测，由于预测范围过小可能无法完成避障要求。

(a)中由于预测轨迹范围过小无法成功避障，对比(b)(c)两图，较大的预测范围可以在避障时有更安全的距离。

动态窗口评价函数

采样的速度组中，有若干组采样轨迹是可行的，因此需要采用评价函数的方式对每条轨迹的预测终点进行评估。在传统动态窗口法中，评价函数只考虑三个指标，分别是方位角评价函数、空隙评价函数以及速度评价函数。

\[G(\nu,\omega)=\alpha\cdot normal(heading(\nu,\omega))+\beta\cdot normal(dist(\nu,\omega))+\gamma\cdot normal(vel(\nu,\omega)) \]

方位角评价

方位是衡量机器人当前方向与目标方向对准的程度，\(heading\)函数输出的最终结果为\(180^\circ-\theta\)。

空隙评价

空隙评价可以理解为机器人距离最近障碍物的距离。需要注意的是，如果距离障碍物距离非常远或周围没有障碍物时，应设置一个较大常数约束空隙评价。

速度评价

对机器人移动速度绝对值的评价。

最终评价

分别对上述三个评价进行归一化，使用\(\alpha,\beta,\gamma\)进行加权计算得到最终评价。从所有预测轨迹中找到最终评价最大值对应的\(v_p,\omega_p\)最为下一时刻输入。为了便于处理信息，设置一组向量存储评价函数加权信息及预测时间

\[Eval= \left[ \begin{matrix} \alpha & \beta & \gamma & t_p \end{matrix} \right] \]

不足与改进

终点靠近障碍物

在临近终点时，由于终点距离障碍过近导致空隙评价得分较低，进而导致选取的预测轨迹已经越过终点，使得预测轨迹无法准确的聚焦在终点上，最后由于评价函数限制使得机器人在障碍物和终点附近运动，无法收敛到终点。

考虑到现有DWA评价函数指标只包括方位角，空隙，速度三个指标，导致轨迹无法保证全局最优性。在原有评价函数引入\(A^*\)算法的启发式函数，采用欧式距离计算。

\[\begin{aligned} G(\nu,\omega)&=\alpha\cdot normal(heading)+\beta\cdot normal(dist)+\gamma\cdot normal(vel) + \zeta \cdot normal(astar) \\astar&=g(n)+h(n) \end{aligned} \]

原\(Eval\)修改为

\[Eval= \left[ \begin{matrix} \alpha & \beta & \gamma & \zeta & t_p \end{matrix} \right] \]

具体效果如下图

应用代码思路解析

该文档只对\(MATLAB\)代码进行解释，不对DWA原理进行详细阐述，具体原理请详细查看另一文件。

代码框架

│  Dwa_main.m
│  README.en.md
├─DwaFunction
└─InitializeData

\(main.m\)运行\(DWA\)主要过程。
\(README.en.md\)为中文讲解文档。
\(DwaFunction\)存储\(DWA\)用到的函数。
\(InitializeData\)存储栅格地图、运动障碍等信息。

初始化过程

%% 初始化参数设置
% 设置仿真步长
global dt;
dt = 0.1;
% 设置最长仿真时间200s
% 超出仿真时间路径依旧不收敛记为规划失败
End_Time = 200;
% 循环次数
cycles = End_Time/dt;
% 起点下标[y,x]
startpossub = [20,36];
% 起点索引
startposind = sub2ind([42,42],startpossub(1),startpossub(2));
% 目标点下标[y,x]
goal = [15,5];
% 目标点索引
goalposind = sub2ind([42,42],goal(1),goal(2));
% 初始化参数
[field,obstacle,RM_parameters,Lidar] = DwaInitialization(field,startpossub);

仿真步长\(dt\)设置为全局变量，仿真时间(\(End_Time\))设置为\(200s\)超出时间未找到终点视为规划失败，\(cycles\)为仿真的循环次数。

起始点可自行设置，\(startposind,goalposind\)为一维索引，这里对应\(42\times42\)主要是因为后续对\(40\times40\)地图进行边缘扩充。

DwaInitialization

function [field,obstacle,RM_parameters,Lidar,IMM] = DwaInitialization(field,startpossub)
global dt;
% 填充边缘
field = padarray(field, [1 1], 1);
% 获取障碍二维坐标
obstacle = GetAllobstacle(field);
%% 机器人模型参数
% 状态变量[x,y,v,theta,w]
RM_parameters.x = [startpossub(1);startpossub(2);0;0;toRadian(0)];
% 机器人运动学限制:最高速度[m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss],速度分辨率[m/s],转速分辨率[rad/s]
% RM_parameters.KinematicLimit = [2.3,toRadian(100.0),1.0,toRadian(80.0),0.1,toRadian(5)];
RM_parameters.KinematicLimit = [1.5,toRadian(100.0),1.0,toRadian(80.0),0.1,toRadian(5)];
% 动态窗口
RM_parameters.DynamicWin = [];
% 评价函数参数 [heading,dist,velocity,predictDT]
RM_parameters.evalParam=[0.08,0.3,0.12,0.1,20*dt];
% 记录一次评价参数
RM_parameters.evalDB = [];
% 记录一次动态窗口轨迹
RM_parameters.trajDB = [];
% 机器人移动状态汇总
RM_parameters.savex = [];
%% Lidar参数
% 雷达半径
Lidar.r = 10;
% 雷达全局坐标
Lidar.x = RM_parameters.x(1);
Lidar.y = RM_parameters.x(2);
% 雷达数据(极坐标格式)
Lidar.Data = [];
% 局部障碍位置
Lidar.PartObstacle = [];
% 最小距离索引
Lidar.indmin = 0;
% 检测动态点标志
Lidar.dynamic = 0;
end

边缘填充目的是对原地图增加边框限制，防止跑出栅格地图外。

padding

获取栅格地图内所有障碍的二维索引保存至\(obstacle\)，用于局部窗口的筛选和雷达的仿真。

预处理

%% 预处理
% 速度色阶
colorv = cmap(1+ceil(255*RM_parameters.x(3)/RM_parameters.KinematicLimit(1)),:);
Fig=figure;
filename = 'dynamic2.gif';      % 输出路径+保存的文件名.gif
% 移动障碍索引
j = 1;

\(RM_parameters.x(3)\)为当前线速度。
\(RM_parameters.KinematicLimit(1)\)为动力学最大线速度。
\(ceil(255*RM_parameters.x(3)/RM_parameters.KinematicLimit(1))+1\)相当于将当前速度映射到\(0-255\)的色阶上。

\(colorv\)用于存储每次仿真速度对应的\(RGB\)信息。

\(j\)为移动障碍位置信信息的索引。

Lidar模拟

DWA过程

动态窗口计算

%{
动态窗口计算
输入：当前机器人状态，机器人动力学限制
输出：动态窗口[vmin,vmax,wmin,wmax](下一个步长的所有可能速度和角速度范围)
%}
function DynamicWin=CalcDynamicWindow(x,KinematicLimit)  
% 仿真步长
global dt;  
% 参数重命名
v = x(3);
w = x(5);
vmax = KinematicLimit(1);
wmax = KinematicLimit(2);
a = KinematicLimit(3);
wa = KinematicLimit(4);
%% 计算动态窗口
% 机器人速度和角速度限制
Vs=[0 vmax -wmax wmax];  
% 根据当前速度以及加速度限制计算的动态窗口  
Vd=[v - a*dt v+a*dt w-wa*dt w+wa*dt];  
% 最终的Dynamic Window  
Vtmp=[Vs;Vd];  
DynamicWin = [max(Vtmp(:,1)) min(Vtmp(:,2)) max(Vtmp(:,3)) min(Vtmp(:,4))];  
end

Lidar局部区域检索

%{
仿真Lidar获取周围无遮挡障碍信息
输入：所有障碍坐标，雷达参数，动态障碍信息
输出：Lidar参数
%}
function Lidar = LidarSearch(obstacle,Lidar,dynamicOb)
% 清除标志
Lidar.dynamic = 0;
%% 重命名坐标
x = Lidar.x;
y = Lidar.y;
r = Lidar.r;
% 记录局部无遮挡障碍(局部极坐标形式)
partObstacle = [];
% 获取局部障碍
for i = floor(x-r):1:ceil(x+r)
    for j = floor(y-r):1:ceil(y+r)
        ind = find(obstacle(:,1) == i&obstacle(:,2) == j);
        if ~isempty(ind)
            partObstacle = [partObstacle;obstacle(ind,:)+0.5];
        end
    end
end
% 添加动态点
if sqrt((dynamicOb(1) - x)^2+(dynamicOb(2) - y)^2) < r
    Lidar.dynamic = 1;
    partObstacle = [partObstacle;dynamicOb'];
end
[p,Lidar.PartObstacle] = obstacleDetection(x,y,partObstacle);
Lidar.Data = p ;
end

\(floor\)为向下取整，\(ceil\)为向上取整

for i = floor(x-r):1:ceil(x+r)
    for j = floor(y-r):1:ceil(y+r)
        ind = find(obstacle(:,1) == i&obstacle(:,2) == j);
        if ~isempty(ind)
            partObstacle = [partObstacle;obstacle(ind,:)+0.5];
        end
    end
end

上面部分代码实际选取的是\([x-r，x+r],[y-r,y+r]\)的矩形区域障碍。

% 添加动态点
if sqrt((dynamicOb(1) - x)^2+(dynamicOb(2) - y)^2) < r
    Lidar.dynamic = 1;
    partObstacle = [partObstacle;dynamicOb'];
end

动态点运行到以雷达坐标为中心、检测半径的圆形范围内时，将动态点加入局部障碍。

obstacleDetection

%{
仿真Lidar检索局部范围内无遮挡障碍
输入：Lidar x坐标，Lidar y坐标，局部障碍集合
输出：无遮挡的障碍信息(极坐标格式)，无遮挡的障碍信息(全局二维格式)，
%}
function [polar,partob] = obstacleDetection(x,y,ob,r)
polarsum = [];
partob = [];
replace = 0;
% 遍历局部区域障碍筛选满足条件障碍
for i = 1:size(ob,1)
    % 计算新障碍的极坐标模长(长度)
    L = sqrt((x - ob(i,1))^2+(y - ob(i,2))^2);
    % 在雷达检测范围内才会进行测距
    if L<=r
        % 计算新障碍的极坐标角度(弧度制)
        angle = atan2(ob(i,1) - x,ob(i,2) - y);
        % 使用新障碍信息与所有已录入障碍信息进行比较
        j = 1;
        while j <= size(polarsum,1)
            % 由于该循环内有对polarsum的删减，所以需要注意数组访问越界问题
            if j > size(polarsum,1)
                break;
            end
            % 如果已录入障碍与新障碍的角度相近
            if abs(polarsum(j,2) - angle) <= atan(0.75/polarsum(j,1))
                % 新增障碍的模长比已录入障碍的模长短则进行替换
                if L < polarsum(j,1)
                    % 如果新障碍信息已替换，删除当前障碍信息
                    if replace == 1
                        partob(j,:) = [];
                        polarsum(j,:)=[];
                        % 抵消当前索引增加
                        j = j - 1;
                    else % 新障碍信息还未录入
                        % 将已录入障碍信息替换为新障碍信息
                        replace = 1;
                        polarsum(j,:) = [L,angle];
                        partob(j,:) = [ob(i,:)];
                    end
                else % 新增障碍的模长比已录入障碍的模长更长，则不在录入，仅进行删减
                    replace = 1;
                end
            end
            j = j + 1;
        end
        % 如果没有发生障碍的删减，将新障碍信息录入局部障碍
        if ~replace
            polarsum = [polarsum;[L,angle]];
            partob = [partob;ob(i,:)];
        end
        replace = 0;
    end
end
    polar = polarsum;
end

该函数主要解决的问题是：如何选取圆形区域内无遮挡的障碍信息？

传入该函数的\(ob\)为边长为\(r\)的矩形区域内的所有障碍位置信息，后续通过

    % 计算新障碍的极坐标模长(长度)
    L = sqrt((x - ob(i,1))^2+(y - ob(i,2))^2);
    % 在雷达检测范围内才会进行测距
    if L<=r
    ...
    END

筛选圆形区域内障碍，再进行遮挡区域去除。

例如上图中绿色直虚线指向的障碍实际应该是测不到的(由于有更近的障碍物遮挡)，所以筛选的主要思路是排除指定角度范围内，距离更远的障碍，因为距离更近的障碍会遮挡远距离的障碍。

角度相邻的范围\(\theta\)，可以由上述示意图计算。其中红色虚线部分在\(\theta\)范围内，且被该角度范围内更近的障碍物遮挡，所以会排除红色虚线指向的障碍。地图中的栅格尺寸为\(1\times1\)，所以\(\theta=arctan(\sqrt2/2/L)\)，其中\(L\)为到该障碍的距离。

            % 如果已录入障碍与新障碍的角度相近
            if abs(polarsum(j,2) - angle) <= atan(0.75/polarsum(j,1))
                % 新增障碍的模长比已录入障碍的模长短则进行替换
                if L < polarsum(j,1)
                    % 如果新障碍信息已替换，删除当前障碍信息
                    if replace == 1
                        partob(j,:) = [];
                        polarsum(j,:)=[];
                        % 抵消当前索引增加
                        j = j - 1;
                    else % 新障碍信息还未录入
                        % 将已录入障碍信息替换为新障碍信息
                        replace = 1;
                        polarsum(j,:) = [L,angle];
                        partob(j,:) = [ob(i,:)];
                    end
                else % 新增障碍的模长比已录入障碍的模长更长，则不在录入，仅进行删减
                    replace = 1;
                end
            end

DWA 过程

另一文档中有原理说明不在赘述。

绘图部分

    % 计算速度色阶
    colorv = [colorv;cmap(1+ceil(254*RM_parameters.x(3)/RM_parameters.KinematicLimit(1)),:)];

将每次循环计算的速度映射到\(0-255\)的色阶上，并保存到\(colorv\)中。

% 绘制速度映射的色阶轨迹
if USE_Traj
    for c =1:size(RM_parameters.savex,2)
        scatter(RM_parameters.savex(2,c),RM_parameters.savex(1,c),6,...
            'MarkerEdgeColor',colorv(c,:),...
            'MarkerFaceColor',colorv(c,:)...
            );hold on;
    end
end

从\(RM_parameters\)结构体中，找出指定的位置信息，并配合该速度的色阶信息，绘制圆点。

    % 动态窗口轨迹
    if ~isempty(RM_parameters.trajDB)
        for it=1:length(RM_parameters.trajDB(:,1))/5
            ind=1+(it-1)*5;
            plot(RM_parameters.trajDB(ind+1,:),RM_parameters.trajDB(ind,:),'-g');hold on;
        end
    end

\(trajDB\)中每五行为动态窗口的一条预测轨迹，所以每绘制完一条轨迹线，\(ind\)索引需要增加5。

Gitee

参考:

局部路径规划 DWA 算法完全解析（理论推导+代码实现，包你看懂！）
DWA算法总结
 基于环境信息的局部路径规划算法改进与应用
 融合动态窗口法与A*算法的港口AGV路径规划方法研究

posted @ 2024-02-24 14:10 DOinging 阅读(214) 评论(0) 编辑收藏举报

刷新页面返回顶部

Doing's Blog

动态窗口法

动态窗口法

运动模型建立

动态窗口设置

动态窗口评价函数

方位角评价

空隙评价

速度评价

最终评价

不足与改进

终点靠近障碍物

应用代码思路解析

代码框架

初始化过程

DwaInitialization

预处理

Lidar模拟

DWA过程

动态窗口计算

Lidar局部区域检索

obstacleDetection

DWA 过程

绘图部分

公告