2. 基础知识

行为决策与轨迹生成系统是自动驾驶与智能机器人的核心中枢，其输入输出均建立在精确的数学描述之上。在深入算法细节之前，必须首先统一三块基石：坐标系定义（我们如何描述世界与自身）、运动学模型（我们如何描述运动约束）以及决策与规划的分层架构（我们如何组织计算资源）。本节将系统梳理这些基础概念，并明确路径与轨迹的本质区别，为后续的碰撞检测、优化求解与控制执行奠定严格的语言基础。

2.1 坐标系定义与变换

核心原理：多坐标系协同

在行为决策与轨迹生成系统中，单一坐标系无法满足所有需求。通常至少涉及三个层次的坐标系：世界坐标系（World Frame, W）用于全局路径规划与地图匹配，通常采用UTM或高斯-克吕格投影，原点固定于地球表面某点；车体坐标系（Body Frame, B）固连于车辆，原点常取后轴中心（对于阿克曼转向车辆）或几何中心（对于全向机器人），x轴指向车头前进方向，y轴指向左侧（符合ISO 8855标准）；传感器坐标系（Sensor Frame, S）则依附于LiDAR、相机或毫米波雷达的安装位置。三者之间通过刚体变换（旋转矩阵R与平移向量t）相互关联，齐次变换矩阵T∈SE(2)或SE(3)是统一描述的核心工具。

判断方法：坐标变换的工程流程

1. 标定：通过外参标定获取传感器到车体的固定变换T_SB，以及车体到世界的实时变换T_BW（由定位模块提供）。
2. 投影：将感知目标从传感器坐标系投影至车体坐标系，再投影至世界坐标系，形成统一的环境模型。
3. 逆变换：规划模块生成的轨迹通常在世界坐标系描述，控制模块需将其变换至车体坐标系以计算跟踪误差。

案例拆解：多传感器融合中的坐标对齐

某L4级自动驾驶测试车同时搭载64线LiDAR（安装于车顶前部，偏移量Δx=+1.2m, Δy=0m, Δz=+1.8m，俯仰角+2°）和前视相机。LiDAR检测到前方障碍物在S系下的坐标为(10.5, -1.2, 0.3)m。通过外参矩阵T_SB变换至B系，再结合车辆当前位姿(x=100m, y=50m, θ=30°)变换至W系。若忽略传感器安装偏移，直接使用S系坐标进行决策，将导致约1.2m的横向偏差，在狭窄车道中可能引发误判。实际工程中，必须建立统一的坐标变换服务，并定期校验外参漂移。

常见误区与总结

• 误区1：混淆车体坐标系与质心坐标系。对于自行车模型，后轴中心是运动学基准，而质心是动力学基准，两者在高速转向时差异显著。
• 误区2：忽略Z轴分量。在非平坦路面（坡度、坡道）上，仅使用2D坐标变换会导致高程误差累积，影响轨迹的纵向加速度规划。
• 总结：坐标系是系统的“语言”，统一且精确的坐标变换是决策规划正确性的前提。建议在系统启动时进行坐标变换自检，输出各坐标系原点间的相对位姿矩阵。

2.2 车辆运动学模型：自行车模型与机器人运动学

核心原理：从约束到状态方程

自行车模型（Bicycle Model）是阿克曼转向车辆最常用的运动学简化模型。它将前后轮分别等效为位于前轴中心与后轴中心的单个车轮，忽略轮胎侧偏角（低速或小曲率工况下成立）。状态量通常取为后轴中心位置(x, y)、横摆角θ、前轮转角δ和纵向速度v。其连续时间运动学方程为：

ẋ = v * cos(θ)
ẏ = v * sin(θ)
θ̇ = v * tan(δ) / L
δ̇ = ω_δ   (转向角速度，作为控制输入之一)

其中L为轴距。该模型的关键特征在于非完整约束：车辆不能横向平移，即满足 ẋ sinθ - ẏ cosθ = 0。这一约束决定了轨迹的曲率上限 κ_max = tan(δ_max)/L。

差速机器人运动学（如两轮差速底盘）则采用另一套描述：状态为(x, y, θ)，控制输入为线速度v与角速度ω。运动学方程为：

ẋ = v * cos(θ)
ẏ = v * sin(θ)
θ̇ = ω

其约束形式与自行车模型类似，但控制维度不同——差速机器人可以原地旋转（v=0, ω≠0），而自行车模型在v=0时无法改变航向（需倒车或使用特殊 maneuver）。

判断方法：如何选择运动学模型

案例拆解：基于自行车模型的轨迹生成约束

某园区接驳车轴距L=3.2m，最大前轮转角δ_max=35°（约0.61rad），最高运行速度v_max=8m/s。在生成一条曲率连续的换道轨迹时，必须满足曲率约束 κ ≤ tan(35°)/3.2 ≈ 0.219 rad/m，对应最小转弯半径约4.57m。若规划算法生成了一条曲率峰值为0.3 rad/m的路径，车辆将无法跟踪。实际处理中，可在轨迹优化目标函数中加入曲率变化率惩罚项，并设置硬约束 κ ≤ 0.9 * κ_max 以留出控制余量。同时，由于车辆在高速时轮胎侧偏角不可忽略，运动学模型仅适用于v < 5m/s的工况；更高速度需引入动力学模型（如单轨模型或双轨模型）。

常见误区与总结

• 误区1：将自行车模型用于倒车轨迹生成。倒车时车辆的运动学行为不同（前轮转向导致后轴轨迹曲率方向相反），需使用专门的倒车模型或增加状态约束。
• 误区2：忽略转向执行机构的延迟与速率限制。运动学方程中的δ̇并非无限大，实际转向电机有最大转速（如30°/s），轨迹生成时需考虑δ的时间导数约束。
• 总结：运动学模型是轨迹可行性的第一道过滤器。在生成轨迹前，先检查起点与终点的状态是否满足模型约束；在生成轨迹后，需用同一模型进行前向仿真验证。

2.3 路径与轨迹的本质区别

核心原理：几何与时间的解耦

路径（Path）是空间中的一条连续曲线，通常表示为弧长参数s的函数：P(s) = [x(s), y(s), θ(s)]。它只描述“在哪里”，不包含“何时到达”以及“以多快速度通过”。路径规划的核心任务是寻找一条从起点到终点、避开障碍物且满足曲率约束的几何曲线。

轨迹（Trajectory）则是时间参数化的运动描述：T(t) = [x(t), y(t), θ(t), v(t), a(t)]。它回答了“何时到达何处、速度与加速度如何变化”。轨迹规划在路径的基础上叠加了时间律（速度分配），因此必须考虑动力学约束（如最大加速度、最大jerk）以及时间最优或能量最优等优化目标。

判断方法：何时需要区分二者

• 若仅需静态避障（如离线规划一条从A到B的路线），使用路径规划即可。
• 若需动态避障（如与移动障碍物交互）、速度平滑或时间最优控制，则必须生成轨迹。
• 在行为决策层，通常先进行路径选择（如选择哪条车道），再在选定路径上进行轨迹生成（如决定何时、以多大加速度并入）。

案例拆解：从路径到轨迹的转换

考虑一个自动泊车场景。路径规划模块生成了一条由三段圆弧和两段直线组成的泊车路径，总长12.5m，最小曲率半径5.0m。该路径本身是可行的，但若直接以恒定速度0.5m/s执行，车辆在圆弧段将产生0.05m/s²的向心加速度，虽在舒适范围内，但停车精度可能因积分误差而超差。轨迹规划模块进一步为路径分配时间律：先以0.8m/s²加速至1.0m/s，在进入圆弧前减速至0.3m/s，泊车末端以-0.5m/s²减速至零，并预留0.2s的稳定时间。最终轨迹的总时长为18.7s，比匀速方案缩短了约15%，且终点误差控制在±2cm内。这一案例说明，轨迹生成不仅是对路径的时间标注，更是对车辆动态性能的深度利用。

常见误区与总结

• 误区1：将路径点直接作为轨迹点使用，导致速度规划缺失，控制模块无法获得期望速度。
• 误区2：在轨迹生成时忽略路径的曲率连续性。若路径本身存在曲率突变，即使速度规划合理，车辆转向机构也无法瞬时响应，产生跟踪误差。
• 总结：在系统设计中，应明确路径与轨迹的数据结构差异。路径使用弧长参数化（s），轨迹使用时间参数化（t），两者之间的转换需通过速度分配函数 v(s) 或 s(t) 完成。

2.4 决策与规划的分层架构

核心原理：分而治之的计算范式

自动驾驶或机器人系统的决策与规划问题极其复杂，难以用单一算法端到端求解。分层架构将问题分解为三个层次：任务决策层（L1）、运动规划层（L2）与控制执行层（L3）。L1负责理解场景意图，输出行为指令（如“跟车”、“变道”、“停车”）；L2接收行为指令，结合环境模型生成具体的路径或轨迹；L3则负责将轨迹转化为油门、刹车、转向指令，并处理执行误差。这种架构的优势在于：各层可以采用最适合自身问题的算法（如L1用状态机或强化学习，L2用优化或采样，L3用PID或MPC），且便于调试与安全验证。

操作流程：分层间的信息交互

1. 上游输入：感知模块输出障碍物列表、车道线、交通标志；定位模块输出车辆位姿与速度。
2. L1决策：根据全局任务（如“从A到B”）与当前交通场景，决策模块输出一个或多个候选行为，并附带约束条件（如“变道必须在200m内完成”）。
3. L2规划：运动规划模块将行为指令转化为无碰撞、满足运动学/动力学约束的轨迹。通常先进行路径搜索（如Hybrid A*），再进行轨迹优化（如时间弹性带TEB或模型预测控制MPC）。
4. L3控制：控制器跟踪轨迹，计算前馈+反馈控制量，并输出至执行器。
5. 反馈回路：控制误差、车辆状态与新的感知信息反馈至L1与L2，触发重新决策或轨迹重规划。

案例拆解：高速公路上自动变道

某车辆以25m/s（90km/h）行驶在高速最右侧车道，前方200m处有一辆慢车以15m/s行驶。L1决策模块评估后输出“向左变道”行为，并设定约束：变道起始点距慢车≥80m，变道持续时间≤6s，变道过程中横向加速度≤2m/s²。L2规划模块首先在Frenet坐标系下生成候选轨迹集，每条轨迹由横向位移d(t)与纵向位移s(t)的多项式表示。通过代价函数评估（包括碰撞风险、舒适度、效率），选择最优轨迹：横向位移从0m平滑过渡至3.5m（相邻车道中心线），纵向速度从25m/s先减速至22m/s再加速至28m/s。L3控制器采用MPC跟踪该轨迹，实际最大横向跟踪误差为0.12m，纵向速度误差±0.3m/s。整个变道过程耗时5.2s，符合预期。

常见误区与总结

• 误区1：层间耦合过紧。例如，L2直接依赖L1输出的精确轨迹点，而非行为意图，导致L1的微小变化引发L2的剧烈重规划。
• 误区2：忽视各层的时间预算。L1决策通常需要10-50ms，L2规划需要50-200ms，L3控制需要1-10ms。若某层超时，应使用上一周期的结果或降级策略，而非阻塞等待。
• 总结：分层架构的核心价值在于“隔离复杂度”与“明确责任边界”。每一层只解决一个明确定义的问题，并通过标准接口通信，从而提升系统的可维护性与可测试性。