[论文解读] A Nonlinear Constrained Optimization Framework for Comfortable and Customizable Motion Planning of Nonholonomic Mobile Robots - Part II

Shilpa Gulati, Chetan Jhurani+1 more|arXiv (Cornell University)|May 22, 2013

Robotic Path Planning Algorithms参考文献 76被引用 4

一句话总结

该论文提出了一种非线性约束优化框架，用于使用有限元离散化在非完整移动机器人中规划舒适且可定制的轨迹。通过将运动不适感建模为加权代价泛函，并利用高质量的初始猜测求解由此产生的稀疏大规模优化问题，该方法在7,500种多样的运动规划案例中实现了可靠且亚秒级的收敛，其中99%的路径初始猜测在0.2秒内完成，99%的速度初始猜测在0.12秒内完成。

ABSTRACT

In this series of papers, we present a motion planning framework for planning comfortable and customizable motion of nonholonomic mobile robots such as intelligent wheelchairs and autonomous cars. In Part I, we presented the mathematical foundation of our framework, where we model motion discomfort as a weighted cost functional and define comfortable motion planning as a nonlinear constrained optimization problem of computing trajectories that minimize this discomfort given the appropriate boundary conditions and constraints. In this paper, we discretize the infinite-dimensional optimization problem using conforming finite elements. We describe shape functions to handle different kinds of boundary conditions and the choice of unknowns to obtain a sparse Hessian matrix. We also describe in detail how any trajectory computation problem can have infinitely many locally optimal solutions and our method of handling them. Additionally, since we have a nonlinear and constrained problem, computation of high quality initial guesses is crucial for efficient solution. We show how to compute them.

研究动机与目标

• 解决非完整机器人（如智能轮椅和自动驾驶汽车）中缺乏显式优化人类舒适度的运动规划方法的问题。
• 将运动舒适度建模为一个约束非线性优化问题，通过最小化基于加速度、加加速度和曲率有界性的不适感代价泛函来实现。
• 开发一种基于有限元的离散化方法，以保持海森矩阵的稀疏性，并能处理常规和奇异边界条件。
• 设计一种鲁棒的初始猜测计算策略，以确保大规模非线性优化中的快速且可靠的收敛。
• 通过支持多种边界条件和动态约束，实现可定制的运动规划。

提出的方法

• 使用针对不同边界条件定制的形函数，采用符合有限元方法对无限维优化问题进行离散化。
• 引入辅助变量，即使在存在不等式和等式约束的情况下，也能保持全局海森矩阵的稀疏性。
• 通过一个惩罚高切向和法向加速度及加加速度的代价泛函来建模不适感，同时对速度、加速度和曲率施加边界限制。
• 采用两阶段方法：首先利用解析近似和几何约束计算路径和速度的初始猜测，然后使用Ipopt求解完整非线性问题。
• 在C++中显式计算梯度和海森矩阵，以相比有限差分近似提高收敛速度和鲁棒性。
• 在7,500个具有不同初始/最终位置、朝向以及速度/加速度约束的多样化运动规划问题上验证了该框架。

实验结果

研究问题

• RQ1 如何将非完整移动机器人的运动不适感数学建模为一个连续且有界的代价泛函，以支持定制化和避障？
• RQ2 何种有限元离散化策略能够在轨迹优化中准确处理常规和奇异边界条件的同时，实现海森矩阵的稀疏计算？
• RQ3 如何系统性地生成高质量初始猜测，以确保大规模非线性优化在运动规划中实现快速且可靠的收敛？
• RQ4 所提出的框架在具有不同边界条件的多样化大规模运动规划场景集中的计算性能如何？
• RQ5 该框架在保持舒适性和约束满足的前提下，能够在多大程度上生成多个局部最优轨迹？

主要发现

• 该框架在7,500个具有不同边界条件的测试案例中成功计算出至少一个解，包括多样的最终位置、朝向以及速度/加速度约束。
• 99%的路径初始猜测在0.2秒内完成，99%的速度初始猜测在0.12秒内完成，表明计算效率极高。
• 所有案例中，完整的不适感最小化问题均在10秒内求解，其中99%的案例在4秒内完成，90%的案例在100次迭代内收敛。
• 平均每道问题找到3.6条局部最优轨迹，60%的问题产生四个或更多解，表明解的多样性与鲁棒性极强。
• 在C++中显式计算梯度和海森矩阵显著提升了收敛速度和鲁棒性，相比有限差分近似效果更优。
• 使用辅助变量保持了海森矩阵的稀疏性，从而在存在复杂不等式和等式约束的情况下，仍能实现高效的大型优化。