CODEWITHSUNDEEP
pythonsimulationphysicsroboticskinematics
Imm
Imm

Reputation: 31

Fixing fluctuations, precision errors and more in velocity control algorithm. Is my approach correct?

Problem:

I want my "Follow Coordinates System (FCS)" to reach the position of a "Set Coordinates System (SCS)" while maintaining a trapezoidal velocity profile using given parameters like limited velocity and acceleration. However, I am encountering issues such as fluctuations in the velocity and missing the target position, possibly due to precision errors. Despite testing various solutions, I have not been able to achieve consistently positive results.

System Behavior:

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:

To track progress, I log the data and display it on graphs. That's why e.g. velocity is array of arrays now. It won't be when I finish. Here's one set of graphs: As you can see FCS trajectory missed SCS at first, but then it hit and hit again it's next positions with slowing down correctly.

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

Wrong legend: orange here is linear acceleration! And here is linear velocity after dividing to XYZ

Concerns:

I could keep trying to fix it, but I’m not sure if this is the right approach in the first place. I lack a background in control theory and motion planning.

I’m looking for feedback and any advice on addressing the issues I'm facing!

Upvotes: 1

Views: 35

Answers (1)

Max Headroom
Max Headroom

Reputation: 21

If you want to do this in fixed time steps you need to take the following into account:

  • the velocity can be trapezoidal or, if the maximum speed is not reached given the acceleration a and distance d, triangular with v below v_max. A triangular velocity exactly peaking at v_max is an edge case.
  • v_max will only be exactly reached from a linear slope if that takes an integer number of time steps.
  • by letting the velocity deviate slightly from v_max a fully symmetrical profile can always be calculated ahead of time.
  • it matters whether an odd or an even number of time steps are needed for the velocity profile, at least in the static case where v_0 = v_final = 0.
  • Using the method above, you are guaranteed to exactly hit the target (ignoring numerical accuracy limitations).
  • in discrete space the situation is complicated by the need for a Bresenham type algorithm and the requirement to vary the time steps.

For a moving target some kind of control law is needed. The optimal solution depends on various details of the problem you're trying to solve.

Upvotes: 0

Related Questions