Inverse solution method, device and readable medium of five-axis loading and unloading robot

Abstract

The present invention discloses an inverse solution method, device and readable medium for a five-axis loading and unloading robot, which relates to the field of logistics automation equipment. The method establishes a connecting rod coordinate system based on a three-dimensional model of a mechanical arm of the five-axis loading and unloading robot, and establishes a conversion matrix between the coordinate systems; a first control point and a first control direction are set in the robot base coordinate system {OB}, and a second control point and a second control direction are set in the fifth joint axis dynamic coordinate system {OJ5d}; a kinematic relationship between the second control point and the second control direction and the first control point and the first control direction is established based on the conversion matrix; θ1, θ2, θ3, θ4 and θ5 are solved by a geometric method based on the equation relationship, where θ n The nth rotation axis's rotation angle is used, and the arm lengths of the first and second support arms are calculated based on θ2 and θ3, achieving five degrees of freedom of motion of the terminal rotating arm in Cartesian space. This invention solves the problem of automated loading and unloading of luggage, improves loading and unloading efficiency, and reduces labor costs.

Description

Technical Field

[0001] The present invention relates to the field of inverse solution methods for automated loading and unloading equipment, and in particular to an inverse solution method, device and readable medium for a five-axis loading and unloading robot. Background Art

[0002] Goods are usually packed in cartons or wooden boxes before loading. Rectangular boxes and bags can be neatly placed in the truck compartment, saving space and improving transportation efficiency.

[0003] With the rapid development of the manufacturing, food, logistics, and e-commerce industries in recent years, "small, lightweight" cargo boxes and bags are increasingly appearing in the loading and unloading process. Traditional handling tools such as forklifts are inefficient and extremely impractical when it comes to unloading and loading small, lightweight cargo boxes and bags. Manual handling is labor-intensive and inefficient, and often leads to the embarrassing situation of being unable to recruit workers. Existing logistics processes face the problem of difficult to automate the loading and unloading of "small, lightweight, and large" boxes and bags, as well as low loading and unloading efficiency and high labor costs. Therefore, designing dedicated automated loading and unloading equipment has become an effective solution to this problem. Summary of the Invention

[0004] In response to the above-mentioned technical problems, the embodiment of the present application aims to propose an inverse solution method, device and readable medium for a five-axis loading and unloading robot to solve the technical problems mentioned in the above background technology section.

[0005] In a first aspect, the present invention provides an inverse solution method for a five-axis loading and unloading robot, comprising the following steps:

[0006] S1, based on a three-dimensional model of a five-axis loading and unloading robot arm, establish its connecting rod coordinate system and establish a transformation matrix between the coordinate systems, wherein the five-axis loading and unloading robot includes a first longitudinal rotating arm, a second longitudinal rotating arm, a third longitudinal rotating arm, a transverse rotating arm, a first support arm, and a second support arm;

[0007] S2, setting a first control point and a first control direction in the robot base coordinate system {OB}, setting a second control point and a second control direction in the fifth joint axis dynamic coordinate system {OJ5d}, and establishing a kinematic relationship between the second control point and the second control direction and the first control point and the first control direction according to the transformation matrix;

[0008] S3, according to the equation relationship, use the geometric method to solve θ1, θ2, θ3, θ4, θ5, where θ n is the nth rotation axis J n The rotation angle, n = 1, 2, 3, 4, 5;

[0009] S4 , calculating the arm length D1 of the first support arm and the arm length D2 of the second support arm according to θ2 and θ3 .

[0010] Preferably, the five-axis loading and unloading robot also includes a movable base and a rotating platform, the rotation axis of the rotating platform is J1, the first longitudinal rotating arm is connected to the rotating platform, and the rotation axis between the two is J2; the second longitudinal rotating arm is connected to the first longitudinal rotating arm, and the rotation axis between the two is J3; the third longitudinal rotating arm is connected to the second longitudinal rotating arm, and the rotation axis between the two is J4; the transverse rotating arm is connected to the third longitudinal rotating arm, and the rotation axis between the two is J5; one side of the first support arm is installed on the first longitudinal rotating arm, and the other side is installed on the rotating platform, S1 is the installation dimension of the first support arm on the first longitudinal rotating arm, and S2 is the installation dimension of the first support arm on the rotating platform; one side of the second support arm is installed on the second longitudinal rotating arm, and the other side is installed on the rotating platform, S3 is the installation dimension of the second support arm on the second longitudinal rotating arm, and S4 is the installation dimension of the second support arm on the rotating platform.

[0011] Preferably, the connecting rod coordinate system includes the robot base coordinate system {OB}, the first joint axis static coordinate system {OJ1s}, the first joint axis dynamic coordinate system {OJ1d}, the second joint axis static coordinate system {OJ2s}, the second joint axis dynamic coordinate system {OJ2d}, the third joint axis static coordinate system {OJ3s}, the third joint axis dynamic coordinate system {OJ3d}, the fourth joint axis static coordinate system {OJ4s}, the fourth joint axis dynamic coordinate system {OJ4d}, the fifth joint axis static coordinate system {OJ5s}, and the fifth joint axis dynamic coordinate system {OJ5d};

[0012] Establish the transformation matrix between the two adjacent coordinate systems above in sequence

[0013]

[0014]

[0015]

[0016]

[0017]

[0018]

[0019]

[0020]

[0021]

[0022]

[0023]

[0024] Among them, L n is the length of the nth joint arm.

[0025] Preferably, step S2 specifically includes:

[0026] Set the first control point in the robot base coordinate system {OB} B P = [xyz 1] T and the first control direction B V=[ij k 0] T , set the second control point in the fifth joint axis dynamic coordinate system {OJ5d} J5d P=[0 L5 0 1] T and the second control direction J5d V=[0 1 0 0] T ;

[0027] Transform the second control point and the second control direction in the fifth joint axis dynamic coordinate system {OJ5d} to the robot base coordinate system {OB} and establish the equation:

[0028]

[0029] Preferably, step S3 includes:

[0030] Calculate the origin coordinates of the fifth joint axis dynamic coordinate system {OJ5d} in the robot base coordinate system {OB} B P OJ5d =[x OJ5d y OJ5d z OJ5d 1] T , the calculation formula is: B P OJ5d = B P-L5*norm( B V), where norm( B V) is a vector B The normalized vector of V is expanded to give the following result:

[0031]

[0032] θ1 is determined by x OJ5d with y OJ5d The calculation formula is as follows:

[0033] θ1=-arctan2(x OJ5d ,y OJ5d);

[0034] The first longitudinal rotation arm, the second longitudinal rotation arm, and the third longitudinal rotation arm together form a longitudinal rotation plane. The unit normal vector of the longitudinal rotation plane is calculated in the robot base coordinate system {OB}. B V ver-lon-plane :

[0035]

[0036] θ5 is the first control direction B V=[ijk 0] T The angle between θ5 and the longitudinal rotation plane is as follows:

[0037]

[0038] At the same time, find the first control direction B V=[ijk 0] T Projection vector in the longitudinal rotation plane B V pro-in-lon-plane , the formula is as follows:

[0039] B V pro-in-lon-plane =norm( B V)-(norm( B V) B V ver-lon-plane )* B V ver-lon-plane ;

[0040] Calculate the origin coordinates of the fourth joint axis dynamic coordinate system {OJ4d} in the robot base coordinate system {OB} B P OJ4d =[x OJ4d y OJ4d z OJ4d 1] T , the formula is as follows:

[0041] B P OJ4d = B P OJ5d -L4*norm( B V pro-in-lon-plane );

[0042] Calculate the angle between the third longitudinal rotating arm and the rotating platform in the longitudinal rotating plane in the robot base coordinate system {OB} B C J4 , the formula is as follows:

[0043] B C J4 =arccos(norm(B V pro-in-lon-plane )·[0 0 1 0] T );

[0044] In the static coordinate system {OJ2s} of the second joint axis, solve θ2, θ3, and θ4 using the following formulas:

[0045]

[0046] Preferably, step S4 specifically uses the following formula to convert θ2 and θ3 into D1 and D2:

[0047]

[0048] Preferably, it also includes: using a servo hydraulic cylinder to indirectly control θ2 and θ3, and sending θ1, D1, D2, θ4, and θ5 to the drivers and motors on their respective corresponding joints to achieve five-degree-of-freedom movement of the lateral rotating arm in Cartesian space.

[0049] In a second aspect, the present invention provides an inverse solution device for a five-axis loading and unloading robot, comprising:

[0050] a coordinate establishment module configured to establish a link coordinate system based on a three-dimensional model of a robotic arm of a five-axis loading and unloading robot, and to establish a transformation matrix between the coordinate systems, wherein the five-axis loading and unloading robot includes a first longitudinal rotating arm, a second longitudinal rotating arm, a third longitudinal rotating arm, a transverse rotating arm, a first support arm, and a second support arm;

[0051] a kinematic relationship building module configured to set a first control point and a first control direction in the robot base coordinate system {OB}, set a second control point and a second control direction in the fifth joint axis dynamic coordinate system {OJ5d}, and establish a kinematic relationship between the second control point and the second control direction and the first control point and the first control direction according to a transformation matrix;

[0052] The first solving module is configured to solve θ1, θ2, θ3, θ4, and θ5 using a geometric method according to the equation relationship, where θ n is the nth rotation axis J n The rotation angle, n = 1, 2, 3, 4, 5;

[0053] The second solving module is configured to calculate the arm length D1 of the first support arm and the arm length D2 of the second support arm according to θ2 and θ3.

[0054] In a third aspect, the present invention provides an electronic device comprising one or more processors; a storage device for storing one or more programs, wherein when the one or more programs are executed by one or more processors, the one or more processors implement the method described in any implementation manner in the first aspect.

[0055] In a fourth aspect, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the method described in any implementation manner in the first aspect.

[0056] Compared with the prior art, the present invention has the following beneficial effects:

[0057] (1) The present invention realizes the five-degree-of-freedom motion control of the end link of the five-axis loading and unloading robot by performing kinematic modeling on the five-axis loading and unloading robot to derive the kinematic relationship and using the geometric method to obtain the inverse solution.

[0058] (2) The present invention solves the problem of "small size, light weight, large quantity" luggage being difficult to load and unload automatically in the logistics process, thereby improving loading and unloading efficiency and reducing labor costs. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0060] Figure 1 is a diagram of an exemplary device architecture to which an embodiment of the present application may be applied;

[0061] Figure 2 Schematic diagram of the flow of the inverse solution method of the five-axis loading and unloading robot according to an embodiment of the present application;

[0062] Figure 3 A front view of a five-axis loading and unloading robot according to an embodiment of the present application;

[0063] Figure 4 A top view of a five-axis loading and unloading robot according to an inverse solution method of the five-axis loading and unloading robot according to an embodiment of the present application;

[0064] Figure 5 A schematic diagram of a coordinate system involved in establishing a kinematic relationship for an inverse solution method of a five-axis loading and unloading robot according to an embodiment of the present application;

[0065] Figure 6 Schematic diagram of the geometric relationship when solving D1 and D2 in the inverse solution method of the five-axis loading and unloading robot according to an embodiment of the present application;

[0066] Figure 7 Schematic diagram of an inverse solution device of a five-axis loading and unloading robot according to an embodiment of the present application;

[0067] Figure 8 It is a structural diagram of a computer device suitable for implementing the electronic device of the embodiment of the present application. DETAILED DESCRIPTION

[0068] To make the objectives, technical solutions, and advantages of the present invention more apparent, the present invention will be further described in detail below with reference to the accompanying drawings. It is apparent that the embodiments described are only some, not all, of the present invention. All other embodiments derived by persons of ordinary skill in the art based on the embodiments of the present invention without creative effort are intended to fall within the scope of protection of the present invention.

[0069] Figure 1 An exemplary device architecture 100 is shown to which an inverse solution method of a five-axis loading and unloading robot or an inverse solution device of a five-axis loading and unloading robot according to an embodiment of the present application can be applied.

[0070] like Figure 1 As shown, the device architecture 100 may include terminal devices 101, 102, 103, a network 104, and a server 105. The network 104 is used to provide a medium for communication links between the terminal devices 101, 102, 103 and the server 105. The network 104 may include various connection types, such as wired or wireless communication links or fiber optic cables.

[0071] Users can use terminal devices 101, 102, 103 to interact with server 105 via network 104 to receive or send messages, etc. Various applications, such as data processing applications and file processing applications, can be installed on terminal devices 101, 102, 103.

[0072] Terminal devices 101, 102, and 103 can be hardware or software. When terminal devices 101, 102, and 103 are hardware, they can be various electronic devices, including but not limited to smartphones, tablet computers, laptop computers, and desktop computers. When terminal devices 101, 102, and 103 are software, they can be installed in the electronic devices listed above. They can be implemented as multiple software or software modules (for example, software or software modules used to provide distributed services), or they can be implemented as a single software or software module. No specific limitations are given here.

[0073] The server 105 may be a server that provides various services, such as a background data processing server that processes files or data uploaded by the terminal devices 101, 102, and 103. The background data processing server may process the acquired files or data and generate processing results.

[0074] It should be noted that the inverse solution method of the five-axis loading and unloading robot provided in the embodiment of the present application can be executed by the server 105, or by the terminal devices 101, 102, and 103. Accordingly, the inverse solution device of the five-axis loading and unloading robot can be set in the server 105, or in the terminal devices 101, 102, and 103.

[0075] It should be understood that Figure 1 The number of terminal devices, networks, and servers in the above description is merely illustrative. Any number of terminal devices, networks, and servers may be provided as needed. If the processed data does not need to be acquired remotely, the above-described apparatus architecture may not include a network, but only require servers or terminal devices.

[0076] Figure 2 An inverse solution method for a five-axis loading and unloading robot provided in an embodiment of the present application is shown, comprising the following steps:

[0077] S1. Establish a connecting rod coordinate system based on the three-dimensional model of the robotic arm of the five-axis loading and unloading robot, and establish a transformation matrix between each coordinate system, wherein the five-axis loading and unloading robot includes a first longitudinal rotating arm, a second longitudinal rotating arm, a third longitudinal rotating arm, a transverse rotating arm, a first support arm, and a second support arm.

[0078] Specifically, refer to Figure 3-6The five-axis loading and unloading robot includes a movable base 1, a rotating platform 2, three longitudinal rotating arms, a transverse rotating arm 6, and two support arms. Specifically, the five-axis loading and unloading robot includes a first longitudinal rotating arm 3, a second longitudinal rotating arm 4, a third longitudinal rotating arm 5, a transverse rotating arm 6, a first support arm 7, and a second support arm 8. The five-axis loading and unloading robot is used to transport cargo 9. The movable base 1 enables the robot's overall spatial movement, and the robot only moves as a whole when its target position exceeds its reach. The rotating platform 2 rotates about an axis J1, and all other equipment is mounted on the rotating platform 2. The first longitudinal rotating arm 3 is connected to the rotating platform 2, and the rotation axis therebetween is J2; the second longitudinal rotating arm 4 is connected to the first longitudinal rotating arm 3, and the rotation axis therebetween is J3; the third longitudinal rotating arm 5 is connected to the second longitudinal rotating arm 4, and the rotation axis therebetween is J4; the transverse rotating arm 6 is connected to the third longitudinal rotating arm 5, and the rotation axis therebetween is J5; one side of the first support arm 7 is installed on the first longitudinal rotating arm 3, and the other side is installed on the rotating platform 2, S1 is the installation dimension of the first support arm 7 on the first longitudinal rotating arm 3, and S2 is the installation dimension of the first support arm 7 on the rotating platform 2; one side of the second support arm 8 is installed on the second longitudinal rotating arm 4, and the other side is installed on the rotating platform 2, S3 is the installation dimension of the second support arm 8 on the second longitudinal rotating arm 4, and S4 is the installation dimension of the second support arm 8 on the rotating platform 2. L1 is the arm length of the first articulated arm, that is, the length from the rotation axis J2 to the rotation axis J1; L2 is the arm length of the second articulated arm, that is, the length from the rotation axis J3 to the rotation axis J2, which is also the length of the first longitudinal rotation arm 3; L3 is the arm length of the third articulated arm, that is, the length from the rotation axis J4 to the rotation axis J3, which is also the length of the second longitudinal rotation arm 4; L4 is the arm length of the fourth articulated arm, that is, the length from the rotation axis J5 to the rotation axis J4, which is also the length of the third longitudinal rotation arm 5; L5 is the arm length of the fifth articulated arm, that is, the length of the transverse rotation arm 6.

[0079] In a specific embodiment, reference Figure 5 , step S1 specifically includes: the connecting rod coordinate system includes the robot base coordinate system {OB}, the first joint axis static coordinate system {OJ1s}, the first joint axis dynamic coordinate system {OJ1d}, the second joint axis static coordinate system {OJ2s}, the second joint axis dynamic coordinate system {OJ2d}, the third joint axis static coordinate system {OJ3s}, the third joint axis dynamic coordinate system {OJ3d}, the fourth joint axis static coordinate system {OJ4s}, the fourth joint axis dynamic coordinate system {OJ4d}, the fifth joint axis static coordinate system {OJ5s}, and the fifth joint axis dynamic coordinate system {OJ5d};

[0080] Establish the transformation matrix between the two adjacent coordinate systems above in sequence

[0081]

[0082]

[0083]

[0084]

[0085]

[0086]

[0087]

[0088]

[0089]

[0090]

[0091] Among them, L n is the length of the nth joint arm.

[0092] L1, L2, L3, and L4 involved in the above conversion matrix are all mechanical structure length dimensions or installation distance dimensions. Figure 3 , are all known quantities.

[0093] S2, set the first control point and the first control direction in the robot base coordinate system {OB}, set the second control point and the second control direction in the fifth joint axis dynamic coordinate system {OJ5d}, and establish the kinematic relationship between the second control point and the second control direction and the first control point and the first control direction according to the transformation matrix.

[0094] In a specific embodiment, step S2 specifically includes:

[0095] Establish kinematic relationship and set the first control point in the robot base coordinate system {OB} B P = [xyz 1] T and the first control direction B V=[ijk 0] T , set the second control point in the fifth joint axis dynamic coordinate system {OJ5d} J5d P=[0 L50 1] T and the second control direction

[0096] Transform the second control point and the second control direction in the fifth joint axis dynamic coordinate system {OJ5d} to the robot base coordinate system {OB} and establish the equation:

[0097]

[0098] Specifically, the unknowns in the above formula are the five rotation angles θ1, θ2, θ3, θ4, and θ5. The first control point set in the robot base coordinate system {OB} B P = [xyz 1] T and the first control direction B V=[ijk 0] T It is a known quantity. In the scenario involved in this application, the original data can be measured by a distance measuring device or a visual device, and then converted into a processing code (G code) and input into the control system.

[0099] S3, according to the equation relationship, use the geometric method to solve θ1, θ2, θ3, θ4, θ5, where θ n is the nth rotation axis J n The rotation angle is n=1, 2, 3, 4, 5.

[0100] In a specific embodiment, step S3 includes:

[0101] Calculate the origin coordinates of the fifth joint axis dynamic coordinate system {OJ5d} in the robot base coordinate system {OB} B P OJ5d =[x OJ5d y OJ5d z OJ5d 1] T , the calculation formula is: B P OJ5d = B P-L5*norm( B V), where norm( B V) is a vector B The normalized vector of V is expanded to give the following result:

[0102]

[0103] exist Figure 4 In the equation, θ1 is given by x OJ5d with y OJ5d The calculation formula is as follows:

[0104] θ1=-arctan 2(x OJ5d ,y OJ5d );

[0105] The first longitudinal rotating arm, the second longitudinal rotating arm and the third longitudinal rotating arm together form a longitudinal rotating surface. B P OJ5d The unit normal vector of the longitudinal rotation plane can be calculated in the robot base coordinate system {OB} BV ver-lon-plane :

[0106]

[0107] θ5 is essentially the first control direction B V=[ijk 0] T The angle between the longitudinal rotation plane and the normal vector of the longitudinal rotation plane B V ver-lon-plane So the calculation formula of θ5 is as follows:

[0108]

[0109] At the same time, find the first control direction B V=[ijk 0] T Projection vector in the longitudinal rotation plane B V pro-in-lon-plane , the formula is as follows:

[0110] B V pro-in-lon-plane =norm( B V)-(norm( B V) B V ver-lon-plane )* B V ver-lon-plane ;

[0111] Calculate the origin coordinates of the fourth joint axis dynamic coordinate system {OJ4d} in the robot base coordinate system {OB} B P OJ4d =[x OJ4d y OJ4d z OJ4d 1] T , the formula is as follows:

[0112] B P OJ4d = B P OJ5d -L4*norm( B V pro-in-lon-plane );

[0113] Calculate the angle between the third longitudinal rotating arm and the rotating platform in the longitudinal rotating plane in the robot base coordinate system {OB} B C J4 , the formula is as follows:

[0114] B C J4 =arccos(norm( B V pro-in-lon-plane )·[0 0 1 0] T );

[0115] In the static coordinate system {OJ2s} of the second joint axis, solve θ2, θ3, and θ4 using the following formulas:

[0116]

[0117] S4 , calculating the arm length D1 of the first support arm and the arm length D2 of the second support arm according to θ2 and θ3 .

[0118] In a specific embodiment, reference Figure 6 , For the auxiliary angle in the calculation process, let the intersection of the second support arm and the rotating platform be A2. In the triangle A2J2J3, the angle between J2J3 and J3A2 is Assume that the intersection of the second longitudinal rotating arm and the second supporting arm is A1. In the triangle A1J3A2, the angle between J3A2 and J3A1 is Therefore, step S4 specifically uses the following formula to convert θ2 and θ3 into D1 and D2:

[0119]

[0120] Specifically, θ2 and θ3 can be converted into D1 and D2 using trigonometric functions. This is because the three rotation angles θ1, θ4, and θ5 can be adjusted by rotating the motor installed on the joint, and the equivalent moment of inertia of θ2 and θ3 is large. If the motor is directly used to drive the joint, stability is difficult to guarantee. Therefore, a servo hydraulic cylinder is used to indirectly control θ2 and θ3.

[0121] In a specific embodiment, it also includes: using a servo hydraulic cylinder to indirectly control θ2 and θ3, and sending θ1, D1, D2, θ4, and θ5 to the drivers and motors on their respective corresponding joints to achieve five-degree-of-freedom movement of the lateral rotating arm in Cartesian space.

[0122] Further references Figure 7 As an implementation of the methods shown in the above figures, the present application provides an embodiment of an inverse solution device for a five-axis loading and unloading robot. Figure 2 Corresponding to the method embodiment shown, the device can be specifically applied to various electronic devices.

[0123] The embodiment of the present application provides an inverse solution device for a five-axis loading and unloading robot, comprising:

[0124] a coordinate establishment module 1 configured to establish a link coordinate system based on a three-dimensional model of a five-axis loading and unloading robot arm and to establish a transformation matrix between the coordinate systems, wherein the five-axis loading and unloading robot includes a first longitudinal rotating arm, a second longitudinal rotating arm, a third longitudinal rotating arm, a transverse rotating arm, a first support arm, and a second support arm;

[0125] a kinematic relationship building module 2 configured to set a first control point and a first control direction in the robot base coordinate system {OB}, set a second control point and a second control direction in the fifth joint axis dynamic coordinate system {OJ5d}, and establish a kinematic relationship between the second control point and the second control direction and the first control point and the first control direction according to a transformation matrix;

[0126] The first solving module 3 is configured to solve θ1, θ2, θ3, θ4, and θ5 according to the equation relationship using a geometric method, where θ n is the nth rotation axis J n The rotation angle, n = 1, 2, 3, 4, 5;

[0127] The second solving module 4 is configured to calculate the arm length D1 of the first support arm and the arm length D2 of the second support arm according to θ2 and θ3.

[0128] Reference below Figure 8 , which shows an electronic device (eg Figure 1 A schematic structural diagram of a computer device 800 (a server or terminal device as shown). Figure 8 The electronic device shown is merely an example and should not limit the functions and scope of use of the embodiments of the present application.

[0129] like Figure 8 As shown, the computer device 800 includes a central processing unit (CPU) 801 and a graphics processing unit (GPU) 802, which can perform various appropriate actions and processes according to the program stored in the read-only memory (ROM) 803 or the program loaded from the storage part 809 to the random access memory (RAM) 804. In the RAM 804, various programs and data required for the operation of the device 800 are also stored. The CPU 801, GPU 802, ROM 803 and RAM 804 are connected to each other through a bus 805. An input / output (I / O) interface 806 is also connected to the bus 805.

[0130] The following components are connected to the I / O interface 806: an input section 807 including a keyboard, a mouse, and the like; an output section 808 including a display such as a liquid crystal display (LCD), a speaker, and the like; a storage section 809 including a hard disk and the like; and a communication section 810 including a network interface card such as a LAN card or a modem. The communication section 810 performs communication processing via a network such as the Internet. A drive 811 may also be connected to the I / O interface 806 as needed. A removable medium 812, such as a magnetic disk, an optical disk, a magneto-optical disk, a semiconductor memory, and the like, is installed in the drive 811 as needed, so that a computer program read therefrom can be installed into the storage section 809 as needed.

[0131] In particular, according to an embodiment of the present disclosure, the process described above with reference to the flowchart can be implemented as a computer software program. For example, an embodiment of the present disclosure includes a computer program product, which includes a computer program carried on a computer-readable medium, and the computer program includes a program code for executing the method shown in the flowchart. In such an embodiment, the computer program can be downloaded and installed from the network through the communication part 810, and / or installed from the removable medium 812. When the computer program is executed by the central processing unit (CPU) 801 and the graphics processing unit (GPU) 802, the above-mentioned functions defined in the method of the present application are executed.

[0132] It should be noted that the computer-readable medium described in this application may be a computer-readable signal medium or a computer-readable medium, or any combination thereof. Computer-readable media may be, for example, but not limited to, electrical, magnetic, optical, electromagnetic, infrared, or semiconductor devices, apparatuses, or components, or any combination thereof. More specific examples of computer-readable media may include, but are not limited to, an electrical connection having one or more conductors, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination thereof. In this application, a computer-readable medium may be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution device, apparatus, or component. In this application, a computer-readable signal medium may include a data signal propagated in baseband or as part of a carrier wave, which carries computer-readable program code. This propagated data signal may take a variety of forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination thereof. A computer-readable signal medium may also be any computer-readable medium other than a computer-readable medium that can transmit, propagate, or transport a program for use by or in conjunction with an instruction execution apparatus, device, or device. Program code embodied on a computer-readable medium may be transmitted using any suitable medium, including but not limited to wireless, wireline, optical cable, RF, or any suitable combination thereof.

[0133] Computer program code for performing the operations of the present application can be written in one or more programming languages, or a combination thereof, including object-oriented programming languages such as Java, Smalltalk, C++, and conventional procedural programming languages such as "C" or similar programming languages. The program code can be executed entirely on the user's computer, partially on the user's computer, as a stand-alone software package, partially on the user's computer and partially on a remote computer, or entirely on the remote computer or server. In cases involving a remote computer, the remote computer can be connected to the user's computer through any type of network, including a local area network (LAN) or a wide area network (WAN), or can be connected to an external computer (e.g., through the Internet using an Internet service provider).

[0134] The flowcharts and block diagrams in the accompanying drawings illustrate the possible architectures, functions and operations of the devices, methods and computer program products according to various embodiments of the present application. In this regard, each box in the flowchart or block diagram can represent a module, program segment or part of code, and the module, program segment or part of code contains one or more executable instructions for implementing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in an order different from that marked in the accompanying drawings. For example, two boxes represented in succession can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or flowchart and the combination of boxes in the block diagram and / or flowchart can be implemented with a dedicated hardware-based device that performs the specified function or operation, or can be implemented with a combination of dedicated hardware and computer instructions.

[0135] The modules involved in the embodiments described in this application may be implemented in software or hardware, and may also be set in a processor.

[0136] As another aspect, the present application also provides a computer-readable medium, which may be included in the electronic device described in the above embodiment; or it may exist independently and not be assembled into the electronic device. The above computer-readable medium carries one or more programs. When the above one or more programs are executed by the electronic device, the electronic device: establishes its connecting rod coordinate system based on the three-dimensional model of the robotic arm of the five-axis loading and unloading robot, and establishes a transformation matrix between each coordinate system, wherein the five-axis loading and unloading robot includes a first longitudinal rotation arm, a second longitudinal rotation arm, a third longitudinal rotation arm, a transverse rotation arm, a first support arm and a second support arm; sets a first control point and a first control direction in the robot base coordinate system {OB}, sets a second control point and a second control direction in the fifth joint axis dynamic coordinate system {OJ5d}, and establishes a kinematic relationship between the second control point and the second control direction and the first control point and the first control direction according to the transformation matrix; uses a geometric method to solve θ1, θ2, θ3, θ4, and θ5 according to the equation relationship, wherein θ n is the nth rotation axis J n The rotation angle is n=1, 2, 3, 4, 5; the arm length D1 of the first support arm and the arm length D2 of the second support arm are calculated according to θ2 and θ3.

[0137] The above description is merely a preferred embodiment of the present application and an illustration of the technical principles employed. Those skilled in the art should understand that the scope of the invention involved in this application is not limited to the technical solutions formed by the specific combination of the above-mentioned technical features, but also encompasses other technical solutions formed by any combination of the above-mentioned technical features or their equivalents without departing from the above-mentioned inventive concept. For example, a technical solution formed by replacing the above-mentioned features with (but not limited to) technical features with similar functions disclosed in this application.

Claims

1. An inverse solution method for a five-axis loading and unloading robot, characterized in that: The following steps are involved: S1, based on the three-dimensional model of the robotic arm of the five-axis loading and unloading robot, its connecting rod coordinate system is established, and the conversion matrix between each coordinate system is established, wherein the five-axis loading and unloading robot includes a first longitudinal rotating arm, a second longitudinal rotating arm, a third longitudinal rotating arm, a transverse rotating arm, a first support arm and a second support arm, and the five-axis loading and unloading robot also includes a movable base and a rotating platform. The rotation axis around which the rotating platform rotates is J1. The first longitudinal rotating arm is connected to the rotating platform, and the rotation axis between the two is J2; the second longitudinal rotating arm is connected to the first longitudinal rotating arm, and the rotation axis between the two is J3; the third longitudinal rotating arm is connected to the first longitudinal rotating arm, and the rotation axis between the two is J3. The arm is connected to the second longitudinal rotating arm, and the rotation axis between the two is J4; the transverse rotating arm is connected to the third longitudinal rotating arm, and the rotation axis between the two is J5; one side of the first support arm is installed on the first longitudinal rotating arm, and the other side is installed on the rotating platform. S1 is the installation dimension of the first support arm on the first longitudinal rotating arm, and S2 is the installation dimension of the first support arm on the rotating platform; one side of the second support arm is installed on the second longitudinal rotating arm, and the other side is installed on the rotating platform. S3 is the installation dimension of the second support arm on the second longitudinal rotating arm, and S4 is the installation dimension of the second support arm on the rotating platform; S2, setting a first control point and a first control direction in the robot base coordinate system {OB}, setting a second control point and a second control direction in the fifth joint axis dynamic coordinate system {OJ5d}, and establishing a kinematic relationship between the second control point and the second control direction and the first control point and the first control direction according to the transformation matrix; S3, according to the equation relationship, use the geometric method to solve θ1, θ2, θ3, θ4, θ5, where θ n is the nth rotation axis J n The rotation angle, n = 1, 2, 3, 4, 5; S4 , calculating the arm length D1 of the first support arm and the arm length D2 of the second support arm according to θ2 and θ3 .

2. The inverse solution method of the five-axis loading and unloading robot according to claim 1, characterized in that: The step S1 specifically includes: the connecting rod coordinate system includes the robot base coordinate system {OB}, the first joint axis static coordinate system {OJ1s}, the first joint axis dynamic coordinate system {OJ1d}, the second joint axis static coordinate system {OJ2s}, the second joint axis dynamic coordinate system {OJ2d}, the third joint axis static coordinate system {OJ3s}, the third joint axis dynamic coordinate system {OJ3d}, the fourth joint axis static coordinate system {OJ4s}, the fourth joint axis dynamic coordinate system {OJ4d}, the fifth joint axis static coordinate system {OJ5s}, and the fifth joint axis dynamic coordinate system {OJ5d}; Establish the transformation matrix between the two adjacent coordinate systems above in sequence Among them, L n is the length of the nth joint arm.

3. The inverse solution method of the five-axis loading and unloading robot according to claim 2, characterized in that: The step S2 specifically includes: Set the first control point in the robot base coordinate system {OB} B P = [xyz 1] T and the first control direction B V=[ij k0] T , set the second control point in the fifth joint axis dynamic coordinate system {OJ5d} J5d P=[0L5 01] T and the second control direction J5d V=[0100] T ; The second control point and the second control direction in the fifth joint axis dynamic coordinate system {OJ5d} are transformed into the robot base coordinate system {OB} to establish the equation relationship:

4. The inverse solution method of the five-axis loading and unloading robot according to claim 3 is characterized in that: The step S3 comprises: Calculate the origin coordinates of the fifth joint axis dynamic coordinate system {OJ5d} in the robot base coordinate system {OB} B P OJ5d =[x OJ5d y OJ5d z OJ5d 1] T , the calculation formula is: B P OJ5d = B P-L5*norm( B V), where norm( B V) is a vector B The normalized vector of V is expanded to give the following result: θ1 is determined by x OJ5d with y OJ5d The calculation formula is as follows: θ1=-arctan2(x OJ5d ,y OJ5d ); The first longitudinal rotation arm, the second longitudinal rotation arm, and the third longitudinal rotation arm together form a longitudinal rotation plane. The unit normal vector of the longitudinal rotation plane is calculated in the robot base coordinate system {OB}. B V ver-lon-plane : θ5 is the first control direction B V=[ijk 0] T The angle between θ5 and the longitudinal rotation plane is as follows: At the same time, find the first control direction B V=[ijk 0] T Projection vector in the longitudinal rotation plane B V pro-in-lon-plane , the formula is as follows: B V pro-in-lon-plane =norm( B V)-(norm( B V)· B V ver-lon-plane )* B V ver-lon-plane ; Calculate the origin coordinates of the fourth joint axis dynamic coordinate system {OJ4d} in the robot base coordinate system {OB} B P OJ4d =[x OJ4d y OJ4d z OJ4d 1] T , the formula is as follows: B P OJ4d = B P OJ5d -L4*norm( B V pro-in-lon-plane ); Calculate the angle between the third longitudinal rotating arm and the rotating platform in the longitudinal rotating plane in the robot base coordinate system {OB} B C J4 , the formula is as follows: B C J4 arccos(norm( B V pro-in-lon-plane )·[0010] T ); In the static coordinate system {OJ2s} of the second joint axis, solve θ2, θ3, and θ4 using the following formulas:

5. The inverse solution method of the five-axis loading and unloading robot according to claim 4, characterized in that: The step S4 specifically converts θ2 and θ3 into D1 and D2 using the following formula:

6. The inverse solution method of the five-axis loading and unloading robot according to claim 1, characterized in that: Also includes: A servo hydraulic cylinder is used to indirectly control θ2 and θ3, and θ1, D1, D2, θ4, and θ5 are sent to the drivers and motors on their corresponding joints to achieve five-degree-of-freedom motion of the lateral rotating arm in Cartesian space.

7. An inverse solution device for a five-axis loading and unloading robot, characterized in that: include: The coordinate establishment module is configured to establish the link coordinate system of the five-axis loading and unloading robot based on the three-dimensional model of the robot arm, and to establish a conversion matrix between the coordinate systems, wherein the five-axis loading and unloading robot includes a first longitudinal rotating arm, a second longitudinal rotating arm, a third longitudinal rotating arm, a transverse rotating arm, a first support arm and a second support arm, and the five-axis loading and unloading robot also includes a movable base and a rotating platform, the rotation axis around which the rotating platform rotates is J1, the first longitudinal rotating arm is connected to the rotating platform, and the rotation axis between the two is J2; the second longitudinal rotating arm is connected to the first longitudinal rotating arm, and the rotation axis between the two is J3; the third longitudinal rotating arm is connected to the first longitudinal rotating arm, and the rotation axis between the two is J3; The longitudinal rotating arm is connected to the second longitudinal rotating arm, and the rotation axis therebetween is J4; the transverse rotating arm is connected to the third longitudinal rotating arm, and the rotation axis therebetween is J5; one side of the first support arm is mounted on the first longitudinal rotating arm, and the other side is mounted on the rotating platform. S1 is the installation dimension of the first support arm on the first longitudinal rotating arm, and S2 is the installation dimension of the first support arm on the rotating platform; one side of the second support arm is mounted on the second longitudinal rotating arm, and the other side is mounted on the rotating platform. S3 is the installation dimension of the second support arm on the second longitudinal rotating arm, and S4 is the installation dimension of the second support arm on the rotating platform. a kinematic relationship building module, configured to set a first control point and a first control direction in the robot base coordinate system {OB}, set a second control point and a second control direction in the fifth joint axis dynamic coordinate system {OJ5d}, and establish a kinematic relationship for transformation between the second control point and the second control direction and the first control point and the first control direction according to the transformation matrix; The first solving module is configured to solve θ1, θ2, θ3, θ4, and θ5 using a geometric method according to the equation relationship, where θ n is the nth rotation axis J n The rotation angle, n = 1, 2, 3, 4, 5; The second solving module is configured to calculate the arm length D1 of the first support arm and the arm length D2 of the second support arm according to θ2 and θ3.

8. An electronic device comprising: one or more processors; a storage device for storing one or more programs, When the one or more programs are executed by the one or more processors, the one or more processors implement the method according to any one of claims 1 to 6.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the method according to any one of claims 1 to 6 is implemented.

Images