CN201728656U - Embedded light mechanical arm controller - Google Patents

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 ₀, y ₀, z ₀) (position on support and direction can be chosen wantonly, as long as z ₀Axle 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 ₀, 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. 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 ₀(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 _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 ₀(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 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

Between) to P ₀Distance be

(C _{Sd (i)}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), (i is step number of each interpolated point, 0 with

Between) 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):

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

(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\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 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;

θ ₁..., θ ₆------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)

Denotation coordination is 1 and the homogeneous transformation matrix of coordinate system 0

Denotation coordination is 2 and the homogeneous transformation matrix of coordinate system 1

Denotation coordination is 3 and the homogeneous transformation matrix of coordinate system 2

Denotation coordination is 4 and the homogeneous transformation matrix of coordinate system 3

Denotation coordination is 5 and the homogeneous transformation matrix of coordinate system 4

Denotation coordination is 6 and the homogeneous transformation matrix of coordinate system 5

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:

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

$k=\frac{{p_x}^2+{{\mathrm{<figure-callout\; id="2"\; label="p\; y"\; filenames="HSA00000202224700021.png,HSA00000202224700031.png"\; state="\{\{state\}\}">p</figure-callout>}}_y}^2+{p_z}^2-{a_2}^2-{d_4}^2}{2a_2}$

${\mathrm{\&theta;}}_3=-a\tan 2(k,\&PlusMinus;\sqrt{{d_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.

$s_{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="HSA00000202224700021.png,HSA00000202224700031.png"\; state="\{\{state\}\}">p</figure-callout>}}_z}^2+{(c_1{p}_x+s_1{p}_y)}^2}$ $c_{23}=\frac{(-d_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="HSA00000202224700021.png,HSA00000202224700031.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≤θ ₁≤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 (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.

Images