4、路径规划问题定义：起点与终点、约束条件（运动学、动力学、安全距离）、优化目标（最短路径、最小能耗、平滑性）
路径规划的本质，是在一个充满约束的空间中，为运动体寻找一条从起点到终点的可行且优良的轨迹。初学者往往将路径规划简单理解为“找一条最短的线”，但在实际工程中，无人机、自动驾驶车辆、机械臂等系统的路径规划远非如此。一个完整的路径规划问题定义，必须回答三个核心问题：从哪里来、到哪里去（起点与终点）；哪些行为是物理上允许的（运动学与动力学约束）；什么样的路径是“好”的（优化目标）。本章将系统拆解这三个要素，并引入安全距离这一工程中不可妥协的硬约束，帮助读者建立从“几何路径”到“可行轨迹”的认知跃迁。
4.1 起点与终点：不只是两个坐标点
核心原理：在路径规划中，起点（Start）与终点（Goal）的定义决定了整个搜索空间的边界和求解难度。初学者常犯的错误是将起点和终点仅视为二维或三维空间中的坐标点，而忽略了运动体自身的状态信息。实际上，一个完整的起点/终点定义应包含位姿（Pose）和运动状态（Motion State）两个层面。位姿指位置（x, y, z）与姿态（横滚角、俯仰角、偏航角）；运动状态则包括速度矢量、角速度、加速度等。例如，固定翼无人机在起飞阶段的起点不仅是一个经纬度坐标，还包括当前空速、爬升率和航向角；若终点要求“以巡航速度水平进入着陆窗口”，则终点还附加了速度方向和高度变化率的约束。
判断方法与操作流程：在实际工程中，定义起点与终点需要遵循以下步骤：
- 明确任务类型：是点到点（Point-to-Point）、路径跟随（Path Following）还是区域覆盖（Area Coverage）？不同任务对终点的定义截然不同。
- 提取初始状态：通过传感器（GPS/IMU/编码器）获取运动体当前的位置、速度、姿态，并评估状态估计的不确定性。
- 设定终点容差：对于精确着陆或对接任务，终点容差可能为厘米级；对于区域巡逻任务，终点容差可为数十米。容差大小直接影响搜索算法的收敛速度。
- 检查可达性：在定义终点前，需快速判断运动体在物理约束下是否可能到达该点。例如，一个最小转弯半径为50米的固定翼无人机，无法到达与其当前航向相反且距离小于100米的目标点。
4.2 约束条件：从“几何可行”到“物理可行”
核心原理：约束条件是路径规划问题中区分“几何路径”与“可行轨迹”的关键。几何路径只要求不与障碍物相交，而可行轨迹必须满足运动体自身的物理限制。约束条件可分为三大类：运动学约束（Kinematic Constraints）、动力学约束（Dynamic Constraints）和安全距离约束（Safety Distance Constraints）。
运动学约束描述的是运动体在几何空间中的运动能力限制，不涉及力与质量。典型例子包括：车辆的最小转弯半径（曲率上限）、无人机最大俯仰角、机械臂各关节的角度范围与角速度限制。动力学约束则涉及力、力矩、质量和能量，例如：最大加速度、最大加加速度（Jerk）、电机推力上限、制动距离等。安全距离约束是环境交互层面的硬性要求，包括与静态障碍物的最小间隙、与动态障碍物的时间间隔（Time-to-Collision）、无人机与地面的最小离地高度等。
| 约束类型 | 典型参数 | 数学表达示例 | 违反后果 |
|---|---|---|---|
| 运动学约束 | 最小转弯半径 R_min、最大爬升角 θ_max | κ ≤ 1/R_min（κ为曲率） | 路径不可跟踪，运动体偏离规划路线 |
| 动力学约束 | 最大加速度 a_max、最大加加速度 j_max | |a(t)| ≤ a_max, |j(t)| ≤ j_max | 执行器饱和、跟踪延迟、能耗剧增 |
| 安全距离约束 | 最小间隙 d_safe、最小高度 h_min | dist(p(t), O_i) ≥ d_safe | 碰撞风险、任务失败、安全事故 |
案例拆解：以城市环境中的自动驾驶车辆为例，假设车辆以36km/h（10m/s）行驶，前方20米处出现静止障碍物。仅考虑运动学约束（最小转弯半径5米），车辆可以通过转向绕行；但若考虑动力学约束——车辆最大横向加速度为3m/s²，则完成变道所需的最小纵向距离为：换道时间 t = 横向位移/横向速度 ≈ 3.5m / (3m/s² × t) 的积分求解，实际需要约25米的纵向距离。此时20米的距离不足以安全完成变道，必须施加制动。这个例子说明，忽略动力学约束的路径规划会生成“看起来可行、实际无法执行”的危险路径。
4.3 优化目标：多目标权衡的艺术
核心原理：优化目标（Cost Function / Objective Function）定义了“什么样的路径是更好的”。单一目标（如最短路径）在简单环境中有效，但在真实工程中，路径规划通常是多目标优化问题。三个最核心的优化目标为：最短路径（Shortest Path）、最小能耗（Minimum Energy）和平滑性（Smoothness）。这三个目标往往相互冲突：最短路径可能包含尖锐转角，导致能耗增加和平滑性下降；而追求极致平滑的路径可能绕行较远，增加路径长度和飞行时间。
判断方法与操作流程：构建优化目标函数时，推荐采用加权求和法（Weighted Sum Method）：
J = w₁ · L_path + w₂ · E_energy + w₃ · S_smoothness
其中：
L_path = ∫ ds（路径总长度）
E_energy = ∫ P(t) dt（功率对时间积分）
S_smoothness = ∫ (κ'(s))² ds（曲率变化率平方积分）
w₁ + w₂ + w₃ = 1，权重根据任务需求调整权重选择是工程中的关键决策。对于巡检任务，w₁（路径长度）权重较高，因为需要快速覆盖目标点；对于精密制造中的机械臂，w₃（平滑性）权重极高，以避免振动和冲击；对于续航有限的无人机，w₂（能耗）权重最大。权重的标定可通过层次分析法（AHP）或仿真调参完成。
4.4 常见误区与总结复盘
误区一：混淆“路径”与“轨迹”。路径（Path）是几何概念，只包含空间位置序列；轨迹（Trajectory）是时间概念，包含每个时刻的位置、速度和加速度。许多初学者在规划阶段不考虑时间分配，导致生成的路径无法被底层控制器跟踪。正确做法是：先规划路径，再通过时间最优或能量最优的速度规划（Time Scaling）生成轨迹。
误区二：约束条件“事后检查”而非“事前建模”。有些算法先搜索出候选路径，再检查是否满足约束，不满足则丢弃。这种“采样-检查”方法在复杂约束下效率极低。更优的做法是将约束直接编码到搜索空间或优化函数中，例如在A*算法中使用曲率约束的节点扩展方式，或在RRT*中通过控制输入采样生成满足动力学约束的局部路径。
误区三：优化目标权重固定不变。实际任务中，环境动态变化（如风速突变、障碍物移动），固定权重可能导致规划结果在某种场景下失效。高级方法采用自适应权重调整策略，例如当检测到电量低于30%时，自动增大能耗权重w₂；当飞行高度接近地面时，增大安全距离约束的惩罚项。
总结复盘：本章的核心在于建立“问题定义决定算法上限”的工程认知。一个定义良好的路径规划问题，应包含：明确的起点/终点状态（含容差）、完整的约束集合（运动学+动力学+安全距离）、以及可量化的多目标优化函数。在实际项目中，建议按以下顺序推进：
- 与任务方确认起点/终点的物理含义和允许误差范围；
- 查阅运动体手册或通过实验测定运动学/动力学参数（如最小转弯半径、最大加速度）；
- 根据传感器精度和控制误差设定安全距离；
- 通过仿真对比不同权重组合下的路径质量，确定最终优化目标；
- 将问题定义文档化，作为算法选型和开发的输入基线。