正经夜光杯

这个屌丝很懒，什么也没留下！

热门标签

【L4】深蓝学院-高飞老师移动机器人规划笔记动力学约束下的路径规划_深蓝学院路径规划第四章

作者：正经夜光杯 | 2024-07-27 20:39:31

踩

深蓝学院路径规划第四章

深蓝学院-高飞老师移动机器人规划笔记动力学约束下的路径规划

前言
小例子
- - 自行车
  - 差速驱动机器人
方法
Hybrid A*
Kinodynamic RRT*

传送门
【L3】深蓝学院-高飞老师移动机器人规划笔记基于采样的最优路径规划算法&进阶
 【L2】深蓝学院-高飞老师移动机器人规划笔记基于搜索的路径规划
 【L1】深蓝学院-高飞老师移动机器人规划笔记

前言

Q1: 首先，为什么要kinodynamic呢，也就是考虑运动学和动力学（后续简称为动力学）呢？
因为在现实世界中的机器人，他们是有自己的运动模型的，仅仅将他们转化为质点考虑运动路径肯定是有失偏颇的。
Q2: 当然，前面其实提到过，在整个pipeline中，还有后端做轨迹优化的部分，会考虑到机器人的动力学，那在前端为什么要考虑动力学这个事情呢？
简单来讲，这样去做可以降低后续后端路径优化的负担。而且，后端优化的时候，往往都是在局部进行优化，如果前端完全不考虑动力学的话，可能会产生对于机器人来说动力学代价更大的路径。

小例子

自行车

一般比较简单，输入就是速度和角速度，
$\left($

\begin{matrix} \dot{x} \\ \dot{y} \\ \dot{θ} \end{matrix}

$\begin{array}{c}\dot{x} \\ \dot{y} \\ \dot{\theta}\end{array}$ \right)=\left(

\begin{matrix} \cos θ \\ \sin θ \\ 0 \end{matrix}

$\begin{array}{c}\cos \theta \\ \sin \theta \\ 0\end{array}$ \right) \cdot v+\left(

\begin{array}{l} 0 \\ 0 \\ 1 \end{array}

$\begin{array}{l}0 \\ 0 \\ 1\end{array}$ \right) \cdot \omega

\overset{x}{˙} \overset{y}{˙} \dot{θ} = cos θ sin θ 0 \cdot v + 001 \cdot ω

约束就是速度和角速度的最大值，
在这里插入图片描述

差速驱动机器人

$\left($

\begin{matrix} \dot{x} \\ \dot{y} \\ \dot{θ} \end{matrix}

$\begin{array}{c}\dot{x} \\ \dot{y} \\ \dot{\theta}\end{array}$ \right)=\left(

\begin{matrix} \frac{r}{2} (u_{l} + u_{r}) \cos θ \\ \frac{r}{2} (u_{l} + u_{r}) \sin θ \\ \frac{r}{L} (u_{r} - u_{l}) \end{matrix}

$\begin{array}{c}\frac{r}{2}\left(u_l+u_r\right) \cos \theta \\ \frac{r}{2}\left(u_l+u_r\right) \sin \theta \\ \frac{r}{L}\left(u_r-u_l\right)\end{array}$ \right)

\overset{x}{˙} \overset{y}{˙} \dot{θ} = \frac{r}{2} (u_{l} + u_{r}) cos θ \frac{r}{2} (u_{l} + u_{r}) sin θ \frac{r}{L} (u_{r} - u_{l})

类似的约束，

这几个比较经典的动力学模型网上的教学还比较多，随便挂一个差速驱动和自行车模型的二维运动学模型。

方法

状态栅格搜索算法：State Lattice Search

前面的基于采样的、基于图搜索的方法都有一个共同需要关注的问题，那就是搜索树的构建问题，然后就可以用各类的图搜索的方法解决问题。但是在之前的方法中，我们都默认机器人是一个质点，它可以向各个方向移动，但是现在就需要一个节点之间的可以给机器人进行执行的feasible motion connection 而不是之前的直接连接。
有两种思路去实现这样的建图（边）：

正向-离散化控制空间：驱动机器人运动得到feasible motion connection。
反向-离散化状态空间：找到机器人周围的很多个状态，找到当前状态到某个状态的连接。

对于一个机器人的运动模型： $\dot{s}=f(s, u)$ ，即状态s的导数是一个由状态和输入为变量去描述的函数。一般会给一个初始状态 $s_0$ 。

对应上面的两种思路，

离散化控制空间：给系统不同的u，并保持一定的时间，看系统在不同的激励下会变成什么样子。前向仿真，易于实现，有点类似djisktra算法，没有目的性的扩展，容易造成较低的效率。
离散化状态空间：给定状态，从两个状态之间解析运动的边（解出u和T），看什么样的u和T能让机器人从一个状态到另外一个状态。反向的计算，比较难实现，但是自然地具备heuristic的效果。

在这里插入图片描述

在控制空间采样的示例

在这里插入图片描述
用位置和速度作为状态 $s$ ，用加速度作为输入 $u$ ，构成一个线性系统方程： $\dot{s}=A \cdot s+B \cdot u$ 。（再往高阶的话，也可以用三阶导jerk作为输入，状态填入加速度的三个维度）。中间的图即表明了，在以加速度为输入的情况下，以[1，0，0]作为初速度，选取不同加速度后经时间t后得到的离散的控制空间的点。右边的图就是用jerk作输入的结果。不断进行这个过程就会得到lattice graph，如下所示，但是其实不用把整个lattice graph建完再搜索路径（因为这个图与前面用的已经构建好的栅格图不一样，这个图最开始是没有的），可以利用heuristic函数类似的思路去提高效率。
在这里插入图片描述

需要注意一下，上面系统方程中的A矩阵，叫做nilpotent，即幂零矩阵，这个矩阵的性质很适合去除展开式里的高次项，方便我们求解。

具体来说，给定sample后，可以根据系统方程输入计算最后的末状态（给定输入和时间），驱动系统向前前进，那怎么确定中间的轨迹呢？这就涉及到了如下的状态转移方程：
$s(t)=\underbrace{e^{A t}}_{F(t)} s_0+\underbrace{\left[\int_0^t e^{A(t-\sigma)} B d \sigma\right]}_{G(t)} u_m$

简单来讲，就是状态 $s$ 随时间变化的函数，是与初状态和整个过程中选取的控制量有关的。前半项是零输入响应，后半项是零状态响应。如果给定时刻 $t$ ，一般对 $e^{A t}$ 做展开计算， $e^{A t}=I+\frac{A t}{1 !}+\frac{(A t)^2}{2 !}+\frac{(A t)^3}{3 !}+\cdots+\frac{(A t)^k}{k !}+\cdots$ ，这样A矩阵作为幂零矩阵就很有作用了。

对上述的整个过程举一个简单的小车模型的例子，在这里插入图片描述

在状态空间采样的示例

如下图所示，给定一个初始的状态，先离散出他周围临近的状态，然后反推状态之间的可行性路径。
在这里插入图片描述
下面图展现了，两者比较直观的对比，

看起来，状态空间采样的方法要好一些，因为不会出现那些失败的路径。而且如果控制空间的输入变化的范围控制不好的话，可能会导致一系列的路径都容易是失败的，因为他们比较接近。那为什么不完全使用状态空间采样的方法呢，原因在于其实现的难度。下面介绍一下，已知起点终点状态，怎么去解中间的可行路径的一个方法。

Boundary Value Problem （BVP）

在这里插入图片描述
比如，以x轴方向为例，可以用多项式做参数化，

以上可能有很多解，因为限制条件比较少，获得一个解相对而言比较容易，但是获得比较优的解还需要其他的方法。

Optimal Boundary Value Problem （OBVP）

任务：给定初始和终点两个状态，我们想要去找到中间的最优 $s^*$ 和 $u^*$ 。
在这里插入图片描述

目标函数： $J_{\Sigma}=\sum_{k=1}^3 J_k, J_k=\frac{1}{T} \int_0^T j_k(t)^2 d t$
这个目标函数描述的是：首先，将三维的运动的任务分解为单独一个维度再一起考虑，然后每一个维度考虑的是一段时间T内jerk的平均变化程度，这里平方是因为jerk自身表示加速度的导数，有正有负。
状态： $s_k=\left(p_k, v_k, a_k\right)$ ，状态某一个维度上的位置、速度以及加速度
输入： $u_k=j_k$ ，输入为加速度的导数jerk
系统方程： $\dot{s}=f_s(s, u)=(v, a, j)$

这里以庞特里亚金极小值定理为例展现整个计算的过程，

首先根据系统方程中变量的个数，需要定义协态 $\lambda=\left(\lambda_1, \lambda_2, \lambda_3\right)$
而后构造汉密尔顿函数
$\begin{aligned} H (s, u, λ) & = \frac{1}{T} j^{2} + λ^{T} f_{s} (s, u) \\ = \frac{1}{T} j^{2} + λ_{1} v + λ_{2} a + λ_{3} j \end{aligned}$
极小值定理做的事情，就是协变量 $\lambda$ 是如下方程的解：
$\dot{\lambda}(t)=-\nabla_s H\left(s^*(t), u^*(t), \lambda(t)\right)$
即汉密尔顿函数在状态与输入都是最优的情况下对状态求的偏导数。公式表示 $\lambda(t)$ 的时间导数等于汉密尔顿函数在最优状态、最优控制和当前伴随变量下对状态的梯度的相反数。边界条件： $\lambda(T)=-\nabla h\left(s^*(T)\right)$ ，表示控制最后的时刻对末状态的惩罚。
通过这种方式最优控制量： $u^*(t)=\arg \min _{u(t)} H\left(s^*(t), u(t), \lambda(t)\right)$ ，即在找到最优的 $s^*$ ，后怎么找到最优的输入 $u^*$ ，使得汉密尔顿函数最小。
根据 $\dot{\lambda}(t)=-\nabla_s H\left(s^*(t), u^*(t), \lambda(t)\right)$ 与本例中的汉密尔顿函数：
可以求出 $\dot{\lambda}=-\nabla_s H\left(s^*, u, \lambda\right)=\left(0,-\lambda_1,-\lambda_2\right)$ ，即汉密尔顿函数对位置、速度、加速度的偏导数，再加个负号。
根据 $\dot{\lambda}$ ，求出 $\lambda$ 的一组解，
$\begin{matrix} - 2 α \\ 2 α t + 2 β \\ - α t^{2} - 2 β t - 2 γ \end{matrix}$ ，因为本例中没有施加终末状态的软约束，是强制的必须让系统达到最后的状态，所以这个求解所需的边界条件即是我们规定的最后的状态。
现在我们利用得到的 $\lambda$ 去计算最优控制量， $\begin{aligned} u^{*} (t) & = j^{*} (t) = \arg min_{j (t)} H (s^{*} (t), j (t), λ (t)) \\ = \frac{1}{2} α t^{2} + β t + γ \end{aligned}$
这里就是把协态的那一组解，带入汉密尔顿函数，然后对jerk做偏导，得到极值点对应的j的值。需要注意的是，由于s已经取到optimal了，其中的v和a其实也已经固定了，所以求偏导的时候不需要管他们了。
jerk得到最优了之后，其实最优的s就可以得到了，因为p，v，a其实就是jerk一次次向上的积分。再加上初始的边界条件 $s(0)=\left(p_0, v_0, a_0\right)$ ，即可得到状态的最优解：
$\begin{matrix} \frac{α}{120} t^{5} + \frac{β}{24} t^{4} + \frac{γ}{6} t^{3} + \frac{a_{0}}{2} t^{2} + v_{0} t + p_{0} \\ \frac{α}{24} t^{4} + \frac{β}{6} t^{3} + \frac{γ}{2} t^{2} + a_{0} t + v_{0} \\ \frac{α}{6} t^{3} + \frac{β}{2} t^{2} + γ t + a_{0} \end{matrix}$
当然，这里面的系数还没有定，这时候就要用到终点的边界条件 $s_f$ 去确认了。
整理之后，
$\begin{array}{lll} \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}$
其中， $\begin{matrix} Δ p \\ Δ v \\ Δ a \end{matrix}$ ，
最后即可解得， $\begin{array}{l} α \\ β \\ γ \end{array}$ 。
带入计算目标函数 $J$ ： $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$ ，最后得到的结果应该只跟 $T$ 相关了，通过求 $J$ 的极值也可以求出最优的 $T$ 。
终点边界条件可以完全fix也可以部分fix：

启发式设计

从状态空间采样搜索路径的方法不具备启发式函数，也就是漫无目的的去搜索。结合之前几个章节的启发式函数的内容，在这里同样可以引入heuristic。
当然在这里有两种假设:

假设没有障碍物存在
假设没有动力学存在

Hybrid A*

思路：A* + lattice graph，对比较稠密的图进行剪枝（栅格地图每一个格子里只要一个状态），跟A*的思路一样，区别就是邻居节点的选择是选择相邻的一些邻状态点，然后启发式函数的选择有一些区别。
在这里插入图片描述

Kinodynamic RRT*

上面介绍了基于搜索的算法，接下来介绍一个基于采样的运动学约束的path finding方法
在这里插入图片描述

其实思路差不多，就是在采样点之间生成路径的时候不是直连了，而是通过初始状态解出运动学约束的路径，当然他这里的求解方式跟上面讲的利用汉密尔顿函数的不太一样，这里不去细说了。

RRT*用球形的范围找周围采样点，那个时候的cost就是距离，那这里图里面的点都是不同的状态点，cost是很复杂的一个高维的公式，那就需要在当前状态找到cost在给定范围内的节点（很多个高维的椭球），找利用cost在给定范围r找父节点的集合就是前向可达集，同理有后向可达集。
在这里插入图片描述