CN114967714A - Anti-interference motion control method and system for autonomous underwater robot - Google Patents
Anti-interference motion control method and system for autonomous underwater robot Download PDFInfo
- Publication number
- CN114967714A CN114967714A CN202210728589.1A CN202210728589A CN114967714A CN 114967714 A CN114967714 A CN 114967714A CN 202210728589 A CN202210728589 A CN 202210728589A CN 114967714 A CN114967714 A CN 114967714A
- Authority
- CN
- China
- Prior art keywords
- state
- model
- nominal
- disturbance
- autonomous underwater
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 41
- 239000011159 matrix material Substances 0.000 claims description 34
- 230000007246 mechanism Effects 0.000 claims description 13
- 238000005457 optimization Methods 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 6
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 4
- 230000008859 change Effects 0.000 claims description 3
- 238000013016 damping Methods 0.000 claims description 3
- 238000009795 derivation Methods 0.000 claims description 3
- 238000001914 filtration Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 230000009467 reduction Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 230000000694 effects Effects 0.000 abstract description 4
- 230000006872 improvement Effects 0.000 description 6
- 238000005516 engineering process Methods 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 238000013461 design Methods 0.000 description 3
- 230000003044 adaptive effect Effects 0.000 description 2
- 238000013528 artificial neural network Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000005096 rolling process Methods 0.000 description 2
- 241000282414 Homo sapiens Species 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/04—Control of altitude or depth
- G05D1/06—Rate of change of altitude or depth
- G05D1/0692—Rate of change of altitude or depth specially adapted for under-water vehicles
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses an anti-interference motion control method of an autonomous underwater robot, which comprises the following steps: step 1, establishing an autonomous underwater robot dynamics and kinematics simplified linear model; step 2, establishing a system nominal model; step 3, constructing a disturbance observer; step 4, designing a nominal model predictive controller; step 5, designing an auxiliary model predictive controller; and 6, measuring the system state at the next moment, taking the moment as a new current moment, and returning to the step 3. According to the control method for the disturbance rejection motion of the autonomous underwater robot, the double-layer model prediction control framework is established, the influence caused by uncertainty can be effectively responded, the reference value can be tracked, and the control effect is better.
Description
Technical Field
The invention belongs to the technical field of autonomous underwater robot control, and particularly relates to an autonomous underwater robot anti-interference motion control method and system.
Background
An underwater robot is underwater equipment which replaces human beings to finish various complex operations in a marine environment. The autonomous underwater robot is increasingly used in the fields of marine science investigation, rescue and salvage, submarine resource survey and the like by virtue of the characteristics of flexibility, autonomy, intelligence and the like. In various application scenes, the autonomous underwater robot can realize the position and posture control of depth setting, submerging and surfacing, hovering and positioning and the like in a high-quality manner, which is a necessary condition for accurately and efficiently completing tasks.
The control technology of the autonomous underwater robot is the core technology of the autonomous underwater robot. The autonomous underwater robot has the characteristics of high nonlinearity, strong coupling and the like, so that the autonomous underwater robot is more difficult to apply to motion control. In addition, the motion control of the underwater robot is greatly influenced by common complex environments such as dynamic ocean currents, storms, deep water pressure and the like in the marine environment. Based on the characteristics, an accurate control model of the autonomous underwater robot is difficult to establish, and some un-modeled dynamic establishment simplified models are often ignored, so that the established models are not matched with the reality, and the control accuracy is influenced.
At present, the research on the motion control method of the autonomous underwater robot mainly comprises various methods such as PID control, fuzzy control, sliding mode control, neural network control, adaptive control and the like, and also has the technology of combining two or more control methods such as fuzzy sliding mode variable structure control, adaptive neural network control and the like. The patent ' a depth interval control method and system of an underwater robot ' (patent number: 202010310143.8) owned by Huazhong university of science and technology ' discloses a depth interval control method and system of an underwater robot under the constraint of rudder angle and rudder speed, which is beneficial to reducing the frequency and amplitude of rudder striking, but the method does not consider disturbance in the optimization process, only depends on the robustness of a controller, and may not ensure the control effect when large disturbance exists. The patent 'an underwater robot path tracking control method based on model predictive control' (patent number: 201811383664.5) owned by Qinghua university discloses an underwater robot path tracking control method based on model predictive control, which is realized by online numerical optimization and rolling time domain, processes the constraint of control input, has certain robustness, but is still insufficient to process model uncertainty and external interference. Therefore, a robust prediction control method capable of effectively processing disturbance is needed to be provided, so that the autonomous underwater robot system can strictly meet constraint conditions under the condition of uncertainty, and meanwhile, the robustness of the system is effectively improved.
Disclosure of Invention
Aiming at the defects in the prior art, the invention aims to solve the problem of motion control of an autonomous underwater robot under unknown bounded disturbance, and provides a control method and a control system with robustness.
In order to realize the purpose, the invention provides the following technical scheme: an anti-interference motion control method for an autonomous underwater robot comprises the following steps:
step 1, establishing an autonomous underwater robot dynamics and kinematics simplified linear model, converting the model into a continuous time model in a state space, and discretizing the model to obtain a discrete time model of a system;
step 2, establishing a system nominal model used as a prediction equation of a nominal model prediction controller, and converting the system state and control input constraint into a tightened state and control input constraint of a nominal system;
step 3, constructing a disturbance observer, taking a disturbance variable as an expanded state quantity, estimating a system disturbance value, and sending the estimated value to an auxiliary prediction controller;
designing a nominal model predictive controller, constructing a cost function, and solving a finite time domain optimization problem to obtain a nominal optimal control input and a corresponding nominal state;
step 5, designing an auxiliary model prediction controller, receiving nominal prediction input and output signals and a disturbance signal provided by a disturbance observer, solving optimal control aiming at an actual system model, and acting the first control quantity of a control sequence on the autonomous underwater robot system;
and 6, measuring the system state at the next moment, taking the moment as a new current moment, and returning to the step 3.
As a further improvement of the present invention, the tightened state and control input constraints of the nominal system in step 2 are specifically expressed as:
wherein α and γ represent a reduction coefficient in the range of (0, 1).
As a further improvement of the present invention, the specific steps of constructing the disturbance observer in step 3 are as follows:
step 31, regarding the disturbance as a newly added state quantity, adding the newly added state quantity into an uncertain system, and establishing a linear state observer as follows:
wherein the state estimate is
Outputting the estimated value
L, H is a matrix of the gains of the observer,
for disturbance estimation, β is an auxiliary variable;
step 32, calculating a state estimation error and a disturbance error:
then to e x The derivation yields:
obtaining the first derivative of the disturbance estimate:
wherein the estimate of the derivative of the disturbance
Step 33, the state estimation error and the disturbance estimation error are combined into an estimation error system as follows:
wherein G ═ e x T e w ] T ，
An estimation error representing the derivative of the disturbance.
As a further improvement of the present invention, the specific steps of designing a nominal model predictive controller in step 4, constructing a cost function, and solving a finite time domain optimization problem to obtain a nominal optimal control input and a corresponding nominal state are as follows:
step 41, designing an optimal control input for each sampling time, driving the nominal system state to a target point and minimizing a cost function, wherein the constructed cost function is as follows:
wherein x ref Is a target state vector, N p Q, R respectively represent positive definite weighting matrixes of a prediction state and control input for predicting a time domain, P is a terminal weighting matrix, and the positive definite weighting matrix is obtained by solving a Riccati equation;
step 42, the nominal prediction model constraints and the nominal state constraints and nominal control constraints are satisfied, and the nominal states and nominal controls are respectively constrained within the subsets of the actual constraints:
and (3) state constraint to be met by the model in the last prediction interval in the prediction time domain:
terminal constraints
Satisfy the requirement of
And the like. Wherein A is K ＝A+BK，
Is a robust invariant set;
step 43, representing the optimal control problem at any time k in a limited time domain:
wherein,
the linear feedback gain K is determined by a linear quadratic regulator.
As a further improvement of the present invention, the method for solving the optimal control for the actual system model in step 5 is to solve the following optimization problem:
wherein
Estimated by a disturbance observer.
As a further improvement of the present invention, the specific steps of establishing an autonomous underwater robot dynamics and kinematics simplified linear model in step 1, converting the model into a continuous time model in a state space, and discretizing the model to obtain a discrete time model of the system are as follows:
step 11, defining a required coordinate system: the system comprises an inertial coordinate system and a satellite coordinate system, wherein the inertial coordinate system is fixed on the ground, the satellite coordinate system is fixed on the autonomous underwater robot, the E-xi eta zeta represents the inertial coordinate system, and the O-xyz represents the satellite coordinate system;
step 12, defining six-freedom-degree speed V ═ u V l p q r of the autonomous underwater robot in the satellite coordinate system] T And the position and attitude angle eta relative to the fixed coordinate system [ x y z phi theta phi DEG] T The autonomous underwater robot motion equation is as follows:
MV+C(V)V+D(V)V+g(η)＝τ
wherein M is the sum of rigid mass and additional mass matrix, C (V) is Coriolis force and centripetal force matrix, D (V) is damping matrix, g (eta) is restoring moment, tau is propulsion system input matrix, J (eta) is Jacobian transformation matrix between inertial coordinate system and random coordinate system;
step 13, assuming that the transverse velocity v, the course angular velocity p, the roll angular velocity r and the roll angle phi of the relevant state quantities of the horizontal plane are all approximate to 0, the change of the longitudinal velocity u is smooth and stable, u is approximate to a constant, the model is decoupled only by considering the motion of the autonomous underwater robot in the vertical x-z plane, and the simplified vertical three-order depth control model is established as follows:
wherein z is the vertical depth under the inertial coordinate system, theta is the longitudinal inclination angle, q is the longitudinal inclination angle speed, delta is the rudder angle, I y For moment of inertia about the y-axis, M q 、M δ ，
To add a mass matrix, W is gravity, B 1 Is buoyancy, Δ q Representing the uncertainty part of the model, τ q Representing external time-varying disturbances caused by ocean currents and wind waves;
step 14, collecting position information and attitude information of the autonomous underwater robot by using a sensor, taking z, theta and q as state quantities, taking a rudder angle delta as a control input, and taking x as [ z, theta, q ]] T For describing the depth control problem of autonomous underwater robots. Converting the model into a state space equation:
wherein the state transition matrix
Input matrix
Discretizing a continuous time state space equation according to sampling time delta T to obtain:
x k+1 ＝Ax k +Bδ k +w k
therein, state
Control input
As a further improvement of the present invention, in step 14, it is assumed that the perturbation is bounded, and the bounded set is a convex set, which is represented as:
wherein w max Is the perturbation upper bound.
The invention also provides a system applying the method, which comprises a data acquisition device, a calculation unit, an execution mechanism and a driving mechanism, wherein the calculation unit is used for carrying the method and controlling the execution mechanism and the driving mechanism after the method is executed, the data acquisition device comprises a posture sensor and a depth sensor, the posture sensor is used for acquiring underwater information, robot posture and inertial navigation information and carrying out mean value filtering on the information, and the depth sensor is used for measuring the height distance to the water surface.
The invention has the beneficial effects that:
(1) according to the invention, a double-layer model predictive control framework is established, so that the influence caused by uncertainty can be effectively coped with and a reference value can be tracked, and a better control effect is achieved;
(2) the method estimates the prediction error caused by the uncertainty of the autonomous underwater robot system by using the disturbance observer, and compensates the estimated disturbance, so that the actual state track is close to the nominal system state track, and the control precision is improved;
(3) the invention explicitly processes the state and the control constraint, strictly ensures the constraint to be satisfied, keeps the state of the autonomous underwater robot system in a pipeline taking a nominal state track as a center, and improves the control robustness of the system.
Drawings
FIG. 1 is an inertial coordinate system and a satellite coordinate system of autonomous underwater robot motion in accordance with the present invention;
fig. 2 is a block diagram of autonomous underwater robot depth control in an embodiment provided by the present invention.
Detailed Description
The invention will be further described in detail with reference to the following examples, which are given in the accompanying drawings.
Taking the depth control of an autonomous underwater robot as an example, the method comprises the following specific steps:
the method comprises the following steps: and establishing a simplified prediction model according to the kinematics and dynamics model of the autonomous underwater robot.
First, the required coordinate system is defined: the system comprises an inertial coordinate system and a body-following coordinate system, wherein the inertial coordinate system is fixed on the ground, the body-following coordinate system is fixed on the autonomous underwater robot, and as shown in figure 1, E- ξ η ζ represents the inertial coordinate system, and O-xyz represents the body-following coordinate system.
And then defining the speed V ═ u V l p q r of the autonomous underwater robot with six degrees of freedom in the satellite coordinate system] T And the position and attitude angle eta relative to the fixed coordinate system [ x y z phi theta phi DEG] T The autonomous underwater robot motion equation is as follows:
MV+C(V)V+D(V)V+g(η)＝τ
wherein M is the sum of the rigid body mass and the additional mass matrix, C (V) is a Coriolis force and centripetal force matrix, D (V) is a damping matrix, g (eta) is a restoring moment, tau is a propulsion system input matrix, and J (eta) is a Jacobian transformation matrix between an inertial coordinate system and a random coordinate system.
Assuming that the transverse velocity v, the course angular velocity p, the roll angular velocity r and the roll angle phi of the relevant state quantities of the horizontal plane are all approximate to 0, the change of the longitudinal velocity u is gentle and stable, u is approximate to a constant, the model is decoupled only by considering the movement of the autonomous underwater robot in the vertical x-z plane, and the simplified vertical three-order depth control model is established as follows:
wherein z is the vertical depth under the inertial coordinate system, theta is the longitudinal inclination angle, q is the longitudinal inclination angle speed, delta is the rudder angle, I y For moment of inertia about the y-axis, M q 、M δ ，
To add a mass matrix, W is gravity, B 1 Is buoyancy, Δ q Representing the uncertainty part of the model, τ q Representing external time-varying disturbances caused by ocean currents and waves.
The unmodeled dynamics of the system is internal disturbance, and various uncertainties brought by ocean currents, storms and the like are external disturbance, and the internal disturbance delta is converted into q External disturbance τ q Are all considered as bounded disturbances and are equivalent to the total disturbance Δ of the system w 。
The position information and the attitude information of the autonomous underwater robot are acquired by using a sensor, z, theta and q are used as state quantities, a rudder angle delta is used as control input, and x is [ z, theta and q ]] T For describing the depth control problem of autonomous underwater robots. Converting the model into a state space equation:
wherein the state transition matrix
Input matrix
Discretizing a continuous time state space equation according to sampling time delta T to obtain:
x k+1 ＝Ax k +Bδ k +w k
wherein the state
Control input
Due to the complexity and time variability of the underwater environment, it is difficult to obtain an accurate value of the disturbance through a model or a sensor, and it can be assumed that the disturbance is bounded, and the bounded set is a convex set, which can be expressed as:
wherein w max Is the perturbation upper bound.
Step two: and establishing a nominal model of the autonomous underwater robot system and calculating nominal constraints.
The state and control input constraints are first set according to the actual physical conditions of the system and the saturation constraints of the mechanism. The submergence speed of the autonomous underwater robot is related to the pitch angle and the torque, and the large pitch angle and the large torque enable the autonomous underwater robot to submerge rapidly but are not suitable to be too large. In order to ensure the safety and stability of the sports, certain pitching constraint conditions must be met to avoid the phenomena of rolling and the like. Thus, the given state constraints include constraints on pitch angle and pitch angular velocity, and the control input constraints refer to the range of rudder angle changes that allow for input:
wherein, aggregate
Are all tight convex sets, x, containing the origin min 、x max 、δ min 、δ max Is a known constant.
Nominal model means a virtual but precisely known model built in the control system, written in the form of a state space, ignoring all uncertainties of the system:
wherein,
is the nominal state at the time of k,
is the nominal control input at time k.
Part of the phase difference between the actual model and the nominal model:
and calculating the state and input constraint of the nominal system according to the state and control constraint of the autonomous underwater robot system. Due to neglecting the system uncertainty, the nominal system has more compact constraints compared with the actual system, and the nominal state constraint and the nominal control constraint are calculated and tightened on the basis of the actual constraints, which are respectively expressed as:
wherein α and γ represent a reduction coefficient in the range of (0, 1).
Step three: the system disturbance is estimated using a disturbance observer. Firstly, taking the disturbance as an extended state quantity to be added into an uncertain system, and establishing a linear state observer as follows:
wherein the state estimate is
Outputting the estimated value
L, H is a matrix of observer gains which,
for disturbance estimation, β is an auxiliary variable.
The state estimation error and the disturbance estimation error are as follows:
then to e x The derivation yields:
the first derivative of the disturbance estimate can then be expressed as:
wherein the estimated value of the derivative of the disturbance
The state estimation error and the disturbance estimation error constitute an estimation error system as follows:
wherein G ═ e x T e w ] T ，
An estimation error representing the derivative of the disturbance.
Through designing a proper observer gain matrix, the characteristic values of the error system are all positioned on a left half complex plane, so that the estimation error of the error system is converged to zero, and the stability of the disturbance observer is ensured. The obtained disturbance estimation value is fed back to the auxiliary prediction controller, and the influence of the disturbance on the controlled object is counteracted as much as possible.
Step four: and designing a nominal model prediction controller, and calculating the optimal nominal control input in a prediction time domain.
An optimal control input is designed for each sampling instant, driving the nominal system state to the target point and minimizing the cost function. The control target is to design a nominal input rudder angle under the condition of not considering system uncertainty and unknown time-varying external disturbance, so that the error between a nominal state and a given reference state value in a prediction time domain is minimum, a cost function reflects the control requirement of the system on the state and input of each prediction stage, and the cost function adopts the following form:
wherein x is ref Is a target state vector, N p For the prediction horizon Q, R represents the positive definite weighting matrix of the prediction states and control inputs, respectively. And P is a terminal weight matrix and is obtained by solving the Riccati equation.
Solving the optimal problem requires satisfying the nominal prediction model constraints as well as the nominal state constraints and the nominal control constraints. In the finite time domain, the optimal control problem at any time k is represented as:
wherein,
the linear feedback gain K is determined by a linear quadratic regulator.
The nominal state and nominal control inputs are constrained within a subset of the actual constraints, respectively:
and (3) state constraint to be met by the model in the last prediction interval in the prediction time domain:
terminal constraints
Satisfy the requirement of
And the like. Wherein A is K ＝A+BK，
Is a robust invariant set.
The optimization problem is intended to predict the state as close to the target state value as possible and the rudder angle input as small as possible. Solving the optimization problem to obtain a nominal optimal control sequence
And corresponding nominal state sequence
And sent to the controller of the lower layer as a reference instruction for assisting the predictive controller.
Step five: and designing an auxiliary predictive controller, and designing a control strategy based on the actual predictive model.
Since autonomous underwater robotic systems may be affected by disturbances, future state trajectories may differ from nominal predicted trajectories. To counteract the effects of the disturbance, the auxiliary predictive controller is designed so that the system state and control inputs are close to the nominal sequence. Solving the following optimization problem:
wherein
Estimated by a disturbance observer.
Final control signal
Solving two optimization problems through the fourth step and the fifth step to obtain a first control signal of the sequence
And acts as a rudder angle signal to the controlled object.
Step six: and measuring the system state at the next moment, taking the moment as the new current moment, and returning to the step three.
In another aspect, the present embodiment provides a system, including a data acquisition device, a calculation unit, an execution mechanism, and a driving mechanism, where the calculation unit is configured to carry the above method and control the execution mechanism and the driving mechanism after executing the method, the data acquisition device includes a posture sensor and a depth sensor, the posture sensor is configured to acquire underwater information, robot posture and inertial navigation information and perform mean filtering on the information, the depth sensor is configured to measure a height distance to a water surface, and under consideration of unknown disturbance caused by unmodeled dynamic and dynamic ocean currents, storms, and the like, a system model satisfying state and control constraints is established, and a double-layer model prediction controller is designed in combination with a disturbance observer. The upper layer designs a nominal predictive controller for a nominal model, and finds the nominal input and the nominal state so that the error between the nominal state and a reference value is minimum. And the lower layer designs an auxiliary prediction controller aiming at the actual model, and solves the control input actually acting on the autonomous underwater robot so as to compensate the error between the nominal state and the actual state. The method can ensure that the state of the autonomous underwater robot system is strictly kept in a small neighborhood of a target state to fluctuate so as to accurately realize motion control.
The above description is only a preferred embodiment of the present invention, and the protection scope of the present invention is not limited to the above embodiments, and all technical solutions belonging to the idea of the present invention belong to the protection scope of the present invention. It should be noted that modifications and embellishments within the scope of the invention may occur to those skilled in the art without departing from the principle of the invention, and are considered to be within the scope of the invention.
Claims (8)
1. An anti-interference motion control method of an autonomous underwater robot is characterized by comprising the following steps: the method comprises the following steps:
step 1, establishing an autonomous underwater robot dynamics and kinematics simplified linear model, converting the model into a continuous time model in a state space, and discretizing the model to obtain a discrete time model of a system;
step 2, establishing a system nominal model used as a prediction equation of a nominal model prediction controller, and converting the system state and control input constraint into a tightened state and control input constraint of a nominal system;
step 3, constructing a disturbance observer, taking a disturbance variable as an expanded state quantity, estimating a system disturbance value, and sending the estimated value to an auxiliary prediction controller;
designing a nominal model predictive controller, constructing a cost function, and solving a finite time domain optimization problem to obtain a nominal optimal control input and a corresponding nominal state;
step 5, designing an auxiliary model prediction controller, receiving nominal prediction input and output signals and a disturbance signal provided by a disturbance observer, solving optimal control aiming at an actual system model, and acting the first control quantity of a control sequence on the autonomous underwater robot system;
and 6, measuring the system state at the next moment, taking the moment as a new current moment, and returning to the step 3.
2. The autonomous underwater vehicle disturbance rejection motion control method according to claim 1, characterized in that: the tightened state of the nominal system and the control input constraints in step 2 are specifically represented as:
wherein α and γ represent a reduction coefficient in the range of (0, 1).
3. The autonomous underwater robot disturbance rejection motion control method according to claim 1 or 2, characterized in that: the specific steps of constructing the disturbance observer in the step 3 are as follows:
step 31, adding the disturbance as a newly added state quantity into an uncertain system, and establishing a linear state observer as follows:
wherein the state estimate is
Outputting the estimated value
L, H is a matrix of the gains of the observer,
for disturbance estimation, β is an auxiliary variable;
step 32, calculating a state estimation error and a disturbance error:
then to e x The derivation yields:
obtaining the first derivative of the disturbance estimate:
wherein the estimate of the derivative of the disturbance
Step 33, the state estimation error and the disturbance estimation error are combined into an estimation error system as follows:
wherein G ═ e x T e w ] T ，
Representing the estimation error of the derivative of the disturbance.
4. The autonomous underwater vehicle disturbance rejection motion control method according to claim 3, characterized in that: the specific steps of designing a nominal model predictive controller, constructing a cost function, solving a finite time domain optimization problem, and obtaining a nominal optimal control input and a corresponding nominal state in the step 4 are as follows:
step 41, designing an optimal control input for each sampling time, driving the nominal system state to a target point and minimizing a cost function, wherein the constructed cost function is as follows:
wherein x ref Is a target state vector, N p For predicting the time domain, Q, R represents positive definite weighting matrix of prediction state and control input respectively, P is terminal weighting matrix, and is obtained by solving Riccati equation;
step 42, the nominal prediction model constraints and the nominal state constraints and nominal control constraints are satisfied, and the nominal state and nominal control are respectively constrained in the subsets of the actual constraints:
the state constraint to be satisfied by the last prediction interval in the prediction time domain is as follows:
terminal constraints
Satisfy the requirement of
And the like. Wherein A is K ＝A+BK，
Is a robust invariant set;
step 43, representing the optimal control problem at any time k in a limited time domain:
wherein,
the linear feedback gain K is determined by a linear quadratic regulator.
5. The autonomous underwater vehicle disturbance rejection motion control method according to claim 4, characterized in that: in the step 5, the following optimization problem is solved in a manner of solving the optimal control for the actual system model:
wherein
Estimated by a disturbance observer.
6. The autonomous underwater vehicle disturbance rejection motion control method of claim 5, characterized in that: the specific steps of establishing an autonomous underwater robot dynamics and kinematics simplified linear model in the step 1, converting the model into a continuous time model in a state space, and discretizing the model to obtain a discrete time model of the system are as follows:
step 11, defining a required coordinate system: the system comprises an inertial coordinate system and a satellite coordinate system, wherein the inertial coordinate system is fixed on the ground, the satellite coordinate system is fixed on the autonomous underwater robot, the E-xi eta zeta represents the inertial coordinate system, and the O-xyz represents the satellite coordinate system;
step 12, defining six-freedom-degree speed V ═ u V l p q r of the autonomous underwater robot in the satellite coordinate system] T And the position and attitude angle eta relative to the fixed coordinate system [ x y z phi theta phi DEG] T The autonomous underwater robot motion equation is as follows:
MV+C(V)V+D(V)V+g(η)＝τ
wherein M is the sum of rigid mass and additional mass matrix, C (V) is Coriolis force and centripetal force matrix, D (V) is damping matrix, g (eta) is restoring moment, tau is propulsion system input matrix, J (eta) is Jacobian transformation matrix between inertial coordinate system and random coordinate system;
step 13, assuming that the transverse velocity v, the course angular velocity p, the roll angular velocity r and the roll angle phi of the relevant state quantities of the horizontal plane are all approximate to 0, the change of the longitudinal velocity u is smooth and stable, u is approximate to a constant, the model is decoupled only by considering the motion of the autonomous underwater robot in the vertical x-z plane, and the simplified vertical three-order depth control model is established as follows:
wherein z is the vertical depth under the inertial coordinate system, theta is the longitudinal inclination angle, q is the longitudinal inclination angle speed, delta is the rudder angle, I y For moment of inertia about the y-axis, M q 、M δ ，
To add a mass matrix, W is gravity, B 1 Is buoyancy, Δ q Representing the uncertainty part of the model, τ q Representing external time-varying disturbances caused by ocean currents and wind waves;
step 14, collecting position information and attitude information of the autonomous underwater robot by using a sensor, taking z, theta and q as state quantities, taking a rudder angle delta as a control input, and taking x as [ z, theta, q ]] T For describing the depth control problem of autonomous underwater robots. Converting the model into a state space equation:
wherein the state transition matrix
Input matrix
Discretizing a continuous time state space equation according to sampling time delta T to obtain:
x k+1 ＝Ax k +Bδ k +w k
wherein the state
Control input
7. The autonomous underwater vehicle disturbance rejection motion control method of claim 6, characterized in that: in step 14, it is assumed that the perturbation is bounded, and the bounded set is a convex set, which is expressed as:
wherein w max Is the perturbation upper bound.
8. A system to which the disturbance rejection motion control method of the autonomous underwater robot of any of claims 1 to 7 is applied, characterized in that: the system comprises a data acquisition device, a calculation unit, an execution mechanism and a driving mechanism, wherein the calculation unit is used for carrying the method, and controls the execution mechanism and the driving mechanism after the method is executed, the data acquisition device comprises a posture sensor and a depth sensor, the posture sensor is used for acquiring underwater information, robot posture and inertial navigation information and carrying out mean value filtering on the information, and the depth sensor is used for measuring the height distance to the water surface.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210728589.1A CN114967714A (en) | 2022-06-24 | 2022-06-24 | Anti-interference motion control method and system for autonomous underwater robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210728589.1A CN114967714A (en) | 2022-06-24 | 2022-06-24 | Anti-interference motion control method and system for autonomous underwater robot |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114967714A true CN114967714A (en) | 2022-08-30 |
Family
ID=82964877
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210728589.1A Pending CN114967714A (en) | 2022-06-24 | 2022-06-24 | Anti-interference motion control method and system for autonomous underwater robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114967714A (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115303455A (en) * | 2022-09-16 | 2022-11-08 | 北京大学 | Underwater bionic robot motion control method, device, equipment and storage medium |
| CN115562317A (en) * | 2022-11-08 | 2023-01-03 | 浙江大学 | Four-rotor underwater robot water flow disturbance resistance control method based on self-adaptive strategy |
| CN115586782A (en) * | 2022-10-17 | 2023-01-10 | 湖南大学 | Autonomous underwater robot motion control method and device |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2015016826A (en) * | 2013-07-12 | 2015-01-29 | 三菱重工業株式会社 | Underwater sailing body, underwater sailing body controller, and underwater sailing body control method |
| EP3598262A1 (en) * | 2018-07-17 | 2020-01-22 | Ge Aviation Systems Llc, Inc. | Method and system for determining a climb profile |
| CN113485390A (en) * | 2021-08-26 | 2021-10-08 | 湖南大学 | Autonomous underwater robot dynamic positioning method and system capable of resisting ocean current disturbance |
-
2022
- 2022-06-24 CN CN202210728589.1A patent/CN114967714A/en active Pending
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2015016826A (en) * | 2013-07-12 | 2015-01-29 | 三菱重工業株式会社 | Underwater sailing body, underwater sailing body controller, and underwater sailing body control method |
| EP3598262A1 (en) * | 2018-07-17 | 2020-01-22 | Ge Aviation Systems Llc, Inc. | Method and system for determining a climb profile |
| CN113485390A (en) * | 2021-08-26 | 2021-10-08 | 湖南大学 | Autonomous underwater robot dynamic positioning method and system capable of resisting ocean current disturbance |
Non-Patent Citations (2)
| Title |
|---|
| TIANDONG ZHANG: "Design and Locomotion Control of a Dactylopteridae-Inspired Biomimetic Underwater Vehicle With Hybrid Propulsion", IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING, 8 April 2021 (2021-04-08) * |
| 韩涛: "多关节水下机器人方案设计与关键技术应用", 控制与信息技术, 15 December 2023 (2023-12-15) * |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115303455A (en) * | 2022-09-16 | 2022-11-08 | 北京大学 | Underwater bionic robot motion control method, device, equipment and storage medium |
| CN115586782A (en) * | 2022-10-17 | 2023-01-10 | 湖南大学 | Autonomous underwater robot motion control method and device |
| CN115586782B (en) * | 2022-10-17 | 2024-04-12 | 湖南大学 | Autonomous underwater robot motion control method and device |
| CN115562317A (en) * | 2022-11-08 | 2023-01-03 | 浙江大学 | Four-rotor underwater robot water flow disturbance resistance control method based on self-adaptive strategy |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107168312B (en) | Space trajectory tracking control method for compensating UUV kinematic and dynamic interference | |
| Zhang et al. | MPC-based 3-D trajectory tracking for an autonomous underwater vehicle with constraints in complex ocean environments | |
| CN106444806B (en) | The drive lacking AUV three-dimensional track tracking and controlling method adjusted based on biological speed | |
| CN107024863B (en) | UUV trajectory tracking control method for avoiding differential explosion | |
| Moreira et al. | Path following control system for a tanker ship model | |
| Yan et al. | Consensus formation tracking for multiple AUV systems using distributed bioinspired sliding mode control | |
| Fossen et al. | Kalman filtering for positioning and heading control of ships and offshore rigs | |
| Li et al. | Robust adaptive motion control for underwater remotely operated vehicles with velocity constraints | |
| CN114967714A (en) | Anti-interference motion control method and system for autonomous underwater robot | |
| CN105807789B (en) | UUV control methods based on the compensation of T-S Fuzzy Observers | |
| CN112068440B (en) | AUV recovery butt joint power positioning control method based on model prediction control | |
| Avila et al. | Modeling and identification of an open-frame underwater vehicle: The yaw motion dynamics | |
| CN114115262B (en) | Multi-AUV actuator saturation cooperative formation control system and method based on azimuth information | |
| Sebastián et al. | Adaptive fuzzy sliding mode controller for the kinematic variables of an underwater vehicle | |
| CN111857165B (en) | Trajectory tracking control method of underwater vehicle | |
| CN113467231A (en) | Unmanned ship path tracking method based on sideslip compensation ILOS guidance law | |
| Deng et al. | UKF based nonlinear offset-free model predictive control for ship dynamic positioning under stochastic disturbances | |
| Liu et al. | A hierarchical disturbance rejection depth tracking control of underactuated AUV with experimental verification | |
| Gao et al. | Command filtered path tracking control of saturated ASVs based on time‐varying disturbance observer | |
| CN117389312A (en) | Model-based three-dimensional tracking control method for counter roll of underwater vehicle | |
| CN112925332B (en) | Cooperative intersection butt joint control method for unmanned ship and underwater unmanned submersible vehicle combined system | |
| Du et al. | Research on UUV recovery active disturbance rejection control based on LMNN compensation | |
| Jagannathan et al. | One-layer neural-network controller with preprocessed inputs for autonomous underwater vehicles | |
| de Cerqueira Gava et al. | AUV cruising auto pilot for a long straight confined underwater tunnel | |
| Wang et al. | Adaptive cooperative diving of saucer-type underwater gliders subject to model uncertainties and input constraints |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |