[Python] Fixing fluctuations, precision errors and more in velocity control algorithm. Is my...

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 / linear_distance if linear_distance != 0 else 0

new_axis_velocity = linear_velocity[-1] * factor

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


acceleration.append((velocity[-1] - velocity[-2]) / cycle_time)

current_fcs += velocity[-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!

Continue reading...