一、Introduction

1. autonomous robot

2. Motion planning

1. Basic requirements

2. Old-school pipeline（通用规划算法）
1. 前端路径搜索，首先在前端利用图搜索等方法搜索出一条初始无碰撞的安全路径（低维，离散空间）
2. 后端轨迹生成（优化），再在后端对已生成路径进行优化以获得一条可执行的优化轨迹（高维，连续空间）
 

二、Course Outline

motion planning

1. 路径规划的前端、路径搜索（基于采样/搜索/满足动力学约束的一些搜索）

2. 后端轨迹优化、轨迹生成

1. Front-end: Path Finding

1. Search-based methods

2. Sampling-based methods

2. Search-based Method

1. Dijkstra’s算法

2. A*算法

3. JPS

3. Sampling-based Method

4. Kinodynamic Path Finding

5. Back-end: Trajectory Optimization

Typical Planning Methods Overview

三、Map Representation

Map

1. Data Structure 数据结构

2. Fusion Method 用什么方法去做地图融合

1. Occupancy grid map

2. octo-map：

1. 八叉树（Octree），查询时需要根据树的结构进行递归索引的查询

2. 基于3D Occupancy Grid Map的开源库：octo-map

3. Voxel hashing

1. 用哈希表的方式存储起来

4. Point cloud map

1. 用于表示比较稠密的障碍物的信息

2. 开源库：PCL http://pointclous.org/

5. TSDF map

1. 开源软件OpenChisel

6. ESDF map

1. 通过计算每个点到最近障碍物的有符号距离来提供环境的详细信息。

2. 生成：通常需要一个占据栅格地图（Occupancy Grid Map），通过算法（如快速行进法 Fast Marching Method 或 Dijkstra 算法）计算每个栅格到最近障碍物的欧几里得距离，生成一个与输入地图分辨率相同的 ESDF 地图，每个栅格包含有符号的距离值。

3. VoxBlox, FIESTA, TRR’s Local Map

欧几里得距离

计算两点之间的直线距离

More

1. Free-space Roadmap

2. Voronoi Diagram Map 拓扑地图

Lecture 2 基于搜索的路径规划

SEARCH-BASED PATH FINDING

一. Graph Search Basis

1. Configuration Space

1. Robot configuration（机器人配置）

2. Robot degree of freedom(DOF)自由度，如无人机轨迹规划有四个自由度：x轴，y轴，z轴和偏航角

3. Robot configuration space （机器人配置空间）
n（自由度数）个维度的空间（包含了所有机器人可能存在的配置）
C-space

4. Each robot pose is a point in the C-space
机器人任何一个可能存在的位置在C-space中都表示唯一一个点

2. Configuration Space Obstacle

1. Planning in workspace（通过做碰撞检测，费时费力）

2. Planning in configuration space
1. 将机器人表示为点
2. 将工作空间的障碍物转化为控制空间的障碍物
3. 将障碍物按照无人机大小膨胀

3. Workspace and Configuration Space Obstacle

1. Graph and Search method
Search-based Method

2. Graph Search Overview
对于图进行图搜索，我们总是能产生一棵搜索树——搜索起始节点到终止节点的一条路径——快且最优

4. Graph Search Overview

Maintain a container to store all the nodes to be visited

container，容器，是一个数据结构，用于存储待访问的节点

• 如果是广度优先搜索（BFS），容器通常是一个队列（先进先出）。

• 如果是深度优先搜索（DFS），容器通常是一个栈（后进先出）。

• 如果是A*算法，容器通常是一个优先队列（按某种评分函数排序）。

The container is initialized with the start state XS

容器初始时只包含一个节点，即起始状态，这是搜索的起点

Loop

算法不断从容器中取出节点并处理，直到满足终止条件（找到目标节点或容器为空）

• Remove a node from the container according to some pre-defined score function —— visit a node

移除的规则由评分函数（score function）决定：

• 如果是BFS，评分函数是“最早进入容器的节点优先”。

• 如果是DFS，评分函数是“最晚进入容器的节点优先”。

• 如果是A*算法，评分函数是“代价最小的节点优先”。

visit a node：

• 对取出的节点进行“访问”，即检查该节点是否满足目标条件。

• 如果满足目标条件，算法可以终止并返回结果。

• 如果不满足目标条件，继续下一步。

• Expansion: Obtain all neighbors of the node —— discover all its neighbors

获取当前节点的所有邻居节点。

• 邻居节点的定义取决于具体问题：
• 在路径规划中，邻居节点可能是相邻的网格点。
• 在状态空间搜索中，邻居节点可能是通过某种操作（例如移动、旋转等）得到的新状态。

discover all its neighbors

• 发现当前节点的所有邻居节点。

• 这一步通常包括检查邻居节点是否合法（例如是否在边界内、是否满足约束条件等）。

• Push them (neighbors) into the container

将所有合法的邻居节点加入容器中，等待后续访问。

• 加入的顺序和容器的类型有关：
• 如果是BFS，邻居节点会被加入队列的末尾。
• 如果是DFS，邻居节点会被加入栈的顶部。
• 如果是A*算法，邻居节点会根据评分函数插入优先队列的合适位置。

End Loop

最基本的图的遍历

Breadth First Search 广度优先搜索（队列，先进先出）

二. Dijkstra and A*

Dijkstra算法

Dijkstra和之前搜索路径时用的广度优先搜索BFS算法来说最大的不同就是从容器中弹出接下来要访问的节点不同。D每次弹出的节点具有最小的Gn（起点到n节点的cost总和最小）

图搜索主要的循环三大部分：弹出-扩展-新节点压入堆栈

启发式函数帮助下的搜索算法A*

将Grid Map转换成Graph

其核心在于将离散的网格单元抽象为图的节点，并通过连接规则定义节点间的边

Tie Breaker

对于两个cost相同的path，找到一个倾向性去倾向某一条路

三. Jump Point Search

A*可能会做很多无谓的探索

JPS如何工作的？

为什么 JPS 先进行直线移动，再执行对角线移动？

JPS 的扩展策略是：

1. 沿直线方向尽可能远地跳跃（跳过所有非 Jump Point 节点）。

2. 到达 Jump Point 后，再检查对角线移动方向。

3. 对角线方向也是尽可能跳跃，直到遇到障碍或强制邻居。

直线移动优先的原因

1. 减少冗余搜索
• 先沿 水平/垂直方向 跳跃可以快速到达潜在的 Jump Point，避免 A* 中逐步扩展每个格子。
• 如果优先进行对角线搜索，可能会遇到一些不必要的拐弯情况，从而引入额外的计算量。

2. 保证最优路径
• 直线搜索可以保证当前路径的 g 值 是最小的，确保启发式计算 (f=g+h) 正确。
• 对角线移动有更复杂的邻居检查（因为可能要绕开障碍物），所以放到后面处理更合理。

1. 不断的迭代的去往前推进

2. 遇见“Forced Neighbor”或者“终点”时，停止跳跃并将当前点标记为 Jump Point 进行扩展。（从x节点直接跳到有forced neighbor的节点y上）

1. 对角线跳跃：直线跳跃停止在障碍物（或边界上），跳跃失败，进行对角线上的跳跃到达y节点

2. 进行水平和垂直方向的跳跃到z节点（z有forced neighbor），返回到上一层y节点，将y加入open list（优先级队列）

1. 同样的，x还会天然具备一个forced neighbor 即w节点，则需要将w也加入open list

2. x的分析完毕，从openlist弹出，加入close list

Lecture 3 基于采样的规划算法

基于采样——探索解空间的连续性——在连续的构型空间中采样出离散的样本来构成树的结构（拓扑连续性的信息）

Two Fundamental Tasks

1. exploration 搜索

2. exploitation 剪切

methods

1. 可行路径规划算法
1. PRM
2. RRT

2. 最优路径规划算法
RRT*

3. 加速收敛
RRT#,Informed-PPT*, and GuILD

Feasible Path Planning Methods

PRM（Probabilistic Road Map）

先得到所有的连通性，再在图中做搜索

1. learning phase 构建图
1. 随机点检测空间（采样n个点），碰撞的点去掉
2. 连接节点各自的neighbor，去掉碰撞的边
——得到一个表征构型空间连接的图结构

2. Query phase
1. 加入始终点
2. 使用基于图搜索的算法找到一条路径 graph search

3. 可行解的搜索
1. 去掉碰撞边
2. 。。。。。

RRT（Rapidly-exploring Random Trees）

增量式的采样，不知道图的连通性，边采样 边做图搜索 边构建图的连通性

1. sample一个点

2. 找到离它最近的

3. 在上述两点基础上

4. 。。。。。

5. 直到树长到目标点处（一般来说节点和终点的距离小于某个范围就会尝试去连接一下，如果没有障碍物，即找到）

Optimal Path Planning Methods

层级决策，无环结构，能够用直接动态规划去求解一个直接的代价值 前驱节点值已知

1. RRT*（Rapidly-exploring Random Tree*）——应用最优性准则实现渐进最优
1. 得到
2. 通过找到一个范围内的点集，运用最优性准则，从点集中选择一个作为x_{new}的父节点（RRT是找最近的）
3. rewire：改变已有的树结构——点集中哪些节点可以通过新加入的节点到达而改变已有的cost-value
4. 如何确定range范围？



• 往前生长的距离
• 集合的势
• 规划的维度（考虑时间维度则为d+1）
• 取值是为了保证渐进最优性
• 低维空间下单位球的体积，所规划的空间freespace的估计大小
——我们没有时间去保证收敛到全局最优，因此将range设为一个常数，比 稍微大一点点

2. 加速效率
1. Bias Sampling 向目标节点采样
2. Sample Rejection 拒绝某些采样（障碍物内部的节点，到起始节点+到目标节点估计值大于现有最优解的节点）
3. Branch-and-bound (Tree Pruning) 剪枝
4. Graph Sparsify 图更稀疏——分为很多格子，一个格子中只留一个采样点（次优性）
5. Neighbor Query 找最近的k个点k-nearest ；K-D tree 的数据结构；range tree
6. Delay Collision Check 找到潜在父节点时按照什么顺序检查—— by potential cost-to-come values ——以潜在的cost value进行排序，从低到高进行parent搜索，一旦找到就不再对其他节点进行check
7. Bi-directional search 一棵树从起点开始长，一棵树从终点开始长
8. Conditional Rewire 这种额外的消耗可以在找到第一个解之后再进行（RRT找到解之后，RRT*开始rewire）
9. Data Re-use 重复计算的重新利用（例如相同两个点的碰撞检测的结果）

Accelerate Convergence

从exploitation角度

RRT#

1. Over-exploitation:No need to “Rewire” non-promising vertexes.

2. Under-exploitation

从exploration角度提升收敛

Informed RRT*

• 全知集：在该椭圆范围内的cost小于等于当前cost——在全知集当中直接采样

1. Bounding boxes

2. Path heuristics

3. L2 heuristic 以起终点为焦点，以当前最优解的cost为长轴长做椭圆，在椭圆范围内采样

• Direct Sampling in the L2 Informed set VS Reject sampling in the bounding rectangular ——维度变高，reject的效率就会变得很低

1. unit Bay 单位圆内采样

2. scale 长轴和短轴放缩

3. rotation 旋转

4. translation 平移

1. 如果不是路径长度L2 norm作为cost，inform就不能显式的表现出来

GuILD （Guided Incremental Local Densification）

窄缝情况，inform范围非常大，难以采样到窄缝 当路径轨迹不能实时更新而其他的树状节点被更新，也可以立马利用起来

• 定义一个local subset sampling

More

现有的比较好的路径规划的算法

1. Open Motion Planning Library (OMPL)
https://ompl.kavrakilab.org/

2. Moveit with ROS（机械臂规划）
https://moveit.ros.org/

3. Fast-lab
sampling-based-path-finding
ZJU-FAST-Lab • Updated Jun 6, 2025

作业

Lecture 4 动力学约束下的运动规划

如何在考虑机器人的运动学模型下去找这样一个安全可行的路径

一、Introduction 动力学概念介绍

1. Kinodynamic: 一种生成机器人运动同时受限于运动学约束（如避障）和动力学约束（如速度、加速度、力的上下限），现实情况下的机器人无法被当做一个质点去处理
1. 避障的安全性
2. 高阶模型受到微分约束

• 既然在后端有轨迹优化问题，那么为什么要在前段去考虑一个运动学约束呢？
——在前端尽可能的考虑多一点机器人的运动学模型，后端的任务就能减轻。
——轨迹通常只能在局部进行优化的

二、状态栅格搜索算法（state Lattice Search）

workflow

路径规划算法的核心：怎么根据实际的应用场景去灵活构建一棵搜索树search tree

• 正向的方法：离散机器人的控制空间，驱动机器人往前运动

• 反向的方法：离散机器人的状态空间，找一条从当前状态到某一个状态的连接

lattice planning

1. 建立一个描述机器人模型的微分方程（状态方程）S

2. 在控制空间里采样：
给定一个不同的控制量u，在不同的u的激励下，状态会有不同的表现 ——系统状态的仿真和前向的积分
没有目标导向

3. 在状态空间里采样：
算出来u和T，使得车到达目标状态。
难以从两个状态反算出空间的（从状态到状态是如何转移的一个时间的一个表达式）。
可以得到更好的解的质量。

Sample in control space

1. 无人机系统模型写成一个多阶的一个积分器的模型

2. 在给定了对控制空间的一个sample的一个采 样以后，可以往前积分得到一个末状态

3. 状态转移方程
• （整个状态的函数）和初状态，和控制量之间的关系
• 零状态响应+零输入响应

在控制空间离散化

1. lattice graph

给定一个初始状态，然后把所有的控制变量加上去往前驱动，然后再对每一个节点把所有的控制变量加上去往前驱动。

the lattice graph obtained by searching

2. 想要去离散化控制输入，然后得到一个搜索树，树上有很多local_motion_collision：
• 拿出树上的一个状态（如dynamic RRT*采样一个点或者lattice_graph上连接的一个节点）
• 选择一个控制输入一个给定的u，固定一个时间
• 然后把状态往前去积分
• 积分出来的边碰到障碍物——不加到树里
• 积分出来没有碰到障碍物——加到树里

在状态空间离散化

lattice graph复杂度还是很高的。如果想去做一个在线的，完全可以构建很少的几层比如三层的一个lg也是可以用的

三、两点边界值最优控制问题

Boundary Value Problem (BVP)

Optimal Boundary Value Problem (OBVP)

给定两个状态，算它们之间连接的一个局部轨迹

极小值原理

离线的去进行一个轨迹生成 在线的时候就是把lattice graph贴在机器人无人车状态上，然后进行一个搜索

Trajectory library

Heuristic

• 假设不存在障碍物

• 假设不存在动力学

工程上会考虑将两个加在一起或者取一个最大值的处理策略

Assume no obstacle existence

Planning in Frenet-serret Frame

三、混合A*算法 Hybrid A*

将栅格地图的路径搜索算法跟lattice graph结合起来

• A*算法和JPS的区别：A*算法怎么去找邻居？

四、Kinodynamic RRT*

我在高维的state定义的一棵随机树里面，采样一个新的节点，计算它给定一个cost范围，找一个反向的可达集，将范围内所有的树上的节点全部拿出来，在这些节点中选择一个能通向它的cost最低的作为这个节点的父亲——choose parent

算新采样的节点能到哪些节点——一个前向的可达集

Lecture 5 最优轨迹生成

基于优化的运动规划方法——目的性很强，能保证特别快的收敛率

5.1 preliminaries

一、全局方法和局部方法

1. Global Minimum 全局方法：基于采样和图搜索的办法（AO: 先采样，动态构建图，图搜索 PO: 先固定分辨率离散，再进行图搜索）
1. 难以实现高维，难以做到实时
2. 难以引入动力学约束
3. 目的性不明确，收敛速度慢
4. 全局最优性
5. 容易处理环境复杂性
6. 便携性

2. Local Minimum 局部方法：基于确定性优化的轨迹规划方法
1. 不管机器人有多少维度都可以做到快速收敛
2. 处理动态复杂性
3. 利用轨迹优化快速逼近，可以在高维空间收敛快
4. 需要显示建模
5. 需要利用高阶信息
6. 容易陷入局部最优

3. 区别：算力趋近于无穷大时是否一定能找到最优解

• 前端搜索一个低维的拓扑路径（可行域）然后再进行优化

二、轨迹优化

1. 轨迹的表示方法
1. 二维位置的轨迹：（轨迹可以参数化任何东西）
1. 二维位置的轨迹：二维上的x和y坐标用时间去参数化
2. 位置、速度和偏航的轨迹——把定义的轨迹映射到机器人的一些位置、速度、偏航变量

2. smoothness
1. 满足动力学微分约束
2. 让轨迹更符合运行期望（减小能量损耗）

3. 为什么需要后端优化

5.2 multicopter dynamics and differential flatness

一、Differential flatness

由常微分方程描述的一种动力学系统的特性 如无人机：系统状态x：p,v,R,w（多旋翼里通常是三维的位置，三维的速度，无人机的姿态，无人机角速率），输入的控制u:tur,torque（总推力，三个轴上的总力矩向量）——常微分方程描述状态和输入之间确定的关系

• Yaw defined by body X axis projection without drag:
• 把body系的x轴投影到水平面上和惯性系的x轴构成的夹角作为偏航角
• 在上述定义下考虑线性风阻 ——4个奇异点

• Yaw+Tilt

二、flatness transformation

• 多旋翼状态

• 多旋翼控制输入

• 常微分方程

• 平坦输出xyz的位置和无人机的偏航角，四维的输入

1. 微分方程的意义
1. 位置对时间求导
2. 重力矢量，（惯性系下的速度转换到无人机的body系）空气相对于无人机机体的相对速度在无人机机体系下的表示 乘以 一个非线性速度大小相关的比例项 乘以一个和xyz三轴相关的对角矩阵，z轴的单位矢量*f总推力大小*旋转矩阵（从机体系变换到惯性系）
3. 姿态、旋转矩阵和瞬时角速率的关系：姿态求导
4. 常见欧拉公式+修正项转动导致的反向阻尼的力矩+修正项风速影响运动平动导致的力矩在无人机机体系下的表示

2. 微分平坦变换体系推导

5.3 trajectory optimization for multicopters

轨迹优化越方便越好，因此利用平坦输出来避免处理这样一个约束

一、problem formulation of trajectory optimization

1. Trajectory Optimization
平坦输出轨迹优化的general formulation：



• : 轨迹
• : 轨迹时间（不仅优化轨迹本身，还要优化轨迹的总时长）
• : 将其高阶导作为平滑项
• W: 把W变成一个对角矩阵，则先优化这样一个导数的平方
• min(): 最小化能量，为了使问题的定义更好，还需要考虑有限的时间T，使轨迹在时间效率和能量效率之间权衡。
• :
• ;

可以表示很灵活的约束的定义，在上有一个等价的平坦输出上的形式

• : 边界条件
• 定义为;
• 和是初始和终端的一个确定的状态，是给定的量

难点： 关于f：选择拓扑路径，环境上的复杂度组合爆炸，无法进行穷举； 这些约束都可以随意定义； 系数的大小决定。

minimize velocity minimize acceleration minimize jerk 可以让无人机的滚转速率尽可能小，缓解角速率弧度，让无人机状态比较稳定 minimize snap

2. Unconstrained Case

轨迹优化在无约束（等一个或多个不存在）情况下怎么解



• s={0，1，2，3，4}，s次连续可微，{位置p，p的1阶导速度v，p的2阶导加速度a，p的3阶导jerk，p的4阶导snap}，如s=4，则要求p、v、a、j、s都是存在且连续的（最优解的一个特质）
这里的 s 就是代价函数里所惩罚的那一阶导数的阶数，也可以叫「平滑度阶」



• d可以比s小
• 最优性的充要条件：解是最优的，当且仅当以下条件成立：
• z在每一段轨迹上可以以一个2s-1次的多项式来描述。
• 边界和中间值一定要满足
• 最优解在时刻一定是连续可微的

二、Unconstrained case：BVP （一）

1. Boundary value problem (BVP)
简化，轨迹minimize s阶导的能量并且只需要指定初始和末尾的状态(f,p,v,a)。

生成大量边界值问题的解并逐个判断是否满足约束（是否会撞到障碍物，是否满足动力学约束），在可行的里面选最好的



solve矩阵构建

2. Smooth multi-segment trajectory via BVPs
1. 显示指定第n段的初始末尾状态，不断解每一段的BVP，让轨迹变得很顺滑
2. 在每一段连接点给相同末状态和初状态
3. T很短或者两段距离很短的时候有速度稳定性的问题



• T很小的时候这个求解就会变得困难，数值不稳定

3. How to check feasibility of primitives

如何判断是否每个点都满足约束条件——检查用多元多项式表示的连续时间约束





1. 离散时间采样
2. 递归边界检查器：（效率高，只适用于某些特定的连续时间约束）
1. check加速度在整段0-t中有没有超约束：
2. 将三维向量加速度的模长平方用每个分量的最大值来做上界，检查bound有没有超。
3. 进行递归分割时域，直到T收敛于0的时候
3. 极值检查
1. 解关于t的一个多元多项式的值
4. 多项式轨迹具有多元多项式形式的连续时间约束的验证
1. 转化为在 上是否有根的问题，有根则违反约束
2. ——将连续时间约束变成是否有根问题
• Sturm Theorem
• 符号跳变次数
Sturm Theorem

三、Unconstrained case：BVP （二）

初始状态和末端状态确定，让轨迹经过一些航点

1. Boundary-intermediate value problem (BIVP)

2. Spline trajectory generation via BIVP
1. BVIP问题的解法：根据最优性条件把问题变成Mc=b,b是一些边界条件和中间条件，M是关于c的一些在t等于不同时刻时的一些系数，c就是我们待求的多段多样条的一个系数矩阵

3. Hierarchical approach via BIVP
• path planning + trajectory generation

4. 用例如RRT*的global method找到一个比较短的前端的路径

5. 在路径上取一些对应的关键的waypoint生成轨迹，近似获得一些不碰到障碍物连接起始点的轨迹。（可以采用DP算法去简化线段为点）

6. 得到关键点之后去调用BVIP

7. 只能保证经过的waypoint是可行的，如果中间与障碍物发生碰撞

8. 一种解决方法：首先保证轨迹本身是可行的，一旦发生碰撞，继续把path中间的位置点当做新的waypoint加入轨迹，轨迹就会更贴合freepath，生成更安全的轨迹。
• 由此可以有效的把RRT*这种只能生成低维的global method用在决定后端优化的waypoint的选取上，可以在不那么稠密的环境下达到比较顺滑的飞行效果

四、Constrained Case

安全约束和动力学input约束

• 最佳参数化是未知的

• 时空剖面涉及到许多点

• 动态可行性可以是非凸的

• 安全区域产生高复杂性

1. Convex formulation
1. 给定首尾相接的凸多面体（Ax≤b）
2. 将规划的轨迹每段各自约束在凸多面体中
3. 进行轨迹上某点的安全约束：
4. 相比于BIVP或者BVP最关键的就是多了安全性约束

1. 凸函数与凸集

2. 凸优化

怎么把一个general的轨迹优化问题变成一个凸优化问题，或者是把一个平坦输出的轨迹规划问题变成一个

1. 最简单的凸优化就是线性变化
2. QP，将一个轨迹规划的问题formulate成一个二次规划的问题
1. QP里的P矩阵必须是正定或半正定的二次型
3. SOCP二次锥规划
4. 如果有一个问题可以规划成LP,QCQP,QP，那这个问题是可以获得一个全局最优解的

凸优化处理不了时间分配的问题

1. 不分配waypoint之间的时间，分配每个凸多面体的时间
1. 通常采用梯形速度
2. （只能获得一个不错的相对时间分配）

五、Constrained Case:Spatial-Temporal Deformation(Brief)

怎样处理general case也就是无人机的轨迹优化，要考虑free space的安全约束，也要考虑通用的定义在x,u上的动力学的约束，并且要优化Time allocation

1. Spatial-temporal deformation

用航点航时参数化会比用向量压缩很多个维度，维度降低一般来说可以提高优化求解速率

Lecture 6 模型预测控制与运动规划

6.1 reactive control & optimal control

一、Reactive Control

考虑当前状态误差

二、Optimal Control 最优控制

怎么获得最佳的控制策略——需要知道当前策略对未来的影响

1. 系统动力学模型：
通过模型映射到下一个时刻的状态

2. 给定初始状态和未来一系列的控制输入，就可以预测未来一系列时间的系统状态

3. 建立目标函数

4. 考虑约束
1. 等式约束：模型状态方程+other等式约束
2. 不等式约束：系统状态和输入量等，避开障碍物时设置的约束等

5. 开环最优控制难点：
动态模型通常是不准确的。模型误差随着时间的推移而累积

6.2 Model Predictive Control （MPC）

一、MPC

在每一个时刻都从系统当前状态（一般为传感器测量得到）出发，把它设为优化问题的初始状态，优化出未来一小段时间的控制量，只执行第一个time的输出，再次求解未来一小段时间…（滚动优化一小段轨迹）

二、MPC Design

1. 最小化系统误差，权衡输入变化量
y：系统输出（如小车位置）
r：输出的参考（如参考线）

2. 等式约束：系统的状态方程

3. 不等式约束：box约束（状态和控制的上下界约束）

4. 初始状态：选择当前状态

• warm-started：由于mpc只取第一个time staple的控制量，再replan，因此我们用上一次求解的结果作为下一次求解的初值，来提高收敛速度

1. 模型选择

2. 目标函数设计

3. 预测时长

4. 终端约束

6.3 线性MPC Linear MPC

1. 贝尔曼最优性法则
把动态问题拆解为一系列的子问题来递归求解

Lecture 8 经典local planning 框架

8.1 Fast-planner

1. 轨迹规划部分分为三个模块

2. 前端Front-End：Hybrid A*，用于生成比较粗糙的路径/轨迹

3. 后端

一、Front-End

通过前端得到一条满足动力学约束的轨迹

1. A* vs Hybrid A*

2. Hybrid A*

3. 怎么生成primitives（L4）
1. 三个独立的一维多项式轨迹
2. 给出初始状态和控制量u，可以得出整条轨迹
3. 在三维各自的上下界范围内进行平均的离散
• 每一个新节点当作初始状态，去离散很多不一样的输入量，算出非常多的轨迹
• 在fast-planner中n=2

4. 怎么计算primitive的cost
1. 将cost函数定义：
2. 先前我们将u离散化，因此对于每一小段primitives，输入u也是固定的，一条primitive的cost很好计算

5. 怎么计算Heuristic
1. 设计一个heuristic function：
2. 基于这个函数设计一条轨迹，时长已知
3. 计算一条三阶多项式轨迹，首尾满足速度和位置约束
4. 计算位置的轨迹
5. 求两次导计算出加速度的轨迹，即为fast-planner中的控制输入
6. 希望heuristic函数值越小越好——关于T求导，得出当前状态到终点使定义的cost最小的heuristic的值

二、Back-End

基于前端的这条轨迹生成一条同时满足其他约束的轨迹

1. B-Spline B样条轨迹类
1. B样条是轨迹的一种表达方式
2. 贝塞尔曲线
3. B样条：分段的特殊的贝塞尔曲线
1. 特性：两两连接处保持平滑连续
2. B样条：基函数x控制点再累加起来
3. 区别：
1. 贝塞尔曲线t：0-1，B样条曲线无限制；
2. 基函数定义方式不同
4. 均匀B样条：相邻节点的差是相同的
5. 基函数：
• B样条自变量可以有很多形式，不只是在时间域上（注意t和u是不是一个概念）
• 基函数是一个递归的形式



• p指阶数



• p=0的时候，它的每一个半开区间里面基函数都是可以直接写出来的
• p=1时两两推导新的，一层一层推上去
• 基函数非0区域在逐渐变大
• p=3时

6. B样条性质
1. 性质1：（局部支撑性）1个控制点的影响区域范围是有限的
1. 控制点要乘以基函数，基函数有值的区域就在三角形范围内，因此控制点影响的区域有限
2. 性质2：在已定义控制点{}和基函数的情况下，可以获得一个p次B样条函数
3. 性质3：连续性（p≥3）
1. 对于节点
2. p=0时，存在跳变——一个一个的点
3. p=1时，控制点之间的连接
4. p=2时，得到一条轨迹
5. p=3时，常用，四个控制点得到一条轨迹
4. 性质4：B样条的导数也是B样条
7. 凸包性质
3. 将原本的对整条轨迹的约束，变成只对速度和加速度控制点的约束
4. ：Q到距离它最近的障碍物的距离
• 用esdf（特殊的栅格地图，存放的是当前的栅格到最近的障碍物的距离）来得到
8. Time Adjustment 迭代的时间重分配
1. 通常先把quality调大一点，生成一条几何空间上满足避障要求的轨迹，然后通过调整控制点之间的时间大小的方式，来让控制点上的每一个点都满足动力学的约束
2. 将均匀的B样条换成非均匀B样条，只改变时间分配，不改变控制点，曲线的几何形状不变

8.2 Ego-Planner

最大的优点：ESDF-free

1. 前端只需要生成一条满足起点和终点状态约束的轨迹作为初值，不用考虑障碍物。

2. Trajectory Optimization

3. collision cost

4. 时间重分配
1. fast：非均匀B样条的时间重分配
1. 轨迹初状态的高阶量发生变化会导致抖动
2. ego：均匀B样条的时间重分配
1. 使用一条时间分配更加合理，更长一些的均匀B样条来做时间分配
2. 目标是拿一条时间分配更加合理的B样条，使其位置尽可能贴合我们之前规划出来的轨迹，且满足动力学约束要求和smooth要求
3. 需要得到：均匀的，轨迹的初值
4. 获得优化初值

时间重分配

1. 新的时间如何获得
1. 查询每一个控制点的每一维，去查是否超过了限度的最大值，基于这个信息计算一个新的让时间扩展的系数

2. 基于新的时间节点长度来计算轨迹的初值



·
新的轨迹的控制点就是我们要求的东西
新扩展的轨迹在每个新扩展的时间上，都希望与原来的轨迹在旧的时间上的位置对应相同