CN106054599A - Master-slave underwater robotic arm delay control method - Google Patents

Abstract

The invention relates to the technical field of robots, and provides a master-slave underwater robotic arm delay control method which can achieve coordinated control on the master arm and the salve arm of a master-slave underwater robotic arm in a bad operating environment and under the condition of transmission communication delay. The method comprises the following steps: giving the structure parameters of an underwater robotic arm, and making a delay analysis of the master and slave robotic arms according to a kinematics model of the underwater robotic arm; collecting hydrodynamic factors, and verifying whether the performance of the robotic arm meets the demand of the model according to the kinematics model of the underwater robotic arm; and delaying an action signal of the master arm, caching joint control signals at first n moments in an underwater controller, and calculating the ideal motion position of the joint of the slave arm at a moment n+1 based on a multi-power curve fitting method. The problem that an underwater robotic arm works intermittently due to a bad operating environment and transmission communication delay is solved.

Description

A kind of delay control method of master-slave mode submarine mechanical arm

Technical field

The present invention relates to robotics, it is provided that be that one ensure that master-slave mode submarine mechanical arm is at operation ring Border is severe, under transmission communication time delay condition, it is achieved the main hands of submarine mechanical arm and the master-slave mode submarine mechanical coordinating to control from hands The delay control method of arm.

Background technology

Owing to underwater environment is complicated, exist seawater pressure, low visibility, at a temperature of degradation be unfavorable for the environment of human work Condition, available principal and subordinate's multifunctional underwater mechanical arm coordinating operation type underwater robot replaces the mankind in hazardous environment works such as deep-seas Making, submarine mechanical arm can carry out sea floor exploration exploitation, and submarine pipeline keeps in repair, and petroleum pipeline spreads work such as building.Submarine mechanical arm is not Only reduce the danger of mankind's underwater performance operation, also meet the mankind and extend perception, the demand that tera incognita is explored.Therefore set Meter principal and subordinate's submarine mechanical arm control system has important research and engineering significance.

The motor control aspect of many expert's mechanical arms under water proposes a lot of method.On-off control is submarine mechanical arm The most also being the most ancient control mode, water controller is made up of operating platform shift knob, and surface operations personnel pass through The switch folding in the corresponding joint of operation controls the motion from each joint of hands of the submarine mechanical arm, and operator take the photograph under water by observing The image information being transmitted back to as device judges the movement position from joints of hand, from the foregoing, submarine mechanical arm switch controlling party Formula is opened loop control, and the fixing from hands movement speed, so control accuracy depends on operator's of on-off control Micro-judgment, underwater performance work limited efficacy, and bigger to operator's difficulty when multi-joint assists to control.

Compared to on-off control, the great advantage of speed controlling is exactly the controllability of joint motions speed.Under ideal conditions, The process through first accelerating, slowing down the most at the uniform velocity, again is affirmed in joint from initial position to target location, then the joint of on-off control Movement velocity is fixed, and is so difficult to be parked in accurately target location last, and speed controlling can adjust joint fortune efficiently Dynamic.Speed control method passing ratio clack box controls each joint fluid flow thus controls the mechanical arm motion speed from each joint of hands Degree.But speed controlling and on-off control have a common shortcoming, that is, the requirement to surface operations personnel is higher.Speed Controlling to use action bars as water controller, the direction of operating of action bars may be different, such as large arm from the direction of motion in joint Joint be upper and lower elevating movement, action bars direction of operating be side-to-side movement, so can increase operator's workload, and can not Accomplish to control the speed in multiple joint simultaneously.Although speed controlling solves on-off control from the highest the asking of hands movement precision Topic, but still do not solve to manipulate, during reality controls, the problem that personnel manipulate difficulty.

Along with technology develops, it is considered to the drawback of first two control mode, the control mode of research worker design attitude feedback, Also referred to as master-slave control method, carries out position feedback from hands scaled down version model by being modified to by action bars so that operator Can visualize and learn from hand position information, need not be as on-off control and speed controlling are like building manipulator motion in brains Model, so can avoid the operation damage to mechanical arm, and facilitate manipulation personnel to control multiple joint motions simultaneously.Principal and subordinate Control mode makes operator have the sensation of robot arm one when operating submarine mechanical arm, is effectively improved operator and works effect Rate.And position feedback control also has the advantage of speed controlling, by operating the speed command that main hands sends, it is also possible to by speed Device is integrated is transformed to positional information in degree command value control, carries out position feedback control.

But under water in the actual control of operation, submarine mechanical arm is when deep ocean work, and mechanical arm main hand control signal passes Defeated distance is remote, and 485 traditional bus communication transmission speeds are slow, so the transmission delay time is long, thus causes and receives control from hands Signal lag processed is in surface operations, and submarine mechanical arm occurs the bad phenomenon such as delayed, descontinuous motion at work from hands, affects water Face operator, to from the judgement of hand position, so that the operation easier of operator strengthens, and then have affected subsea tasks Working performance.

Summary of the invention

Present invention aim at providing a kind of form simple, reduce amount of calculation, it is possible to obtain on demand optimizing solution, have logical Delay control method with the master-slave mode submarine mechanical arm of property and rapidity.

The object of the present invention is achieved like this:

The present invention comprises the following steps:

(1) structural parameters of given submarine mechanical arm, according to the kinematics model of submarine mechanical arm, enter slave mechanical arm Row time-delay analysis；

(2) gathering hydrodynamic factor, according to submarine mechanical arm kinetic model, whether proof machine mechanical arm performance meets model Demand；

(3) main hands actuating signal is carried out delay process, front n moment joint control signal is buffered in underwater manipulator In, it is then based on repeatedly power curve approximating method and calculates the ideal movements position, joint from the hands n+1 moment；

(4) analyze the main hands of mechanical arm whether exist jerk break-in emergency operation and mechanical arm from hands whether exist ocean current do Disturb factor, by delays time to control link and the judgement of ocean current feedback compensation link, it is achieved principal and subordinate's submarine mechanical arm stable and continuous control System；

(5) control system model is set up according to kinematics model, kinetic model and delays time to control algorithm, and based on this Model emulates, the effectiveness of checking delay control method and reliability.

Submarine mechanical arm kinetic model described in step 2, when not considering hydrodynamic factor, is to use newton Euler Equation carries out Dynamic Modeling to mechanical arm, and wherein submarine mechanical arm is in arbitrary motion moment, by each joint angle of mechanical arm Degree variable q_i, speed variablesAcceleration variableI=1 ... n, as known conditions, resolves the moment variable Q in each joint_i, I=1 ... n, according to speed and acceleration, the Qi Zhongji of each connecting rod of Newton-Euler method iterative computation mechanical arm the most forward Seat initial motion state determines that, if pedestal is fixed, then and basis coordinates angular velocity⁰ω₀=0, angular accelerationAccording to cattle The power in each joint of Euler method iterative computation mechanical arm backward and moment；Initial condition is momentPowerSubmarine mechanical arm distal point can be the most movable in mechanical arm work space, then M_end、F_endIt is zero；

(2.1) the joint angles variable of each connecting rod of Iterative mechanical arm, speed variables forward:

${\mathrm{\ω}_{i+1}}_{i+1}=R_{i+1,i}({\mathrm{\ω}_i}_i+{\stackrel{\·}{q}}_{i+1}{e_i}_i)$

${\stackrel{\·}{\mathrm{\ω}}_{i+1}}_{i+1}=R_{i+1,i}({\stackrel{\·}{\mathrm{\ω}}_i}_i+{\stackrel{\·\·}{q}}_{i+1}{e_i}_i+{\stackrel{\·}{q}}_{i+1}{\mathrm{\ω}_i}_i\×{e_i}_i)$

${\stackrel{\·}{v}_{i+1}}_{i+1}=R_{i+1,i}{\stackrel{\·}{v}_i}_i+{\stackrel{\·}{\mathrm{\ω}}_{i+1}}_{i+1}\×{l_{i+1}}_{i+1}+{\mathrm{\ω}_{i+1}}_{i+1}\×({\mathrm{\ω}_{i+1}}_{i+1}\×{l_{i+1}}_{i+1})$

ⁱv_ci=ⁱv_i+ⁱω_i×ⁱρ_i

${\stackrel{\·}{v}_i}_{ci}={\stackrel{\·}{v}_i}_i+{\stackrel{\·}{\mathrm{\ω}}_i}_i\×{\mathrm{\ρ}_i}_i+{\mathrm{\ω}_i}_i\×({\mathrm{\ω}_i}_i\×{\mathrm{\ρ}_i}_i)$

${F_{i+1}}_{i+1}=m_{i+1}{\stackrel{\·}{v}_i}_{ci}$

${}^{i+1}{M}_{i+1}={I_c}_{i+1}{\stackrel{\·}{\mathrm{\ω}}_{i+1}}_{i+1}+{\mathrm{\ω}_{i+1}}_{i+1}\×\left({I_c}_{i+1}{\mathrm{\ω}_{i+1}}_{i+1}\right)$

Wherein,ⁱ⁺¹ω_i+1Angular velocity for i+1 joint；Angular acceleration for i+1 joint；R_{I+1, i}For The spin matrix shut down to i+1 in i-th joint；Linear acceleration for i+1 joint；ⁱv_ciFor i-th joint Linear acceleration at barycenter；Angular velocity for i+1 joint self；ⁱe_iRotation axis for i-th joint；ⁱ⁺¹l_i+1For The length vector of i+1 connecting rod；ⁱρ_iIt is tied to the position vector at i connecting rod barycenter for i coordinate；^cI_i+1Arrive for i+1 joint Inertial tensor at center-of-mass coordinate initial point；m_i+1Quality for i+1 joint；ⁱ⁺¹F_i+1Inertia force for i+1 joint；^{i+ 1}M_i+1Moment of inertia for i+1 joint；

(2.2) power F of Iterative joint of mechanical arm backward_iWith moment Q_iI=n, n-1 ..., 1:

Wherein,ⁱh_iThe position vector of i connecting rod stress point it is tied to for i coordinate；Joint power for i-th joint；For I-th joint is relative to the joint moment of i coordinate system；Q^M _iFor i-th joint relative to the joint moment of i-1 coordinate system；

When the weight in robot linkage joint is as Consideration, ifFixing robot base is subject to Supporting role gravity acceleration g the most upwards, it is considered to during hydrodynamic factor, 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:

$\mathrm{\φ}=\frac{1}{2}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}(k_{ix}\stackrel{.}{{x_i}^2}+k_{iy}{y_i}^2+k_{iz}{z_i}^2),$

Can be with the Lagrange's equation of auspicious thunder dissipative function:

$\frac{d}{dt}\frac{\∂T}{\∂{\tilde{q}}_s}-\frac{\∂T}{\∂q_s}=-\frac{\∂\mathrm{\φ}}{\∂q_s}+Q_s,(s=1,2,\mathrm{3.....}n),$

Wherein q_sFor broad sense position vector,For corresponding velocity vector, T is Lagrangian；

Owing to Nuton-Euler method obtains the moment without hydrodynamic force variable

Then

$Q_s=\mathrm{\τ}+\frac{\∂\mathrm{\φ}}{\∂q_s},(s=1,2,\mathrm{3.....}n)$

To six degree of freedom submarine mechanical arm deriving analysis, each joint is shown as relative to the speedometer in previous joint^i-1v_i(i= 1,2,3.....6), according to rigid body fixed-axis rotation theorem, each joint translational velocity is drawn, and then obtain the center of mass point in each joint Motion absolute velocity⁰v_i(i=1,2,3.....6)；

Solve the dissipative function φ in each joint of mechanical arm_i(i=1,2 ... ..6),

${\mathrm{\φ}}_i=\frac{u^0{v_i}^2}{2}=\frac{u}{2}{\∫}_0^{l_i}v_0{{}_i}^2{\mathrm{dx}}_i$

The dissipative function obtaining submarine mechanical arm system is φ=φ₁+φ₂+φ₃+φ₄+φ₅+φ₆, by auspicious thunder dissipative function band Enter Lagrange's equation to derive, the general velocity of derivation auspicious thunder dissipative function φParameter substitutes into equation Hydrodynamic damping coefficient can be tried to achieve.

Power curve approximating method described in step 3 includes: curve matching is limited to test data by experiment acquisition (x_i,y_i), utilize these data to ask for approximate function y=f (x), wherein x is output, and y is measurand, time delay control In system, the time is output, and joint angles is measurand；Curve-fitting method is applied to principal and subordinate's submarine mechanical arm time delay control In system, time delay process set of time is two parameter cycles, and this method is referred to as the time delay of two cycles, adds due to 485 buses The signal lag that transmission causes, from three periodic movements of delayed main hands of hands, in the main hands movement i+3 moment, due to 485 communication letters Number time delay, i+3 moment underwater manipulator has just received main hands i+2 moment position, and the location point in three moment learnt by controller, The i.e. i+3 moment from hand position, the main hand control signal in i+1 moment, the main hand control signal in i+2 moment, according to joint position Temporal information, judges the motion change trend from hands in advance, thus utilizes two cycle delay control methods, calculate lower a period of time Carve from hands ideal movements point；

The most main joints of hand i is at t_k-2Time control controlling angle value is θ_k-2, at t_k-1Time control controlling angle value is θ_k-1, exist from joints of hand t_kMoment actual corners angle value is θ_k, obtain coordinate and be respectively (t_k-2,θ_k-2)、(t_k-1,θ_k-1) and (t_k,θ_k), according to closing pitch curve fortune Dynamic fit equation, obtains t_k+1The angle predictive value in moment is θ_k+1:

θ_k+1=θ_k+v(k)×T+aT²/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；

Obtained by uniform acceleration equation:

$\left\{\begin{array}{c}v(k-1)=({\mathrm{\θ}}_{k-1}-{\mathrm{\θ}}_{k-2})/T\\ v\left(k\right)=({\mathrm{\θ}}_k-{\mathrm{\θ}}_{k-1})/T\\ a=(v\left(k\right)-v\left(k-1\right))/T\end{array}\right.$

Calculate, obtain t_k+1The ideal value from joints of hand angle in moment

${\mathrm{\θ}}_{k+1}=\frac{5}{2}{\mathrm{\θ}}_k-2{\mathrm{\θ}}_{k-1}+\frac{1}{2}{\mathrm{\θ}}_{k-2}$

The final main joints of hand angle by current time from joints of hand angle and two moment of time delay calculates subsequent time From joints of hand speed, angular velocity, thus obtain preferable movement angle.

Delays time to control link described in step (4) includes: is analyzed main hands movement curve, thus judges mechanical arm master Whether hands exists break-in, accelerates, the mutation movement link of deceleration, and then carries out motor control to from hands；

Wherein kth walks the differential seat angle between main hands output desired value x (k) and kth+1 step main hands output desired value x (k+1) For e (k)=x (k+1)-x (k), this differential seat angle is carried out discriminatory analysis, according to the difference of differential seat angle scope, control from subordinate one How step moves:

When | e (k) | < 0.2 °, main hand control changes, and is in hands movement limit of power, there is not emergency operation, prolong Shi Huanjie normally works, and utilizes multicycle delay algorithm to draw the ideal movements point from hands subsequent time；

When | e (k) | > 0.2 °, now differential seat angle exceeds mechanical arm calibration capability scope, and main hands occurs that jerk avoidance is transported Dynamic；Mechanical arm stop motion, keeps current location, until main hands signal from newly returning to after hands halted state, restarts fortune Dynamic.

Ocean current feedback compensation link mainly judges whether that mechanical arm is impacted by ocean current interference from hands movement, if Ocean currents is beyond mechanical arm correction of movement limit of power, it is believed that ocean current interference effect is excessive, stops current from mobile phone mechanical arm Motion, until the main hands of mechanical arm continues to follow main hands movement from hands from hands stop position, mechanical arm from newly returning to mechanical arm；If Ocean currents is less, and impact is in manipulator motion limit of power, and mechanical arm carries out appropriate position according to main hands target location Put correction, then carry out time delay process control so that mechanical arm is from hands easy motion；

Wherein kth walks main joints of hand output desired value x (k) and from joints of hand actual motion positionBetween error beThis error is carried out discriminatory analysis, according to the difference of range of error, determines how to move after hands；

When | e (k) | < 0.02 °, after correction from hands output valveWithout correction, keep currently transporting from hands Dynamic state；

When 0.02 ° < when | e (k) | < 0.2 °, after correction from hands output valveError is at mechanical arm In correcting range ability, it is corrected from hands current location；

When | e (k) | > 0.2 °, from hands output valve after correctionError exceeds mechanical arm calibration capability model Enclose, mechanical arm stop motion, keep current location, until main hands signal from newly returning to after hands halted state, restarts fortune Dynamic.

The beneficial effects of the present invention is:

By combining construction features and the working method of slave mechanical arm, delay control method is incorporated into principal and subordinate's machine under water In the motor control of mechanical arm, the method can the most quickly realize the smooth steady of master-slave mode submarine mechanical arm and control.Based on The delays time to control algorithm of power curve matching, carries out preferable position, joint to from hands by the way of to known position information successive ignition Putting and determine, the method can effectively realize under water from manipulator motion smooth trajectory, it is to avoid ocean current interference and misoperation cause Motion jitter and pause.Delay control method based on slave mechanical arm, the robot worked for remote tele-operation mode controls Method provides reference, has certain versatility and practicality.The final present invention solve submarine mechanical arm working environment severe, The difficult problems such as the submarine mechanical arm work discontinuity that transmission communication time delay causes.The method form is succinct, have the highest precision and Solving speed, meets the operation needs of submarine mechanical arm curve movement smooth steady.

Accompanying drawing explanation

The control system flow chart of Fig. 1 slave mechanical arm；

The rod member graph of a relation of Fig. 2 submarine mechanical arm；

Fig. 3 curve-fitting method schematic diagram；

The correction of Fig. 4 ocean current judges；

Fig. 5 Simulink control system figure；

Fig. 6 submarine mechanical arm configuration sketch；

Fig. 7 ocean current interference simulation figure；

The slave mechanical arm simple joint two cycle delays time to control analogous diagram of Fig. 8 ocean current interference.

Detailed description of the invention

Below in conjunction with the accompanying drawings the present invention is described further.

The present invention relates to principal and subordinate's submarine mechanical arm Control System Design field, it is provided that a kind of master-slave mode submarine mechanical arm Delay control method.The present invention starts with the time-delay analysis of master-slave mode submarine mechanical arm, utilizes the delays time to control of power curve matching Algorithm, determines that master-slave mode mechanical arm coordinates the process controlled, anti-by the foundation of kinematics model, delays time to control link and ocean current The correction of feedback link, dynamics check, be quickly found out submarine mechanical arm from optimization position, hands each joint, it is possible to ensures master-slave mode water Lower mechanical arm is severe at working environment, under transmission communication time delay condition, it is achieved the main hands of submarine mechanical arm controls with the coordination from hands. The master-slave mode mechanical arm that the present invention solves coordinates the versatility problem controlled, and is independent of robot configuration, and form is simple, reduces The difficulty solved, reduces amount of calculation, it is possible to obtains on demand optimizing solution, has versatility and rapidity, meets and appoints to setting the goal Master-slave mode submarine mechanical arm is coordinated the requirement controlled by business and environment.

Step 1: the structural parameters of given submarine mechanical arm, sets up the kinematics model of mechanical arm, calculates main hands simultaneously To from the communication delay time of hands.

Step 2: consider hydrodynamic factor, set up submarine mechanical arm kinetic model, in order to proof machine mechanical arm performance whether Meet model requirements, and can apply in actual control operation.

Step 3: main hands actuating signal is carried out delay process, is buffered in front n moment joint control signal and controls under water In device processed, it is then based on repeatedly power curve approximating method and calculates the ideal movements position, joint from the hands n+1 moment.

Step 4: the analysis main hands of mechanical arm whether there is the emergency operations such as jerk break-in and whether mechanical arm exists from hands The factors such as ocean current interference, by delays time to control link and the judgement of ocean current feedback compensation link, it is achieved principal and subordinate's submarine mechanical arm is put down Steady continuous control.

Step 5: control system model can be set up according to kinematics model, kinetic model and delays time to control algorithm, And emulate based on this model, the effectiveness of checking delay control method and reliability.

Embodiment 1, in conjunction with accompanying drawing 1, the method for the present invention comprises the steps:

Step 1: the structural parameters of given submarine mechanical arm, sets up the kinematics model of mechanical arm, calculates main hands simultaneously To from the communication delay time of hands.

Step 2: consider hydrodynamic factor, set up submarine mechanical arm kinetic model, in order to proof machine mechanical arm performance whether Meet model requirements, and can apply in actual control operation.

Step 3: main hands actuating signal is carried out delay process, is buffered in front n moment joint control signal and controls under water In device processed, it is then based on repeatedly power curve approximating method and calculates the ideal movements position, joint from the hands n+1 moment.

Step 4: the analysis main hands of mechanical arm whether there is the emergency operations such as jerk break-in and whether mechanical arm exists from hands The factors such as ocean current interference, by delays time to control link and the judgement of ocean current feedback compensation link, it is achieved principal and subordinate's submarine mechanical arm is put down Steady continuous control.

Step 5: control system model can be set up according to kinematics model, kinetic model and delays time to control algorithm, And emulate based on this model, the effectiveness of checking delay control method and reliability.

Embodiment 2, in conjunction with accompanying drawing 2, sets up the kinetic model of submarine mechanical arm.The most do not consider hydrodynamic factor, fortune With newton euler equations, mechanical arm is carried out Dynamic Modeling.

Submarine mechanical arm is in arbitrary motion moment, by mechanical arm each joint angles variable q_i, speed variablesAcceleration VariableI=1 ... n, as known conditions, resolves the moment variable Q in each joint_i, i=1 ... n.According to Newton-Euler method The speed of each connecting rod of iterative computation mechanical arm and acceleration the most forward.Assume what pedestal initial motion state determined that, as Really pedestal is fixed, then⁰ω₀=0,Afterwards according to the power in each joint of Newton-Euler method iterative computation mechanical arm backward And moment.Initial condition is Assume that submarine mechanical arm distal point can be at mechanical arm work space Interior the most movable, then M_end、F_endIt is zero.

(1) q of each connecting rod of Iterative mechanical arm forward_i、

${\mathrm{\&omega;}_{i+1}}_{i+1}=R_{i+1,i}({\mathrm{\&omega;}_i}_i+{\stackrel{\&CenterDot;}{q}}_{i+1}{e_i}_i)---\left(1\right)$

${\stackrel{\&CenterDot;}{\mathrm{\&omega;}}_{i+1}}_{i+1}=R_{i+1,i}({\stackrel{\&CenterDot;}{\mathrm{\&omega;}}_i}_i+{\stackrel{\&CenterDot;\&CenterDot;}{q}}_{i+1}{e_i}_i+{\stackrel{\&CenterDot;}{q}}_{i+1}{\mathrm{\&omega;}_i}_i\&times;{e_i}_i)---\left(2\right)$

${\stackrel{\&CenterDot;}{v}_{i+1}}_{i+1}=R_{i+1,i}{\stackrel{\&CenterDot;}{v}_i}_i+{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}_{i+1}}_{i+1}\&times;{l_{i+1}}_{i+1}+{\mathrm{\&omega;}_{i+1}}_{i+1}\&times;({\mathrm{\&omega;}_{i+1}}_{i+1}\&times;{l_{i+1}}_{i+1})---\left(3\right)$

ⁱv_ci=ⁱv_i+ⁱω_i×ⁱρ_i (4)

${\stackrel{\&CenterDot;}{v}_i}_{ci}={\stackrel{\&CenterDot;}{v}_i}_i+{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}_i}_i\&times;{\mathrm{\&rho;}_i}_i+{\mathrm{\&omega;}_i}_i\&times;({\mathrm{\&omega;}_i}_i\&times;{\mathrm{\&rho;}_i}_i)---\left(5\right)$

${F_{i+1}}_{i+1}=m_{i+1}{\stackrel{\&CenterDot;}{v}_i}_{ci}---\left(6\right)$

${}^{i+1}{M}_{i+1}={I_c}_{i+1}{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}_{i+1}}_{i+1}+{\mathrm{\&omega;}_{i+1}}_{i+1}\&times;\left({I_c}_{i+1}{\mathrm{\&omega;}_{i+1}}_{i+1}\right)---\left(7\right)$

Wherein:ⁱ⁺¹ω_i+1Angular velocity for i+1 joint；

Angular acceleration for i+1 joint；

R_{I+1, i}The spin matrix shut down for i-th joint to i+1；

Linear acceleration for i+1 joint；

ⁱv_ciFor the linear acceleration at the barycenter in i-th joint；

Angular velocity for i+1 joint self；

ⁱe_iRotation axis for i-th joint；

ⁱ⁺¹l_i+1Length vector for i+1 connecting rod；

ⁱρ_iIt is tied to the position vector at i connecting rod barycenter for i coordinate；

^cI_i+1For the inertial tensor at i+1 joint to center-of-mass coordinate initial point；

m_i+1Quality for i+1 joint；

ⁱ⁺¹F_i+1Inertia force for i+1 joint；

ⁱ⁺¹M_i+1Moment of inertia for i+1 joint.

(2) power F of Iterative joint of mechanical arm backward_iWith moment Q_iI=n, n-1 ..., 1:

Wherein:ⁱh_iThe position vector of i connecting rod stress point it is tied to for i coordinate；

Joint power for i-th joint；

For i-th joint relative to the joint moment of i coordinate system；

Q^M _iFor i-th joint relative to the joint moment of i-1 coordinate system.

When the weight in robot linkage joint is as Consideration, can setFixing robot base is subject to Supporting role gravity acceleration g the most upwards.So process just the same with the impact 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 motion feelings of object under viscous damping and fluid resistance Condition.The auspicious thunder dissipative function process specifically solving mechanical arm is as follows:

Auspicious thunder dissipative function is introduced in lagrangian dynamics algorithm

$\mathrm{\&phi;}=\frac{1}{2}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}(k_{ix}\stackrel{.}{{x_i}^2}+k_{iy}{y_i}^2+k_{iz}{z_i}^2)---\left(11\right)$

Can be with the Lagrange's equation of auspicious thunder dissipative function

$\frac{d}{dt}\frac{\&part;T}{\&part;{\tilde{q}}_s}-\frac{\&part;T}{\&part;q_s}=-\frac{\&part;\mathrm{\&phi;}}{\&part;q_s}+Q_s,(s=1,2,\mathrm{3.....}n)---\left(12\right)$

Wherein q_sFor broad sense position vector,For corresponding velocity vector, T is Lagrangian.

Owing to Nuton-Euler method obtains the moment without hydrodynamic force variable

Then

$Q_s=\mathrm{\&tau;}+\frac{\&part;\mathrm{\&phi;}}{\&part;q_s},(s=1,2,\mathrm{3.....}n)---\left(13\right)$

To six degree of freedom submarine mechanical arm deriving analysis, each joint is shown as i-1v relative to the speedometer in previous joint_i(i =1,2,3.....6).According to rigid body fixed-axis rotation theorem, each joint translational velocity can be drawn, and then each joint can be obtained The motion absolute velocity of center of mass point⁰v_i(i=1,2,3.....6).

Solve the dissipative function φ in each joint of mechanical arm_i(i=1,2 ... ..6),

${\mathrm{\&phi;}}_i=\frac{u^0{v_i}^2}{2}=\frac{u}{2}{\&Integral;}_0^{l_i}v_0{{}_i}^2{\mathrm{dx}}_i---\left(14\right)$

The dissipative function that thus can obtain submarine mechanical arm system is φ=φ₁+φ₂+φ₃+φ₄+φ₅+φ₆.Auspicious thunder is consumed Scattered function is brought Lagrange's equation into and is derived, the general velocity of derivation auspicious thunder dissipative function φParameter substitutes into equationHydrodynamic damping coefficient can be tried to achieve.

Embodiment 3, in conjunction with accompanying drawing 3, curve matching is limited to test data (x by experiment acquisition_i,y_i), utilize these Data ask for approximate function y=f (x).In formula, x is output, and y is measurand.In delays time to control, the time is output, Joint angles is measurand.

Curve-fitting method being applied in principal and subordinate's submarine mechanical arm delays time to control, time delay process set of time is two parameters In the cycle, this method is referred to as the time delay of two cycles.Plus the signal lag caused due to 485 bus transfer, from the delayed master of hands Three periodic movements of hands, process analysis: in the main hands movement i+3 moment, due to the time delay of 485 signals of communication, the i+3 moment is controlled under water Device processed has just received main hands i+2 moment position, and thus the location point in three moment learnt by controller, i.e. the i+3 moment from hands position Put (learning according to underwater control closed loop feedback), the main hand control signal in i+1 moment, the main hand control signal in i+2 moment.According to Joint position temporal information, can judge the motion change trend from hands in advance, the situation such as such as break-in, jerk, speed change, from And utilize two cycle delay control methods, subsequent time can be calculated from hands ideal movements point.

Assuming that main joints of hand i is at t_k-2Time control controlling angle value is θ_k-2, at t_k-1Time control controlling angle value is θ_k-1, exist from joints of hand t_kMoment actual corners angle value is θ_k, then can obtain its coordinate and be respectively (t_k-2,θ_k-2)、(t_k-1,θ_k-1) and (t_k,θ_k), bent according to joint Line motion fitting equation, can obtain t_k+1The angle predictive value in moment is θ_k+1, its value is

θ_k+1=θ_k+v(k)×T+aT²/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.

Can be released by uniform acceleration equation

$\left\{\begin{array}{c}v(k-1)=({\mathrm{\&theta;}}_{k-1}-{\mathrm{\&theta;}}_{k-2})/T\\ v\left(k\right)=({\mathrm{\&theta;}}_k-{\mathrm{\&theta;}}_{k-1})/T\\ a=(v\left(k\right)-v\left(k-1\right))/T\end{array}\right.---\left(16\right)$

Comprehensively (15), (16) formula calculate, and obtain t_k+1The ideal value from joints of hand angle in moment

${\mathrm{\&theta;}}_{k+1}=\frac{5}{2}{\mathrm{\&theta;}}_k-2{\mathrm{\&theta;}}_{k-1}+\frac{1}{2}{\mathrm{\&theta;}}_{k-2}---\left(17\right)$

Here it is the computing formula of two cycles time delay.By current time from joints of hand angle and the main hands in two moment of time delay Joint angles calculates subsequent time from joints of hand speed, angular velocity, thus obtains preferable movement angle.

Embodiment 4, in conjunction with accompanying drawing 4, delays time to control makes submarine mechanical arm lag behind main hands movement from hands so that controller Learn the subsequent motion track of main hands, it can be deduced that from chirokinesthetic ideal value, but it is tight to there is jerk break-in etc. in practical operation Anxious operation, so needing to be analyzed main hands movement curve, thus judging whether the main hands of mechanical arm exists break-in, accelerating, subtracting The mutation movement link of speed, and then carry out motor control to from hands.

Assume that kth walks the differential seat angle between main hands output desired value x (k) and kth+1 step main hands output desired value x (k+1) For e (k)=x (k+1)-x (k), this differential seat angle is carried out discriminatory analysis, according to the difference of differential seat angle scope, control from subordinate one How step moves.

When | e (k) | < 0.2 °, main hand control changes, and is in hands movement limit of power, there is not avoidance, becomes rapidly To waiting emergency operation, time delay process normally works, and utilizes multicycle delay algorithm to draw the ideal movements point from hands subsequent time.

When | e (k) | > 0.2 °, now differential seat angle exceeds mechanical arm calibration capability scope, and this situation is to be occurred suddenly by main hands Stop avoidance motion.Mechanical arm stop motion, keeps current location, until main hands signal is from newly returning to after hands halted state, and weight New setting in motion.

The feature of ocean current feedback compensation link is that calculating should moment target location from hands actual motion position and main palmistry Difference, interpretation schematic diagram as shown in Figure 4, mainly judges whether that mechanical arm is impacted by ocean current interference from hands movement, If ocean currents is beyond mechanical arm correction of movement limit of power, it is believed that ocean current interference effect is excessive, stop from mobile phone mechanical arm Current kinetic, until the main hands of mechanical arm continues to follow main hands movement from hands from hands stop position, mechanical arm from newly returning to mechanical arm, Whereas if ocean currents is less, impact is in manipulator motion limit of power, and mechanical arm is carried out according to main hands target location Appropriate position correction, then carry out time delay process control so that mechanical arm is from hands easy motion.

Assume that kth walks main joints of hand output desired value x (k) and from joints of hand actual motion positionBetween error beThis error is carried out discriminatory analysis, according to the difference of range of error, determines how to move after hands.

When | e (k) | < 0.02 °, after correction from hands output valveError is little, it is not necessary to correction, keeps From hands current motion state.

When 0.02 ° < when | e (k) | < 0.2 °, after correction from hands output valveError is at mechanical arm In correcting range ability, it is corrected from hands current location.

When | e (k) | > 0.2 °, from hands output valve after correctionThis time error exceeds mechanical arm calibration capability Scope, this situation is to cause owing to ocean current interference is excessive.Mechanical arm stop motion, keep current location, until main hands signal from Newly return to after hands halted state, restart motion.

Implementing 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 movement locus in seven each joints of function machine mechanical arm, motion rail Mark is continuous print curve, it is carried out segment processing, samples control signal according to control algolithm every the unit interval, fortune By M file edit control algolithm.

According to delays time to control algorithm, it can be deduced that new joint motions impact point.Bring new moving target point into motion Learn in model, and be encapsulated in M file, calculate the movement locus in each joint.

Kinetic model is encapsulated in corresponding M file, after obtaining each joint motions track, each joint is carried out power Learn torque analysis, thus draw the relevant control moment in each joint.

Simulink control system block diagram is as shown in Figure 5.

Implementing 6, in conjunction with accompanying drawing 6, citing is verified, the joint parameter of citing seven function machine mechanical arm under water is as shown in table 1, Under seven function water, mechanical arm structure diagram is as shown in Figure 6.

Table 1 seven function machine mechanical arm each bar parameter

Use Pro/E instrument that mechanical arm under multifunctional water carries out model buildings, thus calculate each connecting rod of mechanical arm The parameter such as center of gravity vector, quality and rotator inertia square, the seven each joint parameters of function machine mechanical arm are such as table 2 below, shown in 3.

The each connecting rod quality of table 2 seven function machine mechanical arm 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)

Table 3 seven function machine mechanical arm each connecting rod the moment of inertia

Assume that six degree of freedom submarine mechanical arm each joint original state is: qz=[0,0,0,0,0,0]

Each joint dbjective state is:

Using one group of identical random number to disturb as ocean current, ocean current disturbs as shown in Figure 7.Main hands operator operate Main hands simple joint, the slowest rear fast increase joint angles, unexpected jerk break-in, operate main joints of hand and come back at hands stopping After, continuing operation joint angles increases, and changes joint angles change direction afterwards, and operation joint angles at the uniform velocity reduces.

The simple joint two cycle delays time to control of ocean current interference emulates as shown in Figure 8.

Claims (4)

1. the delay control method of a master-slave mode submarine mechanical arm, it is characterised in that comprise the following steps:

(1) structural parameters of given submarine mechanical arm, according to the kinematics model of submarine mechanical arm, when carrying out slave mechanical arm Prolong analysis；

(2) gathering hydrodynamic factor, according to submarine mechanical arm kinetic model, whether proof machine mechanical arm performance meets model need Ask；

(3) main hands actuating signal is carried out delay process, front n moment joint control signal is buffered in underwater manipulator, It is then based on repeatedly power curve approximating method and calculates the ideal movements position, joint from the hands n+1 moment；

(4) analyze the main hands of mechanical arm whether exist jerk break-in emergency operation and mechanical arm from hands whether exist ocean current interference because of Element, by delays time to control link and the judgement of ocean current feedback compensation link, it is achieved principal and subordinate's submarine mechanical arm stable and continuous controls；

(5) control system model is set up according to kinematics model, kinetic model and delays time to control algorithm, and based on this model Emulate, the effectiveness of checking delay control method and reliability.

The delay control method of a kind of master-slave mode submarine mechanical arm the most according to claim 1, it is characterised in that: step (2) the submarine mechanical arm kinetic model described in, when not considering hydrodynamic factor, is to use newton euler equations to machinery Arm carries out Dynamic Modeling, and wherein submarine mechanical arm is in arbitrary motion moment, by mechanical arm each joint angles variable q_i, speed Degree variableAcceleration variableI=1 ... n, as known conditions, resolves the moment variable Q in each joint_i, i=1 ... n, The speed of each connecting rod according to Newton-Euler method iterative computation mechanical arm the most forward and acceleration, wherein pedestal initial motion State determines that, if pedestal is fixed, then and basis coordinates angular velocity⁰ω₀=0, angular accelerationAccording to Newton-Euler method The power in each joint of iterative computation mechanical arm and moment backward；Initial condition is momentPowerWater Lower mechanical arm tail end point can be the most movable in mechanical arm work space, then M_end、F_endIt is zero；

(2.1) the joint angles variable of each connecting rod of Iterative mechanical arm, speed variables forward:

${\mathrm{\&omega;}_{i+1}}_{i+1}=R_{i+1,i}({\mathrm{\&omega;}_i}_i+{\stackrel{\&CenterDot;}{q}}_{i+1}{e_i}_i)$

${\stackrel{\&CenterDot;}{\mathrm{\&omega;}}_{i+1}}_{i+1}=R_{i+1,i}({\stackrel{\&CenterDot;}{\mathrm{\&omega;}}_i}_i+{\stackrel{\&CenterDot;\&CenterDot;}{q}}_{i+1}{e_i}_i+{\stackrel{\&CenterDot;}{q}}_{i+1}{\mathrm{\&omega;}_i}_i\&times;{e_i}_i)$

${\stackrel{\&CenterDot;}{v}_{i+1}}_{i+1}=R_{i+1,i}{\stackrel{\&CenterDot;}{v}_i}_i+{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}_{i+1}}_{i+1}\&times;{l_{i+1}}_{i+1}+{\mathrm{\&omega;}_{i+1}}_{i+1}\&times;({\mathrm{\&omega;}_{i+1}}_{i+1}\&times;{l_{i+1}}_{i+1})$

ⁱv_ci=ⁱv_i+ⁱω_i×ⁱρ_i

${\stackrel{\&CenterDot;}{v}_i}_{ci}={\stackrel{\&CenterDot;}{v}_i}_i+{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}_i}_i\&times;{\mathrm{\&rho;}_i}_i+{\mathrm{\&omega;}_i}_i\&times;({\mathrm{\&omega;}_i}_i\&times;{\mathrm{\&rho;}_i}_i)$

${F_{i+1}}_{i+1}=m_{i+1}{\stackrel{\&CenterDot;}{v}_i}_{ci}$

${}^{i+1}{M}_{i+1}={I_c}_{i+1}{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}_{i+1}}_{i+1}+{\mathrm{\&omega;}_{i+1}}_{i+1}\&times;\left({I_c}_{i+1}{\mathrm{\&omega;}_{i+1}}_{i+1}\right)$

Wherein,ⁱ⁺¹ω_i+1Angular velocity for i+1 joint；Angular acceleration for i+1 joint；R_{I+1, i}For i-th The spin matrix shut down to i+1 in joint；Linear acceleration for i+1 joint；ⁱv_ciBarycenter for i-th joint The linear acceleration at place；Angular velocity for i+1 joint self；ⁱe_iRotation axis for i-th joint；ⁱ⁺¹l_i+1Be i-th+ The length vector of 1 connecting rod；ⁱρ_iIt is tied to the position vector at i connecting rod barycenter for i coordinate；^cI_i+1For i+1 joint to barycenter Inertial tensor at zero；m_i+1Quality for i+1 joint；ⁱ⁺¹F_i+1Inertia force for i+1 joint；ⁱ⁺¹M_i+1 Moment of inertia for i+1 joint；

(2.2) power F of Iterative joint of mechanical arm backward_iWith moment Q_iI=n, n-1 ..., 1:

Wherein,ⁱh_iThe position vector of i connecting rod stress point it is tied to for i coordinate；Joint power for i-th joint；For i-th Joint is relative to the joint moment of i coordinate system；Q^M _iFor i-th joint relative to the joint moment of i-1 coordinate system；

When the weight in robot linkage joint is as Consideration, ifThe support that fixing robot base is subject to is made With gravity acceleration g the most upwards, it is considered to during hydrodynamic factor, auspicious thunder dissipative function is selected to describe viscous damping and fluid resistance The motion conditions of object under power:

Auspicious thunder dissipative function is introduced in lagrangian dynamics algorithm:

$\mathrm{\&phi;}=\frac{1}{2}\underset{i=1}{\overset{N}{\&Sigma;}}(k_{ix}\stackrel{\&CenterDot;}{{x_i}^2}+k_{iy}{y_i}^2+k_{iz}{z_i}^2),$

Can be with the Lagrange's equation of auspicious thunder dissipative function:

$\frac{d}{dt}\frac{\&part;T}{\&part;{\tilde{q}}_s}-\frac{\&part;T}{\&part;q_s}=-\frac{\&part;\mathrm{\&phi;}}{\&part;q_s}+Q_s,(s=1,2,\mathrm{3.....}n),$

Wherein q_sFor broad sense position vector,For corresponding velocity vector, T is Lagrangian；

Owing to Nuton-Euler method obtains the moment without hydrodynamic force variable

Then

$Q_s=\mathrm{\&tau;}+\frac{\&part;\mathrm{\&phi;}}{\&part;q_s},(s=1,2,\mathrm{3.....}n)$

To six degree of freedom submarine mechanical arm deriving analysis, each joint is shown as relative to the speedometer in previous joint^i-1v_i(i=1,2, 3.....6), according to rigid body fixed-axis rotation theorem, draw each joint translational velocity, and then obtain the motion of the center of mass point in each joint Absolute velocity⁰v_i(i=1,2,3.....6)；

Solve the dissipative function φ in each joint of mechanical arm_i(i=1,2 ... ..6),

${\mathrm{\&phi;}}_i=\frac{u^0{v_i}^2}{2}=\frac{u}{2}{\&Integral;}_0^{l_i}v_0{{}_i}^2{\mathrm{dx}}_i$

The dissipative function obtaining submarine mechanical arm system is φ=φ₁+φ₂+φ₃+φ₄+φ₅+φ₆, by auspicious thunder dissipative function band Enter Lagrange's equation to derive, the general velocity of derivation auspicious thunder dissipative function φParameter substitutes into equation Hydrodynamic damping coefficient can be tried to achieve.

The delay control method of a kind of master-slave mode submarine mechanical arm the most according to claim 1, it is characterised in that: step (3) the power curve approximating method described in includes: curve matching is limited to test data (x by experiment acquisition_i,y_i), utilize this A little data ask for approximate function y=f (x), and wherein x is output, and y is measurand, and in delays time to control, the time is defeated Output, joint angles is measurand；Curve-fitting method is applied in principal and subordinate's submarine mechanical arm delays time to control, time delay process Set of time is two parameter cycles, and this method is referred to as the time delay of two cycles, adds the signal caused due to 485 bus transfer Time delay, from three periodic movements of delayed main hands of hands, in the main hands movement i+3 moment, due to the time delay of 485 signals of communication, during i+3 Carve underwater manipulator just received main hands i+2 moment position, the location point in three moment learnt by controller, i.e. the i+3 moment from Hand position, the main hand control signal in i+1 moment, the main hand control signal in i+2 moment, according to joint position temporal information, in advance Judge the motion change trend from hands, thus utilize two cycle delay control methods, calculate subsequent time and transport from hands ideal Dynamic point；

The most main joints of hand i is at t_k-2Time control controlling angle value is θ_k-2, at t_k-1Time control controlling angle value is θ_k-1, from joints of hand at t_kTime Carving actual corners angle value is θ_k, obtain coordinate and be respectively (t_k-2,θ_k-2)、(t_k-1,θ_k-1) and (t_k,θ_k), intend according to joint curvilinear motion Close equation, obtain t_k+1The angle predictive value in moment is θ_k+1:

θ_k+1=θ_k+v(k)×T+aT²/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；

Obtained by uniform acceleration equation:

$\left\{\begin{array}{c}v(k-1)=({\mathrm{\&theta;}}_{k-1}-{\mathrm{\&theta;}}_{k-2})/T\\ v\left(k\right)=({\mathrm{\&theta;}}_k-{\mathrm{\&theta;}}_{k-1})/T\\ a=(v\left(k\right)-v\left(k-1\right))/T\end{array}\right.$

Calculate, obtain t_k+1The ideal value from joints of hand angle in moment

${\mathrm{\&theta;}}_{k+1}=\frac{5}{2}{\mathrm{\&theta;}}_k-2{\mathrm{\&theta;}}_{k-1}+\frac{1}{2}{\mathrm{\&theta;}}_{k-2}$

The final main joints of hand angle by current time from joints of hand angle and two moment of time delay calculates subsequent time from hands Joint velocity, angular velocity, thus obtain preferable movement angle.

The delay control method of a kind of master-slave mode submarine mechanical arm the most according to claim 1, it is characterised in that: step (4) the delays time to control link described in includes: be analyzed main hands movement curve, thus judges whether the main hands of mechanical arm exists change To, accelerate, the mutation movement link of deceleration, and then carry out motor control to from hands；

The differential seat angle that wherein kth walks between main hands output desired value x (k) and kth+1 step main hands output desired value x (k+1) is e K ()=x (k+1)-x (k), carries out discriminatory analysis to this differential seat angle, according to the difference of differential seat angle scope, control from hands next step such as What motion:

When | e (k) | < 0.2 °, main hand control changes, and is in hands movement limit of power, there is not emergency operation, time delay ring The normal work of joint, utilizes multicycle delay algorithm to draw the ideal movements point from hands subsequent time；

When | e (k) | > 0.2 °, now differential seat angle exceeds mechanical arm calibration capability scope, and main hands occurs that jerk avoidance is moved；Machine Mechanical arm stop motion, keeps current location, until main hands signal from newly returning to after hands halted state, restarts motion.

Ocean current feedback compensation link mainly judges whether that mechanical arm is impacted by ocean current interference from hands movement, if ocean current Impact is beyond mechanical arm correction of movement limit of power, it is believed that ocean current interference effect is excessive, stops current kinetic from mobile phone mechanical arm, Until the main hands of mechanical arm continues to follow main hands movement from hands from hands stop position, mechanical arm from newly returning to mechanical arm；If ocean current Affecting less, impact is in manipulator motion limit of power, and mechanical arm carries out appropriate school, position according to main hands target location Just, then carry out time delay process control so that mechanical arm is from hands easy motion；

Wherein kth walks main joints of hand output desired value x (k) and from joints of hand actual motion positionBetween error beThis error is carried out discriminatory analysis, according to the difference of range of error, determines how to move after hands；

When | e (k) | < 0.02 °, after correction from hands output valveWithout correction, keep from hands current kinetic shape State；

When 0.02 ° < when | e (k) | < 0.2 °, after correction from hands output valveError corrects at mechanical arm In range capability, it is corrected from hands current location；

When | e (k) | > 0.2 °, from hands output valve after correctionError exceeds mechanical arm calibration capability scope, machinery Arm stop motion, keeps current location, until main hands signal from newly returning to after hands halted state, restarts motion.