Motion Regularization \(\mathcal{P}_{\mathrm{add}}\)
The motion regularization term \(\mathcal P_{\mathrm{add}}\) complements the graph-based formation maintenance cost
\(\mathcal P_{\mathrm{form}}\) with relative-position, relative-velocity, and lateral route-anchoring costs.
Here, \(\mathbf p_u\) and \(\dot{\mathbf p}_u\) denote the position and velocity of WMR \(u\in\mathcal V\),
while \(\mathbf r_u\) and \(\dot{\mathbf r}_u\) denote its TVFR reference and reference velocity.
Overbars denote averages over all \(N\) WMRs.
Each WMR optimizes its own trajectory using received trajectories of the other WMRs.
All positions, velocities, and references are evaluated at the same global timestamp \(\tau_j\).
The ego position is \(\mathbf p_i(t_j)\), while the position \(\mathbf p_u(\tau_j)\) of each other WMR \(u\) is evaluated from its received trajectory.
The local sample time is \(t_j=jT_i/K_i\), with \(\tau_j=\sum_{r=1}^{i-1}T_r+t_j\).
Here, \(u\) indexes WMRs, while \(i\) and \(j\) index polynomial segments and samples, respectively.
Time arguments are omitted below for brevity.
1. Relative-Position Cost
The graph-based formation cost does not uniquely determine formation orientation.
With isotropic distance weights (\(a=1\)), it is invariant to global rotation.
We therefore penalize centroid-relative position errors to align the formation while preserving translational freedom:
\[
\begin{gathered}
\mathbf e_{\mathrm{pos},u}=(\mathbf p_u-\bar{\mathbf p})-(\mathbf r_u-\bar{\mathbf r}),\\
g_{\mathrm{pos}}=\frac{1}{N}\sum_{u\in\mathcal V}\|\mathbf e_{\mathrm{pos},u}\|_2^2,\\
\frac{\partial g_{\mathrm{pos}}}{\partial\mathbf p_u}=\frac{2}{N}\mathbf e_{\mathrm{pos},u}.
\end{gathered}
\]
This term promotes the relative positions and orientation specified by the TVFR reference without fixing the formation centroid.
2. Relative-Velocity Cost
Tracking relative positions alone may cause repeated acceleration and braking.
We therefore penalize the difference between centroid-relative velocities and the corresponding TVFR reference derivatives:
\[
\begin{gathered}
\mathbf e_{\mathrm{vel},u}=(\dot{\mathbf p}_u-\overline{\dot{\mathbf p}})-(\dot{\mathbf r}_u-\overline{\dot{\mathbf r}}),\\
g_{\mathrm{vel}}=\frac{1}{N}\sum_{u\in\mathcal V}\|\mathbf e_{\mathrm{vel},u}\|_2^2,\\
\frac{\partial g_{\mathrm{vel}}}{\partial\dot{\mathbf p}_u}=\frac{2}{N}\mathbf e_{\mathrm{vel},u}.
\end{gathered}
\]
During formation transitions, this term coordinates the relative motion specified by TVFR.
Once the reference becomes constant, it encourages a common team velocity without requiring the WMRs to stop.
With acceleration \(\ddot{\mathbf p}_u\), the common-time derivatives of the position and velocity costs are:
\[
\begin{gathered}
\frac{\mathrm d g_{\mathrm{pos}}}{\mathrm dt}=\frac{2}{N}\sum_{u\in\mathcal V}\mathbf e_{\mathrm{pos},u}^{\top}(\dot{\mathbf p}_u-\dot{\mathbf r}_u),\\
\frac{\mathrm d g_{\mathrm{vel}}}{\mathrm dt}=\frac{2}{N}\sum_{u\in\mathcal V}\mathbf e_{\mathrm{vel},u}^{\top}(\ddot{\mathbf p}_u-\ddot{\mathbf r}_u).
\end{gathered}
\]
3. Lateral Route Anchoring
Relative-position and relative-velocity costs do not prevent collective lateral drift.
During formation transitions along straight route segments, we penalize lateral centroid position error and velocity.
The fixed lateral target \(y_c=y_{\mathrm{route}}+\delta_y\) combines the route coordinate with the mean assigned-slot offset \(\delta_y\):
\[
\begin{gathered}
g_y=(\bar y-y_c)^2,
\qquad g_{\dot y}=\overline{\dot y}^{\,2},\\
\frac{\partial g_y}{\partial\mathbf p_u}=\frac{2}{N}(\bar y-y_c)\mathbf e_y,
\qquad \frac{\partial g_{\dot y}}{\partial\dot{\mathbf p}_u}=\frac{2}{N}\overline{\dot y}\,\mathbf e_y.
\\
\mathbf e_y=(0,1)^{\mathsf T}.
\end{gathered}
\]
These terms suppress collective lateral drift while leaving longitudinal motion unpenalized.
With \(y_c\) fixed on each straight segment, \(\mathrm d g_y/\mathrm dt=2(\bar y-y_c)\overline{\dot y}\) and
\(\mathrm d g_{\dot y}/\mathrm dt=2\overline{\dot y}\,\overline{\ddot y}\).
The instantaneous motion regularization cost combines the three components:
\[
\begin{gathered}
g_{\mathrm{lat}}=\alpha_p g_y+\alpha_v g_{\dot y},\\
g_{\mathrm{add}}=\alpha_p g_{\mathrm{pos}}+\alpha_v g_{\mathrm{vel}}+g_{\mathrm{lat}},
\qquad\alpha_p,\alpha_v\geq0.
\end{gathered}
\]
Trapezoidal integration over the trajectory yields the motion regularization term used in JSTP:
\[
\begin{gathered}
\mathcal P_{\mathrm{add}}
=\sum_{i=1}^{M}\frac{T_i}{K_i}
\sum_{j=0}^{K_i}\omega_j\,g_{\mathrm{add}}(t_j,\tau_j),\\
t_j=\frac{jT_i}{K_i},\qquad
\tau_j=\sum_{r=1}^{i-1}T_r+t_j.
\end{gathered}
\]
Here \(\omega_0=\omega_{K_i}=1/2\) and the interior weights equal one.
The ego state is evaluated at local time \(t_j\); received trajectories of other WMRs and TVFR references
are evaluated at the corresponding global time \(\tau_j\), using a common time origin.
The nonnegative weights \(\alpha_p\) and \(\alpha_v\) balance the position and velocity costs.
Gradients are propagated through the \(\mathfrak{T}_{\mathrm{MINCO}}\) trajectory parameterization,
accounting for both local sampling times and global timestamps when differentiating segment durations.