Perpendicular Avoidance Maneuver Questions

skybound · July 24, 2020, 8:04pm

Hey all,

I have questions about the perpendicular avoidance vector.

Is there a theory paper I can read that describes the math used in these functions? Or some documentation? Plainly seeing the logic would be a lot more efficient than reverse engineering.
How does ardupilot create vectors? Do all vectors start at the origin? Is the ownship considered the origin? When an obstacle tells the ownship it’s velocity via adsb, I’m assuming that velocity is relative to itself. Does Ardupilot have code in place to correct it in the ownship frame?
What frame of reference are the vectors calculated in? I see 3 possible options. I made graphs of these using the service GeoGebra (please forgive the username, I was getting frustrated when it made me create an account). Here’s a brief summary/legend of what is going on:
1. Ownship is what I am calling the drone that the user is controlling, the one we are looking at, the one who is making the maneuver. The nose of the ownship is aligned in the negative y direction, the right wing with the negative x direction, and the top of the tail with the positive z direction.
2. The Obstacle is the intruder in the ownship’s flight path, and is what we are avoiding. The location of the obstacle relative to the ownship, ie delta_pos_xyz is (-3, 4, -2)m (I am aware that a displacement this small would be a worse case scenario, it’s just random numbers I thought up that are simple enough to conceptualize).
3. The velocity of the Obstacle relative to it’s own reference frame (nose in positive y direction, right wing in positive x direction, top of tail in positive z direction) is (4, 9, -2) m/s.
4. The point XProduct is the point created if you cross v1_xyz with delta_pos_xyz. Perp is the point created using (my best guess of) the function ‘perpendicular’ from vector3.h, lines 253-263 (more questions about this follow).
5. Grf denotes global reference frame, lfr is local reference frame.
  1. Opt. A (https://www.geogebra.org/m/sgpeytp6): The Ownship can translate the obstacle’s velocity vector to it’s own frame of reference
    1. This would require Euler angles or quaternions. I know there’s functionality for this, but it only seems to be for controls, so I’m not sure if this is likely.
    2. As you can see, the point Perp is nearly sitting directly on the velocity vector of the Obstacle, so using this as an avoidance trajectory does not make a lot of sense.
  2. Opt. B (https://www.geogebra.org/m/hck8gy6f): The Ownship directly uses the adsb message containing the velocity components of the obstacle and places it in it’s own reference frame as is. So if the Obstacle is flying with a velocity of +9m/s in the direction of its nose, 4m/s in the direction of it’s right wing, and -2m/s in the direction of it’s tail, then the Ownship will interpret that directly, 1 to 1, so that it believes the velocity of the Obstacle in it’s own reference frame is +9m/s in the direction of its nose, 4m/s in the direction of it’s right wing, and -2m/s in the direction of it’s tail. This velocity would have the point (-4, -9, -2) in the coordinate frame of the Obstacle. The result of vec_neu, Perp, is coplanar with the plane created by v1_xyz and delta_pos_xyz. Seems to make the most sense of all the options, but I still think I’m doing something wrong because I don’t think a great avoidance maneuver would be to put the Ownship closer to the ground.
  3. Opt. C (https://www.geogebra.org/m/npeudmru): All vectors are calculated using a global reference frame, and there is no rotation or translation of any type. The exact inputs from the aircraft are used. In this example, there are two planes and two perpendicular vectors. One is made relative to the origin, the other is made relative to the intersection of the vectors ‘v1_xyz’ and ‘delta_pos_xyz’ where the Obstacle and Ownship are. They are parallel, and one is merely a translation of the other. I’m assuming that Ardupilot calculates the perpendicular vector at the origin, and then tells the Ownship to that new calculated position relative to itself, and not the destination calculated in global 3D space. Ie, Ardupilot is saying “fly to a position 10 meters in the x direction, -14 in the y direction, and 43 in the z direction relative to your current position (which is the point (1, -18, 50) in the global frame) and not ‘fly to the point (10, -14, 43) in the global frame. Is this assumption correct? The point Perp (there are two, one is the result of the calculations when done at the origin, and the other is just the first point translated to be in the same plane as the Obstacle and Ownship) is now on the other side of the velocity vector of the Obstacle. So to get to this position, the Ownship would have to cross paths with the Obstacle, making it a pretty terrible avoidance maneuver. This is a bummer, because I think this interpretation is likely the most correct of the three, so I don’t know why/what I’m doing wrong in my math.
When I tried coding the logic in python, I get a point, vec_nue, that is coplanar with vectors ‘v1_xyz’ and ‘delta_pos_xyz. How is this a perpendicular vector/what am I doing wrong with my calculations?
Here’s my calculations:

attempting to emulate vector3.h lines 253-263. length squared is just a dot product

import numpy as np 
def perpv3h(p1,v1):
    d = np.dot(p1,v1)
    parallel = (v1*d)/np.dot(v1,v1)
    perpendicular = p1 - parallel
        return perpendicular
    v1 = np.array([-4, -9, -2])
    d = np.array([3, -4, -2])
    vec_neu = perpv3h(v1, d)
    print(vec_neu)
    # just for case 3, otherwise ignore
    print(vec_neu[1])
        perp = np.array([vec_neu[0] + (-9), vec_neu[1] - 4, vec_neu[2] +7])

        print(perp)

What are the calculations being used in vector3.h lines 253 to 263? This defines the perpendicular function, but I don’t understand how
1. Line 255: d is defined as the dot (The ‘*’ operator is defined 5 different times for 5 different cases. Dot product seems like the only likely candidate, but I am not certain) product of the position and velocity of the obstacle (are they both vectors? I though p1 was a point?)
2. Line 256: If the norm of d is less than flight epsilon, return p1. Does that mean the destination/result of the perpendicular function is the location of the obstacle?
3. Line 259: Dot(v1,d) / Dot (v1, v1). Is the ‘/’ operator truly divide? It’s also defined multiple times. Also, how is this a parallel vector? I don’t understand the math here (forgive me hahaha)
4. Line 260: a point minus a parallel line is a perpendicular vector?
In AP_Avoidance.cpp, line 610, the function get_vector_perpendicular requires two inputs: the current obstacle, and vec_neu. However, vec_neu is created inside of this function, so how can a required input be a variable that is only made after the function is called?
Is vec_neu the destination vector, as in that point in 3D space will be given an NEU location to then fly to? What units is vec_neu currently in? It looks like when ‘get_vector_perpendicular’ is called, it first runs the ‘perpendicular_xyz’ function, which first uses ‘get_distance_NE’ which scales the difference between the obstacle and the ownship by long and lat scaling factors. After ‘perpendicular_xyz’ is called, ‘get_vector_perpendicular’ checks if ‘vec_nue’ is zero. It then normalizes ‘vec_nue’.
1. What happens when vec_neu is zero/when a false is returned, what happens?
2. When vec_neu is normalized, it’s unit-less, right? So then when is it attributed a meaningful value for ArduPilot to use?
After an avoidance maneuver is taken, is there a time limit assigned to the maneuver so it doesn’t constantly try to recalculate a perpendicular vector when it’s trying to reach its first calculated destination? Once a maneuver is assigned and completed, how does Ardupilot know not to redo the maneuver. For example, if once the maneuver is completed and the obstacle is still in the fail safe distance, but will not be on a collision course with the ownship, how will ArduPilot know not to maneuver again? Does it come down to the update_threat_level which ranks closest approach? That goes back to the discussion of frames of reference.
Once vec_neu is normalized, how does ardupilot assign the scale of how far away it should fly? Will it fly to a point such that the obstacle is no longer in the fail safe distance?
How does the closest_approach_xy function (AP_Avoidance.cpp, line 279) work? Specifically the time horizon bit and the closest_distance_between_radial_and_point? What does the time horizon represent? Is it the look ahead time for ardupilot? I’m having a hard time following the math for the closest distance between radial and point. It looks like it’s defined in the vector2 header, and when it’s created in the vector2.cpp, it takes the square root of the ‘closest distance… squared.’ the closet_distance_between_radial_and_point_squared looks like its defined as such:
1. Closest = closest_point(p, w); is referring to either one of two definitions. One on lines 317-339, and the other on lines 345 to 362. Which function does it defer to?
2. In the update threat level function (AP_Avoidance lines 332-385), does ardupilot basically assign threat levels if the scalar distance of the closest approach is within the boundaries of the fail safe and warn distances? So essentially, if the obstacle has a large velocity differential and is relatively close, the closest approach return will be a relatively small number?
Does North East Up correspond to X, Y, Z, which corresponds to 0, 1, 2 (positions in an array)? For example, obstacle_vel[0] describes the X coordinate, which is synonymous with the North coordinate, obstacle_vel[1] goes with Y and East, obstacle_vel[2] goes with Z and Up?
If both drones are equipped with adsb and ardupilot, will they decide which one to move and which to continue? Will they take identical but opposite movements (geofences and all that notwithstanding)?

I think the best way to summarize my questions is by asking how the get_vector_perpendicular (on AP_Avoidance line 610), closest approach for xy and z, and update_threat_level functions work. I’m trying to emulate the logic in python 3.7 right now, and I want to make it as faithful as possible. Basically, given the same inputs, I want to get identical (or an error less than 1e-3) outputs for the avoidance maneuver. How can I best do this?

I know this is a behemoth of a question, so I apologize to, and deeply thank, anyone who reads through it. Any and all help is appreciated!