Underwater unmanned vehicle manual operation and automatic navigation switching control system and method

Abstract

The invention relates to a manual operation and automatic navigation switching control system and method for an unmanned underwater vehicle, and the system comprises a manual operation calculation controller, an automatic navigation controller, a switching controller and a logic actuator, is connected with a propeller and a steering engine through a CAN bus, and is used for converting an instruction outputted by the switching controller into a control signal, and transmitting the control signal to an execution mechanism. According to the invention, a set of efficient and reliable manual operation and automatic navigation switching mechanism is constructed, the manual operation resolving controller and the automatic navigation controller are arranged, and the switching controller is used for performing instruction switching by taking a manual operation signal as a high priority. The manual operation resolving controller receives an operator instruction through wireless communication and analyzes the operator instruction into a propeller value and a steering engine value, and the automatic navigation controller automatically calculates a control value according to data of various sensors. The switching controller can quickly and accurately write proper instructions into the logic executor according to actual conditions.

Description

Technical Field

[0001] The present invention relates to the technical field of the operation and control of unmanned underwater vehicles, in particular to a system and method for switching between manual operation and automatic navigation of an unmanned underwater vehicle. Background Art

[0002] As a kind of equipment that can independently carry out operations underwater, unmanned underwater vehicles play an important role in many fields such as ocean exploration, military reconnaissance, and underwater engineering. Usually, an unmanned underwater vehicle can operate independently underwater for a long time, and at the same time, it also has the function of being remotely controlled by an operator through radio. However, in the actual application process, there are many problems to be solved urgently in the existing switching control technology between manual operation and automatic navigation of unmanned underwater vehicles.

[0003] On the one hand, when an operator remotely controls an unmanned underwater vehicle through radio, the vehicle is extremely vulnerable to wave interference. Once the vehicle is submerged below the water surface by waves, the radio signal will suddenly be interrupted. At this time, devices such as propellers often remain in the on and uncontrolled state. In this case, the vehicle may deviate from the predetermined route due to loss of control, or even collide with other objects, causing serious damage to the vehicle itself and greatly threatening the safe execution of the navigation mission.

[0004] On the other hand, during the automatic navigation of the vehicle, various emergencies may occur. For example, an obstacle appears ahead, or the navigation environment suddenly changes. At this time, it is necessary for the vehicle to quickly surface, be directly controlled by the operator through radio, and quickly stop the automatic operation state. However, the existing technology is difficult to achieve a fast and reliable switch between manual operation and automatic navigation, resulting in the inability to respond to the operator's instructions in a timely manner in case of emergencies, increasing the operation risk of the vehicle.

[0005] In addition, the existing switching control technology also has deficiencies in signal detection, instruction priority processing, and device communication. For example, the signal detection is not accurate enough to timely and accurately judge whether manual operation intervenes and whether the signal is interrupted; the instruction priority setting is unreasonable, resulting in the inability to correctly execute high-priority instructions when multiple instructions exist simultaneously; the device communication method is not stable enough, affecting the accurate transmission and execution of instructions.

[0006] In summary, the existing switching control technology between manual operation and automatic navigation of unmanned underwater vehicles has problems such as equipment out-of-control when the signal is interrupted, inability to quickly switch in case of emergencies, and problems in signal detection, instruction processing, and communication. These problems seriously affect the safety and reliability of unmanned underwater vehicles, and an effective solution is urgently needed to overcome the above technical problems.

[0007] For this reason, we propose an underwater unmanned vehicle manual operation and automatic navigation switching control system and method. Summary of the Invention

[0008] In view of the above-mentioned disadvantages in the existing production technology, the applicant provides an underwater unmanned vehicle manual operation and automatic navigation switching control system and method. By constructing an efficient and reliable manual operation and automatic navigation switching mechanism, adopting accurate signal detection and judgment methods, and optimizing communication and device connection methods, the problems existing in the manual operation and automatic navigation switching control of underwater unmanned vehicles are effectively solved.

[0009] The technical solution adopted by the present invention is as follows:

[0010] An underwater unmanned vehicle manual operation and automatic navigation switching control system, characterized in that it includes: a manual operation solution controller, connected to a handle control computer through wireless communication, for receiving the digital signal of the manual operation handle and parsing it to generate a propeller speed command and a rudder angle command;

[0011] An automatic navigation controller, connected to a depth gauge, a navigation system, and a Doppler velocimeter through wired communication respectively, for calculating and generating an automatic control command based on the depth, position, attitude, and speed information of the vehicle; a switching controller, connected to the manual operation solution controller and the automatic navigation controller respectively, for selecting the propeller speed command and the rudder angle command according to the priority, wherein the command priority of the manual operation solution controller is higher than that of the automatic navigation controller;

[0012] A logic executor, connected to a thruster and a rudder through a CAN bus, for converting the command output by the switching controller into a control signal and sending it to the actuator.

[0013] In one embodiment, the wireless communication method is the UDP protocol, and the wired communication methods include the RS485 protocol and the RS232 protocol, wherein:

[0014] The automatic navigation controller is connected to a depth gauge through the RS485 protocol to obtain depth and depth change rate information;

[0015] Connected to a navigation system through the RS232 protocol to obtain position, heading angle, and pitch angle information;

[0016] Connected to a Doppler velocimeter through the RS232 protocol to obtain speed information.

[0017] In one embodiment, the priority determination logic of the switching controller includes:

[0018] Set a manual operation intervention flag and a propeller shutdown flag, and periodically accumulate the flag values through a timer;

[0019] When the manual operation intervention flag does not exceed the set threshold, the automatic navigation controller outputs an instruction;

[0020] When a manual operation signal is detected, the manual operation intervention flag is forced to zero to ensure that the manual operation instruction is executed first;

[0021] Among them, the timing loop task period is 1 second, the automatic navigation control task period is 300 milliseconds, the manual operation signal execution task period is 200 milliseconds, and the logic execution task period is 100 milliseconds.

[0022] In one embodiment, the connection between the logic actuator and the thruster and the rudder is in a straight-line CAN bus structure, where:

[0023] The male waterproof connectors of each device are connected to the female heads of adjacent devices, and the communication pin definitions of the male and female heads are reversed;

[0024] The waterproof connector plugs at both ends of the bus are built-in with 120Ω terminal resistors.

[0025] A method for switching between manual operation and automatic navigation of an underwater unmanned vehicle, which uses the above-mentioned underwater unmanned vehicle manual operation and automatic navigation switching control system, and is characterized by including:

[0026] Horizontal tail rudder control: Based on the double combined parallel PID control of depth and longitudinal inclination, output a rudder angle instruction;

[0027] Vertical tail rudder control: Based on the PD control of heading and heading change rate, output a rudder angle instruction;

[0028] Propeller control: Based on the feedforward + PID feedback control of the ship speed, output a rotational speed instruction.

[0029] In one embodiment, the method of the horizontal tail rudder control is: adopting the control method of the double combined parallel PID of the vehicle depth and longitudinal inclination, which respectively uses the PD control loops of the submersible depth and depth change rate and the PD control loops of the longitudinal inclination and longitudinal inclination change rate for accumulation to control the vehicle depth, and finally the integral control of the accumulated depth is used to eliminate the depth net difference, and its expression is as described in the following formula:

[0030]

[0031] Among them: are the proportional and differential coefficients of the PD control loops of depth and depth change rate respectively, are the proportional and differential coefficients of the PD control loops of longitudinal inclination and longitudinal inclination change rate respectively, is the integral coefficient of depth integration, z d 、θ dare the target depth and the target pitch angle, which are set values, z, θ, are the current depth, depth change rate, pitch angle, and pitch angle change rate information obtained by the depth gauge and the navigation system, and ΔT is the current sampling period.

[0032] In one embodiment, the method for controlling the vertical rudder is as follows: The PD control method of the submarine's heading and heading change rate is adopted, and its expression is as described in the following formula:

[0033]

[0034] Where: are the proportional and differential coefficients of the PD control loops of the heading and heading change rate respectively, and δ d is the target heading value, which is a set value, δ, are the current heading and heading change rate information obtained by the navigation system.

[0035] In one embodiment, the method for controlling the propeller is as follows: The method of feedforward control price + PID feedback control is adopted. Using the fitting relationship between the ship speed and the propeller speed in still water, the propeller speed required in the feedforward control is obtained, and then the ship speed, ship speed change rate, and integral of the ship speed deviation are used for PID feedback control. Its expression is as described in the following formula:

[0036]

[0037] Where: v d is the set ship speed, v is the current ship speed, are the PID control parameters, ΔT is the sampling time interval, and ξ is the propeller control amount.

[0038] In one embodiment, the priority order of the instructions executed by the logic executor is as follows:

[0039] Highest priority: Propeller and rudder servo instructions for executing tasks from the manual operation signal;

[0040] Medium priority: Instructions from the automatic navigation control task;

[0041] Lowest priority: Propeller shutdown instructions from the timed loop task.

[0042] In one embodiment, it also includes initializing the manual operation and automatic navigation switching control system of the underwater unmanned vehicle. The initialization method includes the following steps:

[0043] Set the propeller shutdown flag to zero;

[0044] Set the manual operation intervention flag to exceed the threshold, and force the initial state to be the automatic navigation mode;

[0045] Start the parallel timing loop task, manual operation signal execution task, automatic navigation control task, and logic execution task.

[0046] In one embodiment, when the manual operation signal execution task detects a manual operation instruction, it immediately resets the manual operation intervention flag and overwrites the output instruction of the automatic navigation controller.

[0047] When the logic executor receives any valid instruction, it zeros the propeller shutdown flag to prevent the timing loop task from sending a propeller shutdown instruction.

[0048] The beneficial effects of the present invention are as follows:

[0049] The structure of the present invention is compact, reasonable, and easy to operate. By constructing an efficient and reliable manual operation and automatic navigation switching mechanism, adopting accurate signal detection and judgment methods, and optimizing the communication and device connection methods, the problems existing in the switching control of manual operation and automatic navigation of underwater unmanned vehicles are effectively solved. The manual operation solution controller and the automatic navigation controller process manual and automatic instructions respectively, and the switching controller gives high priority to the manual operation signal for instruction switching to ensure that the vehicle can respond to manual control in a timely manner. The accurate signal detection and judgment method accurately judges the manual operation intervention and signal interruption situations by setting the manual operation intervention flag bit and the propeller shutdown flag bit, providing a reliable basis for instruction switching. The optimized communication and device connection method uses a one-line CAN bus and a watertight connector with specific pin definitions to ensure underwater sealing, communication stability, and expandability. The combination of these structures and methods enables the underwater unmanned vehicle to operate safely, stably, and reliably in various complex environments, greatly improving its practical application value and safety.

[0050] At the same time, the present invention also has the following advantages:

[0051] The present invention constructs a set of efficient and reliable manual operation and automatic navigation switching mechanisms. In the background technology, existing underwater unmanned vehicles are prone to out-of-control when the signal is interrupted. The present invention effectively solves this problem by setting a manual operation solution controller and an automatic navigation controller, and using the switching controller to give high priority to the manual operation signal for instruction switching. The manual operation solution controller receives the operator's instructions through wireless communication and parses them into propeller and rudder values, while the automatic navigation controller automatically calculates the control values based on various sensor data. The switching controller can quickly and accurately write the appropriate instructions into the logic executor according to the actual situation. For example, when the operator issues an instruction through the handle, the switching controller will immediately give priority to the instruction of the manual operation solution controller to ensure that the vehicle can respond to manual control in a timely manner, greatly improving the safety and controllability of the vehicle in complex environments and avoiding dangerous situations caused by signal interruption or untimely switching.

[0052] The present invention adopts a precise signal detection and judgment method, effectively overcoming the problem of inaccurate signal detection in the background art. By setting a manual operation intervention flag bit and a propeller shutdown flag bit, they are respectively used to detect whether manual operation intervenes and whether the signal is interrupted. The manual operation intervention flag bit adopts three operation modes: flag reset, flag increment by one, and flag threshold judgment, which can accurately update the flag status in each timing loop task and manual operation signal execution task, so as to judge whether manual operation intervenes. The propeller shutdown flag bit also adopts a similar operation mode to update the status in each timing loop task and logic execution task to judge whether the signal is interrupted. This precise detection method can timely detect the state changes of the vehicle, provide a reliable basis for subsequent instruction switching, ensure the stable operation of the vehicle in various situations, and improve the stability and reliability of the system.

[0053] The present invention optimizes the communication and device connection method, solving the problem of unstable device communication in the background art. The logic actuator, the thruster, and the steering gear are connected in a linear CAN bus manner, and two CAN port watertight connectors are set on each CAN bus device. By short-circuiting the terminals with the same communication definition, stable connection between devices is achieved. At the same time, a watertight plug made by modifying a 120-ohm resistor is connected to the terminals of the head and tail devices, further improving the communication stability. This connection method not only ensures the underwater sealing performance but also has good scalability, enabling convenient addition or replacement of devices. In addition, the specific pin definitions and connection methods of the female and male watertight connectors ensure the accuracy and reliability of signal transmission, effectively avoiding instruction transmission errors caused by communication problems and improving the overall performance of the vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 It is a diagram of the manual operation and automatic navigation switching device for the unmanned vehicle in the present invention.

[0055] Figure 2 It is a connection diagram of the linear CAN bus in the present invention.

[0056] Figure 3 It is a signal flow diagram of the manual operation and automatic navigation switching method for the unmanned vehicle in the present invention.

[0057] Figure 4 It is a detailed flow chart of the manual operation and automatic navigation switching method for the unmanned vehicle in the present invention.

[0058] Figure 5 It is a conversion diagram of converting the propeller and steering gear instructions into CAN bus underlying characters in the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0059] The following describes the specific embodiments of the present invention in conjunction with the accompanying drawings.

[0060] Embodiment 1

[0061] As Figure 1 - Figure 2 shown, the present invention provides an underwater unmanned vehicle manual operation and automatic navigation switching control system and method for the problems in the above-mentioned existing production technologies, which helps to solve the problem of rapid switching between manual operation and automatic navigation.

[0062] The underwater unmanned vehicle manual operation and automatic navigation switching device in this embodiment is composed of three independent arithmetic units and a switching controller. The three arithmetic units are respectively: a manual operation arithmetic controller, an automatic navigation controller, and a logic executor.

[0063] The manual operation arithmetic controller is connected to the handle control computer by wireless communication in the UDP manner. The handle control computer is connected to the manual operation handle. The operator converts the operation of the handle into a digital signal and inputs it into the handle control computer, and then the computer sends it to the manual operation arithmetic controller through wireless communication. The manual operation arithmetic controller parses the handle signal into the propeller and rudder values to be controlled and outputs them to the switching controller.

[0064] The automatic navigation controller is connected to the depth gauge in the RS485 manner to obtain the current depth and depth change rate information of the vehicle. It is connected to the navigation system in the RS232 manner to obtain the current position information and attitude information of the vehicle. The attitude includes heading angle, heading angle rate, pitch angle, and pitch angle rate information. It is connected to the Doppler velocimeter in the RS232 manner to obtain the current speed information of the vehicle. By automatic calculation, the propeller and rudder values to be controlled are obtained and output to the switching controller.

[0065] The switching controller selectively writes the propeller speed and rudder angle values of the manual operation arithmetic controller and the automatic navigation controller into the logic executor, and gives the signal of the manual operation arithmetic controller a high priority.

[0066] The logic executor sends the input propeller and rudder command values to the thruster and rudder through CAN communication. The connection method with the thruster and rudder adopts the one-word CAN bus method.

[0067] Specifically, as Figure 1 shown, the figure shows the underwater unmanned vehicle manual operation and automatic navigation switching device in the present invention. It can be seen from the figure that the device is composed of a manual operation arithmetic controller, an automatic navigation controller, a switching controller, and a logic executor.

[0068] The specific switching logic is as follows: The manual operation calculation controller is connected to the handle control computer via UDP through wireless communication. The handle control computer is connected to the manual operation handle. The operator converts the operation of the handle into digital signals and inputs them into the handle control computer, which then sends them to the manual operation calculation controller through wireless communication. The manual operation calculation controller parses the handle signals into the propellers and rudder servo values to be controlled and outputs them to the switching controller; The automatic navigation controller is connected to the depth gauge in the RS485 manner to obtain the current depth and depth change rate information of the vehicle. It is connected to the navigation system in the RS232 manner to obtain the current position information and attitude information of the vehicle. The attitude includes heading angle, heading angle rate, pitch angle, and pitch angle rate information. It is connected to the Doppler velocimeter in the RS232 manner to obtain the current speed information of the vehicle. The current propellers and rudder servo values to be controlled are obtained through automatic calculation and output to the switching controller; The switching controller selectively writes the propellers and rudder servo values of the manual operation calculation controller and the automatic navigation controller into the logic actuator, with the signal of the manual operation calculation controller having a higher priority; The logic actuator can send the input propeller and rudder servo command values to the thrusters and rudder servos through CAN communication, and the connection method with the thrusters and rudder servos adopts the one-line CAN bus method.

[0069] Meanwhile, in order to ensure underwater sealing and expandability in this embodiment. The feature of the one-line CAN bus is that two CAN port watertight connectors are respectively set on each CAN bus device. The two connectors consist of male and female connectors. The CAN communication definitions of each CAN port connector are CAN_H and CAN_L respectively. The terminals with the same communication definition of the two CAN port watertight connectors are short-circuited inside each device. When connecting the devices externally, the two connectors of each device are connected end to end. A watertight connector plug made of a 120-ohm resistor is connected to the terminals of the head and tail devices.

[0070] Specifically, as Figure 2As shown, a linear CAN bus connection diagram is presented. The pin 1 of the female waterproof connector is defined as CAH_H, and the pin 2 is defined as CAH_L. The pin 1 of the male waterproof connector is defined as CAH_L, and the pin 2 is defined as CAH_H. Inside each device, the terminals with the same communication definition of the two CAN port waterproof connectors are short-circuited, that is, the pin 1 of the female connector is connected to the pin 2 of the male connector, and the pin 2 of the female connector is connected to the pin 1 of the male connector. When connecting each device in a linear manner, the two connectors of each device are connected end to end, that is, the male and female connectors of each device are connected one by one through male-female extended waterproof cables, achieving the connection of the pin 1 of the male connector to the pin 2 of the female connector and the pin 2 of the male connector to the pin 1 of the female connector. Waterproof plugs with electrical characteristics for both male and female are set and inserted into the spare waterproof connectors of the head and tail devices respectively, and a 120Ω resistor is connected between the pins 1 and 2 of the plugs.

[0071] Embodiment 2

[0072] A method for switching between manual operation and automatic navigation of an underwater unmanned vehicle, mainly characterized by: it mainly consists of 1 detection and switching method and 4 operation tasks.

[0073] The 1 detection and switching method in this embodiment includes an instruction detection method and an instruction switching method. The detection method is used to detect whether manual operation intervenes and whether the signal is interrupted, and can be used to rank the priorities of the operation signal execution task instruction, the automatic navigation control task instruction, and the signal interruption shutdown propeller instruction in the timing loop task, and perform instruction switching according to the priority ranking. The specific ranking and switching method is as follows: the manual operation task instruction has the highest priority. When it exists, the automatic navigation task instruction is stopped from being executed, and the shutdown propeller instruction in the timing loop task is stopped; the automatic navigation task instruction has the second highest priority and starts to be executed when there is no manual operation instruction; the shutdown propeller instruction in the timing loop task has the lowest priority and is executed when there are no instructions for both of the above, closing the propeller.

[0074] The instruction detection method in this embodiment includes: a detection method for whether manual operation intervenes and a detection method for whether the signal is interrupted. The detection method for whether the signal is interrupted can be used to detect whether there is a manual operation task or an automatic execution task instruction, so as to distinguish between the manual operation task or the automatic execution task and the timing loop shutdown propeller task. The detection method for whether manual operation intervenes can be used to detect whether there is a manual operation task, so as to distinguish between the manual operation task and the automatic execution task.

[0075] The detection method for whether manual operation intervenes in this embodiment is mainly used to quickly detect whether manual remote control intervenes, and then quickly cut off the automatic task execution. The detection of whether manual operation intervenes is achieved by setting a manual operation intervention flag bit. There are three operation modes for the manual operation intervention flag: flag reset to zero, flag increment by one, and fetch flag threshold judgment. In each timed loop task, the flag is incremented by one. In each manual operation signal execution task, the flag is reset to zero each time a manual operation instruction is received. In the automatic navigation control task, the flag is fetched each time, and then it is queried whether the manual operation intervention flag exceeds the set threshold. The judgment method for whether manual remote control intervenes is that when the manual operation intervention flag is 0, it means that manual remote control is intervening; when the manual operation intervention flag exceeds the set threshold, it means that there is no manual remote control intervention. The threshold is set to a number slightly greater than 0, which can be 2, but should not be too large, representing a delay amount, that is, 2 seconds after manual operation stops intervening, it is considered that manual remote control stops intervening. The purpose of setting the delay amount is to prevent detection errors caused by inaccurate cycle periods of 4 operation tasks.

[0076] The detection method for whether the signal is interrupted in this embodiment is mainly used to quickly detect whether the signal is interrupted after the vehicle is submerged when being interfered by waves, so as to help quickly stop the propeller. After the signal is interrupted, the manual operation calculation task cannot receive the manual operation command sent by the handle control computer, that is, there is no manual operation task. The detection of whether the signal is interrupted is achieved by setting a propeller shutdown flag bit. There are three operation modes for the propeller shutdown flag: flag reset to zero, flag increment by one, and fetch flag threshold judgment. In each timed loop task, the flag is incremented by one. In each logic execution task, the flag is reset to zero. The judgment method for whether the signal is interrupted is that when the propeller shutdown flag is 0, the signal is not interrupted; when the propeller shutdown flag exceeds the set threshold, the signal is interrupted. The threshold is set to a number slightly greater than 0, which can be 3, but should not be too large, representing a delay amount, that is, 2 seconds after manual operation stops intervening (the flag has been incremented by one before each judgment), it is considered that the signal is interrupted. The purpose of setting the delay amount is to prevent detection errors caused by inaccurate cycle periods of 4 operation tasks.

[0077] The characteristics of the 4 operation tasks are that the slowest cycle period of the timed loop task is 1 second, the fastest cycle period of the logic execution task is 100 milliseconds, the cycle period of the manual operation signal execution task is the second fastest at 200 milliseconds, and the cycle period of the automatic navigation control task is the second fastest at 300 milliseconds.

[0078] The timed loop task is a task that executes in a fixed cycle, with a cycle period of once per 1 second. In each cycle period, the manual operation intervention flag and the propeller shutdown flag are incremented by one. In each cycle period, it is judged whether the propeller shutdown flag exceeds the set threshold, and when it exceeds, the propeller is shut down. The feature of the timeout shutdown propeller instruction is that its shutdown method is a periodic repeated sending mode according to the set threshold, rather than a continuous sending mode after exceeding the threshold, that is, after exceeding the set threshold, the propeller shutdown flag is set to zero, so that in the next timing period, it will not exceed the set threshold, and the propeller shutdown instruction will not be repeatedly sent, and it will be resent only after the flag bit accumulates to a certain extent, thus reducing the repeated waste of control resources.

[0079] The manual operation signal execution task is a task that parses and executes after receiving a remote control instruction signal. After receiving and correctly parsing the manual operation command, the manual operation flag is set to zero, and then the propeller rotation and servo rotation instructions are sent.

[0080] The automatic navigation control task is a task that can automatically operate the propeller and the servo to control the vehicle to navigate automatically according to the set course and depth target. The feature of the automatic navigation control task is that it judges whether the manual operation mode intervenes by whether the manual operation intervention flag exceeds the set threshold. The priority of the manual operation mode is higher than that of the automatic navigation mode. The automatic control task starts only when there is no manual operation intervention. Once the manual operation task intervenes, the automatic navigation control task stops immediately. The automatic navigation control task can compare the feedback information from the depth gauge and the navigation system with the set value, automatically calculate the required propeller rotation speed and the steering angle control amount to be controlled, and send relevant control instructions.

[0081] Specifically, as Figure 3 shown, it can be seen from the figure that the various signals are circulated among the 4 major tasks, namely the manual operation signal execution task, the timed loop task, the automatic navigation control task, and the logic execution task. The manual operation and automatic navigation switching control is carried out around the manual operation intervention flag and the propeller shutdown flag, and finally the logic execution task is responsible for implementing the execution. The instruction signals obtained by the logic execution task have three sources, which are arranged in descending order of priority as the instruction rotation speed and steering angle control instruction from the manual operation signal execution task, the instruction rotation speed and steering angle control instruction from the automatic navigation control task, and the propeller shutdown control instruction from the timed loop task.

[0082] The purposes of high, medium, and low priorities mean that if there are high-priority instructions, low-priority instructions will be ignored and only high-priority instructions will be executed. The methods for implementing high, medium, and low priorities are as follows: First, a timer increments the manual operation intervention flag and the propeller shutdown flag by 1 every 1 second; Second, the autonomous navigation control task checks every 300 milliseconds whether the manual operation intervention flag is greater than the set threshold. If it is greater, it executes the autonomous navigation task and sends command speed and rudder angle control commands to the logic execution task; Then, the manual operation signal execution task checks every 200 milliseconds whether there is a manual operation command. If there is, it sets the manual operation intervention flag to zero. Because the cycle of the manual operation signal execution task is faster than that of the timed loop task, if there is a manual operation task, the manual operation intervention flag will necessarily remain at zero and will not be greater than the set threshold, so the autonomous navigation control task will not be executed. Therefore, the priority of the manual operation signal execution task is higher than that of the autonomous navigation control task. Additionally, the timed loop task checks the propeller shutdown flag. When it is greater than the set threshold, it sends a propeller shutdown control command. When either the manual operation signal execution task or the autonomous navigation control task sends a command to the logic execution task, the logic execution task sets the propeller shutdown flag to 0. Since the cycle of the logic execution task is 100 milliseconds, which is the fastest, it can detect the manual operation signal and the autonomous navigation command without omission and then set the propeller shutdown flag to 0, preventing it from exceeding the set threshold. At this time, the timed loop task will not send a propeller shutdown control command, so its priority is the lowest.

[0083] In this embodiment, the autonomous navigation control task can automatically control the depth, heading, and speed of the vehicle by calculation, control the depth of the vehicle through the horizontal tail rudder, control the heading of the vehicle through the vertical tail rudder, and control the speed of the vehicle through the propeller.

[0084] The control method for the horizontal tail rudder to control the depth of the vehicle is as follows: The control method of the combined parallel PID of the vehicle depth and trim is adopted. It respectively uses the PD control loops of the submersible depth and depth change rate and the PD control loops of the trim angle and trim angle change rate for accumulation to control the vehicle depth, and finally the integral control of the accumulated depth is used to eliminate the depth net difference. Its expression is as follows.

[0085]

[0086] . Where: are respectively the proportional and differential coefficients of the PD control loops of the depth and depth change rate, are respectively the proportional and differential coefficients of the PD control loops of the trim angle and trim angle change rate, is the integral coefficient of the depth integral, z d 、θ d are the target depth and target trim angle, which are set values, z, θ、 are the current depth, depth change rate, pitch angle, and pitch angle change rate information obtained by the depth gauge and the navigation system, and ΔT is the current sampling period.

[0087] The control method for the vertical rudder to control the heading of the vehicle is: adopt the PD control method for the vehicle heading and heading change rate. Its expression is as follows.

[0088]

[0089] Where: are the proportional and differential coefficients of the PD control loops for the heading and heading change rate respectively, and δ d is the target heading value which is a set value, δ, are the current heading and heading change rate information obtained by the navigation system.

[0090] The control method for the propeller to control the vehicle speed is: adopt the feedforward control price + PID feedback control method. Using the fitting relationship between the vehicle speed and the propeller speed in still water, obtain the propeller speed required in the feedforward control, and then use the vehicle speed, vehicle speed change rate, and integral of the vehicle speed deviation for PID feedback control. Its expression is as follows.

[0091]

[0092] Where, v d is the set vehicle speed, v is the current vehicle speed, are the PID control parameters, ΔT is the sampling time interval, and ξ is the propeller control amount.

[0093] The logical execution task is to convert the received propeller and rudder rotation commands into CAN bus underlying characters after receiving them, and send relevant commands to the propeller and rudder through the CAN bus to control their rotation. The characteristic of the logical execution task is that it receives the hand operation signal execution task and the automatic navigation control task commands. After receiving the relevant commands for the propeller rotation, it will set the propeller shutdown flag to zero to prevent the timing loop task from shutting down the propeller after determining that the shutdown flag exceeds the set threshold. The method of converting the propeller and rudder rotation commands into CAN bus underlying characters is that one frame of the CAN bus underlying characters includes a frame address, a frame type, a message header, a control quantity, and a check bit. The address to be sent to the device is filled in the frame address and represented in hexadecimal as 0x01, 0x02, and 0x03 for the propeller, horizontal rudder, and vertical rudder respectively; the frame type is used to fill in whether this frame is an extended frame or a standard frame, represented by 1 and 0; the message header is used to indicate the start of the actual control quantity of this frame, represented by two bytes 0xFF and 0xAF; the control quantity is represented by two bytes for the rotational speed value of the propeller and the rudder angle value of the rudder. Among them, the rotational speed value of the propeller adopts a direct expression method, directly converting the rotational speed control command into a short integer type and storing it in the control quantity, with an accuracy of 1 RPM. The rudder angle value of the rudder is converted into a short integer type after being extended by 10 times and stored in the control quantity, with an accuracy of 0.1°; the check bit is obtained by accumulating all the previous bytes and storing the last byte.

[0094] In this embodiment, specifically, as Figure 5 shown, the frame address contains 4 bytes, and the address to be sent to the device is filled in and represented in hexadecimal as 0x01, 0x02, and 0x03 for the propeller, horizontal rudder, and vertical rudder respectively; the frame type contains 1 byte, which is used to fill in whether this frame is an extended frame or a standard frame, represented by 1 and 0; the message header contains 2 bytes, which is used to indicate the start of the actual control quantity of this frame, represented by two bytes 0xFF and 0xAF; the control quantity contains 2 bytes, representing the rotational speed value of the propeller and the rudder angle value of the rudder. Among them, the rotational speed value of the propeller adopts a direct expression method, directly converting the rotational speed control command into a short integer type and storing it in the control quantity, with an accuracy of 1 RPM. The rudder angle value of the rudder is converted into a short integer type after being extended by 10 times and stored in the control quantity, with an accuracy of 0.1°; the check bit is obtained by accumulating the 1st byte starting from the frame address to the 9th byte at the end of the control quantity and storing the last byte.

[0095] As Figure 4 shown, the specific switching method in this embodiment includes the following steps:

[0096] At the beginning, initialization work will be carried out first. Mainly, the propeller shutdown flag is set to zero, indicating that propeller shutdown protection is not carried out initially. Additionally, the manual operation intervention flag is set beyond the set threshold, indicating that the manual operation state has not started. After initialization, four tasks are created to execute in parallel, namely the timed loop task, the manual operation signal execution task, the autonomous navigation control task, and the logic execution task.

[0097] Within each 1-second loop cycle of the timed loop task, it is judged whether the timed time has arrived. When it has arrived, the manual operation intervention flag and the propeller shutdown flag are incremented by one. Then it is judged whether the propeller shutdown flag exceeds the set threshold. When it exceeds the threshold, the propeller shutdown flag is set to zero, and then a command to shut down the propeller is sent.

[0098] Within each 200-millisecond loop cycle of the manual operation signal execution task, first it is judged whether a manual operation command sent by the handle control computer via UDP communication through wireless communication is received. When the command is received, the command is parsed. After judging that the received command is correct, the manual operation intervention flag is set to zero, and then the propeller and rudder rotation commands that need to be sent for the manual operation are sent.

[0099] Within each 300-millisecond loop cycle of the autonomous navigation control task, first it is judged whether the manual operation intervention flag exceeds the set threshold. When it exceeds, it is judged that there is no manual operation intervention. The autonomous control task of automatically calculating the propeller and rudder values according to the set information is started. The autonomous navigation control task can automatically control the depth, heading, and speed of the vehicle through calculation. The depth of the vehicle is controlled by the horizontal tail rudder, the heading of the vehicle is controlled by the vertical tail rudder, and the speed of the vehicle is controlled by the propeller.

[0100] By adopting the control method of double combined parallel PID for the depth and trim of the vehicle, the PD control loops of the vehicle depth and depth change rate and the PD control loops of the trim angle and trim angle change rate are respectively used for accumulation to control the vehicle depth, and finally the integral control of the accumulated depth is used to eliminate the depth net difference.

[0101] By adopting the PD control method of the vehicle heading and heading change rate, the control method of controlling the vehicle heading by the vertical tail rudder is obtained.

[0102] At the same time, by adopting the method of feedforward control price + PID feedback control. Using the fitting relationship between the speed and the propeller speed in still water, the propeller speed required in the feedforward control is obtained. Then, using the speed, speed change rate, and integral of the speed deviation for PID feedback control, the control method of controlling the vehicle speed by the propeller is obtained.

[0103] After obtaining the horizontal tail rudder angle, vertical tail rudder angle, and propeller speed values through the above methods, a propeller rotation and rudder rotation command is sent to the logic execution task.

[0104] Within each 100 - millisecond cycle of the logical execution task, first, it is judged whether the commands for the propeller and servo rotation are received. After receiving the commands, the propeller shutdown flag is set to zero. Then, the commands for the propeller and servo rotation are converted into CAN - bus underlying characters. Finally, the commands are sent to the propeller and servo through the CAN bus.

[0105] In summary, through a series of detections in the manual operation and automatic navigation states, the rapid switching between automatic control and manual control is achieved, the rapid detection and then the cutting off of the propeller in case of signal loss are realized, ensuring the safety of the vehicle.

[0106] The above description is an explanation of the present invention, not a limitation of the invention. The scope defined by the present invention can be seen in the claims, and within the protection scope of the present invention, any form of modification can be made.

Claims

1. An underwater unmanned vehicle manual operation and automatic navigation switching control system, characterized in that, Comprising: A manual operation solving controller, connected to a handle control computer through a wireless communication method, for receiving digital signals of a manual operation handle and parsing to generate propeller rotation speed commands and rudder angle commands; An automatic navigation controller, connected to a depth gauge, a navigation system, and a Doppler velocimeter through a wired communication method respectively, for calculating and generating automatic control commands based on information such as the depth, position, attitude, and speed of the vehicle; A switching controller, connected to the manual operation solving controller and the automatic navigation controller respectively, for selecting the propeller rotation speed commands and rudder angle commands according to priorities, wherein the commands of the manual operation solving controller have a higher priority than those of the automatic navigation controller; A logic executor, connected to a thruster and a rudder through a CAN bus, for converting the commands output by the switching controller into control signals and sending them to the actuator.

2. The underwater unmanned vehicle manual and automatic navigation switching control system according to claim 1, wherein, The wireless communication method is the UDP protocol, and the wired communication method includes the RS485 protocol and the RS232 protocol, wherein: The automatic navigation controller is connected to the depth gauge through the RS485 protocol to obtain depth and depth change rate information; Connected to the navigation system through the RS232 protocol to obtain position, heading angle, and pitch angle information; Connected to the Doppler velocimeter through the RS232 protocol to obtain speed information.

3. An underwater unmanned vehicle manual and automatic navigation switching control system according to claim 1, characterized in that, The priority determination logic of the switching controller includes: Setting a manual operation intervention flag and a propeller shutdown flag, and periodically accumulating the flag values through a timer; When the manual operation intervention flag exceeds the set threshold, the automatic navigation controller outputs commands; When a manual operation signal is detected, the manual operation intervention flag is forced to zero to ensure the priority execution of manual operation commands; wherein, the period of the timing loop task is 1 second, the period of the automatic navigation control task is 300 milliseconds, the period of the manual operation signal execution task is 200 milliseconds, and the period of the logic execution task is 100 milliseconds.

4. An underwater unmanned vehicle manual operation and automatic navigation switching control system according to claim 1, characterized in that, The connection between the logic executor and the thruster and the rudder adopts a one-line CAN bus structure, wherein: The male waterproof connectors of each device are connected to the female connectors of adjacent devices, and the communication pin definitions of the male and female connectors are reversed; The waterproof connector plugs at both ends of the bus are internally provided with 120Ω terminal resistors.

5. A method for switching between manual operation and automatic navigation of an underwater unmanned vehicle, which uses a control system for switching between manual operation and automatic navigation of an underwater unmanned vehicle according to any one of claims 1-4, characterized in that, Comprising: Horizontal rudder control: Dual combined parallel PID control based on depth and pitch angle, outputting rudder angle commands; Vertical rudder control: PD control based on heading and heading change rate, outputting rudder angle commands; Propeller control: Feedforward + PID feedback control based on speed, outputting rotation speed commands.

6. The method for switching between manual operation and automatic navigation of an underwater unmanned vehicle according to claim 5, wherein The method of the horizontal rudder control is: Adopting the control method of dual combined parallel PID of the vehicle depth and pitch angle, which respectively uses the PD control loops of the submersible depth and depth change rate and the PD control loops of the pitch angle and pitch angle change rate to accumulate to control the vehicle depth, and finally accumulates the integral control of the depth to eliminate the depth net difference, and its expression is as described in the following formula: Wherein: are respectively the proportional and differential coefficients of the PD control loop for depth and depth change rate, are respectively the proportional and differential coefficients of the PD control loop for pitch angle and pitch angle change rate, is the integral coefficient of depth integration, z d and θ d are the target depth and target pitch angle, which are set values, z, θ, are the current depth, depth change rate, pitch angle, and pitch angle change rate information obtained by the depth gauge and the navigation system, and ΔT is the current sampling period.

7. A method for switching between manual operation and automatic navigation of an underwater unmanned vehicle according to claim 5, characterized in that, The method of the vertical rudder control is: Adopting the PD control method of the submersible heading and heading change rate, and its expression is as described in the following formula: Wherein: are respectively the proportional and differential coefficients of the PD control loops for the heading and the heading change rate, δ d is the target heading value which is a set value, δ, are the current heading and heading change rate information obtained by the navigation system.

8. A method for switching between manual operation and automatic navigation of an underwater unmanned vehicle according to claim 5, characterized in that The propeller control method is as follows: a method of using feedforward control + PID feedback control, utilizing the fitting relationship between the ship speed and the propeller speed in still water to obtain the propeller speed required in the feedforward control, and then using the ship speed, the rate of change of ship speed, and the integral of the ship speed deviation for PID feedback control. Its expression is as described in the following formula: where: v d is the set speed, v is the current speed, are the PID control parameters, ΔT is the sampling time interval, and ξ is the propeller control amount.

9. A method for switching between manual operation and automatic navigation of an underwater unmanned vehicle according to claim 5, It is characterized in that The priority order of the instructions executed by the logic executor is as follows: Highest priority: Propeller and rudder instructions for executing tasks from manual operation signals; Medium priority: Instructions from the automatic navigation control task; Lowest priority: Propeller shutdown instructions from the timed loop task.

10. A method for switching between manual operation and automatic navigation of an underwater unmanned vehicle according to claim 5, It is characterized in that It further includes initializing the control system for switching between manual operation and automatic navigation of the underwater unmanned vehicle. The initialization method includes the following steps: Set the propeller shutdown flag to zero; Set the manual operation intervention flag to exceed the threshold, forcing the initial state to be the automatic navigation mode; Parallelly start the timed loop task, the manual operation signal execution task, the automatic navigation control task, and the logic execution task; When the manual operation signal execution task detects a manual operation instruction, it immediately resets the manual operation intervention flag and overwrites the output instruction of the automatic navigation controller; When the logic executor receives any valid instruction, it zeros the propeller shutdown flag to prevent the timed loop task from sending propeller shutdown instructions.

Images