5

速度和加速度约束的源码

 2 years ago
source link: https://charon-cheung.github.io/2022/08/26/%E8%B7%AF%E5%BE%84%E8%A7%84%E5%88%92/TEB%E7%AE%97%E6%B3%95/%E9%80%9F%E5%BA%A6%E5%92%8C%E5%8A%A0%E9%80%9F%E5%BA%A6%E7%BA%A6%E6%9D%9F%E7%9A%84%E6%BA%90%E7%A0%81/#%E8%B5%B7%E7%82%B9%E7%9A%84%E5%8A%A0%E9%80%9F%E5%BA%A6%E7%BA%A6%E6%9D%9F
Go to the source link to view the article. You can view the picture content, updated content and better typesetting reading experience. If the link is broken, please click the button below to view the snapshot at that time.
neoserver,ios ssh client

速度和加速度约束的源码 | 沉默杀手

速度和加速度约束的源码
2022-08-26|路径规划TEB算法|
Word count: 1k|Reading time: 4 min

所有约束在teb_local_planner\include\teb_local_planner\g2o_types，都是头文件。

最小化的函数是 penaltyInterval([v,omega]T)⋅weightpenaltyInterval([v,omega]T)⋅weight

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

errorcosterrorcost 向量的维度是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函数是保证速度连续可微，很有借鉴意义。

Olb4Bm3IWUsko1D.png


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

加速度约束

默认为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？


report

Recommend

About Joyk


Aggregate valuable and interesting links.
Joyk means Joy of geeK