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

最新推荐文章于 2025-03-11 10:28:43 发布

不掉发码农

最新推荐文章于 2025-03-11 10:28:43 发布

阅读量1w

点赞数 31

分类专栏： MinimunSnap轨迹规划

本文链接：https://blog.youkuaiyun.com/q597967420/article/details/79031791

版权

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

1. QP等式约束构建

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

A i p i = d i, A i = [A 0 A t] T i, d i = [d 0, d T] i

$A_ip_i=d_i,~A_i=[A_0~A_t]^T_i,~d_i=[d_0,d_T]_i$
其中

d0,dT $d_0,d_T$ 为第

i $i$ 段poly的起点和终点的各阶导数组成的向量，比如只考虑PVA：

d0=[p0,v0,a0]T $d_0=[p_0,v_0,a_0]^T$ ，当然也可以把jerk，snap等加入到向量。注意：这里是不管每段端点的PVA是否已知，都写进来。
块合并各段轨迹的约束方程得到

A t o t a l ⏟ k (n + 1) \times 6 k ⎡ ⎣ ⎢ ⎢ ⎢ p 1 ⋮ p k ⎤ ⎦ ⎥ ⎥ ⎥ = ⎡ ⎣ ⎢ ⎢ ⎢ d 1 ⋮ d k ⎤ ⎦ ⎥ ⎥ ⎥ = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ p 1 (t 0) v 1 (t 0) a 1 (t 0) p 1 (t 1) v 1 (t 1) a 1 (t 1) ⋮ p k (t k - 1) v k (t k - 1) a k (t k - 1) p k (t k) v k (t k) a k (t k) ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥      6 k \times 1

$\underbrace{A_{total}}_{k(n+1) \times 6k} \left[ \begin{matrix} p_1 \\ \vdots \\ p_k \\ \end{matrix} \right] = \left[ \begin{matrix} d_1 \\ \vdots \\ d_k \\ \end{matrix} \right] =\underbrace{ \left[ \begin{matrix} p_1(t_0)\\ v_1(t_0)\\ a_1(t_0)\\ p_1(t_1)\\ v_1(t_1)\\ a_1(t_1)\\ \vdots \\ p_k(t_{k-1})\\ v_k(t_{k-1})\\ a_k(t_{k-1})\\ p_k(t_k)\\ v_k(t_k)\\ a_k(t_k)\\ \end{matrix} \right]}_{6k \times 1}$

k $k$ 为轨迹段数，