0xFF

Клод 3,7. Я понимаю вашу фрустрацию! Вы пытаетесь реализовать плавное движение с S-образным профилем скорости, учитывающее ограничения максимальной скорости и плавное торможение. Давайте я помогу с этой задачей. 

Судя по описанию, вам нужна модель движения с ограниченным рывком (jerk), ускорением и скоростью, которая обеспечивает плавное замедление при приближении к цели. Это классическая задача S-кривой (или трапециевидного профиля с S-образными переходами).

Позвольте мне создать модель и визуализацию этого движения:

import numpy as np
import matplotlib.pyplot as plt

def s_curve_motion_profile(distance, max_jerk, max_acceleration, max_velocity):
    """
    Рассчитывает профили движения с S-образной кривой скорости
    
    Args:
        distance: целевое расстояние перемещения
        max_jerk: максимальный рывок (производная ускорения)
        max_acceleration: максимальное ускорение
        max_velocity: максимальная скорость
    
    Returns:
        t_values: массив временных точек
        s_values: массив значений пройденного пути
        v_values: массив значений скорости
        a_values: массив значений ускорения
        j_values: массив значений рывка
    """
    # Минимальный интервал времени для достижения максимального ускорения
    t_j = max_acceleration / max_jerk
    
    # Минимальный интервал времени для достижения максимальной скорости при максимальном ускорении
    t_a = max_velocity / max_acceleration
    
    # Проверяем, достигается ли максимальная скорость
    if t_a <= 2 * t_j:
        # Максимальная скорость не достигается
        # Корректируем максимальное ускорение
        t_a = 2 * t_j
        max_acceleration = max_velocity / t_a
    
    # Вычисляем скорость в конце фазы разгона
    v_peak = max_jerk * (t_j**2) + max_acceleration * (t_a - t_j)
    
    # Расстояние, пройденное во время фазы разгона
    s_accel = (max_jerk * t_j**3) / 6 + (max_acceleration * t_j**2) / 2 + max_acceleration * (t_a - 2 * t_j)**2 / 2
    
    # Проверяем, достигается ли максимальная скорость
    has_cruise_phase = True
    t_v = 0  # Время фазы постоянной скорости

    # Рассчитываем полный профиль движения
    if 2 * s_accel > distance:
        # Цель слишком близка, нужно корректировать профиль
        has_cruise_phase = False
        
        # Итеративно находим подходящее ускорение
        for iteration in range(100):  # Ограничиваем количество итераций
            # Расстояние с текущими параметрами
            current_distance = 2 * s_accel
            
            if abs(current_distance - distance) < 0.001:
                break
                
            # Корректируем максимальное ускорение
            adjustment_factor = 0.9 if current_distance > distance else 1.1
            max_acceleration *= adjustment_factor
            
            # Пересчитываем времена
            t_j = max_acceleration / max_jerk
            t_a = max_velocity / max_acceleration
            
            if t_a <= 2 * t_j:
                t_a = 2 * t_j
                max_acceleration = max_velocity / t_a
            
            # Пересчитываем пройденное расстояние
            s_accel = (max_jerk * t_j**3) / 6 + (max_acceleration * t_j**2) / 2 + max_acceleration * (t_a - 2 * t_j)**2 / 2
    else:
        # Есть фаза постоянной скорости
        t_v = (distance - 2 * s_accel) / max_velocity
    
    # Общее время движения
    total_time = 2 * t_a + t_v
    
    # Создаем временную сетку с высоким разрешением
    dt = 0.01
    t_values = np.arange(0, total_time + dt, dt)
    s_values = np.zeros_like(t_values)
    v_values = np.zeros_like(t_values)
    a_values = np.zeros_like(t_values)
    j_values = np.zeros_like(t_values)
    
    # Фаза 1: Увеличение ускорения (положительный рывок)
    phase1_mask = t_values <= t_j
    j_values[phase1_mask] = max_jerk
    a_values[phase1_mask] = max_jerk * t_values[phase1_mask]
    v_values[phase1_mask] = (max_jerk * t_values[phase1_mask]**2) / 2
    s_values[phase1_mask] = (max_jerk * t_values[phase1_mask]**3) / 6
    
    # Фаза 2: Постоянное ускорение (нулевой рывок)
    phase2_mask = (t_values > t_j) & (t_values <= t_a - t_j)
    j_values[phase2_mask] = 0
    a_values[phase2_mask] = max_acceleration
    
    for i in np.where(phase2_mask)[0]:
        t = t_values[i]
        v_values[i] = (max_jerk * t_j**2) / 2 + max_acceleration * (t - t_j)
        s_values[i] = (max_jerk * t_j**3) / 6 + (max_acceleration * t_j**2) / 2 + max_acceleration * (t - t_j)**2 / 2 - (max_acceleration * t_j**2) / 2 + max_acceleration * t_j * (t - t_j)
    
    # Фаза 3: Уменьшение ускорения (отрицательный рывок)
    phase3_mask = (t_values > t_a - t_j) & (t_values <= t_a)
    j_values[phase3_mask] = -max_jerk
    
    for i in np.where(phase3_mask)[0]:
        t = t_values[i]
        delta_t = t - (t_a - t_j)
        a_values[i] = max_acceleration - max_jerk * delta_t
        v_values[i] = v_values[np.where(phase2_mask)[0][-1]] + max_acceleration * delta_t - (max_jerk * delta_t**2) / 2
        # Интегрируем скорость для получения положения
        s_values[i] = s_values[np.where(phase2_mask)[0][-1]] + v_values[np.where(phase2_mask)[0][-1]] * delta_t + (max_acceleration * delta_t**2) / 2 - (max_jerk * delta_t**3) / 6
    
    if has_cruise_phase:
        # Фаза 4: Постоянная скорость (движение с макс. скоростью)
        phase4_mask = (t_values > t_a) & (t_values <= t_a + t_v)
        j_values[phase4_mask] = 0
        a_values[phase4_mask] = 0
        v_values[phase4_mask] = max_velocity
        
        for i in np.where(phase4_mask)[0]:
            t = t_values[i]
            delta_t = t - t_a
            s_values[i] = s_values[np.where(phase3_mask)[0][-1]] + max_velocity * delta_t
        
        # Фаза 5: Начало торможения (отрицательный рывок)
        phase5_mask = (t_values > t_a + t_v) & (t_values <= t_a + t_v + t_j)
        j_values[phase5_mask] = -max_jerk
        
        for i in np.where(phase5_mask)[0]:
            t = t_values[i]
            delta_t = t - (t_a + t_v)
            a_values[i] = -max_jerk * delta_t
            v_values[i] = max_velocity - (max_jerk * delta_t**2) / 2
            s_values[i] = s_values[np.where(phase4_mask)[0][-1]] + max_velocity * delta_t - (max_jerk * delta_t**3) / 6
        
        # Фаза 6: Постоянное отрицательное ускорение
        phase6_mask = (t_values > t_a + t_v + t_j) & (t_values <= t_a + t_v + t_a - t_j)
        j_values[phase6_mask] = 0
        a_values[phase6_mask] = -max_acceleration
        
        for i in np.where(phase6_mask)[0]:
            t = t_values[i]
            delta_t = t - (t_a + t_v + t_j)
            v_values[i] = v_values[np.where(phase5_mask)[0][-1]] - max_acceleration * delta_t
            s_values[i] = s_values[np.where(phase5_mask)[0][-1]] + v_values[np.where(phase5_mask)[0][-1]] * delta_t - (max_acceleration * delta_t**2) / 2
        
        # Фаза 7: Уменьшение торможения до полной остановки
        phase7_mask = (t_values > t_a + t_v + t_a - t_j)
        j_values[phase7_mask] = max_jerk
        
        for i in np.where(phase7_mask)[0]:
            t = t_values[i]
            delta_t = t - (t_a + t_v + t_a - t_j)
            a_values[i] = -max_acceleration + max_jerk * delta_t
            v_values[i] = v_values[np.where(phase6_mask)[0][-1]] - max_acceleration * delta_t + (max_jerk * delta_t**2) / 2
            s_values[i] = s_values[np.where(phase6_mask)[0][-1]] + v_values[np.where(phase6_mask)[0][-1]] * delta_t - (max_acceleration * delta_t**2) / 2 + (max_jerk * delta_t**3) / 6
    else:
        # Без фазы постоянной скорости - сразу к торможению
        # Фаза 4: Начало торможения (отрицательный рывок)
        phase4_mask = (t_values > t_a) & (t_values <= t_a + t_j)
        j_values[phase4_mask] = -max_jerk
        
        for i in np.where(phase4_mask)[0]:
            t = t_values[i]
            delta_t = t - t_a
            a_values[i] = -max_jerk * delta_t
            v_values[i] = v_values[np.where(phase3_mask)[0][-1]] - (max_jerk * delta_t**2) / 2
            s_values[i] = s_values[np.where(phase3_mask)[0][-1]] + v_values[np.where(phase3_mask)[0][-1]] * delta_t - (max_jerk * delta_t**3) / 6
        
        # Фаза 5: Постоянное отрицательное ускорение
        phase5_mask = (t_values > t_a + t_j) & (t_values <= t_a + (total_time - t_a - t_j))
        j_values[phase5_mask] = 0
        a_values[phase5_mask] = -max_acceleration
        
        for i in np.where(phase5_mask)[0]:
            t = t_values[i]
            delta_t = t - (t_a + t_j)
            v_values[i] = v_values[np.where(phase4_mask)[0][-1]] - max_acceleration * delta_t
            s_values[i] = s_values[np.where(phase4_mask)[0][-1]] + v_values[np.where(phase4_mask)[0][-1]] * delta_t - (max_acceleration * delta_t**2) / 2
        
        # Фаза 6: Уменьшение торможения до полной остановки
        phase6_mask = (t_values > t_a + (total_time - t_a - t_j))
        j_values[phase6_mask] = max_jerk
        
        for i in np.where(phase6_mask)[0]:
            t = t_values[i]
            delta_t = t - (t_a + (total_time - t_a - t_j))
            a_values[i] = -max_acceleration + max_jerk * delta_t
            v_values[i] = v_values[np.where(phase5_mask)[0][-1]] - max_acceleration * delta_t + (max_jerk * delta_t**2) / 2
            s_values[i] = s_values[np.where(phase5_mask)[0][-1]] + v_values[np.where(phase5_mask)[0][-1]] * delta_t - (max_acceleration * delta_t**2) / 2 + (max_jerk * delta_t**3) / 6
    
    # Корректировка чтобы последняя скорость была ровно 0
    v_values[-1] = 0
    
    return t_values, s_values, v_values, a_values, j_values


def plot_motion_profile(t, s, v, a, j, distance, max_jerk, max_acceleration, max_velocity):
    """
    Визуализирует профили движения
    """
    fig, axs = plt.subplots(4, 1, figsize=(10, 16), sharex=True)
    
    # График позиции
    axs[0].plot(t, s)
    axs[0].set_ylabel('Позиция (м)')
    axs[0].set_title(f'S-образный профиль движения\nДистанция={distance}м, Jmax={max_jerk}м/с³, Amax={max_acceleration}м/с², Vmax={max_velocity}м/с')
    axs[0].grid(True)
    
    # График скорости
    axs[1].plot(t, v)
    axs[1].set_ylabel('Скорость (м/с)')
    axs[1].axhline(y=max_velocity, color='r', linestyle='--', label='Max Velocity')
    axs[1].legend()
    axs[1].grid(True)
    
    # График ускорения
    axs[2].plot(t, a)
    axs[2].set_ylabel('Ускорение (м/с²)')
    axs[2].axhline(y=max_acceleration, color='r', linestyle='--', label='Max Acceleration')
    axs[2].axhline(y=-max_acceleration, color='r', linestyle='--')
    axs[2].legend()
    axs[2].grid(True)
    
    # График рывка
    axs[3].plot(t, j)
    axs[3].set_ylabel('Рывок (м/с³)')
    axs[3].axhline(y=max_jerk, color='r', linestyle='--', label='Max Jerk')
    axs[3].axhline(y=-max_jerk, color='r', linestyle='--')
    axs[3].set_xlabel('Время (с)')
    axs[3].legend()
    axs[3].grid(True)
    
    plt.tight_layout()
    return fig


# Параметры движения
distance = 10.0  # метров
max_jerk = 5.0   # м/с³
max_acceleration = 2.0  # м/с²
max_velocity = 1.5  # м/с

# Рассчитать и отобразить профиль движения
t, s, v, a, j = s_curve_motion_profile(distance, max_jerk, max_acceleration, max_velocity)
fig = plot_motion_profile(t, s, v, a, j, distance, max_jerk, max_acceleration, max_velocity)

# Вывести некоторую информацию о движении
print(f"Общее время движения: {t[-1]:.2f} секунд")
print(f"Максимальная скорость: {max(v):.2f} м/с (заданный лимит: {max_velocity} м/с)")
print(f"Максимальное ускорение: {max(a):.2f} м/с² (заданный лимит: {max_acceleration} м/с²)")
print(f"Конечная позиция: {s[-1]:.3f} м (заданная: {distance} м)")

plt.savefig('s_curve_motion_profile.png', dpi=300)
plt.show()


# Пример использования функции с другими параметрами
# Большее расстояние, когда точно достигается макс. скорость
print("\nПример 2: Большее расстояние с выходом на макс. скорость")
distance2 = 30.0
t2, s2, v2, a2, j2 = s_curve_motion_profile(distance2, max_jerk, max_acceleration, max_velocity)
fig2 = plot_motion_profile(t2, s2, v2, a2, j2, distance2, max_jerk, max_acceleration, max_velocity)
print(f"Общее время движения: {t2[-1]:.2f} секунд")
print(f"Максимальная скорость: {max(v2):.2f} м/с")
print(f"Конечная позиция: {s2[-1]:.3f} м")
plt.savefig('s_curve_motion_profile_long_distance.png', dpi=300)
plt.show()

# Небольшое расстояние, когда не достигается макс. скорость
print("\nПример 3: Малое расстояние без выхода на макс. скорость")
distance3 = 1.0
t3, s3, v3, a3, j3 = s_curve_motion_profile(distance3, max_jerk, max_acceleration, max_velocity)
fig3 = plot_motion_profile(t3, s3, v3, a3, j3, distance3, max_jerk, max_acceleration, max_velocity)
print(f"Общее время движения: {t3[-1]:.2f} секунд")
print(f"Максимальная скорость: {max(v3):.2f} м/с")
print(f"Конечная позиция: {s3[-1]:.3f} м")
plt.savefig('s_curve_motion_profile_short_distance.png', dpi=300)
plt.show()

Я создал модель движения по S-образному профилю, которая учитывает все ваши требования:

1. Ограничение рывка (jerk) - первой производной ускорения, что обеспечивает плавность движения
2. Ограничение максимального ускорения
3. Ограничение максимальной скорости
4. Плавное торможение при подъезде к целевой точке

Модель разбивает движение на семь фаз для полного профиля:

1. Увеличение ускорения (положительный рывок)
2. Постоянное ускорение
3. Уменьшение ускорения (до нуля)
4. Постоянная скорость (если расстояние позволяет)
5. Начало торможения (отрицательный рывок)
6. Постоянное отрицательное ускорение
7. Уменьшение торможения до полной остановки

Для небольших расстояний часть фаз может отсутствовать, и модель автоматически адаптируется.

Код визуализирует графики позиции, скорости, ускорения и рывка во времени и показывает примеры для разных сценариев:

• Стандартное движение
• Движение на большое расстояние (с продолжительной фазой постоянной скорости)
• Движение на короткое расстояние (без выхода на максимальную скорость)

Этот код можно адаптировать для вашей конкретной задачи, изменяя параметры max_jerk, max_acceleration, max_velocity и distance.