【控制算法详解】从 PID 到 PPO一条路线讲完机器人控制的主流算法——附真实代码和调参经验前言做机器人绕不开控制。这篇文章把控制算法按学习顺序串成一条线从最基础的 PID 一直讲到强化学习的 PPO。每个算法不讲满纸公式讲清楚三件事它到底在干什么、关键参数怎么调、真实代码里长什么样。文中所有代码片段和参数都来自实际工程PX4 无人机导航代码、ROS 仿真平台的 MPC 配置、竞赛机器人的卡尔曼PID 跟踪代码、开源小车项目的 TEB/DWA 配置。贯穿全文有一条主线模型利用程度递减数据需求递增。PID 需要一个模型吗不需要但调参靠经验。LQR/MPC 要模型而且模型越准控制越好。PPO 不要模型但要海量数据。理解这条线学新算法时会快很多。第 1 层经典反馈经典反馈是控制的地基。它不需要知道系统的模型只盯着误差干活误差大就用力纠误差小就少纠。1.1 PIDPID 是误差乘以三个系数u(t) Kp·e(t) Ki·∫e(t)dt Kd·d e(t)/dtP比例误差 × Kp 直接输出。误差大推得多误差小推得少但永远追不上目标留下稳态误差。I积分把历史误差累计起来专门消掉 P 留下的那点残差。D微分看误差的变化趋势误差正在快速变小就刹车抑制超调。工程里最常见的写法是位置式和增量式。竞赛代码里有一个典型的离散实现注意它的积分限幅——这是工程必备防止积分饱和后输出失控doublecalculate(doubleerror){doublePKp_*error;integral_error;if(std::abs(integral_)integralLimit_)// 积分限幅integral_std::copysign(integralLimit_,integral_);doubleIKi_*integral_;doublederivativeerror-lastError_;// 误差差分近似微分doubleDKd_*derivative;lastError_error;returnPID;}调参经验大多数场景一个 P 就够或者 PI、PD 组合。I 和 D 一般都要调得很小很小——I 大了超调D 大了被噪声激出抖动。项目里跟靶云台用的是eKp1.0, eKi0.01, eKd0.03P 是 I 的 100 倍。整定顺序固定 P→I→DP 定基本行为I 补残差D 最后加顺序反了参数互相打架。双环结构移动机器人很少单环用 PID。PX4 无人机常用的px4ctrl就是典型的位置-速度双环// controller.cppdes_accdes.aKv.asDiagonal()*(des.v-odom.v)Kp.asDiagonal()*(des.p-odom.p);des_accEigen::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_distfabs(vt)*lookahead_time;// 速度 × 时间returnclamp(lookahead_dist,min_lookahead_dist,max_lookahead_dist);}前视距离 当前速度 × 前视时间再夹在[0.26, 0.30]之间。“看多远要匹配刹得住多远”——速度快看远弯急看近。调参经验前视太近 → 蛇形抖动太远 → 切弯内切。实际工程里纯粹用纯追踪思想跟线时前瞻距离直接写死0.6m转向用航向角误差 横向偏差的 PD 式输出效果稳定。这也是为什么纯追踪是工程首选参数少、鲁棒、好解释。1.3 StanleyStanley 是对纯追踪的理论改进转向角由两个量组成δ 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.yamlgoal_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.yamldt_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* / DijkstraA* 在栅格地图上从起点向外扩散搜索每个格子记录从起点到这里的代价。Dijkstra 均匀扩散所以慢A* 多一个启发函数f g hh是到目标的估计距离通常欧氏距离朝着目标方向的格子优先扩展所以快得多。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.processNoiseCovcv::Mat::eye(8,8,CV_32F)*1e-2;// 过程噪声 Qkf.measurementNoiseCovcv::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 LQRLQR 的问题设定线性系统x Ax Bu代价函数J Σ xᵀQx uᵀRuQ 是偏差的代价R 是控制量的代价。解出来是状态反馈u u_r K·e前馈 u_r 反馈 K·e。K 是常数矩阵解一次用一辈子。真实实现lqr_purepursuit.cpp。先按 30Hz 控制周期把连续模型离散化Eigen::Matrix3d AEigen::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 BEigen::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, ω]再迭代解离散黎卡提方程求 KEigen::Matrix3d PQ_;for(inti0;imax_iter_;i){Eigen::Matrix2d tempR_B.transpose()*P*B;Eigen::Matrix2d temp_invtemp.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;// 收敛判定PP_;}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_size5个点做 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 MPCMPC 和 LQR 是近亲区别在三点只算有限步、带硬约束、滚动重算。每次只算未来 N 步的控制序列但只执行第 1 步下一周期用最新状态重算——“走一步看一步”LQR 解 ∞ 步有闭式解 KMPC 解 N 步没有闭式解每步在线做数值优化LQR 的约束靠输出限幅兜底MPC 把umin/umax直接写进优化问题硬约束自带真实参数mpc_config.yamlT:0.2# 采样周期 [s]N:10# 预测步数注释N 20 太耗 CPUQ1:[[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]# 最大线速度/角速度N10, T0.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ᵀRuMPC 的Q1/R1就是 LQR 的Q/R求解方式LQR 算 ∞ 步有闭式解K解一次用一辈子MPC 只算 N 步没有闭式解每步在线数值求解约束处理LQR 没有硬约束输出只能靠限幅兜底MPC 把umin/umax直接写进优化问题约束自带环境适应LQR 的K是常数固定不重算MPC 滚动重算环境一变立刻跟上前馈位置LQR 一目了然u u_ff K·eMPC 里前馈被优化器融合写不出单独项一个常见疑问MPC 的前馈在哪LQR 里前馈反馈分开写MPC 里优化器把两者融合了——约束、参考、代价一起解没有独立的 u_ff。第 5 层非线性控制进阶飞控方向前几层都建立在系统近似线性上。这层处理真正的非线性主要用在飞控。5.1 反馈线性化找一个状态变换把非线性系统掰直掰直后直接套 LQR/PID。和 LQR 的区别LQR 是每步在工作点局部线性化泰勒展开反馈线性化是全局掰直——准得多但完全吃模型精度。一句话模型准它神模型不准它崩。5.2 滑模控制 SMC设计一个滑面误差 误差变化率的组合控制律把状态死命推到滑面上然后沿面滑到零。鲁棒性是天花板级别——模型误差、外部扰动全扛。代价是抖振控制量高频来回切换工程上要加边界层把开关函数换成饱和函数消抖。类比珠子掉漏斗沿缝滑到底。5.3 反步法 Backstepping剥洋葱。从最外层子系统开始每层把状态当虚拟控制量设计让下一层去追这个目标逐层往里推。无人机姿态-位置级联结构天然契合。设计是机械式递推工程照着套路走。5.4 INDI 增量非线性动态逆不算绝对控制量只算增量用实测角加速度 增量模型只输出现在偏了一点再修一点点。因为差分掉了模型的主体部分天然抗模型误差和慢变扰动。ArduPilot 飞控核心用 INDIPX4 用级联 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-CriticActor 选动作Critic 评价动作。Critic 用优势函数A r γV(s) - V(s)告诉 Actor这个动作比平均水平好多少。DDPG / TD3DQN 的连续动作版。TD3 用双 Q 网络取最小防过估计 延迟策略更新 目标策略平滑稳多了。SACTD3 熵奖励鼓励探索性能上限最高但超参数多。一句话定位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(π)熵奖励鼓励探索防止过早收敛到次优策略其他关键参数gamma0.99折扣因子越接近 1 越看重长期回报、lambda0.95GAE 优势估计的折中越小方差越小、越大偏差越小。训练流程on-policy1. 用当前策略和环境交互收集一批轨迹 (s, a, r, s) 2. 计算累计回报用 Critic 算优势 A 3. 保存旧策略概率 π_old 4. 多轮 mini-batch 更新 - 算新策略概率比 rClip 目标 → 更新 Actor - MSE 损失 → 更新 Critic和 MPC 的关系MPC 要模型 约束保证PPO 不要模型但要数据MPC 数学最优PPO 数据最优。现实中混合用——RL 学高层避障策略底层还是传统导航兜底。第 7 层辅助与理论按需深入APF 人工势场目标引力 障碍斥力。5 分钟能实现但局部极小会卡住已被 DWA/TEB 取代。模糊控制if-else 语言规则表不用建模型工业现场用得多。GA / PSO不是控制算法是自动调参工具——参数组合当基因进化/鸟群觅食仿真里自动搜最优参数。H∞把扰动当最坏敌人优化最坏情况。保守但保证鲁棒航空航天用。LQGLQR 卡尔曼滤波最优估计 最优控制分离定理保证两者可以分开设计。MRAC在线学模型误差并补偿飞机气动参数变化时用。MPC 变体NMPC、GPC、tube-MPC——先把基础 MPC 吃透再谈。总结回头看这条学习路线控制算法不是孤立的点而是一条连续的光谱PID/纯追踪不要模型靠误差直觉调参靠经验DWA/TEB要局部感知采样或优化选轨迹LQR/MPC要模型模型越准控制越优MPC 用算力换约束和自适应PPO/SAC不要模型用数据替代模型训练换泛化模型利用程度递减数据需求递增。抓住这条主线学新算法时先问一句它站在光谱的哪个位置理解成本会低很多。本文供个人学习使用如有问题欢迎交流。