CN203019366U - Double-mechanical-arm coordinated control system - Google Patents

Abstract

The utility model discloses a double-mechanical-arm coordinated control system comprising a control keyboard, a main manipulation hand, a display, a main control cabinet and two secondary control cabinets. According to the double-mechanical-arm coordinated control system disclosed by the utility model, pulse data of the data keyboard and the manipulation hand is collected to be sent to a lower computer in an instruction form, so as to finish the control over the movement of two mechanical arms. An industrial personal computer of the main control cabinet is a computer connected with the main manipulation hand and is communicated with industrial personal computers of the secondary control cabinets for controlling the mechanical arms through serial ports, so as to send out a movement instruction. The industrial personal computers of the secondary control cabinets are used for planning the movement of the mechanical arms according to the movement instruction of the main control cabinet. According to the double-mechanical-arm coordinated control system disclosed by the utility model, a clamping hand connected with the tail end and a special tool to replace arms of people to finish tasks of electrified line breaking, electrified line connection, electrified dropped fuse replacement and the like with higher electrified working frequency of 10-KV line; and on one hand, the labor intensity of workers can be reduced, and on the other hand, the safety of the workers is guaranteed.

Description

A kind of dual robot coordination control system

Technical field

The utility model relates to a kind of Robotics, especially a kind of dual robot coordination control system.

Background technology

The livewire work mode can say artificial livewire work in the livewire work technical development stage, i.e. all constantly be in the threat of high voltage, highfield during operating personnel's operation.Artificial livewire work is generally work high above the ground, steps on that the bar operation is frequent, operating condition is abominable, labour intensity is large, nervous, easily causes human casualty accident.In the typical livewire work accident statistics in the nineteen ninety-five whole nation 100 times, the power distribution live-wire accident reaches 55 times, reaches 55%, has 19 people to die from the power distribution live-wire accident in 39 people of death, accounts for 61%.So, in very long a period of time, power distribution live-wire is limited in the past, cause the distribution line power failure operation frequent, the distribution reliability index can not be completed, thereby has brought very large economic loss to electric power enterprise, gives people's lives and produces and brought very large inconvenience.Therefore, in the national livewire work meeting of holding in 1987, proposed again to strengthen energetically the requirement of power distribution live-wire.

Domestic research aspect robot from the middle and later periods eighties, is supported because national high-tech research development plan (being 863 Program) is special, has obtained very big development, has become one of major country of studying in the world robot.From the research system that China has formed, the research that has basically formed industrial robot and specialized robot two large main class, especially specialized robots 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 because these environment are strong to necessity and the urgency that robot requires, thereby market prospects are good.The 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, replace the people to carry out livewire work very necessary, and abreast of the times requirement.This kind robot can alleviate operating personnel's labour intensity, and operating personnel and high voltage electric field are isolated fully, guarantees to greatest extent operating personnel's safety.

The utility model content

The purpose of this utility model is for overcoming above-mentioned the deficiencies in the prior art, the clamping hand, the specific purpose tool that provide a kind of utilization to be connected in its end, replace people's arm to complete the tasks such as the higher charged broken string of 10KV line live-line work frequency, charged wiring, charged for replacement fall insurance, can alleviate operating personnel's labour intensity on the one hand, the dual robot coordination control system of assurance also is provided for operating personnel's safety on the other hand.

For achieving the above object, the utility model adopts following technical proposals.

A kind of dual robot coordination control system comprises that supervisory keyboard, main operating hand, display, main control cabinet, two are 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 is communicated by letter 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 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 is given respectively industrial computer, motion controller, motor servo driver, collector power supply; 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 the SERVO CONTROL function.

Described mechanical arm comprises left arm and the right arm that structure is identical, and the frame for movement of mechanical arm comprises pedestal, and support arm is installed on pedestal, the support arm upper end is shoulder joint, and upper arm is arranged on 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 waist gyroaxis, upper arm pitch axis and the forearm pitch axis that sets gradually, and the motion of waist revolution, upper arm pitching and forearm pitching is provided.Forearm comprises wrist pitch axis, wrist swinging shaft, the wrist rotating shaft that sets gradually, and provides wrist pitching, wrist to wave the motion of rotating with wrist.

The frame for movement of described main operating hand and mechanical arm is identical, and main operating hand also comprises quadrature rotary encoder and control button, when it is mainly used in the master slave control mode, and robot position control signal input.The quadrature rotary encoder is connected in each joint of main operating hand, changes to detect each joint position of main operating hand, and the main operating hand control button is used 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, but real time modifying when pid parameter moves in system, and inner trapezoidal speed track arithmetic unit can be realized automatic deceleration control.

0-10V pattern amount is adopted in the input of described motor servo driver.

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

(1) position and the posture of input mechanical arm tail end 2 points (initial point and terminal point) in the space, use the pose matrix T ₁, T ₂Expression;

${\mathrm{<figure-callout\; id="1"\; label="T"\; filenames="HDA00002728575800011.png,HDA00002728575800081.png"\; state="\{\{state\}\}">T</figure-callout>}}_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}& {\mathrm{<figure-callout\; id="1"\; label="p\; z"\; filenames="HDA00002728575800011.png,HDA00002728575800081.png"\; state="\{\{state\}\}">p</figure-callout>}}_z\\ 0& 0& 0& 1\end{array}\right]$ -----initial pose matrix;

$\begin{array}{c}{p}_{x1}\\ p_{y1}\\ {\mathrm{<figure-callout\; id="1"\; label="p\; z"\; filenames="HDA00002728575800011.png,HDA00002728575800081.png"\; state="\{\{state\}\}">p</figure-callout>}}_z\end{array}$ The initial position of------expression mechanical arm tail end in world coordinate system;

$\begin{array}{ccc}{n}_{x1}& o_{x1}& a_{x1}\\ n_{y1}& o_{y1}& a_{y1}\\ n_{z1}& o_{z1}& a_{\mathrm{<figure-callout\; id="1"\; label="z"\; filenames="HDA00002728575800011.png,HDA00002728575800081.png"\; state="\{\{state\}\}">z</figure-callout>}1}\end{array}$ The initial attitude of------expression mechanical arm tail end in world coordinate system;

${\mathrm{<figure-callout\; id="2"\; label="T"\; filenames="HDA00002728575800011.png,HDA00002728575800021.png"\; state="\{\{state\}\}">T</figure-callout>}}_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}& {\mathrm{<figure-callout\; id="2"\; label="p\; z"\; filenames="HDA00002728575800011.png,HDA00002728575800021.png"\; state="\{\{state\}\}">p</figure-callout>}}_z\\ 0& 0& 0& 1\end{array}\right]$ -----terminal point pose matrix;

$\begin{array}{c}{p}_{x2}\\ p_{y2}\\ {\mathrm{<figure-callout\; id="2"\; label="p\; z"\; filenames="HDA00002728575800011.png,HDA00002728575800021.png"\; state="\{\{state\}\}">p</figure-callout>}}_z\end{array}$ The final position of------expression mechanical arm tail end in world coordinate system;

$\begin{array}{ccc}{n}_{x2}& o_{x2}& a_{x2}\\ n_{y2}& o_{y2}& a_{y2}\\ n_{z2}& o_{z2}& a_{\mathrm{<figure-callout\; id="2"\; label="z"\; filenames="HDA00002728575800011.png,HDA00002728575800021.png"\; state="\{\{state\}\}">z</figure-callout>}2}\end{array}$ The terminal point attitude of------expression mechanical arm tail end in world coordinate system;

(2) carry out inverse kinematic and calculate, 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 point joint variable value;

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

(3) input 1 to 6 axle is at speed, acceleration, the initial joint variable value (q of initial point and terminal point _o1, q _o2... .., q _oN), terminal point 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) utilize each orbit segment multinomial coefficient of 3-5-3 trajectory planning formula calculator tool arm 1 to 6 axle;

(5) calculate 1 to 6 axle at each instantaneous τ (0＜τ＜position T), speed, acceleration;

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

The operation principle of double mechanical arms controller of the present utility model is: by gathering the pulse data of keyboard and main operating hand, send to slave computer with instruction type, thereby complete the motion control to two mechanical arms.The industrial computer of main control cabinet is the computer that connects main operating hand, carries out communication by serial ports and the industrial computer from switch board of controlling mechanical arm, sends movement instruction.The motion of mechanical arm is planned according to the movement instruction of the industrial computer of main control cabinet from the industrial computer of switch board.The motion control task of bottom is by realizing based on the motion controller of PC bus.Private takes the servomotor that motor adopts integrated absolute encoder, and the contact encoders signal interface board is responsible for reading the absolute location information in each joint and is carried out communication with the industrial computer from switch board.

Adopt such scheme, the utlity model has following advantage, the one, the dual robot coordination system of designed, designed replaces people's arm to complete the tasks such as the higher charged broken string of 10KV line live-line work frequency, charged wiring, charged for replacement fall insurance through experimental verification; The 2nd, embedded mechanical arm from heavy and light, size is little, control system is low in energy consumption and size is little, is fit to the application needs of move operation hot line robot.The 3rd, double mechanical arms can be realized complicated linear interpolation, circular interpolation motion; The 4th, system adopts modularized design, has opening, readability, extensibility, maintainability, in order to continue exploitation.The 5th, the manipulator motion controller adopts the master-slave mode industrial computer, and the industrial computer of main control cabinet is realized the positive and negative solution of kinematics, interpolation algorithm, realizes motion control from the industrial computer of switch board, and processing speed is fast.The 6th, controller is with driver control interface, absolute value encoder acquisition port, and multiple functional, positional precision is high.

Description of drawings

Fig. 1 is the utility model structural principle general diagram;

Fig. 2 is the utility model main operating hand structure chart;

Fig. 3 is the utility model mechanical arm structural representation;

Fig. 4 is the utility model mechanical arm coordinate system schematic diagram;

Fig. 5 is the total program flow diagram of the utility model;

Fig. 6 is the utility model principal and subordinate movement function flow chart;

Fig. 7 is the utility model autokinetic movement function flow chart;

Fig. 8 is the utility model PTP movement locus planning flow chart;

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; 26, quadrature rotary encoder; 27, control button; 28, paw locking press button; 29, stop 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.

The specific embodiment

Below in conjunction with drawings and Examples, the utility model is further illustrated.

Referring to Fig. 1, a kind of dual robot coordination control system comprises that supervisory keyboard 1, main operating hand 2, display 3, main control cabinet 4, first are 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 is communicated by letter from the network interface of switch board 6 from switch board 5, second with first respectively.The 3rd industrial computer 7 of described main control cabinet 4 gathers the instruction of supervisory keyboard 1, be handed down to first the first industrial computer 8 from switch board 5, second the second industrial computer 9, the first industrial computers 8 from switch board 6, the second industrial computer 9 downloading speed instructions, 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 is given respectively the first industrial computer 8, motion controller 9, motor servo driver 13, collector 11 power supplies; 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 the 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.

Referring to 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 quadrature rotary encoder 26 and control button 27, when it is mainly used in the master slave control mode, and robot position control signal input.Quadrature rotary encoder 26 is connected in each joint of main operating hand 2, changes to detect main operating hand 2 each joint positions, and the control button 27 of main operating hand 2 is used for controlling the validity of main operating hand 2 position signallings.The forearm of main operating hand 2 is partly gone up with paw locking press button 28, is stopped and SR 29, paw spreading-retracting button 30; Be provided with on off state indicator lamp 31, stop mode status indicator lamp 32, lock-out state indicator lamp 33 on the base panel of the upper arm part lower end of main operating hand 2.

The input function of supervisory keyboard 1 has: the forward and reverse single action of carrying out three speed switchings of 6 axles of mechanical arm; Three world coordinates motion and work coordinate motions that speed is forward and reverse; The switching of left arm 14 and right arm 15; The work origin of left arm 14 and right arm 15 resets and resets with the transportation initial point; The switching of supervisory keyboard 1/ main operating hand 2; Locked (to prevent user's malfunction) of supervisory keyboard 1; First closes and opens from the servo of switch board 6 from switch board 5 and second; 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 the working mode selection switch, working mode selection switch (Mode Selection): be the three-phase selector switch, be used for selecting the switching of mechanical arm working method.The electrification in high voltage mechanical arm has three kinds of working methods: reset (Home), programming (Prog), operation (Run).Due to the employing incremental encoder, the work that must at first reset when at every turn starting shooting, otherwise other every operations are all invalid.When mechanical arm resets completely, can carry out working method conversion, if mechanical arm is carried out off-line programing, mechanical arm must be in the programing work mode.If make mechanical arm according to the method for operation motion of appointment, must make mechanical arm be operated in the method for operation; Operation Mode Selection switch (Run Selection): be the three-phase selector switch, be used for selecting the switching of mechanical arm motion mode.The 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, the mechanical arm pose can change according to the variation of main operating hand pose, controls requirement thereby reach the operator.During single step run, mechanical arm is according to the manipulator motion program that realizes establishment, and each run one goes on foot.Automatically during operation, mechanical body according to the manipulator motion program that realizes establishment once or the circulation self-operating; Scram button (Emergence): when being in an emergency, press scram button, servo-drive system is hung up, withdraw from the mechanical arm control program, the mechanical arm stop motion; Motion start-stop button: be the trigger-type button, be used for the startup (Start) of Crush trigger arm motion control and stop (Stop).

Referring to Fig. 3, described mechanical arm is 6DOF, described mechanical arm comprises structure identical left arm 14 and right arm 15, left arm 14 is connected with right arm and is connected respectively first 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, support arm 17 is installed on pedestal 16, support arm 17 upper ends are shoulder joint 18, upper arm 19 is arranged on 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.Described right arm 15 is identical with left arm 14, repeats no more.

Referring to Fig. 4, upper arm 19 comprises waist gyroaxis, upper arm pitch axis and the forearm pitch axis that sets gradually, and the motion of waist revolution, upper arm pitching and forearm pitching is provided.

Forearm 21 comprises wrist pitch axis, wrist swinging shaft, the wrist rotating shaft that sets gradually, and provides wrist pitching, wrist to wave the motion of rotating with wrist.

Can set up a regular Cartesian coordinates (x to each rod member at the joint shaft place _i, y _i, z _i) (i is all positive integers between 1 to 6, and 6 is free degree number), add base coordinate system (x ₀, y ₀, z ₀) (position on support and direction can be chosen wantonly, as long as z ₀Axle gets final product along the first articulating shaft).This utility model is determined and set up each coordinate system should be according to following three rules: the motion of each axle i (i is all positive integers between 1 to 6, and 6 is free degree number) is all around z _iThe axle motion; x _iThe vertical z of axle _i-1Axle also points to and leaves z _i-1The direction of axle; y _iAxle must be asked foundation by right-handed coordinate system.

Referring to 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, is used for controlling the work of whole mechanical arm software systems, and its main completion system initializes, the tasks such as input and output are read in, system state monitoring.Executive program just brings into operation after industrial control computer powers on automatically, only has when power supply disconnects, and is just out of service.It adopts the mechanism of cycle detection to carry out the timing detection to each input port, comes sequential control and carries out the corresponding module function according to input control signal.

Referring to Fig. 6, mechanical arm operates mainly in principal and subordinate's motion mode, and main operating hand 2 positions that mechanical arm is controlled according to the operator and the variation of attitude change.The method that adopts timing sampling main operating hand 2 poses to change when design principal and subordinate movement function obtains the motion control data, controls the motion of mechanical arm, to reach, the pose of main operating hand 2 is followed function.Therefore, principal and subordinate's movement function control accuracy determined by the sampling time, and the sampling time, shorter control accuracy was higher.

Referring to Fig. 7, when mechanical arm is in automatic motion mode, carry out according to the motion control program automatic cycle that writes in advance, until press Stop button on control panel.

Referring to Fig. 8, the algorithm of mechanical arm PTP movement locus planning is as follows:

(1) position and the posture of input mechanical arm tail end 2 points (initial point and terminal point) in the space, use the pose matrix T ₁, T ₂Expression;

${\mathrm{<figure-callout\; id="1"\; label="T"\; filenames="HDA00002728575800011.png,HDA00002728575800081.png"\; state="\{\{state\}\}">T</figure-callout>}}_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}& {\mathrm{<figure-callout\; id="1"\; label="p\; z"\; filenames="HDA00002728575800011.png,HDA00002728575800081.png"\; state="\{\{state\}\}">p</figure-callout>}}_z\\ 0& 0& 0& 1\end{array}\right]$ -----initial pose matrix;

$\begin{array}{c}{p}_{x1}\\ p_{y1}\\ {\mathrm{<figure-callout\; id="1"\; label="p\; z"\; filenames="HDA00002728575800011.png,HDA00002728575800081.png"\; state="\{\{state\}\}">p</figure-callout>}}_z\end{array}$ The initial position of------expression mechanical arm tail end in world coordinate system;

$\begin{array}{ccc}{n}_{x1}& o_{x1}& a_{x1}\\ n_{y1}& o_{y1}& a_{y1}\\ n_{z1}& o_{z1}& a_{\mathrm{<figure-callout\; id="1"\; label="z"\; filenames="HDA00002728575800011.png,HDA00002728575800081.png"\; state="\{\{state\}\}">z</figure-callout>}1}\end{array}$ The initial attitude of------expression mechanical arm tail end in world coordinate system;

${\mathrm{<figure-callout\; id="2"\; label="T"\; filenames="HDA00002728575800011.png,HDA00002728575800021.png"\; state="\{\{state\}\}">T</figure-callout>}}_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}& {\mathrm{<figure-callout\; id="2"\; label="p\; z"\; filenames="HDA00002728575800011.png,HDA00002728575800021.png"\; state="\{\{state\}\}">p</figure-callout>}}_z\\ 0& 0& 0& 1\end{array}\right]$ -----terminal point pose matrix;

$\begin{array}{c}{p}_{x2}\\ p_{y2}\\ {\mathrm{<figure-callout\; id="2"\; label="p\; z"\; filenames="HDA00002728575800011.png,HDA00002728575800021.png"\; state="\{\{state\}\}">p</figure-callout>}}_z\end{array}$ The final position of------expression mechanical arm tail end in world coordinate system;

$\begin{array}{ccc}{n}_{x2}& o_{x2}& a_{x2}\\ n_{y2}& o_{y2}& a_{y2}\\ n_{z2}& o_{z2}& a_{\mathrm{<figure-callout\; id="2"\; label="z"\; filenames="HDA00002728575800011.png,HDA00002728575800021.png"\; state="\{\{state\}\}">z</figure-callout>}2}\end{array}$ The terminal point attitude of------expression mechanical arm tail end in world coordinate system;

(2) carry out inverse kinematic and calculate, 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 point joint variable value;

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

(3) input 1 to 6 axle is at speed, acceleration, the initial joint variable value (q of initial point and terminal point _o1, q _o2... .., q _oN), terminal point 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) utilize each orbit segment multinomial coefficient of 3-5-3 trajectory planning formula calculator tool arm 1 to 6 axle;

(5) calculate 1 to 6 axle at each instantaneous τ (0＜τ＜position T), speed, acceleration;

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

Although above-mentionedly by reference to the accompanying drawings the specific embodiment of the present utility model is described; but be not the restriction to the utility model protection domain; one of ordinary skill in the art should be understood that; on the basis of the technical solution of the utility model, those skilled in the art do not need to pay various modifications that creative work can make or distortion still in protection domain of the present utility model.

Claims (3)

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

Described mechanical arm comprises left arm and the right arm that structure is identical, and the frame for movement of mechanical arm comprises pedestal, and support arm is installed on pedestal, the support arm upper end is shoulder joint, and upper arm is arranged on 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 waist gyroaxis, upper arm pitch axis and the forearm pitch axis that sets gradually, and the motion of waist revolution, upper arm pitching and forearm pitching is provided; Forearm comprises wrist pitch axis, wrist swinging shaft, the wrist rotating shaft that sets gradually, and provides wrist pitching, wrist to wave the motion of rotating with wrist;

The frame for movement of described main operating hand and mechanical arm is identical, and main operating hand also comprises quadrature rotary encoder and control button, when it is mainly used in the master slave control mode, and robot position control signal input; The quadrature rotary encoder is connected in each joint of main operating hand, changes to detect each joint position of main operating hand, and the main operating hand control button is used 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 is communicated by letter 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 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 is given respectively industrial computer, motion controller, motor servo driver, collector power supply; 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 the SERVO CONTROL function.

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

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

Images