讯飞线上赛总结（三）

讯飞线上赛总结（三）——局部路径规划

  局部路劲规划是在未知环境中，基于传感器获取周围环境信息，并使机器人自主获得一条无碰撞的最优路径。用于局部路径规划的经典算法有人工势场法、模拟退火法、模糊逻辑法、TEB局部规划以及DWA算法等路径规划方法。

简介

  ①人工势场法结构简单、计算量较小，但存在容易产生局部最小值的问题。
  ②模拟退火法将热力学的理论套用到统计学上，利用概率的突跳性，实现随机优化问题的求解，避免出现局部极值问题，可弥补人工势场法的缺陷。
  ③模糊逻辑法是根据模糊的环境信息，依照对应表格规划的信息，实现局部路径规划，优点在于容易计算，能够实时跟踪规划。
  ④DWA算法的思想是采样加评估的方法，减少了优化部分的求解比重，但是采样过程是基于运动学参数采样，受算法复杂性限制，每条曲线上每个点的运动学参数需要保持一致，如果采样曲线过长，容易造成曲线发散；如果曲线过短，则会由于看的不够远导致相邻帧之间轨迹的跳动。
  ⑤State Lattice算法在状态空间中进行采样，比较适合在结构优化道路中使用，但是缺点是它的计算复杂度随着它采样的密度增加而增加，并且算法最终的效果和cost的设置也有关系，不合理的设置会导致路径选择的跳动。
  ⑥TEB算法将整个运动路径比作为一条橡皮筋，连接起点和目标点，并让这个路径可以变形，变形的条件就是将所有约束当做橡皮筋的外力，采用优化方式，将所有评估指标加入评估函数，通过调整系数得到所期望的安全、平滑稳定的曲线。当环境场景复杂时，优化函数设计过于复杂，当参数优化空间较大时，很难落入最优解，迭代时间要求提高。
  比赛中主要使用了TEB和DWA两种算法，最初使用的是DWA算法在多次调参没能达到理想效果后，选择了TEB算法，这篇博客也主要讲解我比较熟悉的这两种算法。

DWA算法

  DWA算法（dynamic window approach）即动态窗口法，主要是在速度 空间中采样多组速度，并模拟机器人在这些速度下一定时间内的轨迹。在得到多组轨迹后，对这些轨迹进行评价，选取最优轨迹所对应的速度来驱动机器人运动。该算法突出点在于动态窗口，它的含义是依据移动机器人的加减速性能限定速度采样空间在一个可行的动态范围内。

1、速度采样

  在机器人轨迹运动模型已有的情况下，就可以根据速度推算出轨迹。因此，只需要采样很多速度，推算轨迹，然后评价这些轨迹好不好就可以了。在速度 的二维空间中，存在无穷多组速度，但是根据机器人本身的限制和环境限制可以将采样速度控制在一定范围内：
  ①移动机器人受自身最大速度、最小速度的限制：
V m = { v ∈ [ v min ⁡ , v max ⁡ ] , w ∈ [ w min ⁡ , w max ⁡ ] } V_{m}=\left\{v \in\left[v_{\min }, v_{\max }\right], w \in\left[w_{\min }, w_{\max }\right]\right\} Vm​={v∈[vmin​,vmax​],w∈[wmin​,wmax​]}
  ②移动机器人受电机性能的影响：
   由于电机力矩，存在最大的加减速度限制，因此移动机器人轨迹前向模拟的周期sim_period内，存在一个动态窗口，在该窗口内的速度是机器人能够实际达到的速度：
V d = { ( v , w ) ∣ v ∈ [ v c − v ˙ b Δ t , v c + v ˙ a Δ t ] ∧ w ∈ [ w c − w ˙ b Δ t , w c + w ˙ a Δ t ] } V_{d}=\left\{(v, w) \mid v \in\left[v_{c}-\dot{v}_{b} \Delta t, v_{c}+\dot{v}_{a} \Delta t\right] \wedge w \in\left[w_{c}-\dot{w}_{b} \Delta t, w_{c}+\dot{w}_{a} \Delta t\right]\right\} Vd​={(v,w)∣v∈[vc​−v˙b​Δt,vc​+v˙a​Δt]∧w∈[wc​−w˙b​Δt,wc​+w˙a​Δt]}
   其中 $v_c,w_c$ 是机器人的当前速度，其他标志对应最大加速度和最大减速度。
  ③基于移动机器人安全的考虑：
   为了能够在碰到障碍物前停下来，因此在最大减速度条件下，速度有一个范围：
$$V_a=\left\{(v,w)\mid v\le \sqrt{2\cdot \mathrm{dist}(v,w)\cdot {\dot{v}}_b}\wedge w\le \sqrt{2\cdot \mathrm{dist}(v,w)\cdot {\dot{w}}_b}\right\}$$
   其中$dist(v,w)$为速度$(v,w)$对应轨迹上离障碍物最近的距离，如图弧线所示：

  注：这个条件并不是在采样一开始就能得到的。需要我们模拟出来机器人轨迹以后，找到障碍物位置，计算出机器人到障碍物之间的距离，然后看当前采样的这对速度能否在碰到障碍物之前停下来，如果能够停下来，那这对速度就是可接收的admissible。如果不能停下来，这对速度就得抛弃。为了简化每组速度对应轨迹的计算，该算法假设机器人在往前模拟轨迹的这段时间（sim_period）内速度不变，直到下一时刻采样给定新的速度命令。

2、评价函数

  在采样的速度组中，有若干组轨迹是可行的，因此采用评价函数的方式为每条轨迹进行评价。在原始论文中，采用的评价函数为：
$$G(v,w)=\sigma (\alpha \cdot \mathrm{heading}(v,w)+\beta \cdot \mathrm{dist}(v,w)+\gamma \cdot \mathrm{velocity}(v,w))$$

(1)方位角评价函数

   $\mathrm{heading}(v,w)$是用来评价机器人在当前设定的采样速度下，达到模拟轨迹末端时的朝向和目标之间的角度差距。如图所示：

论文中采用的是 $180-\theta$的方式来评价, $\theta$也就是越小，评价得分越高

(2)空隙

   $dist(v,w)$代表机器人在当前轨迹上与最近的障碍物之间的距离。如果在这条轨迹上没有障碍物，那就将其设定为一个常数。

(3)速度

  $\mathrm{velocity}(v,w)$用来评价当前轨迹的速度大小。

(4)平滑处理

  即归一化处理，上面三个部分计算出来以后不是直接相加，而是每一部分在归一化以后再相加。
$$\text{ normal-head }(i)=\frac{\text{ head }(i)}{{\sum }_{i=1}^n\text{ head }(i)}$$
$$\text{ normal-dist }(i)=\frac{\text{ dist }(i)}{{\sum }_{i=1}^n\text{ dist }(i)}$$
$$\text{ normal-velocity }(i)=\frac{\text{ velocity }(i)}{{\sum }_{i=1}^n\text{ velocity }(i)}$$
  其中，n为采样的所有轨迹，i为待评价的当前轨迹。
  归一化的目的是平滑（smoothing）：例如障碍物距离，机器人传感器检测到的最小障碍物距离在二维空间中是不连续的，这条轨迹能够遇到障碍物，旁边的一条不一定会遇到，并且这条轨迹最小的障碍物距离是1m，旁边的一条就是10m。那么障碍物距离的这种评价标准导致评价函数不连续，也会导致某项在评价函数中太占优势，如这里的障碍物距离10m相对于1m就非常占优势，所以进行归一化处理：
$$\text{ normal-dist }(i)=\frac{\text{ dist }(i)}{{\sum }_{i=1}^n\text{ dist }(i)}$$
  这样三项都变成同一的百分比，每个障碍物最小距离都是这100份中的一份。
  因此，三者构成的评价函数的物理意义是：在局部导航中，是的机器人避开障碍物，朝着目标以较快速度行驶，缺一不可。

TEB算法

  时间有限，后续会单独就该算法详细写一篇博客，请见谅！！！

写在最后

  博客中有相关错误，希望大佬们指出。（感谢感谢！！）