Doni/QDAirPortTestSystemBackend
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

优化碰撞检测逻辑，添加冲突释放策略和时间戳记录

Browse Source
This commit is contained in:
sladro 2026-02-08 14:42:43 +08:00
parent 3463926e98
commit c8727fb665
1 changed files with 32 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
src/detector/CollisionDetector.cpp
View File

@ -3,9 +3,18 @@
#include "config/SystemConfig.h"
#include "utils/Logger.h"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <unordered_set>
namespace {
// 冲突释放参数：
// 1) 当前距离达到预警阈值的倍数且不再预测碰撞 -> 立即释放
// 2) 连续一段时间无风险 -> 强制释放（防止历史记录粘连）
constexpr double kResolveDistanceMultiplier = 1.5;
constexpr int64_t kResolveNoRiskTimeoutMs = 5000;
}
CollisionDetector::CollisionDetector(const AirportBounds& bounds, const ControllableVehicles& controllableVehicles)
: airportBounds_(bounds),
vehicleTree_(bounds.getAirportBounds(), 8), // 使用机场总边界初始化四叉树
@ -61,6 +70,8 @@ void CollisionDetector::updateTraffic(const std::vector<Aircraft>& aircraft,
std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
std::vector<CollisionRisk> risks;
const auto now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::system_clock::now().time_since_epoch()).count();
// 获取配置参数
const auto& predictionConfig =
@ -136,6 +147,7 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
record.maxLevel = std::max(record.maxLevel, level);
record.collisionPoint =
collisionResult.collisionPoint; // 更新冲突点
record.detectedTime = now_ms;
Logger::debug("更新冲突记录: obj1=", aircraft.id,
", obj2=", vehicle.id, ", maxLevel=",
static_cast<int>(record.maxLevel),
@ -145,9 +157,7 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
// 创建新的冲突记录
record = { collisionResult
.collisionPoint, // 使用当前检测到的冲突点
std::chrono::system_clock::now()
.time_since_epoch()
.count(),
now_ms,
level, false };
Logger::debug("创建新的冲突记录: obj1=", aircraft.id,
", obj2=", vehicle.id,
@ -178,12 +188,26 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
", timeToCollision=", collisionResult.timeToCollision, "s",
", level=", static_cast<int>(level));
} else if (hasUnresolvedConflict) {
// 检查是否满足解除条件
if (checkCollisionResolved(aircraft, vehicle, it->second,
safeDistance)) {
// 专业化释放策略：
// A. 距离已明显拉开且当前不再预测碰撞 -> 立即释放
// B. 连续无风险超过阈值 -> 强制释放，防止历史 maxLevel 粘连
// C. 其余场景沿用原有几何释放条件
const bool farEnoughAndNoRisk =
!collisionResult.willCollide &&
collisionResult.distance >= safeDistance * kResolveDistanceMultiplier;
const bool noRiskTimeout =
!collisionResult.willCollide &&
(now_ms - it->second.detectedTime) >= kResolveNoRiskTimeoutMs;
const bool geometricResolved =
checkCollisionResolved(aircraft, vehicle, it->second, safeDistance);
if (farEnoughAndNoRisk || noRiskTimeout || geometricResolved) {
it->second.resolved = true;
Logger::debug("冲突已解除: obj1=", aircraft.id,
", obj2=", vehicle.id);
", obj2=", vehicle.id,
", reason=",
farEnoughAndNoRisk ? "far_enough_no_risk" :
(noRiskTimeout ? "no_risk_timeout" : "geometric_condition"));
} else {
// 虽然当前没有检测到风险，但由于未满足解除条件，继续保持原有风险等级
currentCollisions.insert(collisionKey);
@ -509,4 +533,4 @@ CollisionResult CollisionDetector::predictCircleBasedCollision(
result.object2State = CollisionObjectState(p2_hit, speed2, heading2);
return result;
}
}