CN106054599B - A kind of delay control method of master-slave mode submarine mechanical arm - Google Patents
A kind of delay control method of master-slave mode submarine mechanical arm Download PDFInfo
- Publication number
- CN106054599B CN106054599B CN201610352534.XA CN201610352534A CN106054599B CN 106054599 B CN106054599 B CN 106054599B CN 201610352534 A CN201610352534 A CN 201610352534A CN 106054599 B CN106054599 B CN 106054599B
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- hand
- joint
- moment
- control
- 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
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
Abstract
The present invention relates to robotic technology fields, providing one kind can guarantee master-slave mode submarine mechanical arm operating environment is severe, under transmission communication time delay condition, realizes the delay control method of the main hand of submarine mechanical arm with the master-slave mode submarine mechanical arm of the coordinated control from hand.The present invention includes: the structural parameters of given submarine mechanical arm, according to the kinematics model of submarine mechanical arm, carries out time-delay analysis to principal and subordinate's mechanical arm；Hydrodynamic factor is acquired, according to submarine mechanical arm kinetic model, verifies whether mechanical arm performance meets model requirements；Delay process is carried out to main hand action signal, preceding n moment joint control signal is buffered in underwater manipulator, multiple power curve approximating method is then based on and calculates from the joint ideal movements position at hand n+1 moment.The present invention solves the problems such as submarine mechanical arm operating environment is severe, submarine mechanical arm work discontinuity caused by transmission communication delay.
Description
Technical field
The present invention relates to robotic technology fields, and providing one kind can guarantee master-slave mode submarine mechanical arm in operation ring
Border is severe, under transmission communication time delay condition, realizes the master-slave mode submarine mechanical of the main hand of submarine mechanical arm with the coordinated control from hand
The delay control method of arm.
Background technique
Due to underwater environment complexity, there are the environment that seawater pressure, low visibility, temperature decline etc. are unfavorable for human work
Condition can replace the mankind in the hazardous environments work such as deep-sea with principal and subordinate's multifunctional underwater tool arm coordinating operation type underwater robot
The work such as work, submarine mechanical arm can carry out sea floor exploration exploitation, and submarine pipeline repairs, and petroleum pipeline paving is built.Submarine mechanical arm is not
The danger for only reducing mankind's underwater operation operation also meets the needs of mankind extend perception, explore to tera incognita.Therefore it sets
Counting principal and subordinate's submarine mechanical arm control system has important research and engineering significance.
Many methods are proposed in terms of the motion control of many experts mechanical arm under water.Switch control is submarine mechanical arm
Most simple is also most ancient control mode, and water controller is made of operating platform switch button, and surface operations personnel pass through
The switch of operation corresponding joint opens and closes to control the movement of submarine mechanical arm from each joint of hand, and operator is taken the photograph under water by observation
The image information that is transmitted back to as device judges the movement position from swivel of hand, it can be seen from the above, submarine mechanical arm switch controlling party
Formula is opened loop control, and the slave hands movement speed of switch control is fixed, so control precision depends on operator's
Micro-judgment, underwater operation limited efficacy, and multi-joint are larger to operator's difficulty when assisting to control.
Compared to switch control, the great advantage of speed control is exactly the controllability of joint motions speed.Under ideal conditions,
The process for first accelerating, slowing down at the uniform velocity, again again is passed through in joint certainly from initial position to target position, then the joint of switch control
Movement velocity is fixed, and is difficult accurately to be parked in target position finally in this way, and speed control can efficiently adjust joint fortune
It is dynamic.Speed control method controls each joint oil liquid flow by ratio clack box to control movement speed of the mechanical arm from each joint of hand
Degree.But speed control and switch control have the shortcomings that one it is common, that is, to the more demanding of surface operations personnel.Speed
Using operating stick as water controller, the operation direction of operating stick may different, such as large arm from the direction of motion in joint for control
Joint is pitching movement up and down, operating stick operation direction is side-to-side movement, will increase operator's workload in this way, and cannot
Accomplish the speed for controlling multiple joints simultaneously.Although speed control solves switch control and asks from hands movement precision is not high
Topic, but still do not solve the problems, such as that operator manipulates difficult in practical control.
As technology develops, consider first two control mode the drawbacks of, researcher's design position feedback control mode,
Also referred to as master-slave control method carries out position feedback from hand scaled down version model by being modified to operating stick, so that operator
It can visualize and learn from hand position information, without seemingly obtaining as switch control and speed control and constructing manipulator motion in brains
Model, in this way can be to avoid the damage operated to mechanical arm, and facilitates operator while controlling multiple joint motions.Principal and subordinate
Control mode makes operator have the feeling of robot arm one when operating submarine mechanical arm, effectively improves operator's work effect
Rate.And position feedback control also has the advantages that speed control, the speed command issued by operating main hand, can also will be fast
Degree instruction value control device is integrated to be transformed to location information, carries out position feedback control.
But under water in the practical control of operation, in deep ocean work, the main hand control signal of mechanical arm passes submarine mechanical arm
Defeated distance is remote, and 485 traditional bus communication transmission speeds are slow, so the transmission delay time is long, results in receiving from hand in this way and control
Signal lag processed occurs the bad phenomenons such as lag, descontinuous motion at work from hand, influences water in surface operations, submarine mechanical arm
Face operator is to the judgement from hand position, so that the operation difficulty of operator be made to increase, and then influences to complete subsea tasks
Operating efficiency.
Summary of the invention
It is an object of that present invention to provide a kind of form is simple, calculation amount is reduced, optimization solution can be obtained as needed, is had logical
With the delay control method of the master-slave mode submarine mechanical arm of property and rapidity.
The object of the present invention is achieved like this:
The present invention the following steps are included:
(1) structural parameters for giving submarine mechanical arm, according to the kinematics model of submarine mechanical arm, to principal and subordinate's mechanical arm into
Row time-delay analysis；
(2) hydrodynamic factor is acquired, according to submarine mechanical arm kinetic model, verifies whether mechanical arm performance meets model
Demand；
(3) delay process is carried out to main hand action signal, preceding n moment joint control signal is buffered in underwater manipulator
In, it is then based on multiple power curve approximating method and calculates from the joint ideal movements position at hand n+1 moment；
(4) the analysis main hand of mechanical arm is dry with the presence or absence of ocean current from hand with the presence or absence of emergency stop deflecting emergency operation and mechanical arm
Factor is disturbed, by the judgement of delays time to control link and ocean current feedback compensation link, realizes the stable and continuous control of principal and subordinate's submarine mechanical arm
System；
(5) control system model is established according to kinematics model, kinetic model and delays time to control algorithm, and being based on should
Model is emulated, and the validity and reliability of delay control method are verified.
Submarine mechanical arm kinetic model described in step 2 is with newton-Euler when not considering hydrodynamic factor
Equation carries out Dynamic Modeling to mechanical arm, and wherein submarine mechanical arm is at any movement moment, by each joint angle of mechanical arm
Spend variable qi, speed variablesAcceleration variableI=1 ... n resolves the torque variable Q in each joint as known conditionsi,
I=1 ... n iterates to calculate forward the velocity and acceleration of each connecting rod of mechanical arm according to Newton-Euler Method, wherein base first
Seat initial motion state is determining, if pedestal is fixed, basis coordinates angular speed0ω0=0, angular accelerationAccording to ox
- Euler method iterate to calculate backward machinery arm each joint power and torque；Primary condition is torquePowerSubmarine mechanical arm distal point can be move freely in mechanical arm working space, then Mend、FendIt is zero；
(2.1) joint angles variable, speed variables of each connecting rod of Iterative mechanical arm forward:
ivci=ivi+iωi×iρi
Wherein,i+1ωi+1For the angular speed in i+1 joint；For the angular acceleration in i+1 joint；RI+1, iFor
The spin matrix that i-th of joint is shut down to i+1；For the linear acceleration in i+1 joint；ivciFor i-th joint
Linear acceleration at mass center；For the angular speed in i+1 joint itself；ieiFor the rotation axis in i-th of joint；i+1li+1For
The length vector of i+1 connecting rod；iρiFor position vector of the i coordinate system at i connecting rod mass center；cIi+1It is arrived for i+1 joint
Inertial tensor at center-of-mass coordinate origin；mi+1For the quality in i+1 joint；i+1Fi+1For the inertia force in i+1 joint；i+ 1Mi+1For the moment of inertia in i+1 joint；
(2.2) the power F of Iterative joint of mechanical arm backwardiWith torque QiI=n, n-1 ..., 1:
Wherein,ihiFor i coordinate system to the position vector of i connecting rod stress point；For the joint power in i-th of joint；For
Joint moment of i-th of joint relative to i coordinate system；QM iJoint moment for i-th of joint relative to i-1 coordinate system；
When the weight in robot linkage joint is as Consideration, ifWhat fixed robot base was subject to
The quite upward gravity acceleration g of supporting role, consider hydrodynamic factor when, select auspicious thunder dissipative function describe viscous damping and
The motion conditions of object under fluid resistance:
Auspicious thunder dissipative function is introduced in lagrangian dynamics algorithm:
The Lagrange's equation with auspicious thunder dissipative function can be obtained:
Wherein qsFor broad sense position vector,For corresponding velocity vector, T is Lagrangian；
Since newton-Euler's method finds out the torque without hydrodynamic force variable
Then
To six degree of freedom submarine mechanical arm deriving analysis, each joint is expressed as relative to the speed in previous jointi-1vi(i=
1,2,3.....6), according to rigid body fixed-axis rotation theorem, each joint translational velocity is obtained, and then obtain the center of mass point in each joint
Move absolute velocity0vi(i=1,2,3.....6)；
Solve the dissipative function φ in each joint of mechanical armi(i=1,2 ... ..6),
The dissipative function for obtaining submarine mechanical arm system is φ=φ1+φ2+φ3+φ4+φ5+φ6, by auspicious thunder dissipation letter
Number is brought Lagrange's equation into and is derived, the general velocity of the auspicious thunder dissipative function φ of derivationParameter substitutes into equationHydrodynamic damping coefficient can be acquired.
Power curve approximating method described in step 3 includes: that curve matching is limited to test data by experiment acquisition
(xi,yi), approximate function y=f (x) is sought using these data, wherein x is output quantity, and y is measurand, delay control
The time is output quantity in system, and joint angles are measurand；Curve-fitting method is applied to the delay control of principal and subordinate's submarine mechanical arm
In system, the time delay process time is set as two parameter periods, this method is known as the delay of two periods, in addition due to 485 buses
The delay of signal caused by transmission, from three periodic motions of main hand of the lag of hand, at the main hands movement i+3 moment, due to 485 communication letters
Number delay, i+3 moment underwater manipulator just received the main moment position hand i+2, and controller learns the location point at three moment,
I.e. the slave hand position at i+3 moment, the main hand at i+1 moment control signal, the main hand at i+2 moment control signal, according to joint position
Temporal information judges the motion change trend from hand in advance, to calculate lower a period of time using two period delay control methods
It carves from hand ideal movements point；
Wherein main swivel of hand i is in tk-2When control angle value be θk-2, in tk-1When control angle value be θk-1, exist from swivel of hand
tkMoment actual angle value is θk, obtaining coordinate is respectively (tk-2,θk-2)、(tk-1,θk-1) and (tk,θk), it is transported according to joint curve
Dynamic fit equation, obtains tk+1The angle predicted value at moment is θk+1:
θk+1=θk+v(k)×T+aT2/2
Wherein, a is the angular acceleration in joint, and T is transmission cycle 0.1s；V (k) is k moment joint i velocity of rotation；
It is obtained by uniform acceleration equation:
It is calculated, obtains tk+1The ideal value of the slave swivel of hand angle at moment
Finally subsequent time is calculated from swivel of hand angle and the main swivel of hand angle at two moment of delay by current time
From swivel of hand speed, angular speed, to obtain ideal movement angle.
Delays time to control link described in step (4) includes: to analyze main hands movement curve, to judge mechanical arm master
Hand whether there is deflecting, accelerate, the mutation movement link of deceleration, and then carry out motion control to from hand；
Wherein kth walks the differential seat angle between main hand output target value x (k) and main hand output target value x (k+1) of+1 step of kth
For e (k)=x (k+1)-x (k), discriminatory analysis is carried out to the differential seat angle, according to the difference of differential seat angle range, is controlled from subordinate one
How step moves:
When | e (k) | at < 0.2 °, main hand control variation is not present emergency operation, prolongs in out of hands movement limit of power
Shi Huanjie is worked normally, and obtains the ideal movements point from hand subsequent time using multicycle delay algorithm；
When | e (k) | at > 0.2 °, differential seat angle exceeds mechanical arm calibration capability range at this time, and emergency stop avoidance fortune occurs in main hand
It is dynamic；Mechanical arm stop motion keeps current location, until main hand signal returns to after hand halted state from newly, restart to transport
It is dynamic.
Ocean current feedback compensation link mainly judges whether there is ocean current interference and impacts to mechanical arm from hands movement, if
Ocean currents have exceeded mechanical arm correction of movement limit of power, it is believed that ocean current interference effect is excessive, stops from hand mechanical arm current
Movement, until the main hand of mechanical arm returns to mechanical arm from hand stop position from newly, mechanical arm continues to follow main hands movement from hand；If
Ocean currents are smaller, influence in manipulator motion limit of power, mechanical arm carries out appropriate position according to main hand target position
Correction is set, then carries out time delay process control, so that mechanical arm is from hand smooth motion；
Wherein kth walk main swivel of hand output target value x (k) with from swivel of hand actual motion positionBetween error beDiscriminatory analysis is carried out to the error, according to the difference of error range, determines how to move after hand；
When | e (k) | slave hand output valve at < 0.02 °, after correctionWithout correction, holding is currently transported from hand
Dynamic state；
When 0.02 ° < | e (k) | slave hand output valve at < 0.2 °, after correctionError is in mechanical arm
In correcting range ability, it is corrected to from hand current location；
When | e (k) | at > 0.2 °, from hand output valve after correctionError exceeds mechanical arm calibration capability model
It encloses, mechanical arm stop motion, keeps current location, until main hand signal returns to after hand halted state from newly, restart to transport
It is dynamic.
The beneficial effects of the present invention are:
By combining the design feature and working method of principal and subordinate's mechanical arm, delay control method is introduced into the underwater machine of principal and subordinate
In the motion control of tool arm, this method intuitively can quickly realize the smooth steady control of master-slave mode submarine mechanical arm.It is based on
The delays time to control algorithm of power curve fitting carries out ideal joint position to from hand by way of to known position information successive ignition
Set determination, this method can effectively be realized smooth from mechanical arm motion profile under water, and ocean current interference and misoperation is avoided to cause
Motion jitter and pause.Based on the delay control method of principal and subordinate's mechanical arm, the robot control to work for remote tele-operation mode
Method provides reference, has certain versatility and practicability.The final present invention solve submarine mechanical arm operating environment it is severe,
The problems such as the work discontinuity of submarine mechanical arm caused by transmission communication is delayed.This method form is succinct, have very high precision and
Solving speed meets the operation needs of submarine mechanical arm curve movement smooth steady.
Detailed description of the invention
The control system flow chart of Fig. 1 principal and subordinate's mechanical arm；
The rod piece relational graph of Fig. 2 submarine mechanical arm；
Fig. 3 curve-fitting method schematic diagram；
The correction judgement of Fig. 4 ocean current；
Fig. 5 Simulink control system figure；
Fig. 6 submarine mechanical arm configuration schematic diagram；
Fig. 7 ocean current interference simulation figure；
Principal and subordinate's mechanical arm two period of simple joint delays time to control analogous diagram of Fig. 8 ocean current interference.
Specific embodiment
The present invention is described further with reference to the accompanying drawing.
The present invention relates to principal and subordinate's submarine mechanical arm Control System Design field, a kind of master-slave mode submarine mechanical arm is provided
Delay control method.The present invention is started with the time-delay analysis of master-slave mode submarine mechanical arm, the delays time to control being fitted using power curve
Algorithm determines the process of master-slave mode mechanical arm coordinated control, anti-by the foundation of kinematics model, delays time to control link and ocean current
The correction of feedback link, dynamics check are quickly found out submarine mechanical arm from each joint optimization of hand position, can guarantee master-slave mode water
Lower mechanical arm is operating environment is severe, under transmission communication time delay condition, realizes the main hand of submarine mechanical arm and the coordinated control from hand.
The versatility problem for the master-slave mode mechanical arm coordinated control that the present invention solves does not depend on robot configuration, and form is simple, reduces
The difficulty of solution reduces calculation amount, can obtain optimization solution as needed, have versatility and rapidity, meets and appoints to setting the goal
Business and requirement of the environment to the coordinated control of master-slave mode submarine mechanical arm.
Step 1: the structural parameters of given submarine mechanical arm establish the kinematics model of mechanical arm, while calculating main hand
To the communication delay time from hand.
Step 2: considering hydrodynamic factor, submarine mechanical arm kinetic model is established, whether to verify mechanical arm performance
Meet model requirements, and can be applied in practical control operation.
Step 3: delay process being carried out to main hand action signal, preceding n moment joint control signal caching is controlled under water
In device processed, it is then based on multiple power curve approximating method and calculates from the joint ideal movements position at hand n+1 moment.
Step 4: the analysis main hand of mechanical arm whether there is with the presence or absence of the emergency operations such as emergency stop deflecting and mechanical arm from hand
The factors such as ocean current interference realize that principal and subordinate's submarine mechanical arm is flat by the judgement of delays time to control link and ocean current feedback compensation link
Steady continuous control.
Step 5: control system model can establish according to kinematics model, kinetic model and delays time to control algorithm,
And emulated based on this model, verify the validity and reliability of delay control method.
Embodiment 1, in conjunction with attached drawing 1, the method for the present invention includes following steps:
Step 1: the structural parameters of given submarine mechanical arm establish the kinematics model of mechanical arm, while calculating main hand
To the communication delay time from hand.
Step 2: considering hydrodynamic factor, submarine mechanical arm kinetic model is established, whether to verify mechanical arm performance
Meet model requirements, and can be applied in practical control operation.
Step 3: delay process being carried out to main hand action signal, preceding n moment joint control signal caching is controlled under water
In device processed, it is then based on multiple power curve approximating method and calculates from the joint ideal movements position at hand n+1 moment.
Step 4: the analysis main hand of mechanical arm whether there is with the presence or absence of the emergency operations such as emergency stop deflecting and mechanical arm from hand
The factors such as ocean current interference realize that principal and subordinate's submarine mechanical arm is flat by the judgement of delays time to control link and ocean current feedback compensation link
Steady continuous control.
Step 5: control system model can establish according to kinematics model, kinetic model and delays time to control algorithm,
And emulated based on this model, verify the validity and reliability of delay control method.
Embodiment 2 establishes the kinetic model of submarine mechanical arm in conjunction with attached drawing 2.Hydrodynamic factor is not considered first, is transported
Dynamic Modeling is carried out to mechanical arm with newton-Eulerian equation.
Submarine mechanical arm is at any movement moment, by each joint angles variable q of mechanical armi, speed variablesAcceleration
VariableI=1 ... n resolves the torque variable Q in each joint as known conditionsi, i=1 ... n.According to newton-Euler
Method iterates to calculate forward the velocity and acceleration of each connecting rod of mechanical arm first.Assuming that pedestal initial motion state be it is determining,
If pedestal is fixed,0ω0=0,Iterate to calculate each joint of mechanical arm backward according to Newton-Euler Method later
Power and torque.Primary condition is Assuming that submarine mechanical arm distal point can be empty in mechanical arm work
In move freely, then Mend、FendIt is zero.
(1) q of each connecting rod of Iterative mechanical arm forwardi、
ivci=ivi+iωi×iρi (4)
Wherein:i+1ωi+1For the angular speed in i+1 joint；
For the angular acceleration in i+1 joint；
RI+1, iThe spin matrix to shut down for i-th of joint to i+1；
For the linear acceleration in i+1 joint；
ivciFor the linear acceleration at the mass center in i-th of joint；
For the angular speed in i+1 joint itself；
ieiFor the rotation axis in i-th of joint；
i+1li+1For the length vector of i+1 connecting rod；
iρiFor position vector of the i coordinate system at i connecting rod mass center；
cIi+1For inertial tensor of the i+1 joint at center-of-mass coordinate origin；
mi+1For the quality in i+1 joint；
i+1Fi+1For the inertia force in i+1 joint；
i+1Mi+1For the moment of inertia in i+1 joint.
(2) the power F of Iterative joint of mechanical arm backwardiWith torque QiI=n, n-1 ..., 1:
Wherein:ihiFor i coordinate system to the position vector of i connecting rod stress point；
For the joint power in i-th of joint；
Joint moment for i-th of joint relative to i coordinate system；
QM iJoint moment for i-th of joint relative to i-1 coordinate system.
When the weight in robot linkage joint is as Consideration, can setFixed robot base by
The quite upward gravity acceleration g of supporting role.It handles so just the same with the influence of each module gravity.There is above-mentioned derivation
Manipulator Dynamic can be obtained.
When considering hydrodynamic factor, auspicious thunder dissipative function is selected to describe the movement feelings of object under viscous damping and fluid resistance
Condition.The specific auspicious thunder dissipative function process for solving mechanical arm is as follows:
Auspicious thunder dissipative function is introduced in lagrangian dynamics algorithm
The Lagrange's equation with auspicious thunder dissipative function can be obtained
Wherein qsFor broad sense position vector,For corresponding velocity vector, T is Lagrangian.
Since newton-Euler's method finds out the torque without hydrodynamic force variable
Then
To six degree of freedom submarine mechanical arm deriving analysis, each joint is expressed as i-1v relative to the speed in previous jointi(i
=1,2,3.....6).According to rigid body fixed-axis rotation theorem, each joint translational velocity can be obtained, and then available each joint
The movement absolute velocity of center of mass point0vi(i=1,2,3.....6).
Solve the dissipative function φ in each joint of mechanical armi(i=1,2 ... ..6),
It can thus be concluded that the dissipative function of submarine mechanical arm system is φ=φ1+φ2+φ3+φ4+φ5+φ6.Auspicious thunder is consumed
Scattered function is brought Lagrange's equation into and is derived, the general velocity of the auspicious thunder dissipative function φ of derivationParameter substitutes into equationHydrodynamic damping coefficient can be acquired.
Embodiment 3, in conjunction with attached drawing 3, curve matching is limited to test data (x by experiment acquisitioni,yi), utilize these
Data seek approximate function y=f (x).X is output quantity in formula, and y is measurand.The time is output quantity in delays time to control,
Joint angles are measurand.
Curve-fitting method is applied in principal and subordinate's submarine mechanical arm delays time to control, the time delay process time is set as two parameters
This method is known as the delay of two periods by the period.In addition the signal as caused by 485 bus transfers is delayed, from the lag master of hand
Three periodic motions of hand, process analysis: at the main hands movement i+3 moment, due to the delay of 485 signals of communication, the i+3 moment is controlled under water
Device processed has just received the main moment position hand i+2, and thus controller learns the location point at three moment, the i.e. slave hand position at i+3 moment
It sets and (is learnt according to underwater control closed loop feedback), the main hand at i+1 moment control signal, the main hand at i+2 moment control signal.According to
Joint position temporal information can judge the motion change trend from hand in advance, such as situations such as deflecting, emergency stop, speed change, from
And two period delay control methods are utilized, subsequent time can be calculated from hand ideal movements point.
It is assumed that main swivel of hand i is in tk-2When control angle value be θk-2, in tk-1When control angle value be θk-1, exist from swivel of hand
tkMoment actual angle value is θk, then can obtain its coordinate is respectively (tk-2,θk-2)、(tk-1,θk-1) and (tk,θk), according to joint song
Line motion fitting equation, can obtain tk+1The angle predicted value at moment is θk+1, value is
θk+1=θk+v(k)×T+aT2/2 (15)
Wherein, a is the angular acceleration in joint, and T is transmission cycle 0.1s；V (k) is k moment joint i velocity of rotation.
It can be released by uniform acceleration equation
Comprehensive (15), (16) formula are calculated, and t is obtainedk+1The ideal value of the slave swivel of hand angle at moment
Here it is the calculation formula of two periods delay.By current time from swivel of hand angle and delay two moment main hand
Joint angles calculate subsequent time from swivel of hand speed, angular speed, to obtain ideal movement angle.
Embodiment 4, in conjunction with attached drawing 4, delays time to control makes submarine mechanical arm lag behind main hands movement from hand, so that controller
Learn the subsequent motion track of main hand, it can be deduced that it is tight that there are emergency stop deflectings etc. from the ideal value of hands movement, but in practical operation
Anxious operation, so needing to analyze main hands movement curve, to judge that the main hand of mechanical arm with the presence or absence of deflecting, accelerates, subtracts
The mutation movement link of speed, and then motion control is carried out to from hand.
Assuming that kth walks the differential seat angle between main hand output target value x (k) and main hand output target value x (k+1) of+1 step of kth
For e (k)=x (k+1)-x (k), discriminatory analysis is carried out to the differential seat angle, according to the difference of differential seat angle range, is controlled from subordinate one
How step moves.
As | e (k) | at < 0.2 °, main hand control variation there is no avoidance, rapidly becomes in out of hands movement limit of power
To equal emergency operations, time delay process is worked normally, and obtains the ideal movements point from hand subsequent time using multicycle delay algorithm.
When | e (k) | at > 0.2 °, differential seat angle exceeds mechanical arm calibration capability range at this time, this situation is that occurred suddenly by main hand
Stop avoidance movement.Mechanical arm stop motion keeps current location, until main hand signal returns to after hand halted state from newly, weighs
New setting in motion.
Ocean current feedback compensation link, which is characterized in calculating from hand actual motion position and main palmistry, answers moment target position
Difference, interpretation schematic diagram is as shown in Fig. 4, mainly judges whether there is ocean current interference and impacts to mechanical arm from hands movement,
If ocean currents have exceeded mechanical arm correction of movement limit of power, it is believed that ocean current interference effect is excessive, stops from hand mechanical arm
Current kinetic, until the main hand of mechanical arm returns to mechanical arm from hand stop position from newly, mechanical arm continues to follow main hands movement from hand,
, whereas if ocean currents are smaller, influence in manipulator motion limit of power, mechanical arm is carried out according to main hand target position
Appropriate position correction, then carry out time delay process control, so that mechanical arm is from hand smooth motion.
Assuming that kth walk main swivel of hand output target value x (k) with from swivel of hand actual motion positionBetween error beDiscriminatory analysis is carried out to the error, according to the difference of error range, determines how to move after hand.
When | e (k) | slave hand output valve at < 0.02 °, after correctionError is little, without correction, keeps
From hand current motion state.
When 0.02 ° < | e (k) | slave hand output valve at < 0.2 °, after correctionError is in mechanical arm
In correcting range ability, it is corrected to from hand current location.
When | e (k) | at > 0.2 °, from hand output valve after correctionError exceeds mechanical arm calibration capability at this time
Range, this situation are caused since ocean current interference is excessive.Mechanical arm stop motion, keep current location, until main hand signal from
It newly returns to after hand halted state, restarts to move.
Implement 5, in conjunction with Fig. 5, submarine mechanical arm is the system of one six input, six output, and control system includes control algolithm
Part, kinematics model part and kinetic model part set the motion profile in the seven each joints of functional machinery arm, move rail
Mark is continuous curve, carries out segment processing to it, is sampled, is transported to control signal according to control algolithm every the unit time
With M file edit control algolithm.
According to delays time to control algorithm, it can be deduced that new joint motions target point.Bring new moving target point into movement
It learns in model, and is encapsulated in M file, calculate the motion profile in each joint.
Kinetic model is encapsulated in corresponding M file, after obtaining each oint motion trajectory, carries out power to each joint
Torque analysis is learned, to obtain the relevant control torque in each joint.
Simulink control system block diagram is as shown in Fig. 5.
Implement 6, in conjunction with attached drawing 6, citing is verified, and the joint parameter for underwater seven functional machineries arm of illustrating is as shown in table 1,
Mechanical arm structure diagram is as shown in Fig. 6 under seven function waters.
Each bar parameter of 1 seven functional machinery arm of table
Model buildings are carried out to mechanical arm under multifunctional water with Pro/E tool, to calculate each connecting rod of mechanical arm
The parameters such as center of gravity vector, quality and rotator inertia square, seven each joint parameters of functional machinery arm are as shown in the following table 2,3.
Each connecting rod quality of 2 seven functional machinery arm of table and center of gravity vector
| Connecting rod quality (kg) | Connecting rod center of gravity vector (x, y, z) (m) | |
| Joint 1 | 2.096 | (- 0.003, -0.062,0.0063) |
| Joint 2 | 9.67 | (0.219, -0.003,0.0449) |
| Joint 3 | 2.814 | (- 0.002, -0.08,0.002) |
| Joint 4 | 6.092 | (0.169, -0.005,0.045) |
| Joint 5 | 0.098 | (- 0.0073, -0.0053,0.041) |
| Joint 6 | 0.103 | (- 0.44,0.049,0.11) |
Each connecting rod the moment of inertia of 3 seven functional machinery arm of table
Assuming that each joint original state of six degree of freedom submarine mechanical arm are as follows: qz=[0,0,0,0,0,0]
Each joint dbjective state are as follows:
It is interfered using identical one group of random number as ocean current, ocean current interference is as shown in Fig. 7.Main hand operator operation
Main hand simple joint, fast increase joint angles, unexpected emergency stop deflecting operate main swivel of hand and come back to from hand stopping place after first slow
Afterwards, continue to operate joint angles increase, change joint angles change direction later, operation joint angles at the uniform velocity reduce.
The simple joint two periods delays time to control emulation of ocean current interference is as shown in Fig. 8.
Claims (3)
1. a kind of delay control method of master-slave mode submarine mechanical arm, which comprises the following steps:
(1) structural parameters for giving submarine mechanical arm, according to the kinematics model of submarine mechanical arm, when being carried out to principal and subordinate's mechanical arm
Prolong analysis；
(2) hydrodynamic factor is acquired, according to submarine mechanical arm kinetic model, verifies whether mechanical arm performance meets model need
It asks；
(3) delay process is carried out to main hand action signal, preceding n moment joint control signal is buffered in underwater manipulator,
Multiple power curve approximating method is then based on to calculate from the joint ideal movements position at hand n+1 moment；
(4) the analysis main hand of mechanical arm with the presence or absence of emergency stop deflecting emergency operation and mechanical arm from hand with the presence or absence of ocean current interfere because
Element realizes the control of principal and subordinate's submarine mechanical arm stable and continuous by the judgement of delays time to control link and ocean current feedback compensation link；
(5) control system model is established according to kinematics model, kinetic model and delays time to control algorithm, and based on this model
It is emulated, verifies the validity and reliability of delay control method；
Submarine mechanical arm kinetic model described in step (2) is with newton-Euler's square when not considering hydrodynamic factor
Journey carries out Dynamic Modeling to mechanical arm, and wherein submarine mechanical arm is at any movement moment, by each joint angles of mechanical arm
Variable qi, speed variablesAcceleration variableAs known conditions, the torque variable Q in each joint is resolvedi, i=
1 ... n iterates to calculate forward the velocity and acceleration of each connecting rod of mechanical arm according to Newton-Euler Method, wherein pedestal first
Initial motion state is determining, if pedestal is fixed, basis coordinates angular speed0ω0=0, angular accelerationAccording to ox
- Euler method iterate to calculate backward machinery arm each joint power and torque；Primary condition is torquePowerSubmarine mechanical arm distal point can be move freely in mechanical arm working space, then Mend、FendIt is zero；
(2.1) joint angles variable, speed variables of each connecting rod of Iterative mechanical arm forward:
ivci=ivi+iωi×iρi
Wherein,i+1ωi+1For the angular speed in i+1 joint；For the angular acceleration in i+1 joint；RI+1, iIt is i-th
The spin matrix that joint is shut down to i+1；For the linear acceleration in i+1 joint；ivciFor the mass center in i-th of joint
The linear acceleration at place；For the angular speed in i+1 joint itself；ieiFor the rotation axis in i-th of joint；i+1li+1It is i-th
The length vector of+1 connecting rod；iρiFor position vector of the i coordinate system at i connecting rod mass center；cIi+1For i+1 joint to mass center
Inertial tensor at coordinate origin；mi+1For the quality in i+1 joint；i+1Fi+1For the inertia force in i+1 joint；i+1Mi+1
For the moment of inertia in i+1 joint；
(2.2) the power F of Iterative joint of mechanical arm backwardiWith torque Qi, i=n, n-1 ..., 1:
QM i=Ri-1,i iMJi·i-1ei-1
Wherein,ihiFor i coordinate system to the position vector of i connecting rod stress point；iFJiFor the joint power in i-th of joint；iMJiIt is i-th
Joint moment of the joint relative to i coordinate system；QM iJoint moment for i-th of joint relative to i-1 coordinate system；
When the weight in robot linkage joint is as Consideration, ifThe support that fixed robot base is subject to
Quite upward gravity acceleration g is acted on, when considering hydrodynamic factor, auspicious thunder dissipative function is selected to describe viscous damping and fluid
The motion conditions of object under resistance:
Auspicious thunder dissipative function is introduced in lagrangian dynamics algorithm:
The Lagrange's equation with auspicious thunder dissipative function can be obtained:
Wherein qsFor broad sense position vector, qsFor corresponding velocity vector, T is Lagrangian；
Since newton-Euler's method finds out the torque without hydrodynamic force variable
Then
To six degree of freedom submarine mechanical arm deriving analysis, each joint is expressed as relative to the speed in previous jointi-1vi, i=1,2,
3.....6, it according to rigid body fixed-axis rotation theorem, obtains each joint translational velocity, and then obtains the movement of the center of mass point in each joint
Absolute velocity0vi, i=1,2,3.....6；
Solve the dissipative function φ in each joint of mechanical armi, i=1,2 ... ..6,
The dissipative function for obtaining submarine mechanical arm system is φ=φ1+φ2+φ3+φ4+φ5+φ6, by auspicious thunder dissipative function band
Enter Lagrange's equation to be derived, the general velocity of the auspicious thunder dissipative function φ of derivationParameter substitutes into equation Hydrodynamic damping coefficient can be acquired.
2. a kind of delay control method of master-slave mode submarine mechanical arm according to claim 1, it is characterised in that: step
(3) the power curve approximating method described in includes: that curve matching is limited to test data (x by experiment acquisitioni,yi), utilize this
A little data seek approximate function y=f (x), and wherein x is output quantity, and y is measurand, the time is defeated in delays time to control
Output, joint angles are measurand；Curve-fitting method is applied in principal and subordinate's submarine mechanical arm delays time to control, time delay process
Time is set as two parameter periods, this method is known as the delay of two periods, in addition the signal as caused by 485 bus transfers
Delay, from three periodic motions of main hand of the lag of hand, at the main hands movement i+3 moment, due to the delay of 485 signals of communication, when i+3
Carve underwater manipulator and just received the main moment position hand i+2, controller learns the location point at three moment, i.e., the i+3 moment from
Hand position, the main hand at i+1 moment control signal, the main hand at i+2 moment control signal, according to joint position temporal information, in advance
The motion change trend from hand is judged, to calculate subsequent time using two period delay control methods and transport from hand ideal
Dynamic point；
Wherein main swivel of hand i is in tk-2When control angle value be θk-2, in tk-1When control angle value be θk-1, from swivel of hand in tkWhen
Quarter actual angle value is θk, obtaining coordinate is respectively (tk-2,θk-2)、(tk-1,θk-1) and (tk,θk), it is quasi- according to joint curvilinear motion
Equation is closed, t is obtainedk+1The angle predicted value at moment is θk+1:
θk+1=θk+v(k)×T+aT2/2
Wherein, a is the angular acceleration in joint, and T is transmission cycle 0.1s；V (k) is k moment joint i velocity of rotation；
It is obtained by uniform acceleration equation:
It is calculated, obtains tk+1The ideal value of the slave swivel of hand angle at moment
Finally subsequent time is calculated from hand from swivel of hand angle and the main swivel of hand angle at two moment of delay by current time
Joint velocity, angular speed, to obtain ideal movement angle.
3. a kind of delay control method of master-slave mode submarine mechanical arm according to claim 1, it is characterised in that: step
(4) the delays time to control link described in includes: to analyze main hands movement curve, to judge the main hand of mechanical arm with the presence or absence of change
To, acceleration, the mutation movement link of deceleration, and then motion control is carried out to from hand；
The differential seat angle that wherein kth walks between main hand output target value x (k) and main hand output target value x (k+1) of+1 step of kth is e
(k)=x (k+1)-x (k) carries out discriminatory analysis to the differential seat angle, according to the difference of differential seat angle range, controls from hand next step such as
What is moved:
When | e (k) | at < 0.2 °, emergency operation, time delay ring is not present in out of hands movement limit of power in main hand control variation
Section works normally, and obtains the ideal movements point from hand subsequent time using multicycle delay algorithm；
When | e (k) | at > 0.2 °, differential seat angle exceeds mechanical arm calibration capability range at this time, and the movement of emergency stop avoidance occurs in main hand；Machine
Tool arm stop motion keeps current location, until main hand signal returns to after hand halted state from newly, restart to move；
Ocean current feedback compensation link mainly judges whether there is ocean current interference and impacts to mechanical arm from hands movement, if ocean current
Influence has exceeded mechanical arm correction of movement limit of power, it is believed that and ocean current interference effect is excessive, stops current kinetic from hand mechanical arm,
Until the main hand of mechanical arm returns to mechanical arm from hand stop position from newly, mechanical arm continues to follow main hands movement from hand；If ocean current
Influence is smaller, influences in manipulator motion limit of power, mechanical arm carries out appropriate position school according to main hand target position
Just, then time delay process control is carried out, so that mechanical arm is from hand smooth motion；
Wherein kth walk main swivel of hand output target value x (k) with from swivel of hand actual motion positionBetween error beDiscriminatory analysis is carried out to the error, according to the difference of error range, determines how to move after hand；
When | e (k) | slave hand output valve at < 0.02 °, after correctionWithout correction, keep from hand current kinetic shape
State；
When 0.02 ° < | e (k) | slave hand output valve at < 0.2 °, after correctionError is corrected in mechanical arm
In range capability, it is corrected to from hand current location；
When | e (k) | at > 0.2 °, from hand output valve after correctionError exceeds mechanical arm calibration capability range, machine
Tool arm stop motion keeps current location, until main hand signal returns to after hand halted state from newly, restart to move.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610352534.XA CN106054599B (en) | 2016-05-25 | 2016-05-25 | A kind of delay control method of master-slave mode submarine mechanical arm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610352534.XA CN106054599B (en) | 2016-05-25 | 2016-05-25 | A kind of delay control method of master-slave mode submarine mechanical arm |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN106054599A CN106054599A (en) | 2016-10-26 |
| CN106054599B true CN106054599B (en) | 2019-06-14 |
Family
ID=57175764
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610352534.XA Active CN106054599B (en) | 2016-05-25 | 2016-05-25 | A kind of delay control method of master-slave mode submarine mechanical arm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN106054599B (en) |
Families Citing this family (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108972536B (en) * | 2017-05-31 | 2021-06-22 | 西门子（中国）有限公司 | System and method for determining kinetic parameters of mechanical arm and storage medium |
| CN107283425A (en) * | 2017-06-20 | 2017-10-24 | 巨轮（广州）机器人与智能制造有限公司 | A kind of pair of six-joint robot cooperative motion Controlling model and algorithm |
| CN109394231B (en) * | 2018-12-10 | 2021-06-11 | 吉林大学 | Standing motion balance monitoring and dynamics analysis system |
| CN110561433B (en) * | 2019-08-31 | 2022-07-19 | 中山大学 | Method for improving mechanical arm motion planning control precision |
| CN111086004B (en) * | 2020-01-08 | 2022-09-13 | 山东理工大学 | Human-simulated flexible joint arm electromechanical coupling modeling method |
| CN111515958B (en) * | 2020-05-14 | 2022-08-09 | 重庆邮电大学 | Network delay estimation and compensation method of robot remote control system |
| CN111482966B (en) * | 2020-05-14 | 2022-08-09 | 重庆邮电大学 | Force and position control method of robot force sense remote control system |
| CN111694368A (en) * | 2020-06-04 | 2020-09-22 | 哈尔滨工业大学 | Six-degree-of-freedom platform control method |
| CN112140098B (en) * | 2020-09-15 | 2022-06-21 | 天津大学 | Underwater snake-shaped robot high-speed gait generation method based on near-end strategy optimization |
| CN112511243B (en) * | 2020-11-18 | 2022-10-28 | 深圳市通用测试系统有限公司 | Continuous sampling test device, system, method, equipment and storage medium for wireless performance |
| CN112716608B (en) * | 2021-01-20 | 2022-06-24 | 山东威高手术机器人有限公司 | Master-slave tracking control method for minimally invasive surgery robot |
| CN113246144B (en) * | 2021-06-29 | 2021-10-15 | 深之蓝海洋科技股份有限公司 | Position control system and method for underwater manipulator |
| CN116494249B (en) * | 2023-06-26 | 2023-12-19 | 极限人工智能(北京)有限公司 | Real-time manual control device, control method and cooperation system of cooperation mechanical arm |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101332604B (en) * | 2008-06-20 | 2010-06-09 | 哈尔滨工业大学 | Control method of man machine interaction mechanical arm |
| CN103331756A (en) * | 2013-06-04 | 2013-10-02 | 浙江工业大学 | Mechanical arm motion control method |
| CN102528802B (en) * | 2010-12-31 | 2014-12-03 | 北京中科广视科技有限公司 | Motion driving method for robot with nine degrees of freedom |
| CN104503231A (en) * | 2014-11-25 | 2015-04-08 | 北京理工大学 | Swinging arm driving-type motion control method for amphibious frog board robot |
-
2016
- 2016-05-25 CN CN201610352534.XA patent/CN106054599B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101332604B (en) * | 2008-06-20 | 2010-06-09 | 哈尔滨工业大学 | Control method of man machine interaction mechanical arm |
| CN102528802B (en) * | 2010-12-31 | 2014-12-03 | 北京中科广视科技有限公司 | Motion driving method for robot with nine degrees of freedom |
| CN103331756A (en) * | 2013-06-04 | 2013-10-02 | 浙江工业大学 | Mechanical arm motion control method |
| CN104503231A (en) * | 2014-11-25 | 2015-04-08 | 北京理工大学 | Swinging arm driving-type motion control method for amphibious frog board robot |
Non-Patent Citations (2)
| Title |
|---|
| 大时延水下机械手位置伺服控制技术研究;安江波;《中国优秀硕士学位论文全文数据库》;20100615(第6期);正文第5章 |
| 改进幂次趋近律的机械臂滑模控制律设计;席雷平等;《计算机测量与控制》;20121031;第20卷;第380-390页 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN106054599A (en) | 2016-10-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106054599B (en) | A kind of delay control method of master-slave mode submarine mechanical arm | |
| Chen et al. | RBFNN-based adaptive sliding mode control design for delayed nonlinear multilateral telerobotic system with cooperative manipulation | |
| CN108942924B (en) | Model uncertainty mechanical arm motion control method based on multilayer neural network | |
| CN103213129B (en) | Position/force hybrid control method for space manipulator | |
| Dai et al. | Design of an indirect adaptive controller for the trajectory tracking of UVMS | |
| CN108748147A (en) | A kind of control system and method for ectoskeleton mechanical arm | |
| CN102962838B (en) | Six-degrees-of-freedom parallel mechanism with closed kinematics positive solution and analytic method thereof | |
| Han et al. | Active use of restoring moments for motion control of an underwater vehicle-manipulator system | |
| CN108527372A (en) | A kind of joint of robot self-adaptation control method of variation rigidity series elastic driver | |
| CN103433924A (en) | High-accuracy position control method for serial robot | |
| CN112091976B (en) | Task space control method for underwater mechanical arm | |
| Soylu et al. | Comprehensive underwater vehicle-manipulator system teleoperation | |
| Hu et al. | Adaptive variable impedance control of dual-arm robots for slabstone installation | |
| Qi | Spatial target path following control based on Nussbaum gain method for underactuated underwater vehicle | |
| Ye et al. | Adaptive tracking control of a class of constrained Euler–Lagrange systems by factorization of dynamic mass matrix | |
| You et al. | Semi-autonomous bilateral teleoperation of hexapod robot based on haptic force feedback | |
| CN110744552A (en) | Flexible mechanical arm motion control method based on singular perturbation theory | |
| CN106570296A (en) | Six degree of freedom parallel mechanism real-time forward displacement solution | |
| Kumar et al. | Stabilization of Stewart platform using higher order sliding mode control | |
| Zhao et al. | System modeling and motion control of a cable-driven parallel platform for underwater camera stabilization | |
| Han et al. | Integral backstepping based computed torque control for a 6 DOF arm robot | |
| Shukla et al. | Teleoperation by using nonisomorphic mechanisms in the master–slave configuration for speed control | |
| Wu et al. | Development and control of a humanoid underwater robot | |
| Zainan et al. | Virtual reality-based teleoperation with robustness against modeling errors | |
| Sun et al. | UVMS Controller Design Based on Double Close-Loop Integral Sliding Mode |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |