Submersible motion trajectory repair method and system
Through Archimedes spiral and cubic spline interpolation encryption technology, the problem of submersible motion trajectory data is solved, accurate prediction of submersible manipulation is achieved, and navigation safety is improved.
Patent Information
- Application Number
- CN202211012157.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-08-23
- Publication Date
- 2024-12-03
- Estimated Expiration
- 2042-08-23
AI Technical Summary
The motion trajectory data collected by the submersible in actual tests are sparse and non-smooth and continuous, resulting in a reduced accuracy of manipulation forecasting and affecting navigation safety.
The sparse test data of the submersible is interpolated and encrypted by the Archimedes spiral and cubic spline curve, and the submersible control system is identified by the least squares method to repair the motion trajectory data of the submersible.
It realizes accurate repair of the submersible movement trajectory, with continuous curvature and smooth curves, improves the accuracy of manipulation forecasting and enhances navigation safety guarantees.
Smart Images

Figure CN115328177B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of submersible manipulation and control, in particular, to a method and system for repairing a submersible motion trajectory, and more particularly, to a strategy for repairing a submersible motion trajectory based on Archimedean spiral and cubic spline curve interpolation encryption. Background Art
[0002] At present, submersibles play an important role in the fields of ocean survey, maritime rescue, military reconnaissance, etc. Good maneuverability is an important basis for the safe navigation of submersibles, so it is very important to accurately predict the maneuverability of submersibles. In recent years, many scholars have used system identification methods to predict the maneuverability of submersibles, including traditional methods such as least squares method, Kalman filter method, etc.; also including identification methods based on emerging neural networks and support vector machines in recent years. With the continuous deepening of research, it is found in actual experiments that the state data of submersibles collected by attitude sensors are often sparse rather than smooth and continuous. After numerical simulation, it is found that this phenomenon will reduce the prediction accuracy of submersibles. Therefore, accurately repairing the navigation trajectory data of submersibles is of great significance to improve the prediction accuracy of submersible maneuverability and strengthen the safety of submersible navigation. To this end, the patent of this invention proposes a repair strategy for interpolating and encrypting the trajectory using Archimedean spirals and cubic spline curves, so as to provide a new solution for accurately predicting the maneuverability of submersibles.
[0003] Patent document CN109726355A (application number: 201910008797.2) discloses a ship track repair method based on vector interpolation. The method of repairing the ship track first uses a bidirectional iterative method to interpolate the data processed by Gaussian projection of the ship's geographic coordinates, obtains two sets of ship coordinate prediction values, and then selects weights to perform weighted average on the two sets of coordinate prediction values, and finally calculates the ship's geographic coordinates according to Gaussian back projection, thereby realizing the repair of the ship's track. The present invention mainly uses Archimedean spirals and cubic spline curves to perform interpolation and encryption of maneuvering trajectories to achieve higher-precision identification of the submersible model system, which is completely different from the technical innovation methods and application ideas used in the aforementioned inventions.
[0004] Patent document CN111858814A (application number: 202010707304.7) discloses a method, device, equipment and storage medium for repairing a motion trajectory, including: generating an initial motion trajectory based on positioning point information obtained during the user's movement; the positioning point information includes longitude and latitude, speed and timestamp; analyzing the initial motion trajectory to obtain problem positioning points; correcting the problem positioning points; repairing the initial motion trajectory according to the corrected problem positioning points to obtain the target motion trajectory. Summary of the invention
[0005] In view of the defects in the prior art, an object of the present invention is to provide a method and system for repairing the motion trajectory of a submersible.
[0006] A method for repairing a submersible motion trajectory provided by the present invention comprises:
[0007] Step S1: using Archimedean spiral and cubic spline curve to interpolate and encrypt the sparse test data respectively to obtain the repaired test data;
[0008] Step S2: The repaired test data is used to identify the submersible control system based on the first-order nonlinear control response model of the submersible using the least squares method.
[0009] Preferably, in step S1, the Archimedean spiral is used to interpolate and encrypt the sparse test data:
[0010] The polar coordinate formula of the Archimedean spiral is expressed as:
[0011] r=aθ (1)
[0012] Among them, r, θ represent the polar diameter and polar angle respectively, and a represents a positive constant;
[0013] Using ψ i represents the data point for a given heading angle ψ, ψ i and ψ i+1 The Archimedean spiral is connected between i ψ i+1 , between which ψ i The direction of i+1 Only the position is specified but not the direction; in order to ensure the continuity of the curvature of the trajectory, the discretized values with equal time intervals Δt are set, and the last interpolation point in the previous spiral is expressed as Then determine ψ i ψ i+1 The initial direction is From this we get ψ i ψ i+1 :
[0014]
[0015] Where σ represents the initial angle; ρ represents the direction of the Archimedean spiral; (t i ,ψ i ) represents the data point encrypted by Archimedean spiral interpolation between two sparse points.
[0016] Preferably, in step S1, the sparse test data is interpolated and encrypted using a cubic spline curve by:
[0017] Set the time interval to Δt, and let the reconstructed time series be represented by t′. When there are n+1 data nodes, the number of node intervals is n. At this time, there are n cubic spline interpolation polynomials about the heading angle ψ:
[0018] ψ i (t′) = a i +b i (t′-t i )+c i (t′-t i ) 2 +d i (t′-t i ) 3 (3)
[0019] Among them, a i ,b i ,c i and d i are the polynomial coefficients of cubic spline interpolation, (t i ,ψ i ) represents the data point encrypted by cubic spline interpolation between two sparse points.
[0020] Preferably, the submersible first-order nonlinear maneuvering response model in step S2 adopts:
[0021]
[0022] Where, T and K represent the manipulability index; α represents the nonlinear term coefficient; is the heading angular velocity of the submersible; ψ is the heading angle of the submersible; δ is the rudder angle of the submersible; It represents the first-order derivative of the submersible's bow angular velocity with respect to time.
[0023] Preferably, the step S2 adopts:
[0024] The first-order nonlinear control response model of the submersible is transformed into the relationship between ψ and δ, and then substituted into have to:
[0025]
[0026] in, and They represent the first and second order derivatives of the heading angle with respect to time respectively;
[0027] Take the sampling time interval Δt, based on the difference approximation processing and We can get:
[0028]
[0029] Among them, ψ(t) represents the heading angle of the submersible at time t; ψ(t-1) represents the heading angle of the submersible at time t-1; ψ(t-2) represents the heading angle of the submersible at time t-2; δ(t-2) represents the rudder angle of the submersible at time t-2;
[0030] Writing this in least squares format y(t)=θx(t):
[0031] Input matrix x(t):
[0032]
[0033] Output scalar y(t):
[0034] y(t)=ψ(t)-2ψ(t-1)+ψ(t-2) (8)
[0035] The parameter matrix θ to be identified:
[0036]
[0037] A submersible motion trajectory repair system provided by the present invention comprises:
[0038] Module M1: Use Archimedean spiral and cubic spline curve to interpolate and encrypt the sparse test data to obtain the repaired test data;
[0039] Module M2: The repaired test data is based on the first-order nonlinear control response model of the submersible, and the submersible control system is identified using the least squares method.
[0040] Preferably, the module M1 uses the Archimedean spiral to interpolate and encrypt the sparse test data by:
[0041] The polar coordinate formula of the Archimedean spiral is expressed as:
[0042] r=aθ (1)
[0043] Among them, r, θ represent the polar diameter and polar angle respectively, and a represents a positive constant;
[0044] Using ψ i represents the data point for a given heading angle ψ, ψ i and ψ i+1 The Archimedean spiral is connected between i ψ i+1 , between which ψ i The direction of i+1Only the position is specified but not the direction; in order to ensure the continuity of the curvature of the trajectory, the discretized values with equal time intervals Δt are set, and the last interpolation point in the previous spiral is expressed as Then determine ψ i ψ i+1 The initial direction is From this we get ψ i ψ i+1 :
[0045]
[0046] Where σ represents the initial angle; ρ represents the direction of the Archimedean spiral; (t i ,ψ i ) represents the data point encrypted by Archimedean spiral interpolation between two sparse points.
[0047] Preferably, the module M1 uses a cubic spline curve to interpolate and encrypt the sparse test data by:
[0048] Set the time interval to Δt, and let the reconstructed time series be represented by t′. When there are n+1 data nodes, the number of node intervals is n. At this time, there are n cubic spline interpolation polynomials about the heading angle ψ:
[0049] ψ i (t′) = a i +b i (t′-t i )+c i (t′-t i ) 2 +d i (t′-t i ) 3 (3)
[0050] Among them, a i ,b i ,c i and d i are the polynomial coefficients of cubic spline interpolation, (t i ,ψ i ) represents the data point encrypted by cubic spline interpolation between two sparse points.
[0051] Preferably, the submersible first-order nonlinear maneuvering response model in the module M2 adopts:
[0052]
[0053] Where, T and K represent the manipulability index; α represents the nonlinear term coefficient; is the heading angular velocity of the submersible; ψ is the heading angle of the submersible; δ is the rudder angle of the submersible; It represents the first-order derivative of the submersible's bow angular velocity with respect to time.
[0054] Preferably, the module M2 adopts:
[0055] The first-order nonlinear control response model of the submersible is transformed into the relationship between ψ and δ, and then substituted into have to:
[0056]
[0057] in, and They represent the first and second order derivatives of the heading angle with respect to time respectively;
[0058] Take the sampling time interval Δt, based on the difference approximation processing and We can get:
[0059]
[0060] Among them, ψ(t) represents the heading angle of the submersible at time t; ψ(t-1) represents the heading angle of the submersible at time t-1; ψ(t-2) represents the heading angle of the submersible at time t-2; δ(t-2) represents the rudder angle of the submersible at time t-2;
[0061] Writing this in least squares format y(t)=θx(t):
[0062] Input matrix x(t):
[0063]
[0064] Output scalar y(t):
[0065] y(t)=ψ(t)-2ψ(t-1)+ψ(t-2) (8)
[0066] The parameter matrix θ to be identified:
[0067]
[0068] Compared with the prior art, the present invention has the following beneficial effects: Aiming at the phenomenon that the motion trajectory collected in the actual test of the submersible is sparse, the present invention proposes a submersible motion trajectory repair strategy based on Archimedean spiral and cubic spline curve interpolation encryption, which can accurately restore and reproduce the original motion trajectory of the submersible, and the trajectory repaired by this strategy has continuous curvature and smooth curves, which conforms to the law of the actual motion of the submersible, and lays the foundation for accurately predicting the maneuverability of the submersible. This method has unique advantages in repairing the motion trajectory of the submersible and thus improving the accuracy of maneuverability prediction. BRIEF DESCRIPTION OF THE DRAWINGS
[0069] Other features, objects and advantages of the present invention will become more apparent from the detailed description of non-limiting embodiments made with reference to the following drawings:
[0070] Figure 1 A schematic diagram of the motion trajectory repair strategy of the submersible interpolation encryption of the present invention.
[0071] Figure 2 This is the identification result of the motion trajectory of the submersible based on the least squares method of the present invention, which is an interpolation and encryption of the Archimedean spiral.
[0072] Figure 3 This is the identification result of the submersible motion trajectory encrypted by interpolation of cubic spline curves based on the least squares method of the present invention. DETAILED DESCRIPTION
[0073] The present invention is described in detail below in conjunction with specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but are not intended to limit the present invention in any form. It should be noted that, for those of ordinary skill in the art, several changes and improvements can also be made without departing from the concept of the present invention. These all belong to the protection scope of the present invention.
[0074] Example 1
[0075] The present invention provides a submersible trajectory repair strategy based on Archimedean spiral and cubic spline curve interpolation encryption. Aiming at the problem that sparse test data of the submersible will reduce the prediction accuracy of its maneuverability, a repair strategy for interpolation encryption of the missing trajectory of the submersible based on Archimedean spiral and cubic spline curve is proposed.
[0076] Firstly, the principle and specific implementation method of the Archimedean spiral and cubic spline interpolation encryption strategy are explained; then, the sparse test data are brought into the formula of the Archimedean spiral and cubic spline for processing, the coordinates of the interpolation points are obtained, and then the submersible test data after interpolation encryption is obtained, so as to repair the missing trajectory of the submersible; finally, the least squares method is used to identify the experimental data, and the root mean square error is introduced to judge the accuracy. Figure 1 As shown, the present invention theoretically and rigorously expounds a submersible motion trajectory repair strategy based on Archimedean spiral and cubic spline curve interpolation encryption technology, performs interpolation encryption repair processing on sparse test data, and provides a new idea for improving the prediction accuracy of submersible maneuverability.
[0077] According to the present invention, a method for repairing a submersible motion trajectory based on Archimedean spiral and cubic spline interpolation encryption is proposed, comprising:
[0078] The steps of trajectory repair using Archimedean spiral interpolation encryption: Explain the principle of Archimedean spiral interpolation and encrypt the sparse test data;
[0079] The steps of trajectory repair using cubic spline interpolation encryption: explain the principle of cubic spline interpolation and encrypt the sparse test data;
[0080] The steps of using the least squares method to identify the system after the test data is repaired: the principle of least squares method system identification is explained, and based on the first-order nonlinear control response model of the submersible, the least squares method is used to identify the system of the repaired test data;
[0081] The first-order nonlinear control response model of the submersible is a black box modeling concept that takes the rudder angle as input and the heading angle as output, thereby achieving an accurate description of the response of the heading angle to the rudder angle.
[0082] In the trajectory repair step using Archimedean spiral interpolation encryption, the interpolation encryption steps are as follows:
[0083] The polar coordinate formula of the Archimedean spiral can be expressed as:
[0084] r=aθ
[0085] Where r, θ represent the polar diameter and polar angle respectively, and a is a positive constant. i represents the data point for a given heading angle ψ, ψ i and ψ i+1 The Archimedean spiral is connected between i ψ i+1 , between which ψ i The direction of i+1 In order to ensure the continuity of the curvature of the trajectory, the discretized values with equal time intervals Δt are set, and the last interpolation point in the previous spiral is expressed as Then determine ψ i ψ i+1 The initial direction is From this we get ψ i ψ i+1 :
[0086]
[0087] Where σ is the initial angle:
[0088] σ=atan2(υ iψ ,υ it )
[0089]
[0090] ρ is the direction of the Archimedean spiral, expressed as:
[0091] ρ=-sign(t i+1 ·ψ i +ψ i+1 ·t i )
[0092] When ρ=1, it means that the spiral path propagates in a clockwise direction from the origin of the polar coordinates, and vice versa.
[0093] The exact proportions of the Archimedean spiral segments depend on the positive constant a, whose value can be determined by the end points ψ i+1 The constraints of are solved, and then the data points encrypted by the Archimedean spiral interpolation between two sparse points can be obtained (t i ,ψ i ).
[0094] In the trajectory repair step using cubic spline curve interpolation encryption, the interpolation encryption steps are as follows:
[0095] Set the time interval to Δt, and let the reconstructed time series be represented by t′. When there are n+1 data nodes, the number of node intervals is n. At this time, there are n cubic spline interpolation polynomials about the heading angle ψ:
[0096] ψ i (t′) = a i +b i (t′-t i )+c i (t′-t i ) 2 +d i (t′-t i ) 3
[0097] Where i = 0, 1…n, the derivative of ψ yields:
[0098]
[0099]
[0100] set up After substituting into the above formula, the coefficients of each node interval can be calculated according to the second-order differential continuity:
[0101]
[0102] At this time, the number of unknown quantities m is n+1, and the number of equations is n-1, so we choose the natural boundary condition, and we have m 0 =0,m n =0, according to the continuity of the first-order differential, we can get:
[0103]
[0104] b i ,c i and d i Substituting into the above formula, we can get:
[0105]
[0106] Express it in matrix form:
[0107]
[0108] At this point, the interpolation polynomial function ψ of each node interval can be obtained i (t′), and then the encrypted data point (t i ,ψ i ).
[0109] In the step of using the least square method to perform system identification after interpolation encryption repair, the steps of system identification are as follows:
[0110] The first-order nonlinear control response model of the submersible can be expressed as:
[0111]
[0112] In the formula, T and K are the manipulability indexes, and α is the coefficient of the nonlinear term; is the heading angular velocity of the submersible; ψ is the heading angle of the submersible; δ is the rudder angle of the submersible.
[0113] In the experiment, the actual collected submersible model state data is discrete data, so the control response model needs to be discretized:
[0114] First, transform the equation into the relationship between ψ and δ, and substitute have to:
[0115]
[0116] Take the sampling time interval Δt, based on the difference approximation processing and We can get:
[0117]
[0118] Writing this in least squares format y(t)=θx(t):
[0119] Input matrix x(t):
[0120]
[0121] Output scalar y(t):
[0122] y(t)=ψ(t)-2ψ(t-1)+ψ(t-2)
[0123] The parameter matrix θ to be identified:
[0124]
[0125] In summary, the parameter matrix θ can be identified based on the repaired encrypted data through the least squares method.
[0126] The identified maneuverability index is substituted into the submersible maneuvering response equation, and the 30° Z-shaped test simulation prediction is performed on the bow angle data. Among them, the least squares (LS) identification prediction results based on the Archimedean spiral (AS) interpolation repair data are as follows: Figure 2 As shown; the LS identification prediction results based on the cubic spline interpolation repair data are as follows Figure 3 As shown; the root mean square error (RMSE) of the forecast results compared with the original data is shown in the table below.
[0127] Table 1. Comparison of root mean square error
[0128]
[0129] According to the present invention, a submersible motion trajectory repair system based on Archimedean spiral and cubic spline interpolation encryption is proposed, comprising:
[0130] Trajectory repair module using Archimedean spiral interpolation encryption: Explain the Archimedean spiral interpolation principle and interpolate and encrypt sparse test data;
[0131] Trajectory repair module using cubic spline interpolation encryption: Explain the cubic spline interpolation principle and interpolate and encrypt sparse test data;
[0132] System identification module using least squares method after test data repair: Explain the principle of least squares system identification, and use least squares method to identify the repaired test data based on the first-order nonlinear control response model of the submersible;
[0133] The first-order nonlinear control response model of the submersible is a black box modeling concept that takes the rudder angle as input and the heading angle as output, thereby achieving an accurate description of the response of the heading angle to the rudder angle.
[0134] In the trajectory repair module using Archimedean spiral interpolation encryption, the interpolation encryption is as follows:
[0135] The polar coordinate formula of the Archimedean spiral can be expressed as:
[0136] r=aθ
[0137] Where r, θ represent the polar diameter and polar angle respectively, and a is a positive constant. i represents the data point for a given heading angle ψ, ψ i and ψ i+1 The Archimedean spiral is connected between i ψ i+1 , between which ψ i The direction of i+1 In order to ensure the continuity of the curvature of the trajectory, the discretized values with equal time intervals Δt are set, and the last interpolation point in the previous spiral is expressed as Then determine ψ i ψ i+1 The initial direction is From this we get ψ i ψ i+1 :
[0138]
[0139] Where σ is the initial angle:
[0140] σ=atan2(υ iψ ,υ it )
[0141]
[0142] Among them, ψ i-1ilast express
[0143] ρ is the direction of the Archimedean spiral, expressed as:
[0144] ρ=-sign(t i+1 ·ψ i +ψ i+1 ·t i )
[0145] When ρ=1, it means that the spiral path propagates in a clockwise direction from the origin of the polar coordinates, and vice versa.
[0146] The exact proportions of the Archimedean spiral segments depend on the positive constant a, whose value can be determined by the end points ψ i+1 The constraints of are solved, and then the data points encrypted by the Archimedean spiral interpolation between two sparse points can be obtained (t i ,ψ i ).
[0147] In the trajectory repair module using cubic spline curve interpolation encryption, the interpolation encryption is as follows:
[0148] Set the time interval to Δt, and let the reconstructed time series be represented by t′. When there are n+1 data nodes, the number of node intervals is n. At this time, there are n cubic spline interpolation polynomials about the heading angle ψ:
[0149] ψ i (t′) = a i +b i (t′-t i )+c i (t′-t i ) 2 +d i (t′-t i ) 3
[0150] Where i = 0, 1…n, the derivative of ψ yields:
[0151]
[0152]
[0153] set up After substituting into the above formula, the coefficients of each node interval can be calculated according to the second-order differential continuity:
[0154]
[0155] At this time, the number of unknown quantities m is n+1, and the number of equations is n-1, so we choose the natural boundary condition, and we have m 0 =0,m n =0, according to the continuity of the first-order differential, we can get:
[0156]
[0157] b i ,c i and d i Substituting into the above formula, we can get:
[0158]
[0159] Express it in matrix form:
[0160]
[0161] At this point, the interpolation polynomial function ψ of each node interval can be obtained i (t′), and then the encrypted data point (t i ,ψ i ).
[0162] In the module for performing system identification using the least square method after interpolation encryption repair, the system identification is as follows:
[0163] The first-order nonlinear control response model of the submersible can be expressed as:
[0164]
[0165] In the formula, T and K are the manipulability indexes, and α is the coefficient of the nonlinear term; is the heading angular velocity of the submersible; ψ is the heading angle of the submersible; δ is the rudder angle of the submersible.
[0166] In the experiment, the actual collected submersible model state data is discrete data, so the control response model needs to be discretized:
[0167] First, transform the equation into the relationship between ψ and δ, and substitute have to:
[0168]
[0169] Take the sampling time interval Δt, based on the difference approximation processing and We can get:
[0170]
[0171] Writing this in least squares format y(t)=θx(t):
[0172] Input matrix x(t):
[0173]
[0174] Output scalar y(t):
[0175] y(t)=ψ(t)-2ψ(t-1)+ψ(t-2)
[0176] The parameter matrix θ to be identified:
[0177]
[0178] In summary, the parameter matrix θ can be identified based on the repaired encrypted data through the least squares method.
[0179] The identified maneuverability index is substituted into the submersible maneuvering response equation, and the 30° Z-shaped test simulation prediction is performed on the bow angle data. Among them, the least squares (LS) identification prediction results based on the Archimedean spiral (AS) interpolation repair data are as follows: Figure 2 As shown; the LS identification prediction results based on the cubic spline interpolation repair data are as follows Figure 3As shown; the root mean square error (RMSE) of the forecast results compared with the original data is shown in the table below.
[0180] Table 1. Comparison of root mean square error
[0181]
[0182] Those skilled in the art know that, in addition to implementing the system, device and its various modules provided by the present invention in a purely computer-readable program code, it is entirely possible to implement the same program in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers and embedded microcontrollers by logically programming the method steps. Therefore, the system, device and its various modules provided by the present invention can be considered as a hardware component, and the modules included therein for implementing various programs can also be considered as structures within the hardware component; the modules for implementing various functions can also be considered as both software programs for implementing the method and structures within the hardware component.
[0183] The above describes the specific embodiments of the present invention. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various changes or modifications within the scope of the claims, which does not affect the essence of the present invention. In the absence of conflict, the embodiments of the present application and the features in the embodiments can be combined with each other arbitrarily.
Claims
1. A method for repairing a submersible motion trajectory, characterized in that: include: Step S1: using Archimedean spiral and cubic spline curve to interpolate and encrypt the sparse test data respectively to obtain the repaired test data; Step S2: The repaired test data is used to identify the submersible control system based on the submersible first-order nonlinear control response model using the least square method; In step S1, the Archimedean spiral is used to interpolate and encrypt the sparse test data: The polar coordinate formula of the Archimedean spiral is expressed as: (1) in, , denote the polar diameter and polar angle respectively, represents a positive constant; use Represents a given Heading angle data points, and The time intervals are connected by an Archimedean spiral, which is represented by , between The direction of Only the position is specified but not the direction; in order to ensure the continuity of the curvature of the trajectory, equal time intervals are set The discretized value of is taken, and the last interpolation point in the previous spiral is expressed as , then determine The initial direction is , thus we get : (2) in, represents the initial angle; Indicates the direction of the Archimedean spiral; Represents the data points encrypted by Archimedean spiral interpolation between two sparse points; In step S1, the sparse test data is interpolated and encrypted using a cubic spline curve: Set the time interval to , let the reconstructed time series be Indicates that when there is data nodes, then the number of node intervals is , at this time there is Heading Angle Cubic spline interpolation polynomial: (3) in, , , and are the polynomial coefficients of cubic spline interpolation, Represents the data points encrypted by cubic spline interpolation between two sparse points; where i is a positive integer from 0 to n.
2. The method for repairing the motion trajectory of a submersible according to claim 1, characterized in that: In step S2, the submersible first-order nonlinear maneuvering response model adopts: (4) in, and represents the manipulability index; represents the coefficient of nonlinear term; is the submersible’s bow angular velocity; is the heading angle of the submersible; is the submersible rudder angle; It represents the first-order derivative of the submersible's bow angular velocity with respect to time.
3. The method for repairing the motion trajectory of a submersible according to claim 2, characterized in that: The step S2 adopts: The first-order nonlinear maneuvering response model of the submersible is transformed into and relationship, substitute have to: (5) in, and They represent the first and second order derivatives of the heading angle with respect to time respectively; Sampling time interval , based on the difference approximation and We can get: (6) in, express The heading angle of the submersible at the moment; express The heading angle of the submersible at the moment; express The heading angle of the submersible at the moment; express The submersible's rudder angle at the moment; Write it in least squares format : Input Matrix : (7) Output scalar : (8) Parameter matrix to be identified : (9)。 4. A submersible motion trajectory repair system, characterized in that: include: Module M1: Use Archimedean spiral and cubic spline curve to interpolate and encrypt the sparse test data to obtain the repaired test data; Module M2: The repaired test data is based on the first-order nonlinear control response model of the submersible, and the submersible control system is identified using the least squares method; In the module M1, the Archimedean spiral is used to interpolate and encrypt the sparse test data: The polar coordinate formula of the Archimedean spiral is expressed as: (1) in, , denote the polar diameter and polar angle respectively, represents a positive constant; use Represents a given Heading angle data points, and The time intervals are connected by an Archimedean spiral, which is represented by , between The direction of Only the position is specified but not the direction; in order to ensure the continuity of the curvature of the trajectory, equal time intervals are set The discretized value of is taken, and the last interpolation point in the previous spiral is expressed as , then determine The initial direction is , thus we get : (2) in, represents the initial angle; Indicates the direction of the Archimedean spiral; Represents the data points encrypted by Archimedean spiral interpolation between two sparse points; The module M1 uses cubic spline curves to interpolate and encrypt sparse test data: Set the time interval to , let the reconstructed time series be Indicates that when there is data nodes, then the number of node intervals is , at this time there is Heading Angle Cubic spline interpolation polynomial: (3) in, , , and are the polynomial coefficients of cubic spline interpolation, Represents the data points encrypted by cubic spline interpolation between two sparse points; where i is a positive integer from 0 to n.
5. The submersible motion trajectory repair system according to claim 4, characterized in that: The first-order nonlinear maneuvering response model of the submersible in the module M2 adopts: (4) in, and represents the manipulability index; represents the coefficient of nonlinear term; is the submersible’s bow angular velocity; is the heading angle of the submersible; is the submersible rudder angle; It represents the first-order derivative of the submersible's bow angular velocity with respect to time.
6. The submersible motion trajectory repair system according to claim 5, characterized in that: The module M2 adopts: The first-order nonlinear maneuvering response model of the submersible is transformed into and relationship, substitute have to: (5) in, and They represent the first and second order derivatives of the heading angle with respect to time respectively; Sampling time interval , based on the difference approximation and We can get: (6) in, express The heading angle of the submersible at the moment; express The heading angle of the submersible at the moment; express The heading angle of the submersible at the moment; express The submersible's rudder angle at the moment; Write it in least squares format : Input Matrix : (7) Output scalar : (8) Parameter matrix to be identified : (9)。
