多项式轨迹--直线和抛物线轨迹

多项式轨迹–直线和抛物线轨迹

1.1 polynomial function

大多数简单情况，可定义初始时刻${{t}_{0}}$和终止时刻${{t}_{1}}$ ,以及位置、速度和加速度条件，确定运动。从数学的角度，问题是找到函数

$$q=q\left(t \right),t\in \left[ {{t}_{0}},{{t}_{1}} \right] \tag{1-1}$$
满足给定条件。该问题可考虑找到一个多项式
$$q\left(t \right)={{a}_{0}}+{{a}_{1}}t+{{a}_{2}}{{t}^{2}}+\cdots +{{a}_{n}}{{t}^{n}} \tag{1-2}$$
这里确定 $n+1$ 个系数 ${{a}_{i}}$ 使得初始和终止约束条件都满足。多项式的自由度 $n$ 取决于约束条件的个数和运动期望的“平滑度”。多项式的自由度通常是奇数的，例如3,5,7等。

除了轨迹的初始和结束条件，同时可以指定一般时刻${{t}_{j}}\in \left[ {{t}_{0}},{{t}_{1}} \right]$ 关于时间的微分（速度，加速度，加加速度，…）。这些条件可以指定为$q\left( t \right)$ 关于时间的$k$ 阶微分${{q}^{\left( k \right)}}\left({{t}_{j}} \right)$

$$k!{{a}_{k}}+\left(k+1 \right)!{{a}_{k+1}}{{t}{j}}+\cdots +\frac{n!}{\left( n-k\right)!}{{a}_{n}}t_{j}^{n-k}={{q}^{(k)}}\left( {{t}_{j}} \right) \tag{1-3}$$
矩阵形式可以表述为
$$\mathbf{Ma}=\mathbf{b} \tag{1-4}$$
其中 $\mathbf{M}$ 已知的 $\left( n+1 \right)\times \left( n+1 \right)$ 的矩阵， $\mathbf{b}$ 为给定的 $\left( n+1 \right)$ 需要满足的条件。 $\mathbf{a}={{\left[ {{a}_{0}},{{a}_{1}},...,{{a}_{n}} \right]}^{T}}$是需要计算的未知参数构成的向量。方程的解为

​

$$\mathbf{a}={{\mathbf{M}}^{\mathbf{-1}}}\mathbf{b}\tag{1-5}$$

1.2 Linear trajectory

最简单的点${{q}_{0}}$到${{q}_{1}}$ 的运动，定义为

$$q\left(t \right)={{a}_{0}}+{{a}{1}}\left( t-{{t}_{0}} \right) \tag{1-6}$$
初始时刻和结束时刻分别为 ${{t}_{0}},{{t}_{1}}$，则满足初始条件和结束条件为
$$\begin{align} & q\left( {{t}_{0}} \right)={{q}_{0}} \\ & q\left( {{t}_{1}} \right)={{q}_{1}} \\ \end{align} \tag{1-7}$$
即
$$\left[ \begin{matrix} 1 & 0 \\ 1 & T \\ \end{matrix} \right]\ \ \left[ \begin{matrix} {{a}_{0}} \\ {{a}_{1}} \\ \end{matrix} \right]=\left[ \begin{matrix} {{q}_{0}} \\ {{q}_{1}} \\ \end{matrix} \right] \tag{1-8}$$
这里 $T={{t}_{1}}-{{t}_{0}}$是运动时间。因此解得
$$\begin{cases} \begin{matrix} {{a}_{0}}&=&{{q}_{0}} \\ {{a}_{1}}&=&\frac{{{q}_{1}}-{{q}_{0}}}{{{t}_{1}}-{{t}_{0}}}=\frac{h}{T} \\ \end{matrix} \end{cases} \tag{1-9}$$
这里 $h={{q}_{1}}-{{q}_{0}}$ 是位移。在时间间隔 $\left[{{t}_{0}},{{t}_{1}} \right]$内速度恒定，即
$$\dot{q}\left(t \right)=\frac{h}{T} \tag{1-10}$$

1.3 Parabolic trajectory

图 1 关于拐点对称的抛物线轨迹

该轨迹被广泛称为重力轨迹或加速度恒定轨迹。如下图所示，假设拐点时刻为${{t}_{f}}$ 。不妨首先考虑关于拐点对称的情形。运动轨迹可定义为${{t}_{f}}=\frac{{{t}_{0}}+{{t}_{1}}}{2}$ ，$q\left( {{t}_{f}}\right)={{q}_{f}}=\frac{{{q}_{0}}+{{q}_{1}}}{2}$ 。注意到，${{T}_{a}}=\left( {{t}_{f}}-{{t}_{0}} \right)=T/2,\left({{q}_{f}}-{{q}_{0}} \right)=h/2$ 。

第一段，加速段。轨迹定义为

$${{q}_{a}}\left(t \right)={{a}_{0}}+{{a}_{1}}\left( t-{{t}_{0}} \right)+{{a}_{2}}{{\left(t-{{t}_{0}} \right)}^{2}},t\in \left[ {{t}_{0}},{{t}_{f}} \right] \tag{1-11}$$

参数由位置和速度条件计算

$$\begin{cases} \begin{matrix} {{q}_{a}}\left( {{t}_{0}} \right)&=&{{q}_{0}}={{a}_{0}} \\ {{q}_{a}}\left( {{t}_{f}} \right)&=&{{q}_{f}}={{a}_{0}}+{{a}_{1}}\left({{t}_{f}}-{{t}_{0}} \right)+{{a}_{2}}{{\left( {{t}_{f}}-{{t}_{0}}\right)}^{2}} \\ {{{\dot{q}}}_{a}}\left( {{t}_{0}} \right)&=&{{v}_{0}}={{a}_{1}}. \\ \end{matrix} \end{cases} \tag{1-12}$$
有上述方程可求得
$${a}_{0}={{q}_{0}},{{a}_{1}}={{v}_{0}},{{a}_{2}}=\frac{2}{{{T}^{2}}}\left(h-{{v}_{0}}T \right). \tag{1-13}$$

因此，对$t\in \left[{{t}_{0}},{{t}_{f}} \right]$ ，轨迹定义为

$$\begin{cases} \begin{matrix} {{q}_{a}}\left( t \right)&=&{{q}_{0}}+{{v}_{0}}\left( t-{{t}_{0}}\right)+\frac{2}{{{T}^{2}}}\left( h-{{v}_{0}}T \right){{\left( t-{{t}_{0}}\right)}^{2}} \\ {{{\dot{q}}}_{a}}\left( t \right)&=&{{v}_{0}}+\frac{4}{{{T}^{2}}}\left(h-{{v}_{0}}T \right)\left( t-{{t}_{0}} \right) \\ {{{\ddot{q}}}_{a}}\left( t \right)&=&\frac{4}{{{T}^{2}}}\left(h-{{v}_{0}}T \right) \\ \end{matrix} \end{cases} \tag{1-14}$$

拐点速度为

$${{v}_{\max}}={{\dot{q}}_{a}}\left( {{t}_{f}} \right)=2\frac{h}{T}-{{v}_{0}}.$$

第二段，减速段。同理，根据位置和速度条件，可求得对于$t\in \left[ {{t}_{f}},{{t}_{1}} \right]$ 的轨迹表达式

$$\begin{cases} \begin{matrix} {{q}_{b}}\left( t \right)&=&{{q}_{f}}+\left( 2\frac{h}{T}-{{v}_{1}}\right)\left( t-{{t}_{f}} \right)+\frac{2}{{{T}^{2}}}\left( {{v}_{1}}T-h\right){{\left( t-{{t}_{f}} \right)}^{2}} \\ {{{\dot{q}}}_{b}}\left( t \right)&=&2\frac{h}{T}-{{v}_{1}}+\frac{4}{{{T}^{2}}}\left({{v}_{1}}T-h \right)\left( t-{{t}_{f}} \right) \\ {{{\ddot{q}}}_{b}}\left( t \right)&=&\frac{4}{{{T}^{2}}}\left({{v}_{1}}T-h \right). \\ \end{matrix} \end{cases} \tag{1-15}$$

1.3.1 Trajectory with asymmetric constant acceleration

​ 图 2 具有对称恒定加速度的抛物线轨迹

考虑更一般的情况，拐点不一定在时间中点时刻。这时轨迹可定义为两个多项式

$${{q}_{a}}\left(t \right)={{a}_{0}}+{{a}_{1}}\left( t-{{t}_{0}} \right)+{{a}_{2}}{{\left(t-{{t}_{0}} \right)}^{2}},{{t}_{0}}\le t<{{t}_{f}} \tag{1-17}$$

$${{q}_{b}}\left(t \right)={{a}_{3}}+{{a}_{4}}\left( t-{{t}_{f}} \right)+{{a}_{5}}{{\left(t-{{t}_{f}} \right)}^{2}},{{t}_{f}}\le t<{{t}_{1}} \tag{1-18}$$

根据位置和速度在起始和结束时刻的四个条件，以及位置和速度两个连续性条件可得

$$\begin{cases} \begin{matrix} {{q}_{a}}\left( {{t}_{0}} \right)&=&{{a}_{0}}&=&{{q}_{0}} \\ {{q}_{b}}\left( {{t}_{1}} \right)&=&{{a}_{3}}+{{a}_{4}}\left({{t}_{1}}-{{t}_{f}} \right)+{{a}_{5}}{{\left( {{t}_{1}}-{{t}_{f}}\right)}^{2}}&=&{{q}_{1}} \\ {{{\dot{q}}}_{a}}\left( {{t}_{0}} \right)&=&{{a}_{1}}&=&{{v}_{0}} \\ {{{\dot{q}}}_{b}}\left( {{t}_{1}} \right)&=&{{a}_{4}}+2{{a}_{5}}\left({{t}_{1}}-{{t}_{f}} \right)&=&{{v}_{1}} \\ {{q}_{a}}\left( {{t}_{f}} \right)&=&{{a}_{0}}+{{a}_{1}}\left({{t}_{f}}-{{t}_{0}} \right)+{{a}_{2}}{{\left( {{t}_{f}}-{{t}_{0}}\right)}^{2}}&=&{{a}_{3}}\left( ={{q}_{b}}\left( {{t}_{f}} \right) \right) \\ {{{\dot{q}}}_{a}}\left( {{t}_{f}} \right)&=&{{a}_{1}}+2{{a}_{2}}\left({{t}_{f}}-{{t}{0}} \right)&=&{{a}{4}}\left( ={{{\dot{q}}}_{b}}\left( {{t}_{f}}\right) \right) \\ \end{matrix} \end{cases} \tag{1-19}$$
再定义 ${{T}_{a}}=\left({{t}_{f}}-{{t}_{0}} \right),{{T}_{b}}=\left( {{t}_{1}}-{{t}_{f}} \right)$ ，求得多项式系数为
$$\begin{cases} \begin{matrix} {{a}_{0}}&=&{{q}_{0}} \\ {{a}_{1}}&=&{{v}_{0}} \\ {{a}_{2}}&=&\frac{2h-{{v}_{0}}\left( T+{{T}_{a}}\right)-{{v}_{1}}{{T}_{d}}}{2T{{T}_{d}}} \\ {{a}_{3}}&=&\frac{2{{q}_{1}}{{T}_{a}}+{{T}_{d}}\left(2{{q}_{0}}+{{T}_{a}}\left( {{v}_{0}}-{{v}_{1}} \right) \right)}{2T} \\ {{a}_{4}}&=&\frac{2h-{{v}_{0}}{{T}_{a}}-{{v}_{1}}{{T}_{d}}}{T} \\ {{a}_{5}}&=&-\frac{2h-{{v}_{0}}{{T}_{a}}-{{v}_{1}}\left( T+{{T}_{d}}\right)}{2T{{T}_{d}}} \\ \end{matrix} \end{cases} \tag{1-20}$$
速度和加速度可由位移轨迹多项式求导得到。

欢迎指正，留言交流。

---

参考文献

Biagiotti L, Melchiorri C. Trajectory Planning for Automatic Machines and Robots[M]. Springer Berlin Heidelberg, 2009.