CN101913149A - Embedded light manipulator controller and control method thereof - Google Patents

Abstract

The present invention relates to an embedded light-duty mechanical arm controller, comprising a connected teaching box controller and an embedded master-slave DSP controller, wherein the teaching box controller includes a microprocessor 1, and the microprocessor 1 communicates with the Man-machine interface unit links to each other with embedded master-slave DSP controller; Described embedded master-slave DSP controller comprises microprocessor II and microprocessor III, and described microprocessor II is connected with microprocessor I and dual-port RAM respectively Connection, the dual-port RAM is connected to the microprocessor III, the microprocessor III is connected to the motion control chip I and the motion control chip II respectively, and the motion control chip I and the motion control chip II are respectively connected to the motor driver, the origin switch and the limit switch , the motor driver is connected with the motor, and the output shaft of the motor is connected with the mechanical arm. The invention also discloses a control method of an embedded light mechanical arm. The invention has the advantages of light weight, fast processing speed, low cost, good stability and easy expansion of functions.

Description

Embedded light mechanical arm controller and control method thereof

Technical field

The present invention relates to a kind of Robotics, especially a kind of embedded light mechanical arm controller and control method thereof.

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.

Summary of the invention

The objective of the invention 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, good stability, the function easily embedded light mechanical arm controller and the control method thereof of expansion.

For achieving the above object, the present invention 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.

A kind of embedded light mechanical arm control method is characterized in that, 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 ₀, y ₀, z ₀) (position on support and direction can be chosen wantonly, as long as z ₀Axle 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 ", calculate the position and the attitude of interpolated point while calculating; " walk " to be meant that the joint position that each interpolated point is carried out obtaining after the inverse kinematics conversion need not store while calculating, and directly again by these joint position setting in motions;

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.

In the described step 1) to determine and set up each coordinate system and should adopt following three rules: the motion of each joint i (i is all positive integers between 1 to 6, and 6 is free degree number) is all around z _iThe axle motion; x _iThe vertical z of axle _I-1Axle also points to and leaves z _I-1The direction of axle; y _iAxle must be asked foundation by right-handed coordinate system.

The structure of mechanical arm control system of the present invention 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 present invention has the following advantages, and 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, and is multiple functional, the positional precision height.

Description of drawings

Fig. 1 is a general diagram of the present invention;

Fig. 2 is a teach box hardware circuit interface connection layout of the present invention;

Fig. 3 is a mechanical arm master board hardware interface connection layout of the present invention;

Fig. 4 is that mechanical arm of the present invention is from control panel hardware interface connection layout;

Fig. 5 is a keyboard schematic diagram of the present invention;

Fig. 6 is a mechanical arm schematic diagram of the present invention;

Fig. 7 is an initial point search utility flow chart of the present invention;

Fig. 8 is a joint coordinate system motor program flow chart of the present invention;

Fig. 9 is a single shaft motion subroutine flow chart of the present invention;

Figure 10 is a rectangular coordinate system motor program flow chart of the present invention;

Figure 11 is a regularly right angle moving interpolation schematic diagram of the present invention;

Figure 12 is that tool coordinates of the present invention is the motor program flow chart;

Figure 13 is a cylindrical-coordinate system motor program flow chart of the present invention;

Figure 14 is an inverse kinematic schematic diagram of the present invention.

The specific embodiment

The present invention is further described below in conjunction with drawings and Examples.

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.

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 ₀, y ₀, z ₀) (position on support and direction can be chosen wantonly, as long as z ₀Axle 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. 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 ₀, y ₀, z ₀) (position on support and direction can be chosen wantonly, as long as z ₀Axle gets final product along first articulating shaft).This invention is determined and set up each coordinate system should be according to following three rules: the motion of each joint i (i is all positive integers between 1 to 6, and 6 is free degree number) is all around z _iThe axle motion; x _iThe vertical z of axle _I-1Axle also points to and leaves z _I-1The direction of axle; y _iAxle must be asked foundation by right-handed coordinate system.

Referring to Fig. 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 invention space line interpolation can be divided into following a few step and finish:

Input robot motion's initial point P ₀(x ₀, y ₀, z ₀) 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 _aWith 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 ₀(x ₀, y ₀, z ₀) and P _f(x _f, y _f, z _f) obtain the actual motion distance P _d=| P ₀P _f|; By P _vAnd T _aCan calculate the accelerating and decelerating part required separation distance

If C _d〉=P _d, then actual motion speed Otherwise C _v=P _vBy time T _aWith the interpolation time T _cDraw and quicken step number S _aBy P ₀(x ₀, y ₀, z ₀) and P _f(x _f, y _f, z _f), can get the space the parametric equation of the straight line

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

Between all positive integers) to P ₀Distance be

$C_{\mathrm{Sd}\left(i\right)}=\left|P_i{P}_0\right|=\sqrt{{(x_i-x_0)}^2+{(y_i-y_0)}^2+{(z_i-z_0)}^2}={\mathrm{kP}}_d(C_{\mathrm{Sd}\left(i\right)}$ Expression P _i(x _i, y _i, z _i) to P ₀Distance,

P _d＝|P ₀P _f|) (2)

Making n interpolation section move distance is S _{D (n)}(n=1 ..., i) (n be 1 to all positive integers of i), (i be step number of each interpolated point, 0 and

Between all positive integers) but invocation point P _iTo P ₀Distance (C _{Sd (i-1)}Expression P _I-1(x _I-1, y _I-1, z _I-1) to P ₀Distance, 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):

$k=\frac{C_{\mathrm{Sd}\left(i\right)}}{P_d}=\frac{\underset{n=1}{\overset{i}{\mathrm{\Σ}}}{S}_{d\left(n\right)}}{P_d}=\frac{C_{\mathrm{Sd}(i-1)}+S_{d\left(i\right)}}{P_d}$ 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

(unit is m/s^2), so the speed S of i interpolated point of acceleration Duan Shangdi _{Cv (i)}=iT _cA can obtain

$S_{d\left(i\right)}=\frac{1}{2}(S_{\mathrm{cv}\left(i\right)}+S_{\mathrm{cv}(i-1)})\·T_c=\frac{1}{2}(2i-1){\mathrm{aT}}_c^2(S_{\mathrm{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\left(m\right)}=\frac{1}{2}(S_{\mathrm{cv}\left(m\right)}+S_{\mathrm{cv}(m-1)})\·T_c=\frac{1}{2}[2C_v+a\·(2m-1)\·{T_c}_{}]---\left(5\right)$

This invention 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 _z--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;

θ ₁..., θ ₆------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) $A_1=\left[\begin{array}{cccc}\cos \left({\mathrm{\θ}}_1\right)& -\sin \left({\mathrm{\θ}}_1\right)& 0& 0\\ \sin \left({\mathrm{\θ}}_1\right)& \cos \left({\mathrm{\θ}}_1\right)& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ Denotation coordination is 1 and the homogeneous transformation matrix of coordinate system 0 $A_2=\left[\begin{array}{cccc}\cos \left({\mathrm{\θ}}_2\right)& -\sin \left({\mathrm{\θ}}_2\right)& 0& 0\\ 0& 0& 1& 0\\ -\sin \left({\mathrm{\θ}}_2\right)& -\cos \left({\mathrm{\θ}}_2\right)& 0& 0\\ 0& 0& 0& 1\end{array}\right]$ Denotation coordination is 2 and the homogeneous transformation matrix of coordinate system 1 $A_3=\left[\begin{array}{cccc}\cos \left({\mathrm{\θ}}_3\right)& -\sin \left({\mathrm{\θ}}_3\right)& 0& a_2\\ \sin \left({\mathrm{\θ}}_3\right)& \cos \left({\mathrm{\θ}}_3\right)& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ Denotation coordination is 3 and the homogeneous transformation matrix of coordinate system 2 $A_4=\left[\begin{array}{cccc}\cos \left({\mathrm{\θ}}_4\right)& -\sin \left({\mathrm{\θ}}_4\right)& 0& 0\\ 0& 0& 1& d_4\\ -\sin \left({\mathrm{\θ}}_4\right)& -\cos \left({\mathrm{\θ}}_4\right)& 0& 0\\ 0& 0& 0& 1\end{array}\right]$ Denotation coordination is 4 and the homogeneous transformation matrix of coordinate system 3 $A_5=\left[\begin{array}{cccc}\cos \left({\mathrm{\θ}}_5\right)& -\sin \left({\mathrm{\θ}}_5\right)& 0& 0\\ 0& 1& -1& 0\\ \sin \left({\mathrm{\θ}}_5\right)& \cos \left({\mathrm{\θ}}_5\right)& 0& 0\\ 0& 0& 0& 1\end{array}\right]$ Denotation coordination is 5 and the homogeneous transformation matrix of coordinate system 4 $A_6=\left[\begin{array}{cccc}\cos \left({\mathrm{\θ}}_6\right)& -\sin \left({\mathrm{\θ}}_6\right)& 0& 0\\ 0& 0& 1& 0\\ -\sin \left({\mathrm{\θ}}_6\right)& -\cos \left({\mathrm{\θ}}_6\right)& 0& 0\\ 0& 0& 0& 1\end{array}\right]$ Denotation coordination is 6 and the homogeneous transformation matrix of coordinate system 5 ${}^{0}T_6({\mathrm{\θ}}_1,...,{\mathrm{\θ}}_6)=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]$ 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 ₂, d ₄∈ R represents the length of mechanical arm respective link respectively.Can be with the terminal coordinate system O of motion arm ₆At base coordinate system O ₀Under pose write as following expression: ⁰T ₆=A ₁A ₂A ₃A ₄A ₅A ₆When finding the solution the equation of motion, from ⁰T ₆Begin to find the solution joint position.Make ⁰T ₆Each element of character expression equal ⁰T ₆General type, and determine θ in view of the above ₁In case try to achieve θ ₁Afterwards, can be by A ₁ ^-1Premultiplication ⁰T ₆General type,

A ^-1 ₁ ⁰T ₆＝ ¹T ₆ (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 ₂ ^-1A ₁ ^-10T ₆＝ ²T ₆ (8)

A ₃ ^-1A ₂ ^-1A ₁ ^-10T ₆＝ ³T ₆ (9)

A ₄ ^-1A ₃ ^-1A ₂ ^-1A ₁ ^-10T ₆＝ ⁴T ₆ (10)

A ₅ ^-1A ₄ ^-1A ₃ ^-1A ₂ ^-1A ₁ ^-10T ₆＝ ⁵T ₆ (11)

The levoform of each equation of following formula is ⁰T ₆With the function of preceding (i-1) individual joint variable, available these equations are determined the position in each joint:

θ ₁＝atan2(p _y，p _x)(-3.1415≤θ ₁≤3.1415) (12)

$k=\frac{{p_x}^2+{{\mathrm{<figure-callout\; id="2"\; label="p\; y"\; filenames="HSA00000202232600021.png"\; state="\{\{state\}\}">p</figure-callout>}}_y}^2+{p_z}^2-{a_2}^2-{{\mathrm{<figure-callout\; id="4"\; label="d"\; filenames="HSA00000202232600031.png"\; state="\{\{state\}\}">d</figure-callout>}}_4}^2}{{2a}_2}$

${\mathrm{\&theta;}}_3=-a\tan 2(k,\&PlusMinus;\sqrt{{{\mathrm{<figure-callout\; id="4"\; label="d"\; filenames="HSA00000202232600031.png"\; state="\{\{state\}\}">d</figure-callout>}}_4}^2-k^2})(-3.9268\&le;{\mathrm{\&theta;}}_1\&le;0.7853)---\left(13\right)$

Attention: in the formula, positive and negative number corresponding θ ₃Two kinds of feasible solutions.

${\mathrm{<figure-callout\; id="23"\; label="s"\; filenames="HSA00000202232600031.png"\; state="\{\{state\}\}">s</figure-callout>}}_{23}=\frac{{-a}_2{c}_3{p}_z+(c_1{p}_x+s_1{p}_y)(a_2{s}_3-d_4)}{{{\mathrm{<figure-callout\; id="2"\; label="p\; z"\; filenames="HSA00000202232600021.png"\; state="\{\{state\}\}">p</figure-callout>}}_z}^2+{(c_1{p}_x+s_1{p}_y)}^2}$ $c_{23}=\frac{(-{\mathrm{<figure-callout\; id="4"\; label="d"\; filenames="HSA00000202232600031.png"\; state="\{\{state\}\}">d</figure-callout>}}_4+a_2{s}_3)p_z+(c_1{p}_x+s_1{p}_y)a_2{c}_3}{{{\mathrm{<figure-callout\; id="2"\; label="p\; z"\; filenames="HSA00000202232600021.png"\; state="\{\{state\}\}">p</figure-callout>}}_z}^2+{(c_1{p}_x+s_1{p}_y)}^2}$

θ ₂＝atan2(s ₂₃，c ₂₃)-θ ₃(-1.5707≤θ ₁≤1.5707) (14)

θ ₄＝atan2(-a _xs ₁+a _yc ₁，-a _xc ₁c ₂₃-a _ys ₁c ₂₃+a _zs ₂₃)(-3.1415≤θ1≤3.1415) (15)

Attention: work as s ₅=0 o'clock, mechanical arm was in unusual morpheme.At this moment, joint shaft 4 and 6 overlaps, and can only solve θ ₄With θ ₆And or poor.Whether unusual morpheme can all be judged near zero by two variablees of atan2 in the formula (15).

θ ₅＝atan2(-a _x(c ₁c ₂₃c ₄+s ₁s ₄)-a _y(s ₁c ₂₃c ₄-c ₁s ₄)+a _zs ₂₃c ₄，-a _xc ₁s ₂₃-a _ys ₂₃s ₁-a _zc ₂₃)(-3.9268≤θ ₁≤0.7853) (16)

k ₁＝-n _x(c ₁c ₂₃s ₄-s ₁c ₄)-n _y(s ₁c ₂₃s ₄+c ₁c ₄)+n _zs ₂₃s ₄

k ₂＝n _x((c ₁c ₂₃c ₄+s ₁s ₄)c ₅-c ₁s ₂₃s ₅)+n _y((s ₁c ₂₃c ₄-c ₁s ₄)c ₅-s ₁s ₂₃s ₅)-n _z(s ₂₃c ₄c ₅+c ₂₃s ₅)

θ ₆＝atan2(k ₁，k ₂)(-3.1415≤θ ₁≤3.1415) (17)

Claims (9)

1. an embedded light mechanical arm controller, is characterized in that: comprise connected teaching box controller and embedded master-slave DSP controller, described teaching box controller comprises microprocessor 1, microprocessor I is connected with man-machine interface unit and embedded master-slave DSP controller respectively; Described embedded master-slave DSP controller comprises microprocessor II and microprocessor III, and described microprocessor II is connected with microprocessor I and microprocessor respectively The dual-port RAM is connected, the dual-port RAM is connected with the microprocessor III, the microprocessor III is respectively connected with the motion control chip I and the motion control chip II, and the motion control chip I and the motion control chip II are connected with the motor driver, the origin switch and the limiter respectively. The bit switch is connected, the motor driver is connected with the motor, and the output shaft of the motor is connected with the mechanical arm. 2. The embedded light-duty robotic arm controller according to claim 1, wherein the man-machine interface unit includes a display and a keyboard. 3. The embedded light-duty robotic arm controller according to claim 1, characterized in that: the microprocessor I and the microprocessor II are connected through a serial port communication. 4. The embedded light-duty robotic arm controller according to claim 1, characterized in that: said microprocessor I, microprocessor II and microprocessor III all use TMS320F2812 chips. Both motion control chip I and motion control chip II use MCX314 chip. 5. The embedded light-duty robotic arm controller according to claim 1, characterized in that: the data bus, address bus, and control bus of the microprocessor II and the left data bus, address bus, and control bus of the dual-port RAM Connected, the data bus, address bus, and control bus of the microprocessor III are connected to the right data bus, address bus, and control bus of the dual-port RAM. 6. The embedded light-duty mechanical arm controller according to claim 1, characterized in that: the data bus, address bus, and control bus of the microprocessor III are respectively connected with the data bus of the motion control chip I and the motion control chip II , Address bus, and control bus are connected. 7. The embedded light-duty mechanical arm controller according to claim 1 is characterized in that: the pulse output port 1-4 of the motion control chip 1 is connected with the input port 1-4 of the stepper motor driver, and the motion control chip 1 The I origin signal collection port is connected with the output port 1-4 of the origin switch, and the motion control chip I limit switch collection port is connected with the output port 1-8 of the limit switch; the pulse output port 1-3 of the motion control chip II is connected with the step The input port 5-7 of the motor driver is connected, the motion control chip II origin signal collection port is connected with the output port 5-7 of the origin switch, and the motion control chip II limit switch collection port is connected with the limit switch output port 9-12. 8. A method for controlling an embedded light-duty mechanical arm, comprising the steps of: 1). A regular Cartesian coordinate system (xi _, y _, zi ₎ can be established for each member at the joint axis, where i is any positive integer between 1 and 6, and 6 is a free The number of degrees, plus the base coordinate system (x ₀ , y ₀ , z ₀ ), the position and direction on the machine base are optional, as long as the z ₀ axis is along the first joint motion axis; 2). Establish a 4×4 odd-order transformation matrix for the member coordinate system at each joint, indicating the relationship with the previous member coordinate system; 3).Use the timing interpolation algorithm of "calculate while walking" to calculate the position and attitude of the interpolation point; 4).Using the formula method to calculate the kinematic inverse solution of each axis, and obtain the motion angle of each axis within the interpolation period; 5). The obtained motion angle of each axis is output to the microprocessor III, and the position command of the microprocessor III is output to the motion control chip I and the motion control chip II to control the interpolation motion of each axis. 9. In the step 1) to determine and establish each coordinate system, the following three rules should be adopted: the movement of each joint i moves around the z _i- axis; the x _i- axis is perpendicular to the z _i-1 axis and points away from the z _i- The direction of the ₁ axis; the y _i axis is established according to the requirements of the right-hand coordinate system; where i is all positive integers between 1 and 6, and 6 is the number of degrees of freedom.

Images