CN103823467A - Control method of industrial robot demonstration planner with motion planning function - Google Patents

Abstract

The invention discloses a control method of an industrial robot demonstration planner with a motion planning function. The method comprises: establishing a new demonstration file or opening a demonstration file, setting the system parameters and demonstration parameters of a robot, controlling the robot in a robot task space to carry out hand-operated demonstration, and utilizing the programming instruction of a VAL-III language to carry out demonstration programming; performing robot kinetics inversion, converting a three-dimensional locus into a joint space locus, inserting a locus intermediate point in the joint space locus, planning a locus equation, and obtaining the locus equation of a joint space; carrying out time interval interpolation calculating, and obtaining a robot control sequence point; and sending the obtained robot control sequence point to a robot controller through an RS-485 serial port. According to the invention, a demonstration system and a motion planning system are integrally combined, a robot motion planning function is provided, and the demonstration working efficiency is high.

Description

Control method of industrial robot teaching planner with motion planning function

This patent application is a divisional application of the invention patent application with the application number 201210161778.1, the application date is May 23, 2012, and the invention title is "Industrial robot teaching planner with motion planning function and its control method".

technical field

The invention relates to a control method of a teaching planner, in particular to a control method of an industrial robot teaching planner with a motion planning function.

Background technique

As computer integrated manufacturing technology approaches from the laboratory stage to the practical stage of factories, the demand for industrial robots is increasing, and the market for robotic automated production is also increasing. The wide application of industrial robots has a great impact on human-computer interaction. Higher requirements are put forward. As an important optional part of the robot controller, the teaching box can provide a good human-machine interface, with teaching programming and on-site monitoring functions, and is widely used in the safe teaching and production control of industrial robots. . However, at present, most of the existing teaching boxes are composed of a control panel and a display screen, which are relatively large in size and have many buttons, which makes the teaching operation inconvenient and the work efficiency is not high. Moreover, a planning controller, such as an industrial computer, is needed between the robot controller and the teaching box to communicate with the industrial computer, and then the industrial computer is connected to the robot controller. Not only is the cost high, but it is also easy to receive The electromagnetic interference brought by it, and the communication path is cumbersome, which may easily cause system communication failure and cause safety hazards. Therefore, it is of great economic value and application prospect to design a teaching box with low cost, small size, simple and efficient, reliable communication, and integrated motion planning function control method.

Contents of the invention

In view of the problems that the existing robot teaching box has a large volume, many buttons, and needs to be connected with an industrial computer and is inconvenient to operate, the purpose of the present invention is to provide a control method for an industrial robot teaching planner with a motion planning function.

The technical solution adopted in the present invention comprises the following steps:

S1. Create a teaching file or open a teaching file: if creating a teaching file, proceed to step S2; if opening a teaching file, proceed to step S10;

S2, set the system parameters and teaching parameters of the robot, the system parameters include the D-H parameters of the robot structure, and the teaching parameters include selecting motion mode and speed mode;

S3. Control the robot to perform manual teaching in the robot task space, and confirm the key points and running time of the task space according to the pose of the manual teaching of the robot. The key points include the specified starting point, end point and gripper action;

S4. Use the programming instructions of VAL-Ⅲ language to perform teaching programming;

S5. Carry out inverse solution of robot kinematics, convert key points in task space into passing points in joint space, and convert 3D space trajectory into joint space trajectory;

S6. Insert a trajectory intermediate point into the joint space trajectory. The trajectory intermediate point includes a starting point, a lifting point, a lowering point and an ending point;

S7. According to the trajectory equation of each trajectory intermediate point obtained in step S6, the planning method of the trajectory equation is 4-3-4 polynomial trajectory planning or 3-5-3 polynomial trajectory planning, calculate the optimal planning equation, and obtain the trajectory of the joint space equation;

S8. Carry out interpolation calculation at fixed time intervals to the trajectory equation obtained in step S7 to obtain robot control sequence points;

S9. If the teaching work is not over, return to step S2, reset the system parameters and teaching parameters, and perform the teaching work again; if the teaching work is over, directly execute the next step;

S10. Send the robot control sequence point obtained in step S8 to the robot controller through the RS-485 serial bus.

The setting of the system parameters in the step S2 adopts the four-joint teaching operation, the movement mode of the teaching parameters is selected as continuous, and the speed mode is selected as medium speed.

The step S3 specifically includes: manually teaching the robot to move through the expected key points in the task space, and then passing through the key points in the task space that require robot operation actions, and recording the coordinates of the above two key points in the task space to confirm the accuracy of the teaching movement. operation hours.

The VAL-Ⅲ language programming instruction in the described step S4 is made up of movement instruction, operation instruction and auxiliary instruction; Movement instruction includes MOVJ, MOVL, MOVC, MOVS; Operation instruction includes PICKUP, PUTDOWN; Auxiliary instruction includes START, END, DELAY .

The step S5 specifically solves the key points of the task space through the inverse kinematics of the robot to obtain the key points of each joint space in the 4 joints, and through continuous solving, the continuous trajectory of the three-dimensional Cartesian space can be converted into the trajectories of the 4 joints. Continuous trajectory in joint space.

The trajectory equation of the joint space obtained in the step S7 is a jerk continuous trajectory equation, a time optimal trajectory equation or an optimal smooth trajectory equation.

The fixed time interval for the fixed time interval interpolation in the step S8 is 20 ms, and the trajectory equation is solved every 20 ms to obtain the corresponding time trajectory point, that is, the control sequence point of the robot joint space.

Compared with background technology, the beneficial effect that the present invention has is:

It not only provides an efficient and practical teaching system, but also has the function of robot motion trajectory planning, and sends the command results of trajectory planning to the robot controller directly through RS-485 serial communication. Such a control strategy can organically combine the teaching system and the motion planning system, save the use of the trajectory planning industrial computer, not only reduce the cost, but also reduce the complex communication links, reduce the probability of system communication failure, and improve the data quality. The stability of the transmission, while avoiding the electromagnetic interference brought by the PC, improves the safety of the on-site operation.

Description of drawings

Fig. 1 is the overall structure diagram of the teaching planner of the present invention.

Fig. 2 is a software structure diagram of the teaching planner control method of the present invention.

Figure 3 is a teaching flow chart of the teaching planner.

Figure 4 is a structural diagram of the serial communication protocol.

Figure 5 is a planning trend diagram of the trajectory equation.

Detailed ways

The present invention will be described in further detail below in conjunction with the accompanying drawings and specific examples, but the embodiments of the present invention are not limited thereto.

As shown in Figure 1, the present invention comprises core processor, program data memory, USB extension interface, RS-485 serial port communication interface, Ethernet communication interface, power supply and reset circuit and liquid crystal touch screen; Core processor is respectively connected with program data memory, USB expansion interface, RS-485 serial communication interface, Ethernet communication interface, power supply and reset circuit are connected with LCD touch screen, the core processor adopts ARM9 embedded core processing chip, Ethernet communication interface is connected with PC, RS-485 serial communication The interface is connected with the robot controller.

The core processing chip adopts an embedded operating system.

The main components of the teaching planner of the present invention are described as follows:

The core processor adopts the S3C2240A microprocessor of SAMSUNG Company, based on the ARM920T core, has the characteristics of low power consumption and high integration, integrates rich internal resources, and provides a powerful external storage controller.

The program data memory mainly includes Nor Flash interface, Nand Flash interface and SDRAM interface circuit. NorFlash and Nand Flash have the function of power-down protection. SDRAM stores the program in execution and the generated data for dynamic storage. The operating speed of SDRAM is Fast, large capacity, and low cost. In this embodiment, two 16-bit 32MB (4Banks×4M×16-bit) K4S561632 are selected as the SDRAM of the system. Considering the needs of later porting Linux systems and QT graphical interfaces, this system uses K9F1208 as the SDRAM. Nand Flash memory, its capacity is 64M.

The USB expansion interface is used to expand the mobile storage device, which can realize the storage and transfer of teaching data, including the storage and reading management of teaching files, the extraction of command characters and data characters in teaching files, and the storage and storage of system parameter files. read.

The LCD touch screen adopts the TFT LCD TOUCH Module model of CHILIN Company, without other control panel buttons, small in size, easy to operate, and provides a good human-computer interaction interface.

The Ethernet communication interface adopts the 16-bit Ethernet controller CS8900A of Cirrus Logic Company, which provides 10M Ethernet access bandwidth. The Ethernet controller can be connected with a PC for customizing and transplanting the embedded Linux operating system. During the operating system process, the embedded system needs to download dozens of megabytes of image data from the PC, and at the same time, it needs to establish an NFS network file system, use ztelnet to log in to transfer files and other operations, and has high communication efficiency through Ethernet.

The RS-485 serial communication interface adopts the integrated serial communication interface of S3C2440A, communicates with the robot controller, and sends control instructions. The controller controls each joint according to the instructions received from the teaching box.

In addition, the serial communication interface used for debugging and the spare serial communication interface are also connected to the core processor. The serial communication interface used for debugging is used as a debugging console (console) when transplanting and debugging the Linux system.

The main functions of the robot teaching planner in the present invention are to monitor the state of the robot, set system parameters, operate the robot movement, edit the robot task, plan the robot movement trajectory, and complete the communication transmission of data and commands with the robot controller. The core processor is responsible for the control and processing of the entire teaching planning system, and is connected to the program data memory and external memory through its perfect storage interface, connected to the built-in LCD controller through the LCD touch screen display interface, and connected to the built-in LCD controller through the Ethernet communication interface. Connected to PC, used to transplant embedded operating system, connected to PC through serial communication interface for debugging, console for debugging embedded operating system, connected to robot controller through RS-485 serial communication interface, used to send robot control commands.

The software structure block diagram of the teaching planner control method with motion planning function of the present invention is as shown in Figure 2: first set up a cross-compilation environment on the PC, customize and compile the Linux embedded operating system kernel, and then transplant the embedded operating system module Into the core processor of the teaching planner, on this basis, the teaching planner completes the development of the underlying hardware driver, and then completes the design and development of the user program. The user program includes the human-computer interaction user interface module, communication module and robot Teaching and motion planning modules.

The embedded operating system module considers the stability and real-time requirements of the teaching planner, and facilitates the management of multi-tasks and human-computer interaction functions. In this embodiment, the embedded operating system Linux is used, and the Linux operating system supports various Mainstream hardware devices and the latest hardware technology have abundant driver resources. At the kernel level, the kernel of the Linux system is efficient and stable. The kernel is divided into five parts: process scheduling, memory management, inter-process communication, virtual file system and network interface. The customized mechanism is convenient for users to tailor according to their needs, and adapts to the needs of embedded systems; at the application level, it has a complete network communication and file management mechanism, as well as rich open source codes and convenient development and debugging tools. For the development of graphical interface on embedded systems, there is a relatively mature graphical interface library ready-made to integrate the development environment, making it as convenient and direct for users as programming on a PC. The development steps of the embedded Linux platform generally include downloading Bootloader, customizing and compiling the kernel, making the root file system, and running user programs. After BIOS and Bootloader complete the hardware initialization of the system, including system clock setting, memory area mapping, and stack pointer setting, etc., they jump to the entry of the operating system kernel and hand over the control authority to the operating system. U-Boot is used, which includes some common Peripheral driver is a powerful board support package, which reduces the development of subsequent underlying hardware drivers. You can burn BootLoader into Flash through JTAG tools; after completing BootLoader programming, you need to customize and compile the Linux kernel. Conditional compilation to tailor the corresponding functions and modules, use the make menuconifg command to configure the kernel module through the graphical interface, set the correct device number and baud rate to ensure the normal use of the console terminal, and specify the file system support, specify the network, touch screen , real-time clock and other common drivers, and at the same time write and configure the underlying drivers of the newly added hardware, and finally compile and generate a kernel image; the last step needs to create a root file system, and the system uses the Yaffs (Yet AnotherFlash File System) file system; The program is transplanted to the embedded Linux system to run, realize the function of robot teaching operation, and complete the robot kinematics solution, trajectory planning and communication functions.

The human-computer interaction interface module adopts QT software. It is a cross-platform graphical interface application program that supports Windows, Linux and other operating systems. QT only needs to develop the application program once, without rewriting the source code, and can deploy these applications across different desktop and embedded operating systems. The excellent cross-platform features make QT widely used in embedded systems, and various functions required for compilation can be developed on the PC, and transplanted to the embedded operating system through the Ethernet at one time through the cross-compiler. It can be deployed across different desktop and embedded operating systems without rewriting the source code. It is simple and convenient, and it is developed with object-oriented C++ language. The good packaging mechanism makes QT highly modular and reusable. It is more flexible, and also has rich API and a large number of development documents, which is very convenient for users.

The communication interface module uses the RS-485 serial communication interface to connect with the underlying robot controller. The controller controls each joint according to the instructions received from the teaching box, is responsible for receiving teaching instructions and task files, and sending robot status information. Let the robot execute the trajectory movement required by the program to achieve program playback. Since the communication between the teaching box and the main controller is relatively frequent, in order to ensure the stability of communication and the accuracy of data, a communication protocol must be established. The communication protocol considers the data function type The division of data and the definition of data format type, the communication protocol structure is shown in Figure 4 below, including 2 bytes of function code, 2 bytes of file beginning mark, 2 bytes of file statement number, 32 bytes of file data body, The end-of-file flag is 2 bytes.

The robot teaching and motion planning module mainly completes the following functions: 1. Robot jogging motion, including individual motion of four joints and motion in XYZ three directions, and the motion speed can be changed by setting the speed coefficient; 2. Robot display Teaching, including teaching operations, creating, saving, editing, deleting teaching files, executing teaching instructions, returning to zero, emergency stop, etc. Among them, the teaching language adopts the simplified VAL-Ⅲ language. Common instructions include: movement instructions, such as MOVJ, MOVL, MOVC, MOVS, operation instructions: PICKUP, PUTDOWN, auxiliary instructions: START, END, DELAY, etc., the specific format and Command explanation can refer to Table 1, using four-joint teaching operation; 3. Robot motion command and status display: including the expected and actual movement of each joint of the robot and the expected and actual movement of the robot's three-dimensional space; 4. Function keys and number keys . 5. Robot planning method: 4-3-4 polynomial trajectory planning or 3-5-3 polynomial trajectory planning.

As shown in Fig. 3, the method steps of the present invention are as follows embodiment:

S1. Create a teaching file or open a teaching file: if creating a teaching file, proceed to step S2; if opening a teaching file, proceed to step S10;

S2, set the system parameters and teaching parameters of the robot, the system parameters include the D-H parameters of the robot structure, and the teaching parameters include selecting motion mode and speed mode;

The setting of the system parameters adopts the four-joint teaching operation, and the D-H system parameters of the robot are shown in Table 1; the teaching parameter motion mode is selected as continuous, and the speed mode is selected as medium speed.

Table 1

joint θ/° d/mm a/mm α/° 1 θ ₁ 0 a ₁ 0 2 θ ₂ 0 a ₂ 0 3 0 d ₃ 0 90 4 θ ₄ 0 0 0

S3. Control the robot to perform manual teaching in the robot task space, and confirm the key points and running time of the task space according to the pose of the manual teaching of the robot. The key points include the specified starting point, end point and gripper action;

According to the actual work requirements, manually teach the robot to move through the expected key points in the task space, such as bypassing some obstacle points, and then pass through the key points in the task space that require robot operation actions, and record the coordinates of the above two key points in the task space, Confirm the running time of the teaching movement;

S4. Use the programming instructions of VAL-Ⅲ language to perform teaching programming;

Programming instructions in VAL-Ⅲ language are composed of movement instructions, operation instructions and auxiliary instructions; movement instructions include MOVJ, MOVL, MOVC, MOVS; operation instructions include PICKUP, PUTDOWN; auxiliary instructions include START, END, DELAY. The specific format and instruction explanation of programming instructions are shown in Table 2;

Table 2

S5. Carry out inverse solution of robot kinematics, convert key points in task space into passing points in joint space, and convert 3D space trajectory into joint space trajectory;

Through inverse kinematics of the robot, the key points of the task space are solved to obtain the key points of each joint space among the four joints. Through continuous solution, the continuous trajectory of the three-dimensional Cartesian space can be converted into the continuous trajectory of the joint space of the four joints;

S6. Insert a trajectory intermediate point into the joint space trajectory. The trajectory intermediate point includes a starting point, a lifting point, a lowering point and an ending point;

Set the middle point of the trajectory including the starting point, lifting point, lowering point and end point to make the trajectory of the entire joint space smooth and continuous, and there will be no large mutations and impacts during the trajectory movement, ensuring the stability of the robot's motion;

S7, according to step S6 obtains each track intermediate point planning track equation;

According to kinematics and dynamics constraints, choose 4-3-4 polynomial trajectory planning or 3-5-3 polynomial trajectory planning planning method to carry out trajectory planning, calculate the optimal planning equation, and obtain the trajectory equation of the joint space, the trajectory equation is added Acceleration continuous trajectory equation, time optimal trajectory equation or optimal smooth trajectory equation;

S8. Carry out interpolation calculation at fixed time intervals to the trajectory equation obtained in step S7 to obtain robot control sequence points;

The fixed time interval for fixed time interval interpolation is 20ms, and the trajectory equation is solved every 20ms to obtain the trajectory points corresponding to the time, that is, the control sequence points of the robot joint space.

S9. If the teaching work is not over, return to step S2; if the teaching work is over, perform the next step;

If the teaching work is not finished, go back to step S2, reset the system parameters and teaching parameters, and perform the teaching work again; if the teaching work is finished, directly execute the next step.

S10. Send the robot control sequence points obtained in step S8 to the robot controller through the RS-485 serial bus.

The RS-485 serial port communication interface is used to connect with the bottom robot controller. The controller controls each joint according to the instructions received from the teaching box, and is responsible for receiving teaching instructions and task files, sending robot status information, and letting the robot execute the program. The required trajectory movement realizes program playback. Since the communication between the teaching box and the main controller is relatively frequent, in order to ensure the stability of communication and the accuracy of data, a communication protocol must be established. The communication protocol considers the division of data function types and data The definition of the format type, the structure of the communication protocol is shown in Figure 4 below, including 2 bytes of function code, 2 bytes of file beginning mark, 2 bytes of file statement number, 32 bytes of file data body, and 2 bytes of file end mark bytes.

As shown in Figure 5, when the planning method of the trajectory equation is selected as 4-3-4 polynomial trajectory planning, the motion of the robot is divided into the starting point, lifting point, lowering point and end point to complete the trajectory planning. The trend of the planning trajectory equation can be Refer to Figure 5. The starting point requires both speed and acceleration to be 0, the lifting point and lowering point require continuous movement, and the ending point also requires the speed and acceleration to be 0. The first section is expressed by a quartic polynomial from the starting point to the lifting point, the trajectory of the middle section from the lifting point to the lowering point is expressed by a cubic polynomial, and the last section is expressed by a quartic polynomial from the lowering point to the end point. The connection points of each section guarantee speed and Acceleration is continuous.

As shown in Figure 5, when the planning method of the trajectory equation is selected as 3-5-3 polynomial trajectory planning, the motion of the robot is divided into the starting point, the lifting point, the lowering point and the end point to complete the motion trajectory planning. The trend of the planning trajectory equation can be Refer to Figure 5. The starting point requires both speed and acceleration to be 0, the lifting point and lowering point require continuous movement, and the ending point also requires the speed and acceleration to be 0. The first section is expressed by a cubic polynomial from the starting point to the lifting point, the trajectory of the middle section from the lifting point to the lowering point is expressed by a fifth-degree polynomial, and the last section is expressed by a cubic polynomial from the lowering point to the end point. The connection points of each section guarantee the speed and acceleration continuous.

The above-mentioned embodiment is only the preferred embodiment of the present invention, but the embodiment of the present invention is not limited by the above-mentioned embodiment, and any other changes, substitutions and combinations that do not deviate from the essential principle of the present invention within the scope of the technical solution All are included in the scope of protection of the present invention.

Claims (7)

1. a control method that possesses the industrial robot teaching planner of motion planning function, is characterized in that comprising the following steps:

S1, newly-built teaching file or open teaching file: if newly-built teaching file carries out step S2; If open teaching file, carry out step S10;

S2, the systematic parameter that robot is set and teaching parameter, systematic parameter comprises robot architecture's D-H parameter, teaching parameter comprises selects motor pattern and velocity mode;

S3, in robot task space, control carries out manual teaching, confirms task space key point and working time according to the pose of robot hand teaching, and key point comprises specifies starting point, terminal and handgrip action;

S4, utilize the programming instruction of VAL-III language to carry out teach programming;

S5, carry out that robot kinematics is contrary to be separated, task space key point is converted to joint space through point, three dimensions track is converted to joint space track;

S6, in joint space track, insert track intermediate point, track intermediate point comprises starting point, hoist point, setting point and terminating point;

S7, the each track intermediate point planned trajectory equation obtaining according to step S6, the planning mode of equation of locus is 4-3-4 polynomial expression trajectory planning or 3-5-3 polynomial expression trajectory planning, calculation optimization is planned equation, obtains the equation of locus of joint space;

S8, the equation of locus that step S7 the is obtained interval interpolation calculation of fixing time, obtains robot control sequence point;

If S9 teaching work does not also finish, get back to S2 step, reset systematic parameter and teaching parameter, re-start teaching work; If teaching end-of-job, directly carries out next step;

S10, the robot control sequence point that step S8 is obtained send in robot controller by RS-485 serial bus.

2. the control method of a kind of industrial robot teaching planner that possesses motion planning function according to claim 1, it is characterized in that: arranging of the systematic parameter in described step S2 adopts four joint teaching operations, the motor pattern of teaching parameter is chosen as continuously, and velocity mode is chosen as medium speed.

3. the control method of a kind of industrial robot teaching planner that possesses motion planning function according to claim 1, it is characterized in that: described step S3 specifically: manually teaching robot moves through the task space key point of expection, task space key point through needing robot manipulation to move again, and record the task space key point coordinate of above-mentioned two kinds, confirm the working time of teaching campaign.

4. the control method of a kind of industrial robot teaching planner that possesses motion planning function according to claim 1, is characterized in that: the VAL-III Programming with Pascal Language instruction in described step S4 is made up of movement instruction, operational order and house-keeping instruction; Movement instruction comprises MOVJ, MOVL, MOVC, MOVS; Operational order comprises PICKUP, PUTDOWN; House-keeping instruction comprises START, END, DELAY.

5. the control method of a kind of industrial robot teaching planner that possesses motion planning function according to claim 1, it is characterized in that: described step S5 specifically solves the key point of task space by robot inverse kinematics the key point of 4 each joint spaces in joint, by continuous solving, the continuous path of three-dimensional cartesian space can be converted to the joint space continuous path in 4 joints.

6. the control method of a kind of industrial robot teaching planner that possesses motion planning function according to claim 1, is characterized in that: the equation of locus of the joint space that described step S7 obtains is acceleration continuous path equation, time optimal equation of locus or optimal smoothing equation of locus.

7. the control method of a kind of industrial robot teaching planner that possesses motion planning function according to claim 1, it is characterized in that: the fixing time of interval interpolation of fixing time in described step S8 is spaced apart 20ms, every 20ms solves equation of locus, obtain the tracing point of corresponding time, i.e. the control sequence of robotic joint space point.

Images