[论文解读] Belief Space Planning Simplified: Trajectory-Optimized LQG (T-LQG) (Extended Report)

Mohammadhussein Rafieisakhaei, Suman Chakravorty+1 more|arXiv (Cornell University)|Aug 10, 2016

Machine Learning and Algorithms参考文献 23被引用 4

一句话总结

该论文提出了一种计算高效的信念空间规划方法——轨迹优化LQG（T-LQG），适用于具有不确定性的非线性系统。通过将基准轨迹作为优化变量，并利用分离原理，T-LQG将信念空间问题的维度从$O(n^2 + n)$降低至$O(n)$，从而通过标准非线性规划（NLP）实现可扩展求解，计算复杂度为$O(Kn^3)$，在计算效率和理论严谨性方面优于现有方法。

ABSTRACT

Planning under motion and observation uncertainties requires solution of a stochastic control problem in the space of feedback policies. In this paper, we reduce the general (n^2+n)-dimensional belief space planning problem to an (n)-dimensional problem by obtaining a Linear Quadratic Gaussian (LQG) design with the best nominal performance. Then, by taking the underlying trajectory of the LQG controller as the decision variable, we pose a coupled design of trajectory, estimator, and controller design through a Non-Linear Program (NLP) that can be solved by a general NLP solver. We prove that under a first-order approximation and a careful usage of the separation principle, our approximations are valid. We give an analysis on the existing major belief space planning methods and show that our algorithm has the lowest computational burden. Finally, we extend our solution to contain general state and control constraints. Our simulation results support our design.

研究动机与目标

• 解决非线性系统中存在过程噪声和观测噪声时信念空间规划的高计算负担问题。
• 克服启发式方法（如最可能观测，MLO）的局限性，后者缺乏正式的控制器设计，且依赖于有缺陷的假设。
• 开发一种可扩展的、理论基础坚实的统一优化框架，将轨迹、估计器和控制器设计耦合。
• 在复杂环境中（包括非凸障碍物场）实现在线重规划和约束处理。
• 通过LQG和一阶近似方法，在保持最优估计与控制性能的同时，最小化计算复杂度。

提出的方法

• 通过将基准轨迹作为主要优化变量，将信念空间规划建模为非线性规划（NLP），将维度从$n^2 + n$降低至$n$。
• 应用分离原理，解耦估计器与控制器设计，确保LQR控制器能在基准轨迹周围稳定系统。
• 利用动态Riccati方程计算与轨迹相关的估计协方差，实现在无需完整信念传播的情况下评估性能。
• 通过在基准轨迹周围对系统动力学进行一阶近似来优化基准性能，确保理论有效性。
• 通过修改代价函数来惩罚与基于机器人几何形状生成的膨胀多边形障碍物的接近程度，实现障碍物避让。
• 当线性化误差超过容差阈值时，支持在线重规划，确保执行过程中的鲁棒性。

实验结果

研究问题

• RQ1 是否可以在不牺牲估计或控制性能的前提下，将信念空间规划简化为$O(n)$维的优化问题？
• RQ2 如何利用分离原理以理论有效的方式耦合轨迹、估计器和控制器设计，以应对不确定性？
• RQ3 与MLO或POMDP求解器等现有方法相比，采用轨迹优化LQG的信念空间规划器的计算复杂度是多少？
• RQ4 T-LQG是否能在计算成本更低的前提下，实现优于启发式或基于粒子滤波方法的估计性能？
• RQ5 T-LQG如何在不增加维度或计算负担的前提下处理非凸环境和碰撞约束？

主要发现

• T-LQG的计算复杂度为$O(Kn^3)$，显著低于MLO方法的$O(n_{tr}(Kn^3 + kn^2) + Kn^6)$复杂度。
• 通过将基准轨迹作为优化变量，该方法将信念空间规划问题的维度从$n^2 + n$降低至$n$。
• 在youBot机器人上的仿真结果表明，T-LQG成功在狭窄通道中避障，同时优化了信息丰富的轨迹。
• 仿真中的优化轨迹避开了禁止区域（以椭球体表示），并更接近地标，从而提升了估计性能。
• T-LQG在理论严谨性上优于MLO方法，因其采用分离原理并包含稳定控制器，而MLO依赖于启发式假设。
• 由于采用低维NLP公式，重规划可行且高效，支持在线适应，当线性化误差超过容差时可实现实时调整。