# How do I move with linear velocity for an exact distance?

I want to move a robot in stageros for a distance that is known. I publish Twist messages with a linear velocity and no angular velocity in a loop until it moves the exact distance.

How would I write the loop so that it stops after moving the distance required with no going over or under the distance?

edit retag close merge delete

Sort by » oldest newest most voted

That is a relatively complex problem, involving differential equations plus feedback control.

You don't mention the initial speed of the robot. Assuming it was stopped, the simplest solution is to send steadily increasing velocity commands until a desired velocity is achieved, then send that constant velocity until you reach a point from which the desired stopping point can be reached using an appropriate constant deceleration. In other words, the velocity commands trace out a trapezoid.

For stopping at a desired point, the dynamical system is:

x_dot = -k * sqrt(x)


Solving analytically with initial condition x(0) = D and v(0) = V yields these equations of motion:

 x(t) = (sqrt(D) - V*t/(2*sqrt(D)))**2  (parabolic drop)
v(t) = dx/dt = (V**2/2*D)*t + V        (linear velocity)
a(t) = dv/dt = V**2/(2*D) = A      (constant deceleration)


Note that the initial velocity V is negative in these equations, because it represents motion from positive x to zero. The system stops in finite time T = -2*D/V, with x(T) = 0, and v(T) = 0.

For example, when D = 10m from stop line and V = -5m/s, the vehicle stops in 4 seconds at a constant 1.25m/s/s deceleration.

For a more advanced implementation with smoother movement, instead of constant accelerations limit the "jerk" (first derivative of acceleration, third derivative of position) to some reasonable value.

Since the robot will never exactly achieve your commanded velocity (even in simulation), you will need some kind of feedback controller to monitor progress and adjust your commands. The common solution is a PID controller, using the velocity from the odometry (twist.twist.linear.x) for feedback.

more

1

You can also approximate this by just having your linear velocity be proportional to 1/distance to target, plus some constant to ensure you actually make it. Not as correct or elegant as joq's answer, but it should work in practice.

Good point, Dan. That gives an exponential decay, much easier to implement.

if I am not wrong, Joq is mentioning linear segments with parabolic lends. There are other ways, you can check out such as cubic and quintic polynomial trajectories