teb_local_planner论文解读(2): Efficient Trajectory Optimization using a Sparse Model

teb2013年的一篇文章

摘要：

teb的最优轨迹问题，可以转换为多变量的非线性最优化问题。大多数的变量成员都与一小部分周围的变量产生联系。进而构成稀疏结构的H矩阵。文章主要就是描述，怎么用g2o来求解teb的优化问题。

g2o在VSLAM和SBA中已经广泛应用。

g2o优化第一步首先构建图模型：

图模型是由节点和边组成：

其中teb所有的节点：

边构成节点之间的约束。用f(x)表示。

最终生成的图的样子：

最终的转换为如下公式：

非线性最小二次优化问题：