所有约束在teb_local_planner\include\teb_local_planner\g2o_types，都是头文件。 动态障碍约束EdgeDynamicObstacle，最短路径约束EdgeShortestPath，优先转向约束EdgePreferRotDir，路径点约束EdgeViaPoint没有求雅格比矩阵。

速度约束

最小化的函数是 ${penaltyInterval}( [v,omega]^T ) \cdot weight$

penaltyInterval表示惩罚函数，见penaltyBoundToInterval()函数，它只用于速度约束、加速度约束和edge_velocity_obstacle_ratio

$\frac{error}{cost}$ 向量的维度是2，第一个元素代表线速度，第二个是角速度。
调用：TebOptimalPlanner::AddEdgesVelocity 和 setTebConfig()

class EdgeVelocity : public BaseTebMultiEdge<2, double>
{
	public:  
	EdgeVelocity()
	{
		this->resize(3); // from a g2o::BaseMultiEdge, set the desired number of vertices
	}
	// 代价函数
void computeError()
{
    ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocity()");
    const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
    const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
    const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
    // 两点的向量差
    const Eigen::Vector2d deltaS = conf2->estimate().position() - conf1->estimate().position();
    
    double dist = deltaS.norm();
    const double angle_diff = g2o::normalize_theta(conf2->theta() - conf1->theta());
    // 使用实际弧长，而不是两点的直线距离
    if (cfg_->trajectory.exact_arc_length && angle_diff != 0)
    {
        double radius =  dist/(2*sin(angle_diff/2));
        dist = fabs( angle_diff * radius ); // actual arg length!
    }
    double vel = dist / deltaT->estimate();  // 线速度

    // fast_sigmoid函数是保证速度连续可微
    // vel *= g2o::sign(deltaS[0]*cos(conf1->theta()) + deltaS[1]*sin(conf1->theta())); // consider direction
    vel *= fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction
    const double omega = angle_diff / deltaT->estimate();  // 角速度
    // 线速度需要限制在最大和最大倒退的范围内
    _error[0] = penaltyBoundToInterval(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x, cfg_->optim.penalty_epsilon);
    // 角速度只需限制在最大内
    _error[1] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta, cfg_->optim.penalty_epsilon);

    ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeVelocity::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]);
}

人工智能开发中很少用到不是0就是1的阶跃函数，而是用更为平滑过渡的sigmoid的函数，在深度学习里用的很多。这里的fast_sigmoid函数是保证速度连续可微，很有借鉴意义。

Rösmann还使用了sign/sigmoid函数决定当前的运动方向。

计算误差函数用到的penaltyBoundToInterval，比较好理解

 /*  Linear penalty function for bounding  var to the interval   $-a < var < a $
 * var       The scalar that should be bounded
 * a         lower and upper absolute bound
 * epsilon   safty margin (move bound to the interior of the interval)
 *           penaltyBoundToIntervalDerivative
 *  return Penalty / cost value that is nonzero if the constraint is not satisfied
 */

inline double penaltyBoundToInterval(const double& var,const double& a,const double& epsilon)
{
  if (var < -a+epsilon)
  {
    return (-var - (a - epsilon));
  }
  // 怀疑这里应该是 >=
  if (var <= a-epsilon)
  {
    return 0.;
  }
  else
  {
    return (var - (a - epsilon));
  }
}

/**
 *  Linear penalty function for bounding \c var to the interval \f$ a < var < b \f$
 *  var The scalar that should be bounded
 *  a       lower bound
 *  b       upper bound
 *  epsilon safty margin (move bound to the interior of the interval)
 *  penaltyBoundToIntervalDerivative
 *  return Penalty / cost value that is nonzero if the constraint is not satisfied
 */
inline double penaltyBoundToInterval(const double& var,const double& a, const double& b, const double& epsilon)
{
  if (var < a+epsilon)
  {
    return (-var + (a + epsilon));
  }
  if (var <= b-epsilon)
  {
    return 0.;
  }
  else
  {
    return (var - (b - epsilon));
  }
}

雅格比矩阵的推导

雅格比矩阵的计算被注释了，以为使用重写的雅格比可以加快运算速度，但是Github上有作者的解释，看了似懂非懂：对于一些nontrivial(非显而易见的，难以直观理解的) cost functions (比如 penalizing the distance between a line and a polygon) 最终表达式实在太长了，是否 central difference approximation is slower (2 times evaluating the function) then evaluating the exact Jacobian.

雅克比矩阵的维度为 误差维度 × 优化变量的维度，class EdgeVelocity : public BaseTebMultiEdge<2, double>，是三元边，两个VertexPose顶点，类型PoseSE2(3维)。顶点VertexTimeDiff，类型double(1维)。对于前者，雅格比矩阵是 2x3，对后者是 2x1，也就是如下

_jacobianOplus[0].resize(2,3); // conf1
_jacobianOplus[1].resize(2,3); // conf2
_jacobianOplus[2].resize(2,1); // deltaT

我们把雅格比矩阵简写为 $J$，$J[0]$ 表示第1个VertexPose，$J[1]$ 表示第2个VertexPose，$J[2]$ 表示VertexTimeDiff。$J[0]$ 是两行三列的矩阵，第一行是线速度的误差方程penaltyBoundToInterval对 $x_1$, $y_1$, $\theta_1$ 求偏导。第二行是角速度的误差方程(penaltyBoundToInterval另一个重载形式)对 $x_1$, $y_1$, $\theta_1$ 求偏导。

$J[1]$ 和 $J[2]$ 以此类推