【控制算法详解】从 PID 到 PPO，一条路线讲完机器人控制的主流算法—

【控制算法详解】从 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（微分）：看误差的变化趋势，误差正在快速变小就刹车，抑制超调。

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

doublecalculate(doubleerror){doubleP=Kp_*error;integral_+=error;if(std::abs(integral_)>integralLimit_)// 积分限幅integral_=std::copysign(integralLimit_,integral_);doubleI=Ki_*integral_;doublederivative=error-lastError_;// 误差差分近似微分doubleD=Kd_*derivative;lastError_=error;returnP+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.cppdes_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.cppdoublegetLookAheadDistance(doublevt,doublelookahead_time,doublemin_lookahead_dist,doublemax_lookahead_dist){doublelookahead_dist=fabs(vt)*lookahead_time;// 速度 × 时间returnclamp(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:10vtheta_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.4max_vel_theta:0.3min_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, vhkf.transitionMatrix=(cv::Mat_<float>(8,8)<<1,0,0,0,dt,0,0,0,0,1,0,0,0,dt,0,0,// 匀速模型：位置 += 速度·dt0,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;// 过程噪声 Qkf.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(inti=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"混合体，几个工程细节值得抄：

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

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

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

企业官网建设流程全解析