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

teb2013年的一篇文章

摘要:

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

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

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

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

其中teb所有的节点:

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

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

最终生成的图的样子:

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

最终的转换为如下公式:

非线性最小二次优化问题:
teb_local_planner论文解读(2): Efficient Trajectory Optimization using a Sparse Model

上一篇:Apache Calcite VolcanoPlanner源码学习


下一篇:Hybrid Astar