Continuous-Time Line-of-Sight Constrained Trajectory Planning for Six Degree of Freedom Systems

Teaser Video

Perception algorithms are ubiquitous in modern autonomy stacks, providing necessary environmental information to operate in the real world. Many of these algorithms depend on the visibility of keypoints, which must remain within the robot’s line-of-sight (LoS), for reliable operation. This paper tackles the challenge of maintaining LoS on such keypoints during robot movement. We propose a novel method that addresses these issues by ensuring applicability to various sensor footprints, adaptability to arbitrary nonlinear dynamics and constantly enforces LoS throughout the robot's path. We show through our experiments that the proposed approach achieves significantly reduced LoS violation and runtime when compared to existing state-of-the-art methods in several representative and challenging scenarios.

Getting Started

Installation

The main packages are:

• cvxpy - is used to formulation and solve the convex subproblems
• jax - is used for determining the Jacobians using automatic differentiation as well as the just-in-time (JIT) compilation of the dynamics and their Jacobians
• numpy - is used for numerical operations
• scipy - is used for the numerical integration of the dynamics
• pyaml - is used for reading the configuration files
• termcolor - is used for pretty command line output
• plotly - is used for all visualizations

These can be installed via conda or pip.

Via Conda (Recommended)

1. Clone the repo

   git clone https://github.com/UW-ACL/los_guidance.git
   git submodule update --init --recursive
   cd los_guidance/los_guidance

2. Install environment packages (this will take about a minute or two):

   conda env create -f environment.yml

3. Activate the environment:

   conda activate los_guidance

Via Pip

0. Prerequisites Python = 3.11
1. Clone the repo

   git clone https://github.com/UW-ACL/los_guidance.git
   git submodule update --init --recursive
   cd los_guidance/los_guidance

2. Install environment packages:

   pip install -r requirements.txt

(back to top)

Usage

The main script is main.py which can be run with the following command:

python main.py --params-file quadsim/params/dr_vp.py --plot trajectory

The valid options for the --params-file flag are:

• quadsim/params/dr_vp.py - the relative navigation scenario using CT-LoS
• quadsim/params/dr_vp_nodal.py - the relative navigation scenario using DT-LoS
• quadsim/params/cinema_vp.py - the cinematography scenario using CT-LoS
• quadsim/params/cinema_vp_nodal.py - the cinematography scenario using DT-LoS.

You can also add the --plot flag to visualize the results, in which case the valid options are the following:

• trajectory - plots the 3D trajectory and the LoS constraints as an animation
• camera_view - plots the camera view of the keypoints as an animation
• conic_view - plots the conic view of the keypoints as an animation
• scp_iters - plots the SCvx iterations
• constr_vio - plots the LoS constraint violation
• control - plots the control inputs
• losses - plots the components of the convex subproblems objective
• states - plots the full state trajectory of the system

(back to top)

Acknowledgements

This work was supported by a NASA Space Technology Graduate Research Opportunity and the Office of Naval Research under grant N00014-17-1-2433. The authors would like to acknowledge Natalia Pavlasek, Griffin Norris, Samuel Buckner, and Purnanand Elango for their many helpful discussions and support throughout this work.

License

Distributed under the GPL-3.0 License. See LICENSE.txt for more information.

(back to top)