Underwater mechanical arm-movable base system control method based on infinite time domain MPC, program, equipment and storage medium

Abstract

The invention belongs to the technical field of underwater robot motion control, and particularly relates to an underwater mechanical arm-movable base system control method based on an infinite time domain MPC, a program, equipment and a storage medium. The method comprises the following steps: establishing a motion model of an underwater mechanical arm-movable base system; based on a motion model of an underwater mechanical arm-movable base system, considering the influence of hydrodynamic force, constructing an underwater dynamic model, meanwhile, quantitatively analyzing a coupling effect generated by structural connection and interaction between the mechanical arm and the movable base, and identifying coupling parameters; based on an underwater dynamic model, a system state space equation is constructed, a system state in a future time domain is predicted by adopting an infinite time domain model prediction control method, a control strategy is updated in real time, and a closed-loop control system is formed by combining state feedback optimization control input. The problem that the control precision is insufficient due to motion coupling of an underwater mechanical arm-movable base system in a complex underwater environment is solved, and meanwhile the trajectory tracking capacity in the dynamic environment is improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of underwater robot motion control, and particularly relates to an underwater manipulator-movable base system control method, program, device and storage medium based on infinite time domain MPC. BACKGROUND

[0002] With the deepening of the development of marine resources, the demand for underwater operation equipment is increasing, and underwater manipulators, as key operation tools, play an important role in the fields of marine resource exploration, seabed engineering operation, deep sea scientific research, etc. Compared with humans, underwater robots have more advantages in deep sea and complex environments, can efficiently perform information collection, data processing and other tasks, and are widely used in marine energy development.

[0003] Underwater manipulator-movable base system (UVMS) is widely used in underwater operations in complex environments due to its high degree of freedom and strong operation ability. Due to the fact that the density of water is much higher than that of air in the marine environment, underwater manipulators will be significantly affected by fluid dynamics during movement, including viscous drag, added mass effect, and external disturbances such as ocean currents and waves. These factors will cause trajectory deviation of the manipulator, affecting operation accuracy. In addition, underwater manipulators are usually carried on underwater vehicles such as autonomous underwater vehicles (AUV) or remotely operated underwater vehicles (ROV), forming a coupled system with the movable base, and the movement of the manipulator will affect the attitude and position of the movable base, and in turn, the movement of the movable base will interfere with the trajectory tracking of the manipulator. This two-way coupling effect increases the difficulty of controlling the system, making it difficult for traditional independent control strategies to meet the requirements of high-precision operations. In recent years, researchers have conducted extensive research on the control method of underwater manipulator-movable base system, mainly including PID control, adaptive control, sliding mode control and model predictive control (MPC), etc. However, the traditional MPC method fails to fully consider the water dynamic disturbance, manipulator-movable base coupling effect and uncertainty of system parameters, which limits the control accuracy in practical applications.

[0004] In view of the above problems, the application provides a coupling motion control method for an underwater manipulator-mobile base system based on infinite horizon MPC (IH-MPC). SUMMARY

[0005] The application aims to solve the problem of insufficient control accuracy of the underwater manipulator-mobile base system caused by coupling motion in a complex underwater environment, and improve the trajectory tracking capability of the system in a dynamic environment.

[0006] A control method for an underwater manipulator-mobile base system based on infinite horizon MPC, comprising the following steps:

[0007] Step 1: establish a motion model of the underwater manipulator-mobile base system, and based on the motion model of the underwater manipulator-mobile base system, consider the influence of hydrodynamic force, build an underwater dynamics model, and quantitatively analyze the coupling effect between the manipulator and the mobile base caused by structural connection and interaction, and identify the coupling parameters.

[0008] Step 2: based on the underwater dynamics model, build a system state space equation, predict the system state in the future time domain by using the infinite horizon model predictive control method, update the control strategy in real time, and combine the state feedback to optimize the control input, forming a closed-loop control system.

[0009] Further, the step 1 is specifically: the Newton-Euler method is used to model the dynamics of the manipulator-mobile base system, and the influence of hydrodynamic force is considered, the Morrison equation is used to analyze the hydrodynamic performance of the underwater manipulator in the static water environment, the water viscosity and additional mass effect are solved, the additional mass force, additional mass torque, viscous resistance and viscous torque are calculated, the hydrodynamic model of the underwater manipulator in the static water environment is built, and the hydrodynamic model is introduced into the dynamics model of the underwater manipulator-mobile base system:

[0010]

[0011] wherein, θ, respectively represent the joint angle, joint velocity and joint acceleration of the manipulator, θ = [θ1 θ2… θ n ], θ1, θ2… θ n respectively represent the 1st, 2nd…nth joint angle of the underwater manipulator, respectively represent the 1st, 2nd…nth joint angle velocity of the underwater manipulator, respectively represent the 1st, 2nd…nth joint angle acceleration of the underwater manipulator, M(θ) is the 3x3 order mass matrix of the manipulator, containing the joint velocity related term, τ = [τ1, τ2, f x ] T are the joint 1 driving torque (N·m), joint 2 driving torque (N·m) and base x direction driving force (N) in turn, which contains the water dynamic related part.

[0012] Further, the step 2 is specifically:

[0013] Step 2.1: transforming the underwater manipulator-moving base model from a strong nonlinear model into a linearized model;

[0014] Step 2.2: constructing the corresponding state space equation based on the linearized model;

[0015] Step 2.3: using the infinite time domain MPC method to control design the linearized model, setting the performance index as J;

[0016] Step 2.4: in each control cycle, using the MPC algorithm to predict the model state in the future time domain, calculating the optimal control sequence u * (k), and constantly updating the prediction model according to the real-time measured model state, so as to adapt to the environmental changes and model dynamic characteristics;

[0017] Step 2.5: applying the optimized control input to the underwater manipulator-moving base model to form a closed-loop control system, and through continuous state feedback and MPC optimization, ensuring the control and stability of the system in complex underwater environment.

[0018] Further, the step 2.1 transforms the underwater manipulator-moving base model from a strong nonlinear model into a linearized model specifically:

[0019]

[0020] wherein, define then q = [θ1 θ2 x] T , control vector u = [τ1 τ2 f x T ​, α(δx, δu) is a higher-order term with respect to δx and δu; These are the partial derivatives of vector f(x,u) with respect to vectors x and u, respectively.

[0021] Furthermore, step 2.3 employs the infinite time-domain MPC method to design the control of the linearized model, setting the performance index as J, specifically as follows:

[0022]

[0023] Where: J represents the objective cost function of model predictive control, e(k) = x(k) - x d (k) represents the system error, and x(k) represents the actual output state of the system. d (k) represents the desired reference state of the system, u(k) represents the control input sequence, Δu(k) represents the control input increment sequence, Q represents the weight matrix of the system prediction error, R represents the weight matrix of the prediction control input, and F represents the system error terminal cost matrix. Both Q and R are set as diagonal matrices.

[0024] Furthermore, in step 2.4, within each control cycle, the MPC algorithm is used to predict the model state in the future time domain, and the optimal control sequence u is calculated. * (k), and continuously update the prediction model based on the real-time measured model state to adapt to environmental changes and model dynamic characteristics, specifically:

[0025] Step 2.4.1: Transform the cost function J into a standard quadratic programming form;

[0026] Step 2.4.2: Consider the input constraints of the robotic arm system;

[0027]

[0028]

[0029] Where Ω is the system's terminal domain, calculated from the input constraints. Once the system enters the terminal domain, its trajectory will remain within it. k Let u(k|k) represent the optimal control sequence obtained in the prediction time domain at the current time k, u(k|k) represent the system control input acting on the system's current state x(k) in the prediction time domain at the current time k, u(k+1|k) represent the system control input predicting the system state variable x(k+1) at time k+1 in the prediction time domain at the current time k, and u(k+N-1|k) represent the system control input predicting the state variable x(k+N-1) at time k+N-1 in the prediction time domain at the current time k. and respectively, are state weight matrices corresponding to the optimal cost function, Γ = [I, 0,..., 0] T the optimal control input sequence calculated the first control input is applied to the underwater manipulator-mooring system, so that the system performs the corresponding action and adjusts its motion state.

[0030] A computer device / apparatus / system, comprising a memory, a processor and a computer program stored on the memory, wherein the processor executes the computer program to implement the steps of the underwater manipulator-mooring system control method based on infinite-time domain MPC.

[0031] A computer readable storage medium, having a computer program / instruction stored thereon, wherein the computer program / instruction is executed by a processor to implement the steps of the underwater manipulator-mooring system control method based on infinite-time domain MPC.

[0032] A computer program product, comprising a computer program / instruction, wherein the computer program / instruction is executed by a processor to implement the steps of the underwater manipulator-mooring system control method based on infinite-time domain MPC.

[0033] The beneficial effects of the present application are:

[0034] 1. Motion coupling compensation, by establishing a complete dynamic model and combining hydrodynamic effects, the motion coupling compensation strategy of the manipulator and the mooring is optimized, and the adaptability of the system to environmental disturbances is improved.

[0035] 2. Precise trajectory tracking control, an infinite-time domain MPC optimization control strategy is adopted to realize high-precision trajectory tracking of the manipulator end effector, and the stability of the system is guaranteed through Lyapunov stability theory.

[0036] 3. Adaptive control strategy, combined with embedded control architecture, the decomposition of control tasks is realized, so that the system can adapt to different underwater working environments, and the real-time performance and calculation efficiency of the controller are improved.

[0037] 4. System robustness is improved, by modeling and analyzing environmental uncertainties such as ocean currents and waves, the prediction and optimization method of infinite-time domain MPC is introduced, and the adaptability and robustness of the system in complex underwater environment are improved. BRIEF DESCRIPTION OF DRAWINGS

[0038] Figure 1 is a schematic diagram of the underwater manipulator-mooring coupling motion system.

[0039] Figure 2Numerical simulation diagram of water dynamics of the underwater manipulator under different joint angles.

[0040] Figure 3 Analysis diagram of the underwater manipulator-movable base model without coupling motion influence.

[0041] Figure 4 Analysis diagram of the underwater manipulator-movable base model with coupling motion influence.

[0042] Figure 5 Analysis of joint and base motion following effect under a sine signal.

[0043] Figure 6 Trajectory tracking of the underwater manipulator motion model DETAILED DESCRIPTION

[0044] Embodiment one: the specific process of the underwater manipulator-movable base system control method based on the infinite time domain MPC is as follows:

[0045] The purpose of the present application is to design an underwater manipulator-movable base system control method based on the infinite time domain MPC, to solve the problem of the influence of the motion coupling effect between the underwater manipulator and the underwater vehicle on the system accuracy. In the presence of complex ocean environment disturbance and potential problems of abnormal state of the manipulator or movable base, the control strategy of the underwater manipulator-movable base system is optimized, and the stability and control accuracy of the system are improved.

[0046] The purpose of the present application is achieved by the following technical solutions:

[0047] Step 1: based on the kinematics and dynamics characteristics of the UVMS, the kinematics model of the underwater manipulator-movable base system is established by using the geometric analysis method, the system dynamics model is constructed according to the Newton-Euler formula, and the water dynamics model is formed by considering the influence of water dynamics combined with the Morison equation.

[0048] Step 2: quantitatively analyze the coupling effect between the manipulator and the movable base due to the structural connection and interaction, and identify the key coupling parameters. Considering the nonlinear influence introduced by external disturbances such as ocean current and wave, data support and theoretical basis are provided for subsequent control design.

[0049] Step 3: construct the system state space equation, and use the infinite time domain model predictive control (MPC) method to predict the system state in the future time domain in view of the possible motion instability under the ocean current and uncertain environment. Combined with the real-time measured system state, the prediction model and control input are constantly updated to form a closed-loop control system.

[0050] Specific embodiment two: the embodiment is different from specific embodiment one, and the underwater mechanical arm-vehicle system is simplified in step 1, the vehicle part is simplified into a slide rail moving base affected by the mechanical arm movement according to the system movement form (as shown in the accompanying drawings), the kinematics and dynamics models of the underwater mechanical arm-moving base are respectively established by using the geometric analysis method and the Newton-Euler method, the mapping relationship between the end effector space and the mechanical arm joint angle space and the relationship between the joint driving force and the system movement are obtained, and the specific process is as follows: Figure 1 First, the movement position relationship between the joints of the mechanical arm is derived, and the spatial description of the mechanical arm end in the base coordinate system during the system movement process is derived according to the position relationship between the joints.

[0051] First, the movement position relationship between the joints of the mechanical arm is derived, and the spatial description of the mechanical arm end in the base coordinate system during the system movement process is derived according to the position relationship between the joints.

[0052] In order to facilitate kinematics description, the two-link mechanical arm model structure is simplified, the mechanical arm upper arm end is regarded as the mechanical arm executor position, the mechanical arm base is regarded as the coordinate system origin, the mechanical arm lower arm link length is represented as l1, and the joint angle is represented as q1 (joint control axis 1 angle position) ; the upper arm link length is represented as l2, and the joint angle is represented as q2 (joint control axis 2 angle position). The mechanical arm one-axis two-axis joint rotation speed (rad / s) is represented as x f , y f The end displacement (m) is represented as x

[0053]

[0054] Similarly, the inverse kinematics model expression of the two-degree-of-freedom mechanical arm is:

[0055]

[0056] The two-link mechanical arm of the application sets the link as l1=l2, so that when the end effector reaches the (0,0) point position, the joint one angle can be any value, that is, the joint two rotates 180°, the mechanical arm end effector and the base origin are in the same position, in order to avoid the infinite solution caused by this case, the subsequent end trajectory planning will avoid passing through the coordinate origin position.

[0057] Further considering the hydrodynamic part, the hydrodynamic performance of the underwater manipulator in the static water environment is analyzed using the empirical formula Morrison equation, the water viscosity and the added mass effect are solved, the added mass force, the added mass torque, the viscous resistance, the viscous torque are calculated, the hydrodynamic model of the underwater manipulator in the static water environment is built, and it is introduced into the dynamics model of the underwater manipulator-moving base system. The vector form of the Morrison formula can be written as follows:

[0058]

[0059] The water resistance, water resistance torque and added mass force and added mass torque of the moving object in the static water can be calculated as follows:

[0060]

[0061] Wherein, F d represents the water resistance acting on the structure length, T d represents the water resistance torque acting on the structure length, F m represents the added mass force, T m represents the added mass torque, C m represents the added mass force coefficient, C d represents the water resistance coefficient, ρ represents the water density, D represents the diameter of the rod, A represents the projection area of the cylindrical rod in the flow direction of the fluid, dl represents the length direction differential of the rod.

[0062]

[0063]

[0064] Wherein, the two formulas are the solving process of the joint torque required to be provided during the underwater movement of the two-link manipulator, τ d1 and τ d2 are the water resistance torques of the two joints, τ m1 and τ m2 are the added mass torques of the two joints, θ1, θ2 are the angles of joint one and joint two respectively, are the angular velocities of joint one and joint two respectively, l1, l2 are the lengths of the first rod and the second rod respectively.

[0065] Finally, the Newton-Euler method is used to model the dynamics of the manipulator-moving base model, and the complete dynamics model is obtained based on the above.

[0066]

[0067] Wherein θ、 represent the joint angle, joint speed and joint acceleration of the manipulator respectively, which are functions of time t; M(θ) is a 3x3 order mass matrix of the manipulator, including joint speed related terms, τ = [τ1, τ2, f x ] T In turn, joint 1 drive torque (N.m), joint 2 drive torque (N.m), base x direction drive force (N).

[0068] Other steps and parameters are the same as in the first embodiment.

[0069] Embodiment three: different from one of the first two embodiments, the step 2 can be seen from the derivation of the underwater manipulator-moving base dynamics model that there is a certain motion coupling relationship between the manipulator motion and the base movement, and the coupling effect between the manipulator and the moving base due to structural connection and interaction is analyzed; the specific process is:

[0070] Without considering the influence of the displacement change of the base caused by the movement of the manipulator, the error of the underwater manipulator-moving base system in tracking the group of trajectories is observed. The joint angle planning trajectory is set to: (A cos(0.5π·t)-A) rad, and the system joint and base motion trajectory comparison analysis is carried out under different swing amplitude motion. In order to meet the actual operation requirements, according to the mass ratio of the underwater manipulator to the underwater vehicle, the mass ratio of the manipulator to the moving base is set to 1:10 in this simulation, and the wave amplitude A is set to 0.1 rad, 0.2 rad, 0.3 rad, 0.4 rad, 0.5 rad, as shown in the accompanying Figure 3 .

[0071] On the basis of the above model, by including the base part into the modeling range, fully considering the influence of the base movement on the whole system, the control accuracy of the manipulator working end trajectory tracking is observed again. The given two joints have the same motion reference trajectory as the above test: (A cos(0.5π·t)-A) rad, and the wave amplitude A is set to 0.1 rad, 0.2 rad, 0.3 rad, 0.4 rad, 0.5 rad, and the base expected position is the initial origin. The motion deviation of the manipulator joint and the base in tracking the group of trajectories is observed. Both use infinite time domain MPC as the system control algorithm, and the system joint and base motion under the condition of considering only the underwater manipulator model and considering the underwater manipulator-moving base coupling motion model are set. As shown in the accompanying Figure 4 .

[0072] Other steps and parameters are the same as in the first or second embodiment.

[0073] Specific implementation four: this embodiment is different from one of the specific implementation one to three, the step 3 constructs system state space equation, for the motion instability that may appear in the sea current and uncertain environment, the infinite time domain model predictive control (MPC) method is used to predict the system state in the future time domain.Combined with the real-time measured system state, the prediction model and the control input are updated constantly, forming a closed-loop control system; The specific process is as follows:

[0074] Step 3.1: underwater manipulator-movable base model is a typical strong nonlinear model, which is linearized into a linear system.

[0075] Step 3.2: based on the linearized model, the corresponding state space equation is constructed.

[0076] Step 3.3: the infinite time domain MPC method is used to control the linearized system, and the performance index J is set.

[0077] Step 3.4: in each control cycle, the MPC algorithm is used to predict the system state in the future time domain, and the optimal control sequence u * (k) is calculated, and the prediction model is updated according to the real-time measured system state to adapt to the environmental change and system dynamic characteristics.

[0078] Step 3.5: the optimized control input is applied to the underwater manipulator-movable base system, forming a closed-loop control system. Through continuous state feedback and MPC optimization, the system ensures high-precision control and stability in complex underwater environment.

[0079] Other steps and parameters are the same as one of the specific implementation one to three.

[0080] Specific implementation five: this embodiment is different from one of the specific implementation one to four, the step 3.1 underwater manipulator-movable base model is a typical strong nonlinear model, which is linearized into a linear system; The specific process is as follows:

[0081] The system dynamic variable is defined as q=[θ1 θ2 x] T , The control vector is u=[τ1 τ2 f x ] T , the state equation of the manipulator system is obtained as follows:

[0082]

[0083] Where, θ, x, are the angle vector of the manipulator joint, the angular velocity vector, the base displacement and the base moving speed, respectively.

[0084] Let x0, u0be a set of solutions satisfying the above equation, i.e.,

[0085]

[0086] When the input u deviates from x0by δu, the corresponding x deviates from x0by δx. Accordingly, the system is linearized once, and Taylor expansion of f is made near x0and u0:

[0087]

[0088] where α(δx, δu) is a high-order term with respect to δx and δu; are the partial derivatives of the vector f(x, u) with respect to the vectors x and u, respectively, and are 6x6 and 6x3 matrices, respectively.

[0089] The other steps and parameters are the same as one of the first to fourth embodiments.

[0090] The sixth embodiment is different from the first to fifth embodiments in that the step 32 constructs a corresponding state space equation based on a linearized model. The specific process is as follows:

[0091] Neglecting the high-order term α(δx, δu) and expressing the micro-increments δx and δu near the equilibrium point as x-x0and u-u0respectively in the equation, we have Simplify H:

[0092]

[0093] The linear dynamic model under continuous time is obtained as follows:

[0094]

[0095] The linearized state space equation under discrete time can be expressed as follows by rearranging the above equation:

[0096] x(k+1) = A c x(k) + B c u(k) + h(k) (13)

[0097] where T is a sampling period, A c = e AT , B c = A -1 (e AT -I)B, h(k) = A -1 (e AT ​I) H(k), the linearized discrete underwater manipulator-base state space equation is derived by analyzing the system control loop design and modeling of the control object, thus providing a model theoretical basis for system controller design.

[0098] Other steps and parameters are the same as one of the first to fifth embodiments.

[0099] The seventh embodiment is different from one of the first to sixth embodiments in that the step 33 uses an infinite time domain MPC method to control the linearized system, and sets a performance index J. The specific process is as follows:

[0100] Considering the influence of system input change on system state change, the state space expression is changed to an incremental form:

[0101]

[0102] Subtracting the two equations gives:

[0103] △x(k+1)=A c △x(k)+B c △u(k)+△h(k) (15)

[0104] Where △x(k) = x(k) - x(k-1), △u(k) = u(k) - u(k-1), △h(k) = h(k) - h(k-1).

[0105] According to the incremental form state space expression obtained from equation (3-4), the system motion state in the future N time is predicted to obtain the following form:

[0106]

[0107] In order to unify the cost function with the quadratic programming, it is necessary to convert the system state increment Δx into a function of the system input increment Δu:

[0108]

[0109] According to equation (17), the system state to the future N time is only related to the current system state, the current system increment and the future system input increment. The equation is arranged, and the predicted output sequence at time k and the input sequence at time k are defined as:

[0110]

[0111] The incremental system prediction equation is obtained:

[0112] X k+1 =Ix(k)+A△x(k)+B△U k+H△h(k) (19)

[0113] Expanding the above equation, we have

[0114]

[0115] Thus, the system prediction equation is finally obtained, i.e. in the process of state prediction, the actual system state at the future time is not needed, and the future state of the system can be determined by the current state of the system, the current state increment and the system input variation.

[0116] The system output is defined as x, the system reference is x d , and the system error is e = x - x d . The state sequence at time k is defined as X k , the control input increment sequence is ΔU k , the reference state sequence is X dk , and the system error sequence is E k . The goal of predictive control is to make the system output more close to the reference state sequence X d at the future time. Under the action of the control input ΔU k , the error value between the reference state sequence and the predicted output state sequence is minimized, i.e. the cost function in the following form can be defined to describe it:

[0117]

[0118] In equation (21), Q is the weight matrix of the system prediction error; R is the weight matrix of the predictive control input; F is the terminal cost matrix of the system error, and Q and R are both diagonal matrices.

[0119] The other steps and parameters are the same as one of the first to sixth embodiments.

[0120] The eighth embodiment is different from one of the first to seventh embodiments in that the step 34 uses the MPC algorithm to predict the system state in the future time domain in each control period, calculates the optimal control sequence u * (k), and constantly updates the prediction model according to the real-time measured system state to adapt to the environmental changes and system dynamic characteristics; the specific process is as follows:

[0121] Step 341: convert the cost function J into a standard quadratic programming form;

[0122] At time k, predict backward for N steps, the control input is U K , the error column vector is E K , and it satisfies

[0123]

[0124] The above equation can be simplified as

[0125] E k = Ix(k) + A△x(k) + B△U(k) + H△h(k) - X d (23)

[0126] The tracking controller is designed with the above equation as the prediction model. In the tracking controller, we set the following performance index, and simplify the cost function (21) as follows:

[0127]

[0128] Substituting the above equation into the cost function (26) gives

[0129]

[0130] Expanding the above equation gives

[0131]

[0132] In the cost function (26), the third term is a real number and is irrelevant to the quadratic programming and can be ignored. The first and second terms are in the general form of the quadratic programming. By solving the quadratic programming, the optimal control variable U k is obtained. The first control variable is selected as the input of the system, and the solution is obtained.

[0133] Step 342: Consider the input constraint of the robotic arm system;

[0134] Suppose the input constraint condition is u min ≤ u ≤ u max Since the model predictive control predicts N steps backward, in order to meet the constraint condition, the constraint condition can be converted into U min ≤ U k ≤ U max

[0135] The control objective is to make the predicted state variable of the robotic arm follow the expected trajectory set above, based on which the optimal control sequence is obtained, and the first position of the sequence is applied to the control system. In order to achieve the control objective, the optimal control problem is converted into a quadratic programming problem to obtain the optimal control sequence:

[0136]

[0137] In the above equation, Ω is the terminal domain of the system, which is calculated from the input constraint condition. Once the system enters the terminal domain, its trajectory will remain in the terminal domain.

[0138] The optimal solution of equation (27) is denoted as the optimal control law. Since only the first element of the predicted control sequence is selected, the optimal control law can be derived as

[0139]

[0140] where Γ = [I, 0, …, 0] T .

[0141] The other steps and parameters are the same as one of the first to seventh embodiments.

[0142] The ninth embodiment is different from the first to eighth embodiments in that the step 35 applies the optimized control input to the underwater manipulator-movable base system to form a closed-loop control system. Through continuous state feedback and MPC optimization, the system ensures high-precision control and stability in complex underwater environments; the specific process is as follows:

[0143] Control input application, the optimal control input sequence calculated is:

[0144]

[0145] where the first control input is applied to the underwater manipulator-movable base system to make the system perform corresponding actions and adjust its motion state.

[0146] State feedback acquisition, the state data of the underwater manipulator and movable base are collected in real time through sensors (IMU, DVL, force sensor, etc.):

[0147]

[0148] State error calculation, the error between the current system state and the desired state is calculated:

[0149] e(k) = x(k) - x ref (k)

[0150] MPC optimization update, in each control period, based on the latest state feedback, the infinite horizon MPC (IH-MPC) is used to calculate the new control input:

[0151]

[0152] Constraint conditions:

[0153] x(i+1) = Ax(i) + Bu(i), u min ≤ u(i) ≤ u max

[0154] A closed loop iterative control, which iterates continuously, performs the following steps in each control cycle: calculates optimal control input and applies it to the system; collects new state feedback x(k+1); updates error e(k+1) and re-optimizes control input. Through the above closed loop control system, the method can effectively improve the trajectory tracking accuracy and motion stability of the underwater manipulator-movable base system, even under complex marine environments (such as sea current disturbance, carrier motion uncertainty, etc.) to ensure high-precision control.

[0155] The other steps and parameters are the same as one of the first to eighth embodiments.

[0156] The following examples are used to verify the beneficial effects of the present application:

[0157] Example 1:

[0158] The present application is further described in detail below in conjunction with the accompanying drawings and specific embodiments.

[0159] The present application aims to solve the problem of the influence of the motion coupling effect between the underwater manipulator and the underwater vehicle on the system accuracy. The input-output constraints of the actual joint angle of the underwater manipulator during operation are considered, the motion control accuracy of the underwater manipulator-underwater vehicle system is improved, and the adaptability in complex underwater environments is enhanced. At the same time, a method based on infinite time model predictive control (MPC) is proposed to optimize the control strategy and reduce the coupling effect. The purpose of the present application is achieved by the following steps:

[0160] Step 1: Based on the kinematics and dynamics characteristics of UVMS, a geometric analysis method is used to establish the kinematics model of the underwater manipulator-movable base system, the system dynamics model is constructed according to the Newton-Euler formula, and the water dynamic model is formed by considering the influence of water dynamics combined with the Morrison equation.

[0161] Step 2: Quantitative analysis of the coupling effect between the manipulator and the movable base due to structural connection and interaction, identification of key coupling parameters. Considering the nonlinear influence of external disturbances such as sea currents and waves, data support and theoretical basis are provided for subsequent control design.

[0162] Step 3: Construct the system state space equation, use the infinite time domain model predictive control (MPC) method to predict the system state in the future time domain in view of the possible motion instability under sea current and uncertain environment. Combine the real-time measured system state to continuously update the prediction model and control input, forming a closed loop control system.

[0163] In order to verify the effectiveness of the present application, the simulation software is used to simulate and verify the coupling motion control method of the underwater manipulator-movable base system.

[0164] The accuracy of the water dynamic model is verified, and the model is brought into the simulation test of the mechanical arm model, because the water resistance term is infinitely small relative to the inertia term, the model only considers the inertia term part, and the simulation software is used to verify and analyze the derivation results:

[0165] The two joints of the mechanical arm are set as (A cos(0.5π·t)-A) rad as the joint swing trajectory, and the water dynamic values calculated when the joint swing A=0.3, 0.4, 0.5, 0.6, 0.7 are gradually increased, and the actual stress is obtained by using a numerical simulation method. Figure 2 It can be seen that during the simulation process, the water dynamic part load of the first joint and the second joint of the mechanical arm is periodically fluctuated with the periodic swing of the joint, and the water dynamic load gradually increases with the increase of the swing of the mechanical arm from 0.3 rad to 0.7 rad, the water dynamic result obtained by simulation is consistent with the actual calculation value, and it can be proved that the modeling of the water dynamic part of the mechanical arm is effective.

[0166] Example two:

[0167] According to the periodic development of the mechanical arm-moving base system following effect analysis, the initial angle of the mechanical arm is 0 rad, the sine signal is offset, the signal trajectory also starts from 0 rad, the reference trajectories of the two joints are set as (cos(ω·π·t)-1) rad, the swing of the first joint and the second joint is fixed as 1 rad, and the swing period of the mechanical arm is adjusted to 5s, 4s, 3s, 2s by continuously increasing the fluctuation frequency ω of the sine signal.

[0168] The maximum error of the first joint increases from 0.005 rad to 0.013 rad, and the maximum error of the second joint increases from 0.002 rad to 0.031 rad.

[0169] The maximum error of the system joint under PID control changes: the maximum error of the first joint increases from 0.035 rad to 0.118 rad, and the maximum error of the second joint increases from 0.016 rad to 0.034 rad.

[0170] At the same time, the base under the infinite time domain MPC control does not appear obvious displacement, but the base under the PID control starts to move periodically, and the maximum offset increases from 0.010 m to 0.036 m. Figure 5 Compared with the underwater mechanical arm-moving base system using the infinite time domain MPC controller, the following effect of the joint and the stabilization effect of the base during the joint sine signal following process are better than those of the PID control.

[0171] Example three:

[0172] Under the premise of studying joint following effect, the end position trajectory tracking of the mechanical arm is studied, and a design is made as follows Figure 6 The planning trajectory graph is used as the end tracking trajectory of the mechanical arm. The trajectory is composed of a straight line and a circle. Since the lengths of the two arms of the mechanical arm are both 1 m, in order to make the end of the mechanical arm follow the trajectory to obtain a complete circle, the initial position (2, 0) of the end of the mechanical arm in the extended state is set as the starting position of the planning trajectory, passes through (1, 1), rotates 360° around the center (0.5, 1) and finally returns to the starting point (1, 1) of the circle.

[0173] In the process of tracking the circular reference trajectory, the following Figure 6 For the position tracking effect of the planar trajectory, it can be seen that compared with the PID controller, the infinite time domain MPC controller can still make the end of the mechanical arm better track the reference trajectory stably, and from the following Figure 6 It can also be seen from the joint tracking error graph that the joint error under the MPC control is smaller than that under the PID control, especially at the turning angle of the trajectory. In the following Figure 6 In the base position offset effect graph, the maximum offset under the MPC control is 0.001 m, and the maximum offset under the PID control is 0.08 m, so the MPC control has better control effect.

[0174] The above only describes the preferred embodiments of the present application and is not used to limit the present application. For those skilled in the art, the present application can have various modifications and changes. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A control method for an underwater manipulator-mooring system based on infinite horizon MPC, characterized in that: The specific steps are as follows: Step 1: Establish the motion model of the underwater manipulator-movable base system, based on the motion model of the underwater manipulator-movable base system, consider the influence of hydrodynamic force, build the underwater dynamics model, and quantitatively analyze the coupling effect between the manipulator and the movable base due to structural connection and interaction, and identify the coupling parameters; Step 2: Based on the underwater dynamics model, construct the system state space equation, predict the system state in the future time domain using the infinite time domain model predictive control method, update the control strategy in real time, and combine the state feedback to optimize the input, forming a closed-loop control system.

2. The infinite horizon MPC based manipulator-mooring system control method of claim 1, wherein: The step 1 is specifically: the Newton-Euler method is used for dynamic modeling of the manipulator-movable base system, considering the influence of hydrodynamic force, the Morrison equation is used to analyze the hydrodynamic performance of the underwater manipulator in the static water environment, the water viscosity and additional mass effect are solved, the additional mass force, additional mass torque, viscous resistance and viscous torque are calculated, and the hydrodynamic model of the underwater manipulator in the static water environment is built, which is introduced into the dynamic model of the underwater manipulator-movable base system: wherein θ, respectively represent joint angle, joint velocity and joint acceleration of the robot arm, θ = [θ1 θ2 … θ n ], θ1, θ2 … θ n respectively represent the 1st, 2nd … nth joint angle of the underwater robot arm, respectively represent the 1st, 2nd … nth joint angle velocity of the underwater robot arm, respectively represent the 1st, 2nd … nth joint angle acceleration of the underwater robot arm, M(θ) is a 3x3 order mass matrix of the robot arm, containing joint velocity related terms, τ = [τ1, τ2, f x ] T are the joint 1 driving torque (N·m), joint 2 driving torque (N·m) and base x direction driving force (N) in turn, wherein the hydrodynamic force related part is contained.

3. The infinite horizon MPC based manipulator-seats system control method for underwater vehicles according to claim 1, wherein: The step 2 is specifically: Step 2.1: the underwater manipulator-movable base model is converted from a strong nonlinear model to a linearized model; Step 2.2: based on the linearized model, the corresponding state space equation is constructed; Step 2.3: the infinite time domain MPC method is used for control design of the linearized model, and the performance index J is set; Step 2.4: In each control cycle, the MPC algorithm is used to predict the model states in the future time domain, calculate the optimal control sequence u * (k), and continuously update the prediction model according to the real-time measured model states to adapt to environmental changes and model dynamic characteristics; Step 2.5: the optimized control input is applied to the underwater manipulator-movable base model to form a closed-loop control system, and through continuous state feedback and MPC optimization, the control and stability of the system in complex underwater environment are ensured.

4. The infinite horizon MPC based control method for manipulator-base system of underwater vehicle according to claim 3, wherein: The step 2.1 converts the underwater manipulator-movable base model from a strong nonlinear model to a linearized model, which is specifically: where the definitions then q = [θ1 θ2 x] T , control vector u = [τ1 τ2 f x ] T , α(δx, δu) is a higher order term in δx, δu; are the partial derivatives of the vector f(x, u) with respect to the vectors x and u, respectively.

5. The infinite horizon MPC based control method for manipulator-base system of underwater vehicle according to claim 3, wherein: The step 2.3 adopts the infinite time domain MPC method for control design of the linearized model, and sets the performance index J, which is specifically: wherein: J represents the target cost function of model predictive control, e(k) = x(k) - x d (k) is the error of the system, x(k) is the actual state output of the system, x d (k) is the desired reference state of the system, u(k) is the control input sequence, Δu(k) is the control input increment sequence, Q is the weight matrix of the prediction error of the system, R is the weight matrix of the prediction control input; F is the terminal cost matrix of the system error, and Q and R are both set as diagonal matrices.

6. The infinite horizon MPC based control method for manipulator-base system of underwater vehicle according to claim 3, wherein: The step 2.4 uses the MPC algorithm to predict the model state in the future time domain in each control cycle, calculates the optimal control sequence u * (k), and continuously updates the prediction model according to the real-time measured model state to adapt to environmental changes and model dynamic characteristics. Specifically: Step 2.4.1: convert the cost function J into a standard quadratic programming form; Step 2.4.2: consider the input constraints of the manipulator system; where Ω is the terminal region of the system, which is calculated from the input constraints, once the system enters the terminal region, its trajectory will remain in the terminal region, U k represents the optimal control sequence obtained in the prediction horizon at the current time k, u(k|k) represents the system control input acting on the current state x(k) of the system in the prediction horizon at the current time k, u(k+1|k) represents the system control input for predicting the state variable x(k+1) of the system at time k+1 in the prediction horizon at the current time k, and u(k+N-1|k) represents the system control input for predicting the state variable x(k+N-1) at time k+N-1 in the prediction horizon at the current time k, and are the state weight matrices corresponding to the optimal cost function, respectively, Γ=[I,0,...,0] T . The optimal control input sequence calculated The first control input is applied to the underwater manipulator-mobile base system to make the system perform the corresponding action and adjust its motion state.

7. A computer apparatus / device / system comprising a memory, a processor, and a computer program stored on the memory, characterized in that: The processor executes the computer program to realize the steps of the method of any one of claims 1 to 6.

8. A computer readable storage medium having stored thereon computer programs / instructions, characterized in that: The computer program / instruction is executed by the processor to realize the steps of the method of any one of claims 1 to 6.

9. A computer program product comprising computer programs / instructions, characterized in that: The computer program / instruction is executed by the processor to realize the steps of the method of any one of claims 1 to 6.

Images