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树中查询转换时的容差（秒）

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

@ -141,7 +141,15 @@ public:
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]);
@ -336,7 +344,15 @@ public:
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]);
@ -428,7 +444,15 @@ public:
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]);
@ -527,7 +551,15 @@ public:
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]);
@ -610,7 +642,15 @@ public:
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]);
@ -702,7 +742,15 @@ public:
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]);

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

@ -113,7 +113,16 @@ public:
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]);
}
@ -256,7 +265,16 @@ public:
_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