企业官网建设流程全解析

简介：这份资源面向无人机自主飞行算法方向的研究者与爱好者，聚焦在Gazebo仿真平台上跑通EGO-Planner的完整实践流程，帮助解决从环境搭建到算法测试中反复踩坑的问题。压缩包共2个文件，以inscode工程配置与html说明页为主，整体仅4KB，属于轻量级引导型资料，需配合描述中提到的ROS、Gazebo、MAVROS、PX4及百度云盘源码包使用。内容覆盖软件安装、算法配置、仿真启动与实测案例，并针对无人机高度控制不稳、障碍物碰撞、速度过快导致失控等常见现象给出调试思路与改进方向，适合初学者快速建立仿真测试框架。目前已有555人学习下载，可作为EGO-Planner仿真入门的参考起点。

1. 从一次“规划器原地打转”说起：ego planner 仿真到底在解决什么

如果你在移动机器人或无人机方向做过一阵子，大概率遇到过这种场面：算法在纸面上跑得通，一放进仿真，轨迹要么贴着障碍物边缘反复横跳，要么在狭窄通道口直接停住不动，日志里也看不出明显报错。ego planner 这类基于梯度的局部规划器，核心卖点就是在动态、拥挤环境里实时生成一条平滑、无碰撞、动力学可行的轨迹。而“仿真教程”这四个字，真正要解决的不是“怎么把代码跑起来”这么简单，而是让你在一个可控、可复现的环境里，把规划器的输入输出、参数敏感度、失败边界全部摸清楚。

这篇笔记面向三类人：刚接触局部规划、想跑通第一个可运行源码的新手；已经用过类似规划器、但被参数和坐标系折磨过的熟手；以及准备把仿真结果往真机迁移、想知道哪些坑必须提前踩的工程师。我会按“先立住概念 → 再搭环境 → 再调参 → 再排错 → 最后讲验证技巧”的顺序展开，中间所有步骤都尽量落到可抄作业的命令和代码块上。你不需要先成为优化理论专家，但需要愿意动手改参数、看日志、对比轨迹。

2. 把 ego planner 仿真环境搭起来：从依赖到第一个可运行源码

2.1 先想清楚：为什么局部规划器一定要在仿真里先跑

ego planner 这类方法通常把轨迹表示成一系列控制点，然后用梯度下降去优化一个包含平滑项、障碍物项、动力学可行性项的目标函数。它在真实环境里最怕两件事：一是感知噪声导致障碍物项梯度方向突变，二是局部最优导致轨迹卡在“鞍点”附近。仿真环境的最大价值，是让你能反复注入同样的初始条件，观察规划器在相同输入下是否稳定复现同一条轨迹。如果仿真里都时好时坏，真机上只会更糟。

常见做法是选一个轻量级仿真器，比如基于 Python 的二维平面仿真，或者带物理引擎的三维环境。为了让你能直接复现，我这里用一个纯 Python + NumPy 的最小仿真框架来演示，不依赖任何重型机器人中间件。它的好处是：所有状态量、障碍物、梯度计算都暴露在你眼前，调参时不会有一层黑盒挡着。

2.2 最小可运行源码：一个二维 ego planner 仿真骨架

下面这段代码实现了一个简化版 ego planner：用五次多项式生成初始轨迹，用障碍物斥力项和光滑项做梯度下降。你可以直接复制到一个.py文件里运行。

import numpy as np import matplotlib.pyplot as plt # 仿真参数 N = 30 # 轨迹控制点数量 dt = 0.1 # 时间步长 max_iter = 200 # 梯度下降迭代次数 lr = 0.05 # 学习率 w_smooth = 1.0 # 光滑项权重 w_obs = 50.0 # 障碍物项权重 w_dyn = 5.0 # 动力学可行性权重 v_max = 2.0 # 最大速度 a_max = 3.0 # 最大加速度 # 障碍物：圆形，格式 (x, y, radius) obstacles = np.array([ [3.0, 1.0, 0.8], [5.0, -1.0, 0.6], [7.0, 0.5, 0.7], ]) # 起点和终点 start = np.array([0.0, 0.0]) goal = np.array([10.0, 0.0]) def init_trajectory(start, goal, N): """用五次多项式生成初始轨迹""" t = np.linspace(0, 1, N) # 简单线性插值 + 平滑 x = start[0] + (goal[0] - start[0]) * (3*t**2 - 2*t**3) y = start[1] + (goal[1] - start[1]) * (3*t**2 - 2*t**3) return np.stack([x, y], axis=1) def smooth_cost(traj): """光滑项：相邻点加速度平方和""" acc = traj[2:] - 2*traj[1:-1] + traj[:-2] return np.sum(acc**2) def obstacle_cost(traj, obstacles): """障碍物项：对进入安全距离的点施加斥力""" cost = 0.0 grad = np.zeros_like(traj) safe_dist = 1.0 for obs in obstacles: diff = traj - obs[:2] dist = np.linalg.norm(diff, axis=1) for i in range(len(traj)): if dist[i] < safe_dist + obs[2]: # 斥力大小与距离成反比 cost += w_obs * (1.0 / (dist[i] - obs[2] + 1e-3)) grad[i] += w_obs * diff[i] / (dist[i] - obs[2] + 1e-3)**2 return cost, grad def dynamic_cost(traj, dt): """动力学可行性：速度、加速度超限惩罚""" vel = (traj[1:] - traj[:-1]) / dt acc = (vel[1:] - vel[:-1]) / dt cost = 0.0 grad = np.zeros_like(traj) # 速度惩罚 for i in range(len(vel)): speed = np.linalg.norm(vel[i]) if speed > v_max: cost += w_dyn * (speed - v_max)**2 grad[i] += w_dyn * 2 * (speed - v_max) * vel[i] / speed # 加速度惩罚 for i in range(len(acc)): acc_norm = np.linalg.norm(acc[i]) if acc_norm > a_max: cost += w_dyn * (acc_norm - a_max)**2 grad[i+1] += w_dyn * 2 * (acc_norm - a_max) * acc[i] / acc_norm return cost, grad def optimize(traj, obstacles, dt, max_iter, lr): """梯度下降主循环""" for it in range(max_iter): # 光滑项梯度 grad_smooth = np.zeros_like(traj) acc = traj[2:] - 2*traj[1:-1] + traj[:-2] grad_smooth[1:-1] = 2 * acc # 障碍物项 c_obs, grad_obs = obstacle_cost(traj, obstacles) # 动力学项 c_dyn, grad_dyn = dynamic_cost(traj, dt) # 总梯度 grad = w_smooth * grad_smooth + grad_obs + grad_dyn # 起点和终点固定 grad[0] = 0 grad[-1] = 0 traj -= lr * grad if it % 50 == 0: print(f"Iter {it}, obs_cost={c_obs:.2f}, dyn_cost={c_dyn:.2f}") return traj # 运行 traj = init_trajectory(start, goal, N) traj = optimize(traj, obstacles, dt, max_iter, lr) # 可视化 plt.figure(figsize=(10, 4)) plt.plot(traj[:, 0], traj[:, 1], 'b-', label='optimized trajectory') plt.plot(start[0], start[1], 'go', label='start') plt.plot(goal[0], goal[1], 'ro', label='goal') for obs in obstacles: circle = plt.Circle((obs[0], obs[1]), obs[2], color='k', alpha=0.3) plt.gca().add_patch(circle) plt.axis('equal') plt.legend() plt.grid(True) plt.show()

这段代码的逻辑很直白：init_trajectory用五次多项式给一条从起点到终点的平滑初始线；smooth_cost惩罚相邻控制点的二阶差分，让轨迹别抖；obstacle_cost对每个进入安全距离的控制点施加斥力，距离越近梯度越大；dynamic_cost检查速度和加速度是否超限，超了就拉回来。主循环里把三项梯度加权求和，起点终点梯度置零，然后做梯度下降。

参数说明：N越大轨迹越细腻，但计算量线性增长；lr太大会震荡，太小收敛慢；w_obs是最敏感的参数，设小了会撞障碍物，设大了会在障碍物附近“僵住”。我一般先用w_obs=50、w_smooth=1跑一遍，看轨迹是否贴障碍物，再微调。

2.3 跑通之后先别急着调参，先检查这三件事

第一，看轨迹是否满足起点和终点约束。如果终点漂了，说明梯度下降把终点也带偏了，检查grad[-1] = 0是否生效。第二，看障碍物项是否真的在起作用。把w_obs临时设为 0，如果轨迹直接穿过障碍物，说明障碍物项逻辑正确；如果设 0 后轨迹没变化，说明梯度没传进去。第三，看动力学项是否在“过度惩罚”。如果轨迹为了满足加速度约束而绕大弯，说明w_dyn太大，可以降到 1 左右再试。

提示：仿真里所有参数都是“相对”的。你换一个障碍物布局，最优参数可能完全不同。所以不要背参数，要理解每个权重在目标函数里代表什么。

3. ego planner 仿真的参数整定：从“能跑”到“跑得稳”

3.1 权重项怎么配：光滑、障碍物、动力学三者的拉扯

ego planner 的目标函数本质上是三个诉求的博弈：轨迹要平滑（光滑项）、要远离障碍物（障碍物项）、要符合机器人动力学（动力学项）。这三项没有绝对的最优比例，只有针对具体场景的平衡。我一般会按这个顺序调：

先固定w_smooth=1，把w_obs从 10 开始往上加，每次翻倍，观察轨迹与障碍物的最小距离。当最小距离稳定在安全距离的 1.2 倍左右时，记下这个w_obs。然后固定w_obs，把w_dyn从 0 开始加，直到轨迹的速度峰值不超过v_max的 1.1 倍。最后再回头微调w_smooth，如果轨迹出现高频抖动，就加大w_smooth；如果轨迹过于“僵硬”、绕不开障碍物，就减小它。

这里有一个血泪经验：w_obs和w_dyn会互相干扰。w_obs太大时，轨迹为了远离障碍物会急转弯，导致加速度超标，w_dyn再把轨迹拉回来，两者来回拉扯，轨迹就会在障碍物附近震荡。解决办法是给障碍物项加一个“衰减半径”，只在距离障碍物一定范围内才施加斥力，而不是全轨迹都算。

3.2 用表格对比不同参数组合下的轨迹表现

下面这张表是我在同一个障碍物布局下，用不同参数组合跑出来的结果。你可以把它当作调参的起点，但不要照搬。

从表里能看出，组合 A 的障碍物距离太近，实际部署有风险；组合 C 虽然安全，但轨迹震荡且速度被压得太低，通行效率差；组合 D 通过加大光滑项权重，在保持安全距离的同时抑制了震荡。我一般会从组合 D 附近开始，再根据具体场景微调。

3.3 迭代次数和学习率：别让规划器“想太多”或“走太急”

max_iter和lr是一对容易翻车的参数。max_iter太小，轨迹还没优化完就停了，障碍物项可能还没生效；太大，不仅浪费时间，还可能让轨迹在局部最优附近来回跳。我一般会先设max_iter=200，然后在日志里打印每 50 次迭代的障碍物代价。如果 150 次之后代价基本不变，说明已经收敛，可以降到 150；如果 200 次还在明显下降，就加到 300。

lr的坑更隐蔽。学习率太大时，轨迹会在障碍物边界上“弹跳”，因为梯度方向变化剧烈；太小时，迭代 200 次轨迹几乎没动。一个实用的技巧是：先用lr=0.01跑 50 次，看轨迹移动了多少；如果几乎没动，就乘 5；如果震荡，就除以 2。重复两三次就能找到合适的量级。

# 自适应学习率示例：根据梯度范数动态调整 grad_norm = np.linalg.norm(grad) if grad_norm > 10.0: lr = 0.01 elif grad_norm > 1.0: lr = 0.05 else: lr = 0.1 traj -= lr * grad

这段代码根据当前梯度范数切换学习率：梯度大时用小步长防止震荡，梯度小时用大步长加速收敛。你可以把它嵌到主循环里，替换原来的固定lr。

4. ego planner 仿真避坑：5 个让我熬夜排查的典型问题

4.1 轨迹在障碍物附近“原地打转”不前进

现象：规划器迭代了很多次，轨迹在障碍物前方反复弯曲，但始终不绕过障碍物。原因：障碍物斥力项和光滑项在障碍物边界附近形成了局部极小值，梯度下降卡住了。解决：给障碍物项加一个“切向分量”，或者用随机扰动跳出局部最优。更简单的做法是增大w_smooth，让轨迹更倾向于走直线，减少在障碍物附近的过度弯曲。

4.2 仿真里跑得好，换一组障碍物就撞上去

现象：同一套参数，在 A 布局下安全通过，在 B 布局下直接撞障碍物。原因：w_obs是全局固定值，但不同布局下障碍物对轨迹的“压迫感”不同。解决：把w_obs改成与障碍物距离相关的动态权重，距离越近权重越大。或者更直接：在仿真里随机生成 100 组障碍物布局，用同一套参数跑，统计碰撞率，再决定是否要调参。

4.3 轨迹速度曲线出现尖峰，真机执行时电机啸叫

现象：仿真轨迹看起来平滑，但速度曲线在某个点突然飙到v_max以上。原因：动力学项只惩罚了超限，但没有惩罚“速度突变”。解决：在动力学项里加入 jerk（加加速度）惩罚，或者对速度做低通滤波。我一般会在dynamic_cost里加一项：jerk = (acc[1:] - acc[:-1]) / dt，然后惩罚 jerk 的平方。

4.4 起点或终点被“拉走”，轨迹不闭合

现象：优化后的轨迹起点或终点偏离了设定值。原因：梯度下降时，起点和终点的梯度没有置零，或者置零逻辑写错了。解决：检查grad[0] = 0和grad[-1] = 0是否在每次迭代都执行。另外，如果起点终点本身就在障碍物安全距离内，障碍物项会把它们往外推，这时候需要把起点终点的障碍物代价排除。

4.5 迭代日志里代价一直下降，但轨迹肉眼可见地变差

现象：obs_cost和dyn_cost都在降，但轨迹却越来越奇怪。原因：总代价是加权和，某一项降了，另一项可能升了，但总代价在降。解决：不要只看总代价，要分别打印每一项的代价，并且每隔 50 次迭代把轨迹画出来看一眼。我习惯在仿真里加一个debug_plot开关，迭代过程中实时显示轨迹，比看数字直观得多。

注意：仿真里的“收敛”不等于“正确”。梯度下降只保证代价函数下降，不保证轨迹符合你的直觉。所以每次调参后，一定要可视化检查。

5. 让仿真结果更可信：验证方法与一个实用技巧

5.1 用蒙特卡洛式随机测试检验规划器鲁棒性

单次仿真跑通不代表规划器可靠。我一般会写一个批量测试脚本：随机生成 200 组障碍物布局，每组跑一次规划，记录是否碰撞、是否超速、迭代耗时。然后统计碰撞率和平均耗时。如果碰撞率超过 5%，说明参数还需要收紧；如果平均耗时超过 100ms，说明迭代次数或轨迹点数需要优化。

import random def random_obstacles(n, x_range, y_range, r_range): obs = [] for _ in range(n): x = random.uniform(*x_range) y = random.uniform(*y_range) r = random.uniform(*r_range) obs.append([x, y, r]) return np.array(obs) collision_count = 0 for trial in range(200): obstacles = random_obstacles(5, (2, 8), (-2, 2), (0.4, 0.9)) traj = init_trajectory(start, goal, N) traj = optimize(traj, obstacles, dt, max_iter, lr) # 检查碰撞 for obs in obstacles: dist = np.linalg.norm(traj - obs[:2], axis=1) if np.any(dist < obs[2]): collision_count += 1 break print(f"Collision rate: {collision_count / 200 * 100:.1f}%")

这段脚本随机生成障碍物，跑完规划后检查轨迹是否进入障碍物圆内。如果碰撞率偏高，你可以回到第 3 章调w_obs和w_smooth，再重新跑这个测试。这个循环我重复了不知道多少次，每次都能发现一些“单次仿真看不出来”的问题。

5.2 一个让我少熬很多夜的技巧：把轨迹和梯度场一起画出来

调参时最痛苦的是不知道梯度到底把轨迹往哪个方向推。我的习惯是：在每次迭代后，用quiver把当前轨迹点的梯度方向画出来，叠加在障碍物和轨迹上。这样一眼就能看出哪些点被障碍物推、哪些点被光滑项拉、哪些点梯度几乎为零（卡住了）。这个技巧帮我快速定位了多次“原地打转”的问题——原来梯度在障碍物边界上形成了闭环，轨迹点被推着绕圈。

# 在优化循环里加梯度可视化 if it % 50 == 0: plt.clf() plt.plot(traj[:, 0], traj[:, 1], 'b-') plt.quiver(traj[:, 0], traj[:, 1], -grad[:, 0], -grad[:, 1], color='r') for obs in obstacles: circle = plt.Circle((obs[0], obs[1]), obs[2], color='k', alpha=0.3) plt.gca().add_patch(circle) plt.axis('equal') plt.pause(0.1)

这段代码每 50 次迭代刷新一次画面，红色箭头表示梯度反方向（也就是轨迹移动方向）。如果箭头在某个区域形成漩涡，说明那里有局部极小值，需要调整权重或加扰动。

5.3 从仿真到真机：我踩过的最后一个坑

仿真里所有状态都是理想的，真机上你会遇到感知延迟、定位漂移、执行器响应滞后。我的教训是：仿真调参时，故意给障碍物位置加 5% 的随机噪声，给起点加 2cm 的偏移，再跑一遍。如果规划器在这种扰动下还能保持安全距离，那上真机的把握就大很多。另外，仿真里的dt要和真机控制周期一致，别用 0.01 的仿真步长去指导 0.1 的控制周期，否则速度曲线完全对不上。

希望这些步骤和坑能帮你把 ego planner 仿真真正跑稳，而不是停在“能出图”的阶段。

本文还有配套的精品资源，点击获取