## 3 Path-following Control Based on Improved LOS
### Improved LOS
#### 3.2.3 Design of the State-Feedback-following Controller  
The control system for path following consists of a navigation subsystem and a control subsystem. The path following error is obtained by the navigation subsystem based on the improved LOS. The control subsystem then obtains the control command based on the output of the navigation subsystem in order to realize the path-following control. Considering the projected point, \(P^{\prime}\), and the reference path point, \(P_{\text{{LOS}}}\), the LOS path following error, \(e=\left[d_{e},h_{e},\psi_{e},\gamma_{e}\right]^{T}\), can be defined. \(\omega_{\text{h}}\) and \(\omega_{\text{v}}\) are horizontal and vertical angular velocity, respectively. Based on the following error, the path-following control law is designed. The following schematic diagrams in the horizontal plane and in the vertical plane are shown in Figures (a)a and (b)b, respectively. The following error in the horizontal plane, \(e_{h}=\left[d_{e},\psi_{e}\right]^{T}\), is defined as follows:  
\[\left\{ \begin{array}{l}d_{\rm e}=\sqrt{\left(x_{p}-x\right)^{2}+\left(y_{p}-y \right)^{2}}=\sqrt{e_{x}^{2}+e_{y}^{2}}\\ \psi_{\rm e}=\psi_{\rm LOS}-\chi=arctan\Big{(}\frac{y_{los}-y}{x_{los}-x} \Big{)}-\chi\end{array} \right. \tag{8}\]  
The differential equation for the following error can be obtained from Equation (8) as  
\[\left\{ \begin{array}{l}\dot{d}_{\rm e}=V_{p}sin\psi_{\rm e}\\ \dot{\psi}_{\rm e}=\frac{1}{1+\left(\frac{y_{los}-y}{x_{los}-x}\right)^{2}}- \omega_{\rm h}\end{array} \right. \tag{9}\]  
where \(x_{los}\) and \(y_{los}\) are the projected coordinates of the horizontal plane. The acceleration command using the linearization assumption and state feedback method for the robot is designed as follows:  
\[a_{y}^{\rm c}=-V_{\rm f}(k_{1}d_{\rm e}+k_{2}\psi_{\rm e}) \tag{10}\]  
where \(V_{\rm f}=Vcos\gamma\) is the component of velocity in the horizontal plane. The following error in the vertical plane, \(e_{h}=\left[d_{e},\psi_{e}\right]^{T}\), is defined as follows:  
\[\left\{ \begin{array}{l}h_{\rm e}=h_{p}-h\\ \gamma_{\rm e}=\gamma_{\rm LOS}-\gamma=arctan\Big{(}\frac{z_{los}-z}{h_{los} -h}\Big{)}-\chi\end{array} \right. \tag{11}\]  
The differential equation for the following error can be obtained from Equation (11) as follows:  
\[\left\{ \begin{array}{l}\dot{h}_{\rm e}=Vsin\gamma\\ \dot{\gamma}_{\rm e}=\frac{1}{1+\left(\frac{z_{los}-z}{h_{los}-h}\right)^{2}}- \omega_{v}\end{array} \right. \tag{12}\]  
where \(h_{los}\) and \(z_{los}\) are the projected coordinates of the vertical plane. Similarly, the acceleration command for the robot's vertical acceleration command can be derived as follows:  
\[a_{z}^{\rm c}=-Vk_{3}\gamma_{\rm e} \tag{13}\]  
Thus, the control instructions for the 3D path following of robot are as follows:  
\[\left\{ \begin{array}{l}a_{y}^{\rm c}=-V_{\rm f}(k_{1}d_{\rm e}+k_{2}\psi_{ \rm e})\\ a_{z}^{\rm c}=-Vk_{3}\gamma_{\rm e}\end{array} \right. \tag{14}\]  
Figure 8: Schematic of LOS-following control. (**a**) Schematic of following in horizontal plane; (**b**) schematic of following in vertical plane. The pole configuration method can select following control laws (10) and (13) based on the state feedback design with a feedback gain matrix, \(\mathrm{k}=[k_{1},k_{2},k_{3}]\), to ensure the stability of the following closed-loop system. In the pole configuration, the system is designed to achieve the expected damping ratio and control time, thereby avoiding the oscillation of the error curve and ensuring fast convergence. The stability of the following control system is demonstrated using the following error in the horizontal plane as an example. The linear error system is expressed as follows:  
\[\dot{e}=Ae+Bu \tag{15}\]  
The state matrix, \(\mathrm{A}\), and the input matrix, \(B\), are defined as follows:  
\[\mathrm{A}= \begin{bmatrix}-\frac{V_{t}}{\Delta}&V_{t}\\ 0&\frac{V_{t}}{\Delta}\end{bmatrix} ,\ \mathrm{B}= \begin{bmatrix}0\\ 1\end{bmatrix}