固定波动，精度错误以及速度控制算法中的更多。我的方法正确吗？

Question

问题：

我希望我的“关注坐标系统（FCS）”到达“设置坐标系统（SCS）”的位置，同时使用有限的速度和加速度（例如速度和加速度）维持梯形速度曲线。但是，我正在遇到诸如速度波动和缺少目标位置之类的问题，这可能是由于精度错误所致。尽管测试了各种解决方案，但我仍无法达到始终如一的积极结果。

系统行为：

FC和SC的起始位置是随机的。一旦FC到达 SCS（

linear_distance < 1e-3

）的目标位置，SCS被随机移至新位置...目前，但是

未来，我希望SC随时随地随机和不可预测地移动，而无需等待FCS到达它。
• each步骤首先要计算线性速度 FCS，然后将速度按比例分配给x，y， 基于从FCS到SCS的归一化方向向量的Z轴。

  加速是根据速度计算的，位置为
  也根据速度更新。从加速开始我的计算并从
  加速到速度，然后从速度到位置变得太复杂了，并且
  很难调试，但我愿意给它另一个机会。
  

  我失败的算法（Python）： 第1部分。线性速度计算：

• 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)

• 部分2。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

输出：

要跟踪进度，我记录了数据并将其显示在图形上。这就是为什么速度现在是数组的数组。当我完成时不会。这是一组图：

如您所见，FCS轨迹一开始错过了SC，但随后击中并再次击中，它是下一个位置，并正确放慢了速度。

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

分开后，这是线性速度

关注： 我可以继续尝试修复它，但是我不确定这是否是正确的方法。我缺乏控制理论和运动计划的背景。 我正在寻找有关解决我所面临的问题的反馈和任何建议！ 如果您在离散空间或固定时间步骤中执行此操作，则需要考虑以下内容： 速度可以是梯形，或者，如果未达到最大速度，则鉴于A和距离D的加速度为d，三角形的v_max以下V_max。三角形速度正好在v_max处达到峰值。

通过让速度略微偏离v_max，完全对称的轮廓可以提前计算。 至少在v_0 = v_final = 0的静态情况下，是否需要奇数或偶数的时间步骤。

通过上面的方法，您可以确保准确地击中目标（忽略数值精度限制）。 在离散的空间中，由于需要Bresenham型算法的需要，情况变得复杂。

对于移动目标，需要某种控制法。最佳解决方案取决于您要解决的问题的各种细节。