# Waypoint Tracking Control for Autonomous Mobile Sampling and Dissolved Oxygen Enrichment of Unmanned Surface Vehicle

Authors: Jian Yuan; Hailin Liu; Wenxia Zhang
Journal: Journal of Robotics
 (2022)

---

## Abstract

An autonomous monitoring and control system of unmanned surface vehicle (USV) with mobile water quality monitoring, sampling, and oxygenation functions is constructed. The control hardware and monitoring configuration software of the system is designed, respectively, which can be installed on USV and its remote control and monitoring terminal. The kinematic modeling of USV, waypoint trajectory-tracking control, distributed controller, simulation of tracking control, and verification of software and hardware design are carried out. In order to reject the system noise and external noise, a states estimation method with fully observable states is considered in the control law design. The software and hardware are also implemented to verify the effectiveness of the monitoring platform. Through setting a series of monitoring target points and monitoring parameters in the configuration software of the remote user terminal or in the APP of the mobile user terminal, the USV can realize the automatic cruise monitoring using an autonomous navigation and tracking control algorithm, and quantitative water sampling collection. The reliability of the system is verified by the experiment of the shore test station, and the waypoint trajectory tracking and sensors data are replaying in a logview GUI of MOOS-Ivp and APP.


---

## Body

## 1. Introduction

The online monitoring system of water quality can monitor the water quality all day. The data from the water quality monitoring system can reflect the water quality or water pollution, and the system can provide reliable data for water environmental management [1]. At present, the monitoring equipment of water quality on the market mostly are usually in a shore monitoring station for a fixed-point monitoring in the water, which has not the mobile monitoring capability for a large area of water surface. Currently, the water quality monitoring technology can be carried out manually, but it lacks autonomous, frequent, and efficient monitoring schemes [2, 3] and various USV platforms [4]. Recently, the remote-control unmanned boat with sensor monitoring is designed to realize the water surface monitoring, but the unmanned boat does not have the automatic oxygen enrichment and fixed depth water sampling function, which does not have the autonomous navigation and control ability. The fixed-point jetting mode is often used in the water body aerator, which has great disturbance to the water body. The single monitoring, sampling, and aeration equipment cannot complete a large range of monitoring and water sample collection of water area in a short time, so a networked monitoring mobile platform containing many USVs can be used for urgent monitoring. The researchers from the University of California, Berkeley, has developed a floating cylindrical device to monitor the water quality [5]; It has sensors installed at the device bottom and sends sensors information wirelessly. It has the ability of positioning itself with GPS device and moving around with small propellers powered by lithium-ion batteries. Kaizu et al. has developed an unmanned hovercraft for water quality mapping, which can collect temperature, dissolved oxygen, turbidity, conductivity, pH, and chlorophyll data [6]. For water real-time monitoring and control, the integration of microfluidic chip and micro-optical system suggests possibilities. In [7], for the spatially distributed characterization of microfluidic two-phase phenomena, the authors present a polymeric micro-optical system that consists of two coupled miniaturized devices. In [8], the authors proposed a low-cost, completely open source, USV for real-time measurement and detection for the surface water quality in complex environments. The platform is equipped with various water sensors to collect pH, turbidity, and temperature data for actual water quality investigation. Trajectory-tracking control of USV waypoint has important theoretical and engineering significance for the water quality monitoring of USV [9]. However, because of nonholonomic constraints, the USV control system does not satisfy the Brockett necessary condition for stable control [10]. Therefore, Kanayama et al. adopts Taylor linearization and dynamic feedback linearization methods to design a stable tracking controller based on tracking error model [11]. Jiang and Nijmeujer designed a trajectory-tracking control law using the backstepping methodwhich solved both the local and global tracking control problems [12]. In [13], the kinematic and dynamic models are first transformed into a unified standard form, and then, a new dynamic tracking controller is proposed to solve the global tracking problem of the nonholonomic USV. In [14], a global tracking control law is constructed by using Lyapunov direct method, and the nonholonomic constraint control is solved. In [15, 16], the authors proposed the adaptive neural network control method to design the tracking control law. In [17], the authors presented a derivation of autonomous control algorithm, which can regulate the vehicle speed to a time-varying reference speed to improve the forward speed control. For open-loop operation mode of outdoor diving with wind speed limit, Roberts Luke et al. [18] used a low-power on-board processor and proposed a calculation model with an accuracy of 5 m.To realize a large of water monitoring in a short time, we design a self-powered oxygen enrichment control system that can be installed in USV cabin, and the USV can cruise independently according to the monitoring points of users, also can realize the real-time monitoring of water temperature, pH, turbidity, oxygen, and conductivity of the monitoring points. Meanwhile, the monitoring and control system can collect water sampling. The control system includes a photovoltaic module, a remote communication and monitoring module, a sensor module, an aeration module, and a water sample collection module. The system can run autonomously according to the dissolved oxygen value. The reliability of the system is verified by the experiment of the shore test station. It can be applied to the water quality monitoring of a large range of shore surface aquaculture. It can be applied to the water quality monitoring of a large range of shore surface aquaculture. Furthermore, we develop a double-pushing USV with electric propulsions for monitoring. Its thrusters consist of two culvert thrusters symmetrically arranged at the bottom of the USV, which can be reversed and forward. The USV body changes the course of rotation moment caused by the different thrust generated by two culvert thrusters, which has good maneuverability and maneuverability. The USV has functions of PWM motor speed control, automatic heading keeping, waypoint tracking, motion attitude monitoring, remote data communication, water quality sensing, and sampling. The USV has the characteristics of stable hull structure, lightweight, flexible control, and long endurance. At the same time, it can realize the expandable design of measurement module to meet the requirements of multilevel, diversified, and various advanced control algorithm verification experiments and teaching.


## 2. Design of Control and Monitoring System

The self-powered, oxygen enrichment control system for water surface movement monitoring contains two subsystems, that is, the power supply electronic subsystem and the data communication and monitoring control subsystem. The control system consists of the control and navigation system for motion control and the data communication and monitoring control system for water sampling. The former is working for trajectory-tracking and motion control, and the latter is designed for water sampling and monitoring. The structure of the monitoring system is shown in Figure1. The monitoring system is divided into control terminal on working ship and the USV control terminal. The USV terminal consists of main controller, motion controller, driver module, thrust, navigation sensors, and other controller terminals.Figure 1
The control system structure.The data communication and monitoring system for water sampling consists of a remote upper computer or mobile phone and a lower computer installed in the USV, as shown in Figure2. The lower computer is composed of Delta PLC and analog quantity expansion module, intermediate relay, aeration device, water quality sampling equipment, and mps-400 water quality sensor unit, which completes the real-time data collection of dissolved oxygen, turbidity, temperature, pH, and conductivity water quality sensors. Both a computer and a mobile phone can be used as the upper control equipment. The software installed on the computer is mainly composed of the configuration software of Delta, SQLServer2005, and OPC server provided by the GiantControl Co. Ltd. Through the real-time configuration, the user can master whether the operation status of the aeration pump, push rod, and sensor is normal. The alarming threshold and aeration time of dissolved oxygen can be set in the upper computer. The communication between the upper and lower computers is carried out through the data transmission module GRM DTU203G. The remote monitoring and control can also be carried out by a mobile APP accessing the cloud server. The data collected by the system can be stored in the database of the upper computer and the cloud server at the same time, and the synchronization control and display can be realized. Because of the 4G Internet of Things mode, the system can realize the networked monitoring of multiple terminals and realize the integrated monitoring and task scheduling for multiple mobile monitoring platforms only on one upper computer. In this way, multiple mobile monitoring platforms work in parallel and can realize the monitoring, sampling, and aeration of large-scale water.Figure 2
Composition of the sensors monitoring system.


## 3. Waypoint Trajectory-Tracking Control Algorithm

The marine environment is extremely complex, and the external factors such as wind, waves, and current bring challenges to the intelligent design level of the control system of USV [19, 20]. Also, the performance of a USV is influenced by the surface currents, risk of collision with the civilian traffic, and varying depths due to tides and weather [21]. In [22], the performance of a novel control strategy for imperfect systems associated with physical realizations is investigated, which takes advantage of the unavoidable imperfections. In [23], we have investigated the waypoints trajectory-tracking problem for unmanned surface vehicle with environment disturbance. An upper-triangle and diagonal matrix decomposition method of Unscented Kalman Filter for the environment noise is proposed. The estimated states are adopted to design state feedback control laws to achieve the finite-time stabilization. In this paper, in order to improve the adaptability of the USV to complex ocean conditions, a distributed intelligent control system is designed to improve the robustness of the control system to external disturbances. By designing the closed-loop feedback system composed of a shore station control terminal, on-board controller, navigation and positioning sensor, and driving motor, the closed-loop feedback servo controller of the control system of USV is realized. The specific process is kinematic modeling of USV, waypoint trajectory-tracking control, distributed controller, simulation of tracking control method, and verification of software and hardware design. According to the principle of functional modular design and conducive to all kinds of sensors for data measurement and communication, each functional module is designed. The designing of the monitoring and control system can be divided into two parts: the control hardware designing and the control software designing. Considering the mechanical structure of USV, the hardware equipment layout is carried out to meet the requirements of installation space and internal structure layout of the USV.Firstly, two coordinate frames are adopted in this paper, which are inertial reference frame and body reference frame of the USV, respectively. The origin of the inertial reference frame is the coordinates of the starting point of the USV. The state vectors in the inertial frame are positions and steering angle of the USV, which are shown in Figure3.Figure 3
Control system modeling for trajectory-tracking control.After modeling the control system for trajectory-tracking control, the kinematics equation of the USV is further described as follows [12]:(1)Σ1:p˙=x˙y˙θ˙=cosθ0sinθ001q,yc=p,where p=x,y,θT denotes the states vector of the USV, x,y denotes the position coordinates in inertial frame, q=ν,ωT denotes the control input, θ=arctany˙t/x˙t denotes the heading angle, and ν and ω denote the linear velocity and angular velocity, respectively. yc denotes the output of the control system.The trajectory-tracking problem of USV is described as the one how to design a control law aboutν and ω to make the control system of the USV to track the reference trajectories xd,yd,θdT and the reference velocities νd and ωd.So, in the body-ﬁxed reference frame, the error-based control system model shown in Figure3 is given.(2)pe=xeyeθe=cosθsinθ0−sinθcosθ0001xd−xyd−yθd−θ=Texd−xyd−yθd−θ,where Te is the transformation matrix in the inertial reference frame with respect to the body-ﬁxed frame. Furthermore, we take the time-derivative along the solution of the system (2), so the kinematics equation of error-based model is rewritten as(3)Σe:x˙e=ωye−ν+νdcosθe,y˙e=−ωxe+νdsinθe,θ˙e=ωd−ω.Actually, because of the external sensors noise and internal system noise, the control system is easily influenced by them. In order to reject the noise influence on the trajectory-tracking system, a fully observable states based filtering estimation are considered in this paper. The output of the error system isye=pe, so the error system equations with external and internal noise is written as(4)p˙ev=ωye−ν+νdcosθe−ωxe+νdsinθeωd−ω+W,yev=pe+V,where W and V are the system noise and measurement noise, respectively, which are both Gaussian white noises. The covariance matrix of W and V are Q and R, respectively. The vector p^ denotes the estimated value of vector p, vector p^e denotes the estimated value of pe, and y^e denotes the estimated value of ye. Therefore, the stable error system equation with estimations is written as(5)p^˙e=ω^y^e−ν^+νdcosθ^e−ω^x^e+νdsinθ^eωd−ω^,y^e=p^e,To carry out the rapid reach the predefined trajectory of the USV, we study the finite-time tracking control law. Also, the ﬁnite-time trajectory-tracking problem is how to ﬁnd an appropriate velocity control laws aboutν^ and ω^ of the following form.(6)ν^=fx^e,y^e,θ^e,νd,ωd,ω^=gx^e,y^e,θ^e,νd,ωd,where f⋅ is the nonlinear function on x^e,y^e,θ^e,νd,andωd. Also, g⋅ is the nonlinear function on x^e,y^e,θ^e,νd,andωd. For arbitrary initial errors x^e0,y^e0,θ^e0T, the closed-loop trajectories of equation (5) can be stabilized in finite time.The designed block structure of the controlled error system is illustrated in Figure4. The reference trajectory vector pd is the output of the reference system which is controlled by the control input qd. So, the system block structure of Figure 4 is further transformed into the one shown in Figure 5.Figure 4
The control block for the USV trajectory-tracking.Figure 5
The translated control block for the USV trajectory-tracking.In the Figures4 and 5, NC denotes the nonlinear controller, Σ1 is the controlled system (2), Σe is the controlled error system, and Σd denotes the following reference kinematics system of the USV.(7)p˙d=x˙dy˙dθ˙d=cosθd0sinθd001qd.For the control system with system noise and sensors noise, we can design the following control laws aboutν^ in equation (7) and ω^ in equation (8). The control law of ν^ is designed as(8)ν^=νdcosθ^e+ωdy^e−k1sgny^ey^eβ1−k2sgnx^ex^eβ2,whenθ^esgnθ^e>k4k3,νdcosθ^e−k3sgnx^ex^eβ2,whenθ^esgnθ^e≤k4k3.The control law ofω^ is designed as(9)ω^=ωd−k1θ^e−k3sgnθ^e,whenθ^esgnθ^e>k4k3,ωd−k1vdy^esgny^ey^eβ1−k4vdsinθ^e,whenθ^esgnθ^e≤k4k3.withk1>0,k2>0, k3<0,k4<0, and 0<β1<1,β2=2β1/1+β1.With the control law (7) and (8), the close-loop error system is transformed as(1)
Whenθ^e>k4/k3,(10)p^˙e=fx^e,y^e,θ^e,=k1sgny^ey^eβ1+−k3θ^e−k4sgnθ^eye+k2sgnx^ex^eβ2νdsinθ^e−ωd−k1θ^e−k2sgnθ^ex^e+k1θ^e+k2sgnθ^e,y^e=hx^e,y^e,θ^e=p^e.(2)
Whenθ^e≤k4/k3,(11)p^˙e=fx^e,y^e,θ^e,=ωd−k1vdy^esgny^ey^eβ1−k4vdsinθ^ey^e+k3x^e−ωd−k1vdy^esgny^ey^eβ1−k4vdsinθ^ex^e+νdsinθ^e+k3vdy^e+k4vdsinθ^e,y^e=hx^e,y^e,θ^e=p^e.Then, the filtering algorithm can be used to filter the system noise and sensors noise. Then, we can obtain the estimated values of the error system states. With the estimation of error system states, we can further obtain the feedback control laws of the error system based on the estimated states.


## 4. Implement and Experiment

### 4.1. Motion Controller

The core processor is STM32 processor, the power supply voltage is 5VDC, with multi PWM interface and two serial ports, and the on-board temperature sensor. The RS232 serial port is used to communicate the main controller of the lower computer with the STM32 processor which receives the control command from the main controller of the lower computer. Then, it unpacks the information according to the predefined protocol format, extracts the propeller speed command of the corresponding control field, and generates two PWM control signals according to the propeller speed command, which are sent to the electronic governors of the two channel underwater propellers through the PWM port, and then controls the propulsion. The motor rotates according to the given speed. At the same time, the collected lithium battery voltage and the output current signal of electric regulation are packaged according to the predefined communication protocol format and sent to the main controller of the lower computer through the serial port. Two 1500 W ducted thrusters are used in the propulsion device which can be used for forward and reverse propulsion, and differential speed regulation is carried out by using the forward and reverse rotation of the thrusters. There are three control signals, two of which are 5 V·DC power supply and the other is PWM control signal. The driving module adopts the 120 A bidirectional electric regulation module, and the power supply voltage is 24 V–50 VDC, with three-phase output. It receives the PWM signal from the lower motion controller. After receiving the control command from the main controller of the lower computer, the corresponding control information of the protocol is extracted. If the control information is “A,” it is the autonomous control mode. If the control information is “M,” it is the manual remote-control mode. The duration of PWM signal high level is 1000 us–2000 us, 1500 us denotes stopping, less than 1500 us means the thruster is reversed, and more than 1500 us indicates the propeller is positive turn. The multistage forward and reverse control of the propeller is realized by dividing 1000–2000 into several segments. The monitoring software of the upper computer is installed on the ARK2250 control board of the onboard controller. The 5.8 G wireless bridge terminal can receive measurement and control data within 6 km of the water surface. The motion control software of the upper computer is written in C++ language in MOOS-Ivp [24] and Ubuntu system. The waypoints tracking control result and heading and yaw control using MOOS-IvP pMarineViewer, pHelmivp, and pCompass, et al. modules.The tracking control experiment was carried out at the shore experimental station of SDIOI. The experiment of the tracking control algorithm carried out inpAct module of MOOS is shown in Figure6. The tracking effect of the proposed control laws is shown in Figure 7. The monitoring parameters such as temperature, humidity, and voltage with variables SENSOR_tem, SENSOR_hum, and SENSOR_vol in MOOS-IvP alogview are shown in Figures 8 and 9.Figure 6
Shore experiment of USV.Figure 7
The waypoints tracking control result, nav_heading, and desired_heading plots in MOOS-IvP alogview.Figure 8
The monitoring parameters plots of SENSOR_tem (a), SENSOR_hum (b), and SENSOR_vol (c) in MOOS-IvP alogview.
(a)(b)(c)Figure 9
Control interface of APP.


### 4.2. Dissolved Oxygen Control

The minimum value of dissolved oxygen can be set according to the actual situation of the water body. In the software and APP, the default value of minimum dissolved oxygen is defined as 5 mg/L. The start and stop of the oxygen enrichment equipment is based on the average value of dissolved oxygen detected by the sensor within 5 minutes. When the average value of dissolved oxygen is lower than 5 mg/L, the oxygen enrichment equipment will start and run automatically. When the average value of dissolved oxygen in this period is higher than 5.5 mg/L, it will stop. When the dissolved oxygen is lower than 5 mg/L again, it will start again until the accumulated time is 30 minutes. Therefore, the dissolved oxygen in the water is not less than 5 mg/L, and the self-purification capacity of the water is forced to be improved. The minimum value of dissolved oxygen and automatic aeration time can be set on the remote terminal. The minimum value of dissolved oxygen can be set to 4 mg/L, and the maximum value can be set to 6 mg/L. Moreover, the dissolved oxygen value when the aeration pump is shut down is 0.5 mg/L higher than the minimum value, which can prevent frequent start-up and stop of the aeration device. The maximum aeration time is 30 min, and the minimum aeration time is 30 min. Through the 4G Internet of Things technology, the remote interconnection and monitoring of multiple monitoring USV platforms are realized. Through the software configuration, the functions of remote start and stop, and automatic water sampling system fault alarming are realized, which can provide users with accurate data on site and improve the work efficiency.


## 4.1. Motion Controller

The core processor is STM32 processor, the power supply voltage is 5VDC, with multi PWM interface and two serial ports, and the on-board temperature sensor. The RS232 serial port is used to communicate the main controller of the lower computer with the STM32 processor which receives the control command from the main controller of the lower computer. Then, it unpacks the information according to the predefined protocol format, extracts the propeller speed command of the corresponding control field, and generates two PWM control signals according to the propeller speed command, which are sent to the electronic governors of the two channel underwater propellers through the PWM port, and then controls the propulsion. The motor rotates according to the given speed. At the same time, the collected lithium battery voltage and the output current signal of electric regulation are packaged according to the predefined communication protocol format and sent to the main controller of the lower computer through the serial port. Two 1500 W ducted thrusters are used in the propulsion device which can be used for forward and reverse propulsion, and differential speed regulation is carried out by using the forward and reverse rotation of the thrusters. There are three control signals, two of which are 5 V·DC power supply and the other is PWM control signal. The driving module adopts the 120 A bidirectional electric regulation module, and the power supply voltage is 24 V–50 VDC, with three-phase output. It receives the PWM signal from the lower motion controller. After receiving the control command from the main controller of the lower computer, the corresponding control information of the protocol is extracted. If the control information is “A,” it is the autonomous control mode. If the control information is “M,” it is the manual remote-control mode. The duration of PWM signal high level is 1000 us–2000 us, 1500 us denotes stopping, less than 1500 us means the thruster is reversed, and more than 1500 us indicates the propeller is positive turn. The multistage forward and reverse control of the propeller is realized by dividing 1000–2000 into several segments. The monitoring software of the upper computer is installed on the ARK2250 control board of the onboard controller. The 5.8 G wireless bridge terminal can receive measurement and control data within 6 km of the water surface. The motion control software of the upper computer is written in C++ language in MOOS-Ivp [24] and Ubuntu system. The waypoints tracking control result and heading and yaw control using MOOS-IvP pMarineViewer, pHelmivp, and pCompass, et al. modules.The tracking control experiment was carried out at the shore experimental station of SDIOI. The experiment of the tracking control algorithm carried out inpAct module of MOOS is shown in Figure6. The tracking effect of the proposed control laws is shown in Figure 7. The monitoring parameters such as temperature, humidity, and voltage with variables SENSOR_tem, SENSOR_hum, and SENSOR_vol in MOOS-IvP alogview are shown in Figures 8 and 9.Figure 6
Shore experiment of USV.Figure 7
The waypoints tracking control result, nav_heading, and desired_heading plots in MOOS-IvP alogview.Figure 8
The monitoring parameters plots of SENSOR_tem (a), SENSOR_hum (b), and SENSOR_vol (c) in MOOS-IvP alogview.
(a)(b)(c)Figure 9
Control interface of APP.


## 4.2. Dissolved Oxygen Control

The minimum value of dissolved oxygen can be set according to the actual situation of the water body. In the software and APP, the default value of minimum dissolved oxygen is defined as 5 mg/L. The start and stop of the oxygen enrichment equipment is based on the average value of dissolved oxygen detected by the sensor within 5 minutes. When the average value of dissolved oxygen is lower than 5 mg/L, the oxygen enrichment equipment will start and run automatically. When the average value of dissolved oxygen in this period is higher than 5.5 mg/L, it will stop. When the dissolved oxygen is lower than 5 mg/L again, it will start again until the accumulated time is 30 minutes. Therefore, the dissolved oxygen in the water is not less than 5 mg/L, and the self-purification capacity of the water is forced to be improved. The minimum value of dissolved oxygen and automatic aeration time can be set on the remote terminal. The minimum value of dissolved oxygen can be set to 4 mg/L, and the maximum value can be set to 6 mg/L. Moreover, the dissolved oxygen value when the aeration pump is shut down is 0.5 mg/L higher than the minimum value, which can prevent frequent start-up and stop of the aeration device. The maximum aeration time is 30 min, and the minimum aeration time is 30 min. Through the 4G Internet of Things technology, the remote interconnection and monitoring of multiple monitoring USV platforms are realized. Through the software configuration, the functions of remote start and stop, and automatic water sampling system fault alarming are realized, which can provide users with accurate data on site and improve the work efficiency.


## 5. Conclusion

Autonomous control system for mobile water quality monitoring, sampling, and oxygenation is designed. The proposed trajectory-tracking algorithm is carried for the heading control of USV. The tracking error is acceptable with proposed control law. The experiment shows that the USV can stop in the target location although the error is larger during its deploying. The control and monitoring system can transmit the collecting water quality data to the cloud server and PC with OPC server and share the water quality information with the public through the application program. It shows the start-stop exchange for dissolved oxygen enrichment is frequent in the domain of predefined value, and we should improve its control procedure to low the exchanging number using the proposed control algorithms. In addition, the cheap USV platform allows the fabrication of multiple USV units for simultaneous monitoring of water quality at multiple locations. Moreover, some consideration about the possibilities of using lab-on-chip innovative devices and equipment based on microfluidics should be made. In addition, the proposed USV platform can become an effective and interesting teaching tool for students in the field of marine robot’s motion control and autonomous navigation.



---

*Source: 3652329-2022-03-31.xml*