4 运动-动力学的路径发现

4.1 Introduction

1. 什么是 Kinodynamic

   • 定义：kinodynamic = kinematic + dynamic
   • 核心任务：生成一条既避障（运动学约束），又满足物理限制（如最大速度、加速度）的轨迹
   • Bruce Donald：运动-动力学规划问题是综合生成一个机器人运动，使其同时满足运动学约束（如避开障碍物）和动力学约束（如对速度、加速度、力的模值限制）。
   • 注意：kinodynamic 解决方案是一个从时间到广义力或加速度的映射
     → 即输出不是简单的路径，而是可执行的控制指令序列

2. 为什么需要 Kinodynamic Planning

   • 旧式流程
     

     • Path Finding（路径搜索）：找一条几何上可行的路径（如 RRT、A）
     • Trajectory Optimization（轨迹优化）：将路径平滑为可执行轨迹（如样条插值）

   • 问题

     • 直线连接两个状态通常是无效的！
     • 因为系统有 微分约束 differential constraints
     • 所以：先找路径再优化 ≠ 可行轨迹

   • 粗到细过程 Coarse-to-fine process

     • 先用简单模型（如点机器人）找路径，再用复杂模型优化，但最终可能无法实现（如汽车无法原地旋转）

   • Trajectory only optimizes locally

     • 局部优化可能陷入局部最优，忽略全局可行性

   • Infeasible path means nothing to nonholonomic system

     • 对于非完整系统（nonholonomic），即使路径在几何上连通，也可能无法执行

   • 结论：必须从一开始就考虑运动-动力学约束

3. 两个例子
   

4. 经典运动-动力学模型示例

   • 单轮车 Unicycle
     

     • 微分方程：$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right]=\left[\begin{array}{cc}\cos \theta & 0\\ \sin \theta & 0\\ 0& 1\end{array}\right]\cdot \left[\begin{array}{c}v\\ \omega \end{array}\right]$
     • 约束：$|v|\le v_{\max },|\omega |\le {\omega }_{\max }$

   • 差速驱动 Differential Drive
     

     • 微分方程：$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right]=\left[\begin{array}{c}\frac{r}{2}({\omega }_l+{\omega }_r)\cos \theta \\ \frac{r}{2}({\omega }_l+{\omega }_r)\sin \theta \\ \frac{r}{L}({\omega }_r-{\omega }_l)\end{array}\right]$
     • 约束：$|{\omega }_l|\le {\omega }_{l,\max }$，$|{\omega }_r|\le {\omega }_{r,\max }$
     • 速度合成：$v=\frac{r}{2}({\omega }_l+{\omega }_r)$，$\omega =\frac{r}{L}({\omega }_r-{\omega }_l)$

   • 简化汽车模型
     

     • 微分方程：$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right]=\left[\begin{array}{c}v\cos \theta \\ v\sin \theta \\ \frac{r}{L}\tan \varphi \end{array}\right]$

     • 三种变体
       { ∣ v ∣ ≤ v max ⁡ ,   ∣ ϕ ∣ ≤ ϕ max ⁡ < π 2 , Simple car model v ∈ { − v max ⁡ , v max ⁡ } ,   ∣ ϕ ∣ ≤ ϕ max ⁡ < π 2 , Reeds & Shepp’s car v = v max ⁡   ∣ ϕ ∣ ≤ ϕ max ⁡ < π 2 , Dubin’s car \begin{cases} |v| \leq v_{\max},~|\phi|\leq\phi_{\max}<\frac{\pi}{2}, &\text{Simple car model} \\ v\in\{-v_{\max}, v_{\max}\},~|\phi|\leq\phi_{\max}<\frac{\pi}{2}, &\text{Reeds \& Shepp's car} \\ v=v_{\max}~|\phi|\leq\phi_{\max}<\frac{\pi}{2}, &\text{Dubin's car} \end{cases} ⎩ ⎨ ⎧​∣v∣≤vmax​, ∣ϕ∣≤ϕmax​<2π​,v∈{−vmax​,vmax​}, ∣ϕ∣≤ϕmax​<2π​,v=vmax​ ∣ϕ∣≤ϕmax​<2π​,​Simple car modelReeds & Shepp’s carDubin’s car​

4.2 State Lattice Planning

4.2.1 Workflow

1. 基本概念

   • 标准的基于图搜索的方法对真实机器人有用吗？
     • 答案是否定的
     • 因为它假设机器人可以任意移动（如点质量）
     • 忽略了动力学约束（速度、加速度、转向半径等）
   • 新目标
     • 我们不再假设机器人是一个质点（mass point）
     • 而要求图中的每条边都代表一个机器人能实际执行的运动轨迹
   • 动机
     • 以前的图只是“几何可行”
     • 现在要“动力学可行” → 所有边必须是机器人能完成的动作
   • 方法
     • 手动构建一个图，其中所有边都是可执行的运动原语（motion primitives）
   • 两种思路
     • Forward direction：在 控制空间（control space） 中离散采样
     • Reverse direction：在 状态空间（state space） 中离散采样
   • State lattice planning 就是最直接的一种方法

2. 与第 2 章的联系
   

   • 图解说明
     • 第一步，将环境划分为格子（4/8 连接）
     • 第二步，在每个格子中添加多个方向的连接（绿色线）
     • 第三步，红色框内是关键：这其实是在 控制空间离散化
   • 结论
     • 这种“带方向的连接”其实是对控制空间的离散化
     • 构建的是一个 Lattice Graph（格点图）

3. 与第 3 章的联系
   

   • 图解说明
     • 点状网络表示在 状态空间 ${\mathbb{R}}^2$ 中采样得到的节点
     • 边表示两点之间的连接（但未明确说明是否可行）
   • 问题：如何保证边是可执行的

4. 如何生成可行局部运动

   • 核心问题：

     • 如何让机器人从初始状态 $s_0$ 移动到目标状态 $s_f$

   • 选择控制输入 $u$，前向仿真
     

     • 给定 $u$ 和时间 $T$，数值积分得到新状态 $s_1$
     • 优点：易实现，无需反解
     • 缺点：规划效率低，无任务引导

   • 选择目标状态 $s_f$，求逆向轨迹
     

     • 给定 $s_0$ 和 $s_f$，求出控制序列使系统到达
     • 优点：任务导向强，规划效率高
     • 缺点：实现难（需解微分方程），计算复杂

5. 在控制空间中采样

   • 以四旋翼模型为例
     $$\begin{gathered} \text{State: }s=\left[\begin{array}{c}x\\ y\\ z\\ \dot{x}\\ \dot{y}\\ \dot{z}\end{array}\right]\text{Input: }u=\left[\begin{array}{c}\ddot{x}\\ \ddot{y}\\ \ddot{z}\end{array}\right] \\ \text{System equation: }\dot{s}=A\cdot s+B\cdot u \\ A=\left[\begin{array}{ccccccc}0& 0& 0& 1& 0& 0& \\ 0& 0& 0& 0& 1& 0& \\ 0& 0& 0& 0& 0& 1& \\ 0& 0& 0& 0& 0& 0& \\ 0& 0& 0& 0& 0& 0& \\ 0& 0& 0& 0& 0& 0& \end{array}\right]\stackrel{推广到高维}{⟶}A=\left[\begin{array}{ccccc}0& I_3& 0& \cdots & 0\\ 0& 0& I_3& \cdots & 0\\ ⋮& \ddots & \ddots & \ddots & ⋮\\ 0& \cdots & \cdots & 0& I_3\\ 0& \cdots & \cdots & 0& 0\end{array}\right] \\ B=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\stackrel{推广到高维}{⟶}B=\left[\begin{array}{c}0\\ 0\\ ⋮\\ 0\\ I_3\end{array}\right] \end{gathered}$$

   • 从一个点出发的所有可能运动

     • 只离散加速度值 Discretize acceleration
       

       $$v_0={\left[\begin{array}{ccc}1& 0& 0\end{array}\right]}^{\top }$$

     • 离散加加速度值 Discretize jerk
       

       $$v_0={\left[\begin{array}{ccc}1& 0& 0\end{array}\right]}^{\top },a_0={\left[\begin{array}{ccc}0& 1& 0\end{array}\right]}^{\top }$$

     • 说明：给定初始状态 $s_0$，选择不同的控制输入 $u$，每条紫色曲线代表一条 前向仿真轨迹，所有轨迹都满足系统的动力学方程

   • 状态转移与积分
     $$s(t)=e^{At}{s}_0+{\int }_0^t{e}^{A(t-\sigma )}B\mathrm{d}\sigma \cdot u_m$$
     其中

     • $F(t)=e^{At}{s}_0$：自由响应（零输入响应）
     • $G(t)={\int }_0^t{e}^{A(t-\sigma )}B\mathrm{d}\sigma \cdot u_m$：强迫响应（零状态响应）

   • 特殊性质：幂零矩阵 Nilpotent Matrix

     • 如果矩阵 $A$ 是幂零的（存在 $k$ 使得 $A^k=0$），那么
       $$e^{At}=I+\frac{At}{1!}+\frac{(At{)}^2}{2!}+\cdots +\frac{(At{)}^{k-1}}{(k-1)!}$$

     • 这意味着 可以精确计算轨迹，无需数值积分！

     • 举个例子：对于二阶积分器（如位置→速度→加速度）， $A$ 是幂零的，所以 $e^{At}$ 是一个二次多项式。

   • 格点图 Lattice Graph 的生成
     

     • 左图 9 方向离散化：从中心点出发，有 9 个方向的运动原语（motion primitives），每条边代表一个可执行的动作
     • 右图 25 方向离散化：更多方向，更灵活的运动能力，但计算量更大，存储更多
     • 说明：图可以在搜索时动态构建（on-the-fly），新发现节点时才创建状态和边，节省时间和空间。

   • 汽车模型的控制空间采样（Bicycle Model）
     

     • 从搜索树中选一个状态 $s$
     • 选择控制向量 $u$（即速度 $v$ 和转向角 $\varphi$）
     • 在短时间内进行数值积分（如欧拉法或龙格库塔法），得到新状态
     • 如果轨迹无碰撞，则添加到树中

6. 在状态空间中采样

   • 构建 Reeds-Shepp Car Model 的格点图
     

     • 初始状态：
       中心是一个车辆状态（位置 + 方向）
       周围是均匀分布的网格点（代表可能的状态）
     • 找到 8 个邻居的可行路径
       对每个邻居节点，计算从当前状态到该节点的最短路径
       使用的是 Reeds-Shepp 路径（由直线和圆弧组成）
     • 扩展到 24 个邻居
       不仅考虑相邻格点，还考虑更远的可达状态
       每条边都是一个最优路径段
     • 完整格点图
       所有状态之间通过最优路径连接
       形成一个密集的、可搜索的图结构

   • 双层格点图 - 不同初始状态的影响
     

     • 两个图看起来非常相似
     • 但第一层不同（即离初始状态最近的邻居）
     • 因为 初始状态不同 → 导致局部可达性变化

   • 控制空间与状态空间采样的对比
     

     • 控制空间采样：从一个点出发，尝试所有可能的控制输入（如加速度、转向角），生成发散的轨迹；生成大量轨迹，集中在初始角速度方向（因为惯性作用）；探索性强，但可能产生冗余轨迹。
     • 状态空间采样：在状态空间中采样点，然后连接它们（如使用 Reeds-Shepp 路径）；更倾向于生成平滑、符合几何约束的路径；结构化强，但需预先定义可达性。

4.2.2 Boundary Value Problem

1. 为什么需要“边界值问题”？

   因为机器人运动不是“从A点到B点”的简单直线，而是要考虑：

   • 起始时的速度和加速度
   • 终点时的速度和加速度
   • 运动过程中的平滑性（避免抖动）
   • 能耗/舒适性（如最小化“急加速”）

   所以我们需要设计一条满足起终点约束的轨迹 x(t)，这就是边界值问题。

2. 基础概念：边界值问题 BVP
   

   • 定义：给定一个系统，我们希望找到一条轨迹 x(t)，使得它满足
     • 初始状态：x(0)=a
     • 终止状态：x(T)=b
   • 其中：t 是时间，a 和 b 是已知的起点和终点位置（或状态），T 是总时间
   • 为什么难解？
     • 没有通用公式！必须根据具体需求逐个设计。
     • 往往需要复杂的数值优化方法求解。

3. 多项式插值法（Polynomial Trajectory）

   最常用的非最优方法

   • 假设轨迹是一个 5 次多项式
     $$x(t)=c_5{t}^5+c_4{t}^4+c_3{t}^3+c_2{t}^2+c_{1}t+c_0$$

   • 有边界条件

     	Position	Velocity	Acceleration
     t=0	a	0	0
     t=T	b	0	0

   • 求解
     $$\left[\begin{array}{c}a\\ b\\ 0\\ 0\\ 0\\ 0\end{array}\right]=\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 1\\ T^5& T^4& T^3& T^2& T& 1\\ 0& 0& 0& 0& 1& 0\\ 5T^4& 4T^3& 3T^2& 2T& 1& 0\\ 0& 0& 0& 2& 0& 0\\ 20T^3& 12T^2& 6T& 2& 0& 0\end{array}\right]\left[\begin{array}{c}{c}_5\\ c_4\\ c_3\\ c_2\\ c_1\\ c_0\end{array}\right]$$
     解这个方程组即可得到所有系数

   • 结果：一条光滑、连续的轨迹，满足起终点位置、速度、加速度为零

   • 缺点：没有考虑“最优性”，比如如何让轨迹最平稳，如何让能量消耗最小

4. 最优边界值问题（OBVP）

   • 核心思想：不仅要满足边界条件，还要最小化某个代价函数

   • 比如：最小化加加速度 jerk
     $$J_{\sum }=\sum _{k=1}^3{J}_k,J_k=\frac{1}{T}{\int }_0^T{j}_k(t{)}^2\mathrm{d}t$$
     状态变量：$S_k=(p_k,v_k,a_k)$

     输入：$u_k=j_k$

     系统模型：$\dot{s}=f(s,u)=(v,a,j)$

5. 求解最优问题：Pontryagin’s 最小值原理

   • 问题定义

     • 代价函数
       $$J=\underset{\text{终端代价}}{\underbrace{h(s(T))}}+\underset{\text{运行代价}}{\underbrace{{\int }_0^{T}g(s(t),u(t))\cdot \mathrm{d}t}}$$

     • 哈密顿方程和协状态变量
       $$\begin{gathered} H(s,u,\lambda )=g(s,u)+{\lambda }^{\top }f(s,u) \\ \lambda =({\lambda }_1,{\lambda }_2,{\lambda }_3) \end{gathered}$$

     • 假设
       $$\begin{gathered} s^{\ast }\text{：最优状态} \\ {u}^{\ast }\text{：最优输入} \end{gathered}$$

   • 核心思想：一个控制输入 $u^{\ast }(t)$ 是最优的，当且仅当存在一个协状态轨迹 $\lambda (t)$，使得以下三个条件同时成立

   • 状态方程：给定初始状态 $s^{\ast }(0)=s(0)$，考虑
     $${\dot{s}}^{\ast }(t)=f(s^{\ast }(t),u^{\ast }(t))$$

   • 协状态条件

     • 协状态方程：描述协状态 $\lambda (t)$ 如何随时间变化
       $$\dot{\lambda }(t)=-{\nabla }_{s}H(s^{\ast }(t),u^{\ast }(t),\lambda (t))$$

       有几个状态就有几个协状态。

     • 终端边界条件：协状态在终点满足
       $$\lambda (T)=-\nabla h(s^{\ast }(T))$$
       $h(s)$ 是终端代价函数

   • 最优控制输入：最优输入通过最小化哈密顿函数得到
     $$u^{\ast }(t)=\arg \underset{u(t)}{\min }H(s^{\ast }(t),u(t),\lambda (t))$$

6. 详细过程说明

   • 目标函数：最小化加加速度 jerk 的平方积分
     $$J_{\sum }=\sum _{k=1}^3{J}_k,J_k=\frac{1}{T}{\int }_0^T{j}_k(t{)}^{2}dt$$

   • 系统

     • 状态：$s_k=(p_k,v_k,a_k)$

     • 输入：$u_k=j_k$

     • 系统动力学方程：${\dot{s}}_k=f_s(s_k,u_k)=(v_k,a_k,j_k)$

     • 初始状态：$s(0)=(p_0,v_0,a_0)$

   • 求解过程（基于庞加莱最小值原理）

     • 引入协状态变量 costate $\lambda$，表示对状态的“敏感度”
       $$\lambda =({\lambda }_1,{\lambda }_2,{\lambda }_3)$$

       ${\lambda }_1$ 代表总代价 $J$ 对位置 $p$ 的敏感度

       ${\lambda }_2$ 代表总代价 $J$ 对速度 $v$ 的敏感度

       ${\lambda }_3$ 代表总代价 $J$ 对加速度 $a$ 的敏感度

     • 构造哈密顿函数 Hamiltonian $H$
       $$H(s,u,\lambda )=\frac{1}{T}{j}^2+{\lambda }^T{f}_s(s,u)$$
       展开为
       $$H=\frac{1}{T}{j}^2+{\lambda }_{1}v+{\lambda }_{2}a+{\lambda }_{3}j$$

       哈密顿函数：最优控制理论中的核心工具，用于描述系统在最优路径下的“总代价”或“能量”

       哈密顿函数将目标函数和系统动力学结合起来，是判断“当前输入是否最优”的数学依据。

     • 协状态方程
       $$\begin{array}{rl}\dot{\lambda }& =-{\nabla }_{s}H(s^{\ast }(t),u^{\ast }(t),\lambda (t))\\ & =-(\frac{\partial H}{\partial p},\frac{\partial H}{\partial v},\frac{\partial H}{\partial a})\\ & =(0,-{\lambda }_1,-{\lambda }_2)\end{array}$$

     • 通过求解 $H$ 的极小值，可以得到最优输入 $u^{\ast }(t)$，在本例中，$u^{\ast }(t)=\arg \min H$，最终解得 $j^{\ast }(t)=\frac{1}{2}\alpha t^2+\beta t+\gamma$

   • 最优解表达式

     • 共态变量解：求解微分方程 $\dot{\lambda }=(0,-{\lambda }_1,{\lambda }_2)$
       $$\lambda (t)=\frac{1}{T}\left[\begin{array}{c}-2\alpha \\ 2\alpha t+2\beta \\ -\alpha t^2-2\beta t-2\gamma \end{array}\right]$$

     • 最优输入 jerk
       $$\begin{array}{rl}{u}^{\ast }(t)=j^{\ast }(t)& =\arg \underset{j(t)}{\min }H(s^{\ast }(t),j(t),\lambda (t))\\ & =\frac{1}{2}\alpha t^2+\beta t+\gamma \end{array}$$

       假设 $s=s^{\ast }$ 取到最优，那么：$\frac{\partial H}{\partial u}=\frac{2}{T}j+{\lambda }_3$，令 $\frac{\partial H}{\partial u}=0$ 得 $u^{\ast }=-\frac{T}{2}{\lambda }_3$，再代入 ${\lambda }_3$

     • 最优轨迹
       $$s^{\ast }(t)=\left[\begin{array}{c}\frac{\alpha }{120}{t}^5+\frac{\beta }{24}{t}^4+\frac{\gamma }{6}{t}^3+\frac{a_0}{2}{t}^2+v_{0}t+p_0\\ \frac{\alpha }{24}{t}^4+\frac{\beta }{6}{t}^3+\frac{\gamma }{2}{t}^2+a_{d}t+v_0\\ \frac{\alpha }{6}{t}^3+\frac{\beta }{2}{t}^2+\gamma t+a_0\end{array}\right]$$

   • 代价函数：将 $j^{\ast }(t)=\frac{1}{2}\alpha t^2+\beta t+\gamma$ 代入 $J={\int }_0^{T}j(t{)}^2\mathrm{d}t$ 积分得
     $$J={\gamma }^2+\beta \gamma T+\frac{1}{3}{\beta }^2{T}^2+\frac{1}{3}\alpha \gamma T^2+\frac{1}{4}\alpha \beta T^3+\frac{1}{20}{\alpha }^2{T}^4$$

     • $J$ 只依赖于 $T,\alpha ,\beta ,\gamma$，这些参数由边界条件确定
     • 所以可以将 $T$ 也作为优化变量来求解

   • 求解参数 $\alpha ,\beta ,\gamma$，通过边界状态（初始和终止位置、速度、加速度）建立的线性方程组
     $$\left[\begin{array}{ccc}\frac{1}{120}{T}^5& \frac{1}{24}{T}^4& \frac{1}{6}{T}^3\\ \frac{1}{24}{T}^4& \frac{1}{6}{T}^3& \frac{1}{2}{T}^2\\ \frac{1}{6}{T}^3& \frac{1}{2}{T}^2& T\end{array}\right]\left[\begin{array}{c}\alpha \\ \beta \\ \gamma \end{array}\right]=\left[\begin{array}{c}\Delta p\\ \Delta v\\ \Delta a\end{array}\right]$$
     这个矩阵是 Gram 矩阵，可逆，能唯一解出 $\alpha ,\beta ,\gamma$。其中
     $$\left[\begin{array}{c}\Delta p\\ \Delta v\\ \Delta a\end{array}\right]=\left[\begin{array}{c}{p}_f-p_0-v_{0}T-\frac{1}{2}{a}_0{T}^2\\ v_f-v_0-a_{0}T\\ a_f-a_0\end{array}\right]$$
     解得（系数变换矩阵）
     $$\left[\begin{array}{c}\alpha \beta \gamma \end{array}\right]=\frac{1}{T^3}\left[\begin{array}{ccc}720& -360T& 60T^2\\ -360T& 168T^2& -24T^3\\ 60T^2& -24T^3& 3T^4\end{array}\right]\left[\begin{array}{c}\Delta p\\ \Delta v\\ \Delta a\end{array}\right]$$

   • 求解最优时间 $T$：将上式代入代价函数 $J$，得到
     $$J(T)=\text{关于 T 的函数}$$
     由于 $J(T)$ 是 $T$ 的函数，我们可以：

     • 对 $J(T)$ 求导
     • 令导数为零
     • 解出使 $J$ 最小的 $T^{\ast }$

     这变成了一个多项式根寻找问题（polynomial root finding problem）

   • 使用范围说明

     • 此推导适用于 固定终点状态：即 $s(T)=(p_f,v_f,a_f)$ 完全已知。
     • 若终点部分自由（如只知道位置，速度/加速度未知），也可以类似地推导，但形式略有不同。

7. 固定终点与（部分）自由终点问题

   • 常见场景

     • 只知道终点位置，但允许任意速度和加速度（如从 A 到 B 的最短时间）
     • 只知道终点速度为零，但位置可以调整（如平稳降落）
     • 终点状态完全自由（如最大化飞行距离）

   • 固定终点问题

     • 目标：强制要求 $s(T)=s_T$

     • 继续使用 $h$ 函数

       • 引入一个指示函数作为终端代价

       $$h(s)=\{\begin{array}{ll}0,& \text{if s=sT}\\ \infty ,& \text{otherwise}\end{array}$$

       • 问题：$\lambda (T)=-\nabla h(s^{\ast }(T))$ 不可用，因为 $h(s)$ 在 $s=s_T$ 处不连续且不可微。

     • 解决办法：放弃这个条件，改用直接代入法，直接利用已知的终点状态 $s(T)=(p_f,v_f,a_f)$，将其代入最优轨迹表达式
       $$s^{\ast }(t)=\left[\begin{array}{c}\frac{\alpha }{120}{t}^5+\frac{\beta }{24}{t}^4+\frac{\gamma }{6}{t}^3+\frac{{\alpha }_0}{2}{t}^2+v_{0}t+p_0\\ \frac{\alpha }{24}{t}^4+\frac{\beta }{6}{t}^3+\frac{\gamma }{2}{t}^2+a_{0}t+v_0\\ \frac{\alpha }{6}{t}^3+\frac{\beta }{2}{t}^2+\gamma t+a_0\end{array}\right]$$
       令 $s^{\ast }(T)=s(T)$，建立方程组，求解未知参数 $\alpha ,\beta ,\gamma$

   • （部分）自由终点问题

     • 给定 $s_i(T),i\in I$，其它变量是自由变量。

     • 新边界条件：当某些状态分量是自由的，可以建立新的协状态边界条件
       $${\lambda }_j(T)=\frac{\partial h(s^{\ast }(T))}{\partial s_j},\text{ for }j\ne i$$
       其中 $i$ 是那些被约束的状态索引。

     • 对于自由变量：若 $s_j(T)$ 是自由的，则对应的 ${\lambda }_j(T)$ 通常为 0 或由其他物理条件确定；实际上，对于自由变量，我们不再施加终端约束，而是通过优化目标间接影响其值。

8. 另一个例子：OBVP for Vehicle Control

   • 非完整 nonholonomic 系统，控制输入是转向角 $\omega (t)$ 或速度 $v(t)$

   • 将控制输入参数化为多项式
     $$\omega (t)=a+bt+c{t}^2+d{t}^3+\cdots$$

   • 其它要点

     • Constrained Trajectory Generation：在约束下生成轨迹（如最大速度、加速度）

     • Numerical difference evaluated Jacobian：使用数值微分计算雅可比矩阵（用于牛顿法等优化算法）

     • Offline BVP, trajectory generation：离线求解边界值问题 → 生成轨迹

     • Online search：在线进行搜索

       先用离线 OBVP 生成一组高质量的基础轨迹（称为 motion primitives），再在运行时通过在线图搜索，从这些预生成的轨迹中拼接出完整路径。

9. 轨迹库 Trajectory library
   

   • Single layer lattice planning 是局部避障的常用方法。
   • 不是图搜索，而是轨迹选择（trajectory selection）。
   • 每条轨迹根据多项成本函数评分。

4.2.3 启发式函数

1. 启发式设计的基本思想

   • 核心原则：求解一个更简单的问题

   • 两个假设
     

     • 忽略障碍物
     • 忽略动力学约束

2. 忽略动力学约束的例子
   

   • 对每个节点（状态），忽略动力学模型并搜索最短路径
   • 无人机绕飞障碍物，紫色虚线是真实路径，绿色实线是启发式估计的“直飞”路径

3. 假设无障碍的方法

   • 方法：对每个节点 $n$ ，求解一个 OBVP 问题，得到到目标状态的最小代价轨迹

   • 结果：这个代价就是启发式函数 $h(n)$

   • 伪代码
     

     • $g(n)$：从起点到当前节点的实际累积代价
     • $h(n)$：从当前节点到目标的估计代价（即启发式）
     • $f(n)=g(n)+h(n)$ ：总优先级，决定搜索顺序

4. 不同启发式函数的比较
   

   • 左图：使用 Euclidean 2D distance 作为启发式
     • 绿色区域表示高启发值（远离目标）
     • 路径较短，但不考虑非完整性和障碍物
   • 右图：使用 non-holonomic-without-obstacles 作为启发式
     • 更接近真实运动能力（如汽车不能原地转向）
     • 路径更长，但更现实

4.2.4 Planning in Frenet-serret Frame

1. 什么是 Frenet-Serret 坐标系

   • 一种动态参考坐标系，随车辆运动而变化
   • 由三条正交轴构成：
     • ${\mathbf{t}}_c$：切向单位向量（沿道路中心线方向）
     • ${\mathbf{n}}_c$：法向单位向量（指向道路内侧，垂直于切向）
     • $\mathbf{b}$：副法向量（不常用）

2. 汽车车道线保持问题
   

   • 中心线 $\mathbf{r}(s)$：道路的几何中心线

   • 车辆位置表示为 $\mathbf{x}(s,d)$

     • $s$：纵向弧长参数（沿中心线的距离）
     • $d$：横向偏移（相对于中心线的侧向距离）
     • 解耦性：纵向和横向可分别建模

   • 控制参数化：使用五次多项式描述轨迹
     $$\begin{gathered} d(t)=a_{d0}+a_{d2}{t}^2+a_{d3}{t}^3+a_{d4}{t}^4+a_{d5}{t}^5 \\ s(t)=a_{s0}+a_{s1}t+a_{s2}{t}^2+a_{s3}{t}^3+a_{s4}{t}^4+a_{s5}{t}^5 \end{gathered}$$

   • 求解最优控制问题

3. 横向轨迹示例
   

   • 黑色曲线：不同参数组合下生成的横向轨迹（从起点到终点）
   • 绿色曲线：最优轨迹（通常是最短或最平滑的一条）
   • 横轴：时间 $t$，纵轴：横向偏移 $d$

4. 边界条件

   • 初始条件：$D(0)=(d_0,{\dot{d}}_0,{\ddot{d}}_0$
   • 终止条件：$D(T)=(d_f,{\dot{d}}_f,{\ddot{d}}_f)$
   • 车道保持情况：$D(T)=(d_f,0,0)$，即最终横向位移为 $d_f$，速度和加速度都归零（平稳回到车道）

5. 线性方程组求解

   • 将多项式系数写成矩阵形式
     $$\left[\begin{array}{ccc}{T}^3& T^4& T^5\\ 3T^2& 4T^3& 5T^4\\ 6T& 12T^2& 20T^3\end{array}\right]\left[\begin{array}{c}{a}_{d3}\\ a_{d4}\\ a_{d5}\end{array}\right]=\left[\begin{array}{c}\Delta p\\ \Delta v\\ \Delta a\end{array}\right]$$

   • 其中边界条件为
     $$\left[\begin{array}{c}\Delta p\\ \Delta v\\ \Delta a\end{array}\right]=\left[\begin{array}{c}{d}_f-(d_0+{\dot{d}}_{0}T+\frac{1}{2}{\ddot{d}}_0{T}^2)\\ {\dot{d}}_f-({\dot{d}}_0+{\ddot{d}}_{0}T)\\ {\ddot{d}}_f-{\ddot{d}}_0\end{array}\right]$$

4.3 Hybrid A*

4.3.1 workflow

1. 基本概念：剪枝节点以提升效率
   

   • 问题：在线生成密集格点（dense lattice）计算量太大
   • 解决方案：prune（剪枝）一些不必要的节点
   • 规则：使用网格地图（grid map）作为裁剪依据，在栅格地图的每个方块里保留一个节点，如果新的节点的累积cost由于原本方块里保留的节点的累积cost就更新节点，永远保留一个方块里只有一个节点。

2. A 算法的详细流程*

   • 伪代码
     

   • 根据前序PPT选择一个合适的启发式函数 $h(n)$

   • 通过状态积分找到邻居（如车辆运动学模型）

   • 第一个加号：在栅格 m 记录状态

   • 第二个加号：更新栅格 m 记录的状态

3. 四种启发式函数的比较
   

   • a 二维欧氏距离：最简单的直线距离，忽略机器人运动能力

   • b 无障碍非完整启发式：考虑车辆不能原地转向等约束，但假设没有障碍物

   • c 无障碍非完整启发式（在死胡同中表现差）

   • d 带障碍非完整 + 带障碍全向最短路径混合启发式：结合两种模型：先用全向模型算避障最短路，再叠加非完整约束

     混合 A* 通常使用两个启发式函数并取最大值：$h(n)=\max (h_{\text{holonomic}(n)},h_{\text{non-holonomic}(n)})$

4. 智能技巧增强搜索

   • Analytic Expansions: One-hot heuristic

     • 含义：一次性计算启发式，避免重复求解
     • 作用：减少在线计算负担，加快搜索速度
     • 在每个新长出来的节点，以一定的概率，计算从该节点直接到终点的路径，看看这个路径是否可行，这个概率可以根据离终点的远近自适应变化。

   • Control space sample vs State space sample
     

     • 控制空间采样（左图）：
       • 从一个状态出发，尝试所有可能的控制输入（如加速度、转向角）
       • 优点：覆盖全面
       • 缺点：无目标导向，效率低
     • 状态空间采样（中图）：
       • 直接在状态空间中采样目标区域附近的点
       • 优点：带有“目标偏置”，更高效
       • 缺点：可能漏掉某些路径

   • Manual state space sampling
     

     • 含义：人工添加一些关键状态点（如路口、转弯处）
     • 目的：引导搜索进入重要区域
     • 应用场景：复杂环境下的任务规划（如停车场倒车）

4.3.2 应用

1. 自主车
   

   Practical Search Techniques in Path Planning for Autonomous Driving, Dmitri Dolgov, Sebastian Thrun

2. 无人机
   

   Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight, Boyu Zhou, Fei Gao

4.4 Kinodynamic RRT*

4.4.1 workflow

1. 算法伪代码
   

   核心流程

   • Sample：在配置空间 MM 中随机采样一个点
   • Near：找到树中离该点最近的节点
   • Steer：从最近节点向采样点“伸一根线”（固定步长）
   • CollisionFree：检查新点是否安全
   • ChooseParent：选择最优父节点连接
   • rewire：重新连接邻居以优化成本

   注意：这是“点机器人”的规划方法：假设机器人可以瞬间移动到任意位置（如质点）

2. 如何采样？

   • 核心思想
     • 传统 RRT 在 Euclidean space（欧氏空间）采样；
     • Kinodynamic RRT* 必须在 full state space（完整状态空间）采样。
   • 关键变化
     • 不再只采样 $(x,y)$ 位置
     • 而是采样 完整的状态： $(x,y,\dot{x},\dot{y})$
     • 因为机器人不能突然改变速度或方向！

3. 如何定义“近”？

   • 核心思想

     • 传统 RRT 使用 Euclidean 或 Manhattan 距离定义“近”
     • Kinodynamic RRT* 需要用 最优控制理论 来定义“近”

   • 引入最优控制：定义状态间转移的代价函数
     $$c[\pi ]={\int }_0^{\tau }(1+u(t{)}^{\top }Ru(t))\mathrm{d}t$$

     • $\tau$：转移时间
     • $u(t)$：控制输入
     • $R$：权重矩阵（通常正定），控制能量消耗

     注意

     • “两个状态相近”意味着：从一个状态到另一个状态所需的控制代价小
     • 这个代价可能依赖于方向

   • 这类问题有解析解

     • 如果我们知道到达时间 $\tau$ 和控制策略 $u(t)$，就可以计算代价。
     • 这就是经典的 Optimal Control Problem (OBVP)

   • 固定终点 + 固定时间

     • 给定：终点状态 $x_1$，终止时间 $\tau$

     • 最优控制策略为
       $$u^{\ast }(t)=R^{-1}{B}^{\top }{e}^{A^{\top }(\tau -t)}G(\tau {)}^{-1}[x_1-\bar{x}(\tau )]$$

     • 其中：$G(t)$ 是加权可控性 Gramian
       $$G(t)={\int }_0^t{e}^{A(t-t^{\prime })}B{R}^{-1}{B}^{\top }{e}^{A^{\top }(t-t^{\prime })}\mathrm{d}{t}^{\prime }$$

     • 其中：$\bar{x}(t)$ 是无控情况下系统的自然响应（即 $u=0$ 时的状态）
       $$\bar{x}(t)=e^{At}{x}_0+{\int }_0^t{e}^{A(t-t^{\prime })}c\mathrm{d}{t}^{\prime }$$
       是如下微分方程的解
       $$\dot{\bar{x}}(t)=A\bar{x}(t)+c,\bar{x}(0)=x_0$$

     • 且 $G(t)$ 满足 Lyapunov 方程
       $$\dot{G}(t)=AG(t)+G(t)A^{\top }+B{R}^{-1}{B}^{\top },G(0)=0$$

     这个公式给出了从任意初始状态到目标状态的最优控制输入，只要知道最终时间和目标状态。

   • 固定终点 + 自由时间

     • 如果我们希望找到最优到达时间 $\tau$

       S1：把上面的最优控制策略代入代价函数
       $$c[\tau ]=\tau +[x_1-\bar{x}(\tau ){]}^{\top }G(\tau {)}^{-1}[x_1-\bar{x}(\tau )]$$
       S2：对 $\tau$ 求导
       $$\dot{c}[\tau ]=1-2(A{x}_1+c{)}^{\top }d(\tau )-d(\tau {)}^{\top }B{R}^{-1}{B}^{\top }d(\tau )$$
       其中
       $$d(\tau )=G(\tau {)}^{-1}[x_1-\bar{x}(\tau )]$$
       S3：解 $\dot{c}[\tau ]=0$ 得到最优时间 ${\tau }^{\ast }$

     • 结果：一旦找到了最优时间 ${\tau }^{\ast }$，问题就退化为“固定时间、固定终点”的标准 OBVP。

4. 如何选择父节点？

   • 对每个候选父节点 $x_{\text{near}}$，计算从它到 $x_{\text{rand}}$ 的最优控制代价
   • 选择代价最小且满足约束的那个作为父节点
   • 同时检查：中间轨迹是否在状态和控制输入的范围内（如速度不超过限值）
   • 如果没有合格的父节点，就重新采样。

5. 如何高效查询近邻节点？

   • “Near” 查询太慢

     • 每次采样 $x_{\text{rand}}$ 后，要对树中每个节点都做一次 OBVP 求解 来判断是否“接近”
     • 因为“接近”定义为：从该节点到 $x_{\text{rand}}$ 的最小控制代价小于某个阈值
     • 这意味着：每次 Near 都要做 O(n) 次最优控制求解 → 极其低效！

   • 解决方案：引入“代价容忍度” cost tolerance $r$ 和 KD-Tree

   • 核心概念

     • cost tolerance $r$：允许的最大转移代价
     • forward-reachable set：从某状态出发，在代价 $<r$ 内能到达的所有状态集合
     • backward-reachable set：能以代价 $<r$ 到达某状态的所有起始状态集合

   • 方法

     • 给定 $x_{\text{rand}}$ 和代价容忍度 $r$
     • 计算 backward-reachable set of $x_{\text{rand}}$ → 即：哪些状态可以以代价 $<r$ 到达 $x_{\text{rand}}$
     • 将树中的节点存储在 KD-Tree 中（支持快速范围查询）
     • 在 KD-Tree 中查询落在该 reachable set 内的所有节点 → 得到 $X_{\text{near}}$

     这样就避免了对每个节点单独求解 OBVP。

6. 数学推导：可达集是什么？

   • 给出代价函数
     $$c[\tau ]=\tau +[x_1-\bar{x}(\tau ){]}^{\top }G(\tau {)}^{-1}[x_1-\bar{x}(\tau )]$$
     我们希望找到所有满足 $c[\tau ]<r$ 的终点状态 $x_1$

   • 推导过程

     • 令 $d=x_1-\bar{x}(\tau )$，则：$c[\tau ]=\tau +d^{\top }G(\tau {)}^{-1}d<r$
     • 移项得：$d^{\top }G(\tau {)}^{-1}d<r-\tau$
     • 两边除以 $r-\tau$（假设 $r>\tau$）：$d^{\top }{\left(\frac{G(\tau )}{r-\tau }\right)}^{-1}d<1$
     • 定义椭球： ε [ x , M ] = { x ′ ∣ ( x ′ − x ) ⊤ M − 1 ( x ′ − x ) < 1 } \varepsilon[x,M]=\{x'|(x'-x)^\top M^{-1}(x'-x)<1 \} ε[x,M]={x′∣(x′−x)⊤M−1(x′−x)<1}
     • 所以：$\text{forward-reachable set}={\cup }_{0<\tau <r}\epsilon [\bar{x}(\tau ),G(\tau )(r-\tau )]$

   • 结论：从初始状态 $x_0$ 出发，在代价 $<r$ 内能到达的状态集合，是一系列随时间变化的高维椭球的并集。

   • 可视化解释
     

     • 曲线 $\bar{x}(t)$ 是无控情况下系统的自然响应（即 $u=0$ 时的状态）
     • 每个灰色椭球代表：在时刻 $t$，以代价 $<r-t$ 能到达的状态集合
     • 所有椭球的并集就是总的 forward-reachable set
     • 外层轮廓线是这些椭球的包络，表示整体可达区域

   • 简化计算 - 使用轴对称包围盒（Axis-Aligned Bounding Box）

     • 对多个时间点 $t$ 计算对应的椭球

     • 对每个椭球，计算其在每一维上的最大/最小值（即轴对称包围盒）

     • 取所有时间点的包围盒的并集
       $${\Pi }_{k=1}^n\left[\underset{0<t<\tau }{\min }\left(\bar{x}(t{)}_k-\sqrt{G[t{]}_{k,k}(r-t)}\right),\underset{0<t<r}{\max }\left(\bar{x}(t{)}_k+\sqrt{G[t{]}_{k,k}}(r-t)\right)\right]$$

     这是一个 矩形区域（hyper-rectangle），可以用 KD-Tree 快速查询。

     这只是保守估计，实际可达集可能更大，但足以用于近似搜索。

7. 如何重连接？

   • 当执行 “Rewire” 时，我们计算：
     • $x_{\text{rand}}$ 的 forward-reachable set
     • 并对每个候选节点求解 OBVP（最优边界值问题）
   • 为什么是 forward-reachable set？
     • 我们要判断：从 $x_{\text{rand}}$ 出发，能否以更低代价到达某些已有的节点？
     • 所以需要知道：从 $x_{\text{rand}}$ 能到达哪些状态（即 forward-reachable set）
     • 然后在这个集合内查找所有“可被重连”的节点

4.4.2 Demos