CN201728656U - Embedded light mechanical arm controller - Google Patents

Embedded light mechanical arm controller Download PDF

Info

Publication number
CN201728656U
CN201728656U CN2010202686427U CN201020268642U CN201728656U CN 201728656 U CN201728656 U CN 201728656U CN 2010202686427 U CN2010202686427 U CN 2010202686427U CN 201020268642 U CN201020268642 U CN 201020268642U CN 201728656 U CN201728656 U CN 201728656U
Authority
CN
China
Prior art keywords
microprocessor
control chip
motion control
mechanical arm
links
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Expired - Lifetime
Application number
CN2010202686427U
Other languages
Chinese (zh)
Inventor
赵玉良
戚晖
陈凡明
李健
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Electric Power Research Institute of State Grid Shandong Electric Power Co Ltd
Original Assignee
Electric Power Research Institute of State Grid Shandong Electric Power Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Electric Power Research Institute of State Grid Shandong Electric Power Co Ltd filed Critical Electric Power Research Institute of State Grid Shandong Electric Power Co Ltd
Priority to CN2010202686427U priority Critical patent/CN201728656U/en
Application granted granted Critical
Publication of CN201728656U publication Critical patent/CN201728656U/en
Anticipated expiration legal-status Critical
Expired - Lifetime legal-status Critical Current

Links

Images

Abstract

The utility model relates to an embedded light mechanical arm controller, which comprises a teaching box controller and an embedded main-auxiliary DSP controller mutually connected. The teaching box controller includes a microprocessor I respectively connected with a man-machine interface unit and the embedded main-auxiliary DSP controller. The embedded main-auxiliary DSP controller comprises a microprocessor II and a microprocessor III, the microprocessor II is respectively connected with the microprocessor I and a dual-port RAM, the dual-port RAM is connected with the microprocessor III respectively connected with a movement control chip I and a movement control chip II, the movement control chip I and the movement control chip II are respectively connected with a motor driver, an origin switch and a limit switch, the motor driver is connected with a motor, and an output shaft of the motor is connected with a mechanical arm. The embedded light mechanical arm controller has the advantages of light weight, fast processing speed, low cost, fine stability, easy function expansion and the like.

Description

Embedded light mechanical arm controller
Technical field
The utility model relates to a kind of Robotics, especially a kind of embedded light mechanical arm controller.
Background technology
Mechanical arm is a kind of automation equipment with extracting and travelling workpiece function that uses in the automated production process, and it is a kind of new device that grows up in mechanization, automated production process.Mechanical arm can replace the mankind to finish danger, repeat scissors and paste, alleviates human labour intensity, raises labour productivity.Mechanical arm has obtained application more and more widely, and it can be used for the parts assembling in mechanical industry, and the carrying of processing work, loading and unloading are particularly used more general on automation Digit Control Machine Tool, building-block machine.
Mechanical arm control system generally is divided into two classes at present: a class adopts industrial computer and control card, uses WINDOWS operating system, and control system weight is heavier, and mechanical arm algorithm process speed is slow, and price is higher and system is unstable; Another kind of employing 80 series monolithics, control system hardware is too simple, and function is difficult to expansion.
The utility model content
The purpose of this utility model is for overcoming above-mentioned the deficiencies in the prior art, provide a kind of in light weight, processing speed is fast, cost is low, the easy embedded light mechanical arm controller of expansion of good stability, function.
For achieving the above object, the utility model adopts following technical proposals:
A kind of embedded light mechanical arm controller, it is characterized in that: comprise the teach box controller and embedded principal and subordinate DSP (digital signal processor) controller that are connected, described teach box controller comprises microprocessor I, and microprocessor I links to each other with embedded principal and subordinate DSP (digital signal processor) controller with the man-machine interface unit respectively; Described embedded principal and subordinate DSP (digital signal processor) controller comprises microprocessor II and microprocessor III, described microprocessor II is connected with dual port RAM with microprocessor I respectively, dual port RAM links to each other with microprocessor III, microprocessor III links to each other with motion control chip II with motion control chip I respectively, motion control chip I links to each other with motor driver, origin switch and limit switch respectively with motion control chip II, motor driver links to each other with motor, and motor output shaft links to each other with mechanical arm.
Described man-machine interface unit comprises display and keyboard.
Described microprocessor I is connected by serial communication with microprocessor II.
Described microprocessor I, microprocessor II and microprocessor III all adopt the TMS320F2812 chip.
The data/address bus of described microprocessor II, address bus, control bus link to each other with left data/address bus, address bus, the control bus of dual port RAM, and the data/address bus of microprocessor III, address bus, control bus link to each other with right data/address bus, address bus, the control bus of dual port RAM.
The data/address bus of described microprocessor III, address bus, control bus link to each other with data/address bus, address bus, the control bus of motion control chip I and motion control chip II respectively.
Described motion control chip I, motion control chip II all adopt the MCX314 chip.
The pulse delivery outlet 1-4 of described motion control chip I links to each other with the input port 1-4 of stepper motor driver, motion control chip I initial point signals collecting mouth links to each other with the delivery outlet 1-4 of origin switch, and motion control chip I limit switch acquisition port links to each other with the delivery outlet 1-8 of limit switch; The pulse delivery outlet 1-3 of motion control chip II links to each other with the input port 5-7 of stepper motor driver, motion control chip II initial point signals collecting mouth links to each other with the delivery outlet 5-7 of origin switch, and motion control chip II limit switch acquisition port links to each other with limit switch delivery outlet 9-12.
The positive negative pulse stuffing form is adopted in the output of described motor driver, and motor adopts two-phase hybrid stepping motor.
The structure of mechanical arm control system of the present utility model adopts the master-slave mode microprocessor to control, microprocessor II is as main frame, it takes on system management, the compiling of mechanical arm language and human interface function, simultaneously also utilize its operational capability to finish coordinate transform, track interpolation, forward kinematics solution, inverse kinematic, and periodically operation result is delivered to public internal memory as the increment of joint motions, read it for microprocessor III.It is digital control that microprocessor III finishes whole joint positions.It reads set-point from public internal memory, also each joint physical location is sent back in the public internal memory, and microprocessor II uses.Public internal memory is to be the dual port RAM of 8KB by capacity.The control speed of this type systematic is fast, generally can reach 10ms.
Adopt such scheme, the utlity model has following advantage, the one, the manipulator motion control device that designs voluntarily can satisfy mechanical arm control requirement through experimental verification, and the manipulator motion controller that can be used as reliable, with low cost is used and is sold; The 2nd, embedded light mechanical arm from heavy and light, size is little, control system is low in energy consumption and size is little, is fit to move operation manipulation robot's application need.The 3rd, embedded light mechanical arm can be realized complicated linear interpolation, circular interpolation motion; The 4th, system adopts modularized design, has opening, readability, extensibility, maintainability, so that continue exploitation.The 5th, the manipulator motion controller adopts the master-slave mode microprocessor, and microprocessor II realizes that kinematics is positive and negatively separated, interpolation algorithm, and microprocessor III realizes motion control, and processing speed is fast.The 6th, controller has driver control interface, origin switch acquisition port, limit switch acquisition port, and is multiple functional, the positional precision height.
Description of drawings
Fig. 1 is the utility model general diagram;
Fig. 2 is the utility model teach box hardware circuit interface connection layout;
Fig. 3 is the utility model mechanical arm master board hardware interface connection layout;
Fig. 4 is that the utility model mechanical arm is from control panel hardware interface connection layout;
Fig. 5 is the utility model keyboard schematic diagram;
Fig. 6 is the utility model mechanical arm schematic diagram;
Fig. 7 is the utility model initial point search utility flow chart;
Fig. 8 is the utility model joint coordinate system motor program flow chart;
Fig. 9 is the utility model single shaft motion subroutine flow chart;
Figure 10 is the utility model rectangular coordinate system motor program flow chart;
Figure 11 is a regularly right angle moving interpolation schematic diagram of the utility model;
Figure 12 is that the utility model tool coordinates is the motor program flow chart;
Figure 13 is the utility model cylindrical-coordinate system motor program flow chart;
Figure 14 is the utility model inverse kinematic schematic diagram.
The specific embodiment
Below in conjunction with drawings and Examples the utility model is further specified.
Referring to Fig. 1, a kind of controller of embedded light mechanical arm comprises teach box controller and embedded principal and subordinate's dsp controller two parts.The teach box controller is made up of Keysheet module, LCD MODULE, serial communication modular, microprocessor I.The output of supervisory keyboard connects the input of microprocessor I, and the input and output of microprocessor I connect the input and output of liquid crystal display, the serial communication of the serial ports of microprocessor I and mechanical arm microprocessor II.Embedded principal and subordinate's dsp controller unclamps control etc. by microprocessor II, dual port RAM, microprocessor III, motion control chip I, motion control chip II, optoelectronic switch treatment circuit, limit switch treatment circuit, stepper motor driver module, band-type brake and forms.The data/address bus of microprocessor II, address bus, the left data/address bus of control bus and dual port RAM, address bus, control bus links to each other, the right data/address bus of dual port RAM, address bus, control bus links to each other with microprocessor III, the data/address bus of microprocessor III, address bus, control bus and motion control chip I, the data/address bus of motion control chip II, address bus, control bus links to each other, the pulse delivery outlet 1-4 of motion control chip I links to each other with the input port 1-4 of stepper motor driver, motion control chip I initial point signals collecting mouth links to each other with the delivery outlet 1-4 of origin switch, motion control chip I limit switch acquisition port links to each other with the delivery outlet 1-8 of limit switch, the pulse delivery outlet 1-3 of motion control chip II links to each other with the input port 5-7 of stepper motor driver, motion control chip II initial point signals collecting mouth links to each other with the delivery outlet 5-7 of origin switch, and motion control chip II limit switch acquisition port links to each other with limit switch delivery outlet 9-12.
The man-machine interface unit comprises keyboard and liquid crystal display, and they are connected with microprocessor I respectively.
Microprocessor I and microprocessor II pass through serial communication.
Microprocessor I, microprocessor II and microprocessor III all adopt the TMS320F2812 chip.
Microprocessor I gathers the data command of keyboard, is handed down to microprocessor II by serial communication, movement velocity, send instructions down, the position of mechanical arm shows by hydraulic module.
Microprocessor II and microprocessor III carry out data interaction by dual port RAM.The data/address bus of microprocessor II, address bus, control bus link to each other with left data/address bus, address bus, the control bus of dual port RAM, and the data/address bus of microprocessor III, address bus, control bus link to each other with right data/address bus, address bus, the control bus of dual port RAM.
The data/address bus of microprocessor III, address bus, control bus link to each other with motion control chip I, the data/address bus of motion control chip II, address bus, control bus.
Motion control chip I, motion control chip II all adopt the MCX314 chip.
The positive and negative pulse signal control step motor driver 1-4 of motion control chip I output, motion control chip I initial point signals collecting mouth links to each other with the delivery outlet 1-4 of origin switch, and motion control chip I limit switch acquisition port links to each other with the delivery outlet 1-8 of limit switch.The positive and negative pulse signal control step motor driver 5-7 of motion control chip II output, motion control chip II initial point signals collecting mouth links to each other with the delivery outlet 5-7 of origin switch, and motion control chip II limit switch acquisition port links to each other with limit switch delivery outlet 9-12.
The positive negative pulse stuffing form is adopted in the output of driver, and motor adopts two-phase hybrid stepping motor.
The structure of mechanical arm control system adopts the master-slave mode microprocessor to control, microprocessor II is as main frame, it takes on system management, the compiling of mechanical arm language and human interface function, simultaneously also utilize its operational capability to finish coordinate transform, track interpolation, forward kinematics solution, inverse kinematic, and periodically operation result is delivered to public internal memory as the increment of joint motions, read it for microprocessor III.It is digital control that microprocessor III finishes whole joint positions.It reads set-point from public internal memory, also each joint physical location is sent back in the public internal memory, and microprocessor II uses.Public internal memory is to be the dual port RAM of 8KB by capacity.The control speed of this type systematic is fast, generally can reach 10ms.
Referring to Fig. 2, the teach box controller is made up of microprocessor I, Liquid Crystal Module, logic level translator, keyboard administration module, keyboard, voltage stabilizing chip I, voltage stabilizing chip I I, serial ports receiver transmitter, serial ports.Voltage stabilizing chip I, voltage stabilizing chip I are given microprocessor I power supply.The GIPIOB1 of microprocessor is connected with 2 pin of ADG3308, and GPIOB5 is connected with 5 pin, and XINT2 is connected with 6 pin.GPIOA0-7 is connected with the DB0-7 of Liquid Crystal Module, and GPIOB0 is connected with REQ, and GPIOB2 is connected with CS, Liquid Crystal Module 5V power supply.16 pin of ADG3308 are connected with the DATA pin of HD7279, and 15 are connected with the KEY pin.The GPIOB3 of microprocessor I is connected with the CS pin of HD7279, and GPIOB4 is connected with the CLK pin.The output of keyboard meets DIG0-7, the DP-SG of HD7279.The SCITXDA of microprocessor I connects 11 pin of MAX3232, and SCIRXDA connects 12 pin, and 13,14 pin of MAX3232 are received serial ports.Referring to Fig. 3, embedded master controller comprises dual port RAM, microprocessor II, serial ports receiver transmitter, serial ports, slave controller interface.The XD0-15 of microprocessor II connect dual port RAM IO0-15L ,/XRD connects/OEL ,/XWE connect R//WL ,/XZCS2 connects/CEL, XA0-11 meet A0-11L.The M//S of dual port RAM meets 3.3V, is arranged to holotype.The IO0-15R of dual port RAM ,/OER, R//WR ,/CER, A0-11R connect the slave controller interface.The SCITXDA of microprocessor II connects 11 pin of MAX3232, and SCIRXDA connects 12 pin, and 13,14 pin of MAX3232 are received serial ports.
Referring to Fig. 4, embeddedly comprise dual port RAM interface, the active crystal oscillator of microprocessor III, 16M, motion control chip 1, motion control chip 2, light-coupled isolation, driver interface, origin switch interface, limit switch interface from control panel.The IO0-15R of dual port RAM interface ,/OER, R//WR ,/CER, A0-11R connect microprocessor III XD0-15 ,/XRD ,/XWE ,/XZCS2, XA0-11.The XD0-15 of microprocessor III ,/XRD ,/XWE, XA14, XA0-2 meet D0-15, RDN, WRN, CSN, the A0-2 of motion control chip I respectively.The XD0-15 of microprocessor III ,/XRD ,/XWE, XA13, XA0-2 meet D0-15, RDN, WRN, CSN, the A0-2 of motion control chip II respectively.The delivery outlet of the active crystal oscillator of 16M connects 53 pin of motion control chip I, II.The output interface of positive limit switch 1-4 connects 69,87,97,116 pin of motion control chip I respectively through light-coupled isolation; The output interface of negative limit switch 5-8 connects 70,88,98,117 pin of motion control chip I respectively through light-coupled isolation; The delivery outlet of origin switch 1,2,3,4 connects 73,93,101,120 pin of motion control chip I respectively through light-coupled isolation; 35,36 pin of motion control chip I are driver connected 1 positive pulse, negative pulse input port respectively; 38,39 pin of motion control chip I are driver connected 2 positive pulse, negative pulse input port respectively; 40,41 pin of motion control chip I are driver connected 3 positive pulse, negative pulse input port respectively; 42,43 pin of motion control chip I are driver connected 4 positive pulse, negative pulse input port respectively.The delivery outlet of origin switch 5,6,7 connects 73,93,101 pin of motion control chip II respectively through light-coupled isolation; The output interface of positive limit switch 9-10 connects 69,87 pin of motion control chip I respectively through light-coupled isolation; The output interface of negative limit switch 11-12 connects 70,88 pin of motion control chip I respectively through light-coupled isolation; 35,36 pin of motion control chip II are driver connected 5 positive pulse, negative pulse input port respectively; 38,39 pin of motion control chip II are driver connected 6 positive pulse, negative pulse input port respectively; 40,41 pin of motion control chip II are driver connected 7 positive pulse, negative pulse input port respectively.
Referring to Fig. 5, the keyboard schematic diagram, S+ represents that positive motion, rectangular coordinate system X+ motion, the tool coordinates of first of joint of mechanical arm coordinate system are X+ motion, cylindrical-coordinate system θ+motion, and S-represents that negative motion, rectangular coordinate system X-motion, the tool coordinates of first of joint coordinate system are X-motion, cylindrical-coordinate system θ-motion; L+ represents that positive motion, rectangular coordinate system Y+ motion, the tool coordinates of second of joint of mechanical arm coordinate system are Y+ motion, cylindrical-coordinate system r+ motion, and L-represents that negative motion, rectangular coordinate system Y-motion, the tool coordinates of second of joint coordinate system are Y-motion, cylindrical-coordinate system r-motion; U+ represents that positive motion, rectangular coordinate system Z+ motion, the tool coordinates of the 3rd of joint coordinate system are Z+ motion, cylindrical-coordinate system Z+ motion, and U-represents that negative motion, rectangular coordinate system Z-motion, the tool coordinates of the 3rd of joint coordinate system are Z-motion, cylindrical-coordinate system Z-motion; R+ represents the positive motion of the 4th of joint coordinate system, and R-represents the negative motion of the 4th of joint coordinate system; B+ represents the positive motion of the 5th of joint coordinate system, and B-represents the negative motion of the 5th of joint coordinate system; T+ represents the positive motion of the 6th of joint coordinate system, and T-represents the negative motion of the 6th of joint coordinate system; M+ represents opening of paw, and M-represents closing of paw; V+ represents to gather way, and V-represents to reduce speed; Press the initial point search key and carry out the initial point searching moving; When pressing the coordinate system switch key, coordinate system changes with following order: joint-right angle-instrument-cylinder.
Referring to Fig. 6, can set up a regular Cartesian coordinates (x at the joint shaft place to each rod member 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 0, y 0, z 0) (position on support and direction can be chosen wantonly, as long as z 0Axle gets final product along 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 joint i (i is all 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.
A kind of embedded light mechanical arm control method comprises the steps:
1) can set up a regular Cartesian coordinates (x at the joint shaft place to each rod member 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 0, y 0, z 0) (position on support and direction can be chosen wantonly, as long as z 0Axle gets final product along first articulating shaft);
2). for the rod member coordinate system of each joint is set up 4 * 4 odd transformation matrixs, the relation of expression and previous rod member coordinate system;
3) adopt the timing interpolation algorithm of " walking " (" walking while calculating " is meant that the joint position that each interpolated point is carried out obtaining after the inverse kinematics conversion need not store, and directly presses these joint position setting in motions again), the position and the attitude of calculating interpolated point while calculating;
4). adopt equation to calculate each inverse kinematic (inverse kinematic be meant known terminal position and attitude ask the angle in each joint), draw each the movement angle in the interpolation cycle;
5). the movement angle of each that draws spool outputs to microprocessor III, and the position command of microprocessor III outputs to motion control chip I and motion control chip II, controls the moving interpolation of each.
Referring to Fig. 7, the process of returning to mechanical reference point is: parameters such as acceleration-deceleration, speed are set; Close the origin switch acquisition port; Half of each positive direction motor activity space; Open the origin switch acquisition port; Carry out motion in the other direction; Up to searching origin switch, the deceleration of carrying out corresponding axis stops subprogram.
Referring to Fig. 8,9, joint coordinate system motion, the at first acceleration-deceleration of input motion, velocity amplitude are judged pressing of particular key or unclamp that by the state of read port after particular key was pressed, system quickened Continuous Drive to specifying axle according to the parameter of setting; When button unclamped, system sent to slow down and ceases and desist order.
Referring to Figure 10,11,12,13, rectangular coordinate system motion, the at first acceleration-deceleration of input motion, velocity amplitude are judged pressing of particular key or are unclamped by the state of read port, after particular key was pressed, system carried out positive and negative separate algorithm, linear interpolation or circular interpolation motion to specifying axle; When button unclamped, system sent to slow down and ceases and desist order.
This utility model space line interpolation can be divided into following a few step and finish:
Input robot motion's initial point P 0(x 0, y 0, z 0) and terminal point P f(x f, y f, z f) (f is the abbreviation of final), movement velocity P v, the acceleration and deceleration time T a, interpolation cycle T c, running time T;
The method for solving of the definite and interpolated point of basic parameter.Because robot space line motion needs therefore before carrying out moving interpolation, should determine P through acceleration and deceleration and uniform motion section vWhether satisfy the acceleration and deceleration requirement.Method is as follows:
By P 0(x 0, y 0, z 0) and P f(x f, y f, z f) obtain the actual motion distance P d=| P 0P f|; By P vAnd T aCan calculate the accelerating and decelerating part required separation distance
Figure BSA00000202224600061
If C d〉=P d, then actual motion speed
Figure BSA00000202224600062
Otherwise C v=P vBy time T aWith the interpolation time T cDraw and quicken step number S aBy P 0(x 0, y 0, z 0) and P f(x f, y f, z f), can get the space the parametric equation of the straight line Wherein k is scale factor (0≤k≤1) (1)
Therefore by formula (1), can get each interpolated point P i(x i, y i, z i) (i is step number of each interpolated point, 0 with
Figure BSA00000202224600071
Between) to P 0Distance be
Figure BSA00000202224600072
(C Sd (i)Expression P i(x i, y i, z i) to P 0Distance, P d=| P 0P f|) (2)
Making n interpolation section move distance is S D (n)(n=1 ..., i), (i is step number of each interpolated point, 0 with
Figure BSA00000202224600073
Between) but invocation point P iTo P 0Distance
Figure BSA00000202224600074
(C Sd (i-1)Expression P I-1(x I-1, y I-1, z I-1) to P 0Distance, S D (i)Be i interpolation section move distance), so it is as follows to obtain the computing formula of each interpolated point scale factor k by formula (1) and (2):
Figure BSA00000202224600075
Wherein k is scale factor (0≤k≤1) (3)
Just can obtain k by formula (3), and obtain the interpolated point rectangular co-ordinate.Therefore space line interpolation algorithm key is to determine each interpolation section move distance S D (i)Introduce each section of motion below and ask for S D (i)Method:
The accelerated motion section.Because the robot accelerating sections of this paper design is uniformly accelerated motion, so by actual motion speed C vWith the acceleration and deceleration time T aTry to achieve acceleration So speed S of i interpolated point of acceleration Duan Shangdi Cv (i)=iT cA can obtain
Figure BSA00000202224600077
(S Cv (i-1)The speed of representing i-1 interpolated point) (4)
The uniform motion section.Can because the robot of this paper design requires must be through braking section, and interpolation operation be " walking while calculating ", so carry out must calculating last distance and satisfying the system slowdown requirement before the uniform motion section begins at every turn.At the uniform velocity the section each interpolation section move distance S D (i)=C vT c
The retarded motion section.Owing to asking for acceleration step number S aShi Jinhang rounds calculating, therefore simply plans accelerating sections after the negate of the section of will speed up acceleration, can introduce error like this, so the braking section acceleration should recomputate.Behind a front i-1 interpolated point, can get last distance L D (i)=P d-C Sd (i-1), therefore can get the braking section acceleration The speed S of a deceleration Duan Shangdi m interpolated point then Cv (m)=C v+ mT cA can obtain
S d ( m ) = 1 2 ( S cv ( m ) + S cv ( m - 1 ) ) · T c = 1 2 [ 2 C v + a · ( 2 m - 1 ) · T c ] - - - ( 5 )
This utility model adopt equation carry out inverse kinematic (inverse kinematic is meant that known terminal position and attitude ask the angle in each joint, as shown in figure 14):
p x
p y
p x--the position of----expression mechanical arm end in world coordinate system;
n x o x a x
n y o y a y
n zo za z--the attitude of----expression mechanical arm end in world coordinate system;
θ 1..., θ 6------represents each motion angle;
A i∈ R 4 * 4(i=1,2 ..., 6) and------is according to the transition matrix between coordinate system on each connecting rod of D-H coordinate system foundation.
s i------expression sin θ i
c i------expression cos θ i
s Ij------expression sin (θ i+ θ j);
c Ij------expression cos (θ i+ θ j)
Figure BSA00000202224600081
Denotation coordination is 1 and the homogeneous transformation matrix of coordinate system 0
Figure BSA00000202224600082
Denotation coordination is 2 and the homogeneous transformation matrix of coordinate system 1
Figure BSA00000202224600083
Denotation coordination is 3 and the homogeneous transformation matrix of coordinate system 2
Figure BSA00000202224600084
Denotation coordination is 4 and the homogeneous transformation matrix of coordinate system 3
Figure BSA00000202224600091
Denotation coordination is 5 and the homogeneous transformation matrix of coordinate system 4
Figure BSA00000202224600092
Denotation coordination is 6 and the homogeneous transformation matrix of coordinate system 5
Figure BSA00000202224600093
It has determined position and the attitude of manipulator end with respect to the support coordinate system.
The normal vector of n-hand
The sliding vector of s-hand
The a-hand near vector
The position vector of p-hand (6)
Coordinate system O i(i is positive integer i=0,1 ..., 6) then be the D-H coordinate system of on motion arm, setting up; a 2, d 4∈ R represents the length of mechanical arm respective link respectively.Can be with the terminal coordinate system O of motion arm 6At base coordinate system O 0Under pose write as following expression: 0T 6=A 1A 2A 3A 4A 5A 6When finding the solution the equation of motion, from 0T 6Begin to find the solution joint position.Make 0T 6Each element of character expression equal 0T 6General type, and determine θ in view of the above 1In case try to achieve θ 1Afterwards, can be by A 1 -1Premultiplication 0T 6General type,
A -1 1 0T 61T 6 (7)
This formula can be used to find the solution other each joint variables.Constantly use the inverse matrix premultiplication (7) of A, can get following four matrix equation formulas in addition:
A 2 -1A 1 -10T 62T 6 (8)
A 3 -1A 2 -1A 1 -10T 63T 6 (9)
A 4 -1A 3 -1A 2 -1A 1 -10T 64T 6 (10)
A 5 -1A 4 -1A 3 -1A 2 -1A 1 -10T 65T 6 (11)
The levoform of each equation of following formula is 0T 6With the function of preceding (i-1) individual joint variable, available these equations are determined the position in each joint:
θ i=atan2(p y,p x)(-3.1415≤θ 1≤3.1415) (12)
k = p x 2 + p y 2 + p z 2 - a 2 2 - d 4 2 2 a 2
θ 3 = - a tan 2 ( k , ± d 4 2 - k 2 ) ( - 3.9268 ≤ θ 1 ≤ 0.7853 ) - - - ( 13 )
Attention: in the formula, positive and negative number corresponding θ 3Two kinds of feasible solutions.
s 23 = - a 2 c 3 p z + ( c 1 p x + s 1 p y ) ( a 2 s 3 - d 4 ) p z 2 + ( c 1 p x + s 1 p y ) 2 c 23 = ( - d 4 + a 2 s 3 ) p z + ( c 1 p x + s 1 p y ) a 2 c 3 p z 2 + ( c 1 p x + s 1 p y ) 2
θ 2=atan2(s 23,c 23)-θ 3(-1.5707≤θ 1≤1.5707) (14)
θ 4=atan2(-a xs 1+a yc 1,-a xc 1c 23-a ys 1c 23+a zs 23)(-3.1415≤θ 1≤3.1415) (15)
Attention: work as s 5=0 o'clock, mechanical arm was in unusual morpheme.At this moment, joint shaft 4 and 6 overlaps, and can only solve θ 4With θ 6And or poor.Whether unusual morpheme can all be judged near zero by two variablees of atan2 in the formula (15).
θ 5=atan2(-a x(c 1c 23c 4+s 1s 4)-a y(s 1c 23c 4-c 1s 4)+a zs 23c 4,-a xc 1s 23-a ys 23s 1-a zc 23)
(-3.9268≤θ 1≤0.7853) (16)
k 1=-n x(c 1c 23s 4-s 1c 4)-n y(s 1c 23s 4+c 1c 4)+n zs 23s 4
k 2=n x((c 1c 23c 4+s 1s 4)c 5-c 1s 23s 5)+n y((s 1c 23c 4-c 1s 4)c 5-s 1s 23s 5)-n z(s 23c 4c 5+c 23s 5)
θ 6=atan2(k 1,k 2)(-3.1415≤θ 1≤3.1415) (17)

Claims (8)

1. embedded light mechanical arm controller, it is characterized in that: comprise the teach box controller and the embedded principal and subordinate's dsp controller that are connected, described teach box controller comprises microprocessor I, and microprocessor I links to each other with embedded principal and subordinate's dsp controller with the man-machine interface unit respectively; Described embedded principal and subordinate's dsp controller comprises microprocessor II and microprocessor III, described microprocessor II is connected with dual port RAM with microprocessor I respectively, dual port RAM links to each other with microprocessor III, microprocessor III links to each other with motion control chip II with motion control chip I respectively, motion control chip I links to each other with motor driver, origin switch and limit switch respectively with motion control chip II, motor driver links to each other with motor, and motor output shaft links to each other with mechanical arm.
2. embedded light mechanical arm controller according to claim 1 is characterized in that: described man-machine interface unit comprises display and keyboard.
3. embedded light mechanical arm controller according to claim 1 is characterized in that: described microprocessor I is connected by serial communication with microprocessor II.
4. embedded light mechanical arm controller according to claim 1 is characterized in that: described microprocessor I, microprocessor II and microprocessor III all adopt the TMS320F2812 chip.
5. embedded light mechanical arm controller according to claim 1, it is characterized in that: the data/address bus of described microprocessor II, address bus, control bus link to each other with left data/address bus, address bus, the control bus of dual port RAM, and the data/address bus of microprocessor III, address bus, control bus link to each other with right data/address bus, address bus, the control bus of dual port RAM.
6. embedded light mechanical arm controller according to claim 1 is characterized in that: the data/address bus of described microprocessor III, address bus, control bus link to each other with data/address bus, address bus, the control bus of motion control chip I and motion control chip II respectively.
7. embedded light mechanical arm controller according to claim 1 is characterized in that: described motion control chip I, motion control chip II all adopt the MCX314 chip.
8. embedded light mechanical arm controller according to claim 1, it is characterized in that: the pulse delivery outlet 1-4 of described motion control chip I links to each other with the input port 1-4 of stepper motor driver, motion control chip I initial point signals collecting mouth links to each other with the delivery outlet 1-4 of origin switch, and motion control chip I limit switch acquisition port links to each other with the delivery outlet 1-8 of limit switch; The pulse delivery outlet 1-3 of motion control chip II links to each other with the input port 5-7 of stepper motor driver, motion control chip II initial point signals collecting mouth links to each other with the delivery outlet 5-7 of origin switch, and motion control chip II limit switch acquisition port links to each other with limit switch delivery outlet 9-12.
CN2010202686427U 2010-07-23 2010-07-23 Embedded light mechanical arm controller Expired - Lifetime CN201728656U (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN2010202686427U CN201728656U (en) 2010-07-23 2010-07-23 Embedded light mechanical arm controller

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN2010202686427U CN201728656U (en) 2010-07-23 2010-07-23 Embedded light mechanical arm controller

Publications (1)

Publication Number Publication Date
CN201728656U true CN201728656U (en) 2011-02-02

Family

ID=43519815

Family Applications (1)

Application Number Title Priority Date Filing Date
CN2010202686427U Expired - Lifetime CN201728656U (en) 2010-07-23 2010-07-23 Embedded light mechanical arm controller

Country Status (1)

Country Link
CN (1) CN201728656U (en)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101913149A (en) * 2010-07-23 2010-12-15 山东电力研究院 Embedded light mechanical arm controller and control method thereof
CN103085055A (en) * 2013-01-29 2013-05-08 山东电力集团公司电力科学研究院 Hot-line repair robot position feedback master system
CN103317507A (en) * 2013-06-07 2013-09-25 浙江工业大学 Chinese teaching method of manipulator motion controller
CN103778843A (en) * 2012-10-25 2014-05-07 西安航天精密机电研究所 Industrial robot demonstration and reappearance method
CN108189038A (en) * 2018-01-18 2018-06-22 广东工业大学 A kind of industry six shaft mechanical arm straight-line trajectory method and system for planning of practicality

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101913149A (en) * 2010-07-23 2010-12-15 山东电力研究院 Embedded light mechanical arm controller and control method thereof
CN101913149B (en) * 2010-07-23 2012-04-04 山东电力研究院 Embedded light mechanical arm controller and control method thereof
CN103778843A (en) * 2012-10-25 2014-05-07 西安航天精密机电研究所 Industrial robot demonstration and reappearance method
CN103778843B (en) * 2012-10-25 2017-02-15 西安航天精密机电研究所 Industrial robot demonstration and reappearance method
CN103085055A (en) * 2013-01-29 2013-05-08 山东电力集团公司电力科学研究院 Hot-line repair robot position feedback master system
CN103085055B (en) * 2013-01-29 2016-06-22 山东电力集团公司电力科学研究院 Hot-line repair robot position feedback master system
CN103317507A (en) * 2013-06-07 2013-09-25 浙江工业大学 Chinese teaching method of manipulator motion controller
CN103317507B (en) * 2013-06-07 2015-11-04 浙江工业大学 A kind of Chinese teaching method of robot movement controller
CN108189038A (en) * 2018-01-18 2018-06-22 广东工业大学 A kind of industry six shaft mechanical arm straight-line trajectory method and system for planning of practicality

Similar Documents

Publication Publication Date Title
CN101913149B (en) Embedded light mechanical arm controller and control method thereof
CN201728656U (en) Embedded light mechanical arm controller
CN103753534B (en) A kind of Movement Controller of Mobile Robot and control method thereof
CN104339354B (en) A kind of special motion controller hardware platform for 6DOF parallel robot
CN101655708B (en) Intelligent vehicle carrier and control system and control method thereof
CN101817182B (en) Intelligent moving mechanical arm control system
CN103576614A (en) Multi-axis motion control system
CN201426109Y (en) PCI bus bar type multi-shaft impulse type movement control card
CN105573253A (en) Group control system and method for industrial robots
CN1808319A (en) Parallel connection equipment opening type motion control card based on two-stage DSP and control method thereof
CN103358309B (en) A kind of robot movement Controlling System based on Ethernet
CN204832853U (en) Many interfaces motion control ware
CN106346480B (en) A kind of multiple degrees of freedom injection machine arm modeling method based on UG and MATLAB
CN105700465A (en) Robot compliance control system and method based on EtherCAT bus
CN105892412A (en) Multi-axis motion control hardware configuration based on custom bus
CN107450466A (en) A kind of pressing robot controller and its high-speed smooth control method
CN204856122U (en) Punching press robot control system
CN204431250U (en) A kind of reclaimer robot
CN101738987A (en) Five-axis motion control card
CN201804248U (en) Motion control card
CN201625982U (en) Intelligent mobile mechanical arm control system
CN104240263A (en) Motion feature analysis system for Delta parallel-connection mechanical arm
CN202878317U (en) Multi-freedom-degree industrial robot motion controller
CN204883256U (en) High real -time control system of robot framework
CN204807988U (en) Motion control ware

Legal Events

Date Code Title Description
C14 Grant of patent or utility model
GR01 Patent grant
AV01 Patent right actively abandoned

Granted publication date: 20110202

Effective date of abandoning: 20120404