机器人轨迹规划——梯形速度轨迹

Source

机器人轨迹

这一系列轨迹教程将主要包括以下内容:

梯形速度轨迹

梯形速度轨迹表达式与计算

梯形速度轨迹分为三段,分别为加速段、匀速段、减速段,给定初始的 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)=t1t0=TTuTd=tt0=v0+auTu=v0+auTuau=Tuvcv0=vc+adTdad=Tdv1vc=q0+2v0+vcTu=q12vc+v1(TTuTd)=q0+v0t^+2Tuvcv0t^2qc0+vc(t^Tu)qd0+vc(t^TuTc)+21ad(t^TuTc)2t^[0,Tu]t^[Tu,TTd]t^[TTd,T]=q0+v0t^+2Tuvcv0t^2q0+2v0vcTu+vct^q1v1(Tt^)2Tdvcv1(Tt^)2t^[0,Tu]t^[Tu,TTd]t^[TTd,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^TuTc)t^[0,Tu]t^[Tu,Tu+Tc]t^[Tu+Tc,Tu+Tc+Td]=v0+Tuvcv0t^vcv1Tdv1vc(Tt^)t^[0,Tu]t^[Tu,TTd]t^[TTd,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]=Tuvcv00Tdv1vct^[0,Tu]t^[Tu,TTd]t^[TTd,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段,明显要更复杂。