最优控制、轨迹规划、倒立摆上翻控制、车辆运动学约束路径规划、离散点参数化——这些东西很多人是分开学的，但实际做项目时会发现它们全串在一条线上：规划层算出一条可执行路径，控制层负责把它跑出来，中间还夹着一堆离散点怎么变成连续轨迹的问题。这篇笔记是我自己从仿真到实机的一个阶段性整理，用倒立摆上翻和车辆路径规划两个实际案例把这条链路走了一遍，适合正在搞机器人控制、自动驾驶决策、或者机械臂运动规划的同行参考。
这篇笔记里我不会讲太多纯数学推导，重点放在每个环节为什么那样做、踩坑踩在哪里、以及怎么把理论方法落到代码里。内容偏实践，但有控制理论基础的人读起来会更有感觉，因为我讨论的每个方法都会回到“代价函数怎么设”“约束怎么表达”“数值上为什么不收敛”这类工程视角的问题。
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.1），R 取 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 时间分配与几何路径脱节
现象是规划的几何路径很平滑，但跑起来动态效果还是不好，加减速明显过猛。这个问题的根因在于：许多轨迹规划算法只优化了几何路径，速度/时间分配是后处理的，两者没有联合优化。
解决这类问题的常用做法，是把几何规划和速度规划建模成多约束耦合的优化问题。在车辆领域，就是把加速度约束、曲率-速度耦合约束一起放进优化；在机械臂领域，就是同时约束关节空间和笛卡尔空间的极限，而不只约束其中一个。
经验是，先用低阶多项式做时间分配，跑通链路后再逐步加入约束。一上来就叠加所有动力学约束，求解器很容易不收敛，排查起来也无从下手。从简到繁，一步步加约束，是这类优化问题的通用调法。
这套内容前后串起来学习，最大的收获是理解了“几何”和“物理”是两条线，但最终必须汇聚到同一条参考轨迹上。规划层的离散点经过参数化成为连续轨迹，控制层在动力学约束下跟踪这条轨迹，两层的接口就是轨迹的质量指标——连续性、平滑性、可行性。我建议你学习时先在一个具体问题上跑通整个链路，哪怕只是仿真里的小车或倒立摆，再迁移到新场景会容易很多。倒立摆上翻是我做过性价比最高的实验，它几乎覆盖了最优控制里最难的部分，但状态维度又低到可以手工分析，强烈建议实际跑一遍。最后再分享一个调试的小习惯：所有控制算法都要把中间变量可视化出来，光看最终效果是没法定位问题的。
