在运动规划里，代价函数（Cost Function）就是你用来衡量一条轨迹／路径“好坏”的那个标尺——它把一个运动方案映射到一个实数，使得“更优”的方案对应着更小（或更大，取决于定义）的代价值。代价函数在不同的规划框架中，形式各异，但核心都是告诉算法：我想要最短的？最平滑的？能耗最低的？还是避障最安全的？
一、代价函数的通用形式
对于连续时间的最优控制／轨迹优化问题，一般把代价函数分为两部分：
- 运行代价（running cost）
- 是系统状态（位置、速度、姿态……），
- 是控制输入（加速度、舵面偏角、油门开度……），
- 就是在每个时刻需要累积的“即时代价”，例如能耗 u2u^2、抖动平方 j2j^2、与障碍物的距离代价、甚至混合权重的线性组合。
其中
- 终端代价（terminal cost）
用来惩罚或奖励最终状态，比如偏离目标位置的距离、剩余能量不足、到达时间过晚等。
合起来，总代价就是
二、常见的几类代价函数示例
目的 | 运行代价 L 示例 | 终端代价 h 示例 |
最短路径 | 或者边长（离散中） | h=0 |
最小时间 | L = 1 | h=0 |
最小能耗／力矩 | h=0 | |
最平滑轨迹 | （jerk 平方）或（snap 平方） | h=0 or 终点误差惩罚 |
障碍物避让 | 距离罰函数 | h=0 |
混合权衡 |
- 可以是“当距离小于安全阈值才开始加大惩罚”的软约束函数。
- 是权重，用于在“平滑”、“快速”、“安全”之间做折中。
三、在离散图搜索／采样方法中的代价
- 图搜索（A*、Dijkstra）：
- 边代价 c(e)通常就是两节点间的距离、时间或能耗。
- 总代价 g(n)是起点到节点n的累计边代价；再加上启发函数 h(n)构成。
- 采样／路标（PRM、RRT*, RRT-Connect）：
- 每次试连一条新边，给它赋一个代价值，然后在“生长”或“重线”阶段，以最小化路径总代价为准则。
四、为什么代价函数很重要？
- 决定规划目标
你要是只想“最短”，那就把距离当代价；要“最快”，就把时间当代价；要“平滑”，就把 jerk 或 snap 当代价。
- 影响解的可行性和可控性
加入平滑或能耗项，飞行器控制指令就不会突然跳变，更易跟踪、硬件应力小。
- 权衡多目标
通过给不同项加权，可以同时兼顾速度、平滑和避障，得到一个符合你工程需求的综合最优轨迹。
总结：在运动规划里，“代价函数”就是规划算法用来评价和比较不同轨迹“好坏”的数学量。它可以是路径长度、时间、能耗、平滑度、避障程度，或者它们的加权组合。代价函数的设计直接决定了最终生成的轨迹特性，也影响求解方法的收敛速度与计算复杂度。
