Preprocessing-based Kinodynamic Motion Planning Framework for Intercepting Projectiles using a Robot Manipulator

The geometry of $D_{i}$ is such that it tightly encapsulates the object $\mathcal{O}$. In other words, it overestimates the geometry of $\mathcal{O}$ with a simple shape. With this, we simplify the problem setup by making a more conservative requirement that $D_{i}$ must be protected instead of $\mathcal{O}$. We define $D_{i}$ as a cuboid. The outer dome $D_{o}$ captures the reachable workspace of the robot. For simplicity, we also define $D_{o}$ as a cuboid and place it concentric with $D_{i}$. A larger $D_{o}$ would allow more freedom for the robot, but would also increase the demand on preprocessing. On the other hand, a smaller $D_{o}$ would restrict the robot and limit its protection capability. We choose the size of $D_{o}$ such that the manipulator can reach the side it faces at full extension. Although our approach is simple, it can easily be generalized to different robot models. Fig. 3 shows the two domes configured for the ABB robot arm. The volume between $D_{o}$ and $D_{i}$ is where the robot manipulates the shield to intercept the incoming projectiles.

Each side of $D_{i}$ and $D_{o}$ is discretized into cells. The discretization is correlated with the shape and size of $\mathcal{S}$. We use a square-shaped $\mathcal{S}$ in our setup. The discretization of the two domes is shown in Figure 4 (a). It should also be noted that the size of the shield in our application can be made significantly larger to decrease the preprocessing effort. But doing this is not always practical and could violate the task and environment constraints. The volume around the straight line connecting a cell from $D_{i}$ to a cell from $D_{o}$ constitutes a tunnel. A line segment connecting the centers of the cells that form the tunnel is called centerline. Our key idea is that if $\mathcal{S}$ is positioned such that it fully blocks this tunnel, all possible attacks that cross the cell pair are blocked by it. This idea is illustrated in Figure 4 (b).

The size of the cell is proportional to the size of $\mathcal{S}$. Specifically, we choose the cell size to be smaller than the size of $\mathcal{S}$ to allow some tolerance in the pose of $\mathcal{S}$ that blocks the tunnel. This tolerance is also needed for possible planning and execution errors. We performed a thorough geometric analysis of the magnitude of the reduction in cell size that is needed to account for these errors. These analyses have been omitted for brevity.

We approximate the portion of the projectile that lies between the two domes by a line segment. This approximation is made under the assumption that the objects move in a straight line within that region and therefore do not breach the boundaries of the tunnel which they enter. This is not a strong assumption if the distance between $D_{i}$ and $D_{o}$ is small compared to the distance from which attacks are launched. This assumption can be further relaxed by reducing the cell size needed to account for the projectile shape to line-segment approximation error. We leave this analysis for future work.

The goal $g\in\mathcal{G}$ is defined as the centerline of a tunnel. For the motion planner, the goal condition is any pose of $\mathcal{S}$ along the centerline such that $\mathcal{S}$ is oriented orthogonal to it (see Figure 4 (b)). For ease of planning, we allow a small tolerance in $SE(3)$ for the goal pose. In our implementation, we sample equidistant points along the tunnel’s centerline and compute $SE(3)$ poses at each point which are orthogonal to it. The motion planner then attempts to plan to each of these poses sequentially until it succeeds. If it fails to do so, then the corresponding $\mathcal{G}$ is marked as unreachable.

We use a hybrid kinodynamic motion planner called INSAT (INterleaved Search and Trajectory optimization) that combines the benefits of heuristic search and trajectory optimization. We use a heuristic search-based planning approach with motion primitives (see, e.g., [14], [15], [16]) because they have strong theoretical properties. To keep motion planning efficient and tractable, INSAT [17, 18, 19] interleaves heuristic search in the low-dimensional (low-D) subspace with trajectory optimization in the full-dimensional (full-D) planning space. In our setup, the low-D subspace is the configuration space of the arm. Thus, the states and the transitions in low-D implicitly define a graph $G=(S,E)$ where $S$ is the set of all states and $E$ is the set of all transitions defined by the motion primitives.

In the preprocessing stage, for each pair of cells, the tunnel centerlines are computed. Each tunnel is checked for feasibility and the tunnels whose volume snaps to zero anywhere along the length are discarded because no incoming object coming through such a tunnel can reach the tunnel’s end on $D_{i}$. The centerlines of all the feasible tunnels constitute the goal region $\mathcal{G}$. We pick a constant number of equidistant goals on the line segment to plan for the manipulator (computeTargetPoses method on line 6 in Algorithm 1). Our algorithm computes the paths from $\textbf{x}^{S}$ to cover $\mathcal{G}$ by satisfying the goal criteria described above.

In the query stage, the corresponding centerline is identified for a given query $g\in\mathcal{G}$. This process involves first computing the points of intersection of the projectile with $D_{o}$ and $D_{i}$ and then identifying the cells containing these points. Subsequently, the associated path is retrieved from the lookup table constructed in the previous step.

B-splines are smooth and continuous piecewise polynomial functions made of finitely many basis polynomials called B-spline bases. A $k$-th degree B-spline basis with $m$ control points can be calculated using the Cox-de Boor recursion formula [22] as

where $i=0,\ldots,m$, $\frac{t-t_{i}}{t_{i+k-1}-t_{i}}$ and $\frac{t_{i+k}-t}{t_{i+k}-t_{i+1}}$ are the interpolating coefficients between $t_{i}$ and $t_{i+k}$. Let us define a non-decreasing knot vector T and the set of control points $\textbf{P}=\textbf{P}=\{\textbf{p}_{1},\textbf{p}_{2},\ldots,\textbf{p}_{m}\}$ where $\textbf{p}_{i}\in\mathbb{R}^{n},i=0,\ldots,m$. Then a B-spline trajectory can be uniquely determined by the degree of the polynomial $k$, the knot vector T, and the set of control points P called de Boor points.

The pseudocode of INSAT is given in Alg. 2. INSAT alternates between searching a low-D discrete graph and performing trajectory optimization in high-D to produce smooth full-D trajectories. The low-D variables in the algorithm are denoted with subscript $L$ and the full-D trajectories connecting two lifted spaces of low-D state are x and $\textbf{x}^{\prime}$ is denoted as $\phi_{\textbf{x}\textbf{x}^{\prime}}$. For the TrajOpt step we solve the following optimization problem in Eq. 4 using the aforementioned B-spline representation. The finite parameterization of this problem and how its optimization is interleaved with the low-D search is explained in high detail in [21].

The optimization is performed on the control points of the B-splines. The precise details of the optimization are not mentioned here due to the page limit. Once the preprocessing module’s request is received, INSAT kicks in to find a smooth B-spline from start to goal. When the optimization output is valid in terms of dynamic limits but results in a collision, we recover the trajectory by caching the iterates and returning the best trajectory to the preprocessor [21].

To detect the incoming projectile, we employ RGB color filtering, as the projectile is characterized by a distinct colored ball. We establish specific lower and upper color thresholds within the HSV color space and then process each frame to generate a binary mask that isolates the ball.

To obtain the 3D global coordinates relative to the robot’s base, we calibrate the camera to obtain the transformation between the left camera frame and the robot’s base frame. Masked point cloud information and the confidence map are retrieved from the ZED SDK, with our Python API acting as a wrapper around the SDK. We filter outliers and eliminate pointclouds outside the predefined virtual bounding box. Then, we compute the mean values for $X$, $Y$, and $Z$ to derive the 3D centroid point. Only frames that exceed a minimum pixel threshold are considered to reduce noise. Since the ZED camera does not directly measure the depth but estimates it using stereo geometry, some depth points may not be entirely accurate. To address this, we utilize ZED SDK’s built-in confident map function and drop depth points with low confidence. Consequently, we perform depth filtering, only retaining frames where the estimated depth falls within the specified distance threshold. The positions in these frames are recorded along with the timestamps, and this iterative process continues until the required number of frames is collected.

To reconstruct the 3D trajectory of the ball, we use the Ohno method [23]. First, we fit multiple position detections at various time intervals to a projectile equation of motion. It is assumed that the motion is on the $X-Z$ plane (with $Z$ pointing vertically upward). Movements perpendicular to this plane that can occur as a result of wind or other external forces are ignored. Once launched, the motion of the ball is governed purely by gravity, which is expressed as

where $(X(t),Y(t),Z(t))$ and $(V_{X}(t),V_{Y}(t),V_{Z}(t))$ denote the position and velocity of the ball at time $t$, $g$ denotes acceleration due to gravity ($g=9.81m/s^{2}$). Assuming parabolic motion for the projectile, the estimated position at time $t$ depends only on the initial position $(X(0),Y(0),Z(0))$ and the initial velocity $(V_{X}(0),V_{Y}(0),V_{Z}(0))$. We can then estimate the values of initial position and initial velocity $\theta=(X(0),Y(0),Z(0),V_{X}(0),V_{Y}(0),V_{Z}(0))$ by defining an error function that minimizes the sum of squared errors:

where $X(t),Y(t),Z(t)$ are the estimated positions from Eq. 5-7 and $X_{t},Y_{t},Z_{t}$ are observed positions from the detected frames in IV-F2.