默认为5元边约束。 但是包含3个类:EdgeAcceleration
, EdgeAccelerationStart
, EdgeAccelerationGoal
,后两个为3元边
向量的维度是2,第一个元素代表线加速度,第二个是角加速度。EdgeAccelerationStart()
and EdgeAccelerationGoal()
用于边界值
加速度约束和速度约束类似,不过变成两段圆弧和角度差、两个时间差、两个线速度和角速度。两个线速度的差除以两个时间差之和就是加速度。1
2
3
4
5
6
7
8
9double 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
9void 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
里有一段:1
2
3
4
5
6if (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
12if (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;
其他项以此类推