论文记录

Trajectory Planning and Tracking Strategy Applied to an Unmanned Ground Vehicle in the Presence of Obstacles

introduction

介绍了一些当前UGV面临的挑战：未知的环境+物理约束。还有前人的工作以及本文算法。文章设计了一种提升的AFSA(人工鱼群算法)，产生全局轨迹。其中影响因素有平滑性，安全性，能量消耗等。局部轨迹规划是 a trial-based forward search(TFS)。此外还有多模型预测控制器(multiconstrained model predictive controller)

problem description

机器人在有障碍的环境信息。把环境分成了grid-cell-based。其中机器人从当前网格到下一个可以有八个移动动作。当满足以下条件时，开始碰撞检测算法。
其中$x_l(t)$为机器人位置，$x_{obl}(t)$是障碍位置，$D_d$是传感器器探测距离。
机器人速度是${\vec{U}}_1(t)$，${\vec{U}}_2(t)$是障碍物的速度。障碍物速度应该需要实测吧，但是这样会不会有延时？ ${\vec{l}}_1$和${\vec{l}}_2$是从机器人位置出发，到扩展障碍物的两个切线，扩展的距离是机器人的半径。防止碰撞。${\vec{l}}_0$是从机器人出发到障碍的向量。然后相关计算如下：


如果$|{\theta }_1-{\theta }_2|\ge {\theta }_3$。则不会碰撞这个在思考原因。反之，碰撞会出现。UGV就需要重新规划。
原因如下：

Remark 1：采用全局路径规划和局部路径规划。全局路径时根据之前的环境信息得到。在计算可能会发生碰撞之后，将当前位置设置为局部规划的起始位置，全局中的路径点为局部规划的目标点进行局部路径规划。通过这种方式可以实现全局最优以及不可预见的障碍的局部响应。
remark 2：虽然图中只有一个障碍，但是多个的时候也可以应用。当障碍出现在视野中时，无论是之前信息中存在的还是突然出现的障碍，也都能应用。

系统框架如下：为什么不一直开着局部的？他这个轨迹和路径，是不一样的？

一旦检测到会有碰撞发生，则会用局部轨迹规划替代全局的。具体细节如下：

轨迹规划模块的目标是通过两步生成安全的轨迹。在驾驶前，可以获得部分环境信息，包括起点、目的地、道路。为了避开已知的静态障碍物，改进的AFSA可以预先确定全局轨迹。利用车载传感器，车辆可以探测周围环境，并预测是否会与被探测到的入侵者发生碰撞。在存在潜在入侵者的情况下，以低存储和低计算负担为特征的TFS被设计用来通过规划一个局部轨迹来处理预测的冲突。利用碰撞预测结果和初始状态更新，可以显著提高局部规划的收敛速度。

全局路径采用的是改进的人工鱼群算法，暂时先没看；

Local Trajectory Planning

这块目前还没看懂

本节提出了一种基于马尔可夫链的局部轨迹规划方法TFS，并采用该方法实时避免可能出现的碰撞。

state-space ingredients(状态空间组成)

在状态空间中建立局部轨迹规划模型，在状态空间中进行连续决策，将UGV从起始状态转换为最终状态。状态空间组成包括，状态$s\in S$，动作设置action set $a_t\in A$,转换函数$P_{ss}^a$以及奖励R。将驾驶环境视为二维平面，被分成等大小的网格单元。注意平面内的所有状态都必须满足这个条件：

每一个栅格$(x_l,y_l)$都可以标记为如下:

其中r和c分别为地图在x和y方向上cell 的个数。障碍的状态表示为：

UGV的移动方向由标记为1 ~ 8[45]的8个基本动作决定，所有潜在的移动方向都包含在基本动作中。UGV在采取一个动作后，将从当前状态s移动到相邻状态$s^{\prime }$。过渡模型为$T:S\times A\times S\to [0,1]$。伴随着移动的行动，还有一个即时奖励R(s, a)，它可以被建模为R: S×A→R， $R_{s,s^{\prime }}^a$$表示处于s状态的采取a行动后的即时奖励。

Cost Function and Policy Extraction

在预测移动的入侵者对UGV造成危险时，TFS的目标是寻找新的无碰撞动作，从而使UGV偏离全局轨迹，避免入侵者。为了减少计算时间，将UGV的几何关系作为启发式信息集成到代价函数$V^{\pi }(s,t)$中。而且，在不像传统动态规划那样计算整个状态空间[46]的情况下，持续更新初始状态s以计算目标函数并提取正确的策略。
策略π下的状态s∈s的值定义为$V^{\pi }(s,t)$，初始成本函数$V^0(s,t)$与目的地的距离相关，以突出燃油经济性。设$(x_l,y_l)$为局部目标坐标。首先，$V^0(s,t)$计算为:

其中$\Omega >0$,$V^0(((y_l-1)\cdot r+x_l),t)=\Omega$。(这里有个疑问是，把目的地的成本函数设为一个常值，根据公式，距离越远。其越小。所以其和燃油成反比呗？成本函数越小，实际越大？)
UGV能够检测范围$D_d$内的障碍物。$(x_l(t),y_l(t))$和$(x_{obs}(t),y_{obs}(t))分别是UGV和障碍的坐标。

根据式(3)-式(5)，得到三个相邻角即可实现碰撞预测。当条件(20)满足时，UGV在安全方向上运行。因此，积极的奖励 $ϵ$ 与当前的移动行为相关，如(21)

同理，当会发生碰撞的时候。奖励如下：

这里我的理解是，两个 $ϵ$ 是不一样的。如果会有碰撞发生，两个角度的差的正负决定着下一步是1动作还是8动作才能避免。所以分两类。不会碰撞的给的奖励应该要比会发生的大。 如果两个$ϵ$一样，那没必要设置俩。
另外，在${\theta }_1-{\theta }_2=0$这种特殊情况下，UGV将分别选择两个相邻的动作来确定一个更好的动作。除了选定的动作外，其他动作的奖励为0。在$\zeta$障碍的情况下，即时奖励被改写为：

这个意思是在多障碍的情况下。其中$R_{obj}$表示的是第i个障碍的奖励R。

const function可以表示为：

其中γ∈[0,1]为衰减率，π(s, t)表示对状态s的政策，衰减率表示未来奖励对现值的影响。对于贪婪策略，当前状态的值可以计算为：

最优策略π(s, t)可提取为：

也就是奖励最大的那个状态s。后边的V那一部分，根据公式18，越大表示距离目的越近。奖励那一部分，就是距离障碍物越近会越大。？？？除非$R_{obj}$是负的。有待思考

local path planning 的详细步骤如下：

第一步：设置初值代价函数$V^0(s,t)$。
第二步：重复下述实验n次：
将当前位置设置为开始状态s，并重复以下步骤，直到达到目标状态；
计算三个角θ1， θ2，和θ3，并根据碰撞预测确定即时奖励的值；
根据潜在的运动动作$a_t$，通过8个轨迹计算值函数$V^{k+1}(s,t)$；
可以用贪婪策略提取正确的策略，并且当前状态会被下一个状态 s’ 替换，这个状态是由选择的动作$a_t$产生的。
第三步：通过状态st计算agent位置(xt, yt)，提取轨迹点，其中l(st/r)表示对st/r进行舍入运算。

remark 5:
为了提高UGV对突发入侵的响应率，将碰撞预测融入到局部轨迹规划中，这与其他方法有很大不同。碰撞预测结果作为代价函数中的启发式信息，有利于减少决策时的响应时间。
remark 6:
对于基于马尔可夫过程的优化问题，传统的动态规划决策逻辑可能会导致数百万离散状态的沉重存储负担。更具体地说，动态规划的主要缺点是涉及到状态空间中的整个状态，即使当前的状态不允许该状态。这样的话会导致可能超过存储。如果在大型环境就不是很好。

该算法考虑了动态规划中昂贵的存储空间，使算法的起始状态随当前状态更新，并在每个决策步骤中贪婪地选择正确的动作。该算法初始化当前状态，并利用代价函数探索可能的动作，直到达到目标状态。在拟议的计划,将会有以下变量的计算:没有状态行动价值函数,成本函数$\sum V(s,t)=(1000:1000,1)$,奖励$\sum R(s,a,t)=R(1,:81)$,和最优政策$\sum \pi (s,t)=\pi (1,1)$。注意,只有八个方向都集成到网络计算在每一个决策步骤。因此，在线计算所涉及的状态空间总是有限和固定的。因此，所提出的复合轨迹规划策略为提高存储效率提供了一种可行的选择。

TRAJECTORY TRACKING CONTROLLER DESIGN

这一部分采用的是multiconstrained model predictive controller进行轨迹控制跟随。

result

结果方面，采用Matlab，simulink进行仿真。全局路径规划将本文改进的AFSA，初始的AFSA，PSO进行对比。在时间，平滑性上，新提出的都比之前的表现好。

局部路径规划方面，结果显示其距离障碍物的最近距离都要大于0.2米。在计算时间方面也远小于dynamic programming。

还有就是轨迹跟随的结果展示。对比的是model prediction controller和MMPC的对比。包括时间以及规划路径与实际的差异。

实际环境中测试结果轨迹如图：