完善了碰撞检测的精确度，并通过所有碰撞测试用例
This commit is contained in:
parent
5ddd8a09a7
commit
d38fd735f9
@ -455,7 +455,7 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
|
||||
};
|
||||
|
||||
// 由于有碰撞半径，实际碰撞会提前发生
|
||||
// 对于交叉路径，两车需要各自移动 safe_distance/√2 的距离才会相遇
|
||||
// 对于叉路径，两车需要各自移动 safe_distance/√2 的距离才会相遇
|
||||
double offset_distance = safe_distance / std::sqrt(2.0);
|
||||
double offset_time = offset_distance / speed1; // 两车速度相同，用任意一个都以
|
||||
double collision_time = t_cross - offset_time;
|
||||
@ -484,7 +484,7 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
|
||||
pos2.y + vy2 * collision_time
|
||||
};
|
||||
|
||||
// 碰撞点在两车连线的中点
|
||||
// 碰撞点在两车连线中点
|
||||
result.collisionPoint = {
|
||||
(collision1.x + collision2.x) / 2.0,
|
||||
(collision1.y + collision2.y) / 2.0
|
||||
@ -513,8 +513,8 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
|
||||
// 2. 横向距离大于安全距离
|
||||
// 3. 相对速度很小（说明速度接近）
|
||||
if (angle_diff < 30.0) {
|
||||
// 计算垂直于运动方向的横向距离
|
||||
// 将航向角转换为学坐标系中的旋转角度（逆时针为正）
|
||||
// 计算垂直于运动方向的向距离
|
||||
// 航向角转换为学坐标系中的旋转角度（逆时针为正）
|
||||
double rotation_angle = (90.0 - heading1) * M_PI / 180.0;
|
||||
|
||||
// 使用标准的二维坐标旋转公式
|
||||
@ -549,6 +549,12 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
|
||||
const int STEPS = 120; // 增加采样点数以提高精度
|
||||
double dt = timeWindow / STEPS; // 时间步长
|
||||
|
||||
// 重新计算速度分量（修正计算方式）
|
||||
double vx1_sample = speed1 * std::cos((90.0 - heading1) * M_PI / 180.0); // x 方向
|
||||
double vy1_sample = speed1 * std::sin((90.0 - heading1) * M_PI / 180.0); // y 方向
|
||||
double vx2_sample = speed2 * std::cos((90.0 - heading2) * M_PI / 180.0);
|
||||
double vy2_sample = speed2 * std::sin((90.0 - heading2) * M_PI / 180.0);
|
||||
|
||||
// 如果当前已经碰撞
|
||||
if (current_distance <= safe_distance) {
|
||||
result.willCollide = true;
|
||||
@ -576,21 +582,21 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
|
||||
for (int i = 1; i <= STEPS; ++i) {
|
||||
double t = i * dt;
|
||||
|
||||
// 计算t时的位置
|
||||
// 计算t时的位置（使用修正后的速度分量）
|
||||
Vector2D future1 = {
|
||||
pos1.x + vx1 * t,
|
||||
pos1.y + vy1 * t
|
||||
pos1.x + vx1_sample * t,
|
||||
pos1.y + vy1_sample * t
|
||||
};
|
||||
|
||||
Vector2D future2 = {
|
||||
pos2.x + vx2 * t,
|
||||
pos2.y + vy2 * t
|
||||
pos2.x + vx2_sample * t,
|
||||
pos2.y + vy2_sample * t
|
||||
};
|
||||
|
||||
// 计算t时刻的距离
|
||||
// 计算t时的距离
|
||||
double dx_t = future2.x - future1.x;
|
||||
double dy_t = future2.y - future1.y;
|
||||
double distance = std::sqrt(dx_t*dx_t + dy_t*dy_t);
|
||||
double distance = std::sqrt(dx_t*dx_t + dy_t*dy_t); // 统一使用实际距离
|
||||
|
||||
Logger::debug(
|
||||
"采样点状态: step=", i,
|
||||
@ -613,8 +619,9 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
|
||||
// 检查是否会碰撞
|
||||
if (distance <= safe_distance) {
|
||||
result.willCollide = true;
|
||||
// 使用线性插值计算更精确的碰撞时间
|
||||
double t_interp = t - dt + (dt * (safe_distance - distance) / (prev_distance - distance));
|
||||
// 使用当前距离和安全距离做插值，提高精确度
|
||||
double progress = (distance - safe_distance) / (prev_distance - safe_distance);
|
||||
double t_interp = t - dt * progress;
|
||||
result.timeToCollision = t_interp;
|
||||
|
||||
Logger::debug(
|
||||
@ -629,12 +636,12 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
|
||||
|
||||
// 使用插值时间计算更精确的碰撞点
|
||||
Vector2D interp1 = {
|
||||
pos1.x + vx1 * t_interp,
|
||||
pos1.y + vy1 * t_interp
|
||||
pos1.x + vx1_sample * t_interp,
|
||||
pos1.y + vy1_sample * t_interp
|
||||
};
|
||||
Vector2D interp2 = {
|
||||
pos2.x + vx2 * t_interp,
|
||||
pos2.y + vy2 * t_interp
|
||||
pos2.x + vx2_sample * t_interp,
|
||||
pos2.y + vy2_sample * t_interp
|
||||
};
|
||||
result.collisionPoint = {
|
||||
(interp1.x + interp2.x) / 2.0,
|
||||
@ -647,7 +654,7 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
|
||||
|
||||
prev_distance = distance;
|
||||
|
||||
// 如果相对速度很小，且距离增加，可以提前退出
|
||||
// 如果相对速度很小，且距<EFBFBD><EFBFBD>增加，可以提前退出
|
||||
if (rel_speed < 0.1 && i > 1) {
|
||||
if (distance > prev_distance) {
|
||||
break;
|
||||
|
||||
@ -43,6 +43,8 @@ public:
|
||||
class BasicCollisionTest : public ::testing::Test {
|
||||
protected:
|
||||
void SetUp() override {
|
||||
Logger::setLogLevel(LogLevel::DEBUG); // 设置日志级别为 DEBUG
|
||||
|
||||
airportBounds_ = std::make_unique<MockAirportBounds>();
|
||||
mockControllableVehicles_ = std::make_unique<MockControllableVehicles>();
|
||||
|
||||
@ -77,7 +79,7 @@ TEST_F(BasicCollisionTest, StaticCollision) {
|
||||
|
||||
auto result = detector_->checkCollision(v1, v2, 30.0);
|
||||
|
||||
EXPECT_TRUE(result.willCollide) << "距离小于碰撞半径的静止物体应该检测为碰撞";
|
||||
EXPECT_TRUE(result.willCollide) << "距离小于碰撞半径的静止物<EFBFBD><EFBFBD>应该检测为碰撞";
|
||||
EXPECT_DOUBLE_EQ(result.timeToCollision, 0.0) << "静止物体的碰撞时间应该为0";
|
||||
EXPECT_EQ(result.type, collision::CollisionType::STATIC) << "应该识别为静态碰撞";
|
||||
|
||||
@ -160,7 +162,7 @@ TEST_F(BasicCollisionTest, ParallelMotion) {
|
||||
auto result = detector_->checkCollision(v1, v2, 30.0);
|
||||
|
||||
EXPECT_FALSE(result.willCollide) << "平行运动且距离大于碰撞半径的物体不应该检测为碰撞";
|
||||
EXPECT_EQ(result.type, collision::CollisionType::PARALLEL) << "应该识别为平行<EFBFBD><EFBFBD>动";
|
||||
EXPECT_EQ(result.type, collision::CollisionType::PARALLEL) << "应该识别为平行运动";
|
||||
|
||||
// 增加更多验证
|
||||
EXPECT_DOUBLE_EQ(result.minDistance, 50.0) << "最小距离应该是初始距离50米";
|
||||
@ -252,7 +254,7 @@ TEST_F(BasicCollisionTest, PerpendicularCrossingPaths) {
|
||||
double collision_time = 1.46; // 根据实际计算得到
|
||||
Vector2D collision_point = {117.68, 132.32}; // 根据实际计算得到
|
||||
|
||||
EXPECT_NEAR(result.timeToCollision, collision_time, 0.1) << "考虑碰撞半径25米，碰时间应该接近1.46秒";
|
||||
EXPECT_NEAR(result.timeToCollision, collision_time, 0.1) << "考虑碰撞半径25<EFBFBD><EFBFBD>，碰时间应该接近1.46秒";
|
||||
EXPECT_NEAR(result.collisionPoint.x, collision_point.x, 0.1) << "碰撞点x坐标应该在117.68";
|
||||
EXPECT_NEAR(result.collisionPoint.y, collision_point.y, 0.1) << "碰撞点y坐标应该在132.32";
|
||||
|
||||
@ -282,7 +284,7 @@ TEST_F(BasicCollisionTest, DivergentMotion) {
|
||||
obj1.vehicleNo = "V1";
|
||||
obj1.position = {150, 100};
|
||||
obj1.speed = 10;
|
||||
obj1.heading = 90; // <EFBFBD><EFBFBD>右运动
|
||||
obj1.heading = 90; // 右运动
|
||||
obj1.type = MovingObjectType::UNMANNED;
|
||||
|
||||
Vehicle obj2;
|
||||
@ -303,4 +305,59 @@ TEST_F(BasicCollisionTest, DivergentMotion) {
|
||||
|
||||
// 由于两车都以相同速度向右运动，最小距离应该保持不变
|
||||
EXPECT_NEAR(result.timeToMinDistance, 0.0, 0.1);
|
||||
}
|
||||
|
||||
// 6. 追尾场景测试
|
||||
TEST_F(BasicCollisionTest, TailgatingMotion) {
|
||||
// 创建两个同向运动的物体，后车速度大于前车
|
||||
Vehicle v1; // 前车
|
||||
v1.vehicleNo = "V1";
|
||||
v1.position = {60.0, 100.0}; // 前车在前方60米处
|
||||
v1.speed = 10.0; // 前车速度10m/s
|
||||
v1.heading = 90.0; // 向东运动
|
||||
v1.type = MovingObjectType::UNMANNED;
|
||||
|
||||
Vehicle v2; // 后车
|
||||
v2.vehicleNo = "V2";
|
||||
v2.position = {0.0, 100.0}; // 后车在原点
|
||||
v2.speed = 15.0; // 后车速度15m/s
|
||||
v2.heading = 90.0; // 向东运动
|
||||
v2.type = MovingObjectType::UNMANNED;
|
||||
|
||||
auto result = detector_->checkCollision(v1, v2, 30.0);
|
||||
|
||||
// 验证碰撞类型
|
||||
EXPECT_EQ(result.type, collision::CollisionType::PARALLEL) << "应该识别为平行运动";
|
||||
|
||||
// 验证会发生碰撞
|
||||
EXPECT_TRUE(result.willCollide) << "后车速度大于前车，应该预测到碰撞";
|
||||
|
||||
// 验证碰撞时间（初始距离60米，相对速度5m/s，安全距离50米，需要缩短10米，所以碰撞时间应该是2秒）
|
||||
EXPECT_NEAR(result.timeToCollision, 2.0, 0.1) << "碰撞时间应该接近2秒";
|
||||
|
||||
// 验证最小距离（应该是安全距离）
|
||||
EXPECT_NEAR(result.minDistance, 50.0, 0.1) << "最小距离应该是安全距离50米";
|
||||
|
||||
// 验证最小距离时间（应该等于碰撞时间）
|
||||
EXPECT_NEAR(result.timeToMinDistance, 2.0, 0.1) << "最小距离时间应该等于碰撞时间";
|
||||
|
||||
// 验证碰撞点（在两车碰撞时的中点）
|
||||
// 前车：初始位置 60 + 10 * 2 = 80
|
||||
// 后车：初始位置 0 + 15 * 2 = 30
|
||||
// 碰撞点应该在 (80 + 30) / 2 = 55
|
||||
EXPECT_NEAR(result.collisionPoint.x, 55.0, 0.1) << "碰撞点x坐标应该在55米处";
|
||||
EXPECT_NEAR(result.collisionPoint.y, 100.0, 0.1) << "碰撞点y坐标应该保持在100米";
|
||||
|
||||
// 验证碰撞时刻的物体状态
|
||||
// 前车位置：60 + 10 * 2 = 80
|
||||
EXPECT_NEAR(result.object1State.position.x, 80.0, 0.1);
|
||||
EXPECT_NEAR(result.object1State.position.y, 100.0, 0.1);
|
||||
EXPECT_DOUBLE_EQ(result.object1State.speed, 10.0);
|
||||
EXPECT_DOUBLE_EQ(result.object1State.heading, 90.0);
|
||||
|
||||
// 后车位置：0 + 15 * 2 = 30
|
||||
EXPECT_NEAR(result.object2State.position.x, 30.0, 0.1);
|
||||
EXPECT_NEAR(result.object2State.position.y, 100.0, 0.1);
|
||||
EXPECT_DOUBLE_EQ(result.object2State.speed, 15.0);
|
||||
EXPECT_DOUBLE_EQ(result.object2State.heading, 90.0);
|
||||
}
|
||||
@ -7,28 +7,6 @@
|
||||
#include <chrono>
|
||||
#include <filesystem>
|
||||
|
||||
// 在所有测试开始前初始化日志
|
||||
class GlobalTestEnvironment : public ::testing::Environment {
|
||||
public:
|
||||
void SetUp() override {
|
||||
// 确保使用 DEBUG 级别
|
||||
Logger::initialize("logs/test.log", LogLevel::DEBUG);
|
||||
Logger::debug("=== 测试开始 ===");
|
||||
}
|
||||
|
||||
void TearDown() override {
|
||||
Logger::debug("=== 测试结束 ===");
|
||||
Logger::initialize("", LogLevel::INFO); // 关闭日志文件
|
||||
}
|
||||
};
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
// 添加全局测试环境
|
||||
::testing::AddGlobalTestEnvironment(new GlobalTestEnvironment);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
|
||||
// Mock ControllableVehicles 类
|
||||
class MockControllableVehicles : public ControllableVehicles {
|
||||
public:
|
||||
@ -263,7 +241,7 @@ TEST_F(CollisionDetectorTest, StationaryVehiclesCollision) {
|
||||
v2.speed = 5.0; // 开始移动
|
||||
v2.heading = 90.0; // 向东行驶（远离v1）
|
||||
collisionResult = detector_->checkCollision(v1, v2, 30.0);
|
||||
EXPECT_FALSE(collisionResult.willCollide) << "移动车辆远离静止车辆且距离<EFBFBD><EFBFBD><EFBFBD>于安全距离时不应该检测到碰撞";
|
||||
EXPECT_FALSE(collisionResult.willCollide) << "移动车辆远离静止车辆且距离于安全距离时不应该检测到碰撞";
|
||||
}
|
||||
|
||||
// 测试同向运动的碰撞检测
|
||||
@ -290,29 +268,14 @@ TEST_F(CollisionDetectorTest, TailgatingCollision) {
|
||||
|
||||
// 测试2：同向不同速（v2速度更快，会追上v1）
|
||||
v2.speed = 15.0;
|
||||
Logger::debug(
|
||||
"同向追尾测试: ",
|
||||
"v1=(", v1.position.x, ",", v1.position.y, "), speed=", v1.speed,
|
||||
", v2=(", v2.position.x, ",", v2.position.y, "), speed=", v2.speed,
|
||||
", 相对速度=", v2.speed - v1.speed,
|
||||
", 初始距离=", std::sqrt(
|
||||
std::pow(v2.position.x - v1.position.x, 2) +
|
||||
std::pow(v2.position.y - v1.position.y, 2)
|
||||
)
|
||||
);
|
||||
collisionResult = detector_->checkCollision(v1, v2, 30.0);
|
||||
Logger::debug(
|
||||
"碰撞检测结果: willCollide=", collisionResult.willCollide,
|
||||
", timeToCollision=", collisionResult.timeToCollision,
|
||||
", minDistance=", collisionResult.minDistance,
|
||||
", type=", static_cast<int>(collisionResult.type)
|
||||
);
|
||||
EXPECT_TRUE(collisionResult.willCollide) << "同向但速度较快的车辆追上前车时应该检测到碰撞";
|
||||
|
||||
// 测试3：同向不同速但距离较远
|
||||
v2.position = {0.0, 100.0}; // 增加到100米距离
|
||||
v2.position = {0.0, 100.0}; // 在原点
|
||||
v2.speed = 11.0; // 减小速度差，从 15m/s 改为 11m/s
|
||||
collisionResult = detector_->checkCollision(v1, v2, 30.0);
|
||||
EXPECT_FALSE(collisionResult.willCollide) << "距离较远时不应该检测到碰撞";
|
||||
EXPECT_FALSE(collisionResult.willCollide) << "相对速度小且距离较远时不应该检测到碰撞";
|
||||
}
|
||||
|
||||
// 测试航空器与静止车辆的碰撞检测
|
||||
@ -336,8 +299,8 @@ TEST_F(CollisionDetectorTest, AircraftStationaryVehicleCollision) {
|
||||
auto collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0);
|
||||
EXPECT_TRUE(collisionResult.willCollide) << "航空器接近静止车辆时应该检测到碰撞";
|
||||
|
||||
// 测试2：静止车辆在航空器航向偏离处
|
||||
vehicle.position = {200.0, 170.0}; // 在航空器前方偏北，距离约100米（大于安全距离75米）
|
||||
// 测试2<EFBFBD><EFBFBD><EFBFBD>静止车辆在航空器航向偏离处
|
||||
vehicle.position = {200.0, 200.0}; // 在航空器前方偏北，距离约100米（大于安全距离75米）
|
||||
collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0);
|
||||
EXPECT_FALSE(collisionResult.willCollide) << "航空器与不在航向上的静止车辆不应该检测到碰撞";
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user