CN113325716B - 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
- CN113325716B CN113325716B CN202110646833.5A CN202110646833A CN113325716B CN 113325716 B CN113325716 B CN 113325716B CN 202110646833 A CN202110646833 A CN 202110646833A CN 113325716 B CN113325716 B CN 113325716B
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 43
- 238000005312 nonlinear dynamic Methods 0.000 claims abstract description 31
- 238000005259 measurement Methods 0.000 claims abstract description 6
- 239000003921 oil Substances 0.000 claims description 125
- 239000011159 matrix material Substances 0.000 claims description 90
- 238000006073 displacement reaction Methods 0.000 claims description 35
- 239000010720 hydraulic oil Substances 0.000 claims description 17
- 230000033001 locomotion Effects 0.000 claims description 16
- 239000000126 substance Substances 0.000 claims description 14
- 238000012546 transfer Methods 0.000 claims description 11
- 230000001133 acceleration Effects 0.000 claims description 9
- 238000006243 chemical reaction Methods 0.000 claims description 9
- 230000005484 gravity Effects 0.000 claims description 7
- 238000004364 calculation method Methods 0.000 claims description 5
- 241001123248 Arma Species 0.000 claims description 3
- 230000017105 transposition Effects 0.000 claims description 3
- 230000008569 process Effects 0.000 description 13
- 238000004088 simulation Methods 0.000 description 6
- 230000000694 effects Effects 0.000 description 5
- 230000001052 transient effect Effects 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 230000004044 response Effects 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 238000013016 damping Methods 0.000 description 2
- 230000004069 differentiation Effects 0.000 description 2
- 230000007246 mechanism Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 239000012636 effector Substances 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000009472 formulation Methods 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 239000000203 mixture Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
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
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Feedback Control In General (AREA)
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 the extension amount of the push rod of the hydraulic actuator of each joint angle and the corresponding joint meets the following relationComprises the following steps:
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 metThe 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 metRepresenting 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 satisfiesPinThe oil pressure of an oil inlet cavity of each joint hydraulic cylinder is shown, and the requirement of oil pressureThe 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 pressureThe 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 metThe 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 metVoutThe 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 metAndrespectively 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 metThe 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 satisfiesA 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 satisfiesA 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 jointPressure of oil inlet chamberThe transfer function of the spool displacement of (a),valve element displacement of hydraulic control valve for ith jointAnd pressure of oil return chamberValve 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 matrixA coefficient regression matrix corresponding to each parameter in (1);andrespectively an inertia matrix M (q), a Coriolis force matrix and a centrifugal force matrix in the nonlinear dynamics model of the connecting rod mechanical armA 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 satisfyingTheta 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 (29)
wherein Q isLdoRepresenting a second non-linear model compensation parameter, QLds1Representing a second linear robust parameter, QLds2Representing a second uncertainty compensation parameter;is shown inTwo nonlinear model compensation parameters QLdoIn the parameter estimation matrixA 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 cylinderFlow 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 chamberhQ(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 observerD 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 angleAngular velocity of each jointDegree observation valueAnd non-measurable time-varying interference term observationObserved values representing the respective state quantitiesDifferentiation 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:
underwater liquidEach joint angle q of the 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 metThe 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 metRepresenting 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 satisfiesPinThe oil pressure of an oil inlet cavity of each joint hydraulic cylinder is shown, and the requirement of oil pressureThe 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 pressureThe 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 Of hydraulic cylinders representing the ith joint of an underwater hydraulic manipulatorThe area of the oil inlet cavity; a. theoutThe area of an oil return cavity of each joint hydraulic cylinder is shown, and the requirement is metThe 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 metVoutThe 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 metAndrespectively 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 flowThe flow of an oil inlet cavity of a hydraulic cylinder of the ith joint of the underwater hydraulic mechanical arm is represented; qoutRepresents the ith of the underwater hydraulic mechanical armThe flow of an oil return cavity of a hydraulic cylinder of each joint is satisfiedThe 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 satisfiesA 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 satisfiesA flow gain constant of an oil return chamber of the i-th joint is represented; gin(Pin,xv) Valve core position for indicating each joint hydraulic control valveMoving xvPressure 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 jointPressure of oil inlet chamberThe transfer function of the spool displacement of (a),valve element displacement of hydraulic control valve for ith jointAnd pressure of oil return chamberDisplacement of the valve coreConversion function, 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 parameterEstimating a matrixA coefficient regression matrix corresponding to each parameter in (1);andrespectively an inertia matrix M (q), a Coriolis force matrix and a centrifugal force matrix in the nonlinear dynamics model of the connecting rod mechanical armA 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 (i.e., 10-3) or more orders of magnitude;an error matrix representing uncertainty model parameters satisfyingTheta 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:
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 compensation parameter 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 armThe 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 matrixThe 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 a first nonlinear robust control law compensation parameter, a first nonlinear 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 satisfyingTheta is a parameter matrix in the nonlinear dynamical model of the connecting rod mechanical arm;an uncertain nonlinear error parameter representing underwater environment is satisfiedFirst uncertainty compensation parameter P satisfying equation (21)Lds2The method can ensure that the first nonlinear robust control law has parameter uncertainty and uncertainty nonlinearityThe performance can be kept well.
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 matrixA 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 cylinderOf hydraulic cylinders in each jointFlow Q of oil return chamberoutNominal flow Q of the return chambers of the hydraulic cylinders comprising each jointoutmAnd error flow rate of oil return chamberhQ(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 nonlinear robust control 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:
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,the observation value of the angular acceleration of each joint of the mechanical arm is calculated according to the nonlinear dynamics model of the connecting rod mechanical arm.
wherein the content of the first and second substances,andrespectively represent the estimation errors of each angular velocity and each angular acceleration, and respectively satisfyAnd
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 cylinderFlow 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 modelCompensation 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 matrixA 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 observerD 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 angleObserved value of angular velocity of each jointAnd non-measurable time-varying interference term observationObserved values representing the respective state quantitiesDifferentiation 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 satisfiesM, 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 (4)
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) feeding back the angular velocity observation value, the unmeasured time-varying interference quantity observation value and a tracking error value obtained by measurement of a sensor to a nonlinear robust controller in real time, and controlling the underwater hydraulic mechanical arm by the nonlinear robust controller to form a complete closed-loop control system of the underwater hydraulic mechanical arm, so that 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 satisfiesPinThe 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. theoutHydraulic cylinder for indicating each jointThe area of the oil return cavity meets 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 metVoutThe 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 Andrespectively 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; qinIndicating each joint fluidThe flow of the oil inlet cavity of the pressure cylinder meets the requirement 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 jointPressure of oil inlet chamberThe transfer function of the spool displacement of (a),valve element displacement of hydraulic control valve for ith jointAnd pressure of oil return chamberValve 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. 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 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 matrixA coefficient regression matrix corresponding to each parameter in (1);andrespectively an inertia matrix M (q), a Coriolis force matrix and a centrifugal force matrix in the nonlinear dynamics model of the connecting rod mechanical armA 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; first non-linear robust controlCompensation parameter h of 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 satisfyingTheta 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 matrixA 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 cylinderFlow 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 chamberhQ(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:
xv=(AinVin -1kqingin(Pin,xv)+AoutVout -1kqoutgout(Pout,xv))-1QLd (19)
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. 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 observerD 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 angleObserved value of angular velocity of each jointAnd non-measurable time-varying interference term observation Observed values representing the respective state quantitiesDifferentiation 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.
4. 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 CN113325716A (en) | 2021-08-31 |
CN113325716B true 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) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113977578B (en) * | 2021-10-26 | 2022-10-18 | 华东交通大学 | Soft measurement method for end force of hydraulic mechanical arm |
Family Cites Families (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 |
US7403826B2 (en) * | 2004-12-01 | 2008-07-22 | Canadian Space Agency | Method and system for torque/force control of hydraulic actuators |
CN105045101B (en) * | 2015-06-08 | 2018-06-12 | 浙江工业大学 | A kind of mechanical arm servo-drive system full-order sliding mode control method based on extended state observer |
CN108628172B (en) * | 2018-06-25 | 2021-05-07 | 南京理工大学 | Mechanical arm high-precision motion control method based on extended state observer |
CN111319036B (en) * | 2018-12-15 | 2023-03-14 | 天津大学青岛海洋技术研究院 | 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 |
CN112643673B (en) * | 2020-12-14 | 2022-05-03 | 山东大学 | Mobile mechanical arm robust control method and system based on nonlinear disturbance observer |
CN112859600B (en) * | 2021-01-08 | 2024-02-09 | 航天时代飞鸿技术有限公司 | Mechanical system finite time control method based on extended state observer |
-
2021
- 2021-06-10 CN CN202110646833.5A patent/CN113325716B/en active Active
Also Published As
Publication number | Publication date |
---|---|
CN113325716A (en) | 2021-08-31 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111152225B (en) | Uncertain mechanical arm fixed time trajectory tracking control method with input saturation | |
Zhou et al. | Adaptive robust control design for underwater multi-dof hydraulic manipulator | |
CN110456646B (en) | Design method of anchor rod drilling machine drill arm swing angle self-adaptive sliding mode control system based on EKF | |
CN110794678B (en) | Four-channel teleoperation force feedback control method under hysteresis nonlinearity limitation | |
CN110116409B (en) | Four-channel teleoperation bilateral control method based on disturbance observer | |
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 | |
CN113325716B (en) | Underwater hydraulic mechanical arm nonlinear robust control method based on extended observer | |
CN112925208A (en) | Disturbance compensation method for data-driven electro-hydraulic servo system of well drilling machine | |
CN114750167B (en) | Industrial robot joint friction compensation method based on disturbance observation | |
CN110962129A (en) | Impedance control method for mechanical arm | |
CN113799136A (en) | Robot joint high-precision control system and method based on full-state feedback | |
CN112269317A (en) | Bilateral teleoperation control method based on extended Kalman filter | |
CN116339141A (en) | Mechanical arm global fixed time track tracking sliding mode control method | |
CN113219841B (en) | Nonlinear control method for underwater multi-joint hydraulic mechanical arm based on adaptive robustness | |
CN117250971B (en) | Control method of microminiature AUV | |
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 | |
CN112223276A (en) | Multi-joint robot control method based on adaptive neural network sliding mode control | |
WO2023020036A1 (en) | Redundant manipulator tracking control method based on echo state network | |
CN115473467A (en) | Flexible joint mechanical arm instruction filtering backstepping control method based on fuzzy observer | |
CN109814393B (en) | Dimension reduction observer and controller design method of suspended floating object follow-up system | |
CN111762688B (en) | Bridge crane controller generation method, control method and controller generation 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 |
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 |