4. 常用运动控制算法原理

AGV（自动导引车）的运动控制本质上是将路径规划层输出的几何路径（离散点序列或连续曲线）转化为底层执行机构（差速轮、舵轮、麦克纳姆轮等）可执行的转速/转角指令。不同算法在数学建模、计算开销、跟踪精度与鲁棒性上各有取舍。本章从工程部署视角，系统拆解四种主流算法的数学内核、适用边界与调参要点，帮助读者在“精度优先”“鲁棒优先”或“成本受限”等不同项目约束下做出合理选型。

上图从模型依赖、约束处理、典型场景与部署成本四个维度给出了选型总览。实际项目中，PID通常作为底层速度环存在，而路径跟踪层则根据AGV的动力学特性与任务需求在纯追踪、Stanley与MPC之间选择。下面逐一展开数学推导与工程要点。

4.1 PID控制：从电机速度环到位姿闭环

核心原理：PID控制器通过比例（P）、积分（I）、微分（D）三项的线性组合，对误差信号进行校正。连续域表达式为：

u(t) = Kp * e(t) + Ki * ∫e(τ)dτ + Kd * de(t)/dt

在AGV中，PID通常以级联形式出现：内环为电机电流/速度环（周期1-5ms），外环为AGV线速度/角速度环（周期10-50ms）。对于差速驱动模型，左右轮速差Δv与转向角速度ω的关系为：

ω = (v_right - v_left) / L_baseline

其中L_baseline为轮距。PID参数整定可采用Ziegler-Nichols临界比例度法，或基于模型仿真（如Simulink）进行离线优化。

判断方法/操作流程：

1. 确定控制周期：底层速度环建议1-5ms，位姿环10-20ms。
2. 先整定内环：断开外环，给阶跃速度指令，调整Kp使响应快且无超调，再引入Ki消除稳态误差。
3. 再整定外环：给定直线路径，观察横向误差收敛速度，逐步增加Kp直到出现振荡，再回调70-80%。
4. 加入微分项Kd抑制超调，但需注意测量噪声放大问题。

案例拆解：某仓储AGV采用差速驱动，轮距0.6m，最高速度1.5m/s。初始调试时，位姿环Kp=2.0，车辆在直线跟踪时出现左右摆振，频率约2Hz。通过示波器观察左右轮速指令，发现P项对横向误差过于敏感。将Kp降至1.2，并加入Kd=0.3后，摆振消失，横向稳态误差从±3cm降至±0.8cm。但遇到地面湿滑时，积分项导致过冲，最终增加积分限幅为最大转向量的25%，问题解决。

常见误区：PID不是“万能药”。当AGV需要跟踪曲率较大的路径时，纯PID的滞后性会导致弯道内切或外偏。此时应优先考虑几何跟踪算法，或将PID作为MPC等上层算法的底层执行器。

4.2 纯追踪（Pure Pursuit）：基于前视距离的几何跟踪

核心原理：纯追踪算法模拟人类驾驶行为——始终瞄准路径前方一个“前视点”，计算当前位姿到该点的圆弧曲率，并输出对应的转向角。其数学基础是阿克曼转向几何或差速模型的圆弧拟合：

曲率κ = 2 * y_ld / (L_d^2)

其中L_d为前视距离（从AGV当前位置到目标点的直线距离），y_ld为目标点在车辆坐标系下的横向偏移。对于差速AGV，左右轮速可表示为：

v_left = v * (1 - κ * L_baseline/2)
v_right = v * (1 + κ * L_baseline/2)

判断方法/操作流程：前视距离L_d是唯一关键参数，通常与速度相关：

• 低速（<0.5m/s）：L_d = 0.5~1.0m
• 中速（0.5~1.5m/s）：L_d = 1.0~2.0m
• 高速（>1.5m/s）：L_d = 2.0~3.0m或更大

工程上常采用线性插值：L_d = k * v + L_min，其中k为速度增益系数（典型值0.5~1.0），L_min为最小前视距离。

案例拆解：某工厂AGV需在2m宽的通道内以1.0m/s速度行驶，路径为直线+90°弯+直线。初始L_d=1.0m时，弯道处最大横向偏差达25cm，车辆明显切弯。将L_d调整为1.5m后，弯道偏差降至8cm，但直线段出现轻微摆振。最终采用速度自适应：L_d = 0.8*v + 0.6，在1.0m/s时L_d=1.4m，直线稳定且弯道偏差控制在5cm以内。

常见误区：纯追踪假设AGV能瞬时响应曲率指令，但实际执行机构（舵机或差速电机）存在延迟。当AGV轴距较长或响应较慢时，需在前视距离中额外补偿一个“延迟距离”L_delay = v * T_response。

4.3 Stanley：基于前轮中心横向误差的反馈控制

核心原理：Stanley算法由斯坦福大学提出，直接利用前轮中心到最近路径点的横向误差e和航向误差θ_e计算转向角：

δ = θ_e + arctan(k * e / v)

其中δ为前轮转向角，k为增益系数，v为纵向速度。该算法在数学上保证了横向误差的指数收敛，且当e→0时，δ→θ_e，实现航向对齐。

判断方法/操作流程：

1. 计算最近路径点：需建立路径点索引或KD树加速搜索。
2. 计算横向误差e：取当前前轮中心到最近路径点的垂直距离（带符号）。
3. 计算航向误差θ_e：当前航向与最近路径点切线方向的夹角。
4. 代入公式计算δ，并做饱和限幅（如±30°）。

案例拆解：某户外AGV采用前轮转向（类似汽车），轴距1.2m，最大速度2.0m/s。使用Stanley算法时，初始k=1.0，在直线段表现良好，但进入S弯时出现转向抖动。分析发现，当速度降至0.3m/s以下时，arctan(k*e/v)项因分母过小产生巨大转向角。解决方案：设置最小速度阈值v_min=0.1m/s，低于该值时直接使用θ_e作为转向角；同时将k调整为速度自适应：k = 2.0 / (1 + 0.5*v)。

常见误区：Stanley算法假设车辆为前轮转向模型，直接用于差速AGV时需做等效转换——将差速AGV的虚拟前轮置于车体前方L_w处，L_w可取车体长度的一半。否则算法会因模型不匹配而性能下降。

4.4 模型预测控制（MPC）：滚动优化与约束处理

核心原理：MPC在每个控制周期内，基于系统模型预测未来N_p步的输出，通过求解带约束的最优化问题，得到当前时刻的最优控制序列，并仅执行第一步。其数学形式为：

min J = Σ_{i=1}^{N_p} ||y_ref(k+i) - y(k+i)||²_Q + Σ_{i=0}^{N_c-1} ||Δu(k+i)||²_R
s.t. x(k+i+1) = A*x(k+i) + B*u(k+i)
     u_min ≤ u(k+i) ≤ u_max
     Δu_min ≤ Δu(k+i) ≤ Δu_max

对于AGV，状态量通常为[x, y, θ, v, ω]，控制量为[v_cmd, ω_cmd]。预测模型可采用运动学自行车模型或差速模型。

判断方法/操作流程：

1. 确定预测时域N_p（典型值10-30步）和控制时域N_c（典型值3-10步）。
2. 选择求解器：嵌入式环境常用OSQP、CVXGEN或ACADO；算力充足可用MATLAB/Simulink生成代码。
3. 设置权重矩阵Q（状态误差权重）和R（控制增量权重）。
4. 定义约束：如最大转向角、最大加速度、执行器变化率限制。

案例拆解：某重载AGV（载重2吨）在狭窄货架间行驶，要求横向偏差不超过±2cm，且不能与货架碰撞（安全距离10cm）。采用线性MPC，状态量5维，预测时域20步，控制周期100ms。在工业PC（Intel i5）上单步求解时间约8ms，满足实时性。实际运行中，MPC成功处理了以下约束：

• 转向角限幅±25°，避免执行器饱和。
• 加速度限幅0.3m/s²，防止货物滑动。
• 预测路径与障碍物距离约束（通过线性化近似）。

相比纯追踪，MPC在弯道中的最大横向误差从5cm降至1.2cm，且无超调。但调试周期从1天延长至1周，主要时间花在模型辨识和权重整定上。

常见误区：MPC并非“参数越多越好”。权重矩阵Q和R的整定需要系统性方法：先固定R，增大Q中对应横向误差的权重，观察响应；再微调R控制执行器动作幅度。此外，模型失配（如轮胎打滑、载重变化）会显著影响MPC性能，需加入扰动估计或自适应机制。

总结复盘：在实际AGV项目中，四种算法并非互斥。常见架构为：底层电机采用PID速度环，中层路径跟踪采用纯追踪或Stanley，上层避障或复杂工况切换至MPC。选型时需综合评估：

• 控制器算力：MCU（如STM32）适合PID/纯追踪；ARM Cortex-A系列可运行Stanley；x86或高性能ARM需运行MPC。
• 路径复杂度：直线/大半径弯道用纯追踪即可；S弯/狭窄空间需Stanley或MPC。
• 执行器特性：舵机响应慢时慎用Stanley；差速驱动对纯追踪更友好。
• 安全要求：有硬约束（如防撞）时，MPC是唯一能显式处理约束的方案。