加速度约束和雅格比矩阵的推导

加速度约束和雅格比矩阵的推导 | 沉默杀手

默认为5元边约束。 但是包含3个类：EdgeAcceleration, EdgeAccelerationStart, EdgeAccelerationGoal，后两个为3元边

errorcosterrorcost 向量的维度是2，第一个元素代表线加速度，第二个是角加速度。
EdgeAccelerationStart() and EdgeAccelerationGoal() 用于边界值

加速度约束和速度约束类似，不过变成两段圆弧和角度差、两个时间差、两个线速度和角速度。两个线速度的差除以两个时间差之和就是加速度。

double vel1 = dist1 / dt1->dt();
double vel2 = dist2 / dt2->dt();
const double acc_lin  = (vel2 - vel1)*2 / ( dt1->dt() + dt2->dt() );
_error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x, cfg_->optim.penalty_epsilon);

const double omega1 = angle_diff1 / dt1->dt();
const double omega2 = angle_diff2 / dt2->dt();
const double acc_rot  = (omega2 - omega1)*2 / ( dt1->dt() + dt2->dt() );
_error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon);

起点的加速度约束

注意：起点加速度约束和终点加速度约束都是3元边。

对于起点的加速度约束，源码有以下变化:

void setInitialVelocity(const geometry_msgs::Twist& vel_start)
{
	_measurement = &vel_start;
}

ROS_ASSERT_MSG(cfg_ && _measurement, 
"You must call setTebConfig() and setInitialVelocity() on EdgeAccelerationStart()");
vel1 = _measurement->linear.x;
omega1 = _measurement->angular.z;

其他全一样，对于终点的加速度约束，则是vel2和omega2换成了_measurement的成员。那么这个_measurement从何而来？

在TebOptimalPlanner::plan里有一段:

if (start_vel)
  setVelocityStart(*start_vel);
if (free_goal_vel)
  setVelocityGoalFree();
else
  vel_goal_.first = true;

setVelocityStart给起始速度赋值：

// std::pair<bool, geometry_msgs::Twist>  vel_start_;
// 注意vel_start_ 和 vel_start 不同
void TebOptimalPlanner::setVelocityStart(const geometry_msgs::Twist& vel_start)
{
  vel_start_.first = true;
  vel_start_.second.linear.x = vel_start.linear.x;
  vel_start_.second.linear.y = vel_start.linear.y;
  vel_start_.second.angular.z = vel_start.angular.z;
}

然后到了AddEdgesAcceleration里添加起始加速度约束，才用到vel_start_

if (vel_start_.first)
{
  EdgeAccelerationStart* acceleration_edge = new EdgeAccelerationStart;
  acceleration_edge->setVertex(0,teb_.PoseVertex(0));
  acceleration_edge->setVertex(1,teb_.PoseVertex(1));
  acceleration_edge->setVertex(2,teb_.TimeDiffVertex(0));
  // 这里就是上面的 setInitialVelocity, _measurement 就是 vel_start_.second
  acceleration_edge->setInitialVelocity(vel_start_.second);
  acceleration_edge->setInformation(information);
  acceleration_edge->setTebConfig(*cfg_);
  optimizer_->addEdge(acceleration_edge);
}

AddEdgesAcceleration然后添加普通的加速度约束，再就是终点的加速度约束，这里就奇怪了，相应的setVelocityGoal函数没有调用的地方，这里是vel_goal_.second唯一赋值的地方。即使free_goal_vel为false，也没有赋值，意思是终点的速度为0？

雅格比矩阵的推导可以参照速度约束的，不同的地方是这次把角度单独拿出来了，这样有了8个雅格比

_jacobianOplus[0].resize(2,2); // conf1
_jacobianOplus[1].resize(2,2); // conf2
_jacobianOplus[2].resize(2,2); // conf3
_jacobianOplus[3].resize(2,1); // deltaT1
_jacobianOplus[4].resize(2,1); // deltaT2
_jacobianOplus[5].resize(2,1); // angle1
_jacobianOplus[6].resize(2,1); // angle2
_jacobianOplus[7].resize(2,1); // angle3

既然拿出了角度，那么对conf的雅格比矩阵的维度就变成了 2x2，不是2x3。两个误差函数，一个是线速度的，一个是角速度的

代码中的 double aux0 = 2/sum_time_inv; 错了 应当是 double aux0 = 2/sum_time;

其他项以此类推