电机sin曲线规划停止

sin曲线停止方式

电机运动中，停止方式有多种，我最开始用的是s曲线规划停，但是有些场景不太适用，而且计算量比较大。为了方便在当前位置上以当前速度直接平滑减速停车，使用sin曲线方式，虽然没有s曲线的平滑，但是计算量小，直接以当前速度减速规划，每次乘以一个固定时间，当做固定步长来叠加

下面规划的其实只用了速度v，当前位置 += v * Time

代码解析

import numpy as np
import matplotlib.pyplot as plt

def sin_velocity_planning(v0, a_max, j_max):
    # 计算两种约束下的时间
    T1 = (np.pi / 2) * (v0 / a_max)
    T2 = (np.pi / 2) * np.sqrt(v0 / j_max)
    T = max(T1, T2)
    
    # 生成时间序列
    t = np.linspace(0, T, 1000)
    
    # 计算速度、加速度、加加速度、位置
    v = v0 * (1 - np.sin(np.pi * t / (2 * T)))
    a = -v0 * (np.pi / (2 * T)) * np.cos(np.pi * t / (2 * T))
    j = v0 * (np.pi / (2 * T))**2 * np.sin(np.pi * t / (2 * T))
    s = v0 * (t + (2 * T / np.pi) * (np.cos(np.pi * t / (2 * T)) - 1))
    
    fig, axs = plt.subplots(4, 1, figsize=(10, 12))
    
    axs[0].plot(t, s, label='Position')
    axs[0].set_ylabel('Position (m)')
    axs[0].grid(True)
    
    axs[1].plot(t, v, label='Velocity')
    axs[1].set_ylabel('Velocity (m/s)')
    axs[1].grid(True)
    
    axs[2].plot(t, a, label='Acceleration')
    axs[2].set_ylabel('Acceleration (m/s²)')
    axs[2].grid(True)
    
    axs[3].plot(t, j, label='Jerk')
    axs[3].set_ylabel('Jerk (m/s³)')
    axs[3].set_xlabel('Time (s)')
    axs[3].grid(True)
    
    plt.tight_layout()
    plt.show()
    
    return T


v0 = -50000 # 初始速度 m/s
a_max = -1000000  # 最大加速度 m/s²
j_max = -50000000 # 最大加加速度 m/s³

total_time = sin_velocity_planning(v0, a_max, j_max)
print(f"Total deceleration time: {total_time:.2f} seconds");