From 1b5162edaabd83a15c27b2a25f30dbb707aa457e Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Thu, 12 Mar 2026 23:08:40 +0800 Subject: [PATCH] fix teb --- .../config/simulation/nav2_params_sim.yaml | 4 +- .../g2o_types/edge_acceleration.h | 84 +++++++++++++++---- .../g2o_types/edge_velocity.h | 46 ++++++---- .../src/timed_elastic_band.cpp | 7 +- 4 files changed, 106 insertions(+), 35 deletions(-) diff --git a/src/rm_nav/rm_nav_bringup/config/simulation/nav2_params_sim.yaml b/src/rm_nav/rm_nav_bringup/config/simulation/nav2_params_sim.yaml index be44dbe..41eda25 100644 --- a/src/rm_nav/rm_nav_bringup/config/simulation/nav2_params_sim.yaml +++ b/src/rm_nav/rm_nav_bringup/config/simulation/nav2_params_sim.yaml @@ -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树中查询转换时的容差（秒） diff --git a/src/rm_nav/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_acceleration.h b/src/rm_nav/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_acceleration.h index 5d3ff98..f2421cc 100644 --- a/src/rm_nav/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_acceleration.h +++ b/src/rm_nav/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_acceleration.h @@ -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]); diff --git a/src/rm_nav/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity.h b/src/rm_nav/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity.h index 29956c0..23896e7 100755 --- a/src/rm_nav/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity.h +++ b/src/rm_nav/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity.h @@ -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(_vertices[0]); const VertexPose* conf2 = static_cast(_vertices[1]); const VertexTimeDiff* deltaT = static_cast(_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(_vertices[1]); const VertexTimeDiff* deltaT = static_cast(_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]); diff --git a/src/rm_nav/rm_navigation/teb_local_planner/teb_local_planner/src/timed_elastic_band.cpp b/src/rm_nav/rm_navigation/teb_local_planner/teb_local_planner/src/timed_elastic_band.cpp index 4d7d37c..4ec50fa 100644 --- a/src/rm_nav/rm_navigation/teb_local_planner/teb_local_planner/src/timed_elastic_band.cpp +++ b/src/rm_nav/rm_navigation/teb_local_planner/teb_local_planner/src/timed_elastic_band.cpp @@ -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