**1. Introduction**

Unmanned surface vehicles (USVs) are a kind of specific ships with the ability of autonomous mission execution, which are widely used in various applications, including marine resource exploration, water resource transportation, patrol and defense in key areas and river regulation [1,2]. Progress has been made in a large number of research areas, including environmental perception [3,4], formation control [5,6], navigation [7,8], and so on. Environmental perception and trajectory generation are the two most important techniques when the USVs are executing in unknown environments. In particular, when the environment contains dynamic obstacles, USVs struggle to achieve accurate trajectory planning and tracking due to the lack of effective obstacle information. As a result, the autonomous navigation system may fail.

During the navigation process of a USV, the sensing devices, such as radar or camera, are located at a low observation point, which is detrimental to environmental perception because the adjacent obstacles in the front and behind will block each other. Moreover, the input of the sensors often contains noise caused by hull shaking on the water. This makes precise environmental perception a difficult problem for USVs and affects the success rate of trajectory generation. Usually, simultaneous localization and mapping (SLAM) [9] technology is required to construct the global map. However, this kind of method requires a huge computational load, and it is intractable to deal with dynamic objects in the water environment.

**Citation:** Huang, T.; Chen, Z.; Gao, W.; Xue, Z.; Liu, Y. A USV-UAV Cooperative Trajectory Planning Algorithm with Hull Dynamic Constraints. *Sensors* **2023**, *23*, 1845. https://doi.org/10.3390/s23041845

Academic Editor: Sašo Blažiˇc

Received: 10 November 2022 Revised: 19 January 2023 Accepted: 2 February 2023 Published: 7 February 2023

**Copyright:** © 2023 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https:// creativecommons.org/licenses/by/ 4.0/).

A feasible solution is to design a USV-UAV cooperative system to tackle the above problems, where the unmanned aerial vehicle (UAV) plays the role as a flying sensor. As shown in Figure 1, the USV has long cruise capability, but its perception is disturbed and limited by the circumstance. Hence the UAV flies over the USV, providing more stable and comprehensive information. Semantic segmentation [10,11] and 3D projection are used in this paper to transfer obstacle information in the field of vision of the UAV to the coordinate system of the USV. Semantic segmentation extracts pixel information of environmental obstacles, and a camera projection model helps to transfer the pixel information to 3D information. By doing this, global map information around the USV can be obtained efficiently and in real-time, implying the USV-UAV cooperative system can improve the perception ability of the USV effectively, allowing the USV to perform tasks in more complex water circumstances.

**Figure 1.** An illustration of the USV-UAV cooperative system, where the UAV provides wide obstacles and map information to guide the USV to generate an obstacle avoidance trajectory.

An initial obstacle avoidance trajectory is firstly generated by a graph-based search method [12]. However, such a method was originally designed for path searching on vast geographical scenarios, which does not consider the USV's dynamic characteristics. On the other hand, USV is famous for its under-actuated motion characteristics [13], which makes it hard to be controlled well, even when an optimal trajectory is planned. In this paper, we design a numerical optimization method to optimize the trajectory. Specifically, we take the hull dynamic constraints into account when modeling the optimization problem. As a result, the generated trajectory not only allows the obstacle avoidance rule, but also fits the motion characteristics of a USV. This makes the generated trajectory easier to be tracked under the same control conditions.

Finally, a control method with the lowest energy consumption per execution task is designed under a new numerical optimization problem. It ensures that the power consumption is optimal when the USV is actuated to track the given optimal trajectory, which is a very useful technique in real-world applications. The performance of the trajectory generation and tracking is comprehensively compared and analyzed in the simulated environments, and it verifies the effectiveness of our proposed novel framework.

In summary, the contributions of this paper are listed as follows.


• The lowest energy consumption control law is proposed to track the generated trajectory efficiently and accurately, and extensive experiments are conducted to verify the effectiveness of the USV-UAV cooperative system.

#### **2. Related Works**

#### *2.1. Trajectory Planning for USV*

Trajectory planning aims to automatically generate an obstacle avoidance trajectory for a USV when the local or global map is given. Among existing methods, the mainstream trajectory planning methods are mainly divided into two categories, i.e., path search and trajectory generation.

There are two research directions for the path search methods, including graph search and random sampling. Typical graph search methods include the A\* [14] and Dijkstra [15] algorithm, as well as their derivatives [16]. These methods mainly discretize the known map into interconnected grids and find the shortest path according to the heuristic parameters. The disadvantage of this kind of method is that the search dimension in the large map is expanding, and the calculation time shows a rapid upward trend. Among random sampling methods, typical varieties include RRT [17] and its derivatives [18], which dynamically find feasible paths by randomly sampling points in the map and constructing random exploratory trees. The method can show better performance for large maps, but its shortcomings are also very obvious. It is easy to be guided to a locally optimal solution, and it is difficult to generate feasible paths in narrow areas when system's computing resources are limited. The common problem of the above methods is that the generated path curvature is discontinuous, and trajectory smoothing is needed afterward.

For the trajectory generation methods, curve interpolation methods, such as B-spline [19], are commonly used to smooth the trajectory. The smoothness of the trajectory and motion state is guaranteed by the continuity theorem of higher-order derivatives of a curve. Meanwhile, numerical optimization methods are also widely used, such as minimum snap [20] and near-optimal control [21].

Some methods can also combine path search with trajectory generation, such as domain reduction-based RRT\* [22] and Hybrid A\* [23]. In this paper, the proposed method belongs to the numerical optimization method . It adds the dynamic and kinematic constraints of unmanned craft in the trajectory generation part so that the generated trajectory is more in line with the dynamic characteristics of the hull.

#### *2.2. The USV-UAV Cooperative System*

With the rapid development of automation and artificial intelligence technology, unmanned aerial vehicle (UAV) technology has made significant progress in recent years. Compared with USV, the advantage of UAV is that it has a broader field of vision and faster movement speed and can provide more comprehensive and effective data information for USV. In addition, UAV has the advantages of flying height and that its communication ability is less affected by the environment. It can be used to provide communication relay services for multiple USVs located in different positions. Due to the strong complementarity between USV and UAV in perception, communication, operation time, and other aspects, researchers have focused on the coordination of having UAV serve USV and have successfully verified that this method can effectively solve the problem mentioned above of self-awareness of a USV. Ref. [24] focused on the search and rescue of USVs in flood scenes and proposed a collaborative mode of manipulating a UAV to establish the global map first, providing complete map information and target localization for subsequent USV planning. Ref. [25] proposed a cooperative formation control algorithm for a single USV and multiple UAVs. The method is based on the leader-follower distributed consensus model, and the position and orientation of each boat are determined by the RGB image color-space features acquired by the UAV camera. Ref. [26] considered the strong search capability of the UAV in the air, combined with the actual target strike capability of the USV, and proposed a two-stage cooperative path planning algorithm on the water and underwater based on

the particle swarm optimization algorithm. Ref. [27] proposed an effective game incentive mechanism for the task assignment problem in the cooperative operation of USVs and UAVs, which reduced the task cost and improved the task efficiency. Ref. [28] proposed that the LVS-LVA framework to be applied the cooperative motion control of USV-UAV.

Although, most of these methods are cooperative ways to provide UAV environmental data and perceptual information for the navigation task of a USV. With the development of computer vision technology, the accuracy and robustness of the perception algorithm they use need to be improved. In addition, they did not consider the trajectory of the USV and its tracking control link, and the proposed collaborative framework can not be fully applied to the autonomous navigation task of USVs.

#### **3. Cooperative Trajectory Generation**

In the USV-UAV cooperative system, the USV has a stable environmental self-supporting ability, and the UAV is flexible and environmentally adaptable. In the process of autonomous navigation of the USV, relying on the wide field of vision and strong environmental perception provided by the UAV, it can generate a more reasonable trajectory and skillfully avoid various kinds of obstacles.

## *3.1. Environmental Perception and 3D Projection*

Environmental perception is vital when the USV is performing in unknown water areas. Different observation angles have a significant influence on the observed results. As shown in Figure 2, the USV and UAV have different angles of view. The USV observes the environment from a horizontal perspective, which may lead to serious visual occlusion, whereas the UAV performs environmental perception from a top-down perspective, which enables more accurate map-view information.

(a) USV angle of view (b) UAV angle of view

**Figure 2.** Perspective difference between USV and UAV.

Concerning the accuracy of obstacle recognition and the calculation efficiency, we use semantic segmentation technology [29,30] based on deep learning to extract pixel-level obstacle information from the image data obtained by the UAV's camera. For a given image, the position, shape and size of the obstacles in the environment can be judged by assigning each pixel with a two-categorical label: '0' indicates a safety area and '1' denotes an area in which the obstacles are located.

In this paper, we use DeepLab [10] as the semantic segmentation network and replace the backbone with MobileNet [31]. On the one hand, it reduces the amount of computation. On the other hand, in the process of feature extraction, with the help of the atrous spatial pyramid pooling (ASPP) module, it can effectively improve the global receptive field and the recognition effect. The overall network architecture is illustrated in Figure 3.

**Figure 3.** The network architecture of the semantic segmentation algorithm deployed on the UAV.

After obtaining the pixel coordinates of obstacles in the image, it needs to convert the obstacle coordinate information into a unified global coordinate. We define the coordinate system of the UAV as *U*, the camera coordinate system as *C*, and the global coordinate system as *G*. Thus the transformation from *U* to *C* can be represented by *TUC* = [*R*|*T*] ∈ *<sup>R</sup>*4×4, where *<sup>R</sup>* is the rotation matrix and *<sup>T</sup>* is the translation matrix. *TGU* · *TUC* denotes the transformation matrix from *G* to *C*. Assuming that the coordinates of the obstacle point *m* in the pixel coordinate system are (*u*, *v*), according to the imaging principle of the pinhole camera model, the relationship between its position in the camera coordinate system can be expressed as

$$\begin{cases} u = f\_x \cdot \frac{\chi}{z} + c\_x \\ v = f\_y \cdot \frac{\chi}{z} + c\_{y\prime} \end{cases} \tag{1}$$

where *fx* and *fy* denote the focal length in the *x* and *y* direction and *cx* and *cy* are the positions of the origin of the image plane, which can usually be regarded as the center of the image. Thus, the relationship between the 3D points in the global coordinate system *M* = (*x*, *y*, *z*) and the pixel coordinate system *m* = (*u*, *v*) is denoted by

$$\mathbf{s} \cdot \begin{bmatrix} u \\ v \\ 1 \\ 1 \end{bmatrix} = \begin{bmatrix} f\_x & 0 & c\_x \\ 0 & f\_y & c\_y \\ 0 & 0 & 1 \\ 0 & 0 & s \end{bmatrix} \cdot T\_{GII} \cdot T\_{UIC} \cdot \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} \tag{2}$$

where *s* is the scaling factor, which can be regarded as the depth information of each pixel. In this paper, a binocular camera carried by the UAV is used to obtain the pixel depth *s*. Through this way of 3D coordinate projection, the pixel information sensed by the UAV in real-time can be projected into the global coordinate system, forming the 3D perception ability of the USV to the environment.

## *3.2. Initial Trajectory Generation*

In order to generate an obstacle avoidance trajectory, this paper applies the Hybrid A\* algorithm [23] to provide an initial path, as shown in Algorithm 1. Given the initial state of the USV (*s* = (*x*0, *y*0, *ϕ*0)) and the navigation target state (*e* = (*xf* , *yf* , *ϕf*)), the algorithm first puts the initial state into the open list. Then it iteratively reads the node with the lowest cost in the open list as the current parent node, and generates the next child node according to the current node state, system motion mode and obstacle map. Unlike the A\* algorithm, the Hybrid A\* algorithm adds the orientation dimension to the coordinate system. Therefore, the criteria for reaching the target state is that the distance between the coordinates of the node and the target point is less than the threshold of the reaching distance, and the collision-free Reeds–Shepp curve can be generated through the node state and the target point state.

**Algorithm 1** Trajectory Search with Hybrid A\*


#### **4. Trajectory Optimization and Tracking**

The USV is an under-actuated robot operation system where the number of control variables of the system is less than the degrees of freedom of the system. In the trajectory optimization process, if the dynamic constraints of this under-actuated characteristic are added to the optimization process, an optimal trajectory more in line with the characteristics of ship motion can be generated.

#### *4.1. Trajectory Optimization with Dynamics*

The motion model of the USV is a mathematical model with 6 degrees of freedom when it is complete. For simplicity, we can ignore the motion of the hull in the heave, roll and pitch directions, and simplify it into a 3-degrees of freedom with surge, sway and yaw, represented by *x*, *y* and *ϕ*. The mathematical expression of the hull dynamics can be expressed as

$$\begin{cases} \dot{\eta} = \mathcal{J}(\eta)\nu \\ M\dot{\nu} = \pi - \mathcal{C}(\nu)\nu - D\nu \end{cases} \tag{3}$$

where *<sup>η</sup>* = (*x*, *<sup>y</sup>*, *<sup>ϕ</sup>*) ∈ *<sup>R</sup>*3×<sup>1</sup> denotes the state variables, and *<sup>ν</sup>* = (*u*, *<sup>v</sup>*,*r*) ∈ *<sup>R</sup>*3×<sup>1</sup> denotes the speed variables. *<sup>J</sup>* ∈ *<sup>R</sup>*3×<sup>3</sup> is the transition matrix, and *<sup>C</sup>* ∈ *<sup>R</sup>*3×<sup>3</sup> is the Coriolis centripetal force matrix. *<sup>M</sup>* ∈ *<sup>R</sup>*3×<sup>3</sup> is the inertial matrix, and *<sup>D</sup>* ∈ *<sup>R</sup>*3×3is the damping matrix. *<sup>τ</sup>* = (*τu*, 0, *<sup>τ</sup>r*) ∈ *<sup>R</sup>*3×<sup>1</sup> is the thrust matrix. For a catamaran, the thrust matrix can be expressed as

$$\begin{cases} \tau\_{\rm tr} = T\_1 + T\_2\\ \tau\_{\rm l} = \left(T\_1 - T\_2\right) \cdot B\_{\prime} \end{cases} \tag{4}$$

where *T*<sup>1</sup> and *T*<sup>2</sup> are the thrusts of two propellers, and *B* is their distance. The USV can be viewed as a linear time-invariant (LTI) system. Its state variables *X* and control variable *τ* can be represented by

$$\begin{cases} \mathbf{X} = [\mathbf{x}, \mathbf{y}, \mathbf{q}, \mathbf{u}, \mathbf{v}, \mathbf{r}]^T\\ \mathbf{r} = [\mathbf{r}\_{\mathbf{u}}, \mathbf{0}, \mathbf{r}\_{\mathbf{r}}]^T. \end{cases} \tag{5}$$

The system dynamics are as follows

$$\begin{cases} \begin{aligned} \dot{x} &= \iota c \cos(\varrho) - v \sin(\varrho) \\ \dot{y} &= \iota s \sin(\varrho) + v \cos(\varrho) \\ \dot{\varrho} &= r \end{aligned} \\ \begin{cases} m\_{11}\dot{u} - m\_{22}\iota r + d\_{11}u = \tau\_u \\ m\_{22}\dot{v} - m\_{11}\iota r + d\_{22}v = 0 \\ m\_{33}\dot{r} + (m\_{22} - m\_{11})\iota \upsilon + d\_{33}r = \tau\_r \end{aligned} \end{cases} \tag{6}$$

Based on Hybrid A\*, the global trajectory is optimized twice with the following constraints, including position, velocity, angular velocity and control input, as well as waypoint state constraints. The reference waypoint state is the sub-optimal trajectory obtained by considering the vehicle model, which can only provide the simulated optimal information of obstacle avoidance, heading speed and other controls. In this paper, we consider the state vector error in the optimization objective function as a soft constraint. The final optimization objective can be represented as

$$\min \quad \frac{1}{2} \left\{ \sum\_{i=0}^{N} [(\mathbf{X}\_{i} - \mathbf{X}\_{i}^{\rm ref})^{T} \mathbf{W}\_{\mathbf{x}} (\mathbf{X}\_{i} - \mathbf{X}\_{i}^{\rm ref}) + \mathbf{\tau}\_{i}^{T} \mathbf{W}\_{\mathbf{\tau}} \mathbf{\tau}\_{i} \right] + \sum\_{i=1}^{N} (\mathbf{\tau}\_{i} - \mathbf{\tau}\_{i-1})^{T} \mathbf{W}\_{\mathbf{\boldsymbol{z}}} (\mathbf{\tau}\_{i} - \mathbf{\tau}\_{i-1}) \right\}, \tag{7}$$

where *Xref <sup>i</sup>* denotes the reference state variables generated by Hybrid A\*, and *W<sup>x</sup>* = *diag*{50, 50, 20, 15, 15, 15}, *W<sup>τ</sup>* = *diag*{5, 0, 5} and *W<sup>u</sup>* = *diag*{3, 0, 3} represent the positive definite, cost and weight matrices, respectively. Moreover, to ensure adequate accuracy in the trajectory, we choose 0.05 s as the sampling period.

We adopt the methods of minimizing the control quantity and minimizing the continuous control difference to ensure that the global trajectory generated by optimization can take into account the trajectory index factors, such as the smoothing of the control quantity and the minimization of the energy consumption at the same time. The overall algorithm flow is shown in Algorithm 2.



#### *4.2. Tracking Control with NMPC*

Nonlinear model predictive control (NMPC) [32] is famous for its ability to improve local tracking precision. It performs periodic real-time optimization according to the prediction time window to achieve the purpose of iterative control to reduce tracking error. Through the numerical optimization algorithm proposed above, the global trajectory based on the kinematic and dynamic constraints of the USV can be obtained, in which the reference control quantity can be obtained. Therefore, the trajectory optimization uses the error index of control quantity as the optimization target. Setting the current time as *tj* and the prediction time window as *Wn*, the optimization problem in terms of NMPC can be formulated as

$$\begin{split} \min \quad & \frac{1}{2} \sum\_{i=t\_{\bar{i}}}^{t\_{\bar{f}} + \mathsf{W}\_{n}} [(\mathbf{X}\_{i} - \mathbf{X}\_{i}^{\mathrm{ref}})^{\mathrm{T}} \mathbf{W}\_{\mathsf{mpc}} (\mathbf{X}\_{i} - \mathbf{X}\_{i}^{\mathrm{ref}}) + (\boldsymbol{\pi}\_{i} - \boldsymbol{\pi}\_{i}^{\mathrm{ref}})^{\mathrm{T}} \mathbf{W}\_{\mathsf{mpc}\mathsf{r}} (\boldsymbol{\pi}\_{i} - \boldsymbol{\pi}\_{i}^{\mathrm{ref}}) \\ & \qquad \qquad + (\boldsymbol{\pi}\_{i} - \boldsymbol{\pi}\_{i-1})^{\mathrm{T}} \mathbf{W}\_{\mathsf{mpc}\mathsf{r}} (\boldsymbol{\pi}\_{i} - \boldsymbol{\pi}\_{i-1}) \big], \end{split} \tag{8}$$

where the first term represents the error between the state variable and the reference state variable, which is mainly used to improve the accuracy of state tracking and maintenance in the process of real-time control. The second term represents the error between the control variable and the reference control variable. This term is used to meet the index of the lowest energy consumption. Although this problem has been considered in detail in the context of optimization objectives in global trajectory planning, secondary planning in local tracking control can achieve better results. The third term can improve the smoothness of input variables in actual control and meet the needs of practical application control. *Wmpcx* = *diag*{10, 10, 4, 2, 2, 2}, *Wmpc<sup>τ</sup>* = *diag*{2, 0, 2} and *Wmpcu* = *diag*{4, 0, 4} represent the positive definite, cost and weight matrices, respectively. And considering the control requirements of real-time operation and stability, we choose *W<sup>n</sup>* to be 30, 0.05 s as the sampling period, and the cycle of the NMPC algorithm call to be 0.1 s.

#### **5. Experimental Analysis**

In this section, we perform simulation experiments using the open source Otter USV simulator [33] within the ROS environment. The Otter USV simulator is a catamaran 2.0-m long, 1.08-m wide and 1.06-m high. When fully assembled, it weighs 65 kg, and has the ability to be disassembled into parts weighing less than 20 kg, such that a single operator can launch the Otter from a jetty, lake, beach or riverbank. A PX4 drone autopilot is used as the UAV, which is mounted with a monocular camera. The Otter USV is traveling within a 200 × 100 square meter area, with many blocks placed therein as obstacles. We set up several different obstacle terrains to test the crossing ability of the USV-UAV cooperative system.

#### *5.1. Obstacle Recognition Ability*

Firstly, we perform experiments on the ability of obstacle recognition by the USV monocular camera. The semantic segmentation algorithm is used to recognize objects. Several terrains are randomly placed in the virtual environment. Some of the segmentation results are shown in Figure 4, from which we can see that the proposed light-weight segmentation network can successfully identify obstacles in the environment. Although there are some empty areas in the middle or on the edges of the obstacles, the basic shape of the obstacles has been preserved. In the post-processing stage, image expansion can be used to increase the safe collision avoidance area and ensure the reliability of navigation. After that, 3D projection can be performed to convert the pixel information into 3D information in a global coordinate system.

**Figure 4.** Obstacle recognition results of different terrains.
