Ask Your Question

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

asked 2012-08-16 06:03:11 -0500

Clefairy gravatar image

updated 2014-01-28 17:13:21 -0500

ngrennan gravatar image

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 flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2012-08-16 07:49:51 -0500

joq gravatar image

updated 2012-08-16 08:03:25 -0500

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.

edit flag offensive delete link more



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.

Dan Lazewatsky gravatar image Dan Lazewatsky  ( 2012-08-16 08:12:57 -0500 )edit

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

joq gravatar image joq  ( 2012-08-16 09:00:42 -0500 )edit

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

cagatay gravatar image cagatay  ( 2012-08-16 12:58:48 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



Asked: 2012-08-16 06:03:11 -0500

Seen: 2,153 times

Last updated: Aug 16 '12