Robotic vessel navigation using GIS datasets

Question

I am trying to build an autonomous vessel able to plan the best route to a certain waypoint or to follow a pre-defined route.

Currently, I have imported Esri GIS files containing elevation data (coordinates can be extracted from grid size and lower left corner position), and the robot automatically loads in a dynamic vector the correct map using a geofencing algorithm.

The idea is to use elevation data to keep the boat away from shallow water and land, possibly using a D* algorithm to determine the best route. However, those GIS datasets are discrete and sometimes have a pretty low accuracy, while the GPS on board uses RTK to get a 2cm accuracy. The problem comes when I try to convert a discrete distribution in a continuous map, especially for elevation data: there are certain areas where altitude goes from -10 (underwater) to 100 from one point to another. Any ideas?

Answer 1

If I understand your question correctly, you are trying to estimate the position of arbitrary points in an environment given scattered 3D points. For example, your robot is at position (x, y, z) where your x and y are known but you need to determine the z given the scattered GIS data.

This is an interpolation problem, and the matlab doc have a pretty comprehensive page on this exact problem for the griddata method. Depending on the interpolation method you use the equation to find the position along z are going to be different.

If you choose to go with a linear interpolation (the robot is on the plane defined by the three closest GIS points) you can use something like this answer that I will copy here for safekeeping:

p1, p2, p3 define a plane. You can represent it by a point and a normal. For instance, P=p1, N=(p2-P) x (p3-P) (that is, N = cross product of p1p2 and p1p3).

Now for p4 to be in the same plane, it satisfies the plane equation:

  (p4-P) · N = 0  %// dot product
⇒ (x4-x1)*N.x + (y4-y1)*N.y + (z4-z1)*N.z = 0

Re-arranging:

z4 = z1 - ((x4-x1)*N.x + (y4-y1)*N.y)/ N.z

No linear system to solve, you just need a cross product.

Since you have large distances between your GIS measurements, I would use a Cubic interpolation but again, it is mainly important that your interpolation is closest to the physics of your estimated model.