Apollo学习笔记（13）Lattice Planner与OBVP问题

最新推荐文章于 2023-06-18 17:45:59 发布

碎步の流年

最新推荐文章于 2023-06-18 17:45:59 发布

阅读量1.9k

点赞数 5

分类专栏： Apollo 文章标签： c++ 算法线性代数

本文链接：https://blog.csdn.net/qq_24649627/article/details/121541351

版权

Apollo 专栏收录该内容

29 篇文章 119 订阅

订阅专栏

本文是学习了深蓝科技相关课程与阅读了相关文献后，整理所得。
论文为A computationally efficient motio primitive for quadrocopter trajectory generation。

Lattice Planner算法的根本目标就是从车辆当前的位置，结合地图、定位、障碍物等信息，通过采样并搜索出一条设定的目标 cost function 值最小的一组坐标。这就和BVP问题很相像了，都是知道起始状态信息和最终信息（部分也可以），求解出满足要求的多阶曲线。如下图所示，

在这里插入图片描述初始时刻为t=0，终点时刻t=T，起点终点信息知道后，目标就是找到一条五次样条曲线使得车辆在满足约束条件和目标cost方程的情况下找到一条满足的路径。设最终的样条曲线方程为
$x(t)=c_{5}t^{5}+c_{4}t^{4}+c_{3}t^{3}+c_{2}t^{2}+c_{1}t+c_{0}$

给定的初始位置和终点位置分别为a和b，则有下式，
$\begin{bmatrix} a \\ b \\ 0 \\ 0 \\ 0 \\ 0 \\ \end{bmatrix}= \begin{bmatrix} 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 & 2 \\ 20T^3 & 12T^2 & 6T & 2 & 0 & 0 \\ \end{bmatrix}\begin{bmatrix} c_{5} \\ c_{4} \\ c_{3} \\ c_{2} \\ c_{1} \\ c_{0} \\ \end{bmatrix}$

可以看出上述方程可以有很多个解，但是在我们给定了一个目标方程后，满足目标的最优解就只有一个。

在OBVP方法中，我们已经给定了初始位置和终点位置，再加上一个目标方程，我们就可以得到一个可以求解出最有输入控制量的方程。

论文中的 objective function只考虑了jerk的平方（jerk是加速度的加速度），那么问题就只和车辆的位置、速度和加速度相关了，控制系统的输入为jerk，状态控制为位置，速度，加速度和jerk的函数，

Objective Function，使得jerk的平方和为最小值，
$J_{\sum}=\displaystyle\sum_{k=1}^3J_{K}, \\ \space \\ J_{k}=\frac{1}{T}\int^{T}_{0}j_{k}(t)^{2}dt$
State Function：
$State:S_{k}=(p_{k},v_{k},a_{k}) \\ \space \\ Input: u_{k}=j_{k}$
SystemModel:
$\dot{S}_{k}=f_{s}(s_{k},u_{k})=(v_{k},a_{k},j_{k})$

对于上述问题，论文中使用庞特里亚金最小值原理求解，此原理的基本内容为；
$\dot{s}^{*}=f(s^{*}(t),u^{*}(t)), given:s^{*}(t) = s(0) \\ \space \\ \lambda(t) \space is \space the \space solution \space of \space : \\ \dot{\lambda}(t)=-\nabla_{s}H(s^{*}(t),u^{*}(t),\lambda(t)) \\ \space \\ with \space the \space boundary \space condition \space of \space : \\ \lambda(T)=-\nabla h(s^{*}(T)) \\ \space \\ and \space the \space optimal \space control \space input \space is : \\ u^{*}(t)= \arg \underset{u(t)}{\min}\space H(s^{*}(t),u(t),\lambda(t))$

这部分内容一眼看上去就挺懵，可以自己查一下相关资料，这里就不赘述了，下面说一下如何此原理是如何应用的。

第一步，先写出汉密尔顿（hamiltonian）函数
$H(s,u,\lambda)=g(s,u)+\lambda^{T}f(s,u) \\ \lambda=(\lambda_{1},\lambda_{2},\lambda_{3})$
式中， $g$ 就是 objective function 中的 transition cost， $f$ 就是系统的状态方程（system model）。

那么，在本问题中，对应的 transition cost 就是 $j^2$ ， $f$ 是系统状态方程 $v, a, j$ ，则此时汉密尔顿方程为，
$H(s,u,\lambda)=\frac{1}{T}j^{2}+\lambda^{T}f_{s}(s,u)=\frac{1}{T}j^{2}+\lambda_{1}v+\lambda_{2}a+\lambda_{3}j$

第二步，根据庞特里亚金最小值原理
$\dot{\lambda}(t)=-\nabla_{s}H(s^{*}(t),u^{*}(t),\lambda(t))$

可知 $\lambda(t)$ 是对汉密尔顿方程 $H$ 分别求 $p, v, a$ 的偏导，
$\dot{\lambda}=-\nabla_{s}H(s^{*},j^{*},\lambda)=(0,-\lambda_{1},-\lambda_{2})$
然后通过积分获 $\lambda$ 表达式（这里对系数做了一些处理，为了便于后续的计算更方便）：
$\lambda(t)=\frac{1}{T}\begin{bmatrix} -2\alpha \\ 2\alpha t+2\beta \\ -\alpha t^2 - 2\beta t -2\gamma \\ \end{bmatrix}$

第三步，继续看一下最小值原理，最优控制输入为
$u^{*}(t)= \arg \underset{u(t)}{\min}\space H(S^{*}(t),u(t),\lambda(t))$

在第二步中已经计算出 $\lambda$ ，而且其中 $p, v, a$ 都是最优解，现在只需要计算一下 $j$ 的最优解就可以了。

计算一下一阶导数就能计算出 $j$ 的值了，再带入 $H$ 的表达式，则有，
$u^{*}(t)=j^{*}(t)= \arg \underset{j(t)}{\min}\space H(S^{*}(t),u(t),\lambda(t))=\frac{1}{2}\alpha t^{2} + \beta t + \gamma$

对 $j$ 进行一次积分得到加速度 $a$ ，再进行一次积分得到速度 $v$ ，再进行一次积分得到位置信息 $p$ ，
$s^{*}(t)=\begin{bmatrix} \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_{0}t+v_{0} \\ \frac{\alpha}{6}t^{3}+\frac{\beta}{2}t^{2}+{\gamma}t+a_{0} \\ \end{bmatrix}$

稍加整理一下，有

$\begin{bmatrix} \frac{1}{120}T^{5} & \frac{1}{24}T^{4} & \frac{1}{6}T^{3} \\ \frac{1}{124}T^{4} & \frac{1}{6}T^{3} & \frac{1}{2}T^{2} \\ \frac{1}{6}T^{3} & \frac{1}{2}T^{2} & T \\ \end{bmatrix} \begin{bmatrix} \alpha \\ \beta \\ \gamma \\ \end{bmatrix} = \begin{bmatrix} \Delta p \\ \Delta v \\ \Delta a \\ \end{bmatrix}$
式中，
$\begin{bmatrix} \Delta p \\ \Delta v \\ \Delta a \\ \end{bmatrix}=\begin{bmatrix} 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{bmatrix}$

方程的解为，
$\begin{bmatrix} \alpha \\ \beta \\ \gamma \\ \end{bmatrix}=\frac{1}{T^{5}} \begin{bmatrix} 720 & -360T & 60T^{2} \\ -360T & 168T^{2} & -24T^{3} \\ 60T^{2} & -24T^{3} & 3T^{4} \\ \end{bmatrix} \begin{bmatrix} \Delta p \\ \Delta v \\ \Delta a \\ \end{bmatrix}$

最后，计算出 $\alpha,\beta.\gamma$ 后，代入到objection function，
$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$ ，根据实际项目中直接就可以得到 $T$ 的大小，代入上述公式就行。

拓展一下，上述中限制了的是终点位置的三个信息 $p, v, a$ ，如果并不是限制其中全部的信息呢？

这里直接上答案，具体过程可以自行搜索。

固定最终位置和速度：

$\begin{bmatrix} \alpha \\ \beta \\ \gamma \\ \end{bmatrix}=\frac{1}{T^{5}} \begin{bmatrix} 320 & -120T^{2} \\ -200T & 72T^{3} \\ 40T^{2} & -12T^{3} \\ \end{bmatrix} \begin{bmatrix} \Delta p \\ \Delta v \\ \end{bmatrix}$

固定最终位置和加速度：

$\begin{bmatrix} \alpha \\ \beta \\ \gamma \\ \end{bmatrix}=\frac{1}{2T^{5}} \begin{bmatrix} 90 & -15T^{2} \\ -90T & 15T^{3} \\ 30T^{2} & -3T^{4} \\ \end{bmatrix} \begin{bmatrix} \Delta p \\ \Delta a \\ \end{bmatrix}$

固定最终速度和加速度：

$\begin{bmatrix} \alpha \\ \beta \\ \gamma \\ \end{bmatrix}=\frac{1}{T^{3}} \begin{bmatrix} 0 & 0 \\ -12 & 6T \\ 6T & -2T^{2} \\ \end{bmatrix} \begin{bmatrix} \Delta v \\ \Delta a \\ \end{bmatrix}$

单纯的固定位置：

$\begin{bmatrix} \alpha \\ \beta \\ \gamma \\ \end{bmatrix}=\frac{1}{T^{5}} \begin{bmatrix} 20 \\ -20T \\ 10T^{2} \\ \end{bmatrix} \Delta p$

单纯的固定速度：

$\begin{bmatrix} \alpha \\ \beta \\ \gamma \\ \end{bmatrix}=\frac{1}{T^{3}} \begin{bmatrix} 0 \\ -3 \\ 3T \\ \end{bmatrix} \Delta v$

单纯的固定加速度：

$\begin{bmatrix} \alpha \\ \beta \\ \gamma \\ \end{bmatrix}=\frac{1}{T} \begin{bmatrix} 0 \\ 0 \\ 1 \\ \end{bmatrix} \Delta a$

碎步の流年

关注

5
点赞
踩
20

收藏

觉得还不错? 一键收藏
0
评论
Apollo学习笔记（13）Lattice Planner与OBVP问题

本文是学习了深蓝科技相关课程与阅读了相关文献后，整理所得。论文为A computationally efficient motio primitive for quadrocopter trajectory generation。Lattice Planner算法的根本目标就是从车辆当前的位置，结合地图、定位、障碍物等信息，通过采样并搜索出一条设定的目标 cost function 值最小的一组坐标。这就和BVP问题很相像了，都是知道起始状态信息和最终信息（部分也可以），求解出满足要求的多阶曲线。如.
复制链接

扫一扫