0

I'm working on a robot simulation and trying to calculate the robot's distance from the goal along some planned trajectory. The trajectory curves to avoid obstacles, and it is given by a list of coordinates. To find progress, I need to find the arc length from the current position to the goal. I'm familiar with the equation for arc length of a function: enter image description here

The approach I was planning to use was creating a polynomial approximation of the function of the trajectory from the data points using NumPy's polynomial.polyfit, then finding its derivative, squaring that, adding 1, taking the square root, and finally integrating. However, the square root of a polynomial doesn't always exist, so this method wouldn't always work.

Is there some better way to approach this? I'm familiar with numerical integration, but not sure if/how it can be applied to this problem.

EDIT: Figured out how to do this numerically, which is much faster. Compute numerical derivative using numpy.gradient/numpy.diff, plug each element in that derivative into sqrt(1 + (dy/dx)^2), then use numpy.trapz/scipy.integrate.simpson to compute integral numerically.

anirudhc1229
  • 55
  • 7
  • [This](https://stackoverflow.com/questions/44962794/how-to-integrate-arc-lengths-using-python-numpy-and-scipy) Appears Relevant~ – BeRT2me May 26 '22 at 05:06

1 Answers1

1

How will your robot move from one point to another?

If it is a straight line it suffices to do

np.sum(np.sqrt(np.diff(x)**2 + np.diff(y)**2))

If not you should first figure out what the path your robot will follow. Then having those equations you can integrate analytically, or sampling points in the curve. For smooth paths the error on the size tends to be O(1/n^2) where n is the number of points you use in your interpolation.

Bob
  • 13,867
  • 1
  • 5
  • 27
  • The trajectory is not necessarily a straight line. It curves to get around obstacles. Could you elaborate on what you mean by interpolating? Would this get around the problem of taking the square root of a polynomial? – anirudhc1229 May 26 '22 at 12:25
  • When I said a trajectory from one point to another I meant from one point to another in the curve. How do you control your robot, here is an example that says that the trajectory between two points is an arc of constant curvature https://projecteuler.net/problem=208 – Bob May 26 '22 at 13:33
  • @anirudhc1229 : The path is almost certainly not an interpolation polynomial, read up on the deviations that can make between the sampling points. Try a cubic or higher order spline, or some goal-tracking algorithm (goal = next way-point) with enforced continuous acceleration and bounded jerk. Apply the above procedure to a more finely sampled list of way-points. – Lutz Lehmann Jun 16 '22 at 09:18