robofish/MOVE_AI
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix teb

Browse Source
This commit is contained in:
Robofish 2026-03-12 23:08:40 +08:00
parent 28aae00395
commit 1b5162edaa
4 changed files with 106 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/rm_nav/rm_nav_bringup/config/simulation/nav2_params_sim.yaml
View File

@ -141,9 +141,9 @@ controller_server:
#*******************************************************************************
max_vel_x: 3.0 #最大平移速度
max_vel_x_backwards: 3.0 #最大后退速度
max_vel_theta: 6.0 #最大转向角速度
max_vel_theta: 0.0 #最大转向角速度
acc_lim_x: 4.0 #最大平移加速度
acc_lim_theta: 6.0 #最大角加速度
acc_lim_theta: 0.0 #最大角加速度
is_footprint_dynamic: False #如果为真，在检查轨迹可行性之前更新足迹
use_proportional_saturation: False #如果为true，则按比例减少所有扭曲分量（线性x和y以及角度z）（如果任何扭曲分量超过其相应的界限），而不是单独饱和每个扭曲分量
transform_tolerance: 0.5 #在TF树中查询转换时的容差（秒）

84
src/rm_nav/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_acceleration.h
View File

@ -135,13 +135,21 @@ public:
_error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
// ANGULAR ACCELERATION
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);
// 当角加速度限制为0或很小时，几乎完全忽略这个约束
if (cfg_->robot.acc_lim_theta < 0.01) {
_error[1] = 0.0; // 完全忽略
} else if (cfg_->robot.acc_lim_theta < 1.0) {
_error[1] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon * 5.0);
_error[1] *= 0.1; // 大幅降低影响
} else {
_error[1] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon);
}
TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAcceleration::computeError() translational: _error[0]=%f\n",_error[0]);
TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAcceleration::computeError() rotational: _error[1]=%f\n",_error[1]);
@ -330,13 +338,21 @@ public:
const double acc_lin = (vel2 - vel1) / dt->dt();
_error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
// ANGULAR ACCELERATION
const double omega1 = _measurement->angular.z;
const double omega2 = angle_diff / dt->dt();
const double acc_rot = (omega2 - omega1) / dt->dt();
_error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
// 当角加速度限制为0或很小时，几乎完全忽略这个约束
if (cfg_->robot.acc_lim_theta < 0.01) {
_error[1] = 0.0;
} else if (cfg_->robot.acc_lim_theta < 1.0) {
_error[1] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon * 5.0);
_error[1] *= 0.1;
} else {
_error[1] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon);
}
TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationStart::computeError() translational: _error[0]=%f\n",_error[0]);
TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationStart::computeError() rotational: _error[1]=%f\n",_error[1]);
@ -422,13 +438,21 @@ public:
const double acc_lin = (vel2 - vel1) / dt->dt();
_error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
// ANGULAR ACCELERATION
const double omega1 = angle_diff / dt->dt();
const double omega2 = _measurement->angular.z;
const double acc_rot = (omega2 - omega1) / dt->dt();
_error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
// 当角加速度限制为0或很小时，几乎完全忽略这个约束
if (cfg_->robot.acc_lim_theta < 0.01) {
_error[1] = 0.0;
} else if (cfg_->robot.acc_lim_theta < 1.0) {
_error[1] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon * 5.0);
_error[1] *= 0.1;
} else {
_error[1] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon);
}
TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationGoal::computeError() translational: _error[0]=%f\n",_error[0]);
TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationGoal::computeError() rotational: _error[1]=%f\n",_error[1]);
@ -521,13 +545,21 @@ public:
_error[0] = penaltyBoundToInterval(acc_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(acc_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon);
// ANGULAR ACCELERATION
double omega1 = g2o::normalize_theta(pose2->theta() - pose1->theta()) / dt1->dt();
double omega2 = g2o::normalize_theta(pose3->theta() - pose2->theta()) / dt2->dt();
double acc_rot = (omega2 - omega1)*2 / dt12;
_error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
// 当角加速度限制为0或很小时，几乎完全忽略这个约束
if (cfg_->robot.acc_lim_theta < 0.01) {
_error[2] = 0.0;
} else if (cfg_->robot.acc_lim_theta < 1.0) {
_error[2] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon * 5.0);
_error[2] *= 0.1;
} else {
_error[2] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon);
}
TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAcceleration::computeError() translational: _error[0]=%f\n",_error[0]);
@ -604,13 +636,21 @@ public:
_error[0] = penaltyBoundToInterval(acc_lin_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(acc_lin_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon);
// ANGULAR ACCELERATION
double omega1 = _measurement->angular.z;
double omega2 = g2o::normalize_theta(pose2->theta() - pose1->theta()) / dt->dt();
double acc_rot = (omega2 - omega1) / dt->dt();
_error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
// 当角加速度限制为0或很小时，几乎完全忽略这个约束
if (cfg_->robot.acc_lim_theta < 0.01) {
_error[2] = 0.0;
} else if (cfg_->robot.acc_lim_theta < 1.0) {
_error[2] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon * 5.0);
_error[2] *= 0.1;
} else {
_error[2] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon);
}
TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationStart::computeError() translational: _error[0]=%f\n",_error[0]);
TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationStart::computeError() strafing: _error[1]=%f\n",_error[1]);
@ -696,13 +736,21 @@ public:
_error[0] = penaltyBoundToInterval(acc_lin_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(acc_lin_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon);
// ANGULAR ACCELERATION
double omega1 = g2o::normalize_theta(pose_goal->theta() - pose_pre_goal->theta()) / dt->dt();
double omega2 = _measurement->angular.z;
double acc_rot = (omega2 - omega1) / dt->dt();
_error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
// 当角加速度限制为0或很小时，几乎完全忽略这个约束
if (cfg_->robot.acc_lim_theta < 0.01) {
_error[2] = 0.0;
} else if (cfg_->robot.acc_lim_theta < 1.0) {
_error[2] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon * 5.0);
_error[2] *= 0.1;
} else {
_error[2] = penaltyBoundToInterval(acc_rot, cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon);
}
TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationGoal::computeError() translational: _error[0]=%f\n",_error[0]);
TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationGoal::computeError() strafing: _error[1]=%f\n",_error[1]);

46
src/rm_nav/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity.h
View File

@ -88,16 +88,16 @@ public:
/**
* @brief Actual cost function
*/
*/
void computeError()
{
TEB_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)
@ -106,14 +106,23 @@ public:
dist = fabs( angle_diff * radius ); // actual arg length!
}
double vel = dist / deltaT->estimate();
// 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);
// 当旋转速度限制为0或很小时，几乎完全忽略这个约束
if (cfg_->robot.max_vel_theta < 0.01) {
_error[1] = 0.0; // 完全忽略
} else if (cfg_->robot.max_vel_theta < 0.5) {
_error[1] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta, cfg_->optim.penalty_epsilon * 5.0);
_error[1] *= 0.1;
} else {
_error[1] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta, cfg_->optim.penalty_epsilon);
}
TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeVelocity::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]);
}
@ -234,7 +243,7 @@ public:
/**
* @brief Actual cost function
*/
*/
void computeError()
{
TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocityHolonomic()");
@ -242,21 +251,30 @@ public:
const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
Eigen::Vector2d deltaS = conf2->position() - conf1->position();
double cos_theta1 = std::cos(conf1->theta());
double sin_theta1 = std::sin(conf1->theta());
double sin_theta1 = std::sin(conf1->theta());
// transform conf2 into current robot frame conf1 (inverse 2d rotation matrix)
double r_dx = cos_theta1*deltaS.x() + sin_theta1*deltaS.y();
double r_dy = -sin_theta1*deltaS.x() + cos_theta1*deltaS.y();
double vx = r_dx / deltaT->estimate();
double vy = r_dy / deltaT->estimate();
double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) / deltaT->estimate();
_error[0] = penaltyBoundToInterval(vx, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x, cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(vy, cfg_->robot.max_vel_y, 0.0); // we do not apply the penalty epsilon here, since the velocity could be close to zero
_error[2] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon);
// 当旋转速度限制为0或很小时，几乎完全忽略这个约束
if (cfg_->robot.max_vel_theta < 0.01) {
_error[2] = 0.0;
} else if (cfg_->robot.max_vel_theta < 0.5) {
_error[2] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta, cfg_->optim.penalty_epsilon * 5.0);
_error[2] *= 0.1;
} else {
_error[2] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta, cfg_->optim.penalty_epsilon);
}
TEB_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]) && std::isfinite(_error[2]),
"EdgeVelocityHolonomic::computeError() _error[0]=%f _error[1]=%f _error[2]=%f\n",_error[0],_error[1],_error[2]);

7
src/rm_nav/rm_navigation/teb_local_planner/teb_local_planner/src/timed_elastic_band.cpp
View File

@ -55,10 +55,15 @@ namespace
double trans_dist = (end.position() - start.position()).norm();
dt_constant_motion = trans_dist / max_vel_x;
}
if (max_vel_theta > 0) {
// 如果旋转速度限制很小（< 0.1 rad/s），不让旋转时间主导整体时间
// 这样可以避免在旋转速度受限时导致平移速度也变得很慢
if (max_vel_theta > 0.1) {
double rot_dist = std::abs(g2o::normalize_theta(end.theta() - start.theta()));
dt_constant_motion = std::max(dt_constant_motion, rot_dist / max_vel_theta);
}
// 如果旋转速度很小或为0，只考虑平移时间，让优化器自己处理旋转约束
return dt_constant_motion;
}
} // namespace