AUV (Autonomous Underwater Vehicle) autopilot based on universal controller

Abstract

The invention relates to the technical field of underwater navigation control, in particular to an AUV (Autonomous Underwater Vehicle) autopilot based on a universal controller. A user bottom plate is fixedly mounted at the bottom of an autopilot case; the power supply module is mounted on the user bottom plate; the plurality of communication boards are sequentially fixed at the upper part of the user bottom board; and the external interface is fixedly arranged on the outer side shell of the autopilot case and is used for providing a plurality of communication interfaces between the communication board and AUV (Autonomous Underwater Vehicle) external equipment. The plurality of communication boards comprise an acquisition board fixed on the upper part of the user bottom board; the control board is fixed at the upper part of the acquisition board; the I / O and communication expansion board is fixed at the upper part of the control board; and the serial port expansion board is fixed at the upper part of the I / O and communication expansion board. According to the AUV autopilot based on the universal controller, the AUV can work in a complex underwater environment, and various navigation control functions of the AUV can be realized.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater navigation control, and particularly to an AUV autopilot based on a general controller. Background Art

[0002] An autonomous underwater vehicle (AUV) is a cableless autonomous underwater robot, which is widely used in fields such as ocean exploration, ocean hydrological detection, underwater engineering, and underwater target processing. The autopilot is the control center of the AUV, similar to the "brain" of a human, and is mainly composed of two parts: autopilot hardware and autopilot software.

[0003] The AUV autopilot in the prior art cannot enable the AUV to operate in a complex underwater environment and cannot implement various navigation control functions of the AUV. Summary of the Invention

[0004] In view of the above-mentioned disadvantages of the prior art, the purpose of the present invention is to provide an AUV autopilot based on a general controller, which can implement various navigation control functions of the AUV autopilot and finally complete the mission.

[0005] To achieve the above object and other related objects, the present invention provides an AUV autopilot based on a general controller, including:

[0006] A user base plate, which is fixedly installed at the bottom of the autopilot chassis;

[0007] A power module, which is installed on the user base plate;

[0008] Multiple communication boards, which are sequentially fixedly arranged on the upper part of the user base plate;

[0009] An external interface, which is fixedly installed on the outer shell of the autopilot chassis and is used to provide a communication interface between the multiple communication boards and AUV external devices.

[0010] In an embodiment of the present invention, the multiple communication boards include:

[0011] An acquisition board, which is fixedly arranged on the upper part of the user base plate;

[0012] A control board, which is fixedly arranged on the upper part of the acquisition board;

[0013] An I / O and communication expansion board, which is fixedly arranged on the upper part of the control board;

[0014] A serial port expansion board, which is fixedly arranged on the upper part of the I / O and communication expansion board.

[0015] In an embodiment of the present invention, a socket strip is provided on one side inside the autopilot chassis for the user base plate, pin headers are provided on one side of the acquisition board, a socket strip is provided on the other side of the acquisition board, and the pin headers on one side of the acquisition board are inserted into the socket strip of the user base plate and fixed by copper pillars.

[0016] In an embodiment of the present invention, pin headers are provided on one side of the control board, a socket strip is provided on the other side of the control board, pin headers are provided on one side of the I / O and communication expansion board, a socket strip is provided on the other side of the I / O and communication expansion board, and pin headers are provided on one side of the serial port expansion board; the pin headers on one side of the control board are inserted into the socket strip on the other side of the acquisition board and fixed by copper pillars; the pin headers on one side of the I / O and communication expansion board are inserted into the socket strip on the other side of the control board and fixed by copper pillars; the pin headers on one side of the serial port expansion board are inserted into the socket strip on the other side of the I / O and communication expansion board and fixed by copper pillars.

[0017] In an embodiment of the present invention, the autopilot chassis is made of aluminum alloy material.

[0018] In an embodiment of the present invention, an autopilot system is further included, which is communicatively connected to the external interface.

[0019] In an embodiment of the present invention, the autopilot system includes:

[0020] A device layer, which is communicatively connected to the AUV external devices, is used to collect the device information of the AUV external devices and send it to the main control layer, and is used to send the relevant actions of the main control layer related to the task to the AUV external devices;

[0021] A main control layer, which is used to receive the task sent by the task layer, parse the task, and execute the relevant actions related to the task;

[0022] A task layer, which is used to send the task to the main control layer;

[0023] An algorithm layer, which is communicatively connected to the task layer and the main control layer, and is used to be called by the task layer and the main control layer.

[0024] In an embodiment of the present invention, the algorithm layer is used for motion control, and the algorithm layer includes a heading-keeping algorithm, a depth-keeping algorithm, an altitude-keeping algorithm, a speed-keeping algorithm, and an automatic path-tracking control algorithm.

[0025] In an embodiment of the present invention, the algorithm layer is used for task control: including formation control, path planning and formation control in the operation scenarios of multiple AUV external devices, target tracking solution control, and path planning in the target tracking scenario.

[0026] In an embodiment of the present invention, the heading-keeping algorithm, depth-keeping algorithm, altitude-keeping algorithm, speed-keeping algorithm, and automatic tracking control algorithm include:

[0027] Heading-keeping algorithm: Y T = k P (ψ - ψ d ) - k d ψ e ,

[0028] where Y T is the differential speed of the horizontal propulsion motor, k P is the proportionality coefficient, k d is the differential coefficient, ψ is the current heading angle, ψ d is the set heading angle, ψ e is the heading angular velocity;

[0029] Depth-keeping algorithm: δ = k1(dep - dep0) + k2θ,

[0030] Y T1 = k3(dep - dep0) + k4(dep1 - dep0) + k5(dep2 - dep0),

[0031] where δ is the horizontal rudder angle, k1, k2, k3, k4, k5 are proportionality coefficients, dep is the current depth of the submersible, dep0 is the set depth of the submersible, dep1 is the depth collected at the previous moment, dep2 is the depth collected at the moment before the previous moment, Y T1 is the vertical propulsion motor speed, and θ is the pitch angle of the submersible;

[0032] Altitude-keeping algorithm: Y T2 = k6(high - high0) + k7(high1 - high0) + k8(high2 - high0),

[0033] where k6, k7, k8 are proportionality coefficients, high is the current altitude of the submersible, high0 is the set altitude of the submersible, high1 is the altitude value at the previous moment, high2 is the altitude value at the moment before the previous moment, Y T2 is the vertical propulsion motor speed;

[0034] Speed-keeping algorithm:

[0035] Y T3 = k9(V - V0) + k 10 (V1 - V0) + k 11 (V2 - V0),

[0036] where k9, k 10 , k 11k is the proportionality coefficient, V is the current speed of the underwater vehicle, V0 is the set speed of the underwater vehicle, V1 is the speed at the previous moment, V2 is the speed at the moment before the previous moment, and Y T3 is the rotational speed of the horizontal propulsion motor;

[0037] Automatic tracking control algorithm:

[0038]

[0039] Among them, k 12 is the proportionality coefficient, k 13 is the differential coefficient, ψ is the current heading angle of the underwater vehicle, ψ e is the current heading angular velocity of the underwater vehicle, ψ d0 is the heading of the current planned route, ψ d1 is the heading of the line connecting the current point and the target waypoint, dx is the lateral distance to the target point, dy is the longitudinal distance to the target point, and Y T4 is the differential rotational speed of the horizontal propulsion motor, and a and b are weighting coefficients.

[0040] As described above, an AUV autopilot based on a general controller of the present invention has the following beneficial effects:

[0041] (1) The AUV autopilot based on a general controller of the present invention can enable the AUV to operate in a complex underwater environment and realize various navigation control functions of the AUV.

[0042] (2) The AUV autopilot based on a general controller of the present invention has a variety of general hardware interfaces and can be applied to various AUV control occasions.

[0043] (3) The AUV autopilot based on a general controller of the present invention can enable the autopilot software to receive superior tasks, decompose the tasks and then call various software function modules to realize various navigation control functions of the AUV, and finally complete the task mission. Brief Description of the Drawings

[0044] Figure 1 is a schematic structural diagram of an AUV autopilot based on a general controller provided by an embodiment of the present application.

[0045] Figure 2 is a schematic software architecture diagram of an AUV autopilot based on a general controller provided by an embodiment of the present application.

[0046] Description of Component Labels

[0047] 1 Autopilot chassis

[0048] 2 Power supply module

[0049] 3 Control board

[0050] 4 Acquisition Board

[0051] 5 I / O and Communication Expansion Board

[0052] 6 Serial Port Expansion Board

[0053] 7 User Baseboard

[0054] 8 External Interface

[0055] 9 Autopilot System Detailed Implementation Manner

[0056] The following illustrates the implementation manners of the present invention through specific examples. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific implementation manners. Various details in this specification can also be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that, without conflict, the following embodiments and the features in the embodiments can be combined with each other.

[0057] It should be noted that the diagrams provided in the following embodiments only illustrate the basic concept of the present invention in a schematic manner. Therefore, only the components related to the present invention are shown in the diagrams, rather than being drawn according to the number, shape, and size of the components in actual implementation. The types, quantities, and proportions of the components in actual implementation can be arbitrarily changed, and the component layout type may also be more complex.

[0058] To solve the problem that the existing AUV autopilot in the prior art cannot enable the AUV to operate in a complex underwater environment and cannot implement various navigation control functions of the AUV. The main functions of the housing of the AUV autopilot are to provide the installation mechanical interfaces for each internal circuit board and the external installation mechanical interface on the AUV, and to deploy the external electrical connection interface of the controller. The circuit board is divided into a baseboard and several daughter boards. The main function of the baseboard is to provide an electrical path for the electrical connection between the power board, CPU board, acquisition board, serial port expansion board, and I / O expansion board. The daughter boards are the CPU board, power board, acquisition board, I / O expansion board, serial port expansion board, etc. respectively. The autopilot software performs information interaction with devices such as motors, steering gears, collision avoidance processing devices, radio stations, underwater acoustic communication devices, underwater TVs, power control devices, pitching mechanisms, depth gauges, inertial navigation devices, and target handling devices through interfaces such as the CAN port, serial port, network port, and I / O port of the controller. After fusing multi-sensor data, it calls the motion control algorithm to achieve the navigation motion control of the AUV.

[0059] Please refer to Figure 1 , Figure 1Schematic diagram of the structure of an AUV autopilot based on a general controller provided by an embodiment of the present application. The present invention provides an AUV autopilot based on a general controller, including: an autopilot chassis 1, a power module 2, a control board 3, a collection board 4, an I / O and communication expansion board 5, a serial port expansion board 6, a user base plate 7, an external interface 8, and an autopilot system 9. The user base plate 7 is fixedly installed at the bottom of the autopilot chassis 1; the power module 2 is installed on the user base plate 7; a plurality of the communication boards are fixedly arranged on the upper part of the user base plate 7 in sequence; the external interface 8 is fixedly installed on the outer shell of the autopilot chassis 1, and is used to provide a communication interface between a plurality of the communication boards and AUV external devices. The collection board 4 is fixed on the upper part of the user base plate 7; the control board 3 is fixed on the upper part of the collection board 4; the I / O and communication expansion board 5 is fixed on the upper part of the control board 3; the serial port expansion board 6 is fixed on the upper part of the I / O and communication expansion board 5.

[0060] Specifically, the autopilot chassis 1 is made of aluminum alloy material, provides an installation mechanical interface for each internal circuit board and the power module 2 inside, provides an installation mechanical interface on the AUV external device outside, and deploys an external electrical connection interface on the shell. The power module 2 is installed in the internal card slot of the autopilot chassis 1, and mainly converts the input power into power of different voltage and power levels to provide working power for the operation of various AUV external devices.

[0061] Specifically, the user base plate 7 is fixedly installed at the bottom of the autopilot chassis 1, and its main function is to provide an electrical path for the electrical connection between the power board, the control board 3 (i.e., the CPU board), the collection board 4, the serial port expansion board 6, and the I / O and communication expansion board 5.

[0062] A socket strip is arranged on one side of the user base plate 7 inside the autopilot chassis 1. One side of the collection board 4 is provided with pin headers, and the other side of the collection board 4 is provided with a socket strip. The pin headers on one side of the collection board 4 are inserted into the socket strip of the user base plate 7 and fixed by copper posts. One side of the control board 3 is provided with pin headers, and the other side of the control board 3 is provided with a socket strip. One side of the I / O and communication expansion board 5 is provided with pin headers, and the other side of the I / O and communication expansion board 5 is provided with a socket strip. One side of the serial port expansion board 6 is provided with pin headers; the pin headers on one side of the control board 3 are inserted into the socket strip on the other side of the collection board 4 and fixed by copper posts; the pin headers on one side of the I / O and communication expansion board 5 are inserted into the socket strip on the other side of the control board 3 and fixed by copper posts; the pin headers on one side of the serial port expansion board 6 are inserted into the socket strip on the other side of the I / O and communication expansion board 5 and fixed by copper posts.

[0063] Specifically, the pin headers of the acquisition board 4 are directly inserted into the corresponding socket strips on the user motherboard 7 and are supported and fixed by copper pillars.

[0064] The CPU board is the core of the microcomputer controller. Its pin headers are directly inserted into the corresponding socket strips on the acquisition board 4 to achieve serial communication, network port communication, CAN port communication, and IO port control. At the same time, it supports multiple operating systems, including the DOS system, Reworks system, linux3.10.0 system, Vxworks6.8 system, etc. The CPU board is supported and fixed by copper pillars.

[0065] The pin headers of the I / O and communication expansion board 5 are directly inserted into the corresponding socket strips on the CPU board. Its I / O interfaces are connected to the user floor through a cable and are supported and fixed by copper pillars.

[0066] The pin headers of the serial port expansion board 6 are directly inserted into the corresponding socket strips on the I / O and communication expansion board. Its serial interfaces are connected to the user motherboard 7 through a cable, externally connected to the autopilot chassis 1 through the user motherboard 7, and are supported and fixed by copper pillars.

[0067] The external interface 8 is installed and fixed on the outer shell of the autopilot chassis 1 to provide an interconnection interface with the AUV external devices.

[0068] Please refer to Figure 2 , Figure 2 which is the schematic diagram of the software architecture of the AUV autopilot based on a general controller provided by the embodiment of the present application. It also includes an autopilot system 9, which is communicatively connected to the external interface 8.

[0069] The autopilot system 9 includes a device layer, a main control layer, a task layer, and an algorithm layer. The device layer is communicatively connected to the AUV external devices, used to collect the device information of the AUV external devices and send it to the main control layer, and used to send the relevant actions of the main control layer and tasks to the AUV external devices; the main control layer is used to receive the tasks sent by the task layer, parse the tasks, and execute the relevant actions of the tasks; the task layer is used to send the tasks to the main control layer; the algorithm layer is communicatively connected to the task layer and the main control layer, and is used to be called by the task layer and the main control layer.

[0070] The algorithm layer is used for motion control. The algorithm layer includes a heading-keeping algorithm, a depth-keeping algorithm, an altitude-keeping algorithm, a speed-keeping algorithm, and an automatic path-tracking control algorithm.

[0071] The algorithm layer is used for task control: including formation control, path planning and formation control in the operation scenarios of multiple AUV external devices, target tracking solution control, and path planning in the target tracking scenario.

[0072] Specifically, the heading-keeping algorithm, depth-keeping algorithm, altitude-keeping algorithm, speed-keeping algorithm, and automatic tracking control algorithm include:

[0073] Heading-keeping algorithm: Y T = k P (ψ - ψ d ) - k d ψ e ,

[0074] where Y T is the differential speed of the horizontal propulsion motor, k P is the proportionality coefficient, k d is the differential coefficient, ψ is the current heading angle, ψ d is the set heading angle, ψ e is the heading angular velocity;

[0075] Depth-keeping algorithm: δ = k1(dep - dep0) + k2θ,

[0076] Y T1 = k3(dep - dep0) + k4(dep1 - dep0) + k5(dep2 - dep0),

[0077] where δ is the horizontal rudder angle, k1, k2, k3, k4, k5 are proportionality coefficients, dep is the current depth of the submersible, dep0 is the set depth of the submersible, dep1 is the depth collected at the previous moment, dep2 is the depth collected at the moment before the previous moment, Y T1 is the vertical propulsion motor speed, and θ is the pitch angle of the submersible;

[0078] Altitude-keeping algorithm: Y T2 = k6(high - high0) + k7(high1 - high0) + k8(high2 - high0),

[0079] where k6, k7, k8 are proportionality coefficients, high is the current altitude of the submersible, high0 is the set altitude of the submersible, high1 is the altitude value at the previous moment, high2 is the altitude value at the moment before the previous moment, Y T2 is the vertical propulsion motor speed;

[0080] Speed-keeping algorithm:

[0081] Y T3 = k9(V - V0) + k 10 (V1 - V0) + k 11 (V2 - V0),

[0082] where k9, k 10 , k 11is the proportionality coefficient, V is the current speed of the submersible, V0 is the set speed of the submersible, V1 is the speed at the previous moment, V2 is the speed at the moment before the previous moment, and Y T3 is the rotational speed of the horizontal propulsion motor;

[0083] Automatic tracking control algorithm:

[0084]

[0085] where k 12 is the proportionality coefficient, k 13 is the differential coefficient, ψ is the current heading angle of the submersible, ψ e is the current heading angular velocity of the submersible, ψ d0 is the heading of the current planned route, ψ d1 is the heading of the line connecting the current point and the target waypoint, dx is the lateral distance to the target point, dy is the longitudinal distance to the target point, and Y T4 is the differential rotational speed of the horizontal propulsion motor, and a and b are weighting coefficients.

[0086] Specifically, the autopilot system 9 can be an autopilot software, which is divided into a task layer, a main control layer, a device layer, and an algorithm layer according to functions; the task layer is responsible for decomposing the task profile given by the superior and guiding the main control layer to execute relevant action processes; the main control layer is mainly responsible for implementing various specific functions and issuing action commands to the device layer; the device layer mainly realizes information interaction with external devices, obtains the AUV status information, and executes the commands of the main control layer; the algorithm layer exists in the form of modules, which are called by the task layer and the main control layer, calculates results according to requirements and outputs.

[0087] Task layer:

[0088] The task layer is in the form of a "director", decomposes the task profile given by the superior, guides the main control layer to execute relevant action processes, and achieves the task objectives of the task layer. The task layer has customized content and is not part of the general control software.

[0089] Main control layer:

[0090] The main control layer schedules data between each level or module in the form of a "supervisor". Its main functions are: distributing the tasks sent by the task layer to the algorithm layer; completing relevant action processes according to the requirements of the task layer and preparing for the execution of tasks by the task layer; data transfer between the device layer and the algorithm layer, and guiding the device layer to execute the instructions of the algorithm layer.

[0091] Device layer:

[0092] The device layer acts as a "worker" to interact with external devices, collect external information and execute the instructions of the main control layer. It mainly accomplishes the following: communicate with external devices, collect device information and transmit it to the main control layer; send the instructions of the main control layer to the devices and perform actions; transmit the control instructions of the command center and upload the status information of the AUV. The device layer includes communication modules for devices such as motors, servos, inertial navigation devices, Beidou positioning devices, collision avoidance processing devices, power control devices, underwater cameras, depth sensors, etc.

[0093] Algorithm layer:

[0094] The algorithm layer exists in the form of modules as an "advisor" for the task layer and the main control layer to call. It calculates results according to requirements and outputs them to achieve the following main functions:

[0095] Motion control: including speed holding, heading holding, depth holding, altitude holding, automatic path tracking navigation, collision avoidance control, hovering control, etc.

[0096] Task control: formation control, path planning and formation control in the scenario of multi-AUV operation, etc.; target tracking solution control, path planning in the target tracking scenario, etc.

[0097] In summary, the AUV autopilot based on a general controller of the present invention can enable the AUV to operate in a complex underwater environment and achieve various navigation control functions of the AUV. The AUV autopilot based on a general controller of the present invention has a variety of general hardware interfaces and can be applicable to various control occasions of AUVs. The AUV autopilot based on a general controller of the present invention can enable the autopilot software to receive superior tasks, decompose the tasks and then call various software function modules to achieve various navigation control functions of the AUV and finally complete the task mission.

[0098] The above embodiments merely illustrate the principles and effects of the present invention and are not used to limit the present invention. Any person familiar with this technology can modify or change the above embodiments without departing from the spirit and scope of the present invention. Therefore, all equivalent modifications or changes completed by those with ordinary knowledge in the technical field without departing from the spirit and technical ideas disclosed by the present invention should still be covered by the claims of the present invention.

Claims

1. An AUV autopilot based on a general controller, characterized in that, Including: A user base plate (7), which is fixedly installed at the bottom of the autopilot chassis (1); A power module (2), which is installed on the user base plate (7); Multiple communication boards, and the multiple communication boards are sequentially fixed to the upper part of the user base plate (7); An external interface (8), which is fixedly installed on the outer shell of the autopilot chassis (1) and is used to provide a communication interface between the multiple communication boards and AUV external devices.

2. The AUV autopilot based on a general controller according to claim 1, characterized in that, The multiple communication boards include: An acquisition board (4), which is fixed to the upper part of the user base plate (7); A control board (3), which is fixed to the upper part of the acquisition board (4); An I / O and communication expansion board (5), which is fixed to the upper part of the control board (3); A serial port expansion board (6), which is fixed to the upper part of the I / O and communication expansion board (5).

3. The AUV autopilot based on a general controller according to claim 2, characterized in that: A socket strip is provided on one side of the user base plate (7) inside the autopilot chassis (1). A row of pins is provided on one side of the acquisition board (4), and a socket strip is provided on the other side of the acquisition board (4). The row of pins on one side of the acquisition board (4) is inserted into the socket strip of the user base plate (7) and fixed by copper columns.

4. The AUV autopilot based on a general controller according to claim 3, characterized in that: A row of pins is provided on one side of the control board (3), a socket strip is provided on the other side of the control board (3), a row of pins is provided on one side of the I / O and communication expansion board (5), a socket strip is provided on the other side of the I / O and communication expansion board (5), and a row of pins is provided on one side of the serial port expansion board (6); The row of pins on one side of the control board (3) is inserted into the socket strip on the other side of the acquisition board (4) and fixed by copper columns; The row of pins on one side of the I / O and communication expansion board (5) is inserted into the socket strip on the other side of the control board (3) and fixed by copper columns; The row of pins on one side of the serial port expansion board (6) is inserted into the socket strip on the other side of the I / O and communication expansion board (5) and fixed by copper columns.

5. An AUV autopilot based on a general controller according to any one of claims 1 to 4, characterized in that: The autopilot chassis (1) is made of aluminum alloy material.

6. The AUV autopilot based on a general controller according to claim 5, characterized in that: It further includes an autopilot system (9), which is communicatively connected to the external interface (8).

7. The AUV autopilot based on a general controller according to claim 6, characterized in that, The autopilot system (9) includes: An equipment layer, which is communicatively connected to the AUV external equipment and is used to collect the equipment information of the AUV external equipment and send it to the main control layer, and is used to send the relevant actions of the main control layer related to the task to the AUV external equipment; A main control layer, which is used to receive the task sent by the task layer, parse the task, and execute the relevant actions related to the task; A task layer, which is used to send the task to the main control layer; An algorithm layer, which is communicatively connected to the task layer and the main control layer and is used to be called by the task layer and the main control layer.

8. The AUV autopilot based on a general controller according to claim 7, characterized in that: The algorithm layer is used for motion control. The algorithm layer includes a heading holding algorithm, a depth holding algorithm, an altitude holding algorithm, a speed holding algorithm, and an automatic tracking control algorithm.

9. The AUV autopilot based on a general controller according to claim 7, characterized in that: The algorithm layer is used for task control: including formation control, path planning and formation control in the operation scenarios of multiple AUV external devices, target tracking solution control, and path planning in the target tracking scenario.

10. The AUV autopilot based on a general controller according to claim 8, characterized in that, The heading-keeping algorithm, depth-keeping algorithm, altitude-keeping algorithm, speed-keeping algorithm, and automatic path-tracking control algorithm include: Course-keeping algorithm: Y T = k P (ψ - ψ d ) - k d ψ e , Among them, Y T is the differential speed of the horizontal propulsion motor, k P is the proportionality coefficient, k d is the differential coefficient, ψ is the current heading angle, ψ d is the set heading angle, ψ e is the heading angular velocity; Depth-keeping algorithm: δ = k1(dep - dep0) + k2θ, Y T1 = k3(dep - dep0)+k4(dep1 - dep0)+k5(dep2 - dep0), where δ is the horizontal servo angle, k1, k2, k3, k4, and k5 are proportionality coefficients, dep is the current depth of the submersible, dep0 is the set depth of the submersible, dep1 is the depth collected at the previous moment, dep2 is the depth collected at the moment before the previous moment, Y T1 is the rotational speed of the vertical propulsion motor, and θ is the pitch angle of the submersible; Height holding algorithm: Y T2 = k6(high - high0)+k7(high1 - high0)+k8(high2 - high0), where k6, k7, and k8 are proportionality coefficients, high is the current height of the submersible vehicle, high0 is the set height of the submersible vehicle, high1 is the height value at the previous moment, high2 is the height value at the moment before the previous moment, and Y T2 is the rotational speed of the vertical propulsion motor; Speed-keeping algorithm: Y T3 = k9(V - V0) + k 10 (V1 - V0) + k 11 (V2 - V0), where k9, k 10 , k 11 are proportionality coefficients, V is the current speed of the submarine, V0 is the set speed of the submarine, V1 is the speed at the previous moment, V2 is the speed at the moment before the previous moment, and Y T3 is the rotational speed of the horizontal propulsion motor; Automatic path-tracking control algorithm: where k 12 is a proportionality coefficient, k 13 is a differential coefficient, ψ is the current course angle of the submersible, ψ e is the current course angular velocity of the submersible, ψ d0 is the course of the current planned route, ψ d1 is the course of the line connecting the current point and the target waypoint, dx is the lateral distance to the target point, dy is the longitudinal distance to the target point, Y T4 is the differential speed of the horizontal propulsion motor, and a and b are weighting coefficients.

Images