【深蓝学院】移动机器人运动规划--第4章 动力学约束下的运动规划--笔记

0. Outline

1. Introduction

1. 什么是kinodynamic？
   

运动学（Kinematics）和动力学（Dynamics）都是力学的分支，涉及物体的运动，但它们研究的焦点不同。

• 运动学专注于描述物体的运动，而不涉及运动的原因。它包括位置、速度、加速度等概念，并研究这些量之间的关系，以及它们如何随时间变化。运动学问题通常涉及计算物体在没有引力或其他力的作用下的理想运动。
• 动力学则涉及力及其产生的效果，包括物体如何响应这些力。它关注的是运动的原因，即力和力矩是如何影响物体的运动状态。动力学分析会考虑到质量、重力、摩擦力以及其他作用在物体上的力，并试图解释物体是如何开始运动、维持运动或改变运动状态的。

总结来说，运动学是描述运动本身的规律（“发生了什么”），而动力学是研究产生这些运动的力的规律（“为什么会这样”）。

对于我们的运动规划，结合运动学和动力学，就不只是单纯地把运动目标当做质点看待，还要考虑微分约束，力的作用(加速度等)。

2. 为什么是kinodynamic planning？

• 遵循corse-to-fine的过程。
• 后端traj优化只能局部进行。
• 不可行的解对于不完整的系统是无用的。
  
  前面提到，本课程讲述的均为学院派的path planning的方法，也就是遵循上图所示的pipeline的方法：

给定任务->前端(在地图上)找出path->后端优化，生成机器人可执行的trajectory->完成plan任务。

会有一个问题，我们已经有了后端的优化，为什么不直接把考虑kinodynamic的任务交给后端完成？

答案是当然可以完全交给后端，但是这样会把所有动力学相关的约束都加到后端，使得后端非常难以设计。

相反地，如果在前端path finding时考虑一些kinodynamic的约束，前端生成的path在拓扑结构上比较符合实际的动力学模型，那么后端优化就相对容易进行，从前端得到的path会更容易优化。

也即遵循corse-to-fine的过程。

后端的轨迹优化只能局部地进行，上右图为例，drone正在往右上方运动，goal是五星，如果不考虑kinodynamic，则可能生成的path是紫色实现，被后端优化为紫色虚线，则需要减速到0再调头避障，如果考虑kinodynamic，path可能是绿色实线，被优化为绿色虚线，更符合实际的运动情况。

上左图的泊车规划mission，汽车很经典的一个动力学约束即不能横向移动，所以在path finding时就需要考虑这点。

实际上，考虑和不考虑动力学模型，生成的path在地图上的topology(拓扑结构)是不同的，不同拓扑结构的path基本上无法被优化为相同的trajectory。

例子：

独轮车和差速驱动小车，其中$u_l,u_r$分别是左右轮转动的角速度，为了与独轮车的角速度$\omega$做区分，使用了$u$

以上是不同小车模型对应的约束，可以参考UIUC一个professor的这本书，这本书作者的自传还是比较有意思的。

2. State Lattice Planning(状态栅格搜索算法)

回顾基于搜索和基于采样的方法，共性是：构建搜索树对于path planning很关键。

我们之前不同node之间的连线(边)是直线，且假设这个直线是机器人feasiable的，但实际中不一定feasiable，我们可以构建feasiable edge，而构建的方法有两种：

• Forward direction：在控制空间中离散地采样
• Reverse direction：在状态空间中采出很多状态，再从状态生成到当前状态的feasiable edge

下面着重讲这两种方法。

首先看与L2和L3的联系：

L2中的grid map中的四连接或者八连接其实就是一种forward direction方法：在控制空间中均匀地采样。

L3中的PRM实际上就是在状态空间(state space)中采样，然后进行连接，collision checking就是尝试寻找feasiable motion connections的过程。

对于一个机器人模型$s^{\prime }=f(s,u)$，其中$s$为状态state，$u$为输入（类似于SLAM运动和观测方程），初始状态为$s_0$，可以通过以下两种方法生成feasiable local motion：

1. 正向计算：在控制空间中采样，固定$输入u，控制时长T$，算出$s$，此方法目的性不强，虽然实现容易，但是planning的效率很低。
2. 反向计算：在状态空间中采样，采样出$s$，计算出$u,T$(状态$s_0$和$s_f$之间的connection)，当然也可以固定一个$T$，此方法目的指向性很强，难实现，但planning效率高。

以下的各种模型不做推导，默认给出的模型是使用共识。

2.1 Sample in control space

给定如上无人机模型，状态量为$s$，包括位置 [ x y z ] \begin{bmatrix}x\\y\\z\end{bmatrix} ​xyz​ ​和速度 [ x ˙ y ˙ z ˙ ] \begin{bmatrix}\dot{x}\\\dot{y}\\\dot z\end{bmatrix} ​x˙y˙​z˙​ ​，输入为加速度 [ x ¨ y ¨ z ¨ ] \begin{bmatrix}\ddot{x}\\\ddot{y}\\\ddot z\end{bmatrix} ​x¨y¨​z¨​ ​，系统方程为$$\dot{s}=A\cdot s+B\cdot u$$为2阶方程。

当然，输入也可以为加速度的导数，叫做Jerk，则此时A和B分别需要增加维度，系统变为3阶。

在control space中进行8次采样，假设control time $T=1.0s$，初速度 v 0 = [ 1 0 0 ] v_0=\begin{bmatrix}1\\0\\0\end{bmatrix} v0​= ​100​ ​，则由运动学公式$P=P_0+v_{0}T+\frac{1}{2}a{T}^2$，可以得到$1.0s$之后的系统的状态，则到达这个状态过程中的轨迹就是前述的feasiable motion connections。

上最右图为jerk作为输入的结果。

其中系统为several-order integrator若干阶积分器，A的形式比较特殊，为nilpotent（幂零）矩阵（用来形容一个元素或矩阵的幂的某个次幂等于零），这个形式对于计算很重要。

如果我们想知道在运动过程中某个时刻系统处于什么状态，可以使用上右式计算，其中可对$e^{At}$进行无穷级数展开，而由于A nilpotent，所以只需要展开前几项不为0的即可，于是$e^{At}$就有一个closed-form的表达。

一个节点上进行了一次control space中的sample到了新的节点之后，在新的节点上继续进行sample，此过程不断进行，最终就形成了lattice graph，如上图所示为在control space中9离散化和25离散化的的lattice graph。

注意：

• 在搜索过程中，为了节省计算时间，只在必要时才build graph，
• 当是新发现node或者edge时才进行构建。
• 可节省时间和计算资源。

给予kinodynamic的搜索，与之前的搜索不一样的点在于搜索树的构建，可以事先构建lattice graph的搜索树，也可以像上述的necessary时再构建，构建时遵循kinodynamic的约束，最终将collision-free的加入到搜索树中。

2.2 Sample in state space

上图所示对应一个Reeds & Shepp’s car的模型，不能侧移，8个neighbors只有6个能够直接到达，于是得到了6个sample的state，然后反算出如何从现在的state到达sample出的node(得到边)。在下一个neighbor上继续sample，反复sample即完成lattice graph的构建。

上图所示即为一个平面机器人模型，基于state space中的sample，构建了一个两层的lattice graph，从最里面的起点开始，向外sample一层，得到sampled state，反算出从各个state如何连接到当前状态的边，然后再向外sample一层，重复这个过程，得到第二层lattice graph。多层lattice graph仅仅是第一层不同，因为初始状态不同，第一层需要跟初始状态进行连接。

整个lattice graph复杂度比较高，所以实时使用时不会构建很深的lattice graph。

2.3 Comparison

在control space和state space中采样的对比：

• 相同点：且在初始角速度方向的轨迹非常dense，如果某条轨迹发生了碰撞，那么周围其它的轨迹也很可能都碰撞，即对不同的输入有非常相似的输出。
• 不同点：同样地，以汽车为例，在control space中sample会存在很多not collision-free的情况，目的性不强，而state space中采样目的性较强。

讲到这里，自然会产生一个疑问：既然state space中采样这么高效，为什么不直接在state space中采样？放弃control space中采样？
答案是在state space中采样很难实现(hard to implement)，下面将继续讨论Boundary value problem。

3. Boundary value problem(BVP，两点边界值最优控制问题)

3.1 Exmaple：Drone

BVP是state sampled lattice planning的基础，通常转换为复杂的数值优化问题。
给定这样一个例子：以横轴x坐标为状态，初始状态为a，T时刻状态为b

假设x方向运动满足一个5阶多项式，给定了边界条件，求解超定方程，显然有多组解，找到解不难，难点是找到最优解。

最优化边界值问题即OBVP，
状态由p，v，a这3部分组成，输入$u=jerk$，初始状态和目标状态已知，目的是最小化jerk平方的积分（当做cost function），针对无人机，xyz每个方向分别考虑，这里为了简化问题，只考虑一维的情况，把$k$省略。

这里使用"Pontryagin’s minimum principle" （“庞特里亚金最小值原理”）来求解OBVP。（这是控制理论中的一个重要原则，由苏联数学家L.S.庞特里亚金（Lev Semyonovich Pontryagin）提出，主要应用于最优控制问题的解决。这个原理提供了求解确定控制变量以最小化性能指标的问题的必要条件。）

求解过程需要定义对应的costate和 Hamiltonian function（在最优控制理论中，“costate”一般翻译为“共轭态”或者“余态”。它在庞特里亚金最小值原理中扮演着重要角色，与系统状态变量相对应的是一组新的变量，这些变量通过哈密顿体系的方程与状态变量相联系。共轭态变量通常在解决最优控制问题时出现，用于构造系统的哈密顿函数。）

costate维度和状态变量维度相同。

因为我们的目标状态是一个硬约束，必须满足，所以我们的边界条件是不满足的。

由H求出最优的$\lambda$的形式，$H$对$s=[p,v,a]$求导取负得$(0,-{\lambda }_1,-{\lambda }_2)$，设${\lambda }_1=\alpha$，则整个$\lambda$可以表达为

1 T [ − 2 α 2 α t + 2 β − α t 2 − 2 β t − 2 γ ] \begin{align} \frac{1}{T}\begin{bmatrix}-2\alpha\\2\alpha t+2\beta \\-\alpha t^2-2\beta t-2\gamma \end{bmatrix} \end{align} T1​ ​−2α2αt+2β−αt2−2βt−2γ​ ​​​
$\lambda$形式先放在这里，其中$\alpha ,\beta ,\gamma$都是未知数，后面可用硬约束确定。

考虑$u^{\ast }(t)$是对$H$求最小值，因为$s^{\ast }$为最优，$s$中有$p,v,a$已经固定，所以$arg\underset{j(t)}{\min }H(s^{\ast }(t),u(t),\lambda (t))$只跟$$j有关，提出来为

$$\begin{array}{r}H=\frac{1}{T}{j}^2+{\lambda }_{3}j\end{array}$$
求$arg\underset{j(t)}{\min }H$，则求导等于0得
$$\begin{array}{r}{H}^{\prime }=\frac{2}{T}j+{\lambda }_3=0\end{array}$$
得
$$\begin{array}{r}j={\lambda }_3\frac{T}{2}\end{array}$$
由式(1)的推导
$$\begin{array}{r}{\lambda }_3=\frac{1}{T}(-\alpha t^2-2\beta t-2\gamma )\end{array}$$
带入(4)得
$$\begin{array}{r}{u}^{\ast }(t)=\frac{1}{2}\alpha t^2+\beta t+\gamma \end{array}$$

得到$u^{\ast }(t)$后，分别对$u^{\ast }(t)$进行一二三次积分得$s^{\ast }(t)$的三个分量，即可得$s^{\ast }(t)$

其中$s^{\ast }(0)=s(0)=[p_0,v_0,a_0{]}^T$。

再带入我们的硬约束，即目标状态$s(t)=s_f$，其中 [ Δ p Δ v Δ a ] \begin{bmatrix} \Delta p\\\Delta v\\\Delta a \end{bmatrix} ​ΔpΔvΔa​ ​可算出，只与$T$相关，所以最终 [ α β γ ] \begin{bmatrix}\alpha\\\beta\\\gamma \end{bmatrix} ​αβγ​ ​均只与$T$有关，确定$T$之后，可以算出其中任意变量，包括cost $J$，那么最小化cost最终转换为一个多项式求根的问题。

对于我们这里的硬约束，边界条件不满足，无法使用，直接使用给定的fixed final state来计算 [ α β γ ] \begin{bmatrix}\alpha\\\beta\\\gamma \end{bmatrix} ​αβγ​ ​。

最后，回归到问题本身，我们的目的是求出从起点到终点的feasiable connections，求出了最优state $s^{\ast }(t)$的表达式，也就知道了从起点到终点的各个时刻的state，轨迹也就求出了。

一般情况：
前面的情况都是fixed final state，更一般地，如果final state is partially-free部分自由，只固定了一部分，如最终状态$s_f$中只知道$p_f$，而$v_f,a_f$未知，那么最终的自由度也相应的增大，对于costate也有边界条件。(这部分会在作业中涉及到，暂不深入讨论)

3.2 Another example：Car

与drone不同的是，car无法侧移，无法像drone那样再任意一个轴上自由运动，car运动的控制：前后的$v$和打方向盘的$\omega$。

构造满足car的动力学约束，解满足次约束的轨迹生成问题，此问题比drone更复杂，不容易colse-formed linearization，所以一般采用数值求解，求取Jacobian和Hessian，所以该过程一般是offline进行的，offline生成lattice graph，在线地贴到机器人的状态桑，进行online search

上图左是在state space中sample，右下贴到机器人状态里，贴完之后，问题就转换为一个graph search problem，可以使用A*，JPS等求解。右上是在control space中sample。

3.3 Trajectory Library

这里指的是单层的lattice，用于local collision avoidance
不是用于graph search的，而是在单层lattice上对不同的trajectory进行打分(risk,information,energy等)，选择一条collision free的、cost小的trajectory，对于找不到collision free的，可能需要借助global planner进行脱困等。

Trajectory Library的一个应用，ref[6]

3.4 Heuristic(略讲)

kinodynamic planning也希望借助之前的heuristic来加速求解，此处的heuristic function的设计涉及两个方面，ref[7]：

1. 假设不存在障碍物(Assume no obstacle existence)：基于动力学约束进行无障碍物规划。
2. 假设不存在动力学(Assume no dynamic existence)：基于障碍物进行无动力学约束规划。

OBVP的A*中，与之前不同点在于g值的计算，grid map可能直接是格子的距离，而这里需要考虑动力学约束。

上面这个工作使用了“Assume no obstacle existence”的思想，ref[8]。上左图，如果直接使用Euclidean 2D Distance作为heuristic，需要扩展很多节点，而如果使用Assume no obstacle existence进行引导（文章中叫non-holonomic-without-obstacles），会减少很多需要扩展的节点。

---

补充non-holonomic-without-obstacles：

在机器人学和自动控制领域，“non-holonomic”一词用于描述那些不能完全通过其局部速度来控制的系统。非完整性（non-holonomic）系统的典型特点是它们的运动状态被一些不等于系统自由度数量的约束所限制，这些约束是非积分的，这意味着它们不能直接转换为位置的约束。例如，汽车通常被视为非完整性系统，因为它不能直接横向移动，汽车的移动方向受到与车轮方向相一致的约束。

“without obstacles”意味着在考虑系统运动或进行路径规划时，我们假设环境中没有障碍物。因此，系统的运动规划将不需要考虑绕过障碍物的问题。

将这两个概念结合起来，“non-holonomic-without-obstacles”通常指的是对一个非完整性系统（如汽车、机器人等）进行运动规划，而且在这个规划过程中，我们假设环境中没有需要避免的障碍物。简而言之，这是一个关于非完整性系统在空旷环境下运动规划的场景。

---

3.5 Planning in Frenet-serret Frame(略讲)

在运动规划和机器人学领域，Frenet-Serret框架（或简称Frenet框架）提供了一种描述物体在曲线路径上运动的方法。这个框架由三个正交单位向量组成：切向量（T），法向量（N）和副法向量（B）。这三个向量分别描述了路径的方向、路径曲率的方向以及路径扭率的方向。在这个框架下，物体的运动可以通过沿着切线（纵向）的速度、法线（横向）的偏移以及副法线上的变化来描述。

“Planning in Frenet-Serret frame” 指的是在这样的坐标系统下进行路径或运动的规划。在自动驾驶汽车技术中，这种方法常用来计算车辆在道路上的理想轨迹，其中车辆的位置可以根据沿道路中心线测量的距离（纵向）和相对于道路中心线的横向偏移来定义。这种规划方法可以帮助算法有效地处理曲线道路，同时考虑车辆的动态约束和周围环境。

车辆自动驾驶其中有一个任务是跟随车道线，可以把任务分解：

1. 可以把车辆的运动（状态）分解为切向和径向的运动，表示为两个五次多项式（为什么是5次，可以参考论文ref[9],ref[10]），
2. 求解OBVP问题

状态同样为$[p,v,a]$，终止条件我们一般不希望车子出现侧滑，所以$v,a$一般都为0，然后解OBVP。

切向方向比较复杂，具体可以参考ref[9][10].

提供一个python实现的robotics仿真的平台ref[15]

4. Hybrid A*(search based kinodynamic planning具体算法)

一种具体的kinodynamic planning算法。

4.1 Workflow

基本思想：在control space中sample的lattice graph太过稠密，且采样出的traj不够diverge，一条traj发生collision之后，neighbors也容易发生。结合A*和lattice graph的思想，在lattice graph构建过程中每个grid中只保留一个feasiable connection node，通过一种方法计算计算cost，如果当前grid中node的cost比之前node的小，替换之前的node，此做法称为prune剪枝。ref[11,12]

上图为hybrid A* 的伪代码，Hybrid A* 的$f=g+h$中的 $g$ 和 $h$ 都和传统A* 不同，都需要考虑kinodynamic进行设计。

相较于传统A*，Hybrid A* 除了需要维护每个节点的$f$值，还需要维护每个node的state(比如位置，速度，加速度)。

ref[11,12]中的heuristic的设计对比，abcd分别为

• 2D欧式距离，
• non-holonomic-without-obstacles，
• non-holonomic-without-obstacles在死胡同时work bad，
• non-holonomic-without-obstacles + holonomic-with-obstacles(A*,Dijkstra等shortest path算法)。（这里的“+”不一定为数值加）

4.2 Other tricks

Analytic Expansions即one-shot heuristic，指在Hybrid A* 搜索过程中，尝试直接不考虑动力学模型，使用sortest-path算法解出cur to goal的optimal path，然后对该path进行kinodynamic check和collision-check，如果都满足则可直接结束当前hybrid A* 求解，完成planning。

在实际中，频繁地进行one-shot会耗费大量计算时间，所以可以每隔N次(如20次)进行一次one-shot，如果one-shot成功则直接结束，否则继续hybrid A*。随着hybrid A* 的搜索，search graph会越来越接近goal，one-shot成功的概率也会越来越高，相应地，N可以变小，越来越频繁地进行one-shot，以提高搜索效率。

4.3 Application

Boyu Zhou, Fei Gao在phd阶段的一项工作,(ref[14])：(前端)kinodynamic的path search(hybrid A*) + (后端)距离场的梯度进行局部的飞行器轨迹优化

运用kinodynamic而非shortest-path的前端目的是给无人机一个更好的初值，飞得更快。

这里UAV model是nilpotent，状态转移可以close-formed推导出来，可以evaluate cost，为search提供了便利。

5. Kinodynamic RRT*

简记为KRRT*

和RRT* 相似，但是有区别。

如何采样，RRT* 直接采样位置，而Kinodynamic RRT* 需要在full state space中采样，包括p，v，a等。

由于汽车控制是non-holonomic，存在运动约束，所以是一个带约束的最优控制问题OBVP。对于OBVP的求解，之前用的是Pontryagin’s minimum principle构建Hamiltonian function那一套来求解，KRRT* 论文中采用的求解方法是构建一个cost函数$c[\pi ]$，并最小化该函数，ref[17]。

RRT* 中寻找x_near只在低维画个圆，在圆内找，圆的半径实际上就是距离采样点x_new的cost相同的流形，而KRRT* 需要考虑高维的near，此处定义距离x_new的cost相同，“半径”取为$r$，引申出**正向可达集(forward reachable set)，反向可达集(backward reachable set)。**正向是我到哪，反向是哪到我。

确定x_Near，ChooseParent时需要计算backward reachable set(哪到我)，进行rewire时需要计算forward reachable set(我到哪)。

---

本章涉及的内容非常多，很多方面并没有深入探究，提供了很多参考论文，可自行仔细研究。
摘重要的文献看BVP，Hybrid A*，Kinodynamic RRT*这三个工作。

6. Reference

[1] (control space sample参考论文) Search-based Motion Planning for Quadrotors using Linear Quadratic
Minimum Time Control，Sikang Liu, Nikolay Atanasov, Kartik Mohta, and Vijay Kumar.

[2] (state space sample参考论文) Generating Near Minimal Spanning Control Sets for Constrained Motion Planning in Discrete State Spaces，Mihail Pivtoraiko and Alonzo Kelly.

[3] (两种sample方法的对比)State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in Complex Environments, Thomas M. Howard, Colin J. Green, and Alonzo Kelly.

[4] (OBVP推导部分参考教材)A Computationally Efficient Motion Primitive for Quadrocopter Trajectory Generation, Mark W. Mueller, Markus Hehn, and Raffaello D’Andrea Dynamic Programming and Optimal Control, D. P. Bertsekas.

[5] (Car的最优轨迹生成问题参考论文，34)Optimal Rough Terrain Trajectory Generation for Wheeled Mobile Robots, Thomas M. Howard Alonzo Kelly

[6] (Trajectory Library应用参考论文) Maximum Likelihood Path Planning for Fast Aerial Maneuvers and Collision Avoidance Ji Zhang, Chen Hu, Rushat Gupta Chadha, and Sanjiv Singh.

[7] (kinodynamatic heuristic design参考论文) Planning Long Dynamically Feasible Maneuvers for Autonomous Vehicles, Maxim Likhachev, Dave Ferguson.

[8] (Heuristic design的工作参考论文) Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments, Dmitri Dolgov, Sebastian Thrun, Michael Montemerlo, James Diebel.

[9] (Planning in Frenet-serret Frame参考1) Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame, Moritz Werling, Julius Ziegler, Sören Kammel, and Sebastian Thrun

[10] (Planning in Frenet-serret Frame参考2) Optimal trajectories for time-critical street scenarios using discretized terminal manifolds, Moritz Werling, Sören Kammel, Julius Ziegler and Lutz Gröll

[11] (Hybrid A*参考论文1) Practical Search Techniques in Path Planning for Autonomous Driving, Dmitri Dolgov, Sebastian Thrun.

[12] (Hybrid A*参考论文2) Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments, Dmitri Dolgov, Sebastian Thrun.

[13] (Hybrid A*应用参考1) Practical Search Techniques in Path Planning for Autonomous Driving, Dmitri Dolgov, Sebastian Thrun.

[14] (Gao Fei phd阶段hybrid A*相关的drone motion planning工作) Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight, Boyu Zhou, Fei Gao.

[15] (机器人算法仿真平台1) https://pythonrobotics.io/

[16] (机器人算法仿真平台2) https://atsushisakai.github.io/PythonRobotics/index.html

[17] (KRRT* 与3.1节不同的OBVP求解方法) F. Lewis, V. Syrmos. Optimal Control. John Wiley & Sons.