去年整理学习资料的时候翻出来一堆旧笔记关于最优控制和轨迹规划的。当时是为了啃倒立摆上翻和自动驾驶泊车路径规划这两个硬骨头陆陆续续记了很多东西最近重新梳理了一遍顺手把里头的关键推导、实现思路和踩过的坑整理成一份笔记。这份内容覆盖了三个核心模块倒立摆的摆起与稳摆控制、车辆运动学约束下的路径规划、以及离散点参数化轨迹优化的通用方法适合正在学习最优控制理论、刚接触轨迹优化、或者想从仿真走向实物验证的同学参考。我尽量把每个方案背后的动机讲清楚不只是贴公式而是说人话。1. 为什么最优控制和轨迹规划经常被放在一起讨论先聊聊这个大多数人初学时都会困惑的问题。最优控制和轨迹规划在教材里通常分成两门课讲但实际做项目时你会发现它们在数学形式上是同一件事只是出发点和关注点不同。1.1 从倒立摆问题看两类方法的分工倒立摆上翻控制是一个经典的最优控制问题系统状态是摆杆角度和角速度控制输入是施加在小车上的力或者力矩目标是把摆杆从自然下垂的稳定平衡点驱动到竖直向上的不稳定平衡点。这里头的难点在于系统是非线性的而且目标状态是不稳定的——你不仅要“到达”还要“站稳”。轨迹规划解决的是“怎么走”的问题最优控制解决的是“怎么走才最优”的问题。在实际工程中这两者往往耦合在一起路径规划给出了几何路径或者带时间参数的轨迹而底层控制负责跟踪这条轨迹。但在倒立摆上翻这个例子里控制器本身同时承担了规划角色——它直接输出的控制序列就是一条隐式的轨迹。1.2 数学模型是统一的语言不管是倒立摆还是车辆路径规划它们的本质都是受约束的动态系统优化问题统一的数学形式是这样的找到控制输入使系统从初始状态转移到目标状态同时极小化某个代价函数并满足状态方程、控制输入约束和状态约束。在倒立摆案例中状态方程是倒立摆的动力学微分方程在车辆路径规划案例中状态方程是车辆的运动学方程比如自行车模型中位置和航向角的导数关系。代价函数则根据任务不同而变化倒立摆关心能量消耗和摆起速度车辆泊车关心路径长度、平滑度和与障碍物的距离。想通了这个统一性之后后面学任何轨迹规划相关的算法都会轻松很多。我自己的体会是先把这个统一的数学框架刻在脑子里再看LQR、MPC、直接配点法、伪谱法这些具体工具思路就格外清晰——它们只是在不同侧重点下对这个框架的求解策略。2. 倒立摆上翻控制从能量注入到LQR平衡的完整设计倒立摆上翻是控制理论中最经典的验证平台之一也是所有学最优控制的人绕不过去的一道坎。我在实验课上做过一阶倒立摆的实物控制也在仿真里跑过二维倒立摆的摆起和稳定。这段经验里最关键的不是某个具体控制律而是理解整个控制策略的分层结构。2.1 为什么不直接使用LQR做上翻很多初学者接触LQR之后第一反应是直接用LQR把摆杆从下垂状态调节到竖直状态不就行了答案是不行。原因是LQR的本质是线性系统的状态反馈控制器它基于系统在某个平衡点附近的线性化模型。倒立摆竖直向上这个平衡点本身是不稳定的而且对于初始状态远离平衡点的情况线性化模型的近似误差太大LQR会给出不合理的控制量甚至根本没法把摆杆拉上去。教科书上会说LQR的稳定区域是平衡点附近的“吸引域”。对于倒立摆这种强非线性系统这个吸引域非常小大概只有几度的角度偏差。也就是说LQR只能做“稳摆”不能做“上翻”。所以工程上的标准做法是先设计一个非线性控制律用能量注入的方式把摆杆从下垂位置“甩”到竖直位置附近当状态进入LQR的稳定域之后再平滑切换为LQR进行精确稳定。2.2 能量整形控制律的设计思路倒立摆上翻问题中最优雅的一招就是能量整形。思路是这样的摆杆在下垂位置时能量最低在竖直向上的不稳定平衡点处势能最大。如果我们能定义一个反映“离目标状态还有多远”的能量函数然后设计控制律让这个能量单调趋向于目标值就能把摆杆一步步“抬”上去。记摆杆质量为m摆长l重力加速度g转动惯量J摆杆相对竖直向上的角度为θ角速度为ω。系统的机械能为:E 0.5 * J * ω² m * g * l * (cos(θ) - 1)这个能量定义选得很好当摆杆竖直向上时θ 0E 0当摆杆自然下垂时θ πE -2 * m * g * l。目标是让能量从负值增加到0。设计李雅普诺夫函数V 0.5 * (E - E_ref)²其中E_ref 0是目标能量。对V求导利用倒立摆动力学方程可以得到V̇ (E - E_ref) * m * g * l * sin(θ) * u为了让V̇尽可能为负选择控制律为u -k * (E - E_ref) * sign(sin(θ))这里的k是一个正的增益系数。这个控制律的含义直观上可以这样理解当摆杆在某一侧摆动时我们施加一个和摆动方向匹配的力不断给系统“泵入”能量或者“抽取”能量直到总能量逼近目标。我在仿真里调试这个控制律时的实际经验是增益k不能太大也不能太小。太小的话摆杆需要很多个来回摆动才能累积足够的能量上翻过程拖得很长太大则容易引起控制量饱和甚至激发未建模的动态导致摆杆在接近竖直位置时震荡加剧。一般从仿真里逐步翻倍k观察摆杆峰值角度是否能逐步逼近竖直方向直到在竖直附近出现可控的收敛趋势这时的k基本就合适了。2.3 切换到LQR稳定控制的时机与平滑处理当能量控制律把摆杆送入竖直位置附近的一个小邻域比如|θ| 10°|ω| 0.5 rad/s时就可以切换为LQR控制器。这里有一个容易忽略的细节切换瞬间如果直接改变控制律可能会导致控制量跳变在实物上表现为“咔哒”抖动甚至打齿。解决办法是增加一个过渡带在过渡带内对两种控制器的输出做线性加权融合。我用的权重函数很简单当|θ| 15°时全部采用能量控制器当15° ≥ |θ| ≥ 5°时按比例混合两种控制器输出当|θ| 5°时完全采用LQR输出。这个混合过程不需要特别复杂的调度函数线性插值就足够了。实测下来切换平滑度大幅改善摆杆进入LQR稳定域后几乎没有肉眼可见的抖动。2.4 仿真到实物的差异与补偿仿真和实物最大的差别在于两个地方摩擦力矩和控制量延迟。摩擦力矩在仿真里经常被忽略但实物上它直接影响上翻的成功率。我在实物平台上做的第一版控制律仿真里百发百中实物上却经常在半空中掉下来。排查后发现摆杆的旋转关节存在明显的粘滞摩擦能量控制律计算出的微小控制量根本推不动摆杆。解决办法是在控制律里显式补偿摩擦项u_total u_energy_lqr u_friction u_friction f_c * sign(ω) f_v * ω其中f_c是库仑摩擦系数f_v是粘滞摩擦系数。这两个参数的辨识方法很朴素让摆杆自由摆动记录角速度衰减曲线用最小二乘拟合衰减速率就能估计出来。控制量延迟的问题则更隐蔽。实物的电机响应存在几毫秒到几十毫秒的延迟这个延迟在高增益LQR下可能导致极限环振荡。解决办法也不复杂一方面在LQR设计时保留足够的稳定裕度不要把极点配置得太快另一方面在控制器输出端做一阶低通滤波滤掉高频分量。3. 车辆运动学约束下的路径规划从曲线几何到搜索算法的演进车辆路径规划比倒立摆多了一个重要的约束车辆不能像倒立摆那样原地任意转动它有最小转弯半径。这个约束在数学上体现为曲率上限约束任何可行的路径都必须是曲率连续的而且曲率大小不能超过某个阈值。3.1 自行车模型低速泊车规划的核心抽象低速泊车和园区低速自动驾驶场景下最常用的车辆模型是自行车模型Bicycle Model。它将四轮车辆简化为前后轴中心连线的自行车前轮控制转向角后轮提供驱动力。状态量是后轴中心的位置(x, y)、航向角φ、前轮转向角δ和车速v运动学方程如下dx/dt v * cos(φ) dy/dt v * sin(φ) dφ/dt v / L * tan(δ)其中L是轴距。这个模型的关键假设是低速工况下轮胎侧偏角可以忽略车辆运动方向始终与后轴垂直。这在实际泊车速度一般低于5km/h下是合理的近似。很多做路径规划的人会忽略一个点自行车模型虽然是“运动学”模型但它隐含了车辆几何约束。在用混合A*等搜索算法做规划时必须同时考虑车辆的长宽尺寸确保整辆车不与障碍物碰撞而不只是代表点如后轴中心或质心避开障碍物。3.2 Dubins曲线与Reeds-Shepp曲线的使用边界在无碰撞场景下最短路径问题是可以用解析方法求解的。Dubins曲线给出了车辆只能向前行驶时的最短路径Reeds-Shepp曲线则允许倒车更适合泊车这类需要来回调整的场景。这些曲线有一个明显的优点求解速度快代码实现简单而且路径必然是满足最小转弯半径的。但它们也有一个致命缺点对障碍物环境的适应能力很差。一旦空间中存在障碍物解析曲线几乎不可能直接给出可行路径。我在实际项目中的用法是把Reeds-Shepp曲线作为混合A*搜索算法中的“启发式”和“终局连接器”。当搜索树扩展出的某个节点距离目标位姿很接近时调用Reeds-Shepp曲线求解器看是否存在一条无碰撞的曲线路径能直接连到目标。如果存在搜索就可以提前终止大幅提升效率。如果不存在说明还需要继续在状态空间中拓展节点。3.3 混合A*网格离散化与连续状态空间的折中混合A*是解决车辆运动学约束路径规划最实用的算法之一。它的核心思想是在网格地图上进行搜索但每个网格节点内部保存的是连续的车辆状态位姿而不是网格中心点。这样的好处是扩展出来的相邻节点之间一定满足车辆运动学约束而搜索本身又能利用启发式信息快速逼近目标。我在实现混合A*时踩过最深的坑是运动学扩展的细节。一个典型的扩展过程是对当前节点的转向角δ在其范围内采样若干个离散值比如-30°到30°之间采样5-7个对每个采样转向角以固定弧长往前积分运动学方程。这里有几个参数需要仔细调转向角采样数量太少路径粗糙太多计算量爆炸一般5-7个足够单步弧长太短节点数爆炸太长容易穿墙我通常取车身长度的一半到三分之一积分步长内部数值积分用的步长需要比单步弧长更小确保弧线平滑一般取0.1m左右。混合A*的代价函数设计也值得反复斟酌。最基本的代价是路径长度但只考虑长度会出现贴着障碍物边缘走、频繁切换转向的问题。我在代价函数里额外加入了两项一项是转向角变化量的惩罚抑制方向盘的频繁摆动另一项是到障碍物距离的惩罚让路径尽量远离障碍物边缘。调试权重的方法是先让路径长度代价占主导跑通基本功能再逐步增大平滑性代价直到路径看起来“像人开的”。3.4 从搜索路径到可执行轨迹的平滑后处理混合A*输出的路径本质上是一系列离散位姿点组成的折线路径虽然每段圆弧都满足运动学约束但相邻圆弧之间往往存在曲率跳变直接交给底层控制器跟踪会出现明显的卡顿感。后处理阶段我采用的是数值优化方法将路径参数化为B样条曲线以平滑性为目标函数以避障和运动学可行性为约束用优化库求解出一个平滑轨迹。这一步其实就是下一节要讲的离散点参数化轨迹优化的一个典型应用场景。优化输出之后还有一道工序我建议保留用时间参数化给轨迹分配速度。低速泊车场景下速度规划比较简单——直行段允许稍快转弯段限制速度保证向心加速度不超过舒适性阈值。这个速度分配看起来不起眼却是从“轨迹看起来没问题”到“真车跑起来没问题”之间最关键的一步。4. 离散点参数化轨迹优化把连续问题“翻译”成数值问题聊完了两个具体案例现在说一个贯穿始终的方法论——离散点参数化轨迹优化。无论是最优控制问题还是轨迹规划问题真正的求解瓶颈都在于我们面对的是无限维的函数优化问题而计算机只能处理有限维的数值问题。离散化就是在这两者之间架桥的过程。4.1 直接配点法 vs 打靶法离散化的两条路线轨迹优化领域中离散化思路主要分成两大类直接配点法和直接打靶法。直接打靶法把控制输入参数化然后利用数值积分从初始状态一步步前向传播状态轨迹约束和代价都表达在终端状态和路径积分上。直接配点法更激进一些它把状态轨迹和控制轨迹同时离散化每个离散时刻的状态都作为优化变量动力学方程则通过离散化后的等式约束来保证。我在实际使用中的偏好是直接配点法。原因有三点第一它不需要对动力学做前向积分避免了积分误差的累积问题第二它产生的大规模稀疏优化问题可以被现代求解器高效处理比如IPOPT、OSQP等第三它对非线性约束的处理相对直接便于添加路径约束比如避障和状态约束比如速度、加速度限制。4.2 离散点上的动力学残差表达考虑一个一般的最优控制问题连续形式如下min J ∫L(x, u) dt Φ(x(T)) s.t. dx/dt f(x, u) x(0) x0 h(x, u) ≤ 0直接配点法把时间轴[0, T]划分为N个区间每个区间的端点称为配点。状态x_k和控制u_k在配点处定义为优化变量。动力学约束被替代为残差等式约束(x_{k1} - x_k) / dt f(x_k, u_k)这个表达式本质上是一阶欧拉离散格式。更精细的做法是使用梯形法或者Runge-Kutta离散格式x_{k1} x_k (dt/2) * (f(x_k, u_k) f(x_{k1}, u_{k1}))我建议程序中直接用梯形法它实现难度只比欧拉法高一点精度却提升一个量级而且不会引入数值稳定性问题。这里有个常见的数值陷阱如果状态变量量纲差异很大比如角度在弧度级别、位置在米级别、速度在米每秒级别直接求解时数值条件数会很差导致求解器收敛缓慢甚至失败。解决方法是做变量缩放把所有变量的范围归一化到[-1, 1]左右。具体做法是对每个变量定义一个缩放系数s_i优化变量替换为x_i / s_i让约束和代价都基于缩放后的变量。这个简单的预处理经常能把求解时间缩短数倍。4.3 一个通用的离散化代码框架用一个伪代码骨架说明直接配点法的实现结构这个框架在倒立摆和车辆路径规划中都复用得很好import casadi as ca # 决策变量状态轨迹和控制轨迹 X ca.MX.sym(X, nx, N1) # 每个时间步的状态 U ca.MX.sym(U, nu, N) # 每个时间段的控制 # 目标函数 J 0 for k in range(N): x_k X[:, k] u_k U[:, k] J dt * cost_running(x_k, u_k) J cost_terminal(X[:, N]) # 动力学约束 g [] lbg [] ubg [] for k in range(N): x_k X[:, k] x_next X[:, k1] u_k U[:, k] # 梯形法离散动力学残差 f_k dynamics(x_k, u_k) f_next dynamics(x_next, u_k) residual x_next - x_k - (dt/2) * (f_k f_next) g.append(residual) lbg.append([0]*nx) ubg.append([0]*nx) # 状态/控制边界约束 for k in range(N): g.append(X[:, k]) lbg.append(state_lb) ubg.append(state_ub) g.append(U[:, k]) lbg.append(control_lb) ubg.append(control_ub) # 初始状态约束 g.append(X[:, 0]) lbg.append(x0) ubg.append(x0) # 构建并求解 NLP nlp {x: ca.vertcat(ca.reshape(X, -1, 1), ca.reshape(U, -1, 1)), f: J, g: ca.vertcat(*g)} solver ca.nlpsol(solver, ipopt, nlp)这个框架的关键在于dynamics函数不同问题只需替换这个函数和相应的维度参数。倒立摆问题时nx 4小车位置、速度、摆杆角度、角速度nu 1力或力矩车辆路径规划时nx 3或nx 4位置、航向角、转向角nu 2速度和转向角速度。4.4 初值设置直接配点法最容易失败的地方直接配点法最大的痛点不是模型写错而是初值给得不好导致不收敛或者收敛到糟糕的局部最优解。很多初学者会天真地把所有状态初值都设为目标值希望求解器自己“变”出轨迹来。实际效果通常是求解器直接报不可行。正确做法是给一条简单的可行初值轨迹线性插值连接初始状态和目标状态控制量初值可以设置为中位值或者零。举例来说车辆从位姿(0,0,0°)泊车到位姿(5,3,90°)初值构造为for k in range(N1): alpha k / N X_init[:, k] (1 - alpha) * x0 alpha * x_target U_init[:] 0这条初值轨迹虽然不满足动力学约束但它提供了一条有价值的“参考线”让求解器知道大致往哪个方向走。实际测试中这个看似随意的初值设置能把成功率从不到50%提升到95%以上。其次求解过程中的约束松弛也值得注意。如果直接求解就出现不可行问题一个有效的排查手段是暂时去掉代价函数中惩罚过大的项比如避开障碍物的惩罚先求解一条只满足动力学和边界约束的光滑路径。确认这条路径存在之后再逐步加入障碍物惩罚并以前一步的解作为初值重新求解。这种“分层求解”的思路在工程上远比一次性求解一个超大规模问题可靠。4.5 障碍物避让约束的表达在连续优化框架下障碍物避让约束不能像网格搜索那样简单地标记“可达/不可达”。我需要用连续函数描述“安全距离”这个概念。最常用的方案是对每个障碍物用若干个圆或者凸多边形覆盖然后约束车辆与每个几何基元的距离不小于安全阈值。对于凸多边形障碍物一个简洁的做法是使用有向距离函数SDF将它作为约束函数嵌入优化问题中。SDF的梯度信息可以很方便地传递给求解器提高收敛性。如果障碍物形状过于复杂SDF的计算成本会上升工程上常见的妥协方案是预计算一张SDF网格图然后通过双线性插值获取任意位置的SDF值。这个技巧在2D泊车路径规划中表现非常好计算速度远快于在线几何求距离。4.6 数值求解器的选择与调试经验市面上常用的轨迹优化求解器IPOPT和OSQP是两大主流。IPOPT适合非线性规划问题能处理非凸目标函数和非线性约束是通用首选OSQP只适合凸二次规划问题但它求解速度快一个到两个数量级适用于实时性要求高的场景。我的一般经验是如果问题能用凸优化表达比如固定路径上的速度规划优先用OSQP如果问题必须包含非凸约束比如避障或者非凸环境下的路径优化直接上IPOPT不要试图强行凸化。强行凸化往往以牺牲性能为代价得到的解可能根本不可用。IPOPT的收敛性问题除了初值之外还有一个常见原因是约束和代价函数的尺度不一致。在实现时加入约束缩放——将动力学残差除以时间步长使其量纲为“状态量/秒”将代价函数做个归一化让数量级落在1到100之间。这样处理后IPOPT的内点法迭代次数通常会大幅减少。这属于那些写论文时不会提、但实践中极其管用的细节。5. 学习路径与实操建议从复现案例到独立设计的经验总结开头说了这是一份学习笔记最后做个学习路径的梳理这部分纯粹是一家之言但确实是我自己摸索出来的高效路线。5.1 我踩过的坑和绕过弯路的经验第一不要一开始就想着自己写求解器。我在倒立摆项目初期曾经试图从零手写一个SQP算法结果花了大半个月还在处理线性代数库的边界条件后来换用CasADi和IPOPT两天之内就把原本的性能瓶颈优化流程跑通了。自研求解器是研究课题不是工程项目该做的事除非你确实在专门研究优化算法本身。第二状态变量和控制变量的缩放问题一定不能拖到最后再处理。我曾经在车辆路径规划中调试了两天发现所有约束都正确但迭代就是不收敛。后来给三个状态量和两个控制量分别加了一个缩放系数问题立刻解决。造成这个问题的原因很朴素位置量级是几米角度的量级是零点几弧度速度量级是每秒几米不缩放的Hessian矩阵条件数差到求解器根本没法有效计算搜索方向。第三每次只加一类约束逐步验证。最优控制问题调试有一个很通用的做法第一步只施加动力学约束和边界条件确认求解器能找到一条从起点到终点的任意路径第二步加入控制量限幅和工作空间边界第三步加入避障约束第四步调节代价函数权重。我把这套流程固化成自己的标准调试顺序后大大小小的轨迹优化项目成功率都明显提升出现问题时也能更快定位是模型错误、初值问题还是约束矛盾。5.2 推荐的验证顺序与工具链组合如果是新手建议的验证顺序是从仿真到实物层层递进先在纯数学仿真里验证优化结果再加入传感器噪声和执行器延迟最后才上实物硬件。每一步都验证充分再推进到下一步。这个顺序能帮你把问题分层隔离。我的工具链组合是这样的仅供参考符号建模与优化建模CasADi非线性规划求解IPOPT凸二次规划求解OSQP仿真验证Matlab/Simulink或者Python环境下的自定义仿真器可视化Matplotlib或者按需定制这套组合最大的好处是CasADi支持用符号表达式描述动力学生成的目标函数和约束可以直接交给IPOPT求解中间不需要手动推导一大堆偏导数。省下的时间精力可以全部用来打磨模型和调参。5.3 一个意想不到的收获学到最后回头看倒立摆上翻和车辆路径规划这两个题目讨论的其实是同一个问题在一个受物理定律约束的系统中如何找到一条不仅可行、而且最优的轨迹。倒立摆用的是能量控制加LQR的混合策略车辆路径规划用的是搜索加数值优化的组合。前者偏向连续控制后者偏向离散决策但它们的底层逻辑是一致的先把约束梳理清楚再选择合适的求解范式。这份笔记如果只保留一句话的价值那就是多动手实现少纠结哪个方法更高级最好的学习方法就是亲手把两个案例跑通。