Understanding the Frenet-Serret frame

How should a robotic car react when it detects an obstacle on turns? Well, ideally it should lower down its speed and gradually stop as a human driver would.

But things do not remain that simple while programming a robot. Here is an example:

I have a sensor X mounted on my robot to detect an obstacle. The sensor can detect an obstacle and provide me with its position in the inertial frame, i.e. I get the x and the y coordinates of the obstacle with respect to the sensor.

Let’s consider the scenario of a straight path where the obstacle is 25 meters away from the robot, on its path (refer left part of the below figure). Hence, the sensor output would be (25, 0). As the robot approaches the obstacle, the x reading diminishes to the value 0 and accordingly, the robot starts applying brakes to avoid the collision. All good till here!

Now, the robot approaches a turn where once again an obstacle is present (refer right part of the above figure). We know that the obstacle is on our path, but the robot doesn’t. The sensor’s output will be (24, 7). Clearly, for the robot, this means the obstacle is 7 meters away from its path. Hence, there will be no reduction in the robot’s speed. It will be only after the robot reaches a position where the y value lies within a threshold, that it will apply a high percentage of brake resulting in a jerky stopping. This is not what we want!

Here, we can use the Frenet-Serret frame. The wiki link is just to provide you with an idea of what I am talking about. I know, the equations there are quite intimidating. Let’s look into it and understand how we can use this frame to solve our problem.

By looking at the wiki link, you would have understood that this frame has something to do with a tangent, a normal, and a binormal (product of tangent and normal). Yes, that’s correct. Let’s say I have a route, r(t). Then r’(t) will represent the tangent for that route. Further, if I divide it by its magnitude, I get a unit tangent vector, r’(t)/∥ r’(t)∥ . This is nothing but the direction of the motion. Now, using the right-hand rule, if the middle figure is pointing in the direction of the motion(the unit tangent vector), then the index finger is the normal vector and the thumb is the binormal vector. Refer to the figure for better understanding.

The frame represented in the above figure is the Frenet-Serret frame, which is NOT static. We need to convert our sensor readings from the inertial(or the sensor) frame to the Frenet-Serrate frame. Here is a crude pictorial representation of what exactly we are going to do. We are considering the obstacle to be a little distorted (the black dot in the figure below) from the path for better clarity.

Though this step might seem like a complex process, it is not. We just need to find the obstacle distortion(d) from the path and how far is the obstacle from the robot’s position(or the arc length, s), using vector algebra. Before that, let’s be clear with what all data is available. We have,

• Sensor reading in the inertial(or sensor) frame
• The path the vehicle needs to follow. This path is nothing but an array of equidistant points( x and y coordinates) in the vehicle frame.

To get the value of d:

1. Get the sensor reading and the path points in the same frame. Here, we will bring them in the vehicle frame, since the transformation between vehicle and sensor frame is known and will remain constant.
2. Find a point on the path, such that if you join the obstacle center with that point, the line will be perpendicular to the tangent. Or in other words, find the point on the path nearest to the obstacle center. (If you are confused at this point, carefully look the figure again). Let’s call this as point C.
3. Find a point immediately next (point N) and immediately prior (point P) to the point from step (1). If we know the index of point C, then point N will be index + 1 and point P will be index -1.
4. Here we will be approximating some quantities. Since the points are equidistant, (N-P) will result in a vector that is almost tangent to point C. Let’s call this as vector T.
5. Next, we will subtract point P from the obstacle center to obtain vector V1. (refer figure if you are lost).
6. Now comes the vector algebra. Somehow, we need to find the value of d. To do so, we will perform a cross-product operation on vector V1 and vector T. The magnitude of the resultant will be |V1||T|sinΘ. By dividing the term by |T|, I will get |V1|sinΘ, which is nothing but the projection of vector V1 on vector T. I am calling this quantity as an approximated value of d. If you disagree with me, refer to the below image.

Now, we have obtained the value of d. If we look at the figure carefully, this quantity is telling me how much my obstacle is distorted from my path. hence, in our Frenet-Serret frame, the obstacle coordinates will be (0, d).

But we need something more to completely understand the obstacle position. That is, how far is it from the vehicle. Remember, we cannot just subtract the point C from the vehicle’s pose since we are dealing with a curve. The correct approach would be to integrate along the curve and find the arc length. To do so, first, we find the index of the path point closest to the vehicle’s pose. We already know the path index nearest to the obstacle. Since the points are equidistant, we can find the length of the arc. Let’s call this value as s.

Now, we have s and d to define our obstacle’s position. Let’s see if this helps us solve our problem.

In the sensor frame, as described in the very first example, the obstacle co-ordinates were (24, 7) and our robot failed in this case. If I try converting it to the Frenet-Serret frame, then my d value is 0 and s value is the arc length, which can be easily calculated. For convenience, let's assume it as 28, i.e. s = 28. These values clearly mean that my obstacle is on the path, 28 m from the vehicle. Therefore, the problem is solved!

Long press the clap button if this article was helpful. For any doubts or suggestions, please reach out on my LinkedIn account.