基于距离的加减速规划代码

以下是一个基于距离的加减速规划的代码示例，可以用于控制机器人或其他运动设备的运动。

import math

class MotionPlanner:
    def __init__(self, max_vel, max_acc):
        self.max_vel = max_vel
        self.max_acc = max_acc

    def plan(self, start_pos, end_pos, start_vel=0, end_vel=0):
        # 计算距离、时间和加速度
        distance = abs(end_pos - start_pos)
        time_to_max_vel = self.max_vel / self.max_acc
        distance_to_max_vel = 0.5 * self.max_acc * time_to_max_vel ** 2
        time_to_stop = math.sqrt(distance / self.max_acc)

        # 计算三个阶段的时间
        if distance <= 2 * distance_to_max_vel:
            time_to_max_vel = math.sqrt(distance / (2 * self.max_acc))
            time_total = 2 * time_to_max_vel
        else:
            time_total = 2 * time_to_max_vel + (distance - 2 * distance_to_max_vel) / self.max_vel

        # 计算三个阶段的速度和距离
        if time_total == 0:
            vel_max = 0
            vel_end = 0
        else:
            vel_max = min(self.max_vel, math.sqrt(distance / (2 / self.max_acc + 1 / time_total)))
            vel_end = min(self.max_vel, math.sqrt(distance / (2 / self.max_acc + 2 / time_total)))

        distance_first = 0.5 * vel_max ** 2 / self.max_acc
        distance_second = distance - 2 * distance_first
        distance_third = distance_first

        # 计算每个时间点的速度和位置
        times = []
        positions = []
        velocities = []

        for t in range(int(time_total * 1000) + 1):
            t /= 1000.0

            if t <= time_to_max_vel:
                vel = start_vel + t * self.max_acc
                pos = start_pos + 0.5 * t ** 2 * self.max_acc + start_vel * t
            elif t <= time_total - time_to_stop:
                vel = vel_max
                pos = start_pos + distance_first + (t - time_to_max_vel) * vel
            else:
                vel = end_vel + (time_total - t) * self.max_acc
                pos = end_pos - 0.5 * (time_total - t) ** 2 * self.max_acc - end_vel * (time_total - t)

            times.append(t)
            positions.append(pos)
            velocities.append(vel)

        return times, positions, velocities

这个类的 `__init__` 方法需要传入最大速度和最大加速度。`plan` 方法接收起点和终点位置，以及起点和终点的速度（如果需要）作为输入，然后返回一个时间、位置和速度列表，这些列表描述了运动的三个阶段（加速、匀速和减速）的运动状态。

这个规划器的实现基于物理公式，可以保证满足最大速度和最大加速度的限制条件。