Trajectory planning for automatic machines and robots
- 2 Analytic Expressions of Elementary Trajectories 解析轨迹的基本表达式
- 2.1 Polynomial Trajectories 多项式轨迹
- 2.1.1 Linear trajectory (constant velocity) 线性轨迹（连续速度）
- 2.1.2 Parabolic trajectory (constant acceleration) 抛物线轨迹（连续加速）
- 2.1.3 Trajectory with asymmetric constant acceleration 非对称加速轨迹
- 2.1.4 Cubic trajectory 立方曲线轨迹（三次）
- 2.1.5 Polynomial of degree five 五次多项式
- 2.1.6 Polynomial of degree seven 七次多项式
- 2.1.7 Polynomial of higher degree 高次项式
- 2.2 Trigonometric Trajectory 三角轨迹
- 2.3 Exponential Trajectory 指数曲线
- 2.4 Trajectories Based on the Fourier Series Expansion 傅里叶展开轨迹
- 部分总结
2 Analytic Expressions of Elementary Trajectories 解析轨迹的基本表达式
2.1 Polynomial Trajectories 多项式轨迹
多项式的次数n取决于要满足的条件的数量和结果运动的期望的“平滑性”。由于边界条件的数目通常是偶数，因此多项式函数的n度是奇数，三、五、七，等等。
简单的，可通过一个多项式方程来解决
q
(
t
)
=
a
0
+
a
1
t
+
a
2
t
2
+
.
.
.
+
a
n
t
n
q(t)=a_0+a_1t+a_2t^2+...+a_nt^n
q(t)=a0+a1t+a2t2+...+antn
给定
t
j
t_j
tj对于特定
q
k
q_k
qk来说，条件约束的计算如下：（就是求k次导）
k
!
a
k
+
(
k
+
1
)
!
a
k
+
1
t
j
+
.
.
.
+
n
!
(
n
−
k
)
!
a
n
t
j
n
−
k
=
q
(
k
)
(
t
j
)
k!a_k+(k+1)!a_{k+1}t_j+...+\frac{n!}{(n-k)!}a_nt_j^{n-k}=q^(k)(t_j)
k!ak+(k+1)!ak+1tj+...+(n−k)!n!antjn−k=q(k)(tj)
用矩阵表示就是：
M
a
=
b
Ma=b
Ma=b
M是n+1阶的矩阵，b是需满足的给定条件向量，a是未知参数向量。可以通过求M的逆求解出a向量。
2.1.1 Linear trajectory (constant velocity) 线性轨迹（连续速度）
最简单的轨迹
q
(
t
)
=
a
0
+
a
1
(
t
−
t
0
)
q(t)=a_0+a_1(t-t_0)
q(t)=a0+a1(t−t0)
已知条件
q
(
t
0
)
=
a
0
q(t_0)=a_0
q(t0)=a0,也可以转化成矩阵方式解决，加速度如下所示
q
˙
(
t
)
=
h
T
(
=
a
1
)
\dot{q}(t)=\frac{h}{T}(=a_1)
q˙(t)=Th(=a1)
2.1.2 Parabolic trajectory (constant acceleration) 抛物线轨迹（连续加速）
两个二次多项式线组合，
t
0
t_0
t0到
t
f
t_f
tf和
t
f
t_f
tf到
t
1
t_1
t1,令
t
f
=
t
0
+
t
1
2
t_f=\frac{t_0+t_1}{2}
tf=2t0+t1
q
a
(
t
)
=
a
0
+
a
1
(
t
−
t
0
)
+
a
2
(
t
−
t
0
)
2
，
t
∈
[
t
0
,
t
f
]
q_a(t)=a_0+a_1(t-t_0)+a_2(t-t_0)^2，t\in [t_0,t_f]
qa(t)=a0+a1(t−t0)+a2(t−t0)2，t∈[t0,tf]
通过已知的条件——
q
(
t
f
)
=
q
f
=
q
0
+
q
1
2
，
T
a
=
(
t
f
−
t
0
)
=
T
/
2
，
(
q
f
−
q
0
)
=
h
/
2
q(t_f)=q_f=\frac{q_0+q_1}{2}，T_a=(t_f-t_0)=T/2，(q_f-q_0)=h/2
q(tf)=qf=2q0+q1，Ta=(tf−t0)=T/2，(qf−q0)=h/2等，解方程得求解未知参数
{
a
0
=
q
0
a
1
=
v
0
a
2
=
2
h
T
−
v
0
\left\{\begin{matrix} a_0= q_0 \\ a_1=v_0 \\ a_2=2\frac{h}{T}-v_0 \end{matrix}\right.
⎩⎨⎧a0=q0a1=v0a2=2Th−v0
解导数方程得
{
q
a
(
t
)
=
q
0
+
v
0
(
t
−
t
0
)
+
2
T
2
(
h
−
v
0
)
(
t
−
t
0
)
2
q
˙
a
(
t
)
=
v
0
+
4
T
2
(
h
−
v
0
T
)
(
t
−
t
0
)
q
¨
a
(
t
)
=
4
T
2
(
h
−
v
0
T
)
\left\{\begin{matrix} q_a(t)=q_0+v_0(t-t_0)+\frac{2}{T^2}(h-v_0)(t-t_0)^2 \\ \dot{q}_a(t)=v_0+\frac{4}{T^2}(h-v_0T)(t-t_0) \\ \ddot{q}_a(t)=\frac{4}{T^2}(h-v_0T) \end{matrix}\right.
⎩⎨⎧qa(t)=q0+v0(t−t0)+T22(h−v0)(t−t0)2q˙a(t)=v0+T24(h−v0T)(t−t0)q¨a(t)=T24(h−v0T)
连接点的速度为
v
m
a
x
=
q
˙
a
(
t
f
)
=
2
h
T
−
v
0
v_max=\dot{q}_a(t_f)=2\frac{h}{T}-v_0
vmax=q˙a(tf)=2Th−v0
q
b
(
t
)
=
a
0
+
a
1
(
t
−
t
f
)
+
a
2
(
t
−
t
f
)
2
，
t
∈
[
t
f
,
t
1
]
q_b(t)=a_0+a_1(t-t_f)+a_2(t-t_f)^2，t\in [t_f,t_1]
qb(t)=a0+a1(t−tf)+a2(t−tf)2，t∈[tf,t1]
通过已知的条件——
q
(
t
f
)
=
q
f
=
q
0
+
q
1
2
，
T
a
=
(
t
f
−
t
0
)
=
T
/
2
，
(
q
f
−
q
0
)
=
h
/
2
q(t_f)=q_f=\frac{q_0+q_1}{2}，T_a=(t_f-t_0)=T/2，(q_f-q_0)=h/2
q(tf)=qf=2q0+q1，Ta=(tf−t0)=T/2，(qf−q0)=h/2等，解方程得求解未知参数（与上过程类似），最后得到的完整轨迹描述如下：
可以看到速度的连接点曲线是不连续的，如果在
t
f
t_f
tf上加上约束条件，对多个条件同时求解，可以得到连续的速度
{
q
a
(
t
0
)
=
a
0
=
q
0
q
˙
a
(
t
0
)
=
a
1
=
v
0
q
b
(
t
1
)
=
a
3
+
a
4
T
2
+
a
5
(
T
2
)
2
=
q
1
q
b
˙
(
t
1
)
=
a
4
+
2
a
5
T
2
=
v
1
q
a
(
t
f
)
=
a
0
+
a
1
T
2
+
a
2
(
T
2
)
2
=
a
3
=
q
b
(
t
f
)
q
˙
a
(
t
f
)
=
a
1
+
2
a
2
T
2
=
a
4
=
q
˙
b
(
t
f
)
\left\{\begin{matrix} q_a(t_0)=a_0=q_0 \\ \dot{q}_a(t_0)=a_1=v_0 \\ q_b(t_1)=a_3+a_4\frac{T}{2}+a_5(\frac{T}{2})^2=q_1 \\ \dot{q_b}(t_1)=a_4+2a_5\frac{T}{2}=v_1 \\q_a(t_f)=a_0+a_1\frac{T}{2}+a_2(\frac{T}{2 })^2=a_3=q_b(t_f) \\\dot{q}_a(t_f)=a_1+2a_2\frac{T}{2}=a_4=\dot{q}_b(t_f) \end{matrix}\right.
⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧qa(t0)=a0=q0q˙a(t0)=a1=v0qb(t1)=a3+a42T+a5(2T)2=q1qb˙(t1)=a4+2a52T=v1qa(tf)=a0+a12T+a2(2T)2=a3=qb(tf)q˙a(tf)=a1+2a22T=a4=q˙b(tf)
解导数方程得未知参数向量a的解
{
a
0
=
q
0
a
1
=
v
0
a
2
=
4
h
−
T
(
3
v
0
+
v
1
)
2
T
2
a
3
=
4
(
q
0
+
q
1
)
+
T
(
v
0
+
v
1
)
8
a
4
=
4
h
−
T
(
v
0
+
v
1
)
2
T
a
5
=
−
4
h
+
T
(
v
0
+
3
v
1
)
2
T
2
\left\{\begin{matrix} a_0=q_0 \\ a_1=v_0 \\ a_2=\frac{4h-T(3v_0+v_1)}{2T^2} \\ a_3=\frac{4(q_0+q_1)+T(v_0+v_1)}{8} \\a_4=\frac{4h-T(v_0+v_1)}{2T} \\a_5=\frac{-4h+T(v_0+3v_1)}{2T^2} \end{matrix}\right.
⎩⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎧a0=q0a1=v0a2=2T24h−T(3v0+v1)a3=84(q0+q1)+T(v0+v1)a4=2T4h−T(v0+v1)a5=2T2−4h+T(v0+3v1)
2.1.3 Trajectory with asymmetric constant acceleration 非对称加速轨迹
解法类似上面重力曲线轨迹，但是
t
f
t_f
tf不在中点上。条件方程和解方程参考上式。
2.1.4 Cubic trajectory 立方曲线轨迹（三次）
对于初始和终止两点四条件，将方程展开到三次项式进行求解，但是速度和加速度会不连续。
这边还给出了一种简单的方法来计算连续连接点速度的方法
d
k
d_k
dk是连续线段的斜率，通用于多次导下的数值计算（速度，加速度，加加速度）。
2.1.5 Polynomial of degree five 五次多项式
对于初始和终止两点六条件，将方程展开到五次项式进行求解，但是加速度会不连续。
2.1.6 Polynomial of degree seven 七次多项式
对于初始和终止两点八条件，将方程展开到七次项式进行求解。
2.1.7 Polynomial of higher degree 高次项式
正常来说，高次计算下的方程带来了更平滑的轨迹，次数随约束条件的增加而增加，计算难度也变得复杂。引入Bezier/Bernstein方法进行求解可以优化计算，但是也避免不了高次多项式上的数值抖动（引入误差导致轨迹巨幅变化）。
2.2 Trigonometric Trajectory 三角轨迹
(三角轨迹可以提供完美的平滑度方案，但个人感觉对自动驾驶帮助不大，选择略过)
2.2.1 Harmonic trajectory 谐波运动轨迹
2.2.2 Cycloidal trajectory 摆线运动轨迹
2.2.3 Elliptic trajectory 椭圆轨迹
2.3 Exponential Trajectory 指数曲线
在指数曲线上可以直接引入平滑度参数对平滑程度进行约束，
q
˙
(
τ
)
=
v
c
e
−
σ
f
(
τ
,
λ
)
\dot{q}(\tau)=v_ce^{-\sigma{}f(\tau,\lambda)}
q˙(τ)=vce−σf(τ,λ)
f
f
f函数通常有两种不同的选择，见书，但对实际影响不大。
v
c
v_c
vc通常是个常数，可以用
σ
,
f
\sigma,f
σ,f函数求出。对某些σ值和具有最小化残余谱的相应λ所获得的
v
c
v_c
vc数值，当
λ
=
0.20
，
σ
=
7.1
\lambda=0.20，σ=7.1
λ=0.20，σ=7.1，获得最小值
v
c
=
0.018
v_c=0.018
vc=0.018。
然后对指数函数求至二阶导
而不同σ，λ数值对，对于轨迹的影响具体也是体现在曲线变化上，而不会对曲线产生巨幅变化。
2.4 Trajectories Based on the Fourier Series Expansion 傅里叶展开轨迹
傅里叶具体可以看到附录D，因为轨迹往往可以看作是周期性的运动不断地循环往复。
轨迹中往往希望减小加速度轮廓的振幅，以避免由于惯性力或机械结构的振动效应对载荷的影响。
一般来说，加速度振幅的最小化与剖面的连续性形成对比：不连续的加速度剖面使加速度的峰值最小化，但另一方面，由于惯性力的相关不连续，可能会产生振荡或振动。
傅里叶级数是一种数学工具，通常用于分析周期函数，将它们分解为正弦分量函数的加权和，有时称为正常傅里叶模，或简单模。
但是考虑车辆运动轨迹，它并不是一种具有周期性的运动。类似三角曲线，个人感觉基于这一点就很难应用在车辆轨迹上。（后面就不详细展开）
2.4.1 Gutman 1-3 古特曼 1-3项展开
2.4.2 Freudenstein 1-3 弗兰登斯坦 1-3项展开
2.4.3 Freudenstein 1-3-5 弗兰登斯坦 1-3-5项展开
部分总结
这边介绍的轨迹都是基于点到点的轨迹，处理多点就是在中间加上相应的点，然后在具体解方程中增加维度。目前看到的Apollo用的是多项式的方式，比较简单有效率的处理方法。
460
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
