【自动驾驶】运动规划丨速度规划丨T型/S型速度曲线

summer

发布时间 2022.02.23阅读数 5275 评论数 0

ãèªå¨é©¾é©¶ãè¿å¨è§åä¸¨éåº¦è§åä¸¨Tå/Såéåº¦æ²çº¿

正常的速度曲线算法有T型速度曲线控制、S型速度曲线控制、正弦加减速控制的算法，每一种算法都各有特点。

S/T曲线对比

1.T型速度曲线

T型速度曲线整个运动过程分为匀加速、匀速、匀减速三个阶段，在变速过程中，加速度a始终为设定的一个定值，给定若干个控制点，设定加速度为a，加速度为-a，匀速阶段速度为vm(即整个运动最大速度)，以及总的运行时间T，则速度、加速度、加加速度曲线如下图所示：

由图可知，T型函数策略相对容易实现，但其加速度不连续，在加减速阶段与匀速阶段的衔接处，存在突变的情况，这会对车辆执行器造成冲击，影响驾乘舒适性。适用于紧急制动、矿区等对舒适性要求不高的场景。

$\begin{eqnarray} \begin{cases} a_{1} =(v_{m}-v_{0})/ t_{1} & \\ a_{2} =-(v_{m}-v_{0})/( t_{3}- t_{2}) & \end{cases} \end{eqnarray}\\$

$v= \begin{eqnarray} \begin{cases} v_{0} +a_{1} t \left (0<t<t_{1} \right) & \\ v_{m} \left (t_{1}<t<t_{2} \right)& \\ a_{2} (t-t_{3}) \left (t_{2}<t<t_{3} \right) & \end{cases} \end{eqnarray}\\$

$a= \begin{eqnarray} \begin{cases} a_{1} \left (0<t<t_{1} \right) & \\ 0 \left (t_{1}<t<t_{2} \right)& \\ a_{2} \left (t_{2}<t<t_{3} \right) & \end{cases} \end{eqnarray}\\$

2.S型速度曲线

S型速度曲线整个运动过程分为7个阶段，即加加速度阶段、匀加速阶段、减加速阶段、匀速段、加减速段、匀减速段、减减速段，不同阶段的速度衔接处加速度是连续的，且加速度的变化率可控，解决了T型速度曲线加速度存在的突变问题。S速度曲线的速度、加速度和加加速度曲线如下所示：

由图可知，S型速度曲线柔性较好，解决了T型函数存在的加速度突变的问题，避免了车辆运动时候的柔性冲击，但由于分段较多、结构复杂，导致算法计算量较大，实现起来相对复杂。

$\begin{eqnarray} \begin{cases} J_{1} =(a_{m}-a_{0})/ t_{1} & \\ J_{2} =-(a_{m}-a_{0})/( t_{3}- t_{2}) & \end{cases} \end{eqnarray}\\$

$v= \begin{eqnarray} \begin{cases} \int_{0}^{t}(a_{0} +J_{1} t)dt \left (0<t<t_{1} \right) & \\ \int_{t_{1}}^{t}a_{m} dt \left (t_{1}<t<t_{2} \right)& \\ \int_{t_{2}}^{t}J_{2}(t-t_{3}) dt \left (t_{2}<t<t_{3} \right) & \end{cases} \end{eqnarray}\\$

$a= \begin{eqnarray} \begin{cases} a_{0} +J_{1} t \left (0<t<t_{1} \right) & \\ a_{m} \left (t_{1}<t<t_{2} \right)& \\ J_{2}(t-t_{3}) \left (t_{2}<t<t_{3} \right) & \end{cases} \end{eqnarray}\\$

$v= \begin{eqnarray} \begin{cases} \int_{0}^{t}(a_{0} +J_{1} t)dt \left (0<t<t_{1} \right) & \\ \int_{t_{1}}^{t}a_{m} dt \left (t_{1}<t<t_{2} \right)& \\ \int_{t_{2}}^{t}J_{2}(t-t_{3}) dt \left (t_{2}<t<t_{3} \right) & \end{cases} \end{eqnarray}\\$

$\Rightarrow v= \begin{eqnarray} \begin{cases} a_{0}t +J_{1} t ^{2} / 2 \\ a_{m} t+(a_{0}-a_{m})t_{1}+J_{1} t_{1}^{2} /2 \\ J_{2} t ^{2} / 2 -J_{2} t_{3} t+ \left\{ a_{m} t_{2} + (a_{0} - a_{m}) t_{1} + J_{1} t_{1} ^{2} / 2 - (J_{2} t_{2} ^{2} / 2 - J_{2} t_{3} * t_{2})) \right\} \end{cases} \end{eqnarray}\\$

3.代码

"""
"""

import math
import numpy as np
import matplotlib.pyplot as plt


def T_speed(t, t1, t2, t3, v0, vm):
    a1 = (vm - v0) / t1
    a2 = -(vm - v0) / (t3 - t2)
    if (t >= 0 and t < t1):
        T_v = v0 + a1 * t
        T_a = a1
    elif (t >= t1 and t < t2):
        T_v = vm
        T_a = 0
    elif (t >= t2 and t < t3):
        T_v = a2 * (t - t3)
        T_a = a2
    else:
        T_v = 0
        T_a = 0

    return T_v, T_a


def S_speed(t, t1, t2, t3, a0, am):
    J1 = (am - a0) / t1
    J2 = -(am - a0) / (t3 - t2)
    if (t >= 0 and t < t1):
        S_a = a0 + J1 * t
        S_v = a0 * t + J1 * t ** 2 / 2

    elif (t >= t1 and t < t2):
        S_a = am
        S_v = am * t + (a0 - am) * t1 + J1 * t1 ** 2 / 2

    elif (t >= t2 and t < t3):
        S_a = J2 * (t - t3)
        S_v = J2 * t ** 2 / 2 - J2 * t3 * t + \
              (am * t2 + (a0 - am) * t1 + J1 * t1 ** 2 / 2 - J2 * t2 ** 2 / 2 + J2 * t3 * t2)
    else:
        S_a = 0
        t = t2
        vm = J2 * t ** 2 / 2 - J2 * t3 * t + (
                am * t2 + (a0 - am) * t1 + J1 * t1 ** 2 / 2 - J2 * t2 ** 2 / 2 + J2 * t3 * t2)
        S_v = vm
    return S_v, S_a


def main():


    for t in np.arange(0, 30, 1):
        plt.figure(1)
        [T_v,T_a] = T_speed(t, 10, 20, 30, 0, 10)

        ax1 = plt.subplot(2, 1, 1)
        ax2 = plt.subplot(2, 1, 2)
        plt.sca(ax1)
        plt.plot(t, T_v, 'or')  #
        plt.title(" speed" )
        plt.sca(ax2)
        plt.plot(t, T_a, 'og')  #
        plt.title(" acc")

        plt.figure(2)
        [S_v, S_a] = S_speed(t, 10, 20, 30, 0, 2)

        ax1 = plt.subplot(2, 1, 1)
        ax2 = plt.subplot(2, 1, 2)
        plt.sca(ax1)
        plt.plot(t, S_v, 'or')  #
        plt.title(" speed")
        plt.sca(ax2)
        plt.plot(t, S_a, 'og')  #
        plt.title(" acc")



    plt.axis("equal")
    plt.show()


if __name__ == '__main__':
    main()

T型速度曲线 S型速度曲线_魂淡林的博客-CSDN博客_s型曲线blog.csdn.net/u013528298/article/details/85333802

一文教你快速搞懂速度曲线规划之S形曲线（超详细+图文+推导+附件代码）_GREYWALL-CSDN博客_s型速度曲线blog.csdn.net/u010632165/article/details/104951091/?utm_medium=distribute.pc_relevant.none-task-blog-2~default~baidujs_baidulandingword~default-0.fixedcolumn&spm=1001.2101.3001.4242.1

人工智能运动规划建模仿真无人驾驶自动驾驶

打赏 0

上一篇：【自动驾驶】运动规划丨速度规划丨自动驾驶速度规划及状态协调方法

【自动驾驶】运动规划丨速度规划丨T型/S型速度曲线

summer

1.T型速度曲线

2.S型速度曲线

3.代码

为你推荐

pybullet接触力设置-测试环境搭建(二)

第一讲 Matlab/Simulink入门——简单系统模型的Simulink仿真

机器人工程师进阶之路（七）旋量法（下）

PyBullet（八）实时控制机器人手臂参数（以UR5机器人手臂为例）（有需要上传油管的视频）

urdf搭建仿真模型

MATLAB安装Robotic Toolbox机器人工具箱

关于作者

summer

17

0

0

1

【自动驾驶】运动规划丨知识分享丨Apollo核心模块技术专题课程

【自动驾驶】运动规划丨轨迹规划丨Bspline

【自动驾驶】运动规划丨轨迹规划丨Frenet坐标转换

相关推荐

论文阅读笔记《DenseFusion: 6D Object Pose Estimation by Iterative Dense Fusion》

2 机械臂基础（1---Robotics Toolbox安装与机械臂坐标变换）

无人飞行器数学模型

（二十五）通俗易懂理解——批归一化（Batch Normalization）

Matlab机器人工具箱（2-3）：一些demo（轨迹规划）

Simscape Multibody Solid和Rigid Transform模块说明

热门泡泡

30积分失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

【自动驾驶】运动规划丨速度规划丨T型/S型速度曲线

summer

1.T型速度曲线

2.S型速度曲线

3.代码

为你推荐

pybullet接触力设置-测试环境搭建(二)

第一讲 Matlab/Simulink入门——简单系统模型的Simulink仿真

机器人工程师进阶之路（七）旋量法（下）

PyBullet（八）实时控制机器人手臂参数（以UR5机器人手臂为例）（有需要上传油管的视频）

urdf搭建仿真模型

MATLAB安装Robotic Toolbox机器人工具箱

评论（0）

关于作者

summer

17

0

0

1

【自动驾驶】运动规划丨知识分享丨Apollo核心模块技术专题课程

【自动驾驶】运动规划丨轨迹规划丨Bspline

【自动驾驶】运动规划丨轨迹规划丨Frenet坐标转换

相关推荐

论文阅读笔记《DenseFusion: 6D Object Pose Estimation by Iterative Dense Fusion》

2 机械臂基础（1---Robotics Toolbox安装与机械臂坐标变换）

无人飞行器数学模型

（二十五）通俗易懂理解——批归一化（Batch Normalization）

Matlab机器人工具箱（2-3）：一些demo（轨迹规划）

Simscape Multibody Solid和Rigid Transform模块说明

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板