速度控制算法中的波动、精度误差等问题修复：我的实现思路是否正确？

Problem: #

我希望我的"Follow Coordinates System (FCS)"能够在保持梯形速度曲线的前提下，到达"Set Coordinates System (SCS)"的位置，使用给定的限制速度和加速度参数。但我遇到了一些问题，比如速度波动、无法精准到达目标位置，这可能是精度误差导致的。尽管测试了各种解决方案，我还是没能得到持续稳定的正向结果。

System Behavior: #

• FCS和SCS的起始位置是随机的。一旦FCS到达SCS的目标位置（linear_distance < 1e-3），SCS就会随机移动到新的位置……目前是这样，但未来我希望SCS可以在任意时刻随机、不可预测地移动，不用等FCS到达。
• 每一步开始时先计算FCS的线速度，然后根据FCS到SCS的归一化方向向量，将速度按比例分配到X、Y、Z三个轴上。
• 加速度是从速度计算出来的，位置则基于速度更新。从加速度开始计算（加速度→速度→位置）的方式变得过于复杂且难以调试，但我愿意再尝试一次。

My failing algorithm (Python): #

Part 1. Linear velocity calculations: #

for step in range(cycles):

    axis_distance = np.array(current_scs[:3]) - np.array(current_fcs[:3])
    linear_distance = np.linalg.norm(axis_distance)

    breaking_distance = (linear_velocity[-1]**2) / (2 * max_deceleration)
    
    # Acceleration or constant velocity phase
    if linear_distance > breaking_distance:
        if np.abs(max_velocity - linear_velocity[-1]) > max_acceleration * cycle_time: # Preventing overshooting max_velocity
            linear_velocity.append(linear_velocity[-1] + max_acceleration * cycle_time)
        else:
            linear_velocity.append(max_velocity)  
    # Deceleration phase
    else:
        if linear_velocity[-1] > max_deceleration * cycle_time: # Preventing overshooting 0
            linear_velocity.append(linear_velocity[-1] - max_deceleration * cycle_time)
        else:
            linear_velocity.append(0)

    linear_acceleration.append((linear_velocity[-1] - linear_velocity[-2]) / cycle_time)

Part 2. Dividing linear velocity among the XYZ: #

for i in range(3):

        factor = axis_distance[i] / linear_distance if linear_distance != 0 else 0

        new_axis_velocity = linear_velocity[-1] * factor
        
        if velocity[i][-1] < new_axis_velocity:
            velocity[i].append(velocity[i][-1] + np.min([max_acceleration * cycle_time, new_axis_velocity - velocity[i][-1]]))
            #print(f"{step} accelerating at axis {i}")
        elif velocity[i][-1] > new_axis_velocity:
            velocity[i].append(velocity[i][-1] - np.min([max_deceleration * cycle_time, velocity[i][-1] - new_axis_velocity]))
            #print(f"{step} decelerating at axis {i}")
        else:
            velocity[i].append(velocity[i][-1])
            #print(f"{step} target velocity for axis {i} reached")
        
                               
        acceleration[i].append((velocity[i][-1] - velocity[i][-2]) / cycle_time)
        
        current_fcs[i] += velocity[i][-1] * cycle_time

Output: #

为了跟踪进度，我会记录数据并绘制图表。这就是为什么比如速度现在是数组的数组，完成后就不会这样了。以下是一组图表情况：

• 第一张图表：可以看到FCS轨迹一开始没命中SCS，但之后成功命中了后续的位置，减速也正常。
  对应的日志输出：

SCS reached at 5574 : linear_distance = 0.0008199809854013105, linear_velocity = 2.7999999999649416
SCS reached at 9163 : linear_distance = 0.0008934633980521272, linear_velocity = 0.7999999999478358

• 第二张图表：注意图例标注错误，橙色线条实际是线加速度！
• 第三张图表：展示的是线速度分配到XYZ轴之后的情况

Concerns: #

我可以继续尝试修复，但不确定一开始的实现思路是否正确。我没有控制理论和运动规划的背景知识。

我正在寻求反馈和任何解决当前问题的建议！

备注：内容来源于stack exchange，提问作者Imm