Trajectory tracking control method, device and equipment
By dynamically adjusting the Kalman filter gain in conjunction with the MPC controller, the problem that fixed-gain Kalman filters cannot accurately predict the trajectory of underwater autonomous vehicles in complex seabed environments is solved, achieving more accurate trajectory prediction and autonomous control.
Patent Information
- Application Number
- CN202510878389.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-06-27
- Publication Date
- 2025-11-18
AI Technical Summary
In existing technologies, fixed-gain Kalman filters cannot accurately predict the trajectory of underwater autonomous vehicles in complex seabed environments.
By acquiring the predicted trajectory information and signal-to-noise ratio of underwater target nodes, the gain of the Kalman filter is dynamically adjusted, and combined with the MPC controller, the motion trajectory prediction of underwater target nodes is realized.
It improves the accuracy of predicting the movement trajectory of underwater target nodes in complex seabed environments and enhances the autonomous control capability of underwater autonomous vehicles.
Smart Images
Figure CN120973027A_ABST
Abstract
Description
TECHNICAL FIELD
[0001] The present application relates to the field of autonomous underwater vehicle control, and particularly relates to a trajectory tracking control method, device and equipment. BACKGROUND
[0002] With the increasing demand for underwater engineering and ocean exploration tasks, underwater autonomous vehicles have become an important tool in the fields of ocean research, deep sea exploration or ocean survey. Underwater autonomous vehicles can independently work in complex seabed environments. In order to better adapt to the complex seabed working environment, underwater autonomous vehicles need to track and control their own trajectories.
[0003] At present, the existing technical solutions use a Kalman filter with fixed gain to predict the motion trajectory of the underwater autonomous vehicle.
[0004] However, the seabed environment is complex. The Kalman filter with fixed gain cannot meet the complex and changeable seabed environment, and thus cannot accurately predict the motion trajectory of the underwater autonomous vehicle. SUMMARY
[0005] Embodiments of the present application provide a trajectory tracking control method, device and equipment, aiming to solve the technical problem of being unable to accurately predict the motion trajectory of an underwater target node.
[0006] In a first aspect, embodiments of the present application provide a trajectory tracking control method, which includes:
[0007] obtaining predicted trajectory information of an underwater target node at a first time and a signal-to-noise ratio of the underwater target node;
[0008] obtaining a preset Kalman filter;
[0009] determining a gain of the preset Kalman filter according to the signal-to-noise ratio;
[0010] inputting the predicted trajectory information into the preset Kalman filter to obtain first predicted trajectory information of the underwater target node at a second time, the second time being a next time of the first time;
[0011] inputting the first predicted trajectory information and the gain of the preset Kalman filter into an MPC controller to obtain second predicted trajectory information of the underwater target node at the second time, the MPC controller being configured to output predicted trajectory information of the underwater target node.
[0012] Optionally, the obtaining of the preset Kalman filter includes:
[0013] constructing a state equation of the preset Kalman filter according to position information, a roll angle, a pitch angle and a yaw angle of the underwater target node;
[0014] constructing an observation equation of the preset Kalman filter according to the position information of the underwater target node.
[0015] Optionally, the gain of the preset Kalman filter is determined according to the signal-to-noise ratio, and the method comprises:
[0016] constructing a positioning noise covariance of the underwater target node according to the signal-to-noise ratio;
[0017] determining the gain according to the positioning noise covariance, the state equation and the observation equation.
[0018] Optionally, the state equation is:
[0019]
[0020] wherein η = [x, y, z, φ, θ, ψ] T , x, y and z are respectively an X-axis coordinate, a Y-axis coordinate and a Z-axis coordinate of the underwater target node, φ, θ and ψ are respectively a roll angle, a pitch angle and a yaw angle of the underwater target node, k is a discrete time index, T s is a sampling time, R(η) is a rotation matrix from a carrier coordinate system B to I, v = [u, v, w, p, q, r] T , u is a velocity of the underwater target in the X-axis direction, v is a velocity of the underwater target in the Y-axis direction, w is a velocity of the underwater target in the Z-axis direction, p is a rotation velocity of the underwater target in the X-axis direction, q is a rotation velocity of the underwater target in the Y-axis direction, r is a rotation velocity of the underwater target in the Z-axis direction, M is a mass matrix, C(v) is a Coriolis-centripetal force matrix, D(v) is a damping matrix, g(η) is a restoring force, τ = [τ u , τ v , τ w , τ p , τ q , τ r ] T is a control input, and w is an environmental disturbance vector.
[0021] Optionally, the observation equation is:
[0022]
[0023] wherein ε is an observation noise and R k is a positioning noise covariance of the underwater target node, S is a position coordinate of the underwater target node, and b m is a position coordinate of the mth buoy.
[0024] Optionally, the positioning noise covariance is:
[0025] R k = R0·exp(-β·SNR k )
[0026] wherein R0 is a preset noise covariance matrix, β is an attenuation coefficient, SNR k is a signal-to-noise ratio.
[0027] Optionally, the model of the MPC controller is:
[0028]
[0029] The constraint condition is: τ min ≤ τ ≤ τ max ,
[0030] wherein η d is a preset reference trajectory, W p and W τ are weight matrices, γ is an adaptive penalty coefficient, tr(P k+i ) is a state covariance matrix trace, and H p is a prediction time domain.
[0031] Optionally, the method further comprises:
[0032] judging whether the state covariance matrix trace is greater than a preset threshold value;
[0033] if yes, increasing the adaptive penalty coefficient;
[0034] if no, decreasing the adaptive penalty coefficient.
[0035] In a second aspect, the embodiments of the present application further provide a trajectory tracking control device, which comprises units for executing the above method.
[0036] In a third aspect, the embodiments of the present application further provide a computer device, which comprises a memory and a processor, the memory has a computer program stored thereon, and the processor implements the above method when executing the computer program.
[0037] In a fourth aspect, the embodiments of the present application further provide a computer readable storage medium, which stores a computer program, and the computer program can implement the above method when executed by a processor.
[0038] The embodiment of the present application provides a trajectory tracking control method, device and equipment. The method comprises the following steps: obtaining predicted trajectory information of an underwater target node at a first moment and a signal-to-noise ratio of the underwater target node; obtaining a preset Kalman filter; determining the gain of the preset Kalman filter according to the signal-to-noise ratio; inputting the predicted trajectory information into the preset Kalman filter to obtain first predicted trajectory information of the underwater target node at a second moment, wherein the second moment is the next moment of the first moment; and inputting the first predicted trajectory information and the gain of the preset Kalman filter into an MPC controller to obtain second predicted trajectory information of the underwater target node at the second moment, wherein the MPC controller is used for outputting the predicted trajectory information of the underwater target node. Therefore, in the technical scheme of the present application, the predicted trajectory information of the underwater target node at the first moment and the signal-to-noise ratio are obtained. Then, the gain of the preset Kalman filter is determined according to the signal-to-noise ratio. Further, the predicted trajectory information is inputted into the preset Kalman filter to obtain the first predicted trajectory information of the underwater target node at the second moment. Finally, the first predicted trajectory information and the gain are inputted into the MPC controller to obtain the predicted trajectory information of the underwater target node at the second moment. Therefore, the gain of the preset Kalman filter is dynamically adjusted according to the signal-to-noise ratio, and the motion trajectory of the underwater target node is more accurately predicted. BRIEF DESCRIPTION OF DRAWINGS
[0039] The accompanying drawings, which are incorporated herein and form a part of the specification, illustrate embodiments consistent with the present application and, together with the description, further serve to explain the principles of the application.
[0040] In order to more clearly illustrate the technical solutions of the embodiments of the present application or the prior art, the drawings needed to be used in the embodiments or the prior art description will be briefly introduced as follows. Obviously, those drawings can also provide other drawings for those skilled in the art without any creative effort.
[0041] One or more embodiments are exemplarily illustrated by pictures in the drawings corresponding to the embodiments, and the exemplarily illustrations do not constitute a limitation on the embodiments. Elements with the same reference numerals in the drawings represent similar elements, unless otherwise specified. The drawings do not constitute a proportional limitation.
[0042] Figure 1 A flowchart of a trajectory tracking control method provided by the embodiment of the present application is shown in the figure.
[0043] Figure 2 A schematic block diagram of a trajectory tracking control device provided by the embodiment of the present application is shown in the figure.
[0044] Figure 3 A computer device provided by the embodiment of the present application is shown in the figure. DETAILED DESCRIPTION
[0045] In order to make the purposes, technical solutions, and advantages of the embodiments of the present application clearer, the following will be combined with the accompanying drawings for the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments of the present application. Based on the embodiments in the present application, any other embodiments obtained by a person of ordinary skill in the art without creative effort belong to the scope of the present application.
[0046] The following disclosure provides many different embodiments, or examples, for implementing different structures of the present application. For the purpose of simplicity, the elements and arrangements of the specific examples that are described herein are intended to be examples only. It will be apparent, however, to one of ordinary skill in the art that the methods and techniques described herein can be practiced without such specific elements. In other instances, well-known structures and techniques have not been shown in detail in order to avoid obscuring aspects of the present application. This description, therefore, is not to be taken in a limited sense, and the sole purpose of the figures is to illustrate the general structures of the present application. Furthermore, the present application can be implemented in different embodiments and of different types. One of ordinary skill in the art will recognize that the implementations outlined herein are only some of the possible implementations and are not intended to be limiting.
[0047] It should be understood that the terms "comprises" and "comprising" when used in this specification and the following claims, specify the presence of stated features, integers, steps, operations, elements, and / or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and / or groups thereof.
[0048] It should also be understood that the terms used in the specification and the appended claims are intended to describe particular embodiments and do not intend to limit the present application. As used in the specification and the appended claims, the singular forms "a," "an," and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise.
[0049] It should further be understood that the term "and / or" as used in the specification and the appended claims, means any one of the associated listed items, as well as all possible combinations of the items, and includes the combinations.
[0050] As used in the specification and the appended claims, the term "if' can be interpreted as meaning "when" or "once" or "in response to a determination" or "in response to a detection" depending on the context. Similarly, the phrase "if determined" or "if detected [the described condition or event]" can be interpreted to mean "once determined" or "in response to a determination" or "once detected [the described condition or event]" or "in response to a detection [the described condition or event]" depending on the context.
[0051] To solve the technical problem that the motion trajectory of the underwater target node cannot be accurately predicted in the prior art, the application provides a trajectory tracking control device, which can accurately predict the motion trajectory of the underwater target node.
[0052] Figure 1 A flowchart of a trajectory tracking control method provided by an embodiment of the application is shown. In an embodiment, the method comprises:
[0053] S1, obtaining predicted trajectory information of an underwater target node at a first time and a signal-to-noise ratio of the underwater target node.
[0054] The first time is the current time. The underwater target node includes but is not limited to an underwater autonomous vehicle.
[0055] S2, obtaining a preset Kalman filter.
[0056] The preset Kalman filter is used to predict the motion trajectory of the underwater target node.
[0057] S3, determining the gain of the preset Kalman filter according to the signal-to-noise ratio.
[0058] The embodiment of the application adjusts the gain size of the preset Kalman filter according to the signal-to-noise ratio, so as to reduce the influence of the underwater environment on the prediction of the motion trajectory of the underwater target node.
[0059] S4, inputting the predicted trajectory information into the preset Kalman filter to obtain first predicted trajectory information of the underwater target node at a second time.
[0060] The second time is the next time of the first time.
[0061] S5, inputting the first predicted trajectory information and the gain of the preset Kalman filter into an MPC controller to obtain second predicted trajectory information of the underwater target node at the second time.
[0062] The MPC controller is a model predictive controller and is used to output the predicted trajectory information of the underwater target node. The application further predicts the second predicted trajectory information of the underwater target node at the second time in combination with the output result of the preset Kalman filter and the gain of the preset Kalman filter. The second predicted trajectory information includes a plurality of steps of executing a control strategy and is used to instruct the underwater target node to move. It should be noted that the embodiment of the application finally adopts a first step of executing a control strategy in the plurality of steps of executing a control strategy to drive the underwater target node to move forward. The underwater target node needs to execute the above S1-S5 in advance to obtain accurate trajectory information of the underwater target node moving forward.
[0063] The embodiment of the present application provides a trajectory tracking control method. The method comprises the following steps: obtaining predicted trajectory information of an underwater target node at a first moment and a signal-to-noise ratio of the underwater target node; obtaining a preset Kalman filter; determining the gain of the preset Kalman filter according to the signal-to-noise ratio; inputting the predicted trajectory information into the preset Kalman filter to obtain first predicted trajectory information of the underwater target node at a second moment, wherein the second moment is the next moment of the first moment; and inputting the first predicted trajectory information and the gain of the preset Kalman filter into an MPC controller to obtain second predicted trajectory information of the underwater target node at the second moment, wherein the MPC controller is used for outputting the predicted trajectory information of the underwater target node. Therefore, in the technical scheme of the present application, the predicted trajectory information of the underwater target node at the first moment and the signal-to-noise ratio are obtained. Then, the gain of the preset Kalman filter is determined according to the signal-to-noise ratio. Further, the predicted trajectory information is inputted into the preset Kalman filter to obtain the first predicted trajectory information of the underwater target node at the second moment. Finally, the first predicted trajectory information and the gain are inputted into the MPC controller to obtain the predicted trajectory information of the underwater target node at the second moment. Therefore, the gain of the preset Kalman filter is dynamically adjusted according to the signal-to-noise ratio, and the motion trajectory of the underwater target node is more accurately predicted.
[0064] In an embodiment, the obtaining the preset Kalman filter comprises:
[0065] S21, constructing a state equation of the preset Kalman filter according to the position information, the roll angle, the pitch angle and the yaw angle of the underwater target node.
[0066] Specifically, the state equation of the continuous preset Kalman filter is constructed according to the position information, the roll angle, the pitch angle and the yaw angle of the underwater target node.
[0067]
[0068] wherein η=[x,y,z,φ,θ,ψ] T , x, y and z are respectively the X-axis coordinate, the Y-axis coordinate and the Z-axis coordinate of the underwater target node, φ, θ and ψ are respectively the roll angle, the pitch angle and the yaw angle of the underwater target node, R(η) is a rotation matrix from the carrier coordinate system B to I, v=[u,v,w,p,q,r] T , u is the velocity of the underwater target in the X-axis direction, v is the velocity of the underwater target in the Y-axis direction, w is the velocity of the underwater target in the Z-axis direction, p is the rotation velocity of the underwater target in the X-axis direction, q is the rotation velocity of the underwater target in the Y-axis direction, r is the rotation velocity of the underwater target in the Z-axis direction, M is a mass matrix, C(v) is a Coriolis-centripetal force matrix, D(v) is a damping matrix, g(η) is a restoring force, τ=[τu ,τ v ,τ w ,τ p ,τ q ,τ r ] T is the control input, and w is the environmental disturbance vector.
[0069] Then, the state equation of the preset Kalman filter of the continuous type is Euler-discretized to obtain the following state equation:
[0070]
[0071] wherein η=[x,y,z,φ,θ,ψ] T , x, y and z are respectively the X-axis coordinate, Y-axis coordinate and Z-axis coordinate of the underwater target node, φ, θ and ψ are respectively the roll angle, pitch angle and yaw angle of the underwater target node, k is a discrete time index, T s is the sampling time, and R(η) is a rotation matrix from the carrier coordinate system B to I, v=[u,v,w,p,q,r] T , u is the velocity of the underwater target in the X-axis direction, v is the velocity of the underwater target in the Y-axis direction, w is the velocity of the underwater target in the Z-axis direction, p is the rotation velocity of the underwater target in the X-axis direction, q is the rotation velocity of the underwater target in the Y-axis direction, r is the rotation velocity of the underwater target in the Z-axis direction, M is a mass matrix, C(v) is a Coriolis-centripetal force matrix, D(v) is a damping matrix, g(η) is a restoring force, τ=[τ u ,τ v ,τ w ,τ p ,τ q ,τ r ] T is the control input, and w is the environmental disturbance vector.
[0072] S22, constructing an observation equation of the preset Kalman filter according to the position information of the underwater target node.
[0073] The embodiment of the application establishes an observation equation of the preset Kalman filter according to the TDOA positioning error.
[0074] Specifically, the position of the mth buoy is b m =[b mx ,b my ,b mz ] T , and the distance observation value of the underwater target node to the buoy m is:
[0075] d m =||S-b m ||+ε mS = [x, y, z] T
[0076] Wherein, x, y and z are the position coordinates of the underwater target node.
[0077] Taking the buoy 1 as an example, the observation quantity of TDOA is:
[0078] z m1 = d m - d1 = ||S - b m || - ||S - b1|| + ε m1
[0079] Wherein, the observation noise
[0080] Therefore, the observation equation is:
[0081]
[0082] Wherein, ε is the observation noise and R k is the positioning noise covariance of the underwater target node, S is the position coordinates of the underwater target node, b m is the position coordinates of the mth buoy.
[0083] In an embodiment, the gain of the preset Kalman filter is determined according to the signal-to-noise ratio, comprising:
[0084] S31, constructing the positioning noise covariance of the underwater target node according to the signal-to-noise ratio.
[0085] In an embodiment, the positioning noise covariance of the underwater target node is adjusted according to the environment. Specifically, the positioning noise covariance is:
[0086] R k = R0·exp(-β·SNR k )
[0087] Wherein, R0 is a preset noise covariance matrix, β is an attenuation coefficient, and SNR k is the signal-to-noise ratio.
[0088] It should be noted that when the catheter signal-to-noise ratio decreases, R k increases, indicating that the positioning reliability decreases. Conversely, the same is true.
[0089] S32, determining the gain according to the positioning noise covariance, the state equation and the observation equation.
[0090] Specifically, the solving process of the preset Kalman filter used by the embodiment of the application is as follows:
[0091] Prediction stage:
[0092] The next time state is predicted according to the state equation of the preset Kalman filter:
[0093]
[0094] Wherein, x = [S T ,v T ] T is a state vector, is a state transition Jacobian matrix.
[0095] Specifically,
[0096] Wherein, M is a mass matrix, C is a Coriolis-centrifugal force matrix, and D is a damping matrix update stage:
[0097] The gain of the preset Kalman filter is:
[0098]
[0099] State update:
[0100]
[0101] Covariance update:
[0102]
[0103] Wherein, is an observation Jacobian matrix. I is a unit matrix.
[0104] The gain P k of the process state in the embodiment of the application is automatically reduced with the increase of R k , and then effectively suppresses noise interference.
[0105] In an embodiment, the model of the MPC controller is:
[0106]
[0107] The constraint condition is: τ min ≤ τ ≤ τ max ,
[0108] Wherein, η d is a preset reference trajectory, W p and W τ are weight matrices, γ is an adaptive penalty coefficient, tr(P k+i ) is a state covariance matrix trace, H p is a prediction time domain.
[0109] It should be noted that the embodiment of the present application combines the gain of the preset Kalman filter to predict the time domain H p Solving the predicted trajectory information of the underwater target node.
[0110] In an embodiment, the method further comprises:
[0111] S6, judging whether the trace of the state covariance matrix is greater than a preset threshold. If yes, S7 is executed, and if no, S8 is executed.
[0112] The preset threshold is set by the applicant according to actual experience. Herein, the present application is not limited.
[0113] S7, increasing the adaptive penalty coefficient.
[0114] Specifically, when the measurement positioning error is large, the adaptive penalty coefficient is appropriately increased.
[0115] S8, reducing the adaptive penalty coefficient.
[0116] Specifically, when the measurement positioning error is large, the adaptive penalty coefficient is appropriately reduced, and then the control force τ is reduced, effectively controlling the speed of the underwater target node to be small, avoiding the underwater target node to have a large trajectory tracking error.
[0117] It should be noted that in the update link of the preset Kalman filter, the maximum eigenvalue (or the trace of the state covariance matrix) of the state covariance matrix is upper limited, so that the filter gain is "saturated" under extreme noise and does not diverge. When the observation residual exceeds the preset threshold, the residual is limited in amplitude to avoid the filter state from suddenly changing due to a single burst noise. The statistical characteristics (such as short-term variance) of the residual sequence are included in the filter gain calculation, enhancing the adaptive ability to non-Gaussian / time-varying noise.
[0118] The embodiment of the present application monitors the key information such as the jump speed, signal-to-noise ratio, and inertial measurement residual of the target underwater node in real time, and judges the change trend of the measurement noise and the process noise under the current environment. According to the real-time statistical results, the process noise w k and the measurement noise covariance R k and the update rate is limited in amplitude or anti-shake processing, effectively avoiding the occurrence of oscillation. The gain of the preset Kalman filter in the embodiment of the present application is linked with the weight of the MPC controller: when the covariance is significantly amplified, it is as a "positioning unreliable" signal to notify the MPC controller, prompting the MPC controller to increase the uncertainty penalty weight γ.
[0119] The predicted trajectory information of the underwater target node output by the MPC controller is:
[0120] {τ k∣k ,τ k+1∣kτ k+L∣k}
[0121] Referring to Figure 2 , Figure 2 is a schematic block diagram of a trajectory tracking control device provided by an embodiment of the present application. Corresponding to the above trajectory tracking control method, the present application also provides a trajectory tracking control device. The trajectory tracking control device includes units for executing the above trajectory tracking control method, and the trajectory tracking control device can be configured in a terminal such as a desktop computer, a tablet computer, a laptop computer, etc. Specifically, the trajectory tracking control device includes:
[0122] The first acquisition unit 201 is configured to acquire predicted trajectory information of an underwater target node at a first time and a signal-to-noise ratio of the underwater target node.
[0123] The second acquisition unit 202 is configured to acquire a preset Kalman filter.
[0124] The determination unit 203 is configured to determine a gain of the preset Kalman filter according to the signal-to-noise ratio.
[0125] The first input unit 204 is configured to input the predicted trajectory information into the preset Kalman filter to obtain first predicted trajectory information of the underwater target node at a second time, the second time being a next time of the first time.
[0126] The second input unit 205 is configured to input the first predicted trajectory information and the gain of the preset Kalman filter into an MPC controller to obtain second predicted trajectory information of the underwater target node at the second time, the MPC controller being configured to output predicted trajectory information of the underwater target node.
[0127] In an embodiment, the second acquisition unit 202 is specifically configured to construct a state equation of the preset Kalman filter according to position information, a roll angle, a pitch angle and a yaw angle of the underwater target node.
[0128] An observation equation of the preset Kalman filter is constructed according to the position information of the underwater target node.
[0129] In an embodiment, the determination unit 203 is specifically configured to construct a positioning noise covariance of the underwater target node according to the signal-to-noise ratio.
[0130] The gain is determined according to the positioning noise covariance, the state equation and the observation equation.
[0131] In an embodiment, the state equation is:
[0132]
[0133] wherein η = [x, y, z, φ, θ, ψ] T , x, y, z are X-axis coordinate, Y-axis coordinate and Z-axis coordinate of the underwater target node respectively, φ, θ and ψ are roll angle, pitch angle and yaw angle of the underwater target node respectively, k is discrete time index, T s is sampling time, R(η) is rotation matrix from carrier coordinate system B to I, v = [u, v, w, p, q, r] T , u is velocity of the underwater target in X-axis direction, v is velocity of the underwater target in Y-axis direction, w is velocity of the underwater target in Z-axis direction, p is rotation velocity of the underwater target in X-axis direction, q is rotation velocity of the underwater target in Y-axis direction, r is rotation velocity of the underwater target in Z-axis direction, M is mass matrix, C(v) is Coriolis-centripetal force matrix, D(v) is damping matrix, g(η) is restoring force, τ = [τ u , τ v , τ w , τ p , τ q , τ r ] T is control input, w is environmental disturbance vector.
[0134] In an embodiment, the observation equation is:
[0135]
[0136] wherein ε is observation noise and R k is positioning noise covariance of the underwater target node, S is position coordinate of the underwater target node, b m is position coordinate of the mth buoy.
[0137] In an embodiment, the positioning noise covariance is:
[0138] R k = R0·exp(-β·SNR k )
[0139] wherein R0 is preset noise covariance matrix, β is attenuation coefficient, SNR k is signal-to-noise ratio.
[0140] The model of the MPC controller is:
[0141]
[0142] The constraint condition is: τ min ≤ τ ≤ τ max ,
[0143] wherein η dTo preset the reference trajectory, W p and W τ Let γ be the weight matrix, tr(P) be the adaptive penalty coefficient, and tr(P) be the weight matrix. k+i H is the trace of the state covariance matrix. p For prediction in the time domain.
[0144] In one embodiment, the device further includes:
[0145] Judgment unit 206 is used to determine whether the trace of the state covariance matrix is greater than a preset threshold;
[0146] The increasing unit 207 is used to increase the adaptive penalty coefficient;
[0147] The reduction unit 208 is used to reduce the adaptive penalty coefficient.
[0148] like Figure 3 As shown, this application provides a computer device including a processor 31, a communication interface 32, a memory 33, and a communication bus 34. The processor 31, the communication interface 32, and the memory 33 communicate with each other through the communication bus 34. The memory 33 is used to store computer programs.
[0149] In one embodiment of this application, when the processor 31 executes the program stored in the memory 33, it implements the trajectory tracking control method provided in any of the foregoing method embodiments.
[0150] It will be understood by those skilled in the art that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program may be stored in a storage medium, which is a computer-readable storage medium. The computer program is executed by at least one processor in the computer system to implement the process steps of the embodiments of the above methods.
[0151] Therefore, embodiments of this application also provide a computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the steps of the trajectory tracking control method provided in any of the foregoing method embodiments.
[0152] The storage medium is a physical, non-transient storage medium, such as a USB flash drive, external hard drive, read-only memory (ROM), magnetic disk, or optical disk, or any other physical storage medium capable of storing program code. The computer-readable storage medium can be non-volatile or volatile.
[0153] Those skilled in the art can understand that the units and algorithm steps of each example described in combination with the embodiments disclosed herein can be realized in electronic hardware, computer software or a combination of both. In order to clearly illustrate the interchangeability of hardware and software, the components and steps of each example have been described in the above description in a general manner. Whether the functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of the present application.
[0154] In several embodiments provided in the present application, it should be understood that the disclosed devices and methods can be implemented in other ways. For example, the device embodiments described above are only illustrative. For example, the division of each unit is only a logical functional division, and actual implementation can have another division manner. For example, a plurality of units or components can be combined or integrated into another system, or some features can be omitted or not executed.
[0155] The steps in the method embodiments of the present application can be adjusted, combined and deleted in sequence according to actual needs. The units in the device embodiments of the present application can be combined, divided and deleted according to actual needs. In addition, each functional unit in each embodiment of the present application can be integrated in one processing unit, or each unit can exist physically, or two or more units can be integrated in one unit.
[0156] The integrated unit, if realized in the form of a software functional unit and sold or used as an independent product, can be stored in a storage medium. Based on such understanding, the technical solutions of the present application essentially or the part that contributes to the prior art, or the whole or part of the technical solutions can be embodied in the form of a software product. The computer software product is stored in a storage medium, and includes a plurality of instructions for causing a computer device (which can be a personal computer, a terminal or a network device, etc.) to execute all or part of the steps of the methods described in each embodiment of the present application.
[0157] In the above embodiments, the description of each embodiment has its own focus, and the parts not described in detail in a certain embodiment can be referred to the related description of other embodiments.
[0158] Obviously, those skilled in the art can make various modifications and variations to the present application without departing from the spirit and scope of the present application. Thus, these modifications and variations of the present application are intended to be included within the scope of the claims of the present application and their equivalents.
[0159] The above merely provides the specific implementation of the present application, but the protection scope of the present application is not limited to this. Any person skilled in the art can easily think of various equivalent modifications or replacements within the technical range disclosed by the present application, and these modifications or replacements should be covered in the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.
Claims
1. A trajectory tracking control method, characterized in that, include: Obtain the predicted trajectory information of the underwater target node at the first moment and the signal-to-noise ratio of the underwater target node; Obtain the preset Kalman filter; The gain of the preset Kalman filter is determined based on the signal-to-noise ratio; The predicted trajectory information is input into the preset Kalman filter to obtain the first predicted trajectory information of the underwater target node at the second time point, where the second time point is the next time point after the first time point; The first predicted trajectory information and the gain of the preset Kalman filter are input into the MPC controller to obtain the second predicted trajectory information of the underwater target node at the second time. The MPC controller is used to output the predicted trajectory information of the underwater target node.
2. The method according to claim 1, characterized in that, The acquisition of the preset Kalman filter includes: The state equation of the preset Kalman filter is constructed based on the position information, roll angle, pitch angle and yaw angle of the underwater target node; The observation equation of the preset Kalman filter is constructed based on the location information of the underwater target node.
3. The method according to claim 2, characterized in that, Determining the gain of the preset Kalman filter based on the signal-to-noise ratio includes: The positioning noise covariance of the underwater target node is constructed based on the signal-to-noise ratio. The gain is determined based on the localization noise covariance, the state equation, and the observation equation.
4. The method according to claim 2 or 3, characterized in that, The state equation is: Among them, η = [x, y, z, φ, θ, ψ] T x, y, and z are the X-axis, Y-axis, and Z-axis coordinates of the underwater target node, respectively; φ, θ, and ψ are the roll, pitch, and yaw angles of the underwater target node, respectively; k is the discrete-time index; and T... s Let R(η) be the sampling time, and R(η) be the rotation matrix from the carrier coordinate system B to I, where v = [u, v, w, p, q, r]. T u is the velocity of the underwater target along the X-axis, v is the velocity of the underwater target along the Y-axis, w is the velocity of the underwater target along the Z-axis, p is the rotational velocity of the underwater target along the X-axis, q is the rotational velocity of the underwater target along the Y-axis, r is the rotational velocity of the underwater target along the Z-axis, M is the mass matrix, C(v) is the Coriolis-centripetal force matrix, D(v) is the damping matrix, g(η) is the restoring force, and τ = [τ u ,τ v ,τ w ,τ p ,τ q ,τ r ] T For control input, w is the environmental disturbance vector.
5. The method according to claim 2 or 3, characterized in that, The observation equation is: Where ε is the observation noise and R k Let S be the localization noise covariance of the underwater target node, and b be the position coordinates of the underwater target node. m Let be the position coordinates of the m-th buoy.
6. The method according to claim 3, characterized in that, The location noise covariance is: R k =R0·exp(-β·SNR k ) Where R0 is the preset noise covariance matrix, β is the attenuation coefficient, and SNR is... k This refers to the signal-to-noise ratio.
7. The method according to claim 4, characterized in that, The model of the MPC controller is as follows: The constraint condition is: τ min ≤τ≤τ max , Where, η d To preset the reference trajectory, W p and W τ Let γ be the weight matrix, tr(P) be the adaptive penalty coefficient, and tr(P) be the weight matrix. k+i H is the trace of the state covariance matrix. p For prediction in the time domain.
8. The method according to claim 7, characterized in that, The method further includes: Determine whether the trace of the state covariance matrix is greater than a preset threshold; If so, then increase the adaptive penalty coefficient; If not, then reduce the adaptive penalty coefficient.
9. A trajectory tracking control device, characterized in that, include: The first acquisition unit is used to acquire the predicted trajectory information of the underwater target node at the first moment and the signal-to-noise ratio of the underwater target node; The second acquisition unit is used to acquire a preset Kalman filter; A determining unit is configured to determine the gain of the preset Kalman filter based on the signal-to-noise ratio; The first input unit is used to input the predicted trajectory information into the preset Kalman filter to obtain the first predicted trajectory information of the underwater target node at a second time point, where the second time point is the next time point after the first time point. The second input unit is used to input the first predicted trajectory information and the gain of the preset Kalman filter into the MPC controller to obtain the second predicted trajectory information of the underwater target node at the second time. The MPC controller is used to output the predicted trajectory information of the underwater target node.
10. A computer device, characterized in that, The computer device includes a memory and a processor, the memory storing a computer program, and the processor executing the computer program to implement the method as described in any one of claims 1 to 8.