无人机规划仿真项目场景生成机制深度分析

本文档详细分析五个知名无人机规划项目的训练/仿真场景生成机制。

---

目录

1. 项目概览
2. EGO-Planner 场景生成
3. EGO-Planner-Swarm 场景生成
4. GCOPTER 场景生成
5. RACER 场景生成
6. SUPER 场景生成
7. 场景生成技术对比总结
8. 核心算法详解

---

1. 项目概览

项目	主要用途	场景生成方式
EGO-Planner	单机实时局部规划	随机森林、圆环障碍、Perlin噪声
EGO-Planner-Swarm	集群协同规划	随机森林、圆环障碍、Perlin噪声
GCOPTER	全局最优轨迹规划	Perlin噪声、迷宫、3D迷宫
RACER	集群自主探索	随机森林、PCD文件加载、点击生成
SUPER	高速敏捷飞行	PCD地图加载、OpenGL深度渲染

---

2. EGO-Planner 场景生成

2.1 场景生成架构

EGO-Planner 提供两种场景生成方式：

场景生成
├── map_generator (random_forest_sensing.cpp)
│   ├── 随机圆柱体障碍
│   └── 随机圆环障碍
└── mockamap (maps.cpp)
    ├── Perlin噪声3D地图
    ├── 随机障碍物地图
    ├── 2D迷宫地图
    └── 3D迷宫地图

2.2 随机森林生成 (random_forest_sensing.cpp)

核心函数：RandomMapGenerateCylinder()

功能：生成随机分布的圆柱形障碍物和圆环障碍物

算法流程：

// 1. 初始化随机分布器
rand_x = uniform_real_distribution<double>(_x_l, _x_h);  // X轴范围
rand_y = uniform_real_distribution<double>(_y_l, _y_h);  // Y轴范围
rand_w = uniform_real_distribution<double>(_w_l, _w_h);  // 宽度范围
rand_h = uniform_real_distribution<double>(_h_l, _h_h);  // 高度范围

// 2. 生成圆柱形障碍物
for (int i = 0; i < _obs_num; i++) {
    // 随机生成位置
    x = rand_x(eng);
    y = rand_y(eng);
    w = rand_w(eng);
    
    // 避开起点和终点区域
    if (sqrt(pow(x - _init_x, 2) + pow(y - _init_y, 2)) < 2.0) continue;
    if (sqrt(pow(x - 19.0, 2) + pow(y - 0.0, 2)) < 2.0) continue;
    
    // 最小距离约束
    for (auto p : obs_position)
        if ((Eigen::Vector2d(x,y) - p).norm() < _min_dist) continue;
    
    // 栅格化圆柱体
    double radius = (w * inf) / 2;
    for (int r = -widNum/2; r < widNum/2; r++)
        for (int s = -widNum/2; s < widNum/2; s++)
            for (int t = -30; t < heiNum; t++) {
                if ((Eigen::Vector2d(temp_x,temp_y) - Eigen::Vector2d(x,y)).norm() <= radius) {
                    pt_random.x = temp_x;
                    pt_random.y = temp_y;
                    pt_random.z = temp_z;
                    cloudMap.points.push_back(pt_random);
                }
            }
}

// 3. 生成圆环障碍物
for (int i = 0; i < circle_num_; ++i) {
    // 随机旋转矩阵
    Eigen::Matrix3d rotate;
    rotate << cos(theta), -sin(theta), 0.0, 
              sin(theta), cos(theta), 0.0, 
              0, 0, 1;
    
    // 参数化圆环
    for (double angle = 0.0; angle < 6.282; angle += _resolution / 2) {
        cpt(0) = 0.0;
        cpt(1) = radius1 * cos(angle);
        cpt(2) = radius2 * sin(angle);
        cpt_if = rotate * cpt + Eigen::Vector3d(x, y, z);
        cloudMap.push_back(pt_random);
    }
}

关键参数配置：

参数	说明	典型值
map/x_size	地图X方向尺寸(m)	40.0
map/y_size	地图Y方向尺寸(m)	40.0
map/z_size	地图Z方向尺寸(m)	3.0
map/obs_num	圆柱障碍物数量	200
map/circle_num	圆环障碍物数量	200
map/resolution	点云分辨率(m)	0.1
ObstacleShape/lower_rad	障碍物最小半径	0.5
ObstacleShape/upper_rad	障碍物最大半径	0.7
min_distance	障碍物最小间距	1.2

2.3 Mockamap 场景生成

2.3.1 Perlin 噪声 3D 地图

Perlin噪声是一种梯度噪声算法，广泛用于程序化地形生成。

核心实现：

void Maps::perlin3D() {
    // 配置参数
    nh_private->param("complexity", complexity, 0.142857);  // 噪声频率
    nh_private->param("fill", fill, 0.38);                  // 填充比例
    nh_private->param("fractal", fractal, 1);               // 分形层数
    nh_private->param("attenuation", attenuation, 0.5);     // 衰减系数

    PerlinNoise noise(info.seed);  // 使用种子初始化

    // 遍历所有体素
    for (int i = 0; i < info.sizeX; ++i)
        for (int j = 0; j < info.sizeY; ++j)
            for (int k = 0; k < info.sizeZ; ++k) {
                double tnoise = 0;
                // 分形叠加
                for (int it = 1; it <= fractal; ++it) {
                    int dfv = pow(2, it);
                    double ta = attenuation / it;
                    tnoise += ta * noise.noise(dfv * i * complexity,
                                               dfv * j * complexity,
                                               dfv * k * complexity);
                }
                v->push_back(tnoise);
            }

    // 根据填充比例确定阈值
    std::sort(v->begin(), v->end());
    int tpos = info.cloud->width * (1 - fill);
    double threshold = v->at(tpos);

    // 超过阈值的点作为障碍物
    if (tnoise > threshold) {
        info.cloud->points[pos].x = i / info.scale - info.sizeX / (2 * info.scale);
        info.cloud->points[pos].y = j / info.scale - info.sizeY / (2 * info.scale);
        info.cloud->points[pos].z = k / info.scale;
    }
}

Perlin噪声参数影响：

参数	低值效果	高值效果
complexity	大块连续区域	细碎复杂结构
fill	稀疏障碍	密集障碍
fractal	平滑表面	丰富细节
attenuation	细节突出	细节平滑

2.3.2 递归分割迷宫生成

算法原理：递归分割法（Recursive Division）

void Maps::recursiveDivision(int xl, int xh, int yl, int yh, Eigen::MatrixXi& maze) {
    // 基本情况：区域太小
    if (xh - xl < 3 || yh - yl < 3) return;
    
    // 随机选择分割点
    int xm = rand() % (xh - xl - 1) + xl + 1;
    int ym = rand() % (yh - yl - 1) + yl + 1;
    
    // 添加墙壁
    for (int i = xl; i <= xh; i++) maze(i, ym) = 1;
    for (int j = yl; j <= yh; j++) maze(xm, j) = 1;
    
    // 随机选择三面墙开门（保证连通性）
    int decision = rand() % 4;
    switch (decision) {
        case 0: maze(d1, ym) = 0; maze(d2, ym) = 0; maze(xm, d3) = 0; break;
        case 1: maze(d1, ym) = 0; maze(d2, ym) = 0; maze(xm, d4) = 0; break;
        // ...
    }
    
    // 递归处理四个子区域
    recursiveDivision(xl, xm-1, yl, ym-1, maze);
    recursiveDivision(xm+1, xh, yl, ym-1, maze);
    recursiveDivision(xl, xm-1, ym+1, yh, maze);
    recursiveDivision(xm+1, xh, ym+1, yh, maze);
}

2.3.3 Voronoi 3D迷宫生成

算法原理：基于Voronoi图的3D迷宫

void Maps::Maze3DGen() {
    // 随机生成核心点
    for (int i = 0; i < numNodes; i++) {
        pcl::PointXYZ pt_random;
        pt_random.x = rand() % info.sizeX / info.scale - info.sizeX / (2 * info.scale);
        pt_random.y = rand() % info.sizeY / info.scale - info.sizeY / (2 * info.scale);
        pt_random.z = rand() % info.sizeZ / info.scale - info.sizeZ / (2 * info.scale);
        base.push_back(pt_random);
    }

    // 遍历所有点，计算到最近两个核心的距离
    for (int i = 0; i < info.sizeX; i++)
        for (int j = 0; j < info.sizeY; j++)
            for (int k = 0; k < info.sizeZ; k++) {
                // 找到最近的两个核心点
                // 如果该点在Voronoi边界上（到两个核心距离相近）
                if (abs(mp.getDist2() - mp.getDist1()) < 1 / info.scale) {
                    // 根据连通性参数决定是否放置障碍
                    if ((mp.getPoint1() + mp.getPoint2()) > int((1-connectivity)*numNodes) &&
                        (mp.getPoint1() + mp.getPoint2()) < int((1+connectivity)*numNodes)) {
                        // 添加带孔洞的墙
                    } else {
                        info.cloud->points.push_back(mp.getPoint());
                    }
                }
            }
}

2.4 传感器仿真

深度渲染节点 (pointcloud_render_node.cpp)：

void renderSensedPoints(const ros::TimerEvent& event) {
    // 基于KD树的范围搜索
    if (_kdtreeLocalMap.radiusSearch(searchPoint, sensing_horizon,
                                     _pointIdxRadiusSearch,
                                     _pointRadiusSquaredDistance) > 0) {
        for (size_t i = 0; i < _pointIdxRadiusSearch.size(); ++i) {
            pt = _cloud_all_map.points[_pointIdxRadiusSearch[i]];
            
            // 垂直视场约束
            if ((fabs(pt.z - _odom.pose.pose.position.z) / sensing_horizon) > tan(M_PI / 6.0))
                continue;
            
            // 水平视场约束（基于偏航方向）
            if (pt_vec.normalized().dot(yaw_vec) < 0.5) continue;
            
            _local_map.points.push_back(pt);
        }
    }
}

---

3. EGO-Planner-Swarm 场景生成

EGO-Planner-Swarm 的场景生成机制与 EGO-Planner 基本一致，主要区别在于：

3.1 多机协同扩展

• 支持多无人机起点设置
• 共享全局地图
• 独立的局部感知

3.2 场景配置示例

<!-- swarm_simulator.xml -->
<node pkg="mockamap" type="mockamap_node" name="mockamap_node">
    <param name="seed" type="int" value="127"/>
    <param name="type" type="int" value="1"/>  <!-- Perlin噪声 -->
    <param name="complexity" type="double" value="0.05"/>
    <param name="fill" type="double" value="0.12"/>
</node>

---

4. GCOPTER 场景生成

4.1 架构特点

GCOPTER 使用 mockamap 生成场景，但其规划器是离线全局规划器，特点：

• 一次性加载整个地图
• 使用体素地图(VoxelMap)表示
• 支持地图膨胀处理

4.2 地图加载流程

// global_planning.cpp
inline void mapCallBack(const sensor_msgs::PointCloud2::ConstPtr &msg) {
    if (!mapInitialized) {
        // 解析点云数据
        float *fdata = (float *)(&msg->data[0]);
        for (size_t i = 0; i < total; i++) {
            voxelMap.setOccupied(Eigen::Vector3d(fdata[cur + 0],
                                                  fdata[cur + 1],
                                                  fdata[cur + 2]));
        }
        
        // 障碍物膨胀
        voxelMap.dilate(std::ceil(config.dilateRadius / voxelMap.getScale()));
        
        mapInitialized = true;
    }
}

4.3 支持的场景类型

类型	参数	描述
1	type=1	Perlin噪声3D地图
2	type=2	随机障碍物地图
3	type=3	2D迷宫地图
4	type=4	3D Voronoi迷宫

---

5. RACER 场景生成

5.1 场景生成方式

RACER 项目支持三种场景生成方式：

RACER 场景生成
├── random_forest_sensing.cpp  (随机生成)
├── click_map.cpp              (交互式生成)
└── PCD文件加载                (预置场景)

5.2 随机森林生成（增强版）

相比EGO-Planner，RACER的随机森林生成增加了地面平面：

void RandomMapGenerate() {
    // ... 障碍物生成代码 ...
    
    // 添加地面平面
    for (double x = min_x_; x < max_x_; x += _resolution) {
        for (double y = min_y_; y < max_y_; y += _resolution) {
            pt_random.x = x;
            pt_random.y = y;
            pt_random.z = -0.1;  // 地面高度
            cloudMap.push_back(pt_random);
        }
    }
}

5.3 交互式墙体生成 (click_map.cpp)

通过RViz点击生成墙体障碍：

void clickCallback(const geometry_msgs::PoseStamped& msg) {
    points_.push_back(Eigen::Vector3d(x, y, 0));
    if (points_.size() < 2) return;

    // 使用两点生成墙体
    Eigen::Vector3d p1 = points_[0];
    Eigen::Vector3d p2 = points_[1];
    Eigen::Vector3d dir1 = (p2 - p1).normalized();
    double len = (p2 - p1).norm();
    
    // 垂直于墙体方向
    Eigen::Vector3d dir2;
    dir2[0] = -dir1[1];
    dir2[1] = dir1[0];

    // 生成墙体点云
    for (double l1 = 0.0; l1 <= len; l1 += 0.1) {
        for (double l2 = -len2_; l2 <= len2_; l2 += 0.1) {
            for (double h = -0.5; h < 2.5; h += 0.1) {
                pt_random.x = tmp2[0];
                pt_random.y = tmp2[1];
                pt_random.z = h;
                map_cloud_.push_back(pt_random);
            }
        }
    }
}

5.4 预置PCD场景

RACER 提供预置的室内场景PCD文件：

文件	描述
office.pcd	办公室场景
office2.pcd	办公室场景变体2
office3.pcd	办公室场景变体3
pillar.pcd	柱子场景

5.5 SDF地图构建

RACER 使用 概率占据栅格 + ESDF 进行地图表示：

void SDFMap::initMap(ros::NodeHandle& nh) {
    // 概率更新参数
    nh.param("sdf_map/p_hit", mp_->p_hit_, 0.70);   // 命中概率
    nh.param("sdf_map/p_miss", mp_->p_miss_, 0.35); // 未命中概率
    nh.param("sdf_map/p_min", mp_->p_min_, 0.12);   // 最小概率
    nh.param("sdf_map/p_max", mp_->p_max_, 0.97);   // 最大概率
    nh.param("sdf_map/p_occ", mp_->p_occ_, 0.80);   // 占据阈值
    
    // 对数概率转换
    mp_->prob_hit_log_ = logit(mp_->p_hit_);
    mp_->prob_miss_log_ = logit(mp_->p_miss_);
}

---

6. SUPER 场景生成

6.1 架构特点

SUPER 是最先进的项目，采用 PCD地图加载 + OpenGL GPU渲染 的方式：

SUPER 仿真架构
├── PCD地图加载 (ROGMap)
├── OpenGL深度渲染 (marsim_render)
│   ├── 多种LiDAR模式
│   │   ├── AVIA (大疆Livox)
│   │   ├── GENERAL_360 (通用360°)
│   │   └── MID_360 (Livox MID-360)
│   └── GPU加速渲染
└── 完美无人机仿真 (perfect_drone_sim)

6.2 PCD地图加载

// rog_map.cpp
void ROGMap::init() {
    if (cfg_.load_pcd_en) {
        string pcd_path = cfg_.pcd_name;
        PointCloud::Ptr pcd_map(new PointCloud);
        
        if (pcl::io::loadPCDFile(pcd_path, *pcd_map) == -1) {
            cout << "Load pcd file failed!" << endl;
            exit(-1);
        }
        
        // 更新占据地图
        updateOccPointCloud(*pcd_map);
        
        // 更新ESDF
        if(cfg_.esdf_en) {
            esdf_map_->updateESDF3D(robot_state_.p);
        }
    }
}

6.3 预置场景

文件	描述	场景类型
random_map_150.pcd	150点随机场景	稀疏
random_map_50.pcd	50点随机场景	极稀疏
random_map_2_26609.pcd	26609点随机场景	密集
random_map_24_6635.pcd	6635点随机场景	中等密集

6.4 OpenGL LiDAR仿真 (marsim_render)

核心渲染流程

void MarsimRender::render_pointcloud(
    const Eigen::Vector3f& camera_pos,
    const Eigen::Quaternionf& camera_q,
    const decimal_t& t_pattern_start,
    pcl::PointCloud<PointType>::Ptr output_pointcloud) {
    
    // 1. 根据LiDAR类型填充扫描模式矩阵
    if (cfg_.lidar_type == AVIA) {
        // Livox AVIA 非重复扫描模式
        decimal_t w1 = 763.82589;   // 旋转速度1
        decimal_t w2 = -488.41293;  // 旋转速度2
        for (decimal_t t_i = t_start; t_i < t_start + t_duration; t_i += point_duration) {
            int x = round(scale_x * (cos(w1*t_i) + cos(w2*t_i))) + cfg_.width/2;
            int y = round(scale_y * (sin(w1*t_i) + sin(w2*t_i))) + cfg_.height/2;
            pattern_matrix(x, y) = 2;
        }
    }
    else if (cfg_.lidar_type == GENERAL_360) {
        // 通用360° LiDAR (128线)
        double step = static_cast<double>(cfg_.height) / 128.0;
        for (size_t i = 0; i < 128; i++) {
            int y = round(i * step + random_offset);
            for (int j = 0; j < cfg_.width; j++) {
                pattern_matrix(j, y) = 2;
            }
        }
    }
    
    // 2. OpenGL渲染设置
    view = glm::lookAt(cameraPos, cameraPos + cameraFront, cameraUp);
    projection = glm::perspective(atan(cfg_.width / (2*cfg_.fy)) * 2,
                                  cfg_.width / cfg_.height,
                                  cfg_.sensing_blind,
                                  cfg_.sensing_horizon);
    
    // 3. GPU渲染点云
    glDrawElements(GL_POINTS, points_index_infov.size(), GL_UNSIGNED_INT, 0);
    
    // 4. 读取深度缓冲并转换为点云
    glReadPixels(0, 0, cfg_.width, cfg_.height, GL_DEPTH_COMPONENT, GL_FLOAT, mypixels.data());
    depth_to_pointcloud(depth_image, output_pointcloud, depth_ptcloud_vec);
}

深度图转点云

void MarsimRender::depth_to_pointcloud(cv::Mat& depth_image, 
                                        pcl::PointCloud<PointType>::Ptr origin_cloud,
                                        vec_E<Vec3f>& depth_ptcloud_vec) {
    const auto polar_resolution_rad = cfg_.polar_resolution / 180.0 * M_PI;
    
    for (int v = 0; v < cfg_.height; v++) {
        for (int u = 0; u < cfg_.width; u++) {
            if (pattern_matrix(u, cfg_.height-1-v) > 0) {
                float depth = depth_image.at<float>(v, u);
                
                if (depth > cfg_.sensing_blind && depth < cfg_.sensing_horizon) {
                    // 球面坐标转直角坐标
                    Eigen::Vector3f temp_point;
                    temp_point(0) = depth * cos(polar_resolution_rad * (v-v0)) 
                                         * sin(polar_resolution_rad * (u-u0));
                    temp_point(1) = -depth * sin(polar_resolution_rad * (v-v0));
                    temp_point(2) = -depth * cos(polar_resolution_rad * (v-v0)) 
                                          * cos(polar_resolution_rad * (u-u0));
                    
                    // 转换到世界坐标系
                    temp_point_world = camera2world * temp_point + camera;
                    origin_cloud->points.push_back(temp_pclpt);
                }
            }
        }
    }
}

6.5 场景配置示例

# dense.yaml
pcd_name: "random_map_2_26609.pcd"
init_position:
  x: 0
  y: -50
  z: 1.5
init_yaw: 1.5741

# LiDAR配置
is_360lidar: true
polar_resolution: 0.4
downsample_res: 0.1
vertical_fov: 178.0
sensing_blind: 0.1
sensing_horizon: 15
sensing_rate: 10
lidar_type: 2  # GENERAL_360

---

7. 场景生成技术对比总结

7.1 生成方式对比

项目	随机生成	Perlin噪声	迷宫	PCD加载	交互式	GPU渲染
EGO-Planner	✓	✓	✓	✗	✗	✗
EGO-Planner-Swarm	✓	✓	✓	✗	✗	✗
GCOPTER	✓	✓	✓	✗	✗	✗
RACER	✓	✗	✗	✓	✓	✗
SUPER	✗	✗	✗	✓	✗	✓

7.2 传感器仿真对比

项目	传感器类型	渲染方式	实时性
EGO-Planner	点云	CPU KD树	中等
EGO-Planner-Swarm	点云	CPU KD树	中等
GCOPTER	无(离线)	-	-
RACER	点云	CPU KD树	中等
SUPER	多LiDAR	GPU OpenGL	高

7.3 地图表示对比

项目	地图格式	膨胀处理	距离场
EGO-Planner	点云→栅格	✓	ESDF
EGO-Planner-Swarm	点云→栅格	✓	ESDF
GCOPTER	VoxelMap	✓	✗
RACER	概率栅格	✓	SDF/ESDF
SUPER	ROGMap	✓	ESDF

---

8. 核心算法详解

8.1 Perlin噪声算法

Perlin噪声是Ken Perlin于1983年发明的梯度噪声算法：

核心步骤：

1. 将空间划分为网格
2. 每个网格顶点分配一个随机梯度向量
3. 对于任意点，找到包含它的网格单元
4. 计算该点到各顶点的距离向量与梯度的点积
5. 使用平滑插值函数（通常是 6t^5 - 15t^4 + 10t^3）混合结果

double PerlinNoise::noise(double x, double y, double z) {
    // 找到包含点的单元格
    int X = (int)floor(x) & 255;
    int Y = (int)floor(y) & 255;
    int Z = (int)floor(z) & 255;
    
    // 相对位置
    x -= floor(x);
    y -= floor(y);
    z -= floor(z);
    
    // 平滑曲线
    double u = fade(x);
    double v = fade(y);
    double w = fade(z);
    
    // 哈希坐标
    int A = p[X] + Y, AA = p[A] + Z, AB = p[A+1] + Z;
    int B = p[X+1] + Y, BA = p[B] + Z, BB = p[B+1] + Z;
    
    // 三线性插值
    return lerp(w, lerp(v, lerp(u, grad(p[AA], x, y, z),
                                   grad(p[BA], x-1, y, z)),
                           lerp(u, grad(p[AB], x, y-1, z),
                                   grad(p[BB], x-1, y-1, z))),
                   lerp(v, lerp(u, grad(p[AA+1], x, y, z-1),
                                   grad(p[BA+1], x-1, y, z-1)),
                           lerp(u, grad(p[AB+1], x, y-1, z-1),
                                   grad(p[BB+1], x-1, y-1, z-1))));
}

8.2 分形噪声叠加 (fBm)

分形布朗运动通过叠加不同频率的噪声产生自然纹理：

double fractalNoise(double x, double y, double z, int octaves, double attenuation) {
    double result = 0.0;
    double amplitude = 1.0;
    double frequency = 1.0;
    
    for (int i = 0; i < octaves; i++) {
        result += amplitude * noise(x * frequency, y * frequency, z * frequency);
        amplitude *= attenuation;  // 振幅衰减
        frequency *= 2.0;          // 频率倍增
    }
    
    return result;
}

8.3 ESDF计算

欧几里得符号距离场(ESDF)使用动态规划计算：

template <typename F_get_val, typename F_set_val>
void SDFMap::fillESDF(F_get_val f_get_val, F_set_val f_set_val, int start, int end, int dim) {
    int v[map_voxel_num_(dim)];
    double z[map_voxel_num_(dim) + 1];

    int k = start;
    v[start] = start;
    z[start] = -std::numeric_limits<double>::max();
    z[start + 1] = std::numeric_limits<double>::max();

    // 正向扫描
    for (int q = start + 1; q <= end; q++) {
        k++;
        double s;
        do {
            k--;
            s = ((f_get_val(q) + q*q) - (f_get_val(v[k]) + v[k]*v[k])) / (2*q - 2*v[k]);
        } while (s <= z[k]);

        k++;
        v[k] = q;
        z[k] = s;
        z[k + 1] = std::numeric_limits<double>::max();
    }

    // 反向填充
    k = start;
    for (int q = start; q <= end; q++) {
        while (z[k + 1] < q) k++;
        double val = (q - v[k]) * (q - v[k]) + f_get_val(v[k]);
        f_set_val(q, val);
    }
}

---

总结

本文档详细分析了五个无人机规划项目的场景生成机制：

1. EGO-Planner/Swarm 使用经典的随机障碍生成和Perlin噪声
2. GCOPTER 复用mockamap但专注于离线规划
3. RACER 增加了交互式生成和PCD加载能力
4. SUPER 采用最先进的GPU渲染仿真，支持多种LiDAR模式

这些项目展示了从简单随机生成到复杂GPU渲染的技术演进，为无人机规划算法的测试和验证提供了完整的仿真环境支持。