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

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

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

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

1
2
3
4
5
6
7
8
9
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元边。

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

1
2
3
4
5
6
7
8
9
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;

其他全一样,对于终点的加速度约束,则是vel2omega2换成了_measurement的成员。那么这个_measurement从何而来?

TebOptimalPlanner::plan里有一段:

1
2
3
4
5
6
if (start_vel)
setVelocityStart(*start_vel);
if (free_goal_vel)
setVelocityGoalFree();
else
vel_goal_.first = true;

setVelocityStart给起始速度赋值:

1
2
3
4
5
6
7
8
9
// 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_

1
2
3
4
5
6
7
8
9
10
11
12
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个雅格比

1
2
3
4
5
6
7
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;
J[0](0,0)的推导过程
其他项以此类推