# Multi-Layered Interactive Target Guidance with Visual Safety in Convex-Shaped Obstacle Environments
## 4 Planning Layer for Interactive Guidance  
In this section, a trajectory planning method is considered to achieve interactive guidance with visual safety. The trajectory planning is formulated as an optimal control problem, and the trajectory is obtained using the successive convexification (SCvx) algorithm in sequential convex programming (SCP) [19]. The SCvx algorithm is well-suited for use in this paper, as it can quickly solve optimal control problems. Here, \(g_{nav}=(p_{nav},\,e^{\delta\theta_{nav}})\) and \(V_{nav}=[v_{nav}^{T}\,\,\omega_{nav}^{T}]^{T}\) are planned for use in the safety layer. However, to ensure visual safety, the positional relationship between the robot, the target object, and the obstacles is crucial. Therefore, in this context, only the robot's target position trajectory will be designed. After planning \(p_{nav}\) and \(v_{nav}\) with consideration for visual safety, the desired relative orientation is determined by setting \(e^{\delta\theta_{nav}}\) and \(\omega_{nav}\) so that the center of the camera is directed towards the target object. For simplicity, instead of planning the trajectory in terms of the relative relationship between the camera robot and the target, the position and orientation of the camera robot in \(\Sigma_{w},g_{wd}=(p_{wd},\,e^{\delta\theta_{wd}})\), as well as the velocity \(V_{wd}^{b}=[(v_{wd}^{b})^{T}\,\,(\omega_{wd}^{b})^{T}]^{T}\), are first calculated. Afterward, the transformation is performed by  
\[g_{nav}=g_{wd}^{-1}g_{wo},\,\,\,V_{nav}=\left[ \begin{matrix}e^{\delta\theta_{ nd}}&0\\ 0&e^{\delta\theta_{wd}}\end{matrix} \right]V_{wd}. \tag{59}\]  
Then, \(g_{nav}\) and \(V_{nav}\) are used in (20) and (42). In this paper, the future body velocity of the target, \(V_{w0}^{b}\), is not available. Therefore, the optimal control problem is repeatedly solved at fixed time intervals, and the target trajectory is passed to the safety control layer. To ensure real-time performance, time is discretized at intervals of \(T\) seconds, with \(t_{i}:=iT\). In each trajectory planning iteration, the trajectory is planned for \(N\) steps ahead, but only \(\ell\) steps are passed to the safety layer. Hereafter, for simplicity, we denote \(p_{w0}[i]=p_{w0}(iT)\), \(p_{nd}[i]=p_{nd}(iT)\), and \(v_{pd}[i]=v_{nd}(iT)\). Additionally, the sequences are denoted as \(\{p_{nd}[j]\}_{j=1}^{t+N-1}\) and \(\{v_{nd}[j]\}_{j=1}^{t+N-1}\). Between the time intervals \([iT,~{}(i+1)T)\), \(p_{nd}\) and \(v_{nd}\) are linearly interpolated. Next, the objective function of the optimization problem is presented. Here, an objective function is required that can handle both the guiding task and the guarding task. Thus, the objective function at time \(t_{i}\) is as follows:  
\[\mathcal{J}[j\mid i]\coloneqq w_{1}\|v_{ud}[j]\|^{2}+w_{2}\|p_{wd}[j]-p_{w0}[i]\|^{2}\] \[+~{}w_{3}\exp(-\beta\|p_{w0}[i]-p_{task}\|)\frac{(p_{w0}[i]-p_{task })^{T}}{\|p_{w0}[i]-p_{task}\|}(p_{wd}[j]-p_{w0}[i]), \tag{60}\]  
where \(w_{1}\) and \(w_{2}\) are positive real numbers, and \(w_{3}\) and \(\beta\) are real numbers. The cost function (63a) takes the minimum value when the target is positioned between the camera robot and \(p_{task}\) if \(w_{3}\) is negative. On the other hand, if \(w_{3}\) is positive, the minimum value is achieved when the camera robot is positioned between the target and \(p_{task}\). Therefore, by adjusting the sign of \(w_{3}\), the system can switch between the guiding task and the guarding task. Next, the constraints are considered. First, since the generated sequences \(\{p_{nd}[j]\}_{j=i}^{i+N-1}\) and \(\{v_{nd}[j]\}_{j=i}^{i+N-1}\) are linearly interpolated between points, they must satisfy  
\[g_{dyn}[j]\coloneqq p_{nd}[j]-p_{wd}[j-1]-\frac{1}{2}T(v_{wd}[j]+v_{nd}[j-1])=0 \tag{61}\]  
Next, visual safety is considered. Field of view maintenance can be handled by \(e^{\beta_{max}}\) and \(\omega_{max}\), so it is not included as a constraint in trajectory planning. Regarding occlusion avoidance, it can be achieved if \(h_{acc,k}(x_{pe})\geq 0\) is satisfied.