tian/CollisionAvoidance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

在AirportBounds.h 中添加了告警阈值的计算逻辑，修改了碰撞检测逻辑

Browse Source
This commit is contained in:
Tian jianyong 2024-11-18 23:49:54 +08:00
parent 080c8a5f32
commit 16048839bd
6 changed files with 146 additions and 103 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
README.md
View File

@ -18,14 +18,17 @@
- 碰撞检测模块：分析潜在的碰撞风险
- 坐标转换模块：处理不同坐标系统
- 核心数据类型：定义基础的数据结构
- 性能测试模块：使用 Google Benchmark 进行性能评估
## 开发环境
- C++17
- CMake 3.10+
- CMake 3.14+
- Boost 1.86.0
- nlohmann_json
- nlohmann_json 3.11.3
- Google Test
- Google Mock
- Google Benchmark
## 构建说明
@ -33,25 +36,30 @@
# 创建构建目录
mkdir build && cd build
# 配置项目
cmake ..
# 配置项目（启用测试）
cmake -DBUILD_TESTING=ON ..
# 编译
make
cmake --build .
# 运行测试
./bin/unit_tests
# 运行单元测试
ctest --output-on-failure
# 运行性能测试
./bin/benchmark_tests
```
## 测试框架
- 使用 Google Test 进行单元测试
- 单元测试：使用 Google Test 框架
- Mock 测试：使用 Google Mock 进行模拟
- 性能测试：使用 Google Benchmark
- 测试覆盖：
- 基础数据类型
- 碰撞检测逻辑
- 数据采集功能
- HTTP 数据源
- Mock 服务器用于测试
- 性能基准测试
## 目录结构
@ -62,6 +70,7 @@ graph TD
A --> D[tools]
A --> E[docs]
A --> F[build]
A --> G[benchmarks]
B --> BA[collector]
B --> BB[detector]
@ -92,9 +101,26 @@ graph TD
C --> CC[DataCollectorTest.cpp]
C --> CD[HTTPDataSourceTest.cpp]
G --> GA[CollisionDetectorBenchmark.cpp]
G --> GB[CoordinateConverterBenchmark.cpp]
D --> DA[mock_server.py]
```
## 依赖管理
项目使用 CMake 的 FetchContent 模块管理第三方依赖：
```cmake
include(FetchContent)
FetchContent_Declare(
json
URL https://github.com/nlohmann/json/releases/download/v3.11.3/json.tar.xz
)
FetchContent_MakeAvailable(json)
```
## 配置说明
- 数据源配置：
@ -122,6 +148,12 @@ graph TD
## 版本历史
- v1.1.0 (2024-03-20)
- 添加 Google Benchmark 性能测试
- 升级 CMake 最低版本至 3.14
- 规范化第三方库版本管理
- 添加 FetchContent 依赖管理
- v1.0.0 (2024-11-15)
- 初始版本发布
- 基本功能实现

14
src/core/System.cpp
View File

@ -90,28 +90,24 @@ void System::processCollisions(const std::vector<CollisionRisk>& collisions) {
for (const auto& risk : collisions) {
// 根据风险等级选择不同的日志级别
switch (risk.level) {
case RiskLevel::CRITICAL:
case RiskLevel::EMERGENCY:
Logger::error("严重碰撞风险: ", risk.id1, " 与 ", risk.id2,
", 距离: ", risk.distance, "米",
", 相对速度: ", risk.relativeSpeed, "m/s");
break;
case RiskLevel::HIGH:
case RiskLevel::CRITICAL:
Logger::warning("高度碰撞风险: ", risk.id1, " 与 ", risk.id2,
", 距离: ", risk.distance, "米",
", 相对速度: ", risk.relativeSpeed, "m/s");
break;
case RiskLevel::MEDIUM:
Logger::warning("中等碰撞风险: ", risk.id1, " 与 ", risk.id2,
case RiskLevel::WARNING:
Logger::warning("低度碰撞风险: ", risk.id1, " 与 ", risk.id2,
", 距离: ", risk.distance, "米",
", 相对速度: ", risk.relativeSpeed, "m/s");
break;
case RiskLevel::LOW:
Logger::info("低度碰撞风险: ", risk.id1, " 与 ", risk.id2,
", 距离: ", risk.distance, "米",
", 相对速度: ", risk.relativeSpeed, "m/s");
case RiskLevel::NONE:
break;
}
}

107
src/detector/CollisionDetector.cpp
View File

@ -31,8 +31,7 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
// 过滤出可控车辆
std::vector<Vehicle> controlVehicles;
for (const auto& vehicle : allVehicles) {
bool isControl = controllableVehicles_->isControllable(vehicle.vehicleNo);
if (isControl) {
if (controllableVehicles_->isControllable(vehicle.vehicleNo)) {
controlVehicles.push_back(vehicle);
}
}
@ -63,7 +62,7 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
double relativeSpeed = std::sqrt(vx*vx + vy*vy);
// 计算风险等级
RiskLevel level = calculateRiskLevel(distance, threshold);
RiskLevel level = calculateRiskLevel(distance, aircraft.position, true, false);
// 添加碰撞风险信息
risks.push_back({
@ -83,47 +82,9 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
const auto& controlVehicle = controlVehicles[i];
const auto& areaConfig = getCollisionParams(controlVehicle.position);
// 只检查与后面的可控车辆的碰撞，避免重复检测
for (size_t j = i + 1; j < controlVehicles.size(); ++j) {
const auto& otherVehicle = controlVehicles[j];
// 计算平面距离
double dx = controlVehicle.position.x - otherVehicle.position.x;
double dy = controlVehicle.position.y - otherVehicle.position.y;
double distance = std::sqrt(dx*dx + dy*dy);
double threshold = areaConfig.vehicleCollisionRadius;
if (distance <= threshold) {
// 计算相对运动
MovementVector v1v(controlVehicle.speed, controlVehicle.heading);
MovementVector v2v(otherVehicle.speed, otherVehicle.heading);
double vx = v1v.vx - v2v.vx;
double vy = v1v.vy - v2v.vy;
double relativeSpeed = std::sqrt(vx*vx + vy*vy);
// 计算风险等级
RiskLevel level = calculateRiskLevel(distance, threshold);
// 添加碰撞风险信息
risks.push_back({
controlVehicle.vehicleNo,
otherVehicle.vehicleNo,
level,
distance,
relativeSpeed,
{vx, vy}
});
}
}
// 检查与非可控车辆的碰撞
for (const auto& otherVehicle : allVehicles) {
// 跳过可控车辆（已经在上面检查过了）
if (std::find_if(controlVehicles.begin(), controlVehicles.end(),
[&](const Vehicle& v) { return v.vehicleNo == otherVehicle.vehicleNo; })
!= controlVehicles.end()) {
continue;
}
// 检查与所有其他车辆的碰撞（包括可控和非可控）
for (size_t j = i + 1; j < allVehicles.size(); ++j) {
const auto& otherVehicle = allVehicles[j];
// 计算平面距离
double dx = controlVehicle.position.x - otherVehicle.position.x;
@ -140,7 +101,7 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
double relativeSpeed = std::sqrt(vx*vx + vy*vy);
// 计算风险等级
RiskLevel level = calculateRiskLevel(distance, threshold);
RiskLevel level = calculateRiskLevel(distance, controlVehicle.position, false, false);
// 添加碰撞风险信息
risks.push_back({
@ -159,18 +120,23 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
return risks;
}
RiskLevel CollisionDetector::calculateRiskLevel(double distance, double threshold) const {
double ratio = distance / threshold;
RiskLevel CollisionDetector::calculateRiskLevel(double distance, const Vector2D& position,
bool isAircraft1, bool isAircraft2) const {
// 获取当前区域的配置
const auto& areaConfig = getCollisionParams(position);
// 修改风险等级的判断逻辑
if (ratio <= 0.5) {
return RiskLevel::CRITICAL; // 0-50%
} else if (ratio <= 0.75) {
return RiskLevel::HIGH; // 50-75%
} else if (ratio <= 1.0) { // 修改这里，包含等于阈值的情况
return RiskLevel::CRITICAL; // 75-100%
// 获取告警阈值
auto thresholds = areaConfig.getThresholds(isAircraft1, isAircraft2);
// 直接比较距离和阈值
if (distance <= thresholds.emergency) {
return RiskLevel::EMERGENCY;
} else if (distance <= thresholds.critical) {
return RiskLevel::CRITICAL;
} else if (distance <= thresholds.warning) {
return RiskLevel::WARNING;
}
return RiskLevel::LOW; // >100%
return RiskLevel::NONE;
}
bool CollisionDetector::checkAircraftVehicleCollision(const Aircraft& aircraft,
@ -182,18 +148,19 @@ bool CollisionDetector::checkAircraftVehicleCollision(const Aircraft& aircraft,
double dy = aircraft.position.y - vehicle.position.y;
double distanceSquared = dx*dx + dy*dy;
// 使用平方距离比较，避免开方运算
double thresholdSquared = areaConfig.aircraftGroundRadius * areaConfig.aircraftGroundRadius;
double criticalThresholdSquared = thresholdSquared * 0.25; // 0.5 * 0.5 = 0.25
// 获取该区域的告警阈值
auto thresholds = areaConfig.getThresholds(true, false); // aircraft vs vehicle
double emergencyThresholdSquared = thresholds.emergency * thresholds.emergency;
// 如果距离小于阈值的一半，直接报警
if (distanceSquared < criticalThresholdSquared) {
// 如果距离小于紧急阈值，直接报警
if (distanceSquared < emergencyThresholdSquared) {
return true;
}
// 如果距离在阈值范围内，检查相对运动
if (distanceSquared < thresholdSquared) {
// 使用预计算的速度分量
// 如果距离在警告阈值范围内，检查相对运动
double warningThresholdSquared = thresholds.warning * thresholds.warning;
if (distanceSquared < warningThresholdSquared) {
// 计算相对运动
MovementVector av(aircraft.speed, aircraft.heading);
MovementVector vv(vehicle.speed, vehicle.heading);
double vx = av.vx - vv.vx;
@ -209,7 +176,6 @@ bool CollisionDetector::checkAircraftVehicleCollision(const Aircraft& aircraft,
bool CollisionDetector::checkVehicleCollision(const Vehicle& v1,
const Vehicle& v2) const {
// 获取车辆所在区域的配置
const auto& areaConfig = getCollisionParams(v1.position);
// 计算平面距离
@ -217,14 +183,17 @@ bool CollisionDetector::checkVehicleCollision(const Vehicle& v1,
double dy = v1.position.y - v2.position.y;
double distance = std::sqrt(dx*dx + dy*dy);
// 如果距离小于阈值的一半，直接报警
if (distance < areaConfig.vehicleCollisionRadius * 0.5) {
// 获取该区域的告警阈值
auto thresholds = areaConfig.getThresholds(false, false); // vehicle vs vehicle
// 如果距离小于紧急阈值，直接报警
if (distance < thresholds.emergency) {
return true;
}
// 如果距离在阈值范围内，检查相对运动
if (distance < areaConfig.vehicleCollisionRadius) {
// 计算相对速度（修正航向计算）
// 如果距离在警告阈值范围内，检查相对运动
if (distance < thresholds.warning) {
// 计算相对速度
double v1x = v1.speed * std::cos((90 - v1.heading) * M_PI / 180.0);
double v1y = v1.speed * std::sin((90 - v1.heading) * M_PI / 180.0);
double v2x = v2.speed * std::cos((90 - v2.heading) * M_PI / 180.0);

11
src/detector/CollisionDetector.h
View File

@ -10,10 +10,10 @@
// 碰撞风险等级
enum class RiskLevel {
LOW = 0, // 低风险：距离在阈值的 75%-100% 之间
MEDIUM = 1, // 中等风险：距离在阈值的 50%-75% 之间
HIGH = 2, // 高风险：距离在阈值的 25%-50% 之间
CRITICAL = 3 // 严重风险：距离小于阈值的 25%
NONE = 0, // 无风险
WARNING = 1, // 低风险：距离在阈值的 50%-100% 之间
CRITICAL = 2, // 中等风险：距离在阈值的 25%-50% 之间
EMERGENCY = 3, // 高风险：距离在阈值的 0%-25% 之间
};
// 碰撞风险信息
@ -54,7 +54,8 @@ private:
bool checkVehicleCollision(const Vehicle& v1, const Vehicle& v2) const;
// 计算风险等级
RiskLevel calculateRiskLevel(double distance, double threshold) const;
RiskLevel calculateRiskLevel(double distance, const Vector2D& position,
bool isAircraft1, bool isAircraft2) const;
// 计算相对运动
struct MovementVector {

21
src/spatial/AirportBounds.h
View File

@ -18,6 +18,27 @@ struct AreaConfig {
double vehicleCollisionRadius; // 车辆间碰撞检测半径
double aircraftGroundRadius; // 航空器与车辆碰撞检测半径
double heightThreshold; // 高度阈值
// 获取不同类型交通工具间的告警阈值
struct CollisionThresholds {
double warning; // 警告距离
double critical; // 危险距离
double emergency; // 紧急距离
};
// 计算两个交通工具之间的告警阈值
CollisionThresholds getThresholds(bool isAircraft1, bool isAircraft2) const {
// 获取基础安全距离
double baseRadius = isAircraft1 || isAircraft2 ?
aircraftGroundRadius : vehicleCollisionRadius;
// 计算不同级别的阈值
return {
baseRadius, // 警告距离：为基础距离
baseRadius / 2.0, // 危险距离：为基础距离的一半
baseRadius / 4.0 // 紧急距离：为基础距离的四分之一
};
}
};
// 机场区域定义

46
tests/CollisionDetectorTest.cpp
View File

@ -30,7 +30,7 @@ public:
}
const AreaConfig& getAreaConfig(AreaType type) const override {
static const AreaConfig config{20.0, 40.0, 15.0}; // 使用较小的阈值以确保测试通过
static const AreaConfig config{50.0, 100.0, 15.0};
return config;
}
@ -93,7 +93,7 @@ TEST_F(CollisionDetectorTest, DetectControllableVehicleAircraftCollision) {
EXPECT_EQ(risks[0].id1, "TEST001"); // 航空器ID
EXPECT_EQ(risks[0].id2, "VEH001"); // 车辆ID
EXPECT_EQ(risks[0].distance, 20); // 距离应该是20米
EXPECT_EQ(risks[0].level, RiskLevel::CRITICAL); // 20米距离应该是严重风险
EXPECT_EQ(risks[0].level, RiskLevel::EMERGENCY); // 20米距离应该是严重风险
}
}
@ -174,14 +174,30 @@ TEST_F(CollisionDetectorTest, MultipleControllableVehiclesCollision) {
// 设置测试数据
std::vector<Vehicle> vehicles;
for (int i = 0; i < 3; ++i) {
Vehicle vehicle;
vehicle.vehicleNo = "VEH00" + std::to_string(i + 1);
vehicle.position = {100.0 + i * 20, 100}; // 每辆车间隔20米
vehicle.speed = 5;
vehicle.heading = 90;
vehicles.push_back(vehicle);
}
// VEH001 在 (100, 100)
Vehicle v1;
v1.vehicleNo = "VEH001";
v1.position = {100.0, 100.0};
v1.speed = 5;
v1.heading = 90;
vehicles.push_back(v1);
// VEH002 在 (120, 100)，与 VEH001 相距 20 米
Vehicle v2;
v2.vehicleNo = "VEH002";
v2.position = {120.0, 100.0};
v2.speed = 5;
v2.heading = 90;
vehicles.push_back(v2);
// VEH003 在 (200, 100)，与其他车辆距离超过阈值
Vehicle v3;
v3.vehicleNo = "VEH003";
v3.position = {200.0, 100.0};
v3.speed = 5;
v3.heading = 90;
vehicles.push_back(v3);
// 更新交通数据
detector_->updateTraffic({}, vehicles);
@ -190,7 +206,15 @@ TEST_F(CollisionDetectorTest, MultipleControllableVehiclesCollision) {
auto risks = detector_->detectCollisions();
// 验证结果
EXPECT_EQ(risks.size(), 2); // 应该检测到2个碰撞风险（相邻车辆之间）
EXPECT_EQ(risks.size(), 1); // 应该只检测到1个碰撞风险（VEH001和VEH002之间）
if (risks.size() == 1) {
// 验证碰撞风险的详细信息
EXPECT_TRUE((risks[0].id1 == "VEH001" && risks[0].id2 == "VEH002") ||
(risks[0].id1 == "VEH002" && risks[0].id2 == "VEH001"));
EXPECT_EQ(risks[0].distance, 20.0);
EXPECT_EQ(risks[0].level, RiskLevel::CRITICAL); // 20米应该是危险级别
}
}
// 性能测试：模拟真实机场场景