STD-Trees: Spatio-temporal Deformable Trees for Multirotors Kinodynamic Planning （论文解析）
STD-Trees: Spatio-temporal Deformable Trees for Multirotors Kinodynamic Planning （论文解析）
-
- 动态变形树
- 树边表示
- 轨迹树变形
- 数值结果
- 仿真结果
一般的轨迹优化方案中仅考虑到空间约束、障碍物约束、动力学约束等，本文的轨迹运动设计方案增加时间维度的优化设计，提出动力学规划中的 时空变形 方法，使生成的轨迹更偏向于最优轨迹。提出以变形单元的形式对树进行变形，以平衡优化水平和计算量。
主要贡献：
- 提出了一种基于采样的多旋翼动动力规划器。通过优化包含一个树节点和连接到它的边的变形单元的形式的树来繁荣轨迹树的生长。
- 在形状和时间维度上，提出了有效地 细化变形单元。
采用 RRT# 方案来平衡我们的运动学算法框架中的 探索-开发 权衡
动态变形树
非线性约束使得整个解空间高度非凸。直接应用非线性规划必然会陷入局部极小值。我们采用基于抽样的方法来解决这个问题。算法如下：
按照RRT采样的方法迭代采样生成路径，在每次迭代中 变形当前轨迹树的形状和时间持续时间 的额外步骤（第12行），这一步减少了树的整体成本，而不会增加更多的节点，只有在找到第一个可行解之后变形才会被激活。
变形单元 分为四种类型：NODE,TRUNK,BRANCH,TREE; 如下图：
NODE:只优化一个包含n及其连接边的变形单元。
TRUNK:优化几个变形单元，这些单元是通过从n到start的子指针来选择的。然后按照从子节点到n的顺序进行变形。这就是DRRT采用的策略。
BRANCH:优化根为n的子树中的每一个节点和边，顺序从n开始按照宽度优先搜索，排除所有叶子节点。
TREE:优化整棵树中除起始节点和所有叶节点外的所有节点和边。顺序遵循从start的子节点开始的宽度优先搜索。
树边表示
树边通过多项式的方法表示，一条边的代价可以用{c, T}或者{d, T}来计算，
c 表示多项式的系数矩阵(2s维)；
d 表示轨迹片段的边沿条件矩阵(2s维)，
T 表示轨迹片段运行的时间。
这两种轨迹边的表示方式支持了的树状变形过程。
轨迹树变形
变形单元
每个节点 n 包含以下信息：
xn : n的对应状态，
pn: 轨迹树中n的父节点，
Tn: pn到n的边的时间持续时间，
cn: pn到n的边的系数向量，
Cn: 轨迹树中 n 的子节点集，
gn: 轨迹树之后n的从头成本值。
变形是通过优化节点状态和这些边的持续时间来实现的，将变形单元中的元素集合表示为
包含变形节点 n 和节点的连接时间信息。
通过将节点状态设置为决策变量而不是边缘系数，隐式地消除了树节点之间连续性要求所施加的相等约束，减少了决策变量的数量。
设计目标
由于在变形的过程中只对当前节点进行优化变形，已经产生的节点将固定，所以当前变形不会影响已有的节点，只会影响它的后代节点，因此设计包含变形节点的代价和 代价表达式设置为：
Dn表示所有的后代节点。
或者我们可以为每个节点分配一个权重w∈[0,1]，表明它构成最终最佳轨迹的可能性。对成本的启发式估计可以作为此权重的替代方案。
随着树的生长，得到：
C (xi, xj)是节点 i 到节点 j 的边代价，用下式计算：
无约束构造
约束考虑障碍规避和动力可行性(位置、速度、加速度以及更高阶倒数的限制)。 为了消除这些不等式约束，我们构造惩罚函数并将其转化为软函数。 通过对违例约束进行积分，将违例约束转化为有限维的违例约束，并通过采样惩罚函数的加权和估计该积分。 G(p[s](t)) 完全由 ci 和 Ti 决定，设计惩罚函数为：
其中ki是这条边的样本数，X是G中每一项的罚权向量（s+1项），(ω0， ω1，···，ωki−1，ωki) =(1/ 2,1，···，1,1 /2)为符合梯形法则的正交系数， t = j/ki · Ti为采样时间戳。
对每一个变形单元进行无约束优化：
时空优化
对于变形单元中的一条边{ci, Ti}， i∈ {n} || Cn，利用链式法则导出解耦目标w.r.t {xn, Tn}的梯度:
关于矩阵求导运算的推导公式：https://zhuanlan.zhihu.com/p/273729929
数值结果
在确定环境数据后，对四种变形单元类型进行比较，BRANCH的性能略优于TREE，而NODE和TRUNK的性能则要好得多。
在变形比较中对于三种基于抽样的运动动力学规划方法kRRT, kRRT*和kRRT#，我们将提出的时空变形与只发生空间变形和不发生变形进行比较，对于每一种变形，采用可变BRANCH。提出的时空变形进一步大大优于空间变形：
仿真结果
