A seabed exploration tracked vehicle course angle correction control method, device and system

Abstract

This invention belongs to the field of tracked vehicle control technology, and specifically discloses a method, device, and system for controlling the heading angle correction of a tracked seabed exploration vehicle. It includes: acquiring the heading angle deviation θ0 in real time during the seabed exploration tracked vehicle's movement toward the target heading angle; and calculating the desired opening degree of the proportional valve based on the heading angle deviation θ0: ω. l =ω0+k l ·f(θ0), ω r =ω0-k r ·f(θ0); f(·) is the fuzzy control algorithm function, k l The gain is controlled by the proportional valve of the left track wheel, k. r ω0 is the control gain of the proportional valve for the right track wheel; ω0 is the base opening of the proportional valve; the openings of the proportional valves for the left and right track wheels are adjusted in real time according to the desired opening of the proportional valve. This invention can achieve real-time and precise control of the heading angle of a tracked underwater exploration vehicle, and can control the heading angle deviation of the tracked underwater exploration vehicle within ±1° during straight-line travel.

Description

Technical Field

[0001] This invention belongs to the field of tracked vehicle control technology, and more specifically, relates to a method, device and system for correcting the heading angle of a tracked vehicle for seabed exploration. Background Technology

[0002] Over the past few decades, marine technology has made great strides. Among various marine technologies, underwater robots play an important role in ocean exploration as an extremely important tool and powerful assistant. Tracked seabed exploration vehicles are one such example of underwater robots.

[0003] When tracked vehicles for seabed exploration perform tasks such as seabed dredging, mining, and exploration, they require heading angle control. These vehicles typically employ dual-motor drive to control forward, backward, and turning movements. Even with identical control inputs to the left and right track drive motors, the asymmetry in their structure (tension, manufacturing tolerances, etc.) and external loads inevitably prevents them from following a given trajectory, making precise heading angle control impossible. Existing heading angle control methods rely on underwater acoustic beacons or encoders on the track wheels. Underwater acoustic beacons provide coordinate information, allowing control of the vehicle's trajectory; encoders control the speed of the left and right track wheels, thus controlling forward, backward, and turning movements. Using underwater acoustic beacons can achieve large-scale, coarse trajectory and heading control, but as the depth of the mission increases, the error of the underwater acoustic beacons will increase, making it impossible to achieve localized, precise trajectory and heading angle control, and the cost is high. When using encoders for heading angle control, due to slippage when the tracked vehicle is moving, the speed of the track wheels collected by the encoder is not the actual speed of the track, so this method cannot accurately control the heading angle of the tracked vehicle. Summary of the Invention

[0004] In view of the above-mentioned defects or improvement needs of the existing technology, the present invention provides a method, device and system for correcting the heading angle of a tracked vehicle for seabed exploration, the purpose of which is to achieve real-time and accurate control of the heading angle of the tracked vehicle for seabed exploration.

[0005] To achieve the above objectives, according to a first aspect of the present invention, a method for correcting the heading angle of a tracked underwater exploration vehicle is proposed, comprising the following steps:

[0006] During the movement of the tracked underwater exploration vehicle toward the target, the heading angle deviation θ0 is acquired in real time.

[0007] Calculate the desired opening of the proportional valve based on the heading angle deviation θ0:

[0008] ω l=ω0+k l ·f(θ0)

[0009] ω r =ω0-k r ·f(θ0)

[0010] Where, ω l ω r Let f(·) represent the desired opening degrees of the proportional valves for the left and right track wheels, respectively; f(·) is the fuzzy control algorithm function, and k l k r These are the control gains of the proportional valves for the left and right track wheels, respectively; ω0 is the basic opening of the proportional valve.

[0011] Based on the desired opening degree of the proportional valve, the opening degree of the proportional valve of the track wheel is adjusted in real time to achieve the heading angle correction of the tracked vehicle for seabed exploration.

[0012] As a further preferred embodiment, the opening of the track wheel proportional valve is adjusted in real time according to the desired opening degree of the proportional valve, including the following steps:

[0013] Determine the desired opening ω of the proportional valve respectively l ω r Is it within the preset range [ω]? min ,ω max ]Inside:

[0014] If the desired opening of the proportional valve is within the preset range, no adjustment is made; if the desired opening of the proportional valve is less than the lower limit ω of the proportional valve opening... min Let it equal ω min If the desired opening degree of the proportional valve is greater than the upper limit ω of the proportional valve opening degree. max Let it equal ω max ;

[0015] The opening of the proportional valve of the track wheel is adjusted according to the desired opening of the proportional valve after adjustment.

[0016] As a further preferred embodiment, the control gain k of the left and right track wheel proportional valves is... l k r The method for determining k is as follows: conduct a heading angle control experiment in advance to obtain different k values. l and k r Under the given conditions, the heading angle control score of the tracked vehicle for seabed exploration is used, and the set with the highest control score is selected (k). l and k r As the final value.

[0017] As a further preferred method, the method for determining the heading angle control score of the seabed exploration tracked vehicle is as follows:

[0018]

[0019]

[0020] Where score is the heading angle control score of the tracked underwater exploration vehicle, and error is an intermediate variable; θ t It is the target heading angle, θ i is the actual heading angle of the i-th seabed exploration tracked vehicle, and n is the total number of times data was collected in the heading angle control experiment.

[0021] As a further preferred option, the target heading angle θ of the seabed exploration tracked vehicle is preset. t The actual heading angle θ of the tracked seabed exploration vehicle is obtained in real time using a compass, and the heading angle deviation θ0 = θ is calculated. t -θ.

[0022] According to a second aspect of the present invention, an apparatus is provided for implementing the above-described method for controlling the heading angle correction of a tracked underwater exploration vehicle, comprising a compass, a host computer, a controller, and a hydraulic motor driver, wherein:

[0023] The compass is installed at the front of the tracked vehicle to obtain the current heading angle of the tracked vehicle in real time and transmit the current heading angle to the host computer through the controller;

[0024] The host computer is used to calculate the heading angle deviation based on the current heading angle, determine the desired opening of the proportional valve, and then transmit the desired opening of the proportional valve to the controller.

[0025] The controller is used to adjust the opening of the track wheel proportional valve in real time through the hydraulic motor driver, so as to realize the heading angle correction of the seabed exploration tracked vehicle.

[0026] As a further preferred embodiment, the controller and the host computer are connected via an optical switch.

[0027] According to a third aspect of the present invention, a heading angle correction control system for a tracked seabed exploration vehicle is provided, comprising a processor for executing the above-described heading angle correction control method for a tracked seabed exploration vehicle.

[0028] According to a fourth aspect of the present invention, a computer-readable storage medium is provided having a computer program stored thereon, which, when executed by a processor, implements the above-described method for correcting the heading angle of a tracked seabed exploration vehicle.

[0029] In summary, compared with the prior art, the above-described technical solutions conceived by this invention mainly possess the following technical advantages:

[0030] 1. This invention obtains the heading angle deviation and designs a fuzzy control method to calculate the desired opening degree of the tracked wheel proportional valve. By adjusting the proportional valve opening, precise control of the heading angle is achieved, keeping the heading angle deviation of the tracked seabed exploration vehicle within ±1° during straight-line travel. Applying this invention to seabed exploration allows for automatic adjustment of the heading angle, ensuring no deviation occurs. It also enables safe passage through narrow, straight passages without collisions.

[0031] 2. The tracked vehicle used for seabed exploration contains left and right track drive motors. Due to structural asymmetry (tension, manufacturing tolerances, etc.) and the non-symmetrical load characteristics of the left and right tracks, even when the inputs controlling the left and right track drive motors are identical, their outputs are not the same. Therefore, this invention adds a track wheel proportional valve control gain coefficient in fuzzy control to eliminate the influence of structural asymmetry and further improve control accuracy. Attached Figure Description

[0032] Figure 1 This is a simplified model diagram of the tracked underwater exploration vehicle according to an embodiment of the present invention.

[0033] Figure 2 This is a schematic diagram of the heading angle correction device for a tracked underwater exploration vehicle according to an embodiment of the present invention;

[0034] Figure 3 This is a flowchart of the heading angle correction control method for a tracked underwater exploration vehicle according to an embodiment of the present invention. Detailed Implementation

[0035] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0036] like Figure 1 The image shows a model of a tracked underwater exploration vehicle provided in an embodiment of the present invention. The center of mass of the tracked underwater exploration vehicle is simplified to O, the forward speed is V, the geometric radius of the drive wheel is r, the distance between the two drive wheels is 2b, and the angle between the robot's forward direction and the Y-axis is θ. The clockwise direction is defined as positive, and the left side of the track's forward direction is defined as the left track, and the right side of the track's forward direction is defined as the right track. When the left track speed is greater than the right track speed, the tracked underwater exploration vehicle moves to the right front direction, and θ increases. When the left track speed is less than the right track speed, the tracked underwater exploration vehicle moves to the left front direction, and θ decreases.

[0037] Define the angular velocity ω of the left drive wheel Langular velocity ω of the right track wheel R Left track speed V L Right track speed V R The relationship between track speed and track wheel speed can be obtained:

[0038] V L =ω L ·2r#(1)

[0039] V R =ω R ·2r#(2)

[0040] Define the linear velocity V and angular velocity ω of O. According to formulas (1) and (2), we know that:

[0041]

[0042]

[0043] Right now:

[0044]

[0045] As the tracked underwater exploration vehicle moves to the right and forward, θ will increase. ω L -ω R >0, ω L >ω R To ensure that the tracked vehicle for seabed exploration maintains a straight line, θ must remain constant. ω needs to be reduced L Increase ω R This means reducing the opening of the left proportional valve and increasing the opening of the right proportional valve.

[0046] As the tracked underwater exploration vehicle moves to the left and forward, θ will decrease. ω L -ω R <0, ω L <ω R To ensure that the tracked vehicle for seabed exploration maintains a straight line, θ must remain constant. ω needs to be increased L Decrease ω R This means increasing the opening of the left proportional valve and decreasing the opening of the right proportional valve.

[0047] like Figure 2 As shown in the figure, a heading angle correction device for a tracked underwater exploration vehicle provided in an embodiment of the present invention includes: a surface part and an underwater part. The surface part includes: a host computer and an optical switch. The underwater part includes: an optical switch, a controller, a compass installed at the front of the tracked vehicle, and a proportional valve-controlled hydraulic motor drive device; wherein:

[0048] One host computer is used for human-computer interaction and internal calculations; one PLC is used to exchange information with the host computer and control the corresponding equipment of the seabed exploration tracked vehicle; two optical switches are used for conversion between network port signals and fiber optic signals; one compass is set up and installed at the front of the seabed exploration tracked vehicle to detect the current heading angle of the seabed exploration tracked vehicle; two proportional valve controlled hydraulic motor drive devices are used to control the forward, backward and turning of the seabed exploration tracked vehicle.

[0049] The device also includes fiber optic cables, through which the host computer and the controller are connected to transmit signals, allowing operators to remotely control the controller via the host computer.

[0050] Specifically, the host computer is placed on land, allowing operators to control the seabed exploration tracked vehicle from shore. The host computer and the controller communicate via optical fiber using the OPC_UA protocol. The host computer sends a network port signal, which is converted into an optical fiber signal by a surface optical switch and transmitted to an underwater optical switch. The underwater optical switch then converts the optical fiber signal back into a network port signal and transmits it to the controller. The controller collects gyrocompass data via an RS232 module and transmits the network port signal to the underwater optical switch. The underwater optical switch converts the network port signal back into an optical fiber signal, which is then transmitted to a surface optical switch. The surface optical switch converts the optical fiber signal back into a network port signal and sends it to the host computer. The gyrocompass is installed at the front of the tracked vehicle and connected to the RS232 module of the controller. A proportional valve-controlled hydraulic motor drive is used to control the forward, backward, and turning movements of the seabed exploration tracked vehicle. The proportional valve is connected to the AO module of the controller.

[0051] like Figure 3 As shown, this invention provides a method for correcting the heading angle of a tracked underwater exploration vehicle, implemented based on the aforementioned device. The method includes the following steps:

[0052] Step S1: Set the target direction of the seabed exploration tracked vehicle via the host computer: θ t Basic valve opening: ω0, upper limit of valve opening: ω max Valve opening lower limit: ω min Initially, let:

[0053] ω l =ω0

[0054] ω r =ω0

[0055] Where, ω l and ω r These refer to the opening degree of the proportional valves on the left and right track wheels of the seabed exploration tracked vehicle.

[0056] Step S2: Obtain the actual orientation of the seabed exploration tracked vehicle: θ using a compass.

[0057] Step S3: Obtain the angle deviation θ0 = θ through calculation by the host computer. t -θ, when θ0 > 0, represents θ t When the actual heading angle of the seabed exploration tracked vehicle is greater than θ, it is deviated to the left compared to the target heading angle. Therefore, ω needs to be added. l Decrease ω r When θ0 < 0, it represents θ t When the actual heading angle of the seabed exploration tracked vehicle is less than θ, it deviates to the right compared to the target heading angle. Therefore, ω needs to be reduced. l Increase ω r .

[0058] Step S4, in the host computer, connect θ0 and... Fuzzification is performed, followed by fuzzy inference, and the results are then defuzzified to correct the PID controller parameters; simultaneously, θ0 and... Input to the PID controller to obtain the output f(Δθ). Compare f(Δθ) with k l k r Perform calculations, and then compare these calculations with the initial proportional valve opening ω0 set by the host computer. Obtain the desired proportional valve opening:

[0059] ω l =ω0+k l ·f(θ0)

[0060] ω r =ω0-k r ·f(θ0)

[0061] Where f(·) is the fuzzy control algorithm function, k l The gain is controlled by the proportional valve of the left track wheel, k. r The gain is controlled by the proportional valve of the right track wheel.

[0062] Specifically, the tracked vehicle used for seabed exploration contains left and right track drive motors. Due to structural asymmetry (tension, manufacturing tolerances, etc.) and the non-symmetrical load characteristics of the left and right tracks, even with identical inputs, the outputs of the left and right track drive motors will differ. Therefore, to achieve identical output variations for both motors, their inputs must be different. To address this, this invention adds a track wheel proportional valve control gain coefficient after the controller. The control gain of the left track wheel proportional valve is k. l The control gain of the right track wheel proportional valve is k. r Its value can be obtained through the following method:

[0063] First, determine the basic PID control parameters and the fuzzy library rule table. Then, set up the heading angle control and scoring system for the tracked seabed exploration vehicle (the system input is the target heading angle and the actual heading angle during operation, and the output is the heading angle score of the tracked seabed exploration vehicle):

[0064]

[0065]

[0066] Where error is an intermediate variable, θ t It is the target heading angle data, θ i is the actual heading angle data of the i-th seabed exploration tracked vehicle collected, score is the heading angle control score of the seabed exploration tracked vehicle, the higher the score, the higher the control accuracy, and n is the number of times the data was collected.

[0067] Then, a heading angle control experiment was conducted, and different k values ​​were obtained based on the control scoring system. l and k r Under certain conditions, the heading angle control score of the tracked vehicle for seabed exploration is evaluated, and k is determined based on the control score results. l and k r .

[0068] Specifically, for the fuzzy control algorithm function, a fuzzy control library needs to be set, where e is the error and ec is the derivative of the error. When e is large, to accelerate the system's response speed and improve its tracking performance, and to prevent derivative overflow that may be caused by an instantaneous increase in e at the beginning, a larger Kp and a smaller kd should be chosen. When e is small, to reduce the steady-state error and improve the system's steady-state performance, a larger kp should be chosen. To avoid the output response oscillating around the setpoint, and considering the system's anti-interference performance, when ec is large, a smaller kd should be chosen. In some embodiments, the fuzzy control rules are shown in Tables 1 and 2, where NB represents negative large, NM represents negative medium, NS represents negative small, ZO represents zero, PS represents positive small, PM represents positive medium, and PB represents positive large.

[0069] Table 1. Fuzzy Control Rules for KP

[0070]

[0071] Table 2. Fuzzy Control Rules for KD

[0072]

[0073] Step S5: The host computer checks whether the opening degree of the proportional valves of the left and right track wheels is within the range [ω]. min ω max Between ], if ω l ω rIf all values ​​are within the interval, no adjustment is made; if ω l <ω min Let ω l =ω min If ω l >ω max Let ω l =ω max If ω r <ω min Let ω r =ω min If ω r >ω max Let ω r =ω max .

[0074] Step S6: The host computer sends the opening command of the proportional valve of the left and right track wheels to the controller.

[0075] In step S7, the controller controls the opening degree of the proportional valves of the left and right track wheels, thereby controlling the hydraulic motor drive device of the seabed exploration tracked vehicle to achieve control of the heading angle.

[0076] Those skilled in the art will readily understand that the above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for heading angle correction control of a seabed exploration tracked vehicle, characterized by, Includes the following steps: During the movement of the tracked underwater exploration vehicle toward the target, the heading angle deviation θ0 is acquired in real time. Calculate the desired opening of the proportional valve based on the heading angle deviation θ0: ω l = ω0+ k l · f(θ0) ω r = ω0- k r · f(θ0) where ω l , ω r are the desired opening of the proportional valve of the left and right track wheels respectively; f(·) is the fuzzy control algorithm function, k l , k r are the control gains of the proportional valve of the left and right track wheels respectively; ω0 is the basic opening of the proportional valve. Based on the desired opening degree of the proportional valve, the opening degree of the proportional valve of the track wheel is adjusted in real time to achieve the heading angle correction of the tracked vehicle for seabed exploration.

2. The seabed crawling tracked vehicle heading angle correction control method of claim 1, wherein, The opening of the proportional valve on the track wheel is adjusted in real time according to the desired opening degree, including the following steps: Determine the desired opening ω of the proportional valve respectively l ω r Is it within the preset range [ω]? min ,ω max ]Inside: If the desired opening degree of the proportional valve is within the preset range, then no adjustment is made to it; If the desired opening degree of the proportional valve is less than the lower limit ω of the proportional valve opening... min Let it equal ω min If the desired opening degree of the proportional valve is greater than the upper limit ω of the proportional valve opening degree. max Let it equal ω max ; The opening of the proportional valve of the track wheel is adjusted according to the desired opening of the proportional valve after adjustment.

3. The seabed crawling tracked vehicle heading angle correction control method of claim 1, wherein, The left and right track wheel proportional valves control the gain k l k r The method for determining k is as follows: conduct a heading angle control experiment in advance to obtain different k values. l and k r Under the given conditions, the heading angle control score of the tracked vehicle for seabed exploration is used, and the set with the highest control score is selected (k). l and k r As the final value.

4. The seabed crawling track vehicle heading angle correction control method of claim 3, wherein, The method for determining the heading angle control score of a tracked underwater exploration vehicle is as follows: Wherein, score is the score of the underwater crawler's heading angle control, error is an intermediate variable; θ t is the target heading angle, θ i is the i-th actual heading angle of the underwater crawler collected, and n is the total number of data collected in the heading angle control experiment.

5. The seabed crawling track vehicle course angle correction control method according to any one of claims 1 to 4, characterized in that, The target heading angle θ of the seabed exploration tracked vehicle is preset t The actual heading angle θ of the seabed exploration tracked vehicle is obtained in real time through a compass, and the heading angle deviation θ0=θ t -θ is calculated and obtained.

6. An apparatus for implementing the heading angle correction control method of the seabed exploration tracked vehicle according to any one of claims 1 to 5, characterized in that, Includes a compass, host computer, controller, and hydraulic motor driver, among which: The compass is installed at the front of the tracked vehicle to obtain the current heading angle of the tracked vehicle in real time and transmit the current heading angle to the host computer through the controller; The host computer is used to calculate the heading angle deviation based on the current heading angle, determine the desired opening of the proportional valve, and then transmit the desired opening of the proportional valve to the controller. The controller is used to adjust the opening of the track wheel proportional valve in real time through the hydraulic motor driver, so as to realize the heading angle correction of the seabed exploration tracked vehicle.

7. The apparatus of claim 6, wherein, The controller and the host computer are connected via an optical switch.

8. A seabed exploration tracked vehicle heading angle correction control system, characterized by, Includes a processor, the processor being used to execute the heading angle correction control method for a tracked seabed exploration vehicle as described in any one of claims 1-5.

9. A computer readable storage medium having stored thereon a computer program, characterized in that, When the computer program is executed by the processor, it implements the heading angle correction control method for the tracked seabed exploration vehicle as described in any one of claims 1-5.

Images