轨迹生成——三次多项式

最新推荐文章于 2024-07-26 10:26:06 发布

zhf98@

最新推荐文章于 2024-07-26 10:26:06 发布

阅读量903

点赞数 19

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

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

版权

运动规划专栏收录该内容

2 篇文章 0 订阅

订阅专栏

前言

一段平滑的运动轨迹，至少要求位移曲线平滑、速度曲线连续，即至少需要满足四个约束条件：起始位置 $s_0$ 、结束位置 $s_f$ 、起始速度 $\dot{s}_0$ 、结束速度 $\dot{s}_f$

则可以用三次多项式来表示轨迹：
$s(t)=a_{0}+a_{1} \, t+a_{2} \, t^{2}+a_{3} \, t^{3} \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}\\ \dot{s}_{0}=\, & a_{1}\\ \dot{s}_{f}=\, & a_{1}+2 a_{2} \, t_{f}+3 a_{3} \, t_{f}^{2} \end{align} \tag{2}$
式(2)可转化为矩阵形式
$\left[\begin{array}{c}s_{0} \\ s_{f} \\ \dot{s}_{0} \\ \dot{s}_{f}\end{array}\right]=\left[\begin{array}{cccc}1 & 0 & 0 & 0 \\ 1 & t_f & t_{f}^{2} & t_{f}^{3} \\ 0 & 1 & 0 & 0 \\ 0 & 1 & 2 t_f & 3 t_{f}^{2}\end{array}\right]\left[\begin{array}{l}a_{0} \\ a_{1} \\ a_{2} \\ a_{3}\end{array}\right] \tag{3}$
求解矩阵方程组（当 $t_f \neq 0$ 时， $det(T_{4 \times 4}) \neq 0$ ）
$\left[\begin{array}{l}a_{0} \\ a_{1} \\ a_{2} \\ a_{3}\end{array}\right]=\left[\begin{array}{cccc}1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ -\frac{3}{t_{f}^{2}} & \frac{3}{t_{f}^{2}} & -\frac{2}{t_{f}} & -\frac{1}{t_{f}} \\ \frac{2}{ t_{f}^{3}} & -\frac{2}{t_{f}^{2}} & \frac{1}{t_{f}^{2}} & \frac{1}{t_{f}^{2}}\end{array}\right]\left[\begin{array}{c}s_{0} \\ s_{f} \\ \dot{s}_{0} \\ \dot{s}_{f}\end{array}\right] \Large \tag{4}$
式(4)也可转化为代数形式
$\left\{ \begin{array}{l} a_0=s_0\\ a_1=0\\ a_3=\frac{3}{t_{f}^{2}}\left( s_f-s_0 \right)\\ a_4=-\frac{2}{t_{f}^{2}}\left( s_f-s_0 \right)\\ \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\\ \dot{s}\left( t \right) =a_1+2a_2 \, t+3a_3 \, t^2\\ \ddot{s}\left( t \right) =2a_2+6a_3 \, t\\ \end{array} \right. \tag{6}$