主要贡献:
采用 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。提出的时空变形进一步大大优于空间变形: