[论文解读] Kinodynamic RRT*: Optimal Motion Planning for Systems with Linear Differential Constraints

Dustin J. Webb, Jur van den Berg|arXiv (Cornell University)|May 23, 2012

Robotic Path Planning Algorithms参考文献 17被引用 64

一句话总结

本文提出Kinodynamic RRT*，一种针对具有线性微分约束的机器人的渐近最优运动规划算法。该算法通过使用固定终态、自由终时的最优控制器，精确且高效地连接任意状态对，实现了对可控制线性系统的渐近最优性，适用于任意维度，对幂零系统可获得闭式解，并可通过每步局部线性化推广至非线性动力学系统。

ABSTRACT

We present Kinodynamic RRT*, an incremental sampling-based approach for asymptotically optimal motion planning for robots with linear differential constraints. Our approach extends RRT*, which was introduced for holonomic robots (Karaman et al. 2011), by using a fixed-final-state-free-final-time controller that exactly and optimally connects any pair of states, where the cost function is expressed as a trade-off between the duration of a trajectory and the expended control effort. Our approach generalizes earlier work on extending RRT* to kinodynamic systems, as it guarantees asymptotic optimality for any system with controllable linear dynamics, in state spaces of any dimension. Our approach can be applied to non-linear dynamics as well by using their first-order Taylor approximations. In addition, we show that for the rich subclass of systems with a nilpotent dynamics matrix, closed-form solutions for optimal trajectories can be derived, which keeps the computational overhead of our algorithm compared to traditional RRT* at a minimum. We demonstrate the potential of our approach by computing asymptotically optimal trajectories in three challenging motion planning scenarios: (i) a planar robot with a 4-D state space and double integrator dynamics, (ii) an aerial vehicle with a 10-D state space and linearized quadrotor dynamics, and (iii) a car-like robot with a 5-D state space and non-linear dynamics.

研究动机与目标

• 解决采样基于规划器在具有微分约束的运动系统中缺乏渐近最优性的问题。
• 克服RRT*在连接非完整或动态系统中任意状态对时，难以生成可行且最优轨迹的局限性。
• 实现高维状态空间中具有可控制线性动力学系统的最优运动规划。
• 为具有幂零动力学矩阵的系统提供闭式解，实现最优轨迹的高效计算。
• 通过在每一步采样时进行局部线性化，将方法扩展至非线性系统。

提出的方法

• 集成固定终态、自由终时的最优控制公式，以计算任意两状态间精确且最优的轨迹。
• 利用线性二次型调节器（LQR）理论求解具有可控制动力学的线性系统的两点边值问题。
• 推导出具有幂零动力学矩阵系统的闭式最优轨迹，最大限度降低计算开销。
• 在每一步采样迭代中对非线性动力学进行局部线性化，以将方法推广至非线性系统。
• 将最优连接模块集成至RRT*框架中，支持重布线以实现渐近最优性。
• 采用具有自适应衰减的邻域半径策略，在高维空间中平衡探索与计算成本。

实验结果

研究问题

• RQ1 在具有线性微分约束的系统中，采样基于运动规划能否实现渐近最优性？
• RQ2 对于可控制线性系统，能否在任意两状态间实现精确且高效的最优轨迹计算？
• RQ3 能否为一类丰富的系统（幂零动力学矩阵）推导出最优轨迹的闭式解，以降低计算成本？
• RQ4 该算法的性能如何随状态空间维度和系统复杂度变化？
• RQ5 通过局部线性化，非线性系统在多大程度上可被处理，同时保持最优性保证？

主要发现

• Kinodynamic RRT* 对任意维度状态空间中具有可控制线性动力学的系统实现了渐近最优性。
• 对于具有幂零动力学矩阵的系统，最优轨迹可通过闭式解计算，与标准RRT*相比计算开销极低。
• 该算法在三个复杂场景中成功计算出渐近最优轨迹：4D双积分器、10D线性化四旋翼机和5D非线性汽车型机器人。
• 随着树中节点数量的增加，最优解的成本收敛至渐近最优值，所有实验中均观察到快速收敛。
• 在汽车型机器人实验中，使用有限邻域半径显著提升了性能，即使在高维空间下也凸显了局部连接的优势。
• 计算成本随节点数量呈二次方增长，与RRT*的复杂度一致，但高维状态空间和数值条件问题仍对性能造成一定影响。