CN101913149B - Embedded light mechanical arm 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 manipulator controller and control method thereof

technical field

The invention relates to a robot technology, in particular to an embedded light mechanical arm controller and a control method thereof.

Background technique

The robotic arm is an automatic device with the function of grabbing and moving workpieces used in the automated production process. It is a new type of device developed in the mechanized and automated production process. Robotic arms can replace humans to complete dangerous, repetitive and boring tasks, reduce human labor intensity, and improve labor productivity. The mechanical arm has been used more and more widely. In the machinery industry, it can be used for parts assembly, handling, loading and unloading of workpieces, especially in automatic CNC machine tools and combined machine tools.

At present, the manipulator control system is generally divided into two categories: one type uses industrial computer and control card, uses WINDOWS operating system, the control system is heavy, the processing speed of the manipulator algorithm is slow, the price is high, and the system is unstable; the other type uses 80 series single-chip microcomputer, the control system hardware is too simple, and the function is difficult to expand.

Contents of the invention

The object of the present invention is to overcome the shortcomings of the above-mentioned prior art, and provide an embedded light-duty robotic arm controller and its control method that are light in weight, fast in processing speed, low in cost, good in stability, and easily expandable in function.

To achieve the above object, the present invention adopts the following technical solutions:

A kind of embedded light mechanical arm controller is characterized in that: comprise the connected teaching box controller and embedded master-slave DSP (digital signal processor) controller, described teaching box controller comprises microprocessor 1 , microprocessor I is connected with man-machine interface unit and embedded master-slave DSP (digital signal processor) controller respectively; Described embedded master-slave DSP (digital signal processor) controller comprises microprocessor II and microprocessor Device III, the microprocessor II is connected with the microprocessor I and the dual-port RAM respectively, the dual-port RAM is connected with the microprocessor III, the microprocessor III is connected with the motion control chip I and the motion control chip II respectively, and the motion control The chip I and the motion control chip II are respectively connected with 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 man-machine interface unit includes a display and a keyboard.

The microprocessor I and the microprocessor II are connected through a serial port communication.

Described microprocessor I, microprocessor II and microprocessor III all adopt TMS320F2812 chip.

The data bus, address bus, and control bus of the microprocessor II are connected to the left data bus, address bus, and control bus of the dual-port RAM, and the data bus, address bus, and control bus of the microprocessor III are connected to the right side of the dual-port RAM. Data bus, address bus, and control bus are connected.

The data bus, address bus, and control bus of the microprocessor III are respectively connected to the data bus, address bus, and control bus of the motion control chip I and the motion control chip II.

Both the motion control chip I and the motion control chip II are MCX314 chips.

The pulse output port 1-4 of described motion control chip 1 links to each other with the input port 1-4 of stepper motor driver, and motion control chip 1 origin signal acquisition port links to each other with the output port 1-4 of origin switch, and motion control chip 1 limits The position switch acquisition port is connected to the output port 1-8 of the limit switch; the pulse output port 1-3 of the motion control chip II is connected to the input port 5-7 of the stepper motor driver, and the origin signal acquisition port of the motion control chip II is connected to the origin The output port 5-7 of the switch is connected, and the limit switch acquisition port of the motion control chip II is connected with the limit switch output port 9-12.

The output of the motor driver is in the form of positive and negative pulses, and the motor is a two-phase hybrid stepping motor.

A method for controlling an embedded light-duty mechanical arm, comprising the steps of:

1). For each member, a normal Cartesian coordinate system (xi _, y _, zi ₎ can be established at the joint axis (i is all positive integers between 1 and 6, and 6 is the number of degrees of freedom ), 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; "calculate while walking" means that the joint position obtained after inverse kinematic transformation of each interpolation point does not need to be stored , and start to move directly according to these joint positions;

4).Using the formula method to calculate the kinematic inverse solution of each axis (kinematic inverse solution refers to the known end position and attitude to find the angle of each joint), and obtain the motion angle of each axis in the interpolation cycle;

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.

In the said steps 1) the following three rules should be adopted for determining and establishing each coordinate system: the motion of each joint i (i is all positive integers between 1 and 6, and 6 is the number of degrees of freedom) revolves around the _z axis Movement; the x _i axis is perpendicular to the z _i-1 axis and points to the direction away from the z _i-1 axis; the y _i axis is established according to the requirements of the right-handed coordinate system.

The structure of the manipulator control system of the present invention is controlled by a master-slave microprocessor, and the microprocessor II is used as the host computer, which is responsible for system management, manipulator language compilation and man-machine interface functions, and also utilizes its computing power to complete coordinate transformation , track interpolation, kinematics positive solution, kinematics inverse solution, and regularly send the calculation results as joint movement increments to the public memory for the microprocessor III to read it. Microprocessor III completes digital control of all joint positions. It reads the given value from the common memory, and also sends the actual position of each joint back to the common memory, which is used by the microprocessor II. The public memory is a dual-port RAM with a capacity of 8KB. The control rate of this type of system is fast, generally up to 10ms.

By adopting the above scheme, the present invention has the following advantages. One is that the self-designed mechanical arm motion controller can meet the control requirements of the mechanical arm through experimental verification. It is reliable in operation and low in cost and can be used and sold as a mechanical arm motion controller; The lightweight mechanical arm has light weight, small size, low power consumption and small size of the control system, and is suitable for the application needs of mobile operation and operation of robots. The third is that the embedded light-duty robotic arm can realize complex linear interpolation and circular interpolation movements; the fourth is that the system adopts a modular design, which is open, readable, expandable, and maintainable for continuous development. Fifth, the motion controller of the manipulator adopts a master-slave microprocessor. The microprocessor II realizes kinematics positive and negative solution and interpolation algorithm, and the microprocessor III realizes motion control, and the processing speed is fast. Sixth, the controller has a driver control interface and an origin switch acquisition port, with complete functions and high position accuracy.

Description of drawings

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

Fig. 2 is a connection diagram of the hardware circuit interface of the teaching box of the present invention;

Fig. 3 is a hardware interface connection diagram of the main control board of the mechanical arm of the present invention;

Fig. 4 is the connection diagram of the hardware interface of the mechanical arm of the present invention from the control board;

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

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

Fig. 7 is a flow chart of the origin search program of the present invention;

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

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

Fig. 10 is a flow chart of the Cartesian coordinate system motion program of the present invention;

Fig. 11 is a schematic diagram of timing rectangular interpolation motion of the present invention;

Fig. 12 is a flow chart of the tool coordinate system motion program of the present invention;

Fig. 13 is a flowchart of the motion program of the cylindrical coordinate system of the present invention;

Fig. 14 is a schematic diagram of kinematic inverse solution of the present invention.

Detailed ways

The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

See Figure 1, a controller for an embedded light-duty robotic arm, including a teaching box controller and an embedded master-slave DSP controller. The teaching box controller is composed of a keyboard module, a liquid crystal display module, a serial port communication module and a microprocessor 1. The output of the control keyboard is connected to the input of the microprocessor I, the input and output of the microprocessor I is connected to the input and output of the liquid crystal display, and the serial port of the microprocessor I communicates with the serial port of the mechanical arm microprocessor II. Embedded master-slave DSP controller consists of microprocessor II, dual-port RAM, microprocessor III, motion control chip I, motion control chip II, photoelectric switch processing circuit, limit switch processing circuit, stepping motor drive module, holding Brake release control and other components. The data bus, address bus, and control bus of the microprocessor II are connected to the left data bus, address bus, and control bus of the dual-port RAM, and the right data bus, address bus, and control bus of the dual-port RAM are connected to the microprocessor III. The data bus, address bus, and control bus of processor III are connected to the data bus, address bus, and control bus of motion control chip I and motion control chip II, and the pulse output ports 1-4 of motion control chip I are connected to those of the stepper motor driver. The input port 1-4 is connected, the motion control chip I origin signal acquisition port is connected with the output port 1-4 of the origin switch, the motion control chip I limit switch acquisition port is connected with the output port 1-8 of the limit switch, and the motion control chip The pulse output port 1-3 of II is connected with the input port 5-7 of the stepper motor driver, the origin signal acquisition port of the motion control chip II is connected with the output port 5-7 of the origin switch, and the limit switch acquisition port of the motion control chip II is connected with the Limit switch output ports 9-12 are connected.

The man-machine interface unit includes a keyboard and a liquid crystal display, which are connected with the microprocessor 1 respectively.

Microprocessor I communicates with microprocessor II through the serial port.

Microprocessor I, microprocessor II and microprocessor III all use TMS320F2812 chip.

Microprocessor I collects data commands from the keyboard and sends them to microprocessor II through serial port communication. The movement speed, issued commands, and the position of the mechanical arm are displayed through the hydraulic module.

Microprocessor II and microprocessor III carry out data exchange through dual-port RAM. The data bus, address bus, and control bus of the microprocessor II are connected to the left data bus, address bus, and control bus of the dual-port RAM, and the data bus, address bus, and control bus of the microprocessor III are connected to the right data bus of the dual-port RAM , Address bus, and control bus are connected.

The data bus, address bus and control bus of the microprocessor III are connected with the data bus, address bus and control bus of the motion control chip I and the motion control chip II.

Both motion control chip I and motion control chip II use MCX314 chip.

The positive and negative pulse signals of the motion control chip 1 output control the stepper motor driver 1-4, the motion control chip 1 origin signal acquisition port is connected with the output port 1-4 of the origin switch, and the motion control chip 1 limit switch acquisition port is connected with the limit switch. The output port 1-8 of the bit switch is connected. The positive and negative pulse signals output by the motion control chip II control the stepper motor driver 5-7, the origin signal acquisition port of the motion control chip II is connected with the output port 5-7 of the origin switch, and the limit switch acquisition port of the motion control chip II is connected with the limit switch. Bit switch output ports 9-12 are connected.

The output of the driver is in the form of positive and negative pulses, and the motor is a two-phase hybrid stepping motor.

The structure of the manipulator control system is controlled by a master-slave microprocessor, and the microprocessor II is the host computer, which is responsible for system management, manipulator language compilation and human-machine interface functions, and also uses its computing power to complete coordinate transformation and trajectory insertion. Complement, kinematics positive solution, kinematics inverse solution, and regularly send the operation result as the increment of joint movement to the public memory for the microprocessor III to read it. Microprocessor III completes digital control of all joint positions. It reads the given value from the common memory, and also sends the actual position of each joint back to the common memory, which is used by the microprocessor II. The public memory is a dual-port RAM with a capacity of 8KB. The control rate of this type of system is fast, generally up to 10ms.

Referring to Figure 2, the teaching box controller is composed of a microprocessor I, a liquid crystal module, a logic level converter, a keyboard management module, a keyboard, a voltage regulator chip I, a voltage regulator chip II, a serial port receiver transmitter, and a serial port. The voltage stabilizing chip 1 and the voltage stabilizing chip 1 supply power to the microprocessor 1. GIPIOB1 of the microprocessor is connected with 2 pins of ADG3308, GPIOB5 is connected with 5 pins, and XINT2 is connected with 6 pins. GPIOA0-7 is connected to DB0-7 of the liquid crystal module, GPIOB0 is connected to REQ, GPIOB2 is connected to CS, and the liquid crystal module is powered by 5V. The 16 feet of ADG3308 are connected with the DATA feet of HD7279, and the 15 feet are connected with the KEY feet. GPIOB3 of microprocessor 1 is connected with CS pin of HD7279, and GPIOB4 is connected with CLK pin. The output of the keyboard is connected to DIG0-7 and DP-SG of HD7279. SCITXDA of microprocessor I is connected to 11 pins of MAX3232, SCIRXDA is connected to 12 pins, and 13 and 14 pins of MAX3232 are connected to the serial port. Referring to Fig. 3, the embedded main controller includes dual-port RAM, microprocessor II, serial port receiver transmitter, serial port, and slave controller interface. XD0-15 of microprocessor II is connected to IO0-15L of dual-port RAM, /XRD is connected to /OEL, /XWE is connected to R//WL, /XZCS2 is connected to /CEL, XA0-11 is connected to A0-11L. The M//S of the dual-port RAM is connected to 3.3V and set to the master mode. IO0-15R, /OER, R//WR, /CER, A0-11R of the dual-port RAM are connected to the interface of the slave controller. The SCITXDA of the microprocessor II is connected to the 11th pin of the MAX3232, the SCIRXDA is connected to the 12th pin, and the 13th and 14th pins of the MAX3232 are connected to the serial port.

See Figure 4, the embedded slave control board includes dual-port RAM interface, microprocessor III, 16M active crystal oscillator, motion control chip 1, motion control chip 2, optocoupler isolation, driver interface, origin switch interface, limit switch interface . IO0-15R, /OER, R//WR, /CER, A0-11R of the dual-port RAM interface are connected to XD0-15, /XRD, /XWE, /XZCS2, XA0-11 of the microprocessor III. XD0-15, /XRD, /XWE, XA14, and XA0-2 of the microprocessor III are respectively connected to D0-15, RDN, WRN, CSN, and A0-2 of the motion control chip I. XD0-15, /XRD, /XWE, XA13, and XA0-2 of the microprocessor III are respectively connected to D0-15, RDN, WRN, CSN, and A0-2 of the motion control chip II. The output port of the 16M active crystal oscillator is connected to the motion Control pin 53 of chips I and II. The output interfaces of the positive limit switches 1-4 are respectively connected to pins 69, 87, 97, and 116 of the motion control chip I through optocoupler isolation; the output interfaces of the negative limit switches 5-8 are respectively connected to the motion control chip I through optocoupler isolation. The 70, 88, 98, 117 pins of the origin switch; the output ports of the origin switch 1, 2, 3, and 4 are respectively connected to the 73, 93, 101, and 120 pins of the motion control chip I through optocoupler isolation; the 35, 36 pins of the motion control chip I The pins are respectively connected to the positive pulse and negative pulse input port of driver 1; the 38 and 39 pins of the motion control chip 1 are respectively connected to the positive pulse and negative pulse input ports of the driver 2; the 40 and 41 pins of the motion control chip 1 are respectively connected to the driver 3 Positive pulse, negative pulse input port; 42,43 pins of motion control chip 1 connect the positive pulse of driver 4, negative pulse input port respectively. The output ports of origin switches 5, 6, and 7 are respectively connected to pins 73, 93, and 101 of the motion control chip II through optocoupler isolation; the output ports of positive limit switches 9-10 are respectively connected to pin 69 of the motion control chip I through optocoupler isolation. , 87 pins; the output interface of the negative limit switch 11-12 is respectively connected to the 70, 88 pins of the motion control chip I through optocoupler isolation; the 35, 36 pins of the motion control chip II are respectively connected to the positive pulse and negative pulse input of the driver 5 38,39 pins of the motion control chip II are respectively connected to the positive pulse and negative pulse input ports of the driver 6; 40 and 41 pins of the motion control chip II are respectively connected to the positive pulse and negative pulse input ports of the driver 7.

See Figure 5, the schematic diagram of the keyboard, S+ indicates the positive movement of the first axis of the joint coordinate system of the manipulator, the X+ movement of the Cartesian coordinate system, the X+ movement of the tool coordinate system, and the θ+ movement of the cylindrical coordinate system, and S- indicates the movement of the first axis of the joint coordinate system Negative movement, Cartesian coordinate system X-movement, tool coordinate system X-movement, cylindrical coordinate system θ-movement; L+ indicates the positive movement of the second axis of the manipulator joint coordinate system, Cartesian coordinate system Y+ movement, tool coordinate system Y+ movement, Cylindrical coordinate system r+ motion, L- represents the negative motion of the second axis of the joint coordinate system, Y- motion of the Cartesian coordinate system, Y- motion of the tool coordinate system, r- motion of the cylindrical coordinate system; U+ represents the positive motion of the third axis of the joint coordinate system Movement, Cartesian coordinate system Z+ movement, tool coordinate system Z+ movement, cylindrical coordinate system Z+ movement, U- indicates the negative movement of the third axis of the joint coordinate system, Cartesian coordinate system Z-movement, tool coordinate system Z-movement, cylindrical coordinate system Z-movement; R+ indicates the positive movement of the fourth axis of the joint coordinate system, R- indicates the negative movement of the fourth axis of the joint coordinate system; B+ indicates the positive movement of the fifth axis of the joint coordinate system, B- indicates the fifth axis of the joint coordinate system T+ indicates the positive movement of the sixth axis of the joint coordinate system, T- indicates the negative movement of the sixth axis of the joint coordinate system; M+ indicates the opening of the gripper, M- indicates the closing of the gripper; V+ indicates the increase in speed, V - means to reduce the speed; press the origin search key to execute the origin search movement; when the coordinate system switching key is pressed, the coordinate system changes in the following order: joint-right angle-tool-cylindrical.

A method for controlling an embedded light-duty mechanical arm, comprising the steps of:

1) A regular Cartesian coordinate system (x _i , y _i , z _i ) can be established for each member at the joint axis (i is all positive integers between 1 and 6, and 6 is the number of degrees of freedom) , 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 of "walking while calculating" ("walking while calculating" means that the joint positions obtained after the inverse kinematics transformation of each interpolation point do not need to be stored, but directly start to move according to these joint positions) Complementary algorithm to calculate the position and attitude of the interpolation point;

4).Using the formula method to calculate the kinematic inverse solution of each axis (kinematic inverse solution refers to the known end position and attitude to find the angle of each joint), and obtain the motion angle of each axis in the interpolation cycle;

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.

Referring to Figure 6, a normal Cartesian coordinate system (xi _{, y} , zi ₎ can be established for each member at the joint axis (i is all positive integers between 1 and 6, and 6 is the degree of freedom number), 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). The invention determines and establishes each coordinate system according to the following three rules: the motion of each joint i ₍ i is all positive integers between 1 and 6, and 6 is the number of degrees of freedom) moves around the _z axis; The axis is perpendicular to the z _i-1 axis and points away from the z _i-1 axis; the y _i axis is established according to the requirements of the right-handed coordinate system.

Referring to Figure 7, the process of returning to the mechanical origin is: set acceleration and deceleration, speed and other parameters; close the origin switch acquisition port; move half of the active space of each axis in the positive direction; open the origin switch acquisition port; Origin switch, execute the deceleration stop subroutine of the corresponding axis.

Refer to Figures 8 and 9, joint coordinate system movement, first input the acceleration and deceleration and speed values of the movement, and judge whether a specific key is pressed or released by reading the state of the port. Accelerate and drive continuously with the specified parameters; when the button is released, the system issues a deceleration and stop command.

Refer to Figures 10, 11, 12, and 13, Cartesian coordinate system movement, first input the acceleration and deceleration and speed values of the movement, and judge the pressing or release of a specific key by reading the state of the port. After the specific key is pressed, the system will The specified axis performs positive and negative solution algorithm, linear interpolation or circular interpolation; when the key is released, the system sends a deceleration stop command.

The space linear interpolation of this invention can be divided into the following steps to complete:

Input the initial point P ₀ (x ₀ , y ₀ , z ₀ ) and end point P _f (x _f , y _f , z _f ) (f is the abbreviation of final) of the robot movement, the velocity P _v , the acceleration and deceleration time T _a and Interpolation cycle T _c , running time T;

The determination of basic parameters and the solution method of interpolation points. Since the linear motion of the robot in space needs to go through the acceleration and deceleration and uniform motion segments, it should be determined whether P _v meets the acceleration and deceleration requirements before interpolation motion. Methods as below:

若C_d≥P_d，则实际运动速度

否则C_v＝P_v；由时间T_a和插补时间T_c得出加速步数S_a。由P₀(x₀，y₀，z₀)和P_f(x_f，y_f，z_f)，可得空间直线参数方程From P ₀ (x ₀ , y ₀ , z ₀ ) and P _f (x _f , y _f , z _f ), the actual moving distance P _d = |P ₀ P _f |; from P _v and T _a can be calculated Required distance for deceleration section

If C _d ≥ P _d , the actual movement speed

Otherwise C _v =P _v ; the number of acceleration steps S _a is obtained from the time T _a and the interpolation time T _c . From P ₀ (x ₀ , y ₀ , z ₀ ) and P _f (x _f , y _f , z _f ), the parametric equation of the space line can be obtained

之间的所有正整数)到P₀的距离为Therefore, from formula (1), each interpolation point P _i (xi _, y _i , zi ₎ can be obtained (i is the step number of each interpolation point, between 0 and

All positive integers between ) to P ₀ distance is

$C_{\mathrm{SD}\left(i\right)}=\left|P_i{P}_0\right|=\sqrt{{(x_i-x_0)}^2+{({\mathrm{the\; y}}_i-{\mathrm{the\; y}}_0)}^2+{(z_i-z_0)}^2}={\mathrm{kP}}_d(C_{\mathrm{SD}\left(i\right)}$ Indicates the distance from P _i (xi _{, y} , zi ₎ to P ₀ ,

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

(C_Sd(i-1)表示P_i-1(x_i-1，y_i-1，z_i-1)到P₀的距离，S_d(i)是第i插补段运动距离)，故由式(1)和(2)得到各插补点比例因子k的计算公式如下：Let the movement distance of the nth interpolation segment be S _d(n) (n=1,...,i) (n is all positive integers from 1 to i), (i is the step number of each interpolation point, at 0 and All positive integers between) can get the distance from point P _i to P ₀

(C _Sd(i-1) represents the distance from P _i-1 (xi _-1 , y _i-1 , zi _-1 ) to P ₀ , S _d(i) is the movement distance of the i-th interpolation segment), Therefore, the calculation formula of the scale factor k of each interpolation point obtained from formulas (1) and (2) is as follows:

$k=\frac{C_{\mathrm{SD}\left(i\right)}}{P_d}=\frac{\underset{\mathrm{no}=1}{\overset{i}{\mathrm{\Σ}}}{S}_{d\left(\mathrm{no}\right)}}{P_d}=\frac{C_{\mathrm{SD}(i-1)}+S_{d\left(i\right)}}{P_d}$ Where k is a scaling factor (0≤k≤1) (3)

From formula (3), k can be obtained, and the Cartesian coordinates of the interpolation points can be obtained. Therefore, the key to the spatial linear interpolation algorithm is to determine the movement distance S _d(i) of each interpolation segment. The following introduces the method of calculating S _d(i) in each segment of the movement:

(单位是m/s^2)，因此加速度段上第i个插补点的速度S_cv(i)＝iT_c·a，可得到Accelerate the motion segment. Since the acceleration section of the robot designed in this paper is a uniform acceleration motion, the acceleration can be obtained from the actual motion speed C _v and the acceleration and deceleration time T _a

(the unit is m/s^2), so the speed S _cv(i) of the i-th interpolation point on the acceleration section =iT _c ·a, can be obtained

$S_{d\left(i\right)}=\frac{1}{2}(S_{\mathrm{cv}\left(i\right)}+S_{\mathrm{cv}(i-1)})\&Center\; Dot;T_c=\frac{1}{2}(2i-1){\mathrm{aT}}_c^2(S_{\mathrm{cv}(i-1)}$ Indicates the speed of the i-1th interpolation point) (4)

uniform motion segment. Since the robot designed in this paper must go through the deceleration section, and the interpolation operation is "walking while calculating", so it is necessary to calculate whether the remaining distance can meet the deceleration requirements of the system before the start of the uniform motion section. The moving distance of each interpolation segment in the constant speed segment S _d(i) = C _v T _c

则减速度段上第m个插补点的速度S_cv(m)＝C_v+mT_c·a，即可得到Deceleration segment. Since the rounding calculation is performed when calculating the number of acceleration steps S _a , it is not possible to simply invert the acceleration of the acceleration section and then plan the acceleration section. This will introduce errors, so the acceleration of the deceleration section should be recalculated. After passing through the previous i-1 interpolation points, the remaining distance L _d(i) = P _d -C _Sd(i-1) can be obtained, so the acceleration of the deceleration section can be obtained

Then the speed S _cv(m) of the mth interpolation point on the deceleration section = C _v +mT _c ·a, can be obtained

${\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; )</span>}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; cv</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; )</span>}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; cv</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&CenterDot;</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; c</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; [</span>\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; a</span>}<span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&Center\; Dot;</span>{{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; c</span>}}}_{}<span\; class="notranslate">\; ]</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; )</span>$

This invention adopts the formula method to carry out kinematics inverse solution (kinematics inverse solution refers to knowing the end position and posture to find the angle of each joint, as shown in Figure 14):

_x

p _y

P _z ------ indicates the position of the end of the robot arm in the world coordinate system;

n _x o _x a _x

n _y o _y a _y

n _{z o z} a _z ------ Indicates the attitude of the end of the robot arm in the world coordinate system;

θ ₁ ,..., θ ₆ ------ indicates the angle of movement of each axis;

A _i ∈ R ^4×4 (i=1, 2,..., 6)------is the conversion matrix between the coordinate systems on each connecting rod established according to the DH coordinate system.

s _i ----- means sinθ _i ;

c _i ------ means cosθ _i ;

s _ij ------ means sin(θ _i +θ _j );

c _ij ------- means cos(θ _i +θ _j ) $A_1=\left[\begin{array}{cccc}\cos \left({\mathrm{\&theta;}}_1\right)& -\sin \left({\mathrm{\&theta;}}_1\right)& 0& 0\\ \sin \left({\mathrm{\&theta;}}_1\right)& \cos \left({\mathrm{\&theta;}}_1\right)& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ Represents the homogeneous transformation matrix of coordinate system 1 and coordinate system 0 $A_2=\left[\begin{array}{cccc}\cos \left({\mathrm{\&theta;}}_2\right)& -\sin \left({\mathrm{\&theta;}}_2\right)& 0& 0\\ 0& 0& 1& 0\\ -\sin \left({\mathrm{\&theta;}}_2\right)& -\cos \left({\mathrm{\&theta;}}_2\right)& 0& 0\\ 0& 0& 0& 1\end{array}\right]$ Represents the homogeneous transformation matrix of coordinate system 2 and coordinate system 1 $A_3=\left[\begin{array}{cccc}\cos \left({\mathrm{\&theta;}}_3\right)& -\sin \left({\mathrm{\&theta;}}_3\right)& 0& a_2\\ \sin \left({\mathrm{\&theta;}}_3\right)& \cos \left({\mathrm{\&theta;}}_3\right)& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ Represents the homogeneous transformation matrix of coordinate system 3 and coordinate system 2 $A_4=\left[\begin{array}{cccc}\cos \left({\mathrm{\&theta;}}_4\right)& -\sin \left({\mathrm{\&theta;}}_4\right)& 0& 0\\ 0& 0& 1& d_4\\ -\sin \left({\mathrm{\&theta;}}_4\right)& -\cos \left({\mathrm{\&theta;}}_4\right)& 0& 0\\ 0& 0& 0& 1\end{array}\right]$ Represents the homogeneous transformation matrix of coordinate system 4 and coordinate system 3 $A_5=\left[\begin{array}{cccc}\cos \left({\mathrm{\&theta;}}_5\right)& -\sin \left({\mathrm{\&theta;}}_5\right)& 0& 0\\ 0& 1& -1& 0\\ \sin \left({\mathrm{\&theta;}}_5\right)& \cos \left({\mathrm{\&theta;}}_5\right)& 0& 0\\ 0& 0& 0& 1\end{array}\right]$ Represents the homogeneous transformation matrix of coordinate system 5 and coordinate system 4 $A_6=\left[\begin{array}{cccc}\cos \left({\mathrm{\&theta;}}_6\right)& -\sin \left({\mathrm{\&theta;}}_6\right)& 0& 0\\ 0& 0& 1& 0\\ -\sin \left({\mathrm{\&theta;}}_6\right)& -\cos \left({\mathrm{\&theta;}}_6\right)& 0& 0\\ 0& 0& 0& 1\end{array}\right]$ Represents the homogeneous transformation matrix of coordinate system 6 and coordinate system 5 $T_6^0({\mathrm{\&theta;}}_1,...,{\mathrm{\&theta;}}_6)=\left[\begin{array}{cccc}{\mathrm{no}}_x& o_x& a_x& p_x\\ {\mathrm{no}}_{\mathrm{the\; y}}& o_{\mathrm{the\; y}}& a_{\mathrm{the\; y}}& p_{\mathrm{the\; y}}\\ {\mathrm{no}}_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]$ It determines the position and attitude of the end of the manipulator relative to the machine base coordinate system.

n - the normal vector of the hand

s-hand swipe vector

a - the approach vector of the hand

p-hand position vector (6)

The coordinate system O _i (i is a positive integer i=0, 1, ..., 6) is the DH coordinate system established on the manipulator arm; a ₂ , d ₄ ∈ R respectively represent the length. The pose of the manipulator end coordinate system O ₆ in the base coordinate system O ₀ can be written as the following expression: ⁰ T ₆ =A ₁ A ₂ A ₃ A ₄ A ₅ A ₆ . When solving the equation of motion, start to solve the joint position from ⁰ T ₆ . Make each element of the symbolic expression of ⁰ T ₆ equal to the general form of ⁰ T ₆ , and determine θ ₁ accordingly. Once θ ₁ is obtained, the general form of A ₁ ^-1 can be multiplied by ⁰ T ₆ to the left to get

A ^-1 ₁ ⁰ T ₆ ＝ ¹ T ₆ (7)

This formula can be used to solve other joint variables. Constantly multiplying (7) by the inverse matrix of A to the left, the following four other matrix equations can be obtained:

A ₂ ^-1 A ₁ ^-10 T ₆ = ² T ₆ (8)

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

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

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

The left equations of the above equations are functions of ⁰ T ₆ and the first (i-1) joint variables, and these equations can be used to determine the position of each joint:

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

$\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; =</span>\frac{{{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; the\; <figure-callout\; id="2"\; label="y"\; filenames="HSA00000202232600021.png"\; state="\{\{state\}\}">y</figure-callout></span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}{{\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}}$

${\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; a</span>}\mathrm{<span\; class="notranslate">\; the\; tan</span>}\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; \&PlusMinus;</span>\sqrt{{{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; k</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; (</span><span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 3.9268</span>}<span\; class="notranslate">\; \&le;</span>{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; \&le;</span>\mathrm{<span\; class="notranslate">\; 0.7853</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 13</span>}<span\; class="notranslate">\; )</span>$

Note: In the formula, the positive and negative signs correspond to two possible solutions of θ ₃ .

${\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; twenty\; three</span>}}<span\; class="notranslate">\; =</span>\frac{{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}}<span\; class="notranslate">\; +</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}<span\; class="notranslate">\; )</span>}{{{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="p\; z"\; filenames="HSA00000202232600021.png"\; state="\{\{state\}\}">p</figure-callout></span><figure-callout\; id="2"\; label="p\; z"\; filenames="HSA00000202232600021.png"\; state="\{\{state\}\}">\; </figure-callout>}}_{\mathrm{<span\; class="notranslate">\; </span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}$ ${\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; twenty\; three</span>}}<span\; class="notranslate">\; =</span>\frac{<span\; class="notranslate">\; (</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}}<span\; class="notranslate">\; +</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}}{{{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="p\; z"\; filenames="HSA00000202232600021.png"\; state="\{\{state\}\}">p</figure-callout></span><figure-callout\; id="2"\; label="p\; z"\; filenames="HSA00000202232600021.png"\; state="\{\{state\}\}">\; </figure-callout>}}_{\mathrm{<span\; class="notranslate">\; </span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}$

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

θ ₄ ＝atan2(-a _x s ₁ +a _y c ₁ ，-a _x c ₁ c ₂₃ -a _y s ₁ c ₂₃ +a _z s ₂₃ )(-3.1415≤θ1≤3.1415) (15)

Note: when s ₅ =0, the manipulator is in a singular position. At this time, joint axes 4 and 6 coincide, and only the sum or difference of θ ₄ and θ ₆ can be solved. The singular shape can be judged by whether the two variables of atan2 in formula (15) are close to zero.

θ ₅ ＝atan2(-a _x (c ₁ c ₂₃ c ₄ +s ₁ s ₄ )-a _y (s ₁ c ₂₃ c ₄ -c ₁ s ₄ )+a _z s ₂₃ c ₄ ,-a _x c ₁ s ₂₃ -a _y s ₂₃ s ₁ -a _z c ₂₃ )(-3.9268≤θ ₁ ≤0.7853) (16)

k ₁ ＝-n _x (c ₁ c ₂₃ s ₄ -s ₁ c ₄ )-n _y (s ₁ c ₂₃ s ₄ +c ₁ c ₄ )+ _nz s ₂₃ 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 mechanical arm controller according to claim 1, characterized in that: said microprocessor I, microprocessor II and microprocessor III all adopt TMS320F2812 chip, motion control chip I, motion control chip II adopts 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 robotic arm using the embedded light-duty robotic arm controller as claimed in claim 1, comprising the steps of: 1). Establish a regular Cartesian coordinate system (x _i , y _i , z _i ) at the joint axis for each member, where i is all positive integers between 1 and 6, and i is the degree of freedom number, plus the base coordinate system (x ₀ , y ₀ , z ₀ ), the position and direction on the base are optional, as long as the z ₀ axis is along the first joint axis; 2). Establish a 4×4 odd transformation matrix for the Cartesian coordinate system of each member at the joint axis, indicating the relationship with the Cartesian coordinate system of the previous member at the joint axis; 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 kinematics inverse solution of each joint axis, and obtain the motion angle of each joint axis within the interpolation period; 5). The obtained motion angle of each joint 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 joint axis. 9. The method according to claim 8, characterized in that, determining and establishing each coordinate system in the step 1) should adopt the following three rules: the motion of each joint i moves around the z _i axis; x _i The axis is perpendicular to the z _i-1 axis and points away from the z _i-1 axis; the y _i axis is established according to the requirements of the right-handed coordinate system; where i is all positive integers between 1 and 6, and i is the number of degrees of freedom.

Images