文章目录
1 MinimumMinimumMinimum SnapSnapSnap闭式求解的推导1.1 二次规划等式约束构建1.2 求ddd1.3 转成无约束优化问题1 MinimumMinimumMinimum SnapSnapSnap闭式求解的推导
可以看看我的这几篇Blog1,Blog2,Blog3。
1.1 二次规划等式约束构建
闭式法中的 QQQ 矩阵计算和之前MinimumMinimumMinimum SnapSnapSnap当中的一样,但约束的形式与之前略为不同,在之前的方法中, 等式约束只要构造成 […]p=b[\ldots] p=b[…]p=b 的形式就可以了,而闭式法中,每段轨迹都构造成如下:
Aipi=di,Ai=[A0At]iT,di=[d0,dT]iA_{i} p_{i}=d_{i}, A_{i}=\left[A_{0} A_{t}\right]_{i}^{T}, d_{i}=\left[d_{0}, d_{T}\right]_{i} Aipi=di,Ai=[A0At]iT,di=[d0,dT]i
其中 d0、dTd_{0}、d_{T}d0、dT 为第 iii 段轨迹的起点和终点的各阶导数组成的向量,比如只考虑PVA: d0=[p0,v0,a0]Td_{0}=\left[p_{0}, v_{0}, a_{0}\right]^{T}d0=[p0,v0,a0]T , 当然也可以把jerk、snapjerk、snapjerk、snap等加入到向量。注意:这里是不管每段端点的PVAPVAPVA是否已知,都写进来。 块合并各段轨迹的约束方程得到:
Atotal[p1⋮pk]=[d1⋮dk]=[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)]⏟6k×1A_{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)⎦⎤
kkk 为轨迹段数, nnn 为轨迹的阶数,设只考虑pva, AtotalA_{\text {total }}Atotal 的sizesizesize为 (norder+1)k×6k\left(n_{\text {order }}+1\right) k \times 6 k(norder+1)k×6k 。
由上式可以看到, AtotalA_{\text {total }}Atotal 是已知的 ,而 ddd 中只有少部分(起点、终点的PVAPVAPVA等)是已知的,其他大部分是未知的。如果能够求出 d\boldsymbol{d}d ,那么轨迹参数可以通过 p=A−1dp=A^{-1} dp=A−1d 很容易求得。
1.2 求ddd
闭式法的思路是: 将 ddd 向量中的变量分成两部分:"ddd中所有已知量组成的FixFixFix部分 dFd_{F}dF "和”所有末知量组成的FreeFreeFree部分 dPd_{P}dP ”。然后通过推导,根据 dFd_{F}dF 求得 dPd_{P}dP ,从而得到 ddd ,最后求得 ppp 。 下面介绍整个推导过程。
消除重复变量(连续性约束)
可以会发现,上面构造等式约束时,并没有加入连续性约束,连续性约束并不是直接加到等式约束中。 考虑到连续性 (这里假设PVA连续), ddd 向量中很多变量其实重复了,即
pi(ti)=pi+1(ti),vi(ti)=vi+1(ti),ai(ti)=ai+1(ti)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)
因此需要一个映射矩阵将一个变量映射到两个重复的变量上,如 [aa]=[11]a\left[\begin{array}{l}a \\ a\end{array}\right]=\left[\begin{array}{l}1 \\ 1\end{array}\right] a[aa]=[11]a ,将变量 aaa 映射到左边向量中的两个变量。
所以构造映射矩阵 M6k×3(k+1)M_{6 k \times 3(k+1)}M6k×3(k+1) :即 d=Md′d=M d^{\prime}d=Md′ 。
向量元素置换
消除掉重复变量之后,需要调整 d′d^{\prime}d′ 中的变量,把fix部分和free部分分开排列,可以左成一个置换矩阵 CCC ,使得
d′=C[dFdP]d^{\prime}=C\left[\begin{array}{l} d_{F} \\ d_{P} \end{array}\right] d′=C[dFdP]
再来构造CCC矩阵即可,CCC阵的构造参考MinimumMinimumMinimum SnapSnapSnap的构造方法,例如设 d′=[abcd]d^{\prime}=\left[\begin{array}{l}a \\ b \\ c \\ d\end{array}\right]d′=⎣⎡abcd⎦⎤, 其中 a,c,da, c, da,c,d 是已知 (dF),b\left(d_{F}\right) , b(dF),b 末知 (dP)\left(d_{P}\right)(dP) ,构造一个 4×44 \times 44×4 的单位阵,取 dFd_{F}dF 所在的 (1,3,4)(1,3,4)(1,3,4) 列放到左边,再取 dP\boldsymbol{d}_{P}dP 所在的(2)(2)(2)列放到右边,就构造出置换矩阵 C\boldsymbol{C}C :
[abcd]=[1000000101000010]⏟C[acdb]\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=MC[dFdP]p=A−1d=A−1MC⏟K[dFdP]=K[dFdP]\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=MC[dFdP]p=A−1d=KA−1MC[dFdP]=K[dFdP]
代入优化函数:
minJ=pTQpJ=[dFdP]TKTQK⏟R[dFdP]=[dFdP]T[RFFRFPRPFRPP][dFdP]=dFTRFFdF+dFTRFPdP+dPTRPFdF+dPTRPPdPQ对称⇒R对称⇒⇒=dFTRFFdF+2dFTRFPdP+dPTRPPdP\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=[dFdP]TRKTQK[dFdP]=[dFdP]T[RFFRPFRFPRPP][dFdP]=dFTRFFdF+dFTRFPdP+dPTRPFdF+dPTRPPdP=dFTRFFdF+2dFTRFPdP+dPTRPPdP
令 JJJ 对 dPd_{P}dP 的导数 ∂J∂dP=0\frac{\partial J}{\partial d_{P}}=0∂dP∂J=0 求极值点:
⇒2dFTRFP+2dPTRPPdP=0(注意RPPT=RPP)⇒dp=−RPP−1RFPTdF\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
至此求得 dPd_{P}dP ,从而求出 ppp 。
如果觉得《Minimum Snap闭式求解相关公式推导》对你有帮助,请点赞、收藏,并留下你的观点哦!