cyy/car_nav_lite
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Ackermann MPPI: v+kappa sampling, no reverse, progress critic, safety stop-only

Browse Source
This commit is contained in:
cyy
2026-07-03 08:30:17 +00:00
parent 6459f7c6f5
commit 9590900658
2 changed files with 25 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/laser_safety.cpp
View File

@@ -37,9 +37,9 @@ Twist LaserSafetyFilter::filter(const Twist& cmd, const LaserScan& scan) const {
double right_avg = right_n > 0 ? right_sum / right_n : 0; double right_avg = right_n > 0 ? right_sum / right_n : 0;
if (front_min < stop) { if (front_min < stop) {
// Emergency: stop and turn away // Ackermann: cannot turn in place. Stop means stop.
out.vx = 0.0; out.vx = 0.0;
out.wz = (left_avg > right_avg) ? turn_wz : -turn_wz; out.wz = 0.0;
} else if (front_min < slow) { } else if (front_min < slow) {
out.vx *= 0.3; out.vx *= 0.3;
} }

35
src/local_planner.cpp
View File

@@ -77,8 +77,20 @@ double LocalPlanner::evaluateTrajectory(
double end_to_goal = std::hypot(goal.x - sx, goal.y - sy); double end_to_goal = std::hypot(goal.x - sx, goal.y - sy);
cost += W_FOLLOW * end_to_goal; cost += W_FOLLOW * end_to_goal;
// ── Reverse penalty (mild) ── // ── Progress critic: reward forward progress along path ──
if (vx < 0) cost += std::abs(vx) * 3.0; if (total_path_len > 0.5) {
// Find start and end path indices
size_t s0 = 0, s1 = 0;
double d0 = 1e9, d1 = 1e9;
for (size_t i = 0; i < path.size(); ++i) {
double ds = std::hypot(pose.x - path[i].x, pose.y - path[i].y);
if (ds < d0) { d0 = ds; s0 = i; }
double de = std::hypot(sx - path[i].x, sy - path[i].y);
if (de < d1) { d1 = de; s1 = i; }
}
double progress = std::max(0.0, path_dists[s1] - path_dists[s0]);
cost += 20.0 * std::max(0.0, 0.25 - progress); // penalty for too little progress
}
return cost; return cost;
} }
@@ -107,20 +119,22 @@ Twist LocalPlanner::compute(const Pose2D& pose,
for (int k = 0; k < K; ++k) { for (int k = 0; k < K; ++k) {
double vx, wz; double vx, wz;
if (k < exploit_n) { if (k < exploit_n) {
// Exploit: sample around previous optimal // Exploit: sample v + kappa (Ackermann-compatible)
vx = prev_vx_ + VX_STD * randNormal(); vx = prev_vx_ + VX_STD * randNormal();
wz = prev_wz_ + WZ_STD * randNormal(); double kappa = (prev_vx_ > 0.01) ? (prev_wz_ / prev_vx_ + WZ_STD * randNormal()) : (WZ_STD * randNormal());
wz = vx * kappa;
} else { } else {
// Explore: uniform random across full range (including reverse) // Explore: uniform vx + kappa (Ackermann-compatible)
uint32_t x = rng_state_; uint32_t x = rng_state_;
x ^= x << 13; x ^= x >> 17; x ^= x << 5; rng_state_ = x; x ^= x << 13; x ^= x >> 17; x ^= x << 5; rng_state_ = x;
vx = (double)(x & 0xFFFF) / 65535.0 * (ms_ + 0.5) - 0.5; vx = (double)(x & 0xFFFF) / 65535.0 * ms_;
x ^= x << 13; x ^= x >> 17; x ^= x << 5; rng_state_ = x; x ^= x << 13; x ^= x >> 17; x ^= x << 5; rng_state_ = x;
wz = ((double)(x & 0xFFFF) / 65535.0 - 0.5) * 3.0; double kappa = ((double)(x & 0xFFFF) / 65535.0 - 0.5) * (2.0 / mr_);
wz = vx * kappa;
} }
// Clamp + Ackermann (no hard wz limit — Ackermann constraint is enough) // Clamp + Ackermann: no reverse, curvature-based wz
vx = limit(vx, -0.5, ms_); vx = limit(vx, 0.0, ms_);
double max_wz = std::abs(vx) / mr_; double max_wz = std::abs(vx) / mr_;
wz = limit(wz, -max_wz, max_wz); wz = limit(wz, -max_wz, max_wz);
@@ -152,9 +166,6 @@ Twist LocalPlanner::compute(const Pose2D& pose,
cmd.vx = weighted_vx / sum_weights; cmd.vx = weighted_vx / sum_weights;
cmd.wz = weighted_wz / sum_weights; cmd.wz = weighted_wz / sum_weights;
// Enforce minimum reverse speed
if (cmd.vx < 0 && cmd.vx > -0.3) cmd.vx = -0.3;
cmd.wz = limit(cmd.wz, -std::abs(cmd.vx) / mr_, std::abs(cmd.vx) / mr_); cmd.wz = limit(cmd.wz, -std::abs(cmd.vx) / mr_, std::abs(cmd.vx) / mr_);
prev_vx_ = cmd.vx; prev_vx_ = cmd.vx;