基于Webots官方实例moose，自定义dem三维地图以及对路径规划节点的行进仿真实例

Webots官方实例moose，自定义地图以及对路径规划节点的行进仿真实例

• 引言
• 一、环境配置
• 二、自定义构建*DEM*地图
• • 2.1 *DEM* 构建原理
  • 2.2 数据保存与可视化
  • 2.3 技术要点总结
  • 2.4 建图代码
• 三、路径规划算法
• • 3.1 数据预处理与障碍物定义
  • 3.2 A* 算法实现核心逻辑
  • 3.3 路径输出与格式转换
  • 3.4 路径规划代码
• 四、Webots仿真实验步骤
• • 4.1.1 Webots官方实例
  • 4.1.2 修改原始的DEM数据
  • 4.1.3 修改控制器路径节点数据

引言

在本项目中，我们以Webots仿真平台为基础，构建了一个完整的路径规划与仿真验证流程。首先，通过Python语言加载并处理自定义的DEM（数字高程模型）地图，用以还原具有真实地形起伏的仿真环境。在此地图基础上，结合A*算法进行路径规划，同时支持用户替换为自定义的路径规划算法，以生成可行的路径节点序列。最终，将生成的路径节点与DEM地图共同导入Webots仿真环境，驱动虚拟代理（如Moose机器人）沿规划路径进行自主导航，从而完成路径规划算法在实际地形条件下的可视化验证与效果分析。具体的效果如下所示：

moose小车路径规划行进实例（复杂地形，野外环境）

视频中展示了Webots仿真环境中虚拟机器人（Moose）按照规划路径顺利完成任务点访问的全过程。本示例适用于复杂地形环境（野外环境）下的任务调度、目标巡检等场景，为路径规划（路径跟踪）算法提供了清晰直观的技术验证方式。

一、环境配置

Windows， Webots 2023b， Python 3.8， Vscode， Pycharm

二、自定义构建DEM地图

在路径规划与地形仿真任务中，数字高程模型（DEM，Digital Elevation Model） 是用于描述地形起伏变化的重要基础数据。在本项目中，我们使用 Python 编程语言对 DEM 地图进行了自定义构建与可视化，构建过程结合了二维高斯函数模拟山丘地貌，生成具有真实地形特征的高度场。

2.1 DEM 构建原理

为了模拟复杂地形，我们采用了二维高斯函数生成多个山丘模型。这些山丘的叠加结果构成了整个 DEM 地形数据。具体而言，每个山丘由函数 generate_hill 生成，该函数通过设置基础高度 base、山丘最大高度 height、山丘中心位置 (x0, y0) 以及扩散系数 sigma 来控制山体的形态和分布。高斯函数能够模拟自然地形中常见的缓坡、圆顶丘陵等特征，使地图更贴近现实。

为了构建复杂地貌，我们在不同位置叠加了若干大小不同、形态各异的山丘，例如中心的大型山丘、小尺度的山包以及边缘的宽缓丘陵等。最终使用 np.maximum.reduce() 将这些山丘合并，得到一个尺寸为 50x50 的二维整数数组，表示每个坐标点上的地形高度。

2.2 数据保存与可视化

构建完成的 DEM 数据被同时保存为 .txt 文件和 .xlsx Excel 表格，便于在不同平台进行读取与后续处理。保存格式采用逗号分隔，保证通用性和可扩展性。

为了便于对地形直观理解，我们还实现了一个三维可视化函数 visualize_3d()。该函数基于 Matplotlib 的 3D 绘图功能，生成带有真实高差变化的三维曲面图。曲面图采用 terrain 配色方案，可以直观反映地形高低起伏，并在图中标注 X、Y、Z 三轴以及高度颜色条。通过这个可视化工具，我们能够在仿真运行前直观了解 DEM 地形是否符合规划与仿真需求。

2.3 技术要点总结

1. 使用二维高斯函数灵活模拟多样山丘地貌；
2. 高程数据支持导出为 .txt 和 .xlsx 格式，便于集成至其他模块；
3. 支持 DEM 地形的三维可视化，辅助算法开发与仿真验证；
4. 地图尺寸、山丘数量及分布均可自由配置，具有良好的通用性和拓展性。

2.4 建图代码

通过这种自定义 DEM 地图的构建方式，我们为后续路径规划算法的实验提供了可控、可视化的地形输入，有效提升了系统的测试性与仿真真实度。

import pandas as pd
import numpy as np
import matplotlib.pyplot as plt
# 生成山的函数
def generate_hill(base, height, x0, y0, sigma):
    """
    生成一个二维高斯形状的山丘（高程），用于模拟DEM地形数据。
    参数说明：
    - base:   地形的基础高度（整个地形的底部高度）。
    - height: 山丘的最大高度（相对 base 的增量）。
    - x0:     山丘中心在 x 轴的位置。
    - y0:     山丘中心在 y 轴的位置。
    - sigma:  控制山丘“宽度”或“扩散程度”，数值越大，山越宽、越平缓。
    返回：
    - 一个 50 * 50 的二维数组，表示包含该山丘的高程数据。
    """
    x, y = np.meshgrid(np.arange(50), np.arange(50))
    hill = height * np.exp(-((x - x0)**2 + (y - y0)**2) / (2 * sigma**2))
    return base + hill
# 生成DEM
base = 0
hill_big = generate_hill(base, 8, 3, 8, 3)
hill_small1 = generate_hill(base, 4, 15, 2, 2)
hill_small2 = generate_hill(base, 5, 17, 17, 2)
hill_small3 = generate_hill(base, 2, 5, 19, 2)
hill_small4 = generate_hill(base, 8, 19, 19, 9)
hill_small5 = generat