书名：Planning Algorithms
作者：Steven M. LaValle
链接：https://lavalle.pl/planning/
阅读章节：Chapter 8 - Feedback Motion Planning
第8章把第5--7章的"开环路径"升级为反馈规划 (Feedback Plan)：不再是预先计算一条路径再靠控制器跟踪（图1.19的解耦做法），而是直接构造从每个状态出发都知道该怎么走的映射 π:X→U\pi: X\to Uπ:X→U。本章刻意把"反馈"与"动力学"分离（图8.1的四象限）：在仍无动力学与显式不确定性模型的前提下，用隐式方式处理未来状态的不可预测性------万一执行中偏离了预期状态，反馈规划照样给出正确动作。这也等于图算法中的"单目的地最短路问题"（初始状态可不指定）。技术路线：先在离散状态空间建立反馈规划与导航函数概念（8.2节），再补充向量场、积分曲线与光滑流形的数学基础（8.3节），然后给出连续空间的完备方法------单元复形上的向量场、最优导航函数、以及面向动力学的Rimon-Koditschek导航函数（8.4节），最后是采样方法------漏斗组合与带插值的值迭代（8.5节）。
1. 动机：为什么需要反馈
- 物理世界中的未来状态不可预测；传统做法是先规划路径再设计跟踪控制器，高度解耦，算出的路径甚至可能根本不可跟踪。本章采用控制理论的隐性哲学：不建模不确定性，但用状态反馈"以防万一"。
- 反馈规划的另一个独立用途：初始状态未知时仍能工作（从任何状态都有动作指示）。
- 4.4.4节起允许渐近收敛、允许向量场在不连续意义下存在（Filippov解），到8.4.4再对向量场施加速度/加速度约束，为Part IV的动力学模型做铺垫。
2. 离散状态空间 (Discrete State Spaces)
- 形式化8.1 ：有限 XXX、U(x)U(x)U(x)、转移函数 fff、目标集 XGX_GXG、阶段可加成本 (8.3)。不指定初始状态。反馈规划 π:X→U\pi:X\to Uπ:X→U，π(x)=uT\pi(x)=u_Tπ(x)=uT（x∈XGx\in X_Gx∈XG）。它是可行解 当且仅当从所有能到达 XGX_GXG 的状态出发都能到达目标；是最优解 当且仅当从每个状态执行的总成本都最低。只需一个计划即可编码所有初始状态的最优路径------由动态规划原理保证（图8.2网格例子）。
- 导航函数 (Navigation Function) ：势函数 ϕ:X→0,∞\phi:X\to0,\\inftyϕ:X→0,∞ 经局部算子 (8.4)（可行，贪心下降）或 (8.5)（最优，含动作成本）恢复动作。三条性质：
- ϕ∣XG=0\phi|_{X_G}=0ϕ∣XG=0；
- ϕ=∞\phi=\inftyϕ=∞ 当且仅当目标不可达；
- 每个可达非目标状态经局部算子到达更低势状态------除目标外无局部极小，执行不循环 。最优导航函数即最优cost-to-go G∗G^*G∗，满足驻定动态规划方程 (8.6)。π\piπ 与 ϕ\phiϕ 是反馈计划的两种等价表示。
- 计算方法：Dijkstra（从目标反向）或值迭代产出最优导航函数；任意反向搜索（记录到目标的距离）产出可行导航函数。机场航站楼例子（朝小号登机口走）是生活化的导航函数。
- 网格化运动规划 （Manhattan运动模型＝1-邻域）：高分辨率网格"覆盖"整个C空间（区别于5.4.2节只把网格当拓扑图）------反馈要求处处有动作，分辨率必须高（每轴50--100点）。
- 波前传播算法 （图8.4）：单位成本下Dijkstra的简化，按波前 WiW_iWi 逐层并行赋值，O(n)O(n)O(n)。
- 最大间隙导航函数 NF2 ：波前从障碍边界出发向外传播，相向波前相遇处标记骨架点 （近似最大间隙路标图）；骨架上算 ϕ1\phi_1ϕ1（引导到目标）、骨架外算 ϕ2\phi_2ϕ2（引导到骨架），组合 ϕ=ϕ1+ϕ2\phi=\phi_1+\phi_2ϕ=ϕ1+ϕ2。缺点：多连通时不区分各条路线的间隙宽窄。
- Dial算法 ：整数成本的波前推广（桶替代优先队列）；2-邻域可减少阶梯状运动；高维（3--4维）可行，再高维网格法失效------逼出8.3的直接连续空间构造。
3. 向量场与积分曲线 (Vector Fields and Integral Curves)
- Rn\mathbb{R}^nRn 上的向量空间（8条公理、基、维数；函数空间是无限维例子）→ 每点 ppp 的切空间 Tp(Rn)T_p(\mathbb{R}^n)Tp(Rn)（nnn 维）→ 切丛 T(Rn)=⋃pTp(Rn)T(\mathbb{R}^n)=\bigcup_p T_p(\mathbb{R}^n)T(Rn)=⋃pTp(Rn)；向量场 f:Rn→Rnf:\mathbb{R}^n\to\mathbb{R}^nf:Rn→Rn 即"针图"（常值场、指向原点场、旋场三个例子）；光滑性 C∞C^\inftyC∞、微分同胚。
- 把向量场解释成速度场：自治微分方程 x˙=f(x)\dot{x}=f(x)x˙=f(x) (8.13)；积分曲线 τ\tauτ 满足 τ˙(t)=f(τ(t))\dot{\tau}(t)=f(\tau(t))τ˙(t)=f(τ(t)) (8.14)（Carathéodory意义解的积分形式 (8.15)）。存在唯一性：fff 光滑即可，或更一般地满足 Lipschitz条件 (8.16)。
- 分段光滑向量场与切换边界 S={x∣s(x)=0}S=\{x\mid s(x)=0\}S={x∣s(x)=0}：允许 fff 在 SSS 两侧光滑但跳变。采用 Filippov意义解 (8.21)------微分包含：几乎处处 τ˙\dot{\tau}τ˙ 属于邻域内 fff 值集的凸包 。切换边界上的四种流动：一致流（直观）、内向流 （沿 SSS 滑动，物理上引起颤振，可用正则化/滑模控制处理）、外向流（初值在 SSS 上可能非唯一）、切向流（回避）。
- 光滑流形 ：拓扑流形＋光滑结构 （坐标邻域族 U={(U,ϕ)}\mathcal{U}=\{(U,\phi)\}U={(U,ϕ)}：覆盖 MMM、重叠区坐标变换 ψ∘ϕ−1\psi\circ\phi^{-1}ψ∘ϕ−1 光滑（相容）、极大）。SnS^nSn 用 2(n+1)2(n+1)2(n+1) 个半球邻域或两个球极投影；RPn\mathbb{RP}^nRPn 用 n+1n+1n+1 个邻域（ϕi\phi_iϕi 除以 xix_ixi），RP3≅SO(3)\mathbb{RP}^3\cong SO(3)RP3≅SO(3) 由此获得光滑结构（四元数邻域 (8.30)）。趣闻：R4\mathbb{R}^4R4 有不可数多个"怪异"光滑结构。
- 流形上的切空间 ：内蕴定义＝满足线性性 (8.33) 与 Leibniz法则 (8.34) 的方向导数算子全体，构成 nnn 维向量空间；也可由坐标邻域参数化求导得基（S2S^2S2 球坐标例子 (8.35)(8.36)，法向量 ∇g\nabla g∇g）。微分同胚把切空间同构地联系起来------小幅扰动四元数参数即产生切向于SO(3)的运动。向量场与速度场概念自然地推广到流形：x˙=f(x)\dot{x}=f(x)x˙=f(x) 在 MMM 上定义 nnn 个微分方程（经坐标邻域表达）。
4. 连续空间的完备方法 (Complete Methods for Continuous Spaces)
- 形式化8.2 ：XXX 为光滑流形（常为 CfreeC_{free}Cfree）；U(x)=Tx(X)U(x)=T_x(X)U(x)=Tx(X)（任选速度，uTu_TuT＝零速度停）；x˙=u\dot{x}=ux˙=u (8.38)；目标集 XGX_GXG。π\piπ 即 XXX 上的向量场。解＝从可达集 XrX_rXr 中每点出发的积分曲线都到达 XGX_GXG（有限时间；若只渐近收敛则称渐近解）。毛球定理 使偶数维 SnS^nSn 上不存在处处非零的光滑向量场------最强只能要求"除零测集外光滑"。归一化向量场（速度恒为单位或零）只给方向，便于存在性分析。最优版本引入连续成本泛函 (8.39)。
- 连续导航函数 ：梯度下降 π(x)=−∇ϕ/∣∇ϕ∣\pi(x)=-\nabla\phi/|\nabla\phi|π(x)=−∇ϕ/∣∇ϕ∣（Khatib的势场思想）；吸引势（二次势，例8.17）＋排斥势绕障------但组合后局部极小问题依旧，破坏导航函数性质。
- 单元复形上的向量场 （8.4.2）：把 XXX 单元分解后分层构造：
- 在 nnn-单元上定义离散规划问题（超状态＝单元，超动作＝共享 (n−1)(n-1)(n−1) 维面的相邻单元），算离散导航函数；
- 每个单元上定义光滑向量场使所有积分曲线流向"出口面"（目标单元用归一化内向场；其余单元用从"排斥顶点"发出的外向场，图8.11）。结果是不连续向量场，积分曲线按Filippov意义理解，有限时间到 xGx_GxG。
- 最优导航函数（连续Dijkstra，8.4.3） ：单多边形 XXX、欧氏距离成本。可见多边形 V(x)V(x)V(x)＝从 xxx 可见的所有点（线段全在 XXX 内），其上 ϕ(x′)=∥x′−xG∥\phi(x')=\|x'-x_G\|ϕ(x′)=∥x′−xG∥ (8.42)；V(x)V(x)V(x) 边界的路点 (Way Points) 是某些最优路径必经之地；算法从 V(xG)V(x_G)V(xG) 出发迭代扩展：ϕ(x)=ϕ(x′)+∥x−x′∥\phi(x)=\phi(x')+\|x-x'\|ϕ(x)=ϕ(x′)+∥x−x′∥ (8.43)，队列按Dijkstra式排序。可见多边形 O(nlgn)O(n\lg n)O(nlgn)（径向扫描），总 O(n2lgn)O(n^2\lg n)O(n2lgn)；精细实现可到 O(nlgn)O(n\lg n)O(nlgn)。不能推广到三维（3D最短路NP-hard）。
- 迈向动力学（8.4.4） ：加速控制模型 x¨=u\ddot{x}=ux¨=u (8.44)；跟踪指令速度场 f(x)f(x)f(x) 的反馈律 u=K(f(x)−x˙)+x˙⋅∇f(x)u=K(f(x)-\dot{x})+\dot{x}\cdot\nabla f(x)u=K(f(x)−x˙)+x˙⋅∇f(x) (8.47)（增益 KKK，偏差纠正＋场方向导数项）。有界速度/加速度模型 ：通常不影响解的存在性（可以开慢点），但不连续向量场必然违反加速度界（需要无穷大加速度）；Lipschitz常数的场满足界。
- Rimon-Koditschek意义下的导航函数 ：四个充分条件------
- 光滑（C2C^2C2）；
- 在含 xGx_GxG 的连通分支上唯一局部极小在 xGx_GxG（关键：非单连通空间上光滑场必有其他临界点，由Morse理论/Sard定理其吸引域为零测集，故放宽为"几乎处处到达"）；
- 在 ∂Cfree\partial C_{free}∂Cfree 上取极大且为常数；
- 是 Morse函数（临界点处Hessian (8.48) 非退化）。构造法 829：从球/星形等简单区域上已知导航函数出发，用保持四条件的畸变变换（如把球光滑拉伸成可见区域）组合出复杂环境的导航函数。优点是可用Lyapunov稳定性证明收敛。
- 调和势函数 (Harmonic Potential) ：满足Laplace方程 ∇2ϕ=0\nabla^2\phi=0∇2ϕ=0 (8.49)；Dirichlet边界条件 （边界取常值）引导到目标，Neumann条件（速度切于障碍边界）绕障。存在性已证，但需显式边界且数值求解（Gauss-Seidel迭代）昂贵------实用受限；有采样构造法 124。
5. 连续空间的采样方法 (Sampling-Based Methods)
- 漏斗组合 (Composition of Funnels) （Mason的比喻）：导航函数＝一个大漏斗；改为把 XXX 分解为重叠简单区域，每个区域一个小漏斗，把状态"倒"进下一条漏斗（图8.14）。近似覆盖 (8.50) （邻域可重叠、可越界；单元分解是无重叠特例）。离散超状态＝邻域，超动作＝有交集的邻域对；向量场把 OOO 中状态送进 O∩O′O\cap O'O∩O′，到达后切换到 O′O'O′ 的场（交界处不连续，可混合）。各局部导航函数按 (8.51) 拼成全局导航函数（加偏移常数 c ϕ(O)c\,\phi(O)cϕ(O) 保证重叠处取 ϕ\phiϕ 最小的那个邻域）。
- 基于采样的覆盖构造 ：稠密序列 α\alphaα 增量进行：样本自由且未被覆盖→以其到障碍的距离 ddd 构造无碰撞球 B(x,r)B(x,r)B(x,r) (8.52)。半径确定：点机器人 r=dr=dr=d；一般位形空间用机器人位移界（SE(2) 为椭球 (8.53)，重参数化 θ→rθ\theta\to r\thetaθ→rθ 后变球 (8.54)(8.55)）。高维中球偏小、数量庞大是瓶颈。终止条件：随机采样时统计失败率可估计覆盖率并以概率 PcP_cPc 保证覆盖比例 α\alphaα；确定性采样可用分散度判据。缺陷：基于体积的判据无法保证覆盖保持连通性（狭窄通道可能漏盖）。
- 带插值的动态规划 (8.5.2)＝本章压轴、Part II的收尾：值迭代＋插值求数值最优导航函数 。
- 连续状态空间：网格采样 SSS，递推 (8.56) 不变，但 Gk+1(xk+1)G_{k+1}(x_{k+1})Gk+1(xk+1) 由插值得到------一维线性插值 (8.57)(8.58)，nnn 维多线性插值 2n2^n2n 邻居 (8.59)；重心剖分 （Coxeter-Freudenthal-Kuhn三角剖分）把立方体分成单纯形，只需 n+1n+1n+1 个顶点，定位 O(nlgn)O(n\lg n)O(nlgn)。插值区域 R(S)R(S)R(S)：所有邻居都在 SSS 中的点的集合；采样应低分散，收敛速率依赖分散度＋Lipschitz条件。
- 连续动作空间：U(x)U(x)U(x) 也采样（低分散，总含 u=0u=0u=0），或用非线性优化直接解 min\minmin。
- 与运动规划的对接五要点 ：
- U(x)U(x)U(x) 有界（单位球 (8.60)）；
- 时间离散化------Euler近似 x(t+Δt)=x(t)+Δt ux(t+\Delta t)=x(t)+\Delta t\,ux(t+Δt)=x(t)+Δtu (8.62--8.64)（Runge-Kutta更好；Δt\Delta tΔt 须与网格分辨率匹配，否则插值值不更新）；
- 成本离散化 ldl_dld（欧氏最短路直接取 ∥u∥\|u\|∥u∥）(8.65)；
- 障碍：CobsC_{obs}Cobs 中样本 G=∞G=\inftyG=∞，动作的下一状态所有插值邻居须自由（自动收缩 R(S)R(S)R(S)，图8.21b）；
- 目标的 R(G)R(G)R(G) 非空（单点目标必须放大，图8.21c）。解得的 GGG 经插值即导航函数，执行轨迹不必踩格点------Δt\Delta tΔt、网格间距趋于零时逼近真最优。与7.7节呼应：Manhattan/独立关节模型恰是网格上的离散值迭代，欧氏模型靠插值终于可解。
- Dijkstra式实现 ：死/活/未访问三类样本；反投影 B(x)={x′∣∃u, f(x′,u)=x}B(x)=\{x'\mid \exists u,\ f(x',u)=x\}B(x)={x′∣∃u, f(x′,u)=x} (8.66)、边界 (Frontier) Front(S)=(B(R(S))∖S)∩S\mathrm{Front}(S)=(B(R(S))\setminus S)\cap SFront(S)=(B(R(S))∖S)∩S (8.68)；优先队列按当前值弹出最小者并入死集，更新受影响的活样本、从边界扩张------类似连续Dijkstra，可证收敛（值不会高于值迭代的计算值）。特殊情形有波前传播版本。
6. 总结与思考
-
核心收获
- 反馈规划 π:X→U\pi:X\to Uπ:X→U 与导航函数 ϕ\phiϕ 是同一对象的两种表示；导航函数的本质要求就是"除目标外无局部极小"（离散三条件/连续RK四条件），这把"规划"变成了"设计势函数"。
- 离散工具的连续化有清晰的技术链条：网格波前 → 分段光滑向量场（Filippov）→ 光滑流形上的梯度场 → 带插值的值迭代；后者是Part II对"最优反馈运动规划"给出的通用数值答案。
- 8.4.4的约束分析揭示了一个普适矛盾：最优性与可实现性冲突------最优点往往需要不连续场，而加速度约束要求光滑；工程上只能取可行解，并用收敛性证明兜底。
-
个人感悟
- 本章思想正是当今机器人导航栈的骨架：占据栅格＋NDT/ESDF上跑"波前传播/距离变换"生成梯度场（即 ϕ\phiϕ），再由局部规划器跟踪------本质是NF2与值迭代的工程化；人工势场法在无人机集群、机械臂避障中仍随处可见，而"局部极小"教训催生了后续研究（谐波场、导航函数构造、以及直接以采样式反馈规划绕过显式势函数）。
- 带插值的值迭代在今天以"网格世界里的Dijkstra/A*＋插值"（如导航函数、FMT*的cost-to-go估计）和"神经辐射场式的连续cost-to-go回归"等形式回归；漏斗组合则与TAMP（任务与运动规划）中"技能原语组合"、控制里的LQR-Trees一脉相承------先把状态空间盖住，再把每个邻域变成收敛盆，这一"覆盖＋局部策略"范式仍然活跃。