4、全局路径规划A*算法：在非平坦地形下的代价函数设计
各位做机器人路径规划的朋友，今天我们来聊一个很实际的问题——A*算法在非平坦地形上怎么用。
标准的A*算法，大家都很熟悉了。它假设地面是平的，代价只跟距离有关。但现实世界哪有那么平坦？我做过一个项目，机器人在矿区执行巡检任务，地图上看起来直线距离很近，结果实际走过去，要么爬大坡，要么碾碎石堆，电池直接掉了一半。嗯，这就是代价函数没设计好的后果。
4.1 为什么标准A*不够用？
标准A*的代价函数长这样：
f(n) = g(n) + h(n)其中g(n)是从起点到当前节点的实际代价，h(n)是启发式估计代价。在平坦地形上，g(n)通常就是欧几里得距离或曼哈顿距离。
但到了非平坦地形，问题就来了：
- 坡度大：机器人爬坡能耗高，下坡虽然省力但可能打滑
- 地面粗糙：碎石、泥泞、草地，每种地形的通过成本完全不同
- 能耗不均：同样的距离，上坡和下坡消耗的能量差好几倍
说白了，你不能再把每个格子当成等价的。我在项目中遇到过，机器人按照标准A*规划的路径走，结果在30度坡上直接过热保护停机了。从那以后，我设计代价函数时，一定会把地形因素加进去。
4.2 代价函数的三要素
我个人习惯把非平坦地形的代价函数拆成三部分：
Cost_total = w1 * Cost_distance + w2 * Cost_slope + w3 * Cost_roughness其中w1、w2、w3是权重系数，需要根据机器人实际能力来调。你想想看，一个履带式机器人和一个轮式机器人，对坡度的容忍度能一样吗？
4.2.1 坡度代价
坡度代价是最直观的。我建议这样设计：
Cost_slope = α * |slope| + β * (slope > 0 ? 1 : 0) * slope²这里α是基础坡度惩罚，β是上坡额外惩罚。为什么要区分上坡和下坡？
因为上坡时电机需要输出更大扭矩，发热严重。下坡虽然省力，但坡度太大时制动系统压力大，而且容易打滑。我在做户外机器人时，发现下坡超过20度，刹车距离会延长30%以上。
重要经验：坡度代价最好用非线性函数。我试过线性函数，结果机器人专挑缓坡走，绕了很远的路。换成二次函数后，路径更合理了。
4.2.2 粗糙度代价
粗糙度代价，说白了就是地面好不好走。我一般用地形分类来赋值：
| 地形类型 | 粗糙度系数 | 典型场景 |
|---|---|---|
| 平坦路面 | 1.0 | 水泥路、沥青路 |
| 草地 | 1.5 | 公园、草坪 |
| 碎石路 | 2.0 | 矿区、建筑工地 |
| 泥泞地 | 3.0 | 雨后农田、沼泽边缘 |
| 沙地 | 2.5 | 沙漠、海滩 |
这个系数怎么用？很简单：
Cost_roughness = roughness_coefficient * distance举个例子，走10米碎石路，代价相当于走20米平坦路面。这样A*算法就会自动避开那些难走的地形。
小技巧：粗糙度系数最好通过实测标定。我做过一个实验，让机器人在不同地形上跑相同距离，记录能耗和耗时，然后反推系数。这样比拍脑袋准多了。
4.2.3 能耗代价
能耗代价是综合性的。它把坡度和粗糙度都考虑进去，直接估算电池消耗：
Cost_energy = P_base * (1 + k_slope * |slope| + k_rough * roughness) * distance其中P_base是平坦路面上的基础功率，k_slope和k_rough是系数。
为什么要单独提能耗？因为很多项目对续航有硬性要求。我记得有一次，客户要求机器人在复杂地形下连续工作8小时。如果路径规划不考虑能耗，很可能走到一半就没电了。
4.3 权重怎么调？
权重调参是个头疼的问题。我分享一个我常用的方法：
- 先固定距离权重w1=1，作为基准
- 调坡度权重w2：让机器人在30度坡和绕路50米之间做选择，找到平衡点
- 调粗糙度权重w3：让机器人在碎石路和绕路30米之间做选择
- 微调：在实际场景中跑几遍，观察路径是否合理
注意：权重不是越大越好。w2太大，机器人会绕很远的路避开小坡；w3太大，机器人宁愿走泥泞的捷径也不走平坦的大路。我见过有人把w2设成10，结果路径规划出来跟迷宫一样。
4.4 核心逻辑流程图
下面这张图展示了非平坦地形下A*代价函数的整体设计思路：
4.5 代码实现示例
下面是一个简化版的代价函数实现，我把它放在一个类里方便复用：
class TerrainAStarCost:
def __init__(self, slope_weight=2.0, rough_weight=1.5, energy_weight=1.0):
self.w_slope = slope_weight
self.w_rough = rough_weight
self.w_energy = energy_weight
# 地形粗糙度系数表
self.roughness_map = {
'flat': 1.0,
'grass': 1.5,
'gravel': 2.0,
'mud': 3.0,
'sand': 2.5
}
def compute_cost(self, current, neighbor, terrain_data):
# 基础距离代价
dx = neighbor.x - current.x
dy = neighbor.y - current.y
dist = math.sqrt(dx*dx + dy*dy)
cost_dist = dist
# 坡度代价
slope = terrain_data.get_slope(current, neighbor)
cost_slope = abs(slope) + (slope > 0) * slope * slope
# 粗糙度代价
terrain_type = terrain_data.get_type(neighbor)
rough_coef = self.roughness_map.get(terrain_type, 2.0)
cost_rough = rough_coef * dist
# 加权求和
total = (self.w_slope * cost_slope +
self.w_rough * cost_rough +
self.w_energy * dist)
return total使用建议：代码里的权重系数别照搬。我建议先在仿真环境里跑几组数据，看看路径是否合理。调参时记住一个原则——让机器人走的路，换你走也觉得合理。
4.6 避坑指南
最后分享几个我踩过的坑：
- 坡度数据精度不够：我曾经用低分辨率DEM数据，结果坡度计算误差很大，规划出来的路径实际根本走不通。后来我改用激光雷达实时建图，精度好多了。
- 忽略动态地形：泥泞地雨后和晴天差别很大。我建议粗糙度系数做成动态的，根据天气或传感器实时更新。
- 权重调过头：有一次我把能耗权重设得特别高，结果机器人为了省电，专挑陡坡下，差点翻车。安全永远是第一位的。
好了，关于非平坦地形下A*代价函数的设计，核心思路就是这些。记住，代价函数不是越复杂越好，关键是能反映真实物理约束。你设计的时候，多想想机器人实际会遇到什么问题，比堆公式有用得多。
公众号:蓝海资料掘金营，微信deep3321