机器人轨迹
这一系列轨迹教程将主要包括以下内容:
-
点到点轨迹(P2P)
-
在线轨迹规划
- 多项式在线轨迹规划
- 梯形在线轨迹规划
- 双S形在线轨迹规划
- 非线性实时轨迹滤波
-
多点轨迹(Multi-point)
- 三次样条曲线(cubic spline)
- 贝赛尔曲线(Bezier Curve)
- B样条曲线(BSpline)
-
时间最优轨迹
- 三次样条时间最优轨迹
- 任意路径下的时间最优轨迹
- 时间最优停止轨迹
- 不约束路径
- 有路径约束
文章目录
梯形速度轨迹
梯形速度轨迹表达式与计算
梯形速度轨迹分为三段,分别为加速段、匀速段、减速段,给定初始的 q 0 q_0 q0、 v 0 v_0 v0以及加速段的加速度 a u a_u au和加速段时间 T u T_u Tu,匀速段的时间 T c T_c Tc,减速段的加速度 a d a_d ad和 T d T_d Td,就可以写出整个梯形轨迹的表达式。
以下参数是必须给出的:
t 0 : 起 始 时 间 戳 , 一 般 为 0 q 0 : 起 始 位 置 v 0 : 起 始 速 度 t 1 : 起 始 时 间 戳 q 1 : 结 束 位 置 v 1 : 结 束 速 度 (1) \begin{aligned} t_0 &: 起始时间戳,一般为0\\ q_0 &: 起始位置 \\ v_0 &:起始速度 \\ t_1 &: 起始时间戳\\ q_1 &: 结束位置 \\ v_1 &:结束速度 \end{aligned} \tag{1} t0q0v0t1q1v1:起始时间戳,一般为0:起始位置:起始速度:起始时间戳:结束位置:结束速度(1)
同时每段轨迹都需要知道时间长度和加速度:
a u : 加 速 段 加 速 度 a d : 减 速 段 加 速 度 T u : 加 速 段 的 时 间 长 度 T c : 匀 速 段 的 时 间 长 度 T d : 减 速 段 的 时 间 长 度 v c : 匀 速 段 的 速 度 (2) \begin{aligned}a_u &: 加速段加速度\\a_d &: 减速段加速度\\T_u &: 加速段的时间长度\\T_c &: 匀速段的时间长度\\T_d &: 减速段的时间长度\\v_c &: 匀速段的速度\end{aligned} \tag{2} auadTuTcTdvc:加速段加速度:减速段加速度:加速段的时间长度:匀速段的时间长度:减速段的时间长度:匀速段的速度(2)
如果知道(2)中的5个参数,那么就可以写出梯形轨迹的表达式:
位置:
T = t 1 − t 0 T c = T − T u − T d t ^ = t − t 0 v c = v 0 + a u T u v c = v 0 + a u T u ⟹ a u = v c − v 0 T u v 1 = v c + a d T d ⟹ a d = v 1 − v c T d q c 0 = q 0 + v 0 + v c 2 T u q d 0 = q 1 − v c + v 1 2 ( T − T u − T d ) q ( t ) = { q 0 + v 0 t ^ + v c − v 0 2 T u t ^ 2 t ^ ∈ [ 0 , T u ] q c 0 + v c ( t ^ − T u ) t ^ ∈ [ T u , T − T d ] q d 0 + v c ( t ^ − T u − T c ) + 1 2 a d ( t ^ − T u − T c ) 2 t ^ ∈ [ T − T d , T ] = { q 0 + v 0 t ^ + v c − v 0 2 T u t ^ 2 t ^ ∈ [ 0 , T u ] q 0 + v 0 − v c 2 T u + v c t ^ t ^ ∈ [ T u , T − T d ] q 1 − v 1 ( T − t ^ ) − v c − v 1 2 T d ( T − t ^ ) 2 t ^ ∈ [ T − T d , T ] (3) \begin{aligned}T &= t_1 - t_0\\T_c &= T - T_u - T_d\\\hat t &= t - t_0 \\v_{c} &= v_0 + a_uT_u \\v_c &= v_0 + a_uT_u \implies a_u = \frac{v_c-v_0}{T_u}\\v_1 &= v_c + a_dT_d \implies a_d = \frac{v_1-v_c}{T_d}\\q_{c0} &= q_0 + \frac{v_0+v_c}{2}T_u\\q_{d0} &= q_1 - \frac{v_c+v_1}{2}(T-T_u-T_d) \\[3 ex]q(t)&=\begin{cases}q_0 + v_0\hat t + \frac{v_c-v_0}{2T_u}\hat t^2 & &{ \hat t \in [0, T_u]} \\q_{c0} + v_c(\hat t-T_u) & &{\hat t \in [T_u, T - T_d]}\\q_{d0} + v_c(\hat t-T_u-T_c) + \frac{1}{2}a_d(\hat t-T_u-T_c)^2& &{\hat t \in [T - T_d, T]}\end{cases}\\[3ex]&=\begin{cases}q_0 + v_0\hat t + \frac{v_c-v_0}{2T_u}\hat t^2 & &{ \hat t \in [0, T_u]} \\q_0 + \frac{v_0-v_c}{2}T_u + v_c\hat t & &{\hat t \in [T_u, T - T_d]}\\q_1 - v_1(T-\hat t) - \frac {v_c-v_1}{2T_d} (T-\hat t)^2 & &{\hat t \in [T - T_d, T]}\end{cases}\end{aligned} \tag{3} TTct^vcvcv1qc0qd0q(t)=t1−t0=T−Tu−Td=t−t0=v0+auTu=v0+auTu⟹au=Tuvc−v0=vc+adTd⟹ad=Tdv1−vc=q0+2v0+vcTu=q1−2vc+v1(T−Tu−Td)=⎩⎪⎨⎪⎧q0+v0t^+2Tuvc−v0t^2qc0+vc(t^−Tu)qd0+vc(t^−Tu−Tc)+21ad(t^−Tu−Tc)2t^∈[0,Tu]t^∈[Tu,T−Td]t^∈[T−Td,T]=⎩⎪⎨⎪⎧q0+v0t^+2Tuvc−v0t^2q0+2v0−vcTu+vct^q1−v1(T−t^)−2Tdvc−v1(T−t^)2t^∈[0,Tu]t^∈[Tu,T−Td]t^∈[T−Td,T](3)
(3)经过化简得到的表达式和 v c v_c vc、 T u T_u Tu、 T d T_d Td有关系,也就是匀速段的速度值和加速、减速时间长度,一般加速、减速时间也经常设置为相等的,且 v 0 v_0 v0、 v 1 v_1 v1常设置为0,这里给出通用的表达式。
求一阶导数可以得到速度:
q ˙ ( t ) = { v 0 + a u t ^ t ^ ∈ [ 0 , T u ] v c t ^ ∈ [ T u , T u + T c ] v c + a d ( t ^ − T u − T c ) t ^ ∈ [ T u + T c , T u + T c + T d ] = { v 0 + v c − v 0 T u t ^ t ^ ∈ [ 0 , T u ] v c t ^ ∈ [ T u , T − T d ] v 1 − v 1 − v c T d ( T − t ^ ) t ^ ∈ [ T − T d , T ] (4) \begin{aligned}\dot q(t)&=\begin{cases}v_0 + a_u\hat t & &{ \hat t \in [0, T_u]} \\v_c & &{\hat t \in [T_u, T_u + T_c]}\\v_c + a_d(\hat t-T_u-T_c)& &{\hat t \in [T_u + T_c, T_u + T_c + T_d]}\end{cases}\\[4ex]&=\begin{cases}v_0 + \frac{v_c-v_0}{T_u}\hat t & &{ \hat t \in [0, T_u]} \\v_c & &{\hat t \in [T_u, T - T_d]}\\v_1 - \frac{v_1-v_c}{T_d}(T-\hat t)& &{\hat t \in [T - T_d, T]}\end{cases}\end{aligned} \tag{4} q˙(t)=⎩⎪⎨⎪⎧v0+aut^vcvc+ad(t^−Tu−Tc)t^∈[0,Tu]t^∈[Tu,Tu+Tc]t^∈[Tu+Tc,Tu+Tc+Td]=⎩⎪⎨⎪⎧v0+Tuvc−v0t^vcv1−Tdv1−vc(T−t^)t^∈[0,Tu]t^∈[Tu,T−Td]t^∈[T−Td,T](4)
求二阶导数可以得到加速度:
q ¨ ( t ) = { a u t ^ ∈ [ 0 , T u ] 0 t ^ ∈ [ T u , T u + T c ] a d t ^ ∈ [ T u + T c , T u + T c + T d ] = { v c − v 0 T u t ^ ∈ [ 0 , T u ] 0 t ^ ∈ [ T u , T − T d ] v 1 − v c T d t ^ ∈ [ T − T d , T ] (5) \begin{aligned}\ddot q(t)&=\begin{cases}a_u & &{ \hat t \in [0, T_u]} \\0 & &{\hat t \in [T_u, T_u + T_c]}\\a_d & &{\hat t \in [T_u + T_c, T_u + T_c + T_d]}\end{cases}\\[4ex]&=\begin{cases}\frac{v_c-v_0}{T_u} & &{ \hat t \in [0, T_u]} \\0 & &{\hat t \in [T_u, T - T_d]}\\\frac{v_1-v_c}{T_d} & &{\hat t \in [T - T_d, T]}\end{cases}\end{aligned} \tag{5} q¨(t)=⎩⎪⎨⎪⎧au0adt^∈[0,Tu]t^∈[Tu,Tu+Tc]t^∈[Tu+Tc,Tu+Tc+Td]=⎩⎪⎨⎪⎧Tuvc−v00Tdv1−vct^∈[0,Tu]t^∈[Tu,T−Td]t^∈[T−Td,T](5)
特点:
- 速度连续,是保证速度连续的情况下,轨迹时间最短的轨迹,由三段构成,是抛物线过度加上线性轨迹
- 可以定义起始和结束的位置、速度
- 需要额外指定一些参数
计算
给定(1)中的参数以及(2)中的几个参数,根据(3)就可以构造出梯形轨迹。一般指定形式包括:
- 给定匀速段的速度 v c v_c vc,加速时间 T u T_{u} Tu,减速时间 T d T_d Td ,可以直接根据(3)写出表达式
- 给定匀速段的速度 v c v_c vc,加速度 a u a_u au、 a d a_d ad,此时也很容易根据(3)求出表达式
代码实现
这里用python来实现梯形轨迹插值:
未完待续。。。
#!/usr/bin/python3
"""
Copyright © 2021 boldyoungster. All rights reserved.
@file Polynomial.py
@date: 11:06:12, February 28, 2021
"""
import numpy as np
import matplotlib.pyplot as plt
class TrapezoidalTraj:
def __init__(self, t0, t1, q0, q1, vc, au, ad, v0=0.0, v1=0.0):
pass
def __call__(self, ts, deriv_order=0):
pass
@classmethod
def __Compute(cls, t0, t1, q0, q1, vc, au, ad, v0, v1):
pass
def PlotSpline(poly, t0, t1, title):
ts = np.linspace(t0, t1, 200)
qs = poly(ts, 0)
vs = poly(ts, 1)
dvs = poly(ts, 2)
plt.subplots_adjust(hspace=1)
plt.suptitle(title)
plt.subplot(311)
plt.plot(ts, qs)
plt.title("Position")
plt.subplot(312)
plt.plot(ts, vs)
plt.title("Velocity")
plt.subplot(313)
plt.plot(ts, dvs)
plt.title("Acceleration")
plt.savefig(f"./{title}.png")
plt.show()
def TestTrapezoidalTraj(t0, t1, q0, q1, vc, au, ad, v0, v1):
poly = TrapezoidalTraj(t0, t1, q0, q1, vc, au, ad, v0, v1)
title = "TrapezoidalTraj"
PlotSpline(poly, t0, t1, title)
if __name__ == "__main__":
t0, t1 = 0, 3.0
q0, q1 = 0, 1
v0, v1 = 0, 0
a0, a1 = 0, 0
vc = 0.3
au, ad = 2.0, 2.0
TestTrapezoidalTraj(t0, t1, q0, q1, vc, au, ad, v0, v1)
举例
t 0 = 0.0 , t 1 = 3.0 q 0 = 0.0 , q 1 = 1.0 v 0 = 0.0 , v 1 = 0.0 v c = 0.3 a u = a d = 2.0 (6) t_0 = 0.0, \qquad t_1 = 3.0 \\q_0 = 0.0, \qquad q_1 = 1.0 \\v_0 = 0.0, \qquad v_1 = 0.0 \\v_c = 0.3\\a_u = a_d = 2.0\tag{6} t0=0.0,t1=3.0q0=0.0,q1=1.0v0=0.0,v1=0.0vc=0.3au=ad=2.0(6)
这段轨迹是从速度、加速度为0开始,再到速度、加速度为0停止,称作rest-to-rest,其轨迹形状如下:
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-KnWXtZ9z-1614784011493)(./TrapezoidalTraj.png)]
总结
梯形速度轨迹是一种经常使用的分段加速轨迹,它可以保证速度连续,可以定义轨迹两个端点的速度。梯形轨迹的加速度是阶跃的,如果要保证加速度是连续的,就需要进一步使用双S速度轨迹。在计算上梯形轨迹分3段,双S速度轨迹分7段,明显要更复杂。