CN103056883B - Double-manipulator coordination control system and double-manipulator coordination control method - Google Patents

Abstract

The invention discloses a double-manipulator coordination control system which comprises a control keyboard, a main manipulator, a display, a main control cabinet and two auxiliary control cabinets. Motion control on two manipulators is realized by collecting pulse data of the keyboard and the main manipulator and sending the pulse data to a lower computer in a form of instructions. An industrial personal computer of the main control cabinet is connected with a computer of the main manipulator and communicates with industrial personal computers, controlling the manipulators, of the auxiliary control cabinets to send out motion instructions. The industrial personal computers of the auxiliary control cabinets plan motion of the manipulators according to the motion instructions of the industrial personal computer of the main control cabinet. Clamping hands and special tools connected at the tail end of the system are utilized to substitute for human arms to complete tasks such as electrified line breaking, electrified line connecting, electrified fallen fuse replacing on a 10KV line with higher electrified operating frequency, so that on one hand, labor intensity can be reduced for operating personnel, and on the other hand, a guarantee is provided for safety of the operating personnel.

Description

A kind of dual robot coordination control system and control method

Technical field

The present invention relates to a kind of Robotics, especially a kind of dual robot coordination control system and control method.

Background technology

In the livewire work technical development stage, livewire work mode can say it is artificial livewire work, and namely during operating personnel's operation, all the moment is in the threat of high voltage, highfield.Artificial livewire work is generally work high above the ground, steps on that bar operation is frequent, operating condition is severe, labour intensity is large, nervous, easily causes human casualty accident.According in 100 the typical livewire work accident statistics in the nineteen ninety-five whole nation, power distribution live-wire accident reaches 55 times, reaches 55%, has 19 people to die from power distribution live-wire accident, account for 61% in 39 people of death.So, in a period of time very long in the past, power distribution live-wire is limited, cause distribution line power failure operation frequent, distribution reliability index can not complete, thus brings very large economic loss to electric power enterprise, brings very large inconvenience to people's lives and production.Therefore, in the national livewire work meeting to hold for 1987, also been proposed the requirement must strengthening power distribution live-wire energetically.

Domestic research in robot, from the middle and later periods eighties, because national high-tech research development plan (i.e. 863 Program) special project is supported, obtains tremendous expansion, has become one of major country studying robot in the world.From the research system that China has been formed, basically formed industrial robot and the large main class of specialized robot two, especially the research of specialized robot has become domestic study hotspot.Specialized robot mainly refers to the robot of working limit environment and hazardous environment and harm health of human body environment, and by force, thus market prospects are good for the necessity required robot due to these environment and urgency.Distribution line live working robot is exactly very representational specialized robot, therefore development has stronger security and adaptive robot for high-voltage hot-line work, overcome difficulty and the limitation of artificial livewire work, people is replaced to carry out livewire work very necessary, and abreast of the times requirement.This kind of robot can alleviate the labour intensity of operating personnel, make operating personnel and high voltage electric field completely isolated, ensure the safety of operating personnel to greatest extent.

Summary of the invention

The object of the invention is for overcoming above-mentioned the deficiencies in the prior art, holding hand, specific purpose tool that a kind of utilization is connected in its end are provided, the arm of people is replaced to complete the tasks such as the higher charged broken string of 10KV line live-line work frequency, live line connection, charged for replacement fall insurance, the labour intensity of operating personnel can be alleviated on the one hand, on the other hand also for the safety of operating personnel provides dual robot coordination control system and the control method of guarantee.

For achieving the above object, the present invention adopts following technical proposals.

A kind of dual robot coordination control system, comprises supervisory keyboard, main operating hand, display, main control cabinet, two from switch board.

The output of supervisory keyboard connects the input of main control cabinet, and the input and output of main control cabinet connect the input and output of display, and the output of main operating hand connects the input of main control cabinet.

Main control cabinet comprises industrial computer, and the network interface of main control cabinet communicates with the network interface from switch board respectively.The industrial computer of described main control cabinet gathers the instruction of supervisory keyboard, is handed down to the industrial computer from switch board by network interface communication, from the industrial computer downloading speed instruction of switch board, servo-drive instruction to motion controller.

Comprise industrial computer, motion controller, collector, voltage-stabilized power supply, motor servo driver from switch board, voltage-stabilized power supply powers to respectively industrial computer, motion controller, motor servo driver, collector; The private of mechanical arm takes motor and is connected with motor servo driver, collector respectively; Motor servo driver is connected with motion controller, and collector, motion controller are connected with industrial computer, and described collector reads absolute location information, then by serial ports, these information is sent to industrial computer from switch board; Described motion controller receives motion control instruction, completes SERVO CONTROL function.

Described mechanical arm comprises the identical left arm of structure and right arm, and the frame for movement of mechanical arm comprises pedestal, and pedestal is provided with support arm, support arm upper end is shoulder joint, and upper arm is arranged in shoulder joint, and the front end of upper arm is connected with elbow joint, elbow joint is connected with forearm, and the end of forearm is wrist; Servomotor, servomotor decelerator are installed in the bottom of support arm, and shoulder joint and elbow joint are provided with absolute value encoder.Upper arm comprises the waist gyroaxis, upper arm pitch axis and the forearm pitch axis that set gradually, provides that waist turns round, the motion of upper arm pitching and forearm pitching.Forearm comprises the wrist pitch axis, wrist swinging shaft, the wrist rotating shaft that set gradually, provides wrist pitching, wrist waves the motion with wrist rotation.

Described main operating hand is identical with the frame for movement of mechanical arm, and main operating hand also comprises orthogonal rotary encoder and control button, when it is mainly used in master slave control mode, and robot position control signal input.Orthogonal rotary encoder is connected in each joint of main operating hand, and to detect each joint position change of main operating hand, main operating hand control button is for controlling the validity of main operating hand position signalling.

Described industrial computer adopts IPC-6806S compact cabinet, and mainboard adopts PCA-6774 mainboard, 1GHz CPU.

Described motion controller adopts PMAC, Digital PID Algorithm, and pid parameter can real time modifying when system cloud gray model, and inner trapezoidal speed trajectory arithmetic unit, can realize automatic deceleration and control.

The input of described motor servo driver adopts 0-10V pattern amount.

A kind of dual robot coordination control method, comprises the steps:

(1) input mechanical arm tail end in the position in space two point (initial point and terminal) and posture, use position auto―control T ₁, T ₂represent;

$T_1=\left[\begin{array}{cccc}{n}_{x1}& o_{x1}& a_{x1}& p_{x1}\\ n_{y1}& o_{y1}& a_{y1}& p_{y1}\\ n_{z1}& o_{z1}& a_{z1}& p_{z1}\\ 0& 0& 0& 1\end{array}\right]$ -----initial position auto―control;

p _x1

P _y1------represents the initial position of mechanical arm tail end in world coordinate system;

p _z1

n _x1o _x1a _x1

N _y1o _y1a _y1------represents the initial attitude of mechanical arm tail end in world coordinate system;

n _z1o _z1a _z1

$T_2=\left[\begin{array}{cccc}{n}_{x2}& o_{x2}& a_{x2}& p_{x2}\\ n_{y2}& o_{y2}& a_{y2}& p_{y2}\\ n_{z2}& o_{z2}& a_{z2}& p_{z2}\\ 0& 0& 0& 1\end{array}\right]$ -----terminal position auto―control;

p _x2

P _y2------represents the final position of mechanical arm tail end in world coordinate system;

p _z2

n _x2o _x2a _x2

N _y2o _y2a _y2------represents the terminal attitude of mechanical arm tail end in world coordinate system;

n _z2o _z2a _z2

(2) carry out inverse kinematic calculating, try to achieve corresponding to T ₁and T ₂two groups of joint variable value (q _o1, q _o2... .., q _oN), (q _f1, q _f2..., q _fN);

(q _o1, q _o2... .., q _oN)------initial joint variable value;

(q _f1, q _f2..., q _fN)-----terminal joint variable value;

N--------is all positive integers between 1 to 6;

(3) speed, acceleration, the initial joint variable value (q of 1 to 6 axles at initial point and terminal is inputted _o1, q _o2... .., q _oN), terminal joint variable value (q _f1, q _f2..., q _fN); Input is lifted a little and the position (q of drop point ₁, q ₂) and run duration T and each orbit segment time Δ τ _i;

(4) each orbit segment multinomial coefficient of 3-5-3 trajectory planning formulae discovery mechanical arm 1 to 6 axle is utilized;

(5) 1 to 6 axles are calculated at the position of each instantaneous τ (0 < τ < T), speed, acceleration;

(6) mechanical arm 1 to the 6 axle characteristics of motion is exported.

The operation principle of double mechanical arms controller of the present invention is: by gathering the pulse data of keyboard and main operating hand, sending to slave computer, thus complete the motion control to two mechanical arms with instruction type.The industrial computer of main control cabinet is the computer connecting main operating hand, carries out communication, send movement instruction by the industrial computer from switch board of serial ports and controller mechanical arm.Plan from the industrial computer of switch board according to the motion of the movement instruction of the industrial computer of main control cabinet to mechanical arm.The motion control task of bottom is realized by the motion controller of Based PC bus.Private takes the servomotor that motor adopts integrated absolute encoder, contact encoders signal interface board be responsible for reading each joint absolute location information and with carrying out communication from the industrial computer of switch board.

Adopt such scheme, the present invention has the following advantages, one be the dual robot coordination system of designed, designed through experimental verification, replace the arm of people to complete the tasks such as the higher charged broken string of 10KV line live-line work frequency, live line connection, charged for replacement fall insurance; Two be embedded mechanical arm from heavy and light, size is little, control system is low in energy consumption and size is little, is applicable to the application needs of mobile operation hot line robot.Three is that double mechanical arms can realize complicated linear interpolation, circular interpolation motion; Four is that system adopts modularized design, has opening, readability, extensibility, maintainability, so that Persisting exploitation.Five is that manipulator motion controller adopts master-slave mode industrial computer, and the industrial computer of main control cabinet realizes the positive and negative solution of kinematics, interpolation algorithm, and realize motion control from the industrial computer of switch board, processing speed is fast.Six be controller with driver control interface, absolute value encoder acquisition port, multiple functional, positional precision is high.

Accompanying drawing explanation

Fig. 1 is structural principle general diagram of the present invention;

Fig. 2 is main operating hand structure chart of the present invention;

Fig. 3 is mechanical arm structural representation of the present invention;

Fig. 4 is mechanical arm coordinate system schematic diagram of the present invention;

Fig. 5 is the total program flow diagram of the present invention;

Fig. 6 is principal and subordinate's movement function flow chart of the present invention;

Fig. 7 is autokinetic movement function flow chart of the present invention;

Fig. 8 is PTP Motion trajectory flow chart of the present invention;

Wherein, 1, supervisory keyboard; 2, main operating hand; 3, display; 4, main control cabinet; 5, first from switch board; 6, second from switch board; 7, the 3rd industrial computer; 8, the first industrial computer; 9, the second industrial computer; 10, motion controller; 11, collector; 12, voltage-stabilized power supply; 13, motor servo driver; 14, left arm; 15, right arm; 16, pedestal; 17, support arm; 18, shoulder joint; 19, upper arm; 20, elbow joint; 21, forearm; 22, wrist; 23, private takes motor; 24, servomotor decelerator; 25, absolute value encoder; 26, orthogonal rotary encoder; 27, control button; 28, paw locking press button; 29, stopping and SR; 30, paw spreading-retracting button; 31, on off state indicator lamp; 32, stop mode status indicator lamp; 33, lock-out state indicator lamp; 34, waist gyroaxis; 35, upper arm pitch axis; 36, forearm pitch axis; 37, wrist pitch axis; 38, wrist swinging shaft; 39, wrist rotating shaft.

Detailed description of the invention

Below in conjunction with drawings and Examples, the present invention is further described.

See Fig. 1, a kind of dual robot coordination control system, comprises supervisory keyboard 1, main operating hand 2, display 3, main control cabinet 4, first from switch board 5; Second from switch board 6.

The output of supervisory keyboard 1 connects the input of main control cabinet 4, and the input and output of main control cabinet 4 connect the input and output of display 3, and the output of main operating hand 2 connects the input of main control cabinet 4.

Main control cabinet 4 comprises the 3rd industrial computer 7, and the network interface of main control cabinet 4 communicates from the network interface of switch board 6 from switch board 5, second with first respectively.3rd industrial computer 7 of described main control cabinet 4 gathers the instruction of supervisory keyboard 1, first is handed down to from the first industrial computer 8, second of switch board 5 from the second industrial computer 9, first industrial computer 8, second industrial computer 9 downloading speed instruction of switch board 6, servo-drive instruction to motion controller 10 by network interface communication.

Described first comprises the first industrial computer 8, motion controller 10, collector 11, voltage-stabilized power supply 12, motor servo driver 13 from switch board 5, and voltage-stabilized power supply 12 powers to respectively the first industrial computer 8, motion controller 9, motor servo driver 13, collector 11; Motor servo driver 13 is connected with motion controller 10, and collector 11, motion controller 10 are connected with the first industrial computer 8, and described collector 11 reads absolute location information, then by serial ports, these information is sent to the first industrial computer 8; Described motion controller 10 receives motion control instruction, completes SERVO CONTROL function.Motion controller 10 is 6 axle motion controllers.Described second is identical from switch board 5 with first from switch board 6, repeats no more.

See Fig. 2, main operating hand 2 is 6DOF, and described main operating hand 2 is identical with the frame for movement of mechanical arm, and main operating hand 2 also comprises orthogonal rotary encoder 26 and control button 27, when it is mainly used in master slave control mode, and robot position control signal input.Orthogonal rotary encoder 26 is connected in each joint of main operating hand 2, and to detect each joint position change of main operating hand 2, the control button 27 of main operating hand 2 is for controlling the validity of main operating hand 2 position signalling.With paw locking press button 28, stopping and SR 29, paw spreading-retracting button 30 in the forearm part of main operating hand 2; The base panel of the upper arm part lower end of main operating hand 2 is provided with on off state indicator lamp 31, stop mode status indicator lamp 32, lock-out state indicator lamp 33.

The input function of supervisory keyboard 1 has: the forward and reverse single action of carrying out three speed and switching of 6 axles of mechanical arm; The world coordinates motion that three speed is forward and reverse and operating coordinates move; The switching of left arm 14 and right arm 15; The work origin of left arm 14 and right arm 15 resets and transport initial point resets; The switching of supervisory keyboard 1/ main operating hand 2; Locked (to prevent user's malfunction) of supervisory keyboard 1; First closes from switch board 5 and second from the servo of switch board 6 and opens; The power-off operation of the 3rd industrial computer 7 and two slave computers; The procedure auto-control of peeling function.

Described first is equipped with control panel from switch board 5 and second from switch board 6, described control panel is provided with working mode selection switch, working mode selection switch (Mode Selection): be three-phase selector switch, for selecting the switching of mechanical arm working method.Electrification in high voltage mechanical arm has three kinds of working methods: reset (Home), programme (Prog), run (Run).Owing to adopting incremental encoder, first must carry out reset work during each start, otherwise other every operations are all invalid.When mechanical arm resets complete, can carry out working method conversion, if will carry out off-line programing to mechanical arm, mechanical arm must be in programing work mode.If make mechanical arm according to the method for operation motion of specifying, mechanical arm must be made to be operated in the method for operation; Operation Mode Selection switch (RunSelection): be three-phase selector switch, for selecting the switching of manipulator motion mode.Electrification in high voltage mechanical arm has three kinds of motion modes: principal and subordinate's motion (Ms), single step run (Single) and automatic operation (Auto) mode.During principal and subordinate's motion mode, mechanical arm pose can change according to the change of main operating hand pose, thus reaches operator's control overflow.During single step run, the manipulator motion program that mechanical arm is worked out according to realization, each run one step.During automatic operation, mechanical body is according to realizing the manipulator motion program of establishment once or circulation self-operating; Scram button (Emergence): when being in an emergency, press scram button, then make servo-drive system hang up, and exits mechanical arm control program, mechanical arm stop motion; Motion start-stop button: be trigger-type button, for startup (Start) and the stopping (Stop) of the motion control of trigger unit mechanical arm.

See Fig. 3, described mechanical arm is 6DOF, described mechanical arm comprises the identical left arm of structure 14 and right arm 15, left arm 14 and right arm 15 are connected first respectively from switch board 5 and second from switch board 6, left arm 14 and right arm 15 are 6DOF, the frame for movement of left arm 14 comprises pedestal 16, pedestal 16 is provided with support arm 17, support arm 17 upper end is shoulder joint 18, upper arm 19 is arranged in shoulder joint 18, the front end of upper arm 19 is connected with elbow joint 20, and elbow joint 20 is connected with forearm 21, and the end of forearm 21 is wrist 22; Servomotor 23, servomotor decelerator 24 are installed in the bottom of support arm 17, and private takes motor 23 and is connected with motor servo driver 13, collector 11 respectively, and shoulder joint 18 and elbow joint 20 are provided with absolute value encoder 25.Described right arm 15 is identical with left arm 14, repeats no more.

See Fig. 4, upper arm 19 comprises the waist gyroaxis, upper arm pitch axis and the forearm pitch axis that set gradually, provides that waist turns round, the motion of upper arm pitching and forearm pitching.

Forearm 21 comprises the wrist pitch axis, wrist swinging shaft, the wrist rotating shaft that set gradually, provides wrist pitching, wrist waves the motion with wrist rotation.

A regular Cartesian coordinates (x can be set up at joint shaft place to each rod member _i, y _i, z _i) (i is all positive integers between 1 to 6, and 6 is number of degrees of freedom), add base coordinate system (x ₀, y ₀, z ₀) (position on support and direction can be optional, as long as z ₀axle is along the first articulating shaft).This invention is determined and set up each coordinate system should according to three rules below: the motion of each axle i (i is all positive integers between 1 to 6, and 6 is number of degrees of freedom) is all around z _iaxle moves; x _ithe vertical z of axle _i-1axle also points to and leaves z _i-1the direction of axle; y _iaxle must ask foundation by right-handed coordinate system.

See Fig. 5, executive program is the core of mechanical arm software systems sequential control, is the starting point of whole software systems.It is operated on the 3rd industrial computer 7, the tasks such as controlling the work of whole mechanical arm software systems, its main completion system initializes, input and output are read in, system state monitoring.Executive program just automatically brings into operation after industrial control computer powers on, and only has when power supply disconnects, just out of service.It adopts the mechanism of cycle detection to carry out timing to each input port and detects, and carrys out sequential control and perform corresponding module function according to input control signal.

See Fig. 6, mechanical arm operates mainly in principal and subordinate's motion mode, the change of main operating hand 2 position that mechanical arm controls according to operator and attitude and changing.Adopting when designing principal and subordinate's movement function the method for timing sampling main operating hand 2 pose change to obtain motion control data, the motion of controller mechanical arm, to reach, function being followed to the pose of main operating hand 2.Therefore, principal and subordinate's movement function control accuracy determined by the sampling time, and the sampling time, shorter control accuracy was higher.

See Fig. 7, when mechanical arm is in automatic motion mode, perform, until press the Stop button on control panel according to the motion control program automatic cycle write in advance.

See Fig. 8, the algorithm of mechanical arm PTP Motion trajectory is as follows:

(1) input mechanical arm tail end in the position in space two point (initial point and terminal) and posture, use position auto―control T ₁, T ₂represent;

$T_1=\left[\begin{array}{cccc}{n}_{x1}& o_{x1}& a_{x1}& p_{x1}\\ n_{y1}& o_{y1}& a_{y1}& p_{y1}\\ n_{z1}& o_{z1}& a_{z1}& p_{z1}\\ 0& 0& 0& 1\end{array}\right]$ -----initial position auto―control;

p _x1

P _y1------represents the initial position of mechanical arm tail end in world coordinate system;

p _z1

n _x1o _x1a _x1

N _y1o _y1a _y1------represents the initial attitude of mechanical arm tail end in world coordinate system;

n _z1o _z1a _z1

$T_2=\left[\begin{array}{cccc}{n}_{x2}& o_{x2}& a_{x2}& p_{x2}\\ n_{y2}& o_{y2}& a_{y2}& p_{y2}\\ n_{z2}& o_{z2}& a_{z2}& p_{z2}\\ 0& 0& 0& 1\end{array}\right]$ -----terminal position auto―control;

p _x2

P _y2------represents the final position of mechanical arm tail end in world coordinate system;

p _z2

n _x2o _x2a _x2

N _y2o _y2a _y2------represents the terminal attitude of mechanical arm tail end in world coordinate system;

n _z2o _z2a _z2

(2) carry out inverse kinematic calculating, try to achieve corresponding to T ₁and T ₂two groups of joint variable value (q _o1, q _o2... .., q _oN), (q _f1, q _f2..., q _fN);

(q _o1, q _o2... .., q _oN)------initial joint variable value;

(q _f1, q _f2..., q _fN)-----terminal joint variable value;

N--------is all positive integers between 1 to 6;

(3) speed, acceleration, the initial joint variable value (q of 1 to 6 axles at initial point and terminal is inputted _o1, q _o2... .., q _oN), terminal joint variable value (q _f1, q _f2..., q _fN); Input is lifted a little and the position (q of drop point ₁, q ₂) and run duration T and each orbit segment time Δ τ _i;

(4) each orbit segment multinomial coefficient of 3-5-3 trajectory planning formulae discovery mechanical arm 1 to 6 axle is utilized;

(5) 1 to 6 axles are calculated at the position of each instantaneous τ (0 < τ < T), speed, acceleration;

(6) mechanical arm 1 to the 6 axle characteristics of motion is exported.

By reference to the accompanying drawings the specific embodiment of the present invention is described although above-mentioned; but not limiting the scope of the invention; one of ordinary skill in the art should be understood that; on the basis of technical scheme of the present invention, those skilled in the art do not need to pay various amendment or distortion that creative work can make still within protection scope of the present invention.

Claims (5)

1. a dual robot coordination control system, comprises supervisory keyboard, main operating hand, display, main control cabinet, two from switch board;

Described mechanical arm comprises the identical left arm of structure and right arm, and the frame for movement of mechanical arm comprises pedestal, and pedestal is provided with support arm, support arm upper end is shoulder joint, and upper arm is arranged in shoulder joint, and the front end of upper arm is connected with elbow joint, elbow joint is connected with forearm, and the end of forearm is wrist; Servomotor, servomotor decelerator are installed in the bottom of support arm, and shoulder joint and elbow joint are provided with absolute value encoder; Upper arm comprises the waist gyroaxis, upper arm pitch axis and the forearm pitch axis that set gradually, provides that waist turns round, the motion of upper arm pitching and forearm pitching; Forearm comprises the wrist pitch axis, wrist swinging shaft, the wrist rotating shaft that set gradually, provides wrist pitching, wrist waves the motion with wrist rotation;

Described main operating hand is identical with the frame for movement of mechanical arm, and main operating hand also comprises orthogonal rotary encoder and control button, when it is mainly used in master slave control mode, and robot position control signal input; Orthogonal rotary encoder is connected in each joint of main operating hand, and to detect each joint position change of main operating hand, main operating hand control button is for controlling the validity of main operating hand position signalling;

The output of supervisory keyboard connects the input of main control cabinet, and the input and output of main control cabinet connect the input and output of display, and the output of main operating hand connects the input of main control cabinet;

It is characterized in that, main control cabinet comprises industrial computer, and the network interface of main control cabinet communicates with the network interface from switch board respectively; The industrial computer of described main control cabinet gathers the instruction of supervisory keyboard, is handed down to the industrial computer from switch board by network interface communication, from the industrial computer downloading speed instruction of switch board, servo-drive instruction to motion controller;

By gathering the pulse data of supervisory keyboard and main operating hand, slave computer is sent to instruction type, thus the motion control completed two mechanical arms, the industrial computer of main control cabinet is the computer connecting main operating hand, communication is carried out by the industrial computer from switch board of serial ports and controller mechanical arm, send movement instruction, contact encoders signal interface board be responsible for reading each joint absolute location information and with carrying out communication from the industrial computer of switch board;

Comprise industrial computer, motion controller, collector, voltage-stabilized power supply, motor servo driver from switch board, voltage-stabilized power supply powers to respectively industrial computer, motion controller, motor servo driver, collector; The servomotor of mechanical arm is connected with motor servo driver, collector respectively; Motor servo driver is connected with motion controller, and collector, motion controller are connected with industrial computer, and described collector reads absolute location information, then by serial ports, these information is sent to industrial computer from switch board; Described motion controller receives motion control instruction, completes SERVO CONTROL function.

2. dual robot coordination control system as claimed in claim 1, is characterized in that, the industrial computer of described main control cabinet and all adopt IPC-6806S compact cabinet from the industrial computer of switch board, and mainboard adopts PCA-6774 mainboard, 1GHz CPU.

3. dual robot coordination control system as claimed in claim 1, is characterized in that, described motion controller adopts PMAC, Digital PID Algorithm, and pid parameter can real time modifying when system cloud gray model, inner trapezoidal speed trajectory arithmetic unit.

4. dual robot coordination control system as claimed in claim 1, is characterized in that, the input of described motor servo driver adopts 0-10V pattern amount.

5. the control method of dual robot coordination control system as claimed in claim 1, is characterized in that, comprise the steps:

(1) input mechanical arm tail end in the position of initial point and terminal and posture, use position auto―control T ₁, T ₂represent;

-----initial position auto―control;

------represents the initial position of mechanical arm tail end in world coordinate system;

------represents the initial attitude of mechanical arm tail end in world coordinate system;

-----terminal position auto―control;

------represents the final position of mechanical arm tail end in world coordinate system;

------represents the terminal attitude of mechanical arm tail end in world coordinate system;

(2) carry out inverse kinematic calculating, try to achieve corresponding to T ₁and T ₂two groups of joint variable value (q _o1, q _o2... .., q _oN), (q _f1, q _f2..., q _fN);

(q _o1, q _o2... .., q _oN)------initial joint variable value;

(q _f1, q _f2..., q _fN)-----terminal joint variable value;

N--------is 6;

(3) speed, acceleration, the initial joint variable value (q of 1 to 6 axles at initial point and terminal is inputted _o1, q _o2... .., q _oN), terminal joint variable value (q _f1, q _f2..., q _fN); Input is lifted a little and the position (q of drop point ₁, q ₂) and run duration T and each orbit segment time Δ τ _i;

(4) each orbit segment multinomial coefficient of 3-5-3 trajectory planning formulae discovery mechanical arm 1 to 6 axle is utilized;

(5) 1 to 6 axles are calculated at the position of each instantaneous τ (0 < τ < T), speed, acceleration;

(6) mechanical arm 1 to the 6 axle characteristics of motion is exported.