Minimum Snap轨迹规划详解（3）闭式求解

Minimum Snap轨迹规划详解（3）闭式求解
如果QP问题只有等式约束没有不等式约束，那么是可以闭式求解（close form）的。闭式求解效率要快很多，而且只需要用到矩阵运算，不需要QPsolver。
这里介绍Nicholas Roy文章中闭式求解的方法。

1. QP等式约束构建

闭式法中的 $Q$

矩阵计算和之前一样（参照文章一），但约束的形式与之前略为不同，在之前的方法中，等式约束只要构造成[...]p=b $[. . .] p = b$

$[. . .] p = b$

其中d₀,d_T $d_{0}, d_{T}$

$k$

$k$

2. 如何求d？

闭式法的思路是：将 $k$ p。
下面介绍整个推导过程，

2.1. 消除重复变量（连续性约束）

可以会发现，上面构造等式约束时，并没有加入连续性约束，连续性约束并不是直接加到等式约束中。考虑到连续性（这里假设PVA连续）， $d$

向量中很多变量其实重复了，即

$p_{i} (t_{i}) = p_{i + 1} (t_{i}), v_{i} (t_{i}) = v_{i + 1} (t_{i}), a_{i} (t_{i}) = a_{i + 1} (t_{i})$

$k$

$k$
- 如
$[\begin{matrix} a \\ a \end{matrix}] = [\begin{matrix} 1 \\ 1 \end{matrix}] a$

$k$ 映射到左边向量中的两个变量。

所以构造映射矩阵 $k$

$k$

即d=Md' $k$

2.2 向量元素置换

消除掉重复变量之后，需要调整d' $k$

$k$

$k$
- 举个例子，设， $k$
$k$

$[\begin{matrix} a \\ b \\ c \\ d \end{matrix}] = \underset{C}{\underset{⏟}{[\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{matrix}]}} [\begin{matrix} a \\ c \\ d \\ b \end{matrix}]$

2.3 转成无约束优化问题

由上面两步可得

$d = M C [\begin{matrix} d_{F} \\ d_{P} \end{matrix}] p = A^{- 1} d = \underset{K}{\underset{⏟}{A^{- 1} M C}} [\begin{matrix} d_{F} \\ d_{P} \end{matrix}] = K [\begin{matrix} d_{F} \\ d_{P} \end{matrix}]$

代入优化函数

$\begin{aligned} min J & = p^{T} Q p \\ J & = {[\begin{matrix} d_{F} \\ d_{P} \end{matrix}]}^{T} \underset{R}{\underset{⏟}{K^{T} Q K}} [\begin{matrix} d_{F} \\ d_{P} \end{matrix}] \\ = {[\begin{matrix} d_{F} \\ d_{P} \end{matrix}]}^{T} [\begin{matrix} R_{F F} & R_{F P} \\ R_{P F} & R_{P P} \end{matrix}] [\begin{matrix} d_{F} \\ d_{P} \end{matrix}] \\ = d_{F}^{T} R_{F F} d_{F} + d_{F}^{T} R_{F P} d_{P} + d_{P}^{T} R_{P F} d_{F} + d_{P}^{T} R_{P P} d_{P} \\ Q 对称 \Rightarrow R 对称 \Rightarrow & = d_{F}^{T} R_{F F} d_{F} + 2 d_{F}^{T} R_{F P} d_{P} + d_{P}^{T} R_{P P} d_{P} \end{aligned}$

$k$

3. 闭式法步骤

总结一下整个闭式法的步骤：

　　1.先确定轨迹阶数（比如5阶），再确定 $k$

　　2.根据连续性约束构造映射矩阵M，并确定d $d$

　　3.计算QP目标函数中的Q（min Jerk/Snap） $d$ 块。

　　4.填入已知变量得到d_F $d$

　　5.根据公式计算得到参数p $p = K [\begin{matrix} d_{F} \\ d_{P} \end{matrix}] 计算得到轨迹参数 p 。$

$p = K [\begin{matrix} d_{F} \\ d_{P} \end{matrix}] 计算得到轨迹参数 p 。$

闭式法主要计算量就在A矩阵的求逆，其他计算基本上是矩阵构造，所以效率比较高，但由于没有不等式约束，所以在中间点只能加强约束，corridor不能直接加到QP问题中，只能是通过压点来实现corridor。
在对计算效率要求比较高或者不想用QPsolver时，可以使用闭式法求解。

代码见这里，由于效果和文章一中的效果一样，这里就不再贴图。

参考链接：https://blog.csdn.net/q597967420/article/details/79031791
相关阅读:
Linux系统负载
 full nat
close wait 状态的随想
 记录一下性能分析问题
 golang 反射
 socket里面那个又爱又恨的锁
 Difference between skbuff frags and frag_list
浅析TCP协议---转载
 http 怎样关闭
 http 响应 ngx_http_send_header ngx_http_output_filter
原文地址：https://www.cnblogs.com/long5683/p/11912518.html

Minimum Snap轨迹规划详解（3）闭式求解

1. QP等式约束构建

2. 如何求d？

2.1. 消除重复变量（连续性约束）

2.2 向量元素置换

2.3 转成无约束优化问题

3. 闭式法步骤