A path tracking control method for underwater vehicle based on deviation angle

Abstract

The present invention relates to the field of underwater autonomous unmanned vehicle control technology, and in particular to an underwater vehicle route tracking control method based on deviation angle. The main difference between this method and the traditional route tracking control method is that the traditional underwater vehicle control method introduces position deviations from the starting point of the entire route to the end point of the route for deviation correction when tracking the last section of the route, and pays more attention to the control algorithm itself during control without considering the state of the underwater vehicle. The underwater vehicle route tracking control method based on deviation angle proposed by this method introduces the concept of position deviation angle when the underwater vehicle approaches the end point of the route (target point), and considers the heading and longitudinal navigation state of the underwater vehicle in the entire control process, thereby improving the stability of the underwater vehicle control and the accuracy of reaching the target point.

Description

Technical Field

[0001] The invention relates to the technical field of underwater autonomous unmanned vehicle control, and in particular to an underwater vehicle route tracking control method based on a deviation angle. Background Art

[0002] Autonomous underwater vehicles have become a hot research topic. Countries around the world are stepping up the research and development of the latest autonomous underwater vehicles to meet the increasing technical requirements in military and civilian fields. In the military field, they are widely valued because they can navigate outside the enemy's anti-submarine equipment and perform reconnaissance, detection and strike missions in enemy waters.

[0003] At present, various control algorithms such as neural network control, fuzzy control, sliding mode control and fuzzy PID control have been applied to the motion control of underwater vehicles. These control algorithms have their own advantages in different scenarios, but they also have shortcomings. The most obvious is that these control algorithms focus more on the algorithm itself, such as algorithm parameters, algorithm complexity and the actual control effect of the algorithm, and often ignore the state of the underwater vehicle itself. The navigation of underwater vehicles in the fluid such as water flow is a very complex six-degree-of-freedom motion. According to fluid mechanics, its motion state is very unstable. For example, when the underwater vehicle turns, it will roll at the same time. At this time, the upward force provided by the horizontal rudder surface decreases, resulting in an increase in the depth of the underwater vehicle, followed by a series of attitude-related changes. This attitude-related control problem is often not something that the algorithm itself can solve, which also brings great difficulty to its motion control. This control problem has a relatively small impact on the underwater vehicle before it tracks the route to the last segment, but when it navigates to the last segment, it may deviate from the final target point position. Therefore, when controlling the movement of an underwater vehicle, in addition to the control algorithm, the motion state of the underwater vehicle also needs to be considered. The combination of the two can improve the stability of underwater vehicle control and the accuracy of reaching the target point.

[0004] The underwater vehicle control method in the prior art introduces position deviation for correction from the starting point to the end point of the entire route when tracking the last section of the route, and pays more attention to the control algorithm itself during control without considering the state of the underwater vehicle. Therefore, the control stability and navigation position accuracy are not high. Summary of the invention

[0005] The technical problem to be solved by the present invention is to provide an underwater vehicle route tracking control method based on a deviation angle, which can improve the stability of underwater vehicle control and improve the position accuracy of the underwater vehicle when it reaches a target point.

[0006] In order to solve the above technical problems, the technical solution adopted by the present invention is:

[0007] A method for tracking and controlling an underwater vehicle route based on a deviation angle comprises the following steps:

[0008] S1, setting a virtual beam interception area as a switching condition of the underwater vehicle path tracking control method;

[0009] S2, calculating the deviation angle between the underwater vehicle and the desired route based on the navigation information;

[0010] S3, generating guidance instructions for the underwater vehicle based on the deviation angle calculation;

[0011] S4, calculating the deflection angles of the rudder surfaces of the underwater vehicle through the guidance instructions, and performing heading and longitudinal control of the underwater vehicle.

[0012] Further, in step S1, the virtual beam includes a horizontal virtual beam and a vertical virtual beam.

[0013] The horizontal virtual beam generates two virtual rays one in the horizontal section of the last section of the route, with the end point of the route as the starting point, which are opposite to the direction of the route, and the two virtual rays one have the same angle with the last section of the route;

[0014] The vertical virtual beam generates two virtual rays 2 in opposite directions of the route with the end point of the route as the starting point within the vertical section of the last route, and the angles formed by the two virtual rays 2 and the last route are the same;

[0015] The quadrangular pyramid-shaped space region enclosed by the two virtual rays 1 and the two virtual rays 2 is the interception region.

[0016] Further, in step S2, the deviation angle includes a horizontal position deviation angle and a vertical position deviation angle.

[0017] The horizontal position deviation angle μ and the vertical position deviation angle η are calculated by the following formula:

[0018]

[0019] In the formula, Z d L is the distance from the current position of the underwater vehicle to the vertical projection plane of the last section of the route, c is the distance between the current position of the underwater vehicle and the end point of the desired route, Y0 is the vertical distance between the desired route and the horizontal projection plane of the end point of the route, L0 is the projection distance between the desired route and the horizontal projection plane of the end point of the route, and Y c L is the vertical distance between the projection position of the underwater vehicle on the vertical projection plane of the route end point and the horizontal projection plane of the route end point. cIt is the distance between the projection position of the underwater vehicle on the horizontal projection plane of the route endpoint and the route endpoint.

[0020] Furthermore, in step S3, the guidance instruction includes a speed instruction, a depth instruction and a heading instruction.

[0021] The depth instruction Y g and heading command φ g Calculate using the following formula:

[0022]

[0023] Where V c is the current speed of the underwater vehicle, K grads is the gradient of the line connecting the current position of the underwater vehicle and the end point in the disk, S c K is the distance between the current position of the underwater vehicle and the end point of the route, η , K μ , K μI , They are all instruction calculation parameters, which are fixed values ​​and need to be tested and adjusted to obtain them.

[0024] Further, in step S4, the control surfaces include vertical control surfaces, horizontal control surfaces and differential control surfaces.

[0025] Furthermore, in step S4, the heading and longitudinal control of the underwater vehicle is controlled in the following manner:

[0026]

[0027] Where ΔY is the depth deviation, ΔY=Y c -Y g , δ ε ,δ r ,δ d They represent the vertical rudder angle, horizontal rudder angle, and differential rudder angle of the underwater vehicle, respectively. ε0 ,δ r0 ,δ d0 They respectively represent the vertical rudder surface deflection angle, horizontal rudder surface deflection angle, and differential rudder surface deflection angle of the underwater vehicle in a balanced state, which can be obtained through balancing; the other parameters are control instruction calculation parameters, which are all fixed values ​​and need to be obtained through testing and adjustment.

[0028] Furthermore, in step S1, the switching condition of the underwater vehicle route tracking control method is: when the underwater vehicle reaches the last section of the route, and the real-time horizontal position deviation angle and vertical position deviation angle of the underwater vehicle are within the virtual beam interception area.

[0029] Compared with the prior art, the present invention has the following main advantages:

[0030] By adopting the underwater vehicle route tracking control method based on deviation angle proposed in this application, when the underwater vehicle approaches the end point (target point) of the route, the concept of position deviation angle is introduced, and the heading and longitudinal navigation state of the underwater vehicle are considered in the whole control process, thereby improving the stability of underwater vehicle control and the position accuracy when reaching the target point. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] Figure 1 This is an overall flow chart of a method for tracking and controlling an underwater vehicle route based on a deviation angle according to the present invention;

[0032] Figure 2 A schematic diagram of setting a virtual beam interception area in an embodiment of the present invention;

[0033] Figure 3 Schematic diagram of the deviation angle calculation principle in the embodiment of the present invention

[0034] Figure 4 The figure is a flow chart of an implementation of the underwater vehicle route tracking control method in an embodiment of the present invention. DETAILED DESCRIPTION

[0035] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0036] It should be pointed out that, according to the needs of implementation, the various steps / components described in this application can be split into more steps / components, and two or more steps / components or partial operations of steps / components can be combined into new steps / components to achieve the purpose of the present invention.

[0037] The present application provides a method for tracking and controlling an underwater vehicle route based on a deviation angle, including four steps: setting a virtual beam interception area, calculating a deviation angle, calculating a guidance instruction, and controlling a heading and a longitudinal direction. Figure 1 As shown, the specific steps are as follows:

[0038] S1, setting a virtual beam interception area as a switching condition of the route tracking control method;

[0039] S2, calculating the deviation angle between the underwater vehicle and the desired route based on the navigation information;

[0040] S3, generating a guidance instruction based on the deviation angle calculation;

[0041] S4, calculating the deflection angles of the rudder surfaces of the underwater vehicle through the guidance instructions, and performing heading and longitudinal control of the underwater vehicle.

[0042] 1) Setting a virtual beam interception area is used as a switching condition for the path tracking control method based on the deviation angle.

[0043] The virtual beam is divided into a horizontal virtual beam and a vertical virtual beam.

[0044] The horizontal virtual beam, in the horizontal section of the last route, takes the route end point as the starting point and makes two virtual rays in the opposite direction of the route. The two virtual rays have the same angle with the last section of the route, and the angle range can be flexibly set according to the water conditions and the type of underwater vehicle. The vertical virtual beam is generated in a similar manner to the horizontal virtual beam. In the vertical section of the last route, two virtual rays are generated in the opposite direction of the route with the route end point as the starting point. The angle range of the two virtual rays can be flexibly set according to the water conditions and the type of underwater vehicle.

[0045] The four virtual rays present a tetrahedron with the route target point as the vertex. The height of the tetrahedron is set according to actual needs, and the distance can be set according to actual needs.

[0046] 2) Deviation angle calculation is responsible for calculating the deviation angle between the underwater vehicle and the desired route based on the navigation information.

[0047] The deviation angle calculation needs to start when the underwater vehicle enters the last section of the route. When the deviation angle is within the range of the virtual intercept beam, a control method based on the deviation angle is switched to control the underwater vehicle to perform path tracking.

[0048] Before entering the virtual beam range, the traditional distance deviation control method is used for control; when the underwater vehicle tracks the final segment of the route, a virtual ray pointing from the end point of the route to the current position of the underwater vehicle is generated through navigation information, and the virtual ray is projected onto the vertical and horizontal sections where the final segment of the route is located.

[0049] The angle between the virtual ray and its projection ray on the vertical section is the horizontal position deviation angle, and the angle difference between the angle between the virtual ray and its projection line on the horizontal section minus the angle between the final segment and the projection ray of the final segment on the horizontal section is the vertical position deviation angle.

[0050] Among them, the horizontal position deviation angle and the vertical position deviation angle are calculated by the following formula:

[0051]

[0052] In the formula, Z d L is the distance from the current position of the underwater vehicle to the vertical projection plane of the last section of the route, c is the distance between the current position of the underwater vehicle and the end point of the desired route, Y0 is the vertical distance between the desired route and the horizontal projection plane of the end point of the route, L0 is the projection distance between the desired route and the horizontal projection plane of the end point of the route, and Y c L is the vertical distance between the projection position of the underwater vehicle on the vertical projection plane of the route end point and the horizontal projection plane of the route end point. c It is the distance between the projection position of the underwater vehicle on the horizontal projection plane of the route endpoint and the route endpoint.

[0053] 3) Guidance command calculation Generates guidance command through deviation angle.

[0054] In an underwater vehicle, the guidance instructions include speed instructions, depth instructions and heading instructions. If it is sailing at a constant speed, there is no speed instruction.

[0055] The depth command is related to the longitudinal velocity and vertical position deviation angle of the underwater vehicle. The longitudinal velocity is used as the main signal, and the vertical position deviation angle is used as the compensation signal to correct the command. Considering the six-degree-of-freedom motion of the underwater vehicle, the heading and roll are strongly coupled, that is, rolling will occur when yaw, and yaw will also occur when rolling.

[0056] In this guidance command calculation method, the heading command is realized by the roll of the underwater vehicle. This is done because the roll angle is zero during stable navigation, that is, the expected value of the roll angle in the heading control is zero at any time, which is beneficial to the command calculation and heading control. Therefore, the heading command is corrected only by the compensation signal of the underwater position deviation angle. The calculation process is as follows:

[0057]

[0058] Where Y g is the depth instruction, φ g is the heading instruction, V c is the current speed of the underwater vehicle, K grads is the gradient of the line connecting the current position of the underwater vehicle and the end point in the disk, S c K is the distance between the current position of the underwater vehicle and the end point of the route, η , K μ , K μI , All of them are command calculation parameters, which are fixed values ​​and need to be tested and adjusted. It can be clearly observed that in the guidance command calculation, the deviation angle is the main signal and plays a decisive role in the guidance command.

[0059] 4) Heading and longitudinal control The deflection angles of each rudder surface of the underwater vehicle are calculated through guidance instructions.

[0060] The rudder surface mainly includes a vertical rudder surface (corresponding to the vertical rudder angle δ r ), horizontal rudder surface (corresponding to horizontal rudder angle δ e ) and differential control surface (corresponding to differential steering angle δ d ).

[0061] The guidance command calculated in step S3 is used to calculate the deflection of each control surface.

[0062] In the heading and longitudinal control, considering that the attitude of the underwater vehicle has a certain influence on the control effect and the route tracking effect, during the control, the attitude signal related to the longitudinal control is introduced in the longitudinal control to compensate for the longitudinal control deviation. At the same time, due to the six-degree-of-freedom motion of the underwater vehicle, when the longitudinal control and heading control are performed at the same time, the rising force caused by the change of the heading attitude (roll angle) will be reduced, thereby increasing the depth. Therefore, during the longitudinal control, the roll angle needs to be introduced to compensate for this part of the control loss, and the longitudinal control is ultimately reflected on the horizontal rudder surface; in the heading control, the attitude signal related to the heading control is introduced to compensate for the heading control deviation, and the heading control is ultimately reflected on the vertical rudder surface and the differential rudder surface. By compensating the control signal with its attitude signal, the stability of the control can be greatly improved, and its control configuration is as follows:

[0063]

[0064] Where ΔY is the depth deviation, ΔY=Y c -Y g , δ e0 ,δ a0 ,δ d0 They represent the deflection angle of the rudder surface of the underwater vehicle in a balanced state, which can be obtained by balancing. The other parameters are control command calculation parameters, which are fixed values ​​and need to be tested and adjusted. It can be clearly observed that in heading and longitudinal control, the guidance command is the main signal and plays a decisive role in heading and longitudinal control.

[0065] Furthermore, the deviation angle calculation principle diagram is as follows: Figure 3 As shown, it is characterized by:

[0066] 21) When the underwater vehicle is far away from the end point of the route, the deviation angle is small and its control effect is worse than the conventional distance deviation control. However, as the distance to the end point of the route gets closer, the deviation angle signal can be used to amplify the deviation (nonlinear amplification, related to the asin and atan functions). Combined with the control algorithm, the route tracking convergence speed can be faster, the control effect can be more stable, and the accuracy of reaching the target point can be higher.

[0067] 22) When using the position deviation angle, two virtual beam areas need to be set for switching the underwater vehicle control method. When the virtual beam intercepts the underwater vehicle, the position deviation angle control method is used for route tracking. The range of the virtual beam area can be flexibly set according to actual needs or test results.

[0068] Furthermore, the calculation generates a guide instruction, characterized in that:

[0069] 31) Different from the traditional guidance command calculation method, in the longitudinal depth guidance command calculation, the product of the underwater navigation speed and the route gradient is used as the main signal, and the vertical distance deviation angle is used to correct the deviation. Compared with the direct use of height deviation, the advantage of this is that the depth control can be more stable and the control accuracy is higher.

[0070] 32) The heading guidance command is calculated by a conventional PID method based on the horizontal deviation angle. The calculation method can also be flexibly selected according to the use environment and the characteristics of the underwater vehicle, which has very good flexibility.

[0071] Further, the heading and longitudinal control is characterized by:

[0072] 41) In the longitudinal control method, the horizontal rudder control adopts PID control based on depth deviation, which is widely used in engineering. Then, longitudinal related signals such as pitch angle and pitch angle velocity are introduced. In addition, the roll angle signal introduced is a correction signal added considering that the underwater vehicle will cause the depth to increase when rolling. This approach takes into account the influence of various longitudinal signals on depth deviation and greatly improves the depth control accuracy.

[0073] 42) In the heading control method, the vertical rudder control uses the heading-related angle and angular velocity signals for correction; the differential rudder angle uses the heading command deviation for correction. In this control law, the six-degree-of-freedom motion of the underwater vehicle is taken into account, and the rolling is used to control the heading of the underwater vehicle. Similar to the longitudinal control law, the influence of various heading-related angle signals and angular velocity signals on the heading is taken into account, which effectively improves the control accuracy and effect.

[0074] Furthermore, if Figure 4As shown, according to the implementation flow chart of the underwater vehicle route tracking control method of the present invention, before reaching the last route, the traditional position distance deviation control method is still used to track the route. When reaching the last route, the horizontal position deviation angle and the vertical position deviation angle are calculated, and it is determined in real time whether the horizontal position deviation angle and the vertical position deviation angle are within the virtual beam area. If not, the traditional position distance deviation control method is still used to track the route. If within the beam area, the route tracking control method based on the deviation angle is switched to track the route, and the process ends when the underwater vehicle reaches the end of the route.

[0075] In addition, the input interface and output interface of the control method are consistent with the traditional control method, and can perfectly replace the control part of the underwater vehicle that has been used.

[0076] In summary:

[0077] By adopting the underwater vehicle route tracking control method based on deviation angle proposed in this application, when the underwater vehicle approaches the end point (target point) of the route, the concept of position deviation angle is introduced, and the heading and longitudinal navigation state of the underwater vehicle are considered in the whole control process, thereby improving the stability of underwater vehicle control and the position accuracy when reaching the target point.

[0078] It will be easily understood by those skilled in the art that the above description is only 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 in the protection scope of the present invention.

Claims

1. A method for tracking and controlling an underwater vehicle path based on a deviation angle, characterized in that: The steps include: S1, setting a virtual beam interception area as a switching condition of the underwater vehicle path tracking control method; The virtual beam includes a horizontal virtual beam and a vertical virtual beam; The horizontal virtual beam generates two virtual rays one in the horizontal section of the last section of the route, with the end point of the route as the starting point, which are opposite to the direction of the route, and the two virtual rays one have the same angle with the last section of the route; The vertical virtual beam generates two virtual rays 2 in opposite directions of the route with the end point of the route as the starting point within the vertical section of the last route, and the angles formed by the two virtual rays 2 and the last route are the same; The quadrangular pyramid-shaped space region enclosed by the two virtual rays 1 and the two virtual rays 2 is the interception region; S2, calculating the deviation angle between the underwater vehicle and the desired route based on the navigation information; The deviation angle includes a horizontal position deviation angle and a vertical position deviation angle; The horizontal position deviation angle μ and the vertical position deviation angle η are calculated by the following formula: In the formula, Z d is the distance from the current position of the underwater vehicle to the vertical projection plane of the last section of the route, Y0 is the vertical distance between the desired route and the horizontal projection plane of the route end point, L0 is the projection distance between the desired route and the horizontal projection plane of the route end point, Y c L is the vertical distance between the projection position of the underwater vehicle on the vertical projection plane of the route end point and the horizontal projection plane of the route end point. c is the distance between the projection position of the underwater vehicle on the horizontal projection plane of the route end point and the route end point; S3, generating guidance instructions for the underwater vehicle based on the deviation angle calculation; S4, calculating the deflection angles of the rudder surfaces of the underwater vehicle through the guidance instructions, and performing heading and longitudinal control of the underwater vehicle.

2. The underwater vehicle path tracking control method based on deviation angle according to claim 1 is characterized in that In step S3, the guidance instruction includes a speed instruction, a depth instruction and a heading instruction.

3. The underwater vehicle path tracking control method based on deviation angle according to claim 2, characterized in that: The depth instruction Y g and heading command φ g Calculate using the following formula: Where V c is the current speed of the underwater vehicle, K grads is the gradient of the line connecting the current position of the underwater vehicle and the end point in the disk, S c K is the distance between the current position of the underwater vehicle and the end point of the route, η , K μ , K μI , All of them are instruction calculation parameters, all of them are fixed values, and are obtained through testing and parameter adjustment.

4. The underwater vehicle path tracking control method based on deviation angle according to claim 1 is characterized in that In step S4, the control surfaces include vertical control surfaces, horizontal control surfaces and differential control surfaces.

5. The underwater vehicle path tracking control method based on deviation angle according to claim 3 is characterized in that In step S4, the heading and longitudinal control of the underwater vehicle is configured as follows: Where ΔY is the depth deviation, ΔY=Y c -Y g , δ ε , δ r , δ d They represent the vertical rudder angle, horizontal rudder angle, and differential rudder angle of the underwater vehicle, respectively. ε0 , δ r0 , δ d0 They respectively represent the vertical rudder surface deflection angle, horizontal rudder surface deflection angle, and differential rudder surface deflection angle of the underwater vehicle in a balanced state, which are obtained through balancing; the other parameters are control instruction calculation parameters, which are all fixed values ​​and are obtained through testing and parameter adjustment.

6. The underwater vehicle path tracking control method based on deviation angle according to claim 1 is characterized in that In step S1, the switching condition of the underwater vehicle route tracking control method is: when the underwater vehicle reaches the last section of the route, and the real-time horizontal position deviation angle and vertical position deviation angle of the underwater vehicle are within the virtual beam interception area.

Images