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:

• The starting positions for FCS and SCS are random. Once FCS reaches the target position of SCS (linear_distance < 1e-3), the SCS is moved randomly to a new position... For now, but in the future, I want SCS to move randomly and unpredictably at any moment, without waiting for the FCS to reach it.
• Each step begins by calculating the linear velocity of the FCS, and then the velocity is proportionally divided among the X, Y, and Z axes based on the normalized direction vector from FCS to SCS.
• Acceleration is calculated from the velocity, and the position is updated based on velocity too. Starting my calculations from acceleration and going from acceleration to velocity and then from velocity to position became too complex and difficult to debug, but I'm willing to give it another chance.

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!

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:

• The starting positions for FCS and SCS are random. Once FCS reaches the target position of SCS (linear_distance < 1e-3), the SCS is moved randomly to a new position... For now, but in the future, I want SCS to move randomly and unpredictably at any moment, without waiting for the FCS to reach it.
• Each step begins by calculating the linear velocity of the FCS, and then the velocity is proportionally divided among the X, Y, and Z axes based on the normalized direction vector from FCS to SCS.
• Acceleration is calculated from the velocity, and the position is updated based on velocity too. Starting my calculations from acceleration and going from acceleration to velocity and then from velocity to position became too complex and difficult to debug, but I'm willing to give it another chance.

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!

• python
• simulation
• physics
• robotics
• kinematics

1 Answer 1

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.