【控制算法详解】从 PID 到 PPO，一条路线讲完机器人控制的主流算法——附真实代码和调参经验

前言

做机器人绕不开控制。这篇文章把控制算法按学习顺序串成一条线，从最基础的 PID 一直讲到强化学习的 PPO。每个算法不讲满纸公式，讲清楚三件事：它到底在干什么、关键参数怎么调、真实代码里长什么样。

文中所有代码片段和参数都来自实际工程：PX4 无人机导航代码、ROS 仿真平台的 MPC 配置、竞赛机器人的卡尔曼+PID 跟踪代码、开源小车项目的 TEB/DWA 配置。

贯穿全文有一条主线：模型利用程度递减，数据需求递增。PID 需要一个模型吗？不需要，但调参靠经验。LQR/MPC 要模型，而且模型越准控制越好。PPO 不要模型，但要海量数据。理解这条线，学新算法时会快很多。

---

第 1 层：经典反馈

经典反馈是控制的地基。它不需要知道系统的模型，只盯着"误差"干活：误差大就用力纠，误差小就少纠。

1.1 PID

PID 是误差乘以三个系数：

u(t) = Kp·e(t) + Ki·∫e(t)dt + Kd·d e(t)/dt

• P（比例）：误差 × Kp 直接输出。误差大推得多，误差小推得少，但永远追不上目标，留下稳态误差。
• I（积分）：把历史误差累计起来，专门消掉 P 留下的那点残差。
• D（微分）：看误差的变化趋势，误差正在快速变小就刹车，抑制超调。

工程里最常见的写法是位置式和增量式。竞赛代码里有一个典型的离散实现，注意它的积分限幅——这是工程必备，防止积分饱和后输出失控：

double calculate(double error)
{
    double P = Kp_ * error;
    integral_ += error;
    if (std::abs(integral_) > integralLimit_)            // 积分限幅
        integral_ = std::copysign(integralLimit_, integral_);
    double I = Ki_ * integral_;
    double derivative = error - lastError_;              // 误差差分近似微分
    double D = Kd_ * derivative;
    lastError_ = error;
    return P + I + D;
}

调参经验：大多数场景一个 P 就够，或者 P+I、P+D 组合。I 和 D 一般都要调得很小很小——I 大了超调，D 大了被噪声激出抖动。项目里跟靶云台用的是 eKp=1.0, eKi=0.01, eKd=0.03，P 是 I 的 100 倍。整定顺序固定 P→I→D：P 定基本行为，I 补残差，D 最后加，顺序反了参数互相打架。

双环结构：移动机器人很少单环用 PID。PX4 无人机常用的 px4ctrl 就是典型的位置-速度双环：

// controller.cpp
des_acc = des.a + Kv.asDiagonal() * (des.v - odom.v)
                + Kp.asDiagonal() * (des.p - odom.p);
des_acc += Eigen::Vector3d(0, 0, param_.gra);   // 重力补偿

外环位置偏差 → 期望加速度，内环加速度 → 推力/姿态。增益不再是两个标量而是一组向量 Kp0..Kp2, Kv0..Kv2，三个方向可以独立整定。注意代码里的 param_.gra——重力补偿是无人机 PID 不能省的，不补偿的话悬停就要靠积分项硬扛。

1.2 纯追踪 Pure Pursuit

纯追踪是路径跟踪里最简单实用的算法。思想一句话：在车前找一个前瞻点，画一条圆弧跟过去。

几何推导很干净。前视距离 L 是车到前瞻点的直线距离，车头到前瞻点连线的角度误差是 e_theta，那么圆弧曲率：

κ = 2·sin(e_theta) / L

转向角速度 ω = v·κ。整个算法只有一个参数——前视距离 L，而且它是最核心的参数，别的参数对效果的影响远不如它。

前视距离怎么取？固定值在速度变化时不好用。动态前视是一个很好的做法：

// lqr_purepursuit.cpp
double getLookAheadDistance(double vt, double lookahead_time,
    double min_lookahead_dist, double max_lookahead_dist)
{
    double lookahead_dist = fabs(vt) * lookahead_time;   // 速度 × 时间
    return clamp(lookahead_dist, min_lookahead_dist, max_lookahead_dist);
}

前视距离 = 当前速度 × 前视时间，再夹在 [0.26, 0.30] 之间。“看多远"要匹配"刹得住多远”——速度快看远，弯急看近。

调参经验：前视太近 → 蛇形抖动；太远 → 切弯（内切）。实际工程里纯粹用纯追踪思想跟线时，前瞻距离直接写死 0.6m，转向用"航向角误差 + 横向偏差"的 PD 式输出，效果稳定。这也是为什么纯追踪是工程首选：参数少、鲁棒、好解释。

1.3 Stanley

Stanley 是对纯追踪的理论改进，转向角由两个量组成：

δ = e_theta + atan2(k·e_lateral, v)

e_theta 是车头与路径切线的夹角，e_lateral 是车头到路径的横向偏差。横向偏差大时 atan2(k·e_lateral, v) 项直接掰回来，理论上比纯追踪收敛更快、更精确。

经验教训：理论很有道理，但实际死活调不出来——参数 k 比纯追踪敏感得多，对"车头投影到路径上的点"这个几何关系稍理解不到位就发散。数学上更精确不等于工程上更好用。纯追踪参数少、鲁棒，工程上更常用是有原因的。

一句话对比：纯追踪"看前一点画弧"，Stanley"盯脚下一点算夹角"。

---

第 2 层：局部规划

经典反馈回答"怎么走准"，局部规划回答"往哪走一步"。机器人每几十毫秒就要在当前环境里算出一条短轨迹，避开传感器刚看到的障碍。

2.1 DWA 动态窗口法

DWA 的思想：在当前速度附近采样一堆 (v, ω) 组合，模拟各自走出的轨迹，按打分选最优。

关键在"动态窗口"四个字——不是全速度空间乱采，只采当前速度 ± 加速度限制能到达的范围：

v ∈ [v_now - acc_lim·dt, v_now + acc_lim·dt]
ω ∈ [ω_now - acc_lim_th·dt, ω_now + acc_lim_th·dt]

每条模拟轨迹按三个项打分：

score = α·goal_bias(冲向目标) + β·path_bias(贴住全局路径) + γ·occ_dist(躲开障碍)

真实参数（dwa_local_planner_params.yaml）：

goal_distance_bias: 40.0    # 冲目标权重
path_distance_bias: 30.0    # 贴路径权重
occdist_scale: 0.05         # 躲障碍权重
sim_time: 3                 # 轨迹推演时长
vx_samples: 20              # v 采样数
vy_samples: 10
vtheta_samples: 13          # ω 采样数

注意三个 bias 的量级差异——躲障碍权重只有 0.05，不是因为不重要，而是因为障碍代价的数值本身很大，权重要反着配。打分权重的绝对数值没有意义，比例才有意义，这是所有采样类规划器调参的共同坑。

优缺点：反应快、实现简单，但只看轨迹末端打分，路径糙，容易在窄道里摆动。

2.2 TEB 时间弹性带

TEB 把轨迹当成一根橡皮筋：两端固定（起点、终点），中间被障碍顶开，用图优化把整条轨迹"弹"到最短最平滑。

优化目标是加权求和：

f = Σ(weight_optimaltime·(Δt_i)²  +  weight_obstacle·obstacle_penalty
    + weight_kinematics_nh·kinematics_penalty + ...)

真实参数（teb_local_planner_params.yaml）：

dt_ref: 0.3                  # 位姿点时间间隔
min_obstacle_dist: 0.27      # 与障碍期望最小距离
inflation_dist: 0.6          # 障碍缓冲区，要比 min_obstacle_dist 大才有用
weight_optimaltime: 1        # 省时间权重（"直道加速"）
weight_obstacle: 100         # 离障碍远
weight_kinematics_nh: 1000   # 运动学约束（非完整约束）
weight_kinematics_turning_radius: 1   # 最小转向半径
max_vel_x: 0.4
max_vel_theta: 0.3
min_turning_radius: 0.5      # 阿克曼转向半径

经验：TEB 很适合转弯多的场景——它把路径上每个位姿的姿态都纳入优化，连续转弯时优势明显。调参上 min_obstacle_dist 调大更保守但窄道过不去，weight_optimaltime 调大更快但贴着障碍走，两边要平衡。

DWA vs TEB 一句话：采样穷举 vs 优化迭代。DWA 快但糙，TEB 平滑但吃参数、转弯场景强。

---

第 3 层：全局规划 + 状态估计

3.1 A* / Dijkstra

A* 在栅格地图上从起点向外扩散搜索，每个格子记录"从起点到这里的代价"。Dijkstra 均匀扩散所以慢；A* 多一个启发函数 f = g + h，h 是到目标的估计距离（通常欧氏距离），朝着目标方向的格子优先扩展，所以快得多。

RRT / RRT* 是采样式搜索：随机撒点、连接最近邻、逐步长出树。不用建栅格，适合高维空间和复杂环境，RRT* 加了"择优父节点 + 重连接"，渐近最优。

经验：全局路径可调的东西不多，调起来效果也不明显——机器人大部分时间在跑局部规划，全局路径只要不绕远、不穿障碍就够用，真正吃手感的是局部规划器。

3.2 卡尔曼滤波 / EKF

卡尔曼滤波解决一个问题："模型说我在这"和"传感器说我在这"不一致时，信谁？

答案是加权融合，权重看谁更可信：

预测步：x_pred = A·x_prev            （模型往前走一步）
更新步：x_new = x_pred + K·(z - H·x_pred)   （用量测修正）
K = P·Hᵀ / (H·P·Hᵀ + R)             （卡尔曼增益，量测噪声 R 越小 K 越大）

两个量的直觉：

• 先验估计：纯靠运动模型预测的位置
• 卡尔曼增益 K：传感器可信度。K 大 → 量测噪声小、更信传感器；K 小 → 更信模型

真实代码（竞赛机器人常用的 KalmanTracker 类，8 状态 4 量测，匀速模型跟踪目标框）：

// 状态: x, y, w, h, vx, vy, vw, vh
kf.transitionMatrix = (cv::Mat_<float>(8, 8) <<
    1,0,0,0, dt,0,0,0,
    0,1,0,0, 0, dt,0,0,   // 匀速模型：位置 += 速度·dt
    0,0,1,0, 0, 0,dt,0,
    0,0,0,1, 0, 0,0,dt,
    0,0,0,0, 1,0,0,0,
    0,0,0,0, 0,1,0,0,
    0,0,0,0, 0,0,1,0,
    0,0,0,0, 0,0,0,1);
kf.processNoiseCov  = cv::Mat::eye(8, 8, CV_32F) * 1e-2;   // 过程噪声 Q
kf.measurementNoiseCov = cv::Mat::eye(4, 4, CV_32F) * 1e-1; // 量测噪声 R

经验：卡尔曼 + PID 是很常见的组合——KF 把噪声大的量测磨平，给 PID 干净的反馈值。我们做的激光打击移动靶项目就是这套：KF 估计目标的位置和速度，PID 跟靶。工程细节：目标丢失时继续用 predict() 预测撑几帧，丢超过 50 帧才放弃重找——预测步就是卡尔曼的"追丢容错"。

EKF = 非线性系统的卡尔曼。状态转移或量测方程非线性时，在每步工作点做雅可比线性化，其余流程不变。SLAM 里它是绝对主力：激光 SLAM、FAST-LIO 的迭代卡尔曼滤波内核都是它。LQR/MPC 想知道"我在哪"，也靠它。

---

第 4 层：最优控制（重点）

前几层是"纠错"，最优控制是"算总账"——把未来一段时间的表现写成代价函数，一次性解出最优控制序列。模型利用程度从这层开始明显上升。

4.1 LQR

LQR 的问题设定：线性系统 x' = Ax + Bu，代价函数

J = Σ xᵀQx + uᵀRu

Q 是偏差的代价，R 是控制量的代价。解出来是状态反馈：

u = u_r + K·e

前馈 u_r + 反馈 K·e。K 是常数矩阵，解一次用一辈子。

真实实现（lqr_purepursuit.cpp）。先按 30Hz 控制周期把连续模型离散化：

Eigen::Matrix3d A = Eigen::Matrix3d::Identity();
A(0, 2) = -u_r[0] * sin(s_d[2]) * dt_;
A(1, 2) =  u_r[0] * cos(s_d[2]) * dt_;    // 单车模型线性化

Eigen::MatrixXd B = Eigen::MatrixXd::Zero(3, 2);
B(0, 0) = cos(s_d[2]) * dt_;
B(1, 0) = sin(s_d[2]) * dt_;
B(2, 1) = dt_;                            // 输入: [v, ω]

再迭代解离散黎卡提方程求 K：

Eigen::Matrix3d P = Q_;
for (int i = 0; i < max_iter_; ++i) {
    Eigen::Matrix2d temp = R_ + B.transpose() * P * B;
    Eigen::Matrix2d temp_inv = temp.ldlt().solve(Eigen::Matrix2d::Identity());
    P_ = Q_ + A.transpose()*P*A - A.transpose()*P*B*temp_inv*B.transpose()*P*A;
    if ((P - P_).array().abs().maxCoeff() < eps_iter_) break;  // 收敛判定
    P = P_;
}
Eigen::MatrixXd K = -(R_ + B.transpose()*P_*B).inverse() * B.transpose() * P_ * A;

黎卡提方程在算什么：“未来账单”。P 是"偏差不消，未来要付的总账"，迭代算账算到收敛，K 就是最优纠偏力度。"预测∞步"是常见的理解误区——LQR 不是看得远，是"算总账时假装时间没有尽头，换来一个常数 K"。

参数（lqr_params.yaml 的典型配置）：

Q_1: 5.00   # 位置偏差代价，调大更贴合全局路径
Q_2: 5.00   # 角度偏差代价，调大转弯响应更快
R_1: 4.0    # 线速度修正代价，调大更尊重参考速度
R_2: 25.00  # 角速度修正代价，调大运动更平滑，数值过高影响转弯

注意 R2 > R1：控制器不舍得打方向，这就是"稳"的来源。Q 大 R 小的控制器激进、跟线紧但抖；Q 小 R 大的控制器佛系、平滑但偏差大。

整个节点是"纯追踪 + LQR"混合体，几个工程细节值得抄：

1. 参考角用 PCA 平滑：路径点噪声大，直接算切线方向会爆炸。取目标点前后 half_win_size=5 个点做 PCA 主方向当参考角，注释里写得明白：“太小的话，路径并非严格平滑，参考角度值会爆炸”。
2. 大角度偏差先原地自旋：|e_theta| > 1.40 rad（约 80°）时 LQR 的线性化模型失效，直接让位，只用纯角度 P 控制原地转到误差变小再走。
3. 动态前视：前视距离 = lookahead_time(0.7375) × 当前速度，夹在 [0.26, 0.295]。
4. 曲率降速：vt = max_v / (kappaC·|κ| + 1)，弯越急速度越低。
5. 输出限幅：线速度每周期最多变 0.135，角速度最多变 max_w_inc，防止命令突变。

经验：空旷场景下 LQR 效果特别稳——没有障碍挤压、参考路径干净时，跟线精度和平顺性都很好。障碍多、路径频繁变化时，固定增益的局限就出来了。

4.2 MPC

MPC 和 LQR 是近亲，区别在三点：只算有限步、带硬约束、滚动重算。

• 每次只算未来 N 步的控制序列，但只执行第 1 步，下一周期用最新状态重算——“走一步看一步”
• LQR 解 ∞ 步有闭式解 K；MPC 解 N 步没有闭式解，每步在线做数值优化
• LQR 的约束靠输出限幅兜底；MPC 把 umin/umax 直接写进优化问题，硬约束自带

真实参数（mpc_config.yaml）：

T: 0.2      # 采样周期 [s]
N: 10       # 预测步数（注释：N > 20 太耗 CPU）
Q1: [[300,0,0],[0,300,0],[0,0,400]]   # 状态代价，角度 400 最大
QN1: [[300,0,0],[0,300,0],[0,0,400]]  # 终端代价
R1: [[300,0],[0,400]]                 # 控制代价
umin1: [-0.8, -2.24]                  # 最小线速度/角速度
umax1: [ 0.8,  2.24]                  # 最大线速度/角速度

• N=10, T=0.2s → 预测时域 2 秒
• Q1 里角度 400 最大：转弯的姿态精度比位置更苛刻
• 文件注释直接写了 “N > 20 seems to require too much CPU power”——决策变量数随 N 线性涨，求解复杂度近似平方级，N 翻倍算力约 4 倍。这就是 MPC 在实车上的最大代价

内部流程：把控制问题翻译成"几十个决策变量的带约束优化"（决策变量 = N × 控制维度），建超图方程，交给 IPOPT 之类的内点法求解器迭代，用上一帧的解做 warm start 加速收敛。

LQR 和 MPC 的异同，分开看就清楚了：

• 优化目标：两者同构——都是 Σ xᵀQx + uᵀRu，MPC 的 Q1/R1 就是 LQR 的 Q/R
• 求解方式：LQR 算 ∞ 步，有闭式解 K，解一次用一辈子；MPC 只算 N 步，没有闭式解，每步在线数值求解
• 约束处理：LQR 没有硬约束，输出只能靠限幅兜底；MPC 把 umin/umax 直接写进优化问题，约束自带
• 环境适应：LQR 的 K 是常数，固定不重算；MPC 滚动重算，环境一变立刻跟上
• 前馈位置：LQR 一目了然 u = u_ff + K·e；MPC 里前馈被优化器融合，写不出单独项

一个常见疑问：MPC 的前馈在哪？LQR 里前馈反馈分开写，MPC 里优化器把两者融合了——约束、参考、代价一起解，没有独立的 u_ff。

---

第 5 层：非线性控制（进阶，飞控方向）

前几层都建立在"系统近似线性"上。这层处理真正的非线性，主要用在飞控。

5.1 反馈线性化

找一个状态变换把非线性系统"掰直"，掰直后直接套 LQR/PID。和 LQR 的区别：LQR 是每步在工作点局部线性化（泰勒展开），反馈线性化是全局掰直——准得多，但完全吃模型精度。一句话：模型准它神，模型不准它崩。

5.2 滑模控制 SMC

设计一个"滑面"（误差 + 误差变化率的组合），控制律把状态死命推到滑面上，然后沿面滑到零。鲁棒性是天花板级别——模型误差、外部扰动全扛。代价是抖振：控制量高频来回切换，工程上要加边界层（把开关函数换成饱和函数）消抖。类比：珠子掉漏斗，沿缝滑到底。

5.3 反步法 Backstepping

剥洋葱。从最外层子系统开始，每层把状态当"虚拟控制量"设计，让下一层去追这个目标，逐层往里推。无人机姿态-位置级联结构天然契合。设计是机械式递推，工程照着套路走。

5.4 INDI 增量非线性动态逆

不算绝对控制量，只算增量：用实测角加速度 + 增量模型，只输出"现在偏了一点，再修一点点"。因为差分掉了模型的主体部分，天然抗模型误差和慢变扰动。ArduPilot 飞控核心用 INDI，PX4 用级联 PID，两条技术路线并存。

---

第 6 层：强化学习（无模型路线）

到这里模型利用程度降到最低——不建模，直接用数据学。先看家族谱：

Q-Learning（查表法）
  → 神经网络替代表 → DQN（离散动作）
  → 直接学策略 → Policy Gradients（连续动作，但方差大）
  → 加 Critic 评价 → Actor-Critic
  → 连续动作版 DQN → DDPG（不稳）
  → 双网络 + 延迟更新 → TD3（稳）
  → 裁剪限制更新幅度 → PPO（最常用最稳）
  → 熵鼓励探索 → SAC（上限最高，超参数多）

• Q-Learning：维护 Q 表记录"状态-动作"的长期收益，用贝尔曼方程迭代更新。Q-Learning 是 off-policy（学的与做的可不同），Sarsa 是 on-policy（学自己实际做的，更保守）。
• DQN：神经网络替代 Q 表，加经验回放（打破数据相关性）+ 目标网络（防震荡）。能处理图像等高维输入。
• Policy Gradients：直接学策略 π(a|s)，动作回报高就加大概率。能处理连续动作，但方差大、训练不稳定。
• Actor-Critic：Actor 选动作，Critic 评价动作。Critic 用优势函数 A = r + γV(s') - V(s) 告诉 Actor"这个动作比平均水平好多少"。
• DDPG / TD3：DQN 的连续动作版。TD3 用双 Q 网络（取最小，防过估计）+ 延迟策略更新 + 目标策略平滑，稳多了。
• SAC：TD3 + 熵奖励鼓励探索，性能上限最高，但超参数多。

一句话定位：PPO 是"稳"，SAC 是"猛"。

PPO 详解

PPO 解决策略梯度方法的通病：更新太猛，训练崩盘。

核心是裁剪。定义新旧策略的概率比 r = π_new(a|s) / π_old(a|s)，把它限制在 [1-ε, 1+ε]（ε 常用 0.2），更新幅度超出范围就截断梯度——给策略更新装了个安全阀：

L_clip = E[ min( r·A,  clip(r, 1-ε, 1+ε)·A ) ]

完整损失是三部分加权：

L(θ) = L_clip - c₁·L_value + c₂·H(π)

• L_clip：策略损失，裁剪限制更新幅度
• L_value：价值损失，让 Critic 预测更准
• H(π)：熵奖励，鼓励探索，防止过早收敛到次优策略

其他关键参数：gamma=0.99（折扣因子，越接近 1 越看重长期回报）、lambda=0.95（GAE 优势估计的折中，越小方差越小、越大偏差越小）。

训练流程（on-policy）：

1. 用当前策略和环境交互，收集一批轨迹 (s, a, r, s')
2. 计算累计回报，用 Critic 算优势 A
3. 保存旧策略概率 π_old
4. 多轮 mini-batch 更新：
   - 算新策略概率比 r，Clip 目标 → 更新 Actor
   - MSE 损失 → 更新 Critic

和 MPC 的关系：MPC 要模型 + 约束保证，PPO 不要模型但要数据；MPC 数学最优，PPO 数据最优。现实中混合用——RL 学高层避障策略，底层还是传统导航兜底。

---

第 7 层：辅助与理论（按需深入）

• APF 人工势场：目标引力 + 障碍斥力。5 分钟能实现，但局部极小会卡住，已被 DWA/TEB 取代。
• 模糊控制：if-else 语言规则表，不用建模型，工业现场用得多。
• GA / PSO：不是控制算法，是自动调参工具——参数组合当基因进化/鸟群觅食，仿真里自动搜最优参数。
• H∞：把扰动当最坏敌人，优化最坏情况。保守但保证鲁棒，航空航天用。
• LQG：LQR + 卡尔曼滤波，最优估计 + 最优控制，分离定理保证两者可以分开设计。
• MRAC：在线学模型误差并补偿，飞机气动参数变化时用。
• MPC 变体：NMPC、GPC、tube-MPC——先把基础 MPC 吃透再谈。

---

总结

回头看这条学习路线，控制算法不是孤立的点，而是一条连续的光谱：

• PID/纯追踪：不要模型，靠误差直觉，调参靠经验
• DWA/TEB：要局部感知，采样或优化选轨迹
• LQR/MPC：要模型，模型越准控制越优，MPC 用算力换约束和自适应
• PPO/SAC：不要模型，用数据替代模型，训练换泛化

模型利用程度递减，数据需求递增。抓住这条主线，学新算法时先问一句"它站在光谱的哪个位置"，理解成本会低很多。

本文供个人学习使用，如有问题欢迎交流。