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 heavy duty is wirelessly transferred four core constant speed articulated robot controls
System processed, this machine artificially SCARA robots based on four axle rare-earth permanent-magnetic synchronous servomotors, the most described articulated robot is adopted
With rare-earth permanent-magnetic synchronous servomotor X7 driven machine people's large arm rotary motion, rare-earth permanent-magnetic synchronous servomotor Y8 is used to drive
Robot forearm rotary motion, employing rare-earth permanent-magnetic synchronous servomotor Z9 driven machine human wrist rotary motion, employing rare earth
Permanent magnet synchronous servo motor R10 driven machine human wrist elevating movement, described robot's arm is provided with avoidance displacement sensing
Device S1 12, avoidance displacement transducer S2 13, gyroscope 29 and acceleration transducer A1 17, described robot forearm is installed
There are avoidance displacement transducer S3 14, avoidance displacement transducer S4 15, gyroscope 29 and acceleration transducer A2 18, described machine
Alignment sensor S5 16, gyroscope 29 and acceleration transducer A3 19 are installed in device human wrist.
Described control system includes that main website controls computer 1 and controller 2, and described controller 2 includes dsp chip control
Device 3, voice recognition processor 28, PlC controller 20, image capture module 26, wireless transport module 25 and ARM controller 4, institute
State and communicate to connect between image capture module 26 and wireless transport module 25, described dsp chip controller 3, voice recognition processing
Device 28 and PlC controller 20 all controls computer 1 with described main website and communicates to connect, described dsp chip controller 3, speech recognition
Processor 28 and ARM controller 4 are communicatively connected to each other, and described wireless transport module 25 and ARM controller 4 communicate to connect, described
PlC controller 20 communicates to connect with ARM controller 4, is provided with decoding chip 27 in described image capture module 26.
Described rare-earth permanent-magnetic synchronous servomotor X7, rare-earth permanent-magnetic synchronous servomotor Y8, rare-earth permanent-magnetic synchronous servo electricity
Machine Z9 and rare-earth permanent-magnetic synchronous servomotor R10 all communicates to connect with described dsp chip controller 3, described avoidance displacement sensing
Device S1 12, avoidance displacement transducer S2 13, avoidance displacement transducer S3 14, avoidance displacement transducer S4 15, orientation sensing
Device S5 16, gyroscope 29, acceleration transducer A1 17, acceleration transducer A2 18 and acceleration transducer A3 19 are all simultaneously
Communicate to connect with described dsp chip controller 3, PLC 20 and ARM controller 4.
Preferably, the heavy duty of the present invention is wirelessly transferred four core constant speed articulated robot control systems and also includes for described joint
Robot and control system provide main power source 5 and the stand-by power supply 6 of power supply, each the described motor in described articulated robot and
Controller 2 in sensor and control system is all electrically connected with described main power source 5 and/or stand-by power supply 6.
Preferably, described rare-earth permanent-magnetic synchronous servomotor X 7, rare-earth permanent-magnetic synchronous servomotor Y 8, rare earth permanent magnet are together
The even photoelectric encoder 11 that is provided with on step servomotor Z 9 and rare-earth permanent-magnetic synchronous servomotor R10, described photoelectric encoder 11
It is electrically connected with described dsp chip controller 3.
Preferably, described robot's arm is provided with magnetoelectric transducer EM1 21, described robot forearm is provided with
Magnetoelectric transducer EM2 22, described robot wrist is provided with magnetoelectric transducer EM3 23 and EM4 24, described magnetic-electric sensing
Device EM1 21, magnetoelectric transducer EM2 22, magnetoelectric transducer EM3 23 and EM4 24 all with described dsp chip controller 3 and ARM
Controller 4 communicates to connect.These magnetoelectric transducers read respective zero position mark respectively, when four all detect signal
Time, SCARA robot realizes accurately resetting, improves the degree of accuracy of reset.
The present invention uses dsp controller 3(TMS320F2812) voice recognition processor 28(is chip-shaped for+PLC 20+
Number LD3320)+ARM controller 4(STM32F746) four cores carry out system control.Under power-on state, ARM controller is first
Dynamically edit the key words list of LD3320, increase robot voice discrimination, then to robot stand-by power supply SOC(lotus
Electricity condition) and main power source judge, if stand-by power source is relatively low, controller will by wireless device to work master station send
Alarm signal;If stand-by power supply and main power source are working properly, first by master station by PLC or based on LD3320 voice
Identification system, transporting goods large arm, forearm and the wrist anglec of rotation and lifting information inputs to ARM, is calculated machine by ARM
The parameter queue of device people's servosystem;Zero position sensor EM1, EM2, EM3 and EM4 that SCARA robot carries and based on CCD
Image capturing system start working, the two guided robot reset to set zero position, robot enter self-locking state;Once
After carrying command starts, avoidance sensor, alignment sensor and acceleration transducer that robot carries all are opened, SCARA machine
Device people is according to setting the transport path fast removal that ARM optimizes, and DSP adjusts in real time according to servo parameter and sensor feedback
SCARA robot rare-earth permanent-magnetic synchronous servomotor X, rare-earth permanent-magnetic synchronous servomotor Y, rare-earth permanent-magnetic synchronous servomotor Z
Export with the PWM of rare-earth permanent-magnetic synchronous servomotor R, it is achieved the real-time servo of four rare-earth permanent-magnetic synchronous servomotors controls,
CCD camera detects carrying result in real time, and ARM to the process of digital picture and storage and is transmitted by wireless system, and main website leads to
Cross image comparison technology and automatically detect carrying result.If ARM has a question to carrying some position, will be with DSP communication, DSP
Sending cutoff command makes SCARA robot stop, and then carrying information is judged by main website by image comparison, confirms errorless
Being not fully complete of task is continued afterwards by wireless device restarting SCARA robot;In robot kinematics, if run into
Emergency, main website controls computer 1 will input control command, speech recognition device LD3320 by microphone apparatus to controller
With ARM communication after identification, with DSP communication after having ARM to process, DSP controls computer 1 order main website and is converted into each rare earth forever
The control signal of magnetic-synchro servomotor.
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, in order to improve robot voice recognition effect, false recognition rate, ARM are reduced further
Controller first inputs the conventional number order of SCARA robot and instruction to LD3320, is used for absorbing wrong identification, thus reaches
Reduce the purpose of LD3320 false recognition rate.
2) stand-by power supply SOC and main power source state are first judged by ARM, if stand-by power supply SOC is relatively low, DSP will
Forbidding that four rare-earth permanent-magnetic synchronous servomotor work, motor input PWM ripple are blocked, alarm sensor is concurrent by work simultaneously
Go out alarm signal;If battery SOC is normal, SCARA robot enters and treats duty, waits work order.
3) once master station's work order starts, and master station can select to control based on LD3320 speech recognition system or PLC
Device processed and ARM communication, any one controller can pass through the information such as the length of large arm, forearm lengths and elevating lever
RS485 is passed to ARM controller, and then robot starts correcting zero position, magnetoelectric transducer EM1 that SCARA robot carries,
EM2, EM3 and EM4 and image capturing system based on CCD are started working, and EM1, EM2, EM3 and EM4 each find the zero of setting
Tick lables, when magnetoelectric transducer EM1, EM2, EM3 and EM4 all have signal to export, ARM real-time Transmission reset image is given total
Standing, after master station's confirmation is errorless, by wireless device and DSP communication, DSP blocks the PWM of four road rare-earth permanent-magnetic synchronous servomotors
Wave control signal, SCARA robot guides zero position to reset automatically, and now ARM controller sets each anglec of rotation, wrist lifting height。
4) 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.
5) SCARA robot reads its mode of operation, if manual working pattern, main website begin through PLC or
Speech recognition system based on LD3320 and ARM controller communication, by main website input SCARA robot's arm, forearm and wrist
Need the angle rotated,,And SCARA robot wrist needs to rise or the height of declineTo ARM control
Device, ARM controller starts robot forward according to Denavit-Hartenberg algorithm and solves: ARM controller first basis
SCARA robot's arm, forearm and wrist need the angle rotated,,And SCARA robot wrist needs to rise
HeightCalculate the position auto-control between adjacent two member coordinates,,,And use 4*4 two-dimemsional number
Group 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.
6) SCARA robot reads its mode of operation, if normal automatic transporting mode of operation, main website begins through
PLC or speech recognition system based on LD3320 and ARM controller communication, by main website input SCARA robot's arm, little
Initial position residing for arm and wrist and given position three-dimensional coordinate are to ARM controller, and ARM controller is according to Denavit-
It is Converse solved that Hartenberg algorithm starts robot: ARM controller is first according to large arm, forearm lengths and final three-dimensional
X and Y coordinates in coordinate obtain the angle that large arm needs to rotate, and byValue obtainValue, and according to three-dimensional sit
Z coordinate in mark obtains the height that wrist rises or reduces, finally obtain the anglec of rotation, owing to solvingWhen
Equation has bilingual, so SCARA robot obtains the angle that large arm, forearm and wrist need to rotate,,And
SCARA robot wrist needs the height risenAfter, solving result can be optimized by ARM controller, and then ARM controls
Device and DSP communication, most have servo motion parameter to be transferred to dsp controller in robot.
7) 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 in large arm, forearm and wrist ~ S5, EM1, EM2 and
EM3 will open, and first SCARA robot wants zero setting position to judge, after confirming that initial position is errorless, and SCARA robot pair
Barrier in each turning arm anglec of rotation judges, will send interrupt requests to DSP as there is barrier, and DSP can be right
Interrupt doing very first time response, then block four axle PWM wave control signal outputs, forbid that the rare earth permanent magnet of SCARA robot is same
Step servomotor X, rare-earth permanent-magnetic synchronous servomotor Y, rare-earth permanent-magnetic synchronous servomotor Z and motor rare-earth permanent-magnetic synchronous servo
Motor R works, and robot is self-locking in original place, and image capturing system based on CCD is opened, and passes through wireless device after ARM process
Interrupt requests is sent, in order to prevent information from judging by accident, in master station judges range of movement by radio transmitting image secondary to master station
Obstacle information, is artificially processed barrier by master station, prevents barrier impact carrying work.
8) if by image taking results, master station's secondary determines that clear enters moving region, wireless device will be passed through
With DSP communication, according to the speed time curve of Fig. 6, DSP is three anglecs of rotation,,It is converted into three rare earth permanent magnets
The acceleration of synchronous servo motor, speed and position initial order value, then DSP passes in conjunction with current of electric feedback, acceleration
The feedback of sensor, gyroscope and photoelectric encoder, according to its internal three closed loops based onVector controlled servo programe real-time
Adjusting motor X, motor Y and the PWM wave control signal of motor Z, the deviation size that dsp controller inputs according to three closed loops is adjusted in real time
The pid parameter of whole internal SERVO CONTROL program, adjusts it by the number of drive pulses adjusting rare-earth permanent-magnetic synchronous servomotor
The anglec of rotation, is realized the change of angular velocity speed, makes three by the frequency adjusting rare-earth permanent-magnetic synchronous Serve Motor Control signal
Axle rare-earth permanent-magnetic synchronous servomotor timing synchronization works, the location parameter that DSP moment recorder people has moved, and passes through
Wireless in real time to master station's transmission.
9) 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 inputs the anglec of rotation of new position by wireless device to DSP,,, then robot is according to Fig. 6's
Curve is again through three sections of movement locus: accelerated motion, uniform motion and retarded motion, eventually arrives at set point.
10) in moving process, the angular acceleration that accelerometer and gyroscope moment record large arm, forearm and wrist rotate
And angular velocity, and the anglec of rotation of large arm, forearm and wrist is obtained by integration, and compared with the position angle angle value set,
If deviation is more than setting threshold values, in the next sampling period, dsp controller is according to internal three closed loop rare-earth permanent-magnetic synchronous servos
Motor controls program and adjusts rare-earth permanent-magnetic synchronous servomotor X, rare-earth permanent-magnetic synchronous servomotor Y, rare-earth permanent-magnetic synchronous servo
The pwm control signal of motor Z, dsp controller adjusts internal SERVO CONTROL program in real time according to the deviation size that three closed loops input
Pid parameter, by adjust rare-earth permanent-magnetic synchronous servomotor number of drive pulses adjust its anglec of rotation, by adjust dilute
The frequency of soil permanent magnet synchronous servo motor control signal realizes the change of angular velocity speed, and then eliminates a sampling period product
Raw error, makes SCARA robot complete task according to setting track.
11) 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
Rare-earth permanent-magnetic synchronous servomotor X, rare-earth permanent-magnetic synchronous servomotor Y, servomotor Z is to be run for rare-earth permanent-magnetic synchronous
Angle, angle rates and angular acceleration, dsp controller is anti-in conjunction with current of electric, angular acceleration, gyroscope, photoelectric encoder
Feedback, obtains the pwm control signal signal of three rare-earth permanent-magnetic synchronous servomotors according to its internal three Close loop servo control programs,
Dsp controller adjusts the pid parameter of internal SERVO CONTROL program in real time according to the deviation size that three closed loops input, dilute by adjusting
The number of drive pulses of soil permanent magnet synchronous servo motor adjusts its anglec of rotation, by adjusting rare-earth permanent-magnetic synchronous servomotor control
The frequency of signal processed realizes the change of angular velocity speed, makes SCARA robot quickly move ahead according to setting speed.
12) in SCARA robot kinematics, ARM and DSP real time record robot's arm, forearm and wrist rotate
Angle, if master station finds that the robot speed of service is relatively slow, can be accelerated to ARM input by LD3320 speech recognition system
Order, ARM according to acceleration time requirement with DSP communication, DSP, is converted into rare-earth permanent-magnetic synchronous remaining angle and watches after processing
Take motor X, rare-earth permanent-magnetic synchronous servomotor Y, angle to be run for rare-earth permanent-magnetic synchronous servomotor Z, angle rates
Instructing with angular acceleration, dsp controller is in conjunction with current of electric feedback, acceleration transducer, gyroscope and photoelectric encoder
Feedback, according to its internal three closed loops based onVector controlled servo programe readjust three rare-earth permanent-magnetic synchronous servos
The pwm control signal signal of motor, makes robot be rapidly completed task.
13) in SCARA robot kinematics, ARM and DSP real time record robot's arm, forearm and wrist rotate
Angle, if master station find robot need emergency service, can by LD3320 speech recognition system to ARM input stop
Order, ARM process after with DSP communication, DSP according to requirement down time, in conjunction with current of electric feedback, acceleration transducer,
Gyroscope and the feedback of photoelectric encoder, according to its internal three closed loops based onVector controlled servo programe readjust
The pwm control signal signal of three rare-earth permanent-magnetic synchronous servomotors, makes robot be quickly completed parking, and dsp controller passes through
Angle that acceleration and gyro sensor real time record robot rotate also stores.Main website detects complete by image comparison
After, starting order by LD3320 speech recognition system to ARM input, with DSP communication after ARM process, DSP is according to the acceleration time
Requirement, is converted into rare-earth permanent-magnetic synchronous servomotor X, rare-earth permanent-magnetic synchronous servomotor Y, rare earth permanent magnet together remaining angle
Angle, angle rates and angular acceleration instruction to be run for step servomotor Z, dsp controller is anti-in conjunction with current of electric
The feedback of feedback, acceleration transducer, gyroscope and photoelectric encoder, according to its internal three closed loops based onVector controlled
Servo programe readjusts the pwm control signal signal of three rare-earth permanent-magnetic synchronous servomotors, makes robot be rapidly completed surplus
Remaining task.
14) 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 error 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,,Being converted into the acceleration of three rare-earth permanent-magnetic synchronous servomotor fine position, speed and position initial order value, DSP is again
Feed back in conjunction with motor X, motor Y and motor Z current of electric, photoelectric encoder feeds back and the feedback of acceleration transducer, in DSP
Portion three Close loop servo control program adjusts motor X, motor Y and the PWM wave control signal of motor Z, and dsp controller is according to three closed loops
The deviation size of input adjusts the pid parameter of internal SERVO CONTROL program in real time, by adjusting rare-earth permanent-magnetic synchronous servomotor
Number of drive pulses adjust its anglec of rotation, by adjust rare-earth permanent-magnetic synchronous Serve Motor Control signal frequency realize angle
The change of speed speed, makes robot's arm, forearm and wrist by the task again of three axle rare-earth permanent-magnetic synchronous servomotors
Arrive setting position.
15) angle is completed when large arm, forearm and the wrist of SCARA robot,,Angle compensation arrive set
Behind position, the sensor EM4 in wrist will be again turned on, and first SCARA robot wants zero setting position to judge, confirm initial
After position is errorless, DSP is according to the speed time curve of Fig. 6, distance to be lifted for wristIt is converted into rare-earth permanent-magnetic synchronous to watch
Taking the acceleration of motor R, speed and position initial order value, then to combine the current feedback of motor R, photoelectric encoder anti-for DSP
Feedback and the feedback of sensor S5, adjust the PWM wave control signal of motor R in real time through internal three Close loop servo control programs, and DSP is controlled
Device processed adjusts the pid parameter of internal SERVO CONTROL program in real time according to the deviation size that three closed loops input, by adjusting rare earth forever
The number of drive pulses of magnetic-synchro servomotor adjusts its anglec of rotation, believes by adjusting rare-earth permanent-magnetic synchronous Serve Motor Control
Number frequency realize the change of angular velocity speed, make wrist steadily arrive setting position within the setting time.
16) 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 rare earths immediately
The control signal of permanent magnet synchronous servo motor, the then self-locking of robot original place, ARM controller is carried out with main website by wireless device
Communication, the CCD moment gathers peripheral information and by being wirelessly transferred, and image acquisition information is analyzed by main website, and according to collection
Result secondary restarts SCARA robot, inputs carrying information by PLC secondary to SCARA robot.
17) in SCARA robot motor process the most repeatedly, if main website is found to have interim vital task and needs to add
In work queue, LD3320 controller will be opened by main website, by main website by LD3320 controller and ARM communication, to ARM input weight
Wanting the position servo parameter of task, ARM Yu DSP communication, first completed currently outstanding task by DSP, ARM updates simultaneously
DSP original servo control parameter queue, and transmit new servo position parameter to DSP, make DSP next one servo motion
Control important temporary duty.
18) in SCARA robot motor process the most repeatedly, if when magnetoelectric transducer EM1, EM2, EM3 and EM4 read
During to transducing signal, dsp controller will replace existing location parameter to carry out new position servo control with zero position parameter, and
Calculate error, in the next sampling period, DSP by internal three closed loop rare-earth permanent-magnetic synchronous Serve Motor Control programs to mistake
Difference compensates, and eliminates cumulative errors in time.
19) during SCARA robot transport, main website can automatically analyze contrast to collection image based on CCD,
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 current task
And stop setting zero position, main website automatically detect carrying result, automatically update DSP by ARM after detection next
Servo-controlled location parameter.
20) rare-earth permanent-magnetic synchronous servomotor X, rare-earth permanent-magnetic synchronous servomotor Y, rare-earth permanent-magnetic synchronous servo electricity it are contained in
Photoelectric encoder on machine Z, rare-earth permanent-magnetic synchronous servomotor R can export its position signalling A and position signalling B, photoelectric coding
Position signalling A pulse and the B pulsed logic state of device often change once, and the location register in DSP and ARM can be according to motor
Traffic direction adds 1 or subtracts 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 to DSP and ARM internal register, the absolute position of record rare-earth permanent-magnetic synchronous servomotor, then
Being converted into SCARA robot's arm, forearm or the wrist particular location in three-dimensional coordinate system, ARM controller is real-time
By PLC and main website communication, important location parameter is transferred to main website.When magnetoelectric transducer EM1, EM2 and EM3 read sensing
During signal, dsp controller will replace existing location parameter with zero position parameter, eliminate cumulative errors in time.
21) 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
Rare-earth permanent-magnetic synchronous servomotor provides energy, and adjusts four in real time by the internal three closed loop servo-control system programs of DSP
The PWM output of rare-earth permanent-magnetic synchronous servomotor, is adjusted by the number of drive pulses adjusting rare-earth permanent-magnetic synchronous servomotor
Its anglec of rotation, is realized the change of angular velocity speed, makes by the frequency adjusting rare-earth permanent-magnetic synchronous Serve Motor Control signal
SCARA robot completes to carry specifically task, and then SCARA robot carries out communication by PLC and main website, and notifies that main website enters
Row maintenance.
22) in SCARA robot Multi-asis servo system work process, if DSP servo controller detects some
There is pulsation in the torque of rare-earth permanent-magnetic synchronous servomotor, owing to the present invention usesVector control mode, therefore
Controller can be easy to compensate this interference, and according to the pid parameter disturbing size DSP to adjust electric current loop in real time, decreases motor
The torque disturbance impact on SCARA robot kinematics.
23) a series of task is completed when SCARA robot, it is achieved during the zero of position, the acceleration transducer A1 that it carries ~
A3, magnetoelectric transducer EM1, EM2, EM3 and EM4 can work in the moment, first have acceleration transducer to detect its acceleration, work as acceleration
When exceeding pre-set threshold value, the error that cycle brings on next cycle dsp controller can be revised, work as magnetoelectric transducer
When EM1, EM2, EM3, EM4 all have signal to export, ccd image acquisition system Real-time Collection repositioning information, passes through after ARM processes
Wireless device sends image transmitting request to master station, and master station is automatically reset result by image comparison technology, and DSP blocks four tunnels
The PWM wave control signal of rare-earth permanent-magnetic synchronous servomotor, SCARA robot guides zero position to reset automatically, SCARA machine
Device people realizes the playback from certain point to zero-bit zero-bit self-locking according to the Velocity-time curve movement of Fig. 6, waits next group fortune
Dynamic command queue.
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.