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}=\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
其中
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}=\left[p_{0}, v_{0}, a_{0}\right]^{T}
d0=[p0,v0,a0]T , 当然也可以把
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]=[11]a ,将变量
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′=C[dFdP]
再来构造
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
文章来源:https://www.toymoban.com/news/detail-607138.html
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=MC[dFdP]p=A−1d=K
A−1MC[dFdP]=K[dFdP]
代入优化函数:
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=[dFdP]TR
KTQK[dFdP]=[dFdP]T[RFFRPFRFPRPP][dFdP]=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 。文章来源地址https://www.toymoban.com/news/detail-607138.html
到了这里,关于Minimum Snap闭式求解相关公式推导的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!