第1章代价函数基础

第1章：代价函数基础

各位同学好，我是你们的老朋友。今天咱们来聊聊路径规划里最基础、也最核心的一个概念——代价函数。

说实话，我刚开始做机器人那会儿，觉得路径规划不就是找条路嘛，有啥难的？直到第一次在真实场景里跑算法，机器人撞墙了...嗯，从那以后我才真正重视起代价函数来。

什么是路径规划代价函数？

说白了，代价函数就是一个数学表达式。它给每条可能的路径打个分，分数越低，路径越好。

你想想看，从A点到B点，可能有几十上百条路。哪条最短？哪条最安全？哪条最省电？代价函数就是用来回答这些问题的。

核心定义：代价函数 f(path) = w₁·cost₁ + w₂·cost₂ + ... + wₙ·costₙ

其中 cost 可以是距离、时间、能耗、风险等，w 是权重系数。

举个例子。我在做仓储机器人项目时，代价函数就包含了三个部分：

距离代价：路径总长度，单位米
时间代价：预计行驶时间，考虑加减速
安全代价：离障碍物的最小距离，太近就惩罚

这三个加起来，就是一条路径的总代价。

为什么需要代价函数？

这个问题我问过不少新人。有人回答「因为算法需要」，也有人回答「为了找最短路径」。其实都不完全对。

根本原因有三个：

量化比较：没有代价函数，你没法说「这条路比那条路好」。就像考试没分数，怎么排名？
多目标权衡：实际场景里，我们往往要兼顾多个目标。既要快，又要安全，还要省电。代价函数就是做权衡的工具。
约束表达：有些路不能走（比如太窄），有些区域不能进（比如禁行区）。代价函数可以把这些约束变成数学语言。

我的经验：千万别小看代价函数的设计。我在一个项目中，就因为安全代价的权重设得太低，导致机器人贴着墙走，结果刮花了漆面。后来把安全代价权重从0.1调到0.3，问题就解决了。

代价函数在三大算法中的角色

不同算法对代价函数的用法不太一样。我分别说说。

1. Dijkstra算法

Dijkstra是最朴素的。它只考虑从起点到当前点的实际代价，也就是 g(n)。

我记得在大学学这个算法时，老师画了个图，每个节点标上距离。Dijkstra就是不断找「当前已知最短」的节点往外扩展。

代价函数角色：就是 g(n)，表示从起点到节点n的真实代价。

// Dijkstra伪代码
function Dijkstra(start, goal):
    openSet = {start}
    g[start] = 0
    
    while openSet is not empty:
        current = node with smallest g in openSet
        if current == goal:
            return reconstructPath()
        
        for each neighbor of current:
            tentative_g = g[current] + cost(current, neighbor)
            if tentative_g < g[neighbor]:
                g[neighbor] = tentative_g
                parent[neighbor] = current

注意：Dijkstra只保证找到最短路径，但效率不高。因为它没有方向性，会向四面八方扩展。我曾经用它在1000个节点的地图上跑，结果等了3秒才出结果——这在实时系统里是不能接受的。

2. A*算法

A*在Dijkstra基础上加了个启发式函数 h(n)。总代价 f(n) = g(n) + h(n)。

h(n) 是对「从当前点到终点的代价」的估计。常用的有欧几里得距离、曼哈顿距离等。

代价函数角色：f(n) = g(n) + h(n)，g是实际代价，h是估计代价。

// A*伪代码
function AStar(start, goal):
    openSet = {start}
    g[start] = 0
    f[start] = h(start, goal)  // 启发式估计
    
    while openSet is not empty:
        current = node with smallest f in openSet
        if current == goal:
            return reconstructPath()
        
        for each neighbor of current:
            tentative_g = g[current] + cost(current, neighbor)
            if tentative_g < g[neighbor]:
                g[neighbor] = tentative_g
                f[neighbor] = g[neighbor] + h(neighbor, goal)
                parent[neighbor] = current

你想想看，A*比Dijkstra聪明在哪？它知道终点在哪边，所以会优先往那个方向探索。这就是h(n)的作用。

关键点：h(n)必须满足「可采纳性」，即h(n) ≤ 真实代价。否则A*可能找不到最优解。我见过有人用直线距离当h(n)，但地图是网格且有障碍物，结果路径明显不是最优的。

3. RRT算法

RRT（快速随机扩展树）跟前面两个不太一样。它不是显式地维护一个代价函数，而是通过随机采样来探索空间。

代价函数角色：在RRT中，代价函数主要用于路径优化阶段。比如RRT*算法，会在找到初始路径后，用代价函数来优化路径。

// RRT*路径优化伪代码
function RRTStarOptimize(tree, new_node):
    // 在半径r内找邻近节点
    near_nodes = findNearNodes(tree, new_node, r)
    
    for each near_node in near_nodes:
        // 计算从起点到near_node再到new_node的代价
        new_cost = g[near_node] + cost(near_node, new_node)
        if new_cost < g[new_node]:
            // 更新父节点
            parent[new_node] = near_node
            g[new_node] = new_cost

说白了，RRT*就是在RRT的基础上，用代价函数来「修剪」树，让路径越来越优。

三种算法的代价函数对比

算法	代价函数形式	特点	适用场景
Dijkstra	f(n) = g(n)	保证最优，但效率低	小地图、静态环境
A*	f(n) = g(n) + h(n)	有方向性，效率高	中等规模地图、游戏
RRT/RRT*	隐式使用，优化时显式	适合高维空间	机械臂、无人机

避坑指南：我曾经在一个项目中，直接用Dijkstra跑一个200×200的栅格地图，结果每帧要算50ms，根本达不到实时要求。后来换成A*，同样的地图只要5ms。所以选算法时，一定要考虑代价函数的计算开销。

小结

这一章我们聊了代价函数的基础。记住三点：

代价函数就是给路径打分的数学工具
它帮助我们量化比较、多目标权衡、表达约束
不同算法对代价函数的使用方式不同，但核心思想一致

下一章，我会带大家手写一个代价函数，并调参看看效果。到时候你们会发现，调参真是个技术活——嗯，也是个体力活。

公众号:蓝海资料掘金营，微信deep3321

📚 课程目录