最优控制、轨迹规划、倒立摆上翻控制、车辆运动学约束路径规划、离散点参数化——这些东西很多人是分开学的但实际做项目时会发现它们全串在一条线上规划层算出一条可执行路径控制层负责把它跑出来中间还夹着一堆离散点怎么变成连续轨迹的问题。这篇笔记是我自己从仿真到实机的一个阶段性整理用倒立摆上翻和车辆路径规划两个实际案例把这条链路走了一遍适合正在搞机器人控制、自动驾驶决策、或者机械臂运动规划的同行参考。这篇笔记里我不会讲太多纯数学推导重点放在每个环节为什么那样做、踩坑踩在哪里、以及怎么把理论方法落到代码里。内容偏实践但有控制理论基础的人读起来会更有感觉因为我讨论的每个方法都会回到“代价函数怎么设”“约束怎么表达”“数值上为什么不收敛”这类工程视角的问题。1. 最优控制与轨迹规划的整体关系1.1 两个概念的分工先有轨迹再有控制很多初学者容易把最优控制和轨迹规划混在一起或者觉得它们是两个平行方向。从工程角度看两层解决的问题完全不同但又共享同一套数学工具。轨迹规划回答的是“怎么走”的问题给定起点、终点、障碍物约束、车辆或机械臂的运动学限制生成一条从A到B的路径以及沿着这条路径的速度、加速度时间序列。输出通常是一条参考轨迹 x_ref(t)。最优控制回答的是“怎么跟”的问题在系统动力学约束下找到控制输入 u(t)让系统实际状态 x(t) 尽可能接近参考轨迹同时满足输入幅值限制、状态约束并最小化某个代价函数。典型形式是极小化 J ∫(Qe² Ru²)dt 这类二次型指标。但这两个层次又是相通的。倒立摆上翻就是最典型的例子上翻阶段根本没有一条静态路径可以参考只有“把能量灌进去”这个抽象目标本质上是个纯最优控制问题而车辆路径规划反过来更偏几何搜索如果再把速度曲线、转向平滑度加进去就又变成了带约束的优化问题。所以我个人理解最优控制是工具轨迹规划是场景场景里嵌套了工具。还有一层理解很多轨迹规划算法做出来的路径质量最终取决于你在最优控制层面定义了什么指标。比如车辆路径规划如果只考虑“最短”不考虑“转向平滑”Hybrid A* 出来的轨迹就会是一堆圆弧拼出来的锯齿跟踪时方向盘会来回猛打。最优控制和轨迹规划如果在同一个目标函数下联合求解结果往往比各自单独搞要干净得多。1.2 几个关键问题的现实解法从变分法到数值优化最优控制的理论核心有三大块变分法、庞特里亚金极小值原理、动态规划。理论讲起来都很优美但做工程的人马上会碰到一个现实问题手解边值问题基本不可行。倒立摆这种状态维度低还好理论上可以写出哈密顿函数、伴随方程再用打靶法解两点边值问题。但车辆路径规划这类问题状态量和约束条件一多解析推导就成灾难了。所以现在的工程主流是直接法把连续的最优控制问题离散成参数优化问题。核心思路是把控制输入 u(t) 和状态 x(t) 在时间轴上离散成有限个点然后把这些点当成优化变量把系统动力学、状态约束、控制约束全部写成等式或不等式约束目标还是那个代价函数最后扔给非线性规划求解器处理。一句话总结这个转化过程从找函数 u(t) 变成找一组数 u[k]函数空间变成数空间微积分问题变成代数问题。这种做法的好处是通用、鲁棒初值不需要猜测得特别准也能收敛。这也是为什么现在机器人领域的轨迹优化工具大部分都走这条路。直接法看起来不如间接法“优雅”但工程的本质是在给定时间和算力下做出可用的结果不是追求数学上的最优证明。1.3 统一视角离散点、代价函数、约束是同一件事把最优控制和轨迹规划串在一起的是“离散点参数化”这件事。路径规划输出离散路径点最优控制需要输入参数化后的轨迹机械臂直线/圆弧轨迹规划也都绕不开离散点的连续化。我个人的学习路线是先在一个小规模问题上把最优控制跑通再去理解轨迹规划里的搜索和采样最后才体会到离散点参数化是把这一切串起来的桥梁。倒立摆上翻就是那个“小规模问题”它让你看清楚最优控制怎么和动力学交互车辆路径规划让你看到约束如何反过来决定路的形状而离散点参数化让你面对现实里最常见的情况——上层给了几百个点下层到底怎么消费这些点。2. 倒立摆上翻控制最优控制的最小完备实验场2.1 为什么倒立摆是绕不开的经典案例倒立摆这个系统有多经典不用我多说凡是做控制的人一定接触过。它同时具备几个非常有代表性的特征欠驱动只有小车一个输入要控制两个状态非线性大角度摆动阶段是强非线性不稳定平衡点竖直位置就是以及驱动饱和小车加速度有限。上翻问题的定义是从自然下垂的稳定平衡点出发通过网络化的控制把摆荡到竖直倒立位置并且最终稳定在该位置。整个过程分成两段先“荡”上去再“立”住。前一段是典型的能量控制问题后一段是典型的线性稳定控制问题。两段问题的本质差异在于上翻阶段系统远离平衡点非线性必须完整建模线性化方法失效稳定阶段系统在倒立点附近小幅运动线性化后LQR就够用了。所以倒立摆上翻天然是一个“混合控制策略”问题这也是它为什么在学术界和工业界都常被用作验证平台的原因。2.2 能量控制律怎么把摆荡上去上翻阶段的核心思路很巧妙不去追踪某个具体轨迹而是控制系统的能量。系统的机械能定义为E 1/2·m·l²·θ_dot² m·g·l·cosθ其中 m 是摆杆质量、l 是摆杆质心到转轴距离、θ 是摆杆与竖直向下方向的夹角、θ_dot 是角速度。目标状态竖直倒立对应的能量是 E_goal m·g·l即动能零、势能最大。定义能量误差 E~ E - E_goal控制律形式为u sat(k·E~·sgn(θ_dot·cosθ))符号函数的作用是判断该加速还是减速。物理直觉是要让摆获得能量推力方向要和运动方向一致要让摆失去能量推力方向要和运动方向相反。这里 sgn(θ_dot·cosθ) 正是“运动方向”的判据而 E~ 的大小决定了推的力度。参数 k 就是能量误差到推力的增益。k 太小推不动摆荡半天能量误差减不下去k 太大小幅摆动时会来回激振系统反而容易发散。饱和函数 sat 是一定要加的因为小车的加速度有物理上限。实际调参的经验是先固定饱和度边界比如小车加速度限制在 ±2 m/s²然后从很小的 k 开始试观察能量误差曲线是否单调下降而不是看摆角好不好看。能量误差单调下降说明控制方向基本正确这比盯着角度曲线调试要直观得多。2.3 上翻之后怎么稳定LQR切换的设计细节上翻完成的条件是摆角接近竖直位置工程上通常取 |θ - π| 5° 且 |θ_dot| 0.5 rad/s。这时能量控制的符号函数会频繁翻转如果继续用这套控制摆会在竖直位置附近反复穿越没法真正立住。所以必须切换。切换后的控制是在倒立点附近做线性化得到线性系统 dx A·x B·u然后设计LQR增益 K lqr(A, B, Q, R)。代价函数的形式为J ∫(xᵀQx uᵀRu)dt状态通常取 [小车位移 x, 小车速度 x_dot, 摆角 θ, 摆角速度 θ_dot]。Q 矩阵和 R 矩阵的权重分配直接决定控制行为。我自己的整定经验角度权重给最大比如 100角度速度次之比如 5小车位移再次比如 1小车速度最小比如 0.1R 取 0.1 左右。这个参数组合调出来的效果是摆能快速直立但小车不会为了稳住摆而大幅来回跑适合轨道长度有限的实验平台。切换时机很容易被忽视。切早了LQR工作在线性化区域之外线性模型不成立控制会失效切晚了能量控制把摆又推离平衡位置LQR想抓回来都难。所以切换阈值不能省建议在调试时把切换条件可视化画一条竖线看到底在什么时机切的这对理解两个控制器的交接非常有帮助。2.4 实操踩坑记录从仿真到实机的问题点这里列几个我实际碰到过的坑仿真里基本不会出现真机上全是问题。第一个是角速度噪声。仿真里读角速度是精确的但实机的角度微分之后噪声会被放大尤其是低成本编码器。直接拿原始差分信号去算能量误差会发现控制律在目标附近抖动得很厉害。解决方法是加低通滤波或者用观测器估计角速度。第二个是控制周期。上翻阶段系统是大幅摆动10ms的控制周期也够用但LQR稳定阶段对延迟非常敏感控制周期最好跑在1到5ms。这里有个工程技巧不要在整个控制回路里切换周期而是保持统一的高频控制率只是上翻阶段做些计算量小的操作比如查表把时间留给LQR阶段的高频输出。第三个是最容易犯的错误——用一组控制参数跑完全程。很多人一开始图省事上翻用MPC或者全套LQR结果发现性能一团糟。原因在于上翻阶段是大幅非线性运动稳定阶段是小偏差线性运动两个阶段的本质物理规律完全不同强行用一套参数只会两头不讨好。3. 车辆运动学约束路径规划非完整约束下的离散路径生产3.1 非完整约束的含义车辆不是个点车辆路径规划和机械臂路径规划有一个关键不同车辆受非完整约束也就是不能横向移动。用公式表达就是x_dot·sinθ - y_dot·cosθ 0这个式子的物理含义是车辆速度方向始终沿着车头朝向不能像自由质点那样任何方向都能动。这个约束让两轮差速机器人、汽车这一类系统有了完全不同的路径可达集。拿日常例子来说一个不能原地转向的购物车你只能推着它走弧线在窄通道里想掉头必须打几把轮。这个“打几把轮”的过程在数学上就是Reeds-Shepp路径。如果只允许前进那就是Dubins路径。直观上就能理解离散两点之间直线最短在非完整约束下根本不成立。车辆运动学通常简化为自行车模型x_dot v·cosθy_dot v·sinθθ_dot v·tanδ / L其中 δ 是前轮转角L 是轴距。由这个模型可以推出最小转弯半径 R_min L / tan(δ_max)。这个参数是所有路径规划和轨迹跟踪算法的基础因为任何路径的曲率都不能超过 1/R_min否则车辆物理上就跟不上。3.2 Dubins曲线和Reeds-Shepp曲线离散点之间怎么连接Dubins曲线的核心结论是在只允许前进、转弯半径有下界的情况下两点之间的最短路径一定是由圆弧和直线组成的组合形式只有两大类CSC圆弧-直线-圆弧和CCC圆弧-圆弧-圆弧。Reeds-Shepp扩展了这种情况允许车辆倒车最短路径的类型会增加到几十种组合。理论上所有满足运动学约束的路径一定可以用圆弧和直线拼接而成这些弧线的半径就是最小转弯半径。工程落地时Reeds-Shepp通常用作启发式函数而不是最终路径。因为它只保证几何最短不保证避障、不保证平滑更不保证考虑了加速度限制。但它能提供一个非常强的下界估计让搜索算法快速收敛。实际编程中Reeds-Shepp曲线的采样和枚举是纯几何问题市面上有很多现成实现但要注意输入输出坐标系的约定。我在项目里踩过一个坑坐标系用的ENU东北天但库默认NED北东地结果所有路径的转向方向全反了排查了半天才发现是弧线方向定义不一致。3.3 Hybrid A* 的思路把A*从栅格搬到连续空间传统A是在栅格地图上搜索路径出来的是折线车辆根本无法直接跟踪。Hybrid A的核心改进是每个节点不再只是栅格坐标 (x, y)而是连续状态 (x, y, θ)。扩展节点时不再跳向相邻栅格而是向前滚动车辆运动学模型生成一小段车辆真实可执行的曲线。由于节点状态中有连续角度 θ需要做离散化处理工程上通常把角度分成6到16个朝向提高角度分辨率会显著增加搜索空间一般10个左右够用。Hybrid A* 还有一个关键技术点用Reeds-Shepp曲线作为启发函数。搜索过程中的每个节点都计算从它到终点的Reeds-Shepp路径长度这个距离考虑了车辆运动学约束比欧氏距离精确得多能大幅减少无效搜索。搜索出来的路径已经可行但往往不够平滑曲率不连续。所以后续通常会加一个数值优化步骤把路径点作为优化变量最小化曲率变化率得到光滑可跟踪的轨迹。这个平滑步骤在完整系统中是不可缺的否则跟踪算法会非常吃力。3.4 离散路径点的约束匹配问题车辆路径规划的输入常常是一组离散路径点来自感知输出的道路中心线、全局规划的粗路径、人工设定的途径点等。把这些点变成车辆可执行的轨迹有几个关键约束要匹配。栅格分辨率要和车辆最小转弯半径匹配。如果栅格太细搜索空间会爆炸而且生成路径上的冗余点太多太粗则窄通道根本没法表达。经验上栅格大小取 0.5 到 1 倍车辆宽度转弯半径至少覆盖3到4个栅格。角度离散的精度影响的是路径末端能否精确停到目标姿态。停车场场景对终点航向要求极高通常需要角度分辨率到 5° 以内。如果达不到就得靠最终平滑阶段的优化来补偿。实际工程里最好的做法是把搜索和优化分离搜索阶段用粗分辨率快速找到一条可行路径优化阶段在连续空间精细调整。不要试图用搜索参数一次到位因为搜索参数和曲率约束根本是两套评价指标混在一起调会非常痛苦。4. 离散点参数化轨迹从一串离散点变成一条连续路4.1 为什么上层输出离散点不能直接喂给控制器路径规划和感知层输出的路径本质是一堆离散点直接让跟踪控制器消费这些点会出现几个非常现实的问题。点太密控制器收到的参考位置更新频率过高但位置增量很小容易受传感器噪声影响产生抖动点太疏轨迹在两个点之间是直线插值曲率信息完全丢失转弯处的跟踪精度会崩掉。更关键的是离散点只有位置信息没有速度、加速度、朝向的时间序列。控制器需要知道“什么时刻到达哪个位置”而不是“这一串点按什么顺序走”。所以必须做参数化把离散点拟合成连续曲线然后在曲线上做时间分配生成带时间戳的参考轨迹。机械臂领域的直线轨迹规划和圆弧轨迹规划也是同一回事给定起点终点乃至中间点要在关节空间或笛卡尔空间生成一条位置、速度、加速度连续的轨迹。离散点参数化的方法在两个领域完全通用。4.2 多项式、B样条、贝塞尔怎么选参数化方法离散点参数化的常见方法有三类各有优缺点。三次多项式插值计算简单、过点性好保证C2连续二阶导数连续但它的经典问题是不适合做平滑如果原始点有噪声或波动插值曲线会出现明显过冲和振荡而且因为不知道“哪个点是关键点”容易过度拟合噪声。B样条则是我在项目里用得最多的方法。核心优势是局部支撑性——修改一个控制点只影响附近几段曲线不会全局波及同时可以构造任意阶连续5阶B样条可以保证加加速度jerk连续这对机械臂和车辆跟踪都很友好。B样条拟合的关键是控制点数量和节点向量控制点设得比数据点少就自动实现平滑效果。贝塞尔曲线端点性质和凸包性质很好适合做局部小段过渡但高阶贝塞尔全局支撑一个控制点变化会影响整条曲线所以工程上不直接用高阶贝塞尔做长距离轨迹而是分段低阶拼接。选择逻辑可以这样简化如果要严格穿过每个路径点用三次样条并加平滑正则项如果路径点只是大致方向参考用B样条拟合控制点约等于数据点的三分之一平滑效果最好如果只是局部过渡处理用三阶贝塞尔即可。4.3 参数化之后的曲线质量评估不只是看坐标离散点参数化做完之后不能只看曲线是否经过原始点必须做运动学层面的质量评估。我个人的经验是只看位置连续性完全不够还会被仿真效果误导实际问题全都藏在更高阶量上。第一个指标是曲率连续性。曲率曲线如果出现尖角说明C2连续在几何层不成立跟踪时前轮转角会突变。车辆上表现为方向盘猛打机械臂上表现为关节速度突变。第二个指标是速度、加速度、加加速度曲线。加速度曲线不连续物理上意味着力/力矩突变加加速度过大机械结构会振动精度会下降。这是机械臂轨迹规划里非常看重 jerk 限制的原因车辆平顺性也有同样要求。第三个指标是时间分配是否合理。同一段几何路径用不同时间参数化会产生完全不同的动态特性。过快的速度规划会让动力学约束失效过慢则影响效率。实际中常用梯形速度曲线或S形速度曲线来规划时间分配S形曲线能保证加速度连续工程体验好得多。4.4 机械臂直线与圆弧轨迹规划同一套离散点问题的另一个解法机械臂运动规划里直线轨迹规划和圆弧轨迹规划是基础它们的做法和车辆离散点参数化本质是同一件事。直线轨迹规划通常写成多项式插值形式。给定初始位置、速度、加速度以及终端位置、速度、加速度用五次多项式就能唯一确定一条平滑轨迹。五次多项式恰好保证前两阶导数连续对应速度、加速度的连续这在机械臂控制里是基本面要求。圆弧轨迹规划的难点在于空间三点确定圆弧的计算。给定空间中的三个点先算圆心和半径再在圆弧上做角度插值最后映射回笛卡尔坐标。中间涉及到坐标变换和局部坐标系建立比二维圆弧要复杂得多。工程上做这类规划时我建议先把工具函数积累好。比如给定一段路径点序列能自动判断是直线段还是圆弧段然后分段参数化再拼接成完整轨迹。这种工具函数在车辆和机械臂两个领域都能复用写一次省半年。5. 常见问题与排查技巧实录5.1 倒立摆上翻过程震荡发散现象是上翻过程中摆角幅度越来越大能量误差曲线来回跳动不收敛。排查第一步是看能量误差曲线如果符号变化过于频繁说明增益 k 太大控制器在“过推”如果能量误差一直朝同一个方向单调变化说明控制方向判断有误重点检查 sgn 函数里的符号逻辑。还有一个容易被忽略的原因是角速度信号延迟。如果控制周期是10ms而滤波器带来30ms的延迟符号判断会和实际运动状态错位等价于控制器在一个滞后的系统上工作必然震荡。解决方法是调低滤波截止频率或者跑状态观测器。实操经验把控制律里的能量误差和控制输出画在同一张图上如果两者有明显的相位差基本就是信号延迟问题优先处理信号链而不是继续调增益。5.2 车辆路径跟踪时方向盘剧烈抖动现象是Hybrid A* 规划出来的路径看着没问题但跟踪算法跑起来方向盘反复猛打。排查时把路径点上的曲率画出来如果曲率曲线出现锯齿状跳变说明路径平滑度不足。这个问题的根源通常在平滑阶段目标函数。如果只约束了路径最终与原始点位置的偏差没有对曲率变化率做惩罚平滑结果很容易在弯曲段靠近极限曲率跟踪算法为了拉住车就得不停修正前轮转角。调试方法是在平滑优化的代价函数里加入曲率变化率惩罚项权重刚开始设很小时看不出来逐步加大就能解决。同时核查路径的最大曲率是否超过车辆最小转弯半径对应的曲率极限如果超出优化无解只能重新规划路径。5.3 离散点拟合出现过冲现象是三次样条在中间数据点附近出现明显不必要的弯曲走到两点的中间时轨迹偏出去很远。本质原因是样条插值对每个点都严格穿过当点的局部斜率变化很大时插值多项式为了满足所有约束会在中间产生振荡。解决方法有两种。第一种是改用B样条拟合允许曲线不经过原始数据点只逼近第二种是保留样条插值但加正则项比如在目标函数中加入三阶导平方积分项控制曲线总曲率变化率从而压制过冲。实操建议是区分硬约束和软约束路径点。必须经过的途径点比如机械臂的抓取点、车辆的终点停车位做硬插值只是参考方向的路点比如道路中心线采样点做软拟合允许小偏差换平滑性。这种混合处理方式在真实项目里效果最好。5.4 时间分配与几何路径脱节现象是规划的几何路径很平滑但跑起来动态效果还是不好加减速明显过猛。这个问题的根因在于许多轨迹规划算法只优化了几何路径速度/时间分配是后处理的两者没有联合优化。解决这类问题的常用做法是把几何规划和速度规划建模成多约束耦合的优化问题。在车辆领域就是把加速度约束、曲率-速度耦合约束一起放进优化在机械臂领域就是同时约束关节空间和笛卡尔空间的极限而不只约束其中一个。经验是先用低阶多项式做时间分配跑通链路后再逐步加入约束。一上来就叠加所有动力学约束求解器很容易不收敛排查起来也无从下手。从简到繁一步步加约束是这类优化问题的通用调法。这套内容前后串起来学习最大的收获是理解了“几何”和“物理”是两条线但最终必须汇聚到同一条参考轨迹上。规划层的离散点经过参数化成为连续轨迹控制层在动力学约束下跟踪这条轨迹两层的接口就是轨迹的质量指标——连续性、平滑性、可行性。我建议你学习时先在一个具体问题上跑通整个链路哪怕只是仿真里的小车或倒立摆再迁移到新场景会容易很多。倒立摆上翻是我做过性价比最高的实验它几乎覆盖了最优控制里最难的部分但状态维度又低到可以手工分析强烈建议实际跑一遍。最后再分享一个调试的小习惯所有控制算法都要把中间变量可视化出来光看最终效果是没法定位问题的。