接触最优控制和轨迹规划这几年我最大的感受是所有看似炫酷的机器人动作——倒立摆原地起立、无人车在停车场里绕弯、机械臂在狭小空间里穿行——最后都可以归到同一类数学问题在一个系统动态约束下找一条可行路径和一组控制输入让某个代价最小。去年我把倒立摆上翻控制、车辆运动学约束路径规划和离散点参数化这三块内容完整过了一遍边做仿真边写笔记踩了不少坑也理清了它们之间千丝万缕的联系。这篇文章就把这份学习笔记整理出来从原理推导到代码层面的落地细节都会提到适合正在学最优控制却不知道怎么上手、或者卡在“仿真能跑但实物就崩”阶段的同学。先说清楚一件事最优控制不是某一种具体算法而是一种看待问题的框架。倒立摆上翻用的是能量法加LQR切换车辆路径规划用的是非完整约束下的几何曲线加数值优化离散点参数化则是把连续问题交给非线性求解器去算。三者看起来八竿子打不着但它们共享同一套语言状态、控制、约束、代价函数。把这套语言吃透你换任何一个平台都能快速上手。1. 最优控制与轨迹规划两件事一种底子1.1 最优控制到底在“最优”什么先抛开公式用生活里的例子把概念钉住。想象你开一辆手动挡的车面前是一段有红绿灯、有坡度、有弯道的路你知道起点和终点也知道交通规则那你要回答的问题是每个时刻该挂几挡、踩多少油门、什么时候刹车才能在绿灯亮起时不耽误、上坡时不熄火、下坡时不超速同时油耗还最低这就是最优控制问题。它的三个组成部分缺一不可描述车怎么动的微分方程、限制你行为的约束比如限速、挡位范围、以及衡量好坏的性能指标比如油耗。放到数学语境里问题一般写成这种形式给定系统状态方程 x_dot(t) f(x(t), u(t), t)找到控制输入 u(t)使得性能指标 J φ(x(T)) ∫₀ᵀ L(x(t), u(t), t) dt 最小同时满足状态约束、输入约束和边界条件。这里 φ 是终端代价L 是过程代价。如果你希望时间最短L 就是1如果希望能量最小L 就取控制量的二次型如果希望轨迹平滑可以在 L 里加入状态变化率的惩罚项。解决这个问题的经典路径分三条。第一条是变分法和庞特里亚金极小值原理通过引入协态变量把最优控制问题转化为两点边值问题然后用打靶法或松弛法求解这就是“间接法”。第二条是动态规划从末端倒着推Bellman方程算出一个值函数再用值函数反推控制律好处是能处理全局最优坏处是状态维度一高就直接内存爆炸。第三条是直接法把状态和控制都离散到时间网格上把原来无限维的优化问题变成有限维非线性规划NLP然后用序列二次规划SQP或内点法直接求解。实际工程里90%的轨迹规划跑的都是第三条路因为它对初值不敏感、实现简单对复杂约束的处理能力也强。1.2 轨迹规划是怎么被“最优控制”接管的轨迹规划和最优控制在很多场合被混着用实际上它们是有分工的。传统机器人学里轨迹规划关心的是“几何上怎么从 A 点到 B 点”插值出一条不撞障碍物的路径再单独做速度规划。最优控制则更进一步把“几何”和“动态”放在同一个优化问题里考虑我不仅要走一条不撞的路径还要让这条路径在车的加速度上限、摆的力矩上限、机械臂的关节速度上限这些约束下尽量省时、省力、平滑。举个例子你就明白了。给无人车一排离散路点最省事的做法是拿三次样条把这些点穿起来再检查一下曲率是否超过转向极限。这一套做下来很快但结果经常是“能走但不是最优的”可能某段曲率刚刚压线导致车必须频繁减速可能路径在直道上扭来扭去坐车的很快晕车。如果你换用最优控制框架把这些离散点不是当成“必须经过的钉子”而是当成“靠近期望位置的软约束”同时把曲率、加速度、jerk加加速度写进代价函数求解器给出的轨迹就会在“贴近路点”和“开起来舒服”之间自动找平衡。我做轨迹规划时有个习惯先判断这个问题是“路径问题”还是“轨迹问题”。如果机器人速度低到可以忽略动态特性那我只做路径规划只要速度不可忽略比如倒立摆上翻过程中摆杆角速度很大、车辆过弯时横向加速度明显就必须把动态约束纳进规划里。这个判断直接决定了工具选型做错了会在后面花大量时间调那些本来不该出现的bug。2. 倒立摆上翻控制从“站不起来”到“稳稳立住”2.1 为什么上翻比纯稳定更难倒立摆稳定控制在控制论教材里是一道经典题摆杆在竖直向上的不稳定平衡点附近用线性化模型设计LQR或者极点配置就能让它立住。但这里有个隐藏前提——摆杆得先“几乎直立”线性化模型才成立。真实系统摆杆一开始是自然下垂的你要让它从竖直向下稳定平衡点运动到竖直向上不稳定平衡点这中间摆角变化了180度完全是强非线性过程。我最初犯的错误就是天真地想直接给一个大扭矩把摆甩上去不行吗结果发现力矩小了甩不上去力矩大了摆过头到顶端时机身已经冲出去老远。原因在于倒立摆上翻本质上是一个能量问题摆杆从下垂位置到上立位置重力势能增加了 2mgl以摆杆底部为转轴质心升高了2l只有把足够的能量注入摆杆它才能越过中间的水平位置。而控制输入既要注入能量又要避免注入太多导致冲过头这就必须对能量做跟踪控制而非简单的开环猛甩。打个比方你推一个秋千上的小孩如果每次都在正确时机、正确力度推一把秋千会越荡越高推的时机不对力度再大也可能白费。倒立摆上翻就是在一个极度简化版的“秋千系统”上学会踩准时机推。2.2 能量法让摆自己“荡”上去实际工程中做上翻用得最多的是 Astrom 和 Furuta 提出的能量控制思路核心非常直观定义摆杆的相对能量偏差为 Ẽ E − E_target其中目标能量 E_target 是摆杆在竖直向上且静止时的能量。单级旋转倒立摆的机械能量为 E 1/2 m l² θ_dot² m g l (1 − cos θ)竖直向上的时候 θ 0能量取最小值0所以可以把目标能量设为0。控制律的设计目标是让 Ẽ 指数衰减也就是 Ė̃ −k Ẽ。对 E 求导可以发现能量变化率正比于摆角速度与控制量的乘积于是反推出来的控制律有这样一个结构u k_p · sat(Ẽ) · cos(θ)其中 sat 是饱和函数用来限制输出力矩乘上 cos(θ) 是让摆在水平位置附近少用力、在接近竖直时精确修正能量。这个公式虽然看着简单实际调参时你会发现 k_p 的选取决定了摆杆是“悠悠地荡上去”还是“猛甩过头”。在仿真环境里我一般先跑这样一段伪代码观察能量变化# 倒立摆上翻能量控制离散时间仿真 E_target 0.0 k_p 2.5 # 能量项增益需根据摆杆惯量标定 u_max 8.0 # 电机最大输出力矩 for step in range(int(t_end / dt)): # 从传感器读取摆角与角速度 theta, theta_dot read_state() # 计算当前机械能量 E 0.5 * m * l**2 * theta_dot**2 m * g * l * (1 - math.cos(theta)) E_tilde E - E_target # 能量控制律带饱和 u k_p * math.tanh(E_tilde) * math.cos(theta) u np.clip(u, -u_max, u_max) # 积分更新状态...调参数的经验是k_p 太小摆杆需要在返工好几次才能攒够能量上翻慢k_p 太大能量一超调就冲过头到稳定区时角速度还很大切换后LQR根本拉不回来。我个人喜欢先把 k_p 从1开始逐步增大观察 E_tilde 是否单调递减如果出现振荡则往回退半个量级。2.3 LQR接管从切换条件到参数初值能量控制只能把摆杆“送”到竖直附近真正让它站住的是切换后的线性控制器。切换触发条件一般写成角度和角速度的联合阈值|θ| θ_switch 且 |θ_dot| ω_switch。这两个值不能拍脑袋定我的做法是在仿真里先跑通纯LQR测出它能够稳定的最大初始摆角偏移和角速度偏移把这个范围的六折作为切换阈值留出裕量。LQR参数初值也一样有套路。倒立摆线性化模型是车位移 x、车速 x_dot、摆角 θ、摆角速度 θ_dot 四个状态输入是小车加速度或者力。标准做法是取 Q diag(q1, q2, q3, q4)R 为单个输入权重用 lqr() 函数解Riccati方程得到反馈增益。调参时先固定 R1把 q3对应摆角设得比 q1对应车位移大一个数量级比如 q3100q110这样控制器优先保证摆杆竖直其次才管车的位置。为什么这样因为上翻刚完成时最大的威胁是摆杆倒下来位置偏移可以靠后续慢慢回正但如果优先管位置很小的位置误差就会引发很大的加速度直接把摆杆甩倒。真机调试时还有一个切换瞬间力矩跳变的坑。能量控制输出的力矩和LQR输出的力矩大概率不在同一个量级切换那一刻电机会猛地突跳一下轻则听到一声异响重则把摆杆震脱。解决办法是在切换点附近做线性过渡也就是在切换前的0.2秒内把控制量从能量控制输出线性渐变到LQR输出或者统一加一个低通滤波器。这个细节仿真里经常看不出来但到真机上极其关键。3. 车辆运动学约束路径规划不是每条路都能开3.1 非完整约束车为什么不能横着走如果你开车进了一个特别窄的巷子你会立刻理解什么叫非完整约束车身长度的限制让车无法直接横向平移只能在“前进/后退”和“转向”的组合中腾挪。数学上阿克曼转向车辆的运动学模型为 x_dot v·cos θy_dot v·sin θθ_dot (v/L)·tan δ其中 L 是轴距δ 是前轮转角。这三条微分方程说明车辆的速度方向始终和车身朝向一致且转向半径由 R_min L / tan(δ_max) 约束不可能原地转向或者走任意曲率的路径。这种约束在控制理论里被称为非完整约束意思是它不能写成只对位置起作用的代数等式约束而是作用在广义速度上的微分约束。一个直接后果是给你一堆任意分布的离散点拿普通样条穿过后画出来的路径虽然光滑好看但车辆很可能根本开不出来——因为某几段曲率超过了 R_min 的倒数或者某些拐点要求车辆横向滑动。做车辆路径规划时我习惯先问三个问题有没有涉及倒车最大转向角对应最小转弯半径是多少离散点间距大概多少这三个问题决定了后续是用 Dubins 曲线、Reeds-Shepp 曲线还是混合A*加数值优化。3.2 Dubins/Reeds-Shepp满足最小转弯半径的几何解如果车辆只能前进而且转弯半径固定为 R_min那么在无障碍的平面上任意两个姿态位置加朝向之间的最短路径一定由圆弧和直线段组成。Dubins 证明了这种路径的组合只有六种类型比如 RSR、LSL、RSL、LSR 等其中 R 代表右转圆弧、L 代表左转圆弧、S 代表直线段。每条路径都是“在起始点以最小半径转、走直线、在以最小半径转”这样的结构几何关系可以用解析几何加少量数值迭代求出来。Reeds-Shepp 曲线把问题扩展到了允许倒车在拥挤停车场的场景里非常实用。它和 Dubins 的区别不在于多几种路径组合而在于引入了“反向段”倒车段使得路径可以是“前进-倒车-再前进”的形态。这也意味着车辆可以在更小的空间内完成转向代价是换挡次数变多实测中频繁切换前进/倒挡对执行器寿命不友好规划时可以在代价函数里对换挡次数加惩罚。我在项目里一般把 Dubins 当作理论下界如果某段路径连 Dubins 长度都缩短到一定程度说明空间确实不够需要引入倒车或者重新设计离散点。R_min 的取值要根据车型标定——理论轴距除以最大转角的正切值只是一个近似值真实的转向机构在接近最大转角时非线性明显安全做法是乘一个 1.2 的系数留出余量。3.3 离散路径点的平滑与曲率约束处理工程上拿到的高精地图路点通常是稀疏离散的可能来自GPS轨迹、高精地图的车道中心线或者上一级全局规划的采样点。直接操作这些点做几何路径最容易出现的问题是相邻线段方向突变成“折线”折线处的曲率无穷大车辆不可能跟上。我的处理流程分四步第一步把离散点做线性插值或最近邻匹配保证路径是连续折线。第二步用三次样条或五次多项式样条对折线做平滑样条的好处是 C2 连续曲率连续可导。第三步逐个采样样条上的曲率值检查是否超过 1/R_min如果不满足就把对应控制点微调后重新拟合。第四步把曲率信息传给速度规划曲率大的地方限制速度保证横向加速度 a v² / R 不超过舒适范围一般取 2~3 m/s²。这套流程里的坑在于单纯最小化样条曲率会让路径严重偏离原始路点尤其是弯道处样条为了减小曲率会“切内角”侵入对向车道。所以代价函数里必须同时包含“对原路点的贴合度”和“曲率平滑项”两者用权重 λ 平衡。我常用的做法是写成加权最小二乘min ‖P_smooth − P_ref‖² λ ∫ κ² dsλ 从0.01开始试看路径是否被拉得太弯。这个 λ 没有通用值因为它跟路点密度、弯道半径都相关只能用仿真场景测试后标定。4. 离散点参数化把连续优化变成计算机能解的问题4.1 为什么要做“离散点参数化”最优控制问题里状态和控制都是时间的连续函数真让计算机去“搜索”无限维的函数空间是不可能完成的任务。离散点参数化的本质是做一个有限维近似把时间轴切成一串网格点 t₀, t₁, …, t_N在每个网格点上定义状态 x_k 和控制 u_k原来微分方程约束 x_dot f(x, u) 就换成差分或者积分形式的等式约束代价函数里的积分换成求和。这样原来的连续最优控制问题就变成一个带有约束的大规模非线性规划问题。这个转换听起来朴素但里面有非常多的设计空间网格怎么分、状态怎么插值、约束怎么离散不同选择对应收敛速度和数值稳定性的巨大差异。最简单的做法是欧拉法离散即 x_{k1} x_k f(x_k, u_k) · Δt实现容易但精度低需要很密的网格才能满足约束。更精细的做法是直接配点法direct collocation在网格点之间用三次Hermite多项式插值并要求插值多项式在中点满足动力学方程这个方法在同样的网格密度下精度高很多是当前轨迹优化工具的主流选择。4.2 用多项式或样条参数化轨迹除了在时间网格上离散状态还有一种常见的做法是把整个轨迹用一组基函数表示比如多项式、B样条或者傅里叶级数优化变量从“每个时刻的状态值”变成“基函数系数”。这个思路对机械臂路径规划特别常用因为机械臂的任务经常是“从A点运动到B点保持抖动尽量小”这时候拿五次多项式 q(t) a₀ a₁t a₂t² a₃t³ a₄t⁴ a₅t⁵ 来参数化关节角度就很自然。五次多项式的好处是它有六个系数恰好可以匹配起始和终止两个时刻的位置、速度、加速度各三个条件解一个线性方程就能唯一确定轨迹。而且五次多项式的加加速度jerk是有界的连续函数不会出现三次多项式那种加速度突变。做机械臂直线轨迹规划或者圆弧轨迹规划时我基本都采用这个方法生成初始猜测轨迹再交给数值优化微调。用B样条的好处又不一样。B样条具有局部支撑性质移动一个控制点只影响附近一小段曲线不会把整条轨迹都搅乱。这在迭代优化时特别好用——求解器输出的轨迹如果某一段曲率超限直接微调对应控制点就行不用重算全局。代价是约束和目标函数的表达式相对复杂需要手推或者用符号运算工具生成雅可比。4.3 NLP求解约束处理与初值策略离散化之后的问题是一个典型的非线性规划问题变量数目从几百到几万不等。我用过的求解器包括 CasADi 加 IPOPT 的组合、MATLAB 的 fmincon以及专门针对轨迹优化的 TrajOpt、OCS2、CrocoDDL对于机械臂场景等。选型逻辑很简单如果问题规模小几十个变量fmincon 就够如果要做实时非线性模型预测控制就得用基于二次规划的 SQP 方法配合高效求解器如果动力学特别复杂而且需要高精度解CasADi 加 IPOPT 是稳妥之选。优化能否收敛70% 取决于初值给得好不好。我见过太多人把优化变量全设成0然后抱怨求解器不收敛——实际上非线性优化对初值天生敏感尤其是存在碰撞避免约束时从一个离可行域很远的初始点出发求解器很容易陷入局部极小或者直接发散。我的习惯是分阶段启动先跑一个不考虑复杂约束的简化版本拿到一条粗略轨迹把这条轨迹插值到优化网格上作为初值再开启完整约束重新求解。这个“热身启动”策略能解决大部分不收敛问题。约束的处理也讲究顺序。等式约束动力学、初始条件和不等式约束速度限制、力矩限制、障碍距离不应该一口气塞给求解器而是先从物理含义上检查是否冗余比如某些状态约束其实可以由输入约束推导出来就删掉。约束数量太多会让每次梯度计算和求逆都变慢而且劣化求解器的数值条件。实测经验是约束数量减少三分之一左右求解除以分钟计变成秒级且解的质量并不下降。5. 实操问题排查从仿真到落地最常踩的坑5.1 倒立摆上翻发散、切换抖动的排查问题1能量法控制下能量始终降不下去摆杆一直小幅摆动到不了竖直区域。这个大概率是增益 k_p 取得过小或者传感器数据噪声太大导致能量估计有偏。真机上先检查角度陀螺仪零点漂移把静止下垂时的能量归到0以后再判断。仿真里如果也复现多数是饱和函数设置得太保守输出力矩被限幅。问题2上翻成功后摆杆一直做大幅振荡切到LQR后还是稳不住。首先怀疑切换时刻的角速度阈值过大LQR在超出线性范围的状态下启动自然拉不回来。另一种可能是切换区间没有做滞回导致能量控制和LQR来回切换系统在两个控制器之间反复横跳。解决方法就是在切换阈值设计一个滞回区间比如进入LQR时要求 |θ| 25°退出时要求 |θ| 35°留出充足的回环。5.2 车辆路径曲率不连续与优化不收敛的排查问题1三次样条插值后的路径在个别点出现尖角曲率突变。这个往往是样条节点breakpoints选得不均匀离散点间距差距过大造成的。解决办法是先用等弧长重采样原始离散点再做样条拟合。问题2包含了曲率约束的NLP优化不收敛报“infeasible problem”。最常见的起因是最小转弯半径约束设置得比车辆实际能力还紧。我调过一次无人车项目理论最小转弯半径8米我直接用了8米做约束结果优化器一直找不到解后来放宽到10米立马正常。看起来像是“宽松了约束”但实际上8米是理论极限真实转向在极限附近摩擦系数下降根本跑不出这个曲率所以规划的“可行解”并不物理可行。5.3 常见问题速查表问题现象可能原因排查与解决倒立摆上翻过程中能量发散反馈增益过大力矩饱和限幅未生效降低 k_p做力矩饱和前先观察能量曲线切换LQR后摆杆立刻倒下切换阈值过大或角速度未收敛缩小切换阈值检查实际角速度噪声幅值车辆路径穿过最小转弯半径采样点间距不均或样条平滑权重过高等弧长重采样重调平滑权重 λ优化器报 infeasible约束过于严格或初值远离可行域放宽极限约束分阶段启动获得初值轨迹整体光滑但局部抖动B样条控制点过多导致过拟合减少控制点数量或在代价函数中加大曲率惩罚仿真轨迹很好但真机跟踪误差大忽略执行器延迟和动态特性用轨迹追踪控制器如MPC替代纯前馈跟踪我做这些项目最深的体会是最优控制和轨迹规划的工具链已经非常成熟真正的瓶颈往往不是数学公式推导不出来而是对物理系统的理解和建模误差。倒立摆上翻不成功先怀疑能量估计偏了车辆路径不满足运动学约束先核实最小转弯半径有没有留裕量NLP不收敛先检查初值和约束尺度。把这些基础环节抠扎实比盲目堆算法有用得多。最后再分享一个小习惯无论是仿真还是真机我都会把中间变量打出来或者可视化出来特别是能量曲线、曲率分布、优化器迭代过程。这些数据比最终的轨迹本身更能说明问题。最优控制这个领域最迷人的地方就在于你永远可以从一个失败的结果里反推出系统的本质特性——而这个过程就是经验积累的过程。 接触最优控制和轨迹规划这几年我最大的感受是所有看似炫酷的机器人动作——倒立摆原地起立、无人车在停车场里绕弯、机械臂在狭小空间里穿行——最后都可以归到同一类数学问题在一个系统动态约束下找一条可行路径和一组控制输入让某个代价最小。去年我把倒立摆上翻控制、车辆运动学约束路径规划和离散点参数化这三块内容完整过了一遍边做仿真边写笔记踩了不少坑也理清了它们之间千丝万缕的联系。这篇文章就把这份学习笔记整理出来从原理推导到代码层面的落地细节都会提到适合正在学最优控制却不知道怎么上手、或者卡在“仿真能跑但实物就崩”阶段的同学。先说清楚一件事最优控制不是某一种具体算法而是一种看待问题的框架。倒立摆上翻用的是能量法加LQR切换车辆路径规划用的是非完整约束下的几何曲线加数值优化离散点参数化则是把连续问题交给非线性求解器去算。三者看起来八竿子打不着但它们共享同一套语言状态、控制、约束、代价函数。把这套语言吃透你换任何一个平台都能快速上手。1. 最优控制与轨迹规划两件事一种底子1.1 最优控制到底在“最优”什么先抛开公式用生活里的例子把概念钉住。想象你开一辆手动挡的车面前是一段有红绿灯、有坡度、有弯道的路你知道起点和终点也知道交通规则那你要回答的问题是每个时刻该挂几挡、踩多少油门、什么时候刹车才能在绿灯亮起时不耽误、上坡时不熄火、下坡时不超速同时油耗还最低这就是最优控制问题。它的三个组成部分缺一不可描述车怎么动的微分方程、限制你行为的约束比如限速、挡位范围、以及衡量好坏的性能指标比如油耗。放到数学语境里问题一般写成这种形式给定系统状态方程 x_dot(t) f(x(t), u(t), t)找到控制输入 u(t)使得性能指标 J φ(x(T)) ∫₀ᵀ L(x(t), u(t), t) dt 最小同时满足状态约束、输入约束和边界条件。这里 φ 是终端代价L 是过程代价。如果你希望时间最短L 就是1如果希望能量最小L 就取控制量的二次型如果希望轨迹平滑可以在 L 里加入状态变化率的惩罚项。解决这个问题的经典路径分三条。第一条是变分法和庞特里亚金极小值原理通过引入协态变量把最优控制问题转化为两点边值问题然后用打靶法或松弛法求解这就是“间接法”。第二条是动态规划从末端倒着推Bellman方程算出一个值函数再用值函数反推控制律好处是能处理全局最优坏处是状态维度一高就直接内存爆炸。第三条是直接法把状态和控制都离散到时间网格上把原来无限维的优化问题变成有限维非线性规划NLP然后用序列二次规划SQP或内点法直接求解。实际工程里90%的轨迹规划跑的都是第三条路因为它对初值不敏感、实现简单对复杂约束的处理能力也强。1.2 轨迹规划是怎么被“最优控制”接管的轨迹规划和最优控制在很多场合被混着用实际上它们是有分工的。传统机器人学里轨迹规划关心的是“几何上怎么从 A 点到 B 点”插值出一条不撞障碍物的路径再单独做速度规划。最优控制则更进一步把“几何”和“动态”放在同一个优化问题里考虑我不仅要走一条不撞的路径还要让这条路径在车的加速度上限、摆的力矩上限、机械臂的关节速度上限这些约束下尽量省时、省力、平滑。举个例子你就明白了。给无人车一排离散路点最省事的做法是拿三次样条把这些点穿起来再检查一下曲率是否超过转向极限。这一套做下来很快但结果经常是“能走但不是最优的”可能某段曲率刚刚压线导致车必须频繁减速可能路径在直道上扭来扭去坐车的很快晕车。如果你换用最优控制框架把这些离散点不是当成“必须经过的钉子”而是当成“靠近期望位置的软约束”同时把曲率、加速度、jerk加加速度写进代价函数求解器给出的轨迹就会在“贴近路点”和“开起来舒服”之间自动找平衡。我做轨迹规划时有个习惯先判断这个问题是“路径问题”还是“轨迹问题”。如果机器人速度低到可以忽略动态特性那我只做路径规划只要速度不可忽略比如倒立摆上翻过程中摆杆角速度很大、车辆过弯时横向加速度明显就必须把动态约束纳进规划里。这个判断直接决定了工具选型做错了会在后面花大量时间调那些本来不该出现的bug。2. 倒立摆上翻控制从“站不起来”到“稳稳立住”2.1 为什么上翻比纯稳定更难倒立摆稳定控制在控制论教材里是一道经典题摆杆在竖直向上的不稳定平衡点附近用线性化模型设计LQR或者极点配置就能让它立住。但这里有个隐藏前提——摆杆得先“几乎直立”线性化模型才成立。真实系统摆杆一开始是自然下垂的你要让它从竖直向下稳定平衡点运动到竖直向上不稳定平衡点这中间摆角变化了180度完全是强非线性过程。我最初犯的错误就是天真地想直接给一个大扭矩把摆甩上去不行吗结果发现力矩小了甩不上去力矩大了摆过头到顶端时机身已经冲出去老远。原因在于倒立摆上翻本质上是一个能量问题摆杆从下垂位置到上立位置重力势能增加了 2mgl以摆杆底部为转轴质心升高了2l只有把足够的能量注入摆杆它才能越过中间的水平位置。而控制输入既要注入能量又要避免注入太多导致冲过头这就必须对能量做跟踪控制而非简单的开环猛甩。打个比方你推一个秋千上的小孩如果每次都在正确时机、正确力度推一把秋千会越荡越高推的时机不对力度再大也可能白费。倒立摆上翻就是在一个极度简化版的“秋千系统”上学会踩准时机推。2.2 能量法让摆自己“荡”上去实际工程中做上翻用得最多的是 Astrom 和 Furuta 提出的能量控制思路核心非常直观定义摆杆的相对能量偏差为 Ẽ E − E_target其中目标能量 E_target 是摆杆在竖直向上且静止时的能量。单级旋转倒立摆的机械能量为 E 1/2 m l² θ_dot² m g l (1 − cos θ)竖直向上的时候 θ 0能量取最小值0所以可以把目标能量设为0。控制律的设计目标是让 Ẽ 指数衰减也就是 Ė̃ −k Ẽ。对 E 求导可以发现能量变化率正比于摆角速度与控制量的乘积于是反推出来的控制律有这样一个结构u k_p · sat(Ẽ) · cos(θ)其中 sat 是饱和函数用来限制输出力矩乘上 cos(θ) 是让摆在水平位置附近少用力、在接近竖直时精确修正能量。这个公式虽然看着简单实际调参时你会发现 k_p 的选取决定了摆杆是“悠悠地荡上去”还是“猛甩过头”。在仿真环境里我一般先跑这样一段伪代码观察能量变化# 倒立摆上翻能量控制离散时间仿真 E_target 0.0 k_p 2.5 # 能量项增益需根据摆杆惯量标定 u_max 8.0 # 电机最大输出力矩 for step in range(int(t_end / dt)): # 从传感器读取摆角与角速度 theta, theta_dot read_state() # 计算当前机械能量 E 0.5 * m * l**2 * theta_dot**2 m * g * l * (1 - math.cos(theta)) E_tilde E - E_target # 能量控制律带饱和 u k_p * math.tanh(E_tilde) * math.cos(theta) u np.clip(u, -u_max, u_max) # 积分更新状态...调参数的经验是k_p 太小摆杆需要在返工好几次才能攒够能量上翻慢k_p 太大能量一超调就冲过头到稳定区时角速度还很大切换后LQR根本拉不回来。我个人喜欢先把 k_p 从1开始逐步增大观察 E_tilde 是否单调递减如果出现振荡则往回退半个量级。2.3 LQR接管从切换条件到参数初值能量控制只能把摆杆“送”到竖直附近真正让它站住的是切换后的线性控制器。切换触发条件一般写成角度和角速度的联合阈值|θ| θ_switch 且 |θ_dot| ω_switch。这两个值不能拍脑袋定我的做法是在仿真里先跑通纯LQR测出它能够稳定的最大初始摆角偏移和角速度偏移把这个范围的六折作为切换阈值留出裕量。LQR参数初值也一样有套路。倒立摆线性化模型是车位移 x、车速 x_dot、摆角 θ、摆角速度 θ_dot 四个状态输入是小车加速度或者力。标准做法是取 Q diag(q1, q2, q3, q4)R 为单个输入权重用 lqr() 函数解Riccati方程得到反馈增益。调参时先固定 R1把 q3对应摆角设得比 q1对应车位移大一个数量级比如 q3100q110这样控制器优先保证摆杆竖直其次才管车的位置。为什么这样因为上翻刚完成时最大的威胁是摆杆倒下来位置偏移可以靠后续慢慢回正但如果优先管位置很小的位置误差就会引发很大的加速度直接把摆杆甩倒。真机调试时还有一个切换瞬间力矩跳变的坑。能量控制输出的力矩和LQR输出的力矩大概率不在同一个量级切换那一刻电机会猛地突跳一下轻则听到一声异响重则把摆杆震脱。解决办法是在切换点附近做线性过渡也就是在切换前的0.2秒内把控制量从能量控制输出线性渐变到LQR输出或者统一加一个低通滤波器。这个细节仿真里经常看不出来但到真机上极其关键。3. 车辆运动学约束路径规划不是每条路都能开3.1 非完整约束车为什么不能横着走如果你开车进了一个特别窄的巷子你会立刻理解什么叫非完整约束车身长度的限制让车无法直接横向平移只能在“前进/后退”和“转向”的组合中腾挪。数学上阿克曼转向车辆的运动学模型为 x_dot v·cos θy_dot v·sin θθ_dot (v/L)·tan δ其中 L 是轴距δ 是前轮转角。这三条微分方程说明车辆的速度方向始终和车身朝向一致且转向半径由 R_min L / tan(δ_max) 约束不可能原地转向或者走任意曲率的路径。这种约束在控制理论里被称为非完整约束意思是它不能写成只对位置起作用的代数等式约束而是作用在广义速度上的微分约束。一个直接后果是给你一堆任意分布的离散点拿普通样条穿过后画出来的路径虽然光滑好看但车辆很可能根本开不出来——因为某几段曲率超过了 R_min 的倒数或者某些拐点要求车辆横向滑动。做车辆路径规划时我习惯先问三个问题有没有涉及倒车最大转向角对应最小转弯半径是多少离散点间距大概多少这三个问题决定了后续是用 Dubins 曲线、Reeds-Shepp 曲线还是混合A*加数值优化。3.2 Dubins/Reeds-Shepp满足最小转弯半径的几何解如果车辆只能前进而且转弯半径固定为 R_min那么在无障碍的平面上任意两个姿态位置加朝向之间的最短路径一定由圆弧和直线段组成。Dubins 证明了这种路径的组合只有六种类型比如 RSR、LSL、RSL、LSR 等其中 R 代表右转圆弧、L 代表左转圆弧、S 代表直线段。每条路径都是“在起始点以最小半径转、走直线、在以最小半径转”这样的结构几何关系可以用解析几何加少量数值迭代求出来。Reeds-Shepp 曲线把问题扩展到了允许倒车在拥挤停车场的场景里非常实用。它和 Dubins 的区别不在于多几种路径组合而在于引入了“反向段”倒车段使得路径可以是“前进-倒车-再前进”的形态。这也意味着车辆可以在更小的空间内完成转向代价是换挡次数变多实测中频繁切换前进/倒挡对执行器寿命不友好规划时可以在代价函数里对换挡次数加惩罚。我在项目里一般把 Dubins 当作理论下界如果某段路径连 Dubins 长度都缩短到一定程度说明空间确实不够需要引入倒车或者重新设计离散点。R_min 的取值要根据车型标定——理论轴距除以最大转角的正切值只是一个近似值真实的转向机构在接近最大转角时非线性明显安全做法是乘一个 1.2 的系数留出余量。3.3 离散路径点的平滑与曲率约束处理工程上拿到的高精地图路点通常是稀疏离散的可能来自GPS轨迹、高精地图的车道中心线或者上一级全局规划的采样点。直接操作这些点做几何路径最容易出现的问题是相邻线段方向突变成“折线”折线处的曲率无穷大车辆不可能跟上。我的处理流程分四步第一步把离散点做线性插值或最近邻匹配保证路径是连续折线。第二步用三次样条或五次多项式样条对折线做平滑样条的好处是 C2 连续曲率连续可导。第三步逐个采样样条上的曲率值检查是否超过 1/R_min如果不满足就把对应控制点微调后重新拟合。第四步把曲率信息传给速度规划曲率大的地方限制速度保证横向加速度 a v² / R 不超过舒适范围一般取 2~3 m/s²。这套流程里的坑在于单纯最小化样条曲率会让路径严重偏离原始路点尤其是弯道处样条为了减小曲率会“切内角”侵入对向车道。所以代价函数里必须同时包含“对原路点的贴合度”和“曲率平滑项”两者用权重 λ 平衡。我常用的做法是写成加权最小二乘min ‖P_smooth − P_ref‖² λ ∫ κ² dsλ 从0.01开始试看路径是否被拉得太弯。这个 λ 没有通用值因为它跟路点密度、弯道半径都相关只能用仿真场景测试后标定。4. 离散点参数化把连续优化变成计算机能解的问题4.1 为什么要做“离散点参数化”最优控制问题里状态和控制都是时间的连续函数真让计算机去“搜索”无限维的函数空间是不可能完成的任务。离散点参数化的本质是做一个有限维近似把时间轴切成一串网格点 t₀, t₁, …, t_N在每个网格点上定义状态 x_k 和控制 u_k原来微分方程约束 x_dot f(x, u) 就换成差分或者积分形式的等式约束代价函数里的积分换成求和。这样原来的连续最优控制问题就变成一个带有约束的大规模非线性规划问题。这个转换听起来朴素但里面有非常多的设计空间网格怎么分、状态怎么插值、约束怎么离散不同选择对应收敛速度和数值稳定性的巨大差异。最简单的做法是欧拉法离散即 x_{k1} x_k f(x_k, u_k) · Δt实现容易但精度低需要很密的网格才能满足约束。更精细的做法是直接配点法direct collocation在网格点之间用三次Hermite多项式插值并要求插值多项式在中点满足动力学方程这个方法在同样的网格密度下精度高很多是当前轨迹优化工具的主流选择。4.2 用多项式或样条参数化轨迹除了在时间网格上离散状态还有一种常见的做法是把整个轨迹用一组基函数表示比如多项式、B样条或者傅里叶级数优化变量从“每个时刻的状态值”变成“基函数系数”。这个思路对机械臂路径规划特别常用因为机械臂的任务经常是“从A点运动到B点保持抖动尽量小”这时候拿五次多项式 q(t) a₀ a₁t a₂t² a₃t³ a₄t⁴ a₅t⁵ 来参数化关节角度就很自然。五次多项式的好处是它有六个系数恰好可以匹配起始和终止两个时刻的位置、速度、加速度各三个条件解一个线性方程就能唯一确定轨迹。而且五次多项式的加加速度jerk是有界的连续函数不会出现三次多项式那种加速度突变。做机械臂直线轨迹规划或者圆弧轨迹规划时我基本都采用这个方法生成初始猜测轨迹再交给数值优化微调。用B样条的好处又不一样。B样条具有局部支撑性质移动一个控制点只影响附近一小段曲线不会把整条轨迹都搅乱。这在迭代优化时特别好用——求解器输出的轨迹如果某一段曲率超限直接微调对应控制点就行不用重算全局。代价是约束和目标函数的表达式相对复杂需要手推或者用符号运算工具生成雅可比。4.3 NLP求解约束处理与初值策略离散化之后的问题是一个典型的非线性规划问题变量数目从几百到几万不等。我用过的求解器包括 CasADi 加 IPOPT 的组合、MATLAB 的 fmincon以及专门针对轨迹优化的 TrajOpt、OCS2、CrocoDDL对于机械臂场景等。选型逻辑很简单如果问题规模小几十个变量fmincon 就够如果要做实时非线性模型预测控制就得用基于二次规划的 SQP 方法配合高效求解器如果动力学特别复杂而且需要高精度解CasADi 加 IPOPT 是稳妥之选。优化能否收敛70% 取决于初值给得好不好。我见过太多人把优化变量全设成0然后抱怨求解器不收敛——实际上非线性优化对初值天生敏感尤其是存在碰撞避免约束时从一个离可行域很远的初始点出发求解器很容易陷入局部极小或者直接发散。我的习惯是分阶段启动先跑一个不考虑复杂约束的简化版本拿到一条粗略轨迹把这条轨迹插值到优化网格上作为初值再开启完整约束重新求解。这个“热身启动”策略能解决大部分不收敛问题。约束的处理也讲究顺序。等式约束动力学、初始条件和不等式约束速度限制、力矩限制、障碍距离不应该一口气塞给求解器而是先从物理含义上检查是否冗余比如某些状态约束其实可以由输入约束推导出来就删掉。约束数量太多会让每次梯度计算和求逆都变慢而且劣化求解器的数值条件。实测经验是约束数量减少三分之一左右求解除以分钟计变成秒级且解的质量并不下降。5. 实操问题排查从仿真到落地最常踩的坑5.1 倒立摆上翻发散、切换抖动的排查问题1能量法控制下能量始终降不下去摆杆一直小幅摆动到不了竖直区域。这个大概率是增益 k_p 取得过小或者传感器数据噪声太大导致能量估计有偏。真机上先检查角度陀螺仪零点漂移把静止下垂时的能量归到0以后再判断。仿真里如果也复现多数是饱和函数设置得太保守输出力矩被限幅。问题2上翻成功后摆杆一直做大幅振荡切到LQR后还是稳不住。首先怀疑切换时刻的角速度阈值过大LQR在超出线性范围的状态下启动自然拉不回来。另一种可能是切换区间没有做滞回导致能量控制和LQR来回切换系统在两个控制器之间反复横跳。解决方法就是在切换阈值设计一个滞回区间比如进入LQR时要求 |θ| 25°退出时要求 |θ| 35°留出充足的回环。5.2 车辆路径曲率不连续与优化不收敛的排查问题1三次样条插值后的路径在个别点出现尖角曲率突变。这个往往是样条节点breakpoints选得不均匀离散点间距差距过大造成的。解决办法是先用等弧长重采样原始离散点再做样条拟合。问题2包含了曲率约束的NLP优化不收敛报“infeasible problem”。最常见的起因是最小转弯半径约束设置得比车辆实际能力还紧。我调过一次无人车项目理论最小转弯半径8米我直接用了8米做约束结果优化器一直找不到解后来放宽到10米立马正常。看起来像是“宽松了约束”但实际上8米是理论极限真实转向在极限附近摩擦系数下降根本跑不出这个曲率所以规划的“可行解”并不物理可行。5.3 常见问题速查表问题现象可能原因排查与解决倒立摆上翻过程中能量发散反馈增益过大力矩饱和限幅未生效降低 k_p做力矩饱和前先观察能量曲线切换LQR后摆杆立刻倒下切换阈值过大或角速度未收敛缩小切换阈值检查实际角速度噪声幅值车辆路径穿过最小转弯半径采样点间距不均或样条平滑权重过高等弧长重采样重调平滑权重 λ优化器报 infeasible约束过于严格或初值远离可行域放宽极限约束分阶段启动获得初值轨迹整体光滑但局部抖动B样条控制点过多导致过拟合减少控制点数量或在代价函数中加大曲率惩罚仿真轨迹很好但真机跟踪误差大忽略执行器延迟和动态特性用轨迹追踪控制器如MPC替代纯前馈跟踪我做这些项目最深的体会是最优控制和轨迹规划的工具链已经非常成熟真正的瓶颈往往不是数学公式推导不出来而是对物理系统的理解和建模误差。倒立摆上翻不成功先怀疑能量估计偏了车辆路径不满足运动学约束先核实最小转弯半径有没有留裕量NLP不收敛先检查初值和约束尺度。把这些基础环节抠扎实比盲目堆算法有用得多。最后再分享一个小习惯无论是仿真还是真机我都会把中间变量打出来或者可视化出来特别是能量曲线、曲率分布、优化器迭代过程。这些数据比最终的轨迹本身更能说明问题。最优控制这个领域最迷人的地方就在于你永远可以从一个失败的结果里反推出系统的本质特性——而这个过程就是经验积累的过程。