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

TL;DR

This work addresses maintaining line-of-sight (LoS) to keypoints during robot motion, a critical requirement for perception-based autonomy. It introduces CT-LoS, a continuous-time line-of-sight guidance method that is sensor-footprint-agnostic and applicable to arbitrary nonlinear 6-DoF dynamics, by reformulating LoS as an isoperimetric constraint within a continuous-time SCP framework. The method uses time dilation, first-order hold control, linearization, and exact discretization to produce a convex subproblem solved via a prox-linear approach, ensuring continuous-time constraint satisfaction. Empirical results in cinematography and relative navigation scenarios demonstrate significantly lower LoS violations and favorable runtimes compared with discrete-time baselines, with scalable performance as problem size grows, albeit with some trade-offs in the objective (fuel or ToF). Overall, CT-LoS offers a practical, generalizable solution for safe, perception-aware trajectory planning in safety-critical robotic applications.

Abstract

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 system dynamics, and constant enforcement of LoS throughout the robot's path. Our experiments show that the proposed approach achieves significantly reduced LoS violation and runtime compared to existing state-of-the-art methods in several representative and challenging scenarios.

Figures (7)

• Figure 1: LoS Guidance: A simulation result of our LoS guidance method showing the pose of the quadrotor and view cone at several points along the trajectory. Throughout the trajectory, the keypoints are within LoS.
• Figure 2: Norm Cone: The inertial, body, and sensor frames are denoted by the axes with subscripts $\mathcal{I,\ B}$, and $\mathcal{S}$ respectively. The keypoint is shown by the orange point $p$, the boresight vector, $c$, of the cone, $\mathcal{K}$, is represented by the green vector, and the angles of the view cone $\alpha$ and $\beta$ are visualized on the cone.
• Figure 3: The CT-SCvx-based CT-LoS algorithmic outline
• Figure 4: Cinematography and Relative Navigation Scenarios: The drone’s path is shown with a color gradient indicating the $L_2$-norm of velocity for both the baseline and proposed methods. The keypoints are denoted by dots. In the cinematography scenario, the keypoints’ trajectory is shown in solid blue.
• Figure 5: Qualitative Results: The trajectories of keypoints in the sensor frame for both CT-LoS and DT-LoS methods are displayed. $\times$'s represent discrete nodes, while solid lines represent nonlinear keypoint paths. Different colors are used to distinguish multiple keypoint paths. The solid red lines represent the sensor field-of-view.