动态窗口法

运动模型建立

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

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

X (t) = {[\begin{matrix} x (t) & y (t) & v (t) & θ (t) & ω (t) \end{matrix}]}^{T}

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

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

U (n) = {[\begin{matrix} v_{p} (n) & ω_{p} (n) \end{matrix}]}^{T}

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

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

{[\begin{matrix} v_{m a x} & ω_{m a x} & a_{m a x} & α_{m a x} & r a t i o_{v} & r a t i o_{ω} \end{matrix}]}^{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{aligned} x (n) & = x (t_{n}) + 0 \times y (t_{n}) + 0 \times v (t_{n}) + 0 \times θ (t_{n}) + 0 \times ω (t_{n}) + d t \times c o s (θ (t_{n})) \times v_{p} (n) + 0 \times ω_{p} (n) \\ y (n) & = 0 \times x (t_{n}) + y (t_{n}) + 0 \times v (t_{n}) + 0 \times θ (t_{n}) + 0 \times ω (t_{n}) + d t \times s i n (θ (t_{n})) \times v_{p} (n) + 0 \times ω_{p} (n) \\ θ (n) & = 0 \times x (t_{n}) + 0 \times y (t_{n}) + 0 \times v (n) + 1 \times θ (t) + 0 \times ω (t) + 0 \times v_{p} (n) + d t \times ω_{p} (t) \\ v (n) & = 0 \times x (t_{n}) + 0 \times y (t_{n}) + 0 \times v (n) + 0 \times θ (t) + 0 \times ω (t) + 1 \times v_{p} (n) + 0 \times ω_{p} (t) \\ ω (n) & = 0 \times x (t_{n}) + 0 \times y (t_{n}) + 0 \times v (n) + 0 \times θ (t) + 0 \times ω (t) + 0 \times v_{p} (n) + 1 \times ω_{p} (t) \end{aligned}

$\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{aligned} X (n) = F * X (t) + B * U (n) \end{aligned}

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

其中

\begin{aligned} F = [\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}], B = [\begin{matrix} d t \times c o s (θ (t_{n})) & 0 \\ d t \times s i n (θ (t_{n})) & 0 \\ 1 & 0 \\ 0 & d t \\ 0 & 1 \end{matrix}] \end{aligned}

$\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{aligned} v (t_{n + 1}) & \in [v (t_{n}) - d t * a_{m a x}, v (t_{n}) + d t * a_{m a x}] \\ ω (t_{n + 1}) & \in [ω (t_{n}) - d t * α_{m a x}, ω (t_{n}) + d t * α_{m a x}] \end{aligned}

$\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 (ν, ω) = α \cdot n o r m a l (h e a d i n g (ν, ω)) + β \cdot n o r m a l (d i s t (ν, ω)) + γ \cdot n o r m a l (v e l (ν, ω))

$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$ 最为下一时刻输入。为了便于处理信息，设置一组向量存储评价函数加权信息及预测时间

E v a l = [\begin{matrix} α & β & γ & t_{p} \end{matrix}]

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

不足与改进

终点靠近障碍物

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

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

\begin{aligned} G (ν, ω) & = α \cdot n o r m a l (h e a d i n g) + β \cdot n o r m a l (d i s t) + γ \cdot n o r m a l (v e l) + ζ \cdot n o r m a l (a s t a r) \\ a s t a r & = g (n) + h (n) \end{aligned}

$\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$ 修改为

E v a l = [\begin{matrix} α & β & γ & ζ & t_{p} \end{matrix}]

$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

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

获取栅格地图内所有障碍的二维索引保存至 $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