CN114967714B - Autonomous underwater robot anti-interference motion control method and system - Google Patents
Autonomous underwater robot anti-interference motion control method and system Download PDFInfo
- Publication number
- CN114967714B CN114967714B CN202210728589.1A CN202210728589A CN114967714B CN 114967714 B CN114967714 B CN 114967714B CN 202210728589 A CN202210728589 A CN 202210728589A CN 114967714 B CN114967714 B CN 114967714B
- Authority
- CN
- China
- Prior art keywords
- state
- model
- nominal
- underwater robot
- disturbance
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 38
- 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 7
- 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
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 230000008602 contraction Effects 0.000 claims description 3
- 238000013016 damping Methods 0.000 claims description 3
- 238000001914 filtration Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 230000036039 immunity Effects 0.000 claims 4
- 230000000694 effects Effects 0.000 abstract description 5
- 230000006872 improvement Effects 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 4
- 238000013461 design Methods 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 238000013528 artificial neural network Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000005096 rolling process Methods 0.000 description 2
- 241000282414 Homo sapiens Species 0.000 description 1
- 230000006978 adaptation Effects 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
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000007667 floating 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
- 238000011160 research Methods 0.000 description 1
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)
- Feedback Control In General (AREA)
Abstract
The invention discloses an autonomous underwater robot anti-interference motion control method, 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; step5, designing an auxiliary model prediction controller; and 6, measuring the system state at the next moment, taking the moment as the new current moment, and returning to the step 3. According to the autonomous underwater robot anti-interference motion control method, a double-layer model predictive control framework is established, so that the influence of the effect on uncertainty can be effectively achieved, the reference value can be tracked, and a better control effect is achieved.
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 complete various complex operations in a marine environment. Autonomous underwater robots are increasingly used in the fields of marine science investigation, rescue salvage, submarine resource exploration 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 gesture control of depth fixing, submerging, floating, hovering, positioning and the like with high quality, which is a necessary condition for accurately and efficiently completing tasks.
The control technology of autonomous underwater robots is its core technology. 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 in motion control. In addition, the motion control of the underwater robot is greatly influenced by the common and 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 build, and some unmodeled dynamic build simplified models are often ignored, so that the built model is not matched with the actual model, and the control accuracy is affected.
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, self-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, self-adaptive neural network control and the like. The patent ' a control method and a system for the depth interval of an underwater robot ' owned by the university of science and technology of China ' (patent number: 202010310143.8) discloses a control method and a system for the depth interval of an underwater robot under the constraint of rudder angle rudder speed, which are beneficial to reducing the frequency and the amplitude of rudder playing, but the method does not consider disturbance in the optimization process, only depends on the robustness of a controller, and can not guarantee the control effect when larger disturbance exists. The patent 'a model predictive control-based underwater robot path tracking control method' (patent number: 201811383664.5) discloses a model predictive control-based underwater robot path tracking control method, which is realized through online numerical optimization and rolling time domain, processes constraints of control input, has certain robustness, and is still insufficient for processing model uncertainty and external interference. Therefore, it is necessary to provide a robust predictive control method capable of effectively processing disturbance, so as to ensure that an 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 existing 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 robust control method and a robust control system.
In order to achieve the above purpose, the present invention provides the following technical solutions: an autonomous underwater robot anti-interference motion control 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 the system;
Step 2, a system nominal model is established, and is used as a prediction equation of a nominal model prediction controller to convert the system state and control input constraint into a tightened state and control input constraint of the nominal system;
step 3, constructing a disturbance observer, taking disturbance variables as extended state quantities, estimating a system disturbance value, and sending the estimated value to an auxiliary prediction controller;
step 4, designing a nominal model predictive controller, constructing a cost function, solving a finite time domain optimization problem, and obtaining nominal optimal control input and corresponding nominal states;
Step 5, designing an auxiliary model prediction controller, receiving nominal prediction input and output signals and disturbance signals 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 the new current moment, and returning to the step 3.
As a further improvement of the invention, the state of tightening and control input constraints of the nominal system in step 2 are expressed in particular as:
where α and γ represent the contraction coefficients in the (0, 1) range.
As a further improvement of the present invention, the specific steps of constructing the disturbance observer in the step 3 are as follows:
Step 31, regarding the disturbance as a newly added state quantity to the uncertain system, establishing the following linear state observer:
wherein the state estimate Outputting the estimated valueL, H is a matrix of observer gains,Beta is an auxiliary variable for disturbance estimation;
Step 32, calculating state estimation errors and disturbance errors:
And then deriving e x to obtain:
obtaining a first derivative of the disturbance estimate:
wherein the estimated value of the derivative of the disturbance Step 33, the state estimation error and the disturbance estimation error are combined into the following estimation error system:
Wherein g= [ e x T ew]T, Representing the estimated error of the derivative of the disturbance.
As a further improvement of the present invention, the specific steps of designing a nominal model predictive controller in the step 4, constructing a cost function, solving a finite time domain optimization problem, and obtaining 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 the target point and minimizing the cost function, wherein the constructed cost function is as follows:
Wherein x ref is a target state vector, N p is a prediction time domain, Q, R respectively represents a prediction state and a positive fixed weighting matrix of control input, and P is a terminal weighting matrix, and is obtained by solving a Riccati equation;
Step 42, satisfying the nominal prediction model constraints and nominal state constraints and nominal control constraints, the nominal state and nominal control being constrained within a subset of the actual constraints, respectively:
the state constraint to be satisfied by the last prediction interval of the model in the prediction time domain:
Terminal constraints Satisfy the following requirementsAnd the like. Wherein, a K = a + BK,Is a robust invariant set;
Step 43, representing the optimal control problem at any time k in the finite time domain:
Wherein, The linear feedback gain K is obtained by a linear quadratic regulator.
As a further improvement of the present invention, the method of solving the optimal control for the actual system model in the step 5 is to solve the following optimization problem:
Wherein the method comprises the steps of 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 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, E- ζηζ represents the inertial coordinate system, and O-xyz represents the satellite coordinate system;
Step 12, defining six degrees of freedom speeds V= [ u V l p q r ] T and position and attitude angles eta= [ x y z phi ] T of the autonomous underwater robot in a satellite coordinate system, wherein the motion equation of the autonomous underwater robot 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 the coriolis force and centripetal force matrix, D (V) is the damping matrix, g (eta) is the restoring moment, tau is the propulsion system input matrix, J (eta) is the jacobian conversion matrix between the inertial coordinate system and the satellite coordinate system;
Step 13, assuming that the relevant state quantity transverse speed v, heading angular speed p, roll angular speed r and roll angle phi of the horizontal plane are all approximately 0, the change of the longitudinal speed u is gentle and stable, u is approximately a constant, and only the motion of the autonomous underwater robot in the vertical x-z plane is considered, decoupling is carried out on the model, and a simplified vertical third-order depth control model is established as follows:
Wherein z is the vertical depth in the inertial coordinate system, θ is the pitch angle, q is the pitch angle speed, δ is the rudder angle, I y is the moment of inertia about the y-axis, M q、Mδ, For the additional mass matrix, W is gravity, B 1 is buoyancy, delta q represents a model uncertainty part, and tau q represents external time-varying disturbance caused by ocean currents and storms;
And 14, acquiring position information and attitude information of the autonomous underwater robot by using a sensor, wherein z, theta and q are used as state quantities, rudder angle delta is used as control input, and x= [ z, theta and q ] T is used for describing the depth control problem of the autonomous underwater robot. Converting the model into a state space equation:
wherein the state transition matrix Input matrixDiscretizing a continuous time state space equation according to the sampling time delta T to obtain:
xk+1=Axk+Bδk+wk
Wherein the state is Control inputAs a further development of the invention, it is assumed in said step 14 that the disturbance is bounded and that the bounded set is a convex set, expressed as: Where w max is the upper perturbation bound.
The invention further 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, the execution mechanism and the driving mechanism are controlled after the method is executed, the data acquisition device comprises an attitude sensor and a depth sensor, the attitude sensor is used for acquiring underwater information, the attitude of the robot and inertial navigation information, the information is subjected to mean value filtering, and the depth sensor is used for measuring the height distance to the water surface.
The invention has the beneficial effects that:
(1) The invention establishes a double-layer model predictive control framework, can effectively influence uncertainty and track reference values, and has better control effect;
(2) According to the invention, a disturbance observer is utilized to estimate a prediction error caused by uncertainty of the autonomous underwater robot system, and the estimated disturbance is compensated, so that an actual state track is close to a nominal system state track, and the control precision is improved;
(3) The invention explicitly processes the state and control constraint, strictly ensures constraint satisfaction, maintains the state of the autonomous underwater robot system in a pipeline taking the nominal state track as the 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 the present invention;
fig. 2 is a frame 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 examples of embodiments shown in the drawings.
Taking depth control of an autonomous underwater robot as an example, the specific steps are as follows:
step one: and establishing a simplified prediction model according to the kinematic and dynamic model of the autonomous underwater robot.
First, defining a required coordinate system: the inertial coordinate system and the satellite coordinate system are fixed on the ground, and the satellite coordinate system is fixed on the autonomous underwater robot, as shown in fig. 1, wherein E- ζηζ represents the inertial coordinate system, and O-xyz represents the satellite coordinate system.
Then, the velocity v= [ u V l p q r ] T of the autonomous underwater robot in six degrees of freedom in the satellite coordinate system and the position and attitude angle η= [ x y z Φθψ ] T with respect to the fixed coordinate system are defined, and the equation of motion of the autonomous underwater robot 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 the coriolis force and centripetal force matrix, D (V) is the damping matrix, g (eta) is the restoring moment, tau is the propulsion system input matrix, and J (eta) is the jacobian conversion matrix between the inertial coordinate system and the satellite coordinate system.
Assuming that the related state quantity transverse speed v, heading angular speed p, roll angular speed r and roll angle phi of the horizontal plane are all approximately 0, the change of the longitudinal speed u is gentle and stable, u is approximately a constant, and only the motion of the autonomous underwater robot in the vertical x-z plane is considered, decoupling is carried out on the model, and a simplified vertical third-order depth control model is established as follows:
Wherein z is the vertical depth in the inertial coordinate system, θ is the pitch angle, q is the pitch angle speed, δ is the rudder angle, I y is the moment of inertia about the y-axis, M q、Mδ, For the additional mass matrix, W is gravity, B 1 is buoyancy, Δ q represents the model uncertainty, τ q represents the external time-varying disturbance caused by ocean currents and storms.
The unmodeled dynamic of the system is internal disturbance, various uncertainties caused by ocean currents, stormy waves and the like are external disturbance, and the internal disturbance delta q and the external disturbance tau q are regarded as bounded disturbance and are equivalent to the total disturbance delta w of the system.
The position information and the attitude information of the autonomous underwater robot are acquired by using the sensor, z, theta and q are used as state quantities, the rudder angle delta is used as control input, and x= [ z, theta and q ] T is used for describing the depth control problem of the autonomous underwater robot. Converting the model into a state space equation:
wherein the state transition matrix Input matrix
Discretizing a continuous time state space equation according to the sampling time delta T to obtain:
xk+1=Axk+Bδk+wk
Wherein the state is Control input
Because of the complexity and time-variability of the underwater environment, it is difficult to obtain an accurate value of the disturbance by a model or sensor, it can be assumed that the disturbance is bounded and the bounded set is a convex set, which can be expressed as: Where w max is the upper perturbation bound.
Step two: and establishing an autonomous underwater robot system nominal model and calculating nominal constraint.
Firstly, setting a state and control input constraint conditions according to the actual physical condition of a system and the saturation constraint of a 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 the autonomous underwater robot is not suitable to be excessively large. In order to ensure safety and stability during movement, certain pitching constraint conditions must be met so as to avoid phenomena such as rolling. Thus, the given state constraints include constraints on pitch angle and pitch angle speed, control input constraints refer to the range of rudder angle changes that are allowed to be input:
Wherein, the collection Are all tight convex sets containing the origin, x min、xmax、δmin、δmax is a known constant.
Nominal model refers to 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 moment 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. Since the system uncertainty is ignored, the nominal system has more compact constraints than the actual system, and the calculation of the nominal state constraints and the nominal control constraints are tightened on the basis of the actual constraints, expressed as:
where α and γ represent the contraction coefficients in the (0, 1) range.
Step three: the system disturbance is estimated using a disturbance observer. First, the disturbance is regarded as an expansion state quantity to be added into an uncertain system, and the following linear state observer is established:
wherein the state estimate Outputting the estimated valueL, H is a matrix of observer gains,For disturbance estimation, β is an auxiliary variable.
The state estimation error and disturbance estimation error are as follows:
And then deriving e x to obtain:
the first derivative of the disturbance estimate can be expressed as:
wherein the estimated value of the derivative of the disturbance
The state estimation error and the disturbance estimation error form an estimation error system as follows:
Wherein g= [ e x T ew]T, Representing the estimated error of the derivative of the disturbance.
By designing a proper observer gain matrix, the eigenvalues of the error system are all positioned on the left half complex plane, so that the estimation error thereof is converged to zero, and the stability of the disturbance observer is ensured. The obtained disturbance estimated value is fed back to the auxiliary predictive controller, so that the influence of disturbance on the controlled object is counteracted as much as possible.
Step four: a nominal model predictive controller is designed to calculate an optimal nominal control input in the prediction 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 disturbance, so that the error between the nominal state in a prediction time domain and a given reference state value is minimized, the cost function reflects the control requirements of the system on the states and the inputs of each prediction stage, and the cost function in the following form is adopted:
Where x ref is a target state vector, N p is a prediction time domain, and Q, R represents a prediction state and a positive weighting matrix of a control input, respectively. P is a terminal weight matrix, and is obtained by solving a Riccati equation.
Solving the optimal problem needs to satisfy nominal prediction model constraints, nominal state constraints and nominal control constraints. In the finite time domain, the optimal control problem at any time k is expressed as:
Wherein, The linear feedback gain K is obtained by a linear quadratic regulator.
The nominal states and nominal control inputs are constrained within a subset of the actual constraints, respectively:
the state constraint to be satisfied by the last prediction interval of the model in the prediction time domain:
Terminal constraints Satisfy the following requirementsAnd the like. Wherein, a K = a + BK,Is a robust invariant set.
The optimization problem is to predict the state as close as possible to the target state value and to input as small a rudder angle as possible. Solving the optimization problem to obtain a nominal optimal control sequenceCorresponding nominal state sequencesAnd is sent to the lower controller as a reference instruction for the auxiliary predictive controller.
Step five: and designing an auxiliary prediction controller, and designing a control strategy based on the actual prediction model.
Since autonomous underwater robotic systems may be affected by disturbances, future state trajectories may be different from nominal predicted trajectories. To counteract the effects of the disturbance, the auxiliary predictive controller is designed such that the system states and control inputs are close to the nominal sequence. Solving the following optimization problem:
Wherein the method comprises the steps of Estimated by a disturbance observer.
Final control signalIs obtained by solving two optimization problems in the fourth step and the fifth step, and the first control signal of the sequence is obtainedActs as rudder angle signal to the controlled object.
Step six: and (3) measuring the system state at the next moment, taking the moment as the new current moment, and returning to the step (III).
On the other hand, the embodiment provides a system, 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 executing the method, the data acquisition device comprises an attitude sensor and a depth sensor, the attitude sensor is used for acquiring underwater information, robot attitude and inertial navigation information and carrying out mean value filtering on the information, the depth sensor is used for measuring the height distance to the water surface, a system model meeting the state and control constraint is established under the condition of considering unknown disturbance caused by unmodeled dynamic and dynamic ocean currents, storms and the like, and a double-layer model prediction controller is designed by combining a disturbance observer. The upper layer designs a nominal prediction controller aiming at the nominal model, and obtains a nominal state and a nominal input so as to minimize errors between the nominal state and a reference value. The lower layer designs an auxiliary prediction controller aiming at the actual model, and solves the control input actually applied to 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 strictly keeps fluctuating in a small neighborhood of the target state 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 examples, and all technical solutions belonging to the concept of the present invention belong to the protection scope of the present invention. It should be noted that modifications and adaptations to the present invention may occur to one skilled in the art without departing from the principles of the present invention and are intended to be within the scope of the present invention.
Claims (6)
1. An autonomous underwater robot anti-interference motion control method is characterized in that: 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 the system;
Step 2, a system nominal model is established, and is used as a prediction equation of a nominal model prediction controller to convert the system state and control input constraint into a tightened state and control input constraint of the nominal system;
step 3, constructing a disturbance observer, taking disturbance variables as extended state quantities, estimating a system disturbance value, and sending the estimated value to an auxiliary prediction controller;
step 4, designing a nominal model predictive controller, constructing a cost function, solving a finite time domain optimization problem, and obtaining nominal optimal control input and corresponding nominal states;
Step 5, designing an auxiliary model prediction controller, receiving nominal prediction input and output signals and disturbance signals 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;
Step 6, measuring the system state at the next moment, taking the moment as the new current moment, and returning to the step 3; the specific steps of constructing the disturbance observer in the step 3 are as follows:
Step 31, regarding the disturbance as a newly added state quantity to the uncertain system, establishing the following linear state observer:
wherein the state estimate Outputting the estimated valueL, H is a matrix of observer gains,Beta is an auxiliary variable for disturbance estimation;
Step 32, calculating state estimation errors and disturbance errors:
And then deriving e x to obtain:
obtaining a first derivative of the disturbance estimate:
wherein the estimated value of the derivative of the disturbance Step 33, the state estimation error and the disturbance estimation error are combined into the following estimation error system:
Wherein g= [ e x T ew]T, An estimation error representing the derivative of the disturbance;
in the step 4, a nominal model predictive controller is designed, a cost function is constructed, a finite time domain optimization problem is solved, and the specific steps of obtaining nominal optimal control input and corresponding nominal states are as follows:
step 41, designing an optimal control input for each sampling time, driving the nominal system state to the target point and minimizing the cost function, wherein the constructed cost function is as follows:
Wherein x ref is a target state vector, N p is a prediction time domain, Q, R respectively represents a prediction state and a positive fixed weighting matrix of control input, and P is a terminal weighting matrix, and is obtained by solving a Riccati equation;
Step 42, satisfying the nominal prediction model constraints and nominal state constraints and nominal control constraints, the nominal state and nominal control being constrained within a subset of the actual constraints, respectively:
the state constraint to be satisfied by the last prediction interval in the prediction time domain:
Terminal constraints Satisfy the following requirementsThese conditions, where a K =a+bk,Is a robust invariant set;
Step 43, representing the optimal control problem at any time k in the finite time domain:
Wherein, The linear feedback gain K is obtained by a linear quadratic regulator.
2. The autonomous underwater robot immunity motion control method of claim 1, wherein: the tightened state and control input constraints of the nominal system in step 2 are specifically expressed as:
where α and γ represent the contraction coefficients in the (0, 1) range.
3. The autonomous underwater robot immunity motion control method of claim 2, wherein: in the step 5, the following optimization problem is solved by solving the optimal control mode aiming at the actual system model:
Wherein the method comprises the steps of Estimated by a disturbance observer.
4. The autonomous underwater robot immunity motion control method of claim 3, wherein: in the step 1, an autonomous underwater robot dynamics and kinematics simplified linear model is established, the autonomous underwater robot dynamics and kinematics simplified linear model is converted into a continuous time model under a state space, and discretization is carried out on the continuous time model to obtain a discrete time model of the system, wherein the specific steps 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, E- ζηζ represents the inertial coordinate system, and O-xyz represents the satellite coordinate system;
Step 12, defining six degrees of freedom speeds V= [ u V l p q r ] T and position and attitude angles eta= [ x y z phi ] T of the autonomous underwater robot in a satellite coordinate system, wherein the motion equation of the autonomous underwater robot 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 the coriolis force and centripetal force matrix, D (V) is the damping matrix, g (eta) is the restoring moment, tau is the propulsion system input matrix, J (eta) is the jacobian conversion matrix between the inertial coordinate system and the satellite coordinate system;
Step 13, assuming that the relevant state quantity transverse speed v, heading angular speed p, roll angular speed r and roll angle phi of the horizontal plane are all approximately 0, the change of the longitudinal speed u is gentle and stable, u is approximately a constant, and only the motion of the autonomous underwater robot in the vertical x-z plane is considered, decoupling is carried out on the model, and a simplified vertical third-order depth control model is established as follows:
Wherein z is the vertical depth in the inertial coordinate system, θ is the pitch angle, q is the pitch angle speed, δ is the rudder angle, I y is the moment of inertia about the y-axis, M q、Mδ, For the additional mass matrix, W is gravity, B 1 is buoyancy, delta q represents a model uncertainty part, and tau q represents external time-varying disturbance caused by ocean currents and storms;
Step 14, acquiring 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 converting the model into a state space equation by using x= [ z, theta and q ] T for describing the depth control problem of the autonomous underwater robot:
wherein the state transition matrix Input matrixDiscretizing a continuous time state space equation according to the sampling time delta T to obtain:
xk+1=Axk+Bδk+wk
Wherein the state is Control input
5. The autonomous underwater robot immunity motion control method of claim 4, wherein: the disturbance is assumed to be bounded in step 14, and the bounded set is a convex set, expressed as: Where w max is the upper perturbation bound.
6. A system applying the autonomous underwater robot disturbance rejection motion control method as claimed in any of claims 1 to 5, characterized in that: the device 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, controlling the execution mechanism and the driving mechanism after executing the method, the data acquisition device comprises an attitude sensor and a depth sensor, the attitude sensor is used for acquiring underwater information, robot attitude and inertial navigation information, 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 CN114967714B (en) | 2022-06-24 | 2022-06-24 | Autonomous underwater robot anti-interference motion control method and system |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210728589.1A CN114967714B (en) | 2022-06-24 | 2022-06-24 | Autonomous underwater robot anti-interference motion control method and system |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114967714A CN114967714A (en) | 2022-08-30 |
CN114967714B true CN114967714B (en) | 2024-11-01 |
Family
ID=82964877
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210728589.1A Active CN114967714B (en) | 2022-06-24 | 2022-06-24 | Autonomous underwater robot anti-interference motion control method and system |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114967714B (en) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115303455B (en) * | 2022-09-16 | 2023-04-25 | 北京大学 | Motion control method, device and equipment for underwater bionic robot and storage medium |
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 |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP6189119B2 (en) * | 2013-07-12 | 2017-08-30 | 三菱重工業株式会社 | Underwater vehicle, its control device and control method |
US10935984B2 (en) * | 2018-07-17 | 2021-03-02 | Ge Aviation Systems Llc | Method and system for determining a climb profile |
CN113485390B (en) * | 2021-08-26 | 2022-05-03 | 湖南大学 | Autonomous underwater robot dynamic positioning method and system capable of resisting ocean current disturbance |
-
2022
- 2022-06-24 CN CN202210728589.1A patent/CN114967714B/en active Active
Non-Patent Citations (1)
Title |
---|
多关节水下机器人方案设计与关键技术应用;韩涛;控制与信息技术;20231215;全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN114967714A (en) | 2022-08-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN114967714B (en) | Autonomous underwater robot anti-interference motion control method and system | |
Zhu et al. | Global robust adaptive trajectory tracking control for surface ships under input saturation | |
Wu et al. | Depth control of model-free AUVs via reinforcement learning | |
CN114047773B (en) | Underwater ore collection robot backstepping sliding mode self-adaptive attitude control method based on extended state observer | |
Li et al. | Robust adaptive motion control for underwater remotely operated vehicles with velocity constraints | |
Huang et al. | Robust practical fixed-time leader–follower formation control for underactuated autonomous surface vessels using event-triggered mechanism | |
CN107024863B (en) | UUV trajectory tracking control method for avoiding differential explosion | |
CN105807789B (en) | UUV control methods based on the compensation of T-S Fuzzy Observers | |
Guerrero et al. | Observation-based nonlinear proportional–derivative control for robust trajectory tracking for autonomous underwater vehicles | |
CN112068440B (en) | AUV recovery butt joint power positioning control method based on model prediction control | |
Sebastián et al. | Adaptive fuzzy sliding mode controller for the kinematic variables of an underwater vehicle | |
Avila et al. | Modeling and identification of an open-frame underwater vehicle: The yaw motion dynamics | |
Zhang et al. | Anti-disturbance control for dynamic positioning system of ships with disturbances | |
CN114115262B (en) | Multi-AUV actuator saturation cooperative formation control system and method based on azimuth information | |
Qiao et al. | Double-loop chattering-free adaptive integral sliding mode control for underwater vehicles | |
Gao et al. | Command filtered path tracking control of saturated ASVs based on time‐varying disturbance observer | |
Wang et al. | Command filter-based adaptive practical prescribed-time asymptotic tracking control of autonomous underwater vehicles with limited communication angles | |
Kumar et al. | Discrete time-delay control of an autonomous underwater vehicle: Theory and experimental results | |
Jagannathan et al. | One-layer neural-network controller with preprocessed inputs for autonomous underwater vehicles | |
Du et al. | Research on UUV recovery active disturbance rejection control based on LMNN compensation | |
CN112925332B (en) | Cooperative intersection butt joint control method for unmanned ship and underwater unmanned submersible vehicle combined system | |
CN114035566A (en) | Design method, system and device of finite-time anti-saturation controller of unmanned ship | |
Wang et al. | Adaptive cooperative diving of saucer-type underwater gliders subject to model uncertainties and input constraints | |
Gao et al. | Disturbance observer based finite-time trajectory tracking control of unmanned surface vehicles with unknown dead-zones | |
CN117850424A (en) | Multi-USV event triggering error constraint control method considering input saturation |
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 | ||
GR01 | Patent grant |