Background technology
In the industrial production, industrial robot can substitute for the mankind do that some are more dull, the most frequently and repetitive rate relatively
High long working, or the operation under danger, adverse circumstances, be typically used as moving to take part and assembly work, at micro-electricity
The fields such as sub-manufacturing industry, plastics industry, auto industry, electronics industry, pharmaceutical industries and food industry obtain widely should
With, it is for improving production automation level, labor productivity and economic benefit, guarantee product quality, guaranteeing personal safety, change
Kind work situation, reduces labor intensity, save material consumption and reduce production cost etc. and have highly important meaning.
The SCARA industrial robot i.e. robot arm of assembling work is the industrial robot of a kind of circular cylindrical coordinate type, it
Rely on rotary joint large arm and forearm to realize the quick location in X-Y plane, rely on a wrist linear joint and a hands
Wrist rotary joint does flexible and rotary motion in z-direction, and it has four freedoms of motion, and the manipulator of this series moves at it
The four direction making space has finite stiffness, and has infinitely great rigidity on remaining other two direction.This structure
Characteristic makes SCARA robot be good at and captures object from a bit, is the most quickly placed to another point, therefore SCARA robot
Production line for automatically assembling is widely used.SCARA robot architecture is compact, flexible movements, and speed is fast, position is smart
Degree height, its use substantially increases the robot adaptability to Complex Assembly task, also reduces cost simultaneously, improve work
Make space availability ratio.
SCARA robot combines multi-subject knowledge, for promote the manipulative ability of students, team collaboration's ability and
Innovation ability, the digestion promoting student classroom knowledge and the scope of one's knowledge extending student are the most helpful.SCARA machine is used in teaching
People's technology carry out the talent that can cultivate large quantities of association area, and then promote that the technology development of association area and industrialization are entered
Journey.But SCARA robot to judge the location parameter that master controller inputs during transporting goods the moment, and judges around
Environment moment avoidance, then by motion controller repetitive control, it accelerates accurately and deceleration is transported goods, somewhat
Deviation accumulation is possible to cause transporting unsuccessfully in many bouts move.Although the domestic use to SCARA robot has tens
Year, but owing to domestic industry robot development starting ratio is later, being affected by more key technology, SCARA robot is sent out
Exhibition is also affected by institute, and traditional robot principle is as it is shown in figure 1, there is great number of issues during life-time service:
(1) for using the SCARA robot of motor and DC motor Driver, the rotary inertia of system is big, unfavorable
Quickly accelerating and slowing down in robot.
(2) for using the SCARA robot of motor and DC motor Driver, the staring torque of system is relatively
Little, it is unfavorable for the quick startup of robot.
(3) in the whole motor control process of SCARA robot, all angles of rotation do not have Real-time Feedback, Shi Youzao
The three nonsynchronous phenomenons of axle are become to occur.
(4) in the whole motor control process of SCARA robot, do not have between each turning arm and peripheral environment
The avoidance of effect, causes the phenomenon colliding other goods to occur sometimes.
(5) in most of the cases, accumulation over time, can there is certain site error in robot work, rely on
The method being manually directed to zero position, not only degree of accuracy is the highest, nor the beneficially work such as high density carrying.
(6) in SCARA high-volume handling process, carrying error and quality problems happen occasionally, if do not asked similar
Topic checks out, can leave hidden danger for later motor process.At present for the detection of SCARA work, rely primarily on artificial mesh
The methods of inspection such as survey realize, and this does not only exist the drawbacks such as subjective factors impact big, labor intensity height, inefficiency, and mesh exists
In short time, artificial high-volume inspection SCARA working result is the most unrealistic.
(7) in SCARA high-volume carrying produces, sometimes having important temporary duty and add, common SCARA is often
Complete by the way of shutdown, cause inefficient.
Detailed description of the invention
Technical scheme in the embodiment of the present invention will be clearly and completely described below, it is clear that described enforcement
Example is only a part of embodiment of the present invention rather than whole embodiments.Based on the embodiment in the present invention, this area is common
All other embodiments that technical staff is obtained under not making creative work premise, broadly fall into the model of present invention protection
Enclose.
Referring to Fig. 2 to Fig. 7, the embodiment of the present invention includes: a kind of teaching is with being wirelessly transferred double-core constant speed articulated robot
Control system, this machine artificially SCARA robots based on four axle permanent-magnet DC servo motors, the most described articulated robot uses
Permanent-magnet DC servo motor X7 driven machine people's large arm rotary motion, employing permanent-magnet DC servo motor Y8 driven machine people's forearm
Rotary motion, employing permanent-magnet DC servo motor Z9 driven machine human wrist rotary motion, employing permanent-magnet DC servo motor R10
Driven machine human wrist elevating movement, described robot's arm is provided with avoidance displacement transducer S1 12, avoidance displacement sensing
Device S2 13 and acceleration transducer A1 17, described robot forearm is provided with avoidance displacement transducer S3 14, avoidance displacement
Sensor S4 15 and acceleration transducer A2 18, described robot wrist is provided with alignment sensor S5 16 and acceleration
Sensors A 3 19.
Described control system includes that main website controls computer 1 and controller 2, and described controller 2 includes dsp chip control
Device 3, image capture module 21, wireless transport module 20 and ARM controller 4, described image capture module 21 be wirelessly transferred mould
Between block 20 communicate to connect, described dsp chip controller 3, wireless transport module 20 and ARM controller 4 all with the control of described main website
Computer 1 processed communicates to connect, described dsp chip controller 3, wireless transport module 20 and ARM controller 4 communication link each other
Connect.Described permanent-magnet DC servo motor X7, permanent-magnet DC servo motor Y8, permanent-magnet DC servo motor Z9 and DC servo
Motor R10 all communicates to connect with described dsp chip controller 3, described avoidance displacement transducer S1 12, avoidance displacement transducer
S2 13, avoidance displacement transducer S3 14, avoidance displacement transducer S4 15, alignment sensor S5 16, acceleration transducer A1
17, acceleration transducer A2 18 and acceleration transducer A3 19 all simultaneously with described dsp chip controller 3 and ARM controller 4
Communication connection.
Preferably, the teaching of the present invention also includes for described pass with being wirelessly transferred double-core constant speed articulated robot control system
Joint robot and control system provide main power source 5 and stand-by power supply 6, each described motor in described articulated robot of power supply
All it is electrically connected with described main power source 5 and/or stand-by power supply 6 with the controller 2 in sensor and control system.
Preferably, described permanent-magnet DC servo motor X 7, permanent-magnet DC servo motor Y 8, permanent-magnet DC servo motor Z
The even photoelectric encoder 11 that is provided with on 9 and permanent-magnet DC servo motor R10, described photoelectric encoder 11 and described dsp chip control
Device 3 processed is electrically connected with.
Preferably, described image capture module 21 is CCD camera assembly, is provided with solution in described image capture module 21
Code chip 22.
CCD is a kind of opto-electronic conversion formula imageing sensor, and it utilizes photoelectricity transformation principle that image information is directly changed into
The signal of telecommunication, it is achieved the electrical measurement of non electrical quantity, it also has that volume is little, lightweight, noise is low simultaneously, self-scanning, operating rate are fast,
The plurality of advantages such as certainty of measurement height, life-span length.In order to process carrying result in time, native system adds figure in real time based on CCD
As gathering and wireless transmitting system.CCD camera output analogue signal, is decoded it by decoding chip 22, and becoming can
The digital signal of programming, it is simple to ARM to the process of digital picture and storage, then by wireless system and main website carry out communication and
Transmission.
The control system of the present invention uses DSP(TMS320F28335)+ARM (STM32F746) dual-core controller, at power supply
Under open mode, ARM controller is first to robot stand-by power supply SOC(state-of-charge) and main power source judge, if standby
The energy is relatively low, and controller will send alarm signal by wireless to master station;If stand-by power supply and main power source are working properly, first by
Main website inputs to ARM by wireless device the positional information that transports goods, ARM calculate the parameter of robot servo's system;
SCARA robot relies on image capturing system to be automatically led to zero position, and robot is introduced into self-locking state, and once main website sends
Carrying command, avoidance sensor, alignment sensor and acceleration transducer that robot carries all open, and SCARA robot is pressed
Set the transport path fast removal optimized according to ARM, DSP adjusts SCARA machine in real time according to servo parameter and sensor feedback
People permanent-magnet DC servo motor X, permanent-magnet DC servo motor Y, permanent-magnet DC servo motor Z and permanent-magnet DC servo motor R's
PWM exports, it is achieved the real-time servo of four permanent-magnet DC servo motors controls, and CCD camera detects carrying result, ARM in real time
To the process of digital picture and storage and being transmitted by wireless system, main website automatically detects carrying knot by image comparison technology
Really.If ARM has a question to carrying some position, will be with DSP communication, DSP sends cutoff command makes SCARA robot stop
Car, then carrying information is judged by main website by image comparison, confirms errorless rear by wireless device restarting SCARA
Robot continues being not fully complete of task;Main website is newly added position of transporting goods and believes with ARM communication transfer by wireless device in real time
Breath, by ARM update robot servo's systematic parameter, and with DSP communication.
With reference to Fig. 1, Fig. 2, Fig. 3 and Fig. 4, Fig. 5, Fig. 6, its concrete functional realiey is as follows:
1), after SCARA robot power supply is opened, stand-by power supply SOC and main power source can be judged by ARM, if stand-by power supply
When SOC is relatively low, ARM will be in DSP communication, and DSP will forbid that four permanent-magnet DC servo motor work, motor input PWM ripple are sealed
Lock, alarm sensor sends alarm signal to master station by work and by wireless simultaneously;If battery SOC is normal, SCARA machine
People enters and treats duty, waits the wireless work order sent.
2) main website is passed to ARM controller by wireless device information such as large arm, forearm lengths and elevating lever length,
ARM controller opens ccd image acquisition system, and then SCARA robot relies on image capturing system automatically to guide SCARA machine
People is to the zero-bit set of setting work, and SCARA robot sets each anglec of rotation, wrist lifting height。
3) in order to meet the acceleration and deceleration needs of SCARA robot, the present invention uses such as the movement time ladder diagram of Fig. 6, this
The area that ladder diagram comprises is exactly robot's arm, forearm and wrist angle to be rotated or the height of wrist lifting, for
The control, the present invention is facilitated to use single acceleration model.
4) wireless device sends motor pattern, and SCARA robot reads its mode of operation by ARM, if artificial work
Operation mode, main website starts and ARM controller communication, main website input SCARA robot's arm, forearm and wrist need rotation
Angle,,And SCARA robot wrist needs to rise or the height of declineTo ARM controller, ARM controls
Device starts robot forward according to Denavit-Hartenberg algorithm and solves: ARM controller is first according to the SCARA machine National People's Congress
Arm, forearm and wrist need the angle rotated,,And SCARA robot wrist needs the height of risingCalculate
Position auto-control between adjacent two member coordinates,,,And with 4*4 two-dimensional array mark,,,,It is expressed as follows respectively:
,,
,。
Then formula is passed throughJust can obtain wrist executor complete the position after task and
Attitude, then ARM Yu DSP communication, and transmit be manually entered parameter.
5) wireless device sends motor pattern, and SCARA robot reads its mode of operation, if normal automatic transporting
Mode of operation, main website starts and ARM controller communication, at the beginning of residing for main website input SCARA robot's arm, forearm and wrist
Beginning position and given position three-dimensional coordinate start machine to ARM controller, ARM controller according to Denavit-Hartenberg algorithm
Device people is Converse solved: first ARM controller is obtained according to the X and Y coordinates in large arm, forearm lengths and final three-dimensional coordinate
Large arm needs the angle rotated, and byValue obtainValue, and obtain in wrist according to the Z coordinate in three-dimensional coordinate
The height risen or reduce, finally obtain the anglec of rotation, owing to solvingWhen equation have bilingual, so SCARA
Robot obtains the angle that large arm, forearm and wrist need to rotate,,And SCARA robot wrist needs rising
HighlyAfter, solving result can be optimized by ARM controller, then ARM controller and DSP communication, and robot is watched most
Take kinematic parameter and be transferred to dsp controller.
6) dsp controller accepts the angle that SCARA robot's arm, forearm and wrist need to rotate,,And
SCARA robot wrist needs the height risenAfter, the sensor S1 ~ S5 in large arm, forearm and wrist will open, first
Barrier in each turning arm anglec of rotation is judged by SCARA robot, will be same to DSP and ARM as there is barrier
Time send interrupt requests, ARM and DSP can to interrupt do the very first time response, first have DSP to block four axle PWM wave control signals defeated
Go out, forbid the permanent-magnet DC servo motor X of SCARA robot, permanent-magnet DC servo motor Y, permanent-magnet DC servo motor Z and electricity
Machine permanent-magnet DC servo motor R work, robot is self-locking in original place, then ARM by wireless device to master station send interruption ask
Asking, in order to prevent information from judging by accident, master station's secondary judges the obstacle information in range of movement, master station artificially process barrier,
Prevent barrier impact carrying work.
7) if by image taking results, master station's secondary determines that clear enters moving region, wireless device will be passed through
With DSP communication, DSP according to the speed time curve of Fig. 6 three anglecs of rotation,,It is converted into three DCs to watch
Taking the acceleration of motor, speed and position initial order value, DSP combines motor X, motor Y and motor Z current of electric feedback, light
Photoelectric coder feedback and the feedback of acceleration transducer, adjust motor X, motor Y in real time through internal three Close loop servo control programs
With the PWM wave control signal of motor Z, dsp controller adjusts internal SERVO CONTROL in real time according to the deviation size that three closed loops input
The pid parameter of program, adjusts its anglec of rotation by the number of drive pulses adjusting permanent-magnet DC servo motor, by adjusting forever
The frequency of magnetic DC servo motor control signal realizes the change of angular velocity speed, makes for three axle permanent-magnet DC servo motor moment same
Step work, the location parameter that DSP moment recorder people has moved, and transmitted to master station in real time by wireless.
8) in SCARA robot moving process, the moving obstacle in range of movement is carried out by sensor S1 ~ S4 moment
Judging, if there being barrier to enter range of movement, the large arm of SCARA robot, forearm and wrist are stood according to the curve movement of Fig. 6
I.e. stopping, dsp controller records present rotation angel degree,,Information, and by wireless and main website communication;On barrier
After disappearance, main website, by wireless and ARM communication, is recalculated the anglec of rotation to new position by ARM,,And lead to DSP
News, DSP controls the robot curve according to Fig. 6 again through three sections of movement locus: accelerated motion, uniform motion and deceleration fortune
Dynamic, eventually arrive at set point.
9) in moving process, the angular acceleration that accelerometer moment record large arm, forearm and wrist rotate, and by two
Secondary integration obtains the anglec of rotation of large arm, forearm and wrist, and compared with the position angle angle value set, if deviation is more than setting
Determining threshold values, in the next sampling period, dsp controller adjusts permanent-magnet DC servo motor according to internal three closed-loop servo controller
X, permanent-magnet DC servo motor Y, the pwm control signal of permanent-magnet DC servo motor Z, dsp controller inputs according to three closed loops
Deviation size adjusts the pid parameter of internal SERVO CONTROL program in real time, by adjusting the driving pulse of permanent-magnet DC servo motor
Number adjusts its anglec of rotation, is realized the change of angular velocity speed by the frequency adjusting permanent-magnet DC servo motor control signal
Change, and then eliminate the error of a sampling period generation, make SCARA robot complete task according to setting track.
10) in SCARA robot kinematics, DSP can store in the moment the SCARA robot location of process
Or the reference point of process, and it is calculated relatively next reference point SCARA robot according to these range informations by DSP
Permanent-magnet DC servo motor X, the angle that permanent-magnet DC servo motor Y, permanent-magnet DC servo motor Z are to be run, angle speed
Degree and angular acceleration, dsp controller feeds back in conjunction with angular-rate sensor, photoelectric encoder feedback, according to its internal three closed loops
SERVO CONTROL program obtains the pwm control signal signal of three permanent-magnet DC servo motors, and dsp controller inputs according to three closed loops
Deviation size adjust the pid parameter of internal SERVO CONTROL program in real time, by adjusting the driving arteries and veins of permanent-magnet DC servo motor
Strokes per minute mesh adjusts its anglec of rotation, is realized the change of angular velocity speed by the frequency adjusting permanent-magnet DC servo motor control signal
Change, make SCARA robot quickly move ahead according to setting speed.
11) the three axle anglecs of rotation are completed in SCARA robot,,SERVO CONTROL after, DSP secondary detection accelerate
The integrated value of degree sensor, if it find that SCARA robot three anglecs of rotation after motor process is by external interference,
,When exceeding setting threshold values with the difference of set angle, ccd image acquisition system Real-time Collection carrying information, after ARM processes
Sending image transmitting by wireless device to master station to ask, carrying result detects automatically by image comparison technology in master station, if
Being implicitly present in certain error, master station is started and DSP communication by wireless device, and DSP is three anglec of rotation deviations,,Be converted into the acceleration of three permanent-magnet DC servo motor fine position, speed and position initial order value, DSP in conjunction with
Motor X, motor Y and motor Z current of electric feedback, photoelectric encoder feedback and the feedback of acceleration transducer, through DSP internal three
Close loop servo control program adjusts motor X, motor Y and the PWM wave control signal of motor Z, and dsp controller inputs according to three closed loops
Deviation size adjust the pid parameter of internal SERVO CONTROL program in real time, by adjusting the driving arteries and veins of permanent-magnet DC servo motor
Strokes per minute mesh adjusts its anglec of rotation, is realized the change of angular velocity speed by the frequency adjusting permanent-magnet DC servo motor control signal
Change, make robot's arm, forearm and wrist arrive setting position by the task again of three axle permanent-magnet DC servo motors.
12) angle is completed when large arm, forearm and the wrist of SCARA robot,,Angle compensation arrive set position
Postponing, DSP is according to the speed time curve of Fig. 6, distance to be lifted for wristIt is converted into adding of permanent-magnet DC servo motor R
Speed, speed and position initial order value, then DSP combines the current feedback of motor R, photoelectric encoder feedback and sensor S5
Feedback, adjust the PWM wave control signal of motor R in real time through internal three Close loop servo control programs, dsp controller closes according to three
The deviation size of ring input adjusts the pid parameter of internal SERVO CONTROL program in real time, by adjusting permanent-magnet DC servo motor
Number of drive pulses adjusts its anglec of rotation, realizes angular velocity by the frequency adjusting permanent-magnet DC servo motor control signal fast
Slow change, makes wrist steadily arrive setting position within the setting time.
13) if SCARA robot finds that location parameter solves and occurs that endless loop will be in ARM sends in motor process
Disconnected request, ARM can to interrupting doing very first time response, ARM controller will immediately with DSP communication, DSP blocks four permanent magnetism immediately
The control signal of DC servo motor, robot original place self-locking;ARM controller carries out communication, CCD by wireless device with main website
Moment gathers peripheral information and by being wirelessly transferred, and image acquisition information is analyzed by main website, and according to collection result secondary
Restart SCARA robot, input carrying information by wireless device secondary to SCARA robot.
14) permanent-magnet DC servo motor X, permanent-magnet DC servo motor Y, permanent-magnet DC servo motor Z, DC it are contained in
Photoelectric encoder on servomotor R can export its position signalling A and position signalling B, the position signalling A pulse of photoelectric encoder
Often changing once with B pulsed logic state, the location register in DSP can add 1 according to the traffic direction of motor or subtract 1;
When the position signalling A pulse of photoelectric encoder and B pulse and Z pulse are low level simultaneously, just produce an INDEX signal and give
DSP internal register, record permanent-magnet DC servo motor absolute position, be then convert into SCARA robot's arm, forearm or
Person is wrist particular location in three-dimensional coordinate system, and DSP sends data transmission information to master station in real time by wireless device.
15) AC ac main power is monitored by SCARA robot in the running ARM controller moment, if controlled
Device find main power source break down unexpected power-off time, ARM with DSP communication, and can open stand-by power supply, stand-by power supply be four axles
Permanent-magnet DC servo motor provides energy, and adjusts four permanent magnetism in real time by the internal three closed loop servo-control system programs of DSP
The PWM output of DC servo motor, adjusts its anglec of rotation by the number of drive pulses adjusting permanent-magnet DC servo motor, logical
The frequency crossing adjustment permanent-magnet DC servo motor control signal realizes the change of angular velocity speed, makes SCARA robot complete this
Secondary carrying task original place self-locking, then SCARA robot carries out communication with main website by wireless, and notice main website overhauls.
16) in SCARA robot in handling process, if main website has new task to input, main website first passes through without traditional thread binding
Put and ARM communication transmit new task, ARM controller calculate the SERVO CONTROL location parameter made new advances and with DSP communication,
First DSP completes current task, and updates next servo-controlled location parameter.
17) in SCARA robot in handling process, it is right that collection image based on CCD can be automatically analyzed by main website
Ratio, when having problems such as a certain position of pinpointing the problems, main website first passes through wireless device and DSP communication, and first DSP completes as predecessor
Business is also stopped setting zero position, has main website automatically to detect carrying result, automatically after detection by ARM update DSP next
Individual servo-controlled location parameter.
18) in SCARA robot Multi-asis servo system work process, if DSP servo controller detects some
There is pulsation in the torque of permanent-magnet DC servo motor, the permanent-magnet DC servo motor moment used due to the present invention and current of electric
In direct ratio, therefore controller can be easy to compensate this interference, and adjust the PID ginseng of electric current loop in real time according to interference size DSP
Number, decreases the motor torque disturbance impact on SCARA robot kinematics.
19) task is completed when SCARA robot, it is achieved during the zero of position, during the acceleration transducer A1 that it carries ~ A3 meeting
Carve and detect its acceleration, when acceleration exceedes pre-set threshold value, a half period zones on next cycle dsp controller can be revised
The error come, is simultaneously based on ccd image acquisition system real time operation, and guided robot homing guidance sets zero-bit to robot
Putting, SCARA robot realizes the playback from certain point to zero-bit according to the Velocity-time curve movement of Fig. 6, and then zero-bit is certainly
Lock, waits next carrying command.
The foregoing is only embodiments of the invention, not thereby limit the scope of the claims of the present invention, every utilize this
Equivalent structure or equivalence flow process that bright description is made convert, or are directly or indirectly used in other relevant technology neck
Territory, is the most in like manner included in the scope of patent protection of the present invention.