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

这篇具有很好参考价值的文章主要介绍了Minimum Snap闭式求解相关公式推导。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

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} d0dT 为第 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 jerksnap等加入到向量。注意:这里是不管每段端点的 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 p1pk = d1dk =6k×1 p1(t0)v1(t0)a1(t0)p1(t1)v1(t1)a1(t1)pk(tk1)vk(tk1)ak(tk1)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=A1d 很容易求得。

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

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=A1d=K A1MC[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 dPJ=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=RPP1RFPTdF
至此求得 d P d_{P} dP ,从而求出 p p p文章来源地址https://www.toymoban.com/news/detail-607138.html

到了这里,关于Minimum Snap闭式求解相关公式推导的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • PnP算法详解(超详细公式推导)

    博主缺粉丝希望大家能给个关注!!! PnP(Perspective-n-Point)是求解3D到2D点的对应方法。它描述了当知道n个3D空间点及其位置,如何估计相机的位姿。如果两张图像中的一张特征点3D位置已知,那么至少需要3个点对(以及至少一个额外验证点验证结果)就可以计算相机的运动。 P

    2024年02月03日
    浏览(37)
  • 数论 --- 约数和定理公式推导、最大公约数、欧几里得算法

    和试除法判断一个数是不是质数是一个道理 从小到大枚举所有的约数,如果当前数能整除这个数的话,说明这个数就是当前数的约数 优化,与试除法判断质数是一样的 如果 d 是 n 的约数,n / d 也一定能整除 n,一个数的约数也一定是成对出现的,在枚举的时候也可以只枚举

    2023年04月08日
    浏览(83)
  • 人工智能_机器学习065_SVM支持向量机KKT条件_深度理解KKT条件下的损失函数求解过程_公式详细推导_---人工智能工作笔记0105

    之前我们已经说了KKT条件,其实就是用来解决 如何实现对,不等式条件下的,目标函数的求解问题,之前我们说的拉格朗日乘数法,是用来对 等式条件下的目标函数进行求解. KKT条件是这样做的,添加了一个阿尔法平方对吧,这个阿尔法平方肯定是大于0的,那么 可以结合下面的文章去

    2024年02月04日
    浏览(40)
  • 【Paddle】PCA线性代数基础 + 领域应用:人脸识别算法(1.1w字超详细:附公式、代码)

    🌈你好呀!我是 是Yu欸 🌌 2024每日百字篆刻时光,感谢你的陪伴与支持 ~ 🚀 欢迎一起踏上探险之旅,挖掘无限可能,共同成长! 主成分分析(PCA,Principal Component Analysis)是一项在高维数据中,寻找最重要特征的降维技术,大大减少数据的维度,而不显著损失信息量。 本文

    2024年04月28日
    浏览(47)
  • MUSIC算法相关原理知识(物理解读+数学推导+Matlab代码实现)

    部分来自于网络教程,如有侵权请联系本人删除  教程链接:MUSIC算法的直观解释:1,MUSIC算法的背景和基础知识_哔哩哔哩_bilibili  MUSIC算法的直观解释:2,我对于MUSIC算法的理解_哔哩哔哩_bilibili https://blog.csdn.net/zhangziju/article/details/100730081  一、MUSIC算法作用 MUSIC (Multiple

    2024年02月02日
    浏览(40)
  • 【大道至简】机器学习算法之隐马尔科夫模型(Hidden Markov Model, HMM)详解(2)---计算问题:前向算法和后向算法原理详解公式推导及Python实现

    ☕️ 本文系列文章汇总: (1)HMM开篇:基本概念和几个要素 (2)HMM计算问题:前后向算法 (3)HMM学习问题:Baum-Welch算法 (4) HMM预测问题:维特比算法 ☕️ 本文来自专栏: 大道至简之机器学习系列专栏 ❤️各位小伙伴们关注我的大道至简之机器学习系列专栏,一起学

    2024年02月05日
    浏览(45)
  • 投影矩阵推导【线性代数】

    如果两个向量垂直,那么满足。但如果两个向量不垂直,我们就将 b 投影到 a 上,就得到了二者的距离,我们也称为向量 b 到直线 a 的误差。这样就有出现了垂直:                (1) 投影向量 p 在直线上,不妨假设  ,那么误差 。带入式(1)中得到: 投影矩阵:  

    2024年02月06日
    浏览(57)
  • 【线性代数】向量函数求偏导的推导过程

    推导:计算 ∂ ∂ x ( a ⊤ x ) frac{partial}{partial mathbf{x}} (mathbf{a}^top mathbf{x}) ∂ x ∂ ​ ( a ⊤ x ) 定义函数:我们定义函数 f ( x ) = a ⊤ x f(mathbf{x}) = mathbf{a}^top mathbf{x} f ( x ) = a ⊤ x ,其中 a mathbf{a} a 是一个列向量,维度为 n × 1 n times 1 n × 1 , x mathbf{x} x 也是一个列向量

    2024年02月17日
    浏览(45)
  • 【线性代数】20 基变换,基变换公式,坐标变换公式

     前言:基变换在做图像压缩等计算的时候,经常用到。基变换和相似矩阵的定义也有非常密切的联系:基变换的本质就是变换了基向量的一个关联计算,在最小二乘的算法里面,通过选择正确的基可以将计算进行简化。 而正确的的特征向量和特征值的确定,又和本节的基变

    2024年02月07日
    浏览(49)
  • NW小世界网络公式推导

    假设网络中度数为 k k k 的节点有 N k N_k N k ​ 个,总共有 N N N 个节点,则度数为 k k k 的节点出现的概率可以表示为: P ( k ) = N k N P(k) = frac{N_k}{N} P ( k ) = N N k ​ ​ 在NW小世界网络中,每个节点有 k k k 条边,其中 k k k 是一个偶数, k / 2 k/2 k /2 条边与相邻节点相连,另外 k

    2024年02月05日
    浏览(85)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包