Minimum Snap闭式求解相关公式推导

文章目录

• [1 M i n i m u m Minimum Minimum S n a p Snap Snap闭式求解的推导](#1 M i n i m u m Minimum Minimum S n a p Snap Snap闭式求解的推导)
• • [1.1 二次规划等式约束构建](#1.1 二次规划等式约束构建)
  • [1.2 求 d d d](#1.2 求 d d d)
  • [1.3 转成无约束优化问题](#1.3 转成无约束优化问题)

1 M i n i m u m Minimum Minimum S n a p Snap Snap闭式求解的推导

可以看看我的这几篇Blog1，Blog2，Blog3。

1.1 二次规划等式约束构建

闭式法中的 Q Q Q 矩阵计算和之前 M i n i m u m Minimum Minimum S n a p Snap Snap当中的一样，但约束的形式与之前略为不同，在之前的方法中， 等式约束只要构造成 ... p = b \\ldots p=b ...p=b 的形式就可以了，而闭式法中，每段轨迹都构造成如下：
A i p i = d i , A i = A 0 A t i T , d i = d 0 , d T i A_{i} p_{i}=d_{i}, A_{i}=\leftA_{0} A_{t}\\right{i}^{T}, d{i}=\leftd_{0}, d_{T}\\right_{i} Aipi=di,Ai=A0AtiT,di=d0,dTi

其中 d 0 、 d T d_{0}、d_{T} d0、dT 为第 i i i 段轨迹的起点和终点的各阶导数组成的向量，比如只考虑PVA: d 0 = p 0 , v 0 , a 0 T d_{0}=\leftp_{0}, v_{0}, a_{0}\\right^{T} d0=p0,v0,a0T ， 当然也可以把 j e r k 、 s n a p jerk、snap jerk、snap等加入到向量。注意：这里是不管每段端点的 P V A PVA PVA是否已知，都写进来。 块合并各段轨迹的约束方程得到：
A t o t a l 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 × 1 A_{total}\left\\begin{array}{c} p_{1} \\\\ \\vdots \\\\ p_{k} \\end{array}\\right=\left\\begin{array}{c} d_{1} \\\\ \\vdots \\\\ d_{k} \\end{array}\\right=\underbrace{\left\\begin{array}{c} p_{1}\\left(t_{0}\\right) \\\\ v_{1}\\left(t_{0}\\right) \\\\ a_{1}\\left(t_{0}\\right) \\\\ p_{1}\\left(t_{1}\\right) \\\\ v_{1}\\left(t_{1}\\right) \\\\ a_{1}\\left(t_{1}\\right) \\\\ \\vdots \\\\ p_{k}\\left(t_{k-1}\\right) \\\\ v_{k}\\left(t_{k-1}\\right) \\\\ a_{k}\\left(t_{k-1}\\right) \\\\ p_{k}\\left(t_{k}\\right) \\\\ v_{k}\\left(t_{k}\\right) \\\\ a_{k}\\left(t_{k}\\right) \\end{array}\\right}{6 k \times 1} Atotal p1⋮pk = d1⋮dk =6k×1 p1(t0)v1(t0)a1(t0)p1(t1)v1(t1)a1(t1)⋮pk(tk−1)vk(tk−1)ak(tk−1)pk(tk)vk(tk)ak(tk)
k k k 为轨迹段数， n n n 为轨迹的阶数，设只考虑pva， A total A{\text {total }} Atotal 的 s i z e size size为 ( n order + 1 ) k × 6 k \left(n_{\text {order }}+1\right) k \times 6 k (norder +1)k×6k 。

由上式可以看到， A total A_{\text {total }} Atotal 是已知的 ，而 d d d 中只有少部分（起点、终点的 P V A PVA PVA等）是已知的，其他大部分是未知的。如果能够求出 d \boldsymbol{d} d ，那么轨迹参数可以通过 p = A − 1 d p=A^{-1} d p=A−1d 很容易求得。

1.2 求 d d d

闭式法的思路是: 将 d d d 向量中的变量分成两部分：" d d d中所有已知量组成的 F i x Fix Fix部分 d F d_{F} dF "和"所有末知量组成的 F r e e Free Free部分 d P d_{P} dP "。然后通过推导，根据 d F d_{F} dF 求得 d P d_{P} dP ，从而得到 d d d ，最后求得 p p p 。 下面介绍整个推导过程。
消除重复变量（连续性约束）

可以会发现，上面构造等式约束时，并没有加入连续性约束，连续性约束并不是直接加到等式约束中。 考虑到连续性 (这里假设PVA连续)， d d 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 ) p_{i}\left(t_{i}\right)=p_{i+1}\left(t_{i}\right), \quad v_{i}\left(t_{i}\right)=v_{i+1}\left(t_{i}\right), \quad a_{i}\left(t_{i}\right)=a_{i+1}\left(t_{i}\right) pi(ti)=pi+1(ti),vi(ti)=vi+1(ti),ai(ti)=ai+1(ti)

因此需要一个映射矩阵将一个变量映射到两个重复的变量上，如 a a = 1 1 a \left\\begin{array}{l}a \\\\ a\\end{array}\\right=\left\\begin{array}{l}1 \\\\ 1\\end{array}\\right a aa=11a ，将变量 a a a 映射到左边向量中的两个变量。

所以构造映射矩阵 M 6 k × 3 ( k + 1 ) M_{6 k \times 3(k+1)} M6k×3(k+1) ：即 d = M d ′ d=M d^{\prime} d=Md′ 。
向量元素置换

消除掉重复变量之后，需要调整 d ′ d^{\prime} d′ 中的变量，把fix部分和free部分分开排列，可以左成一个置换矩阵 C C C ，使得
d ′ = C d F d P d^{\prime}=C\left\\begin{array}{l} d_{F} \\\\ d_{P} \\end{array}\\right d′=CdFdP

再来构造 C C C矩阵即可， C C C阵的构造参考 M i n i m u m Minimum Minimum S n a p Snap Snap的构造方法，例如设 d ′ = a b c d d^{\prime}=\left\\begin{array}{l}a \\\\ b \\\\ c \\\\ d\\end{array}\\right d′= abcd , 其中 a , c , d a, c, d a,c,d 是已知 ( d F ) ， b \left(d_{F}\right) ， b (dF)，b 末知 ( d P ) \left(d_{P}\right) (dP) ，构造一个 4 × 4 4 \times 4 4×4 的单位阵，取 d F d_{F} dF 所在的 ( 1 , 3 , 4 ) (1,3,4) (1,3,4) 列放到左边，再取 d P \boldsymbol{d}{P} dP 所在的 ( 2 ) (2) (2)列放到右边，就构造出置换矩阵 C \boldsymbol{C} C :
a b c d = 1 0 0 0 0 0 0 1 0 1 0 0 0 0 1 0 ⏟ C a c d b \left\\begin{array}{l} a \\\\ b \\\\ c \\\\ d \\end{array}\\right=\underbrace{\left\\begin{array}{llll} 1 \& 0 \& 0 \& 0 \\\\ 0 \& 0 \& 0 \& 1 \\\\ 0 \& 1 \& 0 \& 0 \\\\ 0 \& 0 \& 1 \& 0 \\end{array}\\right}{C}\left\\begin{array}{l} a \\\\ c \\\\ d \\\\ b \\end{array}\\right abcd =C 1000001000010100 acdb

1.3 转成无约束优化问题

由上面两步可得
d = M C d F d P p = A − 1 d = A − 1 M C ⏟ K d F d P = K d F d P \begin{gathered} d=M C\left\\begin{array}{l} d_{F} \\\\ d_{P} \\end{array}\\right \\ p=A^{-1} d=\underbrace{A^{-1} M C}_{K}\left\\begin{array}{l} d_{F} \\\\ d_{P} \\end{array}\\right=K\left\\begin{array}{l} d_{F} \\\\ d_{P} \\end{array}\\right \end{gathered} d=MCdFdPp=A−1d=K A−1MCdFdP=KdFdP

代入优化函数：
min ⁡ J = p T Q p J = d F d P T K T Q K ⏟ R d F d P = d F d P T R F F R F P R P F R P P d F d P = 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 对称 ⇒ R 对称 ⇒ ⇒ = 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 \begin{aligned} \min J &=p^{T} Q p \\ J &=\left\\begin{array}{l} d_{F} \\\\ d_{P} \\end{array}\\right^{T} \underbrace{K^{T} Q K}{R}\left\\begin{array}{l} d_{F} \\\\ d_{P} \\end{array}\\right \\ &=\left\\begin{array}{l} d_{F} \\\\ d_{P} \\end{array}\\right^{T}\left\\begin{array}{ll} R_{F F} \& R_{F P} \\\\ R_{P F} \& R_{P P} \\end{array}\\right\left\\begin{array}{l} d_{F} \\\\ d_{P} \\end{array}\\right \\ &=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_{\text {对称 } \Rightarrow R \text { 对称 } \Rightarrow} \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} minJJQ对称 ⇒R 对称 ⇒⇒=pTQp=dFdPTR KTQKdFdP=dFdPTRFFRPFRFPRPPdFdP=dFTRFFdF+dFTRFPdP+dPTRPFdF+dPTRPPdP=dFTRFFdF+2dFTRFPdP+dPTRPPdP

令 J J J 对 d P d_{P} dP 的导数 ∂ J ∂ d P = 0 \frac{\partial J}{\partial d_{P}}=0 ∂dP∂J=0 求极值点:
⇒ 2 d F T R F P + 2 d P T R P P d P = 0 (注意 R P P T = R P P ) ⇒ d p = − R P P − 1 R F P T d F \begin{gathered} \Rightarrow 2 d_{F}^{T} R_{F P}+2 d_{P}^{T} R_{P P} d_{P}=0 \text { (注意 } R_{P P}^{T}=R_{P P} \text { ) } \\ \Rightarrow d_{p}=-R_{P P}^{-1} R_{F P}^{T} d_{F} \end{gathered} ⇒2dFTRFP+2dPTRPPdP=0 (注意 RPPT=RPP ) ⇒dp=−RPP−1RFPTdF

至此求得 d P d_{P} dP ，从而求出 p p p 。