4 运动动力学约束下的路径搜索

Introduction

T: 考虑机器人的动力学约束
Kinodynamic: kinematic + Dynamic

运动学规划问题是在同时受到运动学约束(如避障)和动力学约束(如速度、加速度和力的模量界限)的情况下综合机器人运动。动力学解是从时间到广义力或加速度的映射。
——《动力学运动规划》，Bruce Donald, Patrick Xavier, John Canny, John Reif

• 不同的限制
• 最大力(加速度)
  Y:
  Straight-line connections between pairs of states are typically not valid trajectories due to the system’s differential constraints.
  由于系统的微分约束，状态对之间的直线连接通常不是有效的轨迹。
  
• coarse-to-fine process(由粗到精）
• 轨迹只会局部规划
• 不可行路径对非完整系统没有意义
  如大部分车不能平移，如下图所示，不能够走下面
  
  
  无人机的一开始的速度是在向右，但是按路径规划左边路径更短，其实应该沿着速度方向向右飞

单轮

差动轮

简化的汽车模型

r应该标记为v

Workflow

basic idea

We have many weapons to attack graph search.
•假设机器人的质量点不再令人满意。
•我们现在需要一个具有可行运动连接的图。

• 我们手动创建(构建)一个所有边都由机器人可执行的图。
• • 反向，离散机器人的状态空间，连接，如栅格地图
• • 正向。离散机器人的控制空间，如PRM
• 这是所有动动力规划的基本动机。
• 状态点阵规划（State lattice planning）是最直接的一种。

build the graph,sample in control vs state space

说明：s为状态变量，u 为控制变量

如果在控制空间没有任务导向性,在已知被控对象的条件下，固定输入的控制量u uu和积分时间T，由此进行前向积分可以得到被控对象从任意的初始状态 s_0过度到终止状态s f，前向积分可以对被控对象在任意时间T之后的状态进行预测，进而得到一系列的状态集合，这就是控制空间中的采样。由于u 和T是任意给定的，在控制空间中的采样往往没有明确的目的性，采样结果只能分布在某些状态附近，无法精确采样到某一个给定状态，如下图所示：

选择一个𝑢，固定一个时间段𝑇，正向模拟系统(数值积分)。

• 正向模拟
• 确定的u,T
• 无任务导向（缺点）
• 容易实施
• 低效率

选择状态s_f,寻找从s_0到s_f的轨迹

• 反向计算:
• 需要知道T,u(或者估计假设等)
• 很难实施
• 具有任务导向性

sample in control space

选择加速度作为控制输入，紫色的线代表(feasible motion)T=1s（假设）时间内机器人的状态轨迹,为啥会呈现抛物线形状,这是由于物体匀加速运动导致的；jerk:加速度求导 x求三次导
A nilpotent:幂零矩阵(会在A的某次方为零,方便计算)

e^At:状态转移矩阵
F(t):零输入响应
G(t):零状态响应
Um:给定输入
S(t):状态响应
其实就是匀加速运动在高维状态的叠加
对采样结果进行进一步的离散化，控制空间中的最终采样结果如下图：

The lattice graph obtained by searching
搜索得到的点阵图
需要要有目标性,不需要完整的搜索
在搜索过程中，可以根据需要建立图形。
•当节点被新发现时 ，创建节点(状态)和边(初始运动)。
•节省计算时间/空间。

步骤:

• 搜索树按某种规则找到一个周期T内的状态S
• 选择一个控制的输入
• 固定一个较短的时间,堆等式进行积分
• 加入没有碰撞的动作到搜索树

构建一个点阵图:
• 给定一个原点。
• 对于原点周围的8个邻居节点，找到可行路径。
• 向外扩展到24个邻居。
• 完整点阵
参考：Generating Near Minimal Spanning Control Sets for Constrained Motion Planning in Discrete State Spaces，Mihail Pivtoraiko and Alonzo Kelly

两层Lattice graph
第一层不同,不同的初始状态

comparison

论文:State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in Complex Environments,
Thomas M. Howard, Colin J. Green, and Alonzo Kelly

• 在初始角速度方向上轨迹密度更大，受初始化影响很大
• 不同输入的输出非常相似。
• 从图中可以看出，控制空间中的采样没有目的性，离散控制量得到的运动路径是不合格的，会使机器人运动到合理的运动环境之外，得到的局部轨迹仅仅有一部分可以使用；状态空间中的采样可以保证局部轨迹可用。

Boundary Value Problem(BVP)

• BVP是状态采样点阵规划的基础。
• 没有通用的解决方案。只能逐个设计。
• 经常进行复杂的数值优化。
  Y：需要求解状态空间采样的问题。
  根据边界条件求解方程
  

optimal BVP

问题：从一个状态（初始）到另一个状态（结束），如何最小化jerk平方和的积分:
（jerk加加速度）

k表示某个轴，
根据庞特里亚的极小值原理，我们首先引入协态：根据system model来定义
定义哈密尔顿函数


首先定义cost function ，然后定义Hamilton中间变量$\lambda$，使用极小值原理选取u使得H函数最小

参考
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

example optimal state

先简化为1轴的问题，最小化无人机的jerk

没有边界条件，没有final state，一定到达的硬约束。根据s(T)=s_f来求解

最后转换为多项式求根
如果有一些变量不需要给定，求最优也能行
同时，可以将代价函数J 转变为关于时间T 的函数，对代价函数求极小值可以求得最优的时间T 如下：

将两点的边界状态代入状态方程，可以求得α , β , γ 的值，OBVP问题求解完成：

最终求多项式的根T

reference

example

Optimal Rough Terrain Trajectory Generation for Wheeled Mobile Robots, Thomas M. Howard Alonzo Kelly
A Computationally Efficient Motion Primitive for Quadrocopter Trajectory。
Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments , Dmitri Dolgov , Sebastian Thrun,
Michael Montemerlo , James Diebel。
https://pythonrobotics.readthedocs.io/en/latest/modules/path_planning.html#optimal-trajectory-in-a-frenet-frame。
参考论文Generation, Mark W. Mueller, Markus Hehn , and Raffaello D’Andrea
Dynamic Programming and Optimal Control, D. P. Bertseka

轨迹库

根据评分函数来确定那个轨迹较合适

Heuristic heuristic

原则:只解决一个简单的问题，
两种做法
•假定不存在障碍
•假设不考虑动力学

Planning in Frenet-serret Frame

参考文献
Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame, Moritz Werling, Julius Ziegler, Sören Kammel, and Sebastian Thrun。
Optimal trajectories for time-critical street scenarios using discretized terminal manifolds , Moritz Werling, Sören Kammel , Julius Ziegler and Lutz Gröll。

作业

OBVP 问题求解