1
$\begingroup$

I have a set of N robot poses between point A and B. I use a global localization technique to estimate poses at point A and B. As a result, I have a new corrected pose B'. Please see the figure below.

enter image description here Considering the poses A and B' are now fixed, how do I smooth out the in-between poses such that there is a smooth transition between A and B'? I know the relative poses between each node in the white trajectory. Please help me formulate this problem. Is this a graph optimization problem? or is there any other simple approach for smoothing out the jump?

Improve this question
$\endgroup$

3 Answers 3

Reset to default
1
$\begingroup$

You are right. That is absolutly graph optimization problem. Sorry for the answers above but you don't need spline or acceleration for this.

The graph optimization will find 5 poses above in your figure that reduce your sensor observation error at B as well as all the other inter poses. Graph optimization usually includes constraints on relative poses. That does exactly do the job you want.

Improve this answer
$\endgroup$
1
3
$\begingroup$

You could try to use a bezier curve (https://en.wikipedia.org/wiki/B%C3%A9zier_curve) to interpolate with a curve and via points between A and B'. Also, is this a mobile robot or a robot arm?

Do you need to go through all the points ?

Improve this answer
$\endgroup$
2
  • $\begingroup$ This is a hand held mobile platform. The poses are in 3D and I want to go through the first and last point for sure but can move around other points. I wanted to move the trajectory point B to B' as if there was some elasticity built into the trajectory. Can Bezier curve be used to manipulate even the attitude information other than the position? $\endgroup$
    – Vinmean
    Nov 19, 2018 at 23:26
  • $\begingroup$ I have modified the image to explain my scenario better. There should be a smooth shift i all the nodes so that A reaches B' instead of B $\endgroup$
    – Vinmean
    Nov 20, 2018 at 0:07
3
$\begingroup$

To get a smooth trajectory you should not have jumps in acceleration and because you are talking about pose both rotational and translational accelerations should be smooth. You can achieve this with interpolation. If you know the initial position & velocity and also final position & velocity, you have four boundary conditions and there for you can create a polynomial of degree 3 to represent the path between the two points.

$$q(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 $$

where you can define the four parameters from the boundary conditions that you have.

$$q(t_0) = a_0 + a_1 t_0 + a_2 t_0^2 + a_3 t_0^3 = q_0 $$

$$ \dot q(t_0) = a_1 + a_2 t_0 + a_3 t_0^2 = \dot q_0 $$

$$ q(t_f) = a_0 + a_1 t_f + a_2 t_f^2 + a_3 t_f^3 = q_f$$

$$\dot q(t_f) = a_1 + a_2 t_f + a_3 t_f^2 = \dot q_f$$

By solvinfg this problem you will get a smooth trajectory.

http://www-lar.deis.unibo.it/people/cmelchiorri/Files_Robotica/FIR_07_Traj_1.pdf

Improve this answer
$\endgroup$
1
  • $\begingroup$ I do not have access to acceleration and velocity of my mobile platform. I have the relative 3D poses though. Points A and B could be separated by as much as 50 poses each separated by a distance of atleast half a meter. So the poses are well apart. Do you think I can approach the problem with polynomial based interpolation? $\endgroup$
    – Vinmean
    Nov 19, 2018 at 23:19

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.