17th Oct 23 | Research | Blog

Autonomous Driving: Ellipsoidal Constrained Agent Navigation

Motion and path planning in completely unknown environments is an extremely challenging problem. Autonomous navigation frameworks or algorithms for solving such navigation problems can find tremendous use case in various applications, such as, mobile robots navigation in hostile environments, search and rescue robots, exploratory robots and vehicles, and autonomous vehicles in general.

..

Ellipsoidal Constrained Agent Navigation, or ECAN, is an online path planner that allows solving the problem of autonomous navigation in completely unknown and unseen environments, while modelling the autonomous navigation problem, i.e., avoiding obstacles and guiding the agent towards a goal, as a series of online convex optimization problems. Here the term “online” refers to computations happening on-the-fly as the agent navigates in the environment towards a goal location.

In this developmental research work, i.e., integrating the ECAN with our autonomous vehicle, and its existing autonomous driving software pipeline, we demonstrate ECAN on our autonomous vehicle at Swaayatt Robots, enabling seamless navigation through the obstacles at near extremal limits of the steering controller.

 
ECAN is a set of heuristics allowing a mobile robot or an autonomous vehicle (an “agent”) to avoid obstacles in its field-of-view (FOV), and to simultaneously guide the agent towards a goal location. The fundamental algorithm doesn’t require any map of the environment. It was developed to solve the autonomous navigation problem in completely unknown and unseen environments, i.e., without any map and without any pre-computed route to the goal location. Although such information can trivially be integrated with ECAN, to further extend its capabilities and to add smoothness in the online computational process.
 

ECAN traditionally solves the open unknown environments navigation problem, where typically the agent doesn’t have to abide by the specific geometry of the roads or that of the lanes in an environment. It can, however, be extended, although non-trivially, to solve such problems as well. At Swaayatt Robots we are currently fundamentally researching to extend the capabilities of this algorithmic framework, as well as making it adaptable to the real-world navigation problems.

Novelty:

One can go through the original paper [1], to learn how ECAN, at that point in time was a novel algorithm, and its merits in contrast to the conventional approaches at that time, i.e., by 2011, on three different fronts:

    1. In enabling motion or path planning in completely unknown and unseen environments.

    2. When formulating obstacles avoidance as a mathematical optimization problem, ECAN allowed formulating the infeasible regions constraints, produced by obstacles, as convex constraints.

    3. To the best of my knowledge, ECAN was the first algorithm to demonstrate that end-to-end autonomous navigation problem, i.e., online obstacles avoidance and navigation to the goal, could be formulated as a series of online convex programs.

The third point above was not highlighted in the original paper, but in retrospect, ECAN opened a paradigm in motion planning, where convex optimization could be used for end-to-end navigation. To this day, ECAN is an instance of a handful of the algorithms allowing modelling the mathematical optimization problem for online navigation as a convex program.

There has been a recent development, “Optimization Based Obstacles Avoidance” (OBCA), which demonstrates this capability. However, OBCA first requires computation of a path from start to goal, knowing the obstacles geometry accurately, and also requiring us to having a path beforehand to the goal location. ECAN on the other hand allows navigation in previously unknown and unseen environments and doesn’t require any precomputed trajectory to operate on, to the goal location. OBCA however, inherently incorporate an agent’s non-holonomic dynamics constraints, whereas the extension of ECAN is a non-trivial research problem.

Traditionally, solving the problem of obstacles avoidance as a mathematical optimization problem results in a mixed-integer linear program (MILP) or a mixed-integer quadratic program (MIQP), or in general a non-linear non-convex program. This is inherently due to the fact that the feasible region in the presence of convex- or non-convex obstacles cannot be, in general, expressed as a convex-set, and therefore the respective constraints in the mathematical optimization problem will be mixed-integer or non-linear and non-convex.

Convex Formulation for Infeasible Regions

ECAN addresses this problem by taking a practical approach, observing how generally obstacles in real-world are (stochastically-) viewed by the agent using sensors such as a LiDAR or a stereo-camera setup to identify the pixels, and their corresponding depth, associated with obstacles. This observation allowed toppling the mathematical approach towards the formulation, and that of the solution thereof, of the online motion planning and obstacles avoidance problem as a mathematical optimization problem. Whereas the conventional approaches required mixed-integer or generic non-linear-non-convex constraints in the mathematical optimization problem, to represent the infeasible areas, ECAN represents the infeasible regions as a set of convex constraints over the point-cloud masses of the obstacles forming such regions.

Here’s a short introduction of the ECAN algorithm, and its functioning:

 

 
 

Challenges for Integrating ECAN with a Non-Holonomic Agent, i.e., an Autonomous Vehicle

Hereafter our autonomous vehicle is referred as an “agent”. ECAN algorithm doesn’t require precomputed maps of the environment and doesn’t require the knowledge of obstacles’ location beforehand. It assumes that only the knowledge of the obstacles in the agent’s field-of-view, which can be computed using computer vision and deep learning algorithmic frameworks using the data obtained from vehicle’s LiDARs or cameras. That is, it avoids the obstacles in an ad-hoc fashion as they appear in the field-of-view.

In this demo, maps are utilized to provide essential information about the vehicle's current position within its environment. These maps offer insights into the vehicle's location, distance to the goal, and its orientation. Furthermore, obstacles information beyond a certain threshold distance in the field-of-view was discarded. This threshold distance was approximately 15 meters.

The output plan that ECAN generates, at each time-step, for a kinematic robot, is noisy which may cause drastic fluctuations in the steering wheel. For example, as the agent navigates, ECAN can create a zig-zag path pattern ahead of the vehicle as it uses no knowledge of the obstacles beyond the field-of-view. While a kinematic holonomic robot can follow such paths, a real-world autonomous vehicle with non-holonomic (ackermann-) dynamics constraints may not be able to follow such path in general. There is also a max limit on the steering wheel’s angular velocity. Thus, from controls perspective, it was a bit challenging to integrate the raw ECAN algorithm with the autonomous vehicle.

Obstacle Avoidance

At every time-step, ECAN generates an ellipsoid around the agent, such that, the agent is constrained to be strictly inside the ellipsoid (strictly inside its relative interior), obstacles are constrained to be outside the ellipsoid, and the goal location is constrained to be outside of the relative-interior of the ellipsoid. One can read the relevant mathematical literature to know the mathematical definition of the relative interior of a (convex-) set. Solving the resultant optimization problem to compute the ellipsoid is formulated as a semi-definite program. Once the ellipsoid is computed, the Eigenvalue decomposition of the ellipsoid is done to compute the axes as well as their relative lengths.

After computing certain variables as a function of the ellipsoid, its axes, and taking the obstacles configuration into account, a convex-QCQP is solved to compute the movement of the agent inside this ellipsoid. In a later work, in 2013, it was proven that each of the successive ellipsoids are overlapping, and that mathematically there is always an infinitesimal amount of movement available for a kinodynamic agent to ensure safe navigation.

Thus in an online fashion a tunnel of overlapping ellipsoids is formed, guaranteed to be free of any obstacles. Thus, ECAN essentially generates a tunnel of overlapping ellipsoids in an online fashion to allow safe navigation of the agent to the goal location.

This is shown below for one of the instance trials in our autonomous vehicle below:

 

 
 

Experimental Setup and Demonstration

Here’s a demo of ECAN allowing seamless navigation through the obstacles formed using the the cones in the environment.

    1. The vehicle uses HD map for localization in this environment,
    2. Vehicle discards any information about the obstacles beyond the 15 m horizon in its field of view,
    3. The cones represent the obstacles in this environment ,
    4. The goal location is a point straight ahead of the vechicle from its initial starting point. 

 

 
 
 
 

Swaayatt Robots: Future Research on ECAN

At Swaayatt Robots we are developing the advanced version of ECAN, and significantly extending its capabilities to allow ECAN to incorporate both the kinodynamic as well as the dynamics constraints of the agent, inherently incorporating such constraints either in the ellipsoidal formation step, or in computing the motion of the agent inside the ellipsoid, or both, for both the ground and aerial navigation problems.

History of ECAN

Sanjeev Sharma invented ECAN in Fall 2009, when Sanjeev  was a 5th semester undergraduate student in the Electrical Engineering department at IIT Roorkee, and the first paper [1] was published on it in 2011. The original work of ECAN in 2011 utilized CVX, whereas the present implementation used CVXPY in python.

Original Research Paper

[1] Sanjeev Sharma, QCQP-Tunneling: Ellipsoidal Constrained Agent Navigation”. Second IASTED International Conference on Robotics, 2011.


 

Source : https://medium.com/@sanjeev.sharma.iitr/autonomous-driving-ellipsoidal-constrained-agent-navigation-b1d9ca85ff0