五次多项式轨迹生成

最新推荐文章于 2024-07-24 12:00:21 发布

zhf98@

最新推荐文章于 2024-07-24 12:00:21 发布

阅读量922

点赞数 9

分类专栏：运动规划文章标签：机器人算法

本文链接：https://blog.csdn.net/qq_43756951/article/details/139728215

版权

运动规划专栏收录该内容

2 篇文章 0 订阅

订阅专栏

前言

三次多项式轨迹虽然在速度上连续，但加速度却不连续，为了使电机在加速度上也获得连续，因此除了首尾位置和速度的约束，还需要对首尾的加速度进行约束。因此共有六个约束条件：起始位置 $s_0$ 、结束位置 $s_f$ 、起始速度 $\dot{s}_0$ 、结束速度 $\dot{s}_f$ 、起始加速度 $\ddot{s}_0$ 、结束加速度 $\ddot{s}_f$

则可以用五次多项式来表示轨迹：
$s(t)=a_{0}+a_{1} \, t+a_{2} \, t^{2}+a_{3} \, t^{3} + a_{4} \, t^{4} + a_{5} \, t^{5} \tag{1}$

求解轨迹方程未知参数

带入约束条件，其中 $t_0 = 0$
$\begin{align} s_{0}=\,& a_{0}\\ s_{f}=\,& a_{0}+a_{1} \, t_{f}+a_{2} \, t_{f}^{2}+a_{3} \, t_{f}^{3}+ a_{4} \, t_{f}^{4} + a_{5} \, t_{f}^{5} \\ \dot{s}_{0}=\,& a_{1}\\ \dot{s}_{f}=\,& a_{1}+2 a_{2} \, t_{f}+3 a_{3} \, t_{f}^{2}+ 4a_{4} \, t_{f}^{3} + 5a_{5} \, t_{f}^{4} \\ \ddot{s}_{0}=\,& 2a_{2}\\ \ddot{s}_{f}=\,& 2a_{2}+6a_{3} \, t + 12a_{4} \, t_{f}^{2} + 20a_{5} \, t_{f}^{3} \end{align} \tag{2}$
式(2)可转化为矩阵形式
$\left[\begin{array}{c}s_{0} \\ s_{f} \\ \dot{s}_{0} \\ \dot{s}_{f} \\ \ddot{s}_{0} \\ \ddot{s}_{f}\end{array}\right] =\left[\begin{array}{cccc} 1 & 0 & 0 & 0 & 0 & 0\\ 1 & t_f & t_{f}^{2} & t_{f}^{3} & t_{f}^{4} & t_{f}^{5}\\ 0 & 1 & 0 & 0 & 0 & 0\\ 0 & 1 & 2 t_f & 3 t_{f}^{2} & 4 t_{f}^{3} & 5 t_{f}^{4}\\ 0 & 0 & 2 & 0 & 0 & 0\\ 0 & 0 & 2 & 6 t_f & 12 t_{f}^{2} & 20 t_{f}^{3}\end{array}\right] \left[\begin{array}{l} a_{0} \\ a_{1} \\ a_{2} \\ a_{3} \\ a_{4} \\ a_{5}\end{array}\right] \tag{3}$
求解矩阵方程组（当 $\Delta t \neq 0$ 时， $det(T_{6 \times 6}) \neq 0$ ）
$\left[ \begin{array}{l} a_0\\ a_1\\ a_2\\ a_3\\ a_4\\ a_5\\ \end{array} \right] =\left[ \begin{matrix} 1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& \frac{1}{2}& 0\\ -\frac{10}{t_{f}^{3}}& \frac{10}{t_{f}^{3}}& -\frac{6}{t_{f}^{2}}& -\frac{4}{t_{f}^{2}}& -\frac{3}{2t_f}& \frac{1}{2t_f}\\ \frac{15}{t_{f}^{4}}& -\frac{15}{t_{f}^{4}}& \frac{8}{t_{f}^{3}}& \frac{7}{t_{f}^{3}}& \frac{3}{2t_{f}^{2}}& -\frac{1}{t_{f}^{2}}\\ -\frac{6}{t_{f}^{5}}& \frac{6}{t_{f}^{5}}& -\frac{3}{t_{f}^{4}}& -\frac{3}{t_{f}^{4}}& \frac{1}{2t_{f}^{3}}& \frac{1}{2t_{f}^{3}}\\ \end{matrix} \right] \left[ \begin{array}{c} s_0\\ s_f\\ \dot{s}_0\\ \dot{s}_f\\ \ddot{s}_0\\ \ddot{s}_f\\ \end{array} \right] \tag{4}$
式(4)也可转化为代数形式
$\left\{ \begin{array}{l} a_0=s_0\\ a_1=\dot{s}_0\\ a_2=\frac{\ddot{s}_0}{2}\\ a_3=\frac{20s_f-20s_0-\left( 8\dot{s}_f+12\dot{s}_0 \right) t_f-\left( 3\ddot{s}_0-\ddot{s}_f \right) t_{f}^{2}}{2t_{f}^{3}}\\ a_4=\frac{30s_0-30s_f+\left( 14\dot{s}_f+16\dot{s}_0 \right) t_f+\left( 3\ddot{s}_0-2\ddot{s}_f \right) t_{f}^{2}}{2t_{f}^{4}}\\ a_5=\frac{12s_f-12s_0-\left( 6\dot{s}_f+6\dot{s}_0 \right) t_f-\left( \ddot{s}_0-\ddot{s}_f \right) t_{f}^{2}}{2t_{f}^{5}}\\ \end{array} \right. \tag{5}$

输出轨迹方程

$\left\{ \begin{array}{l} s\left( t \right) =\,a_0+a_1\,t+a_2\,t^2+a_3\,t^3+a_4\,t^4+a_5\,t^5\\ \dot{s}\left( t \right) =\,a_1+2a_2\,t+3a_3\,t^2+4a_4\,t^3+5a_5\,t^4\\ \ddot{s}\left( t \right) =\,2a_2+6a_3\,t+12a_4\,t^2+20a_5\,t^3\\ \end{array} \right. \tag{6}$

总结

多项式轨迹曲线都没办法控制匀速，且次数越高，加速时间越长，平均速度低

zhf98@

关注

9
点赞
踩
13

收藏

觉得还不错? 一键收藏
0
评论
五次多项式轨迹生成

三次多项式轨迹虽然在速度上连续，但加速度却不连续，为了使电机在加速度上也获得连续，因此除了首尾位置和速度的约束，还需要对首尾的加速度进行约束。因此共有六个约束条件：起始位置s0s_0s0、结束位置sfs_fsf、起始速度s˙0\dot{s}_0s˙0、结束速度s˙f\dot{s}_fs˙f、起始加速度s¨0\ddot{s}_0s¨0、结束加速度s¨f\ddot{s}_fs¨fsta0a1ta2t。
复制链接

扫一扫

专栏目录