CN113325716A - Underwater hydraulic mechanical arm nonlinear robust control method based on extended observer - Google Patents
Underwater hydraulic mechanical arm nonlinear robust control method based on extended observer Download PDFInfo
- Publication number
- CN113325716A CN113325716A CN202110646833.5A CN202110646833A CN113325716A CN 113325716 A CN113325716 A CN 113325716A CN 202110646833 A CN202110646833 A CN 202110646833A CN 113325716 A CN113325716 A CN 113325716A
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- joint
- hydraulic
- nonlinear
- underwater
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
Abstract
The invention discloses a nonlinear robust control method of an underwater hydraulic mechanical arm based on an extended observer. The method comprises the following steps: establishing a system nonlinear dynamic model of the underwater hydraulic mechanical arm; establishing a nonlinear robust control law of the underwater hydraulic mechanical arm to obtain a nonlinear robust controller of the underwater hydraulic mechanical arm; establishing an extended observer of the underwater hydraulic mechanical arm to obtain an angular velocity observation value and an unmeasured time-varying interference amount observation value of the underwater hydraulic mechanical arm; the angular velocity observation value, the unmeasured time-varying interference quantity observation value and a tracking error value obtained by measurement of the sensor are fed back to the nonlinear robust controller in real time, the nonlinear robust controller controls the underwater hydraulic mechanical arm to form a complete closed-loop control system of the underwater hydraulic mechanical arm, and the nonlinear robust controller can effectively control the underwater hydraulic mechanical arm under the condition of no angular velocity sensor. The invention solves the problem that the existing control method has low control precision on the underwater hydraulic mechanical arm.
Description
Technical Field
The invention belongs to a nonlinear control method of a mechanical arm in the field of motion control of an underwater hydraulic mechanical arm, and particularly relates to a nonlinear robust control method of an underwater hydraulic mechanical arm based on an extended observer.
Background
With the continuous deepening of ocean development, utilization and research, the complexity of underwater operation tasks is continuously increased, the requirement on operation accuracy is higher and higher, and under the condition, the underwater operation tasks are completed by means of the assistance of an underwater robot. As an important component of an underwater robot, an underwater hydraulic mechanical arm is necessary equipment for completing complex underwater operation tasks. The method is applied to various aspects such as pipeline tracking, submarine cable burying, marine resource investigation, submarine oil platform detection, underwater salvage, rescue and rescue. However, the current underwater hydraulic mechanical arm control mode is usually PID control, and the joint movement speed of the multi-joint hydraulic mechanical arm cannot be accurately measured in many scenes. In addition, the underwater hydraulic mechanical arm is often subjected to external interference such as sea waves and ocean currents during operation, the operation precision of the hydraulic mechanical arm is seriously influenced, and higher challenges are provided for the motion control robustness requirements of the hydraulic mechanical arm. Due to the factors, the underwater hydraulic mechanical arm is difficult to ensure good tail end control precision, and underwater operation performance is influenced.
Disclosure of Invention
The invention provides a Nonlinear Robust Control (NRC) method of an underwater hydraulic mechanical arm based on an extended observer, aiming at the aspects of unknown speed signals, insufficient control technology and the like faced by the existing underwater hydraulic mechanical arm during movement. The method is oriented to an application scene that the hydraulic mechanical arm is subjected to model uncertainty and uncertain nonlinear factors which are continuously, unknown and not ignored in an underwater environment, a nonlinear robust controller based on an underwater hydraulic mechanical arm dynamic model is built, and an expansion observer is used for obtaining an underwater hydraulic mechanical arm joint angular velocity observation value and an unmeasured time-varying interference amount observation value which are difficult to directly and accurately measure so as to optimize control performance. Meanwhile, the influence of model uncertainty (modeling error and parameter uncertainty) and uncertain nonlinearity (mechanical friction, hydraulic oil resistance and wave flow influence) on the control precision of the tail end of the mechanical arm in the motion process of the mechanical arm is reduced through a nonlinear robust control strategy and an interference observation item in the extended observer, so that the robustness and the precision of the control system of the underwater hydraulic mechanical arm are further improved, and the problem that the control precision of the existing control method on the underwater hydraulic mechanical arm is not high is solved.
In order to achieve the purpose, the specific technical scheme of the invention is as follows:
the invention comprises the following steps:
1) establishing a system nonlinear dynamic model of the underwater hydraulic mechanical arm;
2) establishing a nonlinear robust control law of the underwater hydraulic mechanical arm based on a system nonlinear dynamical model of the underwater hydraulic mechanical arm, wherein the system nonlinear dynamical model of the underwater hydraulic mechanical arm is connected with the nonlinear robust control law to form a nonlinear robust controller of the underwater hydraulic mechanical arm;
3) establishing an extended observer of the underwater hydraulic mechanical arm to realize angular velocity observation under the condition of no angular velocity sensor; meanwhile, the extended observer observes the unmeasured time-varying interference quantity in the system nonlinear dynamic model to obtain an angular velocity observation value and an unmeasured time-varying interference quantity observation value of the underwater hydraulic mechanical arm;
4) the angular velocity observation value, the unmeasured time-varying interference quantity observation value and a tracking error value obtained by measurement of the sensor are fed back to the nonlinear robust controller in real time, the nonlinear robust controller controls the underwater hydraulic mechanical arm to form a complete closed-loop control system of the underwater hydraulic mechanical arm, and the nonlinear robust controller can effectively control the underwater hydraulic mechanical arm under the condition of no angular velocity sensor.
The step 1) is specifically as follows:
establishing a system nonlinear dynamical model of the underwater hydraulic mechanical arm, wherein the system nonlinear dynamical model of the underwater hydraulic mechanical arm mainly comprises a dynamic relation between a joint angle and a hydraulic cylinder push rod, a nonlinear dynamical model of a connecting rod mechanical arm, a nonlinear dynamical model of a hydraulic system and a dynamic relation between chamber flow and hydraulic valve core displacement;
1.1) establishing a dynamic relation between a joint angle and a hydraulic cylinder push rod, which specifically comprises the following steps:
each joint angle q of the underwater hydraulic mechanical arm satisfies q ═ q1,q2,…,qi,…,qn]TThe extension x of the push rod of each joint hydraulic cylinder meets the condition that x is ═ x1,x2,…,xi,…,xn]TWherein q is1Representing the joint angle, q, of the first joint of an underwater hydraulic manipulatoriRepresenting the joint angle, x, of the ith joint of an underwater hydraulic manipulator1Shows the extension amount, x, of the push rod of the joint hydraulic actuator of the first joint of the underwater hydraulic mechanical armiThe extension amount of a push rod of a joint hydraulic actuator of the ith joint of the underwater hydraulic mechanical arm is represented, i represents the serial number of the joint, n represents the total number of the joints, i is 1,2,3, …, n, T represents a transposition operation, and each joint angle and the extension amount of the push rod of the hydraulic actuator of the corresponding joint meet the following relation:
wherein the content of the first and second substances,
indicating the length between the i-1 th joint and the ith joint,
represents the length between the ith joint and the (i + 1) th joint;
1.2) establishing a nonlinear dynamics model of the connecting rod mechanical arm, and satisfying the following formula:
wherein, M (q),
and G (q) are an inertia matrix, a Coriolis force and centrifugal force matrix and a gravity matrix of the underwater hydraulic mechanical arm respectively;
the angular velocity of each joint of the underwater hydraulic mechanical arm is expressed, and the requirements are met
The joint angular velocity of the ith joint of the underwater hydraulic mechanical arm is shown,
the angular acceleration of each joint of the underwater hydraulic mechanical arm is expressed, and the requirements are met
Representing the joint angular acceleration of the ith joint of the underwater hydraulic mechanical arm;
a full differential matrix representing the extension x of the push rod of each joint hydraulic cylinder to each joint angle q, and satisfies
PinThe oil pressure of an oil inlet cavity of each joint hydraulic cylinder is shown, and the requirement of oil pressure
The oil pressure of an oil inlet cavity of a hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is represented; poutThe oil pressure of an oil return cavity of each joint hydraulic cylinder is shown, and the requirement of oil pressure
The oil pressure of an oil return cavity of a hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is represented; a. theinThe area of the oil inlet cavity of each joint hydraulic cylinder is shown, and the requirement is met
The area of an oil inlet cavity of a hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is represented; a. theoutThe area of an oil return cavity of each joint hydraulic cylinder is shown, and the requirement is met
The area of an oil return cavity of a hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is represented; d represents an interference item in the motion of the underwater hydraulic mechanical arm, wherein the interference item comprises mechanical arm interference factors influenced by mechanical friction, hydraulic oil resistance and wave flow;
1.3) establishing a hydraulic system nonlinear dynamics model, and satisfying the following formula:
wherein, VinThe volume of the oil inlet cavity of each joint hydraulic cylinder of the underwater hydraulic mechanical arm is expressed, and the requirements are met
VoutThe volume of an oil return cavity of each joint hydraulic cylinder of the underwater hydraulic mechanical arm is expressed, and the requirement of the volume of the oil return cavity of each joint hydraulic cylinder of the underwater hydraulic mechanical arm is met
And
respectively representing the volume of an oil inlet cavity and the volume of an oil return cavity of each joint hydraulic cylinder under the initial condition; diag [ 2 ]]Representing a matrix diagonalization operation; beta is aeThe volume modulus of the hydraulic oil is shown; qinThe flow of the oil inlet cavity of each joint hydraulic cylinder is expressed, and the requirement of the flow
The flow of an oil inlet cavity of a hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is represented; qoutThe flow of an oil return cavity of a hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is expressed, and the requirement of the flow of the oil return cavity of the hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is met
The flow of an oil return cavity of a hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is represented;
the differential of the oil pressure of the oil inlet cavity of each joint hydraulic cylinder is represented;
the differential of the oil pressure of the oil return cavity of each joint hydraulic cylinder is shown;
1.4) establishing a dynamic relation between the chamber flow and the displacement of the valve core of the hydraulic valve, and satisfying the following formula:
Qin＝kqingin(Pin,xv)xv (5)
Qout＝kqoutgout(Pout,xv)xv (6)
wherein x isvIs the valve core displacement of each joint hydraulic control valve, and meets the requirements
A valve body displacement of a hydraulic control valve of the ith joint; k is a radical ofqinShows the flow gain constant of each joint oil inlet chamber and satisfies
A flow gain constant representing the oil inlet chamber of the ith joint; k is a radical ofqoutTo representThe flow gain constant of the oil inlet chamber and the oil return chamber is satisfied
A flow gain constant of an oil return chamber of the i-th joint is represented; gin(Pin,xv) Indicating the spool displacement x of each joint hydraulic control valvevPressure P of oil inlet chamberinValve element displacement transfer function of gout(Pout,xv) Indicating the spool displacement x of each joint hydraulic control valvevAnd return chamber pressure PoutThe spool displacement transfer function of (1) satisfies the following equation:
wherein the content of the first and second substances,
valve element displacement of hydraulic control valve for ith joint
Pressure of oil inlet chamber
The transfer function of the spool displacement of (a),
representing the ith jointSpool displacement of hydraulic control valve
And pressure of oil return chamber
Valve element displacement transfer function of, PsIs the supply pressure coefficient, P, of the hydraulic pumprIs the reference pressure coefficient of the hydraulic return tank.
The step 2) is specifically as follows:
2.1) establishing a first nonlinear robust control law P based on the system nonlinear dynamical model established in the step 1)LdThe following formula is satisfied:
PLd＝PLdo+PLds1+PLds2 (16)
wherein, PLdoRepresenting a compensation parameter, P, of the first nonlinear modelLds1Representing a first linear robust parameter, PLds2Representing a first uncertainty compensation parameter;
is an estimation matrix of parameters in the nonlinear dynamical model of the connecting rod mechanical arm,
compensating the parameter P for the first non-linear modelLdoIn the parameter estimation matrix
A coefficient regression matrix corresponding to each parameter in (1);
and
respectively an inertia matrix M (q), a Coriolis force matrix and a centrifugal force matrix in the nonlinear dynamics model of the connecting rod mechanical arm
A gravity matrix g (q) and an estimate of an interference matrix D; z is a radical of2Representing the angle conversion error of the underwater hydraulic mechanical arm; k is a radical of2Is a predetermined angle conversion error z2The coefficient feedback gain positive definite matrix; t represents a transpose operation; h isPA compensation parameter representing a first non-linear robust control law; compensation parameter h of first nonlinear robust control lawPIs set to be more than the first nonlinear model compensation parameter PLdoAnd a first linear robust parameter PLds1Is smaller by three or more orders of magnitude;
an error matrix representing uncertainty model parameters satisfying
Theta is a parameter matrix in the nonlinear dynamical model of the connecting rod mechanical arm;
the uncertain nonlinear error parameter of the underwater environment is expressed and satisfied
2.2) robust control law P based on a first non-linearityLdEstablishing a second nonlinear robust control law QLdAnd taking the second nonlinear robust control law as the underwater hydraulic mechanical armA nonlinear robust control law satisfying the following formula:
QLd＝QLdo+QLds1+QLds2 (29)
wherein Q isLdoRepresenting a second non-linear model compensation parameter, QLds1Representing a second linear robust parameter, QLds2Representing a second uncertainty compensation parameter;
expressed in the second non-linear model compensation parameter QLdoIn the parameter estimation matrix
A coefficient regression matrix corresponding to each parameter in (1); z is a radical of3Representing the pressure parameter error, k, of an underwater hydraulic manipulator3Is a preset pressure parameter error z3The coefficient feedback gain positive definite matrix; oil inlet cavity flow Q of each joint hydraulic cylinderinNominal flow Q of the inlet chambers of the hydraulic cylinders of each jointinmAnd the error flow of the oil inlet cavity of each joint hydraulic cylinder
Flow Q of oil return cavity of each joint hydraulic cylinderoutNominal flow Q of the return chambers of the hydraulic cylinders comprising each jointoutmAnd error flow rate of oil return chamber
hQ(x) A compensation parameter representing a second non-linear robust control law, a compensation parameter h of the second non-linear robust control lawQ(x) Is set to be more than the second nonlinear model compensation parameter QLdoAnd a second linear robust parameter QLds1Is smaller by three or more orders of magnitude;
representing a first non-linear robust control law PLdDifferential of (2)
A non-calculation section;
2.3) constructing a nonlinear robust controller of the underwater hydraulic mechanical arm based on a system nonlinear dynamical model and a nonlinear robust control law, specifically, inputting a second nonlinear model compensation parameter of the nonlinear robust control law into the system nonlinear dynamical model, adding an output of the system nonlinear dynamical model, the second linear robust parameter and the uncertainty compensation parameter, and outputting the sum as an output of the nonlinear robust controller, and obtaining an output of the nonlinear robust controller by simultaneous equations (6) - (9) and (29), wherein the following relations are satisfied:
xv＝(AinVin -1kqingin(Pin,xv)+AoutVout -1kqoutgout(Pout,xv))-1QLd (34)
and the output of the nonlinear robust controller is transmitted to a hydraulic system of the underwater hydraulic mechanical arm so as to realize control.
The step 3) is specifically as follows:
establishing an extended observer of the underwater hydraulic mechanical arm, observing the moment of each joint hydraulic driver of the underwater hydraulic mechanical arm and the unmeasured time-varying interference quantity in the system nonlinear dynamic model by the extended observer, and obtaining an angular velocity observation value and an unmeasured time-varying interference quantity observation value of the underwater hydraulic mechanical arm; observed state of extended observer
D is an immeasurable time-varying interference item in a nonlinear dynamics model of the connecting rod mechanical arm, and specifically comprises mechanical motion friction and hydraulic oil resistance interference;
the extended observer is set by the following formula:
wherein the content of the first and second substances,
for the observed value of each state quantity, including the observed value of each joint angle
Observed value of angular velocity of each joint
And non-measurable time-varying interference term observation
Observed values representing the respective state quantities
Differentiation of (1); l is a feedback gain coefficient matrix; y represents the theoretical joint angles of the underwater hydraulic manipulator,
representing the observation values of all joint angles of the underwater hydraulic mechanical arm; u represents the moment of each joint hydraulic actuator, and satisfies
M, N, J, E each represent a matrix of state equation coefficients for the extended observer;
the state equation coefficient matrix M, N, J, E is represented as:
E＝[I 0 0]
wherein I represents an identity matrix.
The underwater hydraulic mechanical arm is mainly formed by connecting a connecting rod mechanical arm and a hydraulic system, sensors are arranged in the connecting rod mechanical arm and the hydraulic system, measure the first state, the second state, the third state and the fourth state of the underwater hydraulic mechanical arm and transmit the states to the nonlinear robust controller and the expansion observer.
Compared with the prior art, the invention has the following beneficial effects:
1. the invention provides a nonlinear robust control method (NRC) of an underwater hydraulic mechanical arm based on an established nonlinear dynamical model of the underwater hydraulic mechanical arm, and establishes a nonlinear robust control law of the underwater hydraulic mechanical arm, wherein the nonlinear robust control law comprises nonlinear model compensation parameters, linear robust parameters and uncertainty compensation parameters, so that the influence of model uncertainty and uncertain nonlinear factors existing in the establishment process of a controller is overcome, the robust performance of the controller is optimized, the tracking error of the tail end of the mechanical arm is reduced, the control precision of an end effector of the hydraulic mechanical arm is improved, and the control performance of a control system of the underwater hydraulic mechanical arm is improved.
2. The extended observer established by the invention can observe the movement speed of each joint and the immeasurable time-varying interference amount in the movement process of the hydraulic mechanical arm, thereby realizing the effective control of the established nonlinear robust controller on the mechanical arm under the condition that the joint speed signal cannot be directly measured.
Drawings
Fig. 1 is a schematic view of a control target of the present invention.
Fig. 2 is a diagram of the hydraulic drive system of the present invention.
FIG. 3 is a block diagram of a nonlinear robust control system of an underwater hydraulic mechanical arm based on an extended observer, which is established by the invention.
FIG. 4 is a target trajectory for the articulation of the underwater hydraulic robotic arm of the present invention.
FIG. 5 is a velocity observation diagram of the extended observer created by the present invention.
Fig. 6 is a graph of the observation of disturbance torque caused by damping of unknown magnitude in the case of sinusoidal input by the extended observer established by the present invention.
FIG. 7 is a graph comparing the control effect of the extended observer based subsea hydraulic mechanical arm controller (NRC) established by the present invention with a conventional PID controller.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to the accompanying drawings and embodiments. The specific embodiments described herein are merely illustrative of the invention and are not intended to be limiting. Furthermore, the technical features mentioned in the embodiments of the present invention described below are combined with each other as long as they do not conflict with each other.
The invention provides an underwater hydraulic mechanical arm nonlinear robust control method based on an extended observer, which comprises the following steps: firstly, aiming at the mechanical configuration of a connecting rod of an underwater hydraulic mechanical arm and a hydraulic transmission mechanism, considering mechanical arm interference factors including mechanical friction, hydraulic oil resistance and wave flow influence, and establishing a nonlinear dynamic model of the underwater hydraulic mechanical arm. And then, based on the established nonlinear dynamics model of the underwater hydraulic mechanical arm, the influence of model uncertainty and uncertain nonlinearity existing in the establishment process of the controller is overcome through a nonlinear robust control law of the underwater hydraulic mechanical arm, wherein the nonlinear robust control law comprises nonlinear model compensation parameters, linear robust parameters and uncertainty compensation parameters. In addition, the established nonlinear dynamics model of the underwater hydraulic mechanical arm is combined to form an integral nonlinear robust control method of the underwater hydraulic mechanical arm. And finally, considering joint angular velocity parameters existing in the established nonlinear robust control law of the underwater hydraulic mechanical arm, aiming at the specific condition that part of the underwater hydraulic mechanical arms are not provided with angular velocity sensors, the angular velocity observation under the condition of no angular velocity sensors is realized by establishing a multi-joint underwater hydraulic mechanical arm expansion observer. Meanwhile, the time-varying interference (mechanical friction and hydraulic oil resistance) which can not be measured by the dynamic model is observed, and the control performance of the established nonlinear robust controller on the mechanical arm is improved. And finally, feeding back the angular velocity observation value of the hydraulic mechanical arm, the unmeasured time-varying interference quantity observation value and the tracking error value obtained by the measurement of the sensor to the established nonlinear robust control method of the underwater hydraulic mechanical arm in real time to form a complete closed-loop control system of the underwater hydraulic mechanical arm, and realizing the effective control of the established nonlinear robust controller on the multi-joint underwater hydraulic mechanical arm under the condition of no angular velocity sensor.
Therefore, the provided method for controlling the nonlinear robustness of the underwater hydraulic mechanical arm based on the extended observer can effectively reduce the influence of model uncertainty (modeling error and parameter uncertainty) and uncertain nonlinearity (mechanical friction, hydraulic oil resistance and wave flow influence) on the control precision of the tail end of the mechanical arm in the motion process of the underwater hydraulic mechanical arm under the condition that a speed signal cannot be directly measured, reduces the tracking error of the tail end of the mechanical arm while ensuring the stability of a control system, and improves the control performance, thereby solving the problem that the control precision of the existing control method on the multi-joint underwater hydraulic mechanical arm is not high.
The invention will now be further described with reference to fig. 1,2,3, 4, 5, 6, 7:
the implementation technical scheme of the invention is as follows:
as shown in fig. 3, the present invention comprises the steps of:
1) as shown in fig. 1 and 2, aiming at the mechanical configuration and the hydraulic transmission mechanism of a connecting rod of an underwater multi-joint hydraulic mechanical arm, considering mechanical arm interference factors including mechanical friction, hydraulic oil resistance and wave flow influence, and establishing a system nonlinear dynamic model of the underwater hydraulic mechanical arm; the underwater hydraulic mechanical arm is mainly formed by connecting a connecting rod mechanical arm and a hydraulic system, sensors are arranged in the connecting rod mechanical arm and the hydraulic system, and the sensors are used for measuring the state of the underwater hydraulic mechanical arm and transmitting the state to the nonlinear robust controller and the extended observer.
The step 1) is specifically as follows:
establishing a system nonlinear dynamical model of the underwater hydraulic mechanical arm, wherein the system nonlinear dynamical model of the underwater hydraulic mechanical arm mainly comprises a dynamic relation between a joint angle and a hydraulic cylinder push rod, a nonlinear dynamical model of a connecting rod mechanical arm, a nonlinear dynamical model of a hydraulic system and a dynamic relation between chamber flow and hydraulic valve core displacement;
1.1) establishing a dynamic relation between a joint angle and a hydraulic cylinder push rod, which specifically comprises the following steps:
each joint angle q of the underwater hydraulic mechanical arm satisfies q ═ q1,q2,…,qi,…,qn]TThe extension x of the push rod of each joint hydraulic cylinder meets the condition that x is ═ x1,x2,…,xi,…,xn]TWherein q is1Representing the joint angle, q, of the first joint of an underwater hydraulic manipulatoriRepresenting the joint angle, x, of the ith joint of an underwater hydraulic manipulator1Shows the extension amount, x, of the push rod of the joint hydraulic actuator of the first joint of the underwater hydraulic mechanical armiThe hydraulic manipulator push rod elongation of the ith joint of the underwater hydraulic manipulator is represented, i represents the serial number of the joint, n represents the total number of the joints, i is 1,2,3, …, n, and T represents the transposition operation, the hydraulic manipulator push rod elongation of each joint is only related to the joint angle of one corresponding joint, and the hydraulic manipulator push rod elongation of each joint and the corresponding joint satisfies the following relations:
wherein the content of the first and second substances,
indicating the length between the i-1 th joint and the ith joint,
represents the length between the ith joint and the (i + 1) th joint;
1.2) establishing a nonlinear dynamics model of the connecting rod mechanical arm, and satisfying the following formula:
wherein, M (q),
and G (q) are an inertia matrix, a Coriolis force and centrifugal force matrix and a gravity matrix of the underwater hydraulic mechanical arm respectively;
the angular velocity of each joint of the underwater hydraulic mechanical arm is expressed, and the requirements are met
The joint angular velocity of the ith joint of the underwater hydraulic mechanical arm is shown,
the angular acceleration of each joint of the underwater hydraulic mechanical arm is expressed, and the requirements are met
Representing the joint angular acceleration of the ith joint of the underwater hydraulic mechanical arm;
showing the extension x of the push rod of each joint hydraulic cylinder to the total micro of each joint angle qDivide the matrix to satisfy
PinThe oil pressure of an oil inlet cavity of each joint hydraulic cylinder is shown, and the requirement of oil pressure
The oil pressure of an oil inlet cavity of a hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is represented; poutThe oil pressure of an oil return cavity of each joint hydraulic cylinder is shown, and the requirement of oil pressure
The oil pressure of an oil return cavity of a hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is represented; a. theinThe area of the oil inlet cavity of each joint hydraulic cylinder is shown, and the requirement is met
The area of an oil inlet cavity of a hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is represented; a. theoutThe area of an oil return cavity of each joint hydraulic cylinder is shown, and the requirement is met
The area of an oil return cavity of a hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is represented; d represents an interference item in the motion of the underwater hydraulic mechanical arm, wherein the interference item comprises mechanical arm interference factors influenced by mechanical friction, hydraulic oil resistance and wave flow;
1.3) assuming that the oil cylinder has no leakage, establishing a nonlinear dynamic model of the hydraulic system, and satisfying the following formula:
wherein, VinThe volume of the oil inlet cavity of each joint hydraulic cylinder of the underwater hydraulic mechanical arm is expressed, and the requirements are met
VoutThe volume of an oil return cavity of each joint hydraulic cylinder of the underwater hydraulic mechanical arm is expressed, and the requirement of the volume of the oil return cavity of each joint hydraulic cylinder of the underwater hydraulic mechanical arm is met
And
respectively representing the volume of an oil inlet cavity and the volume of an oil return cavity of each joint hydraulic cylinder in the initial condition (x is 0); diag [ 2 ]]Representing a matrix diagonalization operation; beta is aeThe volume modulus of the hydraulic oil is shown; qinThe flow of the oil inlet cavity of each joint hydraulic cylinder is expressed, and the requirement of the flow
The flow of an oil inlet cavity of a hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is represented; qoutThe flow of an oil return cavity of a hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is expressed, and the requirement of the flow of the oil return cavity of the hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is met
The flow of an oil return cavity of a hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is represented;
the differential of the oil pressure of the oil inlet cavity of each joint hydraulic cylinder is represented;
the differential of the oil pressure of the oil return cavity of each joint hydraulic cylinder is shown;
1.4) establishing a dynamic relation between the chamber flow and the displacement of the valve core of the hydraulic valve, and satisfying the following formula:
Qin＝kqingin(Pin,xv)xv (5)
Qout＝kqoutgout(Pout,xv)xv (6)
wherein x isvIs the valve core displacement of each joint hydraulic control valve, and meets the requirements
A valve body displacement of a hydraulic control valve of the ith joint; k is a radical ofqinShows the flow gain constant of each joint oil inlet chamber and satisfies
A flow gain constant representing the oil inlet chamber of the ith joint; k is a radical ofqoutShows the flow gain constant of the oil inlet chamber and the oil return chamber, and satisfies
A flow gain constant of an oil return chamber of the i-th joint is represented; gin(Pin,xv) Indicating the spool displacement x of each joint hydraulic control valvevPressure P of oil inlet chamberinValve element displacement transfer function of gout(Pout,xv) Indicating the spool displacement x of each joint hydraulic control valvevAnd return chamber pressure PoutThe spool displacement transfer function of (1) satisfies the following equation:
wherein the content of the first and second substances,
valve element displacement of hydraulic control valve for ith joint
Pressure of oil inlet chamber
The transfer function of the spool displacement of (a),
valve element displacement of hydraulic control valve for ith joint
And pressure of oil return chamber
Valve element displacement transfer function of, PsIs the supply pressure coefficient, P, of the hydraulic pumprIs the reference pressure coefficient of the hydraulic return tank.
2) Based on a system nonlinear dynamics model of the underwater hydraulic mechanical arm, a nonlinear robust control law of the underwater hydraulic mechanical arm is established, so that model uncertainty and uncertain nonlinear influence existing in the controller establishing process are overcome; in addition, a system nonlinear dynamic model of the underwater hydraulic mechanical arm is connected with a nonlinear robust control law to form a nonlinear robust controller of the underwater hydraulic mechanical arm;
the step 2) is specifically as follows:
2.1) establishing a first nonlinear robust control law P based on the system nonlinear dynamical model established in the step 1)LdThe following formula is satisfied:
PLd＝PLdo+PLds1+PLds2 (16)
wherein, PLdoRepresenting a compensation parameter, P, of the first nonlinear modelLds1Representing a first linear robust parameter, PLds2Representing a first uncertainty compensation parameter;
a matrix is estimated for parameters in a nonlinear dynamical model of the link arm,
compensating the parameter P for the first non-linear modelLdoIn the parameter estimation matrix
A coefficient regression matrix corresponding to each parameter in (1);
and
respectively an inertia matrix M (q), a Coriolis force matrix and a centrifugal force matrix in the nonlinear dynamics model of the connecting rod mechanical arm
A gravity matrix g (q) and an estimate of an interference matrix D; z is a radical of2Representing the angle conversion error of the underwater hydraulic mechanical arm; k is a radical of2Is a predetermined angle conversion error z2The coefficient feedback gain positive definite matrix; t represents a transpose operation; h isPRepresenting a first non-linear robustCompensation parameters of the control law; compensation parameter h of first nonlinear robust control lawPIs set to be more than the first nonlinear model compensation parameter PLdoAnd a first linear robust parameter PLds1Is smaller by three (i.e., 10-3) or more orders of magnitude;
an error matrix representing uncertainty model parameters satisfying
Theta is a parameter matrix in the nonlinear dynamical model of the connecting rod mechanical arm;
the uncertain nonlinear error parameter of the underwater environment is expressed and satisfied
First nonlinear robust control law PLdThe calculation process specifically comprises the following steps:
tracking error z of joint angle of underwater hydraulic mechanical arm1Comprises the following steps:
z1＝qs-qd (11)
wherein q issRepresenting actual measurements of the joint angles of an underwater hydraulic manipulator, qdAnd the control target value of each joint angle of the underwater hydraulic mechanical arm is represented. In addition, the angle conversion error z of the underwater hydraulic mechanical arm2Comprises the following steps:
transforming angle parameters
Comprises the following steps:
wherein k is1Representing preset joint angle tracking error z of underwater hydraulic mechanical arm1Positive determining a diagonal matrix; k is a radical of1The purpose of the establishment of (1) is to ensure that the differential of the lyapunov control function of the first nonlinear robust control law in the nonlinear robust controller is less than or equal to zero, so that the stability of the whole nonlinear robust controller is maintained.
Differentiating two sides of the equal sign of the formula (12) and multiplying the inertia matrix M (q) of the underwater hydraulic mechanical arm to the left, and combining the formulas (2), (11) and (13) to obtain the following results:
due to PinAin-PoutAoutThe method is characterized in that a nonlinear dynamics model of the connecting rod mechanical arm is a high-order item, so that an inversion establishment method is adopted based on the idea of order reduction, and the pressure virtual control input P of the underwater hydraulic mechanical armLComprises the following steps:
PL＝PinAin-PoutAout (15)
considering that the uncertainty of the parameters of the dynamic model exists in the nonlinear dynamic model of the connecting rod mechanical arm, a parameter estimation matrix is required to replace the accurate parameters of the unknown model in the establishment of the nonlinear robust controller. Here, the pressure virtual control input P for the underwater hydraulic robot armLProposing a first nonlinear robust control law PLdAnd the tracking error of each joint angle is reduced while the transient performance of the system is ensured. Established first nonlinear robust control law PLdThe device consists of the following three parts:
PLd＝PLdo+PLds1+PLds2 (16)
wherein, PLdoRepresenting a compensation parameter, P, of the first nonlinear modelLds1Representing a first linear robust parameter, PLds2Representing a first uncertainty compensation parameter.
First nonlinear model compensationParameter PLdoThe specific expression of (a) is written in the form of:
wherein the content of the first and second substances,
respectively an inertia matrix M (q), a Coriolis force matrix and a centrifugal force matrix in the nonlinear dynamics model of the connecting rod mechanical arm
The gravity matrix g (q) and an estimate of the disturbance matrix D.
In addition, the nonlinear dynamical model of the link mechanical arm has the following two properties:
Wherein theta is a parameter matrix in a link mechanical arm dynamic equation,
is a coefficient regression matrix corresponding to each parameter in the parameter matrix theta.
In the process of establishing the nonlinear robust controller, a parameter matrix theta is as follows:
according to the above properties, the first nonlinear model compensates the parameter PLdoWriting into:
wherein the content of the first and second substances,
estimating a matrix for parameters in a multi-joint hydraulic mechanical arm dynamic model;
compensating the parameter P for the first non-linear modelLdoIn the parameter estimation matrix
The coefficient regression matrix corresponding to each parameter in (1).
First linear robust parameter PLds1The specific expression of (a) is written in the form of:
wherein k is2Is a predetermined angle conversion error z2The coefficient feedback gain positive definite matrix. Angle conversion error k2The purpose of (2) is also to ensure that the derivative of the lyapunov control function of the first nonlinear robust control law is less than or equal to zero, so that the stability of the overall nonlinear robust controller is maintained.
In addition, considering that uncertain nonlinear factors also exist in the nonlinear dynamical model of the connecting rod mechanical arm, the influence factors need to be compensated. As uncertainty compensation parameter, a first uncertainty compensation parameter PLds2Cannot be written as a concrete formula, but it needs to satisfy the following constraints:
wherein h isPIs the first non-linear robust control law compensationParameter, first non-linear robust control law compensation parameter hPIs set to be more than the first nonlinear model compensation parameter PLdoAnd a first linear robust parameter PLds1Is smaller by three (i.e., 10-3) or more orders of magnitude.
An error matrix representing uncertainty model parameters satisfying
Theta is a parameter matrix in the nonlinear dynamical model of the connecting rod mechanical arm;
an uncertain nonlinear error parameter representing underwater environment is satisfied
First uncertainty compensation parameter P satisfying equation (21)Lds2The first nonlinear robust control law can be ensured to keep good control performance when parameter uncertainty and uncertainty nonlinearity exist.
2.2) robust control law P based on a first non-linearityLdEstablishing a second nonlinear robust control law QLdAnd taking the second nonlinear robust control law as a nonlinear robust control law of the underwater hydraulic mechanical arm, and satisfying the following formula:
QLd＝QLdo+QLds1+QLds2 (29)
wherein Q isLdoRepresenting a second non-linear model compensation parameter, QLds1Representing a second linear robust parameter, QLds2Representing a second uncertainty compensation parameter;
expressed in the second non-linear model compensation parameter QLdoIn the parameter estimation matrix
A coefficient regression matrix corresponding to each parameter in (1); z is a radical of3Representing the pressure parameter error, k, of an underwater hydraulic manipulator3Is a preset pressure parameter error z3The coefficient feedback gain positive definite matrix; oil inlet cavity flow Q of each joint hydraulic cylinderinNominal flow Q of the inlet chambers of the hydraulic cylinders of each jointinmAnd the error flow of the oil inlet cavity of each joint hydraulic cylinder
Flow Q of oil return cavity of each joint hydraulic cylinderoutNominal flow Q of the return chambers of the hydraulic cylinders comprising each jointoutmAnd error flow rate of oil return chamber
hQ(x) A compensation parameter representing a second non-linear robust control law, a compensation parameter h of the second non-linear robust control lawQ(x) Is set to be more than the second nonlinear model compensation parameter QLdoAnd a second linear robust parameter QLds1Is smaller by three (i.e. 10)-3) Or of the order of the above;
representing a first non-linear robust control law PLdDifferential of (2)
A non-calculation section;
second non-linear robustnessControl law QLdThe calculation process specifically comprises the following steps:
upon completion of virtual control input P to pressureLAfter the control law of (2) is established, the pressure parameter error z3＝PL-PLdAnd establishing a second nonlinear robust control law to make the pressure parameter error z3The transient performance and the accuracy of the system are ensured while the system is converged to zero or a minimum value.
Firstly, the pressure parameter error z3Carrying out differentiation:
first nonlinear robust control law PLdIs the pressure virtual control input P set in 3.1)LThe theoretical established value of (a) is fully differentiated to obtain:
differentiation of the first non-linear robust control law
Is calculated by
Is written as:
wherein the content of the first and second substances,
is an observed value of angular velocity of each joint of the mechanical arm acquired by an extended observer,
is obtained by calculating the mechanical arm joints according to a nonlinear dynamics model of a connecting rod mechanical armObserved values of nodal angular acceleration.
Differentiation of the first non-linear robust control law
Without calculating part of
Is written as:
wherein the content of the first and second substances,
and
respectively represent the estimation errors of each angular velocity and each angular acceleration, and respectively satisfy
And
simultaneous equations (3), (4), and (15) yield:
wherein, the oil inlet cavity flow Q of each joint hydraulic cylinderinNominal flow Q of the inlet chambers of the hydraulic cylinders of each jointinmAnd the error flow of the oil inlet cavity of each joint hydraulic cylinder
Flow Q of oil return cavity of each joint hydraulic cylinderoutNominal flow Q of the return chambers of the hydraulic cylinders comprising each jointoutmAnd error flow rate of oil return chamber
And pressure virtual control input PLSame, flow virtual control input QL：
QL＝AinVin -1Qinm+AoutVout -1Qoutm (27)
Simultaneous equations (22) - (26) to relate the pressure error parameter z3The differential of (d) is expressed as:
input Q for virtual control of flow, similar to the first nonlinear robust control lawLThe second nonlinear robust control law of (a) also includes three parts, specifically expressed in the following form:
QLd＝QLdo+QLds1+QLds2 (29)
wherein Q isLdoRepresenting a second non-linear model compensation parameter, QLds1Representing a second linear robust parameter, QLds2Representing a second uncertainty compensation parameter.
Second nonlinear model compensation parameter QLdoThe specific expression of (a) is written in the form of:
according to the property 2 of the nonlinear dynamical model of the connecting rod mechanical arm, the second nonlinear model compensates the parameter QLdoAlso in short:
wherein the content of the first and second substances,
expressed in the second non-linear model compensation parameter QLdoIn the parameter estimation matrix
A coefficient regression matrix corresponding to each parameter in (1);
second linear robust parameter QLds1The specific expression of (a) is written in the form of:
wherein z is3Representing the pressure parameter error, k, of an underwater hydraulic manipulator3Is a preset pressure parameter error z3The coefficient feedback gain positive definite matrix; k is a radical of3Is established to ensure that the derivative of the lyapunov control function of the second non-linear robust control law is less than or equal to zero, thereby enabling the overall controller to maintain stability.
In addition, with the first uncertainty compensation parameter PLds2Same, second uncertainty compensation parameter QLds2Nor can it be written as a concrete formulation, but it needs to satisfy the following constraints:
wherein h isQ(x) A compensation parameter representing a second non-linear robust control law. Compensation parameter h of the second non-linear robust control lawQ(x) Is set to be more than the second nonlinear model compensation parameter QLdoAnd a second linear robust parameter QLds1Is smaller by three (i.e. 10)-3) Or of the order of the above.
2.3) constructing a nonlinear robust controller of the underwater hydraulic mechanical arm based on a system nonlinear dynamical model and a nonlinear robust control law, specifically, inputting a second nonlinear model compensation parameter of the nonlinear robust control law into the system nonlinear dynamical model, adding an output of the system nonlinear dynamical model, the second linear robust parameter and the uncertainty compensation parameter, and outputting the sum as an output of the nonlinear robust controller, and obtaining an output of the nonlinear robust controller by simultaneous equations (6) - (9) and (29), wherein the following relations are satisfied:
xv＝(AinVin -1kqingin(Pin,xv)+AoutVout -1kqoutgout(Pout,xv))-1QLd (34)
and the output of the nonlinear robust controller is transmitted to a hydraulic system of the underwater hydraulic mechanical arm so as to realize control.
3) Considering that joint angular velocity parameters exist in the nonlinear robust control law of the underwater hydraulic mechanical arm established in the second step, aiming at the specific condition that part of the underwater hydraulic mechanical arm has no angular velocity sensor, establishing an extended observer of the underwater hydraulic mechanical arm to realize angular velocity observation under the condition that no angular velocity sensor exists; meanwhile, the extended observer observes the immeasurable time-varying interference (mechanical friction and hydraulic oil resistance) in the system nonlinear dynamic model, the control performance of the nonlinear robust controller on the underwater hydraulic mechanical arm is improved, and an angular velocity observation value and an immeasurable time-varying interference observation value of the underwater hydraulic mechanical arm are obtained;
the step 3) is specifically as follows:
establishing an extended observer of the underwater hydraulic mechanical arm, observing the moment of each joint hydraulic driver of the underwater hydraulic mechanical arm and the unmeasured time-varying interference quantity in the system nonlinear dynamic model by the extended observer, and obtaining an angular velocity observation value and an unmeasured time-varying interference quantity observation value of the underwater hydraulic mechanical arm; observed state of extended observer
D is an immeasurable time-varying interference item in a nonlinear dynamics model of the connecting rod mechanical arm, and specifically comprises mechanical motion friction and hydraulic oil resistance interference;
the extended observer is set by the following formula:
wherein the content of the first and second substances,
for the observed value of each state quantity, including the observed value of each joint angle
Observed value of angular velocity of each joint
And non-measurable time-varying interference term observation
Observed values representing the respective state quantities
Differentiation of (1); l is a feedback gain coefficient matrix, and L is properly adjusted to enable the observer to obtain higher response speed on the premise of ensuring stability; y represents the theoretical joint angles of the underwater hydraulic manipulator,
representing the observation values of all joint angles of the underwater hydraulic mechanical arm; u represents the moment of each joint hydraulic actuator, and satisfies
M, N, J, E each represent a first, second, third, fourth matrix of state equation coefficients of the extended observer;
the first, second, third, and fourth state equation coefficient matrices M, N, J, E are respectively expressed as:
E＝[I 0 0]
wherein I represents an identity matrix.
4) The angular velocity observation value, the unmeasured time-varying interference quantity observation value and the tracking error value obtained by the measurement of the sensor are fed back to the nonlinear robust control law of the nonlinear robust controller in real time, the nonlinear robust controller controls the underwater hydraulic mechanical arm to form a complete closed-loop control system of the underwater hydraulic mechanical arm, the effective control of the nonlinear robust controller on the underwater hydraulic mechanical arm under the condition of no angular velocity sensor is realized, and the problem that the control precision of the existing control method on the underwater hydraulic mechanical arm is low is solved.
Finally, MATLAB/Simulink simulation based on the hydraulic mechanical arm with two degrees of freedom is carried out on the control method, the control method is compared with a PID controller, the control effect of the control method provided by the invention is verified, and the target track of the joint motion of the underwater hydraulic mechanical arm is shown in FIG. 4.
In terms of controller gain coefficient establishment, the PID controller gain parameters for comparison are selected as: k is a radical ofp＝diag[150,180],kI＝diag[40,40],kD＝diag[17,10](ii) a The established ARC controller gain parameters are selected as: k is a radical of1＝170,k2＝diag[170,100],k3＝diag[80,60]。
The specific simulation parameters of the underwater hydraulic mechanical arm are shown in table 1.
TABLE 1 simulation model parameters
The velocity observation of the extended observer established by the invention is shown in fig. 5. Wherein, the dotted line represents the actual joint angular velocity in the simulation process, and the solid line represents the observed value of the observer established by the patent on the joint angle in the simulation process.
The disturbance observations of the extended observer are set up as shown in fig. 6. Wherein the dotted line represents the magnitude of the actual disturbance moment caused by damping in the case of sinusoidal input, and the solid line represents the observed value of the disturbance moment by the extended observer.
Finally, the simulation result of the multi-joint hydraulic mechanical arm is shown in fig. 7, in the two upper sub-graphs in fig. 7, a thin line shows the control effect of the underwater hydraulic mechanical arm NRC controller based on the extended observer, and a thick line shows the control effect of the PID controller; in the next two sub-diagrams of fig. 7, the thin line indicates the error of the extended-scope-based underwater hydraulic manipulator NRC controller, and the thick line indicates the control error of the PID controller.
The control effect subgraph shows that the underwater hydraulic mechanical arm NRC controller based on the extended observer can accurately track a target track curve under the conditions of model interference (mechanical friction, hydraulic oil resistance and wave flow influence) and unknown speed signals. Meanwhile, the error curve shows that the steady-state error of the angle tracking error of each joint is zero (the angular velocity and the acceleration are kept unchanged) in the whole motion process. Compared with the whole change range of the joint angle, the tracking error of the two joints in the transient change process only has smaller fluctuation.
Compared with the traditional PID controller, the NRC has smaller joint tracking error and shorter transient response time, the method embodies that the underwater hydraulic mechanical arm NRC control method based on the extended observer has more excellent transient response performance and better robustness, can observe joint angular velocity signals under the condition that the joint angular velocity signals cannot be directly measured, and simultaneously observe non-measurable time-varying interference quantities (mechanical friction and hydraulic oil resistance) of a dynamic model and compensate the influence of the non-measurable time-varying interference quantities on the control precision of the tail end of the mechanical arm, reduces the tracking error of the tail end of the mechanical arm and improves the control performance while ensuring the stability of a control system.
The above-mentioned contents are only technical ideas of the present invention, and the protection scope of the present invention is not limited thereby, and any modifications made on the basis of the technical ideas proposed by the present invention fall within the protection scope of the claims of the present invention.
Claims (5)
1. A nonlinear robust control method of an underwater hydraulic mechanical arm based on an extended observer is characterized by comprising the following steps:
1) establishing a system nonlinear dynamic model of the underwater hydraulic mechanical arm;
2) establishing a nonlinear robust control law of the underwater hydraulic mechanical arm based on a system nonlinear dynamical model of the underwater hydraulic mechanical arm, wherein the system nonlinear dynamical model of the underwater hydraulic mechanical arm is connected with the nonlinear robust control law to form a nonlinear robust controller of the underwater hydraulic mechanical arm;
3) establishing an extended observer of the underwater hydraulic mechanical arm to realize angular velocity observation under the condition of no angular velocity sensor; meanwhile, the extended observer observes the unmeasured time-varying interference quantity in the system nonlinear dynamic model to obtain an angular velocity observation value and an unmeasured time-varying interference quantity observation value of the underwater hydraulic mechanical arm;
4) the angular velocity observation value, the unmeasured time-varying interference quantity observation value and a tracking error value obtained by measurement of the sensor are fed back to the nonlinear robust controller in real time, the nonlinear robust controller controls the underwater hydraulic mechanical arm to form a complete closed-loop control system of the underwater hydraulic mechanical arm, and the nonlinear robust controller can effectively control the underwater hydraulic mechanical arm under the condition of no angular velocity sensor.
2. The extended observer-based underwater hydraulic manipulator nonlinear robust control method according to claim 1, wherein the step 1) is specifically as follows:
establishing a system nonlinear dynamical model of the underwater hydraulic mechanical arm, wherein the system nonlinear dynamical model of the underwater hydraulic mechanical arm mainly comprises a dynamic relation between a joint angle and a hydraulic cylinder push rod, a nonlinear dynamical model of a connecting rod mechanical arm, a nonlinear dynamical model of a hydraulic system and a dynamic relation between chamber flow and hydraulic valve core displacement;
1.1) establishing a dynamic relation between a joint angle and a hydraulic cylinder push rod, which specifically comprises the following steps:
each joint angle q of the underwater hydraulic mechanical arm satisfies q ═ q1，q2，...，qi，...，qn]TThe extension x of the push rod of each joint hydraulic cylinder meets the condition that x is ═ x1，x2，...，xi，...，xn]TWherein q is1Representing the joint angle, q, of the first joint of an underwater hydraulic manipulatoriRepresenting the joint angle, x, of the ith joint of an underwater hydraulic manipulator1Shows the extension amount, x, of the push rod of the joint hydraulic actuator of the first joint of the underwater hydraulic mechanical armiThe extension amount of a push rod of a joint hydraulic actuator of the ith joint of the underwater hydraulic mechanical arm is represented, i represents the serial number of the joint, n represents the total number of the joints, i is 1,2,3, the.
Wherein the content of the first and second substances,
indicating the length between the i-1 th joint and the ith joint,
represents the length between the ith joint and the (i + 1) th joint;
1.2) establishing a nonlinear dynamics model of the connecting rod mechanical arm, and satisfying the following formula:
wherein, M (q),
and G (q) are an inertia matrix, a Coriolis force and centrifugal force matrix and a gravity matrix of the underwater hydraulic mechanical arm respectively;
the angular velocity of each joint of the underwater hydraulic mechanical arm is expressed, and the requirements are met
The joint angular velocity of the ith joint of the underwater hydraulic mechanical arm is shown,
the angular acceleration of each joint of the underwater hydraulic mechanical arm is expressed, and the requirements are met
Representing the joint angular acceleration of the ith joint of the underwater hydraulic mechanical arm;
a full differential matrix representing the extension x of the push rod of each joint hydraulic cylinder to each joint angle q, and satisfies
PinThe oil pressure of an oil inlet cavity of each joint hydraulic cylinder is shown, and the requirement of oil pressure
The oil pressure of an oil inlet cavity of a hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is represented; poutThe oil pressure of an oil return cavity of each joint hydraulic cylinder is shown, and the requirement of oil pressure
The oil pressure of an oil return cavity of a hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is represented; a. theinThe area of the oil inlet cavity of each joint hydraulic cylinder is shown, and the requirement is met
The area of an oil inlet cavity of a hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is represented; a. theoutThe area of an oil return cavity of each joint hydraulic cylinder is shown, and the requirement is met
The area of an oil return cavity of a hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is represented; d represents an interference item in the motion of the underwater hydraulic mechanical arm, wherein the interference item comprises mechanical arm interference factors influenced by mechanical friction, hydraulic oil resistance and wave flow;
1.3) establishing a hydraulic system nonlinear dynamics model, and satisfying the following formula:
wherein, VinThe volume of the oil inlet cavity of each joint hydraulic cylinder of the underwater hydraulic mechanical arm is expressed, and the requirements are met
VoutThe volume of an oil return cavity of each joint hydraulic cylinder of the underwater hydraulic mechanical arm is expressed, and the requirement of the volume of the oil return cavity of each joint hydraulic cylinder of the underwater hydraulic mechanical arm is met
And
respectively representing the volume of an oil inlet cavity and the volume of an oil return cavity of each joint hydraulic cylinder under the initial condition; diag [ 2 ]]Representing a matrix diagonalization operation; beta is aeThe volume modulus of the hydraulic oil is shown; qinThe flow of the oil inlet cavity of each joint hydraulic cylinder is expressed, and the requirement of the flow
The flow of an oil inlet cavity of a hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is represented; qoutThe flow of an oil return cavity of a hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is expressed, and the requirement of the flow of the oil return cavity of the hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is met
The flow of an oil return cavity of a hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is represented;
the differential of the oil pressure of the oil inlet cavity of each joint hydraulic cylinder is represented;
the differential of the oil pressure of the oil return cavity of each joint hydraulic cylinder is shown;
1.4) establishing a dynamic relation between the chamber flow and the displacement of the valve core of the hydraulic valve, and satisfying the following formula:
Qin＝kqingin(Pin，xv)xv (5)
Qout＝kqoutgout(Pout，xv)xv (6)
wherein x isvIs the valve core displacement of each joint hydraulic control valve, and meets the requirements
A valve body displacement of a hydraulic control valve of the ith joint; k is a radical ofqinShows the flow gain constant of each joint oil inlet chamber and satisfies
A flow gain constant representing the oil inlet chamber of the ith joint; k is a radical ofqoutShows the flow gain constant of the oil inlet chamber and the oil return chamber, and satisfies
A flow gain constant of an oil return chamber of the i-th joint is represented; gin(Pin，xv) Indicating the spool displacement x of each joint hydraulic control valvevPressure P of oil inlet chamberinValve element displacement transfer function of gout(Pout，xv) Indicating the spool displacement x of each joint hydraulic control valvevAnd return chamber pressure PoutThe spool displacement transfer function of (1) satisfies the following equation:
wherein the content of the first and second substances,
valve element displacement of hydraulic control valve for ith joint
Pressure of oil inlet chamber
The transfer function of the spool displacement of (a),
valve element displacement of hydraulic control valve for ith joint
And pressure of oil return chamber
Valve element displacement transfer function of, PsIs the supply pressure coefficient, P, of the hydraulic pumprIs the reference pressure coefficient of the hydraulic return tank.
3. The extended observer-based underwater hydraulic manipulator nonlinear robust control method according to claim 1, wherein the step 2) is specifically as follows:
2.1) establishing a first nonlinear robust control law P based on the system nonlinear dynamical model established in the step 1)LdThe following formula is satisfied:
PLd＝PLdo+PLds1+PLds2 (11)
wherein, PLdoRepresenting a compensation parameter, P, of the first nonlinear modelLds1Representing a first linear robust parameter, PLds2Representing a first uncertainty compensation parameter;
is a nonlinear dynamics model of a connecting rod mechanical armThe estimation matrix of the medium-sized parameters,
compensating the parameter P for the first non-linear modelLdoIn the parameter estimation matrix
A coefficient regression matrix corresponding to each parameter in (1);
and
respectively an inertia matrix M (q), a Coriolis force matrix and a centrifugal force matrix in the nonlinear dynamics model of the connecting rod mechanical arm
A gravity matrix g (q) and an estimate of an interference matrix D; z is a radical of2Representing the angle conversion error of the underwater hydraulic mechanical arm; k is a radical of2Is a predetermined angle conversion error z2The coefficient feedback gain positive definite matrix; t represents a transpose operation; h isPA compensation parameter representing a first non-linear robust control law; compensation parameter h of first nonlinear robust control lawPIs set to be more than the first nonlinear model compensation parameter PLdoAnd a first linear robust parameter PLds1Is smaller by three or more orders of magnitude;
an error matrix representing uncertainty model parameters satisfying
Theta is a parameter matrix in the nonlinear dynamical model of the connecting rod mechanical arm;
the uncertain nonlinear error parameter of the underwater environment is expressed and satisfied
2.2) robust control law P based on a first non-linearityLdEstablishing a second nonlinear robust control law QLdAnd taking the second nonlinear robust control law as a nonlinear robust control law of the underwater hydraulic mechanical arm, and satisfying the following formula:
QLd＝QLdo+QLds1+QLds2 (15)
wherein Q isLdoRepresenting a second non-linear model compensation parameter, QLds1Representing a second linear robust parameter, QLds2Representing a second uncertainty compensation parameter;
expressed in the second non-linear model compensation parameter QLdoIn the parameter estimation matrix
A coefficient regression matrix corresponding to each parameter in (1); z is a radical of3Representing the pressure parameter error, k, of an underwater hydraulic manipulator3Is a preset pressure parameter error z3The coefficient feedback gain positive definite matrix; oil inlet cavity flow Q of each joint hydraulic cylinderinInvolving hydraulic cylinders of each jointNominal flow Q of the oil chamberinmAnd the error flow of the oil inlet cavity of each joint hydraulic cylinder
Flow Q of oil return cavity of each joint hydraulic cylinderoutNominal flow Q of the return chambers of the hydraulic cylinders comprising each jointoutmAnd error flow rate of oil return chamber
hQ(x) A compensation parameter representing a second non-linear robust control law, a compensation parameter h of the second non-linear robust control lawQ(x) Is set to be more than the second nonlinear model compensation parameter QLdoAnd a second linear robust parameter QLds1Is smaller by three or more orders of magnitude;
representing a first non-linear robust control law PLdDifferential of (2)
A non-calculation section;
2.3) constructing a nonlinear robust controller of the underwater hydraulic mechanical arm based on a system nonlinear dynamical model and a nonlinear robust control law, specifically, inputting a second nonlinear model compensation parameter of the nonlinear robust control law into the system nonlinear dynamical model, adding an output of the system nonlinear dynamical model, the second linear robust parameter and the uncertainty compensation parameter, and outputting the sum as an output of the nonlinear robust controller, and obtaining an output of the nonlinear robust controller by simultaneous equations (6) - (9) and (15), wherein the following relations are satisfied:
and the output of the nonlinear robust controller is transmitted to a hydraulic system of the underwater hydraulic mechanical arm so as to realize control.
4. The extended observer-based underwater hydraulic manipulator nonlinear robust control method according to claim 1, wherein the step 3) is specifically as follows:
establishing an extended observer of the underwater hydraulic mechanical arm, observing the moment of each joint hydraulic driver of the underwater hydraulic mechanical arm and the unmeasured time-varying interference quantity in the system nonlinear dynamic model by the extended observer, and obtaining an angular velocity observation value and an unmeasured time-varying interference quantity observation value of the underwater hydraulic mechanical arm; observed state of extended observer
D is an immeasurable time-varying interference item in a nonlinear dynamics model of the connecting rod mechanical arm, and specifically comprises mechanical motion friction and hydraulic oil resistance interference;
the extended observer is set by the following formula:
wherein the content of the first and second substances,
for the observed value of each state quantity, including the observed value of each joint angle
Observed value of angular velocity of each joint
And non-measurable time-varying interference term observation
Observed values representing the respective state quantities
Differentiation of (1); l is a feedback gain coefficient matrix; y represents the theoretical joint angles of the underwater hydraulic manipulator,
representing the observation values of all joint angles of the underwater hydraulic mechanical arm; u represents the moment of each joint hydraulic actuator, and satisfies
M, N, J, E each represent a first, second, third, fourth matrix of state equation coefficients of the extended observer;
the state equation coefficient matrix M, N, J, E is represented as:
E＝[I 0 0]
wherein I represents an identity matrix.
5. The underwater hydraulic mechanical arm nonlinear robust control method based on the extended observer is characterized in that: the underwater hydraulic mechanical arm is mainly formed by connecting a connecting rod mechanical arm and a hydraulic system, sensors are arranged in the connecting rod mechanical arm and the hydraulic system, and the sensors are used for measuring the state of the underwater hydraulic mechanical arm and transmitting the state to the nonlinear robust controller and the extended observer.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110646833.5A CN113325716B (en) | 2021-06-10 | 2021-06-10 | Underwater hydraulic mechanical arm nonlinear robust control method based on extended observer |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110646833.5A CN113325716B (en) | 2021-06-10 | 2021-06-10 | Underwater hydraulic mechanical arm nonlinear robust control method based on extended observer |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113325716A true CN113325716A (en) | 2021-08-31 |
| CN113325716B CN113325716B (en) | 2022-03-29 |
Family
ID=77420413
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110646833.5A Active CN113325716B (en) | 2021-06-10 | 2021-06-10 | Underwater hydraulic mechanical arm nonlinear robust control method based on extended observer |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113325716B (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11801600B1 (en) * | 2021-10-26 | 2023-10-31 | East China Jiaotong University | Terminal force soft-sensing method of hydraulic manipulator |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5049796A (en) * | 1989-05-17 | 1991-09-17 | The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration | Robust high-performance control for robotic manipulators |
| US20060116783A1 (en) * | 2004-12-01 | 2006-06-01 | Canadian Space Agency | Method and system for torque/force control of hydraulic actuators |
| CN105045101A (en) * | 2015-06-08 | 2015-11-11 | 浙江工业大学 | Mechanical arm servo system full-order slide mold control method based on extended state observer |
| CN108628172A (en) * | 2018-06-25 | 2018-10-09 | 南京理工大学 | A kind of mechanical arm high-precision motion control method based on extended state observer |
| CN110744541A (en) * | 2019-10-08 | 2020-02-04 | 哈尔滨工程大学 | Vision-guided underwater mechanical arm control method |
| CN111319036A (en) * | 2018-12-15 | 2020-06-23 | 天津大学青岛海洋技术研究院 | Self-adaptive algorithm-based mobile mechanical arm position/force active disturbance rejection control method |
| CN111958606A (en) * | 2020-09-10 | 2020-11-20 | 浙江协力机器人技术有限公司 | Distributed robust tracking control method applied to multi-degree-of-freedom mechanical arm |
| CN112643673A (en) * | 2020-12-14 | 2021-04-13 | 山东大学 | Mobile mechanical arm robust control method and system based on non-linear disturbance observer |
| CN112859600A (en) * | 2021-01-08 | 2021-05-28 | 航天时代飞鸿技术有限公司 | Mechanical system finite time control method based on extended state observer |
-
2021
- 2021-06-10 CN CN202110646833.5A patent/CN113325716B/en active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5049796A (en) * | 1989-05-17 | 1991-09-17 | The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration | Robust high-performance control for robotic manipulators |
| US20060116783A1 (en) * | 2004-12-01 | 2006-06-01 | Canadian Space Agency | Method and system for torque/force control of hydraulic actuators |
| CN105045101A (en) * | 2015-06-08 | 2015-11-11 | 浙江工业大学 | Mechanical arm servo system full-order slide mold control method based on extended state observer |
| CN108628172A (en) * | 2018-06-25 | 2018-10-09 | 南京理工大学 | A kind of mechanical arm high-precision motion control method based on extended state observer |
| CN111319036A (en) * | 2018-12-15 | 2020-06-23 | 天津大学青岛海洋技术研究院 | Self-adaptive algorithm-based mobile mechanical arm position/force active disturbance rejection control method |
| CN110744541A (en) * | 2019-10-08 | 2020-02-04 | 哈尔滨工程大学 | Vision-guided underwater mechanical arm control method |
| CN111958606A (en) * | 2020-09-10 | 2020-11-20 | 浙江协力机器人技术有限公司 | Distributed robust tracking control method applied to multi-degree-of-freedom mechanical arm |
| CN112643673A (en) * | 2020-12-14 | 2021-04-13 | 山东大学 | Mobile mechanical arm robust control method and system based on non-linear disturbance observer |
| CN112859600A (en) * | 2021-01-08 | 2021-05-28 | 航天时代飞鸿技术有限公司 | Mechanical system finite time control method based on extended state observer |
Non-Patent Citations (1)
| Title |
|---|
| 黄洪昌 等: "基于观测器的机械臂执行器故障诊断方法研究", 《测控技术》 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11801600B1 (en) * | 2021-10-26 | 2023-10-31 | East China Jiaotong University | Terminal force soft-sensing method of hydraulic manipulator |
| US20230356390A1 (en) * | 2021-10-26 | 2023-11-09 | East China Jiaotong University | Terminal force soft-sensing method of hydraulic manipulator |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113325716B (en) | 2022-03-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111152225B (en) | Uncertain mechanical arm fixed time trajectory tracking control method with input saturation | |
| CN105772917B (en) | A kind of three joint spot welding robot's Trajectory Tracking Control methods | |
| Zhou et al. | Adaptive robust control design for underwater multi-dof hydraulic manipulator | |
| CN108267952B (en) | Self-adaptive finite time control method for underwater robot | |
| CN110116409B (en) | Four-channel teleoperation bilateral control method based on disturbance observer | |
| CN110794678B (en) | Four-channel teleoperation force feedback control method under hysteresis nonlinearity limitation | |
| CN110456646B (en) | Design method of anchor rod drilling machine drill arm swing angle self-adaptive sliding mode control system based on EKF | |
| CN110666794B (en) | Method for compensating load force and moment of end tool of robot | |
| CN110259879B (en) | Powerless feedback vibration isolation control method and system for electric Stewart structure | |
| CN112067217B (en) | Active-disturbance-rejection control method for waveform reproduction of high-order uniaxial earthquake simulation vibration table | |
| CN113325716B (en) | Underwater hydraulic mechanical arm nonlinear robust control method based on extended observer | |
| CN114750167B (en) | Industrial robot joint friction compensation method based on disturbance observation | |
| CN112269317A (en) | Bilateral teleoperation control method based on extended Kalman filter | |
| CN113219841B (en) | Nonlinear control method for underwater multi-joint hydraulic mechanical arm based on adaptive robustness | |
| CN107765548A (en) | Flat pad high-precision motion control method based on double observers | |
| CN108594656B (en) | High-precision anti-interference continuous sliding mode control method for bilateral lifting robot system | |
| WO2023020036A1 (en) | Redundant manipulator tracking control method based on echo state network | |
| CN112223276B (en) | Multi-joint robot control method based on adaptive neural network sliding mode control | |
| CN116339141A (en) | Mechanical arm global fixed time track tracking sliding mode control method | |
| CN109814393B (en) | Dimension reduction observer and controller design method of suspended floating object follow-up system | |
| Zhang et al. | An adaptive robust controller for hydraulic robotic manipulators with a flow-mapping compensator | |
| CN114800522B (en) | Hydraulic mechanical arm contact operation self-adaptive impedance control system without end force sensor | |
| CN117250971B (en) | Control method of microminiature AUV | |
| CN117289612B (en) | Hydraulic mechanical arm self-adaptive neural network control method | |
| CN115343957B (en) | Robust control method of time delay master-slave mechanical arm system based on force observer |
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 | ||
| GR01 | Patent grant |