SHENLAN_Chapter5_HW2_通过闭式求解的方法求解只有等式约束没有不等式约束的Minimun snap路径轨迹生成问题

最新推荐文章于 2022-08-19 15:06:03 发布

原创

最新推荐文章于 2022-08-19 15:06:03 发布 · 301 阅读

1 ·

CC 4.0 BY-SA版权

文章标签：

#matlab #算法

本文详细介绍了使用闭式求解方法解决MinimumSnap路径规划问题，包括多项式系数映射矩阵Mj、选择矩阵C的计算过程及代码实现，适用于等式约束下的平滑路径规划。

通过闭式求解的方法求解只有等式约束没有不等式约束的Minimun snap路径轨迹生成问题

原理
- 映射矩阵Mj的求解方法
- selection matrix（选择矩阵）C的求解
代码分析
- 最终结果

原理

解目标函数为Minimum Snap，多项式表达式为七次多项式
$f(t)=\sum\limits_{i}p_it^i=p_7t^7 + p_6t^6 + p_5t^5 + p_4t^4 + p_3t^3 + p_2t^2 + p_1t + p_0$
直接求解p_0、p_1、p_2、p_3、p_4、p_5、p_6、p_7,这些系数也就是优化参数，若直接求解这些系数可能会导致数值求解不稳定，若把优化项替换成p,v,a,j这些具有实际物理意义的变量的话则求解结果会更稳定。
通过一个mapping矩阵把多项式系数映射到p,v,a,j（即f(t),f’(t),f’’(t),f’’’(t)）。
$M_jp_j=d_j \tag{1}$
Mj就这这个映射矩阵，则：
$p_j=M_j^{-1}d_j \tag{2}$
其转置为：
$p_j^T=d_j^TM_j^{-T} \tag{3}$
$J=\left[ \begin{matrix} p_1 \\ \vdots\\ p_M \\ \end{matrix} \right]^T \left[ \begin{matrix} Q_1 &0 &0 \\ 0 &\ddots &0\\ 0 &0 &Q_M \\ \end{matrix} \right] \left[ \begin{matrix} p_1 \\ \vdots\\ p_M \\ \end{matrix} \right] \tag{4}$
由把式(2)、(3)代入式(4)，则
$J=\left[ \begin{matrix} d_1 \\ \vdots\\ d_M \\ \end{matrix} \right]^T \left[ \begin{matrix} M_1 &0 &0 \\ 0 &\ddots &0\\ 0 &0 &M_M \\ \end{matrix} \right]^{-T} \left[ \begin{matrix} Q_1 &0 &0 \\ 0 &\ddots &0\\ 0 &0 &Q_M \\ \end{matrix} \right] \left[ \begin{matrix} M_1 &0 &0 \\ 0 &\ddots &0\\ 0 &0 &M_M \\ \end{matrix} \right]^{-1} \left[ \begin{matrix} d_1 \\ \vdots\\ d_M \\ \end{matrix} \right] \tag{4}$

映射矩阵Mj的求解方法

Mj代表第j段轨迹曲线所代表的多项式系数对应的映射矩阵。
即由这段轨迹p0、p1、p2、p3、p4、p5、p6、p7多项式系数可以找到一个M矩阵映射到该段轨迹首状态f(0)、f’(0)、f’’(0)、f’’’(0)和末状态f(t)、f’(t)、f’’(t)、f’’’(t)。若知道了首末状态也就能唯一确定多项式系数。

$f(t)= p(t)=p_0 + p_1t +p_2t^2 + p_3t^3+ p_4t^4+ p_5t^5 + p_6t^6 +p_7t^7$